void draw() { dsDrawBox(body.getPosition(), body.getRotation(), sides); } };
private void reset() { DMatrix3 R = new DMatrix3(); R.setIdentity(); top1.setRotation(R); top2.setRotation(R); top1.setPosition(0.8f, -2, 2); top2.setPosition(0.8f, 2, 2); top1.setAngularVel(1,0,7); top2.setAngularVel(1,0,7); top1.setLinearVel(0,0.2f,0); top2.setLinearVel(0,0.2f,0); }
/** * Set the mass of a body. * @param b b * @param mass mass * */ //ODE_API public static void dBodySetMass (DBody b, DMassC mass){ b.setMass(mass); }
private void reset_state() { float sx=-4, sy=-4, sz=2; DQuaternion q = new DQuaternion(); dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); if (BOX) { boxbody.setPosition (sx, sy+1, sz); boxbody.setLinearVel (0,0,0); boxbody.setAngularVel (0,0,0); boxbody.setQuaternion (q); } if (CYL) { cylbody.setPosition (sx, sy, sz); cylbody.setLinearVel (0,0,0); cylbody.setAngularVel (0,0,0); cylbody.setQuaternion (q); } }
obj[i].body.destroy(); for ( k=0; k < GPB; k++ ) if ( random_pos ) obj[i].body.setPosition( dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 ); dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, for ( k=0; k<num; k++ ) DVector3C pos = obj[k].body.getPosition(); if ( pos.get2() > maxheight ) maxheight = pos.get2(); obj[i].body.setPosition( 0,0,maxheight+1 ); dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 ); obj[i].body.setRotation( R ); obj[i].body.setData( i ); obj[i].body.setMass( m ); obj[selected].body.disable(); obj[selected].body.enable();
avgPos = avgPos.sub(body.getPosition()); double ldiff = avgPos.dot(avgPos); DQuaternion adiff = new DQuaternion(); dQMultiply1(adiff, avgQuat, body.getQuaternion()); if (ldiff > linearThreashold || adiff.get0() < angularThreashold) { allIdle = false; bone.pBuffer[autoDisableBufferIndex].set(bone.body.getPosition()); bone.qBuffer[autoDisableBufferIndex].set(bone.body.getQuaternion()); if (autoDisabled) { bone.body.disable(); bone.body.setLinearVel(0, 0, 0); bone.body.setAngularVel(0, 0, 0); bone.body.setPosition(bone.position); bone.body.setQuaternion(bone.quaternion); } else { bone.position.set(bone.body.getPosition()); bone.quaternion.set(bone.body.getQuaternion());
DVector3C w = body[0].getAngularVel(); body[0].addTorque (kd*w.get0(),kd*w.get1()+0.1*Math.cos(a),kd*w.get2()+0.1*Math.sin(a)); a += 0.01; DVector3C p1 = body[0].getPosition(); DVector3C p2 = body[1].getPosition(); DVector3 tmp = new DVector3(); body[0].addForce(tmp); tmp.eqDiff(p1, p2).scale(ks); body[1].addForce(tmp); DMatrix3C R1 = body[0].getRotation(); dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, dRandReal()-0.5,dRandReal()-0.5); body[0].setRotation (R3); DVector3C pos = body[0].getPosition(); body[0].setPosition ( pos.get0()+0.2*(dRandReal()-0.5), pos.get1()+0.2*(dRandReal()-0.5), dsSetTexture (DS_TEXTURE_NUMBER.DS_WOOD); dsSetColor (1,1,0); dsDrawBox (body[0].getPosition(), body[0].getRotation(), sides1); dsSetColor (0,1,1); dsDrawBox (body[1].getPosition(), body[1].getRotation(), sides2);
obj[i].body.destroy(); if (random_pos) obj[i].body.setPosition ( dRandReal()*2-1 + platpos.get0(), dRandReal()*2-1 + platpos.get1(), obj[i].body.setPosition ( platpos.get0(), platpos.get1(), R.setIdentity(); obj[i].body.setRotation (R); obj[i].body.setData (i); obj[i].body.setMass (m);
DMass m = OdeHelper.createMass(); m.setCapsule(1, 3, radius, cyllen); body.setMass(m); DCapsule geom = OdeHelper.createCapsule(space, radius, cyllen); geom.setBody(body); 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()); bones.add(bone); return bone;
body[i].setPosition (dRandReal()*2-1,dRandReal()*2-1, dRandReal()*2+RADIUS); DQuaternion q = new DQuaternion(); for (j=0; j<4; j++) q.set(j, dRandReal()*2-1); body[i].setQuaternion (q); body[i].setLinearVel (dRandReal()*2-1,dRandReal()*2-1, dRandReal()*2-1); body[i].setAngularVel (dRandReal()*2-1,dRandReal()*2-1, dRandReal()*2-1); OdeMath.dRfromQ(R, q); m.rotate (R); body[i].setMass (m); DVector3C pos = body[i].getPosition(); body[i].setPosition (pos.get(0)+dRandReal()*0.2-0.1, pos.get(1)+dRandReal()*0.2-0.1,pos.get(2)+dRandReal()*0.2-0.1);
noGyroBody.setMass( boxMass ); expGyroBody.setMass( boxMass ); impGyroBody.setMass( boxMass ); noGyroBody.setPosition( -sep, 0, sep); expGyroBody.setPosition( 0, 0, sep); impGyroBody.setPosition( sep, 0, sep); noGyroBody.setAngularVel(omega); expGyroBody.setAngularVel(omega); impGyroBody.setAngularVel(omega); noGyroBody.setGyroscopicMode(false); expGyroBody.setGyroscopicMode(false); expGyroBody.setMaxAngularSpeed( 40 );
if (BOX) { body[bodies-1].addForce(speed,0,0); j2.setParamVel2(speed); j2.setParamFMax2(FMAX); j2.getBody(0).enable(); j2.getBody(1).enable(); if (b.isEnabled()) DVector3C lvel = b.getLinearVel(); double lspeed = lvel.lengthSquared(); if (lspeed > DISABLE_THRESHOLD) disable = false; DVector3C avel = b.getAngularVel(); double aspeed = avel.lengthSquared(); if (aspeed > DISABLE_THRESHOLD) b.disable(); dsSetColor(0.5f,0.5f,1); if (b.isEnabled()) dsSetColor(1,1,1); else dsDrawSphere (cannon_ball_body.getPosition(),cannon_ball_body.getRotation(), CANNON_BALL_RADIUS);
private void simLoop (boolean pause) anchor_body.addTorque (torque); test_body.addTorque (torque); world.step (0.03); if (iteration >= 100) { DVector3C w1 = anchor_body.getAngularVel (); DVector3C w2 = test_body.getAngularVel (); DQuaternionC q1 = anchor_body.getQuaternion (); DQuaternionC q2 = test_body.getQuaternion (); double maxdiff = dMaxDifference (w1,w2); System.out.println ("w-error = " + maxdiff + " (" + w1 + ") and (" + w2 + ")"); DVector3 sides3 = new DVector3(3*SIDE,3*SIDE,3*SIDE); dsSetColor (1,1,1); dsDrawBox (anchor_body.getPosition(), anchor_body.getRotation(), sides3); dsSetColor (1,0,0); dsDrawBox (test_body.getPosition(), test_body.getRotation(), sides2); dsSetColor (1,1,0); for (int i=0; i<NUM; i++) dsDrawBox (particle[i].getPosition (), particle[i].getRotation (), sides);
private void constructWorldForTest (double gravity, int bodycount, /* body 1 pos */ double pos1x, double pos1y, double pos1z, /* body 2 pos */ double pos2x, double pos2y, double pos2z, /* body 1 rotation axis */ double ax1x, double ax1y, double ax1z, /* body 1 rotation axis */ double ax2x, double ax2y, double ax2z, /* rotation angles */ double a1, double a2) { // create world world = OdeHelper.createWorld(); world.setERP (0.2); world.setCFM (1e-6); world.setGravity (0,0,gravity); DMass m = OdeHelper.createMass(); m.setBox (1,SIDE,SIDE,SIDE); m.adjust (MASS); body[0] = OdeHelper.createBody (world); body[0].setMass (m); body[0].setPosition(pos1x, pos1y, pos1z); DQuaternion q = new DQuaternion(); dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1); body[0].setQuaternion (q); if (bodycount==2) { body[1] = OdeHelper.createBody (world); body[1].setMass (m); body[1].setPosition (pos2x, pos2y, pos2z); dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2); body[1].setQuaternion (q); } else body[1] = null; }
@Override public void step (boolean pause) { int i; if (!pause) { //static double angle = 0; angle += 0.05; body[NUM-1].addForce(0,0,1.5*(Math.sin(angle)+1.0)); space.collide(0,myNearCallback); world.step(0.05); /* remove all contact joints */ contactgroup.empty(); } dsSetColor (1,1,0); dsSetTexture (DS_TEXTURE_NUMBER.DS_WOOD); for (i=0; i<NUM; i++) dsDrawSphere (body[i].getPosition(), body[i].getRotation(),RADIUS); }
R1 = body[0].getRotation(); dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, dRandReal()-0.5,dRandReal()-0.5); body[0].setRotation(R3); DVector3C pos = body[0].getPosition(); body[0].setPosition( pos.get0()+0.2*(dRandReal()-0.5), pos.get1()+0.2*(dRandReal()-0.5), dsSetTexture (DS_TEXTURE_NUMBER.DS_WOOD); dsSetColor (1,1,0); dsDrawBox (body[0].getPosition(),body[0].getRotation(),sides1); if (body[1]!=null) { dsSetColor (0,1,1); dsDrawBox (body[1].getPosition(),body[1].getRotation(),sides2);
@Override public void step (boolean pause) { if (!pause) { // add random forces and torques to all bodies int i; final double scale1 = 5; final double scale2 = 5; for (i=0; i<NUM; i++) { body[i].addForce ( scale1*(dRandReal()*2-1), scale1*(dRandReal()*2-1), scale1*(dRandReal()*2-1)); body[i].addTorque ( scale2*(dRandReal()*2-1), scale2*(dRandReal()*2-1), scale2*(dRandReal()*2-1)); } world.step (0.05); createTest(); } // float sides[3] = {SIDE,SIDE,SIDE}; dsSetColor (1,1,0); for (int i=0; i<NUM; i++) dsDrawSphere (body[i].getPosition(), body[i].getRotation(), RADIUS); }
zPos = sinA*rampW + sphereRadius; sphereBody[ii] = OdeHelper.createBody(world); sphereBody[ii].setMass(sphereMass); sphereGeom[ii] = OdeHelper.createSphere(space,sphereRadius); sphereGeom[ii].setBody(sphereBody[ii]); sphereBody[ii].setPosition(xPos,yPos,zPos); sphereBody[ii].setAngularVel(0,omega,0);
/** * This is the explicit computation of * gyroscopic forces. */ private static void expStep(DBody body) { // Explicit computation DMatrix3 I = new DMatrix3(), tmp = new DMatrix3(); DMassC m = body.getMass(); DMatrix3C R = body.getRotation(); // compute inertia tensor in global frame OdeMath.dMultiply2_333 (tmp,m.getI(),R); OdeMath.dMultiply0_333 (I,R,tmp); // compute explicit rotational force // we treat 'tmp'like a vector, but that's okay. DVector3C w = body.getAngularVel(); OdeMath.dMultiply0_331 (tmp,I,w); DVector3 tau = new DVector3(); DVector3 tmp2 = new DVector3(tmp.get00(), tmp.get01(), tmp.get02()); OdeMath.dCalcVectorCross3(tau,tmp2,w); body.addTorque(tau); }
kbody.setKinematic(); final double kx = 1, ky = 0, kz = .5; kbody.setPosition(kx, ky, kz); kbox = OdeHelper.createBox(space, 3, 3, .5); kbox.setBody(kbody); matraca.setPosition(kx+0, ky+1, kz+1); matraca_geom = OdeHelper.createBox(space, 0.5, 2, 0.75); matraca_geom.setBody(matraca); DMass mass = OdeHelper.createMass(); mass.setBox(1, 0.5, 2, 0.75); matraca.setMass(mass);