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));
}
}