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;
}
float3 Polygon::MapFrom2D(const float2 &point) const
{
assume(!p.empty());
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (p.empty())
return float3::nan;
#endif
return p[0] + point.x * BasisU() + point.y * BasisV();
}
float2 Polygon::MapTo2D(const float3 &point) const
{
assume(!p.empty());
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (p.empty())
return float2::nan;
#endif
float3 basisU = BasisU();
float3 basisV = BasisV();
float3 pt = point - p[0];
return float2(Dot(pt, basisU), Dot(pt, basisV));
}
bool Polygon::Intersects2D(const LineSegment &localSpaceLineSegment) const
{
if (p.size() < 3)
return false;
const vec basisU = BasisU();
const vec basisV = BasisV();
const vec origin = p[0];
LineSegment edge;
edge.a = POINT_VEC(Dot(p.back(), basisU), Dot(p.back(), basisV), 0); // map to 2D
for (int i = 0; i < (int)p.size(); ++i)
{
edge.b = POINT_VEC(Dot(p[i], basisU), Dot(p[i], basisV), 0); // map to 2D
if (edge.Intersects(localSpaceLineSegment))
return true;
edge.a = edge.b;
}
// The line segment did not intersect with any of the polygon edges, so either the whole line segment is inside
// the polygon, or it is fully outside the polygon. Test one point of the line segment to determine which.
return Contains(MapFrom2D(localSpaceLineSegment.a.xy()));
}
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;
}
float3 Circle::GetPoint(float angleRadians, float d) const
{
return pos + r * d * (Cos(angleRadians) * BasisU() + Sin(angleRadians) * BasisV());
}