public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref Vector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
ManifoldPoint cp, ref Vector3 rel_pos1, ref Vector3 rel_pos2,
CollisionObject colObj0, CollisionObject colObj1, float relaxation,
float desiredVelocity, float cfmSlip)
{
RigidBody body0 = RigidBody.Upcast(colObj0);
RigidBody body1 = RigidBody.Upcast(colObj1);
solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_solverBodyA = body0 != null ? body0 : GetFixedBody();
solverConstraint.m_solverBodyB = body1 != null ? body1 : GetFixedBody();
solverConstraint.m_friction = cp.GetCombinedFriction();
//solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = 0f;
solverConstraint.m_appliedPushImpulse = 0f;
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = body0 != null ? Vector3.TransformNormal(ftorqueAxis1, body0.GetInvInertiaTensorWorld()) * body0.GetAngularFactor() : Vector3.Zero;
}
{
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 != null ? Vector3.TransformNormal(ftorqueAxis1, body1.GetInvInertiaTensorWorld()) * body1.GetAngularFactor() : Vector3.Zero;
}
#if COMPUTE_IMPULSE_DENOM
float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
Vector3 vec;
float denom0 = 0f;
float denom1 = 0f;
if (body0 != null)
{
vec = Vector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
denom0 = body0.GetInvMass() + Vector3.Dot(normalAxis, vec);
}
if (body1 != null)
{
vec = Vector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
denom1 = body1.GetInvMass() + Vector3.Dot(normalAxis, vec);
}
#endif //COMPUTE_IMPULSE_DENOM
float denom = relaxation / (denom0 + denom1);
solverConstraint.m_jacDiagABInv = denom;
MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
#if _USE_JACOBIAN
solverConstraint.m_jac = new JacobianEntry (
ref rel_pos1,ref rel_pos2,ref solverConstraint.m_contactNormal,
body0.getInvInertiaDiagLocal(),
body0.getInvMass(),
body1.getInvInertiaDiagLocal(),
body1.getInvMass());
#endif //_USE_JACOBIAN
{
float rel_vel;
float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : Vector3.Zero)
+ Vector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : Vector3.Zero);
float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : Vector3.Zero)
+ Vector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : Vector3.Zero);
rel_vel = vel1Dotn + vel2Dotn;
//float positionalError = 0f;
float velocityError = desiredVelocity - rel_vel;
float damper = 1f;
float velocityImpulse = (velocityError * solverConstraint.m_jacDiagABInv) * damper;
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref Vector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
ManifoldPoint cp, ref Vector3 rel_pos1, ref Vector3 rel_pos2,
CollisionObject colObj0, CollisionObject colObj1, float relaxation)
{
SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 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 void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
//check magniture of applied impulse from SolverConstraint
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
float deltaVel1Dotn = Vector3.Dot(c.m_contactNormal, body1.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetDeltaAngularVelocity());
float deltaVel2Dotn = -Vector3.Dot(c.m_contactNormal, body2.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
}
Vector3 temp = c.m_contactNormal * body1.InternalGetInvMass();
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse);
temp = -c.m_contactNormal * body2.InternalGetInvMass();
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse);
}
protected void ResolveSplitPenetrationImpulseCacheFriendly(
RigidBody body1,
RigidBody body2,
ref SolverConstraint c)
{
if (c.m_rhsPenetration != 0f)
{
gNumSplitImpulseRecoveries++;
float deltaImpulse = c.m_rhsPenetration - (c.m_appliedPushImpulse * c.m_cfm);
float deltaVel1Dotn = Vector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + Vector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
float deltaVel2Dotn = -Vector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + Vector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetTurnVelocity());
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedPushImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
c.m_appliedPushImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedPushImpulse = sum;
}
body1.InternalApplyPushImpulse(c.m_contactNormal * body1.InternalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.InternalApplyPushImpulse(-c.m_contactNormal * body2.InternalGetInvMass(), c.m_angularComponentB, deltaImpulse);
}
}
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;
}
}
public SolverConstraint AddFrictionConstraint(ref Vector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB, int frictionIndex, ManifoldPoint cp, ref Vector3 rel_pos1, ref Vector3 rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation, float desiredVelocity, float cfmSlip)
{
SolverConstraint solverConstraint = new SolverConstraint();
solverConstraint.m_frictionIndex = frictionIndex;
SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
m_tmpSolverContactFrictionConstraintPool.Add(solverConstraint);
return solverConstraint;
}
public static void PrintSolverConstraint(StreamWriter writer,SolverConstraint constraint,int index)
{
if(writer != null)
{
writer.WriteLine("solverConstraint[{0}]", index);
MathUtil.PrintVector3(writer,"relPos1CrossNormal",constraint.m_relpos1CrossNormal);
MathUtil.PrintVector3(writer, "contactNormal",constraint.m_contactNormal);
MathUtil.PrintVector3(writer, "m_angularComponentA",constraint.m_angularComponentA);
MathUtil.PrintVector3(writer, "m_angularComponentB",constraint.m_angularComponentB);
writer.WriteLine("Friction [{0:0.00000000}] jagDiag[{1:0.00000000}] rhs[{2:0.00000000}] cfm[{3:0.00000000}] lower[{4:0.00000000}] upper[{5:0.00000000}] rhsPen[{6:0.00000000}]", constraint.m_friction, constraint.m_jacDiagABInv,
constraint.m_rhs,constraint.m_cfm,constraint.m_lowerLimit,constraint.m_lowerLimit,constraint.m_rhsPenetration);
}
}