What's wrong with my code?
bool PointBehindPlane(Vec3f p, Vec3f normal, float dist)
{
float result = p.x*normal.x + p.y*normal.y + p.z*normal.z + dist;
//if(result < 0)
if(result < EPSILON)
return true;
return false;
}
// line intersects plane?
bool LineInterPlane(Vec3f* line, Vec3f norm, float d, Vec3f* inter)
{
Vec3f change = line[1] - line[0];
float denom = Dot(norm, change);
if(fabs(denom) <= EPSILON)
return false;
float SegScalar = (d - Dot(norm, line[0])) / denom;
//TODO: Check if SegScalar is [0.0, 1.0]?
if(SegScalar < 0.0f)
return false;
*inter = change * SegScalar + line[0];
return true;
}
// http://thejuniverse.org/PUBLIC/LinearAlgebra/LOLA/planes/std.html
void MakePlane(Vec3f* norm, float* d, Vec3f point, Vec3f setnorm)
{
*norm = setnorm;
*d = Dot(setnorm, point);
}
void MakeHull(Vec3f* norms, float* ds, Vec3f pos, float radius, float height)
{
MakePlane(&norms[0], &ds[0], pos + Vec3f(0, height, 0), Vec3f(0, 1, 0)); //up
MakePlane(&norms[1], &ds[1], pos + Vec3f(0, 0, 0), Vec3f(0, -1, 0)); //down
MakePlane(&norms[2], &ds[2], pos + Vec3f(-radius, 0, 0), Vec3f(-1, 0, 0)); //left
MakePlane(&norms[3], &ds[3], pos + Vec3f(radius, 0, 0), Vec3f(1, 0, 0)); //right
MakePlane(&norms[4], &ds[4], pos + Vec3f(0, 0, -radius), Vec3f(0, 0, -1)); //front
MakePlane(&norms[5], &ds[5], pos + Vec3f(0, 0, radius), Vec3f(0, 0, 1)); //back
}
// line intersects convex hull?
bool LineInterHull(Vec3f* line, Vec3f* norms, float* ds, int numplanes)
{
bool* hasinter = new bool[numplanes];
Vec3f* inter = new Vec3f[numplanes];
for(int i=0; i<numplanes; i++)
{
hasinter = LineInterPlane(line, norms, ds, &inter);
}
for(int i=0; i<numplanes; i++)
{
if(hasinter)
{
bool allin = true;
for(int j=0; j<numplanes; j++)
{
if(i == j)
continue;
if(!PointBehindPlane(inter, norms[j], ds[j]))
{
allin = false;
break;
}
}
if(allin)
{
delete [] hasinter;
delete [] inter;
return true;
}
}
}
delete [] hasinter;
delete [] inter;
return false;
}