public DynamicsWorld(IDispatcher dispatcher, IBroadphaseInterface broadphase, ICollisionConfiguration collisionConfiguration) : base(dispatcher, broadphase, collisionConfiguration) { m_internalTickCallback = null; m_worldUserInfo = null; m_solverInfo = new ContactSolverInfo(); }
public DynamicsWorld(IDispatcher dispatcher,IBroadphaseInterface broadphase,ICollisionConfiguration collisionConfiguration) :base(dispatcher,broadphase,collisionConfiguration) { m_internalTickCallback = null; m_worldUserInfo = null; m_solverInfo = new ContactSolverInfo(); }
//InplaceSolverIslandCallback operator=(InplaceSolverIslandCallback& other) //{ // Debug.Assert(false); // //(void)other; // return *this; //} public void Setup(ContactSolverInfo solverInfo, ObjectArray <TypedConstraint> sortedConstraints, int numConstraints, IDebugDraw debugDrawer) { Debug.Assert(solverInfo != null); m_solverInfo = solverInfo; m_sortedConstraints = sortedConstraints; m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; m_bodies.Resize(0); m_manifolds.Resize(0); m_constraints.Resize(0); }
protected virtual void SolveConstraints(ContactSolverInfo solverInfo) { //sorted version of all btTypedConstraint, based on islandId m_sortedConstraints.Resize(m_constraints.Count); int numConstraints = GetNumConstraints(); for (int i = 0; i < numConstraints; i++) { m_sortedConstraints[i] = m_constraints[i]; } if (numConstraints > 1) { //sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); // If this sort is removed then the constraint gets twitchy... //m_sortedConstraints.Sort(m_islandSortPredicate); m_sortedConstraints.QuickSort(m_islandQuickSortPredicate); } #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("solveConstraints"); } #endif // btAssert(0); if (m_solverIslandCallback == null) { m_solverIslandCallback = new InplaceSolverIslandCallback(solverInfo, m_constraintSolver, m_sortedConstraints, GetNumConstraints(), m_debugDrawer, m_dispatcher1); } else { m_solverIslandCallback.Setup(solverInfo, m_sortedConstraints, numConstraints, m_debugDrawer); } #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("prepareSolve"); } #endif m_constraintSolver.PrepareSolve(GetCollisionWorld().GetNumCollisionObjects(), GetCollisionWorld().GetDispatcher().GetNumManifolds()); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("buildAndProcessIslands"); } #endif /// solve all the constraints for this island m_islandManager.BuildAndProcessIslands(GetCollisionWorld().GetDispatcher(), GetCollisionWorld(), m_solverIslandCallback); m_solverIslandCallback.ProcessConstraints(); m_constraintSolver.AllSolved(solverInfo, m_debugDrawer); }
//response between two dynamic objects without friction, assuming 0 penetration depth public static float ResolveSingleCollision( RigidBody body1, CollisionObject colObj2, ref IndexedVector3 contactPositionWorld, ref IndexedVector3 contactNormalOnB, ContactSolverInfo solverInfo, float distance) { RigidBody body2 = RigidBody.Upcast(colObj2); IndexedVector3 normal = contactNormalOnB; IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin; IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin; IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero; IndexedVector3 vel = vel1 - vel2; float rel_vel = normal.Dot(ref vel); float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution(); float restitution = combinedRestitution * -rel_vel; float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep; float velocityError = -(1.0f + restitution) * rel_vel; // * damping; float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal); float denom1 = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f; float relaxation = 1.0f; float jacDiagABInv = relaxation / (denom0 + denom1); float penetrationImpulse = positionalError * jacDiagABInv; float velocityImpulse = velocityError * jacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse; body1.ApplyImpulse(normal * (normalImpulse), rel_pos1); if (body2 != null) { body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2); } return(normalImpulse); }
//response between two dynamic objects without friction, assuming 0 penetration depth public static float ResolveSingleCollision( RigidBody body1, CollisionObject colObj2, ref IndexedVector3 contactPositionWorld, ref IndexedVector3 contactNormalOnB, ContactSolverInfo solverInfo, float distance) { RigidBody body2 = RigidBody.Upcast(colObj2); IndexedVector3 normal = contactNormalOnB; IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin; IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin; IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2 != null ? body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero; IndexedVector3 vel = vel1 - vel2; float rel_vel = normal.Dot(ref vel); float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution(); float restitution = combinedRestitution * -rel_vel; float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep; float velocityError = -(1.0f + restitution) * rel_vel;// * damping; float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal); float denom1 = body2 != null ? body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f; float relaxation = 1.0f; float jacDiagABInv = relaxation / (denom0 + denom1); float penetrationImpulse = positionalError * jacDiagABInv; float velocityImpulse = velocityError * jacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse; body1.ApplyImpulse(normal * (normalImpulse), rel_pos1); if (body2 != null) { body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2); } return normalImpulse; }
public InplaceSolverIslandCallback( ContactSolverInfo solverInfo, IConstraintSolver solver, ObjectArray <TypedConstraint> sortedConstraints, int numConstraints, IDebugDraw debugDrawer, IDispatcher dispatcher) { m_solverInfo = solverInfo; m_solver = solver; m_sortedConstraints = sortedConstraints; m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; m_dispatcher = dispatcher; m_bodies = new ObjectArray <CollisionObject>(); m_manifolds = new PersistentManifoldArray(); m_constraints = new ObjectArray <TypedConstraint>(); }
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) { ///apply gravity, predict motion PredictUnconstraintMotion(timeStep); DispatcherInfo dispatchInfo = GetDispatchInfo(); dispatchInfo.SetTimeStep(timeStep); dispatchInfo.SetStepCount(0); dispatchInfo.SetDebugDraw(GetDebugDrawer()); ///perform collision detection PerformDiscreteCollisionDetection(); ///solve contact constraints int numManifolds = m_dispatcher1.GetNumManifolds(); if (numManifolds != 0) { PersistentManifoldArray manifoldPtr = (m_dispatcher1 as CollisionDispatcher).GetInternalManifoldPointer(); ContactSolverInfo infoGlobal = new ContactSolverInfo(); infoGlobal.m_timeStep = timeStep; m_constraintSolver.PrepareSolve(0, numManifolds); m_constraintSolver.SolveGroup(null, 0, manifoldPtr, 0, numManifolds, null, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1); m_constraintSolver.AllSolved(infoGlobal, m_debugDrawer); } ///integrate transforms IntegrateTransforms(timeStep); UpdateAabbs(); SynchronizeMotionStates(); ClearForces(); return(1); }
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) { ///apply gravity, predict motion PredictUnconstraintMotion(timeStep); DispatcherInfo dispatchInfo = GetDispatchInfo(); dispatchInfo.SetTimeStep(timeStep); dispatchInfo.SetStepCount(0); dispatchInfo.SetDebugDraw(GetDebugDrawer()); ///perform collision detection PerformDiscreteCollisionDetection(); ///solve contact constraints int numManifolds = m_dispatcher1.GetNumManifolds(); if (numManifolds != 0) { PersistentManifoldArray manifoldPtr = (m_dispatcher1 as CollisionDispatcher).GetInternalManifoldPointer(); ContactSolverInfo infoGlobal = new ContactSolverInfo(); infoGlobal.m_timeStep = timeStep; m_constraintSolver.PrepareSolve(0, numManifolds); m_constraintSolver.SolveGroup(null, 0, manifoldPtr, 0, numManifolds, null, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1); m_constraintSolver.AllSolved(infoGlobal, m_debugDrawer); } ///integrate transforms IntegrateTransforms(timeStep); UpdateAabbs(); SynchronizeMotionStates(); ClearForces(); return 1; }
protected void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp, ContactSolverInfo infoGlobal, ref IndexedVector3 vel, ref float rel_vel, ref float relaxation, out IndexedVector3 rel_pos1, out IndexedVector3 rel_pos2) { RigidBody rb0 = colObj0 as RigidBody;// RigidBody.Upcast(colObj0); RigidBody rb1 = colObj1 as RigidBody;//RigidBody.Upcast(colObj1); IndexedVector3 pos1 = cp.GetPositionWorldOnA(); IndexedVector3 pos2 = cp.GetPositionWorldOnB(); rel_pos1 = pos1 - colObj0.m_worldTransform._origin; rel_pos2 = pos2 - colObj1.m_worldTransform._origin; relaxation = 1f; // cross IndexedVector3 torqueAxis0 = new IndexedVector3(rel_pos1.Y *cp.m_normalWorldOnB.Z - rel_pos1.Z * cp.m_normalWorldOnB.Y, rel_pos1.Z *cp.m_normalWorldOnB.X - rel_pos1.X * cp.m_normalWorldOnB.Z, rel_pos1.X *cp.m_normalWorldOnB.Y - rel_pos1.Y * cp.m_normalWorldOnB.X); IndexedVector3 torqueAxis1 = new IndexedVector3(rel_pos2.Y *cp.m_normalWorldOnB.Z - rel_pos2.Z * cp.m_normalWorldOnB.Y, rel_pos2.Z *cp.m_normalWorldOnB.X - rel_pos2.X * cp.m_normalWorldOnB.Z, rel_pos2.X *cp.m_normalWorldOnB.Y - rel_pos2.Y * cp.m_normalWorldOnB.X); solverConstraint.m_angularComponentA = rb0 != null ? rb0.GetInvInertiaTensorWorld() * torqueAxis0 * rb0.GetAngularFactor() : IndexedVector3.Zero; //IndexedVector3 torqueAxis1 = IndexedVector3.Cross(ref rel_pos2, ref cp.m_normalWorldOnB); solverConstraint.m_angularComponentB = rb1 != null ? rb1.GetInvInertiaTensorWorld() * -torqueAxis1 * rb1.GetAngularFactor() : IndexedVector3.Zero; //IndexedVector3 torqueAxis0 = IndexedVector3.Cross(ref rel_pos1, ref cp.m_normalWorldOnB); //solverConstraint.m_angularComponentA = rb0 != null ? rb0.GetInvInertiaTensorWorld() * torqueAxis0 * rb0.GetAngularFactor() : IndexedVector3.Zero; //IndexedVector3 torqueAxis1 = IndexedVector3.Cross(ref rel_pos2, ref cp.m_normalWorldOnB); //solverConstraint.m_angularComponentB = rb1 != null ? rb1.GetInvInertiaTensorWorld() * -torqueAxis1 * rb1.GetAngularFactor() : IndexedVector3.Zero; { #if COMPUTE_IMPULSE_DENOM float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB); float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else IndexedVector3 vec; float denom0 = 0f; float denom1 = 0f; if (rb0 != null) { vec = IndexedVector3.Cross(ref solverConstraint.m_angularComponentA, ref rel_pos1); denom0 = rb0.GetInvMass() + IndexedVector3.Dot(cp.m_normalWorldOnB, vec); } if (rb1 != null) { vec = IndexedVector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2); denom1 = rb1.GetInvMass() + IndexedVector3.Dot(cp.m_normalWorldOnB, vec); } #endif //COMPUTE_IMPULSE_DENOM float denom = relaxation / (denom0 + denom1); MathUtil.SanityCheckFloat(denom); solverConstraint.m_jacDiagABInv = denom; } solverConstraint.m_contactNormal = cp.m_normalWorldOnB; solverConstraint.m_relpos1CrossNormal = IndexedVector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_relpos2CrossNormal = IndexedVector3.Cross(rel_pos2, -cp.m_normalWorldOnB); IndexedVector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : IndexedVector3.Zero; IndexedVector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero; vel = vel1 - vel2; rel_vel = IndexedVector3.Dot(cp.m_normalWorldOnB, vel); float penetration = cp.GetDistance() + infoGlobal.m_linearSlop; solverConstraint.m_friction = cp.GetCombinedFriction(); float restitution = 0f; if (cp.GetLifeTime() > infoGlobal.m_restingContactRestitutionThreshold) { restitution = 0f; } else { restitution = RestitutionCurve(rel_vel, cp.GetCombinedResitution()); if (restitution <= 0f) { restitution = 0f; } } ///warm starting (or zero if disabled) if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { solverConstraint.m_appliedImpulse = cp.GetAppliedImpulse() * infoGlobal.m_warmstartingFactor; if (rb0 != null) { IndexedVector3 contactNormalTemp = solverConstraint.m_contactNormal; rb0.InternalApplyImpulse(solverConstraint.m_contactNormal * rb0.GetInvMass() * rb0.GetLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse, "SetupContactConstraint-rb0"); } if (rb1 != null) { rb1.InternalApplyImpulse(solverConstraint.m_contactNormal * rb1.GetInvMass() * rb1.GetLinearFactor(), -solverConstraint.m_angularComponentB, -solverConstraint.m_appliedImpulse,"SetupContactConstraint-rb1"); } } else { solverConstraint.m_appliedImpulse = 0f; } solverConstraint.m_appliedPushImpulse = 0f; { float rel_vel2 = 0f; float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : IndexedVector3.Zero)) + IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : IndexedVector3.Zero)); float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : IndexedVector3.Zero)) + IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : IndexedVector3.Zero)); rel_vel2 = vel1Dotn + vel2Dotn; float positionalError = 0f; if (rel_vel2 > 20) { int ibreak = 0; } float velocityError = restitution - rel_vel2;// * damping; if (penetration > 0f) { positionalError = 0f; velocityError -= penetration / infoGlobal.m_timeStep; } else { positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep; } float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0f; } else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } solverConstraint.m_cfm = 0f; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } }
protected virtual float SolveGroupCacheFriendlyFinish(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { m_finishCount++; int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyFinish start [{0}].",numPoolConstraints); } #endif for (int j = 0; j < numPoolConstraints; j++) { SolverConstraint solveManifold = m_tmpSolverContactConstraintPool[j]; ((ManifoldPoint)solveManifold.m_originalContactPoint).SetAppliedImpulse(solveManifold.m_appliedImpulse); if ((infoGlobal.m_solverMode & SolverMode.SOLVER_USE_FRICTION_WARMSTARTING) != 0) { ((ManifoldPoint)solveManifold.m_originalContactPoint).SetAppliedImpulseLateral1(m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse); ((ManifoldPoint)solveManifold.m_originalContactPoint).SetAppliedImpulseLateral2(m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse); } //do a callback here? } numPoolConstraints = m_tmpSolverNonContactConstraintPool.Count; for (int j = 0; j < numPoolConstraints; j++) { SolverConstraint solverConstr = m_tmpSolverNonContactConstraintPool[j]; TypedConstraint constr = solverConstr.m_originalContactPoint as TypedConstraint; constr.InternalSetAppliedImpulse(solverConstr.m_appliedImpulse); if (Math.Abs(solverConstr.m_appliedImpulse) >= constr.GetBreakingImpulseThreshold()) { constr.SetEnabled(false); } m_tmpSolverNonContactConstraintPool[j] = solverConstr; } if (infoGlobal.m_splitImpulse) { for (int i = 0; i < numBodies; i++) { RigidBody rb = RigidBody.Upcast(bodies[i]); if (rb != null) { rb.InternalWritebackVelocity(infoGlobal.m_timeStep); } } } else { for (int i = 0; i < numBodies; i++) { RigidBody rb = RigidBody.Upcast(bodies[i]); if (rb != null) { rb.InternalWritebackVelocity(); } } } m_tmpSolverContactConstraintPool.Resize(0); m_tmpSolverNonContactConstraintPool.Resize(0); m_tmpSolverContactFrictionConstraintPool.Resize(0); m_tmpConstraintInfo2Pool.Resize(0); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyFinish stop."); } #endif return 0f; }
protected virtual float SolveGroupCacheFriendlySetup(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { m_setupCount++; BulletGlobals.StartProfile("solveGroupCacheFriendlySetup"); m_maxOverrideNumSolverIterations = 0; m_counter++; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlySetup start [{0}].", m_counter); } #endif if ((numConstraints + numManifolds) == 0) { // printf("empty\n"); BulletGlobals.StopProfile(); return 0f; } IndexedVector3 zero = IndexedVector3.Zero; if (infoGlobal.m_splitImpulse) { for (int i = 0; i < numBodies; i++) { //RigidBody body = RigidBody.Upcast(bodies[i]); RigidBody body = bodies[i] as RigidBody; if (body != null) { body.InternalSetDeltaLinearVelocity(ref zero); body.InternalSetDeltaAngularVelocity(ref zero); body.InternalSetPushVelocity(ref zero); body.InternalSetTurnVelocity(ref zero); } } } else { for (int i = 0; i < numBodies; i++) { RigidBody body = bodies[i] as RigidBody; if (body != null) { body.InternalSetDeltaLinearVelocity(ref zero); body.InternalSetDeltaAngularVelocity(ref zero); } } } if (true) { int j; int lastConstraint = startConstraint + numConstraints; for (j = startConstraint; j < lastConstraint; j++) { TypedConstraint constraint = constraints[j]; //constraint.BuildJacobian(); constraint.InternalSetAppliedImpulse(0.0f); } } //if (1) { { int totalNumRows = 0; //calculate the total number of contraint rows m_tmpConstraintSizesPool.Resize(numConstraints); for (int i = 0; i < numConstraints; i++) { ConstraintInfo1 info1 = m_tmpConstraintSizesPool[i]; if (constraints[startConstraint + i].IsEnabled()) { constraints[startConstraint + i].GetInfo1(info1); } else { info1.m_numConstraintRows = 0; info1.nub = 0; } totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.Resize(totalNumRows); ///setup the btSolverConstraints int currentRow = 0; for (int i = 0; i < numConstraints; i++) { ConstraintInfo1 info1a = m_tmpConstraintSizesPool[i]; if (info1a.m_numConstraintRows != 0) { Debug.Assert(currentRow < totalNumRows); TypedConstraint constraint = constraints[startConstraint + i]; RigidBody rbA = constraint.GetRigidBodyA(); RigidBody rbB = constraint.GetRigidBodyB(); int overrideNumSolverIterations = constraint.GetOverrideNumSolverIterations() > 0 ? constraint.GetOverrideNumSolverIterations() : infoGlobal.m_numIterations; { if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) { m_maxOverrideNumSolverIterations = overrideNumSolverIterations; } } for (int j = 0; j < info1a.m_numConstraintRows; j++) { int index = currentRow + j; SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[index]; solverConstraint.Reset(); solverConstraint.m_lowerLimit = -MathUtil.SIMD_INFINITY; solverConstraint.m_upperLimit = MathUtil.SIMD_INFINITY; solverConstraint.m_appliedImpulse = 0f; solverConstraint.m_appliedPushImpulse = 0f; solverConstraint.m_solverBodyA = rbA; solverConstraint.m_solverBodyB = rbB; solverConstraint.m_overrideNumSolverIterations = overrideNumSolverIterations; } rbA.InternalSetDeltaLinearVelocity(ref zero); rbA.InternalSetDeltaAngularVelocity(ref zero); rbB.InternalSetDeltaLinearVelocity(ref zero); rbB.InternalSetDeltaAngularVelocity(ref zero); ConstraintInfo2 info2 = m_tmpConstraintInfo2Pool[m_tmpConstraintInfo2Pool.Count]; Debug.Assert(info1a.m_numConstraintRows <= ConstraintInfo2.maxConstraints); info2.m_numRows = info1a.m_numConstraintRows; // MAN - copy the data into the info block for passing to the constraints for (int j = 0; j < info1a.m_numConstraintRows; ++j) { info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j]; } info2.fps = 1f / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; info2.m_numIterations = infoGlobal.m_numIterations; info2.m_damping = infoGlobal.m_damping; constraint.GetInfo2(info2); //FIXME - log the output of the solverconstraints for comparison. for (int j = 0; j < (info1a.m_numConstraintRows); j++) { SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j]; if (solverConstraint.m_upperLimit >= constraint.GetBreakingImpulseThreshold()) { solverConstraint.m_upperLimit = constraint.GetBreakingImpulseThreshold(); } if (solverConstraint.m_lowerLimit <= -constraint.GetBreakingImpulseThreshold()) { solverConstraint.m_lowerLimit = -constraint.GetBreakingImpulseThreshold(); } solverConstraint.m_originalContactPoint = constraint; { IndexedVector3 ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; solverConstraint.m_angularComponentA = constraint.GetRigidBodyA().GetInvInertiaTensorWorld() * ftorqueAxis1 * constraint.GetRigidBodyA().GetAngularFactor(); } { IndexedVector3 ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; solverConstraint.m_angularComponentB = constraint.GetRigidBodyB().GetInvInertiaTensorWorld() * ftorqueAxis2 * constraint.GetRigidBodyB().GetAngularFactor(); } { IndexedVector3 iMJlA = solverConstraint.m_contactNormal * rbA.GetInvMass(); IndexedVector3 iMJaA = rbA.GetInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal; IndexedVector3 iMJlB = solverConstraint.m_contactNormal * rbB.GetInvMass();//sign of normal? IndexedVector3 iMJaB = rbB.GetInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal; float sum = IndexedVector3.Dot(ref iMJlA, ref solverConstraint.m_contactNormal); float a = IndexedVector3.Dot(ref iMJaA, ref solverConstraint.m_relpos1CrossNormal); float b = IndexedVector3.Dot(ref iMJlB, ref solverConstraint.m_contactNormal); float c = IndexedVector3.Dot(ref iMJaB, ref solverConstraint.m_relpos2CrossNormal); sum += a; sum += b; sum += c; solverConstraint.m_jacDiagABInv = 1f / sum; MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv); } ///fix rhs ///todo: add force/torque accelerators { float rel_vel; float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, rbA.GetLinearVelocity()) + IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, rbA.GetAngularVelocity()); float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, rbB.GetLinearVelocity()) + IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, rbB.GetAngularVelocity()); rel_vel = vel1Dotn + vel2Dotn; float restitution = 0f; float positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 float velocityError = restitution - rel_vel * info2.m_damping; float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0f; } #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, j); } #endif //m_tmpSolverNonContactConstraintPool[currentRow + j] = solverConstraint; } //if(BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) //{ // TypedConstraint.PrintInfo2(BulletGlobals.g_streamWriter,constraint,info2); //} } currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; } } { int lastManifold = startManifold + numManifolds; for (int i = startManifold; i < lastManifold; i++) { ConvertContact(manifold[i], infoGlobal); } } } ContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.EnsureCapacity(numNonContactPool);/// m_orderTmpConstraintPool.EnsureCapacity(numConstraintPool); m_orderFrictionConstraintPool.EnsureCapacity(numFrictionPool); { for (int i = 0; i < numNonContactPool; i++) { m_orderNonContactConstraintPool[i] = i; } for (int i = 0; i < numConstraintPool; i++) { m_orderTmpConstraintPool[i] = i; } for (int i = 0; i < numFrictionPool; i++) { m_orderFrictionConstraintPool[i] = i; } } BulletGlobals.StopProfile(); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlySetup stop."); } #endif return 0f; }
public virtual void AllSolved(ContactSolverInfo info, IDebugDraw debugDrawer) { }
protected virtual void SolveGroupCacheFriendlySplitImpulseIterations(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { if (infoGlobal.m_splitImpulse) { for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; for (int j = 0; j < numPoolConstraints; j++) { SolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; ResolveSplitPenetrationImpulseCacheFriendly(solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, ref solveManifold); m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]] = solveManifold; } } } } }
public virtual float SolveGroup(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifoldPtr, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { BulletGlobals.StartProfile("solveGroup"); //you need to provide at least some bodies Debug.Assert(bodies.Count > 0); SolveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, startManifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer, dispatcher); SolveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, startManifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer, dispatcher); SolveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, startManifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer); BulletGlobals.StopProfile(); return 0.0f; }
protected void ConvertContact(PersistentManifold manifold, ContactSolverInfo infoGlobal) { CollisionObject colObj0 = null, colObj1 = null; colObj0 = manifold.GetBody0() as CollisionObject; colObj1 = manifold.GetBody1() as CollisionObject; RigidBody solverBodyA = colObj0 as RigidBody;//RigidBody.Upcast(colObj0); RigidBody solverBodyB = colObj1 as RigidBody;//RigidBody.Upcast(colObj1); ///avoid collision response between two static objects if ((solverBodyA == null || solverBodyA.GetInvMass() == 0f) && (solverBodyB == null || solverBodyB.GetInvMass() == 0f)) { return; } for (int j = 0; j < manifold.GetNumContacts(); j++) { ManifoldPoint cp = manifold.GetContactPoint(j); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false) { String nameA = solverBodyA != null ? (String)solverBodyA.GetUserPointer() : "Null"; String nameB = solverBodyA != null ? (String)solverBodyB.GetUserPointer() : "Null"; BulletGlobals.g_streamWriter.WriteLine("ConvertContact [{0}][{1}][{2}][{3}][{4}]", j, nameA,nameB, cp.GetDistance() ,manifold.GetContactProcessingThreshold()); MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp); } #endif if (cp.GetDistance() <= manifold.GetContactProcessingThreshold()) { // IndexedVector3 pos1 = cp.getPositionWorldOnA(); // IndexedVector3 pos2 = cp.getPositionWorldOnB(); IndexedVector3 rel_pos1; IndexedVector3 rel_pos2; //; float relaxation = 1f; float rel_vel = 0f; IndexedVector3 vel = IndexedVector3.Zero; int frictionIndex = m_tmpSolverContactConstraintPool.Count; // will create if needed. SolverConstraint solverConstraint = m_tmpSolverContactConstraintPool[m_tmpSolverContactConstraintPool.Count]; solverConstraint.Reset(); RigidBody rb0 = solverBodyA;//RigidBody.Upcast(colObj0); RigidBody rb1 = solverBodyB;//RigidBody.Upcast(colObj1); solverConstraint.m_solverBodyA = rb0 != null ? rb0 : GetFixedBody(); solverConstraint.m_solverBodyB = rb1 != null ? rb1 : GetFixedBody(); solverConstraint.m_originalContactPoint = cp; #if DEBUG if (BulletGlobals.g_streamWriter != null && rb0 != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("ConvertContact [{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer()); MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp); } #endif SetupContactConstraint(ref solverConstraint, colObj0, colObj1, cp, infoGlobal, ref vel, ref rel_vel, ref relaxation, out rel_pos1, out rel_pos2); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, 99); } #endif /////setup the friction constraints solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.Count; if (!(TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING)) || !cp.GetLateralFrictionInitialized()) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; float lat_rel_vel = cp.m_lateralFrictionDir1.LengthSquared(); if (!TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > MathUtil.SIMD_EPSILON) { cp.m_lateralFrictionDir1 /= (float)Math.Sqrt(lat_rel_vel); if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = IndexedVector3.Cross(cp.m_lateralFrictionDir1, cp.m_normalWorldOnB); cp.m_lateralFrictionDir2.Normalize();//?? ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2); ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2); AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f); } ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1); ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1); AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f); cp.m_lateralFrictionInitialized = true; } else { //re-calculate friction direction every frame, todo: check if this is really needed IndexedVector3 temp = cp.m_normalWorldOnB; TransformUtil.PlaneSpace1(ref temp, out cp.m_lateralFrictionDir1, out cp.m_lateralFrictionDir2); if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2); ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2); AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f); } ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1); ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1); AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f); cp.m_lateralFrictionInitialized = true; } } else { AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1); if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); } } SetFrictionConstraintImpulse(ref solverConstraint, rb0, rb1, cp, infoGlobal); } } }
protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1, ManifoldPoint cp, ContactSolverInfo infoGlobal) { if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING)) { { SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0 != null) { rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse,"SetupFriction-rb0"); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse, "SetupFriction-rb1"); } } else { frictionConstraint1.m_appliedImpulse = 0f; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1; } if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0 != null) { rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse,"SetFriction-rb0"); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse,"SetFriction-rb1"); } } else { frictionConstraint2.m_appliedImpulse = 0f; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2; } } else { SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; frictionConstraint1.m_appliedImpulse = 0f; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; frictionConstraint2.m_appliedImpulse = 0f; m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1; } }
//protected virtual float solveGroupCacheFriendlyIterations() protected float SolveGroupCacheFriendlyIterations(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifoldPtr, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { m_iterCount++; BulletGlobals.StartProfile("solveGroupCacheFriendlyIterations"); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyIterations start."); } #endif { ///this is a special step to resolve penetrations (just for contacts) SolveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr,startManifold,numManifolds,constraints,startConstraint,numConstraints,infoGlobal,debugDrawer); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) for (int iteration = 0; iteration < maxIterations; iteration++) { SolveSingleIteration(iteration, bodies, numBodies, manifoldPtr,startManifold,numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer); } } #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyIterations stop."); } #endif BulletGlobals.StopProfile(); return 0.0f; }
protected float SolveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver) { BulletGlobals.g_streamWriter.WriteLine( "solveSingleIter [{0}][{1}][{2}].", numNonContactPool, numConstraintPool, numFrictionPool); } #endif //should traverse the contacts random order... if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_RANDMIZE_ORDER)) { if ((iteration & 7) == 0) { for (int j = 0; j < numNonContactPool; ++j) { int tmp = m_orderNonContactConstraintPool[j]; int swapi = RandInt2(j + 1); m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; m_orderNonContactConstraintPool[swapi] = tmp; } //contact/friction constraints are not solved more than if (iteration < infoGlobal.m_numIterations) { for (int j = 0; j < numConstraintPool; ++j) { int tmp = m_orderTmpConstraintPool[j]; int swapi = RandInt2(j + 1); m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; m_orderTmpConstraintPool[swapi] = tmp; } for (int j = 0; j < numFrictionPool; ++j) { int tmp = m_orderFrictionConstraintPool[j]; int swapi = RandInt2(j + 1); m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; m_orderFrictionConstraintPool[swapi] = tmp; } } } } SolverConstraint[] rawTmpSolverNonContactConstraintPool = m_tmpSolverNonContactConstraintPool.GetRawArray(); int[] rawOrderNonContactConstraintPool = m_orderNonContactConstraintPool.GetRawArray(); ///solve all joint constraints for (int j=0;j<m_tmpSolverNonContactConstraintPool.Count;j++) { SolverConstraint constraint = rawTmpSolverNonContactConstraintPool[rawOrderNonContactConstraintPool[j]]; if (iteration < constraint.m_overrideNumSolverIterations) { ResolveSingleConstraintRowGeneric(constraint.m_solverBodyA,constraint.m_solverBodyB,ref constraint); } } if (iteration< infoGlobal.m_numIterations) { SolverConstraint[] rawTmpSolverContactConstraintPool = m_tmpSolverContactConstraintPool.GetRawArray(); int[] rawOrderTmpConstraintPool = m_orderTmpConstraintPool.GetRawArray(); ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; for (int j=0;j<numPoolConstraints;j++) { SolverConstraint solveManifold = rawTmpSolverContactConstraintPool[rawOrderTmpConstraintPool[j]]; ResolveSingleConstraintRowLowerLimit(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,ref solveManifold); } ///solve all friction constraints int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count; SolverConstraint[] rawTmpSolverContactFrictionConstraintPool = m_tmpSolverContactFrictionConstraintPool.GetRawArray(); int[] rawOrderFrictionConstraintPool = m_orderFrictionConstraintPool.GetRawArray(); for (int j = 0; j < numFrictionPoolConstraints; j++) { SolverConstraint solveManifold = rawTmpSolverContactFrictionConstraintPool[rawOrderFrictionConstraintPool[j]]; float totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if (totalImpulse>0f) { solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; ResolveSingleConstraintRowGeneric(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,ref solveManifold); } } } return 0f; }