Beispiel #1
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;
    }
Beispiel #2
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);
      }
    }
Beispiel #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;
        }
      }
    }
Beispiel #4
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);
 }
Beispiel #5
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);
 }