public BoxShape transform(double T[][]) {
   BoxShape bs = new BoxShape();
   bs.sxyz = LinAlg.copy(sxyz);
   bs.vertices = LinAlg.transform(T, vertices);
   bs.planes = LinAlg.transformPlanes(T, planes);
   return bs;
 }
Exemple #2
0
  // if we don't have anything to filter, just use Dead Reckoning
  public double[][] estimate() {
    // use new velocity estimates to estimate xyzrpy
    double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);

    // convert back to RBT
    double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
    // XXX there is a 2nd version of the function above which one is better
    return Erbt;
  }
  public static void mixture() {
    Graph g = new Graph();

    int cnt = 5;

    for (int i = 0; i < cnt; i++) {
      GXYTNode gn = new GXYTNode();
      gn.init = new double[] {i, 0, 0};
      gn.state = LinAlg.copy(gn.init);
      gn.truth = new double[3];
      g.nodes.add(gn);

      if (i > 0) {
        double sig = 0.06;

        // wheel slippage mode
        GXYTEdge ge1 = new GXYTEdge();
        ge1.z = new double[] {0, 0, 0};
        ge1.P = LinAlg.diag(new double[] {sig, sig, sig});
        ge1.nodes = new int[] {i - 1, i};

        // nominal odometry
        GXYTEdge ge2 = new GXYTEdge();
        ge2.z = new double[] {1, 0, 0};
        ge2.P = LinAlg.diag(new double[] {sig, sig, sig});
        ge2.nodes = new int[] {i - 1, i};

        GEdgeMixture ge = new GEdgeMixture(new GEdge[] {ge1, ge2}, new double[] {0.05, 0.95});
        g.edges.add(ge);
      }
    }

    if (true) {
      GXYTEdge ge = new GXYTEdge();
      ge.z = new double[] {cnt, 0, 0};
      ge.P = LinAlg.diag(new double[] {0.1, 0.1, 0.1});
      ge.nodes = new int[] {0, g.nodes.size() - 1};
      g.edges.add(ge);
    }

    try {
      g.write("mixture.graph");
    } catch (IOException ex) {
      System.out.println("ex: " + ex);
    }
  }
  public void addLayer(VisLayer layer, double fviewport[]) {
    FPosition fpos = new FPosition();
    fpos.layer = layer;
    fpos.fviewport = LinAlg.copy(fviewport);
    fposes.add(fpos);

    vc.draw();
  }
Exemple #5
0
 // weight A by alpha and B by 1-alpha if alpha == 1 then returns A
 private double[] weightedSum(double[] A, double[] B, double[] W) {
   assert ((A.length == B.length) && (A.length == W.length)) : "Warning!";
   double[] C = new double[A.length];
   for (int i = 0; i < A.length; i++) {
     C[i] = (1 - W[i]) * A[i] + W[i] * B[i];
     if (C[i] == Double.NaN) {
       System.out.println("Warning generated NaN: T = " + T);
       LinAlg.print(W);
     }
   }
   return C;
 }
Exemple #6
0
  // incorporate new information and return new estimate
  public double[][] estimate(double[][] Urbt) {
    // what rigid body transformation occured over the last frame
    double[] DUxyzrpy = LinAlg.matrixToXyzrpy(Urbt);

    // convert to velocity
    double[] Uxyzrpy;
    if (T != 0) {
      Uxyzrpy = LinAlg.scale(DUxyzrpy, 1 / T);
    } else {
      Uxyzrpy = new double[6];
    }
    /*
    System.out.println("Our observation velocity");
    LinAlg.print(Uxyzrpy);

    System.out.println("Our best estimate of velocity");
    LinAlg.print(Vxyzrpy);
     */
    // calculate relative weighting
    double[] W = constructW(Uxyzrpy);
    /*
    System.out.println("Our weighting matrix");
    LinAlg.print(W);
     */
    // incorporatenew information
    Vxyzrpy = weightedSum(Vxyzrpy, Uxyzrpy, W);
    /*
    System.out.println("After in corporating information");
    LinAlg.print(Vxyzrpy);
     */
    // use new velocity estimates to estimate xyzrpy
    double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);

    // convert back to RBT
    double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
    // XXX there is a 2nd version of the function above which one is better
    return Erbt;
  }
  public static void nomixture() {
    Graph g = new Graph();

    int cnt = 5;

    double u = 0.95;
    double sig = 0.1075;

    for (int i = 0; i < cnt; i++) {
      GXYTNode gn = new GXYTNode();
      gn.init = new double[] {i, 0, 0};
      gn.state = LinAlg.copy(gn.init);
      g.nodes.add(gn);

      if (i > 0) {
        GXYTEdge ge = new GXYTEdge();
        ge.z = new double[] {0.95, 0, 0};
        ge.P = LinAlg.diag(new double[] {sig, sig, sig});
        ge.nodes = new int[] {i - 1, i};
        g.edges.add(ge);
      }
    }

    if (true) {
      GXYTEdge ge = new GXYTEdge();
      ge.z = new double[] {0, 0, 0};
      ge.P = LinAlg.diag(new double[] {0.1, 0.1, 0.1});
      ge.nodes = new int[] {0, g.nodes.size() - 1};
      g.edges.add(ge);
    }

    try {
      g.write("nomixture.graph");
    } catch (IOException ex) {
      System.out.println("ex: " + ex);
    }
  }
  public void read(StructureReader ins) throws IOException {
    int ncomponents = ins.readInt();
    components = new GEdge[ncomponents];
    weights = new double[ncomponents];

    for (int i = 0; i < components.length; i++) {
      weights[i] = ins.readDouble();
      String cls = ins.readString();
      components[i] = (GEdge) ReflectUtil.createObject(cls);
      ins.blockBegin();
      components[i].read(ins);
      ins.blockEnd();
    }

    attributes = Attributes.read(ins);

    nodes = LinAlg.copy(components[0].nodes);
  }
  /** Components should all be of the same edge type. Weights should sum to 1. * */
  public GEdgeMixture(GEdge components[], double weights[]) {
    this.components = components;
    this.weights = weights;

    double total = 0;
    assert (components.length == weights.length);
    for (int i = 0; i < components.length; i++) {
      total += weights[i];
      assert (components[i].getClass().equals(components[0].getClass()));

      assert (components[i].nodes.length == components[0].nodes.length);
      for (int j = 0; j < components[0].nodes.length; j++) {
        assert (components[i].nodes[j] == components[0].nodes[j]);
      }
    }

    assert (total >= 0.99 && total <= 1.01);

    this.nodes = LinAlg.copy(components[0].nodes);
  }
  public BoxShape(double sxyz[]) {
    this.sxyz = LinAlg.copy(sxyz);
    planes = new ArrayList<double[]>();
    vertices = new ArrayList<double[]>();

    // create planes and vertices
    planes.add(new double[] {-1, 0, 0, -sxyz[0] / 2});
    planes.add(new double[] {1, 0, 0, -sxyz[0] / 2});

    planes.add(new double[] {0, -1, 0, -sxyz[1] / 2});
    planes.add(new double[] {0, 1, 0, -sxyz[1] / 2});

    planes.add(new double[] {0, 0, -1, -sxyz[2] / 2});
    planes.add(new double[] {0, 0, 1, -sxyz[2] / 2});

    for (int i = -1; i < 2; i += 2)
      for (int j = -1; j < 2; j += 2)
        for (int k = -1; k < 2; k += 2)
          vertices.add(new double[] {i * sxyz[0] / 2, j * sxyz[1] / 2, k * sxyz[2] / 2});
  }
Exemple #11
0
  public VzOFF(String path, VzMesh.Style style) throws IOException {
    this.path = path;
    this.style = style;

    BufferedReader ins = new BufferedReader(new FileReader(new File(path)));

    FloatArray vertexArray = new FloatArray();
    IntArray indexArray = new IntArray();
    FloatArray normalArray;

    String header = ins.readLine();
    if (!header.equals("OFF")) throw new IOException("Not an OFF file");

    int nvertexArray, nfaces, nedges;
    if (true) {
      String sizes = ins.readLine();
      String toks[] = fastSplit(sizes);
      nvertexArray = Integer.parseInt(toks[0]);
      nfaces = Integer.parseInt(toks[1]);
      nedges = Integer.parseInt(toks[2]);
    }

    for (int i = 0; i < nvertexArray; i++) {
      String line = ins.readLine();
      String toks[] = fastSplit(line);

      float x = Float.parseFloat(toks[0]);
      float y = Float.parseFloat(toks[1]);
      float z = Float.parseFloat(toks[2]);

      xyz_min[0] = Math.min(xyz_min[0], x);
      xyz_min[1] = Math.min(xyz_min[1], y);
      xyz_min[2] = Math.min(xyz_min[2], z);

      xyz_max[0] = Math.max(xyz_max[0], x);
      xyz_max[1] = Math.max(xyz_max[1], y);
      xyz_max[2] = Math.max(xyz_max[2], z);

      vertexArray.add(x);
      vertexArray.add(y);
      vertexArray.add(z);
    }

    float vs[] = vertexArray.getData();
    float ns[] = new float[vs.length];
    normalArray = new FloatArray(ns);

    for (int i = 0; i < nfaces; i++) {
      String line = ins.readLine();
      String toks[] = fastSplit(line);

      int len = Integer.parseInt(toks[0]);
      assert (len + 1 == toks.length);

      for (int j = 2; j + 1 <= len; j++) {
        int a = Integer.parseInt(toks[1]);
        int b = Integer.parseInt(toks[j]);
        int c = Integer.parseInt(toks[j + 1]);

        indexArray.add(a);
        indexArray.add(b);
        indexArray.add(c);

        float vba[] =
            new float[] {
              vs[b * 3 + 0] - vs[a * 3 + 0],
              vs[b * 3 + 1] - vs[a * 3 + 1],
              vs[b * 3 + 2] - vs[a * 3 + 2]
            };

        float vca[] =
            new float[] {
              vs[c * 3 + 0] - vs[a * 3 + 0],
              vs[c * 3 + 1] - vs[a * 3 + 1],
              vs[c * 3 + 2] - vs[a * 3 + 2]
            };

        float n[] = LinAlg.normalize(LinAlg.crossProduct(vba, vca));

        for (int k = 0; k < 3; k++) {
          ns[3 * a + k] += n[k];
          ns[3 * b + k] += n[k];
          ns[3 * c + k] += n[k];
        }
      }
    }

    for (int i = 0; i + 2 < ns.length; i += 3) {
      double mag = Math.sqrt(ns[i + 0] * ns[i + 0] + ns[i + 1] * ns[i + 1] + ns[i + 2] * ns[i + 2]);
      ns[i + 0] /= mag;
      ns[i + 1] /= mag;
      ns[i + 2] /= mag;
    }

    mesh =
        new VzMesh(
            new VisVertexData(vs, vs.length / 3, 3),
            new VisVertexData(ns, ns.length / 3, 3),
            new VisIndexData(indexArray),
            VzMesh.TRIANGLES);
    ins.close();
  }
Exemple #12
0
  /** Call with one or more OFF model paths * */
  public static void main(String args[]) {
    JFrame f = new JFrame("VzOFF " + args[0]);
    f.setLayout(new BorderLayout());

    VisWorld vw = new VisWorld();
    VisLayer vl = new VisLayer(vw);
    VisCanvas vc = new VisCanvas(vl);

    VzMesh.Style defaultMeshStyle = new VzMesh.Style(Color.cyan);

    ArrayList<VzOFF> models = new ArrayList<VzOFF>();

    for (int i = 0; i < args.length; i++) {
      if (args[i].endsWith(".off")) {
        try {
          models.add(new VzOFF(args[i], defaultMeshStyle));
          System.out.printf("Loaded: %20s (%5.2f%%)\n", args[i], i * 100.0 / args.length);
        } catch (IOException ex) {
          System.out.println("ex: " + ex);
        }
      } else {
        System.out.printf("Ignoring file with wrong suffix: " + args[i]);
      }
    }

    if (models.size() == 0) {
      System.out.println("No models specified\n");
      return;
    }

    int rows = (int) Math.sqrt(models.size());
    int cols = models.size() / rows + 1;

    //        VzGrid.addGrid(vw);

    VisWorld.Buffer vb = vw.getBuffer("models");
    for (int y = 0; y < rows; y++) {
      for (int x = 0; x < cols; x++) {
        int idx = y * cols + x;
        if (idx >= models.size()) break;

        VzOFF model = models.get(idx);

        double mx =
            Math.max(
                model.xyz_max[2] - model.xyz_min[2],
                Math.max(model.xyz_max[1] - model.xyz_min[1], model.xyz_max[0] - model.xyz_min[0]));

        vb.addBack(
            new VisChain(
                LinAlg.translate(x + .5, rows - (y + .5), 0),
                new VzRectangle(1, 1, new VzLines.Style(Color.white, 3)),
                new VisChain(
                    LinAlg.translate(0, .4, 0),
                    new VzText(
                        VzText.ANCHOR.CENTER,
                        String.format("<<sansserif-20,scale=.003>>%s", baseName(model.path)))),
                LinAlg.scale(.5, .5, .5),
                LinAlg.scale(1.0 / mx, 1.0 / mx, 1.0 / mx),
                LinAlg.translate(
                    -(model.xyz_max[0] + model.xyz_min[0]) / 2.0,
                    -(model.xyz_max[1] + model.xyz_min[1]) / 2.0,
                    -(model.xyz_max[2] + model.xyz_min[2]) / 2.0),
                model));
      }
    }

    vb.swap();

    vl.cameraManager.fit2D(new double[] {0, 0, 0}, new double[] {cols, rows, 0}, true);
    ((DefaultCameraManager) vl.cameraManager).interfaceMode = 3.0;

    f.add(vc);
    f.setSize(600, 400);
    f.setVisible(true);
  }