## Collision Detection (Point-Polygon)

Member
Posts: 148
Joined: 2003.03
Post: #16
inio, i just tried implementing your example, and i have a few questions about it. what is BACKFACE for? what is T for?

anyway, what i tried still doesn't work . here is the code i am using:

[sourcecode]
-(BOOL)intersectTriangleOrigin:(vertex)orig Direction:(vertex)dir V0:(vertex)vert0 V1:(vertex)vert1 V2:(vertex)vert2 Backface:(BOOL)backface T:(float)t
{
vertex edge1, edge2, tvec, pvec, qvec;
float det, inv_det;

edge1.x = vert1.x-vert0.x;
edge1.y = vert1.y-vert0.y;
edge1.z = vert1.z-vert0.z;

edge2.x = vert2.x-vert0.x;
edge2.y = vert2.y-vert0.y;
edge2.z = vert2.z-vert0.z;

pvec.x = dir.y*edge2.z - dir.z*edge2.y;
pvec.y = dir.z*edge2.x - dir.x*edge2.z;
pvec.z = dir.x*edge2.y - dir.y*edge2.x;

det = edge1.x*pvec.x + edge1.y*pvec.y + edge1.z*pvec.z;

if(det>0)
{
if(t)
inv_det = 1.0/det;

tvec.x = orig.x-vert0.x;
tvec.y = orig.y-vert0.y;
tvec.z = orig.z-vert0.z;

float u = tvec.x*pvec.x + tvec.y*pvec.y + tvec.z*pvec.z;

if(u<0.0 || u>det)
return FALSE;

qvec.x = tvec.y*edge1.z - tvec.z*edge1.y;
qvec.y = tvec.z*edge1.x - tvec.x*edge1.z;
qvec.z = tvec.x*edge1.y - tvec.y*edge1.x;

float v = dir.x*qvec.x + dir.y*qvec.y + dir.z*qvec.z;

if(v<0.0 || u+v>det)
return FALSE;
}
else
{
if(!backface)
return FALSE;

if(t)
inv_det = 1.0/det;

tvec.x = orig.x-vert0.x;
tvec.y = orig.y-vert0.y;
tvec.z = orig.z-vert0.z;

float u = tvec.x*pvec.x + tvec.y*pvec.y + tvec.z*pvec.z;

if(u<0.0 || u>det)
return FALSE;

qvec.x = tvec.y*edge1.z - tvec.z*edge1.y;
qvec.y = tvec.z*edge1.x - tvec.x*edge1.z;
qvec.z = tvec.x*edge1.y - tvec.y*edge1.x;

float v = dir.x*qvec.x + dir.y*qvec.y + dir.z*qvec.z;

if(v<0.0 || u+v>det)
return FALSE;
}

//if(t)
//t = (edge2.x*qvec.x + edge2.y*qvec.y + edge2.z*qvec.z) * inv_det;

return TRUE;
}
[/sourcecode]

and i am testing for collisions using:

[sourcecode]
if([self intersectTriangleOrigin:camera.position Direction:camera.orientation V0:my_object.vertices[0] V1:my_object.vertices[1] V2:my_object.vertices[2] Backface:FALSE T:0.0])
{
//COLLISION OCCURRED
}
[/sourcecode]

is there something i am missing? thanks.
Member
Posts: 145
Joined: 2002.06
Post: #17
Quote:Originally posted by MacFiend
inio, i just tried implementing your example, and i have a few questions about it. what is BACKFACE for? what is T for?
backface: if true, intersectiosn from both directions are found. If false, insersections from the side where the triangle appears to be in clickwise order are found. Useful if you want to short circuit out of some calculations and you have a reliable polygon direction.
t: the number of times that you must move dir from the start point to reach the intersection.

"He who breaks a thing to find out what it is, has left the path of wisdom."
- Gandalf the Gray-Hat

Bring Alistair Cooke's America to DVD!
Member
Posts: 148
Joined: 2003.03
Post: #18
OneSadCookie, i think i am going to try your idea of using perpendicular planes to tell if the point is within the triangle. Would you be able to tell me how I would find the plane perpendicular to the triangle-plane using the edges of the triangle?

Also, i think the reason none of the other examples are working for me is that the camera position and the triangle are no coplanar. I have no way (as of now), to make the positions coplanar.
Moderator
Posts: 869
Joined: 2003.01
Post: #19
Quote:Originally posted by MacFiend
OneSadCookie, i think i am going to try your idea of using perpendicular planes to tell if the point is within the triangle. Would you be able to tell me how I would find the plane perpendicular to the triangle-plane using the edges of the triangle?

Also, i think the reason none of the other examples are working for me is that the camera position and the triangle are no coplanar. I have no way (as of now), to make the positions coplanar.

You can easily make the point on the triangle plane closest to the point you want by projecting the distance vector from the point to any of the triangle's vertices to the triangle's normal, then subtracting this vector from the original point's position. But this has been said before, somewhere.
Member
Posts: 148
Joined: 2003.03
Post: #20
Ahh okay. I'll try that then.
Member
Posts: 304
Joined: 2002.04
Post: #21
Quote:Originally posted by DoooG
The method works for a single triangle, yes, that was asked, ircc. For a mesh, you obviously have to make some more assumptions, for example that the mesh is regular (a single surface). With a regular mesh, and allowing collisions with multiple triangles at the same time, things should work out just fine, since the proposed algorithm correctly determines collision with each individual triangle. Right? (i think)

Technically yes. But in actuality almost always no. Almost always you dont want to collide a point - but a sphere or cylinder or something. In the case of a camera - the camera isnt a point - it has a near clipping plane. So by the time the camera "point" hits the triangle - the triangle has already been clipped by the near plane. So what you almost always want is to collide a sphere with a triangle. And with that - the nearest point from the center to the triangle's plane may well be outside the triangle - while still parts of the sphere (and clipping plane) my knock in to the edge of the triangle.

Still - getting the code working for the point/triangle is a very good start - so dont give up!
Member
Posts: 148
Joined: 2003.03
Post: #22
Yea i understand that all of this would still require a lot more work to get a full collision detection engine going, i just like playing around with small things like this as to provide a basis for the meat.
Member
Posts: 164
Joined: 2002.04
Post: #23
In Lugaru I use line/poly collision to find the approximate location of the camera and then use sphere/poly on the nearest few to fix the near clipping plane, seems to work best like that.

If this helps at all my line-triangle functions are
[SOURCECODE]
bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p)
{
static float d;
static float a1,a2,a3;
static float total,denom,mu;
static XYZ n,pa1,pa2,pa3;

//Calculate the parameters for the plane
n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y);
n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z);
n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x);
Normalise(&n);
d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;

//Calculate the position on the line that intersects the plane
denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
if (abs(denom) < 0.0000001) // Line and plane don't intersect
return 0;
mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
p->x = p1.x + mu * (p2.x - p1.x);
p->y = p1.y + mu * (p2.y - p1.y);
p->z = p1.z + mu * (p2.z - p1.z);
if (mu < 0 || mu > 1) // Intersection not along line segment
return 0;

if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;}

return 1;
}

bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3)
{
static float u0, u1, u2;
static float v0, v1, v2;
static float a, b;
static float max;
static int i, j;
static bool bInter = 0;
static float pointv[3];
static float p1v[3];
static float p2v[3];
static float p3v[3];
static float normalv[3];

bInter=0;

pointv[0]=p->x;
pointv[1]=p->y;
pointv[2]=p->z;

p1v[0]=p1->x;
p1v[1]=p1->y;
p1v[2]=p1->z;

p2v[0]=p2->x;
p2v[1]=p2->y;
p2v[2]=p2->z;

p3v[0]=p3->x;
p3v[1]=p3->y;
p3v[2]=p3->z;

normalv[0]=normal.x;
normalv[1]=normal.y;
normalv[2]=normal.z;

#define ABS(X) (((X)<0.f)?-(X)X) )
#define MAX(A, B) (((A)<(B))?(B)A))
max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
#undef MAX
if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z
if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z
if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y
#undef ABS

u0 = pointv[i] - p1v[i];
v0 = pointv[j] - p1v[j];
u1 = p2v[i] - p1v[i];
v1 = p2v[j] - p1v[j];
u2 = p3v[i] - p1v[i];
v2 = p3v[j] - p1v[j];

if (u1 > -1.0e-05f && u1 < 1.0e-05f)// == 0.0f)
{
b = u0 / u2;
if (0.0f <= b && b <= 1.0f)
{
a = (v0 - b * v2) / v1;
if ((a >= 0.0f) && (( a + b ) <= 1.0f))
bInter = 1;
}
}
else
{
b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
if (0.0f <= b && b <= 1.0f)
{
a = (u0 - b * u2) / u1;
if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
bInter = 1;
}
}

return bInter;
}

void Normalise(XYZ *vectory) {
static float d;
d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z);
if(d==0){return;}
vectory->x /= d;
vectory->y /= d;
vectory->z /= d;
}

float fast_sqrt (register float arg)
{
// Can replace with slower return std::sqrt(arg);
register float result;

if (arg == 0.0) return 0.0;

asm {
frsqrte result,arg // Calculate Square root
}

// Newton Rhapson iterations.
result = result + 0.5 * result * (1.0 - arg * result * result);
result = result + 0.5 * result * (1.0 - arg * result * result);

return result * arg;
}
[/SOURCECODE]

Hope this helps.

BTW if anyone finds anything that could be sped up here it would be great, since I have to do a ton of these every frame.
Luminary
Posts: 5,139
Joined: 2002.04
Post: #24
Quote:Originally posted by David
[SOURCECODE]
void Normalise(XYZ *vectory) {
static float d;
d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z);
if(d==0){return;}
vectory->x /= d;
vectory->y /= d;
vectory->z /= d;
}[/SOURCECODE]

BTW if anyone finds anything that could be sped up here it would be great, since I have to do a ton of these every frame.

OK, I'm just going to pick on this one function. You'll want to move this function to the top of the file (just below fast_sqrt, which should be first and inlined).

Code:
```inline void Normalise(XYZ *vectory) {     float x = vectory->x;     float y = vectory->y;     float z = vectory->z;     float d = 1.0f / fast_sqrt((x*x) + (y*y) + (z*z));     vectory->x = x * d;     vectory->y = y * d;     vectory->z = z * d; }```

I'm sure you can get similar improvements in other places.
Member
Posts: 145
Joined: 2002.06
Post: #25
Quote:Originally posted by David
BTW if anyone finds anything that could be sped up here it would be great, since I have to do a ton of these every frame.
Arrays are inherently slower than regular local variables. Also, on PowerPC there's a fast way to do sqrt:
Code:
```// calculate sqrt(x) with pretty good accuacy // x is double to prevent frsp generation before call // return type is float to prevent generation after call inline float FastSqrt(register double x) {     register float inv;     register float oneHalf = 0.5;     asm {         frsqrte     inv,x; // inv = 1/sqrt(x) // inv = 1/sqrt(x)         fmuls       x,x,inv; // f*inv         fres        inv,inv; // 1/inv         fadds       x,x,inv; // 1/inv+f*inv         fmuls       x,x,oneHalf; // .5*(1/inv+f*inv)     }     return x; }```
which provides about 3 digits of precision. For inverse square root, the even faster:
Code:
```// calculate 1/sqrt(x) with pretty good accuacy // x is double to prevent frsp generation before call // return type is float to prevent generation after call inline float FastSqrtRecip(register double x) {     register float inv;     register float oneHalf = 0.5;     asm {         frsqrte     inv,x; // inv = 1/sqrt(x)         fmuls       x,x,inv; // f*inv         fres        x,x; // 1/(f*inv)         fadds       x,x,inv; // 1/(f*inv)+inv         fmuls       x,x,oneHalf; // .5*(1/(f*inv)+inv)     }     return x; }```
It seems you're doing a slightly less efficient implementation of the same thing.

"He who breaks a thing to find out what it is, has left the path of wisdom."
- Gandalf the Gray-Hat

Bring Alistair Cooke's America to DVD!
Member
Posts: 145
Joined: 2002.06
Post: #26
Code:
```... float d = 1.0f / fast_sqrt((x*x) + (y*y) + (z*z)); ...```
That's a prime candidate for fmadds. Wonder if CW/gcc will generate it there. more likely sequence might be:
Code:
```x *= x; y = y * y + x; z = z * z + y; float d = FastSqrtRecip(z);```

"He who breaks a thing to find out what it is, has left the path of wisdom."
- Gandalf the Gray-Hat

Bring Alistair Cooke's America to DVD!
Luminary
Posts: 5,139
Joined: 2002.04
Post: #27
... except you can't reassign to the locals because you need them later to avoid an extra load. Still, using an extra register won't be a big deal:

Code:
```float norm = x * x; norm += y * y; norm += z * z; float d = FastSqrtRecip(norm);```

In my experience, though, GCC (at least) is pretty good at generating fmadds instructions, so it's probably unnecessary. Disassemble before trying
Member
Posts: 148
Joined: 2003.03
Post: #28
David, i'm trying your example. I found (i think, at least) where you got that from, so i tried implementing that. Here is what i'm basing this on: [url=http://astronomy.swin.edu.au/~pbourke/geometry/linefacet/
]http://astronomy.swin.edu.au/~pbourke/geometry/linefacet/
[/url] Here is the code im using:

[sourcecode]
-(void)testForCollision
{
vertex intersection;

if([self LineFacetSegmentA:camera.position SegmentB:[self moveVertex:camera.position Direction:camera.orientation Amount:CAMERA_MOVE_SPEED] TriangleA:my_object.vertices[0] TriangleB:my_object.vertices[1] TriangleC:my_object.vertices[2] IntersectionResult:&intersection]==TRUE)
{
//COLLISION
}
}
-(BOOL)LineFacetSegmentA:(vertex)p1 SegmentB:(vertex)p2 TriangleA:(vertex)pa TriangleB:(vertex)pb TriangleC:(vertex)pc IntersectionResult:(vertex*)p
{
float d;
float a1,a2,a3;
float total,denom,mu;
vertex n,pa1,pa2,pa3,p;

n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y);
n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z);
n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x);
n = [self NormalizeVertex:n];

d = -n.x*pa.x - n.y*pa.y - n.z*pa.z;

denom = n.x *(p2.x-p1.x) + n.y*(p2.y-p1.y) + n.z*(p2.z-p1.z);

if(fabsf(denom)<0.0000001)
return FALSE;

mu = -(d + n.x*p1.x + n.y*p1.y + n.z*p1.z)/denom;
p.x = p1.x + mu*(p2.x-p1.x);
p.y = p1.y + mu*(p2.y-p1.y);
p.z = p1.z + mu*(p2.z-p1.z);

if(mu<0 || mu>1)
return FALSE;

pa1.x = pa.x - p->x;
pa1.y = pa.y - p->y;
pa1.z = pa.z - p->z;
pa1 = [self NormalizeVertex:pa1];
pa2.x = pb.x - p->x;
pa2.y = pb.y - p->y;
pa2.z = pb.z - p->z;
pa2 = [self NormalizeVertex:pa2];
pa3.x = pc.x - p->x;
pa3.y = pc.y - p->y;
pa3.z = pc.z - p->z;
pa3 = [self NormalizeVertex:pa3];

a1 = pa1.x*pa2.x + pa1.y*pa2.y + pa1.z*pa2.z;
a2 = pa2.x*pa3.x + pa2.y*pa3.y + pa2.z*pa3.z;
a3 = pa3.x*pa1.x + pa3.y*pa1.y + pa3.z*pa1.z;

total = (acos(a1) + acos(a2) + acos(a3)) * 57.2957795;

if(fabsf(total-360)>0.0000001)
return FALSE;

return TRUE;
}

-(vertex)NormalizeVertex:(vertex)p
{
vertex newV;
float length=sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
newV.x = p.x/length;
newV.y = p.y/length;
newV.z = p.z/length;

return newV;
}

-(vertex)moveVertex:(vertex)vert Direction:(vertex)dir Amount:(float)byAmount
{
double yRot,xMov,yMov,zMov,newX,newY,newZ;
double PI=3.14;
vertex newVert;

yRot = dir.y;
yRot = yRot*(PI/180);
xMov = byAmount*sin(yRot);
yMov = 0.0;
zMov = byAmount*cos(yRot);
newX = vert.x+xMov;
newY = vert.y;
newZ = vert.z+zMov;
newVert.x = newX;
newVert.y = newY;
newVert.z = newZ;

return newVert;
}
[/sourcecode]

Now its not working because fabsf(total-360) (near the end of the LineFacet... function) is always some number like 300+. Even when the camera is directly in front of the triangle i want to collide with.

It shouldn't matter if camera.position is coplanar with the triangle plane, just as long as camera.position and its next position intersects the triangle plane, correct?

I was hoping someone could possibly explain to me why fabsf(total-360) is never less than epsilon (0.0000001). I'd really appreciate it, this is frustrating as h***. Thanks for all your help.

btw, the vertices for my triangle are as follows:
Quote: my_object.vertices[0].x = 0.0;
my_object.vertices[0].y = 0.0;
my_object.vertices[0].z = 0.0;

my_object.vertices[1].x = 0.0;
my_object.vertices[1].y = 1.0;
my_object.vertices[1].z = -1.0;

my_object.vertices[2].x = 1.0;
my_object.vertices[2].y = 1.0;
my_object.vertices[2].z = -1.0;
Member
Posts: 164
Joined: 2002.04
Post: #29
I know nothing about optimizing, so: