public void Lerp()
{
ray = null;
if (!lerp)
{
n.position = end.position;
chain.target = n;
follow = true;
lerp = true;
}
else
{
var timeMultiplier = 3.6f;
var minSpeed = 0.12f;
if ((target.position - n.position).sqrMagnitude < Mathf.Pow(minSpeed, 2))
{
n.position = target.position;
return;
}
else
{
var lerp = Vector3.Lerp(n.position, target.position, Time.deltaTime * timeMultiplier);
var x = lerp - n.position;
if (x.sqrMagnitude < Mathf.Pow(minSpeed, 2))
{
n.position += x.normalized * minSpeed;
}
else
{
n.position = lerp;
}
}
}
}
public void DoReset()
{
chain = new IKBoneChain();
if (joints.Count == 0)
{
return;
}
ChangeChain();
joints[0].transform.localPosition = Vector3.zero;
joints[0].parent.transform.localPosition = Vector3.zero;
chain.end = end;
chain.target = target;
ScaleChain();
UpdateValues();
lerp = false;
follow = false;
index = 0;
ray = null;
}
public void SolveAll()
{
ray = null;
UpdateValues();
chain.Solve();
}
public void SolveStep()
{
UpdateValues();
ray = chain.Solve(index % chain.joints.Length);
index++;
}
/// <summary>
/// 计算骨骼链
/// </summary>
/// <param name="关节"></param>
/// <param name="目标位置"></param>
/// <param name="末端效应器"></param>
/// <param name="每次迭代最大旋转角度"></param>
/// <param name="迭代次数"></param>
public static WRay IKSolve(Joint joint, Transform target, Transform end, float limitAngle)
{
// 关节相对于目标位置和末端效应器的位置
Vector3 absJoint2End = end.position - joint.transform.position;
Vector3 absJoint2Target = target.position - joint.transform.position;
// 转为本地坐标系
Quaternion invRotation = joint.transform.rotation.Conjugate();
Vector3 localJoint2End = invRotation * absJoint2End;
Vector3 localJoint2Target = invRotation * absJoint2Target;
float deltaAngle = Mathf.Rad2Deg * Mathf.Acos(Vector3.Dot(localJoint2End.normalized, localJoint2Target.normalized));
if (CloseZero(deltaAngle))
{
return(null);
}
Vector3 rotateAxis = Vector3.Cross(localJoint2Target, localJoint2End).normalized;
Quaternion deltaRotation = Quaternion.AngleAxis(deltaAngle, rotateAxis);
deltaRotation = deltaRotation.Conjugate();
var ray = new WRay(joint.transform.position, rotateAxis, Color.green);
var delta = deltaRotation.eulerAngles;
var cur = joint.transform.localRotation.eulerAngles;
var deltaV = Vector3.zero;
if ((joint.constraints & RigidbodyConstraints.FreezeRotationX) == 0)
{
deltaV += new Vector3(limit(delta.x, limitAngle), 0f, 0f);
}
if ((joint.constraints & RigidbodyConstraints.FreezeRotationY) == 0)
{
deltaV += new Vector3(0f, limit(delta.y, limitAngle), 0f);
}
if ((joint.constraints & RigidbodyConstraints.FreezeRotationZ) == 0)
{
deltaV += new Vector3(0f, 0f, limit(delta.z, limitAngle));
}
deltaRotation.eulerAngles = deltaV;
joint.transform.localRotation *= deltaRotation;
var euler = joint.transform.localRotation.eulerAngles;
var quat = new Quaternion();
quat.eulerAngles = Vector3.zero;
if ((joint.constraints & RigidbodyConstraints.FreezeRotationX) == 0)
{
quat.eulerAngles += new Vector3(Mathf.Clamp(euler.x, joint.minAngle, joint.maxAngle), 0f, 0f);
}
if ((joint.constraints & RigidbodyConstraints.FreezeRotationY) == 0)
{
quat.eulerAngles += new Vector3(0f, Mathf.Clamp(euler.y, joint.minAngle, joint.maxAngle), 0f);
}
if ((joint.constraints & RigidbodyConstraints.FreezeRotationZ) == 0)
{
quat.eulerAngles += new Vector3(0f, 0f, Mathf.Clamp(euler.z, joint.minAngle, joint.maxAngle));
}
joint.transform.localRotation = quat;
return(ray);
}
