added HingeJoint

src/main/java/com/marginallyclever/ro3/Registry.java

@@ -28,11 +28,11 @@ public static void start() {
         nodeFactory.clear();
         Factory.Category<Node> nodule = nodeFactory.getRoot().add("Node", null);
         nodule.add("DHParameter", DHParameter::new);
+        nodule.add("HingeJoint", HingeJoint::new);
         nodule.add("Material", Material::new);
         nodule.add("MeshInstance", MeshInstance::new);
         Factory.Category<Node> pose = nodule.add("Pose", Pose::new);
             pose.add("Camera", Camera::new);
-            pose.add("HingeJoint", HingeJoint::new);
 
         renderPasses.add(new DrawBackground());
         renderPasses.add(new DrawMeshes());

src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java

@@ -16,7 +16,7 @@
 
 /**
  * <p>{@link DHParameter} is a node that translates a sibling {@link Pose} to and from
- * <a href="https://www.youtube.com/watch?v=rA9tm0gTln8">Denavit-Hartenberg parameters</a> for a joint.</p>
+ * <a href="https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters">Denavit-Hartenberg parameters</a> for a joint.</p>
  * <p>The DH parameters can be derived by finding the common normals between two consecutive Z axes.  The new X axis
  * points along the common normal.  The intersection point of the two normals may be outside the physical structure
  * being described.</p>
@@ -29,27 +29,59 @@ public DHParameter() {
         super("DH Parameter");
     }
 
-    void toPose() {
+    private void toPose() {
         Pose pose = findFirstSibling(Pose.class);
-        if(pose==null) return;
+        if (pose != null) pose.setLocal(getDHMatrix());
+    }
 
+    private Matrix4d getDHMatrix() {
         Matrix4d m = new Matrix4d();
         double rt = Math.toRadians(theta);
         double ra = Math.toRadians(alpha);
         double ct = Math.cos(rt);
         double ca = Math.cos(ra);
         double st = Math.sin(rt);
         double sa = Math.sin(ra);
-
         m.m00 = ct;		m.m01 = -st*ca;		m.m02 = st*sa;		m.m03 = r*ct;
         m.m10 = st;		m.m11 = ct*ca;		m.m12 = -ct*sa;		m.m13 = r*st;
         m.m20 = 0;		m.m21 = sa;			m.m22 = ca;			m.m23 = d;
         m.m30 = 0;		m.m31 = 0;			m.m32 = 0;			m.m33 = 1;
+        return m;
+    }
+
+    private void fromPose() {
+        Pose pose = findFirstSibling(Pose.class);
+        if (pose != null) setDHMatrix(pose.getLocal());
+    }
+
+    private void setDHMatrix(Matrix4d m) {
+        // Extract the elements of the matrix
+        double m00 = m.m00;
+        double m01 = m.m01;
+        double m02 = m.m02;
+        double m03 = m.m03;
+        double m10 = m.m10;
+        double m11 = m.m11;
+        double m12 = m.m12;
+        double m13 = m.m13;
+        double m20 = m.m20;
+        double m21 = m.m21;
+        double m22 = m.m22;
+        double m23 = m.m23;
 
-        pose.setLocal(m);
+        // Check if the pose is DH compatible
+        if (m00 * m10 + m01 * m11 + m02 * m12 != 0 || m20 != 0 || m21 * m21 + m22 * m22 != 1) {
+            throw new IllegalArgumentException("The pose is not DH compatible.  pose="+ m);
+        }
+
+        // Calculate the DH parameters
+        r = Math.sqrt(m03 * m03 + m13 * m13);
+        d = m23;
+        theta = Math.toDegrees(Math.atan2(m13, m03));
+        alpha = Math.toDegrees(Math.atan2(m21, m22));
     }
 
-    void toPoseAndAdjustMeshes() {
+    private void toPoseAndAdjustMeshes() {
         toPose();
         adjustMeshes();
     }
@@ -68,39 +100,6 @@ private void adjustMeshes() {
         }
     }
 
-    void fromPose() {
-        Pose pose = findFirstSibling(Pose.class);
-        if(pose==null) return;
-
-        // convert pose.getLocal() to DH parameters
-        Matrix4d local = pose.getLocal();
-
-        // Extract the elements of the matrix
-        double m00 = local.m00;
-        double m01 = local.m01;
-        double m02 = local.m02;
-        double m03 = local.m03;
-        double m10 = local.m10;
-        double m11 = local.m11;
-        double m12 = local.m12;
-        double m13 = local.m13;
-        double m20 = local.m20;
-        double m21 = local.m21;
-        double m22 = local.m22;
-        double m23 = local.m23;
-
-        // Check if the pose is DH compatible
-        if (m00 * m10 + m01 * m11 + m02 * m12 != 0 || m20 != 0 || m21 * m21 + m22 * m22 != 1) {
-            throw new IllegalArgumentException("The pose is not DH compatible.  pose="+ local);
-        }
-
-        // Calculate the DH parameters
-        r = Math.sqrt(m03 * m03 + m13 * m13);
-        d = m23;
-        theta = Math.toDegrees(Math.atan2(m13, m03));
-        alpha = Math.toDegrees(Math.atan2(m21, m22));
-    }
-
     @Override
     public void getComponents(List<JComponent> list) {
         CollapsiblePanel panel = new CollapsiblePanel(DHParameter.class.getSimpleName());

src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java

@@ -1,5 +1,6 @@
 package com.marginallyclever.ro3.node.nodes;
 
+import com.marginallyclever.ro3.node.Node;
 import com.marginallyclever.robotoverlord.swing.CollapsiblePanel;
 import org.json.JSONObject;
 
@@ -12,8 +13,18 @@
 /**
  * {@link HingeJoint} is a joint that can rotate around the local Z axis.
  */
-public class HingeJoint extends Pose {
+public class HingeJoint extends Node {
     private double angle = 0;
+    private double minAngle = 0;
+    private double maxAngle = 180;
+
+    public HingeJoint() {
+        this("HingeJoint");
+    }
+
+    public HingeJoint(String name) {
+        super(name);
+    }
 
     @Override
     public void getComponents(List<JComponent> list) {
@@ -32,9 +43,24 @@ public void getComponents(List<JComponent> list) {
         JFormattedTextField angleField = new JFormattedTextField(formatter);
         angleField.setValue(angle);
         angleField.addPropertyChangeListener("value", (evt) ->{
-            angle = (double)angleField.getValue();
+            angle = ((Number) angleField.getValue()).doubleValue();
+        });
+
+        JFormattedTextField maxAngleField = new JFormattedTextField(formatter);
+        maxAngleField.setValue(maxAngle);
+        maxAngleField.addPropertyChangeListener("value", (evt) ->{
+            maxAngle = ((Number) maxAngleField.getValue()).doubleValue();
+        });
+
+        JFormattedTextField minAngleField = new JFormattedTextField(formatter);
+        minAngleField.setValue(minAngle);
+        minAngleField.addPropertyChangeListener("value", (evt) ->{
+            minAngle = ((Number) minAngleField.getValue()).doubleValue();
         });
+
         addLabelAndComponent(pane, "Angle",angleField);
+        addLabelAndComponent(pane, "Min",minAngleField);
+        addLabelAndComponent(pane, "Max",maxAngleField);
 
         super.getComponents(list);
     }
@@ -43,12 +69,28 @@ public void getComponents(List<JComponent> list) {
     public JSONObject toJSON() {
         JSONObject json = super.toJSON();
         json.put("angle",angle);
+        json.put("minAngle",minAngle);
+        json.put("maxAngle",maxAngle);
         return json;
     }
 
     @Override
     public void fromJSON(JSONObject from) {
         super.fromJSON(from);
-        angle = from.getDouble("angle");
+        if(from.has("angle")) angle = from.getDouble("angle");
+        if(from.has("minAngle")) minAngle = from.getDouble("minAngle");
+        if(from.has("maxAngle")) maxAngle = from.getDouble("maxAngle");
+    }
+
+    public double getAngle() {
+        return angle;
+    }
+
+    public double getMinAngle() {
+        return minAngle;
+    }
+
+    public double getMaxAngle() {
+        return maxAngle;
     }
 }

src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawBoundingBoxes.java

@@ -133,7 +133,7 @@ public void draw() {
         shader.set4f(gl3,"objectColor",1,1,1,0.25f);
         shader.setVector3d(gl3,"specularColor",new Vector3d(0.5,0.5,0.5));
         shader.setVector3d(gl3,"ambientLightColor",new Vector3d(0.2,0.2,0.2));
-        shader.set1f(gl3,"useVertexColor",0);
+        shader.set1i(gl3,"useVertexColor",0);
         shader.set1i(gl3,"useLighting",0);
         shader.set1i(gl3,"diffuseTexture",0);
         shader.set1i(gl3,"useTexture",0);

src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawCameras.java

@@ -114,7 +114,7 @@ public void draw() {
         shader.set4f(gl3,"objectColor",1,1,1,1);
         shader.setVector3d(gl3,"specularColor",new Vector3d(0.5,0.5,0.5));
         shader.setVector3d(gl3,"ambientLightColor",new Vector3d(0.2,0.2,0.2));
-        shader.set1f(gl3,"useVertexColor",1);
+        shader.set1i(gl3,"useVertexColor",1);
         shader.set1i(gl3,"useLighting",0);
         shader.set1i(gl3,"diffuseTexture",0);
         gl3.glDisable(GL3.GL_DEPTH_TEST);

src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawDHParameters.java

@@ -113,7 +113,7 @@ public void draw() {
         shader.set4f(gl3,"objectColor",1,1,1,1f);
         shader.setVector3d(gl3,"specularColor",new Vector3d(0.5,0.5,0.5));
         shader.setVector3d(gl3,"ambientLightColor",new Vector3d(0.2,0.2,0.2));
-        shader.set1f(gl3,"useVertexColor",1);
+        shader.set1i(gl3,"useVertexColor",1);
         shader.set1i(gl3,"useLighting",0);
         shader.set1i(gl3,"diffuseTexture",0);
         shader.set1i(gl3,"useTexture",0);

src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawHingeJoints.java

@@ -9,6 +9,7 @@
 import com.marginallyclever.ro3.node.Node;
 import com.marginallyclever.ro3.node.nodes.Camera;
 import com.marginallyclever.ro3.node.nodes.HingeJoint;
+import com.marginallyclever.ro3.node.nodes.Pose;
 import com.marginallyclever.ro3.render.RenderPass;
 import com.marginallyclever.robotoverlord.systems.render.ShaderProgram;
 import com.marginallyclever.robotoverlord.systems.render.mesh.Mesh;
@@ -24,19 +25,27 @@ public class DrawHingeJoints implements RenderPass {
     private static final Logger logger = LoggerFactory.getLogger(DrawHingeJoints.class);
     private int activeStatus = ALWAYS;
     private final Mesh mesh = new Mesh();
+    private final Mesh circleFan = new Mesh();
     private ShaderProgram shader;
     private int canvasWidth,canvasHeight;
+    private float ringScale = 3;
 
     public DrawHingeJoints() {
         super();
 
         mesh.setRenderStyle(GL3.GL_LINES);
-        mesh.addColor(0,0,0,1);  mesh.addVertex(0,0,0);  // origin
-        mesh.addColor(0,0,0,1);  mesh.addVertex(0,0,0);  // angle unit line
-        //mesh.addColor(0,0,0,1);  mesh.addVertex(0,0,0);  // origin
-        //mesh.addColor(0,0,0,1);  mesh.addVertex(0,0,0);  // min angle?
-        //mesh.addColor(0,0,0,1);  mesh.addVertex(0,0,0);  // origin
-        //mesh.addColor(0,0,0,1);  mesh.addVertex(0,0,0);  // max angle?
+        mesh.addColor(1.0f,1.0f,1.0f,1);  mesh.addVertex(0,0,0);  // origin
+        mesh.addColor(1.0f,1.0f,1.0f,1);  mesh.addVertex(0,0,0);  // angle unit line
+
+        circleFan.setRenderStyle(GL3.GL_TRIANGLE_FAN);
+        circleFan.addColor(1.0f,1.0f,0.0f,0.25f);
+        circleFan.addVertex(0,0,0);  // origin
+        for(int i=0;i<=360;++i) {
+            float x = (float)Math.cos(Math.toRadians(i)) * ringScale;
+            float y = (float)Math.sin(Math.toRadians(i)) * ringScale;
+            circleFan.addColor(1.0f,1.0f,0.0f,0.25f);
+            circleFan.addVertex(x,y,0);
+        }
     }
 
     @Override
@@ -97,11 +106,12 @@ public void draw() {
         shader.set4f(gl3,"objectColor",1,1,1,1);
         shader.setVector3d(gl3,"specularColor",new Vector3d(0.5,0.5,0.5));
         shader.setVector3d(gl3,"ambientLightColor",new Vector3d(0.2,0.2,0.2));
-        shader.set1f(gl3,"useVertexColor",1);
+        shader.set1i(gl3,"useVertexColor",1);
         shader.set1i(gl3,"useLighting",0);
         shader.set1i(gl3,"diffuseTexture",0);
         gl3.glDisable(GL3.GL_DEPTH_TEST);
         gl3.glDisable(GL3.GL_TEXTURE_2D);
+        gl3.glDisable(GL3.GL_CULL_FACE);
 
         List<Node> toScan = new ArrayList<>();
         toScan.add(Registry.getScene());
@@ -110,13 +120,26 @@ public void draw() {
             toScan.addAll(node.getChildren());
 
             if(node instanceof HingeJoint joint) {
-                Matrix4d w = joint.getWorld();
+                double angle = joint.getAngle()-joint.getMinAngle();
+                float x = (float)Math.cos(Math.toRadians(angle)) * ringScale;
+                float y = (float)Math.sin(Math.toRadians(angle)) * ringScale;
+                mesh.setVertex(1,x,y,0);
+                mesh.updateVertexBuffers(gl3);
+
+                Pose pose = joint.findParent(Pose.class);
+                Matrix4d w = (pose==null) ? MatrixHelper.createIdentityMatrix4() : pose.getWorld();
+                Matrix4d rZ = new Matrix4d();
+                rZ.rotZ(Math.toRadians(joint.getMinAngle()));
+                w.mul(rZ);
                 w.transpose();
                 shader.setMatrix4d(gl3,"modelMatrix",w);
                 mesh.render(gl3);
+                int range = Math.max(0, (int)(joint.getMaxAngle()-joint.getMinAngle()) );
+                circleFan.render(gl3,1+range,0);
             }
         }
 
         gl3.glEnable(GL3.GL_DEPTH_TEST);
+        gl3.glEnable(GL3.GL_CULL_FACE);
     }
 }

src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawMeshes.java

@@ -101,7 +101,7 @@ public void draw() {
         shader.set4f(gl3,"objectColor",1,1,1,1);
         shader.setVector3d(gl3,"specularColor",new Vector3d(0.5,0.5,0.5));
         shader.setVector3d(gl3,"ambientLightColor",new Vector3d(0.2,0.2,0.2));
-        shader.set1f(gl3,"useVertexColor",0);
+        shader.set1i(gl3,"useVertexColor",0);
         shader.set1i(gl3,"useLighting",1);
         shader.set1i(gl3,"diffuseTexture",0);
         OpenGLHelper.checkGLError(gl3,logger);

src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawPoses.java

@@ -97,7 +97,7 @@ public void draw() {
         shader.set4f(gl3,"objectColor",1,1,1,1);
         shader.setVector3d(gl3,"specularColor",new Vector3d(0.5,0.5,0.5));
         shader.setVector3d(gl3,"ambientLightColor",new Vector3d(0.2,0.2,0.2));
-        shader.set1f(gl3,"useVertexColor",1);
+        shader.set1i(gl3,"useVertexColor",1);
         shader.set1i(gl3,"useLighting",0);
         shader.set1i(gl3,"diffuseTexture",0);
         gl3.glDisable(GL3.GL_DEPTH_TEST);