public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 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();
#if DEBUG
if (BulletGlobals.g_streamWriter != null && (body0 != null || body1 != null) && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SetupFrictionConstraint[{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
}
#endif
solverConstraint.m_originalContactPoint = null;
//solverConstraint.m_originalContactPointConstraint = null;
solverConstraint.m_appliedImpulse = 0f;
solverConstraint.m_appliedPushImpulse = 0f;
{
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = body0 != null ? body0.GetInvInertiaTensorWorld() * ftorqueAxis1 * body0.GetAngularFactor() : IndexedVector3.Zero;
}
{
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 != null ? body1.GetInvInertiaTensorWorld() * ftorqueAxis1 * body1.GetAngularFactor() : IndexedVector3.Zero;
}
#if COMPUTE_IMPULSE_DENOM
float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
IndexedVector3 vec;
float denom0 = 0f;
float denom1 = 0f;
if (body0 != null)
{
vec = IndexedVector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
denom0 = body0.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
}
if (body1 != null)
{
vec = IndexedVector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
denom1 = body1.GetInvMass() + IndexedVector3.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 = IndexedVector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : IndexedVector3.Zero)
+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : IndexedVector3.Zero);
float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : IndexedVector3.Zero)
+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : IndexedVector3.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;
}
}
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;
}
}
