// Token: 0x06002730 RID: 10032 RVA: 0x000B5A1C File Offset: 0x000B3C1C private Quaternion SingleDegree() { Vector3 target = GenericMath.TransformVector(this.axis, this.joint.transform.localRotation); float num; Vector3 rhs; GenericMath.QuaternionToAngleAxis(GenericMath.ApplyQuaternion(GenericMath.RotateFromTo(this.axis, target), this.joint.localRotation), out num, out rhs); float x = this.hingLimit.x; float y = this.hingLimit.y; float num2 = Vector3.Dot(this.axis, rhs); num = GenericMath.Clamp(num * num2, x, y); this.joint.localRotation = GenericMath.QuaternionFromAngleAxis(num, this.axis); return(this.joint.localRotation); }
/// <summary> /// Process a 2 bones chain with a specific "epsilon" value /// </summary> /// <param name="chain"></param> /// <param name="eps">a specific value, not bounded to the global Epsilon</param> public static void Process(Core.Chain chain, float eps) { if (chain.Initiated == false) { chain.InitiateJoints(); } if (chain.Joints.Count != 3) { Debug.LogError("The Analytical Solver only works with 3-joints(2 bones) chain configurations"); return; } Core.Joint A = chain.Joints[0]; Core.Joint B = chain.Joints[1]; Core.Joint C = chain.Joints[2]; Vector3 T = chain.GetIKTarget(); Vector3 AB = Vector3.Normalize(B.joint.position - A.joint.position); Vector3 AC = Vector3.Normalize(C.joint.position - A.joint.position); Vector3 CB = Vector3.Normalize(B.joint.position - C.joint.position); Vector3 TA = A.joint.position - T; float l_ab = A.length; float l_cb = B.length; float l_at = GenericMath.Clamp(TA.magnitude, eps, l_ab + l_cb - eps); float kneeCurrent = GenericMath.VectorsAngle(AB, CB); float kneeTarget = GenericMath.CosineRule(A.length, B.length, l_at); float kneeDelta = kneeTarget - kneeCurrent; Vector3 axis = GenericMath.TransformVector(Vector3.Normalize(Vector3.Cross(AC, AB)), Quaternion.Inverse(B.joint.rotation)); Quaternion q1 = Quaternion.AngleAxis(kneeDelta, axis); Quaternion knee = Quaternion.Lerp(B.joint.rotation, GenericMath.ApplyQuaternion(B.joint.rotation, q1), chain.Weight); B.joint.rotation = knee; Quaternion q2 = Quaternion.FromToRotation(A.joint.position - C.joint.position, TA); Quaternion thigh = Quaternion.Lerp(A.joint.rotation, GenericMath.ApplyQuaternion(q2, A.joint.rotation), chain.Weight); A.joint.rotation = thigh; }
/// <summary> /// Limit the motion to 1 Degree of freedom /// </summary> /// <param name="_localRot"></param> /// <returns></returns> private Quaternion SingleDegree() { float angle; Vector3 axis; //Hinge only Quaternion Vector3 _localAxis = GenericMath.TransformVector(this.axis, joint.transform.localRotation); Quaternion _delta = GenericMath.RotateFromTo(this.axis, _localAxis); Quaternion _legalRot = GenericMath.ApplyQuaternion(_delta, joint.localRotation); GenericMath.QuaternionToAngleAxis(_legalRot, out angle, out axis); float min = hingLimit.x; float max = hingLimit.y; float dot = Vector3.Dot(this.axis, axis); //clamp values //angle = Mathf.Clamp(angle * dot, min, max); //Unity's Clamp gives NaN values in particular cases so use our own angle = GenericMath.Clamp(angle * dot, min, max); joint.localRotation = GenericMath.QuaternionFromAngleAxis(angle, this.axis); return(joint.localRotation); }