public static bool IntersectRayTriangle(Ray ray, LVector3 t1, LVector3 t2, LVector3 t3, out LVector3 intersection) { intersection = LVector3.zero; LVector3 edge1 = t2.sub(t1); LVector3 edge2 = t3.sub(t1); LVector3 pvec = ray.direction.cross(edge2); LFloat det = edge1.dot(pvec); if (IsZero(det)) { var p = new Plane(t1, t2, t3); if (p.testPoint(ray.origin) == PlaneSide.OnPlane && IsPointInTriangle(ray.origin, t1, t2, t3)) { intersection.set(ray.origin); return(true); } return(false); } det = 1 / det; LVector3 tvec = ray.origin.sub(t1); LFloat u = tvec.dot(pvec) * det; if (u < 0 || u > 1) { return(false); } LVector3 qvec = tvec.cross(edge1); LFloat v = ray.direction.dot(qvec) * det; if (v < 0 || u + v > 1) { return(false); } LFloat t = edge2.dot(qvec) * det; if (t < 0) { return(false); } if (t <= FLOAT_ROUNDING_ERROR) { intersection.set(ray.origin); } else { ray.getEndPoint(intersection, t); } return(true); }
public static bool IntersectSegmentPlane(LVector3 start, LVector3 end, Plane plane, LVector3 intersection) { LVector3 dir = end.sub(start); LFloat denom = dir.dot(plane.getNormal()); LFloat t = -(start.dot(plane.getNormal()) + plane.getD()) / denom; if (t < 0 || t > 1) { return(false); } intersection.set(start).Add(dir.scl(t)); return(true); }
/* * 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)); }