// 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>
/// 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;
}
/// <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: 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);
}
}
// 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();
}
}
/// <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>
/// 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>
/// 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);
}