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