/// [groupSyntax]
float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const
{
///@todo The Triangle-LineSegment test is naive. Optimize!
float3 closestToA = ClosestPoint(lineSegment.a);
float3 closestToB = ClosestPoint(lineSegment.b);
float d;
float3 closestToSegment = ClosestPointToTriangleEdge(lineSegment, 0, 0, &d);
float3 segmentPt = lineSegment.GetPoint(d);
float distA = closestToA.DistanceSq(lineSegment.a);
float distB = closestToB.DistanceSq(lineSegment.b);
float distC = closestToSegment.DistanceSq(segmentPt);
if (distA <= distB && distA <= distC)
{
if (otherPt)
*otherPt = lineSegment.a;
return closestToA;
}
else if (distB <= distC)
{
if (otherPt)
*otherPt = lineSegment.b;
return closestToB;
}
else
{
if (otherPt)
*otherPt = segmentPt;
return closestToSegment;
}
}
float3 Triangle::ClosestPoint(const Line &line, float3 *otherPt) const
{
///\todo Optimize this function.
float3 intersectionPoint;
if (Intersects(line, 0, &intersectionPoint))
{
if (otherPt)
*otherPt = intersectionPoint;
return intersectionPoint;
}
float u1,v1,d1;
float3 pt1 = ClosestPointToTriangleEdge(line, &u1, &v1, &d1);
if (otherPt)
*otherPt = line.GetPoint(d1);
return pt1;
}
float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const
{
///\todo Optimize.
float3 intersectionPoint;
if (Intersects(lineSegment, 0, &intersectionPoint))
{
if (otherPt)
*otherPt = intersectionPoint;
return intersectionPoint;
}
float u1,v1,d1;
float3 pt1 = ClosestPointToTriangleEdge(lineSegment, &u1, &v1, &d1);
float3 pt2 = ClosestPoint(lineSegment.a);
float3 pt3 = ClosestPoint(lineSegment.b);
float D1 = pt1.DistanceSq(lineSegment.GetPoint(d1));
float D2 = pt2.DistanceSq(lineSegment.a);
float D3 = pt3.DistanceSq(lineSegment.b);
if (D1 <= D2 && D1 <= D3)
{
if (otherPt)
*otherPt = lineSegment.GetPoint(d1);
return pt1;
}
else if (D2 <= D3)
{
if (otherPt)
*otherPt = lineSegment.a;
return pt2;
}
else
{
if (otherPt)
*otherPt = lineSegment.b;
return pt3;
}
}
/// [groupSyntax]
float3 Triangle::ClosestPoint(const Line &other, float *outU, float *outV, float *outD) const
{
/** The implementation of the Triangle-Line test is based on the pseudo-code in
Schneider, Eberly. Geometric Tools for Computer Graphics pp. 433 - 441. */
///@todo The Triangle-Line code is currently untested. Run tests to ensure the following code works properly.
// Point on triangle: T(u,v) = a + u*b + v*c;
// Point on line: L(t) = p + t*d;
// Minimize the function Q(u,v,t) = ||T(u,v) - L(t)||.
float3 e0 = b-a;
float3 e1 = c-a;
float3 d = other.dir;
const float d_e0e0 = Dot(e0, e0);
const float d_e0e1 = Dot(e0, e1);
const float d_e0d = Dot(e0, d);
const float d_e1e1 = Dot(e1, e1);
const float d_e1d = Dot(e1, d);
const float d_dd = Dot(d, d);
float3x3 m;
m[0][0] = d_e0e0;
m[0][1] = d_e0e1;
m[0][2] = -d_e0d;
m[1][0] = d_e0e1;
m[1][1] = d_e1e1;
m[1][2] = -d_e1d;
m[2][0] = -d_e0d;
m[2][1] = -d_e1d;
m[2][2] = d_dd;
///@todo Add optimized float3x3::InverseSymmetric().
bool inv = m.Inverse();
if (!inv)
return ClosestPointToTriangleEdge(other, outU, outV, outD);
float3 v_m_p = a - other.pos;
float v_m_p_e0 = v_m_p.Dot(e0);
float v_m_p_e1 = v_m_p.Dot(e1);
float v_m_p_d = v_m_p.Dot(d);
float3 b = float3(-v_m_p_e0, -v_m_p_e1, v_m_p_d);
float3 uvt = m * b;
// We cannot simply clamp the solution to (uv) inside the constraints, since the expression we
// are minimizing is quadratic.
// So, examine case-by-case which part of the space the solution lies in. Because the function is convex,
// we can clamp the search space to the boundary planes.
float u = uvt.x;
float v = uvt.y;
float t = uvt.z;
if (u < 0)
{
if (outU) *outU = 0;
// Solve 2x2 matrix for the (v,t) solution when u == 0.
float m00 = m[2][2];
float m01 = -m[2][1];
float m10 = -m[1][2];
float m11 = m[1][1];
/// @bug This variable should be used somewhere below? Review the code, and test!
float det = m00*m11 - m01*m10;
// 2x2 * 2 matrix*vec mul.
v = m00*b[1] + m01*b[2];
t = m10*b[1] + m11*b[2];
if (outD) *outD = t;
// Check if the solution is still out of bounds.
if (v <= 0)
{
if (outV) *outV = 0;
t = v_m_p_d / d_dd;
return Point(0, 0);
}
else if (v >= 1)
{
if (outV) *outV = 1;
t = (v_m_p_d - d_e1d) / d_dd;
return Point(0, 1);
}
else // (0 <= v <= 1).
{
if (outV) *outV = v;
return Point(0, v);
}
}
else if (v <= 0)
{
if (outV) *outV = 0;
// Solve 2x2 matrix for the (u,t) solution when v == 0.
float m00 = m[2][2];
float m01 = -m[2][0];
float m10 = -m[0][2];
float m11 = m[0][0];
float det = 1.f / (m00*m11 - m01*m10);
// 2x2 * 2 matrix*vec mul.
u = (m00*b[0] + m01*b[2]) * det;
t = (m10*b[0] + m11*b[2]) * det;
if (outD) *outD = t;
// Check if the solution is still out of bounds.
if (u <= 0)
{
if (outU) *outU = 0;
t = v_m_p_d / d_dd;
return Point(0, 0);
}
else if (u >= 1)
{
if (outU) *outU = 1;
t = (v_m_p_d - d_e0d) / d_dd;
return Point(1, 0);
}
else // (0 <= u <= 1).
{
if (outU) *outU = u;
return Point(u, 0);
}
}
else if (u + v >= 1.f)
{
// Set v = 1-u.
float m00 = d_e0e0 + d_e1e1 - 2.f * d_e0e1;
float m01 = -d_e0d + d_e1d;
float m10 = -d_e0d + d_e1d;
float m11 = d_dd;
float det = 1.f / (m00*m11 - m01*m10);
float b0 = d_e1e1 - d_e0e1 + v_m_p_e0 - v_m_p_e1;
float b1 = d_e1d + v_m_p_d;
// Inverse 2x2 matrix.
Swap(m00, m11);
Swap(m01, m10);
m01 = -m01;
m10 = -m10;
// 2x2 * 2 matrix*vec mul.
u = (m00*b0 + m01*b1) * det;
t = (m10*b0 + m11*b1) * det;
if (outD) *outD = t;
// Check if the solution is still out of bounds.
if (u <= 0)
{
if (outU) *outU = 0;
if (outV) *outV = 1;
t = (d_e1d + v_m_p_d) / d_dd;
return Point(0, 1);
}
else if (u >= 1)
{
if (outU) *outU = 1;
if (outV) *outV = 0;
t = (v_m_p_d + d_e0d) / d_dd;
return Point(1, 0);
}
else // (0 <= u <= 1).
{
if (outU) *outU = u;
if (outV) *outV = 1.f - u;
return Point(u, 1.f - u);
}
}
else // Each u, v and t are in appropriate range.
{
if (outU) *outU = u;
if (outV) *outV = v;
if (outD) *outD = t;
return Point(u, v);
}
}
