const dMass& Mass::createMass() { if(!created) { assembleMass(); for(std::list<SimObject*>::const_iterator iter = children.begin(), end = children.end(); iter != end; ++iter) { Mass* childMassDesc = dynamic_cast<Mass*>(*iter); ASSERT(childMassDesc); const dMass& childMass = childMassDesc->createMass(); if(childMassDesc->translation || childMassDesc->rotation) { dMass shiftedChildMass = childMass; if(childMassDesc->rotation) { dMatrix3 matrix; ODETools::convertMatrix(*childMassDesc->rotation, matrix); dMassRotate(&shiftedChildMass, matrix); } if(childMassDesc->translation) dMassTranslate(&shiftedChildMass, childMassDesc->translation->x, childMassDesc->translation->y, childMassDesc->translation->z); dMassAdd(&mass, &shiftedChildMass); } else dMassAdd(&mass, &childMass); } created = true; } return mass; }
void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity ) { TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface; dMass totalMass; dBodyGetMass( m_BodyID, &totalMass ); if ( m_GeomIDs.size() == 0 ) { dMassSetZero( &totalMass ); } dMatrix4 R; dVector3 P; Matrix4ToODE( _bodyToGeomTransform, R, P ); dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID ); dGeomID encapsulatedGeom = 0; dMass currMass; dMassSetZero( &currMass ); encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength ); dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength ); dMassRotate( &currMass, R ); //dMassTranslate(&currMass,P[0],P[1],P[2]); dMassAdd( &totalMass, &currMass ); dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] ); dGeomSetRotation( encapsulatedGeom, R ); dGeomTransformSetCleanup( geomTransform, 1 ); dGeomTransformSetGeom( geomTransform, encapsulatedGeom ); dGeomSetBody( geomTransform, m_BodyID ); m_GeomIDs.push_back( geomTransform ); dBodySetMass( m_BodyID, &totalMass ); }
void PhysicsBody::addMassOf( dBodyID otherBody ) { dMass myMass, otherMass; getMassStruct(myMass); dBodyGetMass(otherBody, &otherMass); dMassAdd(&myMass, &otherMass); setMassStruct(myMass); }
dMass KinematicNodeDummy::getODEMassToAttachToParent(arma::mat44 coordinateFrame) const { dMass compositeMass; dMassSetZero(&compositeMass); for (KinematicMass const& mass : m_masses) { dMass componentMass = mass.getODEMass(coordinateFrame); dMassAdd(&compositeMass, &componentMass); } for (KinematicNode *child : m_children) { arma::mat44 childFrame = coordinateFrame * child->getForwardMatrix(); dMass componentMass = child->getODEMassToAttachToParent(childFrame); dMassAdd(&compositeMass, &componentMass); } return compositeMass; }
void PhysicsObject::attachObject(boost::shared_ptr<PhysicsObject> po, const v3& position, const qv4& orientation) { po->mBodyOffset = position; GeomMapT::const_iterator it = po->mGeometry.begin(); for (; it != po->mGeometry.end(); ++it) { // Calculate new relative position dGeomID geom = it->second.geomId; const dReal* offset = dGeomGetOffsetPosition(geom); v3 newPos(offset); newPos += position; // Attach dGeomSetBody(geom, mOdeBody); dGeomSetOffsetPosition(geom, newPos.x, newPos.y, newPos.z); dSpaceRemove(po->mSpaceId, geom); dSpaceAdd(mSpaceId, geom); } // add the two masses dMass otherMass; dBodyGetMass(po->mOdeBody, &otherMass); // dbglog << "OtherMass: " << otherMass.mass; // dbglog << "OtherCenter: " << odeVectorOut(otherMass.c); // dbglog << "OtherInertia: " << odeMatrixOut(otherMass.I); dBodyGetMass(mOdeBody, &mOriginalMass); // dbglog << "OwnMass: " << mOriginalMass.mass; // dbglog << "OwnCenter: " << odeVectorOut(mOriginalMass.c); // dbglog << "OwnInertia: " << odeMatrixOut(mOriginalMass.I); dMassAdd(&mOriginalMass, &otherMass); dMassTranslate(&mOriginalMass, -mOriginalMass.c[0], -mOriginalMass.c[1], -mOriginalMass.c[2]); dBodySetMass(mOdeBody, &mOriginalMass); // dbglog << "NewMass: " << mOriginalMass.mass; // dbglog << "NewCenter: " << odeVectorOut(mOriginalMass.c); // dbglog << "NewInertia: " << odeMatrixOut(mOriginalMass.I); // Disable old body dBodyDisable(po->mOdeBody); notifyControlAboutChangeInMass(); }
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3], const float r[16]) { dVector3 v; dMatrix3 M; dReal rad; dReal len; dMass add; if (r) set_rotation(M, r); if (dGeomGetClass(geom) != dPlaneClass) { dReal m = get_data(geom)->mass; /* Create a new mass for the given geom. */ switch (dGeomGetClass(geom)) { case dBoxClass: dGeomBoxGetLengths(geom, v); dMassSetBoxTotal(&add, m, v[0], v[1], v[2]); break; case dSphereClass: rad = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&add, m, rad); break; case dCapsuleClass: dGeomCapsuleGetParams(geom, &rad, &len); dMassSetCapsuleTotal(&add, m, 3, rad, len); break; default: dMassSetZero(&add); break; } /* Transform the geom and mass to the given position and rotation. */ if(dGeomGetBody(geom)) { if (p) { dGeomSetOffsetPosition(geom, p[0], p[1], p[2]); dMassTranslate (&add, p[0], p[1], p[2]); } if (r) { dGeomSetOffsetRotation(geom, M); dMassRotate (&add, M); } } else { if (p) dGeomSetPosition(geom, p[0], p[1], p[2]); if (r) dGeomSetRotation(geom, M); } /* Accumulate the new mass with the body's existing mass. */ dMassAdd(mass, &add); } }
int main (int argc, char **argv) { dInitODE2(0); bool fixed = true; // setup pointers to drawstuff callback functions dsFunctions fn; fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = &command; fn.stop = 0; fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; dVector3 offset; dSetZero (offset, 4); // Default test case if (argc >= 2 ) { for (int i=1; i < argc; ++i) { //static int tata = 0; if (1) { if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) Help (argv); if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) ) type = dJointTypeSlider; if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) { int j = i+1; if ( j+1 > argc || // Check if we have enough arguments argv[j] == '\0' || // We should have a path here argv[j][0] == '-' ) // We should have a path not a command line Help (argv); else fn.path_to_textures = argv[++i]; // Increase i since we use this argument } } if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) ) tc = 1; if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) ) tc = 2; if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) ) tc = 3; if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) ) fixed = false; } } world = dWorldCreate(); dWorldSetERP (world, 0.8); space = dSimpleSpaceCreate (0); contactgroup = dJointGroupCreate (0); geom[GROUND] = dCreatePlane (space, 0,0,1,0); dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); dGeomSetCollideBits (geom[GROUND], catBits[ALL]); dMass m; dMatrix3 R; // Create the Obstacle geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]); dGeomSetCategoryBits (geom[OBS], catBits[OBS]); dGeomSetCollideBits (geom[OBS], catBits[ALL]); //Rotation of 45deg around y dRFromAxisAndAngle (R, 1,1,0, -0.25*PI); dGeomSetRotation (geom[OBS], R); dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5); //Rotation of 90deg around y // Will orient the Z axis along X dRFromAxisAndAngle (R, 0,1,0, -0.5*PI); // Create Body2 (Wiil be attached to the world) body[BODY2] = dBodyCreate (world); // Main axis of cylinder is along X=1 dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); dMassAdjust (&m, Mass1); geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); dGeomSetBody (geom[BODY2], body[BODY2]); dGeomSetOffsetRotation (geom[BODY2], R); dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]); dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) ); dBodySetMass (body[BODY2], &m); // Create Body 1 (Slider on the prismatic axis) body[BODY1] = dBodyCreate (world); // Main axis of capsule is along X=1 dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH); dMassAdjust (&m, Mass1); geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH); dGeomSetBody (geom[BODY1], body[BODY1]); dGeomSetOffsetRotation (geom[BODY1], R); dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]); dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]); dMass mRect; dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); dMassAdd (&m, &mRect); // TODO: translate m? geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); dGeomSetBody (geom[RECT], body[BODY1]); dGeomSetOffsetPosition (geom[RECT], (BODY1_LENGTH-RECT_SIDES[0]) /2.0, 0.0, -RADIUS -RECT_SIDES[2]/2.0); dGeomSetCategoryBits (geom[RECT], catBits[RECT]); dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) ); dBodySetMass (body[BODY1], &m); setPositionBodies (tc); if ( fixed ) { // Attache external cylinder to the world dJointID fixed = dJointCreateFixed (world,0); dJointAttach (fixed , NULL, body[BODY2]); dJointSetFixed (fixed ); dWorldSetGravity (world,0,0,-0.8); } else { dWorldSetGravity (world,0,0,0); } // The static is here only to help debugging switch (type) { case dJointTypeSlider : { dSliderJoint *sj = new dSliderJoint (world, 0); sj->attach (body[BODY1], body[BODY2]); sj->setAxis (1, 0, 0); joint = sj; } break; case dJointTypePiston : // fall through default default: { dPistonJoint *pj = new dPistonJoint (world, 0); pj->attach (body[BODY1], body[BODY2]); pj->setAxis (1, 0, 0); dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]); joint = pj; } break; }; // run simulation dsSimulationLoop (argc,argv,400,300,&fn); delete joint; dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; bool setBody = false; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' /* || cmd == 'l' */) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } /* // cylinder option not yet implemented else if (cmd == 'l') { sides[1] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } */ else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x') { setBody = true; // start accumulating masses for the composite geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for composite geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule(space,radius,length); dMassSetCapsule(&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate(&m2,drot[k]); dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd(&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody(obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition(obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass(obj[i].body,&m); } else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex(space, planes, planecount, points, pointcount, polygons); } if (!setBody) { // avoid calling for composite geometries for (k=0; k < GPB; k++) if (obj[i].geom[k]) dGeomSetBody(obj[i].geom[k],obj[i].body); dBodySetMass(obj[i].body,&m); } } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; bool setBody = false; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex(space, planes, planecount, points, pointcount, polygons); } else if (cmd == 'y') { sides[1] *= 0.5; dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'm') { dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); // remember the mesh's dTriMeshDataID on its userdata for convenience. dGeomSetData(obj[i].geom[0], new_tmdata); dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); } else if (cmd == 'x') { setBody = true; // start accumulating masses for the composite geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for composite geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule(space,radius,length); dMassSetCapsule(&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate(&m2,drot[k]); dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd(&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody(obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition(obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass(obj[i].body,&m); } if (!setBody) { // avoid calling for composite geometries for (k=0; k < GPB; k++) if (obj[i].geom[k]) dGeomSetBody(obj[i].geom[k],obj[i].body); dBodySetMass(obj[i].body,&m); } } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
//! add ODE parts with fixed joint void SSimEntity::setMass(double mass) { m_parts.mass = mass; int geomNum = getGeomNum(); if (geomNum != 0) { // if a part is already added // mass per part double ms = mass / geomNum; // refer geometry and body dBodyID body = m_parts.body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // refer the type of geometory dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } const dReal *pos = dGeomGetPosition(geom); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } /* for (int i = 0; i < geomNum; i++) { dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); const dReal *pos = dGeomGetPosition(geom); // Change to a relative position from CoG dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]); } // keep the CoG position //m_parts.pos.set(m.c[0], m.c[1], m.c[2]); */ // Adjustment of the gap from CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // Setting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
//! setting of mass void SSimRobotEntity::setMass(SSimObjParts *parts, double mass) { // if a part is already added int geomNum = parts->geoms.size(); if (geomNum != 0) { // mass per each part double ms = mass / geomNum; // refer geometry and body dBodyID body = parts->body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // Refer the type of geometry dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } // const dReal *pos = dGeomGetPosition(geom); //LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2])); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } // adjustment of the gap between CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // seeting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
static void command (int cmd) { size_t i; int j,k; dReal sides[3]; dMass m; int setBody; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') { setBody = 0; if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRSetIdentity (R); //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*) i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } //<---- Convex Object else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); #if 0 obj[i].geom[0] = dCreateConvex (space, planes, planecount, points, pointcount, polygons); #else obj[i].geom[0] = dCreateConvex (space, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (cmd == 'y') { dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x' && USE_GEOM_OFFSET) { setBody = 1; // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for encapsulated geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule (space,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate (&m2,drot[k]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd (&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody (obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition (obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass (obj[i].body,&m); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCapsule (0,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // Translation *after* rotation dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<GPB; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } if (!setBody) for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } else if (cmd == '1') { write_world = 1; } else if (cmd == 'p'&& selected >= 0) { const dReal* pos = dGeomGetPosition(obj[selected].geom[0]); const dReal* rot = dGeomGetRotation(obj[selected].geom[0]); printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]); printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n", rot[0],rot[1],rot[2],rot[3], rot[4],rot[5],rot[6],rot[7], rot[8],rot[9],rot[10],rot[11]); } else if (cmd == 'f' && selected >= 0 && selected < num) { if (dBodyIsEnabled(obj[selected].body)) doFeedback = 1; } }
static void command (int cmd) { size_t i; int j,k; dReal sides[3]; dMass m; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' /* || cmd == 'l' */) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*) i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]); } /* // cylinder option not yet implemented else if (cmd == 'l') { sides[1] *= 0.5; dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } */ else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCCylinder (0,radius,length); dMassSetCappedCylinder (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<2; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } else if (cmd == '1') { write_world = 1; } }
dBodyID KinematicNodeDummy::attachToODE(PhysicsEnvironment *environment, dSpaceID visualSpaceID, dSpaceID collisionSpaceID, arma::mat44 parentsCoordinateFrame) { // move coordinate frame to current node: arma::mat44 coordinateFrame = parentsCoordinateFrame * forwardMatrix; dWorldID worldID = environment->getWorldID(); if (nullptr == m_parent) { // the body must coincide with the center of mass of the equivalent mass of all dummy children and this nodes masses arma::mat44 frame = arma::eye(4, 4); dMass equivMass = this->getODEMass(frame); for (KinematicNode *child : m_children) { arma::mat44 childFrame = frame * child->getForwardMatrix(); dMass childMass = child->getODEMassToAttachToParent(childFrame); if (childMass.mass > 0.) { dMassAdd(&equivMass, &childMass); } } m_odeNodeBodyOffset = arma::eye(4, 4); m_odeNodeBodyOffset(0, 3) = equivMass.c[0]; m_odeNodeBodyOffset(1, 3) = equivMass.c[1]; m_odeNodeBodyOffset(2, 3) = equivMass.c[2]; dMassTranslate(&equivMass, -m_odeNodeBodyOffset(0, 3), -m_odeNodeBodyOffset(1, 3), -m_odeNodeBodyOffset(2, 3)); // create a body m_odeBody = dBodyCreate(worldID); arma::mat44 globalSystemBodyOffset = coordinateFrame * m_odeNodeBodyOffset; dMatrix3 bodyRotation; dVector3 bodyPosition; odeUtils::getRotationMatrixAsDMat(globalSystemBodyOffset, bodyRotation); odeUtils::getPositionAsDVec(globalSystemBodyOffset, bodyPosition); dBodySetPosition(m_odeBody, bodyPosition[0], bodyPosition[1], bodyPosition[2]); dBodySetRotation(m_odeBody, bodyRotation); if (equivMass.mass > 0.) { dBodySetMass(m_odeBody, &equivMass); } else { dBodyGetMass(m_odeBody, &equivMass); equivMass.mass = std::numeric_limits<double>::min(); dBodySetMass(m_odeBody, &equivMass); } } else { // setup the nodeBodyOffset based on the parent's odeBody dBodyID body = this->getODEBody(); const dReal *bodyPosition = dBodyGetPosition(body); const dReal *bodyRotation = dBodyGetRotation(body); arma::mat44 bodyFrame = odeUtils::getMatFromDMatAndDVec(bodyRotation, bodyPosition); arma::mat44 helperRotation = arma::eye(4, 4); helperRotation.submat(0, 0, 2, 2) = coordinateFrame.submat(0, 0, 2, 2); m_odeNodeBodyOffset = coordinateFrame.i() * bodyFrame; } // create a "box" to visualize the node dGeomID geom = dCreateSphere(visualSpaceID, 0.005); dMatrix3 bodyRotation; dVector3 bodyPosition; // the visuals are defined as "local" coordinates but here we need to incorporate the body offset arma::mat44 visualsFrame = m_odeNodeBodyOffset.i(); odeUtils::getRotationMatrixAsDMat(visualsFrame, bodyRotation); // inverse of rotation part odeUtils::getPositionAsDVec(visualsFrame, bodyPosition); dGeomSetBody(geom, getODEBody()); dGeomSetOffsetRotation(geom, bodyRotation); dGeomSetOffsetPosition(geom, bodyPosition[0], bodyPosition[1], bodyPosition[2]); // finally add all the visuals this->attatchODEVisuals(visualsFrame, getODEBody(), collisionSpaceID); // recurse into everything attached to this node for (KinematicNode *child : m_children) { dBodyID childBody = child->attachToODE(environment, visualSpaceID, collisionSpaceID, coordinateFrame); UNUSED(childBody); } return m_odeBody; }
void CPHFracture::MassUnsplitFromFirstToSecond(const dMass& m) { dMassSub(&m_firstM,&m); dMassAdd(&m_secondM,&m); }
void CPHFracture::MassAddToSecond(const dMass& m) { dMassAdd(&m_secondM,&m); }
void CPHFracture::MassAddToFirst(const dMass& m) { dMassAdd(&m_firstM,&m); }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } else if (cmd == 'y') { sides[1] *= 0.5; dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'm') { dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); // remember the mesh's dTriMeshDataID on its userdata for convenience. dGeomSetData(obj[i].geom[0], new_tmdata); dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCapsule (0,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<2; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
void CODEGeom::add_self_mass(dMass& m,const Fvector& ref_point) { dMass self_mass; get_mass(self_mass,ref_point); dMassAdd(&m,&self_mass); }