示例#1
0
  public boolean collide(
      ConvexShape shape0,
      Transform wtrs0,
      ConvexShape shape1,
      Transform wtrs1,
      float radialmargin /*,
			btStackAlloc* stackAlloc*/,
      Results results) {

    // Initialize
    results.witnesses[0].set(0f, 0f, 0f);
    results.witnesses[1].set(0f, 0f, 0f);
    results.normal.set(0f, 0f, 0f);
    results.depth = 0;
    results.status = ResultsStatus.Separated;
    results.epa_iterations = 0;
    results.gjk_iterations = 0;
    /* Use GJK to locate origin		*/
    gjk.init(
        /*stackAlloc,*/
        wtrs0.basis,
        wtrs0.origin,
        shape0,
        wtrs1.basis,
        wtrs1.origin,
        shape1,
        radialmargin + EPA_accuracy);
    try {
      boolean collide = gjk.SearchOrigin();
      results.gjk_iterations = gjk.iterations + 1;
      if (collide) {
        /* Then EPA for penetration depth	*/
        EPA epa = new EPA(gjk);
        float pd = epa.EvaluatePD();
        results.epa_iterations = epa.iterations + 1;
        if (pd > 0) {
          results.status = ResultsStatus.Penetrating;
          results.normal.set(epa.normal);
          results.depth = pd;
          results.witnesses[0].set(epa.nearest[0]);
          results.witnesses[1].set(epa.nearest[1]);
          return (true);
        } else {
          if (epa.failed) {
            results.status = ResultsStatus.EPA_Failed;
          }
        }
      } else {
        if (gjk.failed) {
          results.status = ResultsStatus.GJK_Failed;
        }
      }
      return (false);
    } finally {
      gjk.destroy();
    }
  }
示例#2
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();
      }
    }
示例#3
0
 public Mkv Support(Vector3f w) {
   // Mkv v = new Mkv();
   Mkv v = stackMkv.get();
   gjk.Support(w, v);
   return v;
 }