protected bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
bool penetration = false;
collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher());
m_currentPosition = m_ghostObject.GetWorldTransform().Translation;
float maxPen = 0f;
for (int i = 0; i < m_ghostObject.GetOverlappingPairCache().GetNumOverlappingPairs(); i++)
{
m_manifoldArray.Clear();
BroadphasePair collisionPair = m_ghostObject.GetOverlappingPairCache().GetOverlappingPairArray()[i];
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
for (int j=0;j<m_manifoldArray.Count;j++)
{
PersistentManifold manifold = m_manifoldArray[j];
float directionSign = manifold.GetBody0() == m_ghostObject ? -1f : 1f;
for (int p=0;p<manifold.GetNumContacts();p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.GetDistance() < 0.0f)
{
if (pt.GetDistance() < maxPen)
{
maxPen = pt.GetDistance();
m_touchingNormal = pt.GetNormalWorldOnB() * directionSign;//??
}
m_currentPosition += pt.GetNormalWorldOnB() * directionSign * pt.GetDistance() * 0.2f;
penetration = true;
} else
{
//printf("touching %f\n", pt.getDistance());
}
}
//manifold->clearManifold();
}
}
Matrix newTrans = m_ghostObject.GetWorldTransform();
newTrans.Translation = m_currentPosition;
m_ghostObject.SetWorldTransform(ref newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return penetration;
}
public virtual void StoreIslandActivationState(CollisionWorld collisionWorld)
{
int index = 0;
ObjectArray<CollisionObject> list = collisionWorld.GetCollisionObjectArray();
foreach (CollisionObject collisionObject in list)
{
if (!collisionObject.IsStaticOrKinematicObject())
{
collisionObject.SetIslandTag(m_unionFind.Find(index));
collisionObject.SetCompanionId(-1);
}
else
{
collisionObject.SetIslandTag(-1);
collisionObject.SetCompanionId(-2);
}
index++;
}
}
public virtual void UpdateActivationState(CollisionWorld collisionWorld, IDispatcher dispatcher)
{
InitUnionFind(collisionWorld.GetCollisionObjectArray().Count);
// put the index into m_controllers into m_tag
{
int index = 0;
ObjectArray<CollisionObject> list = collisionWorld.GetCollisionObjectArray();
foreach(CollisionObject collisionObject in list)
{
collisionObject.SetIslandTag(index);
collisionObject.SetCompanionId(-1);
collisionObject.SetHitFraction(1f);
index++;
}
}
// do the union find
FindUnions(dispatcher, collisionWorld);
}
public SingleSweepCallback(ConvexShape castShape, ref Matrix convexFromTrans,ref Matrix convexToTrans,CollisionWorld world,ConvexResultCallback resultCallback,float allowedPenetration)
{
m_convexFromTrans = convexFromTrans;
m_convexToTrans = convexToTrans;
m_world = world;
m_resultCallback = resultCallback;
m_allowedCcdPenetration = allowedPenetration;
m_castShape = castShape;
Vector3 unnormalizedRayDir = (m_convexToTrans.Translation-m_convexFromTrans.Translation);
Vector3 rayDir = unnormalizedRayDir;
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/1e30
m_rayDirectionInverse.X = MathUtil.CompareFloat(rayDir.X ,0.0f) ? float.MaxValue : 1f / rayDir.X;
m_rayDirectionInverse.Y = MathUtil.CompareFloat(rayDir.Y ,0.0f) ? float.MaxValue : 1f / rayDir.Y;
m_rayDirectionInverse.Z = MathUtil.CompareFloat(rayDir.Z ,0.0f) ? float.MaxValue : 1f / rayDir.Z;
m_signs[0] = m_rayDirectionInverse.X < 0.0;
m_signs[1] = m_rayDirectionInverse.Y < 0.0;
m_signs[2] = m_rayDirectionInverse.Z < 0.0;
m_lambda_max = Vector3.Dot(rayDir,unnormalizedRayDir);
}
public SingleRayCallback(ref Vector3 rayFromWorld,ref Vector3 rayToWorld,CollisionWorld world,RayResultCallback resultCallback)
{
m_rayFromWorld = rayFromWorld;
m_rayToWorld = rayToWorld;
m_world = world;
m_resultCallback = resultCallback;
m_rayFromTrans = Matrix.Identity;
m_rayFromTrans.Translation = m_rayFromWorld;
m_rayToTrans = Matrix.Identity;
m_rayToTrans.Translation = m_rayToWorld;
Vector3 rayDir = (rayToWorld-rayFromWorld);
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/1e30
m_rayDirectionInverse.X = MathUtil.FuzzyZero(rayDir.X) ? float.MaxValue : 1f / rayDir.X;
m_rayDirectionInverse.Y = MathUtil.FuzzyZero(rayDir.Y) ? float.MaxValue : 1f / rayDir.Y;
m_rayDirectionInverse.Z = MathUtil.FuzzyZero(rayDir.Z) ? float.MaxValue : 1f / rayDir.Z;
m_signs[0] = m_rayDirectionInverse.X < 0.0f;
m_signs[1] = m_rayDirectionInverse.Y < 0.0f;
m_signs[2] = m_rayDirectionInverse.Z < 0.0f;
m_lambda_max = Vector3.Dot(rayDir,(m_rayToWorld-m_rayFromWorld));
}
public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld)
{
//BT_PROFILE("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 && 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];
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)
{
colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
}
else
{
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.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 = (CollisionObject)(manifold.GetBody0());
CollisionObject colObj1 = (CollisionObject)(manifold.GetBody1());
///@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)
{
colObj1.Activate();
}
if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
colObj0.Activate();
}
if (m_splitIslands)
{
//filtering for response
if (dispatcher.NeedsResponse(colObj0, colObj1))
{
m_islandmanifold.Add(manifold);
}
}
}
}
}
public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback)
{
ObjectArray<CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
BuildIslands(dispatcher,collisionWorld);
int endIslandIndex=1;
int startIslandIndex;
int numElem = GetUnionFind().GetNumElements();
//BT_PROFILE("processIslands");
if(!m_splitIslands)
{
ObjectArray<PersistentManifold> manifolds = dispatcher.GetInternalManifoldPointer();
int maxNumManifolds = dispatcher.GetNumManifolds();
callback.ProcessIsland(collisionObjects,collisionObjects.Count,manifolds,maxNumManifolds, -1);
}
else
{
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = m_islandmanifold.Count;
if (numManifolds != 0 && m_islandmanifold[0].GetNumContacts() > 0)
{
int ibreak = 0;
}
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
//m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//((ObjectArray<PersistentManifold>)m_islandmanifold).Sort();
m_islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//int islandId;
// printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
if (BulletGlobals.g_streamWriter != null && debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = GetUnionFind().GetElement(endIslandIndex).m_sz;
CollisionObject colObj0 = collisionObjects[i];
m_islandBodies.Add(colObj0);
if (!colObj0.IsActive())
{
islandSleeping = true;
//islandSleeping = false;
}
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
PersistentManifold startManifold = null;
if (startManifoldIndex<numManifolds)
{
int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
}
if (!islandSleeping)
{
// pass shortedned list to callback.
ObjectArray<PersistentManifold> subList = new ObjectArray<PersistentManifold>();
for(int i=0;i<numIslandManifolds;++i)
{
subList.Add(m_islandmanifold[startManifoldIndex+i]);
}
callback.ProcessIsland(m_islandBodies,m_islandBodies.Count,subList,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
m_islandBodies.Clear();
}
} // else if(!splitIslands)
}
public void FindUnions(IDispatcher dispatcher, CollisionWorld collisionWorld)
{
ObjectArray<BroadphasePair> broadphaseList = collisionWorld.GetPairCache().GetOverlappingPairArray();
foreach(BroadphasePair collisionPair in broadphaseList)
{
CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.m_clientObject;
CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.m_clientObject;
if (((colObj0 != null) && ((colObj0).MergesSimulationIslands())) &&
((colObj1 != null) && ((colObj1).MergesSimulationIslands())))
{
m_unionFind.Unite((colObj0).GetIslandTag(),
(colObj1).GetIslandTag());
}
}
}
public void PlayerStep(CollisionWorld collisionWorld, float dt)
{
// quick check...
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0)
{
// printf("\n");
return; // no motion
}
Matrix xform = m_ghostObject.GetWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
StepUp(collisionWorld);
if (m_useWalkDirection)
{
StepForwardAndStrafe(collisionWorld, ref m_walkDirection);
}
else
{
//printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
float dtMoving =
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
m_velocityTimeInterval -= dt;
// how far will we move while we are moving?
Vector3 move = m_walkDirection * dtMoving;
// printf(" dtMoving: %f", dtMoving);
// okay, step
StepForwardAndStrafe(collisionWorld, ref move);
}
StepDown(collisionWorld, dt);
xform.Translation = m_currentPosition;
m_ghostObject.SetWorldTransform(ref xform);
}
public void PreStep(CollisionWorld collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (RecoverFromPenetration(collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
// printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_currentPosition = m_ghostObject.GetWorldTransform().Translation;
m_targetPosition = m_currentPosition;
}
///btActionInterface interface
public virtual void UpdateAction( CollisionWorld collisionWorld,float deltaTime)
{
PreStep ( collisionWorld);
PlayerStep (collisionWorld, deltaTime);
}
protected void StepDown(CollisionWorld collisionWorld, float dt)
{
Matrix start= Matrix.Identity, end = Matrix.Identity;
// phase 3: down
Vector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
Vector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
m_targetPosition -= (step_drop + gravity_drop);
start.Translation = m_currentPosition;
end.Translation = m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
if (callback.hasHit())
{
// we dropped a fraction of the height -> hit floor
m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
else
{
// we dropped the full height
m_currentPosition = m_targetPosition;
}
}
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
{
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe
Matrix start = Matrix.Identity, end = Matrix.Identity;
m_targetPosition = m_currentPosition + walkMove;
float fraction = 1.0f;
float distance2 = (m_currentPosition-m_targetPosition).LengthSquared();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (Vector3.Dot(m_normalizedDirection,m_touchingNormal) > 0.0f)
{
UpdateTargetPositionBasedOnCollision (ref m_touchingNormal,0.0f,1.0f);
}
}
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0)
{
start.Translation = (m_currentPosition);
end.Translation = (m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
float margin = m_convexShape.Margin;
m_convexShape.Margin = margin + m_addedMargin;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
} else
{
collisionWorld.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
m_convexShape.Margin = margin;
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
{
// we moved only a fraction
float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length();
if (hitDistance<0.0f)
{
// printf("neg dist?\n");
}
/* If the distance is farther than the collision margin, move */
if (hitDistance > m_addedMargin)
{
// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
UpdateTargetPositionBasedOnCollision (ref callback.m_hitNormalWorld,0f,1f);
Vector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.LengthSquared();
if (distance2 > MathUtil.SIMD_EPSILON)
{
currentDir.Normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
if (Vector3.Dot(currentDir, m_normalizedDirection) <= 0.0f)
{
break;
}
}
else
{
// printf("currentDir: don't normalize a zero vector\n");
break;
}
}
else
{
// we moved whole way
m_currentPosition = m_targetPosition;
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}
protected void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
Matrix start = Matrix.Identity, end = Matrix.Identity;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
/* FIXME: Handle penetration properly */
start.Translation = (m_currentPosition + upAxisDirection[m_upAxis] * 0.1f);
end.Translation = m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, ref start, ref end, callback,0f);
}
if (callback.hasHit())
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
///btActionInterface interface
public virtual void UpdateAction(CollisionWorld collisionWorld, float step)
{
UpdateVehicle(step);
}
public SingleContactCallback(CollisionObject collisionObject, CollisionWorld world,ContactResultCallback resultCallback)
{
m_collisionObject = collisionObject;
m_world = world;
m_resultCallback = resultCallback;
}
