|
/*********************************************************************************************/
/*
*/
/* Collision Detection Code
*/
/* Its a bit brute force, but we've not got to many poly's yet, so we can
improve it later */
/*
*/
/*********************************************************************************************/
// Two main parts - We check for ray-triangle collision detection,
which is a
// bit brute force - and could really really do with optimsation, but
it works
// for the number of tris we have at present.
// Then secondly we add in extra collision detction code to catch
those little
// slip through occasions when our ray doesn't catch a triangle, but
we might be
// able to get through a wall.
int
CollisionTest( Vector3
vFrom, Vector3 vTo )
{
int
iCollisions = 0;
// PART +1+
iCollisions += TriangleLineCollision(vFrom, vTo);
// PART+2+
// Lets transform all of the
triangles
for(int
i=0; i<m_NumTris; i++)
{
// Save time and check if
our line crosses our tri's plane
// Part+2a
Triangle tri = m_tri[i];
Vector3 p0 =
new
Vector3(tri.pA);
Vector3 p1 =
new
Vector3(tri.pB);
Vector3 p2 =
new
Vector3(tri.pC);
lines[2] =
new
Vector3(tri.pA);
lines[3] =
new
Vector3(tri.pB);
Vector3 v0 =
Vector3.subtract(p1, p0);
Vector3 v1 = Vector3.subtract(p2, p0);
Vector3 vTriNormal =
Vector3.cross( v0, v1 );
vTriNormal = Vector3.normalize(vTriNormal);
//vTriNormal =
Vector3.invert(vTriNormal);
Vector3 vTriPoint = p0;
float
k = Vector3.dot( vTriNormal, vTriPoint );
float
a = Vector3.dot( vTriNormal, vFrom ) - k;
float
b = Vector3.dot( vTriNormal, vTo ) - k;
float
tt = a*b;
//System.out.println( "a:
" + a + " b: " + b + " k: " + k + " a*b:" + tt );
// If it doesn't cross our tri's
plane - lets go onto the next triangle.
if(
a*b > 0.0f )
continue;
//
Part+2b
// This last part is a
sort of hack fix - as there could be a hair-line gap
// between two wall and the person could squeeze through it...!
don't want them
// doing that. Also, at corners, you could walk through them -
so in essence
// I added a sphere collision test, to make sure we can't get
to close to the
// edges of our triangles or corners.
// You could still comment out this code and it would work -
but its just an
// extra little saftey net
int
iSpherecollision = 0;
float
radius = 10.0f;
iSpherecollision += SpherePointCollision( vTo, p0, radius );
iSpherecollision += SpherePointCollision( vTo, p1, radius );
iSpherecollision += SpherePointCollision( vTo, p2, radius );
iSpherecollision += SpherePointCollision( vFrom, p0, radius );
iSpherecollision += SpherePointCollision( vFrom, p1, radius );
iSpherecollision += SpherePointCollision( vFrom, p2, radius );
if(
iSpherecollision > 0 )
{
iCollisions += iSpherecollision;
m_tri[i].c = Color.orange;
}
}// End for loop
return
iCollisions;
}// CollisionTest(..)
int
TriangleLineCollision(
Vector3 vFrom, Vector3 vTo )
{
int
iCollisions = 0;
// Lets transform all of the
triangles
for(int
i=0; i<m_NumTris; i++)
{
//m_tri[i].transform( mat
);
Triangle tri = m_tri[i];
m_tri[i].c = m_tri[i].bc;
Vector3 p0 =
new
Vector3(tri.pA);
Vector3 p1 =
new
Vector3(tri.pB);
Vector3 p2 =
new
Vector3(tri.pC);
lines[2] =
new
Vector3(tri.pA);
lines[3] =
new
Vector3(tri.pB);
Vector3 v0 =
Vector3.subtract(p1, p0);
Vector3 v1 = Vector3.subtract(p2, p0);
Vector3 vTriNormal =
Vector3.cross( v0, v1 );
vTriNormal = Vector3.normalize(vTriNormal);
//vTriNormal =
Vector3.invert(vTriNormal);
Vector3 vTriPoint = p0;
float
k = Vector3.dot( vTriNormal, vTriPoint );
float
a = Vector3.dot( vTriNormal, vFrom ) - k;
float
b = Vector3.dot( vTriNormal, vTo ) - k;
float
tt = a*b;
//System.out.println( "a:
" + a + " b: " + b + " k: " + k + " a*b:" + tt );
if(
a*b > 0.0f )
continue;
Vector3 px = PointOnPlaneAlt( vFrom, vTo,
vTriNormal, p0 );
float
angle = 0;
Vector3 vS, vT, vR;
Vector3 o1 = p0;
Vector3 o2 = p1;
Vector3 o3 = p2;
vS = Vector3.subtract(o1, px );
vT = Vector3.subtract(o2, px );
vS = Vector3.normalize( vS );
vT = Vector3.normalize( vT );
float
cosAngle = Vector3.dot( vS, vT );
angle += Math.acos( cosAngle );
vS = Vector3.subtract(o2, px );
vT = Vector3.subtract(o3, px );
vS = Vector3.normalize( vS );
vT = Vector3.normalize( vT );
cosAngle = Vector3.dot( vS, vT );
angle += Math.acos( cosAngle );
vS = Vector3.subtract(o3, px );
vT = Vector3.subtract(o1, px );
vS = Vector3.normalize( vS );
vT = Vector3.normalize( vT );
cosAngle = Vector3.dot( vS, vT );
angle += Math.acos( cosAngle );
if(
angle > (3.13f*2) )
{
iCollisions++;
m_tri[i].c = Color.orange;
}
}// End for loop
return
iCollisions;
}// End of TriangleLineCollision(..)
int
SpherePointCollision(
Vector3 vPoint,
Vector3 vCentreSphere,
float
fRadius )
{
Vector3 vDiff = Vector3.subtract( vPoint, vCentreSphere );
float
len = Vector3.length(
vDiff );
len = Math.abs(len);
if( len < fRadius )
return
1;
return
0;
}// End of SpherePointCollision(..)
|
