Extras/CDTestFramework/Opcode/OPC_SphereTriOverlap.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 // This is collision detection. If you do another distance test for collision *response*,
  19 // if might be useful to simply *skip* the test below completely, and report a collision.
  20 // - if sphere-triangle overlap, result is ok
  21 // - if they don't, we'll discard them during collision response with a similar test anyway
  22 // Overall this approach should run faster.
  23
  24 // Original code by David Eberly in Magic.
  25 BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
  26 {
  27         // Stats
  28         mNbVolumePrimTests++;
  29
  30         // Early exit if one of the vertices is inside the sphere
  31         Point kDiff = vert2 - mCenter;
  32         float fC = kDiff.SquareMagnitude();
  33         if(fC <= mRadius2)      return TRUE;
  34
  35         kDiff = vert1 - mCenter;
  36         fC = kDiff.SquareMagnitude();
  37         if(fC <= mRadius2)      return TRUE;
  38
  39         kDiff = vert0 - mCenter;
  40         fC = kDiff.SquareMagnitude();
  41         if(fC <= mRadius2)      return TRUE;
  42
  43         // Else do the full distance test
  44         Point TriEdge0  = vert1 - vert0;
  45         Point TriEdge1  = vert2 - vert0;
  46
  47 //Point kDiff   = vert0 - mCenter;
  48         float fA00      = TriEdge0.SquareMagnitude();
  49         float fA01      = TriEdge0 | TriEdge1;
  50         float fA11      = TriEdge1.SquareMagnitude();
  51         float fB0       = kDiff | TriEdge0;
  52         float fB1       = kDiff | TriEdge1;
  53 //float fC      = kDiff.SquareMagnitude();
  54         float fDet      = fabsf(fA00*fA11 - fA01*fA01);
  55         float u         = fA01*fB1-fA11*fB0;
  56         float v         = fA01*fB0-fA00*fB1;
  57         float SqrDist;
  58
  59         if(u + v <= fDet)
  60         {
  61                 if(u < 0.0f)
  62                 {
  63                         if(v < 0.0f)  // region 4
  64                         {
  65                                 if(fB0 < 0.0f)
  66                                 {
  67 //                                      v = 0.0f;
  68                                         if(-fB0>=fA00)                  { /*u = 1.0f;*/         SqrDist = fA00+2.0f*fB0+fC;     }
  69                                         else                                    { u = -fB0/fA00;        SqrDist = fB0*u+fC;                     }
  70                                 }
  71                                 else
  72                                 {
  73 //                                      u = 0.0f;
  74                                         if(fB1>=0.0f)                   { /*v = 0.0f;*/         SqrDist = fC;                           }
  75                                         else if(-fB1>=fA11)             { /*v = 1.0f;*/         SqrDist = fA11+2.0f*fB1+fC;     }
  76                                         else                                    { v = -fB1/fA11;        SqrDist = fB1*v+fC;                     }
  77                                 }
  78                         }
  79                         else  // region 3
  80                         {
  81 //                              u = 0.0f;
  82                                 if(fB1>=0.0f)                           { /*v = 0.0f;*/         SqrDist = fC;                           }
  83                                 else if(-fB1>=fA11)                     { /*v = 1.0f;*/         SqrDist = fA11+2.0f*fB1+fC;     }
  84                                 else                                            { v = -fB1/fA11;        SqrDist = fB1*v+fC;                     }
  85                         }
  86                 }
  87                 else if(v < 0.0f)  // region 5
  88                 {
  89 //                      v = 0.0f;
  90                         if(fB0>=0.0f)                                   { /*u = 0.0f;*/         SqrDist = fC;                           }
  91                         else if(-fB0>=fA00)                             { /*u = 1.0f;*/         SqrDist = fA00+2.0f*fB0+fC;     }
  92                         else                                                    { u = -fB0/fA00;        SqrDist = fB0*u+fC;                     }
  93                 }
  94                 else  // region 0
  95                 {
  96                         // minimum at interior point
  97                         if(fDet==0.0f)
  98                         {
  99 //                              u = 0.0f;
 100 //                              v = 0.0f;
 101                                 SqrDist = MAX_FLOAT;
 102                         }
 103                         else
 104                         {
 105                                 float fInvDet = 1.0f/fDet;
 106                                 u *= fInvDet;
 107                                 v *= fInvDet;
 108                                 SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
 109                         }
 110                 }
 111         }
 112         else
 113         {
 114                 float fTmp0, fTmp1, fNumer, fDenom;
 115
 116                 if(u < 0.0f)  // region 2
 117                 {
 118                         fTmp0 = fA01 + fB0;
 119                         fTmp1 = fA11 + fB1;
 120                         if(fTmp1 > fTmp0)
 121                         {
 122                                 fNumer = fTmp1 - fTmp0;
 123                                 fDenom = fA00-2.0f*fA01+fA11;
 124                                 if(fNumer >= fDenom)
 125                                 {
 126 //                                      u = 1.0f;
 127 //                                      v = 0.0f;
 128                                         SqrDist = fA00+2.0f*fB0+fC;
 129                                 }
 130                                 else
 131                                 {
 132                                         u = fNumer/fDenom;
 133                                         v = 1.0f - u;
 134                                         SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
 135                                 }
 136                         }
 137                         else
 138                         {
 139 //                              u = 0.0f;
 140                                 if(fTmp1 <= 0.0f)               { /*v = 1.0f;*/         SqrDist = fA11+2.0f*fB1+fC;     }
 141                                 else if(fB1 >= 0.0f)    { /*v = 0.0f;*/         SqrDist = fC;                           }
 142                                 else                                    { v = -fB1/fA11;        SqrDist = fB1*v+fC;                     }
 143                         }
 144                 }
 145                 else if(v < 0.0f)  // region 6
 146                 {
 147                         fTmp0 = fA01 + fB1;
 148                         fTmp1 = fA00 + fB0;
 149                         if(fTmp1 > fTmp0)
 150                         {
 151                                 fNumer = fTmp1 - fTmp0;
 152                                 fDenom = fA00-2.0f*fA01+fA11;
 153                                 if(fNumer >= fDenom)
 154                                 {
 155 //                                      v = 1.0f;
 156 //                                      u = 0.0f;
 157                                         SqrDist = fA11+2.0f*fB1+fC;
 158                                 }
 159                                 else
 160                                 {
 161                                         v = fNumer/fDenom;
 162                                         u = 1.0f - v;
 163                                         SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
 164                                 }
 165                         }
 166                         else
 167                         {
 168 //                              v = 0.0f;
 169                                 if(fTmp1 <= 0.0f)               { /*u = 1.0f;*/         SqrDist = fA00+2.0f*fB0+fC;     }
 170                                 else if(fB0 >= 0.0f)    { /*u = 0.0f;*/         SqrDist = fC;                           }
 171                                 else                                    { u = -fB0/fA00;        SqrDist = fB0*u+fC;                     }
 172                         }
 173                 }
 174                 else  // region 1
 175                 {
 176                         fNumer = fA11 + fB1 - fA01 - fB0;
 177                         if(fNumer <= 0.0f)
 178                         {
 179 //                              u = 0.0f;
 180 //                              v = 1.0f;
 181                                 SqrDist = fA11+2.0f*fB1+fC;
 182                         }
 183                         else
 184                         {
 185                                 fDenom = fA00-2.0f*fA01+fA11;
 186                                 if(fNumer >= fDenom)
 187                                 {
 188 //                                      u = 1.0f;
 189 //                                      v = 0.0f;
 190                                         SqrDist = fA00+2.0f*fB0+fC;
 191                                 }
 192                                 else
 193                                 {
 194                                         u = fNumer/fDenom;
 195                                         v = 1.0f - u;
 196                                         SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
 197                                 }
 198                         }
 199                 }
 200         }
 201
 202         return fabsf(SqrDist) < mRadius2;
 203 }