Refactoring:
Moved optional code to Extras: AlgebraicCCD,EPA,quickstep Moved SimpleBroadphase data to OverlappingPairCache, and derive both SimpleBroadphase and AxisSweep3 from OverlappingPairCache. Added ParallelPhysicsEnvironment (prepair more parallel mainloop) Upgraded hardcoded limit from 1024/8192 to 32766/65535 (max objects / max overlapping pairs)
This commit is contained in:
ejcoumans
@@ -48,6 +48,7 @@ void AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const
|
||||
|
||||
|
||||
AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps)
|
||||
:OverlappingPairCache(maxOverlaps)
|
||||
{
|
||||
//assert(bounds.HasVolume());
|
||||
|
||||
|
||||
@@ -21,13 +21,13 @@
|
||||
|
||||
#include "SimdPoint3.h"
|
||||
#include "SimdVector3.h"
|
||||
#include "SimpleBroadphase.h"
|
||||
#include "OverlappingPairCache.h"
|
||||
#include "BroadphaseProxy.h"
|
||||
|
||||
/// AxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
|
||||
/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats.
|
||||
/// The TestOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos
|
||||
class AxisSweep3 : public SimpleBroadphase
|
||||
class AxisSweep3 : public OverlappingPairCache
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
213
Bullet/BroadphaseCollision/OverlappingPairCache.cpp
Normal file
213
Bullet/BroadphaseCollision/OverlappingPairCache.cpp
Normal file
@@ -0,0 +1,213 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "OverlappingPairCache.h"
|
||||
|
||||
#include "Dispatcher.h"
|
||||
#include "CollisionAlgorithm.h"
|
||||
|
||||
|
||||
OverlappingPairCache::OverlappingPairCache(int maxOverlap):
|
||||
m_blockedForChanges(false),
|
||||
m_NumOverlapBroadphasePair(0),
|
||||
m_maxOverlap(maxOverlap)
|
||||
{
|
||||
m_OverlappingPairs = new BroadphasePair[maxOverlap];
|
||||
}
|
||||
|
||||
|
||||
OverlappingPairCache::~OverlappingPairCache()
|
||||
{
|
||||
delete [] m_OverlappingPairs;
|
||||
}
|
||||
|
||||
|
||||
void OverlappingPairCache::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--;
|
||||
}
|
||||
|
||||
|
||||
void OverlappingPairCache::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 OverlappingPairCache::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
|
||||
{
|
||||
//don't add overlap with own
|
||||
assert(proxy0 != proxy1);
|
||||
|
||||
if (!NeedsCollision(proxy0,proxy1))
|
||||
return;
|
||||
|
||||
|
||||
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 >= m_maxOverlap)
|
||||
{
|
||||
//printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
|
||||
#ifdef DEBUG
|
||||
assert(0);
|
||||
#endif
|
||||
} else
|
||||
{
|
||||
m_NumOverlapBroadphasePair++;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
BroadphasePair* OverlappingPairCache::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
|
||||
{
|
||||
BroadphasePair* foundPair = 0;
|
||||
|
||||
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)))
|
||||
{
|
||||
foundPair = &pair;
|
||||
return foundPair;
|
||||
}
|
||||
}
|
||||
|
||||
return foundPair;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void OverlappingPairCache::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 OverlappingPairCache::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy)
|
||||
{
|
||||
int i;
|
||||
for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
|
||||
{
|
||||
BroadphasePair& pair = m_OverlappingPairs[i];
|
||||
if (pair.m_pProxy0 == proxy ||
|
||||
pair.m_pProxy1 == proxy)
|
||||
{
|
||||
RemoveOverlappingPair(pair);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void OverlappingPairCache::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);
|
||||
} else
|
||||
{
|
||||
float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
|
||||
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);
|
||||
} else
|
||||
{
|
||||
float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
|
||||
if (dispatchInfo.m_timeOfImpact > toi)
|
||||
dispatchInfo.m_timeOfImpact = toi;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
m_blockedForChanges = false;
|
||||
|
||||
}
|
||||
85
Bullet/BroadphaseCollision/OverlappingPairCache.h
Normal file
85
Bullet/BroadphaseCollision/OverlappingPairCache.h
Normal file
@@ -0,0 +1,85 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef OVERLAPPING_PAIR_CACHE_H
|
||||
#define OVERLAPPING_PAIR_CACHE_H
|
||||
|
||||
|
||||
#include "BroadphaseInterface.h"
|
||||
#include "BroadphaseProxy.h"
|
||||
#include "SimdPoint3.h"
|
||||
|
||||
|
||||
///OverlappingPairCache maintains the objects with overlapping AABB
|
||||
///Typically managed by the Broadphase, Axis3Sweep or SimpleBroadphase
|
||||
class OverlappingPairCache : public BroadphaseInterface
|
||||
{
|
||||
|
||||
BroadphasePair* m_OverlappingPairs;
|
||||
int m_NumOverlapBroadphasePair;
|
||||
int m_maxOverlap;
|
||||
|
||||
//during the dispatch, check that user doesn't destroy/create proxy
|
||||
bool m_blockedForChanges;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
OverlappingPairCache(int maxOverlap);
|
||||
virtual ~OverlappingPairCache();
|
||||
|
||||
int GetNumOverlappingPairs() const
|
||||
{
|
||||
return m_NumOverlapBroadphasePair;
|
||||
}
|
||||
|
||||
BroadphasePair& GetOverlappingPair(int index)
|
||||
{
|
||||
return m_OverlappingPairs[index];
|
||||
}
|
||||
|
||||
void RemoveOverlappingPair(BroadphasePair& pair);
|
||||
|
||||
void CleanOverlappingPair(BroadphasePair& pair);
|
||||
|
||||
void AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
|
||||
|
||||
BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
|
||||
|
||||
|
||||
|
||||
void CleanProxyFromPairs(BroadphaseProxy* proxy);
|
||||
|
||||
void RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy);
|
||||
|
||||
|
||||
inline bool NeedsCollision(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) const
|
||||
{
|
||||
bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask;
|
||||
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
|
||||
|
||||
return collides;
|
||||
}
|
||||
|
||||
void DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo);
|
||||
|
||||
virtual void RefreshOverlappingPairs() =0;
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
#endif //OVERLAPPING_PAIR_CACHE_H
|
||||
@@ -36,19 +36,16 @@ void SimpleBroadphase::validate()
|
||||
}
|
||||
|
||||
SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap)
|
||||
:m_firstFreeProxy(0),
|
||||
:OverlappingPairCache(maxOverlap),
|
||||
m_firstFreeProxy(0),
|
||||
m_numProxies(0),
|
||||
m_blockedForChanges(false),
|
||||
m_NumOverlapBroadphasePair(0),
|
||||
m_maxProxies(maxProxies),
|
||||
m_maxOverlap(maxOverlap)
|
||||
m_maxProxies(maxProxies)
|
||||
{
|
||||
|
||||
m_proxies = new SimpleBroadphaseProxy[maxProxies];
|
||||
m_freeProxies = new int[maxProxies];
|
||||
m_pProxies = new SimpleBroadphaseProxy*[maxProxies];
|
||||
m_OverlappingPairs = new BroadphasePair[maxOverlap];
|
||||
|
||||
|
||||
|
||||
int i;
|
||||
for (i=0;i<m_maxProxies;i++)
|
||||
@@ -62,7 +59,6 @@ SimpleBroadphase::~SimpleBroadphase()
|
||||
delete[] m_proxies;
|
||||
delete []m_freeProxies;
|
||||
delete [] m_pProxies;
|
||||
delete [] m_OverlappingPairs;
|
||||
|
||||
/*int i;
|
||||
for (i=m_numProxies-1;i>=0;i--)
|
||||
@@ -99,19 +95,7 @@ BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const
|
||||
return proxy;
|
||||
}
|
||||
|
||||
void SimpleBroadphase::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy)
|
||||
{
|
||||
int i;
|
||||
for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
|
||||
{
|
||||
BroadphasePair& pair = m_OverlappingPairs[i];
|
||||
if (pair.m_pProxy0 == proxy ||
|
||||
pair.m_pProxy1 == proxy)
|
||||
{
|
||||
RemoveOverlappingPair(pair);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxyOrg)
|
||||
{
|
||||
@@ -148,93 +132,13 @@ void SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& 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)
|
||||
{
|
||||
//don't add overlap with own
|
||||
assert(proxy0 != proxy1);
|
||||
|
||||
if (!NeedsCollision(proxy0,proxy1))
|
||||
return;
|
||||
|
||||
|
||||
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 >= m_maxOverlap)
|
||||
{
|
||||
//printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
|
||||
#ifdef DEBUG
|
||||
assert(0);
|
||||
#endif
|
||||
} else
|
||||
{
|
||||
m_NumOverlapBroadphasePair++;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
BroadphasePair* SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
|
||||
{
|
||||
BroadphasePair* foundPair = 0;
|
||||
|
||||
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)))
|
||||
{
|
||||
foundPair = &pair;
|
||||
return foundPair;
|
||||
}
|
||||
}
|
||||
|
||||
return foundPair;
|
||||
}
|
||||
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)
|
||||
{
|
||||
@@ -269,9 +173,10 @@ void SimpleBroadphase::RefreshOverlappingPairs()
|
||||
}
|
||||
|
||||
//then remove non-overlapping ones
|
||||
for (i=0;i<m_NumOverlapBroadphasePair;i++)
|
||||
for (i=0;i<GetNumOverlappingPairs();i++)
|
||||
{
|
||||
BroadphasePair& pair = m_OverlappingPairs[i];
|
||||
BroadphasePair& pair = GetOverlappingPair(i);
|
||||
|
||||
SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0);
|
||||
SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1);
|
||||
if (!AabbOverlap(proxy0,proxy1))
|
||||
@@ -284,69 +189,4 @@ void SimpleBroadphase::RefreshOverlappingPairs()
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
} else
|
||||
{
|
||||
float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
|
||||
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);
|
||||
} else
|
||||
{
|
||||
float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
|
||||
if (dispatchInfo.m_timeOfImpact > toi)
|
||||
dispatchInfo.m_timeOfImpact = toi;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
m_blockedForChanges = false;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -16,12 +16,9 @@ subject to the following restrictions:
|
||||
#ifndef SIMPLE_BROADPHASE_H
|
||||
#define SIMPLE_BROADPHASE_H
|
||||
|
||||
//#define SIMPLE_MAX_PROXIES 8192
|
||||
//#define SIMPLE_MAX_OVERLAP 4096
|
||||
|
||||
#include "BroadphaseInterface.h"
|
||||
#include "BroadphaseProxy.h"
|
||||
#include "SimdPoint3.h"
|
||||
#include "OverlappingPairCache.h"
|
||||
|
||||
|
||||
struct SimpleBroadphaseProxy : public BroadphaseProxy
|
||||
{
|
||||
@@ -40,7 +37,7 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy
|
||||
};
|
||||
|
||||
///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks
|
||||
class SimpleBroadphase : public BroadphaseInterface
|
||||
class SimpleBroadphase : public OverlappingPairCache
|
||||
{
|
||||
|
||||
SimpleBroadphaseProxy* m_proxies;
|
||||
@@ -50,14 +47,10 @@ class SimpleBroadphase : public BroadphaseInterface
|
||||
SimpleBroadphaseProxy** m_pProxies;
|
||||
int m_numProxies;
|
||||
|
||||
//during the dispatch, check that user doesn't destroy/create proxy
|
||||
bool m_blockedForChanges;
|
||||
|
||||
BroadphasePair* m_OverlappingPairs;
|
||||
int m_NumOverlapBroadphasePair;
|
||||
|
||||
|
||||
int m_maxProxies;
|
||||
int m_maxOverlap;
|
||||
|
||||
|
||||
inline SimpleBroadphaseProxy* GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
|
||||
{
|
||||
@@ -70,13 +63,8 @@ class SimpleBroadphase : public BroadphaseInterface
|
||||
void validate();
|
||||
|
||||
protected:
|
||||
void RemoveOverlappingPair(BroadphasePair& pair);
|
||||
void CleanOverlappingPair(BroadphasePair& pair);
|
||||
|
||||
void RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy);
|
||||
|
||||
void AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
|
||||
BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
|
||||
virtual void RefreshOverlappingPairs();
|
||||
public:
|
||||
SimpleBroadphase(int maxProxies=4096,int maxOverlap=8192);
|
||||
@@ -88,17 +76,10 @@ public:
|
||||
|
||||
virtual void DestroyProxy(BroadphaseProxy* proxy);
|
||||
virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax);
|
||||
virtual void CleanProxyFromPairs(BroadphaseProxy* proxy);
|
||||
virtual void DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo);
|
||||
|
||||
|
||||
inline bool NeedsCollision(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) const
|
||||
{
|
||||
bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask;
|
||||
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
|
||||
|
||||
return collides;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
@@ -79,7 +79,9 @@ CollisionDispatcher::CollisionDispatcher ():
|
||||
PersistentManifold* CollisionDispatcher::GetNewManifold(void* b0,void* b1)
|
||||
{
|
||||
gNumManifold++;
|
||||
//printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold);
|
||||
|
||||
//ASSERT(gNumManifold < 65535);
|
||||
|
||||
|
||||
CollisionObject* body0 = (CollisionObject*)b0;
|
||||
CollisionObject* body1 = (CollisionObject*)b1;
|
||||
|
||||
@@ -1,360 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "BU_AlgebraicPolynomialSolver.h"
|
||||
#include <math.h>
|
||||
#include <SimdMinMax.h>
|
||||
|
||||
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q)
|
||||
{
|
||||
|
||||
SimdScalar basic_h_local;
|
||||
SimdScalar basic_h_local_delta;
|
||||
|
||||
basic_h_local = p * 0.5f;
|
||||
basic_h_local_delta = basic_h_local * basic_h_local - q;
|
||||
if (basic_h_local_delta > 0.0f) {
|
||||
basic_h_local_delta = SimdSqrt(basic_h_local_delta);
|
||||
m_roots[0] = - basic_h_local + basic_h_local_delta;
|
||||
m_roots[1] = - basic_h_local - basic_h_local_delta;
|
||||
return 2;
|
||||
}
|
||||
else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) {
|
||||
m_roots[0] = - basic_h_local;
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c)
|
||||
{
|
||||
SimdScalar radical = b * b - 4.0f * a * c;
|
||||
if(radical >= 0.f)
|
||||
{
|
||||
SimdScalar sqrtRadical = SimdSqrt(radical);
|
||||
SimdScalar idenom = 1.0f/(2.0f * a);
|
||||
m_roots[0]=(-b + sqrtRadical) * idenom;
|
||||
m_roots[1]=(-b - sqrtRadical) * idenom;
|
||||
return 2;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#define cubic_rt(x) \
|
||||
((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
|
||||
((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f))
|
||||
|
||||
|
||||
|
||||
/* */
|
||||
/* this function solves the following cubic equation: */
|
||||
/* */
|
||||
/* 3 2 */
|
||||
/* lead * x + a * x + b * x + c = 0. */
|
||||
/* */
|
||||
/* it returns the number of different roots found, and stores the roots in */
|
||||
/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0. */
|
||||
/* */
|
||||
int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c)
|
||||
{
|
||||
SimdScalar p, q, r;
|
||||
SimdScalar delta, u, phi;
|
||||
SimdScalar dummy;
|
||||
|
||||
if (lead != 1.0) {
|
||||
/* */
|
||||
/* transform into normal form: x^3 + a x^2 + b x + c = 0 */
|
||||
/* */
|
||||
if (SimdEqual(lead, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* we have a x^2 + b x + c = 0 */
|
||||
/* */
|
||||
if (SimdEqual(a, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* we have b x + c = 0 */
|
||||
/* */
|
||||
if (SimdEqual(b, SIMD_EPSILON)) {
|
||||
if (SimdEqual(c, SIMD_EPSILON)) {
|
||||
return -1;
|
||||
}
|
||||
else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
m_roots[0] = -c / b;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
p = c / a;
|
||||
q = b / a;
|
||||
return Solve2QuadraticFull(a,b,c);
|
||||
}
|
||||
}
|
||||
else {
|
||||
a = a / lead;
|
||||
b = b / lead;
|
||||
c = c / lead;
|
||||
}
|
||||
}
|
||||
|
||||
/* */
|
||||
/* we substitute x = y - a / 3 in order to eliminate the quadric term. */
|
||||
/* we get x^3 + p x + q = 0 */
|
||||
/* */
|
||||
a /= 3.0f;
|
||||
u = a * a;
|
||||
p = b / 3.0f - u;
|
||||
q = a * (2.0f * u - b) + c;
|
||||
|
||||
/* */
|
||||
/* now use Cardano's formula */
|
||||
/* */
|
||||
if (SimdEqual(p, SIMD_EPSILON)) {
|
||||
if (SimdEqual(q, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* one triple root */
|
||||
/* */
|
||||
m_roots[0] = -a;
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
/* */
|
||||
/* one real and two complex roots */
|
||||
/* */
|
||||
m_roots[0] = cubic_rt(-q) - a;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
q /= 2.0f;
|
||||
delta = p * p * p + q * q;
|
||||
if (delta > 0.0f) {
|
||||
/* */
|
||||
/* one real and two complex roots. note that v = -p / u. */
|
||||
/* */
|
||||
u = -q + SimdSqrt(delta);
|
||||
u = cubic_rt(u);
|
||||
m_roots[0] = u - p / u - a;
|
||||
return 1;
|
||||
}
|
||||
else if (delta < 0.0) {
|
||||
/* */
|
||||
/* Casus irreducibilis: we have three real roots */
|
||||
/* */
|
||||
r = SimdSqrt(-p);
|
||||
p *= -r;
|
||||
r *= 2.0;
|
||||
phi = SimdAcos(-q / p) / 3.0f;
|
||||
dummy = SIMD_2_PI / 3.0f;
|
||||
m_roots[0] = r * SimdCos(phi) - a;
|
||||
m_roots[1] = r * SimdCos(phi + dummy) - a;
|
||||
m_roots[2] = r * SimdCos(phi - dummy) - a;
|
||||
return 3;
|
||||
}
|
||||
else {
|
||||
/* */
|
||||
/* one single and one SimdScalar root */
|
||||
/* */
|
||||
r = cubic_rt(-q);
|
||||
m_roots[0] = 2.0f * r - a;
|
||||
m_roots[1] = -r - a;
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* */
|
||||
/* this function solves the following quartic equation: */
|
||||
/* */
|
||||
/* 4 3 2 */
|
||||
/* lead * x + a * x + b * x + c * x + d = 0. */
|
||||
/* */
|
||||
/* it returns the number of different roots found, and stores the roots in */
|
||||
/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0. */
|
||||
/* */
|
||||
int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d)
|
||||
{
|
||||
SimdScalar p, q ,r;
|
||||
SimdScalar u, v, w;
|
||||
int i, num_roots, num_tmp;
|
||||
//SimdScalar tmp[2];
|
||||
|
||||
if (lead != 1.0) {
|
||||
/* */
|
||||
/* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */
|
||||
/* */
|
||||
if (SimdEqual(lead, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* we have a x^3 + b x^2 + c x + d = 0 */
|
||||
/* */
|
||||
if (SimdEqual(a, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* we have b x^2 + c x + d = 0 */
|
||||
/* */
|
||||
if (SimdEqual(b, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* we have c x + d = 0 */
|
||||
/* */
|
||||
if (SimdEqual(c, SIMD_EPSILON)) {
|
||||
if (SimdEqual(d, SIMD_EPSILON)) {
|
||||
return -1;
|
||||
}
|
||||
else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
m_roots[0] = -d / c;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
p = c / b;
|
||||
q = d / b;
|
||||
return Solve2QuadraticFull(b,c,d);
|
||||
|
||||
}
|
||||
}
|
||||
else {
|
||||
return Solve3Cubic(1.0, b / a, c / a, d / a);
|
||||
}
|
||||
}
|
||||
else {
|
||||
a = a / lead;
|
||||
b = b / lead;
|
||||
c = c / lead;
|
||||
d = d / lead;
|
||||
}
|
||||
}
|
||||
|
||||
/* */
|
||||
/* we substitute x = y - a / 4 in order to eliminate the cubic term. */
|
||||
/* we get: y^4 + p y^2 + q y + r = 0. */
|
||||
/* */
|
||||
a /= 4.0f;
|
||||
p = b - 6.0f * a * a;
|
||||
q = a * (8.0f * a * a - 2.0f * b) + c;
|
||||
r = a * (a * (b - 3.f * a * a) - c) + d;
|
||||
if (SimdEqual(q, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* biquadratic equation: y^4 + p y^2 + r = 0. */
|
||||
/* */
|
||||
num_roots = Solve2Quadratic(p, r);
|
||||
if (num_roots > 0) {
|
||||
if (m_roots[0] > 0.0f) {
|
||||
if (num_roots > 1) {
|
||||
if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) {
|
||||
u = SimdSqrt(m_roots[1]);
|
||||
m_roots[2] = u - a;
|
||||
m_roots[3] = -u - a;
|
||||
u = SimdSqrt(m_roots[0]);
|
||||
m_roots[0] = u - a;
|
||||
m_roots[1] = -u - a;
|
||||
return 4;
|
||||
}
|
||||
else {
|
||||
u = SimdSqrt(m_roots[0]);
|
||||
m_roots[0] = u - a;
|
||||
m_roots[1] = -u - a;
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
else {
|
||||
u = SimdSqrt(m_roots[0]);
|
||||
m_roots[0] = u - a;
|
||||
m_roots[1] = -u - a;
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
else if (SimdEqual(r, SIMD_EPSILON)) {
|
||||
/* */
|
||||
/* no absolute term: y (y^3 + p y + q) = 0. */
|
||||
/* */
|
||||
num_roots = Solve3Cubic(1.0, 0.0, p, q);
|
||||
for (i = 0; i < num_roots; ++i) m_roots[i] -= a;
|
||||
if (num_roots != -1) {
|
||||
m_roots[num_roots] = -a;
|
||||
++num_roots;
|
||||
}
|
||||
else {
|
||||
m_roots[0] = -a;
|
||||
num_roots = 1;;
|
||||
}
|
||||
return num_roots;
|
||||
}
|
||||
else {
|
||||
/* */
|
||||
/* we solve the resolvent cubic equation */
|
||||
/* */
|
||||
num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q);
|
||||
if (num_roots == -1) {
|
||||
num_roots = 1;
|
||||
m_roots[0] = 0.0f;
|
||||
}
|
||||
|
||||
/* */
|
||||
/* build two quadric equations */
|
||||
/* */
|
||||
w = m_roots[0];
|
||||
u = w * w - r;
|
||||
v = 2.0f * w - p;
|
||||
|
||||
if (SimdEqual(u, SIMD_EPSILON))
|
||||
u = 0.0;
|
||||
else if (u > 0.0f)
|
||||
u = SimdSqrt(u);
|
||||
else
|
||||
return 0;
|
||||
|
||||
if (SimdEqual(v, SIMD_EPSILON))
|
||||
v = 0.0;
|
||||
else if (v > 0.0f)
|
||||
v = SimdSqrt(v);
|
||||
else
|
||||
return 0;
|
||||
|
||||
if (q < 0.0f) v = -v;
|
||||
w -= u;
|
||||
num_roots=Solve2Quadratic(v, w);
|
||||
for (i = 0; i < num_roots; ++i)
|
||||
{
|
||||
m_roots[i] -= a;
|
||||
}
|
||||
w += 2.0f *u;
|
||||
SimdScalar tmp[2];
|
||||
tmp[0] = m_roots[0];
|
||||
tmp[1] = m_roots[1];
|
||||
|
||||
num_tmp = Solve2Quadratic(-v, w);
|
||||
for (i = 0; i < num_tmp; ++i)
|
||||
{
|
||||
m_roots[i + num_roots] = tmp[i] - a;
|
||||
m_roots[i]=tmp[i];
|
||||
}
|
||||
|
||||
return (num_tmp + num_roots);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,45 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
|
||||
#define BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
|
||||
|
||||
#include "BU_PolynomialSolverInterface.h"
|
||||
|
||||
/// BU_AlgebraicPolynomialSolver implements polynomial root finding by analytically solving algebraic equations.
|
||||
/// Polynomials up to 4rd degree are supported, Cardano's formula is used for 3rd degree
|
||||
class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
|
||||
{
|
||||
public:
|
||||
BU_AlgebraicPolynomialSolver() {};
|
||||
|
||||
int Solve2Quadratic(SimdScalar p, SimdScalar q);
|
||||
int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c);
|
||||
int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c);
|
||||
int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d);
|
||||
|
||||
|
||||
SimdScalar GetRoot(int i) const
|
||||
{
|
||||
return m_roots[i];
|
||||
}
|
||||
|
||||
private:
|
||||
SimdScalar m_roots[4];
|
||||
|
||||
};
|
||||
|
||||
#endif //BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
|
||||
@@ -1,25 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "BU_Collidable.h"
|
||||
#include "CollisionShapes/CollisionShape.h"
|
||||
#include <SimdTransform.h>
|
||||
#include "BU_MotionStateInterface.h"
|
||||
|
||||
BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer )
|
||||
:m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
|
||||
{
|
||||
}
|
||||
@@ -1,57 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BU_COLLIDABLE
|
||||
#define BU_COLLIDABLE
|
||||
|
||||
|
||||
class PolyhedralConvexShape;
|
||||
class BU_MotionStateInterface;
|
||||
#include <SimdPoint3.h>
|
||||
|
||||
class BU_Collidable
|
||||
{
|
||||
public:
|
||||
BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer);
|
||||
|
||||
void* GetUserPointer() const
|
||||
{
|
||||
return m_userPointer;
|
||||
}
|
||||
|
||||
BU_MotionStateInterface& GetMotionState()
|
||||
{
|
||||
return m_motionState;
|
||||
}
|
||||
inline const BU_MotionStateInterface& GetMotionState() const
|
||||
{
|
||||
return m_motionState;
|
||||
}
|
||||
|
||||
inline const PolyhedralConvexShape& GetShape() const
|
||||
{
|
||||
return m_shape;
|
||||
};
|
||||
|
||||
|
||||
private:
|
||||
BU_MotionStateInterface& m_motionState;
|
||||
PolyhedralConvexShape& m_shape;
|
||||
void* m_userPointer;
|
||||
|
||||
};
|
||||
|
||||
#endif //BU_COLLIDABLE
|
||||
@@ -1,581 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "BU_CollisionPair.h"
|
||||
#include "NarrowPhaseCollision/BU_VertexPoly.h"
|
||||
#include "NarrowPhaseCollision/BU_EdgeEdge.h"
|
||||
#include "BU_Collidable.h"
|
||||
|
||||
|
||||
#include "BU_MotionStateInterface.h"
|
||||
#include "CollisionShapes/PolyhedralConvexShape.h"
|
||||
#include <SimdMinMax.h>
|
||||
#include "SimdTransformUtil.h"
|
||||
|
||||
|
||||
|
||||
BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
|
||||
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
|
||||
m_tolerance(tolerance)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
|
||||
// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time
|
||||
|
||||
|
||||
/*
|
||||
bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)
|
||||
|
||||
*/
|
||||
|
||||
bool BU_CollisionPair::calcTimeOfImpact(
|
||||
const SimdTransform& fromA,
|
||||
const SimdTransform& toA,
|
||||
const SimdTransform& fromB,
|
||||
const SimdTransform& toB,
|
||||
CastResult& result)
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
SimdVector3 linvelA,angvelA;
|
||||
SimdVector3 linvelB,angvelB;
|
||||
|
||||
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
|
||||
SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
|
||||
|
||||
|
||||
SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
|
||||
SimdQuaternion angularMotionA(0,0,0,1.f);
|
||||
SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
|
||||
SimdQuaternion angularMotionB(0,0,0,1);
|
||||
|
||||
|
||||
|
||||
result.m_fraction = 1.f;
|
||||
|
||||
SimdTransform impactTransA;
|
||||
SimdTransform impactTransB;
|
||||
|
||||
int index=0;
|
||||
|
||||
SimdScalar toiUnscaled=result.m_fraction;
|
||||
const SimdScalar toiUnscaledLimit = result.m_fraction;
|
||||
|
||||
SimdTransform a2w;
|
||||
a2w = fromA;
|
||||
SimdTransform b2w = fromB;
|
||||
|
||||
/* debugging code
|
||||
{
|
||||
const int numvertsB = m_convexB->GetNumVertices();
|
||||
for (int v=0;v<numvertsB;v++)
|
||||
{
|
||||
SimdPoint3 pt;
|
||||
m_convexB->GetVertex(v,pt);
|
||||
pt = b2w * pt;
|
||||
char buf[1000];
|
||||
|
||||
if (pt.y() < 0.)
|
||||
{
|
||||
sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
} else
|
||||
{
|
||||
sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
SimdTransform b2wp = b2w;
|
||||
|
||||
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
|
||||
b2wp.setRotation( b2w.getRotation() + angularMotionB);
|
||||
|
||||
impactTransB = b2wp;
|
||||
|
||||
SimdTransform a2wp;
|
||||
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
|
||||
a2wp.setRotation(a2w.getRotation()+angularMotionA);
|
||||
|
||||
impactTransA = a2wp;
|
||||
|
||||
SimdTransform a2winv;
|
||||
a2winv = a2w.inverse();
|
||||
|
||||
SimdTransform b2wpinv;
|
||||
b2wpinv = b2wp.inverse();
|
||||
|
||||
SimdTransform b2winv;
|
||||
b2winv = b2w.inverse();
|
||||
|
||||
SimdTransform a2wpinv;
|
||||
a2wpinv = a2wp.inverse();
|
||||
|
||||
//Redon's version with concatenated transforms
|
||||
|
||||
SimdTransform relative;
|
||||
|
||||
relative = b2w * b2wpinv * a2wp * a2winv;
|
||||
|
||||
//relative = a2winv * a2wp * b2wpinv * b2w;
|
||||
|
||||
SimdQuaternion qrel;
|
||||
relative.getBasis().getRotation(qrel);
|
||||
|
||||
SimdVector3 linvel = relative.getOrigin();
|
||||
|
||||
if (linvel.length() < SCREWEPSILON)
|
||||
{
|
||||
linvel.setValue(0.,0.,0.);
|
||||
}
|
||||
SimdVector3 angvel;
|
||||
angvel[0] = 2.f * SimdAsin (qrel[0]);
|
||||
angvel[1] = 2.f * SimdAsin (qrel[1]);
|
||||
angvel[2] = 2.f * SimdAsin (qrel[2]);
|
||||
|
||||
if (angvel.length() < SCREWEPSILON)
|
||||
{
|
||||
angvel.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
|
||||
//Redon's version with concatenated transforms
|
||||
m_screwing = BU_Screwing(linvel,angvel);
|
||||
|
||||
SimdTransform w2s;
|
||||
m_screwing.LocalMatrix(w2s);
|
||||
|
||||
SimdTransform s2w;
|
||||
s2w = w2s.inverse();
|
||||
|
||||
//impactTransA = a2w;
|
||||
//impactTransB = b2w;
|
||||
|
||||
bool hit = false;
|
||||
|
||||
if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
|
||||
{
|
||||
//W = 0 , S = 0 , no collision
|
||||
//toi = 0;
|
||||
/*
|
||||
{
|
||||
const int numvertsB = m_convexB->GetNumVertices();
|
||||
for (int v=0;v<numvertsB;v++)
|
||||
{
|
||||
SimdPoint3 pt;
|
||||
m_convexB->GetVertex(v,pt);
|
||||
pt = impactTransB * pt;
|
||||
char buf[1000];
|
||||
|
||||
if (pt.y() < 0.)
|
||||
{
|
||||
sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
return false;//don't continue moving within epsilon
|
||||
}
|
||||
|
||||
#define EDGEEDGE
|
||||
#ifdef EDGEEDGE
|
||||
|
||||
BU_EdgeEdge edgeEdge;
|
||||
|
||||
//for all edged in A check agains all edges in B
|
||||
for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
|
||||
{
|
||||
SimdPoint3 pA0,pA1;
|
||||
|
||||
m_convexA->GetEdge(ea,pA0,pA1);
|
||||
|
||||
pA0= a2w * pA0;//in world space
|
||||
pA0 = w2s * pA0;//in screwing space
|
||||
|
||||
pA1= a2w * pA1;//in world space
|
||||
pA1 = w2s * pA1;//in screwing space
|
||||
|
||||
int numedgesB = m_convexB->GetNumEdges();
|
||||
for (int eb = 0; eb < numedgesB;eb++)
|
||||
{
|
||||
{
|
||||
SimdPoint3 pB0,pB1;
|
||||
m_convexB->GetEdge(eb,pB0,pB1);
|
||||
|
||||
pB0= b2w * pB0;//in world space
|
||||
pB0 = w2s * pB0;//in screwing space
|
||||
|
||||
pB1= b2w * pB1;//in world space
|
||||
pB1 = w2s * pB1;//in screwing space
|
||||
|
||||
|
||||
SimdScalar lambda,mu;
|
||||
|
||||
toiUnscaled = 1.;
|
||||
|
||||
SimdVector3 edgeDirA(pA1-pA0);
|
||||
SimdVector3 edgeDirB(pB1-pB0);
|
||||
|
||||
if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
|
||||
{
|
||||
//printf("edgeedge potential hit\n");
|
||||
if (toiUnscaled>=0)
|
||||
{
|
||||
if (toiUnscaled < toiUnscaledLimit)
|
||||
{
|
||||
|
||||
//inside check is already done by checking the mu and gamma !
|
||||
|
||||
SimdPoint3 vtx = pA0+lambda * (pA1-pA0);
|
||||
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
|
||||
|
||||
SimdPoint3 hitptWorld = s2w * hitpt;
|
||||
{
|
||||
|
||||
if (toiUnscaled < result.m_fraction)
|
||||
result.m_fraction = toiUnscaled;
|
||||
|
||||
hit = true;
|
||||
|
||||
SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
|
||||
|
||||
hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
|
||||
|
||||
|
||||
hitNormal.normalize();
|
||||
|
||||
//an approximated normal can be calculated by taking the cross product of both edges
|
||||
//take care of the sign !
|
||||
|
||||
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
|
||||
|
||||
SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
|
||||
if (dist > 0)
|
||||
hitNormalWorld *= -1;
|
||||
|
||||
//todo: this is the wrong point, because b2winv is still at begin of motion
|
||||
// not at time-of-impact location!
|
||||
//bhitpt = b2winv * hitptWorld;
|
||||
|
||||
// m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
index++;
|
||||
}
|
||||
};
|
||||
#endif //EDGEEDGE
|
||||
|
||||
#define VERTEXFACE
|
||||
#ifdef VERTEXFACE
|
||||
|
||||
// for all vertices in A, for each face in B,do vertex-face
|
||||
{
|
||||
const int numvertsA = m_convexA->GetNumVertices();
|
||||
for (int v=0;v<numvertsA;v++)
|
||||
//int v=3;
|
||||
|
||||
{
|
||||
SimdPoint3 vtx;
|
||||
m_convexA->GetVertex(v,vtx);
|
||||
|
||||
vtx = a2w * vtx;//in world space
|
||||
vtx = w2s * vtx;//in screwing space
|
||||
|
||||
const int numplanesB = m_convexB->GetNumPlanes();
|
||||
|
||||
for (int p = 0 ; p < numplanesB; p++)
|
||||
//int p=2;
|
||||
{
|
||||
|
||||
{
|
||||
|
||||
SimdVector3 planeNorm;
|
||||
SimdPoint3 planeSupport;
|
||||
|
||||
m_convexB->GetPlane(planeNorm,planeSupport,p);
|
||||
|
||||
|
||||
planeSupport = b2w * planeSupport;//transform to world space
|
||||
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm;
|
||||
|
||||
planeSupport = w2s * planeSupport ; //transform to screwing space
|
||||
planeNorm = w2s.getBasis() * planeNormWorld;
|
||||
|
||||
planeNorm.normalize();
|
||||
|
||||
SimdScalar d = planeSupport.dot(planeNorm);
|
||||
|
||||
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
|
||||
|
||||
BU_VertexPoly vtxApolyB;
|
||||
|
||||
toiUnscaled = 1.;
|
||||
|
||||
if ((p==2) && (v==6))
|
||||
{
|
||||
// printf("%f toiUnscaled\n",toiUnscaled);
|
||||
|
||||
}
|
||||
if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
if (toiUnscaled >= 0. )
|
||||
{
|
||||
//not only collect the first point, get every contactpoint, later we have to check the
|
||||
//manifold properly!
|
||||
|
||||
if (toiUnscaled <= toiUnscaledLimit)
|
||||
{
|
||||
// printf("toiUnscaled %f\n",toiUnscaled );
|
||||
|
||||
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
|
||||
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
|
||||
|
||||
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
|
||||
SimdPoint3 hitptWorld = s2w * hitpt;
|
||||
|
||||
|
||||
hitpt = b2winv * hitptWorld;
|
||||
//vertex has to be 'within' the facet's boundary
|
||||
if (m_convexB->IsInside(hitpt,m_tolerance))
|
||||
{
|
||||
// m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
|
||||
|
||||
if (toiUnscaled < result.m_fraction)
|
||||
result.m_fraction= toiUnscaled;
|
||||
hit = true;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// for all vertices in B, for each face in A,do vertex-face
|
||||
//copy and pasted from all verts A -> all planes B so potential typos!
|
||||
//todo: make this into one method with a kind of 'swapped' logic
|
||||
//
|
||||
{
|
||||
const int numvertsB = m_convexB->GetNumVertices();
|
||||
for (int v=0;v<numvertsB;v++)
|
||||
//int v=0;
|
||||
|
||||
{
|
||||
SimdPoint3 vtx;
|
||||
m_convexB->GetVertex(v,vtx);
|
||||
|
||||
vtx = b2w * vtx;//in world space
|
||||
/*
|
||||
|
||||
char buf[1000];
|
||||
|
||||
if (vtx.y() < 0.)
|
||||
{
|
||||
sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
}
|
||||
sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
|
||||
*/
|
||||
vtx = w2s * vtx;//in screwing space
|
||||
|
||||
const int numplanesA = m_convexA->GetNumPlanes();
|
||||
|
||||
for (int p = 0 ; p < numplanesA; p++)
|
||||
//int p=2;
|
||||
{
|
||||
|
||||
{
|
||||
SimdVector3 planeNorm;
|
||||
SimdPoint3 planeSupport;
|
||||
|
||||
m_convexA->GetPlane(planeNorm,planeSupport,p);
|
||||
|
||||
|
||||
planeSupport = a2w * planeSupport;//transform to world space
|
||||
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm;
|
||||
|
||||
planeSupport = w2s * planeSupport ; //transform to screwing space
|
||||
planeNorm = w2s.getBasis() * planeNormWorld;
|
||||
|
||||
planeNorm.normalize();
|
||||
|
||||
SimdScalar d = planeSupport.dot(planeNorm);
|
||||
|
||||
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
|
||||
|
||||
BU_VertexPoly vtxBpolyA;
|
||||
|
||||
toiUnscaled = 1.;
|
||||
|
||||
if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
|
||||
{
|
||||
if (toiUnscaled>=0.)
|
||||
{
|
||||
if (toiUnscaled < toiUnscaledLimit)
|
||||
{
|
||||
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
|
||||
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
|
||||
//SimdScalar len = hitNormal.length()-1;
|
||||
|
||||
//assert( SimdFuzzyZero(len) );
|
||||
|
||||
|
||||
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
|
||||
SimdPoint3 hitptWorld = s2w * hitpt;
|
||||
hitpt = a2winv * hitptWorld;
|
||||
|
||||
|
||||
//vertex has to be 'within' the facet's boundary
|
||||
if (m_convexA->IsInside(hitpt,m_tolerance))
|
||||
{
|
||||
|
||||
// m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
|
||||
if (toiUnscaled <result.m_fraction)
|
||||
result.m_fraction = toiUnscaled;
|
||||
hit = true;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
index++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif// VERTEXFACE
|
||||
|
||||
//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
|
||||
//in addition there are contact points from previous frames
|
||||
//we have to cleanup the manifold, using an additional epsilon/tolerance
|
||||
//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
|
||||
//else throw it away
|
||||
|
||||
|
||||
if (hit)
|
||||
{
|
||||
|
||||
//try to avoid numerical drift on close contact
|
||||
|
||||
if (result.m_fraction < 0.00001)
|
||||
{
|
||||
// printf("toiUnscaledMin< 0.00001\n");
|
||||
impactTransA = a2w;
|
||||
impactTransB = b2w;
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
//SimdScalar vel = linearMotionB.length();
|
||||
|
||||
//todo: check this margin
|
||||
result.m_fraction *= 0.99f;
|
||||
|
||||
//move B to new position
|
||||
impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
|
||||
SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
|
||||
ornB.normalize();
|
||||
impactTransB.setRotation(ornB);
|
||||
|
||||
//now transform A
|
||||
SimdTransform a2s,a2b;
|
||||
a2s.mult( w2s , a2w);
|
||||
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
|
||||
a2s.multInverseLeft(w2s,a2s);
|
||||
a2b.multInverseLeft(b2w, a2s);
|
||||
|
||||
//transform by motion B
|
||||
impactTransA.mult(impactTransB, a2b);
|
||||
//normalize rotation
|
||||
SimdQuaternion orn;
|
||||
impactTransA.getBasis().getRotation(orn);
|
||||
orn.normalize();
|
||||
impactTransA.setBasis(SimdMatrix3x3(orn));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
{
|
||||
const int numvertsB = m_convexB->GetNumVertices();
|
||||
for (int v=0;v<numvertsB;v++)
|
||||
{
|
||||
SimdPoint3 pt;
|
||||
m_convexB->GetVertex(v,pt);
|
||||
pt = impactTransB * pt;
|
||||
char buf[1000];
|
||||
|
||||
if (pt.y() < 0.)
|
||||
{
|
||||
sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
|
||||
if (debugFile)
|
||||
fwrite(buf,1,strlen(buf),debugFile);
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
return hit;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,54 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BU_COLLISIONPAIR
|
||||
#define BU_COLLISIONPAIR
|
||||
|
||||
#include <NarrowPhaseCollision/BU_Screwing.h>
|
||||
#include <NarrowPhaseCollision/ConvexCast.h>
|
||||
|
||||
|
||||
#include <SimdQuaternion.h>
|
||||
|
||||
class PolyhedralConvexShape;
|
||||
|
||||
|
||||
///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
|
||||
class BU_CollisionPair : public ConvexCast
|
||||
{
|
||||
|
||||
public:
|
||||
BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f);
|
||||
//toi
|
||||
|
||||
virtual bool calcTimeOfImpact(
|
||||
const SimdTransform& fromA,
|
||||
const SimdTransform& toA,
|
||||
const SimdTransform& fromB,
|
||||
const SimdTransform& toB,
|
||||
CastResult& result);
|
||||
|
||||
|
||||
|
||||
|
||||
private:
|
||||
const PolyhedralConvexShape* m_convexA;
|
||||
const PolyhedralConvexShape* m_convexB;
|
||||
BU_Screwing m_screwing;
|
||||
SimdScalar m_tolerance;
|
||||
|
||||
};
|
||||
#endif //BU_COLLISIONPAIR
|
||||
@@ -1,578 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "BU_EdgeEdge.h"
|
||||
#include "BU_Screwing.h"
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdPoint3.h>
|
||||
|
||||
//#include "BU_IntervalArithmeticPolynomialSolver.h"
|
||||
#include "BU_AlgebraicPolynomialSolver.h"
|
||||
|
||||
#define USE_ALGEBRAIC
|
||||
#ifdef USE_ALGEBRAIC
|
||||
#define BU_Polynomial BU_AlgebraicPolynomialSolver
|
||||
#else
|
||||
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
|
||||
#endif
|
||||
|
||||
BU_EdgeEdge::BU_EdgeEdge()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
bool BU_EdgeEdge::GetTimeOfImpact(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,//edge in object A
|
||||
const SimdVector3& u,
|
||||
const SimdPoint3& c,//edge in object B
|
||||
const SimdVector3& v,
|
||||
SimdScalar &minTime,
|
||||
SimdScalar &lambda1,
|
||||
SimdScalar& mu1
|
||||
|
||||
)
|
||||
{
|
||||
bool hit=false;
|
||||
|
||||
SimdScalar lambda;
|
||||
SimdScalar mu;
|
||||
|
||||
const SimdScalar w=screwAB.GetW();
|
||||
const SimdScalar s=screwAB.GetS();
|
||||
|
||||
if (SimdFuzzyZero(s) &&
|
||||
SimdFuzzyZero(w))
|
||||
{
|
||||
//no motion, no collision
|
||||
return false;
|
||||
}
|
||||
|
||||
if (SimdFuzzyZero(w) )
|
||||
{
|
||||
//pure translation W=0, S <> 0
|
||||
//no trig, f(t)=t
|
||||
SimdScalar det = u.y()*v.x()-u.x()*v.y();
|
||||
if (!SimdFuzzyZero(det))
|
||||
{
|
||||
lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
|
||||
mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;
|
||||
|
||||
if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
|
||||
{
|
||||
// single potential collision is
|
||||
SimdScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
|
||||
//if this is on the edge, and time t within [0..1] report hit
|
||||
if (t>=0 && t <= minTime)
|
||||
{
|
||||
hit = true;
|
||||
lambda1 = lambda;
|
||||
mu1 = mu;
|
||||
minTime=t;
|
||||
}
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
//parallel case, not yet
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (SimdFuzzyZero(s) )
|
||||
{
|
||||
if (SimdFuzzyZero(u.z()) )
|
||||
{
|
||||
if (SimdFuzzyZero(v.z()) )
|
||||
{
|
||||
//u.z()=0,v.z()=0
|
||||
if (SimdFuzzyZero(a.z()-c.z()))
|
||||
{
|
||||
//printf("NOT YET planar problem, 4 vertex=edge cases\n");
|
||||
|
||||
} else
|
||||
{
|
||||
//printf("parallel but distinct planes, no collision\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
SimdScalar mu = (a.z() - c.z())/v.z();
|
||||
if (0<=mu && mu <= 1)
|
||||
{
|
||||
// printf("NOT YET//u.z()=0,v.z()<>0\n");
|
||||
} else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
//u.z()<>0
|
||||
|
||||
if (SimdFuzzyZero(v.z()) )
|
||||
{
|
||||
//printf("u.z()<>0,v.z()=0\n");
|
||||
lambda = (c.z() - a.z())/u.z();
|
||||
if (0<=lambda && lambda <= 1)
|
||||
{
|
||||
//printf("u.z()<>0,v.z()=0\n");
|
||||
SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
|
||||
SimdScalar r2 = rotPt.length2();//px*px + py*py;
|
||||
|
||||
//either y=a*x+b, or x = a*x+b...
|
||||
//depends on whether value v.x() is zero or not
|
||||
SimdScalar aa;
|
||||
SimdScalar bb;
|
||||
|
||||
if (SimdFuzzyZero(v.x()))
|
||||
{
|
||||
aa = v.x()/v.y();
|
||||
bb= c.x()+ (-c.y() /v.y()) *v.x();
|
||||
} else
|
||||
{
|
||||
//line is c+mu*v;
|
||||
//x = c.x()+mu*v.x();
|
||||
//mu = ((x-c.x())/v.x());
|
||||
//y = c.y()+((x-c.x())/v.x())*v.y();
|
||||
//y = c.y()+ (-c.x() /v.x()) *v.y() + (x /v.x()) *v.y();
|
||||
//y = a*x+b,where a = v.y()/v.x(), b= c.y()+ (-c.x() /v.x()) *v.y();
|
||||
aa = v.y()/v.x();
|
||||
bb= c.y()+ (-c.x() /v.x()) *v.y();
|
||||
}
|
||||
|
||||
SimdScalar disc = aa*aa*r2 + r2 - bb*bb;
|
||||
if (disc <0)
|
||||
{
|
||||
//edge doesn't intersect the circle (motion of the vertex)
|
||||
return false;
|
||||
}
|
||||
SimdScalar rad = SimdSqrt(r2);
|
||||
|
||||
if (SimdFuzzyZero(disc))
|
||||
{
|
||||
SimdPoint3 intersectPt;
|
||||
|
||||
SimdScalar mu;
|
||||
//intersectionPoint edge with circle;
|
||||
if (SimdFuzzyZero(v.x()))
|
||||
{
|
||||
intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
|
||||
intersectPt.setX( aa*intersectPt.y()+bb );
|
||||
mu = ((intersectPt.y()-c.y())/v.y());
|
||||
} else
|
||||
{
|
||||
intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1)));
|
||||
intersectPt.setY(aa*intersectPt.x()+bb);
|
||||
mu = ((intersectPt.getX()-c.getX())/v.getX());
|
||||
|
||||
}
|
||||
|
||||
if (0 <= mu && mu <= 1)
|
||||
{
|
||||
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
||||
}
|
||||
//only one solution
|
||||
} else
|
||||
{
|
||||
//two points...
|
||||
//intersectionPoint edge with circle;
|
||||
SimdPoint3 intersectPt;
|
||||
//intersectionPoint edge with circle;
|
||||
if (SimdFuzzyZero(v.x()))
|
||||
{
|
||||
SimdScalar mu;
|
||||
|
||||
intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
|
||||
intersectPt.setX(aa*intersectPt.y()+bb);
|
||||
mu = ((intersectPt.getY()-c.getY())/v.getY());
|
||||
if (0.f <= mu && mu <= 1.f)
|
||||
{
|
||||
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
||||
}
|
||||
intersectPt.setY((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
|
||||
intersectPt.setX(aa*intersectPt.y()+bb);
|
||||
mu = ((intersectPt.getY()-c.getY())/v.getY());
|
||||
if (0 <= mu && mu <= 1)
|
||||
{
|
||||
hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
SimdScalar mu;
|
||||
|
||||
intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2*(aa*aa+1.f)));
|
||||
intersectPt.setY(aa*intersectPt.x()+bb);
|
||||
mu = ((intersectPt.getX()-c.getX())/v.getX());
|
||||
if (0 <= mu && mu <= 1)
|
||||
{
|
||||
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
||||
}
|
||||
intersectPt.setX((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
|
||||
intersectPt.setY(aa*intersectPt.x()+bb);
|
||||
mu = ((intersectPt.getX()-c.getX())/v.getX());
|
||||
if (0.f <= mu && mu <= 1.f)
|
||||
{
|
||||
hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//int k=0;
|
||||
|
||||
} else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
//u.z()<>0,v.z()<>0
|
||||
//printf("general case with s=0\n");
|
||||
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
|
||||
if (hit)
|
||||
{
|
||||
lambda1 = lambda;
|
||||
mu1 = mu;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
//printf("general case, W<>0,S<>0\n");
|
||||
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
|
||||
if (hit)
|
||||
{
|
||||
lambda1 = lambda;
|
||||
mu1 = mu;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//W <> 0,pure rotation
|
||||
}
|
||||
|
||||
return hit;
|
||||
}
|
||||
|
||||
|
||||
bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,//edge in object A
|
||||
const SimdVector3& u,
|
||||
const SimdPoint3& c,//edge in object B
|
||||
const SimdVector3& v,
|
||||
SimdScalar &minTime,
|
||||
SimdScalar &lambda,
|
||||
SimdScalar& mu
|
||||
|
||||
)
|
||||
{
|
||||
bool hit = false;
|
||||
|
||||
SimdScalar coefs[4]={0.f,0.f,0.f,0.f};
|
||||
BU_Polynomial polynomialSolver;
|
||||
int numroots = 0;
|
||||
|
||||
//SimdScalar eps=1e-15f;
|
||||
//SimdScalar eps2=1e-20f;
|
||||
SimdScalar s=screwAB.GetS();
|
||||
SimdScalar w = screwAB.GetW();
|
||||
|
||||
SimdScalar ax = a.x();
|
||||
SimdScalar ay = a.y();
|
||||
SimdScalar az = a.z();
|
||||
SimdScalar cx = c.x();
|
||||
SimdScalar cy = c.y();
|
||||
SimdScalar cz = c.z();
|
||||
SimdScalar vx = v.x();
|
||||
SimdScalar vy = v.y();
|
||||
SimdScalar vz = v.z();
|
||||
SimdScalar ux = u.x();
|
||||
SimdScalar uy = u.y();
|
||||
SimdScalar uz = u.z();
|
||||
|
||||
|
||||
if (!SimdFuzzyZero(v.z()))
|
||||
{
|
||||
|
||||
//Maple Autogenerated C code
|
||||
SimdScalar t1,t2,t3,t4,t7,t8,t10;
|
||||
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
|
||||
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
|
||||
SimdScalar t31,t32,t33,t34,t35,t36,t39,t40;
|
||||
SimdScalar t41,t43,t48;
|
||||
SimdScalar t63;
|
||||
|
||||
SimdScalar aa,bb,cc,dd;//the coefficients
|
||||
|
||||
t1 = v.y()*s; t2 = t1*u.x();
|
||||
t3 = v.x()*s;
|
||||
t4 = t3*u.y();
|
||||
t7 = SimdTan(w/2.0f);
|
||||
t8 = 1.0f/t7;
|
||||
t10 = 1.0f/v.z();
|
||||
aa = (t2-t4)*t8*t10;
|
||||
t13 = a.x()*t7;
|
||||
t14 = u.z()*v.y();
|
||||
t15 = t13*t14;
|
||||
t16 = u.x()*v.z();
|
||||
t17 = a.y()*t7;
|
||||
t18 = t16*t17;
|
||||
t19 = u.y()*v.z();
|
||||
t20 = t13*t19;
|
||||
t21 = v.y()*u.x();
|
||||
t22 = c.z()*t7;
|
||||
t23 = t21*t22;
|
||||
t24 = v.x()*a.z();
|
||||
t25 = t7*u.y();
|
||||
t26 = t24*t25;
|
||||
t27 = c.y()*t7;
|
||||
t28 = t16*t27;
|
||||
t29 = a.z()*t7;
|
||||
t30 = t21*t29;
|
||||
t31 = u.z()*v.x();
|
||||
t32 = t31*t27;
|
||||
t33 = t31*t17;
|
||||
t34 = c.x()*t7;
|
||||
t35 = t34*t19;
|
||||
t36 = t34*t14;
|
||||
t39 = v.x()*c.z();
|
||||
t40 = t39*t25;
|
||||
t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40;
|
||||
bb = t41*t8*t10;
|
||||
t43 = t7*u.x();
|
||||
t48 = u.y()*v.y();
|
||||
cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2
|
||||
-2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10;
|
||||
t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30;
|
||||
dd = t63*t8*t10;
|
||||
|
||||
coefs[0]=aa;
|
||||
coefs[1]=bb;
|
||||
coefs[2]=cc;
|
||||
coefs[3]=dd;
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
SimdScalar t1,t2,t3,t4,t7,t8,t10;
|
||||
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
|
||||
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
|
||||
SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
|
||||
SimdScalar p1,p2,p3,p4;
|
||||
|
||||
t1 = uy*s;
|
||||
t2 = t1*vx;
|
||||
t3 = ux*s;
|
||||
t4 = t3*vy;
|
||||
t7 = SimdTan(w/2.0f);
|
||||
t8 = 1/t7;
|
||||
t10 = 1/uz;
|
||||
t13 = ux*az;
|
||||
t14 = t7*vy;
|
||||
t15 = t13*t14;
|
||||
t16 = ax*t7;
|
||||
t17 = uy*vz;
|
||||
t18 = t16*t17;
|
||||
t19 = cx*t7;
|
||||
t20 = t19*t17;
|
||||
t21 = vy*uz;
|
||||
t22 = t19*t21;
|
||||
t23 = ay*t7;
|
||||
t24 = vx*uz;
|
||||
t25 = t23*t24;
|
||||
t26 = uy*cz;
|
||||
t27 = t7*vx;
|
||||
t28 = t26*t27;
|
||||
t29 = t16*t21;
|
||||
t30 = cy*t7;
|
||||
t31 = ux*vz;
|
||||
t32 = t30*t31;
|
||||
t33 = ux*cz;
|
||||
t34 = t33*t14;
|
||||
t35 = t23*t31;
|
||||
t36 = t30*t24;
|
||||
t37 = uy*az;
|
||||
t38 = t37*t27;
|
||||
|
||||
p4 = (-t2+t4)*t8*t10;
|
||||
p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx;
|
||||
p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13
|
||||
*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24;
|
||||
t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38;
|
||||
p1 = t57*t8*t10;
|
||||
|
||||
coefs[0] = p4;
|
||||
coefs[1] = p3;
|
||||
coefs[2] = p2;
|
||||
coefs[1] = p1;
|
||||
|
||||
}
|
||||
|
||||
numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
|
||||
|
||||
for (int i=0;i<numroots;i++)
|
||||
{
|
||||
//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
|
||||
SimdScalar tau = polynomialSolver.GetRoot(i);
|
||||
|
||||
//check whether mu and lambda are in range [0..1]
|
||||
|
||||
if (!SimdFuzzyZero(v.z()))
|
||||
{
|
||||
SimdScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
|
||||
SimdScalar B1=((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+
|
||||
vx*az*SimdTan(1.f/2.f*w)-vx*cz*SimdTan(1.f/2.f*w)+
|
||||
vx*s*tau)/SimdTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
|
||||
lambda = B1/A1;
|
||||
|
||||
mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(SimdTan(w/2.f)))/v.z();
|
||||
|
||||
|
||||
//double check in original equation
|
||||
|
||||
SimdScalar lhs = (a.x()+lambda*u.x())
|
||||
*((1.f-tau*tau)/(1.f+tau*tau))-
|
||||
(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
|
||||
|
||||
lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
|
||||
|
||||
SimdScalar rhs = c.x()+mu*v.x();
|
||||
|
||||
rhs = ((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+vx*az*SimdTan(1.f/2.f*w)-
|
||||
vx*cz*SimdTan(1.f/2.f*w)+vx*s*tau)/(SimdTan(1.f/2.f*w)*vz))-
|
||||
|
||||
(ax-ax*tau*tau-2.f*tau*ay);
|
||||
|
||||
/*SimdScalar res = coefs[0]*tau*tau*tau+
|
||||
coefs[1]*tau*tau+
|
||||
coefs[2]*tau+
|
||||
coefs[3];*/
|
||||
|
||||
//lhs should be rhs !
|
||||
|
||||
if (0.<= mu && mu <=1 && 0.<=lambda && lambda <= 1)
|
||||
{
|
||||
|
||||
} else
|
||||
{
|
||||
//skip this solution, not really touching
|
||||
continue;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
SimdScalar t = 2.f*SimdAtan(tau)/screwAB.GetW();
|
||||
//tau = tan (wt/2) so 2*atan (tau)/w
|
||||
if (t>=0.f && t<minTime)
|
||||
{
|
||||
#ifdef STATS_EDGE_EDGE
|
||||
printf(" ax = %12.12f\n ay = %12.12f\n az = %12.12f\n",a.x(),a.y(),a.z());
|
||||
printf(" ux = %12.12f\n uy = %12.12f\n uz = %12.12f\n",u.x(),u.y(),u.z());
|
||||
printf(" cx = %12.12f\n cy = %12.12f\n cz = %12.12f\n",c.x(),c.y(),c.z());
|
||||
printf(" vx = %12.12f\n vy = %12.12f\n vz = %12.12f\n",v.x(),v.y(),v.z());
|
||||
printf(" s = %12.12f\n w = %12.12f\n", s, w);
|
||||
|
||||
printf(" tau = %12.12f \n lambda = %12.12f \n mu = %f\n",tau,lambda,mu);
|
||||
printf(" ---------------------------------------------\n");
|
||||
|
||||
#endif
|
||||
|
||||
// v,u,a,c,s,w
|
||||
|
||||
// BU_IntervalArithmeticPolynomialSolver iaSolver;
|
||||
// int numroots2 = iaSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
|
||||
|
||||
minTime = t;
|
||||
hit = true;
|
||||
}
|
||||
}
|
||||
|
||||
return hit;
|
||||
}
|
||||
|
||||
|
||||
//C -S
|
||||
//S C
|
||||
|
||||
bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime)
|
||||
{
|
||||
bool hit = false;
|
||||
|
||||
// now calculate the planeEquation for the vertex motion,
|
||||
// and check if the intersectionpoint is at the positive side
|
||||
SimdPoint3 rotPt1(SimdCos(rotW)*rotPt.x()-SimdSin(rotW)*rotPt.y(),
|
||||
SimdSin(rotW)*rotPt.x()+SimdCos(rotW)*rotPt.y(),
|
||||
0.f);
|
||||
|
||||
SimdVector3 rotVec = rotPt1-rotPt;
|
||||
|
||||
SimdVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
|
||||
|
||||
//SimdPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
|
||||
SimdScalar planeD = planeNormal.dot(rotPt1);
|
||||
|
||||
SimdScalar dist = (planeNormal.dot(intersectPt)-planeD);
|
||||
hit = (dist >= -0.001);
|
||||
|
||||
//if (hit)
|
||||
{
|
||||
// minTime = 0;
|
||||
//calculate the time of impact, using the fact of
|
||||
//toi = alpha / screwAB.getW();
|
||||
// cos (alpha) = adjacent/hypothenuse;
|
||||
//adjacent = dotproduct(ipedge,point);
|
||||
//hypothenuse = sqrt(r2);
|
||||
SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
|
||||
SimdScalar hypo = rotRadius;
|
||||
SimdScalar alpha = SimdAcos(adjacent/hypo);
|
||||
SimdScalar t = alpha / rotW;
|
||||
if (t >= 0 && t < minTime)
|
||||
{
|
||||
hit = true;
|
||||
minTime = t;
|
||||
} else
|
||||
{
|
||||
hit = false;
|
||||
}
|
||||
|
||||
}
|
||||
return hit;
|
||||
}
|
||||
|
||||
bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,//edge in object A
|
||||
const SimdVector3& u,
|
||||
const SimdPoint3& c,//edge in object B
|
||||
const SimdVector3& v,
|
||||
SimdScalar &minTime,
|
||||
SimdScalar &lamda,
|
||||
SimdScalar& mu
|
||||
|
||||
)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
@@ -1,76 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BU_EDGEEDGE
|
||||
#define BU_EDGEEDGE
|
||||
|
||||
class BU_Screwing;
|
||||
#include <SimdTransform.h>
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdVector3.h>
|
||||
|
||||
//class BUM_Point2;
|
||||
|
||||
#include <SimdScalar.h>
|
||||
|
||||
///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges.
|
||||
class BU_EdgeEdge
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
BU_EdgeEdge();
|
||||
bool GetTimeOfImpact(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,//edge in object A
|
||||
const SimdVector3& u,
|
||||
const SimdPoint3& c,//edge in object B
|
||||
const SimdVector3& v,
|
||||
SimdScalar &minTime,
|
||||
SimdScalar &lamda,
|
||||
SimdScalar& mu
|
||||
);
|
||||
private:
|
||||
|
||||
bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime);
|
||||
bool GetTimeOfImpactGeneralCase(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,//edge in object A
|
||||
const SimdVector3& u,
|
||||
const SimdPoint3& c,//edge in object B
|
||||
const SimdVector3& v,
|
||||
SimdScalar &minTime,
|
||||
SimdScalar &lamda,
|
||||
SimdScalar& mu
|
||||
|
||||
);
|
||||
|
||||
|
||||
bool GetTimeOfImpactVertexEdge(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,//edge in object A
|
||||
const SimdVector3& u,
|
||||
const SimdPoint3& c,//edge in object B
|
||||
const SimdVector3& v,
|
||||
SimdScalar &minTime,
|
||||
SimdScalar &lamda,
|
||||
SimdScalar& mu
|
||||
|
||||
);
|
||||
|
||||
};
|
||||
|
||||
#endif //BU_EDGEEDGE
|
||||
@@ -1,50 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BU_MOTIONSTATE
|
||||
#define BU_MOTIONSTATE
|
||||
|
||||
|
||||
#include <SimdTransform.h>
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdQuaternion.h>
|
||||
|
||||
class BU_MotionStateInterface
|
||||
{
|
||||
public:
|
||||
virtual ~BU_MotionStateInterface(){};
|
||||
|
||||
virtual void SetTransform(const SimdTransform& trans) = 0;
|
||||
virtual void GetTransform(SimdTransform& trans) const = 0;
|
||||
|
||||
virtual void SetPosition(const SimdPoint3& position) = 0;
|
||||
virtual void GetPosition(SimdPoint3& position) const = 0;
|
||||
|
||||
virtual void SetOrientation(const SimdQuaternion& orientation) = 0;
|
||||
virtual void GetOrientation(SimdQuaternion& orientation) const = 0;
|
||||
|
||||
virtual void SetBasis(const SimdMatrix3x3& basis) = 0;
|
||||
virtual void GetBasis(SimdMatrix3x3& basis) const = 0;
|
||||
|
||||
virtual void SetLinearVelocity(const SimdVector3& linvel) = 0;
|
||||
virtual void GetLinearVelocity(SimdVector3& linvel) const = 0;
|
||||
|
||||
virtual void GetAngularVelocity(SimdVector3& angvel) const = 0;
|
||||
virtual void SetAngularVelocity(const SimdVector3& angvel) = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif //BU_MOTIONSTATE
|
||||
@@ -1,39 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef BUM_POLYNOMIAL_SOLVER_INTERFACE
|
||||
#define BUM_POLYNOMIAL_SOLVER_INTERFACE
|
||||
|
||||
#include <SimdScalar.h>
|
||||
//
|
||||
//BUM_PolynomialSolverInterface is interface class for polynomial root finding.
|
||||
//The number of roots is returned as a result, query GetRoot to get the actual solution.
|
||||
//
|
||||
class BUM_PolynomialSolverInterface
|
||||
{
|
||||
public:
|
||||
virtual ~BUM_PolynomialSolverInterface() {};
|
||||
|
||||
|
||||
// virtual int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) = 0;
|
||||
|
||||
virtual int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0;
|
||||
|
||||
virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0;
|
||||
|
||||
virtual SimdScalar GetRoot(int i) const = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif //BUM_POLYNOMIAL_SOLVER_INTERFACE
|
||||
@@ -1,200 +0,0 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Stephane Redon / Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "BU_Screwing.h"
|
||||
|
||||
|
||||
BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel) {
|
||||
|
||||
|
||||
const SimdScalar dx=relLinVel[0];
|
||||
const SimdScalar dy=relLinVel[1];
|
||||
const SimdScalar dz=relLinVel[2];
|
||||
const SimdScalar wx=relAngVel[0];
|
||||
const SimdScalar wy=relAngVel[1];
|
||||
const SimdScalar wz=relAngVel[2];
|
||||
|
||||
// Compute the screwing parameters :
|
||||
// w : total amount of rotation
|
||||
// s : total amount of translation
|
||||
// u : vector along the screwing axis (||u||=1)
|
||||
// o : point on the screwing axis
|
||||
|
||||
m_w=SimdSqrt(wx*wx+wy*wy+wz*wz);
|
||||
//if (!w) {
|
||||
if (fabs(m_w)<SCREWEPSILON ) {
|
||||
|
||||
assert(m_w == 0.f);
|
||||
|
||||
m_w=0.;
|
||||
m_s=SimdSqrt(dx*dx+dy*dy+dz*dz);
|
||||
if (fabs(m_s)<SCREWEPSILON ) {
|
||||
assert(m_s == 0.);
|
||||
|
||||
m_s=0.;
|
||||
m_u=SimdPoint3(0.,0.,1.);
|
||||
m_o=SimdPoint3(0.,0.,0.);
|
||||
}
|
||||
else {
|
||||
float t=1.f/m_s;
|
||||
m_u=SimdPoint3(dx*t,dy*t,dz*t);
|
||||
m_o=SimdPoint3(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
else { // there is some rotation
|
||||
|
||||
// we compute u
|
||||
|
||||
float v(1.f/m_w);
|
||||
m_u=SimdPoint3(wx*v,wy*v,wz*v); // normalization
|
||||
|
||||
// decomposition of the translation along u and one orthogonal vector
|
||||
|
||||
SimdPoint3 t(dx,dy,dz);
|
||||
m_s=t.dot(m_u); // component along u
|
||||
if (fabs(m_s)<SCREWEPSILON)
|
||||
{
|
||||
//printf("m_s component along u < SCREWEPSILION\n");
|
||||
m_s=0.f;
|
||||
}
|
||||
SimdPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
|
||||
|
||||
// now we have to compute o
|
||||
|
||||
//SimdScalar len = n1.length2();
|
||||
//(len >= BUM_EPSILON2) {
|
||||
if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
|
||||
n1.normalize();
|
||||
SimdVector3 n1orth=m_u.cross(n1);
|
||||
|
||||
float n2x=SimdCos(0.5f*m_w);
|
||||
float n2y=SimdSin(0.5f*m_w);
|
||||
|
||||
m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_o=SimdPoint3(0.f,0.f,0.f);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//Then, I need to compute Pa, the matrix from the reference (global) frame to
|
||||
//the screwing frame :
|
||||
|
||||
|
||||
void BU_Screwing::LocalMatrix(SimdTransform &t) const {
|
||||
//So the whole computations do this : align the Oz axis along the
|
||||
// screwing axis (thanks to u), and then find two others orthogonal axes to
|
||||
// complete the basis.
|
||||
|
||||
if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON))
|
||||
{
|
||||
// to avoid numerical problems
|
||||
float n=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
|
||||
float invn=1.0f/n;
|
||||
SimdMatrix3x3 mat;
|
||||
|
||||
mat[0][0]=-m_u[1]*invn;
|
||||
mat[0][1]=m_u[0]*invn;
|
||||
mat[0][2]=0.f;
|
||||
|
||||
mat[1][0]=-m_u[0]*invn*m_u[2];
|
||||
mat[1][1]=-m_u[1]*invn*m_u[2];
|
||||
mat[1][2]=n;
|
||||
|
||||
mat[2][0]=m_u[0];
|
||||
mat[2][1]=m_u[1];
|
||||
mat[2][2]=m_u[2];
|
||||
|
||||
t.setOrigin(SimdPoint3(
|
||||
m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn,
|
||||
-(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n),
|
||||
-(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2])));
|
||||
|
||||
t.setBasis(mat);
|
||||
|
||||
}
|
||||
else {
|
||||
|
||||
SimdMatrix3x3 m;
|
||||
|
||||
m[0][0]=1.;
|
||||
m[1][0]=0.;
|
||||
m[2][0]=0.;
|
||||
|
||||
m[0][1]=0.f;
|
||||
m[1][1]=float(SimdSign(m_u[2]));
|
||||
m[2][1]=0.f;
|
||||
|
||||
m[0][2]=0.f;
|
||||
m[1][2]=0.f;
|
||||
m[2][2]=float(SimdSign(m_u[2]));
|
||||
|
||||
t.setOrigin(SimdPoint3(
|
||||
-m_o[0],
|
||||
-SimdSign(m_u[2])*m_o[1],
|
||||
-SimdSign(m_u[2])*m_o[2]
|
||||
));
|
||||
t.setBasis(m);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//gives interpolated transform for time in [0..1] in screwing frame
|
||||
SimdTransform BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const
|
||||
{
|
||||
SimdPoint3 org = tr.getOrigin();
|
||||
|
||||
SimdPoint3 neworg (
|
||||
org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t),
|
||||
org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t),
|
||||
org.z()+m_s*CalculateF(t));
|
||||
|
||||
SimdTransform newtr;
|
||||
newtr.setOrigin(neworg);
|
||||
SimdMatrix3x3 basis = tr.getBasis();
|
||||
SimdMatrix3x3 basisorg = tr.getBasis();
|
||||
|
||||
SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t);
|
||||
SimdQuaternion tmpOrn;
|
||||
tr.getBasis().getRotation(tmpOrn);
|
||||
rot = rot * tmpOrn;
|
||||
|
||||
//to avoid numerical drift, normalize quaternion
|
||||
rot.normalize();
|
||||
newtr.setBasis(SimdMatrix3x3(rot));
|
||||
return newtr;
|
||||
|
||||
}
|
||||
|
||||
|
||||
SimdScalar BU_Screwing::CalculateF(SimdScalar t) const
|
||||
{
|
||||
SimdScalar result;
|
||||
if (!m_w)
|
||||
{
|
||||
result = t;
|
||||
} else
|
||||
{
|
||||
result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
@@ -1,77 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef B_SCREWING_H
|
||||
#define B_SCREWING_H
|
||||
|
||||
|
||||
#include <SimdVector3.h>
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdTransform.h>
|
||||
|
||||
|
||||
#define SCREWEPSILON 0.00001f
|
||||
|
||||
///BU_Screwing implements screwing motion interpolation.
|
||||
class BU_Screwing
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel);
|
||||
|
||||
~BU_Screwing() {
|
||||
};
|
||||
|
||||
SimdScalar CalculateF(SimdScalar t) const;
|
||||
//gives interpolated position for time in [0..1] in screwing frame
|
||||
|
||||
inline SimdPoint3 InBetweenPosition(const SimdPoint3& pt,SimdScalar t) const
|
||||
{
|
||||
return SimdPoint3(
|
||||
pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t),
|
||||
pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t),
|
||||
pt.z()+m_s*CalculateF(t));
|
||||
}
|
||||
|
||||
inline SimdVector3 InBetweenVector(const SimdVector3& vec,SimdScalar t) const
|
||||
{
|
||||
return SimdVector3(
|
||||
vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t),
|
||||
vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t),
|
||||
vec.z());
|
||||
}
|
||||
|
||||
//gives interpolated transform for time in [0..1] in screwing frame
|
||||
SimdTransform InBetweenTransform(const SimdTransform& tr,SimdScalar t) const;
|
||||
|
||||
|
||||
//gives matrix from global frame into screwing frame
|
||||
void LocalMatrix(SimdTransform &t) const;
|
||||
|
||||
inline const SimdVector3& GetU() const { return m_u;}
|
||||
inline const SimdVector3& GetO() const {return m_o;}
|
||||
inline const SimdScalar GetS() const{ return m_s;}
|
||||
inline const SimdScalar GetW() const { return m_w;}
|
||||
|
||||
private:
|
||||
float m_w;
|
||||
float m_s;
|
||||
SimdVector3 m_u;
|
||||
SimdVector3 m_o;
|
||||
};
|
||||
|
||||
#endif //B_SCREWING_H
|
||||
@@ -1,91 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef BU_STATIC_MOTIONSTATE
|
||||
#define BU_STATIC_MOTIONSTATE
|
||||
|
||||
|
||||
#include <CollisionShapes/BU_MotionStateInterface.h>
|
||||
|
||||
class BU_StaticMotionState :public BU_MotionStateInterface
|
||||
{
|
||||
public:
|
||||
virtual ~BU_StaticMotionState(){};
|
||||
|
||||
virtual void SetTransform(const SimdTransform& trans)
|
||||
{
|
||||
m_trans = trans;
|
||||
}
|
||||
virtual void GetTransform(SimdTransform& trans) const
|
||||
{
|
||||
trans = m_trans;
|
||||
}
|
||||
virtual void SetPosition(const SimdPoint3& position)
|
||||
{
|
||||
m_trans.setOrigin( position );
|
||||
}
|
||||
virtual void GetPosition(SimdPoint3& position) const
|
||||
{
|
||||
position = m_trans.getOrigin();
|
||||
}
|
||||
|
||||
virtual void SetOrientation(const SimdQuaternion& orientation)
|
||||
{
|
||||
m_trans.setRotation( orientation);
|
||||
}
|
||||
virtual void GetOrientation(SimdQuaternion& orientation) const
|
||||
{
|
||||
orientation = m_trans.getRotation();
|
||||
}
|
||||
|
||||
virtual void SetBasis(const SimdMatrix3x3& basis)
|
||||
{
|
||||
m_trans.setBasis( basis);
|
||||
}
|
||||
virtual void GetBasis(SimdMatrix3x3& basis) const
|
||||
{
|
||||
basis = m_trans.getBasis();
|
||||
}
|
||||
|
||||
virtual void SetLinearVelocity(const SimdVector3& linvel)
|
||||
{
|
||||
m_linearVelocity = linvel;
|
||||
}
|
||||
virtual void GetLinearVelocity(SimdVector3& linvel) const
|
||||
{
|
||||
linvel = m_linearVelocity;
|
||||
}
|
||||
|
||||
virtual void SetAngularVelocity(const SimdVector3& angvel)
|
||||
{
|
||||
m_angularVelocity = angvel;
|
||||
}
|
||||
virtual void GetAngularVelocity(SimdVector3& angvel) const
|
||||
{
|
||||
angvel = m_angularVelocity;
|
||||
}
|
||||
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
SimdTransform m_trans;
|
||||
SimdVector3 m_angularVelocity;
|
||||
SimdVector3 m_linearVelocity;
|
||||
|
||||
};
|
||||
|
||||
#endif //BU_STATIC_MOTIONSTATE
|
||||
@@ -1,159 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "BU_VertexPoly.h"
|
||||
#include "BU_Screwing.h"
|
||||
#include <SimdTransform.h>
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdVector3.h>
|
||||
|
||||
#define USE_ALGEBRAIC
|
||||
#ifdef USE_ALGEBRAIC
|
||||
#include "BU_AlgebraicPolynomialSolver.h"
|
||||
#define BU_Polynomial BU_AlgebraicPolynomialSolver
|
||||
#else
|
||||
#include "BU_IntervalArithmeticPolynomialSolver.h"
|
||||
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
|
||||
#endif
|
||||
|
||||
inline bool TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; }
|
||||
|
||||
|
||||
BU_VertexPoly::BU_VertexPoly()
|
||||
{
|
||||
|
||||
}
|
||||
//return true if a collision will occur between [0..1]
|
||||
//false otherwise. If true, minTime contains the time of impact
|
||||
bool BU_VertexPoly::GetTimeOfImpact(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& a,
|
||||
const SimdVector4& planeEq,
|
||||
SimdScalar &minTime,bool swapAB)
|
||||
{
|
||||
|
||||
bool hit = false;
|
||||
|
||||
// precondition: s=0 and w= 0 is catched by caller!
|
||||
if (TestFuzzyZero(screwAB.GetS()) &&
|
||||
TestFuzzyZero(screwAB.GetW()))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
//case w<>0 and s<> 0
|
||||
const SimdScalar w=screwAB.GetW();
|
||||
const SimdScalar s=screwAB.GetS();
|
||||
|
||||
SimdScalar coefs[4];
|
||||
const SimdScalar p=planeEq[0];
|
||||
const SimdScalar q=planeEq[1];
|
||||
const SimdScalar r=planeEq[2];
|
||||
const SimdScalar d=planeEq[3];
|
||||
|
||||
const SimdVector3 norm(p,q,r);
|
||||
BU_Polynomial polynomialSolver;
|
||||
int numroots = 0;
|
||||
|
||||
//SimdScalar eps=1e-80f;
|
||||
//SimdScalar eps2=1e-100f;
|
||||
|
||||
if (TestFuzzyZero(screwAB.GetS()) )
|
||||
{
|
||||
//S = 0 , W <> 0
|
||||
|
||||
//ax^3+bx^2+cx+d=0
|
||||
coefs[0]=0.;
|
||||
coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
|
||||
coefs[2]=-2*p*a.y()+2*q*a.x();
|
||||
coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
|
||||
|
||||
// numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
|
||||
numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]);
|
||||
|
||||
} else
|
||||
{
|
||||
if (TestFuzzyZero(screwAB.GetW()))
|
||||
{
|
||||
// W = 0 , S <> 0
|
||||
//pax+qay+r(az+st)=d
|
||||
|
||||
SimdScalar dist = (d - a.dot(norm));
|
||||
|
||||
if (TestFuzzyZero(r))
|
||||
{
|
||||
if (TestFuzzyZero(dist))
|
||||
{
|
||||
// no hit
|
||||
} else
|
||||
{
|
||||
// todo a a' might hit sides of polygon T
|
||||
//printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n");
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
SimdScalar etoi = (dist)/(r*screwAB.GetS());
|
||||
if (swapAB)
|
||||
etoi *= -1;
|
||||
|
||||
if (etoi >= 0. && etoi <= minTime)
|
||||
{
|
||||
minTime = etoi;
|
||||
hit = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
//ax^3+bx^2+cx+d=0
|
||||
|
||||
//degenerate coefficients mess things up :(
|
||||
SimdScalar ietsje = (r*s)/SimdTan(w/2.f);
|
||||
if (ietsje*ietsje < 0.01f)
|
||||
ietsje = 0.f;
|
||||
|
||||
coefs[0]=ietsje;//(r*s)/tan(w/2.);
|
||||
coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
|
||||
coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.)));
|
||||
coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
|
||||
|
||||
numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<numroots;i++)
|
||||
{
|
||||
SimdScalar tau = polynomialSolver.GetRoot(i);
|
||||
|
||||
SimdScalar t = 2.f*SimdAtan(tau)/w;
|
||||
//tau = tan (wt/2) so 2*atan (tau)/w
|
||||
if (swapAB)
|
||||
{
|
||||
t *= -1.;
|
||||
}
|
||||
if (t>=0 && t<minTime)
|
||||
{
|
||||
minTime = t;
|
||||
hit = true;
|
||||
}
|
||||
}
|
||||
|
||||
return hit;
|
||||
}
|
||||
@@ -1,43 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef VERTEX_POLY_H
|
||||
#define VERTEX_POLY_H
|
||||
|
||||
|
||||
class BU_Screwing;
|
||||
#include <SimdTransform.h>
|
||||
#include <SimdPoint3.h>
|
||||
#include <SimdScalar.h>
|
||||
|
||||
///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane.
|
||||
class BU_VertexPoly
|
||||
{
|
||||
public:
|
||||
BU_VertexPoly();
|
||||
bool GetTimeOfImpact(
|
||||
const BU_Screwing& screwAB,
|
||||
const SimdPoint3& vtx,
|
||||
const SimdVector4& planeEq,
|
||||
SimdScalar &minTime,
|
||||
bool swapAB);
|
||||
|
||||
private:
|
||||
|
||||
//cached data (frame coherency etc.) here
|
||||
|
||||
};
|
||||
#endif //VERTEX_POLY_H
|
||||
@@ -1,560 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "SimdScalar.h"
|
||||
#include "SimdVector3.h"
|
||||
#include "SimdPoint3.h"
|
||||
#include "SimdTransform.h"
|
||||
#include "SimdMinMax.h"
|
||||
|
||||
#include "CollisionShapes/ConvexShape.h"
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <algorithm>
|
||||
|
||||
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
|
||||
|
||||
#include "NarrowPhaseCollision/EpaCommon.h"
|
||||
|
||||
#include "NarrowPhaseCollision/EpaVertex.h"
|
||||
#include "NarrowPhaseCollision/EpaHalfEdge.h"
|
||||
#include "NarrowPhaseCollision/EpaFace.h"
|
||||
#include "NarrowPhaseCollision/EpaPolyhedron.h"
|
||||
#include "NarrowPhaseCollision/Epa.h"
|
||||
|
||||
const SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
|
||||
const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
|
||||
|
||||
Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
|
||||
m_pConvexShapeB( pConvexShapeB ),
|
||||
m_transformA( transformA ),
|
||||
m_transformB( transformB )
|
||||
{
|
||||
m_faceEntries.reserve( EPA_MAX_FACE_ENTRIES );
|
||||
}
|
||||
|
||||
Epa::~Epa()
|
||||
{
|
||||
}
|
||||
|
||||
bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
|
||||
{
|
||||
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
|
||||
|
||||
SimdVector3 v( 1, 0, 0 );
|
||||
SimdScalar squaredDistance = SIMD_INFINITY;
|
||||
|
||||
SimdScalar delta = 0.f;
|
||||
|
||||
simplexSolver.reset();
|
||||
|
||||
int nbIterations = 0;
|
||||
|
||||
while ( true )
|
||||
{
|
||||
assert( ( v.length2() > 0 ) && "Warning : v has zero magnitude!" );
|
||||
|
||||
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
|
||||
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
|
||||
|
||||
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
SimdPoint3 pWorld = m_transformA( pInA );
|
||||
SimdPoint3 qWorld = m_transformB( qInB );
|
||||
|
||||
SimdVector3 w = pWorld - qWorld;
|
||||
delta = v.dot( w );
|
||||
|
||||
assert( ( delta <= 0 ) && "Shapes are disjoint, EPA should have never been called!" );
|
||||
assert( !simplexSolver.inSimplex( w ) && "Shapes are disjoint, EPA should have never been called!" );
|
||||
|
||||
// Add support point to simplex
|
||||
simplexSolver.addVertex( w, pWorld, qWorld );
|
||||
|
||||
bool closestOk = simplexSolver.closest( v );
|
||||
assert( closestOk && "Shapes are disjoint, EPA should have never been called!" );
|
||||
|
||||
SimdScalar prevVSqrd = squaredDistance;
|
||||
squaredDistance = v.length2();
|
||||
|
||||
// Is v converging to v(A-B) ?
|
||||
assert( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) &&
|
||||
"Shapes are disjoint, EPA should have never been called!" );
|
||||
|
||||
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
++nbIterations;
|
||||
}
|
||||
|
||||
SimdPoint3 simplexPoints[ 5 ];
|
||||
SimdPoint3 wSupportPointsOnA[ 5 ];
|
||||
SimdPoint3 wSupportPointsOnB[ 5 ];
|
||||
|
||||
int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
|
||||
|
||||
// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
|
||||
// by hybrid penetration depth
|
||||
assert( ( ( nbSimplexPoints > 1 ) && ( nbSimplexPoints <= 4 ) ) &&
|
||||
"Hybrid Penetration Depth algorithm failed!" );
|
||||
|
||||
int nbPolyhedronPoints = nbSimplexPoints;
|
||||
|
||||
#ifndef EPA_POLYHEDRON_USE_PLANES
|
||||
int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
|
||||
#endif
|
||||
|
||||
// Prepare initial polyhedron to start EPA from
|
||||
if ( nbSimplexPoints == 1 )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else if ( nbSimplexPoints == 2 )
|
||||
{
|
||||
// We have a line segment inside the CSO that contains the origin
|
||||
// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
|
||||
|
||||
SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
|
||||
d.normalize();
|
||||
|
||||
SimdVector3 v1;
|
||||
SimdVector3 v2;
|
||||
SimdVector3 v3;
|
||||
|
||||
SimdVector3 e1;
|
||||
|
||||
SimdScalar absx = abs( d.getX() );
|
||||
SimdScalar absy = abs( d.getY() );
|
||||
SimdScalar absz = abs( d.getZ() );
|
||||
|
||||
if ( absx < absy )
|
||||
{
|
||||
if ( absx < absz )
|
||||
{
|
||||
e1.setX( 1 );
|
||||
}
|
||||
else
|
||||
{
|
||||
e1.setZ( 1 );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( absy < absz )
|
||||
{
|
||||
e1.setY( 1 );
|
||||
}
|
||||
else
|
||||
{
|
||||
e1.setZ( 1 );
|
||||
}
|
||||
}
|
||||
|
||||
v1 = d.cross( e1 );
|
||||
v1.normalize();
|
||||
|
||||
v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG );
|
||||
v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG );
|
||||
|
||||
nbPolyhedronPoints = 5;
|
||||
|
||||
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
|
||||
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
|
||||
|
||||
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
SimdPoint3 pWorld = m_transformA( p );
|
||||
SimdPoint3 qWorld = m_transformB( q );
|
||||
|
||||
wSupportPointsOnA[ 2 ] = pWorld;
|
||||
wSupportPointsOnB[ 2 ] = qWorld;
|
||||
simplexPoints[ 2 ] = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ];
|
||||
|
||||
seperatingAxisInA = v2 * m_transformA.getBasis();
|
||||
seperatingAxisInB = -v2 * m_transformB.getBasis();
|
||||
|
||||
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
pWorld = m_transformA( p );
|
||||
qWorld = m_transformB( q );
|
||||
|
||||
wSupportPointsOnA[ 3 ] = pWorld;
|
||||
wSupportPointsOnB[ 3 ] = qWorld;
|
||||
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
|
||||
|
||||
seperatingAxisInA = v3 * m_transformA.getBasis();
|
||||
seperatingAxisInB = -v3 * m_transformB.getBasis();
|
||||
|
||||
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
pWorld = m_transformA( p );
|
||||
qWorld = m_transformB( q );
|
||||
|
||||
wSupportPointsOnA[ 4 ] = pWorld;
|
||||
wSupportPointsOnB[ 4 ] = qWorld;
|
||||
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
|
||||
|
||||
#ifndef EPA_POLYHEDRON_USE_PLANES
|
||||
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
|
||||
{
|
||||
initTetraIndices[ 1 ] = 2;
|
||||
initTetraIndices[ 2 ] = 3;
|
||||
initTetraIndices[ 3 ] = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
|
||||
{
|
||||
initTetraIndices[ 0 ] = 1;
|
||||
initTetraIndices[ 1 ] = 2;
|
||||
initTetraIndices[ 2 ] = 3;
|
||||
initTetraIndices[ 3 ] = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
// No tetrahedron contains the origin
|
||||
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else if ( nbSimplexPoints == 3 )
|
||||
{
|
||||
// We have a triangle inside the CSO that contains the origin
|
||||
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
|
||||
|
||||
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
|
||||
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
|
||||
SimdVector3 triangleNormal = v0.cross( v1 );
|
||||
triangleNormal.normalize();
|
||||
|
||||
nbPolyhedronPoints = 5;
|
||||
|
||||
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
|
||||
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
|
||||
|
||||
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
SimdPoint3 pWorld = m_transformA( p );
|
||||
SimdPoint3 qWorld = m_transformB( q );
|
||||
|
||||
wSupportPointsOnA[ 3 ] = pWorld;
|
||||
wSupportPointsOnB[ 3 ] = qWorld;
|
||||
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
|
||||
|
||||
#ifndef EPA_POLYHEDRON_USE_PLANES
|
||||
// We place this check here because if the tetrahedron contains the origin
|
||||
// there is no need to sample another support point
|
||||
if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) )
|
||||
{
|
||||
#endif
|
||||
seperatingAxisInA = -triangleNormal * m_transformA.getBasis();
|
||||
seperatingAxisInB = triangleNormal * m_transformB.getBasis();
|
||||
|
||||
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
pWorld = m_transformA( p );
|
||||
qWorld = m_transformB( q );
|
||||
|
||||
wSupportPointsOnA[ 4 ] = pWorld;
|
||||
wSupportPointsOnB[ 4 ] = qWorld;
|
||||
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
|
||||
|
||||
#ifndef EPA_POLYHEDRON_USE_PLANES
|
||||
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) )
|
||||
{
|
||||
initTetraIndices[ 3 ] = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
// No tetrahedron contains the origin
|
||||
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#ifdef _DEBUG
|
||||
else if ( nbSimplexPoints == 4 )
|
||||
{
|
||||
assert( TetrahedronContainsOrigin( simplexPoints ) && "Initial tetrahedron does not contain the origin!" );
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef EPA_POLYHEDRON_USE_PLANES
|
||||
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
|
||||
simplexPoints[ initTetraIndices[ 1 ] ],
|
||||
simplexPoints[ initTetraIndices[ 2 ] ],
|
||||
simplexPoints[ initTetraIndices[ 3 ] ] };
|
||||
|
||||
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
|
||||
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
|
||||
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
|
||||
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
|
||||
|
||||
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
|
||||
wSupportPointsOnB[ initTetraIndices[ 1 ] ],
|
||||
wSupportPointsOnB[ initTetraIndices[ 2 ] ],
|
||||
wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
|
||||
#endif
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) )
|
||||
#else
|
||||
if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) )
|
||||
#endif
|
||||
{
|
||||
// Failed to create initial polyhedron
|
||||
assert( false && "Failed to create initial polyhedron!" );
|
||||
return false;
|
||||
}
|
||||
|
||||
// Add initial faces to priority queue
|
||||
|
||||
#ifdef _DEBUG
|
||||
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
|
||||
#endif
|
||||
|
||||
std::list< EpaFace* >& faces = m_polyhedron.GetFaces();
|
||||
|
||||
std::list< EpaFace* >::iterator facesItr( faces.begin() );
|
||||
|
||||
while ( facesItr != faces.end() )
|
||||
{
|
||||
EpaFace* pFace = *facesItr;
|
||||
|
||||
if ( !pFace->m_deleted )
|
||||
{
|
||||
//#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
// if ( pFace->m_planeDistance >= 0 )
|
||||
// {
|
||||
// m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
|
||||
// assert( false && "Face's plane distance equal or greater than 0!" );
|
||||
// }
|
||||
//#endif
|
||||
|
||||
if ( pFace->IsAffinelyDependent() )
|
||||
{
|
||||
assert( false && "One initial face is affinely dependent!" );
|
||||
return false;
|
||||
}
|
||||
|
||||
if ( pFace->m_vSqrd <= 0 )
|
||||
{
|
||||
assert( false && "Face containing the origin!" );
|
||||
return false;
|
||||
}
|
||||
|
||||
if ( pFace->IsClosestPointInternal() )
|
||||
{
|
||||
m_faceEntries.push_back( pFace );
|
||||
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
|
||||
}
|
||||
}
|
||||
|
||||
++facesItr;
|
||||
}
|
||||
|
||||
#ifdef _DEBUG
|
||||
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
|
||||
#endif
|
||||
|
||||
assert( !m_faceEntries.empty() && "No faces added to heap!" );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
|
||||
{
|
||||
SimdVector3 v;
|
||||
|
||||
SimdScalar upperBoundSqrd = SIMD_INFINITY;
|
||||
SimdScalar vSqrd = 0;
|
||||
#ifdef _DEBUG
|
||||
SimdScalar prevVSqrd;
|
||||
#endif
|
||||
SimdScalar delta;
|
||||
|
||||
bool isCloseEnough = false;
|
||||
|
||||
EpaFace* pEpaFace = NULL;
|
||||
|
||||
int nbIterations = 0;
|
||||
//int nbMaxIterations = 1000;
|
||||
|
||||
do
|
||||
{
|
||||
pEpaFace = m_faceEntries.front();
|
||||
std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
|
||||
m_faceEntries.pop_back();
|
||||
|
||||
if ( !pEpaFace->m_deleted )
|
||||
{
|
||||
#ifdef _DEBUG
|
||||
prevVSqrd = vSqrd;
|
||||
#endif
|
||||
|
||||
vSqrd = pEpaFace->m_vSqrd;
|
||||
|
||||
if ( pEpaFace->m_planeDistance >= 0 )
|
||||
{
|
||||
v = pEpaFace->m_planeNormal;
|
||||
}
|
||||
else
|
||||
{
|
||||
v = pEpaFace->m_v;
|
||||
}
|
||||
|
||||
#ifdef _DEBUG
|
||||
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
|
||||
assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" );
|
||||
#endif //_DEBUG
|
||||
assert( ( v.length2() > 0 ) && "Zero vector not allowed!" );
|
||||
|
||||
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis();
|
||||
SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
|
||||
|
||||
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
|
||||
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
|
||||
|
||||
SimdPoint3 pWorld = m_transformA( p );
|
||||
SimdPoint3 qWorld = m_transformB( q );
|
||||
|
||||
SimdPoint3 w = pWorld - qWorld;
|
||||
delta = v.dot( w );
|
||||
|
||||
// Keep tighest upper bound
|
||||
upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
|
||||
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
|
||||
|
||||
isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
|
||||
|
||||
if ( !isCloseEnough )
|
||||
{
|
||||
std::list< EpaFace* > newFaces;
|
||||
bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces );
|
||||
|
||||
if ( expandOk )
|
||||
{
|
||||
assert( !newFaces.empty() && "EPA polyhedron not expanding ?" );
|
||||
|
||||
bool check = true;
|
||||
bool areEqual = false;
|
||||
|
||||
while ( !newFaces.empty() )
|
||||
{
|
||||
EpaFace* pNewFace = newFaces.front();
|
||||
assert( !pNewFace->m_deleted && "New face is deleted!" );
|
||||
|
||||
if ( !pNewFace->m_deleted )
|
||||
{
|
||||
assert( ( pNewFace->m_vSqrd > 0 ) && "Face containing the origin!" );
|
||||
assert( !pNewFace->IsAffinelyDependent() && "Face is affinely dependent!" );
|
||||
|
||||
//#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
//// if ( pNewFace->m_planeDistance >= 0 )
|
||||
//// {
|
||||
// // assert( false && "Face's plane distance greater than 0!" );
|
||||
//#ifdef _DEBUG
|
||||
//// m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" );
|
||||
//#endif
|
||||
// //pNewFace->FixOrder();
|
||||
//#ifdef _DEBUG
|
||||
// //m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" );
|
||||
//#endif
|
||||
//// }
|
||||
//#endif
|
||||
//
|
||||
//#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
// //assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" );
|
||||
//#endif
|
||||
|
||||
if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) )
|
||||
{
|
||||
m_faceEntries.push_back( pNewFace );
|
||||
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
|
||||
}
|
||||
}
|
||||
|
||||
newFaces.pop_front();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
|
||||
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
|
||||
|
||||
#ifdef _DEBUG
|
||||
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
|
||||
#endif
|
||||
|
||||
return v.length();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
++nbIterations;
|
||||
}
|
||||
while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/
|
||||
!isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) );
|
||||
|
||||
#ifdef _DEBUG
|
||||
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
|
||||
#endif
|
||||
|
||||
assert( pEpaFace && "Invalid epa face!" );
|
||||
|
||||
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
|
||||
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
|
||||
|
||||
return v.length();
|
||||
}
|
||||
|
||||
bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
|
||||
const SimdPoint3& point2, const SimdPoint3& point3 )
|
||||
{
|
||||
SimdVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
|
||||
( point2 - point1 ).cross( point3 - point1 ),
|
||||
( point3 - point2 ).cross( point0 - point2 ),
|
||||
( point0 - point3 ).cross( point1 - point3 ) };
|
||||
|
||||
return ( ( facesNormals[ 0 ].dot( point0 ) > 0 ) != ( facesNormals[ 0 ].dot( point3 ) > 0 ) ) &&
|
||||
( ( facesNormals[ 1 ].dot( point1 ) > 0 ) != ( facesNormals[ 1 ].dot( point0 ) > 0 ) ) &&
|
||||
( ( facesNormals[ 2 ].dot( point2 ) > 0 ) != ( facesNormals[ 2 ].dot( point1 ) > 0 ) ) &&
|
||||
( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) );
|
||||
}
|
||||
|
||||
bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints )
|
||||
{
|
||||
return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
|
||||
}
|
||||
|
||||
bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB )
|
||||
{
|
||||
return ( pFaceA->m_vSqrd > pFaceB->m_vSqrd );
|
||||
}
|
||||
@@ -1,66 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_H
|
||||
#define EPA_H
|
||||
|
||||
#define EPA_MAX_FACE_ENTRIES 256
|
||||
|
||||
extern const SimdScalar EPA_MAX_RELATIVE_ERROR;
|
||||
extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD;
|
||||
|
||||
class Epa
|
||||
{
|
||||
private :
|
||||
|
||||
//! Prevents copying objects from this class
|
||||
Epa( const Epa& epa );
|
||||
const Epa& operator = ( const Epa& epa );
|
||||
|
||||
public :
|
||||
|
||||
Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB );
|
||||
~Epa();
|
||||
|
||||
bool Initialize( SimplexSolverInterface& simplexSolver );
|
||||
|
||||
SimdScalar CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
|
||||
|
||||
private :
|
||||
|
||||
bool TetrahedronContainsOrigin( SimdPoint3* pPoints );
|
||||
bool TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
|
||||
const SimdPoint3& point2, const SimdPoint3& point3 );
|
||||
|
||||
private :
|
||||
|
||||
//! Priority queue
|
||||
std::vector< EpaFace* > m_faceEntries;
|
||||
|
||||
ConvexShape* m_pConvexShapeA;
|
||||
ConvexShape* m_pConvexShapeB;
|
||||
|
||||
SimdTransform m_transformA;
|
||||
SimdTransform m_transformB;
|
||||
|
||||
EpaPolyhedron m_polyhedron;
|
||||
};
|
||||
|
||||
extern bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB );
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,25 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_COMMON_H
|
||||
#define EPA_COMMON_H
|
||||
|
||||
#define EPA_POLYHEDRON_USE_PLANES
|
||||
|
||||
//#define EPA_USE_HYBRID
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,254 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#include "SimdScalar.h"
|
||||
#include "SimdVector3.h"
|
||||
#include "SimdPoint3.h"
|
||||
|
||||
#include "NarrowPhaseCollision/EpaCommon.h"
|
||||
|
||||
#include "NarrowPhaseCollision/EpaVertex.h"
|
||||
#include "NarrowPhaseCollision/EpaHalfEdge.h"
|
||||
#include "NarrowPhaseCollision/EpaFace.h"
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
SimdScalar PLANE_THICKNESS = 1e-5f;
|
||||
#endif
|
||||
|
||||
EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false )
|
||||
{
|
||||
m_pVertices[ 0 ] = m_pVertices[ 1 ] = m_pVertices[ 2 ] = 0;
|
||||
}
|
||||
|
||||
EpaFace::~EpaFace()
|
||||
{
|
||||
}
|
||||
|
||||
bool EpaFace::Initialize()
|
||||
{
|
||||
assert( m_pHalfEdge && "Must setup half-edge first!" );
|
||||
|
||||
CollectVertices( m_pVertices );
|
||||
|
||||
const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
|
||||
const SimdScalar e0Sqrd = e0.length2();
|
||||
const SimdScalar e1Sqrd = e1.length2();
|
||||
const SimdScalar e0e1 = e0.dot( e1 );
|
||||
|
||||
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
|
||||
|
||||
const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
|
||||
const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
|
||||
|
||||
m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
|
||||
m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
|
||||
|
||||
if ( IsAffinelyDependent() )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
CalcClosestPoint();
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
if ( !CalculatePlane() )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
bool EpaFace::CalculatePlane()
|
||||
{
|
||||
assert( ( m_pVertices[ 0 ] && m_pVertices[ 1 ] && m_pVertices[ 2 ] )
|
||||
&& "Must setup vertices pointers first!" );
|
||||
|
||||
// Traditional method
|
||||
|
||||
const SimdVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
const SimdVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
|
||||
m_planeNormal = v2.cross( v1 );
|
||||
|
||||
if ( m_planeNormal.length2() == 0 )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
m_planeNormal.normalize();
|
||||
|
||||
m_planeDistance = m_pVertices[ 0 ]->m_point.dot( -m_planeNormal );
|
||||
|
||||
// Robust method
|
||||
|
||||
//SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
//SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
|
||||
//SimdVector3 n;
|
||||
|
||||
//n = _v2.cross( _v1 );
|
||||
|
||||
//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 1 ]->m_point;
|
||||
//_v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 1 ]->m_point;
|
||||
|
||||
//n += ( _v1.cross( _v2 ) );
|
||||
|
||||
//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 2 ]->m_point;
|
||||
//_v2 = m_pVertices[ 1 ]->m_point - m_pVertices[ 2 ]->m_point;
|
||||
|
||||
//n += ( _v2.cross( _v1 ) );
|
||||
|
||||
//n /= 3;
|
||||
//n.normalize();
|
||||
|
||||
//SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
|
||||
//SimdScalar d = c.dot( -n );
|
||||
|
||||
//m_robustPlaneNormal = n;
|
||||
//m_robustPlaneDistance = d;
|
||||
|
||||
// Compare results from both methods and check whether they disagree
|
||||
|
||||
//if ( d < 0 )
|
||||
//{
|
||||
// assert( ( m_planeDistance < 0 ) && "He he! Busted!" );
|
||||
//}
|
||||
//else
|
||||
//{
|
||||
// assert( ( m_planeDistance >= 0 ) && "He he! Busted!" );
|
||||
//}
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
void EpaFace::CalcClosestPoint()
|
||||
{
|
||||
const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
|
||||
|
||||
m_v = m_pVertices[ 0 ]->m_point +
|
||||
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant;
|
||||
|
||||
m_vSqrd = m_v.length2();
|
||||
}
|
||||
|
||||
void EpaFace::CalcClosestPointOnA( SimdVector3& closestPointOnA )
|
||||
{
|
||||
const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
|
||||
const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
|
||||
|
||||
closestPointOnA = m_pVertices[ 0 ]->m_wSupportPointOnA +
|
||||
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
|
||||
m_determinant;
|
||||
}
|
||||
|
||||
void EpaFace::CalcClosestPointOnB( SimdVector3& closestPointOnB )
|
||||
{
|
||||
const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
|
||||
const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
|
||||
|
||||
closestPointOnB = m_pVertices[ 0 ]->m_wSupportPointOnB +
|
||||
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
|
||||
m_determinant;
|
||||
}
|
||||
|
||||
bool EpaFace::IsAffinelyDependent() const
|
||||
{
|
||||
return ( m_determinant <= SIMD_EPSILON );
|
||||
}
|
||||
|
||||
bool EpaFace::IsClosestPointInternal() const
|
||||
{
|
||||
return ( ( m_lambdas[ 0 ] >= 0 ) && ( m_lambdas[ 1 ] >= 0 ) && ( ( m_lambdas[ 0 ] + m_lambdas[ 1 ] <= m_determinant ) ) );
|
||||
}
|
||||
|
||||
void EpaFace::CollectVertices( EpaVertex** ppVertices )
|
||||
{
|
||||
assert( m_pHalfEdge && "Invalid half-edge pointer!" );
|
||||
|
||||
int vertexIndex = 0;
|
||||
|
||||
EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge;
|
||||
|
||||
do
|
||||
{
|
||||
assert( ( ( vertexIndex >= 0 ) && ( vertexIndex < 3 ) ) &&
|
||||
"Face is not a triangle!" );
|
||||
|
||||
assert( pCurrentHalfEdge->m_pVertex && "Half-edge has an invalid vertex pointer!" );
|
||||
|
||||
ppVertices[ vertexIndex++ ] = pCurrentHalfEdge->m_pVertex;
|
||||
|
||||
pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
|
||||
|
||||
}
|
||||
while( pCurrentHalfEdge != m_pHalfEdge );
|
||||
}
|
||||
|
||||
//void EpaFace::FixOrder()
|
||||
//{
|
||||
// EpaHalfEdge* pHalfEdges[ 3 ];
|
||||
//
|
||||
// int halfEdgeIndex = 0;
|
||||
//
|
||||
// EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge;
|
||||
//
|
||||
// do
|
||||
// {
|
||||
// assert( ( ( halfEdgeIndex >= 0 ) && ( halfEdgeIndex < 3 ) ) &&
|
||||
// "Face is not a triangle!" );
|
||||
//
|
||||
// pHalfEdges[ halfEdgeIndex++ ] = pCurrentHalfEdge;
|
||||
//
|
||||
// pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
|
||||
// }
|
||||
// while( pCurrentHalfEdge != m_pHalfEdge );
|
||||
//
|
||||
// EpaVertex* pVertices[ 3 ] = { pHalfEdges[ 0 ]->m_pVertex,
|
||||
// pHalfEdges[ 1 ]->m_pVertex,
|
||||
// pHalfEdges[ 2 ]->m_pVertex };
|
||||
//
|
||||
// // Make them run in the opposite direction
|
||||
// pHalfEdges[ 0 ]->m_pNextCCW = pHalfEdges[ 2 ];
|
||||
// pHalfEdges[ 1 ]->m_pNextCCW = pHalfEdges[ 0 ];
|
||||
// pHalfEdges[ 2 ]->m_pNextCCW = pHalfEdges[ 1 ];
|
||||
//
|
||||
// // Make half-edges point to their correct origin vertices
|
||||
//
|
||||
// pHalfEdges[ 1 ]->m_pVertex = pVertices[ 2 ];
|
||||
// pHalfEdges[ 2 ]->m_pVertex = pVertices[ 0 ];
|
||||
// pHalfEdges[ 0 ]->m_pVertex = pVertices[ 1 ];
|
||||
//
|
||||
// // Make vertices point to the correct half-edges
|
||||
//
|
||||
// //pHalfEdges[ 0 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 0 ];
|
||||
// //pHalfEdges[ 1 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 1 ];
|
||||
// //pHalfEdges[ 2 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 2 ];
|
||||
//
|
||||
// // Flip normal and change the sign of plane distance
|
||||
//
|
||||
//#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
// m_planeNormal = -m_planeNormal;
|
||||
// m_planeDistance = -m_planeDistance;
|
||||
//#endif
|
||||
//}
|
||||
|
||||
@@ -1,83 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_FACE_H
|
||||
#define EPA_FACE_H
|
||||
|
||||
class EpaVertex;
|
||||
class EpaHalfEdge;
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
extern SimdScalar PLANE_THICKNESS;
|
||||
#endif
|
||||
|
||||
//! Note : This class is not supposed to be a base class
|
||||
class EpaFace
|
||||
{
|
||||
private :
|
||||
|
||||
//! Prevents copying objects from this class
|
||||
EpaFace( const EpaFace& epaFace );
|
||||
const EpaFace& operator = ( const EpaFace& epaFace );
|
||||
|
||||
public :
|
||||
|
||||
EpaFace();
|
||||
~EpaFace();
|
||||
|
||||
bool Initialize();
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
bool CalculatePlane();
|
||||
#endif
|
||||
void CalcClosestPoint();
|
||||
void CalcClosestPointOnA( SimdVector3& closestPointOnA );
|
||||
void CalcClosestPointOnB( SimdVector3& closestPointOnB );
|
||||
|
||||
bool IsAffinelyDependent() const;
|
||||
bool IsClosestPointInternal() const;
|
||||
|
||||
void CollectVertices( EpaVertex** ppVertices );
|
||||
|
||||
//void FixOrder();
|
||||
|
||||
public :
|
||||
|
||||
EpaHalfEdge* m_pHalfEdge;
|
||||
|
||||
// We keep vertices here so we don't need to call CollectVertices
|
||||
// every time we need them
|
||||
EpaVertex* m_pVertices[ 3 ];
|
||||
|
||||
#ifdef EPA_POLYHEDRON_USE_PLANES
|
||||
SimdVector3 m_planeNormal;
|
||||
SimdScalar m_planeDistance;
|
||||
|
||||
//SimdVector3 m_robustPlaneNormal;
|
||||
//SimdScalar m_robustPlaneDistance;
|
||||
#endif
|
||||
|
||||
SimdVector3 m_v;
|
||||
SimdScalar m_vSqrd;
|
||||
|
||||
SimdScalar m_determinant;
|
||||
SimdScalar m_lambdas[ 2 ];
|
||||
|
||||
bool m_deleted;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,58 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_HALF_EDGE_H
|
||||
#define EPA_HALF_EDGE_H
|
||||
|
||||
class EpaFace;
|
||||
class EpaVertex;
|
||||
|
||||
//! Note : This class is not supposed to be a base class
|
||||
class EpaHalfEdge
|
||||
{
|
||||
private :
|
||||
|
||||
//! Prevents copying objects from this class
|
||||
EpaHalfEdge( const EpaHalfEdge& epaHalfEdge );
|
||||
const EpaHalfEdge& operator = ( const EpaHalfEdge& epaHalfEdge );
|
||||
|
||||
public :
|
||||
|
||||
EpaHalfEdge() : m_pTwin( 0 ), m_pNextCCW( 0 ), m_pFace( 0 ), m_pVertex( 0 )
|
||||
{
|
||||
}
|
||||
|
||||
~EpaHalfEdge()
|
||||
{
|
||||
}
|
||||
|
||||
public :
|
||||
|
||||
//! Twin half-edge link
|
||||
EpaHalfEdge* m_pTwin;
|
||||
|
||||
//! Next half-edge in counter clock-wise ( CCW ) order
|
||||
EpaHalfEdge* m_pNextCCW;
|
||||
|
||||
//! Parent face link
|
||||
EpaFace* m_pFace;
|
||||
|
||||
//! Origin vertex link
|
||||
EpaVertex* m_pVertex;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,202 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#include "SimdScalar.h"
|
||||
#include "SimdVector3.h"
|
||||
#include "SimdPoint3.h"
|
||||
#include "SimdTransform.h"
|
||||
#include "SimdMinMax.h"
|
||||
|
||||
#include <list>
|
||||
|
||||
#include "CollisionShapes/ConvexShape.h"
|
||||
|
||||
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
|
||||
|
||||
#include "NarrowPhaseCollision/EpaCommon.h"
|
||||
|
||||
#include "NarrowPhaseCollision/EpaVertex.h"
|
||||
#include "NarrowPhaseCollision/EpaHalfEdge.h"
|
||||
#include "NarrowPhaseCollision/EpaFace.h"
|
||||
#include "NarrowPhaseCollision/EpaPolyhedron.h"
|
||||
#include "NarrowPhaseCollision/Epa.h"
|
||||
#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
|
||||
#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
|
||||
|
||||
SimdScalar g_GJKMaxRelError = 1e-3f;
|
||||
SimdScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
|
||||
|
||||
bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver,
|
||||
ConvexShape* pConvexA, ConvexShape* pConvexB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB,
|
||||
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
|
||||
class IDebugDraw* debugDraw )
|
||||
{
|
||||
assert( pConvexA && "Convex shape A is invalid!" );
|
||||
assert( pConvexB && "Convex shape B is invalid!" );
|
||||
|
||||
SimdScalar penDepth;
|
||||
|
||||
#ifdef EPA_USE_HYBRID
|
||||
bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
|
||||
wWitnessOnA, wWitnessOnB, penDepth, v );
|
||||
|
||||
if ( needsEPA )
|
||||
{
|
||||
#endif
|
||||
penDepth = EpaPenDepth( simplexSolver, pConvexA, pConvexB,
|
||||
transformA, transformB,
|
||||
wWitnessOnA, wWitnessOnB );
|
||||
assert( ( penDepth > 0 ) && "EPA or Hybrid Technique failed to calculate penetration depth!" );
|
||||
|
||||
#ifdef EPA_USE_HYBRID
|
||||
}
|
||||
#endif
|
||||
|
||||
return ( penDepth > 0 );
|
||||
}
|
||||
|
||||
#ifdef EPA_USE_HYBRID
|
||||
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
|
||||
ConvexShape* pConvexA, ConvexShape* pConvexB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB,
|
||||
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
|
||||
SimdScalar& penDepth, SimdVector3& v )
|
||||
{
|
||||
SimdScalar squaredDistance = SIMD_INFINITY;
|
||||
SimdScalar delta = 0.f;
|
||||
|
||||
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
|
||||
const SimdScalar marginSqrd = margin * margin;
|
||||
|
||||
simplexSolver.reset();
|
||||
|
||||
int nbIterations = 0;
|
||||
|
||||
while ( true )
|
||||
{
|
||||
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
|
||||
|
||||
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
|
||||
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
|
||||
|
||||
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
|
||||
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
|
||||
|
||||
SimdPoint3 pWorld = transformA( pInA );
|
||||
SimdPoint3 qWorld = transformB( qInB );
|
||||
|
||||
SimdVector3 w = pWorld - qWorld;
|
||||
delta = v.dot( w );
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
|
||||
{
|
||||
// Convex shapes do not overlap
|
||||
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
||||
penDepth = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||
if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
|
||||
{
|
||||
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
|
||||
|
||||
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
|
||||
SimdScalar vLength = sqrt( squaredDistance );
|
||||
|
||||
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
|
||||
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
|
||||
|
||||
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
|
||||
|
||||
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
||||
return true;
|
||||
}
|
||||
|
||||
//add current vertex to simplex
|
||||
simplexSolver.addVertex( w, pWorld, qWorld );
|
||||
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if ( !simplexSolver.closest( v ) )
|
||||
{
|
||||
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
|
||||
|
||||
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
|
||||
SimdScalar vLength = sqrt( squaredDistance );
|
||||
|
||||
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
|
||||
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
|
||||
|
||||
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
|
||||
|
||||
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
||||
return true;
|
||||
}
|
||||
|
||||
SimdScalar previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = v.length2();
|
||||
|
||||
//are we getting any closer ?
|
||||
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
|
||||
{
|
||||
simplexSolver.backup_closest( v );
|
||||
squaredDistance = v.length2();
|
||||
|
||||
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
|
||||
|
||||
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
|
||||
SimdScalar vLength = sqrt( squaredDistance );
|
||||
|
||||
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
|
||||
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
|
||||
|
||||
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
|
||||
|
||||
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
||||
return true;
|
||||
}
|
||||
|
||||
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
|
||||
{
|
||||
// Convex Shapes intersect - we need to run EPA
|
||||
// Returning false means that Hybrid couldn't do anything for us
|
||||
// and that we need to run EPA to calculate the pen depth
|
||||
return false;
|
||||
}
|
||||
|
||||
++nbIterations;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver,
|
||||
ConvexShape* pConvexA, ConvexShape* pConvexB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB,
|
||||
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
|
||||
{
|
||||
Epa epa( pConvexA, pConvexB, transformA, transformB );
|
||||
|
||||
if ( !epa.Initialize( simplexSolver ) )
|
||||
{
|
||||
assert( false && "Epa failed to initialize!" );
|
||||
return 0;
|
||||
}
|
||||
|
||||
return epa.CalcPenDepth( wWitnessOnA, wWitnessOnB );
|
||||
}
|
||||
|
||||
@@ -1,56 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_PENETRATION_DEPTH_H
|
||||
#define EPA_PENETRATION_DEPTH_H
|
||||
|
||||
/**
|
||||
* EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
|
||||
* calculate the penetration depth between two convex shapes.
|
||||
*/
|
||||
|
||||
extern SimdScalar g_GJKMaxRelError;
|
||||
extern SimdScalar g_GJKMaxRelErrorSqrd;
|
||||
|
||||
//! Note : This class is not supposed to be a base class
|
||||
class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver
|
||||
{
|
||||
public :
|
||||
|
||||
bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
|
||||
ConvexShape* pConvexA, ConvexShape* pConvexB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB,
|
||||
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
|
||||
class IDebugDraw* debugDraw );
|
||||
|
||||
private :
|
||||
|
||||
#ifdef EPA_USE_HYBRID
|
||||
bool HybridPenDepth( SimplexSolverInterface& simplexSolver,
|
||||
ConvexShape* pConvexA, ConvexShape* pConvexB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB,
|
||||
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
|
||||
SimdScalar& penDepth, SimdVector3& v );
|
||||
#endif
|
||||
|
||||
SimdScalar EpaPenDepth( SimplexSolverInterface& simplexSolver,
|
||||
ConvexShape* pConvexA, ConvexShape* pConvexB,
|
||||
const SimdTransform& transformA, const SimdTransform& transformB,
|
||||
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
|
||||
};
|
||||
|
||||
#endif // EPA_PENETRATION_DEPTH_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,89 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_POLYHEDRON_H
|
||||
#define EPA_POLYHEDRON_H
|
||||
|
||||
class EpaFace;
|
||||
class EpaVertex;
|
||||
|
||||
//! Note : This class is not supposed to be a base class
|
||||
class EpaPolyhedron
|
||||
{
|
||||
private :
|
||||
|
||||
//! Prevents copying objects from this class
|
||||
EpaPolyhedron( const EpaPolyhedron& epaPolyhedron );
|
||||
const EpaPolyhedron& operator = ( const EpaPolyhedron& epaPolyhedron );
|
||||
|
||||
public :
|
||||
|
||||
EpaPolyhedron();
|
||||
~EpaPolyhedron();
|
||||
|
||||
bool Create( SimdPoint3* pInitialPoints,
|
||||
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
|
||||
const int nbInitialPoints );
|
||||
void Destroy();
|
||||
|
||||
EpaFace* CreateFace();
|
||||
EpaHalfEdge* CreateHalfEdge();
|
||||
EpaVertex* CreateVertex( const SimdPoint3& wSupportPoint,
|
||||
const SimdPoint3& wSupportPointOnA,
|
||||
const SimdPoint3& wSupportPointOnB );
|
||||
|
||||
void DeleteFace( EpaFace* pFace );
|
||||
|
||||
void DestroyAllFaces();
|
||||
void DestroyAllHalfEdges();
|
||||
void DestroyAllVertices();
|
||||
|
||||
bool Expand( const SimdPoint3& wSupportPoint,
|
||||
const SimdPoint3& wSupportPointOnA,
|
||||
const SimdPoint3& wSupportPointOnB,
|
||||
EpaFace* pFace, std::list< EpaFace* >& newFaces );
|
||||
|
||||
std::list< EpaFace* >& GetFaces();
|
||||
int GetNbFaces() const;
|
||||
|
||||
private :
|
||||
|
||||
void DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
|
||||
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges );
|
||||
|
||||
void CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,
|
||||
std::list< EpaFace* >& newFaces );
|
||||
EpaFace* CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge,
|
||||
std::list< EpaHalfEdge* >& halfEdgesToLink );
|
||||
|
||||
#ifdef _DEBUG
|
||||
public :
|
||||
//! Please don't remove this method, it will help debugging if some problems arise in the future
|
||||
bool _dbgSaveToFile( const char* pFileName );
|
||||
#endif
|
||||
|
||||
private :
|
||||
|
||||
//! This is the number of valid faces, m_faces list also contain deleted faces
|
||||
int m_nbFaces;
|
||||
|
||||
std::list< EpaFace* > m_faces;
|
||||
std::list< EpaHalfEdge* > m_halfEdges;
|
||||
std::list< EpaVertex* > m_vertices;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
EPA Copyright (c) Ricardo Padrela 2006
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
#ifndef EPA_VERTEX_H
|
||||
#define EPA_VERTEX_H
|
||||
|
||||
class EpaHalfEdge;
|
||||
|
||||
//! Note : This class is not supposed to be a base class
|
||||
class EpaVertex
|
||||
{
|
||||
private :
|
||||
|
||||
//! Prevents copying objects from this class
|
||||
EpaVertex( const EpaVertex& epaVertex );
|
||||
const EpaVertex& operator = ( const EpaVertex& epaVertex );
|
||||
|
||||
public :
|
||||
|
||||
EpaVertex( const SimdPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point )
|
||||
{
|
||||
}
|
||||
|
||||
EpaVertex( const SimdPoint3& point,
|
||||
const SimdPoint3& wSupportPointOnA,
|
||||
const SimdPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
|
||||
m_wSupportPointOnA( wSupportPointOnA ),
|
||||
m_wSupportPointOnB( wSupportPointOnB )
|
||||
{
|
||||
}
|
||||
|
||||
~EpaVertex()
|
||||
{
|
||||
}
|
||||
|
||||
public :
|
||||
|
||||
//! This is not necessary
|
||||
//EpaHalfEdge* m_pHalfEdge;
|
||||
|
||||
SimdPoint3 m_point;
|
||||
|
||||
SimdPoint3 m_wSupportPointOnA;
|
||||
SimdPoint3 m_wSupportPointOnB;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user