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 void DrawProjectionPlane(PlanarBezierComponent comp, GizmoType gizmoType)
        {
            Vector3 c = comp.planeTransform.position;
            Vector3 r = 0.5f * comp.planeTransform.right * comp.planeTransform.localScale.x;
            Vector3 f = 0.5f * comp.planeTransform.forward * comp.planeTransform.localScale.z;

            Gizmos.DrawLine(c - r - f, c + r - f);
            Gizmos.DrawLine(c - r + f, c + r + f);
            Gizmos.DrawLine(c - r - f, c - r + f);
            Gizmos.DrawLine(c + r - f, c + r + f);

            Gizmos.color = Color.green;
            Gizmos.DrawSphere(comp.planeTransform.position, 0.1f);
        }
        /// <summary>
        /// Given a point on the plane transform, we draw a vector from center of the plane transform
        /// to the pop (point on plane) then use this to intersect the curve...
        /// </summary>
        /// <param name="pop">point on plane</param>
        /// <param name="bc">PlanarBezierComponent data</param>
        /// <returns>Array of vectors that intersect curve with ray</returns>
        public static Vector3[] IntersectCurve(Vector3 pop, PlanarBezierComponent bc)
        {
            List <Vector3> intersectionPoints = new List <Vector3>(2);

            /*  Steps of algo:
             *  1) Transform the curve and point on plane into plane space
             *  2) Get the ray going from the planeTransform position to the transformed pop
             *  3) Make the curve y value a function of t of the ray by rotating it
             *      by theta radians, which is the angle between [1,0] and v vectors
             *  4) analytic root find the intersections for each curve making the entire curve
             */
            Transform PT            = bc.planeTransform;
            Vector3   planeSpacePop = PT.InverseTransformPoint(pop);
            Vector2   v             = new Vector2(planeSpacePop.x, planeSpacePop.z);

            Vector2[] tpts  = GetPlaneSpaceCPs(bc.controlPoints, PT);
            float     angle = -Angle(v);

            RotatePoints(angle, tpts);
            float[] roots = new float[3];

            int CC = CurveCount(bc);

            for (int i = 0; i < CurveCount(bc); i++)
            {
                if (Roots(tpts, 4 * i, ref roots))
                {
                    if (roots[0] > 0)
                    {
                        Vector3 pofi = Evaluate(i + roots[0], bc);
                        intersectionPoints.Add(pofi);
                    }
                    if (roots[1] > 0)
                    {
                        Vector3 pofi = Evaluate(i + roots[1], bc);
                        intersectionPoints.Add(pofi);
                    }
                    if (roots[2] > 0)
                    {
                        Vector3 pofi = Evaluate(i + roots[2], bc);
                        intersectionPoints.Add(pofi);
                    }
                }
            }

            return(intersectionPoints.ToArray());
        }
        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);
        }
        /// <summary>
        /// Project the point p, onto plane of the PlanarBezierComponent
        /// </summary>
        /// <param name="p">point to project</param>
        /// <param name="pbc">PlanarBezierComponent</param>
        /// <returns>Point on the plane closest to p</returns>
        public static Vector3 ProjectPoint(Vector3 p, PlanarBezierComponent pbc)
        {
            Plane plane = new Plane(pbc.planeTransform.up, pbc.planeTransform.position);

            return(plane.ClosestPointOnPlane(p));
        }