public void SetupFoot(int leg)
{
if (m_obj.Transform == null)
{
m_obj.Transform = m_obj.gameObject.GetComponent <Transform>();
}
if (m_obj.IsAnalisisRunning)
{
return;
}
// Calculate heel and toetip positions and alignments
// (The vector from the ankle to the ankle projected onto the ground at the stance pose
// in local coordinates relative to the ankle transform.
// This essentially is the ankle moved to the bottom of the foot, approximating the heel.)
// Get ankle position projected down onto the ground
Matrix4x4 ankleMatrix = Exts.RelativeMatrix(m_obj.Legs[leg].Ankle, m_obj.Transform);
Vector3 anklePosition = ankleMatrix.MultiplyPoint(Vector3.zero);
Vector3 heelPosition = anklePosition;
heelPosition.y = m_obj.GroundPlaneHeight;
// Get toe position projected down onto the ground
Matrix4x4 toeMatrix = Exts.RelativeMatrix(m_obj.Legs[leg].Toe, m_obj.Transform);
Vector3 toePosition = toeMatrix.MultiplyPoint(Vector3.zero);
Vector3 toetipPosition = toePosition;
toetipPosition.y = m_obj.GroundPlaneHeight;
// Calculate foot middle and vector
Vector3 footMiddle = (heelPosition + toetipPosition) / Float.Two;
Vector3 footVector;
if (toePosition == anklePosition)
{
footVector = ankleMatrix.MultiplyVector(m_obj.Legs[leg].Ankle.localPosition);
footVector.y = Float.Zero;
footVector = footVector.normalized;
}
else
{
footVector = (toetipPosition - heelPosition).normalized;
}
Vector3 footSideVector = Vector3.Cross(Vector3.up, footVector);
m_obj.Legs[leg].AnkleHeelVector = (
footMiddle
+ (-m_obj.Legs[leg].FootLength / Float.Two + m_obj.Legs[leg].FootOffset.y) * footVector
+ m_obj.Legs[leg].FootOffset.x * footSideVector
);
m_obj.Legs[leg].AnkleHeelVector = ankleMatrix.inverse.MultiplyVector(m_obj.Legs[leg].AnkleHeelVector - anklePosition);
m_obj.Legs[leg].ToeToetipVector = (
footMiddle
+ (m_obj.Legs[leg].FootLength / Float.Two + m_obj.Legs[leg].FootOffset.y) * footVector
+ m_obj.Legs[leg].FootOffset.x * footSideVector
);
m_obj.Legs[leg].ToeToetipVector = toeMatrix.inverse.MultiplyVector(m_obj.Legs[leg].ToeToetipVector - toePosition);
}