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 updateTargetDistance() { DVector3 p1 = new DVector3(), p2 = new DVector3(); if (node[0].body != null) //dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1); node[0].body.getRelPointPos(anchor1, p1); else //dCopyVector3(p1, anchor1); p1.set(anchor1); if (node[1].body != null) //dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2); node[1].body.getRelPointPos(anchor2, p2); else //dCopyVector3(p2, anchor2); p2.set(anchor2); targetDistance = p1.distance(p2);//dCalcPointsDistance3(p1, p2); }
DVector3 s=new DVector3(),p=new DVector3(),q=new DVector3(),n=new DVector3(), q2=new DVector3(),q3=new DVector3(),q4=new DVector3(); // s = box sides DMatrix3 R=new DMatrix3(); double k; for (j=0; j<3; j++) s.set(j, dRandReal() + 0.1 ); box.setLengths (s); dMakeRandomVector (p,1.0); for (j=0; j<3; j++) q.set(j, (dRandReal()-0.5)*s.get(j) ); i = dRandInt (3); if (dRandReal() > 0.5) q.set(i, 0.99*0.5*s.get(i) ); else q.set(i, -0.99*0.5*s.get(i) ); dMultiply0 (q2,box.getRotation(),q); q2.add(p); ray.setPosition (q2); dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, for (j=0; j<3; j++) q.set(j, (dRandReal()-0.5)*s.get(j) ); i = dRandInt (3); if (dRandReal() > 0.5) q.set(i, 1.01*0.5*s.get(i)); else q.set(i, -1.01*0.5*s.get(i)); dMultiply0 (q2,box.getRotation(),q); q2.add( p ); ray.setPosition (q2); dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, for (j=0; j<3; j++) q.set(j, (dRandReal()-0.5)*0.99*s.get(j) ); dMultiply0 (q2,box.getRotation(),q); q2.add( p ); for (j=0; j<3; j++) q3.set(j, (dRandReal()-0.5)*0.99*s.get(j) );
private DxRagdollBody addBody(DVector3 p1, DVector3 p2, double radius) { p1 = new DVector3(p1); p2 = new DVector3(p2); double cyllen = p1.distance(p2) - radius; DBody body = OdeHelper.createBody(world); DMass m = OdeHelper.createMass(); DCapsule geom = OdeHelper.createCapsule(space, radius, cyllen); geom.setBody(body); DVector3 za = new DVector3(p2); (za.sub(p1)).safeNormalize(); DVector3 xa; DVector3 ya; if (Math.abs(za.dot(new DVector3(1.0, 0.0, 0.0))) < 0.7) { xa = new DVector3(1.0, 0.0, 0.0); ya = new DVector3(); ya.eqCross(za, xa); ya.safeNormalize(); xa.eqCross(ya, za); } else { ya = new DVector3(0.0, 1.0, 0.0); xa = new DVector3(); xa.eqCross(ya, za); xa.safeNormalize(); ya.eqCross(za, xa); body.setPosition(p1.add(p2).scale(0.5)); body.setRotation(new DMatrix3(xa.get0(), ya.get0(), za.get0(), xa.get1(), ya.get1(), za.get1(), xa.get2(), ya.get2(), za.get2())); totalMass += m.getMass(); DxRagdollBody bone = new DxRagdollBody(body, cyllen, radius, body.getPosition(), body.getQuaternion());
DVector3 p=new DVector3(),a=new DVector3(),b=new DVector3(),n=new DVector3(); DMatrix3 R=new DMatrix3(); double r,l,k,x,y; dGeomCapsuleSetParams (ccyl,r,l); dMakeRandomVector (p,1.0); dGeomSetPosition (ccyl,p.get0(),p.get1(),p.get2()); dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, dRandReal()*2-1,dRandReal()*10-5); for (j=0; j<3; j++) a.set(j, dRandReal()-0.5 ); dNormalize3 (a); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) a.set(j, p.get(j) + r*0.99*a.get(j) + k*0.99*R.get(j, 2) );//v[j*4+2] ); for (j=0; j<3; j++) b.set(j, dRandReal()-0.5 ); dNormalize3 (b); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, p.get(j) + r*0.99*b.get(j) + k*0.99*R.get(j, 2) );//v[j*4+2] ); dGeomRaySetLength (ray,a.distance(b)); for (j=0; j<3; j++) b.add(j,-a.get(j) ); dNormalize3 (b); dGeomRaySet (ray,a.get0(),a.get1(),a.get2(),b.get0(),b.get1(),b.get2()); if (dCollide (ray,ccyl,1,contacts) != 0) if (testFAILED()) return false; x = sin(k); y = cos(k); for (j=0; j<3; j++) a.set(j, x*R.get(j, 0) + y*R.get(j, 1) ); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, -a.get(j)*r*2 + k*R.get(j, 2) + p.get(j) );
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"); }
DVector3 globalA1 = new DVector3(), globalA2 = new DVector3(); globalA2.set(anchor2); DVector3 q = new DVector3(); q.eqDiff(globalA1, globalA2); if (q.length() < MIN_LENGTH) { DVector3 v1 = new DVector3(), v2 = new DVector3(); v2.setZero(); q.eqDiff(v1, v2); if (q.length() < MIN_LENGTH) { q.set(1, 0, 0); DVector3 relA1 = new DVector3(); DVector3 v = new DVector3(); dMultiply1_331(v, a1m, q); info.setJ1a(0, v); DVector3 relA2 = new DVector3(); info.setC(0, k * (targetDistance - globalA1.distance(globalA2)));
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; }
DVector3 p=new DVector3(),a=new DVector3(),b=new DVector3(),n=new DVector3(); DMatrix3 R=new DMatrix3(); double r,l,k,x,y; for (j=0; j<3; j++) a.set(j, dRandReal()-0.5 ); a.normalize (); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) a.set(j, p.get(j) + r*0.99*a.get(j) + k*0.99*R.get(j,2) ); for (j=0; j<3; j++) b.set(j, dRandReal()-0.5 ); b.normalize (); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, p.get(j) + r*0.99*b.get(j) + k*0.99*R.get(j,2) ); ray.setLength (a.distance(b)); b.sub( a ); b.normalize (); ray.set (a, b); if (OdeHelper.collide (ray,ccyl,1,contacts) != 0) if (testFAILED()) return false; x = Math.sin(k); y = Math.cos(k); for (j=0; j<3; j++) a.set(j, x*R.get(j,0) + y*R.get(j,1) ); k = (dRandReal()-0.5)*l; for (j=0; j<3; j++) b.set(j, -a.get(j)*r*2 + k*R.get(j,2) + p.get(j) ); ray.set (b, a); ray.setLength (r*0.99); for (j=0; j<3; j++) a.set(j, dRandReal()-0.5 ); a.normalize (); if (dCalcVectorDot3_14(a,R,2) < 0) {
if (g instanceof DRay) { dsSetColor (1,1,1); DVector3 origin = new DVector3(),dir=new DVector3(); ((DRay)g).get (origin,dir); origin.add2( Z_OFFSET ); double length = ((DRay)g).getLength (); dir.eqSum(origin, dir, length); dsDrawLine (origin,dir); dsSetColor (0,0,1); DVector3 pos = new DVector3(); if (!(g instanceof DPlane)) {//dGeomGetClass (g) != dPlaneClass) { pos.set(g.getPosition()); pos.add2( Z_OFFSET ); DVector3 pos2 = new DVector3(); DVector3C n3 = ((DPlane)g).getNormal(); double depth = ((DPlane)g).getDepth(); 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); pos2.set( pos ).add( 0.1*depth, 0.1* depth, 01.*depth); dsDrawLine (pos,pos2); dsSetColorAlpha (1f,0f,1f,0.8f);
DVector3 p1 = new DVector3(), p2 = new DVector3(); double clen = cyl._lz * (0.5); p1.eqSum( o1.final_posr().pos(), o1.final_posr().R().columnAsNewVector(2), clen); p2.eqSum( o1.final_posr().pos(), o1.final_posr().R().columnAsNewVector(2), -clen); double radius = cyl._radius; DVector3 pl = new DVector3(), pb = new DVector3(); DxCollisionUtil.dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb); mindist = 1e-15; if (pl.distance(pb)<mindist) { DVector3 normal = new DVector3(); RefDouble depth = new RefDouble(); RefInt code = new RefInt(); DVector3C capboxside = new DVector3(radiusMul2, radiusMul2, cyl._lz + radiusMul2); int num = DxBox.dBoxBox (c, R, side, o1.final_posr().pos(), o1.final_posr().R(), capboxside, cC.normal.set(normal); cC.g1 = o1; cC.g2 = o2;
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 final void _CalculateAxis(final DVector3C v1, final DVector3C v2, final DVector3C v3, final DVector3 r) { DVector3 t1 = new DVector3(); DVector3 t2 = new DVector3(); t1.eqDiff(v1, v2);//dVector3Subtract(v1,v2,t1); OdeMath.dCalcVectorCross3(t2,t1,v3);//dVector3Cross(t1,v3,t2); OdeMath.dCalcVectorCross3(r,t2,v3);//dVector3Cross(t2,v3,r); }