void Solid::createGeometry(const dSpaceID& i_space)
{
/// need to get the correct geometry from volume.
std::vector< ::Ogre::Vector3> vertices ;
std::vector<unsigned long> indices ;
Ogre::Vector3 scale(1,1,1) ;
m_vertices = NULL ;
m_indices = NULL ;
InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#0") ;
getObject()
->getTrait<Model::Solid>()
->getMesh().getMeshInformation(vertices,indices,scale) ;
if (vertices.size()>0 && indices.size() > 0)
{
m_vertices = new dVector3[vertices.size()];
m_indices = new dTriIndex[indices.size()];
for(unsigned int vertex_index = 0 ;
vertex_index < vertices.size() ;
++vertex_index)
{
m_vertices[vertex_index][0] = (dReal)(vertices[vertex_index].x) ;
m_vertices[vertex_index][1] = (dReal)(vertices[vertex_index].y) ;
m_vertices[vertex_index][2] = (dReal)(vertices[vertex_index].z) ;
m_vertices[vertex_index][3] = 0 ;
}
for(unsigned int index = 0 ;
index < indices.size() ;
++index)
{
m_indices[index] = (int)indices[index] ;
}
m_data = dGeomTriMeshDataCreate() ;
dGeomTriMeshDataBuildSimple(m_data,
(dReal*)m_vertices,(int)vertices.size(),
m_indices,(int)indices.size()) ;
m_geometry1 = dCreateTriMesh(i_space,m_data,0,0,0);
dGeomSetData(m_geometry1,m_data) ;
dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Solid) ;
createApproximatedGeometry(i_space) ;
InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#5") ;
}
}
trimesh_data* create_trimesh_data(std::vector<double>& vertices_vec, std::vector<int>& indices_vec)
{
trimesh_data* new_trimesh = new trimesh_data();
//construct the verticies
new_trimesh->vertex_count = vertices_vec.size() / 3;
if(!new_trimesh->vertex_count)
{
delete new_trimesh;
return 0;
}
new_trimesh->vertices = boost::shared_array<dVector3>(new dVector3[new_trimesh->vertex_count]);
int j;
for(int k = 0, j = 0; k < new_trimesh->vertex_count; k++, j +=3)
{
new_trimesh->vertices[k][0] = vertices_vec[j + 0];
new_trimesh->vertices[k][1] = vertices_vec[j + 1];
new_trimesh->vertices[k][2] = vertices_vec[j + 2];
}
int index_count=indices_vec.size();
if(!index_count)
{
delete new_trimesh;
return 0;
}
new_trimesh->indices = boost::shared_array<dTriIndex>(new dTriIndex[index_count]);
std::copy(indices_vec.begin(), indices_vec.end(), new_trimesh->indices.get());
// Create TriMesh in ODE
new_trimesh->data_id = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSimple(
new_trimesh->data_id,
(dReal*)new_trimesh->vertices.get(),
new_trimesh->vertex_count,
(const dTriIndex*)new_trimesh->indices.get(),
index_count);
// dGeomTriMeshDataPreprocess(new_trimesh->data_id);
return new_trimesh;
}
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/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE();
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);
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]);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void start()
{
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
contact_group = dJointGroupCreate(0);
space = dSimpleSpaceCreate (0);
// first, the ground plane
// it has to coincide with the plane we have in drawstuff
ground = dCreatePlane(space, 0, 0, 1, 0);
// now a ball
dMass m;
dMassSetSphere(&m, 0.1, ball_radius);
ball1_geom = dCreateSphere(space, ball_radius);
ball1_body = dBodyCreate(world);
dGeomSetBody(ball1_geom, ball1_body);
dBodySetMass(ball1_body, &m);
ball2_geom = dCreateSphere(space, ball_radius);
ball2_body = dBodyCreate(world);
dGeomSetBody(ball2_geom, ball2_body);
dBodySetMass(ball2_body, &m);
// tracks made out of boxes
dGeomID trk;
dMatrix3 r1, r2, r3;
dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation};
dMatrix3 s1, s2, s3;
dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation};
dRFromAxisAndAngle(r1, 1, 0, 0, track_angle);
dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl);
dMultiply0_333(r3, r2, r1);
dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle);
dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl);
dMultiply0_333(s3, s2, s1);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]);
dGeomSetRotation(trk, r3);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]);
dGeomSetRotation(trk, s3);
// tracks made out of trimesh
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, s3, box_verts[i]);
dAddVectors3(p, p, so);
dCopyVector3(track_verts[i], p);
}
// trimesh tracks 2, transform all vertices by s3
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, r3, box_verts[i]);
dAddVectors3(p, p, ro);
dCopyVector3(track_verts[n_box_verts + i], p);
}
// copy face indices
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*i+j] = box_faces[3*i+j];
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts;
mesh_data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSimple(mesh_data,
track_verts[0], n_track_verts,
track_faces, 3*n_track_faces);
mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0);
resetSim();
// initial camera position
static float xyz[3] = {-5.9414,-0.4804,2.9800};
static float hpr[3] = {32.5000,-10.0000,0.0000};
dsSetViewpoint (xyz,hpr);
dsSetSphereQuality(3);
}
