/// <summary> /// Rotate the ankle /// </summary> public void RotateAnkle() { Quaternion rot = GenericMath.ApplyQuaternion(_EETargetRot, _EEAnimRot); Quaternion targetRot = Quaternion.Lerp(EE.rotation, rot, Chain.Weight); EE.rotation = targetRot; }
// Token: 0x0600272F RID: 10031 RVA: 0x000B58E8 File Offset: 0x000B3AE8 private Quaternion TwistAndSwing() { object obj = new Func <Quaternion, float, Quaternion>(delegate(Quaternion q, float x) { if (x == 0f) { return(Quaternion.identity); } float num2 = GenericMath.QuaternionAngle(Quaternion.identity, q); float t = Mathf.Clamp01(x / num2); return(Quaternion.Slerp(Quaternion.identity, q, t)); }); Func <float, float> func = (float x) => x * x; Quaternion quaternion = GenericMath.RotateFromTo(GenericMath.TransformVector(this.axis, this.joint.localRotation), this.axis); object obj2 = obj; Quaternion qB = obj2(quaternion, this.maxAngle); Quaternion quaternion2 = GenericMath.ApplyQuaternion(Quaternion.Inverse(quaternion), this.joint.localRotation); float num = Mathf.Sqrt(func(quaternion2.w) + func(quaternion2.x) + func(quaternion2.y) + func(quaternion2.z)); float w = quaternion2.w / num; float x2 = quaternion2.x / num; float y = quaternion2.y / num; float z = quaternion2.z / num; Quaternion qA = obj2(new Quaternion(x2, y, z, w), this.maxTwist); this.joint.localRotation = GenericMath.ApplyQuaternion(qA, qB); return(this.joint.localRotation); }
/// <summary> /// Make sure the joints are initiated correctly /// </summary> public void InitiateJoints() { initLocalRot = new Quaternion[joints.Count]; prevPos = new Vector3[joints.Count]; int i = 0; for (i = 0; i < joints.Count - 1; i++) { joints[i].MapVirtual(); joints[i].localAxis = GenericMath.GetLocalAxisToTarget(joints[i].joint, joints[i + 1].joint.position); joints[i].length = Vector3.Distance(joints[i].joint.position, joints[i + 1].joint.position); initLocalRot[i] = joints[i].joint.localRotation; prevPos[i] = joints[i].joint.position; } joints[i].MapVirtual(); initLocalRot[i] = joints[i].joint.localRotation; prevPos[i] = joints[i].joint.position; joints[i].localAxis = GenericMath.GetLocalAxisToTarget(joints[0].joint, joints[i].joint.position); initiated = true; }
/// <summary> /// Solve the IK for a chain /// </summary> /// <param name="chain"></param> public static bool Process(Core.Chain chain) { if (chain.Joints.Count <= 0) { return(false); } chain.MapVirtualJoints(); for (int j = 0; j < chain.Iterations; j++) { for (int i = chain.Joints.Count - 1; i >= 0; i--) { float _weight = chain.Weight * chain.Joints[i].weight; //relative weight //direction vectors Vector3 _v0 = chain.GetIKTarget() - chain.Joints[i].joint.position; Vector3 _v1 = chain.Joints[chain.Joints.Count - 1].joint.position - chain.Joints[i].joint.position; //Weight Quaternion _sourceRotation = chain.Joints[i].joint.rotation; Quaternion _targetRotation = Quaternion.Lerp(Quaternion.identity, GenericMath.RotateFromTo(_v0, _v1), _weight); //Rotation Math chain.Joints[i].rot = Quaternion.Lerp(_sourceRotation, GenericMath.ApplyQuaternion(_targetRotation, _sourceRotation), _weight); chain.Joints[i].ApplyVirtualMap(false, true); chain.Joints[i].ApplyRestrictions(); } } return(true); }
// Token: 0x060034A9 RID: 13481 RVA: 0x000E5E68 File Offset: 0x000E4068 public static Vector3 TransformVector(Vector3 _v, Quaternion _q) { Quaternion qB = new Quaternion(_v.x, _v.y, _v.z, 0f); Quaternion quaternion = GenericMath.ApplyQuaternion(_q, qB); quaternion = GenericMath.ApplyQuaternion(quaternion, Quaternion.Inverse(_q)); return(new Vector3(quaternion.x, quaternion.y, quaternion.z)); }
// Token: 0x06003498 RID: 13464 RVA: 0x000E567C File Offset: 0x000E387C private static void Solve(Core.Chain chain, Transform endEffector, Vector3 LookAtAxis) { for (int i = 0; i < chain.joints.Count; i++) { Vector3 target = GenericMath.TransformVector(LookAtAxis, endEffector.rotation); Quaternion b = GenericMath.RotateFromTo(chain.GetIKtarget() - endEffector.position, target); Quaternion qA = Quaternion.Lerp(Quaternion.identity, b, chain.weight * chain.joints[i].weight); chain.joints[i].joint.rotation = GenericMath.ApplyQuaternion(qA, chain.joints[i].joint.rotation); chain.joints[i].ApplyRestrictions(); } }
// Token: 0x06002752 RID: 10066 RVA: 0x000B64F8 File Offset: 0x000B46F8 public static void MapSolverOutput(Core.Chain chain) { for (int i = 0; i < chain.joints.Count - 1; i++) { Vector3 source = chain.joints[i + 1].pos - chain.joints[i].pos; Vector3 target = GenericMath.TransformVector(chain.joints[i].localAxis, chain.joints[i].rot); Quaternion qA = GenericMath.RotateFromTo(source, target); chain.joints[i].rot = GenericMath.ApplyQuaternion(qA, chain.joints[i].rot); chain.joints[i].ApplyVirtualMap(true, true); } }
/// <summary> /// Map the vitual solver's joints onto the physical ones /// </summary> /// <param name="chain"></param> public static void MapSolverOutput(Core.Chain chain) { for (int i = 0; i < chain.Joints.Count - 1; i++) { Vector3 _v1 = chain.Joints[i + 1].pos - chain.Joints[i].pos; Vector3 _v2 = GenericMath.TransformVector(chain.Joints[i].localAxis, chain.Joints[i].rot); Quaternion _offset = GenericMath.RotateFromTo(_v1, _v2); chain.Joints[i].rot = GenericMath.ApplyQuaternion(_offset, chain.Joints[i].rot); chain.Joints[i].ApplyVirtualMap(true, true); } }
// Token: 0x0600272A RID: 10026 RVA: 0x000B5748 File Offset: 0x000B3948 private static void MapSolverOutput(Core.KinematicChain chain) { for (int i = 1; i < chain.joints.Count; i++) { Vector3 target = GenericMath.TransformVector(chain.joints[i - 1].localAxis, chain.joints[i - 1].rot); Quaternion qA = GenericMath.RotateFromTo(chain.joints[i].pos - chain.joints[i - 1].pos, target); chain.joints[i - 1].rot = GenericMath.ApplyQuaternion(qA, chain.joints[i - 1].rot); chain.joints[i - 1].ApplyVirtualMap(false, true); chain.joints[i].ApplyVirtualMap(true, false); chain.joints[i - 1].ApplyRestrictions(); chain.joints[i].ApplyRestrictions(); } }
/// <summary> /// Solve the chain to make the offset look at the target /// </summary> /// <param name="chain"></param> /// <param name="endEffector"></param> private static void Solve(Core.Chain chain, Transform endEffector, Vector3 LookAtAxis) { for (int i = 0; i < chain.Joints.Count; i++) { //Vector3 axis = GenericMath.TransformVector(LookAtAxis, Quaternion.Inverse(offsetObj.rotation)); Vector3 axis = GenericMath.TransformVector(LookAtAxis, endEffector.rotation); Quaternion delta = GenericMath.RotateFromTo(chain.GetIKTarget() - endEffector.position, axis); Quaternion final = Quaternion.Lerp(Quaternion.identity, delta, chain.Weight * chain.Joints[i].weight); chain.Joints[i].joint.rotation = GenericMath.ApplyQuaternion(final, chain.Joints[i].joint.rotation); chain.Joints[i].ApplyRestrictions(); } }
// Token: 0x0600273B RID: 10043 RVA: 0x000B5BB0 File Offset: 0x000B3DB0 public void InitiateJoints() { this.MapVirtualJoints(); for (int i = 0; i < this.joints.Count - 1; i++) { this.joints[i].localAxis = GenericMath.GetLocalAxisToTarget(this.joints[i].joint, this.joints[i + 1].joint.position); this.joints[i].length = Vector3.Distance(this.joints[i].joint.position, this.joints[i + 1].joint.position); this.chainLength += this.joints[i].length; } this.joints[this.joints.Count - 1].localAxis = GenericMath.GetLocalAxisToTarget(this.joints[0].joint, this.joints[this.joints.Count - 1].joint.position); this.SetIKTarget(this.GetVirtualEE()); this.initiated = true; }
// 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; }
// Token: 0x060034AD RID: 13485 RVA: 0x000E5F50 File Offset: 0x000E4150 public static Vector3 GetConeNextPoint(Core.Joint joint, Vector3 obj) { if (GenericMath.ConeBounded(joint, obj)) { return(obj); } Vector3 pos = joint.pos; Vector3 v = obj - pos; Vector3 vector = GenericMath.TransformVector(joint.axis, joint.rot); float num = GenericMath.VectorsAngle(v, pos + vector); float num2 = Mathf.Cos(num * 0.017453292f) * v.magnitude; float d = num2 * (Mathf.Tan(num * 0.017453292f) - Mathf.Tan(joint.maxAngle * 0.017453292f)); Vector3 vector2 = joint.joint.position + GenericMath.TransformVector(vector * num2, joint.rot) - obj; float f = Vector3.Dot(joint.joint.position + GenericMath.TransformVector(vector, joint.rot), v.normalized); return((vector2.normalized * d + obj) * Mathf.Clamp01(Mathf.Sign(f)) + pos * Mathf.Clamp01(-Mathf.Sign(f))); }
/// <summary> /// Make sure the joints are initiated correctly /// </summary> public void InitiateJoints() { MapVirtualJoints(); for (int i = 0; i < Joints.Count - 1; i++) { Joints[i].localAxis = GenericMath.GetLocalAxisToTarget(Joints[i].joint, Joints[i + 1].joint.position); Joints[i].length = Vector3.Distance(Joints[i].joint.position, Joints[i + 1].joint.position); ChainLength += Joints[i].length; } Joints[Joints.Count - 1].localAxis = GenericMath.GetLocalAxisToTarget(Joints[0].joint, Joints[Joints.Count - 1].joint.position); SetIKTarget(GetVirtualEE()); Initiated = true; }
/// <summary> /// Cast rays to find pumps in the terrain and sets the IK target to the appropriate hit point. /// (does not solve the IK, you need to Call a Solver separately) /// (The AnalyticalSolver is suggested) /// </summary> public void TerrainAdjustment(LayerMask mask, Transform root) { if (_init == false) { Init(); return; } RaycastHit hit; Vector3 rootUp = root.up; Ray ray = new Ray(EE.position, Vector3.down); bool intersect = Physics.Raycast(ray, out hit, MaxStepHeight, mask, QueryTriggerInteraction.Ignore); #if UNITY_EDITOR if (intersect) { //Debug.DrawLine(ray.origin, hit.point, Color.green); //enable for debug purposes } #endif if (intersect) { float footHeight = root.position.y - EE.position.y; float footFromGround = hit.point.y - root.position.y; float offsetTarget = Mathf.Clamp(footFromGround, -MaxStepHeight, MaxStepHeight) + FootOffset; float currentMaxOffset = Mathf.Clamp(MaxStepHeight - footHeight, 0f, MaxStepHeight); float IK = Mathf.Clamp(offsetTarget, -currentMaxOffset, offsetTarget); IKPointOffset = rootUp * IK; normals = Vector3.Lerp(normals, hit.normal, Time.deltaTime * EaseOutNormals); } else { IKPointOffset = Vector3.Lerp(IKPointOffset, Vector3.zero, Time.deltaTime * EaseOutPos); normals = Vector3.Lerp(normals, rootUp, Time.deltaTime * EaseOutNormals); } Chain.SetIKTarget(EE.position + IKPointOffset); //calculate the ankle rot, before applying the IK _EETargetRot = GenericMath.RotateFromTo(normals, rootUp); _EEAnimRot = EE.rotation; }
// Token: 0x06002749 RID: 10057 RVA: 0x000B5E94 File Offset: 0x000B4094 public void InitiateJoints() { this.initLocalRot = new Quaternion[this.joints.Count]; this.prevPos = new Vector3[this.joints.Count]; int i; for (i = 0; i < this.joints.Count - 1; i++) { this.joints[i].MapVirtual(); this.joints[i].localAxis = GenericMath.GetLocalAxisToTarget(this.joints[i].joint, this.joints[i + 1].joint.position); this.joints[i].length = Vector3.Distance(this.joints[i].joint.position, this.joints[i + 1].joint.position); this.initLocalRot[i] = this.joints[i].joint.localRotation; this.prevPos[i] = this.joints[i].joint.position; } this.joints[i].MapVirtual(); this.initLocalRot[i] = this.joints[i].joint.localRotation; this.prevPos[i] = this.joints[i].joint.position; this.joints[i].localAxis = GenericMath.GetLocalAxisToTarget(this.joints[0].joint, this.joints[i].joint.position); this.initiated = true; }
/// <summary> /// The Full-Restricted motion limit /// </summary> /// <param name="_localRot"></param> /// <returns></returns> private Quaternion TwistAndSwing() { Func <Quaternion, float, Quaternion> LimitByAngle = (q, x) => { if (x == 0) { return(Quaternion.identity); } float angle = GenericMath.QuaternionAngle(Quaternion.identity, q); float t = Mathf.Clamp01(x / angle); Quaternion output = Quaternion.Slerp(Quaternion.identity, q, t); //lerp doesnt work :( return(output); }; Func <float, float> Sqr = x => x * x; Vector3 _localAxis = GenericMath.TransformVector(axis, joint.localRotation); //swing only quaternion Quaternion swing = GenericMath.RotateFromTo(_localAxis, axis); Quaternion limitedSwing = LimitByAngle(swing, maxAngle); //twist only quaternion Quaternion twist = GenericMath.ApplyQuaternion(Quaternion.Inverse(swing), joint.localRotation); //twist decomposition float qM = Mathf.Sqrt(Sqr(twist.w) + Sqr(twist.x) + Sqr(twist.y) + Sqr(twist.z)); float qw = twist.w / qM; float qx = twist.x / qM; float qy = twist.y / qM; float qz = twist.z / qM; Quaternion limitedTwist = LimitByAngle(new Quaternion(qx, qy, qz, qw), maxTwist); joint.localRotation = GenericMath.ApplyQuaternion(limitedTwist, limitedSwing); return(joint.localRotation); }
// Token: 0x0600274B RID: 10059 RVA: 0x000B60A8 File Offset: 0x000B42A8 public static bool Process(Core.Chain chain) { if (chain.joints.Count <= 0) { return(false); } chain.MapVirtualJoints(); for (int i = 0; i < chain.iterations; i++) { for (int j = chain.joints.Count - 1; j >= 0; j--) { float t = chain.weight * chain.joints[j].weight; Vector3 source = chain.GetIKtarget() - chain.joints[j].joint.position; Vector3 target = chain.joints[chain.joints.Count - 1].joint.position - chain.joints[j].joint.position; Quaternion rotation = chain.joints[j].joint.rotation; Quaternion qA = Quaternion.Lerp(Quaternion.identity, GenericMath.RotateFromTo(source, target), t); chain.joints[j].rot = Quaternion.Lerp(rotation, GenericMath.ApplyQuaternion(qA, rotation), t); chain.joints[j].ApplyVirtualMap(false, true); chain.joints[j].ApplyRestrictions(); } } return(true); }
/// <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); }
// Token: 0x060034A8 RID: 13480 RVA: 0x000E5E2C File Offset: 0x000E402C public static Quaternion RotateFromTo(Vector3 _source, Vector3 _target) { _source.Normalize(); _target.Normalize(); return(Quaternion.Inverse(GenericMath.QuaternionFromAngleAxis(GenericMath.VectorsAngle(_source, _target), Vector3.Cross(_source, _target).normalized))); }
// Token: 0x060034AC RID: 13484 RVA: 0x000E5F08 File Offset: 0x000E4108 public static bool ConeBounded(Core.Joint joint, Vector3 obj) { float num = GenericMath.VectorsAngle(obj - joint.pos, joint.pos + GenericMath.TransformVector(joint.axis, joint.rot)); return(joint.maxAngle >= num); }
// Token: 0x060034AB RID: 13483 RVA: 0x000E5ED4 File Offset: 0x000E40D4 public static Vector3 GetLocalAxisToTarget(Transform self, Vector3 target) { Quaternion q = Quaternion.Inverse(self.rotation); return(GenericMath.TransformVector((target - self.position).normalized, q)); }