private Vector3 Clamp(PathEdge edge, Vector3 pos) { Vector3 p00 = edge.P00(); Vector3 p01 = edge.P01(); Vector3 p10 = edge.P10(); Vector3 p11 = edge.P11(); float minx = Mathf.Min(p00.x, p01.x, p10.x, p11.x); float maxx = Mathf.Max(p00.x, p01.x, p10.x, p11.x); float miny = Mathf.Min(p00.y, p01.y, p10.y, p11.y); float maxy = Mathf.Max(p00.y, p01.y, p10.y, p11.y); float minz = Mathf.Min(p00.z, p01.z, p10.z, p11.z); float maxz = Mathf.Max(p00.z, p01.z, p10.z, p11.z); float x = Mathf.Min(maxx, Mathf.Max(minx, pos.x)); float y = Mathf.Min(maxy, Mathf.Max(miny, pos.y)); float z = Mathf.Min(maxz, Mathf.Max(minz, pos.z)); return(new Vector3(x, y, z)); }
private PointToEdgeDistance Distance(PathEdge e, Vector3 pos) { Vector3 p0 = e.P0().transform.position; Vector3 p1 = e.P1().transform.position; if (e.positiveAxisExpand.magnitude > 0.01f || e.minusAxisExpand.magnitude > 0.01f) { // it's a plane Vector3 p2; if (e.positiveAxisExpand.magnitude > e.minusAxisExpand.magnitude) { p2 = p0 + e.positiveAxisExpand; } else { p2 = p0 - e.minusAxisExpand; } Plane plane = new Plane(p0, p1, p2); Vector3 pp = plane.ClosestPointOnPlane(pos); Vector3 pp0 = e.P00(); Vector3 pp1 = e.P01(); Vector3 pp2 = e.P11(); Vector3 pp3 = e.P10(); //Debug.Log(Vector3.Distance(pp, d0) + Vector3.Distance(pp, d1) + " " + Vector3.Distance(d0, d2) + Vector3.Distance(d1, d2)); return(new PointToEdgeDistance(Mathf.Abs(plane.GetDistanceToPoint(pos)), Vector3.Dot(Vector3.Cross(pp1 - pp0, pp - pp0), Vector3.Cross(pp3 - pp2, pp - pp2)) < 0 || Vector3.Dot(Vector3.Cross(pp0 - pp3, pp - pp3), Vector3.Cross(pp2 - pp1, pp - pp1)) < 0)); } else { // it's a line Vector3 p0p1 = p1 - p0; Vector3 p0pos = pos - p0; return(new PointToEdgeDistance(Vector3.Cross(p0p1, p0pos).magnitude / p0p1.magnitude, Vector3.Dot(p0p1, p0pos) < 0)); } }