Main Page   Namespace List   Class Hierarchy   Compound List   File List   Compound Members   File Members  

collision.cpp

Go to the documentation of this file.
00001 // Collision detection routines and helper functions 
00002 // (by Alan Baylis 2001, Adapted from the work of Kasper Fauerby - aka Telemachos)
00003 
00004 #include <math.h>
00005 #include "locmath.h"
00006 #include "collision.h"
00007 #include "vector.h"
00008 
00009 
00010 void SetLength(VECTOR& v, float l)
00011 {
00012  float len = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);    
00013  v.x *= l/len;
00014  v.y *= l/len;
00015  v.z *= l/len;
00016 } 
00017 
00018 bool IsZeroVector(VECTOR& v)
00019 {
00020   if ((v.x == 0.0f) && (v.y == 0.0f) && (v.z == 0.0f))
00021     return TRUE;
00022   return FALSE;    
00023 }
00024 
00025 VECTOR Wedge(VECTOR v1, VECTOR v2)
00026 {
00027  VECTOR result;
00028  
00029  result.x = (v1.y * v2.z) - (v2.y * v1.z);
00030  result.y = (v1.z * v2.x) - (v2.z * v1.x);
00031  result.z = (v1.x * v2.y) - (v2.x * v1.y);     
00032  
00033  return (result);    
00034 }
00035 
00036 float IntersectRayPlane(VECTOR rOrigin, VECTOR rVector, VECTOR pOrigin, VECTOR pNormal) 
00037 {
00038   
00039   float d = - (DotProduct(pNormal,pOrigin));
00040  
00041   float numer = DotProduct(pNormal,rOrigin) + d;
00042   float denom = DotProduct(pNormal,rVector);
00043   
00044   if (denom == 0)  // normal is orthogonal to vector, cant intersect
00045    return (-1.0f);
00046    
00047   return -(numer / denom);    
00048 }
00049 
00050 float IntersectRaySphere(VECTOR rO, VECTOR rV, VECTOR sO, float sR) 
00051 {
00052    VECTOR TempVect;
00053    TempVect.x = sO.x - rO.x;
00054    TempVect.y = sO.y - rO.y;
00055    TempVect.z = sO.z - rO.z;
00056    VECTOR Q = TempVect;
00057    
00058    float c = MagnitudeVector(Q);
00059    float v = DotProduct(Q,rV);
00060    float d = sR*sR - (c*c - v*v);
00061 
00062    // If there was no intersection, return -1
00063    if (d < 0.0) return (-1.0f);
00064 
00065    // Return the distance to the [first] intersecting point
00066    return (v - sqrt(d));
00067 }
00068 
00069 bool CheckPointInTriangle(VECTOR point, VECTOR a, VECTOR b, VECTOR c) 
00070 {
00071   
00072   float total_angles = 0.0f;
00073        
00074   // make the 3 vectors
00075   VECTOR TempVect;
00076   TempVect.x = point.x - a.x;
00077   TempVect.y = point.y - a.y;
00078   TempVect.z = point.z - a.z;
00079   VECTOR v1 = TempVect;
00080   TempVect.x = point.x - b.x;
00081   TempVect.y = point.y - b.y;
00082   TempVect.z = point.z - b.z;
00083   VECTOR v2 = TempVect;
00084   TempVect.x = point.x - c.x;
00085   TempVect.y = point.y - c.y;
00086   TempVect.z = point.z - c.z;
00087   VECTOR v3 = TempVect;
00088   
00089   v1 = GetUnitVector(v1);
00090   v2 = GetUnitVector(v2);
00091   v3 = GetUnitVector(v3);
00092   float Dot1 = DotProduct(v1,v2);
00093   if (Dot1 < -1)
00094     Dot1 = -1;
00095   if (Dot1 > 1)
00096     Dot1 = 1;
00097   total_angles += acos(Dot1);   
00098   float Dot2 = DotProduct(v2,v3);
00099   if (Dot2 < -1)
00100     Dot2 = -1;
00101   if (Dot2 > 1)
00102     Dot2 = 1;
00103   total_angles += acos(Dot2);
00104   float Dot3 = DotProduct(v3,v1);
00105   if (Dot3 < -1)
00106     Dot3 = -1;
00107   if (Dot3 > 1)
00108     Dot3 = 1;
00109   total_angles += acos(Dot3); 
00110      
00111   if (fabs(total_angles-2*pi) <= 0.005)
00112    return (TRUE);
00113      
00114   return(FALSE);
00115 }
00116 
00117 VECTOR ClosestPointOnLine(VECTOR& a, VECTOR& b, VECTOR& p) 
00118 {
00119    // Determine t (the length of the vector from ‘a’ to ‘p’)
00120    VECTOR TempVect;
00121    TempVect.x = p.x - a.x;
00122    TempVect.y = p.y - a.y;
00123    TempVect.z = p.z - a.z;
00124    VECTOR c = TempVect;
00125    TempVect.x = b.x - a.x;
00126    TempVect.y = b.y - a.y;
00127    TempVect.z = b.z - a.z;
00128    VECTOR V = TempVect; 
00129       
00130    float d = MagnitudeVector(V);
00131       
00132    V = GetUnitVector(V);  
00133    double t = DotProduct(V,c);
00134 
00135    // Check to see if ‘t’ is beyond the extents of the line segment
00136    if (t < 0.0f) return (a);
00137    if (t > d) return (b);
00138  
00139    // Return the point between ‘a’ and ‘b’
00140    //set length of V to t. V is normalized so this is easy
00141    V.x = V.x * t;
00142    V.y = V.y * t;
00143    V.z = V.z * t;
00144 
00145    TempVect.x = a.x + V.x;           
00146    TempVect.y = a.y + V.y;           
00147    TempVect.z = a.z + V.z;           
00148    return (TempVect);    
00149 }
00150 
00151 VECTOR ClosestPointOnPolygon(VECTOR a, VECTOR c, VECTOR b, VECTOR p) 
00152 {
00153     
00154   VECTOR Rab = ClosestPointOnLine(a, b, p);
00155   VECTOR Rbc = ClosestPointOnLine(b, c, p);
00156   VECTOR Rca = ClosestPointOnLine(c, a, p);
00157   
00158   VECTOR TempVect;
00159   TempVect.x = p.x - Rab.x;  
00160   TempVect.y = p.y - Rab.y;  
00161   TempVect.z = p.z - Rab.z;  
00162   float dAB = MagnitudeVector(TempVect);
00163   TempVect.x = p.x - Rbc.x;  
00164   TempVect.y = p.y - Rbc.y;  
00165   TempVect.z = p.z - Rbc.z;  
00166   float dBC = MagnitudeVector(TempVect);
00167   TempVect.x = p.x - Rca.x;  
00168   TempVect.y = p.y - Rca.y;  
00169   TempVect.z = p.z - Rca.z;  
00170   float dCA = MagnitudeVector(TempVect);
00171   
00172   float min = dAB;
00173   VECTOR result = Rab;
00174   
00175   if (dBC < min) 
00176   {
00177     min = dBC;
00178     result = Rbc;
00179   }
00180  
00181   if (dCA < min)
00182     result = Rca;
00183   
00184     return (result);    
00185 }
00186 
00187 bool CheckPointInSphere(VECTOR point, VECTOR sO, float sR) 
00188 {
00189   VECTOR TempVect;
00190   TempVect.x = point.x - sO.x;
00191   TempVect.y = point.y - sO.y;
00192   TempVect.z = point.z - sO.z;
00193  
00194   float d = MagnitudeVector(TempVect);
00195  
00196   if(d <= sR) 
00197     return TRUE;
00198   return FALSE;    
00199 }
00200 
00201 VECTOR TangentPlaneNormalOfEllipsoid(VECTOR point, VECTOR eO, VECTOR eR)
00202 {
00203  VECTOR TempVect;
00204  TempVect.x = point.x - eO.x;
00205  TempVect.y = point.y - eO.y;
00206  TempVect.z = point.z - eO.z;
00207  VECTOR p = TempVect;
00208 
00209  float a2 = eR.x * eR.x;
00210  float b2 = eR.y * eR.y;
00211  float c2 = eR.z * eR.z;
00212 
00213  
00214  VECTOR res;
00215  res.x = p.x / a2;
00216  res.y = p.y / b2;
00217  res.z = p.z / c2;
00218 
00219  res = GetUnitVector(res);    
00220  return (res);    
00221 }
00222 
00223 int ClassifyPoint(VECTOR point, VECTOR pO, VECTOR pN)
00224 {
00225  VECTOR TempVect;
00226  TempVect.x = pO.x - point.x;
00227  TempVect.y = pO.y - point.y;
00228  TempVect.z = pO.z - point.z;
00229  VECTOR dir = TempVect;
00230  float d = DotProduct(dir, pN);
00231 
00232  if (d < -0.001f)
00233    return 1;
00234  else
00235    if (d > 0.001f)
00236      return -1;
00237    return 0;
00238 }
00239 
00240 void CheckForCollision(POLYGON* polygon, VECTOR* destination, CollisionPacket* colPackage)
00241 {
00242     colPackage->foundCollision = FALSE;
00243 
00244       // from package
00245       VECTOR eRadius = colPackage->eRadius;
00246       VECTOR source = colPackage->sourcePoint;
00247       VECTOR velocity = colPackage->velocity;
00248 
00249       // keep a copy of this as it's needed a few times
00250       VECTOR normalizedVelocity = GetUnitVector(velocity);
00251 
00252       // intersection data
00253       VECTOR sIPoint;    // sphere intersection point
00254       VECTOR pIPoint;    // plane intersection point
00255       VECTOR polyIPoint; // polygon intersection point
00256 
00257       float distToPlaneIntersection;
00258       float distToSphereIntersection;
00259 
00260      // loop through all the polygons of the cube
00261      for(int i = 0; i < 12; i++)
00262       {
00263 
00264           // Get plane normal
00265           VECTOR pOrigin;
00266           pOrigin.x = polygon[i].Vertex[0].x;        //Set plane origin
00267           pOrigin.y = polygon[i].Vertex[0].y;
00268           pOrigin.z = polygon[i].Vertex[0].z;
00269           VECTOR pNormal = polygon[i].GetNormal();
00270 
00271           // Normalize plane normal
00272           pNormal = GetUnitVector(pNormal);
00273 
00274           if (DotProduct(pNormal, normalizedVelocity) <= 1.0f)
00275           {
00276               // Calculate sphere intersection point
00277               VECTOR eIPoint;
00278               eIPoint.x = source.x - pNormal.x;  //Source point + inverted plane normal
00279               eIPoint.y = source.y - pNormal.y;
00280               eIPoint.z = source.z - pNormal.z;
00281 
00282               // shoot ray along the velocity vector
00283               distToPlaneIntersection = IntersectRayPlane(eIPoint, normalizedVelocity, pOrigin, pNormal);
00284 
00285               // calculate plane intersection point
00286               pIPoint.x = eIPoint.x + distToPlaneIntersection * normalizedVelocity.x;
00287               pIPoint.y = eIPoint.y + distToPlaneIntersection * normalizedVelocity.y;
00288               pIPoint.z = eIPoint.z + distToPlaneIntersection * normalizedVelocity.z;
00289 
00290               int pClass = ClassifyPoint(eIPoint, pOrigin, pNormal);
00291 
00292               // find the plane intersection point
00293               if (pClass == -1)
00294               {               // plane spans sphere
00295 
00296                 // find plane intersection point by shooting a ray from the
00297                 // sphere intersection point along the planes normal.
00298                 distToPlaneIntersection = IntersectRayPlane(eIPoint, pNormal, pOrigin, pNormal);
00299 
00300                 // calculate plane intersection point
00301                 pIPoint.x = eIPoint.x + distToPlaneIntersection * pNormal.x;
00302                 pIPoint.y = eIPoint.y + distToPlaneIntersection * pNormal.y;
00303                 pIPoint.z = eIPoint.z + distToPlaneIntersection * pNormal.z;
00304               }
00305 
00306               // find polygon intersection point. By default we assume its equal to the
00307               // plane intersection point. In that case we already know the distance to it.
00308               polyIPoint = pIPoint;
00309               distToSphereIntersection = distToPlaneIntersection;
00310 
00311               VECTOR a, b, c;
00312               a.x = polygon[i].Vertex[0].x;
00313               a.y = polygon[i].Vertex[0].y;
00314               a.z = polygon[i].Vertex[0].z;
00315               b.x = polygon[i].Vertex[1].x;
00316               b.y = polygon[i].Vertex[1].y;
00317               b.z = polygon[i].Vertex[1].z;
00318               c.x = polygon[i].Vertex[2].x;
00319               c.y = polygon[i].Vertex[2].y;
00320               c.z = polygon[i].Vertex[2].z;
00321 
00322               if(!CheckPointInTriangle(pIPoint, a, b, c))
00323               {
00324                 // if not in triangle
00325                     polyIPoint = ClosestPointOnPolygon(a, b, c, pIPoint);
00326 
00327                     VECTOR invertednormalizedVelocity;
00328                     invertednormalizedVelocity.x = -normalizedVelocity.x;
00329                     invertednormalizedVelocity.y = -normalizedVelocity.y;
00330                     invertednormalizedVelocity.z = -normalizedVelocity.z;
00331                     distToSphereIntersection = IntersectRaySphere(polyIPoint, invertednormalizedVelocity, source, 1.0f);
00332 
00333                     // we cannot know if the ray will actually hit the sphere so we need this check
00334                     if (distToSphereIntersection > 0)
00335                     {
00336                           // calculate true ellipsoid intersection point
00337                           eIPoint.x = polyIPoint.x + distToSphereIntersection * invertednormalizedVelocity.x;
00338                           eIPoint.y = polyIPoint.y + distToSphereIntersection * invertednormalizedVelocity.y;
00339                           eIPoint.z = polyIPoint.z + distToSphereIntersection * invertednormalizedVelocity.z;
00340                     }
00341               }
00342 
00343               // any chance of hit ?
00344               if ((distToSphereIntersection > 0) && (distToSphereIntersection <= MagnitudeVector(velocity)))
00345               {
00346                       // if first hit, or closest hit so far
00347                      if ((colPackage->foundCollision == FALSE) || (distToSphereIntersection < colPackage->nearestDistance))
00348                      {
00349                            colPackage->nearestDistance = distToSphereIntersection;
00350                            colPackage->nearestSphereIntersectionPoint = eIPoint;
00351                            colPackage->nearestPolygonIntersectionPoint = polyIPoint;
00352                            colPackage->foundCollision = TRUE;
00353                      }
00354               }
00355         }
00356     }
00357 
00358     if (!colPackage->foundCollision)
00359         return;
00360 
00361     //Collision response
00362 
00363     // OK, first task is to move close to where we hit something :
00364       VECTOR newSourcePoint;
00365 
00366       // only update if we are not already very close
00367       if (colPackage->nearestDistance >= epsilon)
00368       {
00369 
00370             VECTOR V = velocity;
00371               SetLength(V, colPackage->nearestDistance-epsilon);
00372             newSourcePoint.x = source.x + V.x;
00373             newSourcePoint.y = source.y + V.y;
00374             newSourcePoint.z = source.z + V.z;
00375       }
00376     else
00377             newSourcePoint = source;
00378 
00379     // Determine the sliding plane (we do this now, because we're about to
00380       // change sourcePoint)
00381       VECTOR slidePlaneOrigin = colPackage->nearestPolygonIntersectionPoint;
00382       VECTOR slidePlaneNormal;
00383       slidePlaneNormal.x = newSourcePoint.x - colPackage->nearestPolygonIntersectionPoint.x;
00384       slidePlaneNormal.y = newSourcePoint.y - colPackage->nearestPolygonIntersectionPoint.y;
00385       slidePlaneNormal.z = newSourcePoint.z - colPackage->nearestPolygonIntersectionPoint.z;
00386         // We now project the destination point onto the sliding plane
00387       float l = IntersectRayPlane(*destination, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);
00388      VECTOR newDestinationPoint;
00389       newDestinationPoint.x = destination->x + l * slidePlaneNormal.x;
00390       newDestinationPoint.y = destination->y + l * slidePlaneNormal.y;
00391       newDestinationPoint.z = destination->z + l * slidePlaneNormal.z;
00392 
00393     VECTOR newVelocityVector;
00394       newVelocityVector.x = newDestinationPoint.x - colPackage->nearestPolygonIntersectionPoint.x;
00395       newVelocityVector.y = newDestinationPoint.y - colPackage->nearestPolygonIntersectionPoint.y;
00396      newVelocityVector.z = newDestinationPoint.z - colPackage->nearestPolygonIntersectionPoint.z;
00397 
00398     destination->x = newSourcePoint.x + newVelocityVector.x;
00399       destination->y = newSourcePoint.y + newVelocityVector.y;
00400       destination->z = newSourcePoint.z + newVelocityVector.z;
00401     colPackage->sourcePoint = newSourcePoint;
00402     colPackage->velocity = newVelocityVector;
00403     colPackage->foundCollision = 0;
00404         CheckForCollision(polygon, destination, colPackage);
00405 }

Generated on Fri Dec 23 05:21:20 2005 for Skybox by doxygen1.2.15