pulled in some of the tools from open robotics that will allow extrac…

…ting some tools

src/main/java/us/ihmc/javaFXToolkit/ApplicationNoModule.java

@@ -0,0 +1,160 @@
+package us.ihmc.javaFXToolkit;
+
+import com.sun.javafx.application.LauncherImpl;
+import javafx.application.Application;
+import javafx.application.Application.Parameters;
+import javafx.application.HostServices;
+import javafx.application.Preloader.PreloaderNotification;
+import javafx.stage.Stage;
+
+import java.lang.reflect.InvocationTargetException;
+
+/**
+ * Workaround to create a JavaFX {@link Application} without the use of the modules.
+ */
+public abstract class ApplicationNoModule
+{
+   private ApplicationImpl impl;
+
+   public ApplicationNoModule()
+   {
+   }
+
+   public void init() throws Exception
+   {
+   }
+
+   public abstract void start(Stage primaryStage) throws Exception;
+
+   public void stop() throws Exception
+   {
+   }
+
+   public final HostServices getHostServices()
+   {
+      if (impl == null)
+         impl = new ApplicationImpl(false);
+      return impl.getHostServices();
+   }
+
+   public final Parameters getParameters()
+   {
+      if (impl == null)
+         impl = new ApplicationImpl(false);
+      return impl.getParameters();
+   }
+
+   public final void notifyPreloader(PreloaderNotification info)
+   {
+      if (impl == null)
+         impl = new ApplicationImpl(false);
+      impl.notifyPreloader(info);
+   }
+
+   public static String getUserAgentStylesheet()
+   {
+      return Application.getUserAgentStylesheet();
+   }
+
+   public static void setUserAgentStylesheet(String url)
+   {
+      Application.setUserAgentStylesheet(url);
+   }
+
+   public static class ApplicationImpl extends Application
+   {
+      private static Class<? extends ApplicationNoModule> classToInstatiate;
+      private ApplicationNoModule appToManage;
+
+      private ApplicationImpl(boolean dummyParameter)
+      {
+      }
+
+      public ApplicationImpl()
+            throws InstantiationException, IllegalAccessException, IllegalArgumentException, InvocationTargetException, NoSuchMethodException, SecurityException
+      {
+         if (classToInstatiate != null)
+            appToManage = classToInstatiate.getConstructor().newInstance();
+         classToInstatiate = null;
+         appToManage.impl = this;
+      }
+
+      @Override
+      public void init() throws Exception
+      {
+         appToManage.init();
+      }
+
+      @Override
+      public void start(Stage primaryStage) throws Exception
+      {
+         appToManage.start(primaryStage);
+      }
+
+      @Override
+      public void stop() throws Exception
+      {
+         appToManage.stop();
+      }
+   }
+
+   public static void launch(Class<? extends ApplicationNoModule> appClass, String... args)
+   {
+      ApplicationImpl.classToInstatiate = appClass;
+      LauncherImpl.launchApplication(ApplicationImpl.class, args);
+   }
+
+   @SuppressWarnings({"rawtypes", "unchecked"})
+   public static void launch(String... args)
+   {
+      // Figure out the right class to call
+      StackTraceElement[] cause = Thread.currentThread().getStackTrace();
+
+      boolean foundThisMethod = false;
+      String callingClassName = null;
+      for (StackTraceElement se : cause)
+      {
+         // Skip entries until we get to the entry for this class
+         String className = se.getClassName();
+         String methodName = se.getMethodName();
+         if (foundThisMethod)
+         {
+            callingClassName = className;
+            break;
+         }
+         else if (ApplicationNoModule.class.getName().equals(className) && "launch".equals(methodName))
+         {
+
+            foundThisMethod = true;
+         }
+      }
+
+      if (callingClassName == null)
+      {
+         throw new RuntimeException("Error: unable to determine Application class");
+      }
+
+      try
+      {
+         Class theClass = Class.forName(callingClassName, false, Thread.currentThread().getContextClassLoader());
+
+         if (ApplicationNoModule.class.isAssignableFrom(theClass))
+         {
+            Class<? extends ApplicationNoModule> appClass = theClass;
+            launch(appClass, args);
+         }
+         else
+         {
+            throw new RuntimeException("Error: " + theClass + " is not a subclass of javafx.application.Application");
+         }
+      }
+      catch (RuntimeException ex)
+      {
+         throw ex;
+      }
+      catch (Exception ex)
+      {
+         throw new RuntimeException(ex);
+      }
+   }
+}

src/main/java/us/ihmc/javaFXToolkit/JavaFXGraphicTools.java

@@ -0,0 +1,141 @@
+package us.ihmc.javaFXToolkit;
+
+import javafx.scene.Node;
+import javafx.scene.transform.Affine;
+import us.ihmc.euclid.axisAngle.AxisAngle;
+import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly;
+import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly;
+import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly;
+import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly;
+import us.ihmc.euclid.shape.primitives.Box3D;
+import us.ihmc.euclid.shape.primitives.interfaces.Box3DReadOnly;
+import us.ihmc.euclid.transform.RigidBodyTransform;
+import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly;
+import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
+import us.ihmc.euclid.tuple3D.Point3D;
+import us.ihmc.euclid.tuple3D.Vector3D;
+import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
+import us.ihmc.euclid.tuple4D.Quaternion;
+import us.ihmc.javaFXToolkit.shapes.JavaFXMeshBuilder;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class JavaFXGraphicTools
+{
+   public static void setNodeTransformFromPose(Node node, Pose3DReadOnly pose)
+   {
+      setNodeTransformFromPose(node, pose, 1.0);
+   }
+
+   public static void setNodeTransformFromPose(Node node, Pose3DReadOnly pose, double scale)
+   {
+      node.getTransforms().clear();
+      RigidBodyTransform rigidBodyTransform = new RigidBodyTransform();
+      rigidBodyTransform.set(pose.getOrientation(), pose.getPosition());
+      Affine affine = JavaFXTools.createRigidBodyTransformToAffine(rigidBodyTransform);
+      if (scale != 1.0)
+         affine.appendScale(scale, scale);
+      node.getTransforms().add(affine);
+   }
+
+   public static void setNodePosition(Node node, Tuple3DReadOnly position)
+   {
+      node.setTranslateX(position.getX());
+      node.setTranslateY(position.getY());
+      node.setTranslateZ(position.getZ());
+   }
+
+   public static void drawPlanarRegion(JavaFXMeshBuilder meshBuilder,
+                                       RigidBodyTransformReadOnly transformToWorld,
+                                       List<? extends Point2DReadOnly> concaveHull,
+                                       List<? extends ConvexPolygon2DReadOnly> convexPolygons,
+                                       double lineWidth)
+   {
+      meshBuilder.addMultiLine(transformToWorld, concaveHull, lineWidth, true);
+
+      for (ConvexPolygon2DReadOnly convexPolygon : convexPolygons)
+      {
+         meshBuilder.addPolygon(transformToWorld, convexPolygon);
+      }
+   }
+
+   public static void drawArrow(JavaFXMeshBuilder meshBuilder,
+                                Tuple3DReadOnly position,
+                                Orientation3DReadOnly orientation,
+                                double length,
+                                double radius,
+                                double cylinderToConeLengthRatio,
+                                double coneDiameterMultiplier)
+   {
+      double cylinderLength = cylinderToConeLengthRatio * length;
+      double coneLength = (1.0 - cylinderToConeLengthRatio) * length;
+      double coneRadius = coneDiameterMultiplier * radius;
+
+      AxisAngle axisAngle = new AxisAngle(orientation);
+      meshBuilder.addCylinder(cylinderLength, radius, position, axisAngle);
+
+      Vector3D coneBaseTranslation = new Vector3D(0.0, 0.0, 1.0);
+      orientation.transform(coneBaseTranslation);
+      coneBaseTranslation.scale(length);
+      coneBaseTranslation.scale(cylinderToConeLengthRatio);
+      Point3D coneBase = new Point3D(position);
+      coneBase.add(coneBaseTranslation);
+
+      meshBuilder.addCone(coneLength, coneRadius, coneBase, axisAngle);
+   }
+
+   public static void drawBoxEdges(JavaFXMeshBuilder meshBuilder, BoundingBox3DReadOnly boundingBox, double lineWidth)
+   {
+      drawBoxEdges(meshBuilder, convertBoundingBox3DToBox3D(boundingBox), lineWidth);
+   }
+
+   /**
+    * Creates a 3D Euclid Box (a Shape) out of a 3D Bounding Box. Allocates a new Box3D.
+    *
+    * @param boundingBox
+    * @return box
+    */
+   public static Box3D convertBoundingBox3DToBox3D(BoundingBox3DReadOnly boundingBox)
+   {
+      Point3DReadOnly minPoint = boundingBox.getMinPoint();
+      Point3DReadOnly maxPoint = boundingBox.getMaxPoint();
+
+      Point3D boxCenter = new Point3D();
+      boxCenter.interpolate(minPoint, maxPoint, 0.5);
+      Vector3D size = new Vector3D();
+      size.sub(maxPoint, minPoint);
+
+      return new Box3D(boxCenter, new Quaternion(), size.getX(), size.getY(), size.getZ());
+   }
+
+   public static void drawBoxEdges(JavaFXMeshBuilder meshBuilder, Box3DReadOnly box, double lineWidth)
+   {
+      ArrayList<Point3DReadOnly> orderedVertices = new ArrayList<>();
+
+      orderedVertices.add(box.getVertex(0)); // x+y+z+  draw top
+      orderedVertices.add(box.getVertex(1)); // x-y-z+
+      orderedVertices.add(box.getVertex(3)); // x-y+z+
+      orderedVertices.add(box.getVertex(2)); // x+y-z+
+      orderedVertices.add(box.getVertex(0)); // x+y+z+
+
+      orderedVertices.add(box.getVertex(4)); // x+y+z-  go down
+
+      orderedVertices.add(box.getVertex(5)); // x-y-z-  leg 1
+      orderedVertices.add(box.getVertex(1)); // x-y-z+
+      orderedVertices.add(box.getVertex(5)); // x-y-z-
+
+      orderedVertices.add(box.getVertex(7)); // x-y+z-  leg 2
+      orderedVertices.add(box.getVertex(3)); // x-y+z+
+      orderedVertices.add(box.getVertex(7)); // x-y+z-
+
+      orderedVertices.add(box.getVertex(6)); // x+y-z-  leg 3
+      orderedVertices.add(box.getVertex(2)); // x+y-z+
+      orderedVertices.add(box.getVertex(6)); // x+y-z-
+
+      orderedVertices.add(box.getVertex(4)); // x+y+z-  leg 4
+
+      meshBuilder.addMultiLine(orderedVertices, lineWidth, false);
+   }
+}