00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00018
00019 #include "btBox2dBox2dCollisionAlgorithm.h"
00020 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00021 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00022 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00023 #include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
00024 #include "BulletCollision/CollisionShapes/btBox2dShape.h"
00025 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00026
00027 #define USE_PERSISTENT_CONTACTS 1
00028
00029 btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
00030 : btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
00031 m_ownManifold(false),
00032 m_manifoldPtr(mf)
00033 {
00034 if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()))
00035 {
00036 m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject());
00037 m_ownManifold = true;
00038 }
00039 }
00040
00041 btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
00042 {
00043
00044 if (m_ownManifold)
00045 {
00046 if (m_manifoldPtr)
00047 m_dispatcher->releaseManifold(m_manifoldPtr);
00048 }
00049
00050 }
00051
00052
00053 void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
00054
00055
00056 void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00057 {
00058 if (!m_manifoldPtr)
00059 return;
00060
00061
00062 const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
00063 const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
00064
00065 resultOut->setPersistentManifold(m_manifoldPtr);
00066
00067 b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform());
00068
00069
00070 if (m_ownManifold)
00071 {
00072 resultOut->refreshContactPoints();
00073 }
00074
00075 }
00076
00077 btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
00078 {
00079
00080 return 1.f;
00081 }
00082
00083
00084 struct ClipVertex
00085 {
00086 btVector3 v;
00087 int id;
00088
00089
00090 };
00091
00092 #define b2Dot(a,b) (a).dot(b)
00093 #define b2Mul(a,b) (a)*(b)
00094 #define b2MulT(a,b) (a).transpose()*(b)
00095 #define b2Cross(a,b) (a).cross(b)
00096 #define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
00097
00098 int b2_maxManifoldPoints =2;
00099
00100 static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
00101 const btVector3& normal, btScalar offset)
00102 {
00103
00104 int numOut = 0;
00105
00106
00107 btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
00108 btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
00109
00110
00111 if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
00112 if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
00113
00114
00115 if (distance0 * distance1 < 0.0f)
00116 {
00117
00118 btScalar interp = distance0 / (distance0 - distance1);
00119 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
00120 if (distance0 > 0.0f)
00121 {
00122 vOut[numOut].id = vIn[0].id;
00123 }
00124 else
00125 {
00126 vOut[numOut].id = vIn[1].id;
00127 }
00128 ++numOut;
00129 }
00130
00131 return numOut;
00132 }
00133
00134
00135 static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
00136 const btBox2dShape* poly2, const btTransform& xf2)
00137 {
00138 const btVector3* vertices1 = poly1->getVertices();
00139 const btVector3* normals1 = poly1->getNormals();
00140
00141 int count2 = poly2->getVertexCount();
00142 const btVector3* vertices2 = poly2->getVertices();
00143
00144 btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
00145
00146
00147 btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
00148 btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
00149
00150
00151 int index = 0;
00152 btScalar minDot = BT_LARGE_FLOAT;
00153
00154 if( count2 > 0 )
00155 index = (int) normal1.minDot( vertices2, count2, minDot);
00156
00157 btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
00158 btVector3 v2 = b2Mul(xf2, vertices2[index]);
00159 btScalar separation = b2Dot(v2 - v1, normal1World);
00160 return separation;
00161 }
00162
00163
00164 static btScalar FindMaxSeparation(int* edgeIndex,
00165 const btBox2dShape* poly1, const btTransform& xf1,
00166 const btBox2dShape* poly2, const btTransform& xf2)
00167 {
00168 int count1 = poly1->getVertexCount();
00169 const btVector3* normals1 = poly1->getNormals();
00170
00171
00172 btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
00173 btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
00174
00175
00176 int edge = 0;
00177 btScalar maxDot;
00178 if( count1 > 0 )
00179 edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
00180
00181
00182 btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
00183 if (s > 0.0f)
00184 {
00185 return s;
00186 }
00187
00188
00189 int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
00190 btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
00191 if (sPrev > 0.0f)
00192 {
00193 return sPrev;
00194 }
00195
00196
00197 int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
00198 btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
00199 if (sNext > 0.0f)
00200 {
00201 return sNext;
00202 }
00203
00204
00205 int bestEdge;
00206 btScalar bestSeparation;
00207 int increment;
00208 if (sPrev > s && sPrev > sNext)
00209 {
00210 increment = -1;
00211 bestEdge = prevEdge;
00212 bestSeparation = sPrev;
00213 }
00214 else if (sNext > s)
00215 {
00216 increment = 1;
00217 bestEdge = nextEdge;
00218 bestSeparation = sNext;
00219 }
00220 else
00221 {
00222 *edgeIndex = edge;
00223 return s;
00224 }
00225
00226
00227 for ( ; ; )
00228 {
00229 if (increment == -1)
00230 edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
00231 else
00232 edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
00233
00234 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
00235 if (s > 0.0f)
00236 {
00237 return s;
00238 }
00239
00240 if (s > bestSeparation)
00241 {
00242 bestEdge = edge;
00243 bestSeparation = s;
00244 }
00245 else
00246 {
00247 break;
00248 }
00249 }
00250
00251 *edgeIndex = bestEdge;
00252 return bestSeparation;
00253 }
00254
00255 static void FindIncidentEdge(ClipVertex c[2],
00256 const btBox2dShape* poly1, const btTransform& xf1, int edge1,
00257 const btBox2dShape* poly2, const btTransform& xf2)
00258 {
00259 const btVector3* normals1 = poly1->getNormals();
00260
00261 int count2 = poly2->getVertexCount();
00262 const btVector3* vertices2 = poly2->getVertices();
00263 const btVector3* normals2 = poly2->getNormals();
00264
00265 btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
00266
00267
00268 btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
00269
00270
00271 int index = 0;
00272 btScalar minDot = BT_LARGE_FLOAT;
00273 for (int i = 0; i < count2; ++i)
00274 {
00275 btScalar dot = b2Dot(normal1, normals2[i]);
00276 if (dot < minDot)
00277 {
00278 minDot = dot;
00279 index = i;
00280 }
00281 }
00282
00283
00284 int i1 = index;
00285 int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
00286
00287 c[0].v = b2Mul(xf2, vertices2[i1]);
00288
00289
00290
00291
00292 c[1].v = b2Mul(xf2, vertices2[i2]);
00293
00294
00295
00296 }
00297
00298
00299
00300
00301
00302
00303
00304
00305 void b2CollidePolygons(btManifoldResult* manifold,
00306 const btBox2dShape* polyA, const btTransform& xfA,
00307 const btBox2dShape* polyB, const btTransform& xfB)
00308 {
00309
00310 int edgeA = 0;
00311 btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
00312 if (separationA > 0.0f)
00313 return;
00314
00315 int edgeB = 0;
00316 btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
00317 if (separationB > 0.0f)
00318 return;
00319
00320 const btBox2dShape* poly1;
00321 const btBox2dShape* poly2;
00322 btTransform xf1, xf2;
00323 int edge1;
00324 unsigned char flip;
00325 const btScalar k_relativeTol = 0.98f;
00326 const btScalar k_absoluteTol = 0.001f;
00327
00328
00329 if (separationB > k_relativeTol * separationA + k_absoluteTol)
00330 {
00331 poly1 = polyB;
00332 poly2 = polyA;
00333 xf1 = xfB;
00334 xf2 = xfA;
00335 edge1 = edgeB;
00336 flip = 1;
00337 }
00338 else
00339 {
00340 poly1 = polyA;
00341 poly2 = polyB;
00342 xf1 = xfA;
00343 xf2 = xfB;
00344 edge1 = edgeA;
00345 flip = 0;
00346 }
00347
00348 ClipVertex incidentEdge[2];
00349 FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
00350
00351 int count1 = poly1->getVertexCount();
00352 const btVector3* vertices1 = poly1->getVertices();
00353
00354 btVector3 v11 = vertices1[edge1];
00355 btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
00356
00357
00358 btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
00359 sideNormal.normalize();
00360 btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
00361
00362
00363 v11 = b2Mul(xf1, v11);
00364 v12 = b2Mul(xf1, v12);
00365
00366 btScalar frontOffset = b2Dot(frontNormal, v11);
00367 btScalar sideOffset1 = -b2Dot(sideNormal, v11);
00368 btScalar sideOffset2 = b2Dot(sideNormal, v12);
00369
00370
00371 ClipVertex clipPoints1[2];
00372 clipPoints1[0].v.setValue(0,0,0);
00373 clipPoints1[1].v.setValue(0,0,0);
00374
00375 ClipVertex clipPoints2[2];
00376 clipPoints2[0].v.setValue(0,0,0);
00377 clipPoints2[1].v.setValue(0,0,0);
00378
00379
00380 int np;
00381
00382
00383 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
00384
00385 if (np < 2)
00386 return;
00387
00388
00389 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
00390
00391 if (np < 2)
00392 {
00393 return;
00394 }
00395
00396
00397 btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
00398
00399 int pointCount = 0;
00400 for (int i = 0; i < b2_maxManifoldPoints; ++i)
00401 {
00402 btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
00403
00404 if (separation <= 0.0f)
00405 {
00406
00407
00408
00409
00410
00411
00412 manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
00413
00414
00415
00416 ++pointCount;
00417 }
00418 }
00419
00420
00421 }