public void Forward(float value)
{
var vec = _root.Vector + _mid.Vector;
var target = vec.magnitude + value;
if (target > _root.Vector.magnitude + _mid.Vector.magnitude)
{
return;
}
if (Fix())
{
return;
}
// 利用三角函数计算出要移动的角度，相对法线进行移动
{
var old = Kit.Angle(_root.Vector.magnitude, vec.magnitude, _mid.Vector.magnitude);
var now = Kit.Angle(_root.Vector.magnitude, target, _mid.Vector.magnitude);
var angle = old - now;
var axis = Vector3.Cross(_root.Vector, _mid.Vector).normalized;
_root.RotateAround(_root.Transform.position, axis, angle);
}
{
var old = Kit.Angle(_root.Vector.magnitude, _mid.Vector.magnitude, vec.magnitude);
var now = Kit.Angle(_root.Vector.magnitude, _mid.Vector.magnitude, target);
var angle = old - now;
var axis = Vector3.Cross(_root.Vector, _mid.Vector).normalized;
_mid.RotateAround(_mid.Transform.position, axis, angle);
}
}
public void MoveTo(Vector3 pos)
{
var target = pos - _root.Transform.position;
var max = _root.Vector.magnitude + _end.Vector.magnitude;
if (max < target.magnitude)
{
// Tracer.Log("Reach Max");
return;
}
var angle = Vector3.Angle(Vector, target);
var axis = Vector3.Cross(Vector, target).normalized;
_root.RotateAround(_root.Transform.position, axis, angle);
Forward(target.magnitude - Vector.magnitude);
}
public static void Normals(IFkJoint root, IFkJoint end, float angle)
{
var vec = root.Vector + end.Vector;
var tan = Vector3.Project(root.Vector, vec);
var nor = root.Vector - tan;
root.RotateAround(root.Transform.position, nor, angle);
}
public void MoveTo(Vector3 pos)
{
var target = pos - _root.Transform.position;
var max = _root.Vector.magnitude + _mid.Vector.magnitude;
if (max < target.magnitude)
{
return;
}
var oldRot = _end.Transform.rotation;
var angle = Vector3.Angle(Vector, target);
var axis = Vector3.Cross(Vector, target).normalized;
_root.RotateAround(_root.Transform.position, axis, angle);
Forward(target.magnitude - Vector.magnitude);
_end.TurnTo(oldRot);
}
public static void MoveTo(IFkJoint root, IFkJoint end, Vector3 pos)
{
var vec = root.Vector + end.Vector;
var target = pos - root.Transform.position;
var max = root.Vector.magnitude + end.Vector.magnitude;
if (max < target.magnitude)
{
return;
}
var angle = Vector3.Angle(vec, target);
var axis = Vector3.Cross(vec, target).normalized;
root.RotateAround(root.Transform.position, axis, angle);
Forward(root, end, target.magnitude - vec.magnitude);
}
public static void Forward(IFkJoint root, IFkJoint end, float value)
{
var vec = root.Vector + end.Vector;
var target = vec.magnitude + value;
if (target > root.Vector.magnitude + end.Vector.magnitude)
{
return;
}
{
var old = Kit.Angle(root.Vector.magnitude, vec.magnitude, end.Vector.magnitude);
var now = Kit.Angle(root.Vector.magnitude, target, end.Vector.magnitude);
var angle = old - now;
var axis = Vector3.Cross(root.Vector, end.Vector).normalized;
root.RotateAround(root.Transform.position, axis, angle);
}
{
var old = Kit.Angle(root.Vector.magnitude, end.Vector.magnitude, vec.magnitude);
var now = Kit.Angle(root.Vector.magnitude, end.Vector.magnitude, target);
var angle = old - now;
var axis = Vector3.Cross(root.Vector, end.Vector).normalized;
end.RotateAround(end.Transform.position, axis, angle);
}
}
public static void Tangent(IFkJoint root, IFkJoint end, float angle)
{
var axis = Vector3.Cross(root.Vector, end.Vector);
root.RotateAround(root.Transform.position, axis, angle);
}
public static void Revolution(IFkJoint root, IFkJoint end, float angle)
{
var vec = root.Vector + end.Vector;
root.RotateAround(root.Transform.position, vec, angle);
}