/// <summary>
/// calc the init twist
/// refer: shard/s29/nl/3057637/eb34b758-7e54-4f3d-9cec-2e5bcebe09e3
/// </summary>
public void CalcInitData()
{
Dbg.CAssert(this, m_nextJoint != null, "ConeConstraintMB.CalcInitData: nextJoint not set: {0}", name);
Transform j = tr;
Transform pj = j.parent;
m_initRot = j.localRotation;
m_twistAxis = Misc.InverseTransformDirection(pj, (m_nextJoint.position - j.position).normalized); //parent
}
/// <summary>
/// 1. rotate bone back from current rotation to refAxis; angle is "X"
/// 2. clamp X;
/// 3. rotate back with clamped X;
/// 4. calculate and clamp twist;
/// </summary>
public override void Apply(ISolver solver, int jointIdx)
{
if (m_nextJoint == null)
{
Dbg.CLogWarn(this, "ConeConstraintMB.Apply: nextJoint not set: {0}", name);
return;
}
Dbg.CAssert(this, m_angleLimit >= 0, "ConeConstraintMB.Apply: m_angleLimit should >= 0, but: {0}", m_angleLimit);
Dbg.CAssert(this, -180f <= m_minTwistLimit && m_minTwistLimit <= 180f, "ConeConstraintMB.Apply: minTwistLimit: {0}", m_minTwistLimit);
Dbg.CAssert(this, -180f <= m_maxTwistLimit && m_maxTwistLimit <= 180f, "ConeConstraintMB.Apply: maxTwistLimit: {0}", m_maxTwistLimit);
Dbg.CAssert(this, 0 <= m_angleLimit && m_angleLimit <= 180f, "ConeConstraintMB.Apply: angleLimit: {0}", m_angleLimit);
var joints = solver.GetJoints();
Transform j = joints[jointIdx];
Transform cj = m_nextJoint;
Transform pj = j.parent;
//1
Vector3 boneDirWorld = cj.position - j.position;
Vector3 refDirWorld = Misc.TransformDirection(pj, m_refAxis);
Quaternion q = Quaternion.FromToRotation(boneDirWorld, refDirWorld);
float angle; Vector3 rotAxis;
q.ToAngleAxis(out angle, out rotAxis);
angle = Misc.NormalizeAnglePI(angle);
j.rotation = q * j.rotation;
//Dbg.Log("coneconstraint: angle: {0}, rotAxis: {1}", angle, rotAxis);
//2
angle = Mathf.Clamp(angle, -m_angleLimit, m_angleLimit);
//3
j.Rotate(rotAxis, -angle, Space.World);
//4
if (m_limitTwist)
{ //use swing-twist decomposition of quaternion to limit twist
Quaternion deltaRot = j.localRotation * Quaternion.Inverse(m_initRot);
Vector3 curTwistAxisDirWorld = (m_nextJoint.position - tr.position).normalized;
Vector3 curTwistAxisDirParent = Misc.InverseTransformDirection(pj, curTwistAxisDirWorld);
Quaternion swingRot = Quaternion.FromToRotation(m_twistAxis, curTwistAxisDirParent);
Quaternion twistRot = Quaternion.Inverse(swingRot) * deltaRot;
Vector3 tmpAxis; float twist;
twistRot.ToAngleAxis(out twist, out tmpAxis);
twist = Misc.NormalizeAnglePI(twist);
if (float.IsInfinity(tmpAxis.x)) //SPECIAL case, some extreme data will make tmpAxis to be <inf,inf,inf>
{
tmpAxis = Vector3.right;
twist = 0;
}
if (Misc.IsObtuseAngle(tmpAxis, m_twistAxis))
{
twist = -twist;
tmpAxis = -tmpAxis;
}
twist = Mathf.Clamp(twist, m_minTwistLimit, m_maxTwistLimit);
twistRot = Quaternion.AngleAxis(twist, tmpAxis);
deltaRot = swingRot * twistRot;
var applied = deltaRot * m_initRot;
j.localRotation = applied;
}
}