@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyRotation()
{
Random random = new Random(1766L);
RigidBodyBasics elevator = new RigidBody("elevator", world);
Vector3D jointAxis = RandomGeometry.nextVector3D(random);
jointAxis.normalize();
RigidBodyTransform transformToParent = new RigidBodyTransform();
transformToParent.setIdentity();
RevoluteJoint joint = new RevoluteJoint("joint", elevator, transformToParent, jointAxis);
RigidBodyBasics body = new RigidBody("body", joint, RandomGeometry.nextDiagonalMatrix3D(random), random.nextDouble(), new Vector3D());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector3D linearMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getLinearPart()));
FrameVector3D angularMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getAngularPart()));
FrameVector3D linearMomentumCheck = new FrameVector3D(world);
Matrix3D inertia = new Matrix3D(body.getInertia().getMomentOfInertia());
Vector3D angularMomentumCheckVector = new Vector3D(jointAxis);
angularMomentumCheckVector.scale(joint.getQd());
inertia.transform(angularMomentumCheckVector);
FrameVector3D angularMomentumCheck = new FrameVector3D(body.getInertia().getReferenceFrame(), angularMomentumCheckVector);
angularMomentumCheck.changeFrame(world);
double epsilon = 1e-9;
EuclidCoreTestTools.assertTuple3DEquals(linearMomentumCheck, linearMomentum, epsilon);
EuclidCoreTestTools.assertTuple3DEquals(angularMomentumCheck, angularMomentum, epsilon);
assertTrue(angularMomentum.length() > epsilon);
}