bool Polygon::Contains(const float3 &worldSpacePoint, float polygonThickness) const
{
// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
if (p.size() < 3)
return false;
if (PlaneCCW().Distance(worldSpacePoint) > polygonThickness)
return false;
int numIntersections = 0;
float3 basisU = BasisU();
float3 basisV = BasisV();
mathassert(basisU.IsNormalized());
mathassert(basisV.IsNormalized());
mathassert(basisU.IsPerpendicular(basisV));
mathassert(basisU.IsPerpendicular(PlaneCCW().normal));
mathassert(basisV.IsPerpendicular(PlaneCCW().normal));
float2 localSpacePoint = float2(Dot(worldSpacePoint, basisU), Dot(worldSpacePoint, basisV));
const float epsilon = 1e-4f;
float2 p0 = float2(Dot(p[p.size()-1], basisU), Dot(p[p.size()-1], 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(int i = 0; i < (int)p.size(); ++i)
{
float2 p1 = float2(Dot(p[i], basisU), Dot(p[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-3f)
++numIntersections;
}
}
}
p0 = p1;
}
return numIntersections % 2 == 1;
}
bool Polygon::Intersects(const Ray &ray) const
{
float d;
if (!PlaneCCW().Intersects(ray, &d))
return false;
return Contains(ray.GetPoint(d));
}
bool Polygon::Intersects(const Line &line) const
{
float d;
if (!PlaneCCW().Intersects(line, &d))
return false;
return Contains(line.GetPoint(d));
}
bool Polygon::DiagonalExists(int i, int j) const
{
assume(p.size() >= 3);
assume(i >= 0);
assume(j >= 0);
assume(i < (int)p.size());
assume(j < (int)p.size());
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (p.size() < 3 || i < 0 || j < 0 || i >= (int)p.size() || j >= (int)p.size())
return false;
#endif
assume(IsPlanar());
assume(i != j);
if (i == j) // Degenerate if i == j.
return false;
if (i > j)
Swap(i, j);
assume(i+1 != j);
if (i+1 == j) // Is this LineSegment an edge of this polygon?
return false;
Plane polygonPlane = PlaneCCW();
LineSegment diagonal = polygonPlane.Project(LineSegment(p[i], p[j]));
// First check that this diagonal line is not intersected by an edge of this polygon.
for(int k = 0; k < (int)p.size(); ++k)
if (!(k == i || k+1 == i || k == j))
if (polygonPlane.Project(LineSegment(p[k], p[k+1])).Intersects(diagonal))
return false;
return IsConvex();
}
bool Polygon::Intersects(const Plane &plane) const
{
Line intersectLine;
bool intersects = plane.Intersects(PlaneCCW(), &intersectLine);
if (!intersects)
return false;
return Intersects(intersectLine);
}
bool Triangle::Contains(const float3 &point, float triangleThickness) const
{
if (PlaneCCW().Distance(point) > triangleThickness) // The winding order of the triangle plane does not matter.
return false; ///@todo The plane-point distance test is omitted in Real-Time Collision Detection. p. 25. A bug in the book?
float3 br = BarycentricUVW(point);
return br.y >= 0.f && br.z >= 0.f && (br.y + br.z) <= 1.f;
}
bool Triangle::Contains(const float3 &point, float triangleThickness) const
{
if (PlaneCCW().Distance(point) > triangleThickness) // The winding order of the triangle plane does not matter.
return false; ///@todo The plane-point distance test is omitted in Real-Time Collision Detection. p. 25. A bug in the book?
float3 br = BarycentricUVW(point);
return br.x >= -1e-3f && br.y >= -1e-3f && br.z >= -1e-3f; // Allow for a small epsilon to properly account for points very near the edges of the triangle.
}
bool Polygon::Intersects(const LineSegment &lineSegment) const
{
Plane plane = PlaneCCW();
float t;
bool intersects = Plane::IntersectLinePlane(plane.normal, plane.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
if (!intersects || t < 0.f || t > 1.f)
return false;
return Contains(lineSegment.GetPoint(t));
}
bool Polygon::IsPlanar(float epsilon) const
{
if (p.empty())
return false;
if (p.size() <= 3)
return true;
Plane plane = PlaneCCW();
for(size_t i = 3; i < p.size(); ++i)
if (plane.Distance(p[i]) > epsilon)
return false;
return true;
}
bool Polygon::IsSimple() const
{
assume(IsPlanar());
Plane plane = PlaneCCW();
for(int i = 0; i < (int)p.size(); ++i)
{
LineSegment si = plane.Project(Edge(i));
for(int j = i+2; j < (int)p.size(); ++j)
{
if (i == 0 && j == (int)p.size() - 1)
continue; // These two edges are consecutive and share a vertex. Don't check that pair.
LineSegment sj = plane.Project(Edge(j));
if (si.Intersects(sj))
return false;
}
}
return true;
}
bool Polygon::Intersects(const LineSegment &lineSegment) const
{
Plane plane = PlaneCCW();
// Compute line-plane intersection (unroll Plane::IntersectLinePlane())
float denom = Dot(plane.normal, lineSegment.b - lineSegment.a);
if (Abs(denom) < 1e-4f) // The plane of the polygon and the line are planar? Do the test in 2D.
return Intersects2D(LineSegment(POINT_VEC(MapTo2D(lineSegment.a), 0), POINT_VEC(MapTo2D(lineSegment.b), 0)));
// The line segment properly intersects the plane of the polygon, so there is exactly one
// point of intersection between the plane of the polygon and the line segment. Test that intersection point against
// the line segment end points.
float t = (plane.d - Dot(plane.normal, lineSegment.a)) / denom;
if (t < 0.f || t > 1.f)
return false;
return Contains(lineSegment.GetPoint(t));
}
float3 Polygon::ClosestPoint(const float3 &point) const
{
assume(IsPlanar());
float3 ptOnPlane = PlaneCCW().Project(point);
if (Contains(ptOnPlane))
return ptOnPlane;
float3 closestPt;
float closestDist = FLOAT_MAX;
for(int i = 0; i < NumEdges(); ++i)
{
float3 pt = Edge(i).ClosestPoint(point);
float d = pt.DistanceSq(point);
if (d < closestDist)
{
closestPt = pt;
closestDist = d;
}
}
return ptOnPlane;
}
bool Polygon::Contains(const LineSegment &worldSpaceLineSegment, float polygonThickness) const
{
if (p.size() < 3)
return false;
Plane plane = PlaneCCW();
if (plane.Distance(worldSpaceLineSegment.a) > polygonThickness ||
plane.Distance(worldSpaceLineSegment.b) > polygonThickness)
return false;
// For robustness, project onto the polygon plane.
LineSegment l = plane.Project(worldSpaceLineSegment);
if (!Contains(l.a) || !Contains(l.b))
return false;
for(int i = 0; i < (int)p.size(); ++i)
if (plane.Project(Edge(i)).Intersects(l))
return false;
return true;
}
float3 Polygon::EdgeNormal(int edgeIndex) const
{
return Cross(Edge(edgeIndex).Dir(), PlaneCCW().normal).Normalized();
}
bool Polygon::Contains(const vec &worldSpacePoint, float polygonThicknessSq) const
{
// Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/
if (p.size() < 3)
return false;
vec basisU = BasisU();
vec basisV = BasisV();
assert1(basisU.IsNormalized(), basisU);
assert1(basisV.IsNormalized(), basisV);
assert2(basisU.IsPerpendicular(basisV), basisU, basisV);
assert3(basisU.IsPerpendicular(PlaneCCW().normal), basisU, PlaneCCW().normal, basisU.Dot(PlaneCCW().normal));
assert3(basisV.IsPerpendicular(PlaneCCW().normal), basisV, PlaneCCW().normal, basisV.Dot(PlaneCCW().normal));
vec normal = basisU.Cross(basisV);
// float lenSq = normal.LengthSq(); ///\todo Could we treat basisU and basisV unnormalized here?
float dot = normal.Dot(vec(p[0]) - worldSpacePoint);
if (dot*dot > polygonThicknessSq)
return false;
int numIntersections = 0;
const float epsilon = 1e-4f;
// General strategy: transform all points on the polygon onto 2D face plane of the polygon, where the target query point is
// centered to lie in the origin.
// If the test ray (0,0) -> (+inf, 0) intersects exactly an odd number of polygon edge segments, then the query point must have been
// inside the polygon. The test ray is chosen like that to avoid all extra per-edge computations.
vec vt = vec(p.back()) - worldSpacePoint;
float2 p0 = float2(Dot(vt, basisU), Dot(vt, basisV));
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(int i = 0; i < (int)p.size(); ++i)
{
vt = vec(p[i]) - worldSpacePoint;
float2 p1 = float2(Dot(vt, basisU), Dot(vt, basisV));
if (Abs(p1.y) < epsilon)
p1.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 the line segment p0 -> p1 straddles the line x=0, it could intersect the ray (0,0) -> (+inf, 0)
{
if (Min(p0.x, p1.x) > 0.f) // If both x-coordinates are positive, then there certainly is an intersection with the ray.
++numIntersections;
else if (Max(p0.x, p1.x) > 0.f) // If one of them is positive, there could be an intersection. (otherwise both are negative and they can't intersect ray)
{
// 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 (d.y != 0.f)
{
float t = -p0.y / d.y; // The line segment parameter, t \in [0,1] forms the line segment p0->p1.
float x = p0.x + t * d.x; // The x-coordinate of intersection with the ray.
if (t >= 0.f && t <= 1.f && x > 0.f)
++numIntersections;
}
}
}
p0 = p1;
}
return numIntersections % 2 == 1;
}
bool Polygon::Contains(const float3 &worldSpacePoint, float polygonThickness) const
{
if (PlaneCCW().Distance(worldSpacePoint) > polygonThickness)
return false;
return Contains2D(MapTo2D(worldSpacePoint));
}
float3 Polygon::BasisV() const
{
if (p.size() < 2)
return float3::unitY;
return Cross(PlaneCCW().normal, BasisU()).Normalized();
}
Plane Polygon::PlaneCW() const
{
Plane plane = PlaneCCW();
plane.ReverseNormal();
return plane;
}
float3 Polygon::NormalCCW() const
{
///@todo Optimize temporaries.
return PlaneCCW().normal;
}
