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