CI headless checks.

src/main/java/com/marginallyclever/convenience/helpers/IntersectionHelper.java

@@ -8,6 +8,7 @@
 
 import javax.vecmath.Matrix4d;
 import javax.vecmath.Point3d;
+import javax.vecmath.Tuple3d;
 import javax.vecmath.Vector3d;
 
 
@@ -17,11 +18,11 @@
 public abstract class IntersectionHelper {
 	static final float SMALL_NUM = 0.001f;
 	/**
-	 * test intersection of two cylinders.  From http://geomalgorithms.com/a07-_distance.html
-	 * @param cA cylinder A
-	 * @param cB cylinder B
-	 * @return true if intersect
-	 */
+     * test intersection of two cylinders.  From <a href="http://geomalgorithms.com/a07-_distance.html">...</a>
+     * @param cA cylinder A
+     * @param cB cylinder B
+     * @return true if intersect
+     */
 	static public boolean cylinderCylinder(Cylinder cA, Cylinder cB) {
 	    Vector3d u = new Vector3d(cA.GetP2());  u.sub(cA.GetP1());
 	    Vector3d v = new Vector3d(cB.GetP2());  v.sub(cB.GetP1());
@@ -109,29 +110,37 @@ static protected double CPATime(Vector3d dp,Vector3d dv) {
 		if(dv2 < SMALL_NUM) return 0;  // parallel, all times are the same.
 		return -dv.dot(dp) / dv2;
 	}
-
-	static public double CPADistance(Vector3d a,Vector3d b,Vector3d da,Vector3d db) {
+
+	/**
+	 * find the closest point of approach between two lines
+	 * @param a0 line 1 start
+	 * @param b0 line 2 start
+	 * @param a1 line 1 end
+	 * @param b1 line 2 end
+	 * @return distance between the two lines at the closest point of approach.
+	 */
+	static public double CPADistance(Vector3d a0,Vector3d b0,Vector3d a1,Vector3d b1) {
 		// find CPA time
-		Vector3d dp = new Vector3d(b);
-		dp.sub(a);
-		Vector3d dv = new Vector3d(db);
-		db.sub(da);		
+		Vector3d dp = new Vector3d(b0);
+		dp.sub(a0);
+		Vector3d dv = new Vector3d(b1);
+		b1.sub(a1);
 		double t = CPATime(dp,dv);
 
 		// get both points
-		Vector3d pa = new Vector3d(da);
+		Vector3d pa = new Vector3d(a1);
 		pa.scale(t);
-		pa.add(a);
-		Vector3d pb = new Vector3d(db);
+		pa.add(a0);
+		Vector3d pb = new Vector3d(b1);
 		pb.scale(t);
-		pb.add(b);
+		pb.add(b0);
 		// find difference
 		pb.sub(pa);
 		return pb.length();
 	}
 
 	/**
-	 * separation of axies theorem used to find intersection of two arbitrary boxes.
+	 * separation of axes theorem used to find intersection of two arbitrary boxes.
 	 * @param a first cuboid
 	 * @param b second cuboid
 	 * @return true if cuboids intersect
@@ -163,22 +172,22 @@ static protected boolean cuboidCuboidInternal(AABB a, AABB b) {
 		a.updatePoints();
 		b.updatePoints();
 
-		for (int i = 0; i < n.length; ++i) {
-			// SATTest the normals of A against the 8 points of box A.
-			// SATTest the normals of A against the 8 points of box B.
-			// points of each box are a combination of the box's top/bottom values.
-			double[] aLim = SATTest(n[i], a.p);
-			double[] bLim = SATTest(n[i], b.p);
-			// logger.info("Lim "+axis[i]+" > "+n[i].x+"\t"+n[i].y+"\t"+n[i].z+" :
-			// "+aLim[0]+","+aLim[1]+" vs "+bLim[0]+","+bLim[1]);
-
-			// if the two box projections do not overlap then there is no chance of a
-			// collision.
-			if (!overlaps(aLim[0], aLim[1], bLim[0], bLim[1])) {
-				// logger.info("Miss");
-				return false;
-			}
-		}
+        for (Vector3d vector3d : n) {
+            // SATTest the normals of A against the 8 points of box A.
+            // SATTest the normals of A against the 8 points of box B.
+            // points of each box are a combination of the box's top/bottom values.
+            double[] aLim = SATTest(vector3d, a.p);
+            double[] bLim = SATTest(vector3d, b.p);
+            // logger.info("Lim "+axis[i]+" > "+n[i].x+"\t"+n[i].y+"\t"+n[i].z+" :
+            // "+aLim[0]+","+aLim[1]+" vs "+bLim[0]+","+bLim[1]);
+
+            // if the two box projections do not overlap then there is no chance of a
+            // collision.
+            if (!overlaps(aLim[0], aLim[1], bLim[0], bLim[1])) {
+                // logger.info("Miss");
+                return false;
+            }
+        }
 
 		// intersect!
 		// logger.info("Hit");
@@ -198,11 +207,11 @@ static protected double[] SATTest(Vector3d normal, Point3d[] corners) {
 		values[0] =  Double.MAX_VALUE; // min value
 		values[1] = -Double.MAX_VALUE; // max value
 
-		for (int i = 0; i < corners.length; ++i) {
-			double dotProduct = corners[i].x * normal.x + corners[i].y * normal.y + corners[i].z * normal.z;
-			if (values[0] > dotProduct) values[0] = dotProduct;
-			if (values[1] < dotProduct) values[1] = dotProduct;
-		}
+        for (Point3d corner : corners) {
+            double dotProduct = corner.x * normal.x + corner.y * normal.y + corner.z * normal.z;
+            if (values[0] > dotProduct) values[0] = dotProduct;
+            if (values[1] < dotProduct) values[1] = dotProduct;
+        }
 
 		return values;
 	}
@@ -246,9 +255,9 @@ public static boolean meshMesh(final Matrix4d ma, final Mesh sa, final Matrix4d
 
 	/**
 	 * ray/sphere intersection. see <a href="https://viclw17.github.io/2018/07/16/raytracing-ray-sphere-intersection/">reference</a>.
-	 * @param ray
-	 * @param center
-	 * @param radius
+	 * @param ray start and direction
+	 * @param center center of sphere
+	 * @param radius radius of sphere
 	 * @return distance to first hit.  negative values for no hit/behind start. 
 	 */
 	static public double raySphere(final Ray ray,final Point3d center,final double radius) {
@@ -494,4 +503,24 @@ public static Vector3d buildNormalFrom3Points(Vector3d v0, Vector3d v1, Vector3d
 		normal.normalize();
 		return normal;
 	}
+
+	/**
+	 * is point within r of box (max,min)?
+	 * @param point the test point
+	 * @param r2 the square of the radius
+	 * @param max the max point of the box
+	 * @param min the min point of the box
+	 * @return true if point is within r of box (max,min)
+	 */
+	public static boolean sphereBox(Tuple3d point, double r2, Tuple3d max, Tuple3d min) {
+		if(r2<=0) throw new IllegalArgumentException("r2 must be >0");
+
+		if (point.x < min.x) r2 -= Math.pow(point.x - min.x, 2);
+		else if (point.x > max.x) r2 -= Math.pow(point.x - max.x, 2);
+		if (point.y < min.y) r2 -= Math.pow(point.y - min.y, 2);
+		else if (point.y > max.y) r2 -= Math.pow(point.y - max.y, 2);
+		if (point.z < min.z) r2 -= Math.pow(point.z - min.z, 2);
+		else if (point.z > max.z) r2 -= Math.pow(point.z - max.z, 2);
+		return r2 > 0;
+	}
 }

src/main/java/com/marginallyclever/ro3/node/nodes/pose/AttachmentPoint.java

@@ -1,5 +1,6 @@
 package com.marginallyclever.ro3.node.nodes.pose;
 
+import com.marginallyclever.convenience.helpers.IntersectionHelper;
 import com.marginallyclever.convenience.helpers.MatrixHelper;
 import com.marginallyclever.ro3.Registry;
 import com.marginallyclever.ro3.node.Node;
@@ -9,6 +10,7 @@
 
 import javax.swing.*;
 import javax.vecmath.Matrix4d;
+import javax.vecmath.Vector3d;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -80,20 +82,55 @@ public void attemptAttach() {
         while(!list.isEmpty()) {
             var node = list.remove(0);
             list.addAll(node.getChildren());
-            if(!(node instanceof Pose pose)) continue;
+            if(!(node instanceof Pose pose)) continue;  // grab only Poses
+            if(node instanceof MeshInstance) continue;  // don't grab MeshInstances
             // don't grab yourself
             if(pose == this || pose.hasParent(this) || this.hasParent(pose)) continue;
-            logger.debug("check "+pose.getAbsolutePath());
-            var pos = MatrixHelper.getPosition(pose.getWorld());
-            pos.sub(myPosition);
-            if( pos.lengthSquared() <= r2 ) {
+
+            if(canGrab(pose,myPosition,r2)) {
                 found.add(pose);
             }
         }
 
         attach(found);
     }
 
+    /**
+     * Check if a {@link Pose} is within reach and has a {@link MeshInstance}.
+     * @param pose the pose to check
+     * @param myPosition the position of this node
+     * @param r2 the square of the radius
+     * @return true if the pose is within reach.
+     */
+    private boolean canGrab(Pose pose, Vector3d myPosition, double r2) {
+        var meshInstance = pose.findFirstChild(MeshInstance.class);
+        if (meshInstance == null) return false;
+        var mesh = meshInstance.getMesh();
+        if (mesh == null) return false;
+
+        logger.debug("canGrab " + pose.getAbsolutePath());
+
+        int version=2;
+        if(version==1) {
+            // version 1, radius test.
+            var pos = MatrixHelper.getPosition(pose.getWorld());
+            pos.sub(myPosition);
+            return pos.lengthSquared() < r2;
+        } else {
+            // version 2, bounding box to radius test.
+            var boundingBox = mesh.getBoundingBox();
+            var max = boundingBox.getBoundsTop();
+            var min = boundingBox.getBoundsBottom();
+            if( IntersectionHelper.sphereBox(myPosition,r2,max,min) ) {
+                // TODO version 3, radius to triangles test.
+
+                return true;
+            }
+        }
+
+        return false;
+    }
+
     @Override
     public JSONObject toJSON() {
         var json = super.toJSON();
@@ -119,7 +156,7 @@ public double getRadius() {
     }
 
     public void setRadius(double radius) {
-        if(radius<0) throw new IllegalArgumentException("radius must be >=0");
+        if(radius<=0) throw new IllegalArgumentException("radius must be >0");
         this.radius = radius;
     }
 

src/main/java/com/marginallyclever/ro3/node/nodes/pose/AttachmentPointPanel.java

@@ -17,7 +17,7 @@ public AttachmentPointPanel(AttachmentPoint attachmentPoint) {
 
         // radius
         var formatter = NumberFormatHelper.getNumberFormatter();
-        formatter.setMinimum(0.0);  // no negative radius
+        formatter.setMinimum(0.01);  // radius > 0
         var radiusField = new JFormattedTextField(formatter);
         radiusField.setToolTipText("Radius of the attachment point");
         radiusField.setValue(attachmentPoint.getRadius());

src/main/java/com/marginallyclever/ro3/node/nodes/pose/MeshInstance.java

@@ -3,6 +3,7 @@
 import com.marginallyclever.convenience.Ray;
 import com.marginallyclever.convenience.helpers.MatrixHelper;
 import com.marginallyclever.ro3.Registry;
+import com.marginallyclever.ro3.mesh.AABB;
 import com.marginallyclever.ro3.mesh.Mesh;
 import com.marginallyclever.ro3.raypicking.RayHit;
 import org.json.JSONObject;

src/test/java/com/marginallyclever/convenience/helpers/IntersectionHelperTest.java

@@ -6,7 +6,9 @@
 import org.junit.jupiter.api.Test;
 import static org.junit.jupiter.api.Assertions.*;
 
+import javax.vecmath.Point2d;
 import javax.vecmath.Point3d;
+import javax.vecmath.Tuple3d;
 import javax.vecmath.Vector3d;
 
 public class IntersectionHelperTest {
@@ -29,15 +31,15 @@ public void testCylinderCylinder() {
     @Test
     public void testCuboidCuboid() {
         AABB a = new AABB();
-        a.setBounds(new Point3d(0, 0, 0),new Point3d(1, 1, 1));
+        a.setBounds(new Point3d(0, 0, 0), new Point3d(1, 1, 1));
         a.setPose(MatrixHelper.createIdentityMatrix4());
 
         AABB b = new AABB();
-        b.setBounds(new Point3d(2, 2, 2),new Point3d(3, 3, 3));
+        b.setBounds(new Point3d(2, 2, 2), new Point3d(3, 3, 3));
         b.setPose(MatrixHelper.createIdentityMatrix4());
 
         AABB c = new AABB();
-        c.setBounds(new Point3d(0.5, 0.5, 0.5),new Point3d(1.5, 1.5, 1.5));
+        c.setBounds(new Point3d(0.5, 0.5, 0.5), new Point3d(1.5, 1.5, 1.5));
         c.setPose(MatrixHelper.createIdentityMatrix4());
 
         assertFalse(IntersectionHelper.cuboidCuboid(a, b));
@@ -47,40 +49,61 @@ public void testCuboidCuboid() {
     @Test
     public void testRayBox() {
         AABB a = new AABB();
-        a.setBounds(new Point3d(0, 0, 0),new Point3d(1, 1, 1));
+        a.setBounds(new Point3d(0, 0, 0), new Point3d(1, 1, 1));
         a.setPose(MatrixHelper.createIdentityMatrix4());
 
         var p1 = new Point3d(0.5, 0.5, 1.5);
         var p2 = new Vector3d(0.0, 0.0, 1.0);
         var p3 = new Vector3d(0.0, 0.0, -1.0);
 
-        assertFalse(IntersectionHelper.rayBox(new Ray(p1, p2), a.getBoundsBottom(), a.getBoundsTop())>0);
-        assertTrue(IntersectionHelper.rayBox(new Ray(p1, p3), a.getBoundsBottom(), a.getBoundsTop())>0);
+        assertFalse(IntersectionHelper.rayBox(new Ray(p1, p2), a.getBoundsBottom(), a.getBoundsTop()) > 0);
+        assertTrue(IntersectionHelper.rayBox(new Ray(p1, p3), a.getBoundsBottom(), a.getBoundsTop()) > 0);
     }
 
     @Test
     public void testcenterOfCircumscribedSphere() {
         var result = IntersectionHelper.centerOfCircumscribedSphere(
-                new Vector3d(0,0,1),
-                new Vector3d(1,0,0),
-                new Vector3d(0,1,0),
+                new Vector3d(0, 0, 1),
+                new Vector3d(1, 0, 0),
+                new Vector3d(0, 1, 0),
                 1);
-        assertEquals(0,result.x,1e-6);
-        assertEquals(0,result.y,1e-6);
-        assertEquals(0,result.z,1e-6);
+        assertEquals(0, result.x, 1e-6);
+        assertEquals(0, result.y, 1e-6);
+        assertEquals(0, result.z, 1e-6);
     }
 
     @Test
     public void testRayTriangle() {
-        Ray a = new Ray(new Point3d(0,0,0),new Vector3d(0,0,1));
-        Vector3d v0 = new Vector3d(-1,-1,1);
-        Vector3d v1 = new Vector3d(1,0,1);
-        Vector3d v2 = new Vector3d(0,1,1);
+        Ray a = new Ray(new Point3d(0, 0, 0), new Vector3d(0, 0, 1));
+        Vector3d v0 = new Vector3d(-1, -1, 1);
+        Vector3d v1 = new Vector3d(1, 0, 1);
+        Vector3d v2 = new Vector3d(0, 1, 1);
         var result = IntersectionHelper.rayTriangle(a, v0, v1, v2);
-        assertEquals(1,result,1e-6);
-        Ray b = new Ray(new Point3d(0,0,0),new Vector3d(0,0,-1));
+        assertEquals(1, result, 1e-6);
+        Ray b = new Ray(new Point3d(0, 0, 0), new Vector3d(0, 0, -1));
         result = IntersectionHelper.rayTriangle(b, v0, v1, v2);
-        assertEquals(Double.MAX_VALUE,result,1e-6);
+        assertEquals(Double.MAX_VALUE, result, 1e-6);
+    }
+
+    @Test
+    public void sphereBox() {
+        var p = new Point3d(0, 0,0);
+        double r = 2;
+        var min = new Point3d(1, 1,1);
+        var max = new Point3d(3, 3,3);
+        assert(IntersectionHelper.sphereBox(p,r*r,max,min));
+        r=0.5;
+        assert(!IntersectionHelper.sphereBox(p,r*r,max,min));
+        p.set(4,4,4);
+        assert(!IntersectionHelper.sphereBox(p,r*r,max,min));
+        r=2;
+        assert(IntersectionHelper.sphereBox(p,r*r,max,min));
+        r=0.01;
+        p.set(2,2,2);
+        assert(IntersectionHelper.sphereBox(p,r*r,max,min));
+        r=1;
+        assert(IntersectionHelper.sphereBox(p,r*r,max,min));
+        r=5;
+        assert(IntersectionHelper.sphereBox(p,r*r,max,min));
     }
-    // Add more tests for other methods here...
 }