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);
}
}
}
private void RotateTo(Vector3 ip, RotationContraintComponent bc)
{
// now swing not entire way but only to the curve
Vector3 toIp = ip - bc.transform.position;
toIp.Normalize();
Quaternion qs = SwingTo(bc.transform.up, toIp);
bc.transform.rotation = qs * bc.transform.rotation;
}
