vec Polygon::PointOnEdge(float normalizedDistance) const
{
if (p.empty())
return vec::nan;
if (p.size() < 2)
return p[0];
normalizedDistance = Frac(normalizedDistance); // Take modulo 1 so we have the range [0,1[.
float perimeter = Perimeter();
float d = normalizedDistance * perimeter;
for(int i = 0; i < NumVertices(); ++i)
{
LineSegment edge = Edge(i);
float len = edge.Length();
assume(len != 0.f && "Degenerate Polygon detected!");
if (d <= len)
return edge.GetPoint(d / len);
d -= len;
}
mathassert(false && "Polygon::PointOnEdge reached end of loop which shouldn't!");
return p[0];
}
int Sphere::Intersects(const LineSegment &l, vec *intersectionPoint, vec *intersectionNormal, float *d, float *d2) const
{
float t1, t2;
int numIntersections = IntersectLine(l.a, l.Dir(), pos, r, t1, t2);
if (numIntersections == 0)
return 0;
float lineLength = l.Length();
if (t2 < 0.f || t1 > lineLength)
return 0;
vec hitPoint = l.GetPoint(t1 / lineLength);
if (intersectionPoint)
*intersectionPoint = hitPoint;
if (intersectionNormal)
*intersectionNormal = (hitPoint - pos).Normalized();
if (d)
*d = t1 / lineLength;
if (d2)
*d2 = t2 / lineLength;
return true;
}
float Ray::Distance(const LineSegment &other, float &d, float &d2) const
{
vec c = ClosestPoint(other, d, d2);
return c.Distance(other.GetPoint(d2));
}
bool Polygon_Intersects_Polygon(const Polygon &poly, const T &other, float polygonThickness)
{
Plane plane = poly.PlaneCCW();
Plane plane2 = other.PlaneCCW();
if (!plane.normal.Cross(plane2.normal).IsZero())
{
// General strategy: If two polygon/triangle objects intersect, one
// of them must have an edge that passes through the interior of the other object.
// Test each edge of the this object against intersection of the interior of the other polygon,
// and vice versa.
for(int i = 0; i < other.NumEdges(); ++i)
{
LineSegment lineSegment = other.Edge(i);
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)
continue;
if (poly.Contains(lineSegment.GetPoint(t)))
return true;
}
for(int i = 0; i < poly.NumEdges(); ++i)
{
LineSegment lineSegment = poly.Edge(i);
float t;
bool intersects = Plane::IntersectLinePlane(plane2.normal, plane2.d, lineSegment.a, lineSegment.b - lineSegment.a, t);
if (!intersects || t < 0.f || t > 1.f)
continue;
if (other.Contains(lineSegment.GetPoint(t)))
return true;
}
return false;
}
else // The two polygons are coplanar. Perform the intersection test in 2D.
{
float poly0Pos = plane.normal.Dot(poly.Vertex(0));
float poly1Pos = plane.normal.Dot(other.Vertex(0));
if (Abs(poly0Pos-poly1Pos) > polygonThickness)
return false;
if (other.Contains(poly.Vertex(0), FLOAT_INF) || poly.Contains(other.Vertex(0), FLOAT_INF))
return true;
vec basisU = plane.normal.Perpendicular();
vec basisV = plane.normal.AnotherPerpendicular();
vec pivot = poly.Vertex(0);
vec pt = poly.Vertex(poly.NumVertices()-1)-pivot;
float2 a1 = float2(basisU.Dot(pt), basisV.Dot(pt));
for(int i = 0; i < poly.NumVertices(); ++i)
{
pt = poly.Vertex(i)-pivot;
float2 a2 = float2(basisU.Dot(pt), basisV.Dot(pt));
pt = other.Vertex(other.NumVertices()-1)-pivot;
float2 b1 = float2(basisU.Dot(pt), basisV.Dot(pt));
for(int j = 0; j < other.NumVertices(); ++j)
{
pt = other.Vertex(j)-pivot;
float2 b2 = float2(basisU.Dot(pt), basisV.Dot(pt));
float s, t;
if (LineSegment2DLineSegment2DIntersect(a1, a2-a1, b1, b2-b1, s, t))
return true;
b1 = b2;
}
a1 = a2;
}
return false;
}
}
///\todo Enable this codepath. This if rom Geometric Tools for Computer Graphics,
/// but the algorithm in the book is broken and does not take into account the
/// direction of the gradient to determine the proper region of intersection.
/// Instead using a slower code path above.
/// [groupSyntax]
float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const
{
float3 e0 = b - a;
float3 e1 = c - a;
float3 v_p = a - lineSegment.a;
float3 d = lineSegment.b - lineSegment.a;
// Q(u,v) = a + u*e0 + v*e1
// L(t) = ls.a + t*d
// Minimize the distance |Q(u,v) - L(t)|^2 under u >= 0, v >= 0, u+v <= 1, t >= 0, t <= 1.
float v_p_dot_e0 = Dot(v_p, e0);
float v_p_dot_e1 = Dot(v_p, e1);
float v_p_dot_d = Dot(v_p, d);
float3x3 m;
m[0][0] = Dot(e0, e0); m[0][1] = Dot(e0, e1); m[0][2] = -Dot(e0, d);
m[1][0] = m[0][1]; m[1][1] = Dot(e1, e1); m[1][2] = -Dot(e1, d);
m[2][0] = m[0][2]; m[2][1] = m[1][2]; m[2][2] = Dot(d, d);
float3 B(-v_p_dot_e0, -v_p_dot_e1, v_p_dot_d);
float3 uvt;
bool success = m.SolveAxb(B, uvt);
if (!success)
{
float t1, t2, t3;
float s1, s2, s3;
LineSegment e1 = Edge(0);
LineSegment e2 = Edge(1);
LineSegment e3 = Edge(2);
float d1 = e1.Distance(lineSegment, &t1, &s1);
float d2 = e2.Distance(lineSegment, &t2, &s2);
float d3 = e3.Distance(lineSegment, &t3, &s3);
if (d1 < d2 && d1 < d3)
{
if (otherPt)
*otherPt = lineSegment.GetPoint(s1);
return e1.GetPoint(t1);
}
else if (d2 < d3)
{
if (otherPt)
*otherPt = lineSegment.GetPoint(s2);
return e2.GetPoint(t2);
}
else
{
if (otherPt)
*otherPt = lineSegment.GetPoint(s3);
return e3.GetPoint(t3);
}
}
if (uvt.x < 0.f)
{
// Clamp to u == 0 and solve again.
float m_00 = m[2][2];
float m_01 = -m[1][2];
float m_10 = -m[2][1];
float m_11 = m[1][1];
float det = m_00 * m_11 - m_01 * m_10;
float v = m_00 * B[1] + m_01 * B[2];
float t = m_10 * B[1] + m_11 * B[2];
v /= det;
t /= det;
if (v < 0.f)
{
// Clamp to v == 0 and solve for t.
t = B[2] / m[2][2];
t = Clamp01(t); // The solution for t must also be in the range [0,1].
// The solution is (u,v,t)=(0,0,t).
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return a;
}
else if (v > 1.f)
{
// Clamp to v == 1 and solve for t.
t = (B[2] - m[2][1]) / m[2][2];
t = Clamp01(t);
// The solution is (u,v,t)=(0,1,t).
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return c; // == a + v*e1
}
else if (t < 0.f)
{
// Clamp to t == 0 and solve for v.
v = B[1] / m[1][1];
// mathassert(EqualAbs(v, Clamp01(v)));
v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
// The solution is (u,v,t)=(0,v,0).
if (otherPt)
*otherPt = lineSegment.a;
return a + v * e1;
}
else if (t > 1.f)
{
// Clamp to t == 1 and solve for v.
v = (B[1] - m[1][2]) / m[1][1];
// mathassert(EqualAbs(v, Clamp01(v)));
v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
// The solution is (u,v,t)=(0,v,1).
if (otherPt)
*otherPt = lineSegment.b;
return a + v * e1;
}
else
{
// The solution is (u,v,t)=(0,v,t).
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return a + v * e1;
}
}
else if (uvt.y < 0.f)
{
// Clamp to v == 0 and solve again.
float m_00 = m[2][2];
float m_01 = -m[0][2];
float m_10 = -m[2][0];
float m_11 = m[0][0];
float det = m_00 * m_11 - m_01 * m_10;
float u = m_00 * B[0] + m_01 * B[2];
float t = m_10 * B[0] + m_11 * B[2];
u /= det;
t /= det;
if (u < 0.f)
{
// Clamp to u == 0 and solve for t.
t = B[2] / m[2][2];
t = Clamp01(t); // The solution for t must also be in the range [0,1].
// The solution is (u,v,t)=(0,0,t).
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return a;
}
else if (u > 1.f)
{
// Clamp to u == 1 and solve for t.
t = (B[2] - m[2][0]) / m[2][2];
t = Clamp01(t); // The solution for t must also be in the range [0,1].
// The solution is (u,v,t)=(1,0,t).
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return b;
}
else if (t < 0.f)
{
// Clamp to t == 0 and solve for u.
u = B[0] / m[0][0];
// mathassert(EqualAbs(u, Clamp01(u)));
u = Clamp01(u); // The solution for u must also be in the range [0,1].
if (otherPt)
*otherPt = lineSegment.a;
return a + u * e0;
}
else if (t > 1.f)
{
// Clamp to t == 1 and solve for u.
u = (B[0] - m[0][2]) / m[0][0];
// mathassert(EqualAbs(u, Clamp01(u)));
u = Clamp01(u); // The solution for u must also be in the range [0,1].
if (otherPt)
*otherPt = lineSegment.b;
return a + u * e0;
}
else
{
// The solution is (u, 0, t).
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return a + u * e0;
}
}
else if (uvt.z < 0.f)
{
if (otherPt)
*otherPt = lineSegment.a;
// Clamp to t == 0 and solve again.
float m_00 = m[1][1];
float m_01 = -m[0][1];
float m_10 = -m[1][0];
float m_11 = m[0][0];
float det = m_00 * m_11 - m_01 * m_10;
float u = m_00 * B[0] + m_01 * B[1];
float v = m_10 * B[0] + m_11 * B[1];
u /= det;
v /= det;
if (u < 0.f)
{
// Clamp to u == 0 and solve for v.
v = B[1] / m[1][1];
v = Clamp01(v);
return a + v*e1;
}
else if (v < 0.f)
{
// Clamp to v == 0 and solve for u.
u = B[0] / m[0][0];
u = Clamp01(u);
return a + u*e0;
}
else if (u+v > 1.f)
{
// Set v = 1-u and solve again.
// u = (B[0] - m[0][0]) / (m[0][0] - m[0][1]);
// mathassert(EqualAbs(u, Clamp01(u)));
// u = Clamp01(u); // The solution for u must also be in the range [0,1].
// return a + u*e0;
// Clamp to v = 1-u and solve again.
float m_00 = m[2][2];
float m_01 = m[1][2] - m[0][2];
float m_10 = m_01;
float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
float det = m_00 * m_11 - m_01 * m_10;
float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
float b1 = -m[1][2] + v_p_dot_d;
float u = m_00 * b0 + m_01 * b1;
u /= det;
u = Clamp01(u);
float t = m_10 * b0 + m_11 * b1;
t /= det;
t = Clamp01(t);
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
return a + u*e0 + (1.f-u)*e1;
}
else
{
// The solution is (u, v, 0)
return a + u * e0 + v * e1;
}
}
else if (uvt.z > 1.f)
{
if (otherPt)
*otherPt = lineSegment.b;
// Clamp to t == 1 and solve again.
float m_00 = m[1][1];
float m_01 = -m[0][1];
float m_10 = -m[1][0];
float m_11 = m[0][0];
float det = m_00 * m_11 - m_01 * m_10;
float u = m_00 * (B[0]-m[0][2]) + m_01 * (B[1]-m[1][2]);
float v = m_10 * (B[0]-m[0][2]) + m_11 * (B[1]-m[1][2]);
u /= det;
v /= det;
if (u < 0.f)
{
// Clamp to u == 0 and solve again.
v = (B[1] - m[1][2]) / m[1][1];
v = Clamp01(v);
return a + v*e1;
}
else if (u > 1.f)
{
// Clamp to u == 1 and solve again.
v = (B[1] - m[1][0] - m[1][2]) / m[1][1];
v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above?
// The solution is (u,v,t)=(1,v,1).
return a + e0 + v*e1;
}
else if (u+v > 1.f)
{
// Set v = 1-u and solve again.
// Q(u,1-u) = a + u*e0 + e1 - u*e1 = a+e1 + u*(e0-e1)
// L(1) = ls.a + t*d = ls.b
// Minimize the distance |Q(u,1-u) - L(1)| = |a+e1+ls.b + u*(e0-e1)|
// |K + u*(e0-e1)|^2 = (K,K) + 2*u(K,e0-e1) + u^2 * (e0-e1,e0-e1)
// grad = 2*(K,e0-e1) + 2*u*(e0-e1,e0-e1) == 0
// u == (K,e1-e0) / (e0-e1,e0-e1)
u = (B[0] - m[0][1] - m[0][2]) / (m[0][0] - m[0][1]);
// u = Dot(a + e1 + lineSegment.b, e1 - e0) / Dot(e0-e1, e0-e1);
// mathassert(EqualAbs(u, Clamp01(u)));
u = Clamp01(u);
return a + u*e0 + (1-u)*e1;
}
else
{
// The solution is (u, v, 1)
return a + u*e0 + v*e1;
}
}
else if (uvt.x + uvt.y > 1.f)
{
// Clamp to v = 1-u and solve again.
float m_00 = m[2][2];
float m_01 = m[1][2] - m[0][2];
float m_10 = m_01;
float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1];
float det = m_00 * m_11 - m_01 * m_10;
float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0;
float b1 = -m[1][2] + v_p_dot_d;
float u = m_00 * b0 + m_01 * b1;
float t = m_10 * b0 + m_11 * b1;
u /= det;
t /= det;
t = Clamp01(t);
if (otherPt)
*otherPt = lineSegment.GetPoint(t);
if (u < 0.f)
{
// The solution is (u,v,t)=(0,1,t)
return c;
}
if (u > 1.f)
{
// The solution is (u,v,t)=(1,0,t)
return b;
}
mathassert(t >= 0.f);
mathassert(t <= 1.f);
return a + u*e0 + (1.f-u)*e1;
}
else // All parameters are within range, so the triangle and the line segment intersect, and the intersection point is the closest point.
{
if (otherPt)
*otherPt = lineSegment.GetPoint(uvt.z);
return a + uvt.x * e0 + uvt.y * e1;
}
}
