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