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