00001
00002
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)
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
00063 if (d < 0.0) return (-1.0f);
00064
00065
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
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
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
00136 if (t < 0.0f) return (a);
00137 if (t > d) return (b);
00138
00139
00140
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
00245 VECTOR eRadius = colPackage->eRadius;
00246 VECTOR source = colPackage->sourcePoint;
00247 VECTOR velocity = colPackage->velocity;
00248
00249
00250 VECTOR normalizedVelocity = GetUnitVector(velocity);
00251
00252
00253 VECTOR sIPoint;
00254 VECTOR pIPoint;
00255 VECTOR polyIPoint;
00256
00257 float distToPlaneIntersection;
00258 float distToSphereIntersection;
00259
00260
00261 for(int i = 0; i < 12; i++)
00262 {
00263
00264
00265 VECTOR pOrigin;
00266 pOrigin.x = polygon[i].Vertex[0].x;
00267 pOrigin.y = polygon[i].Vertex[0].y;
00268 pOrigin.z = polygon[i].Vertex[0].z;
00269 VECTOR pNormal = polygon[i].GetNormal();
00270
00271
00272 pNormal = GetUnitVector(pNormal);
00273
00274 if (DotProduct(pNormal, normalizedVelocity) <= 1.0f)
00275 {
00276
00277 VECTOR eIPoint;
00278 eIPoint.x = source.x - pNormal.x;
00279 eIPoint.y = source.y - pNormal.y;
00280 eIPoint.z = source.z - pNormal.z;
00281
00282
00283 distToPlaneIntersection = IntersectRayPlane(eIPoint, normalizedVelocity, pOrigin, pNormal);
00284
00285
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
00293 if (pClass == -1)
00294 {
00295
00296
00297
00298 distToPlaneIntersection = IntersectRayPlane(eIPoint, pNormal, pOrigin, pNormal);
00299
00300
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
00307
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
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
00334 if (distToSphereIntersection > 0)
00335 {
00336
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
00344 if ((distToSphereIntersection > 0) && (distToSphereIntersection <= MagnitudeVector(velocity)))
00345 {
00346
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
00362
00363
00364 VECTOR newSourcePoint;
00365
00366
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
00380
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
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 }