1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 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. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 17 18 #ifndef BT_VORONOI_SIMPLEX_SOLVER_H 19 #define BT_VORONOI_SIMPLEX_SOLVER_H 20 21 #include "btSimplexSolverInterface.h" 22 23 24 25 #define VORONOI_SIMPLEX_MAX_VERTS 5 26 27 ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure 28 #define BT_USE_EQUAL_VERTEX_THRESHOLD 29 #define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f 30 31 32 struct btUsageBitfield{ 33 btUsageBitfield() 34 { 35 reset(); 36 } 37 38 void reset() 39 { 40 usedVertexA = false; 41 usedVertexB = false; 42 usedVertexC = false; 43 usedVertexD = false; 44 } 45 unsigned short usedVertexA : 1; 46 unsigned short usedVertexB : 1; 47 unsigned short usedVertexC : 1; 48 unsigned short usedVertexD : 1; 49 unsigned short unused1 : 1; 50 unsigned short unused2 : 1; 51 unsigned short unused3 : 1; 52 unsigned short unused4 : 1; 53 }; 54 55 56 struct btSubSimplexClosestResult 57 { 58 btVector3 m_closestPointOnSimplex; 59 //MASK for m_usedVertices 60 //stores the simplex vertex-usage, using the MASK, 61 // if m_usedVertices & MASK then the related vertex is used 62 btUsageBitfield m_usedVertices; 63 btScalar m_barycentricCoords[4]; 64 bool m_degenerate; 65 66 void reset() 67 { 68 m_degenerate = false; 69 setBarycentricCoordinates(); 70 m_usedVertices.reset(); 71 } 72 bool isValid() 73 { 74 bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && 75 (m_barycentricCoords[1] >= btScalar(0.)) && 76 (m_barycentricCoords[2] >= btScalar(0.)) && 77 (m_barycentricCoords[3] >= btScalar(0.)); 78 79 80 return valid; 81 } 82 void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) 83 { 84 m_barycentricCoords[0] = a; 85 m_barycentricCoords[1] = b; 86 m_barycentricCoords[2] = c; 87 m_barycentricCoords[3] = d; 88 } 89 90 }; 91 92 /// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. 93 /// Can be used with GJK, as an alternative to Johnson distance algorithm. 94 #ifdef NO_VIRTUAL_INTERFACE 95 ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver 96 #else 97 ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface 98 #endif 99 { 100 public: 101 102 BT_DECLARE_ALIGNED_ALLOCATOR(); 103 104 int m_numVertices; 105 106 btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; 107 btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; 108 btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; 109 110 111 112 btVector3 m_cachedP1; 113 btVector3 m_cachedP2; 114 btVector3 m_cachedV; 115 btVector3 m_lastW; 116 117 btScalar m_equalVertexThreshold; 118 bool m_cachedValidClosest; 119 120 121 btSubSimplexClosestResult m_cachedBC; 122 123 bool m_needsUpdate; 124 125 void removeVertex(int index); 126 void reduceVertices (const btUsageBitfield& usedVerts); 127 bool updateClosestVectorAndPoints(); 128 129 bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); 130 int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); 131 bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result); 132 133 public: 134 135 btVoronoiSimplexSolver() 136 : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) 137 { 138 } 139 void reset(); 140 141 void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); 142 143 void setEqualVertexThreshold(btScalar threshold) 144 { 145 m_equalVertexThreshold = threshold; 146 } 147 148 btScalar getEqualVertexThreshold() const 149 { 150 return m_equalVertexThreshold; 151 } 152 153 bool closest(btVector3& v); 154 155 btScalar maxVertex(); 156 157 bool fullSimplex() const 158 { 159 return (m_numVertices == 4); 160 } 161 162 int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; 163 164 bool inSimplex(const btVector3& w); 165 166 void backup_closest(btVector3& v) ; 167 168 bool emptySimplex() const ; 169 170 void compute_points(btVector3& p1, btVector3& p2) ; 171 172 int numVertices() const 173 { 174 return m_numVertices; 175 } 176 177 178 }; 179 180 #endif //BT_VORONOI_SIMPLEX_SOLVER_H 181 182
