- Timestamp:
- Dec 13, 2008, 11:45:51 PM (16 years ago)
- Location:
- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision
- Files:
-
- 2 deleted
- 14 edited
-
btConvexPenetrationDepthSolver.h (modified) (2 diffs)
-
btGjkEpa.cpp (deleted)
-
btGjkEpa.h (deleted)
-
btGjkEpa2.cpp (modified) (6 diffs)
-
btGjkEpaPenetrationDepthSolver.cpp (modified) (4 diffs)
-
btGjkEpaPenetrationDepthSolver.h (modified) (1 diff)
-
btGjkPairDetector.cpp (modified) (5 diffs)
-
btGjkPairDetector.h (modified) (3 diffs)
-
btManifoldPoint.h (modified) (1 diff)
-
btMinkowskiPenetrationDepthSolver.cpp (modified) (1 diff)
-
btMinkowskiPenetrationDepthSolver.h (modified) (1 diff)
-
btPersistentManifold.cpp (modified) (4 diffs)
-
btPersistentManifold.h (modified) (4 diffs)
-
btSimplexSolverInterface.h (modified) (4 diffs)
-
btVoronoiSimplexSolver.cpp (modified) (10 diffs)
-
btVoronoiSimplexSolver.h (modified) (6 diffs)
Legend:
- Unmodified
- Added
- Removed
-
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
r2192 r2430 22 22 #include "btSimplexSolverInterface.h" 23 23 class btConvexShape; 24 #include "LinearMath/btPoint3.h"25 24 class btTransform; 26 25 … … 34 33 const btConvexShape* convexA,const btConvexShape* convexB, 35 34 const btTransform& transA,const btTransform& transB, 36 btVector3& v, bt Point3& pa, btPoint3& pb,35 btVector3& v, btVector3& pa, btVector3& pb, 37 36 class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc 38 37 ) = 0; -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
r2192 r2430 38 38 { 39 39 40 // Config40 // Config 41 41 42 42 /* GJK */ … … 59 59 60 60 61 // Shorthands62 typedef unsigned int U;63 typedef unsigned char U1;64 65 // MinkowskiDiff66 struct MinkowskiDiff61 // Shorthands 62 typedef unsigned int U; 63 typedef unsigned char U1; 64 65 // MinkowskiDiff 66 struct MinkowskiDiff 67 67 { 68 const btConvexShape* m_shapes[2];69 btMatrix3x3 m_toshape1;70 btTransform m_toshape0;71 btVector3 (btConvexShape::*Ls)(const btVector3&) const;72 void EnableMargin(bool enable)73 { 74 if(enable)75 Ls=&btConvexShape::localGetSupportVertexNonVirtual;68 const btConvexShape* m_shapes[2]; 69 btMatrix3x3 m_toshape1; 70 btTransform m_toshape0; 71 btVector3 (btConvexShape::*Ls)(const btVector3&) const; 72 void EnableMargin(bool enable) 73 { 74 if(enable) 75 Ls=&btConvexShape::localGetSupportVertexNonVirtual; 76 76 else 77 Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;77 Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; 78 78 } 79 inline btVector3 Support0(const btVector3& d) const80 { 81 return(((m_shapes[0])->*(Ls))(d));79 inline btVector3 Support0(const btVector3& d) const 80 { 81 return(((m_shapes[0])->*(Ls))(d)); 82 82 } 83 inline btVector3 Support1(const btVector3& d) const84 { 85 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));83 inline btVector3 Support1(const btVector3& d) const 84 { 85 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); 86 86 } 87 inline btVector3 Support(const btVector3& d) const88 { 89 return(Support0(d)-Support1(-d));87 inline btVector3 Support(const btVector3& d) const 88 { 89 return(Support0(d)-Support1(-d)); 90 90 } 91 btVector3 Support(const btVector3& d,U index) const92 { 93 if(index)94 return(Support1(d));91 btVector3 Support(const btVector3& d,U index) const 92 { 93 if(index) 94 return(Support1(d)); 95 95 else 96 return(Support0(d));96 return(Support0(d)); 97 97 } 98 98 }; 99 99 100 typedef MinkowskiDiff tShape; 101 102 103 // GJK 104 struct GJK 105 { 106 /* Types */ 107 struct sSV 100 typedef MinkowskiDiff tShape; 101 102 103 // GJK 104 struct GJK 108 105 { 109 btVector3 d,w; 110 }; 111 struct sSimplex 112 { 113 sSV* c[4]; 114 btScalar p[4]; 115 U rank; 116 }; 117 struct eStatus { enum _ { 118 Valid, 119 Inside, 120 Failed };}; 121 /* Fields */ 122 tShape m_shape; 123 btVector3 m_ray; 124 btScalar m_distance; 125 sSimplex m_simplices[2]; 126 sSV m_store[4]; 127 sSV* m_free[4]; 128 U m_nfree; 129 U m_current; 130 sSimplex* m_simplex; 131 eStatus::_ m_status; 132 /* Methods */ 133 GJK() 134 { 135 Initialize(); 136 } 137 void Initialize() 138 { 139 m_ray = btVector3(0,0,0); 140 m_nfree = 0; 141 m_status = eStatus::Failed; 142 m_current = 0; 143 m_distance = 0; 144 } 145 eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) 146 { 147 U iterations=0; 148 btScalar sqdist=0; 149 btScalar alpha=0; 150 btVector3 lastw[4]; 151 U clastw=0; 152 /* Initialize solver */ 153 m_free[0] = &m_store[0]; 154 m_free[1] = &m_store[1]; 155 m_free[2] = &m_store[2]; 156 m_free[3] = &m_store[3]; 157 m_nfree = 4; 158 m_current = 0; 159 m_status = eStatus::Valid; 160 m_shape = shapearg; 161 m_distance = 0; 162 /* Initialize simplex */ 163 m_simplices[0].rank = 0; 164 m_ray = guess; 165 const btScalar sqrl= m_ray.length2(); 166 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); 167 m_simplices[0].p[0] = 1; 168 m_ray = m_simplices[0].c[0]->w; 169 sqdist = sqrl; 170 lastw[0] = 171 lastw[1] = 172 lastw[2] = 173 lastw[3] = m_ray; 174 /* Loop */ 175 do { 176 const U next=1-m_current; 177 sSimplex& cs=m_simplices[m_current]; 178 sSimplex& ns=m_simplices[next]; 179 /* Check zero */ 180 const btScalar rl=m_ray.length(); 181 if(rl<GJK_MIN_DISTANCE) 182 {/* Touching or inside */ 183 m_status=eStatus::Inside; 184 break; 185 } 186 /* Append new vertice in -'v' direction */ 187 appendvertice(cs,-m_ray); 188 const btVector3& w=cs.c[cs.rank-1]->w; 189 bool found=false; 190 for(U i=0;i<4;++i) 191 { 192 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) 193 { found=true;break; } 194 } 195 if(found) 196 {/* Return old simplex */ 197 removevertice(m_simplices[m_current]); 198 break; 199 } 200 else 201 {/* Update lastw */ 202 lastw[clastw=(clastw+1)&3]=w; 203 } 204 /* Check for termination */ 205 const btScalar omega=dot(m_ray,w)/rl; 206 alpha=btMax(omega,alpha); 207 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) 208 {/* Return old simplex */ 209 removevertice(m_simplices[m_current]); 210 break; 211 } 212 /* Reduce simplex */ 213 btScalar weights[4]; 214 U mask=0; 215 switch(cs.rank) 216 { 217 case 2: sqdist=projectorigin( cs.c[0]->w, 218 cs.c[1]->w, 219 weights,mask);break; 220 case 3: sqdist=projectorigin( cs.c[0]->w, 221 cs.c[1]->w, 222 cs.c[2]->w, 223 weights,mask);break; 224 case 4: sqdist=projectorigin( cs.c[0]->w, 225 cs.c[1]->w, 226 cs.c[2]->w, 227 cs.c[3]->w, 228 weights,mask);break; 229 } 230 if(sqdist>=0) 231 {/* Valid */ 232 ns.rank = 0; 233 m_ray = btVector3(0,0,0); 234 m_current = next; 235 for(U i=0,ni=cs.rank;i<ni;++i) 236 { 237 if(mask&(1<<i)) 238 { 239 ns.c[ns.rank] = cs.c[i]; 240 ns.p[ns.rank++] = weights[i]; 241 m_ray += cs.c[i]->w*weights[i]; 106 /* Types */ 107 struct sSV 108 { 109 btVector3 d,w; 110 }; 111 struct sSimplex 112 { 113 sSV* c[4]; 114 btScalar p[4]; 115 U rank; 116 }; 117 struct eStatus { enum _ { 118 Valid, 119 Inside, 120 Failed };}; 121 /* Fields */ 122 tShape m_shape; 123 btVector3 m_ray; 124 btScalar m_distance; 125 sSimplex m_simplices[2]; 126 sSV m_store[4]; 127 sSV* m_free[4]; 128 U m_nfree; 129 U m_current; 130 sSimplex* m_simplex; 131 eStatus::_ m_status; 132 /* Methods */ 133 GJK() 134 { 135 Initialize(); 136 } 137 void Initialize() 138 { 139 m_ray = btVector3(0,0,0); 140 m_nfree = 0; 141 m_status = eStatus::Failed; 142 m_current = 0; 143 m_distance = 0; 144 } 145 eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) 146 { 147 U iterations=0; 148 btScalar sqdist=0; 149 btScalar alpha=0; 150 btVector3 lastw[4]; 151 U clastw=0; 152 /* Initialize solver */ 153 m_free[0] = &m_store[0]; 154 m_free[1] = &m_store[1]; 155 m_free[2] = &m_store[2]; 156 m_free[3] = &m_store[3]; 157 m_nfree = 4; 158 m_current = 0; 159 m_status = eStatus::Valid; 160 m_shape = shapearg; 161 m_distance = 0; 162 /* Initialize simplex */ 163 m_simplices[0].rank = 0; 164 m_ray = guess; 165 const btScalar sqrl= m_ray.length2(); 166 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); 167 m_simplices[0].p[0] = 1; 168 m_ray = m_simplices[0].c[0]->w; 169 sqdist = sqrl; 170 lastw[0] = 171 lastw[1] = 172 lastw[2] = 173 lastw[3] = m_ray; 174 /* Loop */ 175 do { 176 const U next=1-m_current; 177 sSimplex& cs=m_simplices[m_current]; 178 sSimplex& ns=m_simplices[next]; 179 /* Check zero */ 180 const btScalar rl=m_ray.length(); 181 if(rl<GJK_MIN_DISTANCE) 182 {/* Touching or inside */ 183 m_status=eStatus::Inside; 184 break; 185 } 186 /* Append new vertice in -'v' direction */ 187 appendvertice(cs,-m_ray); 188 const btVector3& w=cs.c[cs.rank-1]->w; 189 bool found=false; 190 for(U i=0;i<4;++i) 191 { 192 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) 193 { found=true;break; } 194 } 195 if(found) 196 {/* Return old simplex */ 197 removevertice(m_simplices[m_current]); 198 break; 242 199 } 243 200 else 244 { 245 m_free[m_nfree++] = cs.c[i]; 246 } 247 } 248 if(mask==15) m_status=eStatus::Inside; 249 } 250 else 251 {/* Return old simplex */ 252 removevertice(m_simplices[m_current]); 253 break; 254 } 255 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; 256 } while(m_status==eStatus::Valid); 257 m_simplex=&m_simplices[m_current]; 258 switch(m_status) 259 { 260 case eStatus::Valid: m_distance=m_ray.length();break; 261 case eStatus::Inside: m_distance=0;break; 262 } 263 return(m_status); 264 } 265 bool EncloseOrigin() 266 { 267 switch(m_simplex->rank) 268 { 269 case 1: 270 { 271 for(U i=0;i<3;++i) 272 { 273 btVector3 axis=btVector3(0,0,0); 274 axis[i]=1; 275 appendvertice(*m_simplex, axis); 276 if(EncloseOrigin()) return(true); 277 removevertice(*m_simplex); 278 appendvertice(*m_simplex,-axis); 279 if(EncloseOrigin()) return(true); 280 removevertice(*m_simplex); 281 } 282 } 283 break; 284 case 2: 285 { 286 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; 287 for(U i=0;i<3;++i) 288 { 289 btVector3 axis=btVector3(0,0,0); 290 axis[i]=1; 291 const btVector3 p=cross(d,axis); 292 if(p.length2()>0) 293 { 294 appendvertice(*m_simplex, p); 295 if(EncloseOrigin()) return(true); 296 removevertice(*m_simplex); 297 appendvertice(*m_simplex,-p); 298 if(EncloseOrigin()) return(true); 299 removevertice(*m_simplex); 300 } 301 } 302 } 303 break; 304 case 3: 305 { 306 const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, 307 m_simplex->c[2]->w-m_simplex->c[0]->w); 308 if(n.length2()>0) 309 { 310 appendvertice(*m_simplex,n); 311 if(EncloseOrigin()) return(true); 312 removevertice(*m_simplex); 313 appendvertice(*m_simplex,-n); 314 if(EncloseOrigin()) return(true); 315 removevertice(*m_simplex); 316 } 317 } 318 break; 319 case 4: 320 { 321 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, 201 {/* Update lastw */ 202 lastw[clastw=(clastw+1)&3]=w; 203 } 204 /* Check for termination */ 205 const btScalar omega=dot(m_ray,w)/rl; 206 alpha=btMax(omega,alpha); 207 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) 208 {/* Return old simplex */ 209 removevertice(m_simplices[m_current]); 210 break; 211 } 212 /* Reduce simplex */ 213 btScalar weights[4]; 214 U mask=0; 215 switch(cs.rank) 216 { 217 case 2: sqdist=projectorigin( cs.c[0]->w, 218 cs.c[1]->w, 219 weights,mask);break; 220 case 3: sqdist=projectorigin( cs.c[0]->w, 221 cs.c[1]->w, 222 cs.c[2]->w, 223 weights,mask);break; 224 case 4: sqdist=projectorigin( cs.c[0]->w, 225 cs.c[1]->w, 226 cs.c[2]->w, 227 cs.c[3]->w, 228 weights,mask);break; 229 } 230 if(sqdist>=0) 231 {/* Valid */ 232 ns.rank = 0; 233 m_ray = btVector3(0,0,0); 234 m_current = next; 235 for(U i=0,ni=cs.rank;i<ni;++i) 236 { 237 if(mask&(1<<i)) 238 { 239 ns.c[ns.rank] = cs.c[i]; 240 ns.p[ns.rank++] = weights[i]; 241 m_ray += cs.c[i]->w*weights[i]; 242 } 243 else 244 { 245 m_free[m_nfree++] = cs.c[i]; 246 } 247 } 248 if(mask==15) m_status=eStatus::Inside; 249 } 250 else 251 {/* Return old simplex */ 252 removevertice(m_simplices[m_current]); 253 break; 254 } 255 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; 256 } while(m_status==eStatus::Valid); 257 m_simplex=&m_simplices[m_current]; 258 switch(m_status) 259 { 260 case eStatus::Valid: m_distance=m_ray.length();break; 261 case eStatus::Inside: m_distance=0;break; 262 } 263 return(m_status); 264 } 265 bool EncloseOrigin() 266 { 267 switch(m_simplex->rank) 268 { 269 case 1: 270 { 271 for(U i=0;i<3;++i) 272 { 273 btVector3 axis=btVector3(0,0,0); 274 axis[i]=1; 275 appendvertice(*m_simplex, axis); 276 if(EncloseOrigin()) return(true); 277 removevertice(*m_simplex); 278 appendvertice(*m_simplex,-axis); 279 if(EncloseOrigin()) return(true); 280 removevertice(*m_simplex); 281 } 282 } 283 break; 284 case 2: 285 { 286 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; 287 for(U i=0;i<3;++i) 288 { 289 btVector3 axis=btVector3(0,0,0); 290 axis[i]=1; 291 const btVector3 p=cross(d,axis); 292 if(p.length2()>0) 293 { 294 appendvertice(*m_simplex, p); 295 if(EncloseOrigin()) return(true); 296 removevertice(*m_simplex); 297 appendvertice(*m_simplex,-p); 298 if(EncloseOrigin()) return(true); 299 removevertice(*m_simplex); 300 } 301 } 302 } 303 break; 304 case 3: 305 { 306 const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, 307 m_simplex->c[2]->w-m_simplex->c[0]->w); 308 if(n.length2()>0) 309 { 310 appendvertice(*m_simplex,n); 311 if(EncloseOrigin()) return(true); 312 removevertice(*m_simplex); 313 appendvertice(*m_simplex,-n); 314 if(EncloseOrigin()) return(true); 315 removevertice(*m_simplex); 316 } 317 } 318 break; 319 case 4: 320 { 321 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, 322 322 m_simplex->c[1]->w-m_simplex->c[3]->w, 323 323 m_simplex->c[2]->w-m_simplex->c[3]->w))>0) 324 return(true); 325 } 326 break; 327 } 328 return(false); 329 } 330 /* Internals */ 331 void getsupport(const btVector3& d,sSV& sv) const 332 { 333 sv.d = d/d.length(); 334 sv.w = m_shape.Support(sv.d); 335 } 336 void removevertice(sSimplex& simplex) 337 { 338 m_free[m_nfree++]=simplex.c[--simplex.rank]; 339 } 340 void appendvertice(sSimplex& simplex,const btVector3& v) 341 { 342 simplex.p[simplex.rank]=0; 343 simplex.c[simplex.rank]=m_free[--m_nfree]; 344 getsupport(v,*simplex.c[simplex.rank++]); 345 } 346 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) 347 { 348 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- 349 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ 350 a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); 351 } 352 static btScalar projectorigin( const btVector3& a, 353 const btVector3& b, 354 btScalar* w,U& m) 355 { 356 const btVector3 d=b-a; 357 const btScalar l=d.length2(); 358 if(l>GJK_SIMPLEX2_EPS) 359 { 360 const btScalar t(l>0?-dot(a,d)/l:0); 361 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } 362 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } 363 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } 364 } 365 return(-1); 366 } 367 static btScalar projectorigin( const btVector3& a, 368 const btVector3& b, 369 const btVector3& c, 370 btScalar* w,U& m) 371 { 372 static const U imd3[]={1,2,0}; 373 const btVector3* vt[]={&a,&b,&c}; 374 const btVector3 dl[]={a-b,b-c,c-a}; 375 const btVector3 n=cross(dl[0],dl[1]); 376 const btScalar l=n.length2(); 377 if(l>GJK_SIMPLEX3_EPS) 378 { 379 btScalar mindist=-1; 380 btScalar subw[2]; 381 U subm; 382 for(U i=0;i<3;++i) 383 { 384 if(dot(*vt[i],cross(dl[i],n))>0) 385 { 386 const U j=imd3[i]; 387 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); 388 if((mindist<0)||(subd<mindist)) 389 { 390 mindist = subd; 391 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); 392 w[i] = subw[0]; 393 w[j] = subw[1]; 394 w[imd3[j]] = 0; 395 } 324 return(true); 325 } 326 break; 396 327 } 397 } 398 if(mindist<0) 399 { 400 const btScalar d=dot(a,n); 401 const btScalar s=btSqrt(l); 402 const btVector3 p=n*(d/l); 403 mindist = p.length2(); 404 m = 7; 405 w[0] = (cross(dl[1],b-p)).length()/s; 406 w[1] = (cross(dl[2],c-p)).length()/s; 407 w[2] = 1-(w[0]+w[1]); 408 } 409 return(mindist); 410 } 411 return(-1); 412 } 413 static btScalar projectorigin( const btVector3& a, 414 const btVector3& b, 415 const btVector3& c, 416 const btVector3& d, 417 btScalar* w,U& m) 418 { 419 static const U imd3[]={1,2,0}; 420 const btVector3* vt[]={&a,&b,&c,&d}; 421 const btVector3 dl[]={a-d,b-d,c-d}; 422 const btScalar vl=det(dl[0],dl[1],dl[2]); 423 const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; 424 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) 425 { 426 btScalar mindist=-1; 427 btScalar subw[3]; 428 U subm; 429 for(U i=0;i<3;++i) 430 { 431 const U j=imd3[i]; 432 const btScalar s=vl*dot(d,cross(dl[i],dl[j])); 433 if(s>0) 434 { 435 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); 436 if((mindist<0)||(subd<mindist)) 437 { 438 mindist = subd; 439 m = static_cast<U>((subm&1?1<<i:0)+ 328 return(false); 329 } 330 /* Internals */ 331 void getsupport(const btVector3& d,sSV& sv) const 332 { 333 sv.d = d/d.length(); 334 sv.w = m_shape.Support(sv.d); 335 } 336 void removevertice(sSimplex& simplex) 337 { 338 m_free[m_nfree++]=simplex.c[--simplex.rank]; 339 } 340 void appendvertice(sSimplex& simplex,const btVector3& v) 341 { 342 simplex.p[simplex.rank]=0; 343 simplex.c[simplex.rank]=m_free[--m_nfree]; 344 getsupport(v,*simplex.c[simplex.rank++]); 345 } 346 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) 347 { 348 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- 349 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ 350 a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); 351 } 352 static btScalar projectorigin( const btVector3& a, 353 const btVector3& b, 354 btScalar* w,U& m) 355 { 356 const btVector3 d=b-a; 357 const btScalar l=d.length2(); 358 if(l>GJK_SIMPLEX2_EPS) 359 { 360 const btScalar t(l>0?-dot(a,d)/l:0); 361 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } 362 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } 363 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } 364 } 365 return(-1); 366 } 367 static btScalar projectorigin( const btVector3& a, 368 const btVector3& b, 369 const btVector3& c, 370 btScalar* w,U& m) 371 { 372 static const U imd3[]={1,2,0}; 373 const btVector3* vt[]={&a,&b,&c}; 374 const btVector3 dl[]={a-b,b-c,c-a}; 375 const btVector3 n=cross(dl[0],dl[1]); 376 const btScalar l=n.length2(); 377 if(l>GJK_SIMPLEX3_EPS) 378 { 379 btScalar mindist=-1; 380 btScalar subw[2]; 381 U subm; 382 for(U i=0;i<3;++i) 383 { 384 if(dot(*vt[i],cross(dl[i],n))>0) 385 { 386 const U j=imd3[i]; 387 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); 388 if((mindist<0)||(subd<mindist)) 389 { 390 mindist = subd; 391 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); 392 w[i] = subw[0]; 393 w[j] = subw[1]; 394 w[imd3[j]] = 0; 395 } 396 } 397 } 398 if(mindist<0) 399 { 400 const btScalar d=dot(a,n); 401 const btScalar s=btSqrt(l); 402 const btVector3 p=n*(d/l); 403 mindist = p.length2(); 404 m = 7; 405 w[0] = (cross(dl[1],b-p)).length()/s; 406 w[1] = (cross(dl[2],c-p)).length()/s; 407 w[2] = 1-(w[0]+w[1]); 408 } 409 return(mindist); 410 } 411 return(-1); 412 } 413 static btScalar projectorigin( const btVector3& a, 414 const btVector3& b, 415 const btVector3& c, 416 const btVector3& d, 417 btScalar* w,U& m) 418 { 419 static const U imd3[]={1,2,0}; 420 const btVector3* vt[]={&a,&b,&c,&d}; 421 const btVector3 dl[]={a-d,b-d,c-d}; 422 const btScalar vl=det(dl[0],dl[1],dl[2]); 423 const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; 424 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) 425 { 426 btScalar mindist=-1; 427 btScalar subw[3]; 428 U subm; 429 for(U i=0;i<3;++i) 430 { 431 const U j=imd3[i]; 432 const btScalar s=vl*dot(d,cross(dl[i],dl[j])); 433 if(s>0) 434 { 435 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); 436 if((mindist<0)||(subd<mindist)) 437 { 438 mindist = subd; 439 m = static_cast<U>((subm&1?1<<i:0)+ 440 440 (subm&2?1<<j:0)+ 441 441 (subm&4?8:0)); 442 w[i] = subw[0]; 443 w[j] = subw[1]; 444 w[imd3[j]] = 0; 445 w[3] = subw[2]; 446 } 442 w[i] = subw[0]; 443 w[j] = subw[1]; 444 w[imd3[j]] = 0; 445 w[3] = subw[2]; 446 } 447 } 448 } 449 if(mindist<0) 450 { 451 mindist = 0; 452 m = 15; 453 w[0] = det(c,b,d)/vl; 454 w[1] = det(a,c,d)/vl; 455 w[2] = det(b,a,d)/vl; 456 w[3] = 1-(w[0]+w[1]+w[2]); 457 } 458 return(mindist); 447 459 } 448 } 449 if(mindist<0) 450 { 451 mindist = 0; 452 m = 15; 453 w[0] = det(c,b,d)/vl; 454 w[1] = det(a,c,d)/vl; 455 w[2] = det(b,a,d)/vl; 456 w[3] = 1-(w[0]+w[1]+w[2]); 457 } 458 return(mindist); 459 } 460 return(-1); 461 } 462 }; 463 464 // EPA 465 struct EPA 466 { 467 /* Types */ 468 typedef GJK::sSV sSV; 469 struct sFace 460 return(-1); 461 } 462 }; 463 464 // EPA 465 struct EPA 470 466 { 471 btVector3 n; 472 btScalar d; 473 btScalar p; 474 sSV* c[3]; 475 sFace* f[3]; 476 sFace* l[2]; 477 U1 e[3]; 478 U1 pass; 479 }; 480 struct sList 481 { 482 sFace* root; 483 U count; 484 sList() : root(0),count(0) {} 485 }; 486 struct sHorizon 487 { 488 sFace* cf; 489 sFace* ff; 490 U nf; 491 sHorizon() : cf(0),ff(0),nf(0) {} 492 }; 493 struct eStatus { enum _ { 494 Valid, 495 Touching, 496 Degenerated, 497 NonConvex, 498 InvalidHull, 499 OutOfFaces, 500 OutOfVertices, 501 AccuraryReached, 502 FallBack, 503 Failed };}; 504 /* Fields */ 505 eStatus::_ m_status; 506 GJK::sSimplex m_result; 507 btVector3 m_normal; 508 btScalar m_depth; 509 sSV m_sv_store[EPA_MAX_VERTICES]; 510 sFace m_fc_store[EPA_MAX_FACES]; 511 U m_nextsv; 512 sList m_hull; 513 sList m_stock; 514 /* Methods */ 515 EPA() 516 { 517 Initialize(); 518 } 519 520 521 static inline void bind(sFace* fa,U ea,sFace* fb,U eb) 522 { 523 fa->e[ea]=(U1)eb;fa->f[ea]=fb; 524 fb->e[eb]=(U1)ea;fb->f[eb]=fa; 525 } 526 static inline void append(sList& list,sFace* face) 527 { 528 face->l[0] = 0; 529 face->l[1] = list.root; 530 if(list.root) list.root->l[0]=face; 531 list.root = face; 532 ++list.count; 533 } 534 static inline void remove(sList& list,sFace* face) 535 { 536 if(face->l[1]) face->l[1]->l[0]=face->l[0]; 537 if(face->l[0]) face->l[0]->l[1]=face->l[1]; 538 if(face==list.root) list.root=face->l[1]; 539 --list.count; 540 } 541 542 543 void Initialize() 544 { 545 m_status = eStatus::Failed; 546 m_normal = btVector3(0,0,0); 547 m_depth = 0; 548 m_nextsv = 0; 549 for(U i=0;i<EPA_MAX_FACES;++i) 550 { 551 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); 552 } 553 } 554 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess) 555 { 556 GJK::sSimplex& simplex=*gjk.m_simplex; 557 if((simplex.rank>1)&&gjk.EncloseOrigin()) 558 { 559 560 /* Clean up */ 561 while(m_hull.root) 562 { 563 sFace* f = m_hull.root; 564 remove(m_hull,f); 565 append(m_stock,f); 566 } 567 m_status = eStatus::Valid; 568 m_nextsv = 0; 569 /* Orient simplex */ 570 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, 571 simplex.c[1]->w-simplex.c[3]->w, 572 simplex.c[2]->w-simplex.c[3]->w)<0) 573 { 574 btSwap(simplex.c[0],simplex.c[1]); 575 btSwap(simplex.p[0],simplex.p[1]); 576 } 577 /* Build initial hull */ 578 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), 467 /* Types */ 468 typedef GJK::sSV sSV; 469 struct sFace 470 { 471 btVector3 n; 472 btScalar d; 473 btScalar p; 474 sSV* c[3]; 475 sFace* f[3]; 476 sFace* l[2]; 477 U1 e[3]; 478 U1 pass; 479 }; 480 struct sList 481 { 482 sFace* root; 483 U count; 484 sList() : root(0),count(0) {} 485 }; 486 struct sHorizon 487 { 488 sFace* cf; 489 sFace* ff; 490 U nf; 491 sHorizon() : cf(0),ff(0),nf(0) {} 492 }; 493 struct eStatus { enum _ { 494 Valid, 495 Touching, 496 Degenerated, 497 NonConvex, 498 InvalidHull, 499 OutOfFaces, 500 OutOfVertices, 501 AccuraryReached, 502 FallBack, 503 Failed };}; 504 /* Fields */ 505 eStatus::_ m_status; 506 GJK::sSimplex m_result; 507 btVector3 m_normal; 508 btScalar m_depth; 509 sSV m_sv_store[EPA_MAX_VERTICES]; 510 sFace m_fc_store[EPA_MAX_FACES]; 511 U m_nextsv; 512 sList m_hull; 513 sList m_stock; 514 /* Methods */ 515 EPA() 516 { 517 Initialize(); 518 } 519 520 521 static inline void bind(sFace* fa,U ea,sFace* fb,U eb) 522 { 523 fa->e[ea]=(U1)eb;fa->f[ea]=fb; 524 fb->e[eb]=(U1)ea;fb->f[eb]=fa; 525 } 526 static inline void append(sList& list,sFace* face) 527 { 528 face->l[0] = 0; 529 face->l[1] = list.root; 530 if(list.root) list.root->l[0]=face; 531 list.root = face; 532 ++list.count; 533 } 534 static inline void remove(sList& list,sFace* face) 535 { 536 if(face->l[1]) face->l[1]->l[0]=face->l[0]; 537 if(face->l[0]) face->l[0]->l[1]=face->l[1]; 538 if(face==list.root) list.root=face->l[1]; 539 --list.count; 540 } 541 542 543 void Initialize() 544 { 545 m_status = eStatus::Failed; 546 m_normal = btVector3(0,0,0); 547 m_depth = 0; 548 m_nextsv = 0; 549 for(U i=0;i<EPA_MAX_FACES;++i) 550 { 551 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); 552 } 553 } 554 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess) 555 { 556 GJK::sSimplex& simplex=*gjk.m_simplex; 557 if((simplex.rank>1)&&gjk.EncloseOrigin()) 558 { 559 560 /* Clean up */ 561 while(m_hull.root) 562 { 563 sFace* f = m_hull.root; 564 remove(m_hull,f); 565 append(m_stock,f); 566 } 567 m_status = eStatus::Valid; 568 m_nextsv = 0; 569 /* Orient simplex */ 570 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, 571 simplex.c[1]->w-simplex.c[3]->w, 572 simplex.c[2]->w-simplex.c[3]->w)<0) 573 { 574 btSwap(simplex.c[0],simplex.c[1]); 575 btSwap(simplex.p[0],simplex.p[1]); 576 } 577 /* Build initial hull */ 578 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), 579 579 newface(simplex.c[1],simplex.c[0],simplex.c[3],true), 580 580 newface(simplex.c[2],simplex.c[1],simplex.c[3],true), 581 581 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; 582 if(m_hull.count==4) 583 { 584 sFace* best=findbest(); 585 sFace outer=*best; 586 U pass=0; 587 U iterations=0; 588 bind(tetra[0],0,tetra[1],0); 589 bind(tetra[0],1,tetra[2],0); 590 bind(tetra[0],2,tetra[3],0); 591 bind(tetra[1],1,tetra[3],2); 592 bind(tetra[1],2,tetra[2],1); 593 bind(tetra[2],2,tetra[3],1); 594 m_status=eStatus::Valid; 595 for(;iterations<EPA_MAX_ITERATIONS;++iterations) 596 { 597 if(m_nextsv<EPA_MAX_VERTICES) 598 { 599 sHorizon horizon; 600 sSV* w=&m_sv_store[m_nextsv++]; 601 bool valid=true; 602 best->pass = (U1)(++pass); 603 gjk.getsupport(best->n,*w); 604 const btScalar wdist=dot(best->n,w->w)-best->d; 605 if(wdist>EPA_ACCURACY) 582 if(m_hull.count==4) 583 { 584 sFace* best=findbest(); 585 sFace outer=*best; 586 U pass=0; 587 U iterations=0; 588 bind(tetra[0],0,tetra[1],0); 589 bind(tetra[0],1,tetra[2],0); 590 bind(tetra[0],2,tetra[3],0); 591 bind(tetra[1],1,tetra[3],2); 592 bind(tetra[1],2,tetra[2],1); 593 bind(tetra[2],2,tetra[3],1); 594 m_status=eStatus::Valid; 595 for(;iterations<EPA_MAX_ITERATIONS;++iterations) 606 596 { 607 for(U j=0;(j<3)&&valid;++j) 608 { 609 valid&=expand( pass,w, 597 if(m_nextsv<EPA_MAX_VERTICES) 598 { 599 sHorizon horizon; 600 sSV* w=&m_sv_store[m_nextsv++]; 601 bool valid=true; 602 best->pass = (U1)(++pass); 603 gjk.getsupport(best->n,*w); 604 const btScalar wdist=dot(best->n,w->w)-best->d; 605 if(wdist>EPA_ACCURACY) 606 { 607 for(U j=0;(j<3)&&valid;++j) 608 { 609 valid&=expand( pass,w, 610 610 best->f[j],best->e[j], 611 611 horizon); 612 } 613 if(valid&&(horizon.nf>=3)) 614 { 615 bind(horizon.cf,1,horizon.ff,2); 616 remove(m_hull,best); 617 append(m_stock,best); 618 best=findbest(); 619 if(best->p>=outer.p) outer=*best; 620 } else { m_status=eStatus::InvalidHull;break; } 621 } else { m_status=eStatus::AccuraryReached;break; } 622 } else { m_status=eStatus::OutOfVertices;break; } 612 } 613 if(valid&&(horizon.nf>=3)) 614 { 615 bind(horizon.cf,1,horizon.ff,2); 616 remove(m_hull,best); 617 append(m_stock,best); 618 best=findbest(); 619 if(best->p>=outer.p) outer=*best; 620 } else { m_status=eStatus::InvalidHull;break; } 621 } else { m_status=eStatus::AccuraryReached;break; } 622 } else { m_status=eStatus::OutOfVertices;break; } 623 } 624 const btVector3 projection=outer.n*outer.d; 625 m_normal = outer.n; 626 m_depth = outer.d; 627 m_result.rank = 3; 628 m_result.c[0] = outer.c[0]; 629 m_result.c[1] = outer.c[1]; 630 m_result.c[2] = outer.c[2]; 631 m_result.p[0] = cross( outer.c[1]->w-projection, 632 outer.c[2]->w-projection).length(); 633 m_result.p[1] = cross( outer.c[2]->w-projection, 634 outer.c[0]->w-projection).length(); 635 m_result.p[2] = cross( outer.c[0]->w-projection, 636 outer.c[1]->w-projection).length(); 637 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; 638 m_result.p[0] /= sum; 639 m_result.p[1] /= sum; 640 m_result.p[2] /= sum; 641 return(m_status); 642 } 623 643 } 624 const btVector3 projection=outer.n*outer.d; 625 m_normal = outer.n; 626 m_depth = outer.d; 627 m_result.rank = 3; 628 m_result.c[0] = outer.c[0]; 629 m_result.c[1] = outer.c[1]; 630 m_result.c[2] = outer.c[2]; 631 m_result.p[0] = cross( outer.c[1]->w-projection, 632 outer.c[2]->w-projection).length(); 633 m_result.p[1] = cross( outer.c[2]->w-projection, 634 outer.c[0]->w-projection).length(); 635 m_result.p[2] = cross( outer.c[0]->w-projection, 636 outer.c[1]->w-projection).length(); 637 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; 638 m_result.p[0] /= sum; 639 m_result.p[1] /= sum; 640 m_result.p[2] /= sum; 641 return(m_status); 642 } 643 } 644 /* Fallback */ 645 m_status = eStatus::FallBack; 646 m_normal = -guess; 647 const btScalar nl=m_normal.length(); 648 if(nl>0) 649 m_normal = m_normal/nl; 650 else 651 m_normal = btVector3(1,0,0); 652 m_depth = 0; 653 m_result.rank=1; 654 m_result.c[0]=simplex.c[0]; 655 m_result.p[0]=1; 656 return(m_status); 644 /* Fallback */ 645 m_status = eStatus::FallBack; 646 m_normal = -guess; 647 const btScalar nl=m_normal.length(); 648 if(nl>0) 649 m_normal = m_normal/nl; 650 else 651 m_normal = btVector3(1,0,0); 652 m_depth = 0; 653 m_result.rank=1; 654 m_result.c[0]=simplex.c[0]; 655 m_result.p[0]=1; 656 return(m_status); 657 } 658 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) 659 { 660 if(m_stock.root) 661 { 662 sFace* face=m_stock.root; 663 remove(m_stock,face); 664 append(m_hull,face); 665 face->pass = 0; 666 face->c[0] = a; 667 face->c[1] = b; 668 face->c[2] = c; 669 face->n = cross(b->w-a->w,c->w-a->w); 670 const btScalar l=face->n.length(); 671 const bool v=l>EPA_ACCURACY; 672 face->p = btMin(btMin( 673 dot(a->w,cross(face->n,a->w-b->w)), 674 dot(b->w,cross(face->n,b->w-c->w))), 675 dot(c->w,cross(face->n,c->w-a->w))) / 676 (v?l:1); 677 face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; 678 if(v) 679 { 680 face->d = dot(a->w,face->n)/l; 681 face->n /= l; 682 if(forced||(face->d>=-EPA_PLANE_EPS)) 683 { 684 return(face); 685 } else m_status=eStatus::NonConvex; 686 } else m_status=eStatus::Degenerated; 687 remove(m_hull,face); 688 append(m_stock,face); 689 return(0); 690 } 691 m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; 692 return(0); 693 } 694 sFace* findbest() 695 { 696 sFace* minf=m_hull.root; 697 btScalar mind=minf->d*minf->d; 698 btScalar maxp=minf->p; 699 for(sFace* f=minf->l[1];f;f=f->l[1]) 700 { 701 const btScalar sqd=f->d*f->d; 702 if((f->p>=maxp)&&(sqd<mind)) 703 { 704 minf=f; 705 mind=sqd; 706 maxp=f->p; 707 } 708 } 709 return(minf); 710 } 711 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) 712 { 713 static const U i1m3[]={1,2,0}; 714 static const U i2m3[]={2,0,1}; 715 if(f->pass!=pass) 716 { 717 const U e1=i1m3[e]; 718 if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) 719 { 720 sFace* nf=newface(f->c[e1],f->c[e],w,false); 721 if(nf) 722 { 723 bind(nf,0,f,e); 724 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; 725 horizon.cf=nf; 726 ++horizon.nf; 727 return(true); 728 } 729 } 730 else 731 { 732 const U e2=i2m3[e]; 733 f->pass = (U1)pass; 734 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& 735 expand(pass,w,f->f[e2],f->e[e2],horizon)) 736 { 737 remove(m_hull,f); 738 append(m_stock,f); 739 return(true); 740 } 741 } 742 } 743 return(false); 744 } 745 746 }; 747 748 // 749 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, 750 const btConvexShape* shape1,const btTransform& wtrs1, 751 btGjkEpaSolver2::sResults& results, 752 tShape& shape, 753 bool withmargins) 754 { 755 /* Results */ 756 results.witnesses[0] = 757 results.witnesses[1] = btVector3(0,0,0); 758 results.status = btGjkEpaSolver2::sResults::Separated; 759 /* Shape */ 760 shape.m_shapes[0] = shape0; 761 shape.m_shapes[1] = shape1; 762 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); 763 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); 764 shape.EnableMargin(withmargins); 657 765 } 658 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)659 {660 if(m_stock.root)661 {662 sFace* face=m_stock.root;663 remove(m_stock,face);664 append(m_hull,face);665 face->pass = 0;666 face->c[0] = a;667 face->c[1] = b;668 face->c[2] = c;669 face->n = cross(b->w-a->w,c->w-a->w);670 const btScalar l=face->n.length();671 const bool v=l>EPA_ACCURACY;672 face->p = btMin(btMin(673 dot(a->w,cross(face->n,a->w-b->w)),674 dot(b->w,cross(face->n,b->w-c->w))),675 dot(c->w,cross(face->n,c->w-a->w))) /676 (v?l:1);677 face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;678 if(v)679 {680 face->d = dot(a->w,face->n)/l;681 face->n /= l;682 if(forced||(face->d>=-EPA_PLANE_EPS))683 {684 return(face);685 } else m_status=eStatus::NonConvex;686 } else m_status=eStatus::Degenerated;687 remove(m_hull,face);688 append(m_stock,face);689 return(0);690 }691 m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;692 return(0);693 }694 sFace* findbest()695 {696 sFace* minf=m_hull.root;697 btScalar mind=minf->d*minf->d;698 btScalar maxp=minf->p;699 for(sFace* f=minf->l[1];f;f=f->l[1])700 {701 const btScalar sqd=f->d*f->d;702 if((f->p>=maxp)&&(sqd<mind))703 {704 minf=f;705 mind=sqd;706 maxp=f->p;707 }708 }709 return(minf);710 }711 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)712 {713 static const U i1m3[]={1,2,0};714 static const U i2m3[]={2,0,1};715 if(f->pass!=pass)716 {717 const U e1=i1m3[e];718 if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)719 {720 sFace* nf=newface(f->c[e1],f->c[e],w,false);721 if(nf)722 {723 bind(nf,0,f,e);724 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;725 horizon.cf=nf;726 ++horizon.nf;727 return(true);728 }729 }730 else731 {732 const U e2=i2m3[e];733 f->pass = (U1)pass;734 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&735 expand(pass,w,f->f[e2],f->e[e2],horizon))736 {737 remove(m_hull,f);738 append(m_stock,f);739 return(true);740 }741 }742 }743 return(false);744 }745 746 };747 748 //749 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,750 const btConvexShape* shape1,const btTransform& wtrs1,751 btGjkEpaSolver2::sResults& results,752 tShape& shape,753 bool withmargins)754 {755 /* Results */756 results.witnesses[0] =757 results.witnesses[1] = btVector3(0,0,0);758 results.status = btGjkEpaSolver2::sResults::Separated;759 /* Shape */760 shape.m_shapes[0] = shape0;761 shape.m_shapes[1] = shape1;762 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());763 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);764 shape.EnableMargin(withmargins);765 }766 766 767 767 } … … 776 776 int btGjkEpaSolver2::StackSizeRequirement() 777 777 { 778 return(sizeof(GJK)+sizeof(EPA));778 return(sizeof(GJK)+sizeof(EPA)); 779 779 } 780 780 781 781 // 782 782 bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, 783 784 785 786 787 783 const btTransform& wtrs0, 784 const btConvexShape* shape1, 785 const btTransform& wtrs1, 786 const btVector3& guess, 787 sResults& results) 788 788 { 789 tShape shape;790 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);791 GJK gjk;792 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);793 if(gjk_status==GJK::eStatus::Valid)789 tShape shape; 790 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); 791 GJK gjk; 792 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); 793 if(gjk_status==GJK::eStatus::Valid) 794 794 { 795 btVector3 w0=btVector3(0,0,0);796 btVector3 w1=btVector3(0,0,0);797 for(U i=0;i<gjk.m_simplex->rank;++i)798 { 799 const btScalar p=gjk.m_simplex->p[i];800 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;801 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;795 btVector3 w0=btVector3(0,0,0); 796 btVector3 w1=btVector3(0,0,0); 797 for(U i=0;i<gjk.m_simplex->rank;++i) 798 { 799 const btScalar p=gjk.m_simplex->p[i]; 800 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; 801 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; 802 802 } 803 results.witnesses[0] = wtrs0*w0;804 results.witnesses[1] = wtrs0*w1;805 results.normal = w0-w1;806 results.distance = results.normal.length();807 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;808 return(true);803 results.witnesses[0] = wtrs0*w0; 804 results.witnesses[1] = wtrs0*w1; 805 results.normal = w0-w1; 806 results.distance = results.normal.length(); 807 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; 808 return(true); 809 809 } 810 810 else 811 811 { 812 results.status = gjk_status==GJK::eStatus::Inside?813 814 815 return(false);812 results.status = gjk_status==GJK::eStatus::Inside? 813 sResults::Penetrating : 814 sResults::GJK_Failed ; 815 return(false); 816 816 } 817 817 } … … 819 819 // 820 820 bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, 821 822 823 824 825 826 821 const btTransform& wtrs0, 822 const btConvexShape* shape1, 823 const btTransform& wtrs1, 824 const btVector3& guess, 825 sResults& results, 826 bool usemargins) 827 827 { 828 tShape shape;829 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);830 GJK gjk;831 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);832 switch(gjk_status)828 tShape shape; 829 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); 830 GJK gjk; 831 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); 832 switch(gjk_status) 833 833 { 834 834 case GJK::eStatus::Inside: 835 835 { 836 EPA epa;837 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);838 if(epa_status!=EPA::eStatus::Failed)839 { 840 btVector3 w0=btVector3(0,0,0);841 for(U i=0;i<epa.m_result.rank;++i)842 { 843 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];836 EPA epa; 837 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); 838 if(epa_status!=EPA::eStatus::Failed) 839 { 840 btVector3 w0=btVector3(0,0,0); 841 for(U i=0;i<epa.m_result.rank;++i) 842 { 843 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; 844 844 } 845 results.status = sResults::Penetrating;846 results.witnesses[0] = wtrs0*w0;847 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);848 results.normal = -epa.m_normal;849 results.distance = -epa.m_depth;850 return(true);845 results.status = sResults::Penetrating; 846 results.witnesses[0] = wtrs0*w0; 847 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); 848 results.normal = -epa.m_normal; 849 results.distance = -epa.m_depth; 850 return(true); 851 851 } else results.status=sResults::EPA_Failed; 852 852 } 853 break;853 break; 854 854 case GJK::eStatus::Failed: 855 results.status=sResults::GJK_Failed;856 break;855 results.status=sResults::GJK_Failed; 856 break; 857 857 } 858 return(false);858 return(false); 859 859 } 860 860 … … 866 866 sResults& results) 867 867 { 868 tShape shape;869 btSphereShape shape1(margin);870 btTransform wtrs1(btQuaternion(0,0,0,1),position);871 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);872 GJK gjk;873 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));874 if(gjk_status==GJK::eStatus::Valid)868 tShape shape; 869 btSphereShape shape1(margin); 870 btTransform wtrs1(btQuaternion(0,0,0,1),position); 871 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); 872 GJK gjk; 873 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); 874 if(gjk_status==GJK::eStatus::Valid) 875 875 { 876 btVector3 w0=btVector3(0,0,0);877 btVector3 w1=btVector3(0,0,0);878 for(U i=0;i<gjk.m_simplex->rank;++i)879 { 880 const btScalar p=gjk.m_simplex->p[i];881 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;882 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;876 btVector3 w0=btVector3(0,0,0); 877 btVector3 w1=btVector3(0,0,0); 878 for(U i=0;i<gjk.m_simplex->rank;++i) 879 { 880 const btScalar p=gjk.m_simplex->p[i]; 881 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; 882 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; 883 883 } 884 results.witnesses[0] = wtrs0*w0;885 results.witnesses[1] = wtrs0*w1;886 const btVector3 delta= results.witnesses[1]-887 888 const btScalar margin= shape0->getMargin()+889 shape1.getMargin();890 const btScalar length= delta.length();891 results.normal = delta/length;892 results.witnesses[0] += results.normal*margin;893 return(length-margin);884 results.witnesses[0] = wtrs0*w0; 885 results.witnesses[1] = wtrs0*w1; 886 const btVector3 delta= results.witnesses[1]- 887 results.witnesses[0]; 888 const btScalar margin= shape0->getMarginNonVirtual()+ 889 shape1.getMarginNonVirtual(); 890 const btScalar length= delta.length(); 891 results.normal = delta/length; 892 results.witnesses[0] += results.normal*margin; 893 return(length-margin); 894 894 } 895 895 else 896 896 { 897 if(gjk_status==GJK::eStatus::Inside)898 { 899 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))900 { 901 const btVector3 delta= results.witnesses[0]-902 903 const btScalar length= delta.length();904 if (length >= SIMD_EPSILON)905 results.normal = delta/length;906 return(-length);897 if(gjk_status==GJK::eStatus::Inside) 898 { 899 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) 900 { 901 const btVector3 delta= results.witnesses[0]- 902 results.witnesses[1]; 903 const btScalar length= delta.length(); 904 if (length >= SIMD_EPSILON) 905 results.normal = delta/length; 906 return(-length); 907 907 } 908 908 } 909 909 } 910 return(SIMD_INFINITY);910 return(SIMD_INFINITY); 911 911 } 912 912 … … 919 919 sResults& results) 920 920 { 921 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))922 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));921 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) 922 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); 923 923 else 924 return(true);924 return(true); 925 925 } 926 926 -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
r2192 r2430 19 19 #include "btGjkEpaPenetrationDepthSolver.h" 20 20 21 #ifndef __SPU__22 //#define USE_ORIGINAL_GJK 123 #endif24 25 #ifdef USE_ORIGINAL_GJK26 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"27 #endif28 21 29 22 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" … … 32 25 const btConvexShape* pConvexA, const btConvexShape* pConvexB, 33 26 const btTransform& transformA, const btTransform& transformB, 34 btVector3& v, bt Point3& wWitnessOnA, btPoint3& wWitnessOnB,27 btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, 35 28 class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) 36 29 { … … 42 35 const btScalar radialmargin(btScalar(0.)); 43 36 44 //#define USE_ORIGINAL_GJK 145 #ifdef USE_ORIGINAL_GJK46 btGjkEpaSolver::sResults results;47 if(btGjkEpaSolver::Collide( pConvexA,transformA,48 pConvexB,transformB,49 radialmargin,stackAlloc,results))50 #else51 37 btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); 52 38 btGjkEpaSolver2::sResults results; … … 55 41 guessVector,results)) 56 42 57 #endif58 43 { 59 44 // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
r2192 r2430 29 29 const btConvexShape* pConvexA, const btConvexShape* pConvexB, 30 30 const btTransform& transformA, const btTransform& transformB, 31 btVector3& v, bt Point3& wWitnessOnA, btPoint3& wWitnessOnB,31 btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, 32 32 class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); 33 33 -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
r2192 r2430 22 22 23 23 #if defined(DEBUG) || defined (_DEBUG) 24 #define TEST_NON_VIRTUAL 124 //#define TEST_NON_VIRTUAL 1 25 25 #include <stdio.h> //for debug printf 26 26 #ifdef __SPU__ … … 54 54 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) 55 55 { 56 m_cachedSeparatingDistance = 0.f; 57 56 58 btScalar distance=btScalar(0.); 57 59 btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); … … 63 65 localTransB.getOrigin() -= positionOffset; 64 66 65 67 #ifdef __SPU__ 66 68 btScalar marginA = m_minkowskiA->getMarginNonVirtual(); 67 69 btScalar marginB = m_minkowskiB->getMarginNonVirtual(); 68 70 #else 71 btScalar marginA = m_minkowskiA->getMargin(); 72 btScalar marginB = m_minkowskiB->getMargin(); 69 73 #ifdef TEST_NON_VIRTUAL 70 btScalar marginAv = m_minkowskiA->getMargin(); 71 btScalar marginBv = m_minkowskiB->getMargin(); 72 74 btScalar marginAv = m_minkowskiA->getMarginNonVirtual(); 75 btScalar marginBv = m_minkowskiB->getMarginNonVirtual(); 73 76 btAssert(marginA == marginAv); 74 77 btAssert(marginB == marginBv); 75 78 #endif //TEST_NON_VIRTUAL 79 #endif 80 81 76 82 77 83 gNumGjkChecks++; … … 118 124 btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); 119 125 126 #ifdef __SPU__ 127 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); 128 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); 129 #else 130 btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); 131 btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); 120 132 #ifdef TEST_NON_VIRTUAL 121 133 btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); 122 134 btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); 123 #endif124 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);125 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);126 #ifdef TEST_NON_VIRTUAL127 135 btAssert((pInAv-pInA).length() < 0.0001); 128 136 btAssert((qInBv-qInB).length() < 0.0001); 129 137 #endif // 130 131 btPoint3 pWorld = localTransA(pInA); 132 btPoint3 qWorld = localTransB(qInB); 138 #endif //__SPU__ 139 140 btVector3 pWorld = localTransA(pInA); 141 btVector3 qWorld = localTransB(qInB); 133 142 134 143 #ifdef DEBUG_SPU_COLLISION_DETECTION … … 332 341 spu_printf("output 1\n"); 333 342 #endif 343 m_cachedSeparatingAxis = normalInB; 344 m_cachedSeparatingDistance = distance; 345 334 346 output.addContactPoint( 335 347 normalInB, -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
r2192 r2430 21 21 22 22 #include "btDiscreteCollisionDetectorInterface.h" 23 #include "LinearMath/btPoint3.h"24 23 #include "BulletCollision/CollisionShapes/btCollisionMargin.h" 25 24 … … 39 38 const btConvexShape* m_minkowskiB; 40 39 bool m_ignoreMargin; 40 btScalar m_cachedSeparatingDistance; 41 41 42 42 … … 69 69 } 70 70 71 const btVector3& getCachedSeparatingAxis() const 72 { 73 return m_cachedSeparatingAxis; 74 } 75 btScalar getCachedSeparatingDistance() const 76 { 77 return m_cachedSeparatingDistance; 78 } 79 71 80 void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) 72 81 { -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
r2192 r2430 33 33 m_appliedImpulse(0.f), 34 34 m_lateralFrictionInitialized(false), 35 m_appliedImpulseLateral1(0.f), 36 m_appliedImpulseLateral2(0.f), 35 37 m_lifeTime(0) 36 38 { -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
r2192 r2430 71 71 const btConvexShape* convexA,const btConvexShape* convexB, 72 72 const btTransform& transA,const btTransform& transB, 73 btVector3& v, bt Point3& pa, btPoint3& pb,73 btVector3& v, btVector3& pa, btVector3& pb, 74 74 class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc 75 75 ) -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
r2192 r2430 28 28 const btConvexShape* convexA,const btConvexShape* convexB, 29 29 const btTransform& transA,const btTransform& transB, 30 btVector3& v, bt Point3& pa, btPoint3& pb,30 btVector3& v, btVector3& pa, btVector3& pb, 31 31 class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc 32 32 ); -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
r2192 r2430 17 17 #include "btPersistentManifold.h" 18 18 #include "LinearMath/btTransform.h" 19 #include <assert.h> 19 20 20 21 21 btScalar gContactBreakingThreshold = btScalar(0.02); … … 67 67 } 68 68 } 69 assert(occurance<=0);69 btAssert(occurance<=0); 70 70 #endif //DEBUG_PERSISTENCY 71 71 … … 165 165 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) 166 166 { 167 assert(validContactDistance(newPoint));167 btAssert(validContactDistance(newPoint)); 168 168 169 169 int insertIndex = getNumContacts(); … … 191 191 btScalar btPersistentManifold::getContactBreakingThreshold() const 192 192 { 193 return gContactBreakingThreshold;193 return m_contactBreakingThreshold; 194 194 } 195 195 -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
r2192 r2430 25 25 struct btCollisionResult; 26 26 27 /// contact breaking and merging threshold27 ///maximum contact breaking and merging threshold 28 28 extern btScalar gContactBreakingThreshold; 29 29 … … 55 55 int m_cachedPoints; 56 56 57 btScalar m_contactBreakingThreshold; 58 57 59 58 60 /// sort cached points so most isolated points come first … … 69 71 btPersistentManifold(); 70 72 71 btPersistentManifold(void* body0,void* body1,int bla) 72 : m_body0(body0),m_body1(body1),m_cachedPoints(0) 73 btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold) 74 : m_body0(body0),m_body1(body1),m_cachedPoints(0), 75 m_contactBreakingThreshold(contactBreakingThreshold) 73 76 { 74 (void)bla;77 75 78 } 76 79 … … 107 110 } 108 111 109 /// 112 ///@todo: get this margin from the current physics / collision environment 110 113 btScalar getContactBreakingThreshold() const; 111 114 -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
r2192 r2430 20 20 21 21 #include "LinearMath/btVector3.h" 22 #include "LinearMath/btPoint3.h"23 22 24 23 #define NO_VIRTUAL_INTERFACE 1 … … 38 37 virtual void reset() = 0; 39 38 40 virtual void addVertex(const btVector3& w, const bt Point3& p, const btPoint3& q) = 0;39 virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0; 41 40 42 41 virtual bool closest(btVector3& v) = 0; … … 46 45 virtual bool fullSimplex() const = 0; 47 46 48 virtual int getSimplex(bt Point3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0;47 virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0; 49 48 50 49 virtual bool inSimplex(const btVector3& w) = 0; … … 54 53 virtual bool emptySimplex() const = 0; 55 54 56 virtual void compute_points(bt Point3& p1, btPoint3& p2) = 0;55 virtual void compute_points(btVector3& p1, btVector3& p2) = 0; 57 56 58 57 virtual int numVertices() const =0; -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
r2192 r2430 78 78 79 79 //add a vertex 80 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const bt Point3& p, const btPoint3& q)80 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) 81 81 { 82 82 m_lastW = w; … … 268 268 269 269 //return the current simplex 270 int btVoronoiSimplexSolver::getSimplex(bt Point3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const270 int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const 271 271 { 272 272 int i; … … 315 315 } 316 316 317 void btVoronoiSimplexSolver::compute_points(bt Point3& p1, btPoint3& p2)317 void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) 318 318 { 319 319 updateClosestVectorAndPoints(); … … 326 326 327 327 328 bool btVoronoiSimplexSolver::closestPtPointTriangle(const bt Point3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result)328 bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result) 329 329 { 330 330 result.m_usedVertices.reset(); … … 426 426 427 427 /// Test if point p and d lie on opposite sides of plane through abc 428 int btVoronoiSimplexSolver::pointOutsideOfPlane(const bt Point3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)428 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) 429 429 { 430 430 btVector3 normal = (b-a).cross(c-a); … … 453 453 454 454 455 bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const bt Point3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult)455 bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) 456 456 { 457 457 btSubSimplexClosestResult tempResult; … … 487 487 { 488 488 closestPtPointTriangle(p, a, b, c,tempResult); 489 bt Point3 q = tempResult.m_closestPointOnSimplex;489 btVector3 q = tempResult.m_closestPointOnSimplex; 490 490 491 491 btScalar sqDist = (q - p).dot( q - p); … … 514 514 { 515 515 closestPtPointTriangle(p, a, c, d,tempResult); 516 bt Point3 q = tempResult.m_closestPointOnSimplex;516 btVector3 q = tempResult.m_closestPointOnSimplex; 517 517 //convert result bitmask! 518 518 … … 542 542 { 543 543 closestPtPointTriangle(p, a, d, b,tempResult); 544 bt Point3 q = tempResult.m_closestPointOnSimplex;544 btVector3 q = tempResult.m_closestPointOnSimplex; 545 545 //convert result bitmask! 546 546 … … 570 570 { 571 571 closestPtPointTriangle(p, b, d, c,tempResult); 572 bt Point3 q = tempResult.m_closestPointOnSimplex;572 btVector3 q = tempResult.m_closestPointOnSimplex; 573 573 //convert result bitmask! 574 574 btScalar sqDist = (q - p).dot( q - p); -
code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
r2192 r2430 51 51 struct btSubSimplexClosestResult 52 52 { 53 bt Point3 m_closestPointOnSimplex;53 btVector3 m_closestPointOnSimplex; 54 54 //MASK for m_usedVertices 55 55 //stores the simplex vertex-usage, using the MASK, … … 98 98 99 99 btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; 100 bt Point3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];101 bt Point3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];100 btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; 101 btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; 102 102 103 103 104 104 105 bt Point3 m_cachedP1;106 bt Point3 m_cachedP2;105 btVector3 m_cachedP1; 106 btVector3 m_cachedP2; 107 107 btVector3 m_cachedV; 108 108 btVector3 m_lastW; … … 117 117 bool updateClosestVectorAndPoints(); 118 118 119 bool closestPtPointTetrahedron(const bt Point3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult);120 int pointOutsideOfPlane(const bt Point3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d);121 bool closestPtPointTriangle(const bt Point3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result);119 bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); 120 int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); 121 bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result); 122 122 123 123 public: … … 125 125 void reset(); 126 126 127 void addVertex(const btVector3& w, const bt Point3& p, const btPoint3& q);127 void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); 128 128 129 129 … … 137 137 } 138 138 139 int getSimplex(bt Point3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;139 int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; 140 140 141 141 bool inSimplex(const btVector3& w); … … 145 145 bool emptySimplex() const ; 146 146 147 void compute_points(bt Point3& p1, btPoint3& p2) ;147 void compute_points(btVector3& p1, btVector3& p2) ; 148 148 149 149 int numVertices() const
Note: See TracChangeset
for help on using the changeset viewer.
