00001
00002
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)
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
00065 if (d < 0.0) return (-1.0f);
00066
00067
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
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
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
00138 if (t < 0.0f) return (a);
00139 if (t > d) return (b);
00140
00141
00142
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 void CheckForCollision(POLYGON* polygon, VECTOR* destination, CollisionPacket* colPackage)
00243 {
00244 colPackage->foundCollision = FALSE;
00245
00246
00247 VECTOR eRadius = colPackage->eRadius;
00248 VECTOR source = colPackage->sourcePoint;
00249 VECTOR velocity = colPackage->velocity;
00250
00251
00252 VECTOR normalizedVelocity = GetUnitVector(velocity);
00253
00254
00255 VECTOR sIPoint;
00256 VECTOR pIPoint;
00257 VECTOR polyIPoint;
00258
00259 float distToPlaneIntersection;
00260 float distToSphereIntersection;
00261
00262
00263 for(int i = 0; i < numPolygons; i++)
00264 {
00265
00266
00267 VECTOR pOrigin;
00268 pOrigin.x = polygon[i].Vertex[0].x;
00269 pOrigin.y = polygon[i].Vertex[0].y;
00270 pOrigin.z = polygon[i].Vertex[0].z;
00271 VECTOR pNormal = polygon[i].GetNormal();
00272
00273
00274 pNormal = GetUnitVector(pNormal);
00275
00276 if (DotProduct(pNormal, normalizedVelocity) <= 1.0f)
00277 {
00278
00279 VECTOR eIPoint;
00280 eIPoint.x = source.x - pNormal.x;
00281 eIPoint.y = source.y - pNormal.y;
00282 eIPoint.z = source.z - pNormal.z;
00283
00284
00285 distToPlaneIntersection = IntersectRayPlane(eIPoint, normalizedVelocity, pOrigin, pNormal);
00286
00287
00288 pIPoint.x = eIPoint.x + distToPlaneIntersection * normalizedVelocity.x;
00289 pIPoint.y = eIPoint.y + distToPlaneIntersection * normalizedVelocity.y;
00290 pIPoint.z = eIPoint.z + distToPlaneIntersection * normalizedVelocity.z;
00291
00292 int pClass = ClassifyPoint(eIPoint, pOrigin, pNormal);
00293
00294
00295 if (pClass == -1)
00296 {
00297
00298
00299
00300 distToPlaneIntersection = IntersectRayPlane(eIPoint, pNormal, pOrigin, pNormal);
00301
00302
00303 pIPoint.x = eIPoint.x + distToPlaneIntersection * pNormal.x;
00304 pIPoint.y = eIPoint.y + distToPlaneIntersection * pNormal.y;
00305 pIPoint.z = eIPoint.z + distToPlaneIntersection * pNormal.z;
00306 }
00307
00308
00309
00310 polyIPoint = pIPoint;
00311 distToSphereIntersection = distToPlaneIntersection;
00312
00313 VECTOR a, b, c;
00314 a.x = polygon[i].Vertex[0].x;
00315 a.y = polygon[i].Vertex[0].y;
00316 a.z = polygon[i].Vertex[0].z;
00317 b.x = polygon[i].Vertex[1].x;
00318 b.y = polygon[i].Vertex[1].y;
00319 b.z = polygon[i].Vertex[1].z;
00320 c.x = polygon[i].Vertex[2].x;
00321 c.y = polygon[i].Vertex[2].y;
00322 c.z = polygon[i].Vertex[2].z;
00323
00324 if(!CheckPointInTriangle(pIPoint, a, b, c))
00325 {
00326
00327 polyIPoint = ClosestPointOnPolygon(a, b, c, pIPoint);
00328
00329 VECTOR invertednormalizedVelocity;
00330 invertednormalizedVelocity.x = -normalizedVelocity.x;
00331 invertednormalizedVelocity.y = -normalizedVelocity.y;
00332 invertednormalizedVelocity.z = -normalizedVelocity.z;
00333 distToSphereIntersection = IntersectRaySphere(polyIPoint, invertednormalizedVelocity, source, 1.0f);
00334
00335
00336 if (distToSphereIntersection > 0)
00337 {
00338
00339 eIPoint.x = polyIPoint.x + distToSphereIntersection * invertednormalizedVelocity.x;
00340 eIPoint.y = polyIPoint.y + distToSphereIntersection * invertednormalizedVelocity.y;
00341 eIPoint.z = polyIPoint.z + distToSphereIntersection * invertednormalizedVelocity.z;
00342 }
00343 }
00344
00345
00346 if ((distToSphereIntersection > 0) && (distToSphereIntersection <= MagnitudeVector(velocity)))
00347 {
00348
00349 if ((colPackage->foundCollision == FALSE) || (distToSphereIntersection < colPackage->nearestDistance))
00350 {
00351 colPackage->nearestDistance = distToSphereIntersection;
00352 colPackage->nearestSphereIntersectionPoint = eIPoint;
00353 colPackage->nearestPolygonIntersectionPoint = polyIPoint;
00354 colPackage->foundCollision = TRUE;
00355 }
00356 }
00357 }
00358 }
00359
00360 if (!colPackage->foundCollision)
00361 return;
00362
00363
00364
00365
00366 VECTOR newSourcePoint;
00367
00368
00369 if (colPackage->nearestDistance >= epsilon)
00370 {
00371
00372 VECTOR V = velocity;
00373 SetLength(V, colPackage->nearestDistance-epsilon);
00374 newSourcePoint.x = source.x + V.x;
00375 newSourcePoint.y = source.y + V.y;
00376 newSourcePoint.z = source.z + V.z;
00377 }
00378 else
00379 newSourcePoint = source;
00380
00381
00382
00383 VECTOR slidePlaneOrigin = colPackage->nearestPolygonIntersectionPoint;
00384 VECTOR slidePlaneNormal;
00385 slidePlaneNormal.x = newSourcePoint.x - colPackage->nearestPolygonIntersectionPoint.x;
00386 slidePlaneNormal.y = newSourcePoint.y - colPackage->nearestPolygonIntersectionPoint.y;
00387 slidePlaneNormal.z = newSourcePoint.z - colPackage->nearestPolygonIntersectionPoint.z;
00388
00389 float l = IntersectRayPlane(*destination, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);
00390 VECTOR newDestinationPoint;
00391 newDestinationPoint.x = destination->x + l * slidePlaneNormal.x;
00392 newDestinationPoint.y = destination->y + l * slidePlaneNormal.y;
00393 newDestinationPoint.z = destination->z + l * slidePlaneNormal.z;
00394
00395 VECTOR newVelocityVector;
00396 newVelocityVector.x = newDestinationPoint.x - colPackage->nearestPolygonIntersectionPoint.x;
00397 newVelocityVector.y = newDestinationPoint.y - colPackage->nearestPolygonIntersectionPoint.y;
00398 newVelocityVector.z = newDestinationPoint.z - colPackage->nearestPolygonIntersectionPoint.z;
00399
00400 destination->x = newSourcePoint.x + newVelocityVector.x;
00401 destination->y = newSourcePoint.y + newVelocityVector.y;
00402 destination->z = newSourcePoint.z + newVelocityVector.z;
00403 colPackage->sourcePoint = newSourcePoint;
00404 colPackage->velocity = newVelocityVector;
00405 colPackage->foundCollision = 0;
00406 CheckForCollision(polygon, destination, colPackage);
00407 }