@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate() throws Exception
{
double dt = 1.0e-8;
for (int i = 0; i < 1000; i++)
{
double yaw = RandomNumbers.nextDouble(random, Math.PI);
double pitch = RandomNumbers.nextDouble(random, Math.PI / 2.0);
double roll = RandomNumbers.nextDouble(random, Math.PI);
double yawRate = RandomNumbers.nextDouble(random, 1.0);
double pitchRate = RandomNumbers.nextDouble(random, 1.0);
double rollRate = RandomNumbers.nextDouble(random, 1.0);
double previousYaw = yaw - yawRate * dt;
double previousPitch = pitch - pitchRate * dt;
double previousRoll = roll - rollRate * dt;
Quaternion rotation = new Quaternion();
rotation.setYawPitchRoll(yaw, pitch, roll);
Quaternion previousRotation = new Quaternion();
previousRotation.setYawPitchRoll(previousYaw, previousPitch, previousRoll);
Vector3D expectedAngularVelocity = new Vector3D();
Quaternion difference = new Quaternion();
difference.difference(previousRotation, rotation);
difference.getRotationVector(expectedAngularVelocity);
expectedAngularVelocity.scale(1.0 / dt);
Vector3D actualAngularVelocity = new Vector3D();
RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity);
EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, 1.0e-7);
}
}