private void stepSimplicies23() { if ((count2 < complex.getSimplicies()[2].getSize()) && (count3 < complex.getSimplicies()[3].getSize())) { Simplex simplex2 = (Simplex) complex.getSimplicies()[2].get(count2); Simplex simplex3 = (Simplex) complex.getSimplicies()[3].get(count3); if (simplex3.getAlpha() < simplex2.getAlpha()) { addTetrahedron(simplex3, Colors.yellow); System.out.println(count3 + ": alpha3 = " + simplex3.getAlpha()); count3++; } else { System.out.println(count2 + ": alpha2 = " + simplex2.getAlpha()); addTriangle(simplex2, Colors.yellow); count2++; } } else { if (count2 < complex.getSimplicies()[2].getSize()) { Simplex simplex2 = (Simplex) complex.getSimplicies()[2].get(count2); System.out.println(count2 + ": alpha2 = " + simplex2.getAlpha()); addTriangle(simplex2, Colors.yellow); count2++; } else { if (count3 < complex.getSimplicies()[3].getSize()) { Simplex simplex3 = (Simplex) complex.getSimplicies()[3].get(count3); addTetrahedron(simplex3, Colors.yellow); System.out.println(count3 + ": alpha3 = " + simplex3.getAlpha()); count3++; } } } }
public static void test(double[][] A, double[] b, double[] c) { Simplex lp = new Simplex(A, b, c); System.out.println("value = " + lp.value()); double[] x = lp.primal(); for (int i = 0; i < x.length; i++) System.out.println("x[" + i + "] = " + x[i]); double[] y = lp.dual(); for (int j = 0; j < y.length; j++) System.out.println("y[" + j + "] = " + y[j]); }
private void stepSimplicies3() { if (count3 < complex.getSimplicies()[3].getSize()) { Simplex simplex3 = (Simplex) complex.getSimplicies()[3].get(count3); addTetrahedron(simplex3, Colors.yellow); System.out.println(count3 + ": alpha3 = " + simplex3.getAlpha()); count3++; } }
public BranchGroup addTriangle(Simplex simplex, Color3f color) { int[] p = simplex.getPoints(); BranchGroup bgTriangle = new BranchGroup(); bgTriangle.setCapability(BranchGroup.ALLOW_DETACH); javax.vecmath.Point3d[] coords = new javax.vecmath.Point3d[6]; TriangleArray ta = new TriangleArray(6, TriangleArray.COORDINATES | TriangleArray.COLOR_3); Point3d p0 = (Point3d) complex.getPoints().get(simplex.getPoints()[0]); Point3d p1 = (Point3d) complex.getPoints().get(simplex.getPoints()[1]); Point3d p2 = (Point3d) complex.getPoints().get(simplex.getPoints()[2]); coords[0] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ()); coords[1] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ()); coords[2] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ()); coords[3] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ()); coords[4] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ()); coords[5] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ()); ta.setCoordinates(0, coords); Color3f[] colors = new Color3f[6]; for (int i = 0; i < 3; i++) colors[i] = Colors.black; for (int i = 3; i < 6; i++) colors[i] = Colors.yellow; ta.setColors(0, colors); Appearance ap = new Appearance(); ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_READ); ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_WRITE); ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_READ); ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE); ap.setLineAttributes(new LineAttributes(2, LineAttributes.PATTERN_SOLID, false)); ap.setMaterial(new Material()); ap.setTransparencyAttributes( new TransparencyAttributes(TransparencyAttributes.SCREEN_DOOR, 0.5f)); Shape3D shape = new Shape3D(ta, ap); shape.setCapability(Shape3D.ALLOW_APPEARANCE_READ); shape.setCapability(Shape3D.ALLOW_APPEARANCE_WRITE); simplex.setTriangle(shape); // simplex.bgTriangle = bgTriangle; TransformGroup tgTriangle = new TransformGroup(); tgTriangle.addChild(shape); bgTriangle.addChild(tgTriangle); spin.addChild(bgTriangle); return bgTriangle; }
public void addSphere(Simplex simplex, double radius, Color3f color) { geom3d.Point3d p = (Point3d) complex.getPoints().get(simplex.getPoints()[0]); BranchGroup bgSphere = new BranchGroup(); bgSphere.setCapability(BranchGroup.ALLOW_DETACH); Appearance ap = new Appearance(); ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_READ); ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE); ap.setColoringAttributes(new ColoringAttributes(color, ColoringAttributes.NICEST)); simplex.setSphere(new Sphere(new Float(radius), ap)); simplex.getSphere().getShape().setCapability(Shape3D.ALLOW_APPEARANCE_READ); simplex.getSphere().getShape().setCapability(Shape3D.ALLOW_APPEARANCE_WRITE); Transform3D tr = new Transform3D(); tr.setTranslation(new javax.vecmath.Vector3d(p.getX(), p.getY(), p.getZ())); TransformGroup tgSphere = new TransformGroup(tr); tgSphere.addChild(simplex.getSphere()); bgSphere.addChild(tgSphere); spin.addChild(bgSphere); }
// test client public static void main(String[] args) { try { test1(); } catch (ArithmeticException e) { e.printStackTrace(); } System.out.println("--------------------------------"); try { test2(); } catch (ArithmeticException e) { e.printStackTrace(); } System.out.println("--------------------------------"); try { test3(); } catch (ArithmeticException e) { e.printStackTrace(); } System.out.println("--------------------------------"); try { test4(); } catch (ArithmeticException e) { e.printStackTrace(); } System.out.println("--------------------------------"); int M = Integer.parseInt(args[0]); int N = Integer.parseInt(args[1]); double[] c = new double[N]; double[] b = new double[M]; double[][] A = new double[M][N]; for (int j = 0; j < N; j++) c[j] = Math.random() * 1000; for (int i = 0; i < M; i++) b[i] = Math.random() * 1000; for (int i = 0; i < M; i++) for (int j = 0; j < N; j++) A[i][j] = Math.random() * 1000; Simplex lp = new Simplex(A, b, c); System.out.println(lp.value()); }
/** * Tests some exceptional behavior. * * @exception IndexOutOfBoundsException * @see java.lang.System#getProperty(java.lang.String) * @see SecurityManager#checkPermission */ @Test(expected = IllegalArgumentException.class) @SuppressWarnings("unused") public void testForException() { Simplex sloser0 = Simplex.getSimplex(new int[] {-1, Integer.MAX_VALUE}); Simplex sloser1 = Simplex.getSimplex(new int[] {1, 1}); }
public BranchGroup addTetrahedron(Simplex simplex, Color3f color) { BranchGroup bgTetrahedron = new BranchGroup(); bgTetrahedron.setCapability(BranchGroup.ALLOW_DETACH); javax.vecmath.Point3d[] coords = new javax.vecmath.Point3d[12]; LineArray la = new LineArray(12, LineArray.COORDINATES | LineArray.COLOR_3); Point3d p0 = (Point3d) complex.getPoints().get(simplex.getPoints()[0]); Point3d p1 = (Point3d) complex.getPoints().get(simplex.getPoints()[1]); Point3d p2 = (Point3d) complex.getPoints().get(simplex.getPoints()[2]); Point3d p3 = (Point3d) complex.getPoints().get(simplex.getPoints()[3]); coords[0] = coords[2] = coords[4] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ()); coords[1] = coords[6] = coords[8] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ()); coords[3] = coords[7] = coords[10] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ()); coords[5] = coords[9] = coords[11] = new javax.vecmath.Point3d(p3.getX(), p3.getY(), p3.getZ()); la.setCoordinates(0, coords); Color3f[] colors = new Color3f[12]; for (int i = 0; i < 12; i++) colors[i] = Colors.black; la.setColors(0, colors); javax.vecmath.Point3d[] coordsTr = new javax.vecmath.Point3d[24]; TriangleArray tr = new TriangleArray(24, TriangleArray.COORDINATES | TriangleArray.COLOR_3); coordsTr[0] = coordsTr[3] = coordsTr[6] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ()); coordsTr[1] = coordsTr[4] = coordsTr[9] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ()); coordsTr[2] = coordsTr[7] = coordsTr[10] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ()); coordsTr[5] = coordsTr[8] = coordsTr[11] = new javax.vecmath.Point3d(p3.getX(), p3.getY(), p3.getZ()); coordsTr[12] = coordsTr[15] = coordsTr[18] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ()); coordsTr[14] = coordsTr[17] = coordsTr[21] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ()); coordsTr[13] = coordsTr[20] = coordsTr[23] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ()); coordsTr[16] = coordsTr[19] = coordsTr[22] = new javax.vecmath.Point3d(p3.getX(), p3.getY(), p3.getZ()); tr.setCoordinates(0, coordsTr); Color3f[] colorsTr = new Color3f[24]; for (int i = 0; i < 24; i++) colorsTr[i] = Colors.green; tr.setColors(0, colorsTr); Appearance ap = new Appearance(); ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_READ); ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_WRITE); ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_READ); ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE); // ap.setCapability(Appearance.ALLOW_MATERIAL_READ); // ap.setCapability(Appearance.ALLOW_MATERIAL_WRITE); ap.setLineAttributes(new LineAttributes(2, LineAttributes.PATTERN_SOLID, false)); ap.setMaterial(new Material()); // ap.setTransparencyAttributes(new // TransparencyAttributes(TransparencyAttributes.SCREEN_DOOR, 0.5f)); // LineArray la = cell.getLineArray(); Shape3D shape = new Shape3D(la, ap); Shape3D shapeTr = new Shape3D(tr, ap); shape.setCapability(Shape3D.ALLOW_APPEARANCE_READ); shape.setCapability(Shape3D.ALLOW_APPEARANCE_WRITE); simplex.setTetrahedron(shape); // cell.setLineArray(la); simplex.setTriangleArray(tr); simplex.setBgTetrahedron(bgTetrahedron); TransformGroup tgTetrahedron = new TransformGroup(); tgTetrahedron.addChild(shape); tgTetrahedron.addChild(shapeTr); bgTetrahedron.addChild(tgTetrahedron); spin.addChild(bgTetrahedron); return bgTetrahedron; }
/** * Compute the closest points between two shapes. Supports any combination of: CircleShape and * PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to * zero. * * @param output * @param cache * @param input */ public final void distance( final DistanceOutput output, final SimplexCache cache, final DistanceInput input) { GJK_CALLS++; final DistanceProxy proxyA = input.proxyA; final DistanceProxy proxyB = input.proxyB; Transform transformA = input.transformA; Transform transformB = input.transformB; // Initialize the simplex. simplex.readCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.vertices; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. // (pooled above) int saveCount = 0; simplex.getClosestPoint(closestPoint); float distanceSqr1 = closestPoint.lengthSquared(); float distanceSqr2 = distanceSqr1; // Main iteration loop int iter = 0; while (iter < MAX_ITERS) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int i = 0; i < saveCount; i++) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_count) { case 1: break; case 2: simplex.solve2(); break; case 3: simplex.solve3(); break; default: assert (false); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.m_count == 3) { break; } // Compute closest point. simplex.getClosestPoint(closestPoint); distanceSqr2 = closestPoint.lengthSquared(); // ensure progress if (distanceSqr2 >= distanceSqr1) { // break; } distanceSqr1 = distanceSqr2; // get search direction; simplex.getSearchDirection(d); // Ensure the search direction is numerically fit. if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } /* * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; */ // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = vertices[simplex.m_count]; Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp); vertex.indexA = proxyA.getSupport(temp); Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA); // Vec2 wBLocal; Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp); vertex.indexB = proxyB.getSupport(temp); Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB); vertex.w.set(vertex.wB).subLocal(vertex.wA); // Iteration count is equated to the number of support point calls. ++iter; ++GJK_ITERS; // Check for duplicate support points. This is the main termination criteria. boolean duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.m_count; } GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter); // Prepare output. simplex.getWitnessPoints(output.pointA, output.pointB); output.distance = MathUtils.distance(output.pointA, output.pointB); output.iterations = iter; // Cache the simplex. simplex.writeCache(cache); // Apply radii if requested. if (input.useRadii) { float rA = proxyA.m_radius; float rB = proxyB.m_radius; if (output.distance > rA + rB && output.distance > Settings.EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; normal.set(output.pointB).subLocal(output.pointA); normal.normalize(); temp.set(normal).mulLocal(rA); output.pointA.addLocal(temp); temp.set(normal).mulLocal(rB); output.pointB.subLocal(temp); } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. // Vec2 p = 0.5f * (output.pointA + output.pointB); output.pointA.addLocal(output.pointB).mulLocal(.5f); output.pointB.set(output.pointA); output.distance = 0.0f; } } }