public Core.Chain BuildChain(HumanPart chain)
{
switch (chain)
{
case HumanPart.Head:
Core.Chain head = new Core.Chain();
head.Joints.Add(Head);
return(head);
case HumanPart.RightArm:
return(RightArmChain());
case HumanPart.LeftArm:
return(LeftArmChain());
case HumanPart.RightLeg:
return(RightLegChain());
case HumanPart.LeftLeg:
return(LeftLegChain());
case HumanPart.Root:
Core.Chain root = new Core.Chain();
root.Joints.Add(Root);
return(root);
}
return(null);
}
/// <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);
}
public void BuildRig()
{
rArm = rigReader.BuildChain(HumanPart.RightArm);
lArm = rigReader.BuildChain(HumanPart.LeftArm);
rLeg = rigReader.BuildChain(HumanPart.RightLeg);
lLeg = rigReader.BuildChain(HumanPart.LeftLeg);
}
/// <summary>
/// Solve the cain
/// </summary>
/// <param name="chain">The chain required</param>
/// <param name="lookAtAxis">Which axis of the end effector to consider</param>
/// <param name="virtualEndEffector">Offset the end effector by this Transform (optional)</param>
public static void Process(Core.Chain chain, Vector3 lookAtAxis, Transform virtualEndEffector)
{
Transform offset = virtualEndEffector ?? chain.GetEndEffector();
for (int i = 0; i < chain.Iterations; i++)
{
Solve(chain, offset, lookAtAxis);
}
}
// Token: 0x06003497 RID: 13463 RVA: 0x000E5648 File Offset: 0x000E3848
public static void Process(Core.Chain chain, Vector3 lookAtAxis, Transform virtualEndEffector)
{
Transform endEffector = virtualEndEffector ?? chain.GetEndEffector();
for (int i = 0; i < chain.iterations; i++)
{
DirectionalSwingSolver.Solve(chain, endEffector, lookAtAxis);
}
}
// 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: 0x06002751 RID: 10065 RVA: 0x000B6420 File Offset: 0x000B4620
public static void SolveOutward(Core.Chain chain)
{
chain.joints[0].pos = chain.joints[0].joint.position;
for (int i = 1; i < chain.joints.Count; i++)
{
Vector3 pos = chain.joints[i - 1].pos;
Vector3 vector = chain.joints[i].pos - pos;
vector.Normalize();
vector *= Vector3.Distance(chain.joints[i - 1].joint.position, chain.joints[i].joint.position);
chain.joints[i].pos = pos + vector;
}
}
/// <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: 0x06002750 RID: 10064 RVA: 0x000B6344 File Offset: 0x000B4544
public static void SolveInward(Core.Chain chain)
{
int count = chain.joints.Count;
chain.joints[count - 1].pos = Vector3.Lerp(chain.GetVirtualEE(), chain.GetIKtarget(), chain.weight);
for (int i = count - 2; i >= 0; i--)
{
Vector3 pos = chain.joints[i + 1].pos;
Vector3 vector = chain.joints[i].pos - pos;
vector.Normalize();
vector *= Vector3.Distance(chain.joints[i + 1].joint.position, chain.joints[i].joint.position);
chain.joints[i].pos = pos + vector;
}
}
/// <summary>
/// Find the virtual new solved position of joints in the chain outward
/// </summary>
/// <param name="chain"></param>
public static void SolveOutward(Core.Chain chain)
{
chain.Joints[0].pos = chain.Joints[0].joint.position;
for (int i = 1; i < chain.Joints.Count; i++)
{
Vector3 _p = chain.Joints[i - 1].pos; //point
Vector3 _d = chain.Joints[i].pos - _p; //direction
_d.Normalize();
_d *= Vector3.Distance(chain.Joints[i - 1].joint.position, chain.Joints[i].joint.position);
chain.Joints[i].pos = _p + _d;
}
}
/// <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: 0x06002772 RID: 10098 RVA: 0x000B70C0 File Offset: 0x000B52C0
public Core.Chain LeftLegChain()
{
if (!this.IsReady())
{
return(null);
}
if (this.rigType != RigReader.RigType.Humanoid)
{
return(null);
}
Core.Chain chain = new Core.Chain();
chain.joints.Add(this.l_lowerbody.upperLeg);
chain.joints.Add(this.l_lowerbody.lowerLeg);
chain.joints.Add(this.l_lowerbody.foot);
chain.InitiateJoints();
return(chain);
}
// Token: 0x06002770 RID: 10096 RVA: 0x000B6FC8 File Offset: 0x000B51C8
public Core.Chain LeftArmChain()
{
if (!this.IsReady())
{
return(null);
}
if (this.rigType != RigReader.RigType.Humanoid)
{
return(null);
}
Core.Chain chain = new Core.Chain();
chain.joints.Add(this.l_upperbody.shoulder);
chain.joints.Add(this.l_upperbody.upperArm);
chain.joints.Add(this.l_upperbody.lowerArm);
chain.joints.Add(this.l_upperbody.hand);
chain.InitiateJoints();
return(chain);
}
private void DrawHumanoidScene(Core.Chain chain)
{
if (chain == null)
{
return;
}
if (chain.Joints.Count <= 0)
{
return;
}
Color yellow = Color.yellow;
Color blue = Color.blue;
Color green = Color.green;
Color red = Color.red;
Dictionary <int, Color> colorMap = new Dictionary <int, Color>();
Action <Vector3> drawSphere = v => Handles.SphereHandleCap(0, v, Quaternion.identity, 0.04f, EventType.Repaint);
Action <Vector3> drawCube = v => Handles.CubeHandleCap(0, v, Quaternion.identity, 0.03f, EventType.Repaint);
Action <int> alternateColor = i => Handles.color = colorMap[(int)Mathf.Pow(-1, i)];
colorMap.Add(1, yellow);
colorMap.Add(-1, green);
for (int i = 0; i < chain.Joints.Count - 1; i++)
{
if (!chain.Joints[i].joint)
{
break;
}
alternateColor(i);
Handles.DrawLine(chain.Joints[i].joint.position, chain.Joints[i + 1].joint.position);
alternateColor(i + 1);
drawSphere(chain.Joints[i].joint.position);
}
Func <float, Color> lerpColor = x => Color.Lerp(green, red, x);
Action <Core.Chain> toTarget = x => Handles.DrawLine(x.GetEndEffector().position, x.Target && x.Weight > 0 ? x.Target.position : x.GetEndEffector().position);
Handles.color = lerpColor(chain.Weight);
toTarget(chain);
drawCube(chain.GetEndEffector().position);
drawCube(chain.Target ? chain.Target.position : chain.GetEndEffector().position);
}
// Token: 0x0600274F RID: 10063 RVA: 0x000B62EC File Offset: 0x000B44EC
public static bool Process(Core.Chain chain)
{
if (chain.joints.Count <= 0)
{
return(false);
}
if (!chain.initiated)
{
chain.InitiateJoints();
}
chain.MapVirtualJoints();
for (int i = 0; i < chain.iterations; i++)
{
FastReachSolver.SolveInward(chain);
FastReachSolver.SolveOutward(chain);
}
FastReachSolver.MapSolverOutput(chain);
return(true);
}
/// <summary>
/// Find the virtual new solved position of joints in the chain inward
/// </summary>
/// <param name="chain"></param>
public static void SolveInward(Core.Chain chain)
{
int c = chain.Joints.Count;
//Use Weight first
chain.Joints[c - 1].pos = Vector3.Lerp(chain.GetVirtualEE(), chain.GetIKTarget(), chain.Weight);
//find the joint on the chain's virtual line
for (int i = c - 2; i >= 0; i--)
{
Vector3 _p = chain.Joints[i + 1].pos; //point
Vector3 _d = chain.Joints[i].pos - _p; //direction
_d.Normalize();
_d *= Vector3.Distance(chain.Joints[i + 1].joint.position, chain.Joints[i].joint.position); //all points in a direction along a length
chain.Joints[i].pos = _p + _d;
}
}
/// <summary>
/// Build the right leg chain
/// </summary>
/// <returns></returns>
private Core.Chain LeftLegChain()
{
if (!IsReady())
{
return(null);
}
if (rigType != RigType.Humanoid)
{
return(null);
}
Core.Chain chain = new Core.Chain();
chain.Joints.Add(l_lowerbody.upperLeg);
chain.Joints.Add(l_lowerbody.lowerLeg);
chain.Joints.Add(l_lowerbody.foot);
chain.InitiateJoints();
return(chain);
}
/// <summary>
/// Build the left arm IK chain
/// </summary>
/// <returns></returns>
private Core.Chain LeftArmChain()
{
if (!IsReady())
{
return(null);
}
if (rigType != RigType.Humanoid)
{
return(null);
}
Core.Chain chain = new Core.Chain();
chain.Joints.Add(l_upperbody.shoulder);
chain.Joints.Add(l_upperbody.upperArm);
chain.Joints.Add(l_upperbody.lowerArm);
chain.Joints.Add(l_upperbody.hand);
chain.InitiateJoints();
return(chain);
}
// 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>
/// Process a 2 bones chain through the Cosine Rule
/// </summary>
/// <param name="chain"></param>
public static void Process(Core.Chain chain)
{
Process(chain, Epsilon);
}
// Token: 0x06003496 RID: 13462 RVA: 0x000E5637 File Offset: 0x000E3837
public static void Process(Core.Chain chain, Vector3 lookAtAxis)
{
DirectionalSwingSolver.Process(chain, lookAtAxis, chain.GetEndEffector());
}
/// <summary>
/// Solve the cain
/// </summary>
/// <param name="chain">The chain required</param>
/// <param name="lookAtAxis">Which axis of the end effector to consider</param>
public static void Process(Core.Chain chain, Vector3 lookAtAxis)
{
Process(chain, lookAtAxis, chain.GetEndEffector());
}
