/// <summary>
/// Get the next close point on the surface of the joint's rotation cone
/// </summary>
/// <param name="joint"></param>
/// <param name="obj"></param>
/// <returns></returns>
public static Vector3 GetConeNextPoint(Core.Joint joint, Vector3 obj)
{
//this algorithm is not as accurate as an optimization partial-differential problem, but its simple and straightforward.
//accuracy doesn't matter in this case
if (ConeBounded(joint, obj))
{
return(obj);
}
Vector3 jointPos = joint.pos;
Vector3 dir = obj - jointPos;
Vector3 axis = TransformVector(joint.axis, joint.rot);
float currAngle = VectorsAngle(dir, jointPos + axis);
float d = Mathf.Cos(currAngle * Mathf.Deg2Rad) * dir.magnitude;
float x = d * (Mathf.Tan(currAngle * Mathf.Deg2Rad) - Mathf.Tan(joint.maxAngle * Mathf.Deg2Rad));
Vector3 coneAxis = joint.joint.position + (TransformVector(axis * d, joint.rot));
Vector3 rx = coneAxis - obj;
float dot = Vector3.Dot(joint.joint.position + (TransformVector(axis, joint.rot)), dir.normalized);
Vector3 point = (rx.normalized * x + obj) * Mathf.Clamp01(Mathf.Sign(dot)) + jointPos * Mathf.Clamp01(-Mathf.Sign(dot));
return(point);
}
/// <summary>
/// check if the obj is inside the boundaries of the joint's rotation cone
/// </summary>
/// <param name="joint"></param>
/// <param name="obj"></param>
/// <returns></returns>
public static bool ConeBounded(Core.Joint joint, Vector3 obj)
{
Vector3 dir = obj - joint.pos;
float angle = VectorsAngle(dir, joint.pos + TransformVector(joint.axis, joint.rot));
return(joint.maxAngle >= angle);
}
// 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>
/// 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: 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: 0x0600276E RID: 10094 RVA: 0x000B6CD0 File Offset: 0x000B4ED0
private void ReadHumanoidRig()
{
this.h_root = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.Hips)
};
this.h_head = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.Head)
};
this.spine.spine = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.Spine)
};
this.spine.chest = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.Chest)
};
this.r_upperbody.shoulder = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightShoulder)
};
this.r_upperbody.upperArm = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightUpperArm)
};
this.r_upperbody.lowerArm = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightLowerArm)
};
this.r_upperbody.hand = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightHand)
};
this.l_upperbody.shoulder = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftShoulder)
};
this.l_upperbody.upperArm = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftUpperArm)
};
this.l_upperbody.lowerArm = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftLowerArm)
};
this.l_upperbody.hand = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftHand)
};
this.r_lowerbody.upperLeg = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightUpperLeg)
};
this.r_lowerbody.lowerLeg = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightLowerLeg)
};
this.r_lowerbody.foot = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.RightFoot)
};
this.l_lowerbody.upperLeg = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftUpperLeg)
};
this.l_lowerbody.lowerLeg = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftLowerLeg)
};
this.l_lowerbody.foot = new Core.Joint
{
joint = this.animator.GetBoneTransform(HumanBodyBones.LeftFoot)
};
}
