Examples with JacobianEntry spacegraph.space3d.phys.solve.JacobianEntry used on opensource projects

Example 1 with JacobianEntry

use of spacegraph.space3d.phys.solve.JacobianEntry in project narchy by automenta.

the class ContactConstraint method resolveSingleBilateral.

/**
 * Bilateral constraint between two dynamic objects.
 */
public static void resolveSingleBilateral(Body3D body1, v3 pos1, Body3D body2, v3 pos2, float distance, v3 normal, float[] impulse, float timeStep) {
    float normalLenSqr = normal.lengthSquared();
    assert (Math.abs(normalLenSqr) < 1.1f);
    if (normalLenSqr > 1.1f) {
        impulse[0] = 0f;
        return;
    }
    v3 tmp = new v3();
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmp));
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmp));
    // this jacobian entry could be re-used for all iterations
    v3 vel1 = new v3();
    body1.getVelocityInLocalPoint(rel_pos1, vel1);
    v3 vel2 = new v3();
    body2.getVelocityInLocalPoint(rel_pos2, vel2);
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    Matrix3f mat1 = body1.getCenterOfMassTransform(new Transform()).basis;
    mat1.transpose();
    Matrix3f mat2 = body2.getCenterOfMassTransform(new Transform()).basis;
    mat2.transpose();
    JacobianEntry jac = new JacobianEntry();
    jac.init(mat1, mat2, rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(new v3()), body1.getInvMass(), body2.getInvInertiaDiagLocal(new v3()), body2.getInvMass());
    float jacDiagAB = jac.Adiag;
    float jacDiagABInv = 1f / jacDiagAB;
    v3 tmp1 = body1.getAngularVelocity(new v3());
    mat1.transform(tmp1);
    v3 tmp2 = body2.getAngularVelocity(new v3());
    mat2.transform(tmp2);
    float rel_vel = jac.getRelativeVelocity(body1.getLinearVelocity(new v3()), tmp1, body2.getLinearVelocity(new v3()), tmp2);
    float a;
    a = jacDiagABInv;
    rel_vel = normal.dot(vel);
    // todo: move this into proper structure
    float contactDamping = 0.2f;
    // #ifdef ONLY_USE_LINEAR_MASS
    // btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
    // impulse = - contactDamping * rel_vel * massTerm;
    // #else
    float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
    impulse[0] = velocityImpulse;
// #endif
}