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);