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 static float GetY(double x, double bx, double by, double cx, double cy)
{
return((float)BezierFunc.GetY(x, bx, by, cx, cy));
}