OPCODE/OPC_SphereTriOverlap.h

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