Code:
#include "CollisionDetectionPrimitives.h"
/*
For every in_plane, get the distance from the Pos to the sphere
if(planedist1 <= -(radius_pfast_tol))
return FALSE;
else if(fabs(planedist1) <= radius_pfast_tol)
{
++num_intersections;
if(planedist1 >= 0)
++pos_intersections;
}
*/
BOOL PointInBSPFace(BSP *pBSP,int faceindex,Vector3 & Pos,float Radius)
{
tBSPFace *pFace = &pBSP->mpFaces[faceindex];
Vector3 dir = pFace->center - Pos;
float startd_sq = dir.BasicLength();
float rad_sq = Radius * Radius;
if(startd_sq <= rad_sq)
return TRUE; //intersects with the center of the bspface
dir.Normalize(Faster_Sqrtf(startd_sq));
Vector3 testpoint = Pos + (dir * Radius);
Plane3 *pPlane = NULL;
float d(0);
for(int plane = 0; plane < pFace->numInPlanes; plane++)
{
pPlane = &pFace->mpInplanes[plane];
d = DotProduct(&pPlane->mNormal,&testpoint) - pPlane->mDist;
if(d < -FastBSPFacetolerance)
return FALSE;
}
return TRUE;
}
...
#include "CollisionDetectionPrimitives.h"
extern std::ofstream trace;
#define CCWPITDEBUG(x) //x
const float globaltolerance = 2.0f;
const float fast_tolerance = globaltolerance * 1.5f;
//Fixme: investigate to see how vector length plays into uncertainty here
//Normal x DiffVector = InwardVector
//InwardVector Dot Point >= 0 or >= -.0001
BOOL CCWPointInTri(float P1X,float P1Y,float P1Z,
float P2X,float P2Y,float P2Z,
float P3X,float P3Y,float P3Z,
float NormalX,float NormalY,float NormalZ,
float PointX,float PointY,float PointZ,float Radius)
{
Vector3 Point1(P1X,P1Y,P1Z); Vector3 Point2(P2X,P2Y,P2Z); Vector3 Point3(P3X,P3Y,P3Z);
Vector3 Normal(NormalX,NormalY,NormalZ);
Vector3 Point(PointX,PointY,PointZ);
Vector3 LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex
Vector3 DiffVector = Point2 - Point1;
Vector3 InwardVector(0,0,0);
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Normalize();
int num_intersections(0);
double planedist1,planedist2,planedist3;
double radius_ptol = Radius + globaltolerance;
planedist1 = DotProduct(&InwardVector,&LocalPoint);
if(planedist1 <= -(radius_ptol))
{
return FALSE;
}
else if(fabs(planedist1) <= radius_ptol)
{
++num_intersections;
}
LocalPoint = Point - Point2;
DiffVector = Point3 - Point2;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Normalize();
planedist2 = DotProduct(&InwardVector,&LocalPoint);
if(planedist2 <= -(radius_ptol))
{
return FALSE;
}
else if(fabs(planedist2) <= radius_ptol)
{
++num_intersections;
}
LocalPoint = Point - Point3;
DiffVector = Point1 - Point3;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Normalize();
planedist3 = DotProduct(&InwardVector,&LocalPoint);
if(planedist3 <= -(radius_ptol))
{
return FALSE;
}
else if(fabs(planedist3) <= radius_ptol)
{
++num_intersections;
}
if(num_intersections > 1)
{
if(planedist1 >= 0 && planedist2 >= 0 && planedist3 >= 0)
{
return TRUE;
}
else
{
return FALSE;
}
}
return TRUE;
}
BOOL CWPointInTri(float P1X,float P1Y,float P1Z,
float P2X,float P2Y,float P2Z,
float P3X,float P3Y,float P3Z,
float NormalX,float NormalY,float NormalZ,
float PointX,float PointY,float PointZ,float Radius)
{
Vector3 Point1(P1X,P1Y,P1Z); Vector3 Point2(P2X,P2Y,P2Z); Vector3 Point3(P3X,P3Y,P3Z);
Vector3 Normal(-NormalX,-NormalY,-NormalZ);
Vector3 Point(PointX,PointY,PointZ);
Vector3 LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex
Vector3 DiffVector = Point2 - Point1;
Vector3 InwardVector(0,0,0);
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Normalize();
int num_intersections(0);
double planedist1,planedist2,planedist3;
double radius_ptol = Radius + globaltolerance;
planedist1 = DotProduct(&InwardVector,&LocalPoint);
if(planedist1 <= -(radius_ptol))
{
return FALSE;
}
else if(fabs(planedist1) <= radius_ptol)
{
++num_intersections;
}
LocalPoint = Point - Point2;
DiffVector = Point3 - Point2;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Normalize();
planedist2 = DotProduct(&InwardVector,&LocalPoint);
if(planedist2 <= -(radius_ptol))
{
return FALSE;
}
else if(fabs(planedist2) <= radius_ptol)
{
++num_intersections;
}
LocalPoint = Point - Point3;
DiffVector = Point1 - Point3;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Normalize();
planedist3 = DotProduct(&InwardVector,&LocalPoint);
if(planedist3 <= -(radius_ptol))
{
return FALSE;
}
else if(fabs(planedist3) <= radius_ptol)
{
++num_intersections;
}
if(num_intersections > 1)
{
if(planedist1 >= 0 && planedist2 >= 0 && planedist3 >= 0)
{
return TRUE;
}
else
{
return FALSE;
}
}
return TRUE;
}
/*
The faster point in tri functions just use faster_sqrt
Note that if the sphere intersects 2 or more of the inward planes,
then the exact center of the sphere must lay in front of all planes
Otherwise you get the 'ghost' collisions where the sphere is definitely *not*
inside of the triangle
*/
BOOL FastCWPointInTri(float P1X,float P1Y,float P1Z,
float P2X,float P2Y,float P2Z,
float P3X,float P3Y,float P3Z,
float NormalX,float NormalY,float NormalZ,
float PointX,float PointY,float PointZ,float Radius)
{
Vector3 Point1(P1X,P1Y,P1Z); Vector3 Point2(P2X,P2Y,P2Z); Vector3 Point3(P3X,P3Y,P3Z);
Vector3 Normal(-NormalX,-NormalY,-NormalZ);
Vector3 Point(PointX,PointY,PointZ);
Vector3 LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex
Vector3 DiffVector = Point2 - Point1;
Vector3 InwardVector(0,0,0);
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Fast_Normalize();
int num_intersections(0);
int pos_intersections(0); //how many intersections occur with it on the positive side
double planedist1,planedist2,planedist3;
double radius_pfast_tol = Radius + fast_tolerance;
planedist1 = DotProduct(&InwardVector,&LocalPoint);
if(planedist1 <= -(radius_pfast_tol))
return FALSE;
else if(fabs(planedist1) <= radius_pfast_tol)
{
++num_intersections;
if(planedist1 >= 0)
++pos_intersections;
}
LocalPoint = Point - Point2;
DiffVector = Point3 - Point2;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Fast_Normalize();
planedist2 = DotProduct(&InwardVector,&LocalPoint);
if(planedist2 <= -(radius_pfast_tol))
return FALSE;
else if(fabs(planedist2) <= radius_pfast_tol)
{
++num_intersections;
if(planedist2 >= 0)
++pos_intersections;
}
LocalPoint = Point - Point3;
DiffVector = Point1 - Point3;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Fast_Normalize();
planedist3 = DotProduct(&InwardVector,&LocalPoint);
if(planedist3 <= -(radius_pfast_tol))
return FALSE;
else if(fabs(planedist3) <= radius_pfast_tol)
{
++num_intersections;
if(planedist3 >= 0)
++pos_intersections;
}
if(num_intersections > 1)
{
/*
if(planedist1 >= 0 && planedist2 >= 0 && planedist3 >= 0)
{
if(num_infront != 3 && num_messages < 1000)
{
trace << "weirdness in pointintri function" << "\n";
}
return TRUE;
}
*/
if(pos_intersections == 3)
{
return TRUE;
}
else
{
return FALSE;
}
}
return TRUE;
}
BOOL FastCCWPointInTri(float P1X,float P1Y,float P1Z,
float P2X,float P2Y,float P2Z,
float P3X,float P3Y,float P3Z,
float NormalX,float NormalY,float NormalZ,
float PointX,float PointY,float PointZ,float Radius)
{
Vector3 Point1(P1X,P1Y,P1Z); Vector3 Point2(P2X,P2Y,P2Z); Vector3 Point3(P3X,P3Y,P3Z);
Vector3 Normal(NormalX,NormalY,NormalZ);
Vector3 Point(PointX,PointY,PointZ);
Vector3 LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex
Vector3 DiffVector = Point2 - Point1;
Vector3 InwardVector(0,0,0);
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Fast_Normalize();
int num_intersections(0);
int pos_intersections(0);
double planedist1,planedist2,planedist3;
double radius_pfast_tol = Radius + fast_tolerance;
planedist1 = DotProduct(&InwardVector,&LocalPoint);
if(planedist1 <= -(radius_pfast_tol))
{
return FALSE;
}
else if(fabs(planedist1) <= radius_pfast_tol)
{
++num_intersections;
if(planedist1 >= 0)
++pos_intersections;
}
LocalPoint = Point - Point2;
DiffVector = Point3 - Point2;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Fast_Normalize();
planedist2 = DotProduct(&InwardVector,&LocalPoint);
if(planedist2 <= -(radius_pfast_tol))
{
return FALSE;
}
else if(fabs(planedist2) <= radius_pfast_tol)
{
++num_intersections;
if(planedist2 >= 0)
++pos_intersections;
}
LocalPoint = Point - Point3;
DiffVector = Point1 - Point3;
InwardVector = CrossProduct(&Normal,&DiffVector);
InwardVector.Fast_Normalize();
planedist3 = DotProduct(&InwardVector,&LocalPoint);
if(planedist3 <= -(radius_pfast_tol))
{
return FALSE;
}
else if(fabs(planedist3) <= radius_pfast_tol)
{
++num_intersections;
if(planedist3 >= 0)
++pos_intersections;
}
if(num_intersections > 1)
{
// if(planedist1 >= 0 && planedist2 >= 0 && planedist3 >= 0)
if(pos_intersections >= 3)
{
return TRUE;
}
else
{
return FALSE;
}
}
return TRUE;
}
/*
//Same thing, different version
BOOL CWPointInTri(float P1X,float P1Y,float P1Z,
float P2X,float P2Y,float P2Z,
float P3X,float P3Y,float P3Z,
float NormalX,float NormalY,float NormalZ,
float PointX, float PointY, float PointZ)
{
Vector3 P1(P1X,P1Y,P1Z);
Vector3 P2(P2X,P2Y,P2Z);
Vector3 P3(P3X,P3Y,P3Z);
Vector3 Point(PointX,PointY,PointZ);
Vector3 Normal(NormalX,NormalY,NormalZ);
Vector3 Edge1 = P1 - P2;
Vector3 Edge2 = P2 - P3;
Vector3 Edge3 = P3 - P1;
Vector3 PminusP2 = Point - P2;
Vector3 PminusP3 = Point - P3;
Vector3 PminusP1 = Point - P1;
if(DotProduct(&Normal,&CrossProduct(&Edge1,&PminusP2)) >= globaltolerance)
if(DotProduct(&Normal,&CrossProduct(&Edge2,&PminusP3)) >= globaltolerance)
if(DotProduct(&Normal,&CrossProduct(&Edge3,&PminusP1)) >= globaltolerance)
return true;
return false;
}
*/
...
#include "CollisionDetectionPrimitives.h"
#define SWEPTSPHEREDEBUG(x) //x
/*
SINGLE PRECISION IMPLEMENTATION
*/
/*
//NEWTON ITERATION SQRT x87 fpu
Number DD ?
TestResult DW?
Two DD 2.0
MaxRelErr DD 0.5E-6
Sqrt:
fld1
REPEAT01:
fld ST
fld Number
fdiv ST,ST(1)
fadd ST,ST(1)
fDiv Two
fst ST(2)
fsub
fabs
fld MaxRelErr
fmul ST,ST(2)
fcompp
fstsw TestResult
fwait
mov ax, TestResult
sahf
jna REPEAT01
ret
.....
_asm
{
FLD basic; //Put basic length into st(0)
FSQRT; //Put SQUARE ROOT of basic into st(0)
FSTP basic; //Put st(0) into basic and pop that motha
}
*/
//spheres may also be penetrating
BOOL AreSpheresTouching(Vector3 &PointA,Vector3 &PointB,double SepDist)
{
BOOL retval;
Vector3 Separation = PointA - PointB;
float SepLenSq = Separation.BasicLength();
if(SepLenSq > (SepDist * SepDist))
{
return FALSE;
}
else
{
return TRUE;
}
}
/*
If (AreSpheresTouching) and AreSpheresGettingCloser, then they've got a collision
This type of collision won't really be picked up by
Normal defined as PointA - PointB
FIXME: does normal need to be normalized? Don't think so!
*/
BOOL AreSpheresGettingCloser(Vector3 &PointA,Vector3 &PointB,
Vector3 &VelA,Vector3 &VelB,
Vector3 &Normal)
{
BOOL retval;
Vector3 RelVel = VelA - VelB; //relative velocity
float NormalRelVel = DotProduct(&RelVel,&Normal); //relative velocity along normal
if(NormalRelVel > .00001f)
{
return FALSE;
}
else
{
return TRUE;
}
}
BOOL DoSpheresCollide(Vector3 &PStart,Vector3 &PEnd,
Vector3 &QStart,Vector3 &QEnd,
float PRadius,float QRadius,float Epsilon)
{
BOOL retval = false;
Vector3 A = PStart - QStart;
Vector3 B = (PEnd - PStart) - (QEnd - QStart); //relative velocity
double ASq = A.BasicLength();
double AdotB = DotProduct(&A,&B);
double AdotB_sq = AdotB * AdotB;
double BSq = B.BasicLength();
double iBSq(0);
if(BSq > .000001f)
{
iBSq = 1/ BSq;
}
else //if b_squared is zero, then they cannot collide (both stationary or moving in same direction)
{
SWEPTSPHEREDEBUG( trace << "DoSpheresCollide(): BSquared <relative velocity) is zero, setting to 1.0f" << "\n"; )
return FALSE;
}
double closest_dist_squared = (ASq - (AdotB_sq * iBSq));
double desired_dist_squared = PRadius + QRadius + Epsilon;
desired_dist_squared *= desired_dist_squared;
if(closest_dist_squared > desired_dist_squared)
{
return FALSE;
}
else
{
return TRUE;
}
return retval;
}
double ClosestSphereSepDistance(Vector3 & PStart,Vector3 &PEnd, Vector3 &QStart,Vector3 &QEnd)
{
Vector3 A = PStart - QStart;
Vector3 B = (PEnd - PStart) - (QEnd - QStart); //relative velocity
double ASq = A.BasicLength();
double AdotB = DotProduct(&A,&B);
double AdotB_sq = AdotB * AdotB;
double BSq = B.BasicLength();
double iBSq(0);// = BSq > .0000001f ? (1/BSq) : 1.0f; //Inverse of |B| squared
if(BSq > .000001f)
{
iBSq = 1/ BSq;
}
else //if b_squared is zero, then they cannot collide (both stationary or moving in same direction)
{
SWEPTSPHEREDEBUG( trace << "BSquared is zero, setting to 1.0f" << "\n"; )
return -3.0f;
// iBSq = 1.0f;
}
double closest_sep_dist_sq = ASq - (AdotB_sq * iBSq);
return closest_sep_dist_sq;
}
double TimeOfSphereIntersection(Vector3 &PStart,Vector3 &PEnd,
Vector3 &QStart, Vector3 &QEnd,
float PRadius, float QRadius, float Epsilon)
{
Vector3 A = PStart - QStart;
Vector3 B = (PEnd - PStart) - (QEnd - QStart); //relative velocity
double ASq = A.BasicLength();
double AdotB = DotProduct(&A,&B);
double AdotB_sq = AdotB * AdotB;
double BSq = B.BasicLength();
double iBSq(0);// = BSq > .0000001f ? (1/BSq) : 1.0f; //Inverse of |B| squared
if(BSq > .000001f)
{
iBSq = 1/ BSq;
}
else //if b_squared is zero, then they cannot collide (both stationary or moving in same direction)
{
SWEPTSPHEREDEBUG( trace << "BSquared is zero, setting to 1.0f" << "\n"; )
return -3.0f;
// iBSq = 1.0f;
}
double sep_dist = PRadius + QRadius + Epsilon;
double sep_dist_sq = sep_dist * sep_dist;
double closest_sep_dist_sq = ASq - (AdotB_sq * iBSq);
if(closest_sep_dist_sq > sep_dist_sq)
{
SWEPTSPHEREDEBUG( trace << "closest_sep_dist_sq > sep_dist_sq, returning -1" << "\n"; )
return -1.0f;
}
double under_root = AdotB_sq - (BSq * (ASq - sep_dist_sq));
if(under_root < 0)
{
SWEPTSPHEREDEBUG( trace << "negative under radical, no sphere intersection, returning -2" << "\n"; )
return -2.0f;
}
double root = sqrt(under_root);
double time = (-AdotB - root) * iBSq;
if(time >= 1.0f)
{
SWEPTSPHEREDEBUG( trace << "Fishy value of time, greater or equal to 1.0f: " << time << "\n"; )
SWEPTSPHEREDEBUG( trace << "Root: " << root << "under_root: " << under_root << "AdotB: " << AdotB << "\n\n"; )
}
else
{
SWEPTSPHEREDEBUG( trace << "Value of time not fish, good" << "\n"; )
}
return time;
}
...
#include "CollisionDetectionPrimitives.h"
extern std::ofstream trace;
#define RAYPLANEDEBUG(x) //x
/*
Returns a floating point number between 0 and 1 representing the time that the ray, represented by a start
point and an end point, intersects the plane
Epsilon is zero or close-to-zero (quake3 uses (1/32))
*/
float TimeIntersectsPlane(float NormalX, float NormalY, float NormalZ,
float StartX, float StartY, float StartZ,
float EndX, float EndY, float EndZ,
float PlaneD, float Radius, float Epsilon)
{
Vector3 Start(StartX, StartY, StartZ);
Vector3 End(EndX, EndY, EndZ);
Vector3 Normal(NormalX, NormalY, NormalZ);
float abs_start_dist(0),abs_end_dist(0);
float total_dist(0), itotal(0); //Start dist + end dist, and 1/(start_dist + end dist)
abs_start_dist = DotProduct(&Start,&Normal) - PlaneD - Radius;
abs_end_dist = DotProduct(&End, &Normal) - PlaneD - Radius;
RAYPLANEDEBUG( trace << "PlaneD: " << PlaneD << "\n"; )
RAYPLANEDEBUG( trace << "Radius: " << Radius << "\n"; )
RAYPLANEDEBUG( trace << "Startdist: " << abs_start_dist << "\n"; )
RAYPLANEDEBUG( trace << "Enddist: " << abs_end_dist << "\n"; )
//FIXME: I probably want a greater tolerance here
if(abs_start_dist >= 0 && abs_end_dist >= 0)
return -1; //both in front of the plane
if(abs_start_dist < 0 && abs_end_dist < 0)
return -2; //both behind the plane
//At this point you know the points span the plane
//but, the math doesn't work if the start point is behind
//so if it is, just swap their values and proceed
#if 1
if(abs_start_dist < 0)
{
float temp = abs_end_dist;
abs_end_dist = abs_start_dist;
abs_start_dist = temp;
}
#endif
abs_start_dist = fabs(abs_start_dist);
abs_end_dist = fabs(abs_end_dist);
total_dist = abs_start_dist + abs_end_dist;
RAYPLANEDEBUG(trace << "total_dist: " << total_dist << "\n"; )
if(total_dist < .00001f)
{
RAYPLANEDEBUG( trace << "Fishy value for total_dist in TimeRayIntersectsPlane function: " << total_dist << "\n"; )
return -3;
}
itotal = 1/total_dist;
float fraction = (abs_start_dist - Epsilon) * itotal;
//FIXME: this would only happen when start_dist less than EPSILON units away from collision position
if(fraction < 0)
fraction = 0.0f;
return fraction;
}