Ego::Math::Relation plane_intersects_aabb_max(const Plane3f& plane, const Vector3f& mins, const Vector3f& maxs)
{
int j;
float dist, tmp;
Ego::Math::Relation retval = Ego::Math::Relation::error;
// find the point-plane distance for the most-positive points of the aabb
dist = 0.0f;
for (j = 0; j < 3; j++)
{
tmp = (plane.getNormal()[j] > 0.0f) ? maxs[j] : mins[j];
dist += tmp * plane.getNormal()[j];
}
dist += plane.getDistance();
if (dist > 0.0f)
{
retval = Ego::Math::Relation::inside;
}
else if (dist < 0.0f)
{
retval = Ego::Math::Relation::outside;
}
else
{
retval = Ego::Math::Relation::intersect;
}
return retval;
}
bool three_plane_intersection(Vector3f& dst_pos, const Plane3f& p0, const Plane3f& p1, const Plane3f& p2)
{
Vector3f n0 = p0.getNormal(),
n1 = p1.getNormal(),
n2 = p2.getNormal();
float d0 = p0.getDistance(),
d1 = p1.getDistance(),
d2 = p2.getDistance();
// the determinant of the matrix
float det =
n0[kX] * (n1[kY] * n2[kZ] - n1[kZ] * n2[kY]) -
n0[kY] * (n1[kX] * n2[kZ] - n2[kX] * n1[kZ]) +
n0[kZ] * (n1[kX] * n2[kY] - n2[kX] * n1[kY]);
// check for system that is too close to being degenerate
if (std::abs(det) < 1e-6) return false;
float tmp;
// the x component
tmp =
d0 * (n1[kZ] * n2[kY] - n1[kY] * n2[kZ]) +
d1 * (n0[kY] * n2[kZ] - n0[kZ] * n2[kY]) +
d2 * (n0[kZ] * n1[kY] - n0[kY] * n1[kZ]);
dst_pos[kX] = tmp / det;
// the y component
tmp =
d0 * (n1[kX] * n2[kZ] - n1[kZ] * n2[kX]) +
d1 * (n0[kZ] * n2[kX] - n0[kX] * n2[kZ]) +
d2 * (n0[kX] * n1[kZ] - n0[kZ] * n1[kX]);
dst_pos[kY] = tmp / det;
// the z component
tmp =
d0 * (n1[kY] * n2[kX] - n1[kX] * n2[kY]) +
d1 * (n0[kX] * n2[kY] - n0[kY] * n2[kX]) +
d2 * (n0[kY] * n1[kX] - n0[kX] * n1[kY]);
dst_pos[kZ] = tmp / det;
return true;
}
bool two_plane_intersection(Vector3f& p, Vector3f& d, const Plane3f& p0, const Plane3f& p1)
{
// Compute \f$\vec{d} = \hat{n}_0 \times \hat{n}_1\f$
const Vector3f &n0 = p0.getNormal();
const Vector3f &n1 = p1.getNormal();
d = n0.cross(n1);
// If \f$\vec{v}\f$ is the zero vector, then the planes do not intersect.
if (0 == d.normalize()) {
return false;
}
if (0.0f != d[kZ])
{
p[kX] = (n0[kY] * n1[kW] - n0[kW] * n1[kY]) / d[kZ];
p[kY] = (n0[kW] * n1[kX] - n0[kX] * n1[kW]) / d[kZ];
p[kZ] = 0.0f;
}
else
{
throw std::runtime_error("not yet supported");
}
return true;
}
