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