public static void dMultiplyAdd0_331(DVector3 A, double[] B, int b, DVector3C C) { A.add0( dCalcVectorDot3(B, b+0, C) ); A.add1( dCalcVectorDot3(B, b+4, C) ); A.add2( dCalcVectorDot3(B, b+8, C) ); } public static void dMultiplyAdd0_331(DVector3 A, double[] B, int b, double[] C, int c) {
public static void dMultiplyAdd0_331(DVector3 A, double[] B, int b, double[] C, int c) { A.add0( dCalcVectorDot3(B, b+0, C, c) ); A.add1( dCalcVectorDot3(B, b+4, C, c) ); A.add2( dCalcVectorDot3(B, b+8, C, c) ); } @Deprecated
public static void dMultiplyAdd0_331(DVector3 A, double[] B, int b, DVector3C C) { A.add0( dCalcVectorDot3(B, b+0, C) ); A.add1( dCalcVectorDot3(B, b+4, C) ); A.add2( dCalcVectorDot3(B, b+8, C) ); } public static void dMultiplyAdd0_331(DVector3 A, double[] B, int b, double[] C, int c) {
public static void dMultiplyAdd0_331(DVector3 A, double[] B, int b, double[] C, int c) { A.add0( dCalcVectorDot3(B, b+0, C, c) ); A.add1( dCalcVectorDot3(B, b+4, C, c) ); A.add2( dCalcVectorDot3(B, b+8, C, c) ); } @Deprecated
/** * Cross product, set a += b x c. * @param a a * @param b b * @param c c */ public static void dAddVectorCross3(DVector3 a, DVector3C b, DVector3C c) { a.add0( b.get1()*c.get2() - b.get2()*c.get1() ); a.add1( b.get2()*c.get0() - b.get0()*c.get2() ); a.add2( b.get0()*c.get1() - b.get1()*c.get0() ); } /**
public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
/** * Cross product, set a -= b x c. * @param a a * @param b b * @param c c */ public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DVector3C c) { a.add0( -b.get1()*c.get2() + b.get2()*c.get1() ); a.add1( -b.get2()*c.get0() + b.get0()*c.get2() ); a.add2( -b.get0()*c.get1() + b.get1()*c.get0() ); }
/** * Cross product, set a += b x c. * @param a a * @param b b * @param c c */ public static void dAddVectorCross3(DVector3 a, DVector3C b, DVector3C c) { a.add0( b.get1()*c.get2() - b.get2()*c.get1() ); a.add1( b.get2()*c.get0() - b.get0()*c.get2() ); a.add2( b.get0()*c.get1() - b.get1()*c.get0() ); } /**
/** * Cross product, set a -= b x c. * @param a a * @param b b * @param c c */ public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DVector3C c) { a.add0( -b.get1()*c.get2() + b.get2()*c.get1() ); a.add1( -b.get2()*c.get0() + b.get0()*c.get2() ); a.add2( -b.get0()*c.get1() + b.get1()*c.get0() ); }
public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
private void nearCallback (Object data, DGeom o1, DGeom o2) { int n; final int N = 100; //dContactGeom contact[N]; DContactGeomBuffer contacts = new DContactGeomBuffer(N); if (o2 instanceof DRay) { n = OdeHelper.collide (o2,o1,N,contacts);//,sizeof(dContactGeom)); } else { n = OdeHelper.collide (o1,o2,N,contacts);//,sizeof(dContactGeom)); } if (n > 0) { DMatrix3 RI = new DMatrix3(); RI.setIdentity(); DVector3 ss = new DVector3(0.01,0.01,0.01); for (int i=0; i<n; i++) { DContactGeom contact = contacts.get(i); contact.pos.add2( Z_OFFSET ); dsDrawBox (contact.pos,RI,ss); DVector3 n2 = new DVector3(); //for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j]; n2.eqSum(contact.pos, contact.normal, 0.1); dsDrawLine (contact.pos,n2); } } }
DVector3 origin = new DVector3(),dir=new DVector3(); ((DRay)g).get (origin,dir); origin.add2( Z_OFFSET ); double length = ((DRay)g).getLength (); pos.add2( Z_OFFSET ); OdeMath.dRFromZAxis (R,n3); pos.set( n3 ).scale( depth ); pos.add2( Z_OFFSET ); DVector3 sides = new DVector3( 2, 2, 0.001 ); dsSetColor (1,0,1);
b.lvel.add2( (cforce[cforcecurrP+2] + b.facc.get2()) * body_invMass_mul_stepsize);
b.lvel.add2( (cforce[cforcecurrP+2] + b.facc.get2()) * body_invMass_mul_stepsize);