bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btBox2dBox2dCollisionAlgorithm.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 * The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. 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
29 btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
30 : btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
31 m_ownManifold(false),
32 m_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
41 btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
42 {
43         
44         if (m_ownManifold)
45         {
46                 if (m_manifoldPtr)
47                         m_dispatcher->releaseManifold(m_manifoldPtr);
48         }
49         
50 }
51
52
53 void b2CollidePolygons(btManifoldResult* manifold,  const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
54
55 //#include <stdio.h>
56 void 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
77 btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
78 {
79         //not yet
80         return 1.f;
81 }
82
83
84 struct 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
98 int b2_maxManifoldPoints =2;
99
100 static 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.
135 static 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.
164 static 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
255 static 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
305 void 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 }