Examples of jinngine.math.Vector3

jinngine.math.Vector3
Vector3d defines a Vector for a three double value tuple. Vector3d can represent any three dimensional value, such as a vertex or normal. The functional methods like add, sub, multiply that returns new instances, and left this unchanged. Static methods store the resulting vector on a existing reference, which avoid allowcation an can improove performances around 20% (profiling performend on vector addition). Deprecated methods will be removed on October 2010 @author Morten Silcowitz @author Pierre Labatut

        assertFalse(a == Vector3.j());
    }


    @Test
    public void testK() {
        Vector3 a = Vector3.k();
        assertEquals(0., a.x);
        assertEquals(0., a.y);
        assertEquals(1., a.z);
        assertFalse(a == Vector3.k());
    }

View Full Code Here

        assertFalse(a == Vector3.k());
    }


    @Test
    public void testScale01() {
        final Vector3 a = new Vector3(1., 2., 3.);
        final Vector3 b = new Vector3(10., 20., 30.);
        final Vector3 r = a.scale(b);
        assertTrue(r != a);
        assertTrue(r != b);
        assertEquals(1., a.x);
        assertEquals(2., a.y);
        assertEquals(3., a.z);

View Full Code Here

        assertEquals(90., r.z);
    }


    @Test(expected = NullPointerException.class)
    public void testScale02() {
        new Vector3().scale(null);
    }

View Full Code Here

      state.v.assign(10,10,10);
    }


    state.iterations = 0;
    state.intersection = false;
      final Vector3 v = state.v;
    final Vector3 w = state.w;
    final Vector3 sa = new Vector3();
    final Vector3 sb = new Vector3();
          
      // initially update the simplex (often results in a quick termination)
      if (state.simplexSize>0)
        updateSimplex(state, Sa, Sb);


      // main loop
    while ( true  ) {
      state.iterations++;    
//        System.out.println("gjk iteration" + " " + v.norm()+ "  : " + state.simplexSize);
      
      // store points of convex objects a and b, and A-B 
      sa.assign( Sa.supportPoint(state.v.negate()));
      sb.assign( Sb.supportPoint(state.v));                    
      w.assign( sa.sub(sb) );


      // termination condition
      // ||v||2 -v.w is an upper bound for ||vk-v(A-B)||2 which converges towards zero as k goes large
      if (  Math.abs(v.dot(v)-v.dot(w)) < epsilon*epsilon  || state.iterations>maxiter || state.simplexSize > 3 )

View Full Code Here


    //recompute the simplex and lambda values
    reduceSimplex( state );
  
    //Calculate the vector v
    state.v.assign(new Vector3());
    for (int i=0; i<state.simplexSize;i++) 
      Vector3.add(state.v, state.simplices[state.permutation[i]][0].multiply( state.lambda[state.permutation[i]]));
  }

View Full Code Here

   * @param v direction vector 
   * @return a new support point on the configuration space obstacle of A and B
   */
  @SuppressWarnings("unused")
  private final Vector3 support( final SupportMap3 Sa, final SupportMap3 Sb, final Vector3 v) {
    final Vector3 sva = Sa.supportPoint(v) ;
    //We need rotate the vector reverse in B space
    final Vector3 svb = Sb.supportPoint(v.multiply(-1));       
    return sva.sub(svb);
  }

View Full Code Here

    if (state.simplexSize == 2) {
      // 1 12
      // 2
      final Vector3[] row0 = matrix[perm[0]];
      final Vector3[] row1 = matrix[perm[1]];
      final Vector3 y1 = row0[0]; 
      final Vector3 y2 = row1[0]; 


      final double d12_1 = y2.sub(y1).dot(y2);
      final double d12_2 = y1.sub(y2).dot(y1);
      
      //y1 (no permutation needed)
      if ( d12_2 <= 0 ) {                  lambda[perm[0]] = 1;  state.simplexSize = 1; return false;}
      //y2 ( (2,1)
      if ( d12_1 <= 0 ) { swap(1,0, perm); lambda[perm[0]] = 1;  state.simplexSize = 1; return true; }


      final double d12 = d12_1 + d12_2;
      //terminate on affinely dependent points in the set (if d12 is zero, we can never use point y2)
      if ( Math.abs(d12) <= epsilon ) {
//        System.out.println("Affinely dependent set in case d12");
        state.simplexSize = 1;
        return false;
      }
      
      if ( d12_1 > 0 && d12_2 > 0 ) {  
        //y1, y2 (no permutation)      
        lambda[perm[0]] = d12_1/d12; lambda[perm[1]] = d12_2/d12;
        return true;
      } else {
//        System.out.println("Unable to determine smallest set, use y1");
        state.simplexSize = 1;
        return false;
      }
        
    } else if (state.simplexSize == 3) {
      // 1 12 123
      // 2 13
      // 3 23
      
      final Vector3[] row0 = matrix[perm[0]];
      final Vector3[] row1 = matrix[perm[1]];
      final Vector3[] row2 = matrix[perm[2]];      
      final Vector3 y1 = row0[0]; 
      final Vector3 y2 = row1[0];
      final Vector3 y3 = row2[0];
      
      //y1, (no permutation)
      final double d13_3 = y1.sub(y3).dot(y1);// d13_3 = Math.abs(d13_3)<epsilon?0:d13_3;
      final double d12_2 = y1.sub(y2).dot(y1);// d12_2 = Math.abs(d12_2)<epsilon?0:d12_2;
      if ( d12_2 <= 0 && d13_3 <=0 ) /*{Vector3.set(v,y1); return new Vector3[] {y1};}*/ 
      {                 lambda[perm[0]]=1; state.simplexSize=1; return true; }


      //y2 (2,1)
      final double d12_1 = y2.sub(y1).dot(y2); //d12_1 = Math.abs(d12_1)<epsilon?0:d12_1;
      final double d23_3 = y2.sub(y3).dot(y2); //d23_3 = Math.abs(d23_3)<epsilon?0:d23_3;
      if ( d12_1 <= 0 && d23_3 <=0 ) //{Vector3.set(v,y2); return new Vector3[] {y2}; }
      { swap(1,0,perm); lambda[perm[0]]=1; state.simplexSize=1; return true; }
        
      //y3 (3,1)
      final double d13_1 = y3.sub(y1).dot(y3); //d13_1 = Math.abs(d13_1)<epsilon?0:d13_1;
      final double d23_2 = y3.sub(y2).dot(y3); //d23_2 = Math.abs(d23_2)<epsilon?0:d23_2;
      if ( d23_2 <= 0 && d13_1 <=0 ) //{Vector3.set(v,y3); return new Vector3[] {y3}; }
      { swap(2,0,perm); lambda[perm[0]]=1; state.simplexSize=1; return true; }
      
      final double d23 = d23_2 + d23_3;
      final double d13 = d13_1 + d13_3;
      final double d12 = d12_1 + d12_2;


      //terminate on affinely dependent points in the set
//      if ( Math.abs(d13)<=epsilon ||
//          Math.abs(d23)<=epsilon ) {
//        
//        //System.out.println("Affinely independent set on d13 or d23 ,  d13="+d13+"  d23="+d23+"  d12="+d12);
//        state.simplexSize = 2;
//        return false;
//      }
      //TODO 2009-08-09,  I've discovered that the above is wrong. Just because d13 is zero, it does not mean that y2 and y3 cannot be 
      //used, and vice versa. The idea of the check, is to prevent GJK from performing useless iterations, using 
      //points that are affinely dependent. It now remains to fix the condition such that it works in the intended way.
      
      


      //y2,y3 (2,1) (3,2)
      final double d123_1 = d23_2 * y2.sub(y1).dot(y2) + d23_3 * y2.sub(y1).dot(y3); //d123_1 = Math.abs(d123_1)<epsilon?0:d123_1;
      if (d123_1 <= 0 && d23_2 > 0 && d23_3 > 0) //{Vector3.set(v,y2.multiply(d23_2/d23).Add(y3.multiply(d23_3/d23))); return new Vector3[] {y2,y3};}
      { swap(1,0,perm); swap(2,1,perm); lambda[perm[0]]=d23_2/d23; lambda[perm[1]]=d23_3/d23; state.simplexSize=2; return true; }
      
      //y1,y3 (3,2)
      final double d123_2 = d13_1 * y1.sub(y2).dot(y1) + d13_3 * y1.sub(y2).dot(y3); //d123_2 = Math.abs(d123_2)<epsilon?0:d123_2;
      if (d123_2 <= 0 && d13_1 > 0 && d13_3 > 0) //{ Vector3.set(v,y1.multiply(d13_1/d13).Add(y3.multiply(d13_3/d13))); return new Vector3[] {y1,y3};}
      {                 swap(2,1,perm); lambda[perm[0]]=d13_1/d13; lambda[perm[1]]=d13_3/d13; state.simplexSize=2; return true; }


      //y1,y2 (no permutation)
      final double d123_3 = d12_1 * y1.sub(y3).dot(y1) + d12_2 * y1.sub(y3).dot(y2); //d123_3 = Math.abs(d123_3)<epsilon?0:d123_3;
      if (d123_3 <= 0 && d12_1 > 0 && d12_2 > 0) //{ /*Vector3.set(v,y1.multiply(d12_1/d12).Add(y2.multiply(d12_2/d12)));*/ return null;  /*return new Vector3[] {y1,y2};*/ }
      {                                 lambda[perm[0]]=d12_1/d12; lambda[perm[1]]=d12_2/d12; state.simplexSize=2; return false; }


      //y1, y2, y3 (no permutation)  
      final double d123 = d123_1 + d123_2 + d123_3;
      //System.out.println("d123 = "+d123);


      if ( d123_1 > 0 && d123_2 > 0 && d123_3 > 0)
      {      
        lambda[perm[0]]=d123_1/d123; lambda[perm[1]]=d123_2/d123; lambda[perm[2]]=d123_3/d123; state.simplexSize=3; 
        return true;
      } else {
//        System.out.println("Unable to determine smallest set, use y1,y2");
        //double d123_3 = d12_1 * y1.minus(y3).dot(y1) + d12_2 * y1.minus(y3).dot(y2); d123_3 = Math.abs(d123_3)<epsilon?0:d123_3;
        lambda[perm[0]]=d12_1/d12; lambda[perm[1]]=d12_2/d12; state.simplexSize=2; return false; 
        //System.exit(-1);
      }
      
    } else if (state.simplexSize == 4) {
      // 1 12 123 1234
      // 2 13 124
      // 3 14 134
      // 4 23 234
            //   24
      //   34
      
      final Vector3[] row0 = matrix[perm[0]];
      final Vector3[] row1 = matrix[perm[1]];
      final Vector3[] row2 = matrix[perm[2]];      
      final Vector3[] row3 = matrix[perm[3]];      
      final Vector3 y1 = row0[0]; 
      final Vector3 y2 = row1[0];
      final Vector3 y3 = row2[0];
      final Vector3 y4 = row3[0];
      
      //y1 (no permutation)
      final double d13_3 = y1.sub(y3).dot(y1); //d13_3= Math.abs(d13_3)<epsilon?0:d13_3;
      final double d12_2 = y1.sub(y2).dot(y1); //d12_2= Math.abs(d12_2)<epsilon?0:d12_2;
      final double d14_4 = y1.sub(y4).dot(y1); //d14_4= Math.abs(d14_4)<epsilon?0:d14_4;
      if ( d12_2 <= 0 && d13_3 <=0 && d14_4 <=0 ) //{Vector3.set(v, y1); return new Vector3[] {y1}; }
      {                lambda[perm[0]] = 1; state.simplexSize=1; return true; }
      
      //y2 (2,1)
      final double d12_1 = y2.sub(y1).dot(y2); //d12_1= Math.abs(d12_1)<epsilon?0:d12_1;
      final double d23_3 = y2.sub(y3).dot(y2); //d23_3= Math.abs(d23_3)<epsilon?0:d23_3;
      final double d24_4 = y2.sub(y4).dot(y2); //d24_4= Math.abs(d24_4)<epsilon?0:d24_4;
      if ( d12_1 <= 0 && d23_3 <=0 && d24_4 <= 0) //{Vector3.set(v, y2); return new Vector3[] {y2}; }
      { swap(1,0,perm); lambda[perm[0]] = 1; state.simplexSize=1; return true; }


      //y3 (3,1)
      final double d13_1 = y3.sub(y1).dot(y3); //d13_1= Math.abs(d13_1)<epsilon?0:d13_1;
      final double d23_2 = y3.sub(y2).dot(y3); //d23_2= Math.abs(d23_2)<epsilon?0:d23_2;
      final double d34_4 = y3.sub(y4).dot(y3); //d34_4= Math.abs(d34_4)<epsilon?0:d34_4;
      if ( d23_2 <= 0 && d13_1 <=0 && d34_4 <=0 ) //{Vector3.set(v, y3); return new Vector3[] {y3}; }
      { swap(2,0,perm); lambda[perm[0]] = 1; state.simplexSize=1; return true; }


      //y4 (4,1)
      final double d14_1 = y4.sub(y1).dot(y4); //d14_1= Math.abs(d14_1)<epsilon?0:d14_1;
      final double d24_2 = y4.sub(y2).dot(y4); //d24_2= Math.abs(d24_2)<epsilon?0:d24_2;
      final double d34_3 = y4.sub(y3).dot(y4); //d34_3= Math.abs(d34_3)<epsilon?0:d34_3;
      if ( d14_1 <= 0 && d24_2 <=0 && d34_3 <=0 ) //{Vector3.set(v, y4); return new Vector3[] {y4}; }
      { swap(3,0,perm); lambda[perm[0]] = 1; state.simplexSize=1; return true; }


      final double d12 = d12_1 + d12_2;
      final double d13 = d13_1 + d13_3;
      final double d14 = d14_1 + d14_4;
      final double d23 = d23_2 + d23_3;
      final double d24 = d24_2 + d24_4;
      final double d34 = d34_3 + d34_4;
      
      //terminate on affinely dependent points in the set
//      if ( (d12)<=epsilon || (d13)<=epsilon || (d14)<=epsilon ||
//          (d23)<=epsilon || (d24)<=epsilon || (d34)<=epsilon ) {
//        //System.out.println("Affinely independent set");
//        state.simplexSize = 3;
//        return false;
//      }


      //y1,y2 (no permutation)
      final double d123_3 = d12_1 * y1.sub(y3).dot(y1) + d12_2 * y1.sub(y3).dot(y2); //d123_3= Math.abs(d123_3)<epsilon?0:d123_3;
      final double d124_4 = d12_1 * y1.sub(y4).dot(y1) + d12_2 * y1.sub(y4).dot(y2); //d124_4= Math.abs(d124_4)<epsilon?0:d124_4;
      //System.out.println("d123_3: " + d123_3 + " d124_4: " + d124_4);
      if( d12_1 > 0 && d12_2 > 0 && d123_3 <=0 && d124_4 <=0) {
//        Vector3.set(v, y1.multiply(d12_1/d12).Add(y2.multiply(d12_2/d12)));
//        return new Vector3[] {y1,y2};
        lambda[perm[0]] = d12_1 / d12; lambda[perm[1]] = d12_2/d12;
        state.simplexSize = 2;
        return true;        
      }


      //y1, y3 (3,2)
      final double d123_2 = d13_1 * y1.sub(y2).dot(y1) + d13_3 * y1.sub(y2).dot(y3); //d123_2= Math.abs(d123_2)<epsilon?0:d123_2;
      final double d134_4 = d13_1 * y1.sub(y4).dot(y1) + d13_3 * y1.sub(y4).dot(y3); //d134_4= Math.abs(d134_4)<epsilon?0:d134_4;
      //System.out.println("d123_2: " + d123_2 + " d134_4: " + d134_4);
      if( d13_1 > 0 && d13_3 > 0 && d123_2 <=0 && d134_4 <=0) {        
//        Vector3.set(v, y1.multiply(d13_1/d13).Add(y3.multiply(d13_3/d13)));
//        return new Vector3[] {y1,y3};        
        swap(2,1,perm);
        lambda[perm[0]] = d13_1 / d13; lambda[perm[1]] = d13_3/d13;
        state.simplexSize = 2;
        return true;


      }


      //y1, y4 (4,2)
      final double d124_2 = d14_1 * y1.sub(y2).dot(y1) + d14_4 * y1.sub(y2).dot(y4); //d124_2= Math.abs(d124_2)<epsilon?0:d124_2;
      final double d134_3 = d14_1 * y1.sub(y3).dot(y1) + d14_4 * y1.sub(y3).dot(y4); //d134_3= Math.abs(d134_3)<epsilon?0:d134_3;
      //System.out.println("d124_2: " + d124_2 + " d134_3: " + d134_3);
      if( d14_1 > 0 && d14_4 > 0 && d124_2 <=0 && d134_3 <=0) {
//        Vector3.set(v, y1.multiply(d14_1/d14).Add(y4.multiply(d14_4/d14)));
//        return new Vector3[] {y1,y4};
        swap(3,1,perm);
        lambda[perm[0]] = d14_1 / d14; lambda[perm[1]] = d14_4/d14;
        state.simplexSize = 2;
        return true;


      }


      //y2,y3 (2,1) (3,2)
      final double d123_1 = d23_2 * y2.sub(y1).dot(y2) + d23_3 * y2.sub(y1).dot(y3); //d123_1= Math.abs(d123_1)<epsilon?0:d123_1;
      final double d234_4 = d23_2 * y2.sub(y4).dot(y2) + d23_3 * y2.sub(y4).dot(y3); //d234_4= Math.abs(d234_4)<epsilon?0:d234_4;
      //System.out.println("d123_1: " + d123_1 + " d234_4: " + d234_4);
      if( d23_2 > 0 && d23_3 > 0 && d123_1 <=0 && d234_4 <=0) {
//        Vector3.set(v, y2.multiply(d23_2/d23).Add(y3.multiply(d23_3/d23)));
//        return new Vector3[] {y2,y3};
        
        swap(1,0,perm); swap(3,1,perm);
        lambda[perm[0]] = d23_2 / d23; lambda[perm[1]] = d23_3/d23;
        state.simplexSize = 2;
        return true;
      }


      //y2,y4 (2,1) (4,2)
      final double d124_1 = d24_2 * y2.sub(y1).dot(y2) + d24_4 * y2.sub(y1).dot(y4); //d124_1= Math.abs(d124_1)<epsilon?0:d124_1;
      final double d234_3 = d24_2 * y2.sub(y3).dot(y2) + d24_4 * y2.sub(y3).dot(y4); //d234_3= Math.abs(d234_3)<epsilon?0:d234_3;
      //System.out.println("d124_1: " + d124_1 + " d234_3: " + d234_3);
      if( d24_2 > 0 && d24_4 > 0 && d124_1 <=0 && d234_3 <=0) {
//        Vector3.set(v, y2.multiply(d24_2/d24).Add(y4.multiply(d24_4/d24)));
//        return new Vector3[] {y2,y4};
        swap(1,0,perm); swap(3,1,perm);
        lambda[perm[0]] = d24_2 / d24; lambda[perm[1]] = d24_4/d24;
        state.simplexSize = 2;
        return true;


      }


      //y3,y4 (3,1) (2,4)
      final double d134_1 = d34_3 * y3.sub(y1).dot(y3) + d34_4 * y3.sub(y1).dot(y4); //d134_1= Math.abs(d134_1)<epsilon?0:d134_1;
      final double d234_2 = d34_3 * y3.sub(y2).dot(y3) + d34_4 * y3.sub(y2).dot(y4); //d234_2= Math.abs(d234_2)<epsilon?0:d234_2;
      //System.out.println("d134_1: " + d134_1 + " d234_2: " + d234_2);
      if( d34_3 > 0 && d34_4 > 0 && d134_1 <=0 && d234_2 <=0) {
//        Vector3.set(v, y3.multiply(d34_3/d34).Add(y4.multiply(d34_4/d34)));
//        return new Vector3[] {y3,y4};        
        swap(2,0,perm); swap(1,3,perm);
        lambda[perm[0]] = d34_3 / d34; lambda[perm[1]] = d34_4/d34;
        state.simplexSize = 2;
        return true;


      }


      //y1,y2,y3 (no permutation)
      final double d1234_4 = d123_1 * (y1.sub(y4).dot(y1)) + d123_2 * (y1.sub(y4).dot(y2) ) + d123_3 * (y1.sub(y4).dot(y3));
      //d1234_4 = Math.abs(d1234_4)<epsilon?0:d1234_4;
      if ( d123_1 > 0 && d123_2 > 0 && d123_3 > 0 && d1234_4 <= 0) {
        final double d123 = d123_1 + d123_2 + d123_3;
        lambda[perm[0]]= d123_1/d123; lambda[perm[1]]= d123_2/d123; lambda[perm[2]] = d123_3/d123;
        state.simplexSize = 3;
        
//        Vector3.set(v, y1.multiply(lambda1).Add(y2.multiply(lambda2)).Add(y3.multiply(lambda3)));
//        return new Vector3[] {y1,y2,y3};
        
        return false;
      }


      //y1,y2,y4 (4,3)
      final double d1234_3 = d124_1 * (y1.sub(y3).dot(y1)) + d124_2 * (y1.sub(y3).dot(y2) ) + d124_4 * (y1.sub(y3).dot(y4));
      //d1234_3 = Math.abs(d1234_3)<epsilon?0:d1234_3;
      if ( d124_1 > 0 && d124_2 > 0 && d124_4 > 0 && d1234_3 <= 0) { 
        final double d124 = d124_1 + d124_2 + d124_4;
//        Vector3.set(v, y1.multiply(d124_1/d124).Add(y2.multiply(d124_2/d124)).Add(y4.multiply(d124_4/d124)));
//        return new Vector3[] {y1,y2,y4};
        swap(3,2,perm);
        lambda[perm[0]]= d124_1/d124; lambda[perm[1]]= d124_2/d124; lambda[perm[2]] = d124_4/d124;
        state.simplexSize = 3;
        return true;


      }


      //y1,y3,y4 (3,2) (4,3)
      final double d1234_2 = d134_1 * (y1.sub(y2).dot(y1)) + d134_3 * (y1.sub(y2).dot(y3) ) + d134_4 * (y1.sub(y2).dot(y4));
      //d1234_2 = Math.abs(d1234_2)<epsilon?0:d1234_2;
      if ( d134_1 > 0 && d134_3 > 0 && d134_4 > 0 && d1234_2 <= 0) { 
        final double d134 = d134_1 + d134_3 + d134_4;
//        Vector3.set(v, y1.multiply(d134_1/d134).Add(y3.multiply(d134_3/d134)).Add(y4.multiply(d134_4/d134)));
//        return new Vector3[] {y1,y3,y4};
        swap(2,1,perm); swap(3,2,perm);
        lambda[perm[0]]= d134_1/d134; lambda[perm[1]]= d134_3/d134; lambda[perm[2]] = d134_4/d134;
        state.simplexSize = 3;
        return true;


      }


      //y2,y3,y4 (2,1)(3,2)(4,3)
      final double d1234_1 = d234_2 * (y2.sub(y1).dot(y2)) + d234_3 * (y2.sub(y1).dot(y3) ) + d234_4 * (y2.sub(y1).dot(y4));
      //d1234_1 = Math.abs(d1234_1)<epsilon?0:d1234_1;
      if ( d234_2 > 0 && d234_3 > 0 && d234_4 > 0 && d1234_1 <= 0) {
        final double d234 = d234_2 + d234_3 + d234_4;
//        Vector3.set(v, y2.multiply(d234_2/d234).Add(y3.multiply(d234_3/d234)).Add(y4.multiply(d234_4/d234)));
//        return new Vector3[] {y2,y3,y4};
        swap(1,0,perm); swap(2,1,perm); swap(3,2,perm);
        lambda[perm[0]]= d234_2/d234; lambda[perm[1]]= d234_3/d234; lambda[perm[2]] = d234_4/d234;
        state.simplexSize = 3;
        return true;
      }


      //y1,y2,y3,y4 (no permute)
      final double d1234 = d1234_1 + d1234_2 + d1234_3 + d1234_4;
      //System.out.println("d1234 = "+d1234);


      //System.out.println("d1234:" + d1234 + "," + d1234_1 +","+d1234_2 + ","+d1234_3+","+d1234_4 + "sum = " + (d1234_1+d1234_2+d1234_3+d1234_4)/d1234 );
      //System.out.println("v is then");
      //v.Print();
      //System.out.println("Simplex*** d14 = " + y1.Minus(y4).Length()/y4.Length());
      //return new Vector3[] {y1,y2,y3,y4};
      if ( d1234_1 > 0 && d1234_2 > 0 && d1234_3 > 0 && d1234_4 > 0) {
        //GJK penetrating state


        //check for the accuracy, v should be the zero vector
        if(  y1.multiply(d1234_1/d1234).add(y2.multiply(d1234_2/d1234)).add(y3.multiply(d1234_3/d1234)).add(y4.multiply(d1234_4/d1234)).norm() > 1 ) {
          //System.out.println("wrong penetration");
          //result is bad, terminate with last good subset {y1,y2,y3}
          state.simplexSize = 3;
          return false;
        }

View Full Code Here

    //public boolean initialised = false;
    public State() {
      // fill out the simplices table with vectors
      for (int i=0;i<4;i++)
        for (int j=0;j<4;j++)
          simplices[i][j] = new Vector3();
    }

View Full Code Here

      final double envelope, 
      final double epsilon, 
      final boolean sweep) {
    
    int iterations = 0; 
    final Vector3 x = point.add(direction.multiply(lambda));
    final double sphere;
    
    // sphere swept volumes?
    if (sweep) {
      sphere = /*envelope*/ + Sb.sphereSweepRadius() + (Sc!=null? Sc.sphereSweepRadius():0);
    } else {
      sphere = 0;
    }
    
    
    // translated support mapping Sc+x
    final SupportMap3 Sa;    
    if (Sc == null) {
      Sa = new SupportMap3() {
        @Override
        public final Vector3 supportPoint(Vector3 direction) { return new Vector3(x); }
        @Override
        public final void supportFeature(Vector3 d, List<Vector3> returnList) {}
        @Override
        public final double sphereSweepRadius() {return 0;}
      };
    } else {
      // if Sc is given, add it to the second support map
      Sa = new SupportMap3() {
        @Override
        public final Vector3 supportPoint(Vector3 direction) { return x.add(Sc.supportPoint(direction)); }
        @Override
        public final void supportFeature(Vector3 d, List<Vector3> returnList) {}
        @Override
        public final double sphereSweepRadius() {return 0;}
        
      };      
    }
    
    // vectors from the GJK internal state (pretty ugly but it works) 
    final Vector3 v = gjkstate.v;
    final Vector3 w = gjkstate.w;
    
    while (true) {
      iterations++;
//      System.out.println("RayCast: iter=" + iterations +" lambda="+lambda);


      // run as many gjk iterations as necessary to get a separating axis. If the distance
      // is within the envelope, run until the error in v is below epsilon. 
      gjk.run(Sa, Sb, pc, pb, envelope+sphere, epsilon, 31);
      //termination
      if (v.norm() < envelope+sphere  || iterations > 31 )
        break;
      // ray miss?
      if ( v.dot(direction) >= 0) {
        return Double.POSITIVE_INFINITY;
      } else {
        // move forward as much as possible, half way into the envelope 
        Vector3 vs = v.sub(v.normalize().multiply(envelope*0.5+sphere));
        lambda = lambda - vs.dot(w) / v.dot(direction);
        x.assign(point.add(direction.multiply(lambda)));
      }      
    }
//    System.out.println("RayCast: Hitpoint lambda=" + lambda);
    return lambda;

View Full Code Here

      
    }
    
    if ( g instanceof Box  ) {
      final List<Vector3> vertices = new ArrayList<Vector3>();
      vertices.add( new Vector3(  0.5,  0.5,  0.5));
      vertices.add( new Vector3( -0.5,  0.5,  0.5));
      vertices.add( new Vector3(  0.5, -0.5,  0.5));
      vertices.add( new Vector3( -0.5, -0.5,  0.5));
      vertices.add( new Vector3(  0.5,  0.5, -0.5));
      vertices.add( new Vector3( -0.5,  0.5, -0.5));
      vertices.add( new Vector3(  0.5, -0.5, -0.5));
      vertices.add( new Vector3( -0.5, -0.5, -0.5));
      final ConvexHull hull = new ConvexHull(vertices);
      
      toDraw.add( new DrawShape() {    
        @Override
        public Iterator<Vector3[]> getFaces() {
          return hull.getFaces();
        }
        @Override
        public Matrix4 getTransform() {
          return g.getTransform();
        }
        @Override
        public Body getReferenceBody() {
          return g.getBody();
        }
      });
    }
    
    if ( g instanceof UniformCapsule  ) {
      UniformCapsule cap = (UniformCapsule)g;
      final List<Vector3> vertices = new ArrayList<Vector3>();
      final List<Vector3> icoicosahedron = new ArrayList<Vector3>();
      
      // point on icosahedron
//      final double t = (1.0 + Math.sqrt(5.0))/ 2.0;
//      final double S = 1.0 / ( Math.sqrt(1+t*t)); 
//      icoicosahedron.add(new Vector3(-1,  t,  0));
//      icoicosahedron.add( new Vector3( 1,  t,  0));
//      icoicosahedron.add( new Vector3(-1, -t,  0));
//      icoicosahedron.add( new Vector3( 1, -t,  0));
//      icoicosahedron.add( new Vector3( 0, -1,  t));
//      icoicosahedron.add( new Vector3( 0,  1,  t));
//      icoicosahedron.add( new Vector3( 0, -1, -t));
//      icoicosahedron.add( new Vector3( 0,  1, -t));
//      icoicosahedron.add( new Vector3( t,  0, -1));
//      icoicosahedron.add( new Vector3( t,  0,  1));
//      icoicosahedron.add( new Vector3(-t,  0, -1));
//      icoicosahedron.add( new Vector3(-t,  0,  1));


      ConvexHull icosphere = buildIcosphere(1, 2);
      
      // scale to unit
//      for (Vector3 v: icoicosahedron)
//        v.assign(v.multiply(S) );


      // add two icos to vertices
      Iterator<Vector3> iter = icosphere.getVertices();
      while(iter.hasNext()) {
        Vector3 v = iter.next();
        vertices.add( v.multiply(cap.getRadius()).add(0,0,cap.getLength()/2));
        vertices.add( v.multiply(cap.getRadius()).add(0,0,-cap.getLength()/2));
      }
        
      final ConvexHull hull = new ConvexHull(vertices);
      
      toDraw.add( new DrawShape() {

View Full Code Here

0 1 2 3 4 5 6 7 8 9

TOP

Related Classes of jinngine.math.Vector3

jinngine.collision.ExhaustiveSearch

jinngine.collision.GJK

jinngine.collision.GJK$State

jinngine.collision.RayCast

jinngine.collision.SweepAndPrune$SweepPoint

jinngine.examples.BasicExample

jinngine.examples.CapsuleExample

jinngine.examples.RagdollExample

jinngine.examples.TriggerExample

jinngine.geometry.contact.BulletNativeContactGenerator

All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.