@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSingleRigidBodyTranslation() { Random random = new Random(1766L); RigidBodyBasics elevator = new RigidBody("elevator", world); Vector3D jointAxis = RandomGeometry.nextVector3D(random); jointAxis.normalize(); PrismaticJoint joint = new PrismaticJoint("joint", elevator, new Vector3D(), 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(joint.getFrameBeforeJoint(), jointAxis); linearMomentumCheck.scale(body.getInertia().getMass() * joint.getQd()); FrameVector3D angularMomentumCheck = new FrameVector3D(world); double epsilon = 1e-9; EuclidCoreTestTools.assertTuple3DEquals(linearMomentumCheck, linearMomentum, epsilon); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumCheck, angularMomentum, epsilon); assertTrue(linearMomentum.length() > epsilon); }