public virtual void SetAabb(BroadphaseProxy proxy, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IDispatcher dispatcher)
{
SimpleBroadphaseProxy sbp = GetSimpleProxyFromProxy(proxy);
sbp.SetMinAABB(ref aabbMin);
sbp.SetMaxAABB(ref aabbMax);
}
protected virtual bool ProcessOverlap(ref BroadphasePair pair)
{
SimpleBroadphaseProxy proxy0 = (SimpleBroadphaseProxy)(pair.m_pProxy0);
SimpleBroadphaseProxy proxy1 = (SimpleBroadphaseProxy)(pair.m_pProxy1);
return((m_targetProxy == proxy0 || m_targetProxy == proxy1));
}
public virtual void GetAabb(BroadphaseProxy proxy, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
SimpleBroadphaseProxy sbp = GetSimpleProxyFromProxy(proxy);
aabbMin = sbp.GetMinAABB();
aabbMax = sbp.GetMaxAABB();
}
public static bool AabbOverlap(SimpleBroadphaseProxy proxy0, SimpleBroadphaseProxy proxy1)
{
IndexedVector3 p0Min = proxy0.GetMinAABB();
IndexedVector3 p0Max = proxy0.GetMaxAABB();
IndexedVector3 p1Min = proxy1.GetMinAABB();
IndexedVector3 p1Max = proxy1.GetMaxAABB();
#if DEBUG
if(BulletGlobals.g_streamWriter != null && BulletGlobals.debugBroadphase)
{
BulletGlobals.g_streamWriter.WriteLine("simple aabOverlap");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter,"0min",p0Min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter,"0max",p0Max);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter,"1min",p1Min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter,"1max",p1Max);
}
#endif
return p0Min.X <= p1Max.X && p1Min.X <= p0Max.X &&
p0Min.Y <= p1Max.Y && p1Min.Y <= p0Max.Y &&
p0Min.Z <= p1Max.Z && p1Min.Z <= p0Max.Z;
}
public virtual void DestroyProxy(BroadphaseProxy proxy, IDispatcher dispatcher)
{
SimpleBroadphaseProxy proxy0 = (SimpleBroadphaseProxy)(proxy);
FreeHandle(proxy0);
m_pairCache.RemoveOverlappingPairsContainingProxy(proxy, dispatcher);
}
public virtual void RayTest(ref IndexedVector3 rayFrom, ref IndexedVector3 rayTo, BroadphaseRayCallback rayCallback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
for (int i = 0; i <= m_LastHandleIndex; i++)
{
SimpleBroadphaseProxy proxy = m_pHandles[i];
if (proxy.m_clientObject == null)
{
continue;
}
rayCallback.Process(proxy);
}
}
public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
if (m_numHandles >= m_maxHandles)
{
Debug.Assert(false);
return null; //should never happen, but don't let the game crash ;-)
}
int position = AllocHandle();
m_pHandles[position] = new SimpleBroadphaseProxy(position, ref aabbMin, ref aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, multiSapProxy); ;
m_pHandles[position].m_uniqueId = ++m_proxyCounter;
return m_pHandles[position];
}
public virtual void AabbTest(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IBroadphaseAabbCallback callback)
{
for (int i = 0; i <= m_LastHandleIndex; i++)
{
SimpleBroadphaseProxy proxy = m_pHandles[i];
if (proxy.m_clientObject == null)
{
continue;
}
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin, ref aabbMax, ref proxy.m_aabbMin, ref proxy.m_aabbMax))
{
callback.Process(proxy);
}
}
}
private void FreeHandle(SimpleBroadphaseProxy proxy)
{
int handle = proxy.GetPosition();
Debug.Assert(handle >= 0 && handle < m_maxHandles);
if (handle == m_LastHandleIndex)
{
m_LastHandleIndex--;
}
proxy.SetNextFree(m_firstFreeHandle);
m_firstFreeHandle = handle;
proxy.m_clientObject = null;
m_numHandles--;
}
public SimpleBroadphase(int maxProxies, IOverlappingPairCache overlappingPairCache)
{
if (overlappingPairCache == null)
{
overlappingPairCache = new HashedOverlappingPairCache();
m_ownsPairCache = true;
}
m_pHandles = new SimpleBroadphaseProxy[maxProxies];
m_pairCache = overlappingPairCache;
m_maxHandles = maxProxies;
m_numHandles = 0;
m_firstFreeHandle = 0;
m_LastHandleIndex = -1;
for (int i = m_firstFreeHandle; i < maxProxies; i++)
{
m_pHandles[i] = new SimpleBroadphaseProxy(i);
m_pHandles[i].SetNextFree(i + 1);
m_pHandles[i].m_uniqueId = ++m_proxyCounter;; //any UID will do, we just avoid too trivial values (0,1) for debugging purposes
}
m_pHandles[maxProxies - 1].SetNextFree(0);
}
public static bool AabbOverlap(SimpleBroadphaseProxy proxy0, SimpleBroadphaseProxy proxy1)
{
IndexedVector3 p0Min = proxy0.GetMinAABB();
IndexedVector3 p0Max = proxy0.GetMaxAABB();
IndexedVector3 p1Min = proxy1.GetMinAABB();
IndexedVector3 p1Max = proxy1.GetMaxAABB();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBroadphase)
{
BulletGlobals.g_streamWriter.WriteLine("simple aabOverlap");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "0min", p0Min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "0max", p0Max);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "1min", p1Min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "1max", p1Max);
}
return(p0Min.X <= p1Max.X && p1Min.X <= p0Max.X &&
p0Min.Y <= p1Max.Y && p1Min.Y <= p0Max.Y &&
p0Min.Z <= p1Max.Z && p1Min.Z <= p0Max.Z);
}
public SimpleBroadphase(int maxProxies, IOverlappingPairCache overlappingPairCache)
{
if (overlappingPairCache == null)
{
overlappingPairCache = new HashedOverlappingPairCache();
m_ownsPairCache = true;
}
m_pHandles = new SimpleBroadphaseProxy[maxProxies];
m_pairCache = overlappingPairCache;
m_maxHandles = maxProxies;
m_numHandles = 0;
m_firstFreeHandle = 0;
m_LastHandleIndex = -1;
for (int i = m_firstFreeHandle; i < maxProxies; i++)
{
m_pHandles[i] = new SimpleBroadphaseProxy(i);
m_pHandles[i].SetNextFree(i + 1);
m_pHandles[i].m_uniqueId = ++m_proxyCounter; ;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
}
m_pHandles[maxProxies - 1].SetNextFree(0);
}
public virtual void CalculateOverlappingPairs(IDispatcher dispatcher)
{
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBroadphase)
{
BulletGlobals.g_streamWriter.WriteLine("simple calculateOverlappingPairs");
}
#endif
//first check for new overlapping pairs
if (m_numHandles > 0)
{
int new_largest_index = -1;
for (int i = 0; i <= m_LastHandleIndex; i++)
{
SimpleBroadphaseProxy proxy0 = m_pHandles[i];
if (proxy0.GetClientObject() == null)
{
continue;
}
new_largest_index = i;
for (int j = i + 1; j <= m_LastHandleIndex; j++)
{
SimpleBroadphaseProxy proxy1 = m_pHandles[j];
//btAssert(proxy0 != proxy1);
if (proxy1.GetClientObject() == null)
{
continue;
}
if (AabbOverlap(proxy0, proxy1))
{
if (m_pairCache.FindPair(proxy0, proxy1) == null)
{
m_pairCache.AddOverlappingPair(proxy0, proxy1);
}
}
else
{
if (!m_pairCache.HasDeferredRemoval())
{
if (m_pairCache.FindPair(proxy0, proxy1) != null)
{
m_pairCache.RemoveOverlappingPair(proxy0, proxy1, dispatcher);
}
}
}
}
}
m_LastHandleIndex = new_largest_index;
if (m_ownsPairCache && m_pairCache.HasDeferredRemoval())
{
IList <BroadphasePair> overlappingPairArray = m_pairCache.GetOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
((List <BroadphasePair>)overlappingPairArray).Sort();
//overlappingPairArray.Capacity = (overlappingPairArray.Count - m_invalidPair);
m_invalidPair = 0;
BroadphasePair previousPair = new BroadphasePair();
for (int i = 0; i < overlappingPairArray.Count; i++)
{
BroadphasePair pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
previousPair = pair;
bool needsRemoval = false;
if (!isDuplicate)
{
bool hasOverlap = TestAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
if (hasOverlap)
{
needsRemoval = false;//callback.processOverlap(pair);
}
else
{
needsRemoval = true;
}
}
else
{
//remove duplicate
needsRemoval = true;
//should have no algorithm
//btAssert(!pair.m_algorithm);
}
if (needsRemoval)
{
m_pairCache.CleanOverlappingPair(pair, dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back();
pair.m_pProxy0 = null;
pair.m_pProxy1 = null;
m_invalidPair++;
BulletGlobals.gOverlappingPairs--;
}
}
///if you don't like to skip the invalid pairs in the array, execute following code:
#if CLEAN_INVALID_PAIRS
//perform a sort, to sort 'invalid' pairs to the end
//overlappingPairArray.quickSort(new BroadphasePairSortPredicate());
((List <BroadphasePair>)overlappingPairArray).Sort();
//overlappingPairArray.Capacity = overlappingPairArray.Count - m_invalidPair;
m_invalidPair = 0;
#endif//CLEAN_INVALID_PAIRS
}
}
}
public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
{
if (m_numHandles >= m_maxHandles)
{
Debug.Assert(false);
return null; //should never happen, but don't let the game crash ;-)
}
int position = AllocHandle();
m_pHandles[position] = new SimpleBroadphaseProxy(position, ref aabbMin, ref aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, multiSapProxy); ;
m_pHandles[position].m_uniqueId = ++m_proxyCounter;
return m_pHandles[position];
}
private void FreeHandle(SimpleBroadphaseProxy proxy)
{
int handle = proxy.GetPosition();
Debug.Assert(handle >= 0 && handle < m_maxHandles);
if (handle == m_LastHandleIndex)
{
m_LastHandleIndex--;
}
proxy.SetNextFree(m_firstFreeHandle);
m_firstFreeHandle = handle;
proxy.m_clientObject = null;
m_numHandles--;
}