public double resolveCenterOfPressureAndNormalTorque(
      FramePoint centerOfPressureToPack,
      SpatialForceVector spatialForceVector,
      ReferenceFrame centerOfPressurePlaneFrame) {
    // First resolve the wrench at the plane origin:
    wrenchResolvedOnPlane.set(spatialForceVector);
    wrenchResolvedOnPlane.changeFrame(centerOfPressurePlaneFrame);

    wrenchResolvedOnPlane.getAngularPart(torqueAtZeroInPlaneFrame);
    wrenchResolvedOnPlane.getLinearPart(forceInPlaneFrame);

    double fz = forceInPlaneFrame.getZ();

    double vector12x = Double.NaN;
    double vector12y = Double.NaN;

    double normalTorqueAtCenterOfPressure;
    if (fz > 1e-7) {
      // with sufficient normal force
      vector12x = -1.0 / fz * torqueAtZeroInPlaneFrame.getY();
      vector12y = 1.0 / fz * torqueAtZeroInPlaneFrame.getX();
      normalTorqueAtCenterOfPressure =
          torqueAtZeroInPlaneFrame.getZ()
              - vector12x * forceInPlaneFrame.getY()
              + vector12y * forceInPlaneFrame.getX();
    } else {
      // without normal force
      normalTorqueAtCenterOfPressure = torqueAtZeroInPlaneFrame.getZ();
    }

    centerOfPressureToPack.setIncludingFrame(centerOfPressurePlaneFrame, vector12x, vector12y, 0.0);
    return normalTorqueAtCenterOfPressure;
  }
  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);
  }
  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 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);
  }
  public void drawBox(BufferedImage image, RigidBodyTransform transform, double scale) {
    DenseMatrix64F rotation = new DenseMatrix64F(3, 3);
    Vector3d translation = new Vector3d();
    transform.getRotation(rotation);
    transform.getTranslation(translation);
    Se3_F64 targetToSensor =
        new Se3_F64(
            rotation, new Vector3D_F64(translation.getX(), translation.getY(), translation.getZ()));

    Graphics2D g2 = image.createGraphics();
    g2.setStroke(new BasicStroke(4));
    g2.setColor(java.awt.Color.RED);
    VisualizeFiducial.drawCube(targetToSensor, this.intrinsicParameters, scale, 2, g2);
    g2.finalize();
  }
 public void setAngularAccelerationInBody(Vector3d angularAccelerationInBody) {
   qdd_wx.set(angularAccelerationInBody.getX());
   qdd_wy.set(angularAccelerationInBody.getY());
   qdd_wz.set(angularAccelerationInBody.getZ());
 }
 public void setAngularVelocityInBody(Vector3d angularVelocityInBody) {
   qd_wx.set(angularVelocityInBody.getX());
   qd_wy.set(angularVelocityInBody.getY());
   qd_wz.set(angularVelocityInBody.getZ());
 }
 public double getBodyVelX(Vector3d linearVelocityToPack) {
   bodyJointID.getLinearVelocity(linearVelocityToPack);
   double velX = linearVelocityToPack.getX();
   return velX;
 }
 public static void packVector3dToGeometry_MsgPoint(Vector3d position, Point point) {
   point.setX(position.getX());
   point.setY(position.getY());
   point.setZ(position.getZ());
 }
 public static void packVector3dToGeometry_msgsVector3(Vector3d vector, Vector3 rosVectorToPack) {
   rosVectorToPack.setX(vector.getX());
   rosVectorToPack.setY(vector.getY());
   rosVectorToPack.setZ(vector.getZ());
 }