git.maemo.org Git - blok/blob - Box2D/Source/Collision/b2Distance.cpp

   1 /*
   2 * Copyright (c) 2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * 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
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2Collision.h"
  20 #include "Shapes/b2CircleShape.h"
  21 #include "Shapes/b2PolygonShape.h"
  22
  23 int32 g_GJK_Iterations = 0;
  24
  25 // GJK using Voronoi regions (Christer Ericson) and region selection
  26 // optimizations (Casey Muratori).
  27
  28 // The origin is either in the region of points[1] or in the edge region. The origin is
  29 // not in region of points[0] because that is the old point.
  30 static int32 ProcessTwo(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
  31 {
  32         // If in point[1] region
  33         b2Vec2 r = -points[1];
  34         b2Vec2 d = points[0] - points[1];
  35         float32 length = d.Normalize();
  36         float32 lambda = b2Dot(r, d);
  37         if (lambda <= 0.0f || length < B2_FLT_EPSILON)
  38         {
  39                 // The simplex is reduced to a point.
  40                 *x1 = p1s[1];
  41                 *x2 = p2s[1];
  42                 p1s[0] = p1s[1];
  43                 p2s[0] = p2s[1];
  44                 points[0] = points[1];
  45                 return 1;
  46         }
  47
  48         // Else in edge region
  49         lambda /= length;
  50         *x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
  51         *x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
  52         return 2;
  53 }
  54
  55 // Possible regions:
  56 // - points[2]
  57 // - edge points[0]-points[2]
  58 // - edge points[1]-points[2]
  59 // - inside the triangle
  60 static int32 ProcessThree(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
  61 {
  62         b2Vec2 a = points[0];
  63         b2Vec2 b = points[1];
  64         b2Vec2 c = points[2];
  65
  66         b2Vec2 ab = b - a;
  67         b2Vec2 ac = c - a;
  68         b2Vec2 bc = c - b;
  69
  70         float32 sn = -b2Dot(a, ab), sd = b2Dot(b, ab);
  71         float32 tn = -b2Dot(a, ac), td = b2Dot(c, ac);
  72         float32 un = -b2Dot(b, bc), ud = b2Dot(c, bc);
  73
  74         // In vertex c region?
  75         if (td <= 0.0f && ud <= 0.0f)
  76         {
  77                 // Single point
  78                 *x1 = p1s[2];
  79                 *x2 = p2s[2];
  80                 p1s[0] = p1s[2];
  81                 p2s[0] = p2s[2];
  82                 points[0] = points[2];
  83                 return 1;
  84         }
  85
  86         // Should not be in vertex a or b region.
  87         B2_NOT_USED(sd);
  88         B2_NOT_USED(sn);
  89         b2Assert(sn > 0.0f || tn > 0.0f);
  90         b2Assert(sd > 0.0f || un > 0.0f);
  91
  92         float32 n = b2Cross(ab, ac);
  93
  94 #ifdef TARGET_FLOAT32_IS_FIXED
  95         n = (n < 0.0)? -1.0 : ((n > 0.0)? 1.0 : 0.0);
  96 #endif
  97
  98         // Should not be in edge ab region.
  99         float32 vc = n * b2Cross(a, b);
 100         b2Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);
 101
 102         // In edge bc region?
 103         float32 va = n * b2Cross(b, c);
 104         if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un+ud) > 0.0f)
 105         {
 106                 b2Assert(un + ud > 0.0f);
 107                 float32 lambda = un / (un + ud);
 108                 *x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
 109                 *x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
 110                 p1s[0] = p1s[2];
 111                 p2s[0] = p2s[2];
 112                 points[0] = points[2];
 113                 return 2;
 114         }
 115
 116         // In edge ac region?
 117         float32 vb = n * b2Cross(c, a);
 118         if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn+td) > 0.0f)
 119         {
 120                 b2Assert(tn + td > 0.0f);
 121                 float32 lambda = tn / (tn + td);
 122                 *x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
 123                 *x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
 124                 p1s[1] = p1s[2];
 125                 p2s[1] = p2s[2];
 126                 points[1] = points[2];
 127                 return 2;
 128         }
 129
 130         // Inside the triangle, compute barycentric coordinates
 131         float32 denom = va + vb + vc;
 132         b2Assert(denom > 0.0f);
 133         denom = 1.0f / denom;
 134
 135 #ifdef TARGET_FLOAT32_IS_FIXED
 136         *x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
 137         *x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
 138 #else
 139         float32 u = va * denom;
 140         float32 v = vb * denom;
 141         float32 w = 1.0f - u - v;
 142         *x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
 143         *x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
 144 #endif
 145         return 3;
 146 }
 147
 148 static bool InPoints(const b2Vec2& w, const b2Vec2* points, int32 pointCount)
 149 {
 150         const float32 k_tolerance = 100.0f * B2_FLT_EPSILON;
 151         for (int32 i = 0; i < pointCount; ++i)
 152         {
 153                 b2Vec2 d = b2Abs(w - points[i]);
 154                 b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
 155
 156                 if (d.x < k_tolerance * (m.x + 1.0f) &&
 157                         d.y < k_tolerance * (m.y + 1.0f))
 158                 {
 159                         return true;
 160                 }
 161         }
 162
 163         return false;
 164 }
 165
 166 template <typename T1, typename T2>
 167 float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
 168                                    const T1* shape1, const b2XForm& xf1,
 169                                    const T2* shape2, const b2XForm& xf2)
 170 {
 171         b2Vec2 p1s[3], p2s[3];
 172         b2Vec2 points[3];
 173         int32 pointCount = 0;
 174
 175         *x1 = shape1->GetFirstVertex(xf1);
 176         *x2 = shape2->GetFirstVertex(xf2);
 177
 178         float32 vSqr = 0.0f;
 179         const int32 maxIterations = 20;
 180         for (int32 iter = 0; iter < maxIterations; ++iter)
 181         {
 182                 b2Vec2 v = *x2 - *x1;
 183                 b2Vec2 w1 = shape1->Support(xf1, v);
 184                 b2Vec2 w2 = shape2->Support(xf2, -v);
 185
 186                 vSqr = b2Dot(v, v);
 187                 b2Vec2 w = w2 - w1;
 188                 float32 vw = b2Dot(v, w);
 189                 if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
 190                 {
 191                         if (pointCount == 0)
 192                         {
 193                                 *x1 = w1;
 194                                 *x2 = w2;
 195                         }
 196                         g_GJK_Iterations = iter;
 197                         return b2Sqrt(vSqr);
 198                 }
 199
 200                 switch (pointCount)
 201                 {
 202                 case 0:
 203                         p1s[0] = w1;
 204                         p2s[0] = w2;
 205                         points[0] = w;
 206                         *x1 = p1s[0];
 207                         *x2 = p2s[0];
 208                         ++pointCount;
 209                         break;
 210
 211                 case 1:
 212                         p1s[1] = w1;
 213                         p2s[1] = w2;
 214                         points[1] = w;
 215                         pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
 216                         break;
 217
 218                 case 2:
 219                         p1s[2] = w1;
 220                         p2s[2] = w2;
 221                         points[2] = w;
 222                         pointCount = ProcessThree(x1, x2, p1s, p2s, points);
 223                         break;
 224                 }
 225
 226                 // If we have three points, then the origin is in the corresponding triangle.
 227                 if (pointCount == 3)
 228                 {
 229                         g_GJK_Iterations = iter;
 230                         return 0.0f;
 231                 }
 232
 233                 float32 maxSqr = -B2_FLT_MAX;
 234                 for (int32 i = 0; i < pointCount; ++i)
 235                 {
 236                         maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
 237                 }
 238
 239 #ifdef TARGET_FLOAT32_IS_FIXED
 240                 if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
 241 #else
 242                 if (pointCount == 3 || vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
 243 #endif
 244                 {
 245                         g_GJK_Iterations = iter;
 246                         v = *x2 - *x1;
 247                         vSqr = b2Dot(v, v);
 248                         return b2Sqrt(vSqr);
 249                 }
 250         }
 251
 252         g_GJK_Iterations = maxIterations;
 253         return b2Sqrt(vSqr);
 254 }
 255
 256 static float32 DistanceCC(
 257         b2Vec2* x1, b2Vec2* x2,
 258         const b2CircleShape* circle1, const b2XForm& xf1,
 259         const b2CircleShape* circle2, const b2XForm& xf2)
 260 {
 261         b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
 262         b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
 263
 264         b2Vec2 d = p2 - p1;
 265         float32 dSqr = b2Dot(d, d);
 266         float32 r1 = circle1->GetRadius() - b2_toiSlop;
 267         float32 r2 = circle2->GetRadius() - b2_toiSlop;
 268         float32 r = r1 + r2;
 269         if (dSqr > r * r)
 270         {
 271                 float32 dLen = d.Normalize();
 272                 float32 distance = dLen - r;
 273                 *x1 = p1 + r1 * d;
 274                 *x2 = p2 - r2 * d;
 275                 return distance;
 276         }
 277         else if (dSqr > B2_FLT_EPSILON * B2_FLT_EPSILON)
 278         {
 279                 d.Normalize();
 280                 *x1 = p1 + r1 * d;
 281                 *x2 = *x1;
 282                 return 0.0f;
 283         }
 284
 285         *x1 = p1;
 286         *x2 = *x1;
 287         return 0.0f;
 288 }
 289
 290 // This is used for polygon-vs-circle distance.
 291 struct Point
 292 {
 293         b2Vec2 Support(const b2XForm&, const b2Vec2&) const
 294         {
 295                 return p;
 296         }
 297
 298         b2Vec2 GetFirstVertex(const b2XForm&) const
 299         {
 300                 return p;
 301         }
 302
 303         b2Vec2 p;
 304 };
 305
 306 // GJK is more robust with polygon-vs-point than polygon-vs-circle.
 307 // So we convert polygon-vs-circle to polygon-vs-point.
 308 static float32 DistancePC(
 309         b2Vec2* x1, b2Vec2* x2,
 310         const b2PolygonShape* polygon, const b2XForm& xf1,
 311         const b2CircleShape* circle, const b2XForm& xf2)
 312 {
 313         Point point;
 314         point.p = b2Mul(xf2, circle->GetLocalPosition());
 315
 316         float32 distance = DistanceGeneric(x1, x2, polygon, xf1, &point, b2XForm_identity);
 317
 318         float32 r = circle->GetRadius() - b2_toiSlop;
 319
 320         if (distance > r)
 321         {
 322                 distance -= r;
 323                 b2Vec2 d = *x2 - *x1;
 324                 d.Normalize();
 325                 *x2 -= r * d;
 326         }
 327         else
 328         {
 329                 distance = 0.0f;
 330                 *x2 = *x1;
 331         }
 332
 333         return distance;
 334 }
 335
 336 float32 b2Distance(b2Vec2* x1, b2Vec2* x2,
 337                                    const b2Shape* shape1, const b2XForm& xf1,
 338                                    const b2Shape* shape2, const b2XForm& xf2)
 339 {
 340         b2ShapeType type1 = shape1->GetType();
 341         b2ShapeType type2 = shape2->GetType();
 342
 343         if (type1 == e_circleShape && type2 == e_circleShape)
 344         {
 345                 return DistanceCC(x1, x2, (b2CircleShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
 346         }
 347
 348         if (type1 == e_polygonShape && type2 == e_circleShape)
 349         {
 350                 return DistancePC(x1, x2, (b2PolygonShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
 351         }
 352
 353         if (type1 == e_circleShape && type2 == e_polygonShape)
 354         {
 355                 return DistancePC(x2, x1, (b2PolygonShape*)shape2, xf2, (b2CircleShape*)shape1, xf1);
 356         }
 357
 358         if (type1 == e_polygonShape && type2 == e_polygonShape)
 359         {
 360                 return DistanceGeneric(x1, x2, (b2PolygonShape*)shape1, xf1, (b2PolygonShape*)shape2, xf2);
 361         }
 362
 363         return 0.0f;
 364 }