From 9105c3af5a4e1b2a2e3c819d615de0158126bc4a Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 29 Jun 2006 20:57:47 +0000 Subject: [PATCH] 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) --- Bullet/BroadphaseCollision/AxisSweep3.cpp | 1 + Bullet/BroadphaseCollision/AxisSweep3.h | 4 +- .../OverlappingPairCache.cpp | 213 ++ .../OverlappingPairCache.h | 85 + .../BroadphaseCollision/SimpleBroadphase.cpp | 176 +- Bullet/BroadphaseCollision/SimpleBroadphase.h | 35 +- .../CollisionDispatch/CollisionDispatcher.cpp | 4 +- ... => SequentialImpulseConstraintSolver.cpp} | 12 +- ....h => SequentialImpulseConstraintSolver.h} | 14 +- BulletDynamics/Dynamics/RigidBody.h | 1 - Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 17 +- Demos/ConcaveDemo/ConcavePhysicsDemo.cpp | 8 +- Demos/ConstraintDemo/ConstraintDemo.cpp | 6 +- Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp | 4 +- .../LinearConvexCastDemo.cpp | 5 +- Demos/Raytracer/Raytracer.cpp | 3 +- .../BU_AlgebraicPolynomialSolver.cpp | 720 +++--- .../BU_AlgebraicPolynomialSolver.h | 90 +- .../AlgebraicCCD}/BU_Collidable.cpp | 50 +- .../AlgebraicCCD}/BU_Collidable.h | 114 +- .../AlgebraicCCD}/BU_CollisionPair.cpp | 1162 +++++----- .../AlgebraicCCD}/BU_CollisionPair.h | 108 +- .../AlgebraicCCD}/BU_EdgeEdge.cpp | 1156 +++++----- .../AlgebraicCCD}/BU_EdgeEdge.h | 152 +- .../AlgebraicCCD}/BU_MotionStateInterface.h | 100 +- .../BU_PolynomialSolverInterface.h | 78 +- .../AlgebraicCCD}/BU_Screwing.cpp | 400 ++-- .../AlgebraicCCD}/BU_Screwing.h | 154 +- .../AlgebraicCCD}/BU_StaticMotionState.h | 182 +- .../AlgebraicCCD}/BU_VertexPoly.cpp | 318 +-- .../AlgebraicCCD}/BU_VertexPoly.h | 86 +- .../EPA}/Epa.cpp | 1120 ++++----- .../NarrowPhaseCollision => Extras/EPA}/Epa.h | 132 +- .../EPA}/EpaCommon.h | 50 +- .../EPA}/EpaFace.cpp | 508 ++--- .../EPA}/EpaFace.h | 166 +- .../EPA}/EpaHalfEdge.h | 116 +- .../EPA}/EpaPenetrationDepthSolver.cpp | 404 ++-- .../EPA}/EpaPenetrationDepthSolver.h | 112 +- .../EPA}/EpaPolyhedron.cpp | 2028 ++++++++--------- .../EPA}/EpaPolyhedron.h | 178 +- .../EPA}/EpaVertex.h | 122 +- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 9 +- .../quickstep}/OdeConstraintSolver.cpp | 533 ++--- .../quickstep}/OdeConstraintSolver.h | 132 +- .../quickstep}/SorLcp.cpp | 1698 +++++++------- .../quickstep}/SorLcp.h | 90 +- msvc/8/libbullet.vcproj | 632 ++--- msvc/8/libbulletccdphysics.vcproj | 254 ++- msvc/8/libbulletdynamics.vcproj | 326 +-- msvc/8/wksbullet.sln | 467 ++-- 51 files changed, 7428 insertions(+), 7107 deletions(-) create mode 100644 Bullet/BroadphaseCollision/OverlappingPairCache.cpp create mode 100644 Bullet/BroadphaseCollision/OverlappingPairCache.h rename BulletDynamics/ConstraintSolver/{SimpleConstraintSolver.cpp => SequentialImpulseConstraintSolver.cpp} (92%) rename BulletDynamics/ConstraintSolver/{SimpleConstraintSolver.h => SequentialImpulseConstraintSolver.h} (81%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_AlgebraicPolynomialSolver.cpp (97%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_AlgebraicPolynomialSolver.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_Collidable.cpp (98%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_Collidable.h (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_CollisionPair.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_CollisionPair.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_EdgeEdge.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_EdgeEdge.h (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_MotionStateInterface.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_PolynomialSolverInterface.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_Screwing.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_Screwing.h (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_StaticMotionState.h (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_VertexPoly.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/AlgebraicCCD}/BU_VertexPoly.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/Epa.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/Epa.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaCommon.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaFace.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaFace.h (96%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaHalfEdge.h (96%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaPenetrationDepthSolver.cpp (97%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaPenetrationDepthSolver.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaPolyhedron.cpp (96%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaPolyhedron.h (97%) rename {Bullet/NarrowPhaseCollision => Extras/EPA}/EpaVertex.h (96%) rename {BulletDynamics/ConstraintSolver => Extras/quickstep}/OdeConstraintSolver.cpp (91%) rename {BulletDynamics/ConstraintSolver => Extras/quickstep}/OdeConstraintSolver.h (97%) rename {BulletDynamics/ConstraintSolver => Extras/quickstep}/SorLcp.cpp (96%) rename {BulletDynamics/ConstraintSolver => Extras/quickstep}/SorLcp.h (97%) diff --git a/Bullet/BroadphaseCollision/AxisSweep3.cpp b/Bullet/BroadphaseCollision/AxisSweep3.cpp index 71b89053f..235afa1f9 100644 --- a/Bullet/BroadphaseCollision/AxisSweep3.cpp +++ b/Bullet/BroadphaseCollision/AxisSweep3.cpp @@ -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()); diff --git a/Bullet/BroadphaseCollision/AxisSweep3.h b/Bullet/BroadphaseCollision/AxisSweep3.h index b29534724..0a4a75d52 100644 --- a/Bullet/BroadphaseCollision/AxisSweep3.h +++ b/Bullet/BroadphaseCollision/AxisSweep3.h @@ -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: diff --git a/Bullet/BroadphaseCollision/OverlappingPairCache.cpp b/Bullet/BroadphaseCollision/OverlappingPairCache.cpp new file mode 100644 index 000000000..9f6c0f590 --- /dev/null +++ b/Bullet/BroadphaseCollision/OverlappingPairCache.cpp @@ -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= 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=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= 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; + +} diff --git a/Bullet/BroadphaseCollision/OverlappingPairCache.h b/Bullet/BroadphaseCollision/OverlappingPairCache.h new file mode 100644 index 000000000..8675437e4 --- /dev/null +++ b/Bullet/BroadphaseCollision/OverlappingPairCache.h @@ -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 \ No newline at end of file diff --git a/Bullet/BroadphaseCollision/SimpleBroadphase.cpp b/Bullet/BroadphaseCollision/SimpleBroadphase.cpp index 5025309b3..5858612fc 100644 --- a/Bullet/BroadphaseCollision/SimpleBroadphase.cpp +++ b/Bullet/BroadphaseCollision/SimpleBroadphase.cpp @@ -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=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= 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= 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; - -} - diff --git a/Bullet/BroadphaseCollision/SimpleBroadphase.h b/Bullet/BroadphaseCollision/SimpleBroadphase.h index be891f403..e67c71056 100644 --- a/Bullet/BroadphaseCollision/SimpleBroadphase.h +++ b/Bullet/BroadphaseCollision/SimpleBroadphase.h @@ -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; - } + + + }; diff --git a/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/Bullet/CollisionDispatch/CollisionDispatcher.cpp index 5749db20a..8bac20f47 100644 --- a/Bullet/CollisionDispatch/CollisionDispatcher.cpp +++ b/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -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; diff --git a/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp similarity index 92% rename from BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp rename to BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp index fe59a4068..c6c5e13ad 100644 --- a/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp @@ -14,7 +14,7 @@ subject to the following restrictions: */ -#include "SimpleConstraintSolver.h" +#include "SequentialImpulseConstraintSolver.h" #include "NarrowPhaseCollision/PersistentManifold.h" #include "Dynamics/RigidBody.h" #include "ContactConstraint.h" @@ -45,14 +45,14 @@ bool MyContactDestroyedCallback(void* userPersistentData) } -SimpleConstraintSolver::SimpleConstraintSolver() +SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver() { gContactCallback = &MyContactDestroyedCallback; } -/// SimpleConstraintSolver Sequentially applies impulses -float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) +/// SequentialImpulseConstraintSolver Sequentially applies impulses +float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) { ContactSolverInfo info = infoGlobal; @@ -113,7 +113,7 @@ SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution) -float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) +float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) { RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0(); @@ -294,7 +294,7 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta return maxImpulse; } -float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) +float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) { RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0(); RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1(); diff --git a/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h similarity index 81% rename from BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h rename to BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h index 99b77623b..598a4cfa9 100644 --- a/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h +++ b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h @@ -13,17 +13,17 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SIMPLE_CONSTRAINT_SOLVER_H -#define SIMPLE_CONSTRAINT_SOLVER_H +#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H #include "ConstraintSolver.h" class IDebugDraw; -/// SimpleConstraintSolver uses a Propagation Method and Sequentially applies impulses +/// SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses /// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com /// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) /// Applies impulses for combined restitution and penetration recovery and to simulate friction -class SimpleConstraintSolver : public ConstraintSolver +class SequentialImpulseConstraintSolver : public ConstraintSolver { float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); @@ -31,13 +31,13 @@ class SimpleConstraintSolver : public ConstraintSolver public: - SimpleConstraintSolver(); + SequentialImpulseConstraintSolver(); - virtual ~SimpleConstraintSolver() {} + virtual ~SequentialImpulseConstraintSolver() {} virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0); }; -#endif //SIMPLE_CONSTRAINT_SOLVER_H +#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/BulletDynamics/Dynamics/RigidBody.h b/BulletDynamics/Dynamics/RigidBody.h index 1a259faf6..41a65586c 100644 --- a/BulletDynamics/Dynamics/RigidBody.h +++ b/BulletDynamics/Dynamics/RigidBody.h @@ -31,7 +31,6 @@ typedef SimdScalar dMatrix3[4*3]; extern float gLinearAirDamping; extern bool gUseEpa; -#define MAX_RIGIDBODIES 8192 /// RigidBody class for RigidBody Dynamics diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index cd402f04b..cef4dbd3e 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -69,14 +69,17 @@ extern float eye[3]; extern int glutScreenWidth; extern int glutScreenHeight; +const int maxProxies = 32766; +const int maxOverlap = 65535; + #ifdef _DEBUG -const int numObjects = 22; +const int numObjects = 5000; #else -const int numObjects = 120; +const int numObjects = 2000; #endif -const int maxNumObjects = 450; +const int maxNumObjects = 32760; MyMotionState ms[maxNumObjects]; CcdPhysicsController* physObjects[maxNumObjects] = {0,0,0,0}; @@ -127,11 +130,11 @@ int main(int argc,char** argv) CollisionDispatcher* dispatcher = new CollisionDispatcher(); - SimdVector3 worldAabbMin(-10000,-10000,-10000); - SimdVector3 worldAabbMax(10000,10000,10000); + SimdVector3 worldAabbMin(-30000,-30000,-30000); + SimdVector3 worldAabbMax(30000,30000,30000); - BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); - //BroadphaseInterface* broadphase = new SimpleBroadphase(); + //BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies,maxOverlap); + BroadphaseInterface* broadphase = new SimpleBroadphase(maxProxies,maxOverlap); physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); physicsEnvironmentPtr->setDeactivationTime(2.f); diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp index efb09b9a6..16309d6f0 100644 --- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp +++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp @@ -22,8 +22,8 @@ subject to the following restrictions: #include "Dynamics/RigidBody.h" #include "BroadphaseCollision/AxisSweep3.h" -#include "ConstraintSolver/SimpleConstraintSolver.h" -#include "ConstraintSolver/OdeConstraintSolver.h" +#include "ConstraintSolver/SequentialImpulseConstraintSolver.h" +//#include "ConstraintSolver/OdeConstraintSolver.h" #include "CollisionDispatch/CollisionDispatcher.h" #include "BroadphaseCollision/SimpleBroadphase.h" #include "CollisionShapes/TriangleMeshShape.h" @@ -216,8 +216,8 @@ int main(int argc,char** argv) // GLDebugDrawer debugDrawer; - //ConstraintSolver* solver = new SimpleConstraintSolver; - ConstraintSolver* solver = new OdeConstraintSolver; + ConstraintSolver* solver = new SequentialImpulseConstraintSolver; + //ConstraintSolver* solver = new OdeConstraintSolver; CollisionDispatcher* dispatcher = new CollisionDispatcher(); diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index cd2c2ae37..44be05bf5 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -22,8 +22,8 @@ subject to the following restrictions: #include "CollisionShapes/EmptyShape.h" #include "Dynamics/RigidBody.h" -#include "ConstraintSolver/SimpleConstraintSolver.h" -#include "ConstraintSolver/OdeConstraintSolver.h" +#include "ConstraintSolver/SequentialImpulseConstraintSolver.h" +//#include "ConstraintSolver/OdeConstraintSolver.h" #include "CollisionDispatch/CollisionDispatcher.h" #include "BroadphaseCollision/SimpleBroadphase.h" #include "IDebugDraw.h" @@ -72,7 +72,7 @@ int main(int argc,char** argv) { - ConstraintSolver* solver = new SimpleConstraintSolver; + ConstraintSolver* solver = new SequentialImpulseConstraintSolver; //ConstraintSolver* solver = new OdeConstraintSolver; CollisionDispatcher* dispatcher = new CollisionDispatcher(); diff --git a/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp b/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp index ef48172d6..4e951527f 100644 --- a/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp +++ b/Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp @@ -48,9 +48,11 @@ subject to the following restrictions: #include "NarrowPhaseCollision/Epa.h" #include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" #include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" +EpaPenetrationDepthSolver epaPenDepthSolver; + SimplexSolverInterface simplexSolver; -EpaPenetrationDepthSolver epaPenDepthSolver; + int screenWidth = 640.f; int screenHeight = 480.f; diff --git a/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp b/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp index 23369020f..8691653ae 100644 --- a/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp +++ b/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp @@ -29,7 +29,10 @@ #include "NarrowPhaseCollision/GjkConvexCast.h" #include "NarrowPhaseCollision/ContinuousConvexCollision.h" #include "NarrowPhaseCollision/SubSimplexConvexCast.h" + +#ifdef USE_ALGEBRAIC_CCD #include "NarrowPhaseCollision/BU_CollisionPair.h" +#endif //USE_ALGEBRAIC_CCD #include "CollisionShapes/SphereShape.h" @@ -191,7 +194,7 @@ void clientDisplay(void) { GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver); //BU_CollisionPair (algebraic version) is currently broken, will look into this - BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]); + //BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]); SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver); gGjkSimplexSolver.reset(); diff --git a/Demos/Raytracer/Raytracer.cpp b/Demos/Raytracer/Raytracer.cpp index d63e76f93..c6578d7e7 100644 --- a/Demos/Raytracer/Raytracer.cpp +++ b/Demos/Raytracer/Raytracer.cpp @@ -30,8 +30,9 @@ Very basic raytracer, rendering into a texture. #include "NarrowPhaseCollision/SubSimplexConvexCast.h" #include "NarrowPhaseCollision/GjkConvexCast.h" #include "NarrowPhaseCollision/ContinuousConvexCollision.h" +#ifdef USE_ALGEBRAIC_CCD #include "NarrowPhaseCollision/BU_CollisionPair.h" - +#endif //USE_ALGEBRAIC_CCD #include "CollisionShapes/SphereShape.h" diff --git a/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp b/Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.cpp similarity index 97% rename from Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp rename to Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.cpp index e318c25da..f20bb7e32 100644 --- a/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp +++ b/Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.cpp @@ -1,360 +1,360 @@ -/* -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 -#include - -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); - } -} - +/* +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 +#include + +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); + } +} + diff --git a/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h b/Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h similarity index 97% rename from Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h rename to Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h index 2d2fe5fc1..85f88ac8a 100644 --- a/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h +++ b/Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h @@ -1,45 +1,45 @@ -/* -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 +/* +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 diff --git a/Bullet/NarrowPhaseCollision/BU_Collidable.cpp b/Extras/AlgebraicCCD/BU_Collidable.cpp similarity index 98% rename from Bullet/NarrowPhaseCollision/BU_Collidable.cpp rename to Extras/AlgebraicCCD/BU_Collidable.cpp index bb06e7edb..4adb08506 100644 --- a/Bullet/NarrowPhaseCollision/BU_Collidable.cpp +++ b/Extras/AlgebraicCCD/BU_Collidable.cpp @@ -1,25 +1,25 @@ -/* -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 -#include "BU_MotionStateInterface.h" - -BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer ) -:m_motionState(motion),m_shape(shape),m_userPointer(userPointer) -{ -} +/* +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 +#include "BU_MotionStateInterface.h" + +BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer ) +:m_motionState(motion),m_shape(shape),m_userPointer(userPointer) +{ +} diff --git a/Bullet/NarrowPhaseCollision/BU_Collidable.h b/Extras/AlgebraicCCD/BU_Collidable.h similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_Collidable.h rename to Extras/AlgebraicCCD/BU_Collidable.h index 0a63d32f6..9ff6229e3 100644 --- a/Bullet/NarrowPhaseCollision/BU_Collidable.h +++ b/Extras/AlgebraicCCD/BU_Collidable.h @@ -1,57 +1,57 @@ -/* -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 - -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 +/* +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 + +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 diff --git a/Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp b/Extras/AlgebraicCCD/BU_CollisionPair.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp rename to Extras/AlgebraicCCD/BU_CollisionPair.cpp index ebd80cc31..739d98e62 100644 --- a/Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp +++ b/Extras/AlgebraicCCD/BU_CollisionPair.cpp @@ -1,581 +1,581 @@ -/* -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 -#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;vGetVertex(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;vGetVertex(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;vGetVertex(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;vGetVertex(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 GetNumVertices(); - for (int v=0;vGetVertex(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; -} - - +/* +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 +#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;vGetVertex(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;vGetVertex(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;vGetVertex(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;vGetVertex(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 GetNumVertices(); + for (int v=0;vGetVertex(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; +} + + diff --git a/Bullet/NarrowPhaseCollision/BU_CollisionPair.h b/Extras/AlgebraicCCD/BU_CollisionPair.h similarity index 97% rename from Bullet/NarrowPhaseCollision/BU_CollisionPair.h rename to Extras/AlgebraicCCD/BU_CollisionPair.h index 8dcea3647..939cf8a40 100644 --- a/Bullet/NarrowPhaseCollision/BU_CollisionPair.h +++ b/Extras/AlgebraicCCD/BU_CollisionPair.h @@ -1,54 +1,54 @@ -/* -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 -#include - - -#include - -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 +/* +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 +#include + + +#include + +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 diff --git a/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp b/Extras/AlgebraicCCD/BU_EdgeEdge.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp rename to Extras/AlgebraicCCD/BU_EdgeEdge.cpp index 75103c0f7..c4aee6726 100644 --- a/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp +++ b/Extras/AlgebraicCCD/BU_EdgeEdge.cpp @@ -1,578 +1,578 @@ -/* -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 -#include - -//#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=0.f && t= -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; -} +/* +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 +#include + +//#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=0.f && t= -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; +} diff --git a/Bullet/NarrowPhaseCollision/BU_EdgeEdge.h b/Extras/AlgebraicCCD/BU_EdgeEdge.h similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_EdgeEdge.h rename to Extras/AlgebraicCCD/BU_EdgeEdge.h index 4823f1dee..6455a8f1c 100644 --- a/Bullet/NarrowPhaseCollision/BU_EdgeEdge.h +++ b/Extras/AlgebraicCCD/BU_EdgeEdge.h @@ -1,76 +1,76 @@ -/* -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 -#include -#include - -//class BUM_Point2; - -#include - -///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 +/* +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 +#include +#include + +//class BUM_Point2; + +#include + +///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 diff --git a/Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h b/Extras/AlgebraicCCD/BU_MotionStateInterface.h similarity index 97% rename from Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h rename to Extras/AlgebraicCCD/BU_MotionStateInterface.h index d06a8fab0..96d021e78 100644 --- a/Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h +++ b/Extras/AlgebraicCCD/BU_MotionStateInterface.h @@ -1,50 +1,50 @@ -/* -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 -#include -#include - -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 +/* +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 +#include +#include + +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 diff --git a/Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h b/Extras/AlgebraicCCD/BU_PolynomialSolverInterface.h similarity index 97% rename from Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h rename to Extras/AlgebraicCCD/BU_PolynomialSolverInterface.h index 15d67faee..044636581 100644 --- a/Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h +++ b/Extras/AlgebraicCCD/BU_PolynomialSolverInterface.h @@ -1,39 +1,39 @@ -/* -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 -// -//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 +/* +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 +// +//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 diff --git a/Bullet/NarrowPhaseCollision/BU_Screwing.cpp b/Extras/AlgebraicCCD/BU_Screwing.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_Screwing.cpp rename to Extras/AlgebraicCCD/BU_Screwing.cpp index 997fcc3a9..8cb982a20 100644 --- a/Bullet/NarrowPhaseCollision/BU_Screwing.cpp +++ b/Extras/AlgebraicCCD/BU_Screwing.cpp @@ -1,200 +1,200 @@ - -/* -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)= 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; -} - + +/* +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)= 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; +} + diff --git a/Bullet/NarrowPhaseCollision/BU_Screwing.h b/Extras/AlgebraicCCD/BU_Screwing.h similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_Screwing.h rename to Extras/AlgebraicCCD/BU_Screwing.h index 17603cec0..a7f95d7db 100644 --- a/Bullet/NarrowPhaseCollision/BU_Screwing.h +++ b/Extras/AlgebraicCCD/BU_Screwing.h @@ -1,77 +1,77 @@ -/* -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 -#include -#include - - -#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 +/* +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 +#include +#include + + +#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 diff --git a/Bullet/NarrowPhaseCollision/BU_StaticMotionState.h b/Extras/AlgebraicCCD/BU_StaticMotionState.h similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_StaticMotionState.h rename to Extras/AlgebraicCCD/BU_StaticMotionState.h index 4202b3dde..e533525c4 100644 --- a/Bullet/NarrowPhaseCollision/BU_StaticMotionState.h +++ b/Extras/AlgebraicCCD/BU_StaticMotionState.h @@ -1,91 +1,91 @@ -/* -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 - -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 +/* +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 + +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 diff --git a/Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp b/Extras/AlgebraicCCD/BU_VertexPoly.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp rename to Extras/AlgebraicCCD/BU_VertexPoly.cpp index 71de3a284..fe5504041 100644 --- a/Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp +++ b/Extras/AlgebraicCCD/BU_VertexPoly.cpp @@ -1,159 +1,159 @@ -/* -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 -#include -#include - -#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=0 && t +#include +#include + +#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=0 && t -#include -#include - -///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 +/* +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 +#include +#include + +///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 diff --git a/Bullet/NarrowPhaseCollision/Epa.cpp b/Extras/EPA/Epa.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/Epa.cpp rename to Extras/EPA/Epa.cpp index 190e4e8ce..fff1069b0 100644 --- a/Bullet/NarrowPhaseCollision/Epa.cpp +++ b/Extras/EPA/Epa.cpp @@ -1,560 +1,560 @@ -/* -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 -#include -#include - -#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 ); -} +/* +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 +#include +#include + +#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 ); +} diff --git a/Bullet/NarrowPhaseCollision/Epa.h b/Extras/EPA/Epa.h similarity index 97% rename from Bullet/NarrowPhaseCollision/Epa.h rename to Extras/EPA/Epa.h index 43ecefaa2..35db7fee1 100644 --- a/Bullet/NarrowPhaseCollision/Epa.h +++ b/Extras/EPA/Epa.h @@ -1,66 +1,66 @@ -/* -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 - +/* +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 + diff --git a/Bullet/NarrowPhaseCollision/EpaCommon.h b/Extras/EPA/EpaCommon.h similarity index 97% rename from Bullet/NarrowPhaseCollision/EpaCommon.h rename to Extras/EPA/EpaCommon.h index 9f0681aee..d566c6ade 100644 --- a/Bullet/NarrowPhaseCollision/EpaCommon.h +++ b/Extras/EPA/EpaCommon.h @@ -1,25 +1,25 @@ -/* -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 - +/* +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 + diff --git a/Bullet/NarrowPhaseCollision/EpaFace.cpp b/Extras/EPA/EpaFace.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/EpaFace.cpp rename to Extras/EPA/EpaFace.cpp index 1e4ccf3fe..b87d89ced 100644 --- a/Bullet/NarrowPhaseCollision/EpaFace.cpp +++ b/Extras/EPA/EpaFace.cpp @@ -1,254 +1,254 @@ -/* -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 -//} - +/* +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 +//} + diff --git a/Bullet/NarrowPhaseCollision/EpaFace.h b/Extras/EPA/EpaFace.h similarity index 96% rename from Bullet/NarrowPhaseCollision/EpaFace.h rename to Extras/EPA/EpaFace.h index 5c2ffbde3..42907864c 100644 --- a/Bullet/NarrowPhaseCollision/EpaFace.h +++ b/Extras/EPA/EpaFace.h @@ -1,83 +1,83 @@ -/* -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 - +/* +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 + diff --git a/Bullet/NarrowPhaseCollision/EpaHalfEdge.h b/Extras/EPA/EpaHalfEdge.h similarity index 96% rename from Bullet/NarrowPhaseCollision/EpaHalfEdge.h rename to Extras/EPA/EpaHalfEdge.h index db38356c2..56ba156bc 100644 --- a/Bullet/NarrowPhaseCollision/EpaHalfEdge.h +++ b/Extras/EPA/EpaHalfEdge.h @@ -1,58 +1,58 @@ -/* -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 - +/* +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 + diff --git a/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp b/Extras/EPA/EpaPenetrationDepthSolver.cpp similarity index 97% rename from Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp rename to Extras/EPA/EpaPenetrationDepthSolver.cpp index b6fc8737e..1837fd5d6 100644 --- a/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp +++ b/Extras/EPA/EpaPenetrationDepthSolver.cpp @@ -1,202 +1,202 @@ -/* -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 - -#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 ); -} - +/* +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 + +#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 ); +} + diff --git a/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h b/Extras/EPA/EpaPenetrationDepthSolver.h similarity index 97% rename from Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h rename to Extras/EPA/EpaPenetrationDepthSolver.h index 8c06048ed..ee772728e 100644 --- a/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h +++ b/Extras/EPA/EpaPenetrationDepthSolver.h @@ -1,56 +1,56 @@ -/* -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 - +/* +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 + diff --git a/Bullet/NarrowPhaseCollision/EpaPolyhedron.cpp b/Extras/EPA/EpaPolyhedron.cpp similarity index 96% rename from Bullet/NarrowPhaseCollision/EpaPolyhedron.cpp rename to Extras/EPA/EpaPolyhedron.cpp index 99a0857c7..413dd3735 100644 --- a/Bullet/NarrowPhaseCollision/EpaPolyhedron.cpp +++ b/Extras/EPA/EpaPolyhedron.cpp @@ -1,1014 +1,1014 @@ -/* -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 "Memory2.h" - -#include -#ifdef _DEBUG -#include -#endif - - -#include "NarrowPhaseCollision/EpaCommon.h" - -#include "NarrowPhaseCollision/EpaVertex.h" -#include "NarrowPhaseCollision/EpaHalfEdge.h" -#include "NarrowPhaseCollision/EpaFace.h" -#include "NarrowPhaseCollision/EpaPolyhedron.h" - - -EpaPolyhedron::EpaPolyhedron() : m_nbFaces( 0 ) -{ -} - -EpaPolyhedron::~EpaPolyhedron() -{ - Destroy(); -} - -bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints, - SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, - const int nbInitialPoints ) -{ -#ifndef EPA_POLYHEDRON_USE_PLANES - assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" ); -#endif - - if ( nbInitialPoints < 4 ) - { - // Insufficient nb of points - return false; - } - - //////////////////////////////////////////////////////////////////////////////// - -#ifdef EPA_POLYHEDRON_USE_PLANES - int nbDiffCoords[ 3 ] = { 0, 0, 0 }; - - bool* pDiffCoords = new bool[ 3 * nbInitialPoints ]; - - - int i; - for (i=0;i nbDiffCoords[ i + 1 ] ) - { - int tmp = nbDiffCoords[ i ]; - nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ]; - nbDiffCoords[ i + 1 ] = tmp; - - tmp = axisOrderIndices[ i ]; - axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ]; - axisOrderIndices[ i + 1 ] = tmp; - } - } - - int nbSuccessfullAxis = 0; - - // The axes with less different coordinates choose first - int minsIndices[ 3 ] = { -1, -1, -1 }; - int maxsIndices[ 3 ] = { -1, -1, -1 }; - - int finalPointsIndex = 0; - - for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis ) - { - int axisIndex = axisOrderIndices[ axis ]; - - SimdScalar axisMin = SIMD_INFINITY; - SimdScalar axisMax = -SIMD_INFINITY; - - for ( int i = 0; i < 4; ++i ) - { - // Among the diff coords pick the min and max coords - - if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] ) - { - if ( pInitialPoints[ i ][ axisIndex ] < axisMin ) - { - axisMin = pInitialPoints[ i ][ axisIndex ]; - minsIndices[ axisIndex ] = i; - } - - if ( pInitialPoints[ i ][ axisIndex ] > axisMax ) - { - axisMax = pInitialPoints[ i ][ axisIndex ]; - maxsIndices[ axisIndex ] = i; - } - } - } - - //assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) && - // "min and max have the same index!" ); - - if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) && - ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) ) - { - ++nbSuccessfullAxis; - - finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ]; - finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ]; - - // Make the choosen points to be impossible for other axes to choose - - //assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); - //assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); - - for ( int i = 0; i < 3; ++i ) - { - pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false; - pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false; - } - } - } - - if ( nbSuccessfullAxis <= 1 ) - { - // Degenerate input ? - assert( false && "nbSuccessfullAxis must be greater than 1!" ); - return false; - } - - delete[] pDiffCoords; -#endif - - ////////////////////////////////////////////////////////////////////////// - -#ifdef EPA_POLYHEDRON_USE_PLANES - SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; - SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; -#else - SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ]; - SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ]; -#endif - - SimdVector3 planeNormal = v1.cross( v0 ); - planeNormal.normalize(); - -#ifdef EPA_POLYHEDRON_USE_PLANES - SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal ); -#else - SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal ); -#endif - -#ifdef EPA_POLYHEDRON_USE_PLANES - assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && - "Point should be on plane!" ); - assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && - "Point should be on plane!" ); - assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && - "Point should be on plane!" ); -#endif - -#ifndef EPA_POLYHEDRON_USE_PLANES - { - if ( planeDistance > 0 ) - { - SimdVector3 tmp = pInitialPoints[ 1 ]; - pInitialPoints[ 1 ] = pInitialPoints[ 2 ]; - pInitialPoints[ 2 ] = tmp; - - tmp = pSupportPointsOnA[ 1 ]; - pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ]; - pSupportPointsOnA[ 2 ] = tmp; - - tmp = pSupportPointsOnB[ 1 ]; - pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ]; - pSupportPointsOnB[ 2 ] = tmp; - } - } - - EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] ); - EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] ); - EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] ); - EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] ); -#else - finalPointsIndices[ 3 ] = -1; - - SimdScalar absMaxDist = -SIMD_INFINITY; - SimdScalar maxDist; - - for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex ) - { - SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance; - SimdScalar absDist = abs( dist ); - - if ( ( absDist > absMaxDist ) && - !SimdEqual( dist, PLANE_THICKNESS ) ) - { - absMaxDist = absDist; - maxDist = dist; - finalPointsIndices[ 3 ] = pointIndex; - } - } - - if ( finalPointsIndices[ 3 ] == -1 ) - { - Destroy(); - return false; - } - - if ( maxDist > PLANE_THICKNESS ) - { - // Can swap indices only - - SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ]; - pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ]; - pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp; - - tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ]; - pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ]; - pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp; - - tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ]; - pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ]; - pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp; - } - - EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] ); - EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] ); - EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] ); - EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] ); -#endif - - EpaFace* pFaceA = CreateFace(); - EpaFace* pFaceB = CreateFace(); - EpaFace* pFaceC = CreateFace(); - EpaFace* pFaceD = CreateFace(); - - EpaHalfEdge* pFaceAHalfEdges[ 3 ]; - EpaHalfEdge* pFaceCHalfEdges[ 3 ]; - EpaHalfEdge* pFaceBHalfEdges[ 3 ]; - EpaHalfEdge* pFaceDHalfEdges[ 3 ]; - - pFaceAHalfEdges[ 0 ] = CreateHalfEdge(); - pFaceAHalfEdges[ 1 ] = CreateHalfEdge(); - pFaceAHalfEdges[ 2 ] = CreateHalfEdge(); - - pFaceBHalfEdges[ 0 ] = CreateHalfEdge(); - pFaceBHalfEdges[ 1 ] = CreateHalfEdge(); - pFaceBHalfEdges[ 2 ] = CreateHalfEdge(); - - pFaceCHalfEdges[ 0 ] = CreateHalfEdge(); - pFaceCHalfEdges[ 1 ] = CreateHalfEdge(); - pFaceCHalfEdges[ 2 ] = CreateHalfEdge(); - - pFaceDHalfEdges[ 0 ] = CreateHalfEdge(); - pFaceDHalfEdges[ 1 ] = CreateHalfEdge(); - pFaceDHalfEdges[ 2 ] = CreateHalfEdge(); - - pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; - pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ]; - pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ]; - pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ]; - - pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ]; - pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ]; - pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ]; - - pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ]; - pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ]; - pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ]; - - pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ]; - pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ]; - pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ]; - - pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ]; - pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ]; - pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ]; - - - pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA; - pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA; - pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA; - - pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB; - pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB; - pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB; - - pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC; - pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC; - pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC; - - pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD; - pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD; - pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD; - - - pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA; - pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB; - pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC; - - pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB; - pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD; - pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC; - - pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD; - pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA; - pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC; - - pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB; - pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA; - pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD; - - //pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; - //pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ]; - //pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ]; - //pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ]; - - pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ]; - pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ]; - pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ]; - - pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ]; - pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ]; - pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ]; - - pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ]; - pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ]; - pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ]; - - pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ]; - pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ]; - pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ]; - - if ( !pFaceA->Initialize() || !pFaceB->Initialize() || - !pFaceC->Initialize() || !pFaceD->Initialize() ) - { - assert( false && "One initial face failed to initialize!" ); - return false; - } - -#ifdef EPA_POLYHEDRON_USE_PLANES - if ( nbInitialPoints > 4 ) - { - for ( int i = 0; i < nbInitialPoints; ++i ) - { - if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) && - ( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) ) - { - std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); - - while ( facesItr != m_faces.end() ) - { - EpaFace* pFace = *facesItr; - - SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance; - - if ( dist > PLANE_THICKNESS ) - { - std::list< EpaFace* > newFaces; - - bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ], - pFace, newFaces ); - - if ( !expandOk ) - { - // One or more new faces are affinely dependent - return false; - } - - assert( !newFaces.empty() && "Polyhedron should have expanded!" ); - - break; - } - - ++facesItr; - } - } - } - } -#endif - - return true; -} - -void EpaPolyhedron::Destroy() -{ - DestroyAllVertices(); - - DestroyAllHalfEdges(); - - DestroyAllFaces(); -} - -EpaFace* EpaPolyhedron::CreateFace() -{ - EpaFace* pNewFace = new EpaFace(); - assert( pNewFace && "Failed to allocate memory for a new EpaFace!" ); - - m_faces.push_back( pNewFace ); - - ++m_nbFaces; - - return pNewFace; -} - -EpaHalfEdge* EpaPolyhedron::CreateHalfEdge() -{ - EpaHalfEdge* pNewHalfEdge = new EpaHalfEdge(); - assert( pNewHalfEdge && "Failed to allocate memory for a new EpaHalfEdge!" ); - - m_halfEdges.push_back( pNewHalfEdge ); - - return pNewHalfEdge; -} - -EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint, - const SimdPoint3& wSupportPointOnA, - const SimdPoint3& wSupportPointOnB ) -{ - EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); - assert( pNewVertex && "Failed to allocate memory for a new EpaVertex!" ); - - m_vertices.push_back( pNewVertex ); - - return pNewVertex; -} - -void EpaPolyhedron::DeleteFace( EpaFace* pFace ) -{ - pFace->m_deleted = true; - --m_nbFaces; -} - -void EpaPolyhedron::DestroyAllFaces() -{ - while ( !m_faces.empty() ) - { - EpaFace* pFace = m_faces.front(); - - delete pFace; - - m_faces.pop_front(); - } - - m_nbFaces = 0; -} - -void EpaPolyhedron::DestroyAllHalfEdges() -{ - while ( !m_halfEdges.empty() ) - { - EpaHalfEdge* pHalfEdge = m_halfEdges.front(); - - delete pHalfEdge; - - m_halfEdges.pop_front(); - } -} - -void EpaPolyhedron::DestroyAllVertices() -{ - while ( !m_vertices.empty() ) - { - EpaVertex* pVertex = m_vertices.front(); - - delete pVertex; - - m_vertices.pop_front(); - } -} - -bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint, - const SimdPoint3& wSupportPointOnA, - const SimdPoint3& wSupportPointOnB, - EpaFace* pFace, std::list< EpaFace* >& newFaces ) -{ - assert( !pFace->m_deleted && "Face is already deleted!" ); - assert( newFaces.empty() && "NewFaces list must be empty!" ); - - assert( !pFace->m_deleted && "Cannot expand deleted face!" ); - - // wSupportPoint must be front of face's plane used to do the expansion - -#ifdef EPA_POLYHEDRON_USE_PLANES - SimdScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance; - if ( dist <= PLANE_THICKNESS ) - { - return false; - } -#endif - - std::list< EpaHalfEdge* > coneBaseEdges; - DeleteVisibleFaces( wSupportPoint, pFace, coneBaseEdges ); - - assert( ( coneBaseEdges.size() >= 3 ) && "Cone base must have at least 3 edges!" ); - - EpaVertex* pConeAppex = CreateVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); - assert( pConeAppex && "Failed to create vertex!" ); - - CreateCone( pConeAppex, coneBaseEdges, newFaces ); - - // Initialize new faces - - std::list< EpaFace* >::iterator newFacesItr( newFaces.begin() ); - - while ( newFacesItr != newFaces.end() ) - { - EpaFace* pNewFace = *newFacesItr; - - if ( !pNewFace->Initialize() ) - { - return false; - } - - ++newFacesItr; - } - - return true; -} - -std::list< EpaFace* >& EpaPolyhedron::GetFaces() -{ - return m_faces; -} - -int EpaPolyhedron::GetNbFaces() const -{ - return m_faces.size(); -} - -void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, - std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ) -{ - assert( !pFace->m_deleted && "Face is already deleted!" ); - - DeleteFace( pFace ); - - EpaHalfEdge* pCurrentHalfEdge = pFace->m_pHalfEdge; - - do - { - assert( pCurrentHalfEdge->m_pTwin && "Half-edge without a twin!" ); - - EpaFace* pAdjacentFace = pCurrentHalfEdge->m_pTwin->m_pFace; - assert( pAdjacentFace && "Invalid adjacent face!" ); - - if ( !pAdjacentFace->m_deleted ) - { -#ifdef EPA_POLYHEDRON_USE_PLANES - assert( ( pAdjacentFace->m_planeNormal.length2() > 0 ) && "Invalid plane!" ); - - SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) + - pAdjacentFace->m_planeDistance; - - if ( pointDist > PLANE_THICKNESS ) -#else - SimdScalar dot = pAdjacentFace->m_v.dot( point ); - if ( dot >= pAdjacentFace->m_vSqrd ) -#endif - { - DeleteVisibleFaces( point, pAdjacentFace, coneBaseTwinHalfEdges ); - } - else - { - coneBaseTwinHalfEdges.push_back( pCurrentHalfEdge->m_pTwin ); - } - } - - pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW; - } - while( pCurrentHalfEdge != pFace->m_pHalfEdge ); -} - -void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges, - std::list< EpaFace* >& newFaces ) -{ - assert( ( baseTwinHalfEdges.size() >= 3 ) && "DeleteVisibleFaces method didn't do its job right!" ); - - std::list< EpaHalfEdge* >::iterator baseHalfEdgesItr( baseTwinHalfEdges.begin() ); - std::list< EpaHalfEdge* > halfEdgesToLink; - - while ( baseHalfEdgesItr != baseTwinHalfEdges.end() ) - { - EpaFace* pNewFace = CreateConeFace( pAppexVertex, *baseHalfEdgesItr, halfEdgesToLink ); - - newFaces.push_back( pNewFace ); - - ++baseHalfEdgesItr; - } - - // Connect consecutive faces by linking twin half-edges - - assert( ( halfEdgesToLink.size() % 2 == 0 ) && "Nb half-edges to link is odd!" ); - - int nbLinksToCreate = halfEdgesToLink.size() / 2; - int nbLinksCreated = 0; - - std::list< EpaHalfEdge* >::iterator halfEdgesItr( halfEdgesToLink.begin() ); - - for ( ; ( halfEdgesItr != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr ) - { - std::list< EpaHalfEdge* >::iterator halfEdgesItr2( halfEdgesItr ); - ++halfEdgesItr2; - - for ( ; ( halfEdgesItr2 != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr2 ) - { - EpaHalfEdge* pHalfEdge1 = *halfEdgesItr; - EpaHalfEdge* pHalfEdge2 = *halfEdgesItr2; - - EpaHalfEdge* pHalfEdgeNextCCW1 = pHalfEdge1->m_pNextCCW; - EpaHalfEdge* pHalfEdgeNextCCW2 = pHalfEdge2->m_pNextCCW; - - if ( ( pHalfEdge2->m_pVertex == pHalfEdgeNextCCW1->m_pVertex ) && - ( pHalfEdgeNextCCW2->m_pVertex == pHalfEdge1->m_pVertex ) ) - { - pHalfEdge1->m_pTwin = pHalfEdge2; - pHalfEdge2->m_pTwin = pHalfEdge1; - - ++nbLinksCreated; - } - } - } - - assert( ( nbLinksCreated == nbLinksToCreate ) && "Mesh topology not ok!" ); -} - -EpaFace* EpaPolyhedron::CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge, - std::list< EpaHalfEdge* >& halfEdgesToLink ) -{ - EpaFace* pNewFace = CreateFace(); - - EpaHalfEdge* pNewHalfEdge0 = CreateHalfEdge(); - EpaHalfEdge* pNewHalfEdge1 = CreateHalfEdge(); - EpaHalfEdge* pNewHalfEdge2 = CreateHalfEdge(); - - // Let new face point to one of its new half-edges - pNewFace->m_pHalfEdge = pNewHalfEdge0; - - // Link new half-edges in a loop - - pNewHalfEdge0->m_pNextCCW = pNewHalfEdge1; - pNewHalfEdge1->m_pNextCCW = pNewHalfEdge2; - pNewHalfEdge2->m_pNextCCW = pNewHalfEdge0; - - // Let new half-edges point to new face - - pNewHalfEdge0->m_pFace = pNewFace; - pNewHalfEdge1->m_pFace = pNewFace; - pNewHalfEdge2->m_pFace = pNewFace; - - // Let new half-edge 0 and base twin half-edge point to each other - - pNewHalfEdge0->m_pTwin = pBaseTwinHalfEdge; - pBaseTwinHalfEdge->m_pTwin = pNewHalfEdge0; - - // Let new half-edges know about their origin vertex - - pNewHalfEdge0->m_pVertex = pBaseTwinHalfEdge->m_pNextCCW->m_pVertex; - pNewHalfEdge1->m_pVertex = pBaseTwinHalfEdge->m_pVertex; - pNewHalfEdge2->m_pVertex = pAppexVertex; - - // Let vertices know about one of their outgoing edges - - //pNewHalfEdge0->m_pVertex->m_pHalfEdge = pNewHalfEdge0; - //pNewHalfEdge1->m_pVertex->m_pHalfEdge = pNewHalfEdge1; - //pNewHalfEdge2->m_pVertex->m_pHalfEdge = pNewHalfEdge2; - - halfEdgesToLink.push_back( pNewHalfEdge1 ); - halfEdgesToLink.push_back( pNewHalfEdge2 ); - - return pNewFace; -} - - -#ifdef DO_SOME_DEBUGGING_ -#ifdef _DEBUG -bool EpaPolyhedron::_dbgSaveToFile( const char* pFileName ) -{ - FILE* fp = NULL; - - if ( fopen_s( &fp, pFileName, "wb" ) != 0 ) - { - return false; - } - - unsigned long int nbBytesWritten; - unsigned long int byteIndex = 0; - - unsigned long int fileID = 0xBADC0DE; - fwrite( &fileID, sizeof( fileID ), 1, fp ); - nbBytesWritten = sizeof( fileID ); - byteIndex += nbBytesWritten; - - unsigned char reservedByte = 0; - fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); - nbBytesWritten = sizeof( reservedByte ); - byteIndex += nbBytesWritten; - fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); - nbBytesWritten = sizeof( reservedByte ); - byteIndex += nbBytesWritten; - fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); - nbBytesWritten = sizeof( reservedByte ); - byteIndex += nbBytesWritten; - - fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); - nbBytesWritten = sizeof( reservedByte ); - byteIndex += nbBytesWritten; - fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); - nbBytesWritten = sizeof( reservedByte ); - byteIndex += nbBytesWritten; - fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); - nbBytesWritten = sizeof( reservedByte ); - byteIndex += nbBytesWritten; - - unsigned char stringSize = 5; - fwrite( &stringSize, sizeof( stringSize ), 1, fp ); - nbBytesWritten = sizeof( stringSize ); - byteIndex += nbBytesWritten; - - char exportedFrom[ 6 ] = "01234"; - fwrite( exportedFrom, stringSize * sizeof( char ), 1, fp ); - nbBytesWritten = stringSize * sizeof( char ); - byteIndex += nbBytesWritten; - - unsigned short int w = 0; - - fwrite( &w, sizeof( w ), 1, fp ); - nbBytesWritten = sizeof( w ); - byteIndex += nbBytesWritten; - fwrite( &w, sizeof( w ), 1, fp ); - nbBytesWritten = sizeof( w ); - byteIndex += nbBytesWritten; - fwrite( &w, sizeof( w ), 1, fp ); - nbBytesWritten = sizeof( w ); - byteIndex += nbBytesWritten; - - fwrite( &w, sizeof( w ), 1, fp ); - nbBytesWritten = sizeof( w ); - byteIndex += nbBytesWritten; - fwrite( &w, sizeof( w ), 1, fp ); - nbBytesWritten = sizeof( w ); - byteIndex += nbBytesWritten; - fwrite( &w, sizeof( w ), 1, fp ); - nbBytesWritten = sizeof( w ); - byteIndex += nbBytesWritten; - - unsigned long int geometryOffsetAtByteNb = byteIndex; - - unsigned long int geometryOffset = 0; - unsigned long int geometrySize = 0; - - fseek( fp, sizeof( geometryOffset ) + sizeof( geometrySize ), SEEK_CUR ); - byteIndex += sizeof( geometryOffset ) + sizeof( geometrySize ); - - unsigned long int mappingOffset = 0; - unsigned long int mappingSize = 0; - - fwrite( &mappingOffset, sizeof( mappingOffset ), 1, fp ); - nbBytesWritten = sizeof( mappingOffset ); - byteIndex += nbBytesWritten; - - fwrite( &mappingSize, sizeof( mappingSize ), 1, fp ); - nbBytesWritten = sizeof( mappingSize ); - byteIndex += nbBytesWritten; - - unsigned long int animationOffset = 0; - unsigned long int animationSize = 0; - - fwrite( &animationOffset, sizeof( animationOffset ), 1, fp ); - nbBytesWritten = sizeof( animationOffset ); - byteIndex += nbBytesWritten; - fwrite( &animationSize, sizeof( animationSize ), 1, fp ); - nbBytesWritten = sizeof( animationSize ); - byteIndex += nbBytesWritten; - - unsigned long int reservedDword = 0; - fwrite( &reservedDword, sizeof( reservedDword ), 1, fp ); - nbBytesWritten = sizeof( reservedDword ); - byteIndex += nbBytesWritten; - fwrite( &reservedDword, sizeof( reservedDword ), 1, fp ); - nbBytesWritten = sizeof( reservedDword ); - byteIndex += nbBytesWritten; - - geometryOffset = byteIndex; - - unsigned short int nbMeshs = 1; - fwrite( &nbMeshs, sizeof( nbMeshs ), 1, fp ); - nbBytesWritten = sizeof( nbMeshs ); - byteIndex += nbBytesWritten; - - char meshName[] = "noname mesh"; - unsigned char meshNameSize = strlen( meshName ); - - fwrite( &meshNameSize, sizeof( meshNameSize ), 1, fp ); - nbBytesWritten = sizeof( meshNameSize ); - byteIndex += nbBytesWritten; - - fwrite( meshName, meshNameSize * sizeof( char ), 1, fp ); - nbBytesWritten = meshNameSize * sizeof( char ); - byteIndex += nbBytesWritten; - - stdext::hash_map< unsigned long int, int > verticesMap; - typedef std::pair< unsigned long int, int > PR; - - int vertexIndex = 0; - - // Hash only vertices from faces that are not deleted - - std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); - int nbFaces = 0; - - while ( facesItr != m_faces.end() ) - { - EpaFace* pFace = *facesItr; - - if ( !pFace->m_deleted ) - { - stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; - - vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] ); - if ( vertexItr == verticesMap.end() ) - { - verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 0 ], vertexIndex ) ); - ++vertexIndex; - } - - vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] ); - if ( vertexItr == verticesMap.end() ) - { - verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 1 ], vertexIndex ) ); - ++vertexIndex; - } - - vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] ); - if ( vertexItr == verticesMap.end() ) - { - verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 2 ], vertexIndex ) ); - ++vertexIndex; - } - - ++nbFaces; - } - - ++facesItr; - } - - unsigned long int nbVertices = verticesMap.size(); - fwrite( &nbVertices, sizeof( nbVertices ), 1, fp ); - nbBytesWritten = sizeof( nbVertices ); - byteIndex += nbBytesWritten; - - // Collect all safe vertices - - float* pVertices = new float[ verticesMap.size() * 3 ]; - - stdext::hash_map< unsigned long int, int >::iterator verticesItr( verticesMap.begin() ); - - while ( verticesItr != verticesMap.end() ) - { - stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; - - PR pr = *verticesItr; - - vertexItr = verticesMap.find( pr.first ); - assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); - - EpaVertex* pVertex = ( EpaVertex* ) vertexItr->first; - - pVertices[ vertexItr->second * 3 ] = pVertex->m_point.x(); - pVertices[ vertexItr->second * 3 + 1 ] = pVertex->m_point.y(); - pVertices[ vertexItr->second * 3 + 2 ] = pVertex->m_point.z(); - - ++verticesItr; - } - - unsigned long int* pIndices = new unsigned long int[ nbFaces * 3 ]; - - facesItr = m_faces.begin(); - - int facesIndex = 0; - while ( facesItr != m_faces.end() ) - { - EpaFace* pFace = *facesItr; - - if ( !pFace->m_deleted ) - { - stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; - - int verticesIndices[ 3 ]; - - vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] ); - assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); - verticesIndices[ 0 ] = vertexItr->second; - - vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] ); - assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); - verticesIndices[ 1 ] = vertexItr->second; - - vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] ); - assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); - verticesIndices[ 2 ] = vertexItr->second; - - pIndices[ facesIndex * 3 ] = verticesIndices[ 0 ]; - pIndices[ facesIndex * 3 + 1 ] = verticesIndices[ 1 ]; - pIndices[ facesIndex * 3 + 2 ] = verticesIndices[ 2 ]; - - ++facesIndex; - } - - ++facesItr; - } - - fwrite( &nbFaces, sizeof( nbFaces ), 1, fp ); - nbBytesWritten = sizeof( nbFaces ); - byteIndex += nbBytesWritten; - - bool hasSmoothingGroups = false; - fwrite( &hasSmoothingGroups, sizeof( hasSmoothingGroups ), 1, fp ); - nbBytesWritten = sizeof( hasSmoothingGroups ); - byteIndex += nbBytesWritten; - - fwrite( pVertices, verticesMap.size() * 3 * sizeof( float ), 1, fp ); - nbBytesWritten = verticesMap.size() * 3 * sizeof( float ); - byteIndex += nbBytesWritten; - - // write indices - fwrite( pIndices, nbFaces * 3 * sizeof( unsigned long int ), 1, fp ); - nbBytesWritten = nbFaces * 3 * sizeof( unsigned long int ); - byteIndex += nbBytesWritten; - - delete[] pVertices; - delete[] pIndices; - - geometrySize = byteIndex - geometryOffset; - - fseek( fp, geometryOffsetAtByteNb, SEEK_SET ); - - fwrite( &geometryOffset, sizeof( geometryOffset ), 1, fp ); - nbBytesWritten = sizeof( geometryOffset ); - byteIndex += nbBytesWritten; - fwrite( &geometrySize, sizeof( geometrySize ), 1, fp ); - nbBytesWritten = sizeof( geometrySize ); - byteIndex += nbBytesWritten; - - fseek( fp, byteIndex, SEEK_SET ); - - fclose( fp ); - - return true; -} -#endif -#endif - +/* +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 "Memory2.h" + +#include +#ifdef _DEBUG +#include +#endif + + +#include "NarrowPhaseCollision/EpaCommon.h" + +#include "NarrowPhaseCollision/EpaVertex.h" +#include "NarrowPhaseCollision/EpaHalfEdge.h" +#include "NarrowPhaseCollision/EpaFace.h" +#include "NarrowPhaseCollision/EpaPolyhedron.h" + + +EpaPolyhedron::EpaPolyhedron() : m_nbFaces( 0 ) +{ +} + +EpaPolyhedron::~EpaPolyhedron() +{ + Destroy(); +} + +bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints, + SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, + const int nbInitialPoints ) +{ +#ifndef EPA_POLYHEDRON_USE_PLANES + assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" ); +#endif + + if ( nbInitialPoints < 4 ) + { + // Insufficient nb of points + return false; + } + + //////////////////////////////////////////////////////////////////////////////// + +#ifdef EPA_POLYHEDRON_USE_PLANES + int nbDiffCoords[ 3 ] = { 0, 0, 0 }; + + bool* pDiffCoords = new bool[ 3 * nbInitialPoints ]; + + + int i; + for (i=0;i nbDiffCoords[ i + 1 ] ) + { + int tmp = nbDiffCoords[ i ]; + nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ]; + nbDiffCoords[ i + 1 ] = tmp; + + tmp = axisOrderIndices[ i ]; + axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ]; + axisOrderIndices[ i + 1 ] = tmp; + } + } + + int nbSuccessfullAxis = 0; + + // The axes with less different coordinates choose first + int minsIndices[ 3 ] = { -1, -1, -1 }; + int maxsIndices[ 3 ] = { -1, -1, -1 }; + + int finalPointsIndex = 0; + + for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis ) + { + int axisIndex = axisOrderIndices[ axis ]; + + SimdScalar axisMin = SIMD_INFINITY; + SimdScalar axisMax = -SIMD_INFINITY; + + for ( int i = 0; i < 4; ++i ) + { + // Among the diff coords pick the min and max coords + + if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] ) + { + if ( pInitialPoints[ i ][ axisIndex ] < axisMin ) + { + axisMin = pInitialPoints[ i ][ axisIndex ]; + minsIndices[ axisIndex ] = i; + } + + if ( pInitialPoints[ i ][ axisIndex ] > axisMax ) + { + axisMax = pInitialPoints[ i ][ axisIndex ]; + maxsIndices[ axisIndex ] = i; + } + } + } + + //assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) && + // "min and max have the same index!" ); + + if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) && + ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) ) + { + ++nbSuccessfullAxis; + + finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ]; + finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ]; + + // Make the choosen points to be impossible for other axes to choose + + //assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); + //assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); + + for ( int i = 0; i < 3; ++i ) + { + pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false; + pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false; + } + } + } + + if ( nbSuccessfullAxis <= 1 ) + { + // Degenerate input ? + assert( false && "nbSuccessfullAxis must be greater than 1!" ); + return false; + } + + delete[] pDiffCoords; +#endif + + ////////////////////////////////////////////////////////////////////////// + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; + SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; +#else + SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ]; + SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ]; +#endif + + SimdVector3 planeNormal = v1.cross( v0 ); + planeNormal.normalize(); + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal ); +#else + SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal ); +#endif + +#ifdef EPA_POLYHEDRON_USE_PLANES + assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && + "Point should be on plane!" ); + assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && + "Point should be on plane!" ); + assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && + "Point should be on plane!" ); +#endif + +#ifndef EPA_POLYHEDRON_USE_PLANES + { + if ( planeDistance > 0 ) + { + SimdVector3 tmp = pInitialPoints[ 1 ]; + pInitialPoints[ 1 ] = pInitialPoints[ 2 ]; + pInitialPoints[ 2 ] = tmp; + + tmp = pSupportPointsOnA[ 1 ]; + pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ]; + pSupportPointsOnA[ 2 ] = tmp; + + tmp = pSupportPointsOnB[ 1 ]; + pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ]; + pSupportPointsOnB[ 2 ] = tmp; + } + } + + EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] ); + EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] ); + EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] ); + EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] ); +#else + finalPointsIndices[ 3 ] = -1; + + SimdScalar absMaxDist = -SIMD_INFINITY; + SimdScalar maxDist; + + for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex ) + { + SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance; + SimdScalar absDist = abs( dist ); + + if ( ( absDist > absMaxDist ) && + !SimdEqual( dist, PLANE_THICKNESS ) ) + { + absMaxDist = absDist; + maxDist = dist; + finalPointsIndices[ 3 ] = pointIndex; + } + } + + if ( finalPointsIndices[ 3 ] == -1 ) + { + Destroy(); + return false; + } + + if ( maxDist > PLANE_THICKNESS ) + { + // Can swap indices only + + SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ]; + pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ]; + pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp; + + tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ]; + pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ]; + pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp; + + tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ]; + pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ]; + pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp; + } + + EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] ); + EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] ); + EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] ); + EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] ); +#endif + + EpaFace* pFaceA = CreateFace(); + EpaFace* pFaceB = CreateFace(); + EpaFace* pFaceC = CreateFace(); + EpaFace* pFaceD = CreateFace(); + + EpaHalfEdge* pFaceAHalfEdges[ 3 ]; + EpaHalfEdge* pFaceCHalfEdges[ 3 ]; + EpaHalfEdge* pFaceBHalfEdges[ 3 ]; + EpaHalfEdge* pFaceDHalfEdges[ 3 ]; + + pFaceAHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceAHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceAHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceBHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceBHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceBHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceCHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceCHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceCHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceDHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceDHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceDHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; + pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ]; + pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ]; + pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ]; + + pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ]; + pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ]; + pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ]; + + pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ]; + pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ]; + pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ]; + + pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ]; + pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ]; + pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ]; + + pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ]; + pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ]; + pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ]; + + + pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA; + pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA; + pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA; + + pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB; + pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB; + pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB; + + pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC; + pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC; + pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC; + + pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD; + pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD; + pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD; + + + pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA; + pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB; + pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC; + + pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB; + pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD; + pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC; + + pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD; + pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA; + pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC; + + pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB; + pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA; + pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD; + + //pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; + //pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ]; + //pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ]; + //pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ]; + + pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ]; + pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ]; + pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ]; + + pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ]; + pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ]; + pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ]; + + pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ]; + pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ]; + pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ]; + + pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ]; + pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ]; + pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ]; + + if ( !pFaceA->Initialize() || !pFaceB->Initialize() || + !pFaceC->Initialize() || !pFaceD->Initialize() ) + { + assert( false && "One initial face failed to initialize!" ); + return false; + } + +#ifdef EPA_POLYHEDRON_USE_PLANES + if ( nbInitialPoints > 4 ) + { + for ( int i = 0; i < nbInitialPoints; ++i ) + { + if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) && + ( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) ) + { + std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); + + while ( facesItr != m_faces.end() ) + { + EpaFace* pFace = *facesItr; + + SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance; + + if ( dist > PLANE_THICKNESS ) + { + std::list< EpaFace* > newFaces; + + bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ], + pFace, newFaces ); + + if ( !expandOk ) + { + // One or more new faces are affinely dependent + return false; + } + + assert( !newFaces.empty() && "Polyhedron should have expanded!" ); + + break; + } + + ++facesItr; + } + } + } + } +#endif + + return true; +} + +void EpaPolyhedron::Destroy() +{ + DestroyAllVertices(); + + DestroyAllHalfEdges(); + + DestroyAllFaces(); +} + +EpaFace* EpaPolyhedron::CreateFace() +{ + EpaFace* pNewFace = new EpaFace(); + assert( pNewFace && "Failed to allocate memory for a new EpaFace!" ); + + m_faces.push_back( pNewFace ); + + ++m_nbFaces; + + return pNewFace; +} + +EpaHalfEdge* EpaPolyhedron::CreateHalfEdge() +{ + EpaHalfEdge* pNewHalfEdge = new EpaHalfEdge(); + assert( pNewHalfEdge && "Failed to allocate memory for a new EpaHalfEdge!" ); + + m_halfEdges.push_back( pNewHalfEdge ); + + return pNewHalfEdge; +} + +EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB ) +{ + EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); + assert( pNewVertex && "Failed to allocate memory for a new EpaVertex!" ); + + m_vertices.push_back( pNewVertex ); + + return pNewVertex; +} + +void EpaPolyhedron::DeleteFace( EpaFace* pFace ) +{ + pFace->m_deleted = true; + --m_nbFaces; +} + +void EpaPolyhedron::DestroyAllFaces() +{ + while ( !m_faces.empty() ) + { + EpaFace* pFace = m_faces.front(); + + delete pFace; + + m_faces.pop_front(); + } + + m_nbFaces = 0; +} + +void EpaPolyhedron::DestroyAllHalfEdges() +{ + while ( !m_halfEdges.empty() ) + { + EpaHalfEdge* pHalfEdge = m_halfEdges.front(); + + delete pHalfEdge; + + m_halfEdges.pop_front(); + } +} + +void EpaPolyhedron::DestroyAllVertices() +{ + while ( !m_vertices.empty() ) + { + EpaVertex* pVertex = m_vertices.front(); + + delete pVertex; + + m_vertices.pop_front(); + } +} + +bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB, + EpaFace* pFace, std::list< EpaFace* >& newFaces ) +{ + assert( !pFace->m_deleted && "Face is already deleted!" ); + assert( newFaces.empty() && "NewFaces list must be empty!" ); + + assert( !pFace->m_deleted && "Cannot expand deleted face!" ); + + // wSupportPoint must be front of face's plane used to do the expansion + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance; + if ( dist <= PLANE_THICKNESS ) + { + return false; + } +#endif + + std::list< EpaHalfEdge* > coneBaseEdges; + DeleteVisibleFaces( wSupportPoint, pFace, coneBaseEdges ); + + assert( ( coneBaseEdges.size() >= 3 ) && "Cone base must have at least 3 edges!" ); + + EpaVertex* pConeAppex = CreateVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); + assert( pConeAppex && "Failed to create vertex!" ); + + CreateCone( pConeAppex, coneBaseEdges, newFaces ); + + // Initialize new faces + + std::list< EpaFace* >::iterator newFacesItr( newFaces.begin() ); + + while ( newFacesItr != newFaces.end() ) + { + EpaFace* pNewFace = *newFacesItr; + + if ( !pNewFace->Initialize() ) + { + return false; + } + + ++newFacesItr; + } + + return true; +} + +std::list< EpaFace* >& EpaPolyhedron::GetFaces() +{ + return m_faces; +} + +int EpaPolyhedron::GetNbFaces() const +{ + return m_faces.size(); +} + +void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, + std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ) +{ + assert( !pFace->m_deleted && "Face is already deleted!" ); + + DeleteFace( pFace ); + + EpaHalfEdge* pCurrentHalfEdge = pFace->m_pHalfEdge; + + do + { + assert( pCurrentHalfEdge->m_pTwin && "Half-edge without a twin!" ); + + EpaFace* pAdjacentFace = pCurrentHalfEdge->m_pTwin->m_pFace; + assert( pAdjacentFace && "Invalid adjacent face!" ); + + if ( !pAdjacentFace->m_deleted ) + { +#ifdef EPA_POLYHEDRON_USE_PLANES + assert( ( pAdjacentFace->m_planeNormal.length2() > 0 ) && "Invalid plane!" ); + + SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) + + pAdjacentFace->m_planeDistance; + + if ( pointDist > PLANE_THICKNESS ) +#else + SimdScalar dot = pAdjacentFace->m_v.dot( point ); + if ( dot >= pAdjacentFace->m_vSqrd ) +#endif + { + DeleteVisibleFaces( point, pAdjacentFace, coneBaseTwinHalfEdges ); + } + else + { + coneBaseTwinHalfEdges.push_back( pCurrentHalfEdge->m_pTwin ); + } + } + + pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW; + } + while( pCurrentHalfEdge != pFace->m_pHalfEdge ); +} + +void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges, + std::list< EpaFace* >& newFaces ) +{ + assert( ( baseTwinHalfEdges.size() >= 3 ) && "DeleteVisibleFaces method didn't do its job right!" ); + + std::list< EpaHalfEdge* >::iterator baseHalfEdgesItr( baseTwinHalfEdges.begin() ); + std::list< EpaHalfEdge* > halfEdgesToLink; + + while ( baseHalfEdgesItr != baseTwinHalfEdges.end() ) + { + EpaFace* pNewFace = CreateConeFace( pAppexVertex, *baseHalfEdgesItr, halfEdgesToLink ); + + newFaces.push_back( pNewFace ); + + ++baseHalfEdgesItr; + } + + // Connect consecutive faces by linking twin half-edges + + assert( ( halfEdgesToLink.size() % 2 == 0 ) && "Nb half-edges to link is odd!" ); + + int nbLinksToCreate = halfEdgesToLink.size() / 2; + int nbLinksCreated = 0; + + std::list< EpaHalfEdge* >::iterator halfEdgesItr( halfEdgesToLink.begin() ); + + for ( ; ( halfEdgesItr != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr ) + { + std::list< EpaHalfEdge* >::iterator halfEdgesItr2( halfEdgesItr ); + ++halfEdgesItr2; + + for ( ; ( halfEdgesItr2 != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr2 ) + { + EpaHalfEdge* pHalfEdge1 = *halfEdgesItr; + EpaHalfEdge* pHalfEdge2 = *halfEdgesItr2; + + EpaHalfEdge* pHalfEdgeNextCCW1 = pHalfEdge1->m_pNextCCW; + EpaHalfEdge* pHalfEdgeNextCCW2 = pHalfEdge2->m_pNextCCW; + + if ( ( pHalfEdge2->m_pVertex == pHalfEdgeNextCCW1->m_pVertex ) && + ( pHalfEdgeNextCCW2->m_pVertex == pHalfEdge1->m_pVertex ) ) + { + pHalfEdge1->m_pTwin = pHalfEdge2; + pHalfEdge2->m_pTwin = pHalfEdge1; + + ++nbLinksCreated; + } + } + } + + assert( ( nbLinksCreated == nbLinksToCreate ) && "Mesh topology not ok!" ); +} + +EpaFace* EpaPolyhedron::CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge, + std::list< EpaHalfEdge* >& halfEdgesToLink ) +{ + EpaFace* pNewFace = CreateFace(); + + EpaHalfEdge* pNewHalfEdge0 = CreateHalfEdge(); + EpaHalfEdge* pNewHalfEdge1 = CreateHalfEdge(); + EpaHalfEdge* pNewHalfEdge2 = CreateHalfEdge(); + + // Let new face point to one of its new half-edges + pNewFace->m_pHalfEdge = pNewHalfEdge0; + + // Link new half-edges in a loop + + pNewHalfEdge0->m_pNextCCW = pNewHalfEdge1; + pNewHalfEdge1->m_pNextCCW = pNewHalfEdge2; + pNewHalfEdge2->m_pNextCCW = pNewHalfEdge0; + + // Let new half-edges point to new face + + pNewHalfEdge0->m_pFace = pNewFace; + pNewHalfEdge1->m_pFace = pNewFace; + pNewHalfEdge2->m_pFace = pNewFace; + + // Let new half-edge 0 and base twin half-edge point to each other + + pNewHalfEdge0->m_pTwin = pBaseTwinHalfEdge; + pBaseTwinHalfEdge->m_pTwin = pNewHalfEdge0; + + // Let new half-edges know about their origin vertex + + pNewHalfEdge0->m_pVertex = pBaseTwinHalfEdge->m_pNextCCW->m_pVertex; + pNewHalfEdge1->m_pVertex = pBaseTwinHalfEdge->m_pVertex; + pNewHalfEdge2->m_pVertex = pAppexVertex; + + // Let vertices know about one of their outgoing edges + + //pNewHalfEdge0->m_pVertex->m_pHalfEdge = pNewHalfEdge0; + //pNewHalfEdge1->m_pVertex->m_pHalfEdge = pNewHalfEdge1; + //pNewHalfEdge2->m_pVertex->m_pHalfEdge = pNewHalfEdge2; + + halfEdgesToLink.push_back( pNewHalfEdge1 ); + halfEdgesToLink.push_back( pNewHalfEdge2 ); + + return pNewFace; +} + + +#ifdef DO_SOME_DEBUGGING_ +#ifdef _DEBUG +bool EpaPolyhedron::_dbgSaveToFile( const char* pFileName ) +{ + FILE* fp = NULL; + + if ( fopen_s( &fp, pFileName, "wb" ) != 0 ) + { + return false; + } + + unsigned long int nbBytesWritten; + unsigned long int byteIndex = 0; + + unsigned long int fileID = 0xBADC0DE; + fwrite( &fileID, sizeof( fileID ), 1, fp ); + nbBytesWritten = sizeof( fileID ); + byteIndex += nbBytesWritten; + + unsigned char reservedByte = 0; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + + unsigned char stringSize = 5; + fwrite( &stringSize, sizeof( stringSize ), 1, fp ); + nbBytesWritten = sizeof( stringSize ); + byteIndex += nbBytesWritten; + + char exportedFrom[ 6 ] = "01234"; + fwrite( exportedFrom, stringSize * sizeof( char ), 1, fp ); + nbBytesWritten = stringSize * sizeof( char ); + byteIndex += nbBytesWritten; + + unsigned short int w = 0; + + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + + unsigned long int geometryOffsetAtByteNb = byteIndex; + + unsigned long int geometryOffset = 0; + unsigned long int geometrySize = 0; + + fseek( fp, sizeof( geometryOffset ) + sizeof( geometrySize ), SEEK_CUR ); + byteIndex += sizeof( geometryOffset ) + sizeof( geometrySize ); + + unsigned long int mappingOffset = 0; + unsigned long int mappingSize = 0; + + fwrite( &mappingOffset, sizeof( mappingOffset ), 1, fp ); + nbBytesWritten = sizeof( mappingOffset ); + byteIndex += nbBytesWritten; + + fwrite( &mappingSize, sizeof( mappingSize ), 1, fp ); + nbBytesWritten = sizeof( mappingSize ); + byteIndex += nbBytesWritten; + + unsigned long int animationOffset = 0; + unsigned long int animationSize = 0; + + fwrite( &animationOffset, sizeof( animationOffset ), 1, fp ); + nbBytesWritten = sizeof( animationOffset ); + byteIndex += nbBytesWritten; + fwrite( &animationSize, sizeof( animationSize ), 1, fp ); + nbBytesWritten = sizeof( animationSize ); + byteIndex += nbBytesWritten; + + unsigned long int reservedDword = 0; + fwrite( &reservedDword, sizeof( reservedDword ), 1, fp ); + nbBytesWritten = sizeof( reservedDword ); + byteIndex += nbBytesWritten; + fwrite( &reservedDword, sizeof( reservedDword ), 1, fp ); + nbBytesWritten = sizeof( reservedDword ); + byteIndex += nbBytesWritten; + + geometryOffset = byteIndex; + + unsigned short int nbMeshs = 1; + fwrite( &nbMeshs, sizeof( nbMeshs ), 1, fp ); + nbBytesWritten = sizeof( nbMeshs ); + byteIndex += nbBytesWritten; + + char meshName[] = "noname mesh"; + unsigned char meshNameSize = strlen( meshName ); + + fwrite( &meshNameSize, sizeof( meshNameSize ), 1, fp ); + nbBytesWritten = sizeof( meshNameSize ); + byteIndex += nbBytesWritten; + + fwrite( meshName, meshNameSize * sizeof( char ), 1, fp ); + nbBytesWritten = meshNameSize * sizeof( char ); + byteIndex += nbBytesWritten; + + stdext::hash_map< unsigned long int, int > verticesMap; + typedef std::pair< unsigned long int, int > PR; + + int vertexIndex = 0; + + // Hash only vertices from faces that are not deleted + + std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); + int nbFaces = 0; + + while ( facesItr != m_faces.end() ) + { + EpaFace* pFace = *facesItr; + + if ( !pFace->m_deleted ) + { + stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] ); + if ( vertexItr == verticesMap.end() ) + { + verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 0 ], vertexIndex ) ); + ++vertexIndex; + } + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] ); + if ( vertexItr == verticesMap.end() ) + { + verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 1 ], vertexIndex ) ); + ++vertexIndex; + } + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] ); + if ( vertexItr == verticesMap.end() ) + { + verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 2 ], vertexIndex ) ); + ++vertexIndex; + } + + ++nbFaces; + } + + ++facesItr; + } + + unsigned long int nbVertices = verticesMap.size(); + fwrite( &nbVertices, sizeof( nbVertices ), 1, fp ); + nbBytesWritten = sizeof( nbVertices ); + byteIndex += nbBytesWritten; + + // Collect all safe vertices + + float* pVertices = new float[ verticesMap.size() * 3 ]; + + stdext::hash_map< unsigned long int, int >::iterator verticesItr( verticesMap.begin() ); + + while ( verticesItr != verticesMap.end() ) + { + stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; + + PR pr = *verticesItr; + + vertexItr = verticesMap.find( pr.first ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + + EpaVertex* pVertex = ( EpaVertex* ) vertexItr->first; + + pVertices[ vertexItr->second * 3 ] = pVertex->m_point.x(); + pVertices[ vertexItr->second * 3 + 1 ] = pVertex->m_point.y(); + pVertices[ vertexItr->second * 3 + 2 ] = pVertex->m_point.z(); + + ++verticesItr; + } + + unsigned long int* pIndices = new unsigned long int[ nbFaces * 3 ]; + + facesItr = m_faces.begin(); + + int facesIndex = 0; + while ( facesItr != m_faces.end() ) + { + EpaFace* pFace = *facesItr; + + if ( !pFace->m_deleted ) + { + stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; + + int verticesIndices[ 3 ]; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + verticesIndices[ 0 ] = vertexItr->second; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + verticesIndices[ 1 ] = vertexItr->second; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + verticesIndices[ 2 ] = vertexItr->second; + + pIndices[ facesIndex * 3 ] = verticesIndices[ 0 ]; + pIndices[ facesIndex * 3 + 1 ] = verticesIndices[ 1 ]; + pIndices[ facesIndex * 3 + 2 ] = verticesIndices[ 2 ]; + + ++facesIndex; + } + + ++facesItr; + } + + fwrite( &nbFaces, sizeof( nbFaces ), 1, fp ); + nbBytesWritten = sizeof( nbFaces ); + byteIndex += nbBytesWritten; + + bool hasSmoothingGroups = false; + fwrite( &hasSmoothingGroups, sizeof( hasSmoothingGroups ), 1, fp ); + nbBytesWritten = sizeof( hasSmoothingGroups ); + byteIndex += nbBytesWritten; + + fwrite( pVertices, verticesMap.size() * 3 * sizeof( float ), 1, fp ); + nbBytesWritten = verticesMap.size() * 3 * sizeof( float ); + byteIndex += nbBytesWritten; + + // write indices + fwrite( pIndices, nbFaces * 3 * sizeof( unsigned long int ), 1, fp ); + nbBytesWritten = nbFaces * 3 * sizeof( unsigned long int ); + byteIndex += nbBytesWritten; + + delete[] pVertices; + delete[] pIndices; + + geometrySize = byteIndex - geometryOffset; + + fseek( fp, geometryOffsetAtByteNb, SEEK_SET ); + + fwrite( &geometryOffset, sizeof( geometryOffset ), 1, fp ); + nbBytesWritten = sizeof( geometryOffset ); + byteIndex += nbBytesWritten; + fwrite( &geometrySize, sizeof( geometrySize ), 1, fp ); + nbBytesWritten = sizeof( geometrySize ); + byteIndex += nbBytesWritten; + + fseek( fp, byteIndex, SEEK_SET ); + + fclose( fp ); + + return true; +} +#endif +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaPolyhedron.h b/Extras/EPA/EpaPolyhedron.h similarity index 97% rename from Bullet/NarrowPhaseCollision/EpaPolyhedron.h rename to Extras/EPA/EpaPolyhedron.h index 29a511981..1f898de64 100644 --- a/Bullet/NarrowPhaseCollision/EpaPolyhedron.h +++ b/Extras/EPA/EpaPolyhedron.h @@ -1,89 +1,89 @@ -/* -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 - +/* +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 + diff --git a/Bullet/NarrowPhaseCollision/EpaVertex.h b/Extras/EPA/EpaVertex.h similarity index 96% rename from Bullet/NarrowPhaseCollision/EpaVertex.h rename to Extras/EPA/EpaVertex.h index 7d5cea373..b0eac4020 100644 --- a/Bullet/NarrowPhaseCollision/EpaVertex.h +++ b/Extras/EPA/EpaVertex.h @@ -1,61 +1,61 @@ -/* -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 - +/* +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 + diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index d871711e4..4304a766b 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -36,8 +36,7 @@ subject to the following restrictions: #include "BroadphaseCollision/Dispatcher.h" #include "NarrowPhaseCollision/PersistentManifold.h" #include "CollisionShapes/TriangleMeshShape.h" -#include "ConstraintSolver/OdeConstraintSolver.h" -#include "ConstraintSolver/SimpleConstraintSolver.h" +#include "ConstraintSolver/SequentialImpulseConstraintSolver.h" //profiling/timings @@ -325,7 +324,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase) :m_scalingPropagated(false), -m_numIterations(10), +m_numIterations(4), m_numTimeSubSteps(1), m_ccdMode(0), m_solverType(-1), @@ -1034,7 +1033,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) if (m_solverType != solverType) { - m_solver = new SimpleConstraintSolver(); + m_solver = new SequentialImpulseConstraintSolver(); break; } @@ -1044,7 +1043,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) default: if (m_solverType != solverType) { - m_solver = new OdeConstraintSolver(); +// m_solver = new OdeConstraintSolver(); break; } diff --git a/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp similarity index 91% rename from BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp rename to Extras/quickstep/OdeConstraintSolver.cpp index 8febbe2ec..00d9f70b9 100644 --- a/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp +++ b/Extras/quickstep/OdeConstraintSolver.cpp @@ -1,265 +1,268 @@ -/* -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 "OdeConstraintSolver.h" - -#include "NarrowPhaseCollision/PersistentManifold.h" -#include "Dynamics/RigidBody.h" -#include "ContactConstraint.h" -#include "Solve2LinearConstraint.h" -#include "ContactSolverInfo.h" -#include "Dynamics/BU_Joint.h" -#include "Dynamics/ContactJoint.h" - -#include "IDebugDraw.h" - -#define USE_SOR_SOLVER - -#include "SorLcp.h" - -#include -#include //FLT_MAX -#ifdef WIN32 -#include -#endif -#include -#include - -#if defined (WIN32) -#include -#else -#if defined (__FreeBSD__) -#include -#else -#include -#endif -#endif - -class BU_Joint; - -//see below - -OdeConstraintSolver::OdeConstraintSolver(): -m_cfm(0.f),//1e-5f), -m_erp(0.4f) -{ -} - - - - - - - -//iterative lcp and penalty method -float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) -{ - m_CurBody = 0; - m_CurJoint = 0; - - - RigidBody* bodies [MAX_RIGIDBODIES]; - - int numBodies = 0; - BU_Joint* joints [MAX_RIGIDBODIES*4]; - int numJoints = 0; - - for (int j=0;jGetNumContacts() > 0) - { - body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); - body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); - ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer); - } - } - - SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal); - - return 0.f; - -} - -///////////////////////////////////////////////////////////////////////////////// - - -typedef SimdScalar dQuaternion[4]; -#define _R(i,j) R[(i)*4+(j)] - -void dRfromQ1 (dMatrix3 R, const dQuaternion q) -{ - // q = (s,vx,vy,vz) - SimdScalar qq1 = 2.f*q[1]*q[1]; - SimdScalar qq2 = 2.f*q[2]*q[2]; - SimdScalar qq3 = 2.f*q[3]*q[3]; - _R(0,0) = 1.f - qq2 - qq3; - _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); - _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); - _R(0,3) = 0.f; - - _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1.f - qq1 - qq3; - _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); - _R(1,3) = 0.f; - - _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); - _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); - _R(2,2) = 1.f - qq1 - qq2; - _R(2,3) = 0.f; - -} - - - -int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) -{ - if (!body || (body->getInvMass() == 0.f) ) - { - return -1; - } - //first try to find - int i,j; - for (i=0;im_facc.setValue(0,0,0,0); - body->m_tacc.setValue(0,0,0,0); - - //are the indices the same ? - for (i=0;i<4;i++) - { - for ( j=0;j<3;j++) - { - body->m_invI[i+4*j] = 0.f; - body->m_I[i+4*j] = 0.f; - } - } - body->m_invI[0+4*0] = body->getInvInertiaDiagLocal()[0]; - body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1]; - body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2]; - - body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0]; - body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1]; - body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2]; - - - - - dQuaternion q; - - q[1] = body->getOrientation()[0]; - q[2] = body->getOrientation()[1]; - q[3] = body->getOrientation()[2]; - q[0] = body->getOrientation()[3]; - - dRfromQ1(body->m_R,q); - - return numBodies-1; -} - - - - - -#define MAX_JOINTS_1 8192 - -static ContactJoint gJointArray[MAX_JOINTS_1]; - - -void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, - RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer) -{ - - - manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(), - ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform()); - - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - - int i,numContacts = manifold->GetNumContacts(); - - bool swapBodies = (bodyId0 < 0); - - - RigidBody* body0,*body1; - - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - - body0 = (RigidBody*)manifold->GetBody1(); - body1 = (RigidBody*)manifold->GetBody0(); - - } else - { - body0 = (RigidBody*)manifold->GetBody0(); - body1 = (RigidBody*)manifold->GetBody1(); - } - - assert(bodyId0 >= 0); - - SimdVector3 color(0,1,0); - for (i=0;iGetContactPoint(i); - - debugDrawer->DrawContactPoint( - cp.m_positionWorldOnB, - cp.m_normalWorldOnB, - cp.GetDistance(), - cp.GetLifeTime(), - color); - - } - assert (m_CurJoint < MAX_JOINTS_1); - -// if (manifold->GetContactPoint(i).GetDistance() < 0.0f) - { - ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); - - cont->node[0].joint = cont; - cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; - - cont->node[1].joint = cont; - cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; - - joints[numJoints++] = cont; - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; - - cont->flags = 0; - } - } - - //create a new contact constraint -}; - +/* +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 "OdeConstraintSolver.h" + +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "Dynamics/RigidBody.h" +#include "ContactConstraint.h" +#include "Solve2LinearConstraint.h" +#include "ContactSolverInfo.h" +#include "Dynamics/BU_Joint.h" +#include "Dynamics/ContactJoint.h" + +#include "IDebugDraw.h" + +#define USE_SOR_SOLVER + +#include "SorLcp.h" + +#include +#include //FLT_MAX +#ifdef WIN32 +#include +#endif +#include +#include + +#if defined (WIN32) +#include +#else +#if defined (__FreeBSD__) +#include +#else +#include +#endif +#endif + +class BU_Joint; + +//see below + +//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island, stored in an array of size MAX_RIGIDBODIES +#define MAX_QUICKSTEP_RIGIDBODIES 8192 + +OdeConstraintSolver::OdeConstraintSolver(): +m_cfm(0.f),//1e-5f), +m_erp(0.4f) +{ +} + + + + + + + +//iterative lcp and penalty method +float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) +{ + m_CurBody = 0; + m_CurJoint = 0; + + + RigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES]; + + int numBodies = 0; + BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4]; + int numJoints = 0; + + for (int j=0;jGetNumContacts() > 0) + { + body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); + body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); + ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer); + } + } + + SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal); + + return 0.f; + +} + +///////////////////////////////////////////////////////////////////////////////// + + +typedef SimdScalar dQuaternion[4]; +#define _R(i,j) R[(i)*4+(j)] + +void dRfromQ1 (dMatrix3 R, const dQuaternion q) +{ + // q = (s,vx,vy,vz) + SimdScalar qq1 = 2.f*q[1]*q[1]; + SimdScalar qq2 = 2.f*q[2]*q[2]; + SimdScalar qq3 = 2.f*q[3]*q[3]; + _R(0,0) = 1.f - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = 0.f; + + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1.f - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = 0.f; + + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1.f - qq1 - qq2; + _R(2,3) = 0.f; + +} + + + +int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) +{ + if (!body || (body->getInvMass() == 0.f) ) + { + return -1; + } + //first try to find + int i,j; + for (i=0;im_facc.setValue(0,0,0,0); + body->m_tacc.setValue(0,0,0,0); + + //are the indices the same ? + for (i=0;i<4;i++) + { + for ( j=0;j<3;j++) + { + body->m_invI[i+4*j] = 0.f; + body->m_I[i+4*j] = 0.f; + } + } + body->m_invI[0+4*0] = body->getInvInertiaDiagLocal()[0]; + body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1]; + body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2]; + + body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0]; + body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1]; + body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2]; + + + + + dQuaternion q; + + q[1] = body->getOrientation()[0]; + q[2] = body->getOrientation()[1]; + q[3] = body->getOrientation()[2]; + q[0] = body->getOrientation()[3]; + + dRfromQ1(body->m_R,q); + + return numBodies-1; +} + + + + + +#define MAX_JOINTS_1 65535 + +static ContactJoint gJointArray[MAX_JOINTS_1]; + + +void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer) +{ + + + manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(), + ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform()); + + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + + int i,numContacts = manifold->GetNumContacts(); + + bool swapBodies = (bodyId0 < 0); + + + RigidBody* body0,*body1; + + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; + + body0 = (RigidBody*)manifold->GetBody1(); + body1 = (RigidBody*)manifold->GetBody0(); + + } else + { + body0 = (RigidBody*)manifold->GetBody0(); + body1 = (RigidBody*)manifold->GetBody1(); + } + + assert(bodyId0 >= 0); + + SimdVector3 color(0,1,0); + for (i=0;iGetContactPoint(i); + + debugDrawer->DrawContactPoint( + cp.m_positionWorldOnB, + cp.m_normalWorldOnB, + cp.GetDistance(), + cp.GetLifeTime(), + color); + + } + assert (m_CurJoint < MAX_JOINTS_1); + +// if (manifold->GetContactPoint(i).GetDistance() < 0.0f) + { + ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); + + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; + + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + joints[numJoints++] = cont; + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; + } + } + + //create a new contact constraint +}; + diff --git a/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h b/Extras/quickstep/OdeConstraintSolver.h similarity index 97% rename from BulletDynamics/ConstraintSolver/OdeConstraintSolver.h rename to Extras/quickstep/OdeConstraintSolver.h index 99840557a..f194e0f70 100644 --- a/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h +++ b/Extras/quickstep/OdeConstraintSolver.h @@ -1,66 +1,66 @@ -/* -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 ODE_CONSTRAINT_SOLVER_H -#define ODE_CONSTRAINT_SOLVER_H - -#include "ConstraintSolver.h" - -class RigidBody; -class BU_Joint; - -/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework -/// It uses the the unmodified version of quickstep solver from the open dynamics project -class OdeConstraintSolver : public ConstraintSolver -{ -private: - - int m_CurBody; - int m_CurJoint; - - float m_cfm; - float m_erp; - - - int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); - void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, - RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer); - -public: - - OdeConstraintSolver(); - - virtual ~OdeConstraintSolver() {} - - virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0); - - ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal - ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' - void setConstraintForceMixing(float cfm) { - m_cfm = cfm; - } - - ///setErrorReductionParamter sets the maximum amount of error reduction - ///which limits energy addition during penetration depth recovery - void setErrorReductionParamter(float erp) - { - m_erp = erp; - } -}; - - - - -#endif //ODE_CONSTRAINT_SOLVER_H +/* +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 ODE_CONSTRAINT_SOLVER_H +#define ODE_CONSTRAINT_SOLVER_H + +#include "ConstraintSolver.h" + +class RigidBody; +class BU_Joint; + +/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework +/// It uses the the unmodified version of quickstep solver from the open dynamics project +class OdeConstraintSolver : public ConstraintSolver +{ +private: + + int m_CurBody; + int m_CurJoint; + + float m_cfm; + float m_erp; + + + int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); + void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer); + +public: + + OdeConstraintSolver(); + + virtual ~OdeConstraintSolver() {} + + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0); + + ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal + ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' + void setConstraintForceMixing(float cfm) { + m_cfm = cfm; + } + + ///setErrorReductionParamter sets the maximum amount of error reduction + ///which limits energy addition during penetration depth recovery + void setErrorReductionParamter(float erp) + { + m_erp = erp; + } +}; + + + + +#endif //ODE_CONSTRAINT_SOLVER_H diff --git a/BulletDynamics/ConstraintSolver/SorLcp.cpp b/Extras/quickstep/SorLcp.cpp similarity index 96% rename from BulletDynamics/ConstraintSolver/SorLcp.cpp rename to Extras/quickstep/SorLcp.cpp index 44ed3f057..0058b6e14 100644 --- a/BulletDynamics/ConstraintSolver/SorLcp.cpp +++ b/Extras/quickstep/SorLcp.cpp @@ -1,849 +1,849 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include "SorLcp.h" - -#ifdef USE_SOR_SOLVER - -// SOR LCP taken from ode quickstep, -// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver - - -#include "SimdScalar.h" - -#include "Dynamics/RigidBody.h" -#include -#include //FLT_MAX -#ifdef WIN32 -#include -#endif -#include -#include - -#if defined (WIN32) -#include -#else -#if defined (__FreeBSD__) -#include -#else -#include -#endif -#endif - -#include "Dynamics/BU_Joint.h" -#include "ContactSolverInfo.h" - -//////////////////////////////////////////////////////////////////// -//math stuff - -typedef SimdScalar dVector4[4]; -typedef SimdScalar dMatrix3[4*3]; -#define dInfinity FLT_MAX - - - -#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ - - - -#define dMULTIPLY0_331NEW(A,op,B,C) \ -{\ - float tmp[3];\ - tmp[0] = C.getX();\ - tmp[1] = C.getY();\ - tmp[2] = C.getZ();\ - dMULTIPLYOP0_331(A,op,B,tmp);\ -} - -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) -#define dMULTIPLYOP0_331(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B+4),(C)); \ - (A)[2] op dDOT1((B+8),(C)); - -#define dAASSERT ASSERT -#define dIASSERT ASSERT - -#define REAL float -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -SimdScalar dDOT1 (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); } -#define dDOT14(a,b) dDOTpq(a,b,1,4) - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - - -#define dMULTIPLYOP2_333(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B),(C+4)); \ - (A)[2] op dDOT1((B),(C+8)); \ - (A)[4] op dDOT1((B+4),(C)); \ - (A)[5] op dDOT1((B+4),(C+4)); \ - (A)[6] op dDOT1((B+4),(C+8)); \ - (A)[8] op dDOT1((B+8),(C)); \ - (A)[9] op dDOT1((B+8),(C+4)); \ - (A)[10] op dDOT1((B+8),(C+8)); -#define dMULTIPLYOP0_333(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); \ - (A)[4] op dDOT14((B+4),(C)); \ - (A)[5] op dDOT14((B+4),(C+1)); \ - (A)[6] op dDOT14((B+4),(C+2)); \ - (A)[8] op dDOT14((B+8),(C)); \ - (A)[9] op dDOT14((B+8),(C+1)); \ - (A)[10] op dDOT14((B+8),(C+2)); - -#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) -#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) -#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) - - -//////////////////////////////////////////////////////////////////// -#define EFFICIENT_ALIGNMENT 16 -#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) -/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste - * up to 15 bytes per allocation, depending on what alloca() returns. - */ - -#define dALLOCA16(n) \ - ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) - - - -///////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////// - -#ifdef DEBUG -#define ANSI_FTOL 1 - -extern "C" { - __declspec(naked) void _ftol2() { - __asm { -#if ANSI_FTOL - fnstcw WORD PTR [esp-2] - mov ax, WORD PTR [esp-2] - - OR AX, 0C00h - - mov WORD PTR [esp-4], ax - fldcw WORD PTR [esp-4] - fistp QWORD PTR [esp-12] - fldcw WORD PTR [esp-2] - mov eax, DWORD PTR [esp-12] - mov edx, DWORD PTR [esp-8] -#else - fistp DWORD PTR [esp-12] - mov eax, DWORD PTR [esp-12] - mov ecx, DWORD PTR [esp-8] -#endif - ret - } - } -} -#endif //DEBUG - - - - - - -#define ALLOCA dALLOCA16 - -typedef const SimdScalar *dRealPtr; -typedef SimdScalar *dRealMutablePtr; -#define dRealArray(name,n) SimdScalar name[n]; -#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar)); - -void dSetZero1 (SimdScalar *a, int n) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = 0; - n--; - } -} - -void dSetValue1 (SimdScalar *a, int n, SimdScalar value) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = value; - n--; - } -} - - -//*************************************************************************** -// configuration - -// for the SOR and CG methods: -// uncomment the following line to use warm starting. this definitely -// help for motor-driven joints. unfortunately it appears to hurt -// with high-friction contacts using the SOR method. use with care - -//#define WARM_STARTING 1 - -// for the SOR method: -// uncomment the following line to randomly reorder constraint rows -// during the solution. depending on the situation, this can help a lot -// or hardly at all, but it doesn't seem to hurt. - -#define RANDOMLY_REORDER_CONSTRAINTS 1 - - - -//*************************************************************************** -// various common computations involving the matrix J - -// compute iMJ = inv(M)*J' - -static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, - RigidBody * const *body, dRealPtr invI) -{ - int i,j; - dRealMutablePtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; igetInvMass(); - for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; - dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); - if (b2 >= 0) { - k = body[b2]->getInvMass(); - for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; - dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); - } - J_ptr += 12; - iMJ_ptr += 12; - } -} - -#if 0 -static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) -{ - int i,j; - - - - dRealMutablePtr out_ptr1 = out + onlyBody1*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - - if (onlyBody2 >= 0) - { - out_ptr1 = out + onlyBody2*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - } - - dRealPtr iMJ_ptr = iMJ; - for (i=0; i= 0) - { - out_ptr = out + b2*6; - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i]; - } - } - - iMJ_ptr += 6; - - } -} -#endif - - -// compute out = inv(M)*J'*in. - -#if 0 -static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dSetZero1 (out,6*nb); - dRealPtr iMJ_ptr = iMJ; - for (i=0; i= 0) { - out_ptr = out + b2*6; - for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; - } - iMJ_ptr += 6; - } -} -#endif - - -// compute out = J*in. - -static void multiply_J (int m, dRealMutablePtr J, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dRealPtr J_ptr = J; - for (i=0; i= 0) { - in_ptr = in + b2*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; - } - J_ptr += 6; - out[i] = sum; - } -} - -//*************************************************************************** -// SOR-LCP method - -// nb is the number of bodies in the body array. -// J is an m*12 matrix of constraint rows -// jb is an array of first and second body numbers for each constraint row -// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) -// -// this returns lambda and fc (the constraint force). -// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda -// -// b, lo and hi are modified on exit - - -struct IndexError { - SimdScalar error; // error to sort on - int findex; - int index; // row index -}; - -static unsigned long seed2 = 0; - -unsigned long dRand2() -{ - seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff; - return seed2; -} - -int dRandInt2 (int n) -{ - float a = float(n) / 4294967296.0f; - return (int) (float(dRand2()) * a); -} - - -static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body, - dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, - dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, - int numiter,float overRelax) -{ - const int num_iterations = numiter; - const float sor_w = overRelax; // SOR over-relaxation parameter - - int i,j; - -#ifdef WARM_STARTING - // for warm starting, this seems to be necessary to prevent - // jerkiness in motor-driven joints. i have no idea why this works. - for (i=0; i= 0) { - for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; - } - iMJ_ptr += 12; - J_ptr += 12; - Ad[i] = sor_w / sum;//(sum + cfm[i]); - } - - // scale J and b by Ad - J_ptr = J; - for (i=0; i= 0) - order[j++].index = i; - dIASSERT (j==m); -#endif - - for (int iteration=0; iteration < num_iterations; iteration++) { - -#ifdef REORDER_CONSTRAINTS - // constraints with findex < 0 always come first. - if (iteration < 2) { - // for the first two iterations, solve the constraints in - // the given order - for (i=0; i v2) ? v1 : v2; - if (max > 0) { - //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; - order[i].error = dFabs(lambda[i]-last_lambda[i]); - } - else { - order[i].error = dInfinity; - } - order[i].findex = findex[i]; - order[i].index = i; - } - } - qsort (order,m,sizeof(IndexError),&compare_index_error); -#endif -#ifdef RANDOMLY_REORDER_CONSTRAINTS - if ((iteration & 7) == 0) { - for (i=1; i= 0) { - hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]); - lo[index] = -hi[index]; - } - - int b1 = jb[index*2]; - int b2 = jb[index*2+1]; - float delta = rhs[index] - lambda[index]*Ad[index]; - dRealMutablePtr fc_ptr = invMforce + 6*b1; - - // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case - delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + - fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + - fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + - fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + - fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; - } - - // compute lambda and clamp it to [lo,hi]. - // @@@ potential optimization: does SSE have clamping instructions - // to save test+jump penalties here? - float new_lambda = lambda[index] + delta; - if (new_lambda < lo[index]) { - delta = lo[index]-lambda[index]; - lambda[index] = lo[index]; - } - else if (new_lambda > hi[index]) { - delta = hi[index]-lambda[index]; - lambda[index] = hi[index]; - } - else { - lambda[index] = new_lambda; - } - - //@@@ a trick that may or may not help - //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); - //delta *= ramp; - - // update invMforce. - // @@@ potential optimization: SIMD for this and the b2 >= 0 case - fc_ptr = invMforce + 6*b1; - fc_ptr[0] += delta * iMJ_ptr[0]; - fc_ptr[1] += delta * iMJ_ptr[1]; - fc_ptr[2] += delta * iMJ_ptr[2]; - fc_ptr[3] += delta * iMJ_ptr[3]; - fc_ptr[4] += delta * iMJ_ptr[4]; - fc_ptr[5] += delta * iMJ_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - fc_ptr[0] += delta * iMJ_ptr[6]; - fc_ptr[1] += delta * iMJ_ptr[7]; - fc_ptr[2] += delta * iMJ_ptr[8]; - fc_ptr[3] += delta * iMJ_ptr[9]; - fc_ptr[4] += delta * iMJ_ptr[10]; - fc_ptr[5] += delta * iMJ_ptr[11]; - } - } - } -} - - - -void SolveInternal1 (float global_cfm, - float global_erp, - RigidBody * const *body, int nb, - BU_Joint * const *_joint, - int nj, - const ContactSolverInfo& solverInfo) -{ - - int numIter = solverInfo.m_numIterations; - float sOr = solverInfo.m_sor; - - int i,j; - - SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep); - - // number all bodies in the body list - set their tag values - for (i=0; im_odeTag = i; - - // make a local copy of the joint array, because we might want to modify it. - // (the "BU_Joint *const*" declaration says we're allowed to modify the joints - // but not the joint array, because the caller might need it unchanged). - //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints - BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*)); - memcpy (joint,_joint,nj * sizeof(BU_Joint*)); - - // for all bodies, compute the inertia tensor and its inverse in the global - // frame, and compute the rotational force and add it to the torque - // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. - dRealAllocaArray (I,3*4*nb); - dRealAllocaArray (invI,3*4*nb); -/* for (i=0; im_I,body[i]->m_R); - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force - dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } -*/ - for (i=0; im_I,body[i]->m_R); - dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); - - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force - dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity()); - dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } - - - - - // get joint information (m = total constraint dimension, nub = number of unbounded variables). - // joints with m=0 are inactive and are removed from the joints array - // entirely, so that the code that follows does not consider them. - //@@@ do we really need to save all the info1's - BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1)); - for (i=0, j=0; jGetInfo1 (info+i); - dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); - if (info[i].m > 0) { - joint[i] = joint[j]; - i++; - } - } - nj = i; - - // create the row offset array - int m = 0; - int *ofs = (int*) alloca (nj*sizeof(int)); - for (i=0; i 0) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. - dRealAllocaArray (c,m); - dRealAllocaArray (cfm,m); - dRealAllocaArray (lo,m); - dRealAllocaArray (hi,m); - int *findex = (int*) alloca (m*sizeof(int)); - dSetZero1 (c,m); - dSetValue1 (cfm,m,global_cfm); - dSetValue1 (lo,m,-dInfinity); - dSetValue1 (hi,m, dInfinity); - for (i=0; iGetInfo2 (&Jinfo); - - if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) - Jinfo.c[0] = solverInfo.m_maxErrorReduction; - - - - - // adjust returned findex values for global index numbering - for (j=0; j= 0) - findex[ofs[i] + j] += ofs[i]; - } - } - - // create an array of body numbers for each joint row - int *jb_ptr = jb; - for (i=0; inode[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; - int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; - for (j=0; jgetInvMass(); - for (j=0; j<3; j++) - tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1; - dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); - for (j=0; j<3; j++) - tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1; - } - - // put J*tmp1 into rhs - dRealAllocaArray (rhs,m); - multiply_J (m,J,jb,tmp1,rhs); - - // complete rhs - for (i=0; ilambda,info[i].m * sizeof(SimdScalar)); - } -#endif - - // solve the LCP problem and get lambda and invM*constraint_force - dRealAllocaArray (cforce,nb*6); - - SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr); - -#ifdef WARM_STARTING - // save lambda for the next iteration - //@@@ note that this doesn't work for contact joints yet, as they are - // recreated every iteration - for (i=0; ilambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar)); - } -#endif - - - // note that the SOR method overwrites rhs and J at this point, so - // they should not be used again. - - // add stepsize * cforce to the body velocity - for (i=0; igetLinearVelocity(); - SimdVector3 angvel = body[i]->getAngularVelocity(); - - for (j=0; j<3; j++) - linvel[j] += solverInfo.m_timeStep* cforce[i*6+j]; - for (j=0; j<3; j++) - angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; - - body[i]->setLinearVelocity(linvel); - body[i]->setAngularVelocity(angvel); - - } - - } - - - - // compute the velocity update: - // add stepsize * invM * fe to the body velocity - - for (i=0; igetInvMass(); - SimdVector3 linvel = body[i]->getLinearVelocity(); - SimdVector3 angvel = body[i]->getAngularVelocity(); - - for (j=0; j<3; j++) - { - linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; - } - for (j=0; j<3; j++) - { - body[i]->m_tacc[j] *= solverInfo.m_timeStep; - } - dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); - body[i]->setAngularVelocity(angvel); - - } - - - -} - - -#endif //USE_SOR_SOLVER +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include "SorLcp.h" + +#ifdef USE_SOR_SOLVER + +// SOR LCP taken from ode quickstep, +// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver + + +#include "SimdScalar.h" + +#include "Dynamics/RigidBody.h" +#include +#include //FLT_MAX +#ifdef WIN32 +#include +#endif +#include +#include + +#if defined (WIN32) +#include +#else +#if defined (__FreeBSD__) +#include +#else +#include +#endif +#endif + +#include "Dynamics/BU_Joint.h" +#include "ContactSolverInfo.h" + +//////////////////////////////////////////////////////////////////// +//math stuff + +typedef SimdScalar dVector4[4]; +typedef SimdScalar dMatrix3[4*3]; +#define dInfinity FLT_MAX + + + +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ + + + +#define dMULTIPLY0_331NEW(A,op,B,C) \ +{\ + float tmp[3];\ + tmp[0] = C.getX();\ + tmp[1] = C.getY();\ + tmp[2] = C.getZ();\ + dMULTIPLYOP0_331(A,op,B,tmp);\ +} + +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLYOP0_331(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B+4),(C)); \ + (A)[2] op dDOT1((B+8),(C)); + +#define dAASSERT ASSERT +#define dIASSERT ASSERT + +#define REAL float +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +SimdScalar dDOT1 (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); } +#define dDOT14(a,b) dDOTpq(a,b,1,4) + +#define dCROSS(a,op,b,c) \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); + + +#define dMULTIPLYOP2_333(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B),(C+4)); \ + (A)[2] op dDOT1((B),(C+8)); \ + (A)[4] op dDOT1((B+4),(C)); \ + (A)[5] op dDOT1((B+4),(C+4)); \ + (A)[6] op dDOT1((B+4),(C+8)); \ + (A)[8] op dDOT1((B+8),(C)); \ + (A)[9] op dDOT1((B+8),(C+4)); \ + (A)[10] op dDOT1((B+8),(C+8)); +#define dMULTIPLYOP0_333(A,op,B,C) \ + (A)[0] op dDOT14((B),(C)); \ + (A)[1] op dDOT14((B),(C+1)); \ + (A)[2] op dDOT14((B),(C+2)); \ + (A)[4] op dDOT14((B+4),(C)); \ + (A)[5] op dDOT14((B+4),(C+1)); \ + (A)[6] op dDOT14((B+4),(C+2)); \ + (A)[8] op dDOT14((B+8),(C)); \ + (A)[9] op dDOT14((B+8),(C+1)); \ + (A)[10] op dDOT14((B+8),(C+2)); + +#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) +#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) +#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) + + +//////////////////////////////////////////////////////////////////// +#define EFFICIENT_ALIGNMENT 16 +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + + + +///////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////// + +#ifdef DEBUG +#define ANSI_FTOL 1 + +extern "C" { + __declspec(naked) void _ftol2() { + __asm { +#if ANSI_FTOL + fnstcw WORD PTR [esp-2] + mov ax, WORD PTR [esp-2] + + OR AX, 0C00h + + mov WORD PTR [esp-4], ax + fldcw WORD PTR [esp-4] + fistp QWORD PTR [esp-12] + fldcw WORD PTR [esp-2] + mov eax, DWORD PTR [esp-12] + mov edx, DWORD PTR [esp-8] +#else + fistp DWORD PTR [esp-12] + mov eax, DWORD PTR [esp-12] + mov ecx, DWORD PTR [esp-8] +#endif + ret + } + } +} +#endif //DEBUG + + + + + + +#define ALLOCA dALLOCA16 + +typedef const SimdScalar *dRealPtr; +typedef SimdScalar *dRealMutablePtr; +#define dRealArray(name,n) SimdScalar name[n]; +#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar)); + +void dSetZero1 (SimdScalar *a, int n) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = 0; + n--; + } +} + +void dSetValue1 (SimdScalar *a, int n, SimdScalar value) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = value; + n--; + } +} + + +//*************************************************************************** +// configuration + +// for the SOR and CG methods: +// uncomment the following line to use warm starting. this definitely +// help for motor-driven joints. unfortunately it appears to hurt +// with high-friction contacts using the SOR method. use with care + +//#define WARM_STARTING 1 + +// for the SOR method: +// uncomment the following line to randomly reorder constraint rows +// during the solution. depending on the situation, this can help a lot +// or hardly at all, but it doesn't seem to hurt. + +#define RANDOMLY_REORDER_CONSTRAINTS 1 + + + +//*************************************************************************** +// various common computations involving the matrix J + +// compute iMJ = inv(M)*J' + +static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, + RigidBody * const *body, dRealPtr invI) +{ + int i,j; + dRealMutablePtr iMJ_ptr = iMJ; + dRealMutablePtr J_ptr = J; + for (i=0; igetInvMass(); + for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; + dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); + if (b2 >= 0) { + k = body[b2]->getInvMass(); + for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; + dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); + } + J_ptr += 12; + iMJ_ptr += 12; + } +} + +#if 0 +static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) +{ + int i,j; + + + + dRealMutablePtr out_ptr1 = out + onlyBody1*6; + + for (j=0; j<6; j++) + out_ptr1[j] = 0; + + if (onlyBody2 >= 0) + { + out_ptr1 = out + onlyBody2*6; + + for (j=0; j<6; j++) + out_ptr1[j] = 0; + } + + dRealPtr iMJ_ptr = iMJ; + for (i=0; i= 0) + { + out_ptr = out + b2*6; + for (j=0; j<6; j++) + out_ptr[j] += iMJ_ptr[j] * in[i]; + } + } + + iMJ_ptr += 6; + + } +} +#endif + + +// compute out = inv(M)*J'*in. + +#if 0 +static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dSetZero1 (out,6*nb); + dRealPtr iMJ_ptr = iMJ; + for (i=0; i= 0) { + out_ptr = out + b2*6; + for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; + } + iMJ_ptr += 6; + } +} +#endif + + +// compute out = J*in. + +static void multiply_J (int m, dRealMutablePtr J, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dRealPtr J_ptr = J; + for (i=0; i= 0) { + in_ptr = in + b2*6; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + } + J_ptr += 6; + out[i] = sum; + } +} + +//*************************************************************************** +// SOR-LCP method + +// nb is the number of bodies in the body array. +// J is an m*12 matrix of constraint rows +// jb is an array of first and second body numbers for each constraint row +// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) +// +// this returns lambda and fc (the constraint force). +// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda +// +// b, lo and hi are modified on exit + + +struct IndexError { + SimdScalar error; // error to sort on + int findex; + int index; // row index +}; + +static unsigned long seed2 = 0; + +unsigned long dRand2() +{ + seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff; + return seed2; +} + +int dRandInt2 (int n) +{ + float a = float(n) / 4294967296.0f; + return (int) (float(dRand2()) * a); +} + + +static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body, + dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, + dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, + int numiter,float overRelax) +{ + const int num_iterations = numiter; + const float sor_w = overRelax; // SOR over-relaxation parameter + + int i,j; + +#ifdef WARM_STARTING + // for warm starting, this seems to be necessary to prevent + // jerkiness in motor-driven joints. i have no idea why this works. + for (i=0; i= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = sor_w / sum;//(sum + cfm[i]); + } + + // scale J and b by Ad + J_ptr = J; + for (i=0; i= 0) + order[j++].index = i; + dIASSERT (j==m); +#endif + + for (int iteration=0; iteration < num_iterations; iteration++) { + +#ifdef REORDER_CONSTRAINTS + // constraints with findex < 0 always come first. + if (iteration < 2) { + // for the first two iterations, solve the constraints in + // the given order + for (i=0; i v2) ? v1 : v2; + if (max > 0) { + //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; + order[i].error = dFabs(lambda[i]-last_lambda[i]); + } + else { + order[i].error = dInfinity; + } + order[i].findex = findex[i]; + order[i].index = i; + } + } + qsort (order,m,sizeof(IndexError),&compare_index_error); +#endif +#ifdef RANDOMLY_REORDER_CONSTRAINTS + if ((iteration & 7) == 0) { + for (i=1; i= 0) { + hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]); + lo[index] = -hi[index]; + } + + int b1 = jb[index*2]; + int b2 = jb[index*2+1]; + float delta = rhs[index] - lambda[index]*Ad[index]; + dRealMutablePtr fc_ptr = invMforce + 6*b1; + + // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case + delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + + fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = invMforce + 6*b2; + delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + + fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + + fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; + } + + // compute lambda and clamp it to [lo,hi]. + // @@@ potential optimization: does SSE have clamping instructions + // to save test+jump penalties here? + float new_lambda = lambda[index] + delta; + if (new_lambda < lo[index]) { + delta = lo[index]-lambda[index]; + lambda[index] = lo[index]; + } + else if (new_lambda > hi[index]) { + delta = hi[index]-lambda[index]; + lambda[index] = hi[index]; + } + else { + lambda[index] = new_lambda; + } + + //@@@ a trick that may or may not help + //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); + //delta *= ramp; + + // update invMforce. + // @@@ potential optimization: SIMD for this and the b2 >= 0 case + fc_ptr = invMforce + 6*b1; + fc_ptr[0] += delta * iMJ_ptr[0]; + fc_ptr[1] += delta * iMJ_ptr[1]; + fc_ptr[2] += delta * iMJ_ptr[2]; + fc_ptr[3] += delta * iMJ_ptr[3]; + fc_ptr[4] += delta * iMJ_ptr[4]; + fc_ptr[5] += delta * iMJ_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = invMforce + 6*b2; + fc_ptr[0] += delta * iMJ_ptr[6]; + fc_ptr[1] += delta * iMJ_ptr[7]; + fc_ptr[2] += delta * iMJ_ptr[8]; + fc_ptr[3] += delta * iMJ_ptr[9]; + fc_ptr[4] += delta * iMJ_ptr[10]; + fc_ptr[5] += delta * iMJ_ptr[11]; + } + } + } +} + + + +void SolveInternal1 (float global_cfm, + float global_erp, + RigidBody * const *body, int nb, + BU_Joint * const *_joint, + int nj, + const ContactSolverInfo& solverInfo) +{ + + int numIter = solverInfo.m_numIterations; + float sOr = solverInfo.m_sor; + + int i,j; + + SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep); + + // number all bodies in the body list - set their tag values + for (i=0; im_odeTag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "BU_Joint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints + BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*)); + memcpy (joint,_joint,nj * sizeof(BU_Joint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. + dRealAllocaArray (I,3*4*nb); + dRealAllocaArray (invI,3*4*nb); +/* for (i=0; im_I,body[i]->m_R); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); + dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); + // compute rotational force + dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); + } +*/ + for (i=0; im_I,body[i]->m_R); + dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); + + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); + dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity()); + dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); + } + + + + + // get joint information (m = total constraint dimension, nub = number of unbounded variables). + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + //@@@ do we really need to save all the info1's + BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1)); + for (i=0, j=0; jGetInfo1 (info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // create the row offset array + int m = 0; + int *ofs = (int*) alloca (nj*sizeof(int)); + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dRealAllocaArray (c,m); + dRealAllocaArray (cfm,m); + dRealAllocaArray (lo,m); + dRealAllocaArray (hi,m); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero1 (c,m); + dSetValue1 (cfm,m,global_cfm); + dSetValue1 (lo,m,-dInfinity); + dSetValue1 (hi,m, dInfinity); + for (i=0; iGetInfo2 (&Jinfo); + + if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) + Jinfo.c[0] = solverInfo.m_maxErrorReduction; + + + + + // adjust returned findex values for global index numbering + for (j=0; j= 0) + findex[ofs[i] + j] += ofs[i]; + } + } + + // create an array of body numbers for each joint row + int *jb_ptr = jb; + for (i=0; inode[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; + int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; + for (j=0; jgetInvMass(); + for (j=0; j<3; j++) + tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1; + dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); + for (j=0; j<3; j++) + tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1; + } + + // put J*tmp1 into rhs + dRealAllocaArray (rhs,m); + multiply_J (m,J,jb,tmp1,rhs); + + // complete rhs + for (i=0; ilambda,info[i].m * sizeof(SimdScalar)); + } +#endif + + // solve the LCP problem and get lambda and invM*constraint_force + dRealAllocaArray (cforce,nb*6); + + SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr); + +#ifdef WARM_STARTING + // save lambda for the next iteration + //@@@ note that this doesn't work for contact joints yet, as they are + // recreated every iteration + for (i=0; ilambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar)); + } +#endif + + + // note that the SOR method overwrites rhs and J at this point, so + // they should not be used again. + + // add stepsize * cforce to the body velocity + for (i=0; igetLinearVelocity(); + SimdVector3 angvel = body[i]->getAngularVelocity(); + + for (j=0; j<3; j++) + linvel[j] += solverInfo.m_timeStep* cforce[i*6+j]; + for (j=0; j<3; j++) + angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; + + body[i]->setLinearVelocity(linvel); + body[i]->setAngularVelocity(angvel); + + } + + } + + + + // compute the velocity update: + // add stepsize * invM * fe to the body velocity + + for (i=0; igetInvMass(); + SimdVector3 linvel = body[i]->getLinearVelocity(); + SimdVector3 angvel = body[i]->getAngularVelocity(); + + for (j=0; j<3; j++) + { + linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; + } + for (j=0; j<3; j++) + { + body[i]->m_tacc[j] *= solverInfo.m_timeStep; + } + dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); + body[i]->setAngularVelocity(angvel); + + } + + + +} + + +#endif //USE_SOR_SOLVER diff --git a/BulletDynamics/ConstraintSolver/SorLcp.h b/Extras/quickstep/SorLcp.h similarity index 97% rename from BulletDynamics/ConstraintSolver/SorLcp.h rename to Extras/quickstep/SorLcp.h index 37fe09c26..2948fca30 100644 --- a/BulletDynamics/ConstraintSolver/SorLcp.h +++ b/Extras/quickstep/SorLcp.h @@ -1,45 +1,45 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#define USE_SOR_SOLVER -#ifdef USE_SOR_SOLVER - -#ifndef SOR_LCP_H -#define SOR_LCP_H -class RigidBody; -class BU_Joint; -#include "SimdScalar.h" - -struct ContactSolverInfo; - -void SolveInternal1 (float global_cfm, - float global_erp, - RigidBody * const *body, int nb, - BU_Joint * const *_joint, int nj, const ContactSolverInfo& info); - -int dRandInt2 (int n); - - -#endif //SOR_LCP_H - -#endif //USE_SOR_SOLVER - +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#define USE_SOR_SOLVER +#ifdef USE_SOR_SOLVER + +#ifndef SOR_LCP_H +#define SOR_LCP_H +class RigidBody; +class BU_Joint; +#include "SimdScalar.h" + +struct ContactSolverInfo; + +void SolveInternal1 (float global_cfm, + float global_erp, + RigidBody * const *body, int nb, + BU_Joint * const *_joint, int nj, const ContactSolverInfo& info); + +int dRandInt2 (int n); + + +#endif //SOR_LCP_H + +#endif //USE_SOR_SOLVER + diff --git a/msvc/8/libbullet.vcproj b/msvc/8/libbullet.vcproj index 603e29d20..78481a352 100644 --- a/msvc/8/libbullet.vcproj +++ b/msvc/8/libbullet.vcproj @@ -1,15 +1,17 @@ - + + > + Name="Win32" + /> + + + ATLMinimizesCRunTimeLibraryUsage="false" + > + + + + + + SuppressStartupBanner="true" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="3" + CompileAs="0" + /> - - - - - - + Name="VCManagedResourceCompilerTool" + /> + /> + Name="VCPreLinkEventTool" + /> + Name="VCLibrarianTool" + OutputFile="..\..\out\release8\libs\libbullet.lib" + SuppressStartupBanner="true" + /> + + + + + + ATLMinimizesCRunTimeLibraryUsage="false" + > + + + + + + SuppressStartupBanner="true" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="4" + CompileAs="0" + /> - - - - - - + Name="VCManagedResourceCompilerTool" + /> + /> + Name="VCPreLinkEventTool" + /> + Name="VCLibrarianTool" + OutputFile="..\..\out\debug8\libs\libbullet_d.lib" + SuppressStartupBanner="true" + /> + + + + + + + + > + RelativePath="..\..\Bullet\BroadphaseCollision\AxisSweep3.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\BoxShape.cpp" + > + RelativePath="..\..\Bullet\BroadphaseCollision\BroadphaseProxy.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\BvhTriangleMeshShape.cpp" + > + RelativePath="..\..\Bullet\BroadphaseCollision\CollisionAlgorithm.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\CollisionDispatcher.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\CollisionObject.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\CollisionShape.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\CollisionWorld.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\CompoundCollisionAlgorithm.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\CompoundShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\ConeShape.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ContinuousConvexCollision.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ConvexCast.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\ConvexConvexAlgorithm.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\ConvexHullShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\ConvexShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\ConvexTriangleMeshShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\CylinderShape.cpp" + > + RelativePath="..\..\Bullet\BroadphaseCollision\Dispatcher.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\EmptyCollisionAlgorithm.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\EmptyShape.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\GjkConvexCast.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\GjkPairDetector.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\Hull.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ManifoldContactAddResult.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\ManifoldResult.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\MinkowskiSumShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\MultiSphereShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\OptimizedBvh.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\PersistentManifold.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\PolyhedralConvexShape.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\RaycastCallback.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\Shape.cpp" + > + RelativePath="..\..\Bullet\BroadphaseCollision\SimpleBroadphase.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\Simplex1to4Shape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\SphereShape.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\StridingMeshInterface.cpp" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\SubSimplexConvexCast.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleCallback.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleIndexVertexArray.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleMesh.cpp" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleMeshShape.cpp" + > + RelativePath="..\..\Bullet\CollisionDispatch\UnionFind.cpp" + > - - - - - - - - - - - - - - - - - - - - + RelativePath="..\..\Bullet\NarrowPhaseCollision\VoronoiSimplexSolver.cpp" + > + > + RelativePath="..\..\Bullet\BroadphaseCollision\AxisSweep3.h" + > + RelativePath="..\..\Bullet\CollisionShapes\BoxShape.h" + > + RelativePath="..\..\Bullet\BroadphaseCollision\BroadphaseInterface.h" + > + RelativePath="..\..\Bullet\BroadphaseCollision\BroadphaseProxy.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_AlgebraicPolynomialSolver.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_Collidable.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_CollisionPair.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_EdgeEdge.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_MotionStateInterface.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_PolynomialSolverInterface.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_Screwing.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_StaticMotionState.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\BU_VertexPoly.h" + > + RelativePath="..\..\Bullet\CollisionShapes\BvhTriangleMeshShape.h" + > + RelativePath="..\..\Bullet\BroadphaseCollision\CollisionAlgorithm.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\CollisionDispatcher.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\CollisionMargin.h" + > + RelativePath="..\..\Bullet\CollisionShapes\CollisionMargin.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\CollisionObject.h" + > + RelativePath="..\..\Bullet\CollisionShapes\CollisionShape.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\CollisionWorld.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\CompoundCollisionAlgorithm.h" + > + RelativePath="..\..\Bullet\CollisionShapes\CompoundShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\ConeShape.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ContinuousConvexCollision.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ConvexCast.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\ConvexConcaveCollisionAlgorithm.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\ConvexConvexAlgorithm.h" + > + RelativePath="..\..\Bullet\CollisionShapes\ConvexHullShape.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ConvexPenetrationDepthSolver.h" + > + RelativePath="..\..\Bullet\CollisionShapes\ConvexShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\ConvexTriangleMeshShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\CylinderShape.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.h" + > + RelativePath="..\..\Bullet\BroadphaseCollision\Dispatcher.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\EmptyCollisionAlgorithm.h" + > + RelativePath="..\..\Bullet\CollisionShapes\EmptyShape.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\GjkConvexCast.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\GjkPairDetector.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\Hull.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\HullContactCollector.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ManifoldContactAddResult.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\ManifoldPoint.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\ManifoldResult.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.h" + > + RelativePath="..\..\Bullet\CollisionShapes\MinkowskiSumShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\MultiSphereShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\OptimizedBvh.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\PersistentManifold.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\PointCollector.h" + > + RelativePath="..\..\Bullet\CollisionShapes\PolyhedralConvexShape.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\RaycastCallback.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\Shape.h" + > + RelativePath="..\..\Bullet\BroadphaseCollision\SimpleBroadphase.h" + > + RelativePath="..\..\Bullet\CollisionShapes\Simplex1to4Shape.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\SimplexSolverInterface.h" + > + RelativePath="..\..\Bullet\CollisionShapes\SphereShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\StridingMeshInterface.h" + > + RelativePath="..\..\Bullet\NarrowPhaseCollision\SubSimplexConvexCast.h" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleCallback.h" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleIndexVertexArray.h" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleMesh.h" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleMeshShape.h" + > + RelativePath="..\..\Bullet\CollisionShapes\TriangleShape.h" + > + RelativePath="..\..\Bullet\CollisionDispatch\UnionFind.h" + > - - - - - - - - - - - - - - + RelativePath="..\..\Bullet\NarrowPhaseCollision\VoronoiSimplexSolver.h" + > + + + + diff --git a/msvc/8/libbulletccdphysics.vcproj b/msvc/8/libbulletccdphysics.vcproj index 1d7fc9b1b..26aee2746 100644 --- a/msvc/8/libbulletccdphysics.vcproj +++ b/msvc/8/libbulletccdphysics.vcproj @@ -1,15 +1,17 @@ - + + > + Name="Win32" + /> + + + ATLMinimizesCRunTimeLibraryUsage="false" + > + + + + + + SuppressStartupBanner="true" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="3" + CompileAs="0" + /> - - - - - - + Name="VCManagedResourceCompilerTool" + /> + /> + Name="VCPreLinkEventTool" + /> + Name="VCLibrarianTool" + OutputFile="..\..\out\release8\libs\libbulletccdphysics.lib" + SuppressStartupBanner="true" + /> + + + + + + ATLMinimizesCRunTimeLibraryUsage="false" + > + + + + + + SuppressStartupBanner="true" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="4" + CompileAs="0" + /> - - - - - - + Name="VCManagedResourceCompilerTool" + /> + /> + Name="VCPreLinkEventTool" + /> + Name="VCLibrarianTool" + OutputFile="..\..\out\debug8\libs\libbulletccdphysics_d.lib" + SuppressStartupBanner="true" + /> + + + + + + + + > + RelativePath="..\..\Extras\PhysicsInterface\CcdPhysics\CcdPhysicsController.cpp" + > + RelativePath="..\..\Extras\PhysicsInterface\CcdPhysics\CcdPhysicsEnvironment.cpp" + > + + + > + RelativePath="..\..\Extras\PhysicsInterface\CcdPhysics\CcdPhysicsController.h" + > + RelativePath="..\..\Extras\PhysicsInterface\CcdPhysics\CcdPhysicsEnvironment.h" + > + + diff --git a/msvc/8/libbulletdynamics.vcproj b/msvc/8/libbulletdynamics.vcproj index c32dda9b6..9615cab59 100644 --- a/msvc/8/libbulletdynamics.vcproj +++ b/msvc/8/libbulletdynamics.vcproj @@ -1,15 +1,17 @@ - + + > + Name="Win32" + /> + + + ATLMinimizesCRunTimeLibraryUsage="false" + > + + + + + + SuppressStartupBanner="true" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="3" + CompileAs="0" + /> - - - - - - + Name="VCManagedResourceCompilerTool" + /> + /> + Name="VCPreLinkEventTool" + /> + Name="VCLibrarianTool" + OutputFile="..\..\out\release8\libs\libbulletdynamics.lib" + SuppressStartupBanner="true" + /> + + + + + + ATLMinimizesCRunTimeLibraryUsage="false" + > + + + + + + SuppressStartupBanner="true" + Detect64BitPortabilityProblems="true" + DebugInformationFormat="4" + CompileAs="0" + /> - - - - - - + Name="VCManagedResourceCompilerTool" + /> + /> + Name="VCPreLinkEventTool" + /> + Name="VCLibrarianTool" + OutputFile="..\..\out\debug8\libs\libbulletdynamics_d.lib" + SuppressStartupBanner="true" + /> + + + + + + + + > + RelativePath="..\..\BulletDynamics\Dynamics\BU_Joint.cpp" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\ContactConstraint.cpp" + > + RelativePath="..\..\BulletDynamics\Dynamics\ContactJoint.cpp" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\Generic6DofConstraint.cpp" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\HingeConstraint.cpp" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\Point2PointConstraint.cpp" + > + RelativePath="..\..\BulletDynamics\Dynamics\RigidBody.cpp" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\Solve2LinearConstraint.cpp" + > - - - - - - + RelativePath="..\..\BulletDynamics\ConstraintSolver\TypedConstraint.cpp" + > + > + RelativePath="..\..\BulletDynamics\Dynamics\BU_Joint.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\ConstraintSolver.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\ContactConstraint.h" + > + RelativePath="..\..\BulletDynamics\Dynamics\ContactJoint.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\ContactSolverInfo.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\Generic6DofConstraint.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\HingeConstraint.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\JacobianEntry.h" + > + RelativePath="..\..\BulletDynamics\Dynamics\MassProps.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\OdeConstraintSolver.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\Point2PointConstraint.h" + > + RelativePath="..\..\BulletDynamics\Dynamics\RigidBody.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\SimpleConstraintSolver.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\Solve2LinearConstraint.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\SorLcp.h" + > + RelativePath="..\..\BulletDynamics\ConstraintSolver\TypedConstraint.h" + > + + + + diff --git a/msvc/8/wksbullet.sln b/msvc/8/wksbullet.sln index f95c5d372..aa8552ed6 100644 --- a/msvc/8/wksbullet.sln +++ b/msvc/8/wksbullet.sln @@ -1,34 +1,172 @@ Microsoft Visual Studio Solution File, Format Version 9.00 -# Visual C++ Express 2005 +# Visual Studio 2005 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appCcdPhysicsDemo", "appCcdPhysicsDemo.vcproj", "{7284F809-AF30-6315-88C6-86F1C0798760}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appColladaDemo", "appColladaDemo.vcproj", "{D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}" + ProjectSection(ProjectDependencies) = postProject + {A0958CD9-0E39-4A77-3711-9B488F508FBF} = {A0958CD9-0E39-4A77-3711-9B488F508FBF} + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appCollisionDemo", "appCollisionDemo.vcproj", "{E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appCollisionInterfaceDemo", "appCollisionInterfaceDemo.vcproj", "{F38629D2-EEB2-1A09-FB82-52B8A8DE759B}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appConcaveDemo", "appConcaveDemo.vcproj", "{B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appConstraintDemo", "appConstraintDemo.vcproj", "{DAA547D0-0166-C085-0F93-B88CAB800F97}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appContinuousConvexCollision", "appContinuousConvexCollision.vcproj", "{801CB6D4-A45C-C9D2-B176-9711A74B9164}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appConvexDecompositionDemo", "appConvexDecompositionDemo.vcproj", "{69C821C7-1E18-D894-068D-C55E063F4859}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appEPAPenDepthDemo", "appEPAPenDepthDemo.vcproj", "{1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}" + ProjectSection(ProjectDependencies) = postProject + {A0958CD9-0E39-4A77-3711-9B488F508FBF} = {A0958CD9-0E39-4A77-3711-9B488F508FBF} + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appGjkConvexCastDemo", "appGjkConvexCastDemo.vcproj", "{780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appRaytracer", "appRaytracer.vcproj", "{60F71B6A-F888-C449-EF49-268BB9F7C963}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "appSimplexDemo", "appSimplexDemo.vcproj", "{60A1DC9D-F837-3923-E9DE-A7925394A578}" + ProjectSection(ProjectDependencies) = postProject + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "grpall_bullet", "grpall_bullet.vcproj", "{6210A080-01C0-6D67-F1DB-669393175402}" + ProjectSection(ProjectDependencies) = postProject + {A0958CD9-0E39-4A77-3711-9B488F508FBF} = {A0958CD9-0E39-4A77-3711-9B488F508FBF} + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + {60A1DC9D-F837-3923-E9DE-A7925394A578} = {60A1DC9D-F837-3923-E9DE-A7925394A578} + {60F71B6A-F888-C449-EF49-268BB9F7C963} = {60F71B6A-F888-C449-EF49-268BB9F7C963} + {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1} = {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1} + {69C821C7-1E18-D894-068D-C55E063F4859} = {69C821C7-1E18-D894-068D-C55E063F4859} + {801CB6D4-A45C-C9D2-B176-9711A74B9164} = {801CB6D4-A45C-C9D2-B176-9711A74B9164} + {DAA547D0-0166-C085-0F93-B88CAB800F97} = {DAA547D0-0166-C085-0F93-B88CAB800F97} + {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3} = {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3} + {F38629D2-EEB2-1A09-FB82-52B8A8DE759B} = {F38629D2-EEB2-1A09-FB82-52B8A8DE759B} + {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9} = {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9} + {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF} = {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF} + {7284F809-AF30-6315-88C6-86F1C0798760} = {7284F809-AF30-6315-88C6-86F1C0798760} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "grpapps_bullet", "grpapps_bullet.vcproj", "{9E59B16D-0924-409C-1611-DF2207A0053F}" + ProjectSection(ProjectDependencies) = postProject + {60A1DC9D-F837-3923-E9DE-A7925394A578} = {60A1DC9D-F837-3923-E9DE-A7925394A578} + {60F71B6A-F888-C449-EF49-268BB9F7C963} = {60F71B6A-F888-C449-EF49-268BB9F7C963} + {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1} = {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1} + {69C821C7-1E18-D894-068D-C55E063F4859} = {69C821C7-1E18-D894-068D-C55E063F4859} + {801CB6D4-A45C-C9D2-B176-9711A74B9164} = {801CB6D4-A45C-C9D2-B176-9711A74B9164} + {DAA547D0-0166-C085-0F93-B88CAB800F97} = {DAA547D0-0166-C085-0F93-B88CAB800F97} + {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3} = {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3} + {F38629D2-EEB2-1A09-FB82-52B8A8DE759B} = {F38629D2-EEB2-1A09-FB82-52B8A8DE759B} + {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9} = {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9} + {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF} = {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF} + {7284F809-AF30-6315-88C6-86F1C0798760} = {7284F809-AF30-6315-88C6-86F1C0798760} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "grplibs_bullet", "grplibs_bullet.vcproj", "{DFAF0062-4CD7-9AB8-0683-A6026B326F56}" + ProjectSection(ProjectDependencies) = postProject + {A0958CD9-0E39-4A77-3711-9B488F508FBF} = {A0958CD9-0E39-4A77-3711-9B488F508FBF} + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} + {85BCCE3E-992B-B6D7-28F6-CF0A12680822} = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} + {7C428E76-9271-6284-20F0-9B38ED6931E3} = {7C428E76-9271-6284-20F0-9B38ED6931E3} + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} + {61BD1097-CF2E-B296-DAA9-73A6FE135319} = {61BD1097-CF2E-B296-DAA9-73A6FE135319} + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libbullet", "libbullet.vcproj", "{90F5975E-550B-EEC8-9A8A-B8581D3FCF97}" EndProject @@ -49,232 +187,105 @@ EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "liblibxml", "liblibxml.vcproj", "{A0958CD9-0E39-4A77-3711-9B488F508FBF}" EndProject Global - GlobalSection(SolutionConfiguration) = preSolution - ConfigName.0 = Release - ConfigName.1 = Debug + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Release|Win32 = Release|Win32 EndGlobalSection - GlobalSection(ProjectDependencies) = postSolution - {7284F809-AF30-6315-88C6-86F1C0798760}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {7284F809-AF30-6315-88C6-86F1C0798760}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {7284F809-AF30-6315-88C6-86F1C0798760}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {7284F809-AF30-6315-88C6-86F1C0798760}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {7284F809-AF30-6315-88C6-86F1C0798760}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {7284F809-AF30-6315-88C6-86F1C0798760}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.6 = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.7 = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.8 = {A0958CD9-0E39-4A77-3711-9B488F508FBF} - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {DAA547D0-0166-C085-0F93-B88CAB800F97}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {DAA547D0-0166-C085-0F93-B88CAB800F97}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {DAA547D0-0166-C085-0F93-B88CAB800F97}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {DAA547D0-0166-C085-0F93-B88CAB800F97}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {DAA547D0-0166-C085-0F93-B88CAB800F97}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {DAA547D0-0166-C085-0F93-B88CAB800F97}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {69C821C7-1E18-D894-068D-C55E063F4859}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {69C821C7-1E18-D894-068D-C55E063F4859}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {69C821C7-1E18-D894-068D-C55E063F4859}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {69C821C7-1E18-D894-068D-C55E063F4859}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {69C821C7-1E18-D894-068D-C55E063F4859}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {69C821C7-1E18-D894-068D-C55E063F4859}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {69C821C7-1E18-D894-068D-C55E063F4859}.6 = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} - {69C821C7-1E18-D894-068D-C55E063F4859}.7 = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} - {69C821C7-1E18-D894-068D-C55E063F4859}.8 = {A0958CD9-0E39-4A77-3711-9B488F508FBF} - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {60F71B6A-F888-C449-EF49-268BB9F7C963}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {60F71B6A-F888-C449-EF49-268BB9F7C963}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {60F71B6A-F888-C449-EF49-268BB9F7C963}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {60F71B6A-F888-C449-EF49-268BB9F7C963}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {60F71B6A-F888-C449-EF49-268BB9F7C963}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {60F71B6A-F888-C449-EF49-268BB9F7C963}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {60A1DC9D-F837-3923-E9DE-A7925394A578}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {60A1DC9D-F837-3923-E9DE-A7925394A578}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {60A1DC9D-F837-3923-E9DE-A7925394A578}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {60A1DC9D-F837-3923-E9DE-A7925394A578}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {60A1DC9D-F837-3923-E9DE-A7925394A578}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {60A1DC9D-F837-3923-E9DE-A7925394A578}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {6210A080-01C0-6D67-F1DB-669393175402}.0 = {7284F809-AF30-6315-88C6-86F1C0798760} - {6210A080-01C0-6D67-F1DB-669393175402}.1 = {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF} - {6210A080-01C0-6D67-F1DB-669393175402}.2 = {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9} - {6210A080-01C0-6D67-F1DB-669393175402}.3 = {F38629D2-EEB2-1A09-FB82-52B8A8DE759B} - {6210A080-01C0-6D67-F1DB-669393175402}.4 = {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3} - {6210A080-01C0-6D67-F1DB-669393175402}.5 = {DAA547D0-0166-C085-0F93-B88CAB800F97} - {6210A080-01C0-6D67-F1DB-669393175402}.6 = {801CB6D4-A45C-C9D2-B176-9711A74B9164} - {6210A080-01C0-6D67-F1DB-669393175402}.7 = {69C821C7-1E18-D894-068D-C55E063F4859} - {6210A080-01C0-6D67-F1DB-669393175402}.8 = {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3} - {6210A080-01C0-6D67-F1DB-669393175402}.9 = {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1} - {6210A080-01C0-6D67-F1DB-669393175402}.10 = {60F71B6A-F888-C449-EF49-268BB9F7C963} - {6210A080-01C0-6D67-F1DB-669393175402}.11 = {60A1DC9D-F837-3923-E9DE-A7925394A578} - {6210A080-01C0-6D67-F1DB-669393175402}.12 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {6210A080-01C0-6D67-F1DB-669393175402}.13 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {6210A080-01C0-6D67-F1DB-669393175402}.14 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {6210A080-01C0-6D67-F1DB-669393175402}.15 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {6210A080-01C0-6D67-F1DB-669393175402}.16 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {6210A080-01C0-6D67-F1DB-669393175402}.17 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {6210A080-01C0-6D67-F1DB-669393175402}.18 = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} - {6210A080-01C0-6D67-F1DB-669393175402}.19 = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} - {6210A080-01C0-6D67-F1DB-669393175402}.20 = {A0958CD9-0E39-4A77-3711-9B488F508FBF} - {9E59B16D-0924-409C-1611-DF2207A0053F}.0 = {7284F809-AF30-6315-88C6-86F1C0798760} - {9E59B16D-0924-409C-1611-DF2207A0053F}.1 = {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF} - {9E59B16D-0924-409C-1611-DF2207A0053F}.2 = {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9} - {9E59B16D-0924-409C-1611-DF2207A0053F}.3 = {F38629D2-EEB2-1A09-FB82-52B8A8DE759B} - {9E59B16D-0924-409C-1611-DF2207A0053F}.4 = {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3} - {9E59B16D-0924-409C-1611-DF2207A0053F}.5 = {DAA547D0-0166-C085-0F93-B88CAB800F97} - {9E59B16D-0924-409C-1611-DF2207A0053F}.6 = {801CB6D4-A45C-C9D2-B176-9711A74B9164} - {9E59B16D-0924-409C-1611-DF2207A0053F}.7 = {69C821C7-1E18-D894-068D-C55E063F4859} - {9E59B16D-0924-409C-1611-DF2207A0053F}.8 = {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3} - {9E59B16D-0924-409C-1611-DF2207A0053F}.9 = {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1} - {9E59B16D-0924-409C-1611-DF2207A0053F}.10 = {60F71B6A-F888-C449-EF49-268BB9F7C963} - {9E59B16D-0924-409C-1611-DF2207A0053F}.11 = {60A1DC9D-F837-3923-E9DE-A7925394A578} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.0 = {90F5975E-550B-EEC8-9A8A-B8581D3FCF97} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.1 = {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.2 = {61BD1097-CF2E-B296-DAA9-73A6FE135319} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.3 = {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.4 = {7C428E76-9271-6284-20F0-9B38ED6931E3} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.5 = {85BCCE3E-992B-B6D7-28F6-CF0A12680822} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.6 = {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.7 = {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3} - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.8 = {A0958CD9-0E39-4A77-3711-9B488F508FBF} + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {7284F809-AF30-6315-88C6-86F1C0798760}.Debug|Win32.ActiveCfg = Debug|Win32 + {7284F809-AF30-6315-88C6-86F1C0798760}.Debug|Win32.Build.0 = Debug|Win32 + {7284F809-AF30-6315-88C6-86F1C0798760}.Release|Win32.ActiveCfg = Release|Win32 + {7284F809-AF30-6315-88C6-86F1C0798760}.Release|Win32.Build.0 = Release|Win32 + {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Debug|Win32.ActiveCfg = Debug|Win32 + {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Debug|Win32.Build.0 = Debug|Win32 + {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Release|Win32.ActiveCfg = Release|Win32 + {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Release|Win32.Build.0 = Release|Win32 + {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Debug|Win32.ActiveCfg = Debug|Win32 + {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Debug|Win32.Build.0 = Debug|Win32 + {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Release|Win32.ActiveCfg = Release|Win32 + {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Release|Win32.Build.0 = Release|Win32 + {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Debug|Win32.ActiveCfg = Debug|Win32 + {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Debug|Win32.Build.0 = Debug|Win32 + {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Release|Win32.ActiveCfg = Release|Win32 + {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Release|Win32.Build.0 = Release|Win32 + {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Debug|Win32.ActiveCfg = Debug|Win32 + {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Debug|Win32.Build.0 = Debug|Win32 + {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Release|Win32.ActiveCfg = Release|Win32 + {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Release|Win32.Build.0 = Release|Win32 + {DAA547D0-0166-C085-0F93-B88CAB800F97}.Debug|Win32.ActiveCfg = Debug|Win32 + {DAA547D0-0166-C085-0F93-B88CAB800F97}.Debug|Win32.Build.0 = Debug|Win32 + {DAA547D0-0166-C085-0F93-B88CAB800F97}.Release|Win32.ActiveCfg = Release|Win32 + {DAA547D0-0166-C085-0F93-B88CAB800F97}.Release|Win32.Build.0 = Release|Win32 + {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Debug|Win32.ActiveCfg = Debug|Win32 + {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Debug|Win32.Build.0 = Debug|Win32 + {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Release|Win32.ActiveCfg = Release|Win32 + {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Release|Win32.Build.0 = Release|Win32 + {69C821C7-1E18-D894-068D-C55E063F4859}.Debug|Win32.ActiveCfg = Debug|Win32 + {69C821C7-1E18-D894-068D-C55E063F4859}.Debug|Win32.Build.0 = Debug|Win32 + {69C821C7-1E18-D894-068D-C55E063F4859}.Release|Win32.ActiveCfg = Release|Win32 + {69C821C7-1E18-D894-068D-C55E063F4859}.Release|Win32.Build.0 = Release|Win32 + {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Debug|Win32.ActiveCfg = Debug|Win32 + {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Debug|Win32.Build.0 = Debug|Win32 + {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Release|Win32.ActiveCfg = Release|Win32 + {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Release|Win32.Build.0 = Release|Win32 + {60F71B6A-F888-C449-EF49-268BB9F7C963}.Debug|Win32.ActiveCfg = Debug|Win32 + {60F71B6A-F888-C449-EF49-268BB9F7C963}.Debug|Win32.Build.0 = Debug|Win32 + {60F71B6A-F888-C449-EF49-268BB9F7C963}.Release|Win32.ActiveCfg = Release|Win32 + {60F71B6A-F888-C449-EF49-268BB9F7C963}.Release|Win32.Build.0 = Release|Win32 + {60A1DC9D-F837-3923-E9DE-A7925394A578}.Debug|Win32.ActiveCfg = Debug|Win32 + {60A1DC9D-F837-3923-E9DE-A7925394A578}.Debug|Win32.Build.0 = Debug|Win32 + {60A1DC9D-F837-3923-E9DE-A7925394A578}.Release|Win32.ActiveCfg = Release|Win32 + {60A1DC9D-F837-3923-E9DE-A7925394A578}.Release|Win32.Build.0 = Release|Win32 + {6210A080-01C0-6D67-F1DB-669393175402}.Debug|Win32.ActiveCfg = Debug|Win32 + {6210A080-01C0-6D67-F1DB-669393175402}.Debug|Win32.Build.0 = Debug|Win32 + {6210A080-01C0-6D67-F1DB-669393175402}.Release|Win32.ActiveCfg = Release|Win32 + {6210A080-01C0-6D67-F1DB-669393175402}.Release|Win32.Build.0 = Release|Win32 + {9E59B16D-0924-409C-1611-DF2207A0053F}.Debug|Win32.ActiveCfg = Debug|Win32 + {9E59B16D-0924-409C-1611-DF2207A0053F}.Debug|Win32.Build.0 = Debug|Win32 + {9E59B16D-0924-409C-1611-DF2207A0053F}.Release|Win32.ActiveCfg = Release|Win32 + {9E59B16D-0924-409C-1611-DF2207A0053F}.Release|Win32.Build.0 = Release|Win32 + {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Debug|Win32.ActiveCfg = Debug|Win32 + {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Debug|Win32.Build.0 = Debug|Win32 + {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Release|Win32.ActiveCfg = Release|Win32 + {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Release|Win32.Build.0 = Release|Win32 + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Debug|Win32.ActiveCfg = Debug|Win32 + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Debug|Win32.Build.0 = Debug|Win32 + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Release|Win32.ActiveCfg = Release|Win32 + {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Release|Win32.Build.0 = Release|Win32 + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Debug|Win32.ActiveCfg = Debug|Win32 + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Debug|Win32.Build.0 = Debug|Win32 + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Release|Win32.ActiveCfg = Release|Win32 + {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Release|Win32.Build.0 = Release|Win32 + {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Debug|Win32.ActiveCfg = Debug|Win32 + {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Debug|Win32.Build.0 = Debug|Win32 + {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Release|Win32.ActiveCfg = Release|Win32 + {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Release|Win32.Build.0 = Release|Win32 + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Debug|Win32.ActiveCfg = Debug|Win32 + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Debug|Win32.Build.0 = Debug|Win32 + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Release|Win32.ActiveCfg = Release|Win32 + {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Release|Win32.Build.0 = Release|Win32 + {7C428E76-9271-6284-20F0-9B38ED6931E3}.Debug|Win32.ActiveCfg = Debug|Win32 + {7C428E76-9271-6284-20F0-9B38ED6931E3}.Debug|Win32.Build.0 = Debug|Win32 + {7C428E76-9271-6284-20F0-9B38ED6931E3}.Release|Win32.ActiveCfg = Release|Win32 + {7C428E76-9271-6284-20F0-9B38ED6931E3}.Release|Win32.Build.0 = Release|Win32 + {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Debug|Win32.ActiveCfg = Debug|Win32 + {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Debug|Win32.Build.0 = Debug|Win32 + {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Release|Win32.ActiveCfg = Release|Win32 + {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Release|Win32.Build.0 = Release|Win32 + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Debug|Win32.ActiveCfg = Debug|Win32 + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Debug|Win32.Build.0 = Debug|Win32 + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Release|Win32.ActiveCfg = Release|Win32 + {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Release|Win32.Build.0 = Release|Win32 + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Debug|Win32.ActiveCfg = Debug|Win32 + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Debug|Win32.Build.0 = Debug|Win32 + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Release|Win32.ActiveCfg = Release|Win32 + {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Release|Win32.Build.0 = Release|Win32 + {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Debug|Win32.ActiveCfg = Debug|Win32 + {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Debug|Win32.Build.0 = Debug|Win32 + {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Release|Win32.ActiveCfg = Release|Win32 + {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Release|Win32.Build.0 = Release|Win32 EndGlobalSection - GlobalSection(ProjectConfiguration) = postSolution - {7284F809-AF30-6315-88C6-86F1C0798760}.Release.ActiveCfg = Release|Win32 - {7284F809-AF30-6315-88C6-86F1C0798760}.Release.Build.0 = Release|Win32 - {7284F809-AF30-6315-88C6-86F1C0798760}.Debug.ActiveCfg = Debug|Win32 - {7284F809-AF30-6315-88C6-86F1C0798760}.Debug.Build.0 = Debug|Win32 - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Release.ActiveCfg = Release|Win32 - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Release.Build.0 = Release|Win32 - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Debug.ActiveCfg = Debug|Win32 - {D7BF5DDA-C097-9E8B-5EC1-40DE45FB46BF}.Debug.Build.0 = Debug|Win32 - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Release.ActiveCfg = Release|Win32 - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Release.Build.0 = Release|Win32 - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Debug.ActiveCfg = Debug|Win32 - {E70DB92E-C1F5-AE72-F9E2-DB9B4B3DBEC9}.Debug.Build.0 = Debug|Win32 - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Release.ActiveCfg = Release|Win32 - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Release.Build.0 = Release|Win32 - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Debug.ActiveCfg = Debug|Win32 - {F38629D2-EEB2-1A09-FB82-52B8A8DE759B}.Debug.Build.0 = Debug|Win32 - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Release.ActiveCfg = Release|Win32 - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Release.Build.0 = Release|Win32 - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Debug.ActiveCfg = Debug|Win32 - {B94C19C6-F6E7-2F60-56E2-E0BA681B74B3}.Debug.Build.0 = Debug|Win32 - {DAA547D0-0166-C085-0F93-B88CAB800F97}.Release.ActiveCfg = Release|Win32 - {DAA547D0-0166-C085-0F93-B88CAB800F97}.Release.Build.0 = Release|Win32 - {DAA547D0-0166-C085-0F93-B88CAB800F97}.Debug.ActiveCfg = Debug|Win32 - {DAA547D0-0166-C085-0F93-B88CAB800F97}.Debug.Build.0 = Debug|Win32 - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Release.ActiveCfg = Release|Win32 - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Release.Build.0 = Release|Win32 - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Debug.ActiveCfg = Debug|Win32 - {801CB6D4-A45C-C9D2-B176-9711A74B9164}.Debug.Build.0 = Debug|Win32 - {69C821C7-1E18-D894-068D-C55E063F4859}.Release.ActiveCfg = Release|Win32 - {69C821C7-1E18-D894-068D-C55E063F4859}.Release.Build.0 = Release|Win32 - {69C821C7-1E18-D894-068D-C55E063F4859}.Debug.ActiveCfg = Debug|Win32 - {69C821C7-1E18-D894-068D-C55E063F4859}.Debug.Build.0 = Debug|Win32 - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.Release.ActiveCfg = Release|Win32 - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.Release.Build.0 = Release|Win32 - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.Debug.ActiveCfg = Debug|Win32 - {1125C7F3-9E0D-27B1-C97B-CDAB5CE161A3}.Debug.Build.0 = Debug|Win32 - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Release.ActiveCfg = Release|Win32 - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Release.Build.0 = Release|Win32 - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Debug.ActiveCfg = Debug|Win32 - {780752A8-6322-5D3E-EF42-D0FD8BF9CEA1}.Debug.Build.0 = Debug|Win32 - {60F71B6A-F888-C449-EF49-268BB9F7C963}.Release.ActiveCfg = Release|Win32 - {60F71B6A-F888-C449-EF49-268BB9F7C963}.Release.Build.0 = Release|Win32 - {60F71B6A-F888-C449-EF49-268BB9F7C963}.Debug.ActiveCfg = Debug|Win32 - {60F71B6A-F888-C449-EF49-268BB9F7C963}.Debug.Build.0 = Debug|Win32 - {60A1DC9D-F837-3923-E9DE-A7925394A578}.Release.ActiveCfg = Release|Win32 - {60A1DC9D-F837-3923-E9DE-A7925394A578}.Release.Build.0 = Release|Win32 - {60A1DC9D-F837-3923-E9DE-A7925394A578}.Debug.ActiveCfg = Debug|Win32 - {60A1DC9D-F837-3923-E9DE-A7925394A578}.Debug.Build.0 = Debug|Win32 - {6210A080-01C0-6D67-F1DB-669393175402}.Release.ActiveCfg = Release|Win32 - {6210A080-01C0-6D67-F1DB-669393175402}.Release.Build.0 = Release|Win32 - {6210A080-01C0-6D67-F1DB-669393175402}.Debug.ActiveCfg = Debug|Win32 - {6210A080-01C0-6D67-F1DB-669393175402}.Debug.Build.0 = Debug|Win32 - {9E59B16D-0924-409C-1611-DF2207A0053F}.Release.ActiveCfg = Release|Win32 - {9E59B16D-0924-409C-1611-DF2207A0053F}.Release.Build.0 = Release|Win32 - {9E59B16D-0924-409C-1611-DF2207A0053F}.Debug.ActiveCfg = Debug|Win32 - {9E59B16D-0924-409C-1611-DF2207A0053F}.Debug.Build.0 = Debug|Win32 - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Release.ActiveCfg = Release|Win32 - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Release.Build.0 = Release|Win32 - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Debug.ActiveCfg = Debug|Win32 - {DFAF0062-4CD7-9AB8-0683-A6026B326F56}.Debug.Build.0 = Debug|Win32 - {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Release.ActiveCfg = Release|Win32 - {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Release.Build.0 = Release|Win32 - {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Debug.ActiveCfg = Debug|Win32 - {90F5975E-550B-EEC8-9A8A-B8581D3FCF97}.Debug.Build.0 = Debug|Win32 - {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Release.ActiveCfg = Release|Win32 - {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Release.Build.0 = Release|Win32 - {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Debug.ActiveCfg = Debug|Win32 - {C63CFD5B-23E8-FB4F-79DB-E40F463B0C1E}.Debug.Build.0 = Debug|Win32 - {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Release.ActiveCfg = Release|Win32 - {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Release.Build.0 = Release|Win32 - {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Debug.ActiveCfg = Debug|Win32 - {61BD1097-CF2E-B296-DAA9-73A6FE135319}.Debug.Build.0 = Debug|Win32 - {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Release.ActiveCfg = Release|Win32 - {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Release.Build.0 = Release|Win32 - {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Debug.ActiveCfg = Debug|Win32 - {7D6E339F-9C2C-31DA-FDB0-5EE50973CF2A}.Debug.Build.0 = Debug|Win32 - {7C428E76-9271-6284-20F0-9B38ED6931E3}.Release.ActiveCfg = Release|Win32 - {7C428E76-9271-6284-20F0-9B38ED6931E3}.Release.Build.0 = Release|Win32 - {7C428E76-9271-6284-20F0-9B38ED6931E3}.Debug.ActiveCfg = Debug|Win32 - {7C428E76-9271-6284-20F0-9B38ED6931E3}.Debug.Build.0 = Debug|Win32 - {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Release.ActiveCfg = Release|Win32 - {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Release.Build.0 = Release|Win32 - {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Debug.ActiveCfg = Debug|Win32 - {85BCCE3E-992B-B6D7-28F6-CF0A12680822}.Debug.Build.0 = Debug|Win32 - {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Release.ActiveCfg = Release|Win32 - {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Release.Build.0 = Release|Win32 - {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Debug.ActiveCfg = Debug|Win32 - {5E57E40B-B2C3-7595-57AB-A3936FFBD7D4}.Debug.Build.0 = Debug|Win32 - {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Release.ActiveCfg = Release|Win32 - {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Release.Build.0 = Release|Win32 - {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Debug.ActiveCfg = Debug|Win32 - {8050F819-5B5B-1504-BC6D-7F2B4C6C85F3}.Debug.Build.0 = Debug|Win32 - {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Release.ActiveCfg = Release|Win32 - {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Release.Build.0 = Release|Win32 - {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Debug.ActiveCfg = Debug|Win32 - {A0958CD9-0E39-4A77-3711-9B488F508FBF}.Debug.Build.0 = Debug|Win32 - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - EndGlobalSection - GlobalSection(ExtensibilityAddIns) = postSolution + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE EndGlobalSection EndGlobal