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