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

Generated on Fri Dec 23 05:15:46 2005 for Constructive Solid Geometry by doxygen1.2.15