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