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

Example 1 with ConstraintPersistentData

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

the class ContactConstraint method resolveSingleFriction.

public static float resolveSingleFriction(Body3D body1, Body3D body2, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) {
    v3 tmpVec = new v3();
    v3 pos1 = contactPoint.getPositionWorldOnA(new v3());
    v3 pos2 = contactPoint.getPositionWorldOnB(new v3());
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmpVec));
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmpVec));
    ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
    assert (cpd != null);
    float combinedFriction = cpd.friction;
    float limit = cpd.appliedImpulse * combinedFriction;
    if (// friction
    cpd.appliedImpulse > 0f) {
        // apply friction in the 2 tangential directions
        // 1st tangent
        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);
        float j1, j2;
        {
            float vrel = cpd.frictionWorldTangential0.dot(vel);
            // calculate j that moves us to zero relative velocity
            j1 = -vrel * cpd.jacDiagABInvTangent0;
            float oldTangentImpulse = cpd.accumulatedTangentImpulse0;
            cpd.accumulatedTangentImpulse0 = oldTangentImpulse + j1;
            cpd.accumulatedTangentImpulse0 = Math.min(cpd.accumulatedTangentImpulse0, limit);
            cpd.accumulatedTangentImpulse0 = Math.max(cpd.accumulatedTangentImpulse0, -limit);
            j1 = cpd.accumulatedTangentImpulse0 - oldTangentImpulse;
        }
        // 2nd tangent
        float vrel = cpd.frictionWorldTangential1.dot(vel);
        // calculate j that moves us to zero relative velocity
        j2 = -vrel * cpd.jacDiagABInvTangent1;
        float oldTangentImpulse = cpd.accumulatedTangentImpulse1;
        cpd.accumulatedTangentImpulse1 = oldTangentImpulse + j2;
        cpd.accumulatedTangentImpulse1 = Math.min(cpd.accumulatedTangentImpulse1, limit);
        cpd.accumulatedTangentImpulse1 = Math.max(cpd.accumulatedTangentImpulse1, -limit);
        j2 = cpd.accumulatedTangentImpulse1 - oldTangentImpulse;
        // #ifdef USE_INTERNAL_APPLY_IMPULSE
        v3 tmp = new v3();
        if (body1.getInvMass() != 0f) {
            tmp.scale(body1.getInvMass(), cpd.frictionWorldTangential0);
            body1.internalApplyImpulse(tmp, cpd.frictionAngularComponent0A, j1);
            tmp.scale(body1.getInvMass(), cpd.frictionWorldTangential1);
            body1.internalApplyImpulse(tmp, cpd.frictionAngularComponent1A, j2);
        }
        if (body2.getInvMass() != 0f) {
            tmp.scale(body2.getInvMass(), cpd.frictionWorldTangential0);
            body2.internalApplyImpulse(tmp, cpd.frictionAngularComponent0B, -j1);
            tmp.scale(body2.getInvMass(), cpd.frictionWorldTangential1);
            body2.internalApplyImpulse(tmp, cpd.frictionAngularComponent1B, -j2);
        }
    // #else //USE_INTERNAL_APPLY_IMPULSE
    // body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
    // body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
    // #endif //USE_INTERNAL_APPLY_IMPULSE
    }
    return cpd.appliedImpulse;
}

Example 2 with ConstraintPersistentData

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

the class ContactConstraint method resolveSingleCollisionCombined.

/**
 * velocity + friction<br>
 * response between two dynamic objects with friction
 */
public static float resolveSingleCollisionCombined(Body3D body1, Body3D body2, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) {
    v3 tmpVec = new v3();
    v3 pos1 = contactPoint.getPositionWorldOnA(new v3());
    v3 pos2 = contactPoint.getPositionWorldOnB(new v3());
    v3 normal = contactPoint.normalWorldOnB;
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmpVec));
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmpVec));
    v3 vel1 = body1.getVelocityInLocalPoint(rel_pos1, new v3());
    v3 vel2 = body2.getVelocityInLocalPoint(rel_pos2, new v3());
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    float rel_vel;
    rel_vel = normal.dot(vel);
    float Kfps = 1f / solverInfo.timeStep;
    // btScalar damping = solverInfo.m_damping ;
    float Kerp = solverInfo.erp;
    float Kcor = Kerp * Kfps;
    ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
    assert (cpd != null);
    float distance = cpd.penetration;
    float positionalError = Kcor * -distance;
    // * damping;
    float velocityError = cpd.restitution - rel_vel;
    float penetrationImpulse = positionalError * cpd.jacDiagABInv;
    float velocityImpulse = velocityError * cpd.jacDiagABInv;
    float normalImpulse = penetrationImpulse + velocityImpulse;
    // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    float oldNormalImpulse = cpd.appliedImpulse;
    float sum = oldNormalImpulse + normalImpulse;
    cpd.appliedImpulse = 0f > sum ? 0f : sum;
    normalImpulse = cpd.appliedImpulse - oldNormalImpulse;
    // #ifdef USE_INTERNAL_APPLY_IMPULSE
    v3 tmp = new v3();
    if (body1.getInvMass() != 0f) {
        tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB);
        body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse);
    }
    if (body2.getInvMass() != 0f) {
        tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB);
        body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse);
    }
    // #else //USE_INTERNAL_APPLY_IMPULSE
    // body1.applyImpulse(normal*(normalImpulse), rel_pos1);
    // body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
    // #endif //USE_INTERNAL_APPLY_IMPULSE
    // friction
    body1.getVelocityInLocalPoint(rel_pos1, vel1);
    body2.getVelocityInLocalPoint(rel_pos2, vel2);
    vel.sub(vel1, vel2);
    rel_vel = normal.dot(vel);
    tmp.scale(rel_vel, normal);
    v3 lat_vel = new v3();
    lat_vel.sub(vel, tmp);
    float lat_rel_vel = lat_vel.length();
    float combinedFriction = cpd.friction;
    if (cpd.appliedImpulse > 0) {
        if (lat_rel_vel > BulletGlobals.FLT_EPSILON) {
            lat_vel.scale(1f / lat_rel_vel);
            v3 temp1 = new v3();
            temp1.cross(rel_pos1, lat_vel);
            body1.getInvInertiaTensorWorld(new Matrix3f()).transform(temp1);
            v3 temp2 = new v3();
            temp2.cross(rel_pos2, lat_vel);
            body2.getInvInertiaTensorWorld(new Matrix3f()).transform(temp2);
            v3 java_tmp1 = new v3();
            java_tmp1.cross(temp1, rel_pos1);
            v3 java_tmp2 = new v3();
            java_tmp2.cross(temp2, rel_pos2);
            tmp.add(java_tmp1, java_tmp2);
            float friction_impulse = lat_rel_vel / (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(tmp));
            float normal_impulse = cpd.appliedImpulse * combinedFriction;
            friction_impulse = Math.min(friction_impulse, normal_impulse);
            friction_impulse = Math.max(friction_impulse, -normal_impulse);
            tmp.scale(-friction_impulse, lat_vel);
            body1.impulse(tmp, rel_pos1);
            tmp.scale(friction_impulse, lat_vel);
            body2.impulse(tmp, rel_pos2);
        }
    }
    return normalImpulse;
}

Example 3 with ConstraintPersistentData

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

the class ContactConstraint method resolveSingleCollision.

/**
 * Response between two dynamic objects with friction.
 */
public static float resolveSingleCollision(Body3D body1, Body3D body2, ManifoldPoint contactPoint, ContactSolverInfo solverInfo) {
    v3 tmpVec = new v3();
    v3 pos1_ = contactPoint.getPositionWorldOnA(new v3());
    v3 pos2_ = contactPoint.getPositionWorldOnB(new v3());
    v3 normal = contactPoint.normalWorldOnB;
    // constant over all iterations
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pos1_, body1.getCenterOfMassPosition(tmpVec));
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pos2_, body2.getCenterOfMassPosition(tmpVec));
    v3 vel1 = body1.getVelocityInLocalPoint(rel_pos1, new v3());
    v3 vel2 = body2.getVelocityInLocalPoint(rel_pos2, new v3());
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    float rel_vel;
    rel_vel = normal.dot(vel);
    float Kfps = 1f / solverInfo.timeStep;
    // btScalar damping = solverInfo.m_damping ;
    float Kerp = solverInfo.erp;
    float Kcor = Kerp * Kfps;
    ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
    assert (cpd != null);
    float distance = cpd.penetration;
    float positionalError = Kcor * -distance;
    // * damping;
    float velocityError = cpd.restitution - rel_vel;
    float penetrationImpulse = positionalError * cpd.jacDiagABInv;
    float velocityImpulse = velocityError * cpd.jacDiagABInv;
    float normalImpulse = penetrationImpulse + velocityImpulse;
    // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    float oldNormalImpulse = cpd.appliedImpulse;
    float sum = oldNormalImpulse + normalImpulse;
    cpd.appliedImpulse = 0f > sum ? 0f : sum;
    normalImpulse = cpd.appliedImpulse - oldNormalImpulse;
    // #ifdef USE_INTERNAL_APPLY_IMPULSE
    v3 tmp = new v3();
    if (body1.getInvMass() != 0f) {
        tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB);
        body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse);
    }
    if (body2.getInvMass() != 0f) {
        tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB);
        body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse);
    }
    return normalImpulse;
}