@ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 300000)
  public void testTwoIntersectingBoxes() {
    CombinedTerrainObject3D combinedTerrainObject =
        new CombinedTerrainObject3D("Combined Terrain Object to Test");
    Point3d pointToCheck = new Point3d();
    Point3d expectedIntersection = new Point3d();
    Vector3d expectedNormal = new Vector3d();

    Point3d resultIntersection = new Point3d();
    Vector3d resultNormal = new Vector3d();

    setupTwoIntersectingBoxesMadeFromPolygons(combinedTerrainObject);

    pointToCheck.set(0.4, 0.45, 0.25);
    expectedIntersection.set(0.4, 0.5, 0.25);
    expectedNormal.set(0.0, 1.0, 0.0);

    combinedTerrainObject.checkIfInside(
        pointToCheck.getX(),
        pointToCheck.getY(),
        pointToCheck.getZ(),
        resultIntersection,
        resultNormal);
    JUnitTools.assertTuple3dEquals(expectedIntersection, resultIntersection, 1e-4);
    JUnitTools.assertTuple3dEquals(expectedNormal, resultNormal, 1e-4);
  }
  @DeployableTestMethod(estimatedDuration = 0.1)
  @Test(timeout = 30000)
  public void testGetLegPairs() {
    final QuadrupedSupportPolygon quadrupedSupportPolygon = createSimplePolygon();

    RobotQuadrant[][] legPairs = quadrupedSupportPolygon.getLegPairs();
    assertEquals("not 4", 4, legPairs.length);

    quadrupedSupportPolygon.removeFootstep(RobotQuadrant.FRONT_LEFT);

    legPairs = quadrupedSupportPolygon.getLegPairs();
    assertEquals("not 3", 3, legPairs.length);

    quadrupedSupportPolygon.removeFootstep(RobotQuadrant.FRONT_RIGHT);

    legPairs = quadrupedSupportPolygon.getLegPairs();
    assertEquals("not 1", 1, legPairs.length);

    quadrupedSupportPolygon.removeFootstep(RobotQuadrant.HIND_LEFT);

    JUnitTools.assertExceptionThrown(
        UndefinedOperationException.class,
        new RunnableThatThrows() {
          @Override
          public void run() throws Throwable {
            quadrupedSupportPolygon.getLegPairs();
          }
        });
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSingleRigidBodyRotation() {
    Random random = new Random(1766L);

    RigidBody elevator = new RigidBody("elevator", world);
    Vector3d jointAxis = RandomTools.generateRandomVector(random);
    jointAxis.normalize();
    RigidBodyTransform transformToParent = new RigidBodyTransform();
    transformToParent.setIdentity();
    RevoluteJoint joint =
        ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
    RigidBody body =
        ScrewTools.addRigidBody(
            "body",
            joint,
            RandomTools.generateRandomDiagonalMatrix3d(random),
            random.nextDouble(),
            new Vector3d());

    joint.setQ(random.nextDouble());
    joint.setQd(random.nextDouble());

    Momentum momentum = computeMomentum(elevator, world);

    momentum.changeFrame(world);
    FrameVector linearMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
    FrameVector angularMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());

    FrameVector linearMomentumCheck = new FrameVector(world);
    Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
    Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
    angularMomentumCheckVector.scale(joint.getQd());
    inertia.transform(angularMomentumCheckVector);
    FrameVector angularMomentumCheck =
        new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
    angularMomentumCheck.changeFrame(world);

    double epsilon = 1e-9;
    JUnitTools.assertTuple3dEquals(
        linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
    JUnitTools.assertTuple3dEquals(
        angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
    assertTrue(angularMomentum.length() > epsilon);
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testChainAgainstCentroidalMomentumMatrix() {
    Random random = new Random(17679L);

    ArrayList<RevoluteJoint> joints = new ArrayList<RevoluteJoint>();
    RigidBody elevator = new RigidBody("elevator", world);
    int nJoints = 10;
    Vector3d[] jointAxes = new Vector3d[nJoints];
    for (int i = 0; i < nJoints; i++) {
      Vector3d jointAxis = RandomTools.generateRandomVector(random);
      jointAxis.normalize();
      jointAxes[i] = jointAxis;
    }

    ScrewTestTools.createRandomChainRobot("randomChain", joints, elevator, jointAxes, random);
    InverseDynamicsJoint[] jointsArray = new RevoluteJoint[joints.size()];
    joints.toArray(jointsArray);
    ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator);

    for (RevoluteJoint joint : joints) {
      joint.setQ(random.nextDouble());
      joint.setQd(random.nextDouble());
    }

    Momentum momentum = computeMomentum(elevator, centerOfMassFrame);
    DenseMatrix64F momentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
    momentum.getMatrix(momentumMatrix);

    CentroidalMomentumMatrix centroidalMomentumMatrix =
        new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
    centroidalMomentumMatrix.compute();
    DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getMatrix();
    DenseMatrix64F jointVelocitiesMatrix =
        new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jointsArray), 1);
    ScrewTools.getJointVelocitiesMatrix(jointsArray, jointVelocitiesMatrix);
    DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
    CommonOps.mult(
        centroidalMomentumMatrixMatrix,
        jointVelocitiesMatrix,
        momentumFromCentroidalMomentumMatrix);

    double epsilon = 1e-9;
    assertEquals(momentum.getExpressedInFrame(), centerOfMassFrame);
    JUnitTools.assertMatrixEquals(momentumFromCentroidalMomentumMatrix, momentumMatrix, epsilon);
    double norm = NormOps.normP2(momentumMatrix);
    assertTrue(norm > epsilon);
  }
  @DeployableTestMethod(estimatedDuration = 0.3)
  @Test(timeout = 30000)
  public void test() {
    Random random = new Random(176L);
    int matrixSize = 5;
    YoVariableRegistry parentRegistry = new YoVariableRegistry("test");
    DenseMatrix64F jacobianMatrix = RandomMatrices.createRandom(matrixSize, matrixSize, random);
    DenseMatrix64F taskVelocity = RandomMatrices.createRandom(matrixSize, 1, random);

    DampedLeastSquaresJacobianSolver solver =
        new DampedLeastSquaresJacobianSolver("testSolver", matrixSize, parentRegistry);
    solver.setAlpha(1.0);
    DenseMatrix64F jointVelocities = new DenseMatrix64F(matrixSize, 1);
    solver.setJacobian(jacobianMatrix);
    solver.solve(jointVelocities, taskVelocity);

    DenseMatrix64F taskVelocityBack = new DenseMatrix64F(matrixSize, 1);
    solver.inverseSolve(taskVelocityBack, jointVelocities);

    double delta = 1e-9;
    JUnitTools.assertMatrixEquals(taskVelocity, taskVelocityBack, delta);
  }