DistanceJoint.java

    private final Vector3f anchorA, anchorB;
    private final float distance;

    private Vector3f rA = new Vector3f(), rB = new Vector3f();
    private Vector3f u = new Vector3f();
    private float constraintMass;

    public DistanceJoint(short entityA, short entityB, Vector3f anchorA, Vector3f anchorB, float distance) {
        super(entityA, entityB);
        this.anchorA = anchorA;
        this.anchorB = anchorB;
        this.distance = distance;
    }

    @Override
    public void initVelocityConstraints(Level level) {
        PositionComponent posA = level.getComponent(entityA, PositionComponent.class);
        PositionComponent posB = level.getComponent(entityB, PositionComponent.class);
        PhysicsComponent physA = level.getComponent(entityA, PhysicsComponent.class);
        PhysicsComponent physB = level.getComponent(entityB, PhysicsComponent.class);

        rA.set(anchorA).rotate(posA.getRot()).mul(posA.getScale());
        rB.set(anchorB).rotate(posB.getRot()).mul(posB.getScale());
        u.set(rB).add(posB.getPos()).sub(rA).sub(posA.getPos());
        if (u.lengthSquared() < 0.000001f)
            u.set(0, 0, 0);
        else
            u.normalize();

        // massInv = massInvA + massInvB + (rA x u)T * inertiaInvA * (rA x u) + (rB x u)T * inertiaInvB * (rB x u)
        float massInv = 1 / physA.mass + 1 / physB.mass;

        Vector3f cross = new Vector3f(rA).cross(u);
        massInv += cross.mul(physA.inertiaInv, new Vector3f()).dot(cross);

        cross.set(rB).cross(u);
        massInv += cross.mul(physB.inertiaInv, new Vector3f()).dot(cross);

        constraintMass = massInv == 0 ? 0 : 1 / massInv;
    }

    @Override
    public void solveVelocityConstraints(Level level) {
        PositionComponent posA = level.getComponent(entityA, PositionComponent.class);
        PositionComponent posB = level.getComponent(entityB, PositionComponent.class);
        PhysicsComponent physA = level.getComponent(entityA, PhysicsComponent.class);
        PhysicsComponent physB = level.getComponent(entityB, PhysicsComponent.class);

        // cDot = u . (vA + (wA x rA) - vB - (wB x rB))
        float cDot = new Vector3f(physB.getVelocity()).add(physB.getAngularVelocity(posB.getRot()).cross(rB))
                .sub(physA.getVelocity()).sub(physA.getAngularVelocity(posA.getRot()).cross(rA))
                .dot(u);

        float impulse = -constraintMass * (cDot); // TODO: bias
        Vector3f dp = u.mul(impulse, new Vector3f());

        physA.momentum.sub(dp);
        physA.angularMomentum.sub(rA.cross(dp, new Vector3f()));
        physB.momentum.add(dp);
        physB.angularMomentum.add(rB.cross(dp, new Vector3f()));
    }