Examples with StaticAlloc com.bulletphysics.util.StaticAlloc used on opensource projects

Example 11 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class SequentialImpulseConstraintSolver method addFrictionConstraint.

@StaticAlloc
protected void addFrictionConstraint(Vector3f normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, Vector3f rel_pos1, Vector3f rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation) {
    RigidBody body0 = RigidBody.upcast(colObj0);
    RigidBody body1 = RigidBody.upcast(colObj1);
    SolverConstraint solverConstraint = constraintsPool.get();
    tmpSolverFrictionConstraintPool.add(solverConstraint);
    solverConstraint.contactNormal.set(normalAxis);
    solverConstraint.solverBodyIdA = solverBodyIdA;
    solverConstraint.solverBodyIdB = solverBodyIdB;
    solverConstraint.constraintType = SolverConstraintType.SOLVER_FRICTION_1D;
    solverConstraint.frictionIndex = frictionIndex;
    solverConstraint.friction = cp.combinedFriction;
    solverConstraint.originalContactPoint = null;
    solverConstraint.appliedImpulse = 0f;
    solverConstraint.appliedPushImpulse = 0f;
    solverConstraint.penetration = 0f;
    Stack stack = Stack.enter();
    Vector3f ftorqueAxis1 = stack.allocVector3f();
    Matrix3f tmpMat = stack.allocMatrix3f();
    {
        ftorqueAxis1.cross(rel_pos1, solverConstraint.contactNormal);
        solverConstraint.relpos1CrossNormal.set(ftorqueAxis1);
        if (body0 != null) {
            solverConstraint.angularComponentA.set(ftorqueAxis1);
            body0.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentA);
        } else {
            solverConstraint.angularComponentA.set(0f, 0f, 0f);
        }
    }
    {
        ftorqueAxis1.cross(rel_pos2, solverConstraint.contactNormal);
        solverConstraint.relpos2CrossNormal.set(ftorqueAxis1);
        if (body1 != null) {
            solverConstraint.angularComponentB.set(ftorqueAxis1);
            body1.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentB);
        } else {
            solverConstraint.angularComponentB.set(0f, 0f, 0f);
        }
    }
    //#ifdef COMPUTE_IMPULSE_DENOM
    //	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
    //	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
    //#else
    Vector3f vec = stack.allocVector3f();
    float denom0 = 0f;
    float denom1 = 0f;
    if (body0 != null) {
        vec.cross(solverConstraint.angularComponentA, rel_pos1);
        denom0 = body0.getInvMass() + normalAxis.dot(vec);
    }
    if (body1 != null) {
        vec.cross(solverConstraint.angularComponentB, rel_pos2);
        denom1 = body1.getInvMass() + normalAxis.dot(vec);
    }
    //#endif //COMPUTE_IMPULSE_DENOM
    float denom = relaxation / (denom0 + denom1);
    solverConstraint.jacDiagABInv = denom;
    stack.leave();
}

Example 12 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class TranslationalLimitMotor method solveLinearAxis.

@StaticAlloc
public float solveLinearAxis(float timeStep, float jacDiagABInv, RigidBody body1, Vector3f pointInA, RigidBody body2, Vector3f pointInB, int limit_index, Vector3f axis_normal_on_a, Vector3f anchorPos) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmpVec = stack.allocVector3f();
    // find relative velocity
    Vector3f rel_pos1 = stack.allocVector3f();
    //rel_pos1.sub(pointInA, body1.getCenterOfMassPosition(tmpVec));
    rel_pos1.sub(anchorPos, body1.getCenterOfMassPosition(tmpVec));
    Vector3f rel_pos2 = stack.allocVector3f();
    //rel_pos2.sub(pointInB, body2.getCenterOfMassPosition(tmpVec));
    rel_pos2.sub(anchorPos, body2.getCenterOfMassPosition(tmpVec));
    Vector3f vel1 = body1.getVelocityInLocalPoint(rel_pos1, stack.allocVector3f());
    Vector3f vel2 = body2.getVelocityInLocalPoint(rel_pos2, stack.allocVector3f());
    Vector3f vel = stack.allocVector3f();
    vel.sub(vel1, vel2);
    float rel_vel = axis_normal_on_a.dot(vel);
    // apply displacement correction
    // positional error (zeroth order error)
    tmp.sub(pointInA, pointInB);
    float depth = -(tmp).dot(axis_normal_on_a);
    float lo = -1e30f;
    float hi = 1e30f;
    float minLimit = VectorUtil.getCoord(lowerLimit, limit_index);
    float maxLimit = VectorUtil.getCoord(upperLimit, limit_index);
    // handle the limits
    if (minLimit < maxLimit) {
        {
            if (depth > maxLimit) {
                depth -= maxLimit;
                lo = 0f;
            } else {
                if (depth < minLimit) {
                    depth -= minLimit;
                    hi = 0f;
                } else {
                    stack.leave();
                    return 0.0f;
                }
            }
        }
    }
    float normalImpulse = limitSoftness * (restitution * depth / timeStep - damping * rel_vel) * jacDiagABInv;
    float oldNormalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index);
    float sum = oldNormalImpulse + normalImpulse;
    VectorUtil.setCoord(accumulatedImpulse, limit_index, sum > hi ? 0f : sum < lo ? 0f : sum);
    normalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index) - oldNormalImpulse;
    Vector3f impulse_vector = stack.allocVector3f();
    impulse_vector.scale(normalImpulse, axis_normal_on_a);
    body1.applyImpulse(impulse_vector, rel_pos1);
    tmp.negate(impulse_vector);
    body2.applyImpulse(tmp, rel_pos2);
    stack.leave();
    return normalImpulse;
}