//=========================================================================== void cODEGenericBody::createDynamicBoundingBox(bool a_staticObject) { // check if body image exists if (m_imageModel == NULL) { return; } // create ode dynamic body if object is non static if (!a_staticObject) { m_ode_body = dBodyCreate(m_ODEWorld->m_ode_world); // store pointer to current object dBodySetData (m_ode_body, this); } m_static = a_staticObject; // computing bounding box of geometry representation m_imageModel->computeBoundaryBox(true); // get size and center of box cVector3d center = m_imageModel->getBoundaryCenter(); // compute dimensions cVector3d size = m_imageModel->getBoundaryMax() - m_imageModel->getBoundaryMin(); // build box m_ode_geom = dCreateBox(m_ODEWorld->m_ode_space, size.x, size.y , size.z); // offset box dGeomSetPosition (m_ode_geom, center.x, center.y, center.z); if (!m_static) { // set inertia properties dMassSetBox(&m_ode_mass, 1.0, size.x, size.y, size.z); dMassAdjust(&m_ode_mass, m_mass); dBodySetMass(m_ode_body,&m_ode_mass); // attach body and geometry together dGeomSetBody(m_ode_geom, m_ode_body); } // store dynamic model type m_typeDynamicCollisionModel = ODE_MODEL_BOX; // store dynamic model parameters m_paramDynColModel0 = size.x; m_paramDynColModel1 = size.y; m_paramDynColModel2 = size.z; //m_posOffsetDynColModel; //m_rotOffsetDynColModel; }
IoODEBody *IoODEBody_rawClone(IoODEBody *proto) { IoObject *self = IoObject_rawClonePrimitive(proto); IoObject_setDataPointer_(self, calloc(1, sizeof(IoODEBodyData))); if(DATA(proto)->world) { IoODEWorld *world = DATA(proto)->world; WORLD = world; IoODEWorld_addBody(world, self); BODYID = dBodyCreate(IoODEWorld_rawWorldId(world)); dBodySetData(BODYID, self); } return self; }
/* ================================================================================= createUniversalLeg Use parameters to create leg body/geom and attach to body with universal joint **Warning** mass is not set ================================================================================= */ void createUniversalLeg(ODEObject &leg, ODEObject &bodyAttachedTo, dJointID& joint, dReal xPos, dReal yPos, dReal zPos, dReal xRot, dReal yRot, dReal zRot, dReal radius, dReal length, dReal maxAngle, dReal minAngle, dReal anchorXPos, dReal anchorYPos, dReal anchorZPos) { dMatrix3 legOrient; dRFromEulerAngles(legOrient, xRot, yRot, zRot); //position and orientation leg.Body = dBodyCreate(World); dBodySetPosition(leg.Body, xPos, yPos, zPos); dBodySetRotation(leg.Body, legOrient); dBodySetLinearVel(leg.Body, 0, 0, 0); dBodySetData(leg.Body, (void *)0); //mass dMass legMass; dMassSetCapsule(&legMass, 1, 3, radius, length); //dBodySetMass(leg.Body, &legMass); //geometry leg.Geom = dCreateCapsule(Space, radius, length); dGeomSetBody(leg.Geom, leg.Body); //universal joint joint = dJointCreateUniversal(World, jointgroup); //attach and anchor dJointAttach(joint, bodyAttachedTo.Body, leg.Body); dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos); //axes dJointSetUniversalAxis1(joint, 0, 0, 1); dJointSetUniversalAxis2(joint, 0, 1, 0); //Max and min angles dJointSetUniversalParam(joint, dParamHiStop, maxAngle); dJointSetUniversalParam(joint, dParamLoStop, minAngle); dJointSetUniversalParam(joint, dParamHiStop2, maxAngle); dJointSetUniversalParam(joint, dParamLoStop2, minAngle); }
BarrelBody(Barrel *b) : OdeBody(dBodyCreate(gWorld)), b_(b) { dMass m; dMassSetCylinderTotal(&m, b->mass_, 3, b->radius_, b->height_); dBodySetMass(id_, &m); Vec3 pos(b->pos()); dBodySetPosition(id_, pos.x + b->center_.x, pos.y + b->center_.y, pos.z + b->center_.z); dBodySetData(id_, this); dBodySetAutoDisableFlag(id_, 1); // This will roll forever, very slowly, unless I up the tolerance for sleep. // Compare to a box, which will come to a complete stop easily, and where we // don't want it stopping balancing on an edge. dBodySetAutoDisableAngularThreshold(id_, 0.03f); dBodySetAutoDisableLinearThreshold(id_, 0.03f); dBodySetAutoDisableAverageSamplesCount(id_, 5); }
void Object::MakeBody(dWorldID world) { iBody = dBodyCreate(world); dBodySetQuaternion(iBody,q); dBodySetPosition(iBody,iPosition.x,iPosition.y,iPosition.z); dBodySetLinearVel(iBody,iVel.x,iVel.y,iVel.z); dMassSetBox(&iMass,iM,iSize.x,iSize.y,iSize.z); dMassAdjust(&iMass,iM/2.0); dBodySetMass(iBody,&iMass); disabledSteps = 0; dBodySetAutoDisableFlag(iBody,1); dBodySetData(iBody,data); dBodyDisable(iBody); };
int CProtoHapticDoc::AddShape(CShape* shape) { if(m_shapeCount<MAX_SHAPES) { // create body bodies[m_shapeCount]= dBodyCreate(m_worldID); dMass mass; dMassSetBox (&mass, 100, 1, 1, 1); dBodySetMass (bodies[m_shapeCount], &mass); dBodySetPosition (bodies[m_shapeCount], shape->getLocationX(), shape->getLocationY(), shape->getLocationZ()); //dBodySetRotation (bodies[m_shapeCount], const dMatrix3 R); //dBodySetQuaternion (bodies[m_shapeCount], const dQuaternion q); //odySetLinearVel (bodies[m_shapeCount], 0.1, 0, 0); //dBodySetAngularVel (bodies[m_shapeCount], dReal x, dReal y, dReal z); // create geometry m_geoms[m_shapeCount]= dCreateBox (m_spaceID, 1, 1, 1); // if(m_shapeCount!=0) dGeomSetBody (m_geoms[m_shapeCount], bodies[m_shapeCount]); // else // dGeomSetBody (m_geoms[m_shapeCount], 0); dBodySetData(bodies[m_shapeCount], (void *)m_shapeCount); dGeomSetData(m_geoms[m_shapeCount], (void *)m_shapeCount); m_shapes[m_shapeCount]= shape; m_shapeCount++; return m_shapeCount-1; } else return m_shapeCount; }
void CPHContactBodyEffector::Apply() { const dReal* linear_velocity =dBodyGetLinearVel(m_body); dReal linear_velocity_smag =dDOT(linear_velocity,linear_velocity); dReal linear_velocity_mag =_sqrt(linear_velocity_smag); dReal effect =10000.f*m_recip_flotation*m_recip_flotation; dMass mass; dBodyGetMass(m_body,&mass); dReal l_air=linear_velocity_mag*effect;//force/velocity !!! if(l_air>mass.mass/fixed_step) l_air=mass.mass/fixed_step;//validate if(!fis_zero(l_air)) { dVector3 force={ -linear_velocity[0]*l_air, -linear_velocity[1]*l_air, -linear_velocity[2]*l_air, 0.f }; if(!m_material->Flags.is(SGameMtl::flPassable)) { dVector3& norm=m_contact.geom.normal; accurate_normalize(norm); dMass m; dBodyGetMass(m_body,&m); dReal prg=1.f*dDOT(force,norm);//+dDOT(linear_velocity,norm)*m.mass/fixed_step force[0]-=prg*norm[0]; force[1]-=prg*norm[1]; force[2]-=prg*norm[2]; } dBodyAddForce( m_body, force[0], force[1], force[2] ); } dBodySetData(m_body,NULL); }
SceneObj::SceneObj( Scene *scene_p ) { m_scene_p = scene_p; m_prev_p = 0; m_next_p = m_scene_p->m_obj_p; if( m_next_p ) m_next_p->m_prev_p = this; m_scene_p->m_obj_p = this; m_dbody = dBodyCreate( m_scene_p->m_dworld ); dBodySetData( m_dbody, this ); dBodyDisable( m_dbody ); dMass mass; dMassSetBoxTotal( &mass, 0.1f, 57, 57, 20 ); dBodySetMass( m_dbody, &mass ); m_dgeom = dCreateBox( m_scene_p->m_dspace, 57, 57, 20 ); dGeomSetBody( m_dgeom, m_dbody ); m_texid = 0; }
void CProtoHapticDoc::RemoveShape(CShape* shape) { int i= 0; while(m_shapes[i]!=shape) i++; dGeomDestroy (m_geoms[i]); dBodyDestroy (bodies[i]); int j; for(j= i; j<m_shapeCount; j++) { m_shapes[j]= m_shapes[j+1]; bodies[j]= bodies[j+1]; m_geoms[j]= m_geoms[j+1]; if(j+1<m_shapeCount) { dBodySetData(bodies[j], (void *)j); dGeomSetData(m_geoms[j], (void *)j); } } m_shapeCount--; }
Else::AcrobotArticulatedBody ::AcrobotArticulatedBody( dWorldID argWorldID, dSpaceID argSpaceID, double argScale ) : ArticulatedBody( argWorldID, argSpaceID ), myRadius( 0.1 * argScale ), myLength( argScale ), myJointGroupID( 0 ) { // create body gmtl::Vec3d dims( 2*myRadius, 2*myRadius, 2*myRadius + myLength ); const double density = 1.0; const int zDirection = 3; const double extraSpace = 0.1*myLength; dMass mass; dBodyID body = dBodyCreate( myWorldID ); dBodySetData( body, this ); // create cylinder aligned to z-axis dGeomID geom = dCreateCapsule( mySpaceID, myRadius, myLength ); dGeomSetBody( geom, body ); dBodySetPosition( body, 0, 0, extraSpace + 0.5*myLength + myRadius ); dMassSetCappedCylinder( &mass, density, zDirection, myRadius, myLength ); dBodySetMass( body, &mass ); dJointID joint = dJointCreateHinge( myWorldID, myJointGroupID ); dJointAttach( joint, NULL, body ); dJointSetHingeAnchor( joint, 0, 0, extraSpace + myLength + myRadius ); dVector3 axis; axis[0] = 0; axis[1] = 1; axis[2] = 0; dJointSetHingeAxis( joint, axis[0], axis[1], axis[2] ); myBodies[0] = body; myJoints[0] = joint; myBodyDims[0] = dims; myGeoms.push_back( geom ); }
void SSphereParts::set(dWorldID w, dSpaceID space) { double rad = m_cmpnt.radius(); // konao DUMP(("[SSphereParts::set] ODE geom created (r=%f) [%s:%d]\n", rad, __FILE__, __LINE__)); dGeomID geom = dCreateSphere(0, rad); m_odeobj = ODEObjectContainer::getInstance()->createODEObj ( w, geom, 0.9, 0.01, 0.5, 0.5, 0.8, 0.001, 0.0 ); dBodyID body = m_odeobj->body(); dMass m; dMassSetZero(&m); dMassSetSphere(&m, DENSITY, rad); //dMassAdjust(&m, 1.0); dMassAdjust(&m, m_mass); dBodySetMass(body, &m); dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0); dSpaceAdd(space, geom); dBodySetData(body, this); }
/******************************************************************************* Function to initialize ODE. *******************************************************************************/ void initODE() { ///////////////// Initializing the ODE general features //////////////////// dInitODE(); //Initialize library. World = dWorldCreate(); //Crate a new dynamics, empty world. Space = dSimpleSpaceCreate(0); //Create a new space for collision (independent). ContactGroup = dJointGroupCreate(0); //Create a joints container, without specifying size. dWorldSetGravity( World, 0.0, -9.81, 0 ); //Add gravity to this World. //Define error conrrection constants. dWorldSetERP( World, 0.2 ); dWorldSetCFM( World, 1e-5 ); //Set the velocity that interpenetrating objects will separate at. dWorldSetContactMaxCorrectingVel( World, 0.9 ); //Set the safety area for contacts to be considered at rest. dWorldSetContactSurfaceLayer( World, 0.001 ); //Automatically disable objects that have come to a rest. dWorldSetAutoDisableFlag( World, false ); /////////////// Initializing the rigid bodies in the world ///////////////// //Create a collision plane and add it to space. The next parameters are the //literal components of ax + by + cz = d. dCreatePlane( Space, 0.0, 1.0, 0.0, 0.0 ); const dReal xPos = 0; const dReal yPos = 5; const dReal zPos = 0; const dReal xRot = 0; const dReal yRot = 0; const dReal zRot = 0; const dReal radius = .75; const dReal length = 4; const dReal sides[3] = { 2, 2, 2 }; //Create body body.Body = dBodyCreate(World); dBodySetPosition(body.Body, xPos, yPos, zPos); dMatrix3 Orient3; //dRFromAxisAndAngle(Orient3, 0, 0, 1, 3.14/4); dRFromEulerAngles(Orient3, xRot, yRot, zRot); //dRFromEulerAngles(Orient3, 0, 0, 0); dBodySetRotation(body.Body, Orient3); dBodySetLinearVel(body.Body, 0, 0, 0); dBodySetData(body.Body, (void *)0); dMass bodyMass; dMassSetCapsule(&bodyMass, 1.0, 3, radius, length); //dMassSetBox(&bodyMass, 10, sides[0], sides[1], sides[2]); dBodySetMass(body.Body, &bodyMass); body.Geom = dCreateCapsule(Space, radius, length); //body.Geom = dCreateBox(Space, sides[0], sides[1], sides[2]); dGeomSetBody(body.Geom, body.Body); float head_pos[ 3 ] = { 0.0f, 5.0f, -1.0f }; createInvisibleHead( head_pos ); createFrontLegs(); createMiddleLegs(); createBackLegs(); rng_engine.seed( std::chrono::duration_cast< std::chrono::microseconds >( std::chrono::system_clock::now().time_since_epoch() ).count() ); const int foodPrize = 10; dReal position[3] = { 10.0f, 0.0f, -20.0f }; addFoodParticle( position, foodPrize, &World, &Space ); }
static void createMesh() { int i,k; dReal sides[3]; dMass m; loading = 1; Sleep(100); i = num; num++; obj[i].body = dBodyCreate (ode_world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; 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); dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); cMesh* new_object = new cMesh(world); new_object->loadFromFile("resources\\models\\bunny.obj"); new_object->computeGlobalPositions(); new_object->createAABBCollisionDetector(true, true); new_object->computeAllNormals(); // set material properties of object cMaterial new_material; new_material.setStiffness(20.0); new_material.setDynamicFriction(0.2); new_material.setStaticFriction(0.4); new_material.m_ambient.set(0.3, 0.3, 0.8); new_material.m_diffuse.set(0.8, 0.8, 0.8); new_material.m_specular.set(1.0, 1.0, 1.0); new_material.setShininess(100); new_object->useMaterial(true, true); new_object->setMaterial(new_material, true); new_object->useColors(false, true); cODEProxy* new_proxy = new cODEProxy(default_proxy); new_proxy->enableDynamicProxy(true); cursor->setRenderingMode(RENDER_DEVICE); new_proxy->m_defaultObject = new_object; new_proxy->initialize(world, cursor->m_deviceGlobalPos); cursor->m_pointForceAlgos.push_back(new_proxy); VertexCount = new_object->getNumVertices(true); Vertices = new float[VertexCount*3]; IndexCount = 3*new_object->getNumTriangles(true); Indices = new int[IndexCount]; // This will hold all the parents we're still searching... std::list<cMesh*> meshes_to_descend; meshes_to_descend.push_front(new_object); // While there are still parent meshes to process int cnt = 0; int cnt2 = 0; while(meshes_to_descend.empty() == 0) { // Grab the next parent cMesh* cur_mesh = meshes_to_descend.front(); meshes_to_descend.pop_front(); // Put all his children on the list of parents to process for(unsigned int i=0; i<cur_mesh->getNumChildren(); i++) { cGenericObject* cur_object = cur_mesh->getChild(i); // Only process cMesh children cMesh* cur_mesh = dynamic_cast<cMesh*>(cur_object); if (cur_mesh) { unsigned int i; for (i=0; i<cur_mesh->getNumVertices(false); i++) { Vertices[cnt] = cur_mesh->getVertex(i)->getPos().x; Vertices[cnt+1] = cur_mesh->getVertex(i)->getPos().y; Vertices[cnt+2] = cur_mesh->getVertex(i)->getPos().z; cnt+=3; } for (i=0; i<cur_mesh->getNumTriangles(false); i++) { Indices[cnt2] = cur_mesh->getTriangle(i)->getIndexVertex0(); Indices[cnt2+1] = cur_mesh->getTriangle(i)->getIndexVertex1(); Indices[cnt2+2] = cur_mesh->getTriangle(i)->getIndexVertex2(); cnt2+=3; } meshes_to_descend.push_back(cur_mesh); } } } dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, Indices, IndexCount, 3 * sizeof(int)); 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); dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[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); new_object->setPos(0,0,100); new_object->computeGlobalPositions(1); objects.push_back(new_object); syncPoses(); world->addChild(new_object); loading = 0; Sleep(100); }
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; } }
//=========================================================================== void cODEGenericBody::createDynamicMesh(bool a_staticObject, const cVector3d& a_offsetPos, const cMatrix3d& a_offsetRot) { // create ode dynamic body if object is non static if (!a_staticObject) { m_ode_body = dBodyCreate(m_ODEWorld->m_ode_world); // store pointer to current object dBodySetData (m_ode_body, this); } m_static = a_staticObject; // make sure body image has been defined if (m_imageModel == NULL) { return; } // check if body image is a mesh cMesh* mesh = dynamic_cast<cMesh*>(m_imageModel); if (mesh == NULL) { return; } // store dynamic model type m_typeDynamicCollisionModel = ODE_MODEL_TRIMESH; // store dynamic model parameters m_paramDynColModel0 = 0.0; m_paramDynColModel1 = 0.0; m_paramDynColModel2 = 0.0; m_posOffsetDynColModel = a_offsetPos; m_rotOffsetDynColModel = a_offsetRot; // create a table which lists all vertices and triangles // these vertices must be of type float and not double! // even if we are using the double precision compiled version of ODE ! int numTriangles = mesh->getNumTriangles(true); int vertexCount = mesh->getNumVertices(true); m_vertices = new float[3 * vertexCount]; int indexCount = 3 * numTriangles; m_vertexIndices = new int[indexCount]; // build table of ODE vertices and vertex indices recusevily int nIndexOffset = 0; int nVerticesCount = 0; int nIndexCount = 0; int nTriangles = buildMeshTable(mesh, nIndexOffset, nVerticesCount, nIndexCount); // build box m_ode_triMeshDataID = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(m_ode_triMeshDataID, m_vertices, // vertex positions 3 * sizeof(float), // size of vertex vertexCount, // number of vertices m_vertexIndices, // triangle indices 3 * nTriangles, // number of triangles 3 * sizeof(int)); m_ode_geom = dCreateTriMesh(m_ODEWorld->m_ode_space, m_ode_triMeshDataID, 0, 0, 0); // remember the mesh's dTriMeshDataID on its userdata for convenience. dGeomSetData(m_ode_geom, m_ode_triMeshDataID); // computing bounding box of geometry representation m_imageModel->computeBoundaryBox(true); // compute dimensions cVector3d size = m_imageModel->getBoundaryMax() - m_imageModel->getBoundaryMin(); // set inertia properties if (!m_static) { dMassSetBox(&m_ode_mass, 1.0, size.x, size.y, size.z); dMassAdjust(&m_ode_mass, m_mass); dBodySetMass(m_ode_body,&m_ode_mass); dGeomSetBody(m_ode_geom, m_ode_body); } // adjust position offset dGeomSetPosition (m_ode_geom, a_offsetPos.x, a_offsetPos.y, a_offsetPos.z); // adjust orientation offset dMatrix3 R; R[0] = a_offsetRot.m[0][0]; R[1] = a_offsetRot.m[0][1]; R[2] = a_offsetRot.m[0][2]; R[4] = a_offsetRot.m[1][0]; R[5] = a_offsetRot.m[1][1]; R[6] = a_offsetRot.m[1][2]; R[8] = a_offsetRot.m[2][0]; R[9] = a_offsetRot.m[2][1]; R[10] = a_offsetRot.m[2][2]; dGeomSetRotation (m_ode_geom, R); }
void PhysicsBody::setData(void* someData) { dBodySetData(_BodyID, someData); }
static void command (int cmd) { size_t i; int k; dReal sides[3]; dMass m; int setBody; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y') { 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 if (obj[i].body) { 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 + platpos[0], dRandReal()*2-1 + platpos[1], dRandReal()+2 + platpos[2]); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dBodySetPosition (obj[i].body, platpos[0], platpos[1], platpos[2]+2); dRSetIdentity (R); } 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]); } 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]); } 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); } 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 == ' ') { mov_time = 0; } else if (cmd == 'm') { mov_type = mov_type==1 ? 2 : 1; mov_time = 0; } }
void SCylinderParts::set(dWorldID w, dSpaceID space) { double radius = m_cmpnt.radius(); double length = m_cmpnt.length(); // konao //LOG_MSG(("[SCylinderParts::set] ODE geom created (r, l)=(%f, %f) [%s:%d]\n", radius, length, __FILE__, __LINE__)) // TODO: Ideally, cylinder should be constructed here. However, collision detection // between two cylinders could not be realized. So, Capsule is required // by okamoto@tome on 2011-10-12 //dGeomID geom = dCreateCapsule(0, radius, length); dGeomID geom = dCreateCylinder(0, radius, length); m_odeobj = ODEObjectContainer::getInstance()->createODEObj ( w, geom, 0.9, 0.01, 0.5, 0.5, 0.8, 0.001, 0.0 ); dBodyID body = m_odeobj->body(); dMass m; dMassSetZero(&m); //dMassSetCapsule(&m, DENSITY, 1, radius, length); dMassSetCylinder(&m, DENSITY, 1, radius, length); //TODO: mass of the cylinder should be configurable dMassAdjust(&m, m_mass); dBodySetMass(body, &m); dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body // set the long axis as y axis dReal offq[4] = {0.707, 0.707, 0.0, 0.0}; dReal offq2[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()}; dQuaternion qua; dQMultiply2(qua, offq2, offq); dGeomSetOffsetQuaternion(geom, qua); //dGeomSetOffsetQuaternion(geom, offq2); //dReal tmpq[4] = {0.707, 0.0, 0.0, 0.707}; //dGeomSetQuaternion(geom, tmpq); //dBodySetQuaternion(body, tmpq); /*TODO: Consideration is required whether this procedure is needed * Reflection of orientation of the cylinder * dMatrix3 R; * dRFromAxisAndAngle(dMatrix3 R, dReal rx, dReal ry, dReal rz, dReal angle) * dRFromAxisAndAngle(R,x_axis,y_axis,z_axis,angleData); * dBodySetRotation(body,R); // Request of actual rotation */ // Not used, deleted by inamura // real part of the quaternion // double q = cos(angleData/2.0); // imaginary part of the quaternion // double i,j,k; // i = x_axis * sin(angleData/2.0); // j = y_axis * sin(angleData/2.0); // k = z_axis * sin(angleData/2.0); m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0); dSpaceAdd(space, geom); dBodySetData(body, this); }
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; } }
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; } }
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; } }
void Simulation::nearCallback(void* data, dGeomID o1, dGeomID o2) { int i; // if (o1->body && o2->body) return; // exit without doing anything if the two bodies are connected by a joint dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); //PEXP(b1); //PEXP(b2); //if (dAreConnected (b1, b2)) cout << "debug: CONNECTED CONNECTED OVER\n"; if (b1 && b2 && dAreConnected(b1, b2)) return; int g1 = (o1 == this->ground); int g2 = (o2 == this->ground); int not_ground = (!(g1 ^ g2)); int one_is_link = (o1 != this->ground) && (b1) && ( dBodyGetData(b1) == (void*)tagLink); int two_is_link = (o2 != this->ground) && (b2) && ( dBodyGetData(b2) == (void*)tagLink); int link_link = one_is_link && two_is_link; double mu; // determine friction if (link_link) mu = 0; else if (not_ground) mu = this->mu_non_ground; else mu = this->mu_ground; // profiling this->callback_count++; // debug //if (b1 == sprocket || b2 == sprocket) { cout << "debug: collision with sprocket\n"; } dContact contact[3]; // up to 3 contacts per box if (not_ground) { // except for mu, link_link and link_sprocket are bundled together for (i = 0; i < 3; i++) { // contact[i].surface.mode = dContactSoftCFM | dContactApprox1; contact[i].surface.mode = 0; contact[i].surface.mu = mu; contact[i].surface.soft_cfm = 0.01; } } else { for (i = 0; i < 3; i++) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; // Approx1 means friction only for non zero normal contact[i].surface.mu = mu;//dInfinity; contact[i].surface.slip1 = 0.1; contact[i].surface.slip2 = 0.1; contact[i].surface.soft_erp = this->ground_soft_erp; //0.3; contact[i].surface.soft_cfm = this->ground_soft_cfm; //0.5; } } if (int numc = dCollide(o1, o2, 3, &contact[0].geom, sizeof(dContact))) { // profiling if (not_ground) { this->not_ground_count++; // std::cout << "debug: not_ground collision\n"; } else { this->ground_count++; //cout << "debug: ground collision\n"; // if a link is involved, increment its count of contact joints dBodyID one_link = (o1 == this->ground? b2 : b1); dBodySetData(one_link, (void*)((int)dBodyGetData(one_link) + numc)); }; //dMatrix3 RI; //dRSetIdentity (RI); //const dReal ss[3] = {0.02,0.02,0.02}; //PEXP(numc); for (i = 0; i < numc; i++) { dJointID c = dJointCreateContact(this->world, this->contactgroup, contact + i); dJointAttach(c, b1, b2); // dsDrawBox (contact[i].geom.pos,RI,ss); } } }
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; } }
// called when a key pressed static void command( int cmd ) { int i,k; dReal sides[3]; dMass m; cmd = locase( cmd ); if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' ) { 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 == 's' ) { sides[0] *= 0.5; dMassSetSphere( &m,DENSITY,sides[0] ); obj[i].geom[0] = dCreateSphere( space,sides[0] ); } else if ( cmd == 'v' ) { obj[i].geom[0] = dCreateConvex( space, convexBunnyPlanes, convexBunnyPlaneCount, convexBunnyPoints, convexBunnyPointCount, convexBunnyPolygons ); /// Use equivalent TriMesh to set mass dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount, ( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) ); dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 ); dMassSetTrimesh( &m, DENSITY, triMesh ); dGeomDestroy( triMesh ); dGeomTriMeshDataDestroy( new_tmdata ); 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] ); } 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; } }