/**
* \brief Destroyes a body from a node:
*
* This function checks if not more than one geom is connected to the
* body and destroyes the body in that case. In the other case the counter
* of the connected geoms is decreased.
*
* pre:
* - the body exists in the physical world
*
* post:
* - if more than one geoms are connected to the body, decrease the
* counter of connected geoms
* - otherwise, if only one geom is connected to the body, destroy the body
*/
void WorldPhysics::destroyBody(dBodyID theBody, NodePhysics* node) {
std::vector<body_nbr_tupel>::iterator iter;
std::vector<NodePhysics*>::iterator jter;
for(iter = comp_body_list.begin(); iter != comp_body_list.end(); iter++) {
if((*iter).body == theBody) {
if((*iter).connected_geoms > 1) {
(*iter).connected_geoms--;
for(jter = (*iter).comp_nodes.begin();
jter != (*iter).comp_nodes.end(); jter++) {
if((*jter) == node) {
(*iter).comp_nodes.erase(jter);
break;
}
}
resetCompositeMass(theBody);
return;
}
else {
dBodyDestroy(theBody);
comp_body_list.erase(iter);
return;
}
}
}
// if we get here in the code, the body is not in the list and can
// be removed from the world
dBodyDestroy(theBody);
}
void SkidSteeringVehicle::destroy() {
dBodyDestroy(this->vehicleBody);
dGeomDestroy(this->vehicleGeom);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dBodyDestroy(this->wheelBody[fr][lr]);
dGeomDestroy(this->wheelGeom[fr][lr]);
dJointDestroy(this->wheelJoint[fr][lr]);
}
}
dRigidBodyArrayDestroy(this->bodyArray);
}
void Machine::destroy(void)
{
int i;
dBodyDestroy(body[0]);
dBodyDestroy(body[1]);
dGeomDestroy(geom[0]);
dJointDestroy(joint);
for(i=0; i<2; i++)
dGeomDestroy(geom[i+2]);
for(i=0; i<3; i++)
{
dBodyDestroy(wheel[i]);
dGeomDestroy(sphere[i]);
}
}
void cPhysicsObject::DestroyBody()
{
if (body != NULL) {
dBodyDestroy(body);
body = NULL;
}
}
void YGEBodyAsset::setParent(YGETimeSpace::YGEEntity* entity){
parent = entity;
if(hasBody){
dBodyDestroy(bodyId);
hasBody = false;
}
}
void stop()
{
dGeomDestroy(mesh_geom);
dGeomTriMeshDataDestroy(mesh_data);
dBodyDestroy(ball1_body);
dBodyDestroy(ball2_body);
dGeomDestroy(ground);
dJointGroupDestroy(contact_group);
dSpaceDestroy(space); // will destroy all geoms
dWorldDestroy(world);
}
LaserBeam::~LaserBeam()
{
dGeomDestroy(geom);
dBodyDestroy(me);
printf("Good bye....\n");
}
/**
* This method is called if the joint should be detached. It stores
* the actual Joint-axis, which can change their orientation at
* Hinge2Joints, and destroys the helper body and joint.
**/
void Hinge2Joint::detachJoint() {
if (active && joint)
{
TransformationData entityTrans;
dVector3 vec;
dJointGetHinge2Axis1(joint, vec);
axis1[0] = vec[0];
axis1[1] = vec[1];
axis1[2] = vec[2];
dJointGetHinge2Axis2(joint, vec);
axis2[0] = vec[0];
axis2[1] = vec[1];
axis2[2] = vec[2];
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
gmtl::Quatf entityRot;
gmtl::Vec3f scaleVec;
gmtl::AxisAnglef axAng;
// get the inverse scale values of the mainEntity
/* scaleVec[0] = 1.0f/mainEntity->getXScale();
scaleVec[1] = 1.0f/mainEntity->getYScale();
scaleVec[2] = 1.0f/mainEntity->getZScale();*/
scaleVec[0] = 1.0f/entityTrans.scale[0];
scaleVec[1] = 1.0f/entityTrans.scale[1];
scaleVec[2] = 1.0f/entityTrans.scale[2];
// get the inverse Rotation of the mainEntity
// axAng[0] = -mainEntity->getRotAngle();
// axAng[1] = mainEntity->getXRot();
// axAng[2] = mainEntity->getYRot();
// axAng[3] = mainEntity->getZRot();
// gmtl::set(entityRot, axAng);
entityRot = entityTrans.orientation;
gmtl::invert(entityRot);
axis1 *= entityRot;
axis2 *= entityRot;
axis1[0] *= scaleVec[0];
axis1[1] *= scaleVec[1];
axis1[2] *= scaleVec[2];
axis2[0] *= scaleVec[0];
axis2[1] *= scaleVec[1];
axis2[2] *= scaleVec[2];
gmtl::normalize(axis1);
gmtl::normalize(axis2);
} // if
} // if
if (usedHelperJoint)
{
dJointDestroy(helperJoint);
dBodyDestroy(helperBody);
usedHelperJoint = false;
} // if
} // detachJoint
MyODEGeom::~MyODEGeom() {
if (mBody) {
dBodyDestroy(mBody);
}
if (mODEGeom) {
dGeomDestroy(mODEGeom);
}
}
void PhysicsBody::onDestroy()
{
if(_BodyID)
{
dBodyDestroy(_BodyID);
_BodyID =0;
}
}
ODE_Particle::~ODE_Particle()
{
/*printf("Particle=%u destructor!\n",id);
fflush(stdout);*/
dBodyDestroy(body);
dGeomDestroy(geom);
}
Primitive::~Primitive () {
QMP_CRITICAL(8);
// 20091023; guettler:
// hack for tasked simulations; there are some problems if running in parallel mode,
// if you do not destroy the geom, everything is fine (should be no problem because world is destroying geoms too)
if(destroyGeom && geom) dGeomDestroy( geom );
if(body && ((mode & _Transform) == 0) ) dBodyDestroy( body );
QMP_END_CRITICAL(8);
}
void BoxObstacle::remove() {
dGeomDestroy(boxGeom_);
if (box_ != 0) {
for(int i=0; i< dBodyGetNumJoints(box_); i++) {
dJointDestroy(dBodyGetJoint(box_, i));
}
dBodyDestroy(box_);
}
}
void IoODEBody_free(IoODEBody *self)
{
if(BODYID && WORLD)
{
IoODEWorld_removeBody(WORLD, self);
dBodyDestroy(BODYID);
}
free(IoObject_dataPointer(self));
}
PhysicsActor::~PhysicsActor(){
if (joint>0)
dJointDestroy(joint);
if (geom>0)
dGeomDestroy(geom);
if (body>0)
dBodyDestroy(body);
}
dBodyID set_phys_body_type(dBodyID body, int b)
{
if (body)
dBodyDestroy(body);
if (b)
return dBodyCreate(world);
return 0;
}
void Wheel::reset() {
dSpaceID space = dGeomGetSpace(ph.geom);
dGeomDestroy(ph.geom);
dBodyDestroy(ph.body);
Utils::Xml x(cst.xmlFile, "wheel");
createPhysics(x, space);
disposePhysics(x);
}
TSRODERigidBody::~TSRODERigidBody()
{
if ( m_BodyID )
{
dBodyDestroy( m_BodyID );
}
for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
{
dGeomDestroy( m_GeomIDs[ i ] );
}
}
void TrackedVehicle::destroy() {
this->leftTrack->destroy();
dJointDestroy(this->leftTrackJoint);
this->rightTrack->destroy();
dJointDestroy(this->rightTrackJoint);
dBodyDestroy(this->vehicleBody);
dGeomDestroy(this->vehicleGeom);
dRigidBodyArrayDestroy(this->bodyArray);
}
void CPHActivationShape:: Destroy ()
{
VERIFY(m_geom&&m_body) ;
spatial_unregister () ;
CPHObject::deactivate () ;
dGeomDestroyUserData (m_geom) ;
dGeomDestroy (m_geom) ;
m_geom =NULL ;
dBodyDestroy (m_body) ;
m_body =NULL ;
}
SceneObj::~SceneObj( void )
{
if( m_dgeom )
dGeomDestroy( m_dgeom );
if( m_dbody )
dBodyDestroy( m_dbody );
if( m_prev_p ) m_prev_p->m_next_p = m_next_p;
if( m_next_p ) m_next_p->m_prev_p = m_prev_p;
if( m_scene_p->m_obj_p==this )
m_scene_p->m_obj_p = m_next_p;
}
void ODE_Link::destroy()
{
if(jointType!=FREE_JOINT)
dJointDestroy(odeJointId);
for(int i=0; i<geomIds.size(); i++)
dGeomDestroy(geomIds.at(i));
if(triMeshDataId)
dGeomTriMeshDataDestroy(triMeshDataId);
dBodyDestroy(bodyId);
ODE_Link* link = static_cast<ODE_Link*>(child);
while(link){
ODE_Link* linkToDelete = (ODE_Link*)link;
link = static_cast<ODE_Link*>(link->sibling);
linkToDelete->destroy();
}
}
void CPhysicManager::DelNode(INode* node){
std::vector<CPhysicObject*>::iterator _it=PhysicObjectList.begin();
for (int i=PhysicObjectList.size();i>0;i--){
CPhysicObject* my = (*_it);
if (my->node==node) {
dBodyDestroy(my->myBody);
dGeomDestroy(my->myGeom);
my->Release();
PhysicObjectList.erase(_it);
break;
}
_it++;
};
};
CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
CBoxEntity& c_box) :
CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
m_cBoxEntity(c_box),
m_sGeomData(GEOM_NORMAL) {
/* Check whether the box is movable or not */
if(c_box.GetEmbodiedEntity().IsMovable()) {
/* Movable box */
/* Set the body to its initial position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Create the geometry and the mass */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Set Geom gripping properties. */
m_sGeomData.Type = GEOM_GRIPPABLE;
dGeomSetData(m_tGeom, &m_sGeomData);
/* Create its mass */
dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Associate the body to the geom */
dGeomSetBody(m_tGeom, m_tBody);
/* Set the parent body total mass */
dBodySetMass(m_tBody, &m_tMass);
}
else {
/* Unmovable box, get rid of the body and add only the geometry */
dBodyDestroy(m_tBody);
/* Create the geometry */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
dGeomSetData(m_tGeom, &m_sGeomData);
/* Set the geom to its position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dGeomSetQuaternion(m_tGeom, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Associate the geom to null body (this makes it static) */
dGeomSetBody(m_tGeom, 0);
}
}
void YGEBodyAsset::createBody(){
YGETimeSpace::YGESpace* parentSpace = parent->getSpace();
if(parentSpace != NULL && parent->getHasAbsPosition()){
if(hasBody) {
dBodyDestroy(bodyId);
dGeomDestroy(geomId);
}
bodyId = dBodyCreate(parentSpace->getWorldId());
mass = new dMass();
dMassSetBox(mass,1,1,1,1);
dMassAdjust(mass,bodyMass);
dBodySetMass(bodyId, mass);
dBodySetAutoDisableFlag(bodyId, 0);
YGEMath::Vector3 pos = parent->getAbsPosition();
YGEMath::Quaternion rot = parent->getAbsOrientation();
dBodySetPosition(bodyId,pos.x,pos.y,pos.z);
dQuaternion q;
q[0]=rot.w;
q[1]=rot.x;
q[2]=rot.y;
q[3]=rot.z;
dBodySetQuaternion(bodyId, q);
//dBodyAddForce(bodyId, 0.5, 0, 0);
// a hull, remove this
geomId = dCreateBox(parentSpace->getDSpaceId(), bodySize.x, bodySize.y, bodySize.z);
dGeomSetData(geomId, this);
dGeomSetCategoryBits(geomId, YGEPhysics::ENTITIES );
dGeomSetCollideBits(geomId, YGEPhysics::ENTITIES | YGEPhysics::STATIC_OBJECTS );
dGeomSetBody(geomId, bodyId);
// dBodySetAuto
hasBody = true;
}
}
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--;
}
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;
}
}
~Card()
{
dBodyDestroy(body);
dGeomDestroy(geom);
}
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 == '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;
}
}
