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