forked from bartvdbraak/blender
305 lines
7.6 KiB
C++
305 lines
7.6 KiB
C++
/*
|
|
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
*
|
|
* Permission to use, copy, modify, distribute and sell this software
|
|
* and its documentation for any purpose is hereby granted without fee,
|
|
* provided that the above copyright notice appear in all copies.
|
|
* Erwin Coumans makes no representations about the suitability
|
|
* of this software for any purpose.
|
|
* It is provided "as is" without express or implied warranty.
|
|
*/
|
|
#include "SimpleBroadphase.h"
|
|
#include "BroadphaseCollision/Dispatcher.h"
|
|
#include "BroadphaseCollision/CollisionAlgorithm.h"
|
|
|
|
#include "SimdVector3.h"
|
|
#include "SimdTransform.h"
|
|
#include "SimdMatrix3x3.h"
|
|
#include <vector>
|
|
#include "CollisionShapes/CollisionMargin.h"
|
|
|
|
SimpleBroadphase::SimpleBroadphase()
|
|
: m_firstFreeProxy(0),
|
|
m_numProxies(0),
|
|
m_blockedForChanges(false),
|
|
m_NumOverlapBroadphasePair(0)
|
|
{
|
|
int i;
|
|
for (i=0;i<SIMPLE_MAX_PROXIES;i++)
|
|
{
|
|
m_freeProxies[i] = i;
|
|
}
|
|
}
|
|
|
|
SimpleBroadphase::~SimpleBroadphase()
|
|
{
|
|
/*int i;
|
|
for (i=m_numProxies-1;i>=0;i--)
|
|
{
|
|
BP_Proxy* proxy = m_pProxies[i];
|
|
destroyProxy(proxy);
|
|
}
|
|
*/
|
|
}
|
|
|
|
|
|
BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr)
|
|
{
|
|
if (m_numProxies >= SIMPLE_MAX_PROXIES)
|
|
{
|
|
assert(0);
|
|
return 0; //should never happen, but don't let the game crash ;-)
|
|
}
|
|
assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]);
|
|
|
|
int freeIndex= m_freeProxies[m_firstFreeProxy];
|
|
BroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr);
|
|
m_firstFreeProxy++;
|
|
|
|
m_pProxies[m_numProxies] = proxy;
|
|
m_numProxies++;
|
|
|
|
return proxy;
|
|
}
|
|
void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxy)
|
|
{
|
|
|
|
int i;
|
|
|
|
BroadphaseProxy* proxy1 = &m_proxies[0];
|
|
|
|
int index = proxy - proxy1;
|
|
m_freeProxies[--m_firstFreeProxy] = index;
|
|
|
|
for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
|
|
{
|
|
BroadphasePair& pair = m_OverlappingPairs[i];
|
|
if (pair.m_pProxy0 == proxy ||
|
|
pair.m_pProxy1 == proxy)
|
|
{
|
|
RemoveOverlappingPair(pair);
|
|
}
|
|
}
|
|
|
|
for (i=0;i<m_numProxies;i++)
|
|
{
|
|
if (m_pProxies[i] == proxy)
|
|
{
|
|
m_proxies[i] = m_proxies[m_numProxies-1];
|
|
break;
|
|
}
|
|
}
|
|
m_numProxies--;
|
|
|
|
}
|
|
|
|
SimpleBroadphaseProxy* SimpleBroadphase::GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
|
|
{
|
|
SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxy);
|
|
|
|
int index = proxy0 - &m_proxies[0];
|
|
//assert(index < m_numProxies);
|
|
|
|
SimpleBroadphaseProxy* sbp = &m_proxies[index];
|
|
return sbp;
|
|
}
|
|
void SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
|
|
{
|
|
SimpleBroadphaseProxy* sbp = GetSimpleProxyFromProxy(proxy);
|
|
sbp->m_min = aabbMin;
|
|
sbp->m_max = aabbMax;
|
|
}
|
|
|
|
void SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair)
|
|
{
|
|
for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
|
|
{
|
|
if (pair.m_algorithms[dispatcherId])
|
|
{
|
|
{
|
|
delete pair.m_algorithms[dispatcherId];
|
|
pair.m_algorithms[dispatcherId]=0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
|
|
{
|
|
for (int i=0;i<m_NumOverlapBroadphasePair;i++)
|
|
{
|
|
BroadphasePair& pair = m_OverlappingPairs[i];
|
|
if (pair.m_pProxy0 == proxy ||
|
|
pair.m_pProxy1 == proxy)
|
|
{
|
|
CleanOverlappingPair(pair);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
|
|
{
|
|
|
|
|
|
BroadphasePair pair(*proxy0,*proxy1);
|
|
m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
|
|
|
|
int i;
|
|
for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
|
|
{
|
|
assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
|
|
m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
|
|
}
|
|
|
|
if (m_NumOverlapBroadphasePair >= SIMPLE_MAX_OVERLAP)
|
|
{
|
|
printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
|
|
assert(0);
|
|
}
|
|
|
|
m_NumOverlapBroadphasePair++;
|
|
}
|
|
|
|
bool SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
|
|
{
|
|
bool found = false;
|
|
int i;
|
|
for (i=m_NumOverlapBroadphasePair-1;i>=0;i--)
|
|
{
|
|
BroadphasePair& pair = m_OverlappingPairs[i];
|
|
if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) ||
|
|
((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0)))
|
|
{
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return found;
|
|
}
|
|
void SimpleBroadphase::RemoveOverlappingPair(BroadphasePair& pair)
|
|
{
|
|
CleanOverlappingPair(pair);
|
|
int index = &pair - &m_OverlappingPairs[0];
|
|
//remove efficiently, swap with the last
|
|
m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1];
|
|
m_NumOverlapBroadphasePair--;
|
|
}
|
|
|
|
bool SimpleBroadphase::AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1)
|
|
{
|
|
return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] &&
|
|
proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] &&
|
|
proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2];
|
|
|
|
}
|
|
void SimpleBroadphase::RefreshOverlappingPairs()
|
|
{
|
|
//first check for new overlapping pairs
|
|
int i,j;
|
|
|
|
for (i=0;i<m_numProxies;i++)
|
|
{
|
|
BroadphaseProxy* proxy0 = m_pProxies[i];
|
|
for (j=i+1;j<m_numProxies;j++)
|
|
{
|
|
BroadphaseProxy* proxy1 = m_pProxies[j];
|
|
SimpleBroadphaseProxy* p0 = GetSimpleProxyFromProxy(proxy0);
|
|
SimpleBroadphaseProxy* p1 = GetSimpleProxyFromProxy(proxy1);
|
|
|
|
if (AabbOverlap(p0,p1))
|
|
{
|
|
if ( !FindPair(proxy0,proxy1))
|
|
{
|
|
AddOverlappingPair(proxy0,proxy1);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
//then remove non-overlapping ones
|
|
for (i=0;i<m_NumOverlapBroadphasePair;i++)
|
|
{
|
|
BroadphasePair& pair = m_OverlappingPairs[i];
|
|
SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0);
|
|
SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1);
|
|
if (!AabbOverlap(proxy0,proxy1))
|
|
{
|
|
RemoveOverlappingPair(pair);
|
|
}
|
|
}
|
|
|
|
//BroadphasePair m_OverlappingPairs[SIMPLE_MAX_OVERLAP];
|
|
//int m_NumOverlapBroadphasePair;
|
|
|
|
}
|
|
|
|
void SimpleBroadphase::DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo)
|
|
{
|
|
m_blockedForChanges = true;
|
|
|
|
int i;
|
|
|
|
int dispatcherId = dispatcher.GetUniqueId();
|
|
|
|
RefreshOverlappingPairs();
|
|
|
|
for (i=0;i<m_NumOverlapBroadphasePair;i++)
|
|
{
|
|
|
|
BroadphasePair& pair = m_OverlappingPairs[i];
|
|
|
|
if (dispatcherId>= 0)
|
|
{
|
|
//dispatcher will keep algorithms persistent in the collision pair
|
|
if (!pair.m_algorithms[dispatcherId])
|
|
{
|
|
pair.m_algorithms[dispatcherId] = dispatcher.FindAlgorithm(
|
|
*pair.m_pProxy0,
|
|
*pair.m_pProxy1);
|
|
}
|
|
|
|
if (pair.m_algorithms[dispatcherId])
|
|
{
|
|
if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE)
|
|
{
|
|
pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount,dispatchInfo.m_useContinuous);
|
|
} else
|
|
{
|
|
float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount);
|
|
if (dispatchInfo.m_timeOfImpact > toi)
|
|
dispatchInfo.m_timeOfImpact = toi;
|
|
|
|
}
|
|
}
|
|
} else
|
|
{
|
|
//non-persistent algorithm dispatcher
|
|
CollisionAlgorithm* algo = dispatcher.FindAlgorithm(
|
|
*pair.m_pProxy0,
|
|
*pair.m_pProxy1);
|
|
|
|
if (algo)
|
|
{
|
|
if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE)
|
|
{
|
|
algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount,dispatchInfo.m_useContinuous);
|
|
} else
|
|
{
|
|
float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount);
|
|
if (dispatchInfo.m_timeOfImpact > toi)
|
|
dispatchInfo.m_timeOfImpact = toi;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
m_blockedForChanges = false;
|
|
|
|
}
|
|
|
|
|