using System; using UnityEngine; public class MathCM { public static float calcPointLineDist(ref Vector3 p, ref MathCM.Line l, out Vector3 h, out float t) { float sqrMagnitude = l.v.sqrMagnitude; t = 0f; if (sqrMagnitude > 0f) { t = Vector3.Dot(l.v, p - l.s) / sqrMagnitude; } h = l.s + t * l.v; return (h - p).magnitude; } public static bool isSharpAngle(ref Vector3 p1, ref Vector3 p2, ref Vector3 p3) { return Vector3.Dot(p1 - p2, p3 - p2) >= 0f; } public static float calcPointSegmentDist(ref Vector3 p, ref MathCM.Segment seg, out Vector3 h, out float t) { Vector3 e = seg.e; MathCM.Line line = new MathCM.Line(seg.s, e); float result = MathCM.calcPointLineDist(ref p, ref line, out h, out t); if (!MathCM.isSharpAngle(ref p, ref seg.s, ref e)) { h = seg.s; return (seg.s - p).magnitude; } if (!MathCM.isSharpAngle(ref p, ref e, ref seg.s)) { h = e; return (e - p).magnitude; } return result; } public static bool isParallel(ref Vector3 v1, ref Vector3 r) { float sqrMagnitude = Vector3.Cross(v1, r).sqrMagnitude; return -MathCM._OX_EPSILON_ < sqrMagnitude && sqrMagnitude < MathCM._OX_EPSILON_; } public static float calcLineLineDist(MathCM.Line l1, MathCM.Line l2, out Vector3 p1, out Vector3 p2, out float t1, out float t2) { if (MathCM.isParallel(ref l1.v, ref l2.v)) { float result = MathCM.calcPointLineDist(ref l1.s, ref l2, out p2, out t2); p1 = l1.s; t1 = 0f; return result; } float num = Vector3.Dot(l1.v, l2.v); float sqrMagnitude = l1.v.sqrMagnitude; float sqrMagnitude2 = l2.v.sqrMagnitude; Vector3 rhs = l1.s - l2.s; t1 = (num * Vector3.Dot(l2.v, rhs) - sqrMagnitude2 * Vector3.Dot(l1.v, rhs)) / (sqrMagnitude * sqrMagnitude2 - num * num); p1 = l1.getPoint(t1); t2 = Vector3.Dot(l2.v, p1 - l2.s) / sqrMagnitude2; p2 = l2.getPoint(t2); return (p2 - p1).magnitude; } public static float calcLineLineDistNoParallelCheck(ref MathCM.Segment l1, ref MathCM.Segment l2, out Vector3 p1, out Vector3 p2, out float t1, out float t2) { float num = Vector3.Dot(l1.v, l2.v); float sqrMagnitude = l1.v.sqrMagnitude; float sqrMagnitude2 = l2.v.sqrMagnitude; Vector3 rhs = l1.s - l2.s; t1 = (num * Vector3.Dot(l2.v, rhs) - sqrMagnitude2 * Vector3.Dot(l1.v, rhs)) / (sqrMagnitude * sqrMagnitude2 - num * num); p1 = l1.getPoint(t1); t2 = Vector3.Dot(l2.v, p1 - l2.s) / sqrMagnitude2; p2 = l2.getPoint(t2); return (p2 - p1).magnitude; } public static float calcSegmentSegmentDist(ref MathCM.Segment s1, ref MathCM.Segment s2, out Vector3 p1, out Vector3 p2, out float t1, out float t2) { if (s1.v.sqrMagnitude < MathCM._OX_EPSILON_) { float result; if (s2.v.sqrMagnitude < MathCM._OX_EPSILON_) { result = (s2.s - s1.s).magnitude; p1 = s1.s; p2 = s2.s; t1 = (t2 = 0f); return result; } result = MathCM.calcPointSegmentDist(ref s1.s, ref s2, out p2, out t2); p1 = s1.s; t1 = 0f; t2 = Mathf.Clamp01(t2); return result; } else { float result; if (s2.v.sqrMagnitude < MathCM._OX_EPSILON_) { result = MathCM.calcPointSegmentDist(ref s2.s, ref s1, out p1, out t1); p2 = s2.s; t1 = Mathf.Clamp01(t1); t2 = 0f; return result; } if (MathCM.isParallel(ref s1.v, ref s2.v)) { t1 = 0f; p1 = s1.s; result = MathCM.calcPointSegmentDist(ref s1.s, ref s2, out p2, out t2); if (0f <= t2 && t2 <= 1f) { return result; } } else { result = MathCM.calcLineLineDistNoParallelCheck(ref s1, ref s2, out p1, out p2, out t1, out t2); if (0f <= t1 && t1 <= 1f && 0f <= t2 && t2 <= 1f) { return result; } } t1 = Mathf.Clamp01(t1); p1 = s1.getPoint(t1); result = MathCM.calcPointSegmentDist(ref p1, ref s2, out p2, out t2); if (0f <= t2 && t2 <= 1f) { return result; } t2 = Mathf.Clamp01(t2); p2 = s2.getPoint(t2); result = MathCM.calcPointSegmentDist(ref p2, ref s1, out p1, out t1); if (0f <= t1 && t1 <= 1f) { return result; } t1 = Mathf.Clamp01(t1); p1 = s1.getPoint(t1); return (p2 - p1).magnitude; } } public static int hittest_point_polygon_3d(Vector3 A, Vector3 B, Vector3 C, Vector3 P) { Vector3 lhs = B - A; Vector3 rhs = P - B; Vector3 lhs2 = C - B; Vector3 rhs2 = P - C; Vector3 lhs3 = A - C; Vector3 rhs3 = P - A; Vector3 lhs4 = Vector3.Cross(lhs, rhs); Vector3 rhs4 = Vector3.Cross(lhs2, rhs2); Vector3 rhs5 = Vector3.Cross(lhs3, rhs3); double num = (double)Vector3.Dot(lhs4, rhs4); double num2 = (double)Vector3.Dot(lhs4, rhs5); if (num > 0.0 && num2 > 0.0) { return 0; } return 1; } public static bool TriangleRayIntersect(Vector3 Orig, Vector3 dir, Vector3 v0, Vector3 v1, Vector3 v2, ref float pRetT, ref float pRetU, ref float pRetV) { Vector3 vector = v1 - v0; Vector3 vector2 = v2 - v0; Vector3 rhs = Vector3.Cross(dir, vector2); float num = Vector3.Dot(vector, rhs); float num2; Vector3 rhs2; float num3; if ((double)num > 0.001) { Vector3 lhs = Orig - v0; num2 = Vector3.Dot(lhs, rhs); if (num2 < 0f || num2 > num) { return false; } rhs2 = Vector3.Cross(lhs, vector); num3 = Vector3.Dot(dir, rhs2); if ((double)num3 < 0.0 || num2 + num3 > num) { return false; } } else { if ((double)num >= -0.001) { return false; } Vector3 lhs = Orig - v0; num2 = Vector3.Dot(lhs, rhs); if ((double)num2 > 0.0 || num2 < num) { return false; } rhs2 = Vector3.Cross(lhs, vector); num3 = Vector3.Dot(dir, rhs2); if ((double)num3 > 0.0 || num2 + num3 < num) { return false; } } float num4 = 1f / num; float num5 = Vector3.Dot(vector2, rhs2); num5 *= num4; num2 *= num4; num3 *= num4; pRetT = num5; pRetU = num2; pRetV = num3; return true; } public static bool TriangleRayIntersect(Vector3 tvec, Vector3 dir, Vector3 v0, Vector3 e1, Vector3 e2, ref float pRetT) { Vector3 rhs = Vector3.Cross(dir, e2); float num = Vector3.Dot(e1, rhs); Vector3 rhs2; if ((double)num > 0.001) { float num2 = Vector3.Dot(tvec, rhs); if (num2 < 0f || num2 > num) { return false; } rhs2 = Vector3.Cross(tvec, e1); float num3 = Vector3.Dot(dir, rhs2); if ((double)num3 < 0.0 || num2 + num3 > num) { return false; } } else { if ((double)num >= -0.001) { return false; } float num2 = Vector3.Dot(tvec, rhs); if ((double)num2 > 0.0 || num2 < num) { return false; } rhs2 = Vector3.Cross(tvec, e1); float num3 = Vector3.Dot(dir, rhs2); if ((double)num3 > 0.0 || num2 + num3 < num) { return false; } } float num4 = 1f / num; float num5 = Vector3.Dot(e2, rhs2); num5 *= num4; pRetT = num5; return true; } public static bool TriangleLineIntersect(Vector3 src, Vector3 dest, Vector3 tvec, Vector3 v0, Vector3 e1, Vector3 e2, ref float pRetT) { Vector3 vector = dest - src; if (MathCM.TriangleRayIntersect(tvec, vector.normalized, v0, e1, e2, ref pRetT)) { float num = Mathf.Abs(pRetT); if (0f <= pRetT && num * num <= vector.sqrMagnitude) { return true; } } return false; } public static bool SphireRayIntersect(Vector3 orig, Vector3 dir, Vector3 center, float r, ref Vector3 intersect) { Vector3 vector = orig - center; float num = Vector3.Dot(vector, dir); float num2 = Vector3.Dot(vector, vector) - r * r; if ((double)num2 > 0.0 && (double)num > 0.0) { return false; } float num3 = num * num - num2; if ((double)num3 < 0.0) { return false; } float num4 = -num - Mathf.Sqrt(num3); if ((double)num4 < 0.0) { return false; } intersect = orig + num4 * dir; return true; } public static Vector3 NearPosOnPlane(Vector3 targetPos, Vector3 pointOnPlane, Vector3 normalPlane) { Vector3 rhs = targetPos - pointOnPlane; float d = Vector3.Dot(normalPlane, rhs); return targetPos - normalPlane * d; } public static bool PlaneSegmentIntersect(Plane plane, Vector3 start, Vector3 end, out Vector3 pointOnPlane) { float num = Vector3.Distance(start, end); Vector3 normalized = (end - start).normalized; Ray ray = new Ray(start, normalized); float num2 = 0f; if (plane.Raycast(ray, out num2) && num2 <= num) { pointOnPlane = start + normalized * num2; return true; } pointOnPlane = Vector3.zero; return false; } private static float _OX_EPSILON_ = 1E-06f; public struct Line { public Line(Vector3 fs, Vector3 fe) { this.s = fs; this.e = fe; this.v = fe - fs; } public Vector3 getPoint(float t) { return this.s + t * this.v; } public Vector3 s; public Vector3 e; public Vector3 v; } public struct Segment { public Segment(Vector3 fs, Vector3 fe) { this.s = fs; this.e = fe; this.v = fe - fs; } public Vector3 getPoint(float t) { return this.s + t * this.v; } public Vector3 s; public Vector3 e; public Vector3 v; } }