btBox2dBox2dCollisionAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 * The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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 //#include <stdio.h>
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         //  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
00070         if (m_ownManifold)
00071         {
00072                 resultOut->refreshContactPoints();
00073         }
00074 
00075 }
00076 
00077 btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
00078 {
00079         //not yet
00080         return 1.f;
00081 }
00082 
00083 
00084 struct ClipVertex
00085 {
00086         btVector3 v;
00087         int id;
00088         //b2ContactID id;
00089         //b2ContactID id;
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         // Start with no output points
00104         int numOut = 0;
00105 
00106         // Calculate the distance of end points to the line
00107         btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
00108         btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
00109 
00110         // If the points are behind the plane
00111         if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
00112         if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
00113 
00114         // If the points are on different sides of the plane
00115         if (distance0 * distance1 < 0.0f)
00116         {
00117                 // Find intersection point of edge and plane
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 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
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         // Convert normal from poly1's frame into poly2's frame.
00147         btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
00148         btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
00149 
00150         // Find support vertex on poly2 for -normal.
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 // Find the max separation between poly1 and poly2 using edge normals from poly1.
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         // Vector pointing from the centroid of poly1 to the centroid of poly2.
00172         btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
00173         btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
00174 
00175         // Find edge normal on poly1 that has the largest projection onto d.
00176         int edge = 0;
00177     btScalar maxDot;
00178     if( count1 > 0 )
00179         edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
00180 
00181         // Get the separation for the edge normal.
00182         btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
00183         if (s > 0.0f)
00184         {
00185                 return s;
00186         }
00187 
00188         // Check the separation for the previous edge normal.
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         // Check the separation for the next edge normal.
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         // Find the best edge and the search direction.
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         // Perform a local search for the best edge normal.
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         // Get the normal of the reference edge in poly2's frame.
00268         btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
00269 
00270         // Find the incident edge on poly2.
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         // Build the clip vertices for the incident edge.
00284         int i1 = index;
00285         int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
00286 
00287         c[0].v = b2Mul(xf2, vertices2[i1]);
00288 //      c[0].id.features.referenceEdge = (unsigned char)edge1;
00289 //      c[0].id.features.incidentEdge = (unsigned char)i1;
00290 //      c[0].id.features.incidentVertex = 0;
00291 
00292         c[1].v = b2Mul(xf2, vertices2[i2]);
00293 //      c[1].id.features.referenceEdge = (unsigned char)edge1;
00294 //      c[1].id.features.incidentEdge = (unsigned char)i2;
00295 //      c[1].id.features.incidentVertex = 1;
00296 }
00297 
00298 // Find edge normal of max separation on A - return if separating axis is found
00299 // Find edge normal of max separation on B - return if separation axis is found
00300 // Choose reference edge as min(minA, minB)
00301 // Find incident edge
00302 // Clip
00303 
00304 // The normal points from 1 to 2
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;      // reference poly
00321         const btBox2dShape* poly2;      // incident poly
00322         btTransform xf1, xf2;
00323         int edge1;              // reference edge
00324         unsigned char flip;
00325         const btScalar k_relativeTol = 0.98f;
00326         const btScalar k_absoluteTol = 0.001f;
00327 
00328         // TODO_ERIN use "radius" of poly for absolute tolerance.
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         //btVector3 dv = v12 - v11;
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         // Clip incident edge against extruded edge1 side edges.
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         // Clip to box side 1
00383         np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
00384 
00385         if (np < 2)
00386                 return;
00387 
00388         // Clip to negative box side 1
00389         np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
00390 
00391         if (np < 2)
00392         {
00393                 return;
00394         }
00395 
00396         // Now clipPoints2 contains the clipped points.
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                         //b2ManifoldPoint* cp = manifold->points + pointCount;
00408                         //btScalar separation = separation;
00409                         //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
00410                         //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
00411 
00412                         manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
00413 
00414 //                      cp->id = clipPoints2[i].id;
00415 //                      cp->id.features.flip = flip;
00416                         ++pointCount;
00417                 }
00418         }
00419 
00420 //      manifold->pointCount = pointCount;}
00421 }