public void computeQDDot(Quat4d q, Vector3d angularVelocity, Vector3d angularAcceleration, Quat4d qDDotToPack) { computeQDot(q, angularVelocity, intermediateQDot); computeQDDot(q, intermediateQDot, angularVelocity, angularAcceleration, qDDotToPack); }
public void computeAngularAcceleration(Quat4d q, Quat4d qDDot, Vector3d angularVelocity, Vector3d angularAccelerationToPack) { computeQDot(q, angularVelocity, intermediateQDot); computeAngularAcceleration(q, intermediateQDot, qDDot, angularAccelerationToPack); }
public void computeQDDot(QuaternionReadOnly q, Vector3DReadOnly angularVelocity, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) { computeQDot(q, angularVelocity, intermediateQDot); computeQDDot(q, intermediateQDot, angularVelocity, angularAcceleration, qDDotToPack); }
public void computeAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDDot, Vector3DReadOnly angularVelocity, Vector3DBasics angularAccelerationToPack) { computeQDot(q, angularVelocity, intermediateQDot); computeAngularAcceleration(q, intermediateQDot, qDDot, angularAccelerationToPack); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConversionQDotToAngularVelocityBackAndForth() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0); Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) expectedAngularVelocity.negate(); Vector3D actualAngularVelocity = new Vector3D(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDot(q, expectedAngularVelocity, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, EPSILON)); } }
Vector4D qDDot = new Vector4D(); quaternionCalculus.computeQDot(q, angularVelocity, qDot);