int main (int argc, char **argv) { // 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; if(argc==2) { fn.path_to_textures = argv[1]; } // create world dInitODE2(0); world = dWorldCreate(); space = dSimpleSpaceCreate(0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-0.5); dWorldSetCFM (world,1e-5); dCreatePlane (space,0,0,1,0); memset (obj,0,sizeof(obj)); // note: can't share tridata if intending to trimesh-trimesh collide TriData1 = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); TriData2 = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0); TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0); dGeomSetData(TriMesh1, TriData1); dGeomSetData(TriMesh2, TriData2); {dGeomSetPosition(TriMesh1, 0, 0, 0.9); dMatrix3 Rotation; dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2); dGeomSetRotation(TriMesh1, Rotation);} {dGeomSetPosition(TriMesh2, 1, 0, 0.9); dMatrix3 Rotation; dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2); dGeomSetRotation(TriMesh2, Rotation);} // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
void cPhysicsObject::CreateTrimesh(cWorld* pWorld, const std::vector<spitfire::math::cVec3>& coords, const std::vector<unsigned int>& indices, const physvec_t& pos, const physvec_t& rot) { vVertices = coords; vIndices = indices; bBody = false; bDynamic = false; // Trimeshes are static for the moment v[0] = 0.0f; v[1] = 0.0f; v[2] = 0.0f; dTriMeshDataID trimeshData = dGeomTriMeshDataCreate(); const int VertexCount = vVertices.size(); const int IndexCount = vIndices.size(); dGeomTriMeshDataBuildSingle( trimeshData, (const void*)vVertices.data(), sizeof(spitfire::math::cVec3), (int)VertexCount, // Faces (const void*)vIndices.data(), (int)IndexCount, 3 * sizeof(uint32_t) // Indices ); geom = dCreateTriMesh(pWorld->GetSpaceStatic(), trimeshData, NULL , NULL , NULL); InitCommon(pWorld, pos, rot); }
void Obstacle::on_addToScene() { node_ = SceneGraph::addModel(name_, model_); size_t batchCnt = 0; TriangleBatch const *triBatch = model_->batches(&batchCnt); size_t boneCnt = 0; Bone const *bone = model_->bones(&boneCnt); size_t vSize = model_->vertexSize(); char const * vertex = (char const *)model_->vertices(); unsigned int const *index = (unsigned int const *)model_->indices(); for (size_t bi = 0; bi != batchCnt; ++bi) { dTriMeshDataID tmd = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(tmd, vertex, vSize, triBatch[bi].maxVertexIndex + 1, index + triBatch[bi].firstTriangle * 3, triBatch[bi].numTriangles * 3, 12); dGeomID geom = dCreateTriMesh(gStaticSpace, tmd, 0, 0, 0); tmd_.push_back(tmd); geom_.push_back(geom); Matrix bx; get_bone_transform(bone, triBatch[bi].bone, bx); Vec3 p(bx.translation()); addTo(p, pos()); dGeomSetPosition(geom, p.x, p.y, p.z); dGeomSetRotation(geom, bx.rows[0]); } }
PhyMeshDataPtr PhyMeshDataPool::Load( const String & name, const Float3 * vertexData, int vertexCount, const int * indexData, int indexCount, const Float3 & scale) { d_assert (vertexData != NULL && indexData != NULL); String uname; uname.Format("%s_%.2f_%.2f_%.2f", name.c_str(), scale.x, scale.y, scale.z); // Find PhyMeshData * node = mMeshDataLinker; while (node != NULL) { if (node->_uname == uname) return node; node = LINKER_NEXT(node); } // Create New Mesh PhyMeshData * meshData = new PhyMeshData; meshData->_dataId = dGeomTriMeshDataCreate(); meshData->_uname = uname; meshData->_vertexData = new Float3[vertexCount]; meshData->_vertexCount = vertexCount; meshData->_indexData = new int[indexCount]; meshData->_indexCount = indexCount; for (int i = 0; i < vertexCount; ++i) { meshData->_vertexData[i] = vertexData[i] * scale; } for (int i = 0; i < indexCount; ++i) { meshData->_indexData[i] = indexData[i]; } dGeomTriMeshDataBuildSingle((dTriMeshDataID)(meshData->_dataId), meshData->_vertexData, sizeof(Float3), meshData->_vertexCount, meshData->_indexData, meshData->_indexCount, 3 * sizeof(int)); LINKER_APPEND(mMeshDataLinker, meshData); return meshData; }
void StaticWorldObject::AddLeaf(ssgLeaf *leaf, sgVec3 initialpos) { // traverse the triangles int cnt = leaf->getNumTriangles() ; int nv = leaf->getNumVertices() ; // int nn = leaf->getNumNormals() ; float *vertices = new float[3*nv]; int *indices = new int[3*cnt]; int i; for (i=0; i<nv; i++) { float *v = leaf->getVertex( i ) ; assert(v); memcpy(vertices+3*i, v, 3*sizeof(float)); } for (i=0; i<cnt; i++) { short idx0, idx1, idx2 ; leaf->getTriangle( i, &idx0, &idx1, &idx2 ) ; indices[3*i+0]=idx0; indices[3*i+1]=idx1; indices[3*i+2]=idx2; } dTriMeshDataID data = dGeomTriMeshDataCreate(); dataids.push_back(data); dGeomTriMeshDataBuildSingle ( data, vertices, 3*sizeof(float), nv, indices, cnt*3, 3*sizeof(int) ); //fprintf(stderr,"Adding trimesh with %d verts, %d indices\n", nv, cnt*3); dGeomID trimesh = dCreateTriMesh(space, data, 0,0,0); geomids.push_back(trimesh); dGeomSetPosition(trimesh, initialpos[0], initialpos[1], initialpos[2]); dMatrix3 R; dRFromAxisAndAngle (R, 0,1,0, 0.0); dGeomSetRotation (trimesh, R); dGeomSetData(trimesh, this); }
OdeTriMesh::OdeTriMesh(const Mesh& mesh) : mTriMeshId(0), mMesh(mesh) { mTriMeshId = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle ( mTriMeshId, mMesh.mVertices.get(), sizeof(v3), mMesh.mVertexCount, mMesh.mIndices.get(), mMesh.mIndexCount, sizeof(u32) ); }
void CollisionMesh::Finalize() { //create mesh data structure meshData = dGeomTriMeshDataCreate(); //create indices int indexlist = new int[TriangleList.size()*3]; for( int i = 0; i< TriangleList.size()*3; i++) { indexlist[i] = i; } //copy over vertex data into buffer dReal* vertices = new dReal[TriangleList.size()*9]; int vi=0; for ( int i=0; i<TriangleList.size(); i++ ) { vertices[vi+0] = TriangleList[i].v1.x; vertices[vi+1] = TriangleList[i].v1.y; vertices[vi+2] = TriangleList[i].v1.z; vertices[vi+3] = TriangleList[i].v2.x; vertices[vi+4] = TriangleList[i].v2.y; vertices[vi+5] = TriangleList[i].v2.z; vertices[vi+6] = TriangleList[i].v3.x; vertices[vi+7] = TriangleList[i].v3.y; vertices[vi+8] = TriangleList[i].v3.z; vi += 9; } dGeomTriMeshDataBuildSingle(meshData, vertices, sizeof(dReal)*3, TriangleList.size()*3, (const int*)indexlist, TriangleList.size()*3, 3*sizeof( int )); Geom = dCreateTriMesh(solver->GetSpaceID(true, Location.x, Location.y, Location.z), meshData, 0, 0, 0); dGeomSetData( Geom, &SurfaceDesc ); dGeomSetPosition( Geom, Location.x, Location.y, Location.z ); dMatrix3 R; dRFromEulerAngles (R, pitch, -yaw, roll); dGeomSetRotation( Geom, R ); Initialized = true; }
//=========================================================================== 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); }
int main (int argc, char **argv) { dMass m; dMatrix3 R; // 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/textures"; if(argc==2) fn.path_to_textures = argv[1]; // create world world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-9.8); dWorldSetQuickStepNumIterations (world, 64); // Create a static world using a triangle mesh that we can collide with. int numv = sizeof(world_vertices)/(3*sizeof(float)); int numi = sizeof(world_indices)/ sizeof(int); printf("numv=%d, numi=%d\n", numv, numi); dTriMeshDataID Data = dGeomTriMeshDataCreate(); // fprintf(stderr,"Building Single Precision Mesh\n"); dGeomTriMeshDataBuildSingle ( Data, world_vertices, 3 * sizeof(float), numv, world_indices, numi, 3 * sizeof(int) ); world_mesh = dCreateTriMesh(space, Data, 0, 0, 0); dGeomTriMeshEnableTC(world_mesh, dSphereClass, false); dGeomTriMeshEnableTC(world_mesh, dBoxClass, false); dGeomSetPosition(world_mesh, 0, 0, 0.5); dRSetIdentity(R); //dIASSERT(dVALIDMAT3(R)); dGeomSetRotation (world_mesh, R); float sx=0.0, sy=3.40, sz=6.80; sphbody = dBodyCreate (world); dMassSetSphere (&m,1,RADIUS); dBodySetMass (sphbody,&m); sphgeom = dCreateSphere(0, RADIUS); dGeomSetBody (sphgeom,sphbody); reset_ball(); dSpaceAdd (space, sphgeom); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); // Causes segm violation? Why? // (because dWorldDestroy() destroys body connected to geom; must call first!) dGeomDestroy(sphgeom); dGeomDestroy (world_mesh); dJointGroupEmpty (contactgroup); dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); return 0; }
State* init() { State* state = new State(); dInitODE(); SDL_Init(SDL_INIT_EVERYTHING); state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL); SDL_WM_SetCaption("Physics", NULL); SDL_Flip(state->screen); SDL_ShowCursor(SDL_DISABLE); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); GLfloat light_ambient[] = { 1, 1, 1, 1 }; glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); GLfloat lightpos[] = {0, 4, 0, 1}; glLightfv(GL_LIGHT0, GL_POSITION, lightpos); glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST); glShadeModel(GL_SMOOTH); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glEnable(GL_DEPTH_TEST); glEnable(GL_CULL_FACE); glClearColor(0, 0, 0, 1); state->posx = 0;//21; state->posy = 4;//8; state->posz = 5;//21; state->rotx = 0; state->roty = 0;//-40; state->rotz = 0; state->wkey = false; state->akey = false; state->skey = false; state->dkey = false; state->gkey = false; state->simSpeed = 60; state->carcam = false; state->carbodydrawable = new Drawable("objs/carbody.obj"); state->carwheeldrawable = new Drawable("objs/carwheel.obj"); state->map = new Drawable("objs/jump2.obj"); state->cube = new Drawable("objs/cube.obj"); // state->monkey = new Drawable("objs/monkey.obj"); state->world = dWorldCreate(); dWorldSetGravity(state->world, 0, -9.8, 0); state->worldSpace = dHashSpaceCreate(0); const double carHeight = 0.65; const double carZ = 90; const double carX = 0; const float speed = -1000; const float force = 200; state->carbodybody = dBodyCreate(state->world); dBodySetPosition(state->carbodybody, carX, carHeight, carZ); dMass bodymass; dMassSetBoxTotal(&bodymass, 100, 2, 4, 1); dBodySetMass(state->carbodybody, &bodymass); dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4); dGeomSetBody(carbodygeom, state->carbodybody); state->flcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2); const dMatrix3 m = { 0, 0, 1, 0 , 0, 1, 0, 0 , 0, 0, 1, 0 }; dBodySetRotation(state->flcarwheelbody, m); dMass wheelmass; dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2); dBodySetMass(state->flcarwheelbody, &wheelmass); dJointID joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->flcarwheelbody); dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(flcarwheelgeom, state->flcarwheelbody); dJointID motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->flcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->frcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2); dBodySetRotation(state->frcarwheelbody, m); dBodySetMass(state->frcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->frcarwheelbody); dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(frcarwheelgeom, state->frcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->frcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->blcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2); dBodySetRotation(state->blcarwheelbody, m); dBodySetMass(state->blcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->blcarwheelbody); dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(blcarwheelgeom, state->blcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->blcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->brcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2); dBodySetRotation(state->brcarwheelbody, m); dBodySetMass(state->brcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->brcarwheelbody); dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(brcarwheelgeom, state->brcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->brcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->var = new double[3]; state->var = dBodyGetPosition(state->carbodybody); // cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl; //TODO check if translation matrix from dBody can be used. dSpaceID cubespace = dHashSpaceCreate(state->worldSpace); for(int i = 0; i < NUM_CUBES/10; i++) { for(int k = 0; k < 10; k++) { state->cubebody[i*10+k] = dBodyCreate(state->world); dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1); dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70); dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2); dGeomSetBody(cubegeom, state->cubebody[i*10+k]); } } { int indexSize = state->map->vertices.size()/3; unsigned int* index = new unsigned int[indexSize]; for(int i = 0; i < indexSize; i++) index[i] = i; dTriMeshDataID triMeshData = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12); dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL); dGeomSetPosition(mapGeom, 0, 0, 0); } // state->monkeyBody = dBodyCreate(state->world); // { // int indexSize = state->monkey->vertices.size()/3; // unsigned int* index = new unsigned int[indexSize]; // for(int i = 0; i < indexSize; i++) // index[i] = i; // dTriMeshDataID triMeshData = dGeomTriMeshDataCreate(); // dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12); // dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL); // dGeomSetPosition(monkeyGeom, 0, 2, 0); // dGeomSetBody(monkeyGeom, state->monkeyBody); // } state->physicsContactgroup = dJointGroupCreate(0); return state; }
int main (int argc, char **argv) { // 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; // create world dInitODE2(0); world = dWorldCreate(); space = dSimpleSpaceCreate(0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-0.5); dWorldSetCFM (world,1e-5); //dCreatePlane (space,0,0,1,0); memset (obj,0,sizeof(obj)); Size[0] = 5.0f; Size[1] = 5.0f; Size[2] = 2.5f; Vertices[0][0] = -Size[0]; Vertices[0][1] = -Size[1]; Vertices[0][2] = Size[2]; Vertices[1][0] = Size[0]; Vertices[1][1] = -Size[1]; Vertices[1][2] = Size[2]; Vertices[2][0] = Size[0]; Vertices[2][1] = Size[1]; Vertices[2][2] = Size[2]; Vertices[3][0] = -Size[0]; Vertices[3][1] = Size[1]; Vertices[3][2] = Size[2]; Vertices[4][0] = 0; Vertices[4][1] = 0; Vertices[4][2] = 0; Indices[0] = 0; Indices[1] = 1; Indices[2] = 4; Indices[3] = 1; Indices[4] = 2; Indices[5] = 4; Indices[6] = 2; Indices[7] = 3; Indices[8] = 4; Indices[9] = 3; Indices[10] = 0; Indices[11] = 4; dTriMeshDataID Data = dGeomTriMeshDataCreate(); //dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount); dGeomTriMeshDataBuildSingle(Data, Vertices[0], 3 * sizeof(float), VertexCount, &Indices[0], IndexCount, 3 * sizeof(dTriIndex)); dGeomTriMeshDataPreprocess2(Data, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); TriMesh = dCreateTriMesh(space, Data, 0, 0, 0); //dGeomSetPosition(TriMesh, 0, 0, 1.0); Ray = dCreateRay(space, 0.9); dVector3 Origin, Direction; Origin[0] = 0.0; Origin[1] = 0; Origin[2] = 0.5; Origin[3] = 0; Direction[0] = 0; Direction[1] = 1.1f; Direction[2] = -1; Direction[3] = 0; dNormalize3(Direction); dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]); dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dThreadingImplementationShutdownProcessing(threading); dThreadingFreeThreadPool(pool); dWorldSetStepThreadingImplementation(world, NULL, NULL); dThreadingFreeImplementation(threading); dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
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); }
/***************************************************************************\ * Field Set * \***************************************************************************/ void PhysicsTriMeshGeom::setGeometryNode(NodePtr& node) { PhysicsTriMeshGeomPtr tmpPtr(*this); GeometryPtr geo = GeometryPtr::dcast(node->getCore()); if(geo!=NullFC) { calcVertexNormals(geo, deg2rad( 30)); separateProperties(geo); createSingleIndex(geo); GeoPositions3f::StoredFieldType* positions = GeoPositions3fPtr::dcast( geo->getPositions())->getFieldPtr(); GeoIndicesUI32::StoredFieldType* indices = GeoIndicesUI32Ptr::dcast( geo->getIndices())->getFieldPtr(); GeoNormals3f::StoredFieldType* normals = GeoNormals3fPtr::dcast( geo->getNormals())->getFieldPtr(); GeoPTypesPtr geoTypes = geo->getTypes(); bool triangles = false; //has to be some triangle soup! for( Int32 i=0; i < geoTypes->size(); ++i) { switch( geoTypes->getValue(i)) { case GL_TRIANGLES: triangles=true; break; case GL_TRIANGLE_STRIP: triangles=true; break; case GL_TRIANGLE_FAN: triangles=true; break; } } UInt32 vertexCount = GeoPositions3fPtr::dcast(geo->getPositions())->getSize(); UInt32 vertexStride = 3*sizeof(Real32); UInt32 indexCount = GeoIndicesUI32Ptr::dcast(geo->getIndices())->getSize(); UInt32 indexStride = 3*sizeof(UInt32); //pass the pointers to ODE if(tmpPtr->data) dGeomTriMeshDataDestroy(tmpPtr->data); tmpPtr->data = dGeomTriMeshDataCreate(); if(triangles) { dGeomTriMeshDataBuildSingle(tmpPtr->data, (Real32*)&positions->front(), vertexStride, vertexCount, (Int32*)&indices->front(), indexCount, indexStride/* just can't use this, (Real32*)&normals->front()*/); tmpPtr->setData(tmpPtr->data); /* use this method if you build with single precision dGeomTriMeshDataBuildSingle1(tmpPtr->data, (Real32*)&positions->front(), vertexStride, vertexCount, (Int32*)&indices->front(), indexCount, indexStride, (Real32*)&normals->front()); tmpPtr->setData(tmpPtr->data); */ } else { FWARNING(("No triangle mesh given to ODE! Convert to triangles first!\n")); tmpPtr->setData(tmpPtr->data); } } tmpPtr->geoNode=node; PhysicsTriMeshGeomBase::setGeometryNode(node); }
// 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; } }
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; } }
int main (int argc, char **argv) { // 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; // create world dInitODE2(0); world = dWorldCreate(); space = dSimpleSpaceCreate(0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-0.5); dWorldSetCFM (world,1e-5); dCreatePlane (space,0,0,1,0); memset (obj,0,sizeof(obj)); // note: can't share tridata if intending to trimesh-trimesh collide const unsigned preprocessFlags = (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES) | (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES); TriData1 = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); dGeomTriMeshDataPreprocess2(TriData1, preprocessFlags, NULL); TriData2 = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); dGeomTriMeshDataPreprocess2(TriData2, preprocessFlags, NULL); TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0); TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0); dGeomSetData(TriMesh1, TriData1); dGeomSetData(TriMesh2, TriData2); {dGeomSetPosition(TriMesh1, 0, 0, 0.9); dMatrix3 Rotation; dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2); dGeomSetRotation(TriMesh1, Rotation);} {dGeomSetPosition(TriMesh2, 1, 0, 0.9); dMatrix3 Rotation; dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2); dGeomSetRotation(TriMesh2, Rotation);} dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL); dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dThreadingImplementationShutdownProcessing(threading); dThreadingFreeThreadPool(pool); dWorldSetStepThreadingImplementation(world, NULL, NULL); dThreadingFreeImplementation(threading); dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
int main (int argc, char **argv) { dMass m; dMatrix3 R; // 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; if(argc==2) { fn.path_to_textures = argv[1]; } // create world dInitODE2(0); world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-9.8); dWorldSetQuickStepNumIterations (world, 12); // Create a static world using a triangle mesh that we can collide with. int numv = sizeof(world_vertices)/(3*sizeof(float)); int numi = sizeof(world_indices)/ sizeof(dTriIndex); printf("numv=%d, numi=%d\n", numv, numi); dTriMeshDataID Data = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle ( Data, world_vertices, 3 * sizeof(float), numv, world_indices, numi, 3 * sizeof(dTriIndex) ); world_mesh = dCreateTriMesh(space, Data, 0, 0, 0); dGeomSetPosition(world_mesh, 0, 0, 0.5); dRFromAxisAndAngle (R, 0,1,0, 0.0); dGeomSetRotation (world_mesh, R); #ifdef BOX boxbody = dBodyCreate (world); dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ); dMassAdjust (&m, 1); dBodySetMass (boxbody,&m); boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ); dGeomSetBody (boxgeom,boxbody); dSpaceAdd (space, boxgeom); #endif #ifdef CYL cylbody = dBodyCreate (world); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m,WMASS); dBodySetMass (cylbody,&m); cylgeom = dCreateCylinder(0, RADIUS, WHEELW); dGeomSetBody (cylgeom,cylbody); #if defined(CYL_GEOM_OFFSET) dMatrix3 mat; dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0); dGeomSetOffsetRotation(cylgeom,mat); #endif dSpaceAdd (space, cylgeom); #endif reset_state(); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dJointGroupEmpty (contactgroup); dJointGroupDestroy (contactgroup); // First destroy geoms, then space, then the world. #ifdef CYL dGeomDestroy (cylgeom); #endif #ifdef BOX dGeomDestroy (boxgeom); #endif dGeomDestroy (world_mesh); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; (void)world_normals; // get rid of compiler warnings }
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; } }
// Create a mesh shape object, add it to ODE's collision space, and initialize the mass member. bool CMeshShape::Attach(dSpaceID SpaceID) { if (!CShape::Attach(SpaceID)) FAIL; n_assert(!pVBuffer); n_assert(!pIBuffer); //!!!REVISIT IT! Now it's copypaste from A. Sysoev's code if (pInitMesh) { // load the vertices and indices VertexCount = pInitMesh->GetNumVertices(); IndexCount = 3 * pInitMesh->GetNumIndices() - 6; VertexWidth = pInitMesh->GetVertexWidth(); // allocate vertex and index buffer int VBSize = pInitMesh->GetVertexBufferByteSize(); int IBSize = IndexCount * sizeof(int); pVBuffer = (float*)n_malloc(VBSize); pIBuffer = (int*)n_malloc(IBSize); pInitMesh->SetUsage(nMesh2::ReadOnly); // read vertices and indices float *pVBuf = pInitMesh->LockVertices(); memcpy(pVBuffer,pVBuf,VBSize); pInitMesh->UnlockVertices(); ushort *pIBuf = pInitMesh->LockIndices(); // copy with conversion TriStrip -> TriList pIBuffer[0] = pIBuf[0]; pIBuffer[1] = pIBuf[1]; pIBuffer[2] = pIBuf[2]; for(int i = 3, j = 2; i < pInitMesh->GetNumIndices(); i++) { pIBuffer[++j] = pIBuffer[j - 2]; pIBuffer[++j] = pIBuffer[j - 2]; pIBuffer[++j] = pIBuf[i]; } pInitMesh->UnlockIndices(); } else { n_assert(FileName.IsValid()); if (FileName.CheckExtension("nvx2")) { nNvx2Loader MeshLoader; if (!LoadFromFile(MeshLoader)) FAIL; } else if (FileName.CheckExtension("n3d2")) { nN3d2Loader MeshLoader; if (!LoadFromFile(MeshLoader)) FAIL; } else { n_error("CMeshShape: invalid file extension in '%Sphere'", FileName.Get()); FAIL; } } // fix my collide bits, we don't need to collide against other static and disabled entities SetCategoryBits(Static); SetCollideBits(Dynamic); // create an ODE TriMeshData object from the loaded vertices and indices ODETriMeshDataID = dGeomTriMeshDataCreate(); //!!!CHECK IT! // index buffer оказывается должен быть всегда типа int[] // хм, эта функция похоже никак не реагирует на передаваемый TriStride, // пришлось конвертировать TriStrip в TriList dGeomTriMeshDataBuildSingle(ODETriMeshDataID, pVBuffer, VertexWidth * sizeof(float), VertexCount, pIBuffer, IndexCount, 3 * sizeof(int)); ODETriMeshID = dCreateTriMesh(0, ODETriMeshDataID, 0, 0, 0); AttachGeom(ODETriMeshID, SpaceID); //!!!!!!!FIXME: apply shape mass here! OK; }