Examples with Matrix3f spacegraph.util.math.Matrix3f used on opensource projects

Example 6 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class ConeTwistConstraint method buildJacobian.

@Override
public void buildJacobian() {
    v3 tmp = new v3();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    Transform tmpTrans = new Transform();
    appliedImpulse = 0f;
    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;
    if (!angularOnly) {
        v3 pivotAInW = new v3(rbAFrame);
        rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
        v3 pivotBInW = new v3(rbBFrame);
        rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
        v3 relPos = new v3();
        relPos.sub(pivotBInW, pivotAInW);
        // TODO: stack
        v3[] normal = /*[3]*/
        { new v3(), new v3(), new v3() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].normalize(relPos);
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            Matrix3f mat1 = rbA.getCenterOfMassTransform(new Transform()).basis;
            mat1.transpose();
            Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).basis;
            mat2.transpose();
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        }
    }
    v3 b1Axis1 = new v3(), b1Axis2 = new v3(), b1Axis3 = new v3();
    v3 b2Axis1 = new v3(), b2Axis2 = new v3();
    rbAFrame.basis.getColumn(0, b1Axis1);
    getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);
    rbBFrame.basis.getColumn(0, b2Axis1);
    getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);
    float swing1 = 0f, swing2 = 0f;
    float swx = 0f, swy = 0f;
    float thresh = 10f;
    float fact;
    // Get Frame into world space
    if (swingSpan1 >= 0.05f) {
        rbAFrame.basis.getColumn(1, b1Axis2);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
        // swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis2);
        swing1 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing1 *= fact;
    }
    if (swingSpan2 >= 0.05f) {
        rbAFrame.basis.getColumn(2, b1Axis3);
        getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
        // swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
        swx = b2Axis1.dot(b1Axis1);
        swy = b2Axis1.dot(b1Axis3);
        swing2 = ScalarUtil.atan2Fast(swy, swx);
        fact = (swy * swy + swx * swx) * thresh * thresh;
        fact = fact / (fact + 1f);
        swing2 *= fact;
    }
    float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
    float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
    float EllipseAngle = Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;
    if (EllipseAngle > 1.0f) {
        swingCorrection = EllipseAngle - 1.0f;
        solveSwingLimit = true;
        // Calculate necessary axis & factors
        tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
        tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
        tmp.add(tmp1, tmp2);
        swingAxis.cross(b2Axis1, tmp);
        swingAxis.normalize();
        float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
        swingAxis.scale(swingAxisSign);
        kSwing = 1f / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis) + getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
    }
    // Twist limits
    if (twistSpan >= 0f) {
        // Vector3f b2Axis2 = new Vector3f();
        rbBFrame.basis.getColumn(1, b2Axis2);
        getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
        Quat4f rotationArc = QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, new Quat4f());
        v3 TwistRef = QuaternionUtil.quatRotate(rotationArc, b2Axis2, new v3());
        float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
        float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
        if (twist <= -twistSpan * lockedFreeFactor) {
            twistCorrection = -(twist + twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            twistAxis.scale(-1.0f);
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        } else if (twist > twistSpan * lockedFreeFactor) {
            twistCorrection = (twist - twistSpan);
            solveTwistLimit = true;
            twistAxis.add(b2Axis1, b1Axis1);
            twistAxis.scale(0.5f);
            twistAxis.normalize();
            kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
        }
    }
}

Example 7 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class DistanceConstraint method buildJacobian.

@Override
public void buildJacobian() {
    appliedImpulse = 0;
    Transform posA = rbA.getCenterOfMassTransform(new Transform());
    Transform posB = rbB.getCenterOfMassTransform(new Transform());
    v3 relA = new v3(pivotInA);
    posA.transform(relA);
    v3 relB = new v3(pivotInB);
    posB.transform(relB);
    v3 del = new v3();
    del.sub(posB, posA);
    float currDist = (float) Math.sqrt(del.dot(del));
    v3 ortho = del;
    ortho.scale(1f / currDist);
    Matrix3f tmpMat1 = new Matrix3f(), tmpMat2 = new Matrix3f();
    tmpMat1.transpose(posA.basis);
    tmpMat2.transpose(posB.basis);
    v3 tmp1 = v(pivotInA), tmp2 = v(pivotInB);
    posA.transform(tmp1);
    tmp1.sub(rbA.getCenterOfMassPosition(v()));
    posB.transform(tmp2);
    tmp2.sub(rbB.getCenterOfMassPosition(v()));
    jac.init(tmpMat1, tmpMat2, tmp1, tmp2, ortho, rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
    this.error = (currDist - dist) * speed;
// System.out.println("dist=" + (currDist - dist) + " " + jac.linearJointAxis);
}

Example 8 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Generic6DofConstraint method calculateAngleInfo.

/**
 * Calcs the euler angles between the two bodies.
 */
protected void calculateAngleInfo() {
    Matrix3f mat = new Matrix3f();
    Matrix3f relative_frame = new Matrix3f();
    mat.set(calculatedTransformA.basis);
    MatrixUtil.invert(mat);
    relative_frame.mul(mat, calculatedTransformB.basis);
    matrixToEulerXYZ(relative_frame, calculatedAxisAngleDiff);
    // in euler angle mode we do not actually constrain the angular velocity
    // along the axes axis[0] and axis[2] (although we do use axis[1]) :
    // 
    // to get			constrain w2-w1 along		...not
    // ------			---------------------		------
    // d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
    // d(angle[1])/dt = 0	ax[1]
    // d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
    // 
    // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
    // to prove the result for angle[0], write the expression for angle[0] from
    // GetInfo1 then take the derivative. to prove this for angle[2] it is
    // easier to take the euler rate expression for d(angle[2])/dt with respect
    // to the components of w and set that to 0.
    v3 axis0 = new v3();
    calculatedTransformB.basis.getColumn(0, axis0);
    v3 axis2 = new v3();
    calculatedTransformA.basis.getColumn(2, axis2);
    calculatedAxis[1].cross(axis2, axis0);
    calculatedAxis[0].cross(calculatedAxis[1], axis2);
    calculatedAxis[2].cross(axis0, calculatedAxis[1]);
// if(m_debugDrawer)
// {
// 
// char buff[300];
// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
// m_calculatedAxisAngleDiff[0],
// m_calculatedAxisAngleDiff[1],
// m_calculatedAxisAngleDiff[2]);
// m_debugDrawer->reportErrorWarning(buff);
// }
}

Example 9 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class TriangleMeshShape method getAabb.

@Override
public void getAabb(Transform trans, v3 aabbMin, v3 aabbMax) {
    v3 tmp = new v3();
    v3 localHalfExtents = new v3();
    localHalfExtents.sub(localAabbMax, localAabbMin);
    localHalfExtents.scale(0.5f);
    v3 localCenter = new v3();
    localCenter.add(localAabbMax, localAabbMin);
    localCenter.scale(0.5f);
    Matrix3f abs_b = new Matrix3f(trans.basis);
    MatrixUtil.absolute(abs_b);
    v3 center = new v3(localCenter);
    trans.transform(center);
    v3 extent = new v3();
    abs_b.getRow(0, tmp);
    extent.x = tmp.dot(localHalfExtents);
    abs_b.getRow(1, tmp);
    extent.y = tmp.dot(localHalfExtents);
    abs_b.getRow(2, tmp);
    extent.z = tmp.dot(localHalfExtents);
    float m = getMargin();
    extent.add(m, m, m);
    aabbMin.sub(center, extent);
    aabbMax.add(center, extent);
}

Example 10 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class SequentialImpulseConstrainer method solveGroupCacheFriendlySetup.

public float solveGroupCacheFriendlySetup(Collection<Collidable> bodies, int numBodies, FasterList<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, FasterList<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal) /*,btStackAlloc* stackAlloc*/
{
    if ((numConstraints + numManifolds) == 0) {
        // printf("empty\n");
        return 0f;
    }
    PersistentManifold manifold = null;
    Collidable colObj0 = null, colObj1 = null;
    // btRigidBody* rb0=0,*rb1=0;
    // //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
    // 
    // BEGIN_PROFILE("refreshManifolds");
    // 
    // int i;
    // 
    // 
    // 
    // for (i=0;i<numManifolds;i++)
    // {
    // manifold = manifoldPtr[i];
    // rb1 = (btRigidBody*)manifold->getBody1();
    // rb0 = (btRigidBody*)manifold->getBody0();
    // 
    // manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
    // 
    // }
    // 
    // END_PROFILE("refreshManifolds");
    // //#endif //FORCE_REFESH_CONTACT_MANIFOLDS
    // int sizeofSB = sizeof(btSolverBody);
    // int sizeofSC = sizeof(btSolverConstraint);
    // if (1)
    // if m_stackAlloc, try to pack bodies/constraints to speed up solving
    // btBlock*					sablock;
    // sablock = stackAlloc->beginBlock();
    // int memsize = 16;
    // unsigned char* stackMemory = stackAlloc->allocate(memsize);
    // todo: use stack allocator for this temp memory
    // int minReservation = numManifolds * 2;
    // m_tmpSolverBodyPool.reserve(minReservation);
    // don't convert all bodies, only the one we need so solver the constraints
    /*
				{
				for (int i=0;i<numBodies;i++)
				{
				btRigidBody* rb = btRigidBody::upcast(bodies[i]);
				if (rb && 	(rb->getIslandTag() >= 0))
				{
				btAssert(rb->getCompanionId() < 0);
				int solverBodyId = m_tmpSolverBodyPool.size();
				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
				initSolverBody(&solverBody,rb);
				rb->setCompanionId(solverBodyId);
				}
				}
				}
				*/
    // m_tmpSolverConstraintPool.reserve(minReservation);
    // m_tmpSolverFrictionConstraintPool.reserve(minReservation);
    {
        int i;
        v3 rel_pos1 = new v3();
        v3 rel_pos2 = new v3();
        v3 pos1 = new v3();
        v3 pos2 = new v3();
        v3 vel = new v3();
        v3 torqueAxis0 = new v3();
        v3 torqueAxis1 = new v3();
        v3 vel1 = new v3();
        v3 vel2 = new v3();
        // Vector3f frictionDir1 = new Vector3f();
        // Vector3f frictionDir2 = new Vector3f();
        v3 vec = new v3();
        Matrix3f tmpMat = new Matrix3f();
        for (i = 0; i < numManifolds; i++) {
            // return array[index];
            manifold = manifoldPtr.get(manifold_offset + i);
            colObj0 = (Collidable) manifold.getBody0();
            colObj1 = (Collidable) manifold.getBody1();
            int solverBodyIdA = -1;
            int solverBodyIdB = -1;
            if (manifold.numContacts() != 0) {
                if (colObj0.tag() >= 0) {
                    if (colObj0.getCompanionId() >= 0) {
                        // body has already been converted
                        solverBodyIdA = colObj0.getCompanionId();
                    } else {
                        solverBodyIdA = tmpSolverBodyPool.size();
                        SolverBody solverBody = new SolverBody();
                        tmpSolverBodyPool.add(solverBody);
                        initSolverBody(solverBody, colObj0);
                        colObj0.setCompanionId(solverBodyIdA);
                    }
                } else {
                    // create a static body
                    solverBodyIdA = tmpSolverBodyPool.size();
                    SolverBody solverBody = new SolverBody();
                    tmpSolverBodyPool.add(solverBody);
                    initSolverBody(solverBody, colObj0);
                }
                if (colObj1.tag() >= 0) {
                    if (colObj1.getCompanionId() >= 0) {
                        solverBodyIdB = colObj1.getCompanionId();
                    } else {
                        solverBodyIdB = tmpSolverBodyPool.size();
                        SolverBody solverBody = new SolverBody();
                        tmpSolverBodyPool.add(solverBody);
                        initSolverBody(solverBody, colObj1);
                        colObj1.setCompanionId(solverBodyIdB);
                    }
                } else {
                    // create a static body
                    solverBodyIdB = tmpSolverBodyPool.size();
                    SolverBody solverBody = new SolverBody();
                    tmpSolverBodyPool.add(solverBody);
                    initSolverBody(solverBody, colObj1);
                }
            }
            float relaxation;
            for (int j = 0; j < manifold.numContacts(); j++) {
                ManifoldPoint cp = manifold.getContactPoint(j);
                if (cp.distance1 <= 0f) {
                    cp.getPositionWorldOnA(pos1);
                    cp.getPositionWorldOnB(pos2);
                    rel_pos1.sub(pos1, colObj0.transform);
                    rel_pos2.sub(pos2, colObj1.transform);
                    relaxation = 1f;
                    float rel_vel;
                    int frictionIndex = tmpSolverConstraintPool.size();
                    SolverConstraint solverConstraint = new SolverConstraint();
                    tmpSolverConstraintPool.add(solverConstraint);
                    Body3D rb0 = Body3D.ifDynamic(colObj0);
                    Body3D rb1 = Body3D.ifDynamic(colObj1);
                    solverConstraint.solverBodyIdA = solverBodyIdA;
                    solverConstraint.solverBodyIdB = solverBodyIdB;
                    solverConstraint.constraintType = SolverConstraint.SolverConstraintType.SOLVER_CONTACT_1D;
                    solverConstraint.originalContactPoint = cp;
                    torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
                    if (rb0 != null) {
                        solverConstraint.angularComponentA.set(torqueAxis0);
                        rb0.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentA);
                    } else {
                        solverConstraint.angularComponentA.set(0f, 0f, 0f);
                    }
                    torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
                    if (rb1 != null) {
                        solverConstraint.angularComponentB.set(torqueAxis1);
                        rb1.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentB);
                    } else {
                        solverConstraint.angularComponentB.set(0f, 0f, 0f);
                    }
                    // #ifdef COMPUTE_IMPULSE_DENOM
                    // btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
                    // btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
                    // #else
                    float denom0 = 0f;
                    float denom1 = 0f;
                    if (rb0 != null) {
                        vec.cross(solverConstraint.angularComponentA, rel_pos1);
                        denom0 = rb0.getInvMass() + cp.normalWorldOnB.dot(vec);
                    }
                    if (rb1 != null) {
                        vec.cross(solverConstraint.angularComponentB, rel_pos2);
                        denom1 = rb1.getInvMass() + cp.normalWorldOnB.dot(vec);
                    }
                    // #endif //COMPUTE_IMPULSE_DENOM
                    float denom = relaxation / (denom0 + denom1);
                    solverConstraint.jacDiagABInv = denom;
                    solverConstraint.contactNormal.set(cp.normalWorldOnB);
                    solverConstraint.relpos1CrossNormal.cross(rel_pos1, cp.normalWorldOnB);
                    solverConstraint.relpos2CrossNormal.cross(rel_pos2, cp.normalWorldOnB);
                    if (rb0 != null) {
                        rb0.getVelocityInLocalPoint(rel_pos1, vel1);
                    } else {
                        vel1.zero();
                    }
                    if (rb1 != null) {
                        rb1.getVelocityInLocalPoint(rel_pos2, vel2);
                    } else {
                        vel2.zero();
                    }
                    vel.sub(vel1, vel2);
                    rel_vel = cp.normalWorldOnB.dot(vel);
                    solverConstraint.penetration = Math.min(cp.distance1 + infoGlobal.linearSlop, 0f);
                    // solverConstraint.m_penetration = cp.getDistance();
                    solverConstraint.friction = cp.combinedFriction;
                    solverConstraint.restitution = restitutionCurve(rel_vel, cp.combinedRestitution);
                    if (solverConstraint.restitution <= 0f) {
                        solverConstraint.restitution = 0f;
                    }
                    float penVel = -solverConstraint.penetration / infoGlobal.timeStep;
                    if (solverConstraint.restitution > penVel) {
                        solverConstraint.penetration = 0f;
                    }
                    v3 tmp = new v3();
                    // warm starting (or zero if disabled)
                    if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                        solverConstraint.appliedImpulse = cp.appliedImpulse * infoGlobal.warmstartingFactor;
                        if (rb0 != null) {
                            tmp.scale(rb0.getInvMass(), solverConstraint.contactNormal);
                            // return array[index];
                            tmpSolverBodyPool.get(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, solverConstraint.angularComponentA, solverConstraint.appliedImpulse);
                        }
                        if (rb1 != null) {
                            tmp.scale(rb1.getInvMass(), solverConstraint.contactNormal);
                            // return array[index];
                            tmpSolverBodyPool.get(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, solverConstraint.angularComponentB, -solverConstraint.appliedImpulse);
                        }
                    } else {
                        solverConstraint.appliedImpulse = 0f;
                    }
                    solverConstraint.appliedPushImpulse = 0f;
                    solverConstraint.frictionIndex = tmpSolverFrictionConstraintPool.size();
                    if (!cp.lateralFrictionInitialized) {
                        cp.lateralFrictionDir1.scale(rel_vel, cp.normalWorldOnB);
                        cp.lateralFrictionDir1.sub(vel, cp.lateralFrictionDir1);
                        float lat_rel_vel = cp.lateralFrictionDir1.lengthSquared();
                        if (// 0.0f)
                        lat_rel_vel > BulletGlobals.FLT_EPSILON) {
                            cp.lateralFrictionDir1.scale(1f / (float) Math.sqrt(lat_rel_vel));
                            addFrictionConstraint(cp.lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
                            cp.lateralFrictionDir2.cross(cp.lateralFrictionDir1, cp.normalWorldOnB);
                            // ??
                            cp.lateralFrictionDir2.normalize();
                            addFrictionConstraint(cp.lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
                        } else {
                            // re-calculate friction direction every frame, todo: check if this is really needed
                            TransformUtil.planeSpace1(cp.normalWorldOnB, cp.lateralFrictionDir1, cp.lateralFrictionDir2);
                            addFrictionConstraint(cp.lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
                            addFrictionConstraint(cp.lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
                        }
                        cp.lateralFrictionInitialized = true;
                    } else {
                        addFrictionConstraint(cp.lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
                        addFrictionConstraint(cp.lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
                    }
                    // return array[index];
                    SolverConstraint frictionConstraint1 = tmpSolverFrictionConstraintPool.get(solverConstraint.frictionIndex);
                    if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                        frictionConstraint1.appliedImpulse = cp.appliedImpulseLateral1 * infoGlobal.warmstartingFactor;
                        if (rb0 != null) {
                            tmp.scale(rb0.getInvMass(), frictionConstraint1.contactNormal);
                            // return array[index];
                            tmpSolverBodyPool.get(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint1.angularComponentA, frictionConstraint1.appliedImpulse);
                        }
                        if (rb1 != null) {
                            tmp.scale(rb1.getInvMass(), frictionConstraint1.contactNormal);
                            // return array[index];
                            tmpSolverBodyPool.get(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint1.angularComponentB, -frictionConstraint1.appliedImpulse);
                        }
                    } else {
                        frictionConstraint1.appliedImpulse = 0f;
                    }
                    // return array[index];
                    SolverConstraint frictionConstraint2 = tmpSolverFrictionConstraintPool.get(solverConstraint.frictionIndex + 1);
                    if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                        frictionConstraint2.appliedImpulse = cp.appliedImpulseLateral2 * infoGlobal.warmstartingFactor;
                        if (rb0 != null) {
                            tmp.scale(rb0.getInvMass(), frictionConstraint2.contactNormal);
                            // return array[index];
                            tmpSolverBodyPool.get(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint2.angularComponentA, frictionConstraint2.appliedImpulse);
                        }
                        if (rb1 != null) {
                            tmp.scale(rb1.getInvMass(), frictionConstraint2.contactNormal);
                            // return array[index];
                            tmpSolverBodyPool.get(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint2.angularComponentB, -frictionConstraint2.appliedImpulse);
                        }
                    } else {
                        frictionConstraint2.appliedImpulse = 0f;
                    }
                }
            }
        }
    }
    // TODO: btContactSolverInfo info = infoGlobal;
    int j;
    for (j = 0; j < numConstraints; j++) {
        constraints.get(constraints_offset + j).buildJacobian();
    }
    // int j;
    // for (j = 0; j < numConstraints; j++) {
    // constraints.get(constraints_offset + j).getInfo2(infoGlobal);
    // }
    int numConstraintPool = tmpSolverConstraintPool.size();
    int numFrictionPool = tmpSolverFrictionConstraintPool.size();
    // todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    MiscUtil.resize(orderTmpConstraintPool, numConstraintPool, 0);
    MiscUtil.resize(orderFrictionConstraintPool, numFrictionPool, 0);
    int i;
    for (i = 0; i < numConstraintPool; i++) {
        orderTmpConstraintPool.setBoth(i);
    }
    for (i = 0; i < numFrictionPool; i++) {
        orderFrictionConstraintPool.setBoth(i);
    }
    return 0f;
}