Exemplo n.º 1
0
 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++;
       }
     }
   }
 }
Exemplo n.º 2
0
 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]);
 }
Exemplo n.º 3
0
 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++;
   }
 }
Exemplo n.º 4
0
  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;
  }
Exemplo n.º 5
0
 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);
 }
Exemplo n.º 6
0
  // 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});
 }
Exemplo n.º 8
0
  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;
  }
Exemplo n.º 9
0
  /**
   * 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;
      }
    }
  }