diff --git a/src/main/java/com/marginallyclever/ro3/Registry.java b/src/main/java/com/marginallyclever/ro3/Registry.java index fa0ec88bc..44745edf8 100644 --- a/src/main/java/com/marginallyclever/ro3/Registry.java +++ b/src/main/java/com/marginallyclever/ro3/Registry.java @@ -28,11 +28,11 @@ public static void start() { nodeFactory.clear(); Factory.Category 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 pose = nodule.add("Pose", Pose::new); pose.add("Camera", Camera::new); - pose.add("HingeJoint", HingeJoint::new); renderPasses.add(new DrawBackground()); renderPasses.add(new DrawMeshes()); diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java b/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java index 6d8b93b9e..081d80ec5 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java @@ -16,7 +16,7 @@ /** *

{@link DHParameter} is a node that translates a sibling {@link Pose} to and from - * Denavit-Hartenberg parameters for a joint.

+ * Denavit-Hartenberg parameters for a joint.

*

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.

@@ -29,10 +29,12 @@ 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); @@ -40,16 +42,46 @@ void toPose() { 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 list) { CollapsiblePanel panel = new CollapsiblePanel(DHParameter.class.getSimpleName()); diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java b/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java index d1f702b4d..76d015b60 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java +++ b/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 list) { @@ -32,9 +43,24 @@ public void getComponents(List 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 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; } } diff --git a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawBoundingBoxes.java b/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawBoundingBoxes.java index 234af3a1b..6f1f3733c 100644 --- a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawBoundingBoxes.java +++ b/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); diff --git a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawCameras.java b/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawCameras.java index 4298e26f3..6904065e0 100644 --- a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawCameras.java +++ b/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); diff --git a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawDHParameters.java b/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawDHParameters.java index b718130bc..0b50fecfd 100644 --- a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawDHParameters.java +++ b/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); diff --git a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawHingeJoints.java b/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawHingeJoints.java index 3e20c8303..5b5cf54db 100644 --- a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawHingeJoints.java +++ b/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 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); } } diff --git a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawMeshes.java b/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawMeshes.java index 706013dda..96a2f6113 100644 --- a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawMeshes.java +++ b/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); diff --git a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawPoses.java b/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawPoses.java index 59f3f1a99..4ed90b556 100644 --- a/src/main/java/com/marginallyclever/ro3/render/renderpasses/DrawPoses.java +++ b/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); diff --git a/src/main/java/com/marginallyclever/robotoverlord/systems/render/mesh/Mesh.java b/src/main/java/com/marginallyclever/robotoverlord/systems/render/mesh/Mesh.java index ae818ec39..b77c2190c 100644 --- a/src/main/java/com/marginallyclever/robotoverlord/systems/render/mesh/Mesh.java +++ b/src/main/java/com/marginallyclever/robotoverlord/systems/render/mesh/Mesh.java @@ -184,7 +184,25 @@ private void setupArray(GL3 gl, int attribIndex, int size, long numVertexes,List OpenGLHelper.checkGLError(gl,logger); } + /** + * Render the entire mesh. + * @param gl the OpenGL context + */ public void render(GL3 gl) { + if (hasIndexes) { + render(gl,indexArray.size(),0); + } else { + render(gl,getNumVertices(),0); + } + } + + /** + * Render a portion of the mesh. + * @param gl the OpenGL context + * @param count number of vertices to render + * @param startIndex index of the first vertex to render + */ + public void render(GL3 gl,int count,int startIndex) { if(!isLoaded) { isLoaded=true; isDirty=true; @@ -207,7 +225,7 @@ public void render(GL3 gl) { if (hasIndexes) { gl.glDrawElements(renderStyle, indexArray.size(), GL3.GL_UNSIGNED_INT, 0); } else { - gl.glDrawArrays(renderStyle, 0, getNumVertices()); + gl.glDrawArrays(renderStyle, startIndex, count); } OpenGLHelper.checkGLError(gl,logger); } @@ -224,7 +242,14 @@ public void addVertex(float x,float y,float z) { vertexArray.add(y); vertexArray.add(z); } - + + /** + * Add a color to the mesh. + * @param r red, 0-1 + * @param g green, 0-1 + * @param b blue, 0-1 + * @param a alpha, 0-1 + */ public void addColor(float r,float g,float b,float a) { colorArray.add(r); colorArray.add(g); @@ -233,7 +258,12 @@ public void addColor(float r,float g,float b,float a) { if(a!=1) isTransparent=true; hasColors=true; } - + + /** + * Add a texture coordinate to the mesh. + * @param u 0-1 + * @param v 0-1 + */ public void addTexCoord(float u,float v) { textureArray.add(u); textureArray.add(v);