public static int GetIslandId(PersistentManifold lhs)
{
CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
int islandId = rcolObj0.GetIslandTag();
if (islandId >= 0)
{
return(islandId);
}
CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;
return(rcolObj1.GetIslandTag());
}
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);
}
}
}
public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld)
{
BulletGlobals.StartProfile("islandUnionFindAndQuickSort");
ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
m_islandmanifold.Clear();
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
GetUnionFind().sortIslands();
int numElem = GetUnionFind().GetNumElements();
int endIslandIndex = 1;
//update the sleeping state for bodies, if all are sleeping
for (int startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
if ((colObj0.GetIslandTag() != islandId) && (colObj0.GetIslandTag() != -1))
{
// printf("error in island management\n");
}
Debug.Assert((colObj0.GetIslandTag() == islandId) || (colObj0.GetIslandTag() == -1));
if (colObj0.GetIslandTag() == islandId)
{
if (colObj0.GetActivationState() == ActivationState.ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0.GetActivationState() == ActivationState.DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
int islandTag = colObj0.GetIslandTag();
if (islandTag != islandId && islandTag != -1)
{
// printf("error in island management\n");
}
Debug.Assert((islandTag == islandId) || (islandTag == -1));
if (islandTag == islandId)
{
colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
}
else
{
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
int islandTag = colObj0.GetIslandTag();
if (islandTag != islandId && islandTag != -1)
{
// printf("error in island management\n");
}
Debug.Assert((islandTag == islandId) || (islandTag == -1));
if (islandTag == islandId)
{
if (colObj0.GetActivationState() == ActivationState.ISLAND_SLEEPING)
{
colObj0.SetActivationState(ActivationState.WANTS_DEACTIVATION);
colObj0.SetDeactivationTime(0f);
}
}
}
}
}
int maxNumManifolds = dispatcher.GetNumManifolds();
//#definef SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
for (int i = 0; i < maxNumManifolds; i++)
{
PersistentManifold manifold = dispatcher.GetManifoldByIndexInternal(i);
CollisionObject colObj0 = manifold.GetBody0() as CollisionObject;
CollisionObject colObj1 = manifold.GetBody1() as CollisionObject;
///@todo: check sleeping conditions!
if (((colObj0 != null) && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING) ||
((colObj1 != null) && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObj0.IsKinematicObject() && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (colObj0.HasContactResponse())
{
colObj1.Activate();
}
}
if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (colObj1.HasContactResponse())
{
colObj0.Activate();
}
}
if (m_splitIslands)
{
//filtering for response
if (dispatcher.NeedsResponse(colObj0, colObj1))
{
m_islandmanifold.Add(manifold);
}
}
}
}
BulletGlobals.StopProfile();
}
private static int getIslandId(PersistentManifold lhs)
{
CollisionObject rcolObj0 = lhs.GetBody0() as CollisionObject;
int islandId = rcolObj0.GetIslandTag();
if (islandId >= 0)
return rcolObj0.GetIslandTag();
CollisionObject rcolObj1 = lhs.GetBody1() as CollisionObject;
return rcolObj1.GetIslandTag();
}
