bool WorldManagerServer::loadMap(const Ogre::String& mapName)
{
WorldManager::loadMap(mapName);
//cleanup
for (std::vector<dGeomID>::iterator igeom = mStaticGeometries.begin();
igeom != mStaticGeometries.end(); igeom++) {
dGeomDestroy(*igeom);
}
//creating new game and loading new map
Ogre::DataStreamPtr stream = Ogre::ResourceGroupManager::getSingleton().openResource(mapName);
MyGUI::xml::xmlDocument xml;
if (!xml.open(stream))
return false;
MyGUI::xml::xmlNodePtr root_node = xml.getRoot();
if (root_node->getName() != "map")
return false;
MyGUI::xml::xmlNodeIterator inode = root_node->getNodeIterator();
while (inode.nextNode()) {
if (inode->getName() == "geom") {
Ogre::String type = inode->findAttribute("type");
std::vector<Ogre::String> params = Ogre::StringUtil::split(inode->findAttribute("size"), " ");
dGeomID geom;
if (type == "box") {
geom = dCreateBox(mStaticSpace,
Ogre::StringConverter::parseReal(params[0]),
Ogre::StringConverter::parseReal(params[1]),
Ogre::StringConverter::parseReal(params[2])
);
} else if (type == "sphere") {
geom = dCreateSphere(mStaticSpace,
Ogre::StringConverter::parseReal(params[0])
);
} else if (type == "cylinder") {
geom = dCreateCylinder(mStaticSpace,
Ogre::StringConverter::parseReal(params[0]),
Ogre::StringConverter::parseReal(params[1])
);
} else if (type == "capsule") {
geom = dCreateCapsule(mStaticSpace,
Ogre::StringConverter::parseReal(params[0]),
Ogre::StringConverter::parseReal(params[1])
);
} else if (type == "plane") {
geom = dCreatePlane(mStaticSpace,
Ogre::StringConverter::parseReal(params[0]),
Ogre::StringConverter::parseReal(params[1]),
Ogre::StringConverter::parseReal(params[2]),
Ogre::StringConverter::parseReal(params[3])
);
} else {
return false;
}
mStaticGeometries.push_back(geom);
} else {
return false;
}
}
return true;
}
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;
}
}
dGeomID PhysWorld::AddCylinder(dReal radius, dReal length)
{
return dCreateCylinder(mSpace, radius, length);
}
// ロボットの生成
void create() {
// SHIELDの生成（空間に固定）
rod[0].body = dBodyCreate(world);
dBodySetPosition(rod[0].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
dMassSetZero(&mass);
dMassSetCylinderTotal(&mass, SHIELD_WEIGHT, 2, SHIELD_RADIUS, SHIELD_LENGTH);
dBodySetMass(rod[0].body, &mass);
rod[0].geom = dCreateCylinder(space, SHIELD_RADIUS, SHIELD_LENGTH);
dGeomSetBody(rod[0].geom, rod[0].body);
dRFromAxisAndAngle(R, 1, 0, 0, 0.5 * M_PI); // x軸に90度回転
dBodySetRotation(rod[0].body, R);
rod_joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント
dJointAttach(rod_joint[0], rod[0].body, 0);
dJointSetFixed(rod_joint[0]);
// RODの生成（回転ジョイントy軸に回転軸）
rod[1].body = dBodyCreate(world);
dBodySetPosition(rod[1].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, ROD_WEIGHT, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
dBodySetMass(rod[1].body, &mass);
rod[1].geom = dCreateBox(space, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
dGeomSetBody(rod[1].geom, rod[1].body);
dRFromAxisAndAngle(R, 0, 0, 1, 0.25 * M_PI); // z軸に45度回転
dBodySetRotation(rod[1].body, R);
rod_joint[1] = dJointCreateHinge(world, 0); // ヒンジジョイント
dJointAttach(rod_joint[1], rod[1].body, rod[0].body);
dJointSetHingeAnchor(rod_joint[1], SHIELD_X, SHIELD_Y, SHIELD_Z);
dJointSetHingeAxis(rod_joint[1], 0, 1, 0);// y軸ジョイント
// BODYの生成（たてておくだけ）
rod[2].body = dBodyCreate(world);
dBodySetPosition(rod[2].body, BODY_X, BODY_Y, BODY_Z);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, BODY_WEIGHT, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
dBodySetMass(rod[2].body, &mass);
rod[2].geom = dCreateBox(space, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
dGeomSetBody(rod[2].geom, rod[2].body);
// BULLETの生成（CANNON中心に初期座標）
bullet.body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetSphereTotal(&mass, BULLET_WEIGHT, BULLET_RADIUS);
dBodySetMass(bullet.body,&mass);
dBodySetPosition(bullet.body, CANNON_X, CANNON_Y, CANNON_Z);
bullet.geom = dCreateSphere(space, BULLET_RADIUS);
dGeomSetBody(bullet.geom, bullet.body);
// TARGETの生成
// target.body = dBodyCreate(world);
// dMassSetZero(&mass);
// dMassSetSphereTotal(&mass, 0.0001, BULLET_RADIUS);
// dBodySetMass(target.body,&mass);
// dBodySetPosition(target.body, SHIELD_X, SHIELD_Y, SHIELD_Z);
// target.geom = dCreateSphere(space, BULLET_RADIUS);
// dGeomSetBody(target.geom, target.body);
}
dGeomID Space::addCylinder(dReal radius, dReal length){
return dCreateCylinder(space, radius, length);
}
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;
}
}
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 ) {
for (int i=1; i < argc; ++i) {
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
type = dJointTypePR;
if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
}
dInitODE2(0);
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
// Create the body attached to the World
body[W] = dBodyCreate (world);
// Main axis of cylinder is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[W], body[W]);
dGeomSetCategoryBits (geom[W], catBits[W]);
dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
dBodySetMass (body[W], &m);
// Create the dandling body
body[D] = dBodyCreate (world);
// Main axis of capsule is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[D], body[D]);
dGeomSetCategoryBits (geom[D], catBits[D]);
dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
dBodySetMass (body[D], &m);
// Create the external part of the slider joint
geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
dGeomSetCollideBits (geom[EXT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
// Create the internal part of the slider joint
geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
INT_EXT_RATIO*extDim[Y],
INT_EXT_RATIO*extDim[Z]);
dGeomSetCategoryBits (geom[INT], catBits[INT]);
dGeomSetCollideBits (geom[INT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
dMatrix3 R;
dGeomID id;
// Create the first axis of the universal joi9nt
geom[AXIS1] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
dGeomSetRotation (geom[AXIS1], R);
dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
dGeomSetCollideBits (geom[AXIS1],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS1];
dGeomTransformSetGeom (geom[AXIS1], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the second axis of the universal joint
geom[AXIS2] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
dGeomSetRotation (geom[AXIS2], R);
dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
dGeomSetCollideBits (geom[AXIS2],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS2];
dGeomTransformSetGeom (geom[AXIS2], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the anchor
geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
dGeomSetCollideBits (geom[ANCHOR],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
if (body[W]) {
dBodySetPosition(body[W], 0, 0, 5);
}
if (geom[EXT]) {
dGeomSetPosition (geom[EXT], 0,0,3.8);
}
if (geom[INT]) {
dGeomSetPosition (geom[INT], 0,0,2.6);
}
if (geom[AXIS1]) {
dGeomSetPosition (geom[AXIS1], 0,0,2.5);
}
if (geom[AXIS2]) {
dGeomSetPosition (geom[AXIS2], 0,0,2.5);
}
if (geom[ANCHOR]) {
dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
}
if (body[D]) {
dBodySetPosition (body[D], 0,0,1.5);
}
// Attache the upper box to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[W]);
dJointSetFixed (fixed );
if (type == dJointTypePR) {
dPRJoint *pr = new dPRJoint (world, 0);
pr->attach (body[W], body[D]);
pr->setAxis1 (0, 0, -1);
pr->setAxis2 (1, 0, 0);
joint = pr;
dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
}
else {
dPUJoint *pu = new dPUJoint (world, 0);
pu->attach (body[W], body[D]);
pu->setAxis1 (1, 0, 0);
pu->setAxis2 (0, 1, 0);
pu->setAxisP (0, 0, -1);
joint = pu;
dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
}
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void CManipulator::CreateJoint (SPoint &myptAnchor,SJointPara paraJoint,double Border,dBodyID &mybodyLastJoint,dBodyID &mybodyJoint,dGeomID &mygeomLink,dJointID &myJoint,dWorldID myworld,dSpaceID myspace)
{
SPoint ptBody;
if(!mybodyLastJoint)//如果是第一个关节，上个body=NULL
{
if(paraJoint.typeJoint==CYLINDER)
mygeomLink=dCreateCylinder(myspace,paraJoint.paraManipulatorCylinder.r,paraJoint.paraManipulatorCylinder.length);
else
mygeomLink=dCreateBox(myspace,paraJoint.paraManipulatorBox.x,paraJoint.paraManipulatorBox.y,paraJoint.paraManipulatorBox.z);
mybodyJoint=dBodyCreate(myworld);
dGeomSetBody(mygeomLink,mybodyJoint);
dBodySetMass(mybodyJoint,¶Joint.massBody);
ptBody.x=0;
if(paraJoint.typeJoint==CYLINDER)
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
dBodySetPosition(mybodyJoint,ptBody.x,ptBody.y,ptBody.z);
dMatrix3 rot;
if(paraJoint.typeJoint==CYLINDER)
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorCylinder.angle);
else
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorBox.angle);
dBodySetRotation(mybodyJoint,rot);
myJoint=dJointCreateHinge(myworld,0);//create Joint
dJointAttach(myJoint,0,mybodyJoint);
dJointSetHingeAnchor(myJoint,myptAnchor.x,myptAnchor.y,myptAnchor.z);
if(paraJoint.modeAxis==XX)
dJointSetHingeAxis(myJoint,1,0,0);
else if(paraJoint.modeAxis==YY)
dJointSetHingeAxis(myJoint,0,1,0);
else
dJointSetHingeAxis(myJoint,0,0,1);
if(paraJoint.typeJoint==CYLINDER)
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
}
else
{
if(paraJoint.typeJoint==CYLINDER)
mygeomLink=dCreateCylinder(myspace,paraJoint.paraManipulatorCylinder.r,paraJoint.paraManipulatorCylinder.length);
else
mygeomLink=dCreateBox(myspace,paraJoint.paraManipulatorBox.x,paraJoint.paraManipulatorBox.y,paraJoint.paraManipulatorBox.z);
mybodyJoint=dBodyCreate(myworld);
dGeomSetBody(mygeomLink,mybodyJoint);
dBodySetMass(mybodyJoint,¶Joint.massBody);
ptBody.x=0;
if(paraJoint.typeJoint==CYLINDER)
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
dBodySetPosition(mybodyJoint,ptBody.x,ptBody.y,ptBody.z);
dMatrix3 rot;
if(paraJoint.typeJoint==CYLINDER)
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorCylinder.angle);
else
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorBox.angle);
dBodySetRotation(mybodyJoint,rot);
myJoint=dJointCreateHinge(myworld,0);//create Joint
dJointAttach(myJoint,mybodyLastJoint,mybodyJoint);
dJointSetHingeAnchor(myJoint,myptAnchor.x,myptAnchor.y,myptAnchor.z);
if(paraJoint.modeAxis==XX)
dJointSetHingeAxis(myJoint,1,0,0);
else if(paraJoint.modeAxis==YY)
dJointSetHingeAxis(myJoint,0,-1,0);
else
dJointSetHingeAxis(myJoint,0,0,1);
if(paraJoint.typeJoint==CYLINDER)
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
}
mybodyLastJoint=mybodyJoint;
return;
}
void PCylinder::init()
{
body = dBodyCreate (world);
initPosBody();
setMass(m_mass);
/* if (m_texid!=-1)
{
dTriMeshDataID g = dGeomTriMeshDataCreate();
const int vertexcount = 19*3+1;
const int indexcount = 19*9;
dVector3* vertices = new dVector3[vertexcount];
dTriIndex* indices = new dTriIndex[indexcount];
const int n = 24; // number of sides to the cylinder (divisible by 4)
double l = m_length*0.5;
double a = float(M_PI*2.0)/float(n);
double sa = (float) sin(a);
double ca = (float) cos(a);
double r = m_radius;
double ny=1,nz=0,tmp;
int v=0,f=0;
int i;
for (i=0; i<=n; i++) {
if (i>2 && i<n-2)
{
vertices[v][0] = ny*r;
vertices[v][1] = nz*r;
vertices[v][2] = l;v++;
vertices[v][0] = ny*r;
vertices[v][1] = nz*r;
vertices[v][2] = -l;v++;
indices[f] = v;f++;
indices[f] = v-2;f++;
indices[f] = v-1;f++;
indices[f] = v-1;f++;
indices[f] = v;f++;
indices[f] = v+1;f++;
}
}
//v=38;f=38*3
// rotate ny,nz
tmp = ca*ny - sa*nz;
nz = sa*ny + ca*nz;
ny = tmp;
ny=1; nz=0; // normal vector = (0,ny,nz)
glBegin (GL_TRIANGLE_FAN);
vertices[v][0] = 0;
vertices[v][1] = 0;
vertices[v][2] = l;v++;
for (i=0; i<n; i++) {
if (i>2 && i<n-2)
{
vertices[v][0] = ny*r;
vertices[v][1] = nz*r;
vertices[v][2] = l;v++;
indices[f] = 38;f++;
indices[f] = v;f++;
indices[f] = v-1;f++;
}
// rotate ny,nz
tmp = ca*ny - sa*nz;
nz = sa*ny + ca*nz;
ny = tmp;
}
// draw bottom cap
// ny=1; nz=0; // normal vector = (0,ny,nz)
// glBegin (GL_TRIANGLE_FAN);
// glNormal3d (0,0,-1);
// glVertex3d (0,0,-l+zoffset);
// for (i=0; i<=n; i++) {
// if (i>=2 && i<n-2)
// {
//
// glNormal3d (0,0,-1);
// glVertex3d (ny*r,nz*r,-l+zoffset);
// }
// // rotate ny,nz
// tmp = ca*ny + sa*nz;
// nz = -sa*ny + ca*nz;
// ny = tmp;
// }
dGeomTriMeshDataBuildSimple(g, (dReal*) vertices, vertexcount, indices, indexcount);
geom = dCreateTriMesh(0,g,NULL,NULL,NULL);
}
else */
{
geom = dCreateCylinder(0,m_radius,m_length);
}
dGeomSetBody (geom,body);
dSpaceAdd (space,geom);
}
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;
}
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;
}
}
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;
}
}
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);
}
dGeomID ColCylinder::CreateGeom( dSpaceID spaceID )
{
float scale = PhysicsServer::s_pInstance->GetWorldScale();
return dCreateCylinder( spaceID, m_Radius*scale, m_Height*scale );
} // ColCylinder::CreateGeom
/* --------------------------------
** 全方向移動ロボットの生成, 描画
----------------------------------- */
void MakeOmni()
{
/* ローカル変数の定義 */
dMass mass;
double r, w, d, m_w, x, y, z, lx, ly, lz, m_b;
/* ロボットの個数分だけループ */
for (int i=0; i< OMNI_NUM; i++)
{
/* 車輪 */
r = wheelSize[i][0];
w = wheelSize[i][1];
d = wheelSize[i][2];
m_w = wheelM[i];
/* 土台 */
x = basePos[i][0];
y = basePos[i][1];
z = basePos[i][2];
lx = baseSize[i][0];
ly = baseSize[i][1];
lz = baseSize[i][2];
m_b = baseM[i];
/* 土台の生成 */
base[i].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, m_b, lx, ly, lz);
dBodySetMass(base[i].body,&mass);
base[i].geom = dCreateBox(space, lx, ly, lz);
dGeomSetBody(base[i].geom, base[i].body);
dBodySetPosition(base[i].body, x, y, z); /* 座標 */
/* 車輪の生成 */
double wx[WHEEL_NUM] = {lx/2+w/2+d, - (lx/2+w/2+d), 0, 0};
double wy[WHEEL_NUM] = {0, 0, ly/2+w/2+d, -(ly/2+w/2+d)};
double wz[WHEEL_NUM] = {z, z, z, z};
double jx[WHEEL_NUM] = {lx/2, -lx/2, 0, 0};
double jy[WHEEL_NUM] = {0, 0, ly/2, -ly/2};
double jz[WHEEL_NUM] = {z, z, z, z};
for (int j = 0; j < WHEEL_NUM; j++)
{
wheel[i][j].body = dBodyCreate(world);
dMatrix3 R;
if (j >= 2)
{
dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
dBodySetRotation(wheel[i][j].body,R);
}
else
{
dRFromAxisAndAngle(R, 0, 1, 0, M_PI/2.0);
dBodySetRotation(wheel[i][j].body, R);
}
dMassSetZero(&mass);
if (j < 2)
{
dMassSetCylinderTotal(&mass, m_w, 1, r, w);
}
else
{
dMassSetCylinderTotal(&mass, m_w, 2, r, w);
}
dBodySetMass(wheel[i][j].body,&mass);
wheel[i][j].geom = dCreateCylinder(space, r, w);
dGeomSetBody(wheel[i][j].geom, wheel[i][j].body);
dBodySetPosition(wheel[i][j].body, wx[j], wy[j], wz[j]);
wheel[i][j].joint = dJointCreateHinge(world, 0);
dJointAttach(wheel[i][j].joint, base[i].body, wheel[0][j].body);
if (j < 2)
{
dJointSetHingeAxis(wheel[i][j].joint, 1, 0, 0);
}
else
{
dJointSetHingeAxis(wheel[i][j].joint, 0, -1, 0);
}
dJointSetHingeAnchor(wheel[i][j].joint, jx[j], jy[j], jz[j]);
}
}
}
