public DynamicsWorld(IDispatcher dispatcher,IBroadphaseInterface broadphase,ICollisionConfiguration collisionConfiguration) :base(dispatcher,broadphase,collisionConfiguration) { m_internalTickCallback = null; m_worldUserInfo = null; m_solverInfo = new ContactSolverInfo(); }
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 ObjectArray<PersistentManifold>(); 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) { ObjectArray<PersistentManifold> manifoldPtr = ((CollisionDispatcher)m_dispatcher1).GetInternalManifoldPointer(); ContactSolverInfo infoGlobal = new ContactSolverInfo(); infoGlobal.m_timeStep = timeStep; m_constraintSolver.PrepareSolve(0, numManifolds); m_constraintSolver.SolveGroup(null, 0, manifoldPtr, numManifolds, null, 0, infoGlobal, m_debugDrawer, m_dispatcher1); m_constraintSolver.AllSolved(infoGlobal, m_debugDrawer); } ///integrate transforms IntegrateTransforms(timeStep); UpdateAabbs(); SynchronizeMotionStates(); ClearForces(); return 1; }
protected virtual void SolveConstraints(ContactSolverInfo solverInfo) { //sorted version of all btTypedConstraint, based on islandId ObjectArray<TypedConstraint> sortedConstraints = new ObjectArray<TypedConstraint>(GetNumConstraints()); if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("solveConstraints"); } for (int i = 0; i < GetNumConstraints(); i++) { sortedConstraints.Add(m_constraints[i]); } // btAssert(0); //sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); sortedConstraints.Sort(new SortConstraintOnIslandPredicate()); ObjectArray<TypedConstraint> constraintsPtr = GetNumConstraints() > 0 ? sortedConstraints : null; InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, m_constraintSolver, constraintsPtr, sortedConstraints.Count, m_debugDrawer, m_dispatcher1); if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("prepareSolve"); } m_constraintSolver.PrepareSolve(GetCollisionWorld().GetNumCollisionObjects(), GetCollisionWorld().GetDispatcher().GetNumManifolds()); if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("buildAndProcessIsland"); } /// solve all the constraints for this island m_islandManager.BuildAndProcessIslands(GetCollisionWorld().GetDispatcher(), GetCollisionWorld(), solverCallback); solverCallback.ProcessConstraints(); m_constraintSolver.AllSolved(solverInfo, m_debugDrawer); }
protected float SolveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; //should traverse the contacts random order... if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_RANDMIZE_ORDER)) { if ((iteration & 7) == 0) { 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(); ///solve all joint constraints /// int poolSize = m_tmpSolverNonContactConstraintPool.Count; for (int j = 0; j < poolSize; j++) { SolverConstraint constraint = rawTmpSolverNonContactConstraintPool[j]; ResolveSingleConstraintRowGeneric(constraint.m_solverBodyA, constraint.m_solverBodyB, ref constraint); rawTmpSolverNonContactConstraintPool[j] = constraint; } ///solve all contact constraints /// SolverConstraint[] rawTmpSolverContactConstraintPool = m_tmpSolverContactConstraintPool.GetRawArray(); int[] rawOrderTmpConstraintPool = m_orderTmpConstraintPool.GetRawArray(); 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); rawTmpSolverContactConstraintPool[rawOrderTmpConstraintPool[j]] = 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 = rawTmpSolverContactConstraintPool[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); } rawTmpSolverContactFrictionConstraintPool[rawOrderFrictionConstraintPool[j]] = solveManifold; } return 0f; }
//protected virtual float solveGroupCacheFriendlyIterations() protected float SolveGroupCacheFriendlyIterations(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifoldPtr, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { //BT_PROFILE("solveGroupCacheFriendlyIterations"); //should traverse the contacts random order... { for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { SolveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); } SolveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); } return 0.0f; }
//protected float solveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) //protected float solveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) //{ // return 0f; //} protected virtual float SolveGroupCacheFriendlySetup(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { //BT_PROFILE("solveGroupCacheFriendlySetup"); m_counter++; if ((numConstraints + numManifolds) == 0) { // printf("empty\n"); return 0f; } //if (true) //{ // for (int j=0;j<numConstraints;j++) // { // TypedConstraint constraint = constraints[j]; // constraint.buildJacobian(); // } //} //btRigidBody* rb0=0,*rb1=0; //if (1) { { int totalNumRows = 0; //calculate the total number of contraint rows m_tmpConstraintSizesPool.Clear(); for (int i = 0; i < numConstraints; i++) { ConstraintInfo1 info1 = new ConstraintInfo1(); m_tmpConstraintSizesPool.Add(info1); constraints[i].GetInfo1(info1); totalNumRows += info1.m_numConstraintRows; } ResizeSolverConstraintList(m_tmpSolverNonContactConstraintPool, 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); //SolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow]; TypedConstraint constraint = constraints[i]; RigidBody rbA = constraint.GetRigidBodyA(); RigidBody rbB = constraint.GetRigidBodyB(); for (int j = currentRow; j < (currentRow + info1a.m_numConstraintRows); j++) { SolverConstraint solverConstraint = new SolverConstraint(); solverConstraint.m_lowerLimit = float.MinValue; solverConstraint.m_upperLimit = float.MaxValue; solverConstraint.m_appliedImpulse = 0f; solverConstraint.m_appliedPushImpulse = 0f; solverConstraint.m_solverBodyA = rbA; solverConstraint.m_solverBodyB = rbB; m_tmpSolverNonContactConstraintPool[j] = solverConstraint; } Vector3 zero = Vector3.Zero; rbA.InternalSetDeltaLinearVelocity(ref zero); rbA.InternalSetDeltaAngularVelocity(ref zero); rbB.InternalSetDeltaLinearVelocity(ref zero); rbB.InternalSetDeltaAngularVelocity(ref zero); ConstraintInfo2 info2 = new ConstraintInfo2(); info2.m_solverConstraints = new SolverConstraint[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; constraints[i].GetInfo2(info2); ///finalize the constraint setup /// for (int j = 0; j < info1a.m_numConstraintRows; ++j) { m_tmpSolverNonContactConstraintPool[currentRow + j] = info2.m_solverConstraints[j]; } //FIXME - log the output of the solverconstraints for comparison. for (int j = 0; j < (info1a.m_numConstraintRows); j++) { SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j]; solverConstraint.m_originalContactPoint = constraint; { Vector3 ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; solverConstraint.m_angularComponentA = Vector3.TransformNormal(ftorqueAxis1, constraint.GetRigidBodyA().GetInvInertiaTensorWorld()) * constraint.GetRigidBodyA().GetAngularFactor(); } { Vector3 ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; solverConstraint.m_angularComponentB = Vector3.TransformNormal(ftorqueAxis2, constraint.GetRigidBodyB().GetInvInertiaTensorWorld()) * constraint.GetRigidBodyB().GetAngularFactor(); } { Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.GetInvMass(); Vector3 iMJaA = Vector3.TransformNormal(solverConstraint.m_relpos1CrossNormal, rbA.GetInvInertiaTensorWorld()); Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.GetInvMass();//sign of normal? Vector3 iMJaB = Vector3.TransformNormal(solverConstraint.m_relpos2CrossNormal, rbB.GetInvInertiaTensorWorld()); float sum = Vector3.Dot(iMJlA, solverConstraint.m_contactNormal); float a = Vector3.Dot(iMJaA, solverConstraint.m_relpos1CrossNormal); float b = Vector3.Dot(iMJlB, solverConstraint.m_contactNormal); float c = Vector3.Dot(iMJaB, 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 = Vector3.Dot(solverConstraint.m_contactNormal, rbA.GetLinearVelocity()) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, rbA.GetAngularVelocity()); float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, rbB.GetLinearVelocity()) + Vector3.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;// * damping; float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0f; } if (BulletGlobals.g_streamWriter != null && debugSolver) { TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, j); } m_tmpSolverNonContactConstraintPool[currentRow + j] = solverConstraint; } } currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; } } { PersistentManifold manifold2 = null; CollisionObject colObj0 = null, colObj1 = null; for (int i = 0; i < numManifolds; i++) { manifold2 = manifold[i]; ConvertContact(manifold2, infoGlobal); } } } ContactSolverInfo info = infoGlobal; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints //m_orderTmpConstraintPool.Capacity = numConstraintPool; //m_orderFrictionConstraintPool.Capacity = numFrictionPool; m_orderTmpConstraintPool.Clear(); m_orderFrictionConstraintPool.Clear(); { for (int i = 0; i < numConstraintPool; i++) { m_orderTmpConstraintPool.Add(i); } for (int i = 0; i < numFrictionPool; i++) { m_orderFrictionConstraintPool.Add(i); } } return 0f; }
protected virtual float SolveGroupCacheFriendlyFinish(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; for (int j = 0; j < numPoolConstraints; j++) { SolverConstraint solveManifold = m_tmpSolverContactConstraintPool[j]; ManifoldPoint pt = solveManifold.m_originalContactPoint as ManifoldPoint; pt.SetAppliedImpulse(solveManifold.m_appliedImpulse); if ((infoGlobal.m_solverMode & SolverMode.SOLVER_USE_FRICTION_WARMSTARTING) != 0) { pt.SetAppliedImpulseLateral1(m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse); pt.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; float sum = constr.InternalGetAppliedImpulse(); sum += solverConstr.m_appliedImpulse; constr.InternalSetAppliedImpulse(sum); 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.Clear(); m_tmpSolverNonContactConstraintPool.Clear(); m_tmpSolverContactFrictionConstraintPool.Clear(); return 0f; }
protected virtual void SolveGroupCacheFriendlySplitImpulseIterations(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, 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 void AllSolved(ContactSolverInfo info, IDebugDraw debugDrawer) { }
public virtual float SolveGroup(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifoldPtr, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { //BT_PROFILE("solveGroup"); //you need to provide at least some bodies Debug.Assert(bodies.Count > 0); SolveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer, dispatcher); SolveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer, dispatcher); SolveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); 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 = RigidBody.Upcast(colObj0); RigidBody solverBodyB = 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 (cp.GetDistance() <= manifold.GetContactProcessingThreshold()) { // Vector3 pos1 = cp.getPositionWorldOnA(); // Vector3 pos2 = cp.getPositionWorldOnB(); Vector3 rel_pos1 = Vector3.Zero; Vector3 rel_pos2 = Vector3.Zero; //; float relaxation = 1f; float rel_vel = 0f; Vector3 vel = Vector3.Zero; int frictionIndex = m_tmpSolverContactConstraintPool.Count; SolverConstraint solverConstraint = new SolverConstraint(); //m_tmpSolverContactConstraintPool.Add(solverConstraint); RigidBody rb0 = RigidBody.Upcast(colObj0); RigidBody rb1 = RigidBody.Upcast(colObj1); if (BulletGlobals.g_streamWriter != null && rb0 != null && debugSolver) { MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp); } solverConstraint.m_solverBodyA = rb0 != null ? rb0 : GetFixedBody(); solverConstraint.m_solverBodyB = rb1 != null ? rb1 : GetFixedBody(); solverConstraint.m_originalContactPoint = cp; SetupContactConstraint(ref solverConstraint, colObj0, colObj1, cp, infoGlobal, ref vel, ref rel_vel, ref relaxation, ref rel_pos1, ref rel_pos2); if (BulletGlobals.g_streamWriter != null && debugSolver) { TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, 99); } /////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)System.Math.Sqrt(lat_rel_vel); if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = Vector3.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 TransformUtil.PlaneSpace1(ref cp.m_normalWorldOnB, ref cp.m_lateralFrictionDir1, ref 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); m_tmpSolverContactConstraintPool.Add(solverConstraint); } } }
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); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse); } } 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); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse); } } 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 void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp, ContactSolverInfo infoGlobal, ref Vector3 vel, ref float rel_vel, ref float relaxation, ref Vector3 rel_pos1, ref Vector3 rel_pos2) { RigidBody rb0 = RigidBody.Upcast(colObj0); RigidBody rb1 = RigidBody.Upcast(colObj1); Vector3 pos1 = cp.GetPositionWorldOnA(); Vector3 pos2 = cp.GetPositionWorldOnB(); rel_pos1 = pos1 - colObj0.GetWorldTransform().Translation; rel_pos2 = pos2 - colObj1.GetWorldTransform().Translation; relaxation = 1f; Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 != null ? Vector3.TransformNormal(torqueAxis0, rb0.GetInvInertiaTensorWorld()) * rb0.GetAngularFactor() : Vector3.Zero; Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.GetNormalWorldOnB()); solverConstraint.m_angularComponentB = rb1 != null ? Vector3.TransformNormal(-torqueAxis1, rb1.GetInvInertiaTensorWorld()) * rb1.GetAngularFactor() : Vector3.Zero; { #if COMPUTE_IMPULSE_DENOM float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB); float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else Vector3 vec; float denom0 = 0f; float denom1 = 0f; if (rb0 != null) { vec = Vector3.Cross((solverConstraint.m_angularComponentA), rel_pos1); denom0 = rb0.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), vec); } if (rb1 != null) { vec = Vector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2); denom1 = rb1.GetInvMass() + Vector3.Dot(cp.GetNormalWorldOnB(), 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 = Vector3.Cross(rel_pos1, cp.m_normalWorldOnB); solverConstraint.m_relpos2CrossNormal = Vector3.Cross(rel_pos2, -cp.m_normalWorldOnB); Vector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : Vector3.Zero; Vector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : Vector3.Zero; vel = vel1 - vel2; rel_vel = Vector3.Dot(cp.GetNormalWorldOnB(), 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) { Vector3 contactNormalTemp = solverConstraint.m_contactNormal; Vector3.Multiply(ref contactNormalTemp, rb0.GetInvMass(), out contactNormalTemp); rb0.InternalApplyImpulse(ref contactNormalTemp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); } if (rb1 != null) { Vector3 contactNormalTemp = solverConstraint.m_contactNormal; Vector3.Multiply(ref contactNormalTemp, rb1.GetInvMass(), out contactNormalTemp); Vector3 negAngular = -solverConstraint.m_angularComponentB; rb1.InternalApplyImpulse(ref contactNormalTemp, ref negAngular, -solverConstraint.m_appliedImpulse); } } else { solverConstraint.m_appliedImpulse = 0f; } solverConstraint.m_appliedPushImpulse = 0f; { float rel_vel2 = 0f; float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : Vector3.Zero)) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : Vector3.Zero)); float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : Vector3.Zero)) + Vector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : Vector3.Zero)); rel_vel2 = vel1Dotn + vel2Dotn; float positionalError = 0f; positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep; float velocityError = restitution - rel_vel2;// * damping; 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; } }