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;
}
}