private void ContrainRotation(RotationContraintComponent rcc)
{
Ray ray = new Ray();
Plane plane = new Plane();
PlanarBezierComponent pbc = rcc.pbc;
ray.origin = rcc.transform.position;
ray.direction = rcc.transform.up;
plane.SetNormalAndPosition(pbc.planeTransform.up, pbc.planeTransform.position);
float t;
if (plane.Raycast(ray, out t))
{
Vector3 ip = ray.origin + t * ray.direction;
Debug.DrawLine(rcc.transform.position, ip, Color.blue);
Debug.DrawLine(pbc.planeTransform.position, ip, Color.green);
Debug.DrawLine(pbc.planeTransform.position, rcc.transform.position, Color.green);
Vector3 poc = PlanarBezierSystem.IntersectCurveNearestForward(ip, pbc);
Debug.DrawLine(rcc.transform.position, poc, Color.cyan);
Vector3 v = ip - pbc.planeTransform.position;
Vector3 w = poc - pbc.planeTransform.position;
if (v.sqrMagnitude > w.sqrMagnitude)
{
RotateTo(poc, rcc);
}
}
}
public static Vector3 IntersectCurveNearestForward(Vector3 pop, PlanarBezierComponent bc)
{
Vector3[] intersections = PlanarBezierSystem.IntersectCurve(pop, bc);
Vector3 v = (pop - bc.planeTransform.position);
v.Normalize();
Vector3 w, k = Vector3.zero;
float cd = Mathf.Infinity;
for (int i = 0; i < intersections.Length; i++)
{
w = intersections[i] - bc.planeTransform.position;
float dot = Vector3.Dot(v, w);
if (dot > 0 && dot < cd)
{
cd = dot;
k = intersections[i];
}
}
return(k);
}