00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "btMinkowskiPenetrationDepthSolver.h"
00017 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00018 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00019 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00020 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00021
00022 #define NUM_UNITSPHERE_POINTS 42
00023
00024
00025 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
00026 const btConvexShape* convexA,const btConvexShape* convexB,
00027 const btTransform& transA,const btTransform& transB,
00028 btVector3& v, btVector3& pa, btVector3& pb,
00029 class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
00030 )
00031 {
00032
00033 (void)stackAlloc;
00034 (void)v;
00035
00036 bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
00037
00038 struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
00039 {
00040
00041 btIntermediateResult():m_hasResult(false)
00042 {
00043 }
00044
00045 btVector3 m_normalOnBInWorld;
00046 btVector3 m_pointInWorld;
00047 btScalar m_depth;
00048 bool m_hasResult;
00049
00050 virtual void setShapeIdentifiersA(int partId0,int index0)
00051 {
00052 (void)partId0;
00053 (void)index0;
00054 }
00055 virtual void setShapeIdentifiersB(int partId1,int index1)
00056 {
00057 (void)partId1;
00058 (void)index1;
00059 }
00060 void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
00061 {
00062 m_normalOnBInWorld = normalOnBInWorld;
00063 m_pointInWorld = pointInWorld;
00064 m_depth = depth;
00065 m_hasResult = true;
00066 }
00067 };
00068
00069
00070 btScalar minProj = btScalar(BT_LARGE_FLOAT);
00071 btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
00072 btVector3 minA,minB;
00073 btVector3 seperatingAxisInA,seperatingAxisInB;
00074 btVector3 pInA,qInB,pWorld,qWorld,w;
00075
00076 #ifndef __SPU__
00077 #define USE_BATCHED_SUPPORT 1
00078 #endif
00079 #ifdef USE_BATCHED_SUPPORT
00080
00081 btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00082 btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00083 btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00084 btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
00085 int i;
00086
00087 int numSampleDirections = NUM_UNITSPHERE_POINTS;
00088
00089 for (i=0;i<numSampleDirections;i++)
00090 {
00091 btVector3 norm = getPenetrationDirections()[i];
00092 seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
00093 seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
00094 }
00095
00096 {
00097 int numPDA = convexA->getNumPreferredPenetrationDirections();
00098 if (numPDA)
00099 {
00100 for (int i=0;i<numPDA;i++)
00101 {
00102 btVector3 norm;
00103 convexA->getPreferredPenetrationDirection(i,norm);
00104 norm = transA.getBasis() * norm;
00105 getPenetrationDirections()[numSampleDirections] = norm;
00106 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
00107 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
00108 numSampleDirections++;
00109 }
00110 }
00111 }
00112
00113 {
00114 int numPDB = convexB->getNumPreferredPenetrationDirections();
00115 if (numPDB)
00116 {
00117 for (int i=0;i<numPDB;i++)
00118 {
00119 btVector3 norm;
00120 convexB->getPreferredPenetrationDirection(i,norm);
00121 norm = transB.getBasis() * norm;
00122 getPenetrationDirections()[numSampleDirections] = norm;
00123 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
00124 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
00125 numSampleDirections++;
00126 }
00127 }
00128 }
00129
00130
00131
00132
00133 convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
00134 convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
00135
00136 for (i=0;i<numSampleDirections;i++)
00137 {
00138 btVector3 norm = getPenetrationDirections()[i];
00139 if (check2d)
00140 {
00141 norm[2] = 0.f;
00142 }
00143 if (norm.length2()>0.01)
00144 {
00145
00146 seperatingAxisInA = seperatingAxisInABatch[i];
00147 seperatingAxisInB = seperatingAxisInBBatch[i];
00148
00149 pInA = supportVerticesABatch[i];
00150 qInB = supportVerticesBBatch[i];
00151
00152 pWorld = transA(pInA);
00153 qWorld = transB(qInB);
00154 if (check2d)
00155 {
00156 pWorld[2] = 0.f;
00157 qWorld[2] = 0.f;
00158 }
00159
00160 w = qWorld - pWorld;
00161 btScalar delta = norm.dot(w);
00162
00163 if (delta < minProj)
00164 {
00165 minProj = delta;
00166 minNorm = norm;
00167 minA = pWorld;
00168 minB = qWorld;
00169 }
00170 }
00171 }
00172 #else
00173
00174 int numSampleDirections = NUM_UNITSPHERE_POINTS;
00175
00176 #ifndef __SPU__
00177 {
00178 int numPDA = convexA->getNumPreferredPenetrationDirections();
00179 if (numPDA)
00180 {
00181 for (int i=0;i<numPDA;i++)
00182 {
00183 btVector3 norm;
00184 convexA->getPreferredPenetrationDirection(i,norm);
00185 norm = transA.getBasis() * norm;
00186 getPenetrationDirections()[numSampleDirections] = norm;
00187 numSampleDirections++;
00188 }
00189 }
00190 }
00191
00192 {
00193 int numPDB = convexB->getNumPreferredPenetrationDirections();
00194 if (numPDB)
00195 {
00196 for (int i=0;i<numPDB;i++)
00197 {
00198 btVector3 norm;
00199 convexB->getPreferredPenetrationDirection(i,norm);
00200 norm = transB.getBasis() * norm;
00201 getPenetrationDirections()[numSampleDirections] = norm;
00202 numSampleDirections++;
00203 }
00204 }
00205 }
00206 #endif // __SPU__
00207
00208 for (int i=0;i<numSampleDirections;i++)
00209 {
00210 const btVector3& norm = getPenetrationDirections()[i];
00211 seperatingAxisInA = (-norm)* transA.getBasis();
00212 seperatingAxisInB = norm* transB.getBasis();
00213 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
00214 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
00215 pWorld = transA(pInA);
00216 qWorld = transB(qInB);
00217 w = qWorld - pWorld;
00218 btScalar delta = norm.dot(w);
00219
00220 if (delta < minProj)
00221 {
00222 minProj = delta;
00223 minNorm = norm;
00224 minA = pWorld;
00225 minB = qWorld;
00226 }
00227 }
00228 #endif //USE_BATCHED_SUPPORT
00229
00230
00231
00232 minA += minNorm*convexA->getMarginNonVirtual();
00233 minB -= minNorm*convexB->getMarginNonVirtual();
00234
00235 if (minProj < btScalar(0.))
00236 return false;
00237
00238 btScalar extraSeparation = 0.5f;
00239 minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
00240
00241
00242
00243
00244
00245
00246 #ifdef DEBUG_DRAW
00247 if (debugDraw)
00248 {
00249 btVector3 color(0,1,0);
00250 debugDraw->drawLine(minA,minB,color);
00251 color = btVector3 (1,1,1);
00252 btVector3 vec = minB-minA;
00253 btScalar prj2 = minNorm.dot(vec);
00254 debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
00255
00256 }
00257 #endif //DEBUG_DRAW
00258
00259
00260
00261 btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
00262
00263 btScalar offsetDist = minProj;
00264 btVector3 offset = minNorm * offsetDist;
00265
00266
00267
00268 btGjkPairDetector::ClosestPointInput input;
00269
00270 btVector3 newOrg = transA.getOrigin() + offset;
00271
00272 btTransform displacedTrans = transA;
00273 displacedTrans.setOrigin(newOrg);
00274
00275 input.m_transformA = displacedTrans;
00276 input.m_transformB = transB;
00277 input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);
00278
00279 btIntermediateResult res;
00280 gjkdet.setCachedSeperatingAxis(-minNorm);
00281 gjkdet.getClosestPoints(input,res,debugDraw);
00282
00283 btScalar correctedMinNorm = minProj - res.m_depth;
00284
00285
00286
00287 btScalar penetration_relaxation= btScalar(1.);
00288 minNorm*=penetration_relaxation;
00289
00290
00291 if (res.m_hasResult)
00292 {
00293
00294 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
00295 pb = res.m_pointInWorld;
00296 v = minNorm;
00297
00298 #ifdef DEBUG_DRAW
00299 if (debugDraw)
00300 {
00301 btVector3 color(1,0,0);
00302 debugDraw->drawLine(pa,pb,color);
00303 }
00304 #endif//DEBUG_DRAW
00305
00306
00307 }
00308 return res.m_hasResult;
00309 }
00310
00311 btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
00312 {
00313 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
00314 {
00315 btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
00316 btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
00317 btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
00318 btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
00319 btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
00320 btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
00321 btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
00322 btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
00323 btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
00324 btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
00325 btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
00326 btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
00327 btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
00328 btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
00329 btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
00330 btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
00331 btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
00332 btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
00333 btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
00334 btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
00335 btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
00336 btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
00337 btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
00338 btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
00339 btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
00340 btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
00341 btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
00342 btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
00343 btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
00344 btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
00345 btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
00346 btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
00347 btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
00348 btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
00349 btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
00350 btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
00351 btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
00352 btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
00353 btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
00354 btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
00355 btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
00356 btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
00357 };
00358
00359 return sPenetrationDirections;
00360 }
00361
00362