@Override public double getMin(int i) { return _min.get(i); }
private static boolean box1inside2 (final DVector3 p1, final DMatrix3 R1, final DVector3 side1, final DVector3 p2, final DMatrix3 R2, final DVector3 side2) { for (int i=-1; i<=1; i+=2) { for (int j=-1; j<=1; j+=2) { for (int k=-1; k<=1; k+=2) { DVector3 v=new DVector3(),vv=new DVector3(); v.set0( i*0.5*side1.get0() ); v.set1( j*0.5*side1.get1() ); v.set2( k*0.5*side1.get2() ); dMultiply0_331 (vv,R1,v); vv.add(0, p1.get0() - p2.get0() ); vv.add(1, p1.get1() - p2.get1() ); vv.add(2, p1.get2() - p2.get2() ); for (int axis=0; axis < 3; axis++) { double z = dCalcVectorDot3_14(vv,R2,axis); if (z < (-side2.get(axis)*0.5) || z > (side2.get(axis)*0.5)) return false; } } } } return true; }
void testNormalize3() { HEADER(); int i,j,bad=0; DVector3 n1 = new DVector3(),n2 = new DVector3(); for (i=0; i<1000; i++) { dMakeRandomVector (n1,1.0); for (j=0; j<3; j++) n2.set(j, n1.get(j)); dNormalize3 (n2); if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1; if (dFabs(n2.get0()/n1.get0() - n2.get1()/n1.get1()) > tol) bad |= 2; if (dFabs(n2.get0()/n1.get0() - n2.get2()/n1.get2()) > tol) bad |= 4; if (dFabs(n2.get1()/n1.get1() - n2.get2()/n1.get2()) > tol) bad |= 8; if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16; if (bad != 0) { printf ("\tFAILED (code=%x)\n",bad); return; } } printf ("\tpassed\n"); }
@Override public double getMax(int i) { return _max.get(i); }
a.set1( dRandReal() - 0.5 ); a.set2( 0 ); for (j=0; j<3; j++) b.set(j, a.get0()*p.get(j) + a.get1()*q.get(j) + (a.get2()+d)*n.get(j) ); if (dFabs(dGeomPlanePointDepth (plane,b.get0(),b.get1(),b.get2())) >= tol) if (testFAILED()) return false; a.set1( dRandReal() - 0.5 ); a.set2( dRandReal() - 0.5 ); for (j=0; j<3; j++) b.set(j, a.get0()*p.get(j) + a.get1()*q.get(j) + (a.get2()+d)*n.get(j) ); if (dFabs(dGeomPlanePointDepth (plane,b.get0(),b.get1(),b.get2()) + a.get2()) >= tol) if (testFAILED()) return false; a.set1( dRandReal() - 0.5 ); a.set2( -1 ); for (j=0; j<3; j++) b.set(j, a.get0()*p.get(j) + a.get1()*q.get(j) + (a.get2()+d)*n.get(j) ); if (dFabs(dGeomPlanePointDepth (plane,b.get0(),b.get1(),b.get2()) - 1) >= tol) if (testFAILED()) return false;
@Override public double getMax(int i) { return _max.get(i); }
@Override public double getMin(int i) { return _min.get(i); }
private final void FOO2(int i, int j, int op, DContactGeomBuffer contacts, int skip, DVector3 p, DMatrix3C R, DVector3 side) { contacts.get(i*skip).pos.eqSum(p, R.viewCol(j), op*side.get(j)); }
for (j=0; j<3; j++) q.set(j, q.get(j)*r + p.get(j) ); if (dFabs(dGeomSpherePointDepth (sphere,q.get0(),q.get1(),q.get2())) > tol) if (testFAILED()) return false; for (j=0; j<3; j++) q.set(j, dRandReal()-0.5 ); dNormalize3 (q); for (j=0; j<3; j++) q.set(j, q.get(j)*(r-d) + p.get(j) ); if (dFabs(dGeomSpherePointDepth (sphere,q.get0(),q.get1(),q.get2())-d) > tol) if (testFAILED()) return false;
private final void FOO2(int i, int j, int op, DContactGeomBuffer contacts, int skip, DVector3 p, DMatrix3C R, DVector3 side) { contacts.get(i*skip).pos.eqSum(p, R.viewCol(j), op*side.get(j)); }
a.set1( dRandReal() - 0.5 ); a.set2( 0 ); for (j=0; j<3; j++) b.set(j, a.get0()*p.get(j) + a.get1()*q.get(j) + (a.get2()+d)*n.get(j) ); if (dFabs(plane.getPointDepth (b)) >= tol) if (testFAILED()) return false; a.set1( dRandReal() - 0.5 ); a.set2( dRandReal() - 0.5 ); for (j=0; j<3; j++) b.set(j, a.get0()*p.get(j) + a.get1()*q.get(j) + (a.get2()+d)*n.get(j) ); if (dFabs(plane.getPointDepth (b) + a.get2()) >= tol) if (testFAILED()) return false; a.set1( dRandReal() - 0.5 ); a.set2( -1 ); for (j=0; j<3; j++) b.set(j, a.get0()*p.get(j) + a.get1()*q.get(j) + (a.get2()+d)*n.get(j) ); if (dFabs(plane.getPointDepth (b) - 1) >= tol) if (testFAILED()) return false;
private static void answer(final double t, DVector3 tmp, DVector3C sign, DVector3C p1, DVector3 s, DMatrix3C R, DVector3 lret, DVector3C h, DVector3C c, DVector3C v, DVector3 bret) { //got_answer: // compute closest point on the line //for (int i=0; i<3; i++) lret.v[i] = p1.v[i] + t*tmp.v[i]; // note: tmp=p2-p1 lret.eqSum(p1, tmp, t); // compute closest point on the box for (int i=0; i<3; i++) { tmp.set(i, sign.get(i) * (s.get(i) + t*v.get(i)) ); if (tmp.get(i) < -h.get(i)) tmp.set(i, -h.get(i) ); else if (tmp.get(i) > h.get(i)) tmp.set(i, h.get(i) ); } dMultiply0_331 (s,R,tmp); //for (int i=0; i<3; i++) bret.v[i] = s.v[i] + c.v[i]; bret.eqSum(s, c); }
private static void computeMassParams (DMass m, DVector3[] q, DVectorN pm) { int i,j; double pmi, q0, q1, q2; dMassSetZero (m); DVector3 C = m.getC().clone(); DMatrix3 I = m.getI().clone(); for (i=0; i<NUM; i++) { pmi = pm.get(i); m.setMass(m.getMass() + pmi);// += pmi; for (j=0; j<3; j++) C.add(j, pmi*q[i].get(j)); q0 = q[i].get0(); q1 = q[i].get1(); q2 = q[i].get2(); I.add(0,0, pmi*(q1*q1 + q2*q2)); I.add(1,1, pmi*(q0*q0 + q2*q2)); I.add(2,2, pmi*(q0*q0 + q1*q1)); I.sub(0,1, pmi*(q0*q1)); I.sub(0,2, pmi*(q0*q2)); I.sub(1,2, pmi*(q1*q2)); } //for (j=0; j<3; j++) m.c.v[j] /= m.mass; C.scale(1./m.getMass()); I.set(1,0, I.get(0,1)); I.set(2,0, I.get(0,2)); I.set(2,1, I.get(1,2)); m.setC(C); m.setI(I); }
private static void answer(final double t, DVector3 tmp, DVector3C sign, DVector3C p1, DVector3 s, DMatrix3C R, DVector3 lret, DVector3C h, DVector3C c, DVector3C v, DVector3 bret) { //got_answer: // compute closest point on the line //for (int i=0; i<3; i++) lret.v[i] = p1.v[i] + t*tmp.v[i]; // note: tmp=p2-p1 lret.eqSum(p1, tmp, t); // compute closest point on the box for (int i=0; i<3; i++) { tmp.set(i, sign.get(i) * (s.get(i) + t*v.get(i)) ); if (tmp.get(i) < -h.get(i)) tmp.set(i, -h.get(i) ); else if (tmp.get(i) > h.get(i)) tmp.set(i, h.get(i) ); } dMultiply0_331 (s,R,tmp); //for (int i=0; i<3; i++) bret.v[i] = s.v[i] + c.v[i]; bret.eqSum(s, c); }
private static boolean box1inside2 (final DVector3 p1, final DMatrix3 R1, final DVector3 side1, final DVector3 p2, final DMatrix3 R2, final DVector3 side2) { for (int i=-1; i<=1; i+=2) { for (int j=-1; j<=1; j+=2) { for (int k=-1; k<=1; k+=2) { DVector3 v=new DVector3(),vv=new DVector3(); v.set( side1 ).scale( i*0.5, j*0.5, k*0.5 ); dMultiply0_331 (vv,R1,v); vv.add(p1).sub(p2); for (int axis=0; axis < 3; axis++) { double z = dCalcVectorDot3_14(vv,R2,axis); if (z < (-side2.get(axis)*0.5) || z > (side2.get(axis)*0.5)) return false; } } } } return true; }