コード例 #1
0
ファイル: GjkEpaSolver.java プロジェクト: philjord/jbullet1.1
    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;
    }
コード例 #2
0
ファイル: GjkEpaSolver.java プロジェクト: philjord/jbullet1.1
    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;
    }
コード例 #3
0
ファイル: GjkEpaSolver.java プロジェクト: philjord/jbullet1.1
    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;
    }