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.stop = 0;
fn.command = 0;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE ();
// create world
world = dWorldCreate ();
space = dHashSpaceCreate (0);
dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// create object
sphere0 = dBodyCreate (world);
sphere0_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere0,&m);
dGeomSetBody (sphere0_geom,sphere0);
sphere1 = dBodyCreate (world);
sphere1_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere1,&m);
dGeomSetBody (sphere1_geom,sphere1);
sphere2 = dBodyCreate (world);
sphere2_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere2,&m);
dGeomSetBody (sphere2_geom,sphere2);
// set initial position
dBodySetPosition (sphere0,0,0,4);
dBodySetPosition (sphere1,5,0,4);
dBodySetPosition (sphere2,-2,0,4);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// clean up
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void TSRODERigidBody::AddSphereGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius, float _fDensity )
{
TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
dMass totalMass;
dBodyGetMass( m_BodyID, &totalMass );
if ( m_GeomIDs.size() == 0 )
{
dMassSetZero( &totalMass );
}
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyToGeomTransform, R, P );
dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
dGeomID encapsulatedGeom = 0;
dMass currMass;
dMassSetZero( &currMass );
encapsulatedGeom = dCreateSphere( 0, _fRadius );
dMassSetSphere( &currMass, _fDensity, _fRadius );
dMassRotate( &currMass, R );
dMassTranslate( &currMass, P[ 0 ], P[ 1 ], P[ 2 ] );
dMassAdd( &totalMass, &currMass );
dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
dGeomSetRotation( encapsulatedGeom, R );
dGeomTransformSetCleanup( geomTransform, 1 );
dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
dGeomSetBody( geomTransform,m_BodyID );
m_GeomIDs.push_back( geomTransform );
dBodySetMass( m_BodyID, &totalMass );
}
void CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16 flags)
{
VERIFY(ref_obj);
R_ASSERT(_valid( start_pos ) );
R_ASSERT( _valid( start_size ) );
m_body = dBodyCreate (0) ;
dMass m;
dMassSetSphere(&m,1.f,100000.f);
dMassAdjust(&m,1.f);
dBodySetMass(m_body,&m);
switch(_type)
{
case etBox:
m_geom = dCreateBox (0,start_size.x,start_size.y,start_size.z) ;
break;
case etSphere:
m_geom = dCreateSphere (0,start_size.x);
break;
};
dGeomCreateUserData (m_geom) ;
dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback) ;
dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj) ;
dGeomSetBody (m_geom,m_body) ;
dBodySetPosition (m_body,start_pos.x,start_pos.y,start_pos.z) ;
Island() .AddBody (m_body) ;
dBodyEnable (m_body) ;
m_safe_state .create(m_body) ;
spatial_register () ;
m_flags.set(flags,TRUE);
}
ODEObject::ODEObject(OscObject *obj, dGeomID odeGeom, dWorldID odeWorld, dSpaceID odeSpace)
: m_odeWorld(odeWorld), m_odeSpace(odeSpace)
{
m_object = obj;
m_odeGeom = odeGeom;
m_odeBody = NULL;
m_odeBody = dBodyCreate(m_odeWorld);
assert(m_odeGeom!=NULL);
dBodySetPosition(m_odeBody, 0, 0, 0);
dGeomSetPosition(m_odeGeom, 0, 0, 0);
// note: owners must override this by setting the density. can't
// do it here because obj->m_pSpecial is not yet
// initialized.
dMassSetSphere(&m_odeMass, 1, 1);
dBodySetMass(m_odeBody, &m_odeMass);
dGeomSetBody(m_odeGeom, m_odeBody);
dGeomSetData(m_odeGeom, obj);
if (!obj) return;
obj->m_rotation.setSetCallback(ODEObject::on_set_rotation, this);
obj->m_position.setSetCallback(ODEObject::on_set_position, this);
obj->m_velocity.setSetCallback(ODEObject::on_set_velocity, this);
obj->m_accel.setSetCallback(ODEObject::on_set_accel, this);
obj->m_force.setSetCallback(ODEObject::on_set_force, this);
obj->addHandler("push", "ffffff", ODEObject::push_handler);
}
void OscSphereODE::on_density()
{
ODEObject *ode_object = static_cast<ODEObject*>(special());
dMassSetSphere(&ode_object->mass(), m_density.m_value, m_radius.m_value);
dBodySetMass(ode_object->body(), &ode_object->mass());
m_mass.m_value = ode_object->mass().mass;
}
void VStateSphereMass::Apply()
{
VState::Parent pParent = GetParent();
dMassSetSphere (&m_Mass, GetDensity(), m_fRadius);
dMassAdjust (&m_Mass, GetMass());
dBodySetMass (pParent->GetBodyID(),&m_Mass);
}
IoObject *IoODEMass_setSphereDensity(IoODEMass *self, IoObject *locals, IoMessage *m)
{
const double density = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double radius = IoMessage_locals_doubleArgAt_(m, locals, 1);
dMassSetSphere(DATA(self), density, radius);
return self;
}
SphereGeom::SphereGeom(Body* in_pBody, Space* in_pSpace, double radius)
: Geom(in_pSpace)
{
m_pBody = in_pBody;
dMassSetSphere(&m_mass,1,radius);
dMassAdjust(&m_mass,1);
m_id = dCreateSphere(m_pSpace->id(),radius);
finishGeom();
}
void Sphere::setMass(double mass, bool density){
if(body){
dMass m;
dMassSetSphere(&m, mass, osgsphere->getRadius());
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
void OscSphereODE::on_radius()
{
ODEObject *ode_object = static_cast<ODEObject*>(special());
dGeomSphereSetRadius(ode_object->geom(), m_radius.m_value);
// reset the mass to maintain same density
dMassSetSphere(&ode_object->mass(), m_density.m_value, m_radius.m_value);
m_mass.m_value = ode_object->mass().mass;
}
void Mesh::setMass(double mass, bool density){
if(body){
// we should use the bouding box here
dMass m;
dMassSetSphere(&m, mass, osgmesh->getRadius()); // we use a sphere
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
void CPHCapture::CreateBody()
{
m_body= dBodyCreate(0);
m_island.AddBody(m_body);
dMass m;
dMassSetSphere(&m,1.f,1000000.f);
dMassAdjust(&m,100000.f);
dBodySetMass(m_body,&m);
dBodySetGravityMode(m_body,0);
}
//===========================================================================
void cODEGenericBody::createDynamicSphere(const double a_radius,
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;
// build sphere
m_ode_geom = dCreateSphere(m_ODEWorld->m_ode_space, a_radius);
// 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);
// set inertia properties
if (!m_static)
{
dMassSetSphere(&m_ode_mass, 1.0, a_radius);
dMassAdjust(&m_ode_mass, m_mass);
dBodySetMass(m_ode_body,&m_ode_mass);
// attach body and geometry together
dGeomSetBody(m_ode_geom, m_ode_body);
}
// store dynamic model type
m_typeDynamicCollisionModel = ODE_MODEL_SPHERE;
// store dynamic model parameters
m_paramDynColModel0 = a_radius;
m_paramDynColModel1 = 0.0;
m_paramDynColModel2 = 0.0;
m_posOffsetDynColModel = a_offsetPos;
m_rotOffsetDynColModel = a_offsetRot;
}
int main (int argc, char **argv)
{
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
dWorldSetGravity(world,0,0,-0.1);
dMass m; // a parameter for mass
dMassSetZero (&m); // initialize the parameter
//@a sphere
sphere.body = dBodyCreate (world); // create a rigid body
dReal radius = 0.5; // radius [m]
dMassSetSphere (&m,DENSITY,radius); // calculate a mass parameter for a sphere
dBodySetMass (sphere.body,&m); // set the mass parameter to the body
dBodySetPosition (sphere.body,0,1, 1); // set the position of the body
//@a box
box.body = dBodyCreate (world);
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
dBodySetMass (box.body,&m);
dBodySetPosition (box.body,0,2,1);
// a capsule
capsule.body = dBodyCreate (world);
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule.body,&m);
dBodySetPosition (capsule.body,0,4,1);
// a cylinder
cylinder.body = dBodyCreate (world);
dMassSetCylinder(&m,DENSITY,3,radius,length);
dBodySetMass (cylinder.body,&m);
dBodySetPosition (cylinder.body,0,3,1);
// do the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
dWorldDestroy (world); // destroy the world
dCloseODE(); // close ODE
return 0;
}
void ode::newBall(int xp, int yp)
{
dReal z=1,y=1,x=1;
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
void FixBody(dBodyID body,float ext_param,float mass_param)
{
dMass m;
dMassSetSphere(&m,1.f,ext_param);
dMassAdjust(&m,mass_param);
dBodySetMass(body,&m);
dBodySetGravityMode(body,0);
dBodySetLinearVel(body,0,0,0);
dBodySetAngularVel(body,0,0,0);
dBodySetForce(body,0,0,0);
dBodySetTorque(body,0,0,0);
}
WheelItem::WheelItem(dWorldID world,dSpaceID space,dQuaternion q,dReal radius,dReal mass)
{
body = dBodyCreate(world);
geom = dCreateSphere(space,radius);
dBodySetQuaternion(body,q);
dMass m;
dMassSetSphere(&m,1,radius);
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
}
//since this is just the base level construction, we'll just make a sphere (WHOO, EXCITING!!!!)
bool Construction::Construct( char* descriptionFile, DynamicsSolver* solver, Screen3D& Screen,MeshManager& MM, Position& Location, ICollisionHandler* ch )
{
//deconstruct any old stuff
Deconstruct();
//save the solver pointer
mySolver = solver;
CollisionHandler = ch;
//create the body list
ObjectList = new DynamicsObject[1];
this->nObjects = 1;
//Create the geom group
GeomGroup = dSimpleSpaceCreate (solver->GetSpaceID(false)); //dCreateGeomGroup (solver->GetSpaceID(false));
//Create the actual body ( a sphere! )
ObjectList[0].CreateBody( solver );
dBodySetPosition ( ObjectList[0].Body, Location.x, Location.y, Location.z);
dMassSetSphere ( &ObjectList[0].Mass, 1.0, 5.0 );
dMassAdjust (&ObjectList[0].Mass, 1.0 );
dBodySetMass( ObjectList[0].Body, &ObjectList[0].Mass);
ObjectList[0].Geom = dCreateSphere (0,5.0);
dGeomSetData( ObjectList[0].Geom, &ObjectList[0].SurfaceDesc );
dGeomSetBody (ObjectList[0].Geom,ObjectList[0].Body);
dSpaceAdd (GeomGroup,ObjectList[0].Geom);
ObjectList[0].HasGeom = true;
//set owner
for(int i=0; i<nObjects; i++)
{
ObjectList[i].SurfaceDesc.Owner = &ObjectList[i];
ObjectList[i].SurfaceDesc.ParentConstruction = this;
ObjectList[i].Owner = this;
//ObjectList[i].HasGeom = true;
}
LinearDisableEpsilon = .1;
AngularDisableEpsilon = .01f;
//create the mesh for drawing
D3DXCreateSphere( Screen.D3DDevice, 5.5f, 10, 10, &DrawMesh, NULL );
return true;
}
//set mass by radius and density
void ODE_Particle::setMass(dReal density, dReal rad)
{
dMass m;
dMassSetZero(&m);
if (getShapeType() == dSphereClass)
{
dMassSetSphere (&m,density,rad); // calculate a mass for a sphere
dBodySetMass(body,&m);
}
else
{
printf("ERROR in ODE_Particle.cpp: Setting Mass using density for non spherical object");
exit(0);
}
}
void SSphereParts::set(dWorldID w, dSpaceID space)
{
double rad = m_cmpnt.radius();
// konao
DUMP(("[SSphereParts::set] ODE geom created (r=%f) [%s:%d]\n", rad, __FILE__, __LINE__));
dGeomID geom = dCreateSphere(0, rad);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
dMassSetSphere(&m, DENSITY, rad);
//dMassAdjust(&m, 1.0);
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
void Bola::construir(dWorldID world, dSpaceID space)
{
// Cria objeto e geometria
this->corpo = dBodyCreate(world);
this->geometria = dCreateSphere(0,BOLA_RAIO);
// Define a posição do objeto
dBodySetPosition(this->corpo, this->pegarX(), this->pegarY(), STARTZ);
// Define a massa do objeto
dMass m;
dMassSetSphere(&m,1,BOLA_RAIO);
dMassAdjust(&m,BOLA_MASSA);
dBodySetMass(this->corpo,&m);
// Associa o objeto à geometria
dGeomSetBody(this->geometria,this->corpo);
// Cria um espaço para a bola e a adiciona
this->espaco = dSimpleSpaceCreate(space);
dSpaceSetCleanup(this->espaco,0);
dSpaceAdd(this->espaco,this->geometria);
}
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;
}
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCCylinder (0,radius,length);
dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
}
int main (int argc, char **argv)
{
dMass m;
// 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 = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 32);
dCreatePlane (space,0,0,1, 0.0);
cylbody = dBodyCreate (world);
dQuaternion q;
#if 0
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#else
// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0);
dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);
#endif
dBodySetQuaternion (cylbody,q);
dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH);
dGeomSetBody (cylgeom,cylbody);
dBodySetPosition (cylbody, 0, 0, 3);
dSpaceAdd (space, cylgeom);
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,SPHERERADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, SPHERERADIUS);
dGeomSetBody (sphgeom,sphbody);
dBodySetPosition (sphbody, 0, 0, 5.5);
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dGeomDestroy(sphgeom);
dGeomDestroy (cylgeom);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
static void soy_bodies_camera_real_set_mass (soybodiesBody* base, gfloat density) {
soybodiesCamera * self;
gboolean _tmp0_ = FALSE;
gfloat _tmp1_ = 0.0F;
gboolean _tmp3_ = FALSE;
#line 119 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
self = (soybodiesCamera*) base;
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp1_ = density;
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
if (_tmp1_ != ((gfloat) 0)) {
#line 792 "Camera.c"
gfloat _tmp2_ = 0.0F;
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp2_ = soy_bodies_body_volume ((soybodiesBody*) self);
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp0_ = _tmp2_ != 0.0f;
#line 798 "Camera.c"
} else {
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp0_ = FALSE;
#line 802 "Camera.c"
}
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp3_ = _tmp0_;
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
if (_tmp3_) {
#line 808 "Camera.c"
dMass* mass = NULL;
dMass* _tmp4_ = NULL;
dMass* _tmp5_ = NULL;
gfloat _tmp6_ = 0.0F;
GLfloat _tmp7_ = 0.0F;
struct dxBody* _tmp8_ = NULL;
dMass* _tmp9_ = NULL;
struct dxBody* _tmp10_ = NULL;
#line 121 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp4_ = dMassnew ();
#line 121 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
mass = _tmp4_;
#line 122 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp5_ = mass;
#line 122 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp6_ = density;
#line 122 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp7_ = self->priv->_radius;
#line 122 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
dMassSetSphere (_tmp5_, (dReal) _tmp6_, (dReal) _tmp7_);
#line 123 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp8_ = ((soybodiesBody*) self)->body;
#line 123 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp9_ = mass;
#line 123 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
dBodySetMass (_tmp8_, _tmp9_);
#line 124 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp10_ = ((soybodiesBody*) self)->body;
#line 124 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
dBodySetGravityMode (_tmp10_, 1);
#line 120 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_dMassfree0 (mass);
#line 841 "Camera.c"
} else {
struct dxBody* _tmp11_ = NULL;
#line 126 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
_tmp11_ = ((soybodiesBody*) self)->body;
#line 126 "/home/jeff/Documents/libraries/libsoy/src/bodies/Camera.gs"
dBodySetGravityMode (_tmp11_, 0);
#line 848 "Camera.c"
}
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
void resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
{
int i;
dMass m;
// chassis body
body[bodyI] = dBodyCreate (world);
dBodySetPosition (body[bodyI],x,y,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (body[bodyI],&m);
box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxI],body[bodyI]);
// wheel bodies
for (i=1; i<=4; i++) {
body[bodyI+i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[bodyI+i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[bodyI+i],&m);
sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
}
dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[jointI+i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1));
dJointSetHinge2Axis2 (joint[jointI+i],0,1,0);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
}
//center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
dBodyID b = dBodyCreate (world);
dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (b,&m);
dJointID j = dJointCreateFixed(world, 0);
dJointAttach(j, body[bodyI], b);
dJointSetFixed(j);
//box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
//dGeomSetBody (box[boxI+1],b);
bodyI += 5;
jointI += 4;
boxI += 1;
sphereI += 4;
}
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;
}
}
Simulator::Simulator(const double posx, const double posy)
{
m_collision = false;
//Open Dynamics Engine stuff
m_world = dWorldCreate();
m_space = dHashSpaceCreate(0);
m_contacts = dJointGroupCreate(0);
m_ground = dCreatePlane(m_space, 0, 0, 1, 0);
dGeomSetData(m_ground, (void *) &TYPE_TERRAIN);
dWorldSetGravity(m_world, 0, 0, -0.81);
//create robot
const double LENGTH = 2.50; // chassis length 2.50;
const double WIDTH = 1.00; // chassis width
const double HEIGHT = 0.40; // chassis height
const double RADIUS = 0.45 * WIDTH;//0.45 * WIDTH; // wheel radius
const double STARTZ = 0.05;
dMass m;
dQuaternion q;
double car_center_x= posx + 1.5; //1.5
double car_center_y= posy + 5.3;
// chassis body
m_robotBodyChassis = dBodyCreate(m_world);
dBodySetPosition(m_robotBodyChassis, car_center_x, car_center_y, 0.85 * RADIUS + 0.5 * HEIGHT + STARTZ);
dMassSetBox(&m, 1, LENGTH, WIDTH, HEIGHT);
dBodySetMass(m_robotBodyChassis,&m);
// chassis geometry
m_robotGeomChassis = dCreateBox(m_space, LENGTH, WIDTH, HEIGHT);
dGeomSetBody(m_robotGeomChassis, m_robotBodyChassis);
m_robotBodies.push_back(m_robotBodyChassis);
dGeomSetData(m_robotGeomChassis, (void *) &TYPE_ROBOT);
// wheel bodies
for(int i= 0; i < 3; i++)
{
m_robotBodyWheels[i] = dBodyCreate(m_world);
dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5);
dBodySetQuaternion(m_robotBodyWheels[i], q);
dMassSetSphere(&m, 1, RADIUS);
dBodySetMass(m_robotBodyWheels[i], &m);
m_robotGeomWheels[i] = dCreateSphere(m_space, RADIUS);
dGeomSetBody(m_robotGeomWheels[i], m_robotBodyWheels[i]);
m_robotBodies.push_back(m_robotBodyWheels[i]);
dGeomSetData(m_robotGeomWheels[i], (void *) &TYPE_ROBOT);
}
dBodySetPosition(m_robotBodyWheels[0], car_center_x + 0.5 * LENGTH, car_center_y, RADIUS + STARTZ);
dBodySetPosition(m_robotBodyWheels[1], car_center_x - 0.5 * LENGTH, car_center_y + 0.5 * WIDTH, RADIUS + STARTZ);
dBodySetPosition(m_robotBodyWheels[2], car_center_x - 0.5 * LENGTH, car_center_y - 0.5 * WIDTH, RADIUS + STARTZ);
// front and back wheel hinges
for (int i = 0; i < 3; i++)
{
m_robotJoints[i] = dJointCreateHinge2(m_world, 0); // creat hign-2 joint as wheels
dJointAttach(m_robotJoints[i], m_robotBodyChassis, m_robotBodyWheels[i]);
const dReal *a = dBodyGetPosition(m_robotBodyWheels[i]);
dJointSetHinge2Anchor(m_robotJoints[i], a[0], a[1], a[2]);
dJointSetHinge2Axis1(m_robotJoints[i], 0, 0, 1);
dJointSetHinge2Axis2(m_robotJoints[i], 0, 1, 0);
// set joint suspension
dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionERP, 0.04);
dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionCFM, 0.08);
}
// lock back wheels along the steering axis
for (int i = 1; i < 3; i++)
{
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (m_robotJoints[i], dParamLoStop, 0);
dJointSetHinge2Param (m_robotJoints[i], dParamHiStop, 0);
}
}