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