1/* 2Bullet Continuous Collision Detection and Physics Library 3* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com 4 5This software is provided 'as-is', without any express or implied warranty. 6In no event will the authors be held liable for any damages arising from the use of this software. 7Permission is granted to anyone to use this software for any purpose, 8including commercial applications, and to alter it and redistribute it freely, 9subject to the following restrictions: 10 111. 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. 122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 133. This notice may not be removed or altered from any source distribution. 14*/ 15 16///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library. 17///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross 18 19#include "btBox2dBox2dCollisionAlgorithm.h" 20#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" 21#include "BulletCollision/CollisionShapes/btBoxShape.h" 22#include "BulletCollision/CollisionDispatch/btCollisionObject.h" 23#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" 24#include "BulletCollision/CollisionShapes/btBox2dShape.h" 25#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" 26 27#define USE_PERSISTENT_CONTACTS 1 28 29btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap) 30: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap), 31m_ownManifold(false), 32m_manifoldPtr(mf) 33{ 34 if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject())) 35 { 36 m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()); 37 m_ownManifold = true; 38 } 39} 40 41btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() 42{ 43 44 if (m_ownManifold) 45 { 46 if (m_manifoldPtr) 47 m_dispatcher->releaseManifold(m_manifoldPtr); 48 } 49 50} 51 52 53void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); 54 55//#include <stdio.h> 56void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) 57{ 58 if (!m_manifoldPtr) 59 return; 60 61 62 const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape(); 63 const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape(); 64 65 resultOut->setPersistentManifold(m_manifoldPtr); 66 67 b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform()); 68 69 // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added 70 if (m_ownManifold) 71 { 72 resultOut->refreshContactPoints(); 73 } 74 75} 76 77btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) 78{ 79 //not yet 80 return 1.f; 81} 82 83 84struct ClipVertex 85{ 86 btVector3 v; 87 int id; 88 //b2ContactID id; 89 //b2ContactID id; 90}; 91 92#define b2Dot(a,b) (a).dot(b) 93#define b2Mul(a,b) (a)*(b) 94#define b2MulT(a,b) (a).transpose()*(b) 95#define b2Cross(a,b) (a).cross(b) 96#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) 97 98int b2_maxManifoldPoints =2; 99 100static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], 101 const btVector3& normal, btScalar offset) 102{ 103 // Start with no output points 104 int numOut = 0; 105 106 // Calculate the distance of end points to the line 107 btScalar distance0 = b2Dot(normal, vIn[0].v) - offset; 108 btScalar distance1 = b2Dot(normal, vIn[1].v) - offset; 109 110 // If the points are behind the plane 111 if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; 112 if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; 113 114 // If the points are on different sides of the plane 115 if (distance0 * distance1 < 0.0f) 116 { 117 // Find intersection point of edge and plane 118 btScalar interp = distance0 / (distance0 - distance1); 119 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); 120 if (distance0 > 0.0f) 121 { 122 vOut[numOut].id = vIn[0].id; 123 } 124 else 125 { 126 vOut[numOut].id = vIn[1].id; 127 } 128 ++numOut; 129 } 130 131 return numOut; 132} 133 134// Find the separation between poly1 and poly2 for a give edge normal on poly1. 135static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, 136 const btBox2dShape* poly2, const btTransform& xf2) 137{ 138 const btVector3* vertices1 = poly1->getVertices(); 139 const btVector3* normals1 = poly1->getNormals(); 140 141 int count2 = poly2->getVertexCount(); 142 const btVector3* vertices2 = poly2->getVertices(); 143 144 btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); 145 146 // Convert normal from poly1's frame into poly2's frame. 147 btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]); 148 btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World); 149 150 // Find support vertex on poly2 for -normal. 151 int index = 0; 152 btScalar minDot = BT_LARGE_FLOAT; 153 154 if( count2 > 0 ) 155 index = (int) normal1.minDot( vertices2, count2, minDot); 156 157 btVector3 v1 = b2Mul(xf1, vertices1[edge1]); 158 btVector3 v2 = b2Mul(xf2, vertices2[index]); 159 btScalar separation = b2Dot(v2 - v1, normal1World); 160 return separation; 161} 162 163// Find the max separation between poly1 and poly2 using edge normals from poly1. 164static btScalar FindMaxSeparation(int* edgeIndex, 165 const btBox2dShape* poly1, const btTransform& xf1, 166 const btBox2dShape* poly2, const btTransform& xf2) 167{ 168 int count1 = poly1->getVertexCount(); 169 const btVector3* normals1 = poly1->getNormals(); 170 171 // Vector pointing from the centroid of poly1 to the centroid of poly2. 172 btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid()); 173 btVector3 dLocal1 = b2MulT(xf1.getBasis(), d); 174 175 // Find edge normal on poly1 that has the largest projection onto d. 176 int edge = 0; 177 btScalar maxDot; 178 if( count1 > 0 ) 179 edge = (int) dLocal1.maxDot( normals1, count1, maxDot); 180 181 // Get the separation for the edge normal. 182 btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); 183 if (s > 0.0f) 184 { 185 return s; 186 } 187 188 // Check the separation for the previous edge normal. 189 int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; 190 btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); 191 if (sPrev > 0.0f) 192 { 193 return sPrev; 194 } 195 196 // Check the separation for the next edge normal. 197 int nextEdge = edge + 1 < count1 ? edge + 1 : 0; 198 btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); 199 if (sNext > 0.0f) 200 { 201 return sNext; 202 } 203 204 // Find the best edge and the search direction. 205 int bestEdge; 206 btScalar bestSeparation; 207 int increment; 208 if (sPrev > s && sPrev > sNext) 209 { 210 increment = -1; 211 bestEdge = prevEdge; 212 bestSeparation = sPrev; 213 } 214 else if (sNext > s) 215 { 216 increment = 1; 217 bestEdge = nextEdge; 218 bestSeparation = sNext; 219 } 220 else 221 { 222 *edgeIndex = edge; 223 return s; 224 } 225 226 // Perform a local search for the best edge normal. 227 for ( ; ; ) 228 { 229 if (increment == -1) 230 edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; 231 else 232 edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; 233 234 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); 235 if (s > 0.0f) 236 { 237 return s; 238 } 239 240 if (s > bestSeparation) 241 { 242 bestEdge = edge; 243 bestSeparation = s; 244 } 245 else 246 { 247 break; 248 } 249 } 250 251 *edgeIndex = bestEdge; 252 return bestSeparation; 253} 254 255static void FindIncidentEdge(ClipVertex c[2], 256 const btBox2dShape* poly1, const btTransform& xf1, int edge1, 257 const btBox2dShape* poly2, const btTransform& xf2) 258{ 259 const btVector3* normals1 = poly1->getNormals(); 260 261 int count2 = poly2->getVertexCount(); 262 const btVector3* vertices2 = poly2->getVertices(); 263 const btVector3* normals2 = poly2->getNormals(); 264 265 btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); 266 267 // Get the normal of the reference edge in poly2's frame. 268 btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1])); 269 270 // Find the incident edge on poly2. 271 int index = 0; 272 btScalar minDot = BT_LARGE_FLOAT; 273 for (int i = 0; i < count2; ++i) 274 { 275 btScalar dot = b2Dot(normal1, normals2[i]); 276 if (dot < minDot) 277 { 278 minDot = dot; 279 index = i; 280 } 281 } 282 283 // Build the clip vertices for the incident edge. 284 int i1 = index; 285 int i2 = i1 + 1 < count2 ? i1 + 1 : 0; 286 287 c[0].v = b2Mul(xf2, vertices2[i1]); 288// c[0].id.features.referenceEdge = (unsigned char)edge1; 289// c[0].id.features.incidentEdge = (unsigned char)i1; 290// c[0].id.features.incidentVertex = 0; 291 292 c[1].v = b2Mul(xf2, vertices2[i2]); 293// c[1].id.features.referenceEdge = (unsigned char)edge1; 294// c[1].id.features.incidentEdge = (unsigned char)i2; 295// c[1].id.features.incidentVertex = 1; 296} 297 298// Find edge normal of max separation on A - return if separating axis is found 299// Find edge normal of max separation on B - return if separation axis is found 300// Choose reference edge as min(minA, minB) 301// Find incident edge 302// Clip 303 304// The normal points from 1 to 2 305void b2CollidePolygons(btManifoldResult* manifold, 306 const btBox2dShape* polyA, const btTransform& xfA, 307 const btBox2dShape* polyB, const btTransform& xfB) 308{ 309 310 int edgeA = 0; 311 btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); 312 if (separationA > 0.0f) 313 return; 314 315 int edgeB = 0; 316 btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); 317 if (separationB > 0.0f) 318 return; 319 320 const btBox2dShape* poly1; // reference poly 321 const btBox2dShape* poly2; // incident poly 322 btTransform xf1, xf2; 323 int edge1; // reference edge 324 unsigned char flip; 325 const btScalar k_relativeTol = 0.98f; 326 const btScalar k_absoluteTol = 0.001f; 327 328 // TODO_ERIN use "radius" of poly for absolute tolerance. 329 if (separationB > k_relativeTol * separationA + k_absoluteTol) 330 { 331 poly1 = polyB; 332 poly2 = polyA; 333 xf1 = xfB; 334 xf2 = xfA; 335 edge1 = edgeB; 336 flip = 1; 337 } 338 else 339 { 340 poly1 = polyA; 341 poly2 = polyB; 342 xf1 = xfA; 343 xf2 = xfB; 344 edge1 = edgeA; 345 flip = 0; 346 } 347 348 ClipVertex incidentEdge[2]; 349 FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); 350 351 int count1 = poly1->getVertexCount(); 352 const btVector3* vertices1 = poly1->getVertices(); 353 354 btVector3 v11 = vertices1[edge1]; 355 btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; 356 357 //btVector3 dv = v12 - v11; 358 btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); 359 sideNormal.normalize(); 360 btVector3 frontNormal = btCrossS(sideNormal, 1.0f); 361 362 363 v11 = b2Mul(xf1, v11); 364 v12 = b2Mul(xf1, v12); 365 366 btScalar frontOffset = b2Dot(frontNormal, v11); 367 btScalar sideOffset1 = -b2Dot(sideNormal, v11); 368 btScalar sideOffset2 = b2Dot(sideNormal, v12); 369 370 // Clip incident edge against extruded edge1 side edges. 371 ClipVertex clipPoints1[2]; 372 clipPoints1[0].v.setValue(0,0,0); 373 clipPoints1[1].v.setValue(0,0,0); 374 375 ClipVertex clipPoints2[2]; 376 clipPoints2[0].v.setValue(0,0,0); 377 clipPoints2[1].v.setValue(0,0,0); 378 379 380 int np; 381 382 // Clip to box side 1 383 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); 384 385 if (np < 2) 386 return; 387 388 // Clip to negative box side 1 389 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); 390 391 if (np < 2) 392 { 393 return; 394 } 395 396 // Now clipPoints2 contains the clipped points. 397 btVector3 manifoldNormal = flip ? -frontNormal : frontNormal; 398 399 int pointCount = 0; 400 for (int i = 0; i < b2_maxManifoldPoints; ++i) 401 { 402 btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset; 403 404 if (separation <= 0.0f) 405 { 406 407 //b2ManifoldPoint* cp = manifold->points + pointCount; 408 //btScalar separation = separation; 409 //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); 410 //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); 411 412 manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); 413 414// cp->id = clipPoints2[i].id; 415// cp->id.features.flip = flip; 416 ++pointCount; 417 } 418 } 419 420// manifold->pointCount = pointCount;} 421} 422