OPC_SphereTriOverlap.h
Go to the documentation of this file.
00001 
00002 // This is collision detection. If you do another distance test for collision *response*,
00003 // if might be useful to simply *skip* the test below completely, and report a collision.
00004 // - if sphere-triangle overlap, result is ok
00005 // - if they don't, we'll discard them during collision response with a similar test anyway
00006 // Overall this approach should run faster.
00007 
00008 // Original code by David Eberly in Magic.
00009 BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
00010 {
00011         // Stats
00012         mNbVolumePrimTests++;
00013 
00014         // Early exit if one of the vertices is inside the sphere
00015         Point kDiff = vert2 - mCenter;
00016         float fC = kDiff.SquareMagnitude();
00017         if(fC <= mRadius2)      return TRUE;
00018 
00019         kDiff = vert1 - mCenter;
00020         fC = kDiff.SquareMagnitude();
00021         if(fC <= mRadius2)      return TRUE;
00022 
00023         kDiff = vert0 - mCenter;
00024         fC = kDiff.SquareMagnitude();
00025         if(fC <= mRadius2)      return TRUE;
00026 
00027         // Else do the full distance test
00028         Point TriEdge0  = vert1 - vert0;
00029         Point TriEdge1  = vert2 - vert0;
00030 
00031 //Point kDiff   = vert0 - mCenter;
00032         float fA00      = TriEdge0.SquareMagnitude();
00033         float fA01      = TriEdge0 | TriEdge1;
00034         float fA11      = TriEdge1.SquareMagnitude();
00035         float fB0       = kDiff | TriEdge0;
00036         float fB1       = kDiff | TriEdge1;
00037 //float fC      = kDiff.SquareMagnitude();
00038         float fDet      = fabsf(fA00*fA11 - fA01*fA01);
00039         float u         = fA01*fB1-fA11*fB0;
00040         float v         = fA01*fB0-fA00*fB1;
00041         float SqrDist;
00042 
00043         if(u + v <= fDet)
00044         {
00045                 if(u < 0.0f)
00046                 {
00047                         if(v < 0.0f)  // region 4
00048                         {
00049                                 if(fB0 < 0.0f)
00050                                 {
00051 //                                      v = 0.0f;
00052                                         if(-fB0>=fA00)                  { /*u = 1.0f;*/         SqrDist = fA00+2.0f*fB0+fC;     }
00053                                         else                                    { u = -fB0/fA00;        SqrDist = fB0*u+fC;                     }
00054                                 }
00055                                 else
00056                                 {
00057 //                                      u = 0.0f;
00058                                         if(fB1>=0.0f)                   { /*v = 0.0f;*/         SqrDist = fC;                           }
00059                                         else if(-fB1>=fA11)             { /*v = 1.0f;*/         SqrDist = fA11+2.0f*fB1+fC;     }
00060                                         else                                    { v = -fB1/fA11;        SqrDist = fB1*v+fC;                     }
00061                                 }
00062                         }
00063                         else  // region 3
00064                         {
00065 //                              u = 0.0f;
00066                                 if(fB1>=0.0f)                           { /*v = 0.0f;*/         SqrDist = fC;                           }
00067                                 else if(-fB1>=fA11)                     { /*v = 1.0f;*/         SqrDist = fA11+2.0f*fB1+fC;     }
00068                                 else                                            { v = -fB1/fA11;        SqrDist = fB1*v+fC;                     }
00069                         }
00070                 }
00071                 else if(v < 0.0f)  // region 5
00072                 {
00073 //                      v = 0.0f;
00074                         if(fB0>=0.0f)                                   { /*u = 0.0f;*/         SqrDist = fC;                           }
00075                         else if(-fB0>=fA00)                             { /*u = 1.0f;*/         SqrDist = fA00+2.0f*fB0+fC;     }
00076                         else                                                    { u = -fB0/fA00;        SqrDist = fB0*u+fC;                     }
00077                 }
00078                 else  // region 0
00079                 {
00080                         // minimum at interior point
00081                         if(fDet==0.0f)
00082                         {
00083 //                              u = 0.0f;
00084 //                              v = 0.0f;
00085                                 SqrDist = MAX_FLOAT;
00086                         }
00087                         else
00088                         {
00089                                 float fInvDet = 1.0f/fDet;
00090                                 u *= fInvDet;
00091                                 v *= fInvDet;
00092                                 SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
00093                         }
00094                 }
00095         }
00096         else
00097         {
00098                 float fTmp0, fTmp1, fNumer, fDenom;
00099 
00100                 if(u < 0.0f)  // region 2
00101                 {
00102                         fTmp0 = fA01 + fB0;
00103                         fTmp1 = fA11 + fB1;
00104                         if(fTmp1 > fTmp0)
00105                         {
00106                                 fNumer = fTmp1 - fTmp0;
00107                                 fDenom = fA00-2.0f*fA01+fA11;
00108                                 if(fNumer >= fDenom)
00109                                 {
00110 //                                      u = 1.0f;
00111 //                                      v = 0.0f;
00112                                         SqrDist = fA00+2.0f*fB0+fC;
00113                                 }
00114                                 else
00115                                 {
00116                                         u = fNumer/fDenom;
00117                                         v = 1.0f - u;
00118                                         SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
00119                                 }
00120                         }
00121                         else
00122                         {
00123 //                              u = 0.0f;
00124                                 if(fTmp1 <= 0.0f)               { /*v = 1.0f;*/         SqrDist = fA11+2.0f*fB1+fC;     }
00125                                 else if(fB1 >= 0.0f)    { /*v = 0.0f;*/         SqrDist = fC;                           }
00126                                 else                                    { v = -fB1/fA11;        SqrDist = fB1*v+fC;                     }
00127                         }
00128                 }
00129                 else if(v < 0.0f)  // region 6
00130                 {
00131                         fTmp0 = fA01 + fB1;
00132                         fTmp1 = fA00 + fB0;
00133                         if(fTmp1 > fTmp0)
00134                         {
00135                                 fNumer = fTmp1 - fTmp0;
00136                                 fDenom = fA00-2.0f*fA01+fA11;
00137                                 if(fNumer >= fDenom)
00138                                 {
00139 //                                      v = 1.0f;
00140 //                                      u = 0.0f;
00141                                         SqrDist = fA11+2.0f*fB1+fC;
00142                                 }
00143                                 else
00144                                 {
00145                                         v = fNumer/fDenom;
00146                                         u = 1.0f - v;
00147                                         SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
00148                                 }
00149                         }
00150                         else
00151                         {
00152 //                              v = 0.0f;
00153                                 if(fTmp1 <= 0.0f)               { /*u = 1.0f;*/         SqrDist = fA00+2.0f*fB0+fC;     }
00154                                 else if(fB0 >= 0.0f)    { /*u = 0.0f;*/         SqrDist = fC;                           }
00155                                 else                                    { u = -fB0/fA00;        SqrDist = fB0*u+fC;                     }
00156                         }
00157                 }
00158                 else  // region 1
00159                 {
00160                         fNumer = fA11 + fB1 - fA01 - fB0;
00161                         if(fNumer <= 0.0f)
00162                         {
00163 //                              u = 0.0f;
00164 //                              v = 1.0f;
00165                                 SqrDist = fA11+2.0f*fB1+fC;
00166                         }
00167                         else
00168                         {
00169                                 fDenom = fA00-2.0f*fA01+fA11;
00170                                 if(fNumer >= fDenom)
00171                                 {
00172 //                                      u = 1.0f;
00173 //                                      v = 0.0f;
00174                                         SqrDist = fA00+2.0f*fB0+fC;
00175                                 }
00176                                 else
00177                                 {
00178                                         u = fNumer/fDenom;
00179                                         v = 1.0f - u;
00180                                         SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
00181                                 }
00182                         }
00183                 }
00184         }
00185 
00186         return fabsf(SqrDist) < mRadius2;
00187 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18