bool Polyhedron::IsConvex() const
{
// This function is O(n^2).
/** @todo Real-Time Collision Detection, p. 64:
A faster O(n) approach is to compute for each face F of P the centroid C of F,
and for all neighboring faces G of F test if C lies behind the supporting plane of
G. If some C fails to lie behind the supporting plane of one or more neighboring
faces, P is concave, and is otherwise assumed convex. However, note that just as the
corresponding polygonal convexity test may fail for a pentagram this test may fail for,
for example, a pentagram extruded out of its plane and capped at the ends. */
for(int f = 0; f < NumFaces(); ++f)
{
Plane p = FacePlane(f);
for(int i = 0; i < NumVertices(); ++i)
{
float d = p.SignedDistance(Vertex(i));
if (d > 1e-3f) // Tolerate a small epsilon error.
{
printf("Distance of vertex %d from plane %d: %f", i, f, d);
return false;
}
}
}
return true;
}
PBVolume<6> AABB::ToPBVolume() const
{
PBVolume<6> pbVolume;
for(int i = 0; i < 6; ++i)
pbVolume.p[i] = FacePlane(i);
return pbVolume;
}
bool Polyhedron::ContainsConvex(const float3 &point) const
{
assume(IsConvex());
for(int i = 0; i < NumFaces(); ++i)
if (FacePlane(i).SignedDistance(point) > 0.f)
return false;
return true;
}
void AABB::GetFacePlanes(Plane *outPlaneArray) const
{
assume(outPlaneArray);
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (!outPlaneArray)
return;
#endif
for(int i = 0; i < 6; ++i)
outPlaneArray[i] = FacePlane(i);
}
///
// CheckPoint()
//
// Test the point "alpha" of the way from P1 to P2
// See if it is on a face of the cube
// Consider only faces in "mask"
static
int CheckPoint(const vector3& p1, const vector3& p2, number alpha, long mask)
{
vector3 plane_point;
plane_point.x() = LERP(alpha, p1.x(), p2.x());
plane_point.y() = LERP(alpha, p1.y(), p2.y());
plane_point.z() = LERP(alpha, p1.z(), p2.z());
return(FacePlane(plane_point) & mask);
}
void Polyhedron::OrientNormalsOutsideConvex()
{
float3 center = v[0];
for(size_t i = 1; i < v.size(); ++i)
center += v[i];
center /= (float)v.size();
for(int i = 0; i < (int)f.size(); ++i)
if (FacePlane(i).SignedDistance(center) > 0.f)
f[i].FlipWindingOrder();
}
bool Polyhedron::Intersects(const LineSegment &lineSegment) const
{
if (Contains(lineSegment))
return true;
for(int i = 0; i < NumFaces(); ++i)
{
float t;
Plane plane = FacePlane(i);
bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
if (intersects && t >= 0.f && t <= 1.f)
if (FaceContains(i, lineSegment.GetPoint(t)))
return true;
}
return false;
}
bool Polyhedron::FacesAreNondegeneratePlanar(float epsilon) const
{
for(int i = 0; i < (int)f.size(); ++i)
{
const Face &face = f[i];
if (face.v.size() < 3)
return false;
if (face.v.size() >= 4)
{
Plane facePlane = FacePlane(i);
for(int j = 0; j < (int)face.v.size(); ++j)
if (facePlane.Distance(v[face.v[j]]) > epsilon)
return false;
}
}
return true;
}
bool Polyhedron::ClipLineSegmentToConvexPolyhedron(const float3 &ptA, const float3 &dir,
float &tFirst, float &tLast) const
{
assume(IsConvex());
// Intersect line segment against each plane.
for(int i = 0; i < NumFaces(); ++i)
{
/* Denoting the dot product of vectors a and b with <a,b>, we have:
The points P on the plane p satisfy the equation <P, p.normal> == p.d.
The points P on the line have the parametric equation P = ptA + dir * t.
Solving for the distance along the line for intersection gives
t = (p.d - <p.normal, ptA>) / <p.normal, dir>.
*/
Plane p = FacePlane(i);
float denom = Dot(p.normal, dir);
float dist = p.d - Dot(p.normal, ptA);
// Avoid division by zero. In this case the line segment runs parallel to the plane.
if (Abs(denom) < 1e-5f)
{
// If <P, p.normal> < p.d, then the point lies in the negative halfspace of the plane, which is inside the polyhedron.
// If <P, p.normal> > p.d, then the point lies in the positive halfspace of the plane, which is outside the polyhedron.
// Therefore, if p.d - <ptA, p.normal> == dist < 0, then the whole line is outside the polyhedron.
if (dist < 0.f)
return false;
}
else
{
float t = dist / denom;
if (denom < 0.f) // When entering halfspace, update tFirst if t is larger.
tFirst = Max(t, tFirst);
else // When exiting halfspace, updeate tLast if t is smaller.
tLast = Min(t, tLast);
if (tFirst > tLast)
return false; // We clipped the whole line segment.
}
}
return true;
}
void OBB::GetFacePlanes(Plane *outPlaneArray) const
{
assert(outPlaneArray);
for(int i = 0; i < 6; ++i)
outPlaneArray[i] = FacePlane(i);
}
void Polyhedron::MergeConvex(const float3 &point)
{
// LOGI("mergeconvex.");
std::set<std::pair<int, int> > deletedEdges;
std::map<std::pair<int, int>, int> remainingEdges;
for(size_t i = 0; i < v.size(); ++i)
if (point.DistanceSq(v[i]) < 1e-3f)
return;
// bool hadDisconnectedHorizon = false;
for(int i = 0; i < (int)f.size(); ++i)
{
// Delete all faces that don't contain the given point. (they have point in their positive side)
Plane p = FacePlane(i);
Face &face = f[i];
if (p.SignedDistance(point) > 1e-5f)
{
bool isConnected = (deletedEdges.empty());
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size() && !isConnected; ++j)
{
int v1 = face.v[j];
if (deletedEdges.find(std::make_pair(v1, v0)) != deletedEdges.end())
{
isConnected = true;
break;
}
v0 = v1;
}
if (isConnected)
{
v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
deletedEdges.insert(std::make_pair(v0, v1));
// LOGI("Edge %d,%d is to be deleted.", v0, v1);
v0 = v1;
}
// LOGI("Deleting face %d: %s. Distance to vertex %f", i, face.ToString().c_str(), p.SignedDistance(point));
std::swap(f[i], f.back());
f.pop_back();
--i;
continue;
}
// else
// hadDisconnectedHorizon = true;
}
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
remainingEdges[std::make_pair(v0, v1)] = i;
// LOGI("Edge %d,%d is to be deleted.", v0, v1);
v0 = v1;
}
}
// The polyhedron contained our point, nothing to merge.
if (deletedEdges.empty())
return;
// Add the new point to this polyhedron.
// if (!v.back().Equals(point))
v.push_back(point);
/*
// Create a look-up index of all remaining uncapped edges of the polyhedron.
std::map<std::pair<int,int>, int> edgesToFaces;
for(size_t i = 0; i < f.size(); ++i)
{
Face &face = f[i];
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
edgesToFaces[std::make_pair(v1, v0)] = i;
v0 = v1;
}
}
*/
// Now fix all edges by adding new triangular faces for the point.
// for(size_t i = 0; i < deletedEdges.size(); ++i)
for(std::set<std::pair<int, int> >::iterator iter = deletedEdges.begin(); iter != deletedEdges.end(); ++iter)
{
std::pair<int, int> opposite = std::make_pair(iter->second, iter->first);
if (deletedEdges.find(opposite) != deletedEdges.end())
continue;
// std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
// std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
// if (iter != edgesToFaces.end())
{
// If the adjoining face is planar to the triangle we'd like to add, instead extend the face to enclose
// this vertex.
//float3 newTriangleNormal = (v[v.size()-1]-v[iter->second]).Cross(v[iter->first]-v[iter->second]).Normalized();
std::map<std::pair<int, int>, int>::iterator existing = remainingEdges.find(opposite);
assert(existing != remainingEdges.end());
MARK_UNUSED(existing);
#if 0
int adjoiningFace = existing->second;
if (FaceNormal(adjoiningFace).Dot(newTriangleNormal) >= 0.99999f) ///\todo float3::IsCollinear
{
bool added = false;
Face &adjoining = f[adjoiningFace];
for(size_t i = 0; i < adjoining.v.size(); ++i)
if (adjoining.v[i] == iter->second)
{
adjoining.v.insert(adjoining.v.begin() + i + 1, v.size()-1);
added = true;
/*
int prev2 = (i + adjoining.v.size() - 1) % adjoining.v.size();
int prev = i;
int cur = i + 1;
int next = (i + 2) % adjoining.v.size();
int next2 = (i + 3) % adjoining.v.size();
if (float3::AreCollinear(v[prev2], v[prev], v[cur]))
adjoining.v.erase(adjoining.v.begin() + prev);
else if (float3::AreCollinear(v[prev], v[cur], v[next]))
adjoining.v.erase(adjoining.v.begin() + cur);
else if (float3::AreCollinear(v[cur], v[next], v[next2]))
adjoining.v.erase(adjoining.v.begin() + next2);
*/
break;
}
assert(added);
assume(added);
}
else
#endif
// if (!v[deletedEdges[i].first].Equals(point) && !v[deletedEdges[i].second].Equals(point))
{
Face tri;
tri.v.push_back(iter->second);
tri.v.push_back((int)v.size()-1);
tri.v.push_back(iter->first);
f.push_back(tri);
// LOGI("Added face %d: %s.", (int)f.size()-1, tri.ToString().c_str());
}
}
}
#define mathasserteq(lhs, op, rhs) do { if (!((lhs) op (rhs))) { LOGE("Condition %s %s %s (%g %s %g) failed!", #lhs, #op, #rhs, (double)(lhs), #op, (double)(rhs)); assert(false); } } while(0)
// mathasserteq(NumVertices() + NumFaces(), ==, 2 + NumEdges());
assert(FaceIndicesValid());
// assert(EulerFormulaHolds());
// assert(IsClosed());
// assert(FacesAreNondegeneratePlanar());
// assert(IsConvex());
// if (hadDisconnectedHorizon)
// MergeConvex(point);
}
bool Polyhedron::FaceContains(int faceIndex, const float3 &worldSpacePoint, float polygonThickness) const
{
// N.B. This implementation is a duplicate of Polygon::Contains, but adapted to avoid dynamic memory allocation
// related to converting the face of a Polyhedron to a Polygon object.
// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
const Face &face = f[faceIndex];
const std::vector<int> &vertices = face.v;
if (vertices.size() < 3)
return false;
Plane p = FacePlane(faceIndex);
if (FacePlane(faceIndex).Distance(worldSpacePoint) > polygonThickness)
return false;
int numIntersections = 0;
float3 basisU = v[vertices[1]] - v[vertices[0]];
basisU.Normalize();
float3 basisV = Cross(p.normal, basisU).Normalized();
mathassert(basisU.IsNormalized());
mathassert(basisV.IsNormalized());
mathassert(basisU.IsPerpendicular(basisV));
mathassert(basisU.IsPerpendicular(p.normal));
mathassert(basisV.IsPerpendicular(p.normal));
float2 localSpacePoint = float2(Dot(worldSpacePoint, basisU), Dot(worldSpacePoint, basisV));
const float epsilon = 1e-4f;
float2 p0 = float2(Dot(v[vertices.back()], basisU), Dot(v[vertices.back()], basisV)) - localSpacePoint;
if (Abs(p0.y) < epsilon)
p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
for(size_t i = 0; i < vertices.size(); ++i)
{
float2 p1 = float2(Dot(v[vertices[i]], basisU), Dot(v[vertices[i]], basisV)) - localSpacePoint;
if (Abs(p1.y) < epsilon)
p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly.
if (p0.y * p1.y < 0.f)
{
if (p0.x > 1e-3f && p1.x > 1e-3f)
++numIntersections;
else
{
// P = p0 + t*(p1-p0) == (x,0)
// p0.x + t*(p1.x-p0.x) == x
// p0.y + t*(p1.y-p0.y) == 0
// t == -p0.y / (p1.y - p0.y)
// Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate.
float2 d = p1 - p0;
if (Abs(d.y) > 1e-5f)
{
float t = -p0.y / d.y;
float x = p0.x + t * d.x;
if (t >= 0.f && t <= 1.f && x > 1e-6f)
++numIntersections;
}
}
}
p0 = p1;
}
return numIntersections % 2 == 1;
}
///
// TriCubeIntersection()
//
// Triangle t is compared with a unit cube,
// centered on the origin.
// It returns INSIDE (0) or OUTSIDE(1) if t
// Intersects or does not intersect the cube.
static
int TriCubeIntersection(const TRI& t)
{
int v1_test,v2_test,v3_test;
number d;
vector3 vect12,vect13,norm;
vector3 hitpp,hitpn,hitnp,hitnn;
///
// First compare all three vertexes with all six face-planes
// If any vertex is inside the cube, return immediately!
//
if ((v1_test = FacePlane(t.m_P[0])) == INSIDE) return(INSIDE);
if ((v2_test = FacePlane(t.m_P[1])) == INSIDE) return(INSIDE);
if ((v3_test = FacePlane(t.m_P[2])) == INSIDE) return(INSIDE);
///
// If all three vertexes were outside of one or more face-planes,
// return immediately with a trivial rejection!
//
if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);
///
// Now do the same trivial rejection test for the 12 edge planes
//
v1_test |= Bevel2d(t.m_P[0]) << 8;
v2_test |= Bevel2d(t.m_P[1]) << 8;
v3_test |= Bevel2d(t.m_P[2]) << 8;
if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);
///
// Now do the same trivial rejection test for the 8 corner planes
//
v1_test |= Bevel3d(t.m_P[0]) << 24;
v2_test |= Bevel3d(t.m_P[1]) << 24;
v3_test |= Bevel3d(t.m_P[2]) << 24;
if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);
///
// If vertex 1 and 2, as a pair, cannot be trivially rejected
// by the above tests, then see if the v1-->v2 triangle edge
// intersects the cube. Do the same for v1-->v3 and v2-->v3.
// Pass to the intersection algorithm the "OR" of the outcode
// bits, so that only those cube faces which are spanned by
// each triangle edge need be tested.
//
if ((v1_test & v2_test) == 0)
if (CheckLine(t.m_P[0],t.m_P[1],v1_test|v2_test) == INSIDE) return(INSIDE);
if ((v1_test & v3_test) == 0)
if (CheckLine(t.m_P[0],t.m_P[2],v1_test|v3_test) == INSIDE) return(INSIDE);
if ((v2_test & v3_test) == 0)
if (CheckLine(t.m_P[1],t.m_P[2],v2_test|v3_test) == INSIDE) return(INSIDE);
///
// By now, we know that the triangle is not off to any side,
// and that its sides do not penetrate the cube. We must now
// test for the cube intersecting the interior of the triangle.
// We do this by looking for intersections between the cube
// diagonals and the triangle...first finding the intersection
// of the four diagonals with the plane of the triangle, and
// then if that intersection is inside the cube, pursuing
// whether the intersection point is inside the triangle itself.
// To find plane of the triangle, first perform crossproduct on
// two triangle side vectors to compute the normal vector.
SUB(t.m_P[0],t.m_P[1],vect12);
SUB(t.m_P[0],t.m_P[2],vect13);
CROSS(vect12,vect13,norm)
///
// The normal vector "norm" X,Y,Z components are the coefficients
// of the triangles AX + BY + CZ + D = 0 plane equation. If we
// solve the plane equation for X=Y=Z (a diagonal), we get
// -D/(A+B+C) as a metric of the distance from cube center to the
// diagonal/plane intersection. If this is between -0.5 and 0.5,
// the intersection is inside the cube. If so, we continue by
// doing a point/triangle intersection.
// Do this for all four diagonals.
d = norm.x() * t.m_P[0].x() + norm.y() * t.m_P[0].y() + norm.z() * t.m_P[0].z();
hitpp.x() = hitpp.y() = hitpp.z() = d / (norm.x() + norm.y() + norm.z());
if (fabs(hitpp.x()) <= 0.5)
if (PointTriangleIntersection(hitpp,t) == INSIDE) return(INSIDE);
hitpn.z() = -(hitpn.x() = hitpn.y() = d / (norm.x() + norm.y() - norm.z()));
if (fabs(hitpn.x()) <= 0.5)
if (PointTriangleIntersection(hitpn,t) == INSIDE) return(INSIDE);
hitnp.y() = -(hitnp.x() = hitnp.z() = d / (norm.x() - norm.y() + norm.z()));
if (fabs(hitnp.x()) <= 0.5)
if (PointTriangleIntersection(hitnp,t) == INSIDE) return(INSIDE);
hitnn.y() = hitnn.z() = -(hitnn.x() = d / (norm.x() - norm.y() - norm.z()));
if (fabs(hitnn.x()) <= 0.5)
if (PointTriangleIntersection(hitnn,t) == INSIDE) return(INSIDE);
///
// No edge touched the cube; no cube diagonal touched the triangle.
// We're done...there was no intersection.
//
return(OUTSIDE);
}