public void SetMotorTargets(float deltaTime)
{
float ms = deltaTime * 1000000.0f;
float minFPS = 1000000.0f / 60.0f;
if (ms > minFPS)
{
ms = minFPS;
}
m_Time += ms;
//
// set per-frame sinusoidal position targets using angular motor (hacky?)
//
for (int r = 0; r < m_rigs.Count; r++)
{
HingeConstraint[] hingeConstraints = m_rigs[r].GetJoints();
for (int i = 0; i < 2 * TestRig.NUM_LEGS; i++)
{
HingeConstraint hingeC = hingeConstraints[i];
float fCurAngle = hingeC.GetHingeAngle();
float fTargetPercent = ((int)(m_Time / 1000) % (int)(m_fCyclePeriod)) / m_fCyclePeriod;
float fTargetAngle = 0.5f * (1 + (float)Math.Sin(MathUtil.SIMD_2_PI * fTargetPercent));
float fTargetLimitAngle = hingeC.GetLowerLimit() + fTargetAngle * (hingeC.GetUpperLimit() - hingeC.GetLowerLimit());
float fAngleError = fTargetLimitAngle - fCurAngle;
float fDesiredAngularVel = 1000000.0f * fAngleError / ms;
hingeC.EnableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
}
}
}
/// <summary>
/// Gets the hinge angle.
/// </summary>
/// <returns></returns>
public float GetHingeAngle()
{
var retval = _hci.GetHingeAngle();
return(retval);
}
