private CombinedTerrainObject3D setUpGround(String name) {
    CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);

    combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.DarkGray());
    combinedTerrainObject.addBox(2.0, -0.05, 3.0, 0.05, 2.0, YoAppearance.Beige());
    combinedTerrainObject.addBox(
        3.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.x,
        -0.05,
        4.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.x,
        0.05,
        2.0,
        YoAppearance.Beige());

    return combinedTerrainObject;
  }
  private CombinedTerrainObject3D setUpGround(String name) {
    CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);

    combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.DarkBlue());

    return combinedTerrainObject;
  }
  public void addVerticalDebrisLeaningAgainstAWall(
      double xRelativeToRobot, double yRelativeToRobot, double debrisYaw, double debrisPitch) {
    Point3d tempPosition =
        new Point3d(xRelativeToRobot, yRelativeToRobot, debrisLength / 2.0 * Math.cos(debrisPitch));
    FramePose debrisPose = generateDebrisPose(tempPosition, debrisYaw, debrisPitch, 0.0);
    debrisRobots.add(createDebrisRobot(debrisPose));

    double supportWidth = 0.1;
    double supportLength = 0.2;
    double supportHeight = 1.05 * debrisLength;

    RigidBodyTransform debrisTransform = new RigidBodyTransform();
    debrisPose.getPose(debrisTransform);
    TransformTools.rotate(debrisTransform, -debrisPitch, Axis.Y);
    debrisPose.setPose(debrisTransform);
    debrisPose.setZ(0.0);
    PoseReferenceFrame debrisReferenceFrame =
        new PoseReferenceFrame("debrisReferenceFrame", debrisPose);

    FramePose supportPose = new FramePose(debrisReferenceFrame);

    double x = supportWidth / 2.0 + debrisLength / 2.0 * Math.sin(debrisPitch);
    double y = 0.0;
    double z = supportHeight / 2.0;

    supportPose.setPosition(x, y, z);
    supportPose.changeFrame(constructionWorldFrame);

    RigidBodyTransform supportTransform = new RigidBodyTransform();
    supportPose.getPose(supportTransform);
    combinedTerrainObject.addRotatableBox(
        supportTransform, supportWidth, supportLength, supportHeight, YoAppearance.AliceBlue());
  }
  /** Joints */
  public Step2Robot() {
    super("v2Robot");
    this.setGravity(-9.81);

    // Body (2DOF = Z + Pitch)
    bodyJoint1 = new SliderJoint("bodyZ", new Vector3d(0.0, 0.0, 0.0), this, Axis.Z);
    bodyJoint1.setDynamic(true);
    Link bodyLinkSlider = setNullLink();
    bodyJoint1.setLink(bodyLinkSlider);
    this.addRootJoint(bodyJoint1);
    bodyJoint1.setInitialState(legHeight, 0.0);

    bodyJoint2 = new PinJoint("bodyPitch", new Vector3d(0.0, 0.0, 0.0), this, Axis.Y);
    bodyJoint2.setDynamic(true);
    Link bodyLinkPitch = body();
    bodyJoint2.setLink(bodyLinkPitch);
    bodyJoint1.addJoint(bodyJoint2);

    // Upper Joint
    hipJoint = new PinJoint("hip", new Vector3d(0.0, 0.0, 0.0), this, Axis.Y);
    hipJoint.setDynamic(true);
    hipJoint.setLimitStops(0.0, 1.0, 1e6, 1e3);
    Link upperLink = upperLink();
    hipJoint.setLink(upperLink);
    bodyJoint2.addJoint(hipJoint);

    // Lower Joint
    kneeJoint = new SliderJoint("knee", new Vector3d(0.0, 0.0, -upperLinkLength), this, Axis.Z);
    kneeJoint.setDynamic(true);
    kneeJoint.setLimitStops(0.0, 0.6, 1e9, 1e2);
    Link lowerLink = lowerLink();
    kneeJoint.setLink(lowerLink);
    hipJoint.addJoint(kneeJoint);

    // Visible ground contact point
    GroundContactPoint contactPoint =
        new GroundContactPoint("Foot", new Vector3d(0.0, 0.0, gcOffset), this);
    kneeJoint.addGroundContactPoint(contactPoint);
    Graphics3DObject graphics = kneeJoint.getLink().getLinkGraphics();
    graphics.identity();
    graphics.translate(0.0, 0.0, gcOffset);
    double radius = 0.03;
    graphics.addSphere(radius, YoAppearance.Orange());

    // Ground Model
    GroundContactModel groundModel =
        new LinearGroundContactModel(
            this, 1500, 150, 50000.0, 1e5, this.getRobotsYoVariableRegistry());
    GroundProfile3D profile = new FlatGroundProfile();
    groundModel.setGroundProfile3D(profile);
    this.setGroundContactModel(groundModel);
  }
  public DoublePendulum() {
    super("DoublePendulum");

    j1 = new PinJoint("j1", new Vector3d(0, 0, 2), this, Axis.X);

    Link l1 = new Link("l1");
    l1.setComOffset(0, 0, 0.5);
    l1.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
    l1.addEllipsoidFromMassProperties(YoAppearance.Pink());
    j1.setLink(l1);

    j2 = new PinJoint("j2", new Vector3d(0.0, 0.0, 1.0), this, Axis.X);
    Link l2 = new Link("l2");
    l2.setComOffset(0, 0, 0.5);
    l2.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
    l2.addEllipsoidFromMassProperties(YoAppearance.Purple());

    j2.setLink(l2);

    j1.addJoint(j2);
    addRootJoint(j1);
  }
  private Link ball() {
    Link ret = new Link("ball");

    ret.setMass(M1);
    ret.setMomentOfInertia(Ixx1, Iyy1, Izz1);

    ret.setComOffset(0.0, 0.0, 0.0);

    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.addSphere(R / 2.0, YoAppearance.EarthTexture());
    ret.setLinkGraphics(linkGraphics);

    return ret;
  }
  private void addRock(Vector3d normal, double centroidHeight, double[][] vertices) {
    ArrayList<Point2d> vertexPoints = new ArrayList<Point2d>();

    for (double[] point : vertices) {
      Point2d point2d = new Point2d(point);
      vertexPoints.add(point2d);
    }

    ConvexPolygon2d convexPolygon = new ConvexPolygon2d(vertexPoints);
    RotatableConvexPolygonTerrainObject rock =
        new RotatableConvexPolygonTerrainObject(
            normal, convexPolygon, centroidHeight, YoAppearance.Red());
    this.combinedTerrainObject.addTerrainObject(rock);
  }
  private Link lowerLink() {

    Link ret = new Link("lowerLink");
    ret.setMass(lowerLinkMass);

    // Inertia tensor
    double IxxCyl = (lowerLinkMass / 3) * (Math.pow(lowerLinkLength, 2.0));
    double IzzCyl = IxxCyl;
    ret.setMomentOfInertia(IxxCyl, 0.0, IzzCyl);

    // Graphics
    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.addCylinder(-lowerLinkLength, lowerLinkRadius, YoAppearance.Glass());

    ret.setLinkGraphics(linkGraphics);
    ret.setComOffset(0.0, 0.0, -lowerLinkLength / 2.0);
    ret.addCoordinateSystemToCOM(0.4);

    return ret;
  }
  private void addWall() {
    Vector3d normal = new Vector3d(0.0, 0.0, 1.0);
    double centroidHeight = 2.0;
    ArrayList<Point2d> pointList = new ArrayList<Point2d>();

    Point2d wallPoint0 = new Point2d(WALL_START_X, WALL_Y);
    Point2d wallPoint1 = new Point2d(WALL_START_X + WALL_LENGTH, WALL_Y);
    Point2d wallPoint2 =
        new Point2d(WALL_START_X + WALL_LENGTH, WALL_Y + Math.signum(WALL_Y) * WALL_THICKNESS);
    Point2d wallPoint3 = new Point2d(WALL_START_X, WALL_Y + Math.signum(WALL_Y) * WALL_THICKNESS);
    pointList.add(wallPoint0);
    pointList.add(wallPoint1);
    pointList.add(wallPoint2);
    pointList.add(wallPoint3);

    ConvexPolygon2d convexPolygon = new ConvexPolygon2d(pointList);
    RotatableConvexPolygonTerrainObject rightWall =
        new RotatableConvexPolygonTerrainObject(
            normal, convexPolygon, centroidHeight, YoAppearance.Brown());
    combinedTerrainObject.addTerrainObject(rightWall);
  }
  private void addPillars() {
    Vector3d normal = new Vector3d(0.0, 0.0, 1.0);
    double centroidHeight = 2.0;

    Point2d bottomLeft = new Point2d(-PILLAR_WIDTH / 2.0, PILLAR_WIDTH / 2.0);
    Point2d bottomRight = new Point2d(-PILLAR_WIDTH / 2.0, -PILLAR_WIDTH / 2.0);
    Point2d topLeft = new Point2d(PILLAR_WIDTH / 2.0, PILLAR_WIDTH / 2.0);
    Point2d topRight = new Point2d(PILLAR_WIDTH / 2.0, -PILLAR_WIDTH / 2.0);

    double pillarDistance = WALL_LENGTH / (NUM_PILLARS - 1.0);
    Vector2d offset = new Vector2d(0.0, -WALL_Y + PILLAR_WIDTH / 2.0);

    for (int i = 0; i < NUM_PILLARS; i++) {
      ArrayList<Point2d> points = new ArrayList<Point2d>();
      offset.setX(WALL_START_X + pillarDistance * i);

      Point2d localBottomLeft = new Point2d();
      localBottomLeft.add(bottomLeft, offset);
      Point2d localBottomRight = new Point2d();
      localBottomRight.add(bottomRight, offset);
      Point2d localTopLeft = new Point2d();
      localTopLeft.add(topLeft, offset);
      Point2d localTopRight = new Point2d();
      localTopRight.add(topRight, offset);

      points.add(localBottomLeft);
      points.add(localBottomRight);
      points.add(localTopLeft);
      points.add(localTopRight);

      ConvexPolygon2d convexPolygon = new ConvexPolygon2d(points);
      AppearanceDefinition appearance = YoAppearance.Brown();
      //         YoAppearance.makeTransparent(appearance, 0.7f);
      RotatableConvexPolygonTerrainObject pillar =
          new RotatableConvexPolygonTerrainObject(
              normal, convexPolygon, centroidHeight, appearance);
      combinedTerrainObject.addTerrainObject(pillar);
    }
  }
  private Link body() {

    Link ret = new Link("body");
    ret.setMass(bodyMass);

    // Inertia tensor
    double IxxCube = (bodyMass / 12.0) * (Math.pow(cubeW, 2.0) + Math.pow(cubeL, 2.0));
    double IyyCube = (bodyMass / 12.0) * (Math.pow(cubeH, 2.0) + Math.pow(cubeW, 2.0));
    double IzzCube = (bodyMass / 12.0) * (Math.pow(cubeH, 2.0) + Math.pow(cubeL, 2.0));
    ret.setMomentOfInertia(IxxCube, IyyCube, IzzCube);

    // Graphics
    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.addCube(cubeL, cubeW, cubeH, YoAppearance.Glass());

    ret.setLinkGraphics(linkGraphics);
    ret.setComOffset(0.0, 0.0, cubeH / 2.0); // Move the CoM to the center
    // of the body
    ret.addCoordinateSystemToCOM(0.4);

    return ret;
  }
  public TwoWaypointSwingGenerator(
      String namePrefix,
      double maxSwingHeight,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
    parentRegistry.addChild(registry);

    stepTime = new DoubleYoVariable(namePrefix + "StepTime", registry);
    timeIntoStep = new DoubleYoVariable(namePrefix + "TimeIntoStep", registry);
    isDone = new BooleanYoVariable(namePrefix + "IsDone", registry);
    trajectoryType =
        new EnumYoVariable<>(namePrefix + "TrajectoryType", registry, TrajectoryType.class);
    swingHeight = new DoubleYoVariable(namePrefix + "SwingHeight", registry);
    swingHeight.set(defaultSwingHeight);

    this.maxSwingHeight = new DoubleYoVariable(namePrefix + "MaxSwingHeight", registry);
    this.maxSwingHeight.set(maxSwingHeight);

    trajectory =
        new Position2dOptimizedSwingTrajectoryGenerator(
            namePrefix, registry, yoGraphicsListRegistry, maxTimeIterations);

    for (int i = 0; i < numberWaypoints; i++) waypointPositions.add(new FramePoint());

    if (yoGraphicsListRegistry != null)
      waypointViz =
          new BagOfBalls(
              numberWaypoints,
              0.02,
              namePrefix + "Waypoints",
              YoAppearance.White(),
              registry,
              yoGraphicsListRegistry);
    else waypointViz = null;
  }
  public FallingSphereRobot(String name, boolean useImpulseGroundModel) {
    super(name);

    this.setGravity(0.0, 0.0, -G);

    // Base:

    floatingJoint = new FloatingJoint("base", new Vector3d(0.0, 0.0, 0.0), this);

    Link link1 = ball();
    floatingJoint.setLink(link1);
    this.addRootJoint(floatingJoint);

    YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry();

    // Ground contact points:
    int N = 20; // 8;

    for (int i = 0; i < N; i++) {
      double latitude = -Math.PI / 2.0 + (i * Math.PI) / N;

      int nForThisLatitude = (int) ((Math.cos(latitude) * N) + 0.5);

      for (int j = 0; j < nForThisLatitude; j++) {
        double longitude = (j * 2.0 * Math.PI) / nForThisLatitude;

        double z = R * Math.sin(latitude);
        double x = R * Math.cos(latitude) * Math.cos(longitude);
        double y = R * Math.cos(latitude) * Math.sin(longitude);

        // System.out.println("x,y,z: " + x + ", " + y + ", " + z);
        String gcName = "gc" + i + "_" + j;
        GroundContactPoint gc = new GroundContactPoint(gcName, new Vector3d(x, y, z), this);
        floatingJoint.addGroundContactPoint(gc);

        YoGraphicPosition dynamicGraphicPosition =
            new YoGraphicPosition(
                gcName + "Position", gc.getYoPosition(), 0.01, YoAppearance.Red());
        yoGraphicsListRegistry.registerYoGraphic("FallingSphereGCPoints", dynamicGraphicPosition);

        if (useImpulseGroundModel) {
          YoGraphicVector dynamicGraphicVector =
              new YoGraphicVector(
                  gcName + "Force",
                  gc.getYoPosition(),
                  gc.getYoImpulse(),
                  10.0,
                  YoAppearance.Pink());
          yoGraphicsListRegistry.registerYoGraphic("FallingSphereForces", dynamicGraphicVector);
        } else {
          YoGraphicVector dynamicGraphicVector =
              new YoGraphicVector(
                  gcName + "Force", gc.getYoPosition(), gc.getYoForce(), 1.0 / 50.0);
          yoGraphicsListRegistry.registerYoGraphic("FallingSphereForces", dynamicGraphicVector);
        }
      }
    }

    GroundContactModel groundContactModel;
    if (useImpulseGroundModel) {
      groundContactModel = new CollisionGroundContactModel(this, EPSILON, MU, registry);
    } else {
      double kXY = 1000.0; // 1422.0;
      double bXY = 100.0; // 150.6;
      double kZ = 20.0; // 50.0;
      double bZ = 50.0; // 1000.0;

      groundContactModel = new LinearGroundContactModel(this, kXY, bXY, kZ, bZ, registry);
    }

    double amplitude = 0.1;
    double frequency = 0.3;
    double offset = 0.5;
    GroundProfile3D groundProfile = new RollingGroundProfile(amplitude, frequency, offset);
    groundContactModel.setGroundProfile3D(groundProfile);
    this.setGroundContactModel(groundContactModel);

    initRobot();

    this.getRobotsYoVariableRegistry().addChild(registry);
    this.addDynamicGraphicObjectsListRegistry(yoGraphicsListRegistry);
  }
  public void addHorizontalDebrisLeaningOnTwoBoxes(
      Point3d positionOfCenterOfDebrisWithRespectToRobot, double debrisYaw, double debrisRoll) {
    double supportWidth = 0.1;
    double supportLength = 0.2;
    double supportHeight;

    double x;
    double y;
    double z;

    FramePose debrisPose =
        generateDebrisPose(positionOfCenterOfDebrisWithRespectToRobot, debrisYaw, 0.0, debrisRoll);
    debrisRobots.add(createDebrisRobot(debrisPose));

    RigidBodyTransform debrisTransform = new RigidBodyTransform();
    debrisPose.getPose(debrisTransform);
    TransformTools.rotate(debrisTransform, -debrisRoll, Axis.X);
    debrisPose.setPose(debrisTransform);
    debrisPose.setZ(0.0);
    PoseReferenceFrame debrisReferenceFrame =
        new PoseReferenceFrame("debrisReferenceFrame", debrisPose);

    // add first support
    FramePose firstSupportPose = new FramePose(debrisReferenceFrame);
    supportHeight =
        positionOfCenterOfDebrisWithRespectToRobot.getZ()
            - debrisWidth / 2.0
            + debrisLength / 2.0 * Math.cos(debrisRoll);

    x = 0.0;
    y = -debrisLength / 2.0 * Math.sin(debrisRoll);
    z = supportHeight / 2.0;
    firstSupportPose.setPosition(x, y, z);
    firstSupportPose.changeFrame(constructionWorldFrame);
    RigidBodyTransform firstSupportTransform = new RigidBodyTransform();
    firstSupportPose.getPose(firstSupportTransform);
    combinedTerrainObject.addRotatableBox(
        firstSupportTransform,
        supportLength,
        supportWidth,
        supportHeight,
        YoAppearance.AliceBlue());

    // add second support
    FramePose secondSupportPose = new FramePose(debrisReferenceFrame);
    supportHeight =
        positionOfCenterOfDebrisWithRespectToRobot.getZ()
            - debrisWidth / 2.0
            - debrisLength / 2.0 * Math.cos(debrisRoll);

    x = 0.0;
    y = debrisLength / 2.0 * Math.sin(debrisRoll);
    z = supportHeight / 2.0;
    secondSupportPose.setPosition(x, y, z);
    secondSupportPose.changeFrame(constructionWorldFrame);
    RigidBodyTransform secondSupportTransform = new RigidBodyTransform();
    secondSupportPose.getPose(secondSupportTransform);
    combinedTerrainObject.addRotatableBox(
        secondSupportTransform,
        supportLength,
        supportWidth,
        supportHeight,
        YoAppearance.AliceBlue());
  }
  public CirclePoseTrajectoryGenerator(
      String namePrefix,
      ReferenceFrame trajectoryFrame,
      DoubleProvider trajectoryTimeProvider,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
    this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
    this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);

    //      this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
    // registry);
    this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
    //      this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
    // registry);

    this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;

    this.trajectoryFrame = trajectoryFrame;

    initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
    initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
    initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
    finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);

    isCurrentAngleBeingAdjusted =
        new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
    maximumAngleTrackingErrorTolerated =
        new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
    maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
    currentControlledFrameRelativeAngle =
        new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
    currentAngleTrackingError =
        new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
    currentAdjustedRelativeAngle =
        new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);

    desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
    currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);

    yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
    yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);

    yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
    yoCurrentVelocity =
        new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
    yoCurrentAcceleration =
        new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);

    yoInitialOrientation =
        new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
    yoFinalOrientation =
        new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);

    yoCurrentOrientation =
        new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
    yoCurrentAngularVelocity =
        new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
    yoCurrentAngularAcceleration =
        new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);

    yoInitialPositionWorld =
        new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
    yoFinalPositionWorld =
        new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
    yoCurrentPositionWorld =
        new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
    yoCurrentAdjustedPositionWorld =
        new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);

    circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
    rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
    rotationAxis.set(0.0, 0.0, 1.0);

    circleFrame =
        new ReferenceFrame("CircleFrame", trajectoryFrame) {
          private static final long serialVersionUID = 9102217353690768074L;

          private final Vector3d localTranslation = new Vector3d();
          private final Vector3d localRotationAxis = new Vector3d();
          private final AxisAngle4d localAxisAngle = new AxisAngle4d();

          @Override
          protected void updateTransformToParent(RigidBodyTransform transformToParent) {
            circleOrigin.get(localTranslation);
            rotationAxis.get(localRotationAxis);
            GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
            transformToParent.set(localAxisAngle, localTranslation);
          }
        };

    tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
    yoTangentialCircleFramePose =
        new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);

    if (this.visualize && yoGraphicsListRegistry != null) {
      final YoGraphicPosition currentPositionViz =
          new YoGraphicPosition(
              namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
      final YoGraphicPosition currentAdjustedPositionViz =
          new YoGraphicPosition(
              namePrefix + "CurrentAdjustedPosition",
              yoCurrentAdjustedPositionWorld,
              0.023,
              YoAppearance.Gold());
      final YoGraphicPosition initialPositionViz =
          new YoGraphicPosition(
              namePrefix + "InitialPosition",
              yoInitialPositionWorld,
              0.02,
              YoAppearance.BlueViolet());
      final YoGraphicPosition finalPositionViz =
          new YoGraphicPosition(
              namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
      final YoGraphicCoordinateSystem tangentialFrameViz =
          new YoGraphicCoordinateSystem(
              namePrefix + "TangentialFrame",
              yoTangentialCircleFramePose,
              0.2,
              YoAppearance.Pink());

      YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
      yoGraphicsList.add(currentPositionViz);
      yoGraphicsList.add(currentAdjustedPositionViz);
      yoGraphicsList.add(initialPositionViz);
      yoGraphicsList.add(finalPositionViz);
      yoGraphicsList.add(tangentialFrameViz);
      yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);

      bagOfBalls =
          new BagOfBalls(
              numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);

      showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
      showViz.addVariableChangedListener(
          new VariableChangedListener() {
            public void variableChanged(YoVariable<?> v) {
              boolean visible = showViz.getBooleanValue();
              currentPositionViz.setVisible(visible);
              currentAdjustedPositionViz.setVisible(visible);
              initialPositionViz.setVisible(visible);
              finalPositionViz.setVisible(visible);
              tangentialFrameViz.setVisible(visible);
              bagOfBalls.setVisible(visible);
            }
          });
      showViz.notifyVariableChangedListeners();
    } else {
      visualize = false;
      bagOfBalls = null;
      showViz = null;
    }

    parentRegistry.addChild(registry);
  }
  /** Joints */
  public Step6IDandSCSRobot_pinKnee() {
    super("Robot");

    /** **************** ID ROBOT ********************** */
    elevator = new RigidBody("elevator", elevatorFrame);

    bodyJointID = new SixDoFJoint(JointNames.BODY.getName(), elevator, elevatorFrame);
    createAndAttachBodyRB(LinkNames.BODY_LINK, bodyJointID);

    for (RobotSide robotSide : RobotSide.values) {
      // HIP ID (location, joint, and rigidBody)
      Vector3d hipOffset = new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0);
      RevoluteJoint hipJointID =
          ScrewTools.addRevoluteJoint(
              JointNames.HIP.getName(),
              bodyJointID.getSuccessor(),
              hipOffset,
              jointAxesPinJoints); // The parent rigid body of the hip joint is:
                                   // bodyJointID.getSuccessor()
      allLegJoints.get(robotSide).put(JointNames.HIP, hipJointID);
      createAndAttachCylinderRB(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);

      // KNEE ID
      Vector3d kneeOffset = new Vector3d(0.0, 0.0, kneeOffsetZ);
      RevoluteJoint kneeJointID =
          ScrewTools.addRevoluteJoint(
              JointNames.KNEE.getName(), hipJointID.getSuccessor(), kneeOffset, jointAxesPinJoints);
      allLegJoints.get(robotSide).put(JointNames.KNEE, kneeJointID);
      createAndAttachCylinderRB(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);

      // ANKLE ID (location, joint, and rigidBody)
      Vector3d ankleOffset = new Vector3d(0.0, 0.0, ankleOffsetZ);
      RevoluteJoint ankleJointID =
          ScrewTools.addRevoluteJoint(
              JointNames.ANKLE.getName(),
              kneeJointID.getSuccessor(),
              ankleOffset,
              jointAxesPinJoints);
      allLegJoints.get(robotSide).put(JointNames.ANKLE, ankleJointID);
      createAndAttachFootRB(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
    }

    /** **************** SCS ROBOT ********************** */
    // BODY SCS
    bodyJointSCS = new FloatingPlanarJoint(JointNames.BODY.getName(), this);
    this.addRootJoint(bodyJointSCS);
    createAndAttachBodyLink(LinkNames.BODY_LINK);
    bodyJointSCS.setCartesianPosition(0.0, initialBodyHeight);

    for (RobotSide robotSide : RobotSide.values) {
      // HIP SCS
      PinJoint hipJointSCS =
          new PinJoint(
              robotSide.getSideNameFirstLetter() + JointNames.HIP.getName(),
              new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0),
              this,
              jointAxesPinJoints);
      hipJointSCS.setLimitStops(-0.75, 0.75, 1e3, 1e1);
      // It is NOT necessary to set limits in the ID description because if the SCS description
      // doesn't let the robot move passed a point the ID robot won't be able to pass it either

      if (robotSide == RobotSide.LEFT) {
        hipJointSCS.setQ(-0.6);
      } else {
        hipJointSCS.setQ(0.1);
      }

      bodyJointSCS.addJoint(hipJointSCS);
      idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.HIP), hipJointSCS);
      createAndAttachCylinderLink(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);

      // KNEE SCS
      PinJoint kneeJointSCS =
          new PinJoint(
              robotSide.getSideNameFirstLetter() + JointNames.KNEE.getName(),
              new Vector3d(0.0, 0.0, kneeOffsetZ),
              this,
              jointAxesPinJoints);
      kneeJointSCS.setLimitStops(-0.01, 1.8, 1e5, 1e3);
      hipJointSCS.addJoint(kneeJointSCS);
      idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.KNEE), kneeJointSCS);
      createAndAttachCylinderLink(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);

      // ANKLE SCS
      PinJoint ankleJointSCS =
          new PinJoint(
              robotSide.getSideNameFirstLetter() + JointNames.ANKLE.getName(),
              new Vector3d(0.0, 0.0, ankleOffsetZ),
              this,
              jointAxesPinJoints);
      ankleJointSCS.setLimitStops(-0.7, 0.7, 1e3, 1e2);

      if (robotSide == RobotSide.RIGHT) {
        ankleJointSCS.setQ(-0.1);
      }

      kneeJointSCS.addJoint(ankleJointSCS);
      idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.ANKLE), ankleJointSCS);

      // FEET SCS
      createAndAttachFootLink(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
      GroundContactPoint gcHeel =
          new GroundContactPoint(
              robotSide.getSideNameFirstLetter() + "gcHeel",
              new Vector3d(-0.1, 0.0, gcOffset),
              this);
      GCpointsHeel.set(robotSide, gcHeel);
      ankleJointSCS.addGroundContactPoint(gcHeel);
      Graphics3DObject graphicsGCheel = ankleJointSCS.getLink().getLinkGraphics();
      graphicsGCheel.identity();
      graphicsGCheel.translate(-0.1, 0.0, gcOffset);
      graphicsGCheel.addSphere(gcRadius, YoAppearance.Orange());

      if (robotSide == RobotSide.RIGHT) {
        setFStoTrue(robotSide);
      }

      GroundContactPoint gcToe =
          new GroundContactPoint(
              robotSide.getSideNameFirstLetter() + "gcToe", new Vector3d(0.4, 0.0, gcOffset), this);
      GCpointsToe.set(robotSide, gcToe);
      ankleJointSCS.addGroundContactPoint(gcToe);
      Graphics3DObject graphics = ankleJointSCS.getLink().getLinkGraphics();
      graphics.identity();
      graphics.translate(0.4, 0.0, gcOffset);
      graphics.addSphere(gcRadius, YoAppearance.DarkOliveGreen());
    }

    /** ************** SCS Ground Model ************************ */
    GroundContactModel groundModel =
        new LinearGroundContactModel(this, 1e3, 1e3, 5e3, 1e3, this.getRobotsYoVariableRegistry());
    GroundProfile3D profile = new FlatGroundProfile();
    groundModel.setGroundProfile3D(profile);
    this.setGroundContactModel(groundModel);
  }
public class ClassicCameraController
    implements TrackingDollyCameraController,
        KeyListener,
        MouseListener,
        Mouse3DListener,
        SelectedListener {
  public static final double MIN_FIELD_OF_VIEW = 0.001;
  public static final double MAX_FIELD_OF_VIEW = 2.0;

  private static final double MIN_CAMERA_POSITION_TO_FIX_DISTANCE = 0.1; // 0.8;

  public static final double CAMERA_START_X = 0.0;
  public static final double CAMERA_START_Y = -6.0;
  public static final double CAMERA_START_Z = 1.0;

  private double fieldOfView = CameraConfiguration.DEFAULT_FIELD_OF_VIEW;
  private double clipDistanceNear = CameraConfiguration.DEFAULT_CLIP_DISTANCE_NEAR;
  private double clipDistanceFar = CameraConfiguration.DEFAULT_CLIP_DISTANCE_FAR;

  private double camX, camY, camZ, fixX, fixY, fixZ;

  private double zoom_factor = 1.0;
  private double rotate_factor = 1.0;
  private double rotate_camera_factor = 1.0;

  private boolean isMounted = false;
  private CameraMountInterface cameraMount;

  private boolean isTracking = true, isTrackingX = true, isTrackingY = true, isTrackingZ = false;
  private boolean isDolly = false, isDollyX = true, isDollyY = true, isDollyZ = false;
  private double trackDX = 0.0, trackDY = 0.0, trackDZ = 0.0;
  private double dollyDX = 2.0, dollyDY = 12.0, dollyDZ = 0.0;

  private ViewportAdapter viewportAdapter;
  private JFrame jFrame;

  // Flying
  private boolean fly = true;
  private boolean forward = false;
  private boolean backward = false;
  private boolean left = false;
  private boolean right = false;
  private boolean up = false;
  private boolean down = false;

  private ArrayList<Point3d> storedCameraPositions = new ArrayList<Point3d>(0);
  private ArrayList<Point3d> storedFixPositions = new ArrayList<Point3d>(0);
  private int storedPositionIndex = 0;
  private boolean transitioning = false;

  private static double transitionTime = 500;
  private double camXSpeed;
  private double camYSpeed;
  private double camZSpeed;
  private double fixXSpeed;
  private double fixYSpeed;
  private double fixZSpeed;
  private long lastTransitionTime;

  private ArrayList<Point3d> keyFrameCamPos = new ArrayList<Point3d>(0);
  private ArrayList<Point3d> keyFrameFixPos = new ArrayList<Point3d>(0);
  private ArrayList<Integer> keyFrameTimes = new ArrayList<Integer>(0);

  private Graphics3DNode fixPointNode =
      new Graphics3DNode(
          "cameraFixPoint",
          new Graphics3DObject(new Sphere3d(0.01), YoAppearance.RGBColor(1.0, 0.0, 0.0, 0.5)));

  private boolean toggleCameraKeyPoints = false;
  private int cameraKeyPointIndex;
  private ArrayList<Integer> cameraKeyPoints = new ArrayList<Integer>(0);

  private CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder;

  private Graphics3DAdapter graphics3dAdapter;

  public static ClassicCameraController createClassicCameraControllerAndAddListeners(
      ViewportAdapter viewportAdapter,
      CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder,
      Graphics3DAdapter graphics3dAdapter) {
    return createClassicCameraControllerAndAddListeners(
        viewportAdapter, cameraTrackAndDollyVariablesHolder, graphics3dAdapter, null);
  }

  public static ClassicCameraController createClassicCameraControllerAndAddListeners(
      ViewportAdapter viewportAdapter,
      CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder,
      Graphics3DAdapter graphics3dAdapter,
      JFrame jFrame) {
    ClassicCameraController classicCameraController =
        new ClassicCameraController(
            graphics3dAdapter, viewportAdapter, cameraTrackAndDollyVariablesHolder, jFrame);
    graphics3dAdapter.addKeyListener(classicCameraController);
    graphics3dAdapter.addMouseListener(classicCameraController);
    graphics3dAdapter.addMouse3DListener(classicCameraController);
    graphics3dAdapter.addSelectedListener(classicCameraController);

    return classicCameraController;
  }

  public ClassicCameraController(
      Graphics3DAdapter graphics3dAdapter,
      ViewportAdapter viewportAdapter,
      CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder) {
    this(graphics3dAdapter, viewportAdapter, cameraTrackAndDollyVariablesHolder, null);
  }

  public ClassicCameraController(
      Graphics3DAdapter graphics3dAdapter,
      ViewportAdapter viewportAdapter,
      CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder,
      JFrame jFrame) {
    if (graphics3dAdapter == null) throw new RuntimeException("graphics3dAdapter == null");
    this.graphics3dAdapter = graphics3dAdapter;
    this.viewportAdapter = viewportAdapter;
    this.jFrame = jFrame;

    this.camX = CAMERA_START_X;
    this.camY = CAMERA_START_Y;
    this.camZ = CAMERA_START_Z;
    this.fixX = 0.0;
    this.fixY = 0.0;
    this.fixZ = 0.6;

    this.cameraTrackAndDollyVariablesHolder = cameraTrackAndDollyVariablesHolder;

    // Don't do this stuff by default
    setTracking(false);
    setDolly(false);
  }

  public void setCameraMount(CameraMountInterface mount) {
    this.cameraMount = mount;
  }

  public CameraMountInterface getCameraMount() {
    return cameraMount;
  }

  public boolean isMounted() {
    return isMounted;
  }

  @Override
  public CameraTrackingAndDollyPositionHolder getCameraTrackAndDollyVariablesHolder() {
    return cameraTrackAndDollyVariablesHolder;
  }

  @Override
  public void setConfiguration(CameraConfiguration config, CameraMountList mountList) {
    if (config == null) return;

    this.isMounted = config.isCameraMounted();
    if (isMounted && (mountList != null)) {
      this.cameraMount = mountList.getCameraMount(config.getCameraMountName());
    }

    this.camX = config.camX;
    this.camY = config.camY;
    this.camZ = config.camZ;
    this.fixX = config.fixX;
    this.fixY = config.fixY;
    this.fixZ = config.fixZ;

    this.isTracking = config.isTracking;
    this.isTrackingX = config.isTrackingX;
    this.isTrackingY = config.isTrackingY;
    this.isTrackingZ = config.isTrackingZ;
    this.isDolly = config.isDolly;
    this.isDollyX = config.isDollyX;
    this.isDollyY = config.isDollyY;
    this.isDollyZ = config.isDollyZ;

    this.trackDX = config.trackDX;
    this.trackDY = config.trackDY;
    this.trackDZ = config.trackDZ;
    this.dollyDX = config.dollyDX;
    this.dollyDY = config.dollyDY;
    this.dollyDZ = config.dollyDZ;

    setFieldOfView(config.fieldOfView);
    this.clipDistanceFar = config.clipDistanceFar;
    this.clipDistanceNear = config.clipDistanceNear;

    // this.update();
  }

  @Override
  public boolean isTracking() {
    return isTracking;
  }

  @Override
  public boolean isTrackingX() {
    return isTrackingX;
  }

  @Override
  public boolean isTrackingY() {
    return isTrackingY;
  }

  @Override
  public boolean isTrackingZ() {
    return isTrackingZ;
  }

  @Override
  public boolean isDolly() {
    return isDolly;
  }

  @Override
  public boolean isDollyX() {
    return isDollyX;
  }

  @Override
  public boolean isDollyY() {
    return isDollyY;
  }

  @Override
  public boolean isDollyZ() {
    return isDollyZ;
  }

  @Override
  public void setTracking(boolean track, boolean trackX, boolean trackY, boolean trackZ) {
    setTracking(track);
    setTrackingX(trackX);
    setTrackingY(trackY);
    setTrackingZ(trackZ);
  }

  @Override
  public void setDolly(boolean dolly, boolean dollyX, boolean dollyY, boolean dollyZ) {
    setDolly(dolly);
    setDollyX(dollyX);
    setDollyY(dollyY);
    setDollyZ(dollyZ);
  }

  @Override
  public void setTrackingOffsets(double dx, double dy, double dz) {
    trackDX = dx;
    trackDY = dy;
    trackDZ = dz;
  }

  @Override
  public void setDollyOffsets(double dx, double dy, double dz) {
    dollyDX = dx;
    dollyDY = dy;
    dollyDZ = dz;
  }

  @Override
  public void setTracking(boolean track) {
    isTracking = track;
  }

  @Override
  public void setTrackingX(boolean trackX) {
    isTrackingX = trackX;
  }

  @Override
  public void setTrackingY(boolean trackY) {
    isTrackingY = trackY;
  }

  @Override
  public void setTrackingZ(boolean trackZ) {
    isTrackingZ = trackZ;
  }

  @Override
  public void setDolly(boolean dolly) {
    isDolly = dolly;
  }

  @Override
  public void setDollyX(boolean dollyX) {
    isDollyX = dollyX;
  }

  @Override
  public void setDollyY(boolean dollyY) {
    isDollyY = dollyY;
  }

  @Override
  public void setDollyZ(boolean dollyZ) {
    isDollyZ = dollyZ;
  }

  @Override
  public double getTrackingXOffset() {
    return trackDX;
  }

  @Override
  public double getTrackingYOffset() {
    return trackDY;
  }

  @Override
  public double getTrackingZOffset() {
    return trackDZ;
  }

  @Override
  public double getDollyXOffset() {
    return dollyDX;
  }

  @Override
  public double getDollyYOffset() {
    return dollyDY;
  }

  @Override
  public double getDollyZOffset() {
    return dollyDZ;
  }

  @Override
  public void setTrackingXOffset(double dx) {
    trackDX = dx;
  }

  @Override
  public void setTrackingYOffset(double dy) {
    trackDY = dy;
  }

  @Override
  public void setTrackingZOffset(double dz) {
    trackDZ = dz;
  }

  @Override
  public void setDollyXOffset(double dx) {
    dollyDX = dx;
  }

  @Override
  public void setDollyYOffset(double dy) {
    dollyDY = dy;
  }

  @Override
  public void setDollyZOffset(double dz) {
    dollyDZ = dz;
  }

  @Override
  public void update() {
    if (graphics3dAdapter.getContextManager().getCurrentViewport() != viewportAdapter) {
      forward = false;
      backward = false;
      left = false;
      right = false;
      up = false;
      down = false;
    }

    if (isTracking) {
      if (isTrackingX) {
        double trackX = cameraTrackAndDollyVariablesHolder.getTrackingX();
        if (!Double.isNaN(trackX)) fixX = trackX + trackDX;
      }

      if (isTrackingY) {
        double trackY = cameraTrackAndDollyVariablesHolder.getTrackingY();
        if (!Double.isNaN(trackY)) fixY = trackY + trackDY;
      }

      if (isTrackingZ) {
        double trackZ = cameraTrackAndDollyVariablesHolder.getTrackingZ();
        if (!Double.isNaN(trackZ)) fixZ = trackZ + trackDZ;
      }
    }

    if (isDolly) {
      double dollyX = cameraTrackAndDollyVariablesHolder.getDollyX();
      if (isDollyX) {
        if (!Double.isNaN(dollyX)) camX = dollyX + dollyDX;
      }

      if (isDollyY) {
        double dollyY = cameraTrackAndDollyVariablesHolder.getDollyY();
        if (!Double.isNaN(dollyY)) camY = dollyY + dollyDY;
      }

      if (isDollyZ) {
        double dollyZ = cameraTrackAndDollyVariablesHolder.getDollyZ();
        if (!Double.isNaN(dollyZ)) camZ = dollyZ + dollyDZ;
      }
    }

    double fieldOfView = cameraTrackAndDollyVariablesHolder.getFieldOfView();
    if (!Double.isNaN(fieldOfView)) setFieldOfView(fieldOfView);

    // Flying
    if (fly && !isTracking && !isDolly && !transitioning) {
      if (forward) {
        moveCameraForward(-0.5);
      }

      if (backward) {
        moveCameraForward(0.5);
      }

      if (left) {
        pan(20, 0);
      }

      if (right) {
        pan(-20, 0);
      }

      if (up) {
        pan(00, 20);
      }

      if (down) {
        pan(00, -20);
      }
    }

    // End Flying

    if (transitioning && !isTracking && !isDolly) {
      int numberOfDimensionsThatHaveTransitioned = 0;
      double elapsedTransitionTime = System.currentTimeMillis() - lastTransitionTime;
      lastTransitionTime = System.currentTimeMillis();

      if (Math.abs(camX - storedCameraPositions.get(storedPositionIndex).getX())
          <= Math.abs(camXSpeed * elapsedTransitionTime)) {
        camX = storedCameraPositions.get(storedPositionIndex).getX();
        numberOfDimensionsThatHaveTransitioned++;
      } else {
        camX += camXSpeed * elapsedTransitionTime;
      }

      if (Math.abs(camY - storedCameraPositions.get(storedPositionIndex).getY())
          <= Math.abs(camYSpeed * elapsedTransitionTime)) {
        camY = storedCameraPositions.get(storedPositionIndex).getY();
        numberOfDimensionsThatHaveTransitioned++;
      } else {
        camY += camYSpeed * elapsedTransitionTime;
      }

      if (Math.abs(camZ - storedCameraPositions.get(storedPositionIndex).getZ())
          <= Math.abs(camZSpeed * elapsedTransitionTime)) {
        camZ = storedCameraPositions.get(storedPositionIndex).getZ();
        numberOfDimensionsThatHaveTransitioned++;
      } else {
        camZ += camZSpeed * elapsedTransitionTime;
      }

      if (Math.abs(fixX - storedFixPositions.get(storedPositionIndex).getX())
          <= Math.abs(fixXSpeed * elapsedTransitionTime)) {
        fixX = storedFixPositions.get(storedPositionIndex).getX();
        numberOfDimensionsThatHaveTransitioned++;
      } else {
        fixX += fixXSpeed * elapsedTransitionTime;
      }

      if (Math.abs(fixY - storedFixPositions.get(storedPositionIndex).getY())
          <= Math.abs(fixYSpeed * elapsedTransitionTime)) {
        fixY = storedFixPositions.get(storedPositionIndex).getY();
        numberOfDimensionsThatHaveTransitioned++;
      } else {
        fixY += fixYSpeed * elapsedTransitionTime;
      }

      if (Math.abs(fixZ - storedFixPositions.get(storedPositionIndex).getZ())
          <= Math.abs(fixZSpeed * elapsedTransitionTime)) {
        fixZ = storedFixPositions.get(storedPositionIndex).getZ();
        numberOfDimensionsThatHaveTransitioned++;
      } else {
        fixZ += fixZSpeed * elapsedTransitionTime;
      }

      if (numberOfDimensionsThatHaveTransitioned == 6) {
        transitioning = false;
      }
    }
  }

  public void addKeyFrame(int time) {
    addKeyFrame(keyFrameCamPos.size(), time);
  }

  public void addKeyFrame(int i, int time) {
    keyFrameCamPos.add(i, new Point3d(camX, camY, camZ));
    keyFrameFixPos.add(i, new Point3d(fixX, fixY, fixZ));
    keyFrameTimes.add(i, time);
  }

  public int removeKeyFrameByTime(int time) {
    for (int i = 0; i < keyFrameTimes.size(); i++) {
      if (keyFrameTimes.get(i) == time) {
        removeKeyFrameByIndex(i);

        return i;
      }
    }

    return -1;
  }

  public void removeKeyFrameByIndex(int i) {
    if ((i >= 0) && (i < keyFrameTimes.size())) {
      keyFrameTimes.remove(i);
      keyFrameCamPos.remove(i);
      keyFrameFixPos.remove(i);
    }
  }

  @Override
  public void setKeyFrameTime(int time) {
    for (int i = keyFrameTimes.size() - 1; i >= 0; i--) {
      if (time >= keyFrameTimes.get(i)) {
        if (keyFrameTimes.size() > i + 1) {
          double elapsedTime = time - keyFrameTimes.get(i);
          double totalTime = keyFrameTimes.get(i + 1) - keyFrameTimes.get(i);
          camX =
              keyFrameCamPos.get(i).getX()
                  + (keyFrameCamPos.get(i + 1).getX() - keyFrameCamPos.get(i).getX())
                      * elapsedTime
                      / totalTime;
          camY =
              keyFrameCamPos.get(i).getY()
                  + (keyFrameCamPos.get(i + 1).getY() - keyFrameCamPos.get(i).getY())
                      * elapsedTime
                      / totalTime;
          camZ =
              keyFrameCamPos.get(i).getZ()
                  + (keyFrameCamPos.get(i + 1).getZ() - keyFrameCamPos.get(i).getZ())
                      * elapsedTime
                      / totalTime;

          fixX =
              keyFrameFixPos.get(i).getX()
                  + (keyFrameFixPos.get(i + 1).getX() - keyFrameFixPos.get(i).getX())
                      * elapsedTime
                      / totalTime;
          fixY =
              keyFrameFixPos.get(i).getY()
                  + (keyFrameFixPos.get(i + 1).getY() - keyFrameFixPos.get(i).getY())
                      * elapsedTime
                      / totalTime;
          fixZ =
              keyFrameFixPos.get(i).getZ()
                  + (keyFrameFixPos.get(i + 1).getZ() - keyFrameFixPos.get(i).getZ())
                      * elapsedTime
                      / totalTime;
        }

        break;
      }
    }
  }

  public void gotoKey(int index) {
    if ((index >= 0) && (index < keyFrameCamPos.size())) {
      storedPositionIndex = index;
      camX = keyFrameCamPos.get(index).getX();
      camY = keyFrameCamPos.get(index).getY();
      camZ = keyFrameCamPos.get(index).getZ();

      fixX = keyFrameFixPos.get(index).getX();
      fixY = keyFrameFixPos.get(index).getY();
      fixZ = keyFrameFixPos.get(index).getZ();
    }
  }

  public ArrayList<Integer> getCameraKeyPoints() {
    return cameraKeyPoints;
  }

  @Override
  public double getFixX() {
    return this.fixX;
  }

  @Override
  public double getFixY() {
    return this.fixY;
  }

  @Override
  public double getFixZ() {
    return this.fixZ;
  }

  @Override
  public double getCamX() {
    return this.camX;
  }

  @Override
  public double getCamY() {
    return this.camY;
  }

  @Override
  public double getCamZ() {
    return this.camZ;
  }

  @Override
  public void setFixX(double fx) {
    this.fixX = fx;
  }

  @Override
  public void setFixY(double fy) {
    this.fixY = fy;
  }

  @Override
  public void setFixZ(double fz) {
    this.fixZ = fz;
  }

  @Override
  public void setCamX(double cx) {
    this.camX = cx;
  }

  @Override
  public void setCamY(double cy) {
    this.camY = cy;
  }

  @Override
  public void setCamZ(double cz) {
    this.camZ = cz;
  }

  @Override
  public void setFixPosition(double fx, double fy, double fz) {
    this.fixX = fx;
    this.fixY = fy;
    this.fixZ = fz;
  }

  @Override
  public void setCameraPosition(double cx, double cy, double cz) {
    this.camX = cx;
    this.camY = cy;
    this.camZ = cz;
  }

  private Vector3d v3d = new Vector3d();
  private RigidBodyTransform t3d = new RigidBodyTransform();
  private Vector3d rotVector = new Vector3d();
  private AxisAngle4d rotAxisAngle4d = new AxisAngle4d();

  public void doMouseDraggedLeft(double dx, double dy) {
    // Rotate around fix point:

    double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
    v3d.set(delX0, delY0, delZ0);

    // double offsetDistance = v3d.length();

    t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
    t3d.transform(v3d);

    if (!isDolly || (!isDollyX && !isDollyY)) {
      camX = v3d.getX() + fixX;
      camY = v3d.getY() + fixY;
    }

    delX0 = camX - fixX;
    delY0 = camY - fixY;
    delZ0 = camZ - fixZ;

    // v3d.set(delX0, delY0, delZ0);
    rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
    rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);

    t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
    t3d.transform(v3d);

    if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
      if (!isDolly || (!isDollyX && !isDollyY)) {
        camX = v3d.getX() + fixX;
        camY = v3d.getY() + fixY;
      }

      if (!isDolly || !isDollyZ) {
        camZ = v3d.getZ() + fixZ;

        /*
         * double factor = elevate_factor * Math.abs(offsetDistance);
         * //camZ-fixZ); if (factor < elevate_factor) factor =
         * elevate_factor; //camZ = v3d.z + fixZ + dy * elevate_factor; camZ
         * = v3d.z + fixZ + dy * factor;
         */
      }
    }

    // transformChanged(currXform);
  }

  public void rotateAroundFix(double dx, double dy) {
    double distanceFromCameraToFix =
        Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camZ - fixZ, 2));

    if (distanceFromCameraToFix > 1.0) {
      dx /= distanceFromCameraToFix;
      dy /= distanceFromCameraToFix;
    }

    double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
    v3d.set(delX0, delY0, delZ0);

    t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
    t3d.transform(v3d);

    if (!isDolly || (!isDollyX && !isDollyY)) {
      camX = v3d.getX() + fixX;
      camY = v3d.getY() + fixY;
    }

    delX0 = camX - fixX;
    delY0 = camY - fixY;
    delZ0 = camZ - fixZ;

    rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
    rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);

    t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
    t3d.transform(v3d);

    if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
      if (!isDolly || (!isDollyX && !isDollyY)) {
        camX = v3d.getX() + fixX;
        camY = v3d.getY() + fixY;
      }

      if (!isDolly || !isDollyZ) {
        camZ = v3d.getZ() + fixZ;
      }
    }
  }

  public void doMouseDraggedRight(double dx, double dy) {
    // Elevate up and down
    double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
    v3d.set(delX0, delY0, delZ0);

    // double offsetDistance = v3d.length();

    t3d.setRotationYawAndZeroTranslation(-dx * rotate_camera_factor);
    t3d.transform(v3d);

    if (!isTracking || (!isTrackingX && !isTrackingY)) {
      fixX = camX - v3d.getX();
      fixY = camY - v3d.getY();
    }

    delX0 = camX - fixX;
    delY0 = camY - fixY;
    delZ0 = camZ - fixZ;

    // v3d.set(delX0, delY0, delZ0);

    rotVector.set(-1.0, 0.0, 0.0);
    rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
    rotAxisAngle4d.set(rotVector, dy * rotate_camera_factor / 4.0);

    t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
    t3d.transform(v3d);

    if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
      if (!isTracking || (!isTrackingX && !isTrackingY)) {
        fixX = camX - v3d.getX();
        fixY = camY - v3d.getY();
      }

      if (!isTracking || !isTrackingZ) {
        fixZ = camZ - v3d.getZ();

        /*
         * double factor = elevate_camera_factor * offsetDistance;
         * //Math.abs(camZ-fixZ); if (factor < elevate_camera_factor) factor
         * = elevate_camera_factor; fixZ = camZ - v3d.z + dy factor; //fixZ
         * = camZ - v3d.z + dy * elevate_factor;
         */
      }
    }

    // transformChanged(currXform);
  }

  @Override
  public void setFieldOfView(double fov) {
    fieldOfView = fov;

    if (fieldOfView < MIN_FIELD_OF_VIEW) fieldOfView = MIN_FIELD_OF_VIEW;
    if (fieldOfView > MAX_FIELD_OF_VIEW) fieldOfView = MAX_FIELD_OF_VIEW;
  }

  public void doMouseDraggedMiddle(double dx, double dy) {
    // Zooms in and out

    if ((this.isMounted) && (viewportAdapter != null)) {
      cameraMount.zoom(dy * 0.1);
    } else {
      Vector3d v3d = new Vector3d(camX - fixX, camY - fixY, camZ - fixZ);

      Vector3d offsetVec = new Vector3d(v3d);

      // offsetVec.normalize();
      offsetVec.scale(dy * zoom_factor);

      // if (offsetVec.length() < v3d.length())
      // {
      if (!isDolly || (!isDollyX && !isDollyY)) {
        camX += offsetVec.getX();
        camY += offsetVec.getY();
      }

      if (!isDolly || !isDollyZ) camZ += offsetVec.getZ();

      // }

      v3d.set(camX - fixX, camY - fixY, camZ - fixZ);

      if (v3d.length() < MIN_CAMERA_POSITION_TO_FIX_DISTANCE) {
        v3d.normalize();
        v3d.scale(MIN_CAMERA_POSITION_TO_FIX_DISTANCE);
        camX = v3d.getX() + fixX;
        camY = v3d.getY() + fixY;
        camZ = v3d.getZ() + fixZ;
      }
    }

    // transformChanged(currXform);
  }

  private void moveCameraForward(double distance) {
    double angleXY = Math.atan2(camY - fixY, camX - fixX);
    double angleZ = Math.atan2(camZ - fixZ, Math.hypot(camY - fixY, camX - fixX));

    double distXY = distance * Math.cos(angleZ);

    camX += distXY * Math.cos(angleXY);
    camY += distXY * Math.sin(angleXY);
    camZ += distance * Math.sin(angleZ);

    if (Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camY - fixY, 2))
        < 1) {
      fixX += distXY * Math.cos(angleXY);
      fixY += distXY * Math.sin(angleXY);
      fixZ += distance * Math.sin(angleZ);
    }

    // Vector3d v3d = new Vector3d(camX - fixX, camY - fixY, camZ - fixZ);
    //
    // Vector3d offsetVec = new Vector3d(v3d);
    //
    //// offsetVec.normalize();
    // offsetVec.scale(distance * zoom_factor);
    //
    //// if (offsetVec.length() < v3d.length())
    //// {
    // if (!isDolly || (!isDollyX &&!isDollyY))
    // {
    // camX += offsetVec.x;
    // camY += offsetVec.y;
    // }
    //
    // if (!isDolly ||!isDollyZ)
    // camZ += offsetVec.z;
  }

  public void pan(double dx, double dy) {
    double distanceFromCameraToFix =
        Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camZ - fixZ, 2));
    dx *= distanceFromCameraToFix / viewportAdapter.getPhysicalWidth() * .00023;
    dy *= distanceFromCameraToFix / viewportAdapter.getPhysicalHeight() * .00007;
    double theta = Math.PI / 2 + Math.atan2((camZ - fixZ), Math.hypot(camX - fixX, camY - fixY));
    if (!isTracking || !isTrackingZ) {
      camZ += dy * Math.sin(theta);
      fixZ += dy * Math.sin(theta);
    }

    double d = dy * Math.cos(theta);
    theta = Math.atan2(camY - fixY, camX - fixX);

    if (!isTracking || !isTrackingY) {
      camY += d * Math.sin(theta);
      fixY += d * Math.sin(theta);
    }

    if (!isTracking || !isTrackingX) {
      camX += d * Math.cos(theta);
      fixX += d * Math.cos(theta);
    }

    theta = Math.PI / 2 + Math.atan2(camY - fixY, camX - fixX);

    if (!isTracking || !isTrackingY) {
      camY -= dx * Math.sin(theta);
      fixY -= dx * Math.sin(theta);
    }

    if (!isTracking || !isTrackingX) {
      camX -= dx * Math.cos(theta);
      fixX -= dx * Math.cos(theta);
    }
  }

  public void translateFix(double dx, double dy, double dz) {
    //      double zTiltAngle = Math.PI / 2 + Math.atan2(camZ - fixZ, Math.hypot(camX - fixX, camY -
    // fixY));

    //      double ky = dy * Math.sin(zTiltAngle);
    //      double kz = dz * Math.cos(zTiltAngle);

    double yTiltAngle = Math.atan2(camY - fixY, camX - fixX);

    if (!isTracking || !isTrackingZ) {
      camZ += dz;
      fixZ += dz;
    }

    if (!isTracking || !isTrackingY) {
      camY += dy * Math.sin(yTiltAngle);
      fixY += dy * Math.sin(yTiltAngle);
    }

    if (!isTracking || !isTrackingX) {
      camX += dy * Math.cos(yTiltAngle);
      fixX += dy * Math.cos(yTiltAngle);
    }

    double xTiltAngle = yTiltAngle + Math.PI / 2;

    if (!isTracking || !isTrackingY) {
      camY -= dx * Math.sin(xTiltAngle);
      fixY -= dx * Math.sin(xTiltAngle);
    }

    if (!isTracking || !isTrackingX) {
      camX -= dx * Math.cos(xTiltAngle);
      fixX -= dx * Math.cos(xTiltAngle);
    }
  }

  private void initTransition() {
    camXSpeed = -(camX - storedCameraPositions.get(storedPositionIndex).getX()) / transitionTime;
    camYSpeed = -(camY - storedCameraPositions.get(storedPositionIndex).getY()) / transitionTime;
    camZSpeed = -(camZ - storedCameraPositions.get(storedPositionIndex).getZ()) / transitionTime;

    fixXSpeed = -(fixX - storedFixPositions.get(storedPositionIndex).getX()) / transitionTime;
    fixYSpeed = -(fixY - storedFixPositions.get(storedPositionIndex).getY()) / transitionTime;
    fixZSpeed = -(fixZ - storedFixPositions.get(storedPositionIndex).getZ()) / transitionTime;

    transitioning = true;
    lastTransitionTime = System.currentTimeMillis();
  }

  @Override
  public void toggleCameraKeyMode() {
    setUseCameraKeyPoints(!useKeyCameraPoints());
  }

  public boolean getCameraKeyMode() {
    return toggleCameraKeyPoints;
  }

  @Override
  public void setUseCameraKeyPoints(boolean use) {
    toggleCameraKeyPoints = use;
  }

  @Override
  public boolean useKeyCameraPoints() {
    return toggleCameraKeyPoints;
  }

  @Override
  public boolean setCameraKeyPoint(int time) {
    boolean added = false;
    for (int i = 0; i < cameraKeyPoints.size(); i++) {
      if (cameraKeyPoints.get(i) == time) {
        addKeyFrame(removeKeyFrameByTime(time), time);

        return false;
      }

      if (cameraKeyPoints.get(i) > time) {
        cameraKeyPoints.add(time);
        addKeyFrame(1, time);

        return true;
      }
    }

    if (!added) {
      cameraKeyPoints.add(time);
      addKeyFrame(time);
    }

    return true;
  }

  @Override
  public void nextCameraKeyPoint(int time) {
    // int closestLesserTime = Integer.MIN_VALUE;
    // int index = -1;
    // for (int i = 0; i < cameraKeyPoints.size(); i++)
    // {
    // if (cameraKeyPoints.get(i) < time && cameraKeyPoints.get(i) > closestLesserTime)
    // {
    // index = i;
    // closestLesserTime = cameraKeyPoints.get(i);
    // }
    // }
    // if (index != -1)
    // {
    // camera.initTransition(index);
    // }
    cameraKeyPointIndex++;

    if (cameraKeyPointIndex >= cameraKeyPoints.size()) {
      cameraKeyPointIndex = 0;
    }

    toggleCameraKeyPoints = false;
    gotoKey(cameraKeyPointIndex);
  }

  @Override
  public void previousCameraKeyPoint(int time) {
    cameraKeyPointIndex--;

    if (cameraKeyPointIndex < 0) {
      cameraKeyPointIndex = cameraKeyPoints.size() - 1;
    }

    toggleCameraKeyPoints = false;
    gotoKey(cameraKeyPointIndex);
  }

  @Override
  public void removeCameraKeyPoint(int time) {
    cameraKeyPoints.remove(cameraKeyPointIndex);
    removeKeyFrameByIndex(cameraKeyPointIndex);
  }

  @Override
  public double getTrackXVar() {
    return cameraTrackAndDollyVariablesHolder.getTrackingX();
  }

  @Override
  public double getTrackYVar() {
    return cameraTrackAndDollyVariablesHolder.getTrackingY();
  }

  @Override
  public double getTrackZVar() {
    return cameraTrackAndDollyVariablesHolder.getTrackingZ();
  }

  @Override
  public double getDollyXVar() {
    return cameraTrackAndDollyVariablesHolder.getDollyX();
  }

  @Override
  public double getDollyYVar() {
    return cameraTrackAndDollyVariablesHolder.getDollyY();
  }

  @Override
  public double getDollyZVar() {
    return cameraTrackAndDollyVariablesHolder.getDollyZ();
  }

  public void nextStoredPosition() {
    if (storedCameraPositions.size() > 0) {
      storedPositionIndex++;

      if (storedPositionIndex >= storedCameraPositions.size()) {
        storedPositionIndex = 0;
      }

      initTransition();
    }
  }

  public void previousStoredPosition() {
    if (storedCameraPositions.size() > 0) {
      storedPositionIndex--;

      if (storedPositionIndex < 0) {
        storedPositionIndex = storedCameraPositions.size() - 1;
      }

      initTransition();
    }
  }

  public void storePosition() {
    storedCameraPositions.add(new Point3d(getCamX(), getCamY(), getCamZ()));
    storedFixPositions.add(new Point3d(getFixX(), getFixY(), getFixZ()));
  }

  @Override
  public void reset() {}

  private Matrix3d rotationMatrix = new Matrix3d();
  private Vector3d positionOffset = new Vector3d();

  private Vector3d zAxis = new Vector3d(), yAxis = new Vector3d(), xAxis = new Vector3d();

  @Override
  public void computeTransform(RigidBodyTransform currXform) {
    update();
    CameraMountInterface cameraMount = getCameraMount();
    if (isMounted() && (cameraMount != null)) {
      cameraMount.getTransformToCamera(currXform);

      return;
    }

    positionOffset.set(getCamX(), getCamY(), getCamZ());
    xAxis.set(getFixX(), getFixY(), getFixZ());

    fixPointNode.translateTo(getFixX(), getFixY(), getFixZ());

    xAxis.sub(positionOffset);
    xAxis.normalize();
    zAxis.set(0.0, 0.0, 1.0);
    yAxis.cross(zAxis, xAxis);
    zAxis.cross(xAxis, yAxis);

    rotationMatrix.setColumn(0, xAxis);
    rotationMatrix.setColumn(1, yAxis);
    rotationMatrix.setColumn(2, zAxis);

    currXform.setRotationAndZeroTranslation(rotationMatrix);
    currXform.setTranslation(positionOffset);
    currXform.normalizeRotationPart();
  }

  private boolean shouldAcceptDeviceInput() {
    if (alreadyClosing
        || graphics3dAdapter.getContextManager().getCurrentViewport() != viewportAdapter)
      return false;

    if (jFrame != null && !jFrame.isActive()) return false;

    return true;
  }

  @Override
  public void keyPressed(Key key) {
    if (shouldAcceptDeviceInput()) {
      switch (key) {
        case W:
          forward = true;

          break;

        case S:
          backward = true;

          break;

        case A:
          left = true;

          break;

        case D:
          right = true;

          break;

        case Q:
          up = true;

          break;

        case Z:
          down = true;

          break;

        default:
          break;
      }
    }
  }

  @Override
  public void keyReleased(Key key) {
    if (shouldAcceptDeviceInput()) {
      switch (key) {
        case W:
          forward = false;

          break;

        case S:
          backward = false;

          break;

        case A:
          left = false;

          break;

        case D:
          right = false;

          break;

        case Q:
          up = false;

          break;

        case Z:
          down = false;

          break;

        case RIGHT:
          nextStoredPosition();

          break;

        case LEFT:
          previousStoredPosition();

          break;

        case K:
          storePosition();

          break;
        default:
          break;
      }
    }
  }

  @Override
  public void selected(
      Graphics3DNode graphics3dNode,
      ModifierKeyInterface modifierKeyHolder,
      Point3d location,
      Point3d cameraLocation,
      Quat4d cameraRotation) {
    if (shouldAcceptDeviceInput()) {
      if (modifierKeyHolder.isKeyPressed(Key.SHIFT)) {
        if (!isTracking() || !isTrackingX()) setFixX(location.getX());
        if (!isTracking() || !isTrackingY()) setFixY(location.getY());
        if (!isTracking() || !isTrackingZ()) setFixZ(location.getZ());
      }
    }
  }

  @Override
  public void mouseDragged(MouseButton mouseButton, double dx, double dy) {
    if (shouldAcceptDeviceInput()) {
      switch (mouseButton) {
        case LEFT:
          doMouseDraggedLeft(dx, dy);

          break;

        case RIGHT:
          doMouseDraggedRight(dx, dy);

          break;

        case MIDDLE:
          doMouseDraggedMiddle(dx, dy);

          break;

        case LEFTRIGHT:
          pan(dx, dy);

          break;
      }
    }
  }

  private double rotateGain = 0.15;
  private double translateGain = 0.05;

  @Override
  public void mouseDragged(double dx, double dy, double dz, double drx, double dry, double drz) {
    if (shouldAcceptDeviceInput()) {
      //      doMouseDraggedRight(drz, drx);
      //      doMouseDraggedMiddle(0.0, dz);
      //      moveCameraForward(dy);

      rotateAroundFix(-drz * rotateGain, -drx * rotateGain);
      translateFix(-dx * translateGain, -dy * translateGain, dz * translateGain);
    }
  }

  @Override
  public double getClipNear() {
    if (isMounted) {
      return cameraMount.getClipDistanceNear();
    } else {
      return clipDistanceNear;
    }
  }

  @Override
  public double getClipFar() {
    if (isMounted) {
      return cameraMount.getClipDistanceFar();
    } else {
      return clipDistanceFar;
    }
  }

  @Override
  public double getHorizontalFieldOfViewInRadians() {
    if (isMounted) {
      return cameraMount.getFieldOfView();
    } else {
      return fieldOfView;
    }
  }

  @Override
  public void setClipDistanceNear(double near) {
    clipDistanceNear = near;
  }

  @Override
  public void setClipDistanceFar(double far) {
    clipDistanceFar = far;
  }

  @Override
  public void copyPositionTrackingDollyConfiguration(TrackingDollyCameraController otherCamera) {
    setTracking(
        otherCamera.isTracking(),
        otherCamera.isTrackingX(),
        otherCamera.isTrackingY(),
        otherCamera.isTrackingZ());
    setDolly(
        otherCamera.isDolly(),
        otherCamera.isDollyX(),
        otherCamera.isDollyY(),
        otherCamera.isDollyZ());

    setCameraPosition(otherCamera.getCamX(), otherCamera.getCamY(), otherCamera.getCamZ());

    setFixPosition(otherCamera.getFixX(), otherCamera.getFixY(), otherCamera.getFixZ());

    setDollyOffsets(
        otherCamera.getDollyXOffset(),
        otherCamera.getDollyYOffset(),
        otherCamera.getDollyZOffset());
    setTrackingOffsets(
        otherCamera.getTrackingXOffset(),
        otherCamera.getTrackingYOffset(),
        otherCamera.getTrackingZOffset());

    if (otherCamera instanceof ClassicCameraController) {
      ClassicCameraController classicOtherCamera = (ClassicCameraController) otherCamera;

      keyFrameCamPos = classicOtherCamera.keyFrameCamPos;
      keyFrameFixPos = classicOtherCamera.keyFrameFixPos;
      keyFrameTimes = classicOtherCamera.keyFrameTimes;

      toggleCameraKeyPoints = classicOtherCamera.toggleCameraKeyPoints;
      cameraKeyPointIndex = classicOtherCamera.cameraKeyPointIndex;
      cameraKeyPoints = classicOtherCamera.cameraKeyPoints;

      System.out.println("Copying camera keys");
    }
  }

  private boolean alreadyClosing = false;

  @Override
  public void closeAndDispose() {
    if (alreadyClosing) return;
    alreadyClosing = true;

    this.cameraMount = null;
    this.viewportAdapter = null;
    if (cameraTrackAndDollyVariablesHolder != null) {
      cameraTrackAndDollyVariablesHolder.closeAndDispose();
      cameraTrackAndDollyVariablesHolder = null;
    }
    graphics3dAdapter = null;
  }

  public Graphics3DNode getFixPointNode() {
    return fixPointNode;
  }
}
  public FallingBrickRobot() {
    super("FallingBrick");

    this.setGravity(0.0, 0.0, -G);

    // create the brick as a floating joint
    floatingJoint = new FloatingJoint("base", new Vector3d(0.0, 0.0, 0.0), this);
    Link link1 = base("base", YoAppearance.Red());
    floatingJoint.setLink(link1);
    this.addRootJoint(floatingJoint);

    // add ground contact points to the brick
    GroundContactPoint gc1 =
        new GroundContactPoint("gc1", new Vector3d(BASE_L / 2.0, BASE_W / 2.0, BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc1);
    GroundContactPoint gc2 =
        new GroundContactPoint(
            "gc2", new Vector3d(BASE_L / 2.0, -BASE_W / 2.0, BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc2);
    GroundContactPoint gc3 =
        new GroundContactPoint(
            "gc3", new Vector3d(-BASE_L / 2.0, BASE_W / 2.0, BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc3);
    GroundContactPoint gc4 =
        new GroundContactPoint(
            "gc4", new Vector3d(-BASE_L / 2.0, -BASE_W / 2.0, BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc4);

    GroundContactPoint gc5 =
        new GroundContactPoint(
            "gc5", new Vector3d(BASE_L / 2.0, BASE_W / 2.0, -BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc5);
    GroundContactPoint gc6 =
        new GroundContactPoint(
            "gc6", new Vector3d(BASE_L / 2.0, -BASE_W / 2.0, -BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc6);
    GroundContactPoint gc7 =
        new GroundContactPoint(
            "gc7", new Vector3d(-BASE_L / 2.0, BASE_W / 2.0, -BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc7);
    GroundContactPoint gc8 =
        new GroundContactPoint(
            "gc8", new Vector3d(-BASE_L / 2.0, -BASE_W / 2.0, -BASE_H / 2.0), this);
    floatingJoint.addGroundContactPoint(gc8);

    GroundContactPoint gc9 =
        new GroundContactPoint("gc9", new Vector3d(0.0, 0.0, BASE_H / 2.0 + BASE_H), this);
    floatingJoint.addGroundContactPoint(gc9);
    GroundContactPoint gc10 =
        new GroundContactPoint("gc10", new Vector3d(0.0, 0.0, -BASE_H / 2.0 - BASE_H), this);
    floatingJoint.addGroundContactPoint(gc10);

    this.setController(this); // tells the simulator to call the local doControl() method

    // instantiate ground contact model
    GroundContactModel groundModel =
        new LinearGroundContactModel(
            this, 1422, 150.6, 50.0, 1000.0, this.getRobotsYoVariableRegistry());
    // GroundContactModel groundModel = new CollisionGroundContactModel(this, 0.5, 0.7);

    GroundProfile3D profile = new WavyGroundProfile();
    groundModel.setGroundProfile3D(profile);
    this.setGroundContactModel(groundModel);

    initRobot();
    initControl();
  }