void fv_calcStrideMeanVelocity(bool p_forceStore = false)
{
GaitPlayer player = m_myController.m_player;
bool restarted = player.checkHasRestartedStride_AndResetFlag();
if (!restarted && !p_forceStore)
{
m_temp_currentStrideVelocities.Add(m_myController.m_currentVelocity);
m_temp_currentStrideDesiredVelocities.Add(m_myController.m_desiredVelocity);
}
else
{
Vector3 totalVelocities = Vector3.zero, totalDesiredVelocities = Vector3.zero;
for (int i = 0; i < m_temp_currentStrideVelocities.Count; i++)
{
totalVelocities += m_temp_currentStrideVelocities[i];
// force straight movement behavior from tests, set desired coronal velocity to constant zero:
totalDesiredVelocities += new Vector3(0.0f, 0.0f, m_temp_currentStrideDesiredVelocities[i].z);
}
//Debug.Log("TV: " + totalVelocities + " c " + m_temp_currentStrideVelocities.Count);
totalVelocities /= (float)m_temp_currentStrideVelocities.Count;
totalDesiredVelocities /= (float)m_temp_currentStrideDesiredVelocities.Count;
// add to lists
//Debug.Log("TV: " + totalVelocities.x + " " + totalVelocities.y + " " + totalVelocities.z);
m_fvVelocityDeviations.Add((double)Vector3.Magnitude(totalVelocities - totalDesiredVelocities));
//
m_temp_currentStrideVelocities.Clear();
m_temp_currentStrideDesiredVelocities.Clear();
}
}
void fr_calcRotationDeviations()
{
int legframes = m_myController.m_legFrames.Length;
GaitPlayer player = m_myController.m_player;
for (int i = 0; i < legframes; i++)
{
Quaternion currentDesiredOrientation = m_myController.m_legFrames[i].getCurrentDesiredOrientation(player.m_gaitPhase);
Quaternion currentOrientation = m_myController.m_legFrames[i].transform.rotation;
Quaternion diff = Quaternion.Inverse(currentOrientation) * currentDesiredOrientation;
Vector3 axis; float angle;
diff.ToAngleAxis(out angle, out axis);
m_frBodyRotationDeviations[i].Add(angle);
}
}
