Examples with SolverConstraint spacegraph.space3d.phys.constraint.SolverConstraint used on opensource projects

Example 1 with SolverConstraint

use of spacegraph.space3d.phys.constraint.SolverConstraint 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;
}

Example 2 with SolverConstraint

use of spacegraph.space3d.phys.constraint.SolverConstraint in project narchy by automenta.

the class SequentialImpulseConstrainer method solveGroupCacheFriendlyIterations.

float solveGroupCacheFriendlyIterations(Collection<Collidable> bodies, int numBodies, Collection<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, FasterList<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal) /*,btStackAlloc* stackAlloc*/
{
    int numConstraintPool = tmpSolverConstraintPool.size();
    int numFrictionPool = tmpSolverFrictionConstraintPool.size();
    // should traverse the contacts random order...
    int iteration;
    IntArrayList constraintPool = this.orderTmpConstraintPool;
    IntArrayList frictionPool = this.orderFrictionConstraintPool;
    for (iteration = 0; iteration < infoGlobal.numIterations; iteration++) {
        if ((infoGlobal.solverMode & SolverMode.SOLVER_RANDMIZE_ORDER) != 0) {
            if ((iteration & 7) == 0) {
                for (int j = 0; j < numConstraintPool; ++j) {
                    orderPool(j, constraintPool);
                }
                for (int j = 0; j < numFrictionPool; ++j) {
                    orderPool(j, frictionPool);
                }
            }
        }
        for (int j = 0; j < numConstraints; j++) {
            // return array[index];
            final TypedConstraint constraint = constraints.get(constraints_offset + j);
            // todo: use solver bodies, so we don't need to copy from/to btRigidBody
            final Body3D ca = constraint.getRigidBodyA();
            final int cai = ca.getCompanionId();
            if ((ca.tag() >= 0) && (cai >= 0)) {
                // return array[index];
                tmpSolverBodyPool.get(cai).writebackVelocity();
            }
            Body3D cb = constraint.getRigidBodyB();
            int cbi = cb.getCompanionId();
            if ((cb.tag() >= 0) && (cbi >= 0)) {
                // return array[index];
                tmpSolverBodyPool.get(cbi).writebackVelocity();
            }
            constraint.solveConstraint(infoGlobal.timeStep);
            if ((ca.tag() >= 0) && (cai >= 0)) {
                // return array[index];
                tmpSolverBodyPool.get(cai).readVelocity();
            }
            if ((cb.tag() >= 0) && (cbi >= 0)) {
                // return array[index];
                tmpSolverBodyPool.get(cbi).readVelocity();
            }
        }
        int numPoolConstraints = tmpSolverConstraintPool.size();
        for (int j = 0; j < numPoolConstraints; j++) {
            // return array[index];
            SolverConstraint solveManifold = tmpSolverConstraintPool.get(constraintPool.get(j));
            // return array[index];
            // return array[index];
            resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA), tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal);
        }
        int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
        for (int j = 0; j < numFrictionPoolConstraints; j++) {
            // return array[index];
            SolverConstraint solveManifold = tmpSolverFrictionConstraintPool.get(frictionPool.get(j));
            // return array[index];
            // return array[index];
            float totalImpulse = tmpSolverConstraintPool.get(solveManifold.frictionIndex).appliedImpulse + tmpSolverConstraintPool.get(solveManifold.frictionIndex).appliedPushImpulse;
            // return array[index];
            // return array[index];
            resolveSingleFrictionCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA), tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal, totalImpulse);
        }
    }
    if (infoGlobal.splitImpulse) {
        for (iteration = 0; iteration < infoGlobal.numIterations; iteration++) {
            int numPoolConstraints = tmpSolverConstraintPool.size();
            for (int j = 0; j < numPoolConstraints; j++) {
                // return array[index];
                SolverConstraint solveManifold = tmpSolverConstraintPool.get(constraintPool.get(j));
                // return array[index];
                // return array[index];
                resolveSplitPenetrationImpulseCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA), tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal);
            }
        }
    }
    return 0f;
}

Example 3 with SolverConstraint

use of spacegraph.space3d.phys.constraint.SolverConstraint in project narchy by automenta.

the class SequentialImpulseConstrainer method addFrictionConstraint.

protected void addFrictionConstraint(v3 normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, v3 rel_pos1, v3 rel_pos2, Collidable colObj0, Collidable colObj1, float relaxation) {
    Body3D body0 = Body3D.ifDynamic(colObj0);
    Body3D body1 = Body3D.ifDynamic(colObj1);
    SolverConstraint solverConstraint = new SolverConstraint();
    tmpSolverFrictionConstraintPool.add(solverConstraint);
    solverConstraint.contactNormal.set(normalAxis);
    solverConstraint.solverBodyIdA = solverBodyIdA;
    solverConstraint.solverBodyIdB = solverBodyIdB;
    solverConstraint.constraintType = SolverConstraint.SolverConstraintType.SOLVER_FRICTION_1D;
    solverConstraint.frictionIndex = frictionIndex;
    solverConstraint.friction = cp.combinedFriction;
    solverConstraint.originalContactPoint = null;
    solverConstraint.appliedImpulse = 0f;
    solverConstraint.appliedPushImpulse = 0f;
    solverConstraint.penetration = 0f;
    v3 ftorqueAxis1 = new v3();
    Matrix3f tmpMat = new Matrix3f();
    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
    v3 vec = new v3();
    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;
}

Example 4 with SolverConstraint

use of spacegraph.space3d.phys.constraint.SolverConstraint in project narchy by automenta.

the class SequentialImpulseConstrainer method solveGroupCacheFriendly.

float solveGroupCacheFriendly(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*/
{
    solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal);
    solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal);
    int numPoolConstraints = tmpSolverConstraintPool.size();
    for (int j = 0; j < numPoolConstraints; j++) {
        // return array[index];
        SolverConstraint solveManifold = tmpSolverConstraintPool.get(j);
        ManifoldPoint pt = (ManifoldPoint) solveManifold.originalContactPoint;
        assert (pt != null);
        pt.appliedImpulse = solveManifold.appliedImpulse;
        // return array[index];
        pt.appliedImpulseLateral1 = tmpSolverFrictionConstraintPool.get(solveManifold.frictionIndex).appliedImpulse;
        // return array[index];
        pt.appliedImpulseLateral1 = tmpSolverFrictionConstraintPool.get(solveManifold.frictionIndex + 1).appliedImpulse;
    // do a callback here?
    }
    if (infoGlobal.splitImpulse) {
        for (int i = 0; i < tmpSolverBodyPool.size(); i++) {
            // return array[index];
            tmpSolverBodyPool.get(i).writebackVelocity(infoGlobal.timeStep);
        }
    } else {
        for (int i = 0; i < tmpSolverBodyPool.size(); i++) {
            // return array[index];
            tmpSolverBodyPool.get(i).writebackVelocity();
        }
    }
    // printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
    /*
		printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
		printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
		printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
		printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
		printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
		printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
		*/
    tmpSolverBodyPool.clearFast();
    tmpSolverConstraintPool.clearFast();
    tmpSolverFrictionConstraintPool.clearFast();
    return 0f;
}