public void UpdateSingleAabb(CollisionObject colObj)
{
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
IndexedMatrix wt = colObj.GetWorldTransform();
colObj.GetCollisionShape().GetAabb(ref wt, out minAabb, out maxAabb);
//need to increase the aabb for contact thresholds
IndexedVector3 contactThreshold = new IndexedVector3(BulletGlobals.gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
if (GetDispatchInfo().m_useContinuous && colObj.GetInternalType() == CollisionObjectTypes.CO_RIGID_BODY && !colObj.IsStaticOrKinematicObject())
{
IndexedVector3 minAabb2,maxAabb2;
colObj.GetCollisionShape().GetAabb(colObj.GetInterpolationWorldTransform(),out minAabb2 ,out maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
MathUtil.VectorMin(ref minAabb2,ref minAabb);
MathUtil.VectorMax(ref maxAabb2, ref maxAabb);
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugCollisionWorld)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb);
}
IBroadphaseInterface bp = m_broadphasePairCache as IBroadphaseInterface;
//moving objects should be moderately sized, probably something wrong if not
if (colObj.IsStaticObject() || ((maxAabb - minAabb).LengthSquared() < 1e12f))
{
bp.SetAabb(colObj.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
}
else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj.SetActivationState(ActivationState.DISABLE_SIMULATION);
//static bool reportMe = true;
bool reportMe = true;
if (reportMe && m_debugDrawer != null)
{
reportMe = false;
m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation");
m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n");
m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n");
m_debugDrawer.ReportErrorWarning("Thanks.\n");
}
}
}
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();
}