OPCODE/OPC_LSSAABBOverlap.h

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