www.pudn.com > RCApp-src.zip > Collision.cpp
/*
RedEye Project (http://members.ozemail.com.au/~ndmcevoy/)
Copyright (C) 2003 Nick McEvoy
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Library General Public
License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Library General Public License for more details.
You should have received a copy of the GNU Library General Public
License along with this library; if not, write to the Free
Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
-----------------------------------------------------------------
Commented for use with Doxygen (http://www.doxygen.org)
-----------------------------------------------------------------
*/
/*! \file Collision.cpp
* \brief Collision detection methods.
*
* This file contains methods for collision detection.
*
* Thanks to the following for code:
* - DigiBen (DigiBen@GameTutorials.com)
* - Steve Baker (http://www.sjbaker.org)
* - Tomas Akenine-Möller (Fast 3D Triangle-Box Overlap Testing - tribox.cpp)
* http://www.acm.org/jgt/papers/AkenineMoller01/tribox.html
* http://www.ce.chalmers.se/staff/tomasm/pubs/tribox.pdf
*/
// ODE includes
#include "ode/common.h"
#include "ode/contact.h"
#include "ode/objects.h"
#include "ode/geom.h"
// RedEye includes
#include "Collision.h"
#include "Engine.h"
#include "MaterialManager.h"
#include "List.h"
// These constants are used as return values from reClassifySphere(). Depending
// on where the sphere lies in accordance with the plane being checked, these
// will allow us to label if the sphere is in front, behind or intersecting the plane.
#define BEHIND 0 // This is returned if the sphere is completely behind the plane
#define INTERSECTS 1 // This is returned if the sphere intersects the plane
#define FRONT 2 // This is returned if the sphere is completely in front of the plane
// tri-box overlap test - tribox.cpp (by Tomas Akenine-Möller)
extern int triBoxOverlap(float boxcenter[3],float boxhalfsize[3],float triverts[3][3]);
// ODE internal function
extern int dCollideBP (const dxGeom* o1, const dxGeom* o2, int flags, dContactGeom *contact, int skip);
void
reGetBodyTransform(ssgTransform* dest, dBodyID src)
{
sgMat4 Xform;
const dReal* pPos = dBodyGetPosition(src);
const dReal* pRot = dBodyGetRotation(src);
sgSetVec4(Xform[0], pRot[0], pRot[4], pRot[8], 0);
sgSetVec4(Xform[1], pRot[1], pRot[5], pRot[9], 0);
sgSetVec4(Xform[2], pRot[2], pRot[6], pRot[10], 0);
sgSetVec4(Xform[3], pPos[0], pPos[1], pPos[2], 1);
dest->setTransform(Xform);
}
void
reGetBodyPosition(sgVec3 dest, dBodyID src)
{
const dReal* pPos = dBodyGetPosition(src);
sgSetVec3(dest, pPos[0], pPos[1], pPos[2]);
}
void
reGetBodyVelocity(sgVec3 dest, dBodyID src)
{
const dReal* pLinVel = dBodyGetLinearVel(src);
sgSetVec3(dest, pLinVel[0], pLinVel[1], pLinVel[2]);
}
bool
reCreateContacts(reList& ContactList, dBodyID Body)
{
bool bContact = false;
reList OptimisedList;
// Go thru contact list and filter out duplicate contacts
LISTPOSITION pos = ContactList.GetHeadPosition();
while (pos)
{
dContact* pContact = ContactList.GetNext(pos);
// Check if contact is already in optimised list
bool bContactExists = false;
LISTPOSITION pos_opt = OptimisedList.GetHeadPosition();
while (pos_opt)
{
dContact* pContactOpt = OptimisedList.GetNext(pos_opt);
if (sgCompareVec3(pContact->geom.pos, pContactOpt->geom.pos, 0.001f) &&
sgCompareVec3(pContact->geom.normal, pContactOpt->geom.normal, 0.001f))
{
bContactExists = true;
break;
}
}
// If not then add it to optimised list
if (!bContactExists)
{
dContact* pContactOpt = new dContact;
*pContactOpt = *pContact;
OptimisedList.AddTail(pContactOpt);
}
}
// Go thru optimised list and genarate contacts
pos = OptimisedList.GetHeadPosition();
while (pos)
{
dContact* pContact = OptimisedList.GetNext(pos);
/*printf("pos <%.1f:%.1f:%.1f> nrm <%.1f:%.1f:%.1f>\n",
pContact->geom.pos[0],
pContact->geom.pos[1],
pContact->geom.pos[2],
pContact->geom.normal[0],
pContact->geom.normal[1],
pContact->geom.normal[2]);*/
bContact = true;
dJointID c = dJointCreateContact(
reGetODEWorldId(),
reGetODEJointGroupId(),
pContact);
dJointAttach(c, Body, NULL);
}
return bContact;
}
bool
reTriangleSphereContact(int iNumHits,
ssgHit* pHitList,
dBodyID SphereBody,
dGeomID SphereGeom)
{
reList ContactList;
// Get sphere info
sgVec3 vCenter;
reGetBodyPosition(vCenter, SphereBody);
float fRadius = dGeomSphereGetRadius(SphereGeom);
// Check for hits
if (iNumHits > 0 && pHitList)
{
//printf("** hits <%d>\n", iNumHits);
for (int i = 0; i < iNumHits; i++)
{
ssgHit* h = &pHitList[i];
sgVec3 vTriangle[3];
sgVec3 vTriangleNormal;
sgVec3 vPosition;
float fDepth;
// Get 'hit' triangle in leaf
short v1, v2, v3;
h->leaf->getTriangle(h->triangle, &v1, &v2, &v3);
sgCopyVec3(vTriangle[0], h->leaf->getVertex(v1));
sgCopyVec3(vTriangle[1], h->leaf->getVertex(v2));
sgCopyVec3(vTriangle[2], h->leaf->getVertex(v3));
sgSetVec3(vTriangleNormal, h->plane[0], h->plane[1], h->plane[2]);
// Apply current matrix to 'hit' triangle
sgXformPnt3(vTriangle[0], vTriangle[0], h->matrix);
sgXformPnt3(vTriangle[1], vTriangle[1], h->matrix);
sgXformPnt3(vTriangle[2], vTriangle[2], h->matrix);
// Triangle-sphere collision test
if (reTriangleSphereIntersectionPoint(
vCenter, fRadius, vTriangle, vTriangleNormal, vPosition, fDepth))
{
// Get contact surface properties depending on material hit
ssgState* pState = h->leaf->getState();
if (pState->isA(ssgTypeSimpleState()))
{
dSurfaceParameters Surface;
if (reGetMaterialManager()->GetSurfaceParams(
pState->getExternalPropertyIndex(),
Surface))
{
dContact* pContact = new dContact;
pContact->surface = Surface;
pContact->geom.pos[0] = vPosition[0];
pContact->geom.pos[1] = vPosition[1];
pContact->geom.pos[2] = vPosition[2];
pContact->geom.normal[0] = vTriangleNormal[0];
pContact->geom.normal[1] = vTriangleNormal[1];
pContact->geom.normal[2] = vTriangleNormal[2];
pContact->geom.depth = fDepth;
pContact->geom.g1 = SphereGeom;
pContact->geom.g2 = NULL;
ContactList.AddTail(pContact);
}
}
}
}
}
// If no hits then do a height over terrain check
// to stop objects falling thru terrain
if (ContactList.GetCount() == 0)
{
sgVec3 vTriangleNormal;
ssgHit* h;
float fHOT = reGetHeightAndNormal(reGetWorld(), vCenter, vTriangleNormal, &h);
float fDepth = fHOT-vCenter[SG_Z];
if (fDepth > fRadius)
{
//printf("** depth <%.2f>\n", fDepth);
// todo: improve reGetHeightAndNormal() to return position
// same as reTriangleSphereIntersectionPoint() above.
sgVec3 vOffset;
sgVec3 vPosition;
sgScaleVec3(vOffset, vTriangleNormal, -fDepth);
sgSubVec3(vPosition, vCenter, vOffset);
ssgState* pState = h->leaf->getState();
if (pState->isA(ssgTypeSimpleState()))
{
dSurfaceParameters Surface;
if (reGetMaterialManager()->GetSurfaceParams(
pState->getExternalPropertyIndex(),
Surface))
{
dContact* pContact = new dContact;
pContact->surface = Surface;
pContact->geom.pos[0] = vPosition[0];
pContact->geom.pos[1] = vPosition[1];
pContact->geom.pos[2] = vPosition[2];
pContact->geom.normal[0] = vTriangleNormal[0];
pContact->geom.normal[1] = vTriangleNormal[1];
pContact->geom.normal[2] = vTriangleNormal[2];
pContact->geom.depth = fDepth;
pContact->geom.g1 = SphereGeom;
pContact->geom.g2 = NULL;
ContactList.AddTail(pContact);
}
}
}
}
return reCreateContacts(ContactList, SphereBody);
}
bool
reTriangleBoxContact(int iNumHits,
ssgHit* pHitList,
dBodyID BoxBody,
dGeomID BoxGeom)
{
reList ContactList;
// Check for hits
if (iNumHits > 0 && pHitList)
{
//printf("** hits <%d>\n", iNumHits);
for (int i = 0; i < iNumHits; i++)
{
ssgHit* h = &pHitList[i];
sgVec3 vTriangle[3];
sgVec3 vTriangleNormal;
// Get 'hit' triangle in leaf
short v1, v2, v3;
h->leaf->getTriangle(h->triangle, &v1, &v2, &v3);
sgCopyVec3(vTriangle[0], h->leaf->getVertex(v1));
sgCopyVec3(vTriangle[1], h->leaf->getVertex(v2));
sgCopyVec3(vTriangle[2], h->leaf->getVertex(v3));
sgSetVec3(vTriangleNormal, h->plane[0], h->plane[1], h->plane[2]);
// Apply current matrix to 'hit' triangle
sgXformPnt3(vTriangle[0], vTriangle[0], h->matrix);
sgXformPnt3(vTriangle[1], vTriangle[1], h->matrix);
sgXformPnt3(vTriangle[2], vTriangle[2], h->matrix);
// Get axis aligned bounding box (AABB) info
dVector3 BoxSides;
sgVec3 BoxExtents;
sgVec3 BoxCenter = {0,0,0};
dGeomBoxGetLengths(BoxGeom, BoxSides);
sgSetVec3(BoxExtents, BoxSides[0]*0.5f, BoxSides[1]*0.5f, BoxSides[2]*0.5f);
// To test against oriented bounding box (OBB) need to
// translate triangle verticies by inverse box transform
sgMat4 Xform;
const dReal* pPos = dBodyGetPosition(BoxBody);
const dReal* pRot = dBodyGetRotation(BoxBody);
sgSetVec4(Xform[0], pRot[0], pRot[4], pRot[8], 0);
sgSetVec4(Xform[1], pRot[1], pRot[5], pRot[9], 0);
sgSetVec4(Xform[2], pRot[2], pRot[6], pRot[10], 0);
sgSetVec4(Xform[3], pPos[0], pPos[1], pPos[2], 1);
sgTransposeNegateMat4(Xform);
sgXformPnt3(vTriangle[0], Xform);
sgXformPnt3(vTriangle[1], Xform);
sgXformPnt3(vTriangle[2], Xform);
// Triangle-box collision test
if (triBoxOverlap(BoxCenter, BoxExtents, vTriangle))
{
// Use ODE box-plane collide to generate contact geom
dContactGeom BoxContacts[3];
dGeomID TriPlaneGeom = dCreatePlane(reGetODESpaceId(),
h->plane[0], h->plane[1], h->plane[2], -h->plane[3]);
int n = dCollideBP(BoxGeom, TriPlaneGeom, 3, BoxContacts, sizeof(dContactGeom));
for (int j = 0; j < n; j++)
{
// Get contact surface properties depending on material hit
ssgState* pState = h->leaf->getState();
if (pState->isA(ssgTypeSimpleState()))
{
dSurfaceParameters Surface;
if (reGetMaterialManager()->GetSurfaceParams(
pState->getExternalPropertyIndex(),
Surface))
{
dContact* pContact = new dContact;
pContact->surface = Surface;
pContact->geom = BoxContacts[j];
pContact->geom.g1 = BoxGeom;
pContact->geom.g2 = NULL;
ContactList.AddTail(pContact);
}
}
}
dGeomDestroy(TriPlaneGeom);
}
}
}
// If no hits then do a height over terrain check
// to stop objects falling thru terrain
if (ContactList.GetCount() == 0)
{
// Quick hack: just approximate box as a sphere !? :)
sgVec3 vCenter;
dVector3 BoxSides;
reGetBodyPosition(vCenter, BoxBody);
dGeomBoxGetLengths(BoxGeom, BoxSides);
float fRadius = BoxSides[0];
if (fRadius < BoxSides[1])
fRadius = BoxSides[1];
if (fRadius < BoxSides[2])
fRadius = BoxSides[2];
fRadius *= 0.5f;
ssgHit* h;
sgVec3 vTriangleNormal;
float fHOT = reGetHeightAndNormal(reGetWorld(), vCenter, vTriangleNormal, &h);
float fDepth = fHOT-vCenter[SG_Z];
if (fDepth > fRadius)
{
//printf("** depth <%.2f>\n", fDepth);
// todo: improve reGetHeightAndNormal() to return position
// same as reTriangleSphereIntersectionPoint() above.
sgVec3 vOffset;
sgVec3 vPosition;
sgScaleVec3(vOffset, vTriangleNormal, -fDepth);
sgSubVec3(vPosition, vCenter, vOffset);
//sgCopyVec3(vPosition, vCenter);//JUST TESTING CONTACT POINT AT BOX CENTER!?
ssgState* pState = h->leaf->getState();
if (pState->isA(ssgTypeSimpleState()))
{
dSurfaceParameters Surface;
if (reGetMaterialManager()->GetSurfaceParams(
pState->getExternalPropertyIndex(),
Surface))
{
dContact* pContact = new dContact;
pContact->surface = Surface;
pContact->geom.pos[0] = vPosition[0];
pContact->geom.pos[1] = vPosition[1];
pContact->geom.pos[2] = vPosition[2];
pContact->geom.normal[0] = vTriangleNormal[0];
pContact->geom.normal[1] = vTriangleNormal[1];
pContact->geom.normal[2] = vTriangleNormal[2];
pContact->geom.depth = fDepth;
pContact->geom.g1 = BoxGeom;
pContact->geom.g2 = NULL;
ContactList.AddTail(pContact);
}
}
}
}
return reCreateContacts(ContactList, BoxBody);
}
float
reGetHeightAndNormal(ssgRoot* pScene,
const sgVec3 vMyPosition,
sgVec3 vNormal,
ssgHit** pHit)
{
//
// Code adapted from Steve Baker (TuxKart & TuxAQFH)
//
ssgHit* pResults;
int iNumHits;
float fHOT; // H.O.T == Height Of Terrain
sgVec3 HOTvec;
sgMat4 InvMat;
// Look for the nearest polygon *beneath* vMyPosition
sgMakeIdentMat4(InvMat);
InvMat[3][0] = -vMyPosition[0];
InvMat[3][1] = -vMyPosition[1];
InvMat[3][2] = 0.0;
sgSetVec3(HOTvec, 0.0f, 0.0f, HIGHEST_HEAVEN);
iNumHits = ssgHOT(pScene, HOTvec, InvMat, &pResults);
fHOT = DEEPEST_HELL;
for (int i = 0; i < iNumHits; i++)
{
ssgHit* h = &pResults[i];
float fHGT = - h->plane[3] / h->plane[2];
if (fHGT >= fHOT)
{
fHOT = fHGT;
*pHit = h;
if (vNormal != NULL)
sgCopyVec3(vNormal, h->plane);
}
}
return fHOT;
}
void
reNormal(sgVec3 vNormal, const sgVec3 vTriangle[3])
{
sgVec3 vVector1;
sgVec3 vVector2;
// Get 2 vectors from the polygon (2 sides), remember the order (counter-clockwise)!
sgSubVec3(vVector1, vTriangle[2], vTriangle[0]);
sgSubVec3(vVector2, vTriangle[1], vTriangle[0]);
// Take the cross product of our 2 vectors to get a perpendicular vector
sgVectorProductVec3(vNormal, vVector1, vVector2);
// Normalize the normal (make it a length of one)
sgNormaliseVec3(vNormal);
}
float
rePlaneDistance(const sgVec3 vNormal,
const sgVec3 vPoint)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// Plane equation Ax + By + Cz + D = 0
// Where ABC = plane normal, xyz = point on plane & D = plane distance from origin
float fDistance = -sgScalarProductVec3(vNormal, vPoint);
return fDistance;
}
void
reDefinePlane(const sgVec3 vTriangle[3],
const sgVec3 vNormal,
sgVec4 vPlane)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// Either calculate normal (if null) or copy normal
sgVec3 vN;
if (!vNormal)
reNormal(vN, vTriangle);
else
sgCopyVec3(vN, vNormal);
// Plane equation Ax + By + Cz + D = 0
// Where ABC = plane normal, xyz = point on plane & D = plane distance from origin
vPlane[0] = vN[0];
vPlane[1] = vN[1];
vPlane[2] = vN[2];
vPlane[3] = rePlaneDistance(vN, vTriangle[0]);
}
bool
reLineIntersectsPlane(const sgVec3 vLine[2],
const sgVec4 vPlane)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// First 3 elements of vPlane is the normal the last element is D
sgVec3 vNormal;
sgCopyVec3(vNormal, vPlane);
float fD = vPlane[3];
// Get line points distance from plane using: Ax + By + Cz + D = (distance from plane)
float fDistance1 = (sgScalarProductVec3(vNormal, vLine[0]) + fD);
float fDistance2 = (sgScalarProductVec3(vNormal, vLine[1]) + fD);
// Check to see if both point's distances are both negative or both positive
if(fDistance1 * fDistance2 >= 0)
return false; // distances have same sign thus each point is same side of plane
else
return true; // distances have different signs thus line intersects plane
}
bool
reLinePlaneIntersectionPoint(const sgVec3 vLine[2],
const sgVec4 vPlane,
sgVec3 vPoint)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// Check if line intersects the plane
if (!reLineIntersectsPlane(vLine, vPlane))
return false;
// First 3 elements of vPlane is the normal the last element is D
sgVec3 vNormal;
sgCopyVec3(vNormal, vPlane);
float fD = vPlane[3];
// Get the vector (direction) of our line, then normalize it
sgVec3 vLineDir;
sgSubVec3(vLineDir, vLine[1], vLine[0]);
sgNormaliseVec3(vLineDir);
// Use plane equation (distance = Ax + By + Cz + D) to find distance from one of
// our lines points to the plane. Choose an arbitrary point, negate the distance
// because we want to eventually go BACKWARDS from our point to the plane.
// By doing this is will basically bring us back to the plane to find our intersection point.
double dNumerator = -(sgScalarProductVec3(vNormal, vLine[0]) + fD);
// Take dot product between our line vector and plane normal, to give the cosine of the angle
// between the 2 (since they are both normalized - length 1). We will then divide our Numerator
// by this value to find the offset towards the plane from our arbitrary point.
double dDenominator = sgScalarProductVec3(vNormal, vLineDir);
// Check for divide by zero (if so normal was perpendicular to line and there are INFINATE
// points because line is on the plane, thus just return an arbitrary point on the line).
if(dDenominator == 0.0)
{
sgCopyVec3(vPoint, vLine[0]);
return true;
}
// Divide (distance from the point to plane) by (the dot product) to get the distance
// that we need to move from our arbitrary point. Times this distance by our line's
// vector (direction) thus we move from our arbitrary point on the line BACK to the
// plane along the lines vector.
double dDist = dNumerator / dDenominator;
vPoint[0] = (float)(vLine[0][0] + (vLineDir[0] * dDist));
vPoint[1] = (float)(vLine[0][1] + (vLineDir[1] * dDist));
vPoint[2] = (float)(vLine[0][2] + (vLineDir[2] * dDist));
return true;
}
bool
reLineTriangleIntersectionPoint(const sgVec3 vLine[2],
const sgVec3 vTriangle[3],
const sgVec3 vNormal,
sgVec3 vPoint)
//CVector3 vPoly[], CVector3 vLine[], int verticeCount)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// Either calculate normal (if null) or copy normal
sgVec3 vN;
if (!vNormal)
reNormal(vN, vTriangle);
else
sgCopyVec3(vN, vNormal);
// Define the triangle's plane
sgVec4 vPlane;
reDefinePlane(vTriangle, vNormal, vPlane);
// Check if line intersects plane
if (!reLinePlaneIntersectionPoint(vLine, vPlane, vPoint))
return false;
// Check if intersection point is inside the perimeter of the triangle
if (reInsidePolygon(vPoint, vTriangle, 3))
return true;
else
return false;
}
void
reClosestPointOnLine(const sgVec3 vLine[2],
const sgVec3 vPoint,
sgVec3 vLinePoint)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// Create vector from line end point the point
sgVec3 vVector1;
sgSubVec3(vVector1, vPoint, vLine[0]);
// Create a normalized direction vector from line end points
sgVec3 vVector2;
sgSubVec3(vVector2, vLine[1], vLine[0]);
sgNormaliseVec3(vVector2);
// Use the distance formula to find the distance of the line segment (or magnitude)
float fD = sgDistanceVec3(vLine[0], vLine[1]);
// Using the dot product, we project the vVector1 onto the vector vVector2.
// This essentially gives us the distance from our projected vector from line p0.
float fT = sgScalarProductVec3(vVector2, vVector1);
// If our projected distance from line p0, "t", is less than or equal to 0, it must
// be closest to the end line p0.
if (fT <= 0)
{
sgCopyVec3(vLinePoint, vLine[0]);
return;
}
// If our projected distance from line p0, "t", is greater than or equal to the magnitude
// or distance of the line segment, it must be closest to the end line p1.
if (fT >= fD)
{
sgCopyVec3(vLinePoint, vLine[1]);
return;
}
// Here we create a vector that is of length t and in the direction of vVector2
sgVec3 vVector3;
sgScaleVec3(vVector3, vVector2, fT);
// Add vVector3 to the original line end p0 to get closest point on the line segment
sgAddVec3(vLinePoint, vLine[0], vVector3);
return;
}
// Returns true if a point is inside the ranges of a polygon
bool
reInsidePolygon(const sgVec3 vPoint,
const sgVec3 vPoly[],
int iVertexCount)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
double dAngle = 0.0;
sgVec3 vA, vB;
// Go in a circle to each vertex and get the angle between
for (int i = 0; i < iVertexCount; i++)
{
// Subtract the point from the current vertex
sgSubVec3(vA, vPoly[i], vPoint);
// Subtract the point from the next vertex
sgSubVec3(vB, vPoly[(i + 1) % iVertexCount], vPoint);
// Find the angle between the 2 vectors and add them all up as we go along
// Also make sure that the angle is not -1.#IND0000000 number, which means indefinate
double dA = sgAngleBetweenVec3(vA, vB);
if (_isnan(dA) == 0)
dAngle += dA;
}
// Point is inside polygon if angle is greater than 2 PI, (360 degrees)
if(dAngle >= 355.0) // Not quite 360 ... cover up the error in floating point
return true;
else
return false;
}
bool
reEdgeSphereCollision(const sgVec3 vCenter,
const sgVec3 vPoly[],
int iVertexCount,
float fRadius)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
sgVec3 vPoint;
// Go through all of the vertices in the polygon
for(int i = 0; i < iVertexCount; i++)
{
// Get the closest point on the current edge to the center of the sphere
sgVec3 vLine[2];
sgCopyVec3(vLine[0], vPoly[i]);
sgCopyVec3(vLine[1], vPoly[(i + 1) % iVertexCount]);
reClosestPointOnLine(vLine, vCenter, vPoint);
// Now calculate the distance between the closest point and the center
float fDistance = sgDistanceVec3(vPoint, vCenter);
// If the distance is less than the radius, there must be a collision so return true
if(fDistance < fRadius)
return true;
}
// The was no intersection of the sphere and the edges of the polygon
return false;
}
bool
reEdgePlanesCollision(const sgVec3 vCenter,
const sgVec3 vTriangle[],
const sgVec3 vNormal,
float fRadius)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// Create some variables to hold each edge planes classification with the sphere
int edge1 = 0, edge2 = 0, edge3 = 0;
float distance = 0;
//////////////// Calculate Edge 1 ////////////////
// Get a vector from the edge of the triangle
sgVec3 vVector;
sgSubVec3(vVector, vTriangle[1], vTriangle[0]);
// Get the vector perpendicular from the normal of the triangle and this edge vector
sgVec3 vEdgeNormal;
sgVectorProductVec3(vEdgeNormal, vVector, vNormal);
// Before we can call this new vector a normal, we need to normalize it
sgNormaliseVec3(vEdgeNormal);
// If we pass in the sphere's center, the new planes normal, a point on the plane,
// the radius of the sphere and an empty float to hold the distance.
edge1 = reClassifySphere(vCenter, vEdgeNormal, vTriangle[0], fRadius, distance);
//////////////// Calculate Edge 2 ////////////////
// Get a vector from the second edge of the triangle
sgSubVec3(vVector, vTriangle[2], vTriangle[1]);
// Get the vector perpendicular from the normal of the triangle and this edge vector
sgVectorProductVec3(vEdgeNormal, vVector, vNormal);
// Before we can call this new vector a normal, we need to normalize it
sgNormaliseVec3(vEdgeNormal);
// Find the classification of the sphere and this second plane
edge2 = reClassifySphere(vCenter, vEdgeNormal, vTriangle[1], fRadius, distance);
//////////////// Calculate Edge 3 ////////////////
// Get a vector from the edge of the triangle
sgSubVec3(vVector, vTriangle[0], vTriangle[2]);
// Get the vector perpendicular from the normal of the triangle and this edge vector
sgVectorProductVec3(vEdgeNormal, vVector, vNormal);
// Before we can call this new vector a normal, we need to normalize it
sgNormaliseVec3(vEdgeNormal);
// Find the classification of the sphere and this third plane
edge3 = reClassifySphere(vCenter, vEdgeNormal, vTriangle[0], fRadius, distance);
// We only collide if NONE of the classifications are BEHIND
if(edge1 != BEHIND && edge2 != BEHIND && edge3 != BEHIND)
return true;
// One of the classifications must have been BEHIND, because we didn't collide
return false;
}
int
reClassifySphere(const sgVec3 vCenter,
const sgVec3 vNormal,
const sgVec3 vPoint,
float fRadius,
float& fDistance)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// First we need to find the distance our triangle plane is from the origin.
// We need this for the distance formula below.
float d = rePlaneDistance(vNormal, vPoint);
// Here we use the famous distance formula to find the distance the center point
// of the sphere is from the triangle's plane.
// Remember that the formula is Ax + By + Cz + d = 0 with ABC = Normal, xyz = Point
fDistance = (sgScalarProductVec3(vNormal, vCenter) + d);
// If the absolute value of the distance we just found is less than the radius,
// the sphere intersected the plane.
if(sgAbs(fDistance) < fRadius)
return INTERSECTS;
// Else, if the distance is greater than or equal to the radius, the sphere is
// completely in FRONT of the plane.
else if(fDistance >= fRadius)
return FRONT;
// If the sphere isn't intersecting or in FRONT of the plane, it must be BEHIND
return BEHIND;
}
bool
reTriangleSphereIntersectionPoint(const sgVec3 vSphereCenter,
float fSphereRadius,
const sgVec3 vTriangle[3],
const sgVec3 vNormal,
sgVec3 vPoint,
float& fDepth)
{
//
// Code adapted from DigiBen (DigiBen@GameTutorials.com)
//
// 1) STEP ONE - Finding the sphere's classification
// This will store the distance our sphere is from the plane
float fDistance = 0.0f;
// Either calculate normal (if null) or copy normal
sgVec3 vN;
if (!vNormal)
reNormal(vN, vTriangle);
else
sgCopyVec3(vN, vNormal);
// This is where we determine if the sphere is in FRONT, BEHIND, or INTERSECTS the plane
// of the triangle. We pass is our sphere center, the polygon's normal, a point on
// the plane (vertex), the sphere's radius and an empty float to fill the distance with.
int classification = reClassifySphere(vSphereCenter, vN, vTriangle[0], fSphereRadius, fDistance);
// Calculate the depth of penetration of sphere into polygon
fDepth = fSphereRadius - fDistance;
// If the sphere intersects the polygon's plane, then we need to check further,
// otherwise the sphere did NOT intersect the triangle. Pretty fast so far huh?
if(classification == INTERSECTS)
{
// 2) STEP TWO - Finding the psuedo intersection point on the plane
// Now we want to project the sphere's center onto the triangle's plane,
// in the direction of the normal. This is done by multiplying the "normal"
// by the "distance" the sphere center is from the plane. We got the distance
// from the reClassifySphere() function call up above. 2 return values were given
// through the "distance" variable being passed in as a reference. If projecting
// is confusing to you, just think of it as this: "I am starting at the center
// of the sphere and I am going to just run into the plane. I will move in the
// direction that is reverse from the normal. When do I know when to stop? Well,
// I just go in that direction until my distance from the center is the same as
// the distance the center of the sphere is from the plane." By doing this
// we get an offset to subtract from the center of the sphere.
sgVec3 vOffset;
sgScaleVec3(vOffset, vN, fDistance);
// Once we have the offset to the plane, we just subtract it from the center
// of the sphere. "vPoint" now a point that lies on the plane of the triangle.
// Whether it is inside the triangle's perimeter is another story. Usually it
// is though, unless we get near the edges of the triangle.
sgSubVec3(vPoint, vSphereCenter, vOffset);
// 3) STEP THREE - Check if the intersection point is inside the triangles perimeter
// If the intersection point is inside the perimeter of the triangle, it returns true.
// We pass in the intersection point, the list of vertices and vertex count of the poly.
if(reInsidePolygon(vPoint, vTriangle, 3))
return true; // We collided!
else
{
// 4) STEP FOUR - Check the sphere to see if it's inside the edge planes
// If we get here, we didn't find an intersection point in the perimeter.
// There is still one more chance to redeem our sphere that it can hit the mark.
// If any part of the sphere intersects the edges of the polygon, we collided.
// This is only checked if the sphere's center point is outside the edges of the
// polygon. We pass in the center of the sphere, the list of verts, the polygon
// vertex count and the sphere's radius. If this returns true we have a collision.
if(reEdgeSphereCollision(vSphereCenter, vTriangle, 3, fSphereRadius))
{
return true; // We collided! "And you doubted me..." - Sphere
}
}
}
// If we get here, there is obviously no collision happening up in this crib
return false;
}