// Token: 0x0600349C RID: 13468 RVA: 0x000E5940 File Offset: 0x000E3B40 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); } }
// Token: 0x0600274E RID: 10062 RVA: 0x000B6234 File Offset: 0x000B4434 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(); } }
/// <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: 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> /// 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; }
/// <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); }
/// <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: 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> /// 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); }