public override void AddContactPoint(ref Vector3 normalOnBInWorld,ref Vector3 pointInWorld,float depth)
{
bool isSwapped = m_manifoldPtr.GetBody0() != m_body0;
Vector3 pointA = pointInWorld + normalOnBInWorld * depth;
Vector3 localA = Vector3.Zero;
Vector3 localB = Vector3.Zero;;
if (isSwapped)
{
localA = MathUtil.InverseTransform(ref m_rootTransB,ref pointA );
localB = MathUtil.InverseTransform(ref m_rootTransA,ref pointInWorld);
}
else
{
localA = MathUtil.InverseTransform(ref m_rootTransA,ref pointA );
localB = MathUtil.InverseTransform(ref m_rootTransB,ref pointInWorld);
}
ManifoldPoint newPt = new ManifoldPoint(ref localA,ref localB,ref normalOnBInWorld,depth);
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
//BP mod, store contact triangles.
if (isSwapped)
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
}
else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//experimental feature info, for per-triangle material etc.
CollisionObject obj0 = isSwapped? m_body1 : m_body0;
CollisionObject obj1 = isSwapped? m_body0 : m_body1;
m_resultCallback.AddSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
}
public static void PrintContactPoint(StreamWriter streamWriter, ManifoldPoint mp)
{
if (streamWriter != null)
{
streamWriter.WriteLine("ContactPoint");
PrintVector3(streamWriter, "localPointA", mp.m_localPointA);
PrintVector3(streamWriter, "localPointB", mp.m_localPointB);
PrintVector3(streamWriter, "posWorldA", mp.m_positionWorldOnA);
PrintVector3(streamWriter, "posWorldB", mp.m_positionWorldOnB);
PrintVector3(streamWriter, "normalWorldB", mp.m_normalWorldOnB);
}
}
public abstract float AddSingleResult(ManifoldPoint cp, CollisionObject colObj0,int partId0,int index0,CollisionObject colObj1,int partId1,int index1);
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 virtual void AddContactPoint(ref Vector3 normalOnBInWorld,ref Vector3 pointInWorld,float depth)
{
Debug.Assert(m_manifoldPtr != null);
//order in manifold needs to match
if (depth > m_manifoldPtr.GetContactBreakingThreshold())
{
return;
}
bool isSwapped = m_manifoldPtr.GetBody0() != m_body0;
Vector3 pointA = pointInWorld + normalOnBInWorld * depth;
Vector3 localA = Vector3.Zero;
Vector3 localB = Vector3.Zero;
if (isSwapped)
{
localA = MathUtil.InverseTransform(ref m_rootTransB,ref pointA);
localB = MathUtil.InverseTransform(ref m_rootTransA,ref pointInWorld);
}
else
{
localA = MathUtil.InverseTransform(ref m_rootTransA,ref pointA);
localB = MathUtil.InverseTransform(ref m_rootTransB,ref pointInWorld);
}
ManifoldPoint newPt = new ManifoldPoint(ref localA,ref localB,ref normalOnBInWorld,depth);
newPt.SetPositionWorldOnA(ref pointA);
newPt.SetPositionWorldOnB(ref pointInWorld);
int insertIndex = m_manifoldPtr.GetCacheEntry(newPt);
newPt.SetCombinedFriction(CalculateCombinedFriction(m_body0,m_body1));
newPt.SetCombinedRestitution(CalculateCombinedRestitution(m_body0,m_body1));
//BP mod, store contact triangles.
if (isSwapped)
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
}
else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//printf("depth=%f\n",depth);
///@todo, check this for any side effects
if (insertIndex >= 0)
{
//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
m_manifoldPtr.ReplaceContactPoint(newPt,insertIndex);
} else
{
insertIndex = m_manifoldPtr.AddManifoldPoint(newPt);
}
//User can override friction and/or restitution
if (BulletGlobals.gContactAddedCallback != null &&
//and if either of the two bodies requires custom material
((m_body0.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0 ||
(m_body1.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK)!=0))
{
//experimental feature info, for per-triangle material etc.
CollisionObject obj0 = isSwapped? m_body1 : m_body0;
CollisionObject obj1 = isSwapped? m_body0 : m_body1;
//gContactAddedCallback.callback(m_manifoldPtr.getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
BulletGlobals.gContactAddedCallback.Callback(m_manifoldPtr.GetContactPoint(insertIndex), obj0, newPt.m_partId0, newPt.m_index0, obj1, newPt.m_partId1, newPt.m_index1);
}
}
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 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 void Copy(ManifoldPoint other)
{
m_localPointA = other.m_localPointA;
m_localPointB = other.m_localPointB;
m_positionWorldOnA = other.m_positionWorldOnA;
m_positionWorldOnB = other.m_positionWorldOnB;
m_normalWorldOnB = other.m_normalWorldOnB;
m_distance1 = other.m_distance1;
m_combinedFriction = other.m_combinedFriction;
m_partId0 = other.m_partId0;
m_partId1 = other.m_partId1;
m_index0 = other.m_index0;
m_index1 = other.m_index1;
m_userPersistentData = other.m_userPersistentData;
m_appliedImpulse = other.m_appliedImpulse;
m_lateralFrictionInitialized = other.m_lateralFrictionInitialized;
m_appliedImpulseLateral1 = other.m_appliedImpulseLateral1;
m_appliedImpulseLateral2 = other.m_appliedImpulseLateral2;
m_lifeTime = other.m_lifeTime;
m_lateralFrictionDir1 = other.m_lateralFrictionDir1;
m_lateralFrictionDir2 = other.m_lateralFrictionDir2;
}