void Update()
{
var tarPo = _target.position;
var oriPo = _origin.position;
var oriUp = _origin.up;
var oriFw = _origin.forward;
// test code STARTS here -----------------------------------------------
var distToPlane =
fun.point.IsBelowPlane(in tarPo, in oriFw, in oriPo)
? 0
: fun.point.DistanceToPlane(in tarPo, in oriFw, in oriPo);
var lenAll = (float)(len1 + len2 + len1);
var relDistToPlane = distToPlane / lenAll;
Vector3 oriRt;
fun.vector.GetNormal(in oriFw, in oriUp, out oriRt);
Vector3 tarPoOnPlane;
fun.point.ProjectOnPlane(in tarPo, in oriRt, in oriPo, out tarPoOnPlane);
tarPo = Vector3.Lerp(tarPoOnPlane, tarPo, (float)BezierFunc.GetY(relDistToPlane, 0.20, 0.00, 0.00, 1.00));
Vector3 j0, j1;
fun.inverseKinematics.ThreeJoinOnVertPlane(oriPo, oriFw, oriUp, tarPo, len1, len2, out j0, out j1);
// test code ENDS here -------------------------------------------------
var toJ0 = (j0 - oriPo).normalized;
var toJ1 = (j1 - j0).normalized;
var toTarg = (tarPo - j1).normalized;
_join1.rotation = Quaternion.LookRotation(toJ0, toJ0.GetRealUp(oriUp, oriFw));
Debug.DrawLine(_join1.position + Vector3.one * 0.02f, _join1.position + Vector3.one * 0.02f + oriFw * 1f, Color.blue);
Debug.DrawLine(_join1.position + Vector3.one * 0.02f, _join1.position + Vector3.one * 0.02f + oriUp * 1f, Color.green);
Debug.DrawLine(_join1.position, _join1.position + _join1.forward * 0.1f, Color.blue);
Debug.DrawLine(_join1.position, _join1.position + _join1.up * 0.1f, Color.green);
_join2.rotation = Quaternion.LookRotation(toJ1, toJ1.GetRealUp(_join1.rotation * Vector3.up, _join1.rotation * Vector3.forward));
_join2.position = j0;
_join3.rotation = Quaternion.LookRotation(toTarg, toTarg.GetRealUp(_join2.rotation * Vector3.up, _join2.rotation * Vector3.forward));
_join3.position = j1;
}
public BezierVector3(BezierFunc <float> x, BezierFunc <float> y, BezierFunc <float> z)
{
this.x = x;
this.y = y;
this.z = z;
}
public static float GetY(double x, double bx, double by, double cx, double cy)
{
return((float)BezierFunc.GetY(x, bx, by, cx, cy));
}
public static Vector3 GetPoint(double x, Vector3 start, Vector3 control, Vector3 end)
{
return(BezierFunc.GetPointQuadratic(x, start, control, end));
}
public static Vector3 GetPoint2D(double x, Vector2 start, Vector2 control, Vector2 end)
{
return(BezierFunc.GetPointQuadratic2D(x, start, control, end));
}
public static Vector3 GetPoint(double x, Vector3 start, Vector3 control1, Vector3 control2, Vector3 end)
{
return(BezierFunc.GetPointCubic(x, start, control1, control2, end));
}
public static Vector3 GetPoint2D(double x, Vector2 start, Vector2 control1, Vector2 control2, Vector2 end)
{
return(BezierFunc.GetPointCubic2D(x, start, control1, control2, end));
}
