Extras/CDTestFramework/Opcode/OPC_LSSAABBOverlap.h

   1 /*
   2  *      OPCODE - Optimized Collision Detection
   3  * http://www.codercorner.com/Opcode.htm
   4  *
   5  * Copyright (c) 2001-2008 Pierre Terdiman,  pierre@codercorner.com
   6
   7 This software is provided 'as-is', without any express or implied warranty.
   8 In no event will the authors be held liable for any damages arising from the use of this software.
   9 Permission is granted to anyone to use this software for any purpose,
  10 including commercial applications, and to alter it and redistribute it freely,
  11 subject to the following restrictions:
  12
  13 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.
  14 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  15 3. This notice may not be removed or altered from any source distribution.
  16 */
  17
  18 // Following code from Magic-Software (http://www.magic-software.com/)
  19 // A bit modified for Opcode
  20
  21 inline_ float OPC_PointAABBSqrDist(const Point& point, const Point& center, const Point& extents)
  22 {
  23         // Compute coordinates of point in box coordinate system
  24         Point Closest = point - center;
  25
  26         float SqrDistance = 0.0f;
  27
  28         if(Closest.x < -extents.x)
  29         {
  30                 float Delta = Closest.x + extents.x;
  31                 SqrDistance += Delta*Delta;
  32         }
  33         else if(Closest.x > extents.x)
  34         {
  35                 float Delta = Closest.x - extents.x;
  36                 SqrDistance += Delta*Delta;
  37         }
  38
  39         if(Closest.y < -extents.y)
  40         {
  41                 float Delta = Closest.y + extents.y;
  42                 SqrDistance += Delta*Delta;
  43         }
  44         else if(Closest.y > extents.y)
  45         {
  46                 float Delta = Closest.y - extents.y;
  47                 SqrDistance += Delta*Delta;
  48         }
  49
  50         if(Closest.z < -extents.z)
  51         {
  52                 float Delta = Closest.z + extents.z;
  53                 SqrDistance += Delta*Delta;
  54         }
  55         else if(Closest.z > extents.z)
  56         {
  57                 float Delta = Closest.z - extents.z;
  58                 SqrDistance += Delta*Delta;
  59         }
  60         return SqrDistance;
  61 }
  62
  63 static void Face(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, const Point& rkPmE, float* pfLParam, float& rfSqrDistance)
  64 {
  65     Point kPpE;
  66     float fLSqr, fInv, fTmp, fParam, fT, fDelta;
  67
  68     kPpE[i1] = rkPnt[i1] + extents[i1];
  69     kPpE[i2] = rkPnt[i2] + extents[i2];
  70     if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0])
  71     {
  72         if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0])
  73         {
  74             // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
  75             if(pfLParam)
  76             {
  77                 rkPnt[i0] = extents[i0];
  78                 fInv = 1.0f/rkDir[i0];
  79                 rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
  80                 rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
  81                 *pfLParam = -rkPmE[i0]*fInv;
  82             }
  83         }
  84         else
  85         {
  86             // v[i1] >= -e[i1], v[i2] < -e[i2]
  87             fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
  88             fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
  89             if(fTmp <= 2.0f*fLSqr*extents[i1])
  90             {
  91                 fT = fTmp/fLSqr;
  92                 fLSqr += rkDir[i1]*rkDir[i1];
  93                 fTmp = kPpE[i1] - fT;
  94                 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
  95                 fParam = -fDelta/fLSqr;
  96                 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
  97
  98                 if(pfLParam)
  99                 {
 100                     *pfLParam = fParam;
 101                     rkPnt[i0] = extents[i0];
 102                     rkPnt[i1] = fT - extents[i1];
 103                     rkPnt[i2] = -extents[i2];
 104                 }
 105             }
 106             else
 107             {
 108                 fLSqr += rkDir[i1]*rkDir[i1];
 109                 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
 110                 fParam = -fDelta/fLSqr;
 111                 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
 112
 113                 if(pfLParam)
 114                 {
 115                     *pfLParam = fParam;
 116                     rkPnt[i0] = extents[i0];
 117                     rkPnt[i1] = extents[i1];
 118                     rkPnt[i2] = -extents[i2];
 119                 }
 120             }
 121         }
 122     }
 123     else
 124     {
 125         if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
 126         {
 127             // v[i1] < -e[i1], v[i2] >= -e[i2]
 128             fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
 129             fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
 130             if(fTmp <= 2.0f*fLSqr*extents[i2])
 131             {
 132                 fT = fTmp/fLSqr;
 133                 fLSqr += rkDir[i2]*rkDir[i2];
 134                 fTmp = kPpE[i2] - fT;
 135                 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
 136                 fParam = -fDelta/fLSqr;
 137                 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
 138
 139                 if(pfLParam)
 140                 {
 141                     *pfLParam = fParam;
 142                     rkPnt[i0] = extents[i0];
 143                     rkPnt[i1] = -extents[i1];
 144                     rkPnt[i2] = fT - extents[i2];
 145                 }
 146             }
 147             else
 148             {
 149                 fLSqr += rkDir[i2]*rkDir[i2];
 150                 fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
 151                 fParam = -fDelta/fLSqr;
 152                 rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
 153
 154                 if(pfLParam)
 155                 {
 156                     *pfLParam = fParam;
 157                     rkPnt[i0] = extents[i0];
 158                     rkPnt[i1] = -extents[i1];
 159                     rkPnt[i2] = extents[i2];
 160                 }
 161             }
 162         }
 163         else
 164         {
 165             // v[i1] < -e[i1], v[i2] < -e[i2]
 166             fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
 167             fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
 168             if(fTmp >= 0.0f)
 169             {
 170                 // v[i1]-edge is closest
 171                 if ( fTmp <= 2.0f*fLSqr*extents[i1] )
 172                 {
 173                     fT = fTmp/fLSqr;
 174                     fLSqr += rkDir[i1]*rkDir[i1];
 175                     fTmp = kPpE[i1] - fT;
 176                     fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
 177                     fParam = -fDelta/fLSqr;
 178                     rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
 179
 180                     if(pfLParam)
 181                     {
 182                         *pfLParam = fParam;
 183                         rkPnt[i0] = extents[i0];
 184                         rkPnt[i1] = fT - extents[i1];
 185                         rkPnt[i2] = -extents[i2];
 186                     }
 187                 }
 188                 else
 189                 {
 190                     fLSqr += rkDir[i1]*rkDir[i1];
 191                     fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
 192                     fParam = -fDelta/fLSqr;
 193                     rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
 194
 195                     if(pfLParam)
 196                     {
 197                         *pfLParam = fParam;
 198                         rkPnt[i0] = extents[i0];
 199                         rkPnt[i1] = extents[i1];
 200                         rkPnt[i2] = -extents[i2];
 201                     }
 202                 }
 203                 return;
 204             }
 205
 206             fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
 207             fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
 208             if(fTmp >= 0.0f)
 209             {
 210                 // v[i2]-edge is closest
 211                 if(fTmp <= 2.0f*fLSqr*extents[i2])
 212                 {
 213                     fT = fTmp/fLSqr;
 214                     fLSqr += rkDir[i2]*rkDir[i2];
 215                     fTmp = kPpE[i2] - fT;
 216                     fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
 217                     fParam = -fDelta/fLSqr;
 218                     rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
 219
 220                     if(pfLParam)
 221                     {
 222                         *pfLParam = fParam;
 223                         rkPnt[i0] = extents[i0];
 224                         rkPnt[i1] = -extents[i1];
 225                         rkPnt[i2] = fT - extents[i2];
 226                     }
 227                 }
 228                 else
 229                 {
 230                     fLSqr += rkDir[i2]*rkDir[i2];
 231                     fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
 232                     fParam = -fDelta/fLSqr;
 233                     rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
 234
 235                     if(pfLParam)
 236                     {
 237                         *pfLParam = fParam;
 238                         rkPnt[i0] = extents[i0];
 239                         rkPnt[i1] = -extents[i1];
 240                         rkPnt[i2] = extents[i2];
 241                     }
 242                 }
 243                 return;
 244             }
 245
 246             // (v[i1],v[i2])-corner is closest
 247             fLSqr += rkDir[i2]*rkDir[i2];
 248             fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2];
 249             fParam = -fDelta/fLSqr;
 250             rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
 251
 252             if(pfLParam)
 253             {
 254                 *pfLParam = fParam;
 255                 rkPnt[i0] = extents[i0];
 256                 rkPnt[i1] = -extents[i1];
 257                 rkPnt[i2] = -extents[i2];
 258             }
 259         }
 260     }
 261 }
 262
 263 static void CaseNoZeros(Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
 264 {
 265     Point kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z);
 266
 267     float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
 268
 269     fProdDxPy = rkDir.x*kPmE.y;
 270     fProdDyPx = rkDir.y*kPmE.x;
 271     if(fProdDyPx >= fProdDxPy)
 272     {
 273         fProdDzPx = rkDir.z*kPmE.x;
 274         fProdDxPz = rkDir.x*kPmE.z;
 275         if(fProdDzPx >= fProdDxPz)
 276         {
 277             // line intersects x = e0
 278             Face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
 279         }
 280         else
 281         {
 282             // line intersects z = e2
 283             Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
 284         }
 285     }
 286     else
 287     {
 288         fProdDzPy = rkDir.z*kPmE.y;
 289         fProdDyPz = rkDir.y*kPmE.z;
 290         if(fProdDzPy >= fProdDyPz)
 291         {
 292             // line intersects y = e1
 293             Face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
 294         }
 295         else
 296         {
 297             // line intersects z = e2
 298             Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
 299         }
 300     }
 301 }
 302
 303 static void Case0(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
 304 {
 305     float fPmE0 = rkPnt[i0] - extents[i0];
 306     float fPmE1 = rkPnt[i1] - extents[i1];
 307     float fProd0 = rkDir[i1]*fPmE0;
 308     float fProd1 = rkDir[i0]*fPmE1;
 309     float fDelta, fInvLSqr, fInv;
 310
 311     if(fProd0 >= fProd1)
 312     {
 313         // line intersects P[i0] = e[i0]
 314         rkPnt[i0] = extents[i0];
 315
 316         float fPpE1 = rkPnt[i1] + extents[i1];
 317         fDelta = fProd0 - rkDir[i0]*fPpE1;
 318         if(fDelta >= 0.0f)
 319         {
 320             fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
 321             rfSqrDistance += fDelta*fDelta*fInvLSqr;
 322             if(pfLParam)
 323             {
 324                 rkPnt[i1] = -extents[i1];
 325                 *pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
 326             }
 327         }
 328         else
 329         {
 330             if(pfLParam)
 331             {
 332                 fInv = 1.0f/rkDir[i0];
 333                 rkPnt[i1] -= fProd0*fInv;
 334                 *pfLParam = -fPmE0*fInv;
 335             }
 336         }
 337     }
 338     else
 339     {
 340         // line intersects P[i1] = e[i1]
 341         rkPnt[i1] = extents[i1];
 342
 343         float fPpE0 = rkPnt[i0] + extents[i0];
 344         fDelta = fProd1 - rkDir[i1]*fPpE0;
 345         if(fDelta >= 0.0f)
 346         {
 347             fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
 348             rfSqrDistance += fDelta*fDelta*fInvLSqr;
 349             if(pfLParam)
 350             {
 351                 rkPnt[i0] = -extents[i0];
 352                 *pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
 353             }
 354         }
 355         else
 356         {
 357             if(pfLParam)
 358             {
 359                 fInv = 1.0f/rkDir[i1];
 360                 rkPnt[i0] -= fProd1*fInv;
 361                 *pfLParam = -fPmE1*fInv;
 362             }
 363         }
 364     }
 365
 366     if(rkPnt[i2] < -extents[i2])
 367     {
 368         fDelta = rkPnt[i2] + extents[i2];
 369         rfSqrDistance += fDelta*fDelta;
 370         rkPnt[i2] = -extents[i2];
 371     }
 372     else if ( rkPnt[i2] > extents[i2] )
 373     {
 374         fDelta = rkPnt[i2] - extents[i2];
 375         rfSqrDistance += fDelta*fDelta;
 376         rkPnt[i2] = extents[i2];
 377     }
 378 }
 379
 380 static void Case00(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
 381 {
 382     float fDelta;
 383
 384     if(pfLParam)
 385         *pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0];
 386
 387     rkPnt[i0] = extents[i0];
 388
 389     if(rkPnt[i1] < -extents[i1])
 390     {
 391         fDelta = rkPnt[i1] + extents[i1];
 392         rfSqrDistance += fDelta*fDelta;
 393         rkPnt[i1] = -extents[i1];
 394     }
 395     else if(rkPnt[i1] > extents[i1])
 396     {
 397         fDelta = rkPnt[i1] - extents[i1];
 398         rfSqrDistance += fDelta*fDelta;
 399         rkPnt[i1] = extents[i1];
 400     }
 401
 402     if(rkPnt[i2] < -extents[i2])
 403     {
 404         fDelta = rkPnt[i2] + extents[i2];
 405         rfSqrDistance += fDelta*fDelta;
 406         rkPnt[i1] = -extents[i2];
 407     }
 408     else if(rkPnt[i2] > extents[i2])
 409     {
 410         fDelta = rkPnt[i2] - extents[i2];
 411         rfSqrDistance += fDelta*fDelta;
 412         rkPnt[i2] = extents[i2];
 413     }
 414 }
 415
 416 static void Case000(Point& rkPnt, const Point& extents, float& rfSqrDistance)
 417 {
 418     float fDelta;
 419
 420     if(rkPnt.x < -extents.x)
 421     {
 422         fDelta = rkPnt.x + extents.x;
 423         rfSqrDistance += fDelta*fDelta;
 424         rkPnt.x = -extents.x;
 425     }
 426     else if(rkPnt.x > extents.x)
 427     {
 428         fDelta = rkPnt.x - extents.x;
 429         rfSqrDistance += fDelta*fDelta;
 430         rkPnt.x = extents.x;
 431     }
 432
 433     if(rkPnt.y < -extents.y)
 434     {
 435         fDelta = rkPnt.y + extents.y;
 436         rfSqrDistance += fDelta*fDelta;
 437         rkPnt.y = -extents.y;
 438     }
 439     else if(rkPnt.y > extents.y)
 440     {
 441         fDelta = rkPnt.y - extents.y;
 442         rfSqrDistance += fDelta*fDelta;
 443         rkPnt.y = extents.y;
 444     }
 445
 446     if(rkPnt.z < -extents.z)
 447     {
 448         fDelta = rkPnt.z + extents.z;
 449         rfSqrDistance += fDelta*fDelta;
 450         rkPnt.z = -extents.z;
 451     }
 452     else if(rkPnt.z > extents.z)
 453     {
 454         fDelta = rkPnt.z - extents.z;
 455         rfSqrDistance += fDelta*fDelta;
 456         rkPnt.z = extents.z;
 457     }
 458 }
 459
 460 static float SqrDistance(const Ray& rkLine, const Point& center, const Point& extents, float* pfLParam)
 461 {
 462     // compute coordinates of line in box coordinate system
 463     Point kDiff = rkLine.mOrig - center;
 464     Point kPnt = kDiff;
 465     Point kDir = rkLine.mDir;
 466
 467     // Apply reflections so that direction vector has nonnegative components.
 468     bool bReflect[3];
 469     for(int i=0;i<3;i++)
 470     {
 471         if(kDir[i]<0.0f)
 472         {
 473             kPnt[i] = -kPnt[i];
 474             kDir[i] = -kDir[i];
 475             bReflect[i] = true;
 476         }
 477         else
 478         {
 479             bReflect[i] = false;
 480         }
 481     }
 482
 483     float fSqrDistance = 0.0f;
 484
 485     if(kDir.x>0.0f)
 486     {
 487         if(kDir.y>0.0f)
 488         {
 489             if(kDir.z>0.0f)     CaseNoZeros(kPnt, kDir, extents, pfLParam, fSqrDistance);               // (+,+,+)
 490             else                        Case0(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance);    // (+,+,0)
 491         }
 492         else
 493         {
 494             if(kDir.z>0.0f)     Case0(0, 2, 1, kPnt, kDir, extents, pfLParam, fSqrDistance);    // (+,0,+)
 495             else                        Case00(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance);   // (+,0,0)
 496         }
 497     }
 498     else
 499     {
 500         if(kDir.y>0.0f)
 501         {
 502             if(kDir.z>0.0f)     Case0(1, 2, 0, kPnt, kDir, extents, pfLParam, fSqrDistance);    // (0,+,+)
 503             else                        Case00(1, 0, 2, kPnt, kDir, extents, pfLParam, fSqrDistance);   // (0,+,0)
 504         }
 505         else
 506         {
 507             if(kDir.z>0.0f)     Case00(2, 0, 1, kPnt, kDir, extents, pfLParam, fSqrDistance);   // (0,0,+)
 508             else
 509             {
 510                                                         Case000(kPnt, extents, fSqrDistance);                                                   // (0,0,0)
 511                                                         if(pfLParam)    *pfLParam = 0.0f;
 512             }
 513         }
 514     }
 515     return fSqrDistance;
 516 }
 517
 518 inline_ float OPC_SegmentOBBSqrDist(const Segment& segment, const Point& c0, const Point& e0)
 519 {
 520         float fLP;
 521         float fSqrDistance = SqrDistance(Ray(segment.GetOrigin(), segment.ComputeDirection()), c0, e0, &fLP);
 522         if(fLP>=0.0f)
 523         {
 524                 if(fLP<=1.0f)   return fSqrDistance;
 525                 else                    return OPC_PointAABBSqrDist(segment.mP1, c0, e0);
 526         }
 527         else                            return OPC_PointAABBSqrDist(segment.mP0, c0, e0);
 528 }
 529
 530 inline_ BOOL LSSCollider::LSSAABBOverlap(const Point& center, const Point& extents)
 531 {
 532         // Stats
 533         mNbVolumeBVTests++;
 534
 535         float s2 = OPC_SegmentOBBSqrDist(mSeg, center, extents);
 536         if(s2<mRadius2) return TRUE;
 537
 538         return FALSE;
 539 }