Пример #1
0
    public void Support(Vector3f d, Mkv v) {
      v.r.set(d);

      Vector3f tmp1 = LocalSupport(d, 0, Stack.alloc(Vector3f.class));

      Vector3f tmp = Stack.alloc(Vector3f.class);
      tmp.set(d);
      tmp.negate();
      Vector3f tmp2 = LocalSupport(tmp, 1, Stack.alloc(Vector3f.class));

      v.w.sub(tmp1, tmp2);
      v.w.scaleAdd(margin, d, v.w);
    }
Пример #2
0
    public Vector3f LocalSupport(Vector3f d, /*unsigned*/ int i, Vector3f out) {
      Vector3f tmp = Stack.alloc(Vector3f.class);
      MatrixUtil.transposeTransform(tmp, d, wrotations[i]);

      shapes[i].localGetSupportingVertex(tmp, out);
      wrotations[i].transform(out);
      out.add(positions[i]);

      return out;
    }
Пример #3
0
    public boolean SolveSimplex3a(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f cabc) {
      // TODO: optimize

      Vector3f tmp = Stack.alloc(Vector3f.class);
      tmp.cross(cabc, ab);

      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      tmp2.cross(cabc, ac);

      if (tmp.dot(ao) < -GJK_insimplex_eps) {
        order = 1;
        simplex[0].set(simplex[1]);
        simplex[1].set(simplex[2]);
        return SolveSimplex2(ao, ab);
      } else if (tmp2.dot(ao) > +GJK_insimplex_eps) {
        order = 1;
        simplex[1].set(simplex[2]);
        return SolveSimplex2(ao, ac);
      } else {
        float d = cabc.dot(ao);
        if (Math.abs(d) > GJK_insimplex_eps) {
          if (d > 0) {
            ray.set(cabc);
          } else {
            ray.negate(cabc);

            Mkv swapTmp = new Mkv();
            swapTmp.set(simplex[0]);
            simplex[0].set(simplex[1]);
            simplex[1].set(swapTmp);
          }
          return false;
        } else {
          return true;
        }
      }
    }
Пример #4
0
 public boolean FetchSupport() {
   int h = Hash(ray);
   He e = table[h];
   while (e != null) {
     if (e.v.equals(ray)) {
       --order;
       return false;
     } else {
       e = e.n;
     }
   }
   // e = (He*)sa->allocate(sizeof(He));
   // e = new He();
   e = stackHe.get();
   e.v.set(ray);
   e.n = table[h];
   table[h] = e;
   Support(ray, simplex[++order]);
   return (ray.dot(simplex[order].w) > 0);
 }
Пример #5
0
 public boolean SolveSimplex2(Vector3f ao, Vector3f ab) {
   if (ab.dot(ao) >= 0) {
     Vector3f cabo = Stack.alloc(Vector3f.class);
     cabo.cross(ab, ao);
     if (cabo.lengthSquared() > GJK_sqinsimplex_eps) {
       ray.cross(cabo, ab);
     } else {
       return true;
     }
   } else {
     order = 0;
     simplex[0].set(simplex[1]);
     ray.set(ao);
   }
   return (false);
 }
Пример #6
0
    public float EvaluatePD(float accuracy) {
      pushStack();
      try {
        Vector3f tmp = Stack.alloc(Vector3f.class);

        // btBlock* sablock = sa->beginBlock();
        Face bestface = null;
        int markid = 1;
        depth = -cstInf;
        normal.set(0f, 0f, 0f);
        root = null;
        nfaces = 0;
        iterations = 0;
        failed = false;
        /* Prepare hull		*/
        if (gjk.EncloseOrigin()) {
          // const U* pfidx = 0;
          int[][] pfidx_ptr = null;
          int pfidx_index = 0;

          int nfidx = 0;
          // const U* peidx = 0;
          int[][] peidx_ptr = null;
          int peidx_index = 0;

          int neidx = 0;
          Mkv[] basemkv = new Mkv[5];
          Face[] basefaces = new Face[6];
          switch (gjk.order) {
              // Tetrahedron
            case 3:
              {
                // pfidx=(const U*)fidx;
                pfidx_ptr = tetrahedron_fidx;
                pfidx_index = 0;

                nfidx = 4;

                // peidx=(const U*)eidx;
                peidx_ptr = tetrahedron_eidx;
                peidx_index = 0;

                neidx = 6;
              }
              break;
              // Hexahedron
            case 4:
              {
                // pfidx=(const U*)fidx;
                pfidx_ptr = hexahedron_fidx;
                pfidx_index = 0;

                nfidx = 6;

                // peidx=(const U*)eidx;
                peidx_ptr = hexahedron_eidx;
                peidx_index = 0;

                neidx = 9;
              }
              break;
          }
          int i;

          for (i = 0; i <= gjk.order; ++i) {
            basemkv[i] = new Mkv();
            basemkv[i].set(gjk.simplex[i]);
          }
          for (i = 0; i < nfidx; ++i, pfidx_index++) {
            basefaces[i] =
                NewFace(
                    basemkv[pfidx_ptr[pfidx_index][0]],
                    basemkv[pfidx_ptr[pfidx_index][1]],
                    basemkv[pfidx_ptr[pfidx_index][2]]);
          }
          for (i = 0; i < neidx; ++i, peidx_index++) {
            Link(
                basefaces[peidx_ptr[peidx_index][0]],
                peidx_ptr[peidx_index][1],
                basefaces[peidx_ptr[peidx_index][2]],
                peidx_ptr[peidx_index][3]);
          }
        }
        if (0 == nfaces) {
          // sa->endBlock(sablock);
          return (depth);
        }
        /* Expand hull		*/
        for (; iterations < EPA_maxiterations; ++iterations) {
          Face bf = FindBest();
          if (bf != null) {
            tmp.negate(bf.n);
            Mkv w = Support(tmp);
            float d = bf.n.dot(w.w) + bf.d;
            bestface = bf;
            if (d < -accuracy) {
              Face[] cf = new Face[] {null};
              Face[] ff = new Face[] {null};
              int nf = 0;
              Detach(bf);
              bf.mark = ++markid;
              for (int i = 0; i < 3; ++i) {
                nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff);
              }
              if (nf <= 2) {
                break;
              }
              Link(cf[0], 1, ff[0], 2);
            } else {
              break;
            }
          } else {
            break;
          }
        }
        /* Extract contact	*/
        if (bestface != null) {
          Vector3f b = GetCoordinates(bestface, Stack.alloc(Vector3f.class));
          normal.set(bestface.n);
          depth = Math.max(0, bestface.d);
          for (int i = 0; i < 2; ++i) {
            float s = i != 0 ? -1f : 1f;
            for (int j = 0; j < 3; ++j) {
              tmp.scale(s, bestface.v[j].r);
              gjk.LocalSupport(tmp, i, features[i][j]);
            }
          }

          Vector3f tmp1 = Stack.alloc(Vector3f.class);
          Vector3f tmp2 = Stack.alloc(Vector3f.class);
          Vector3f tmp3 = Stack.alloc(Vector3f.class);

          tmp1.scale(b.x, features[0][0]);
          tmp2.scale(b.y, features[0][1]);
          tmp3.scale(b.z, features[0][2]);
          VectorUtil.add(nearest[0], tmp1, tmp2, tmp3);

          tmp1.scale(b.x, features[1][0]);
          tmp2.scale(b.y, features[1][1]);
          tmp3.scale(b.z, features[1][2]);
          VectorUtil.add(nearest[1], tmp1, tmp2, tmp3);
        } else {
          failed = true;
        }
        // sa->endBlock(sablock);
        return (depth);
      } finally {
        popStack();
      }
    }
Пример #7
0
    public boolean Set(Face f, Mkv a, Mkv b, Mkv c) {
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      Vector3f tmp3 = Stack.alloc(Vector3f.class);

      Vector3f nrm = Stack.alloc(Vector3f.class);
      tmp1.sub(b.w, a.w);
      tmp2.sub(c.w, a.w);
      nrm.cross(tmp1, tmp2);

      float len = nrm.length();

      tmp1.cross(a.w, b.w);
      tmp2.cross(b.w, c.w);
      tmp3.cross(c.w, a.w);

      boolean valid =
          (tmp1.dot(nrm) >= -EPA_inface_eps)
              && (tmp2.dot(nrm) >= -EPA_inface_eps)
              && (tmp3.dot(nrm) >= -EPA_inface_eps);

      f.v[0] = a;
      f.v[1] = b;
      f.v[2] = c;
      f.mark = 0;
      f.n.scale(1f / (len > 0f ? len : cstInf), nrm);
      f.d = Math.max(0, -f.n.dot(a.w));
      return valid;
    }
Пример #8
0
    public Vector3f GetCoordinates(Face face, Vector3f out) {
      Vector3f tmp = Stack.alloc(Vector3f.class);
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);

      Vector3f o = Stack.alloc(Vector3f.class);
      o.scale(-face.d, face.n);

      float[] a = floatArrays.getFixed(3);

      tmp1.sub(face.v[0].w, o);
      tmp2.sub(face.v[1].w, o);
      tmp.cross(tmp1, tmp2);
      a[0] = tmp.length();

      tmp1.sub(face.v[1].w, o);
      tmp2.sub(face.v[2].w, o);
      tmp.cross(tmp1, tmp2);
      a[1] = tmp.length();

      tmp1.sub(face.v[2].w, o);
      tmp2.sub(face.v[0].w, o);
      tmp.cross(tmp1, tmp2);
      a[2] = tmp.length();

      float sm = a[0] + a[1] + a[2];

      out.set(a[1], a[2], a[0]);
      out.scale(1f / (sm > 0f ? sm : 1f));

      floatArrays.release(a);

      return out;
    }
Пример #9
0
    public boolean EncloseOrigin() {
      Vector3f tmp = Stack.alloc(Vector3f.class);
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);

      switch (order) {
          // Point
        case 0:
          break;
          // Line
        case 1:
          {
            Vector3f ab = Stack.alloc(Vector3f.class);
            ab.sub(simplex[1].w, simplex[0].w);

            Vector3f[] b =
                new Vector3f[] {
                  Stack.alloc(Vector3f.class),
                  Stack.alloc(Vector3f.class),
                  Stack.alloc(Vector3f.class)
                };
            b[0].set(1f, 0f, 0f);
            b[1].set(0f, 1f, 0f);
            b[2].set(0f, 0f, 1f);

            b[0].cross(ab, b[0]);
            b[1].cross(ab, b[1]);
            b[2].cross(ab, b[2]);

            float m[] =
                new float[] {b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared()};

            Quat4f tmpQuat = Stack.alloc(Quat4f.class);
            tmp.normalize(ab);
            QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f);

            Matrix3f r = Stack.alloc(Matrix3f.class);
            MatrixUtil.setRotation(r, tmpQuat);

            Vector3f w = Stack.alloc(Vector3f.class);
            w.set(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]);

            tmp.normalize(w);
            Support(tmp, simplex[4]);
            r.transform(w);
            tmp.normalize(w);
            Support(tmp, simplex[2]);
            r.transform(w);
            tmp.normalize(w);
            Support(tmp, simplex[3]);
            r.transform(w);
            order = 4;
            return (true);
          }
          // Triangle
        case 2:
          {
            tmp1.sub(simplex[1].w, simplex[0].w);
            tmp2.sub(simplex[2].w, simplex[0].w);
            Vector3f n = Stack.alloc(Vector3f.class);
            n.cross(tmp1, tmp2);
            n.normalize();

            Support(n, simplex[3]);

            tmp.negate(n);
            Support(tmp, simplex[4]);
            order = 4;
            return (true);
          }
          // Tetrahedron
        case 3:
          return (true);
          // Hexahedron
        case 4:
          return (true);
      }
      return (false);
    }
Пример #10
0
    public boolean SearchOrigin(Vector3f initray) {
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      Vector3f tmp3 = Stack.alloc(Vector3f.class);
      Vector3f tmp4 = Stack.alloc(Vector3f.class);

      iterations = 0;
      order = -1;
      failed = false;
      ray.set(initray);
      ray.normalize();

      Arrays.fill(table, null);

      FetchSupport();
      ray.negate(simplex[0].w);
      for (; iterations < GJK_maxiterations; ++iterations) {
        float rl = ray.length();
        ray.scale(1f / (rl > 0f ? rl : 1f));
        if (FetchSupport()) {
          boolean found = false;
          switch (order) {
            case 1:
              {
                tmp1.negate(simplex[1].w);
                tmp2.sub(simplex[0].w, simplex[1].w);
                found = SolveSimplex2(tmp1, tmp2);
                break;
              }
            case 2:
              {
                tmp1.negate(simplex[2].w);
                tmp2.sub(simplex[1].w, simplex[2].w);
                tmp3.sub(simplex[0].w, simplex[2].w);
                found = SolveSimplex3(tmp1, tmp2, tmp3);
                break;
              }
            case 3:
              {
                tmp1.negate(simplex[3].w);
                tmp2.sub(simplex[2].w, simplex[3].w);
                tmp3.sub(simplex[1].w, simplex[3].w);
                tmp4.sub(simplex[0].w, simplex[3].w);
                found = SolveSimplex4(tmp1, tmp2, tmp3, tmp4);
                break;
              }
          }
          if (found) {
            return true;
          }
        } else {
          return false;
        }
      }
      failed = true;
      return false;
    }
Пример #11
0
 public boolean SearchOrigin() {
   Vector3f tmp = Stack.alloc(Vector3f.class);
   tmp.set(1f, 0f, 0f);
   return SearchOrigin(tmp);
 }
Пример #12
0
    public boolean SolveSimplex4(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f ad) {
      // TODO: optimize

      Vector3f crs = Stack.alloc(Vector3f.class);

      Vector3f tmp = Stack.alloc(Vector3f.class);
      tmp.cross(ab, ac);

      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      tmp2.cross(ac, ad);

      Vector3f tmp3 = Stack.alloc(Vector3f.class);
      tmp3.cross(ad, ab);

      if (tmp.dot(ao) > GJK_insimplex_eps) {
        crs.set(tmp);
        order = 2;
        simplex[0].set(simplex[1]);
        simplex[1].set(simplex[2]);
        simplex[2].set(simplex[3]);
        return SolveSimplex3a(ao, ab, ac, crs);
      } else if (tmp2.dot(ao) > GJK_insimplex_eps) {
        crs.set(tmp2);
        order = 2;
        simplex[2].set(simplex[3]);
        return SolveSimplex3a(ao, ac, ad, crs);
      } else if (tmp3.dot(ao) > GJK_insimplex_eps) {
        crs.set(tmp3);
        order = 2;
        simplex[1].set(simplex[0]);
        simplex[0].set(simplex[2]);
        simplex[2].set(simplex[3]);
        return SolveSimplex3a(ao, ad, ab, crs);
      } else {
        return (true);
      }
    }
Пример #13
0
 public boolean SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac) {
   Vector3f tmp = Stack.alloc(Vector3f.class);
   tmp.cross(ab, ac);
   return (SolveSimplex3a(ao, ab, ac, tmp));
 }
Пример #14
0
 public void set(Mkv m) {
   w.set(m.w);
   r.set(m.r);
 }