public LFloat Estimate(Triangle node, Triangle endNode) { LFloat dst2; LFloat minDst2 = LFloat.MaxValue; A_AB = (node.a).Add(node.b) * LFloat.half; A_AB = (node.b).Add(node.c) * LFloat.half; A_AB = (node.c).Add(node.a) * LFloat.half; B_AB = (endNode.a).Add(endNode.b) * LFloat.half; B_BC = (endNode.b).Add(endNode.c) * LFloat.half; B_CA = (endNode.c).Add(endNode.a) * LFloat.half; if ((dst2 = A_AB.dst2(B_AB)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_AB.dst2(B_BC)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_AB.dst2(B_CA)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_BC.dst2(B_AB)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_BC.dst2(B_BC)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_BC.dst2(B_CA)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_CA.dst2(B_AB)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_CA.dst2(B_BC)) < minDst2) { minDst2 = dst2; } if ((dst2 = A_CA.dst2(B_CA)) < minDst2) { minDst2 = dst2; } return((LFloat)LMath.Sqrt(minDst2)); }
/** Projects a point to a line segment. This implementation is thread-safe. */ public static LFloat nearestSegmentPointSquareDistance (LVector3 nearest, LVector3 start, LVector3 end, LVector3 point) { nearest.set(start); var abX = end.x - start.x; var abY = end.y - start.y; var abZ = end.z - start.z; var abLen2 = abX * abX + abY * abY + abZ * abZ; if (abLen2 > 0) // Avoid NaN due to the indeterminate form 0/0 { var t = ((point.x - start.x) * abX + (point.y - start.y) * abY + (point.z - start.z) * abZ) / abLen2; var s = LMath.Clamp01(t); nearest.x += abX * s; nearest.y += abY * s; nearest.z += abZ * s; } return(nearest.dst2(point)); }
/* * Find the closest point on the triangle, given a measure point. * This is the optimized algorithm taken from the book "Real-Time Collision Detection". * <p> * This implementation is NOT thread-safe. */ public static LFloat getClosestPointOnTriangle(LVector3 a, LVector3 b, LVector3 c, LVector3 p, ref LVector3 _out) { // Check if P in vertex region outside A var ab = b.sub(a); var ac = c.sub(a); var ap = p.sub(a); var d1 = ab.dot(ap); var d2 = ac.dot(ap); if (d1 <= 0 && d2 <= 0) { _out = a; return(p.dst2(a)); } // Check if P in vertex region outside B var bp = p.sub(b); var d3 = ab.dot(bp); var d4 = ac.dot(bp); if (d3 >= 0 && d4 <= d3) { _out = b; return(p.dst2(b)); } // Check if P in edge region of AB, if so return projection of P onto AB var vc = d1 * d4 - d3 * d2; if (vc <= 0 && d1 >= 0 && d3 <= 0) { var v = d1 / (d1 - d3); _out.set(a).mulAdd(ab, v); // barycentric coordinates (1-v,v,0) return(p.dst2(_out)); } // Check if P in vertex region outside C var cp = p.sub(c); var d5 = ab.dot(cp); var d6 = ac.dot(cp); if (d6 >= 0 && d5 <= d6) { _out = c; return(p.dst2(c)); } // Check if P in edge region of AC, if so return projection of P onto AC var vb = d5 * d2 - d1 * d6; if (vb <= 0 && d2 >= 0 && d6 <= 0) { var w = d2 / (d2 - d6); _out.set(a).mulAdd(ac, w); // barycentric coordinates (1-w,0,w) return(_out.dst2(p)); } // Check if P in edge region of BC, if so return projection of P onto BC var va = d3 * d6 - d5 * d4; if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { var w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); _out.set(b).mulAdd(c.sub(b), w); // barycentric coordinates (0,1-w,w) return(_out.dst2(p)); } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) var denom = 1 / (va + vb + vc); { LFloat v = vb * denom; LFloat w = vc * denom; _out.set(a).mulAdd(ab, v).mulAdd(ac, w); } return(_out.dst2(p)); }