void RigidBodyEnvironment::createWorld(void)
{
// BEGIN SETTING UP AN OPENDE ENVIRONMENT
// ***********************************
bodyWorld = dWorldCreate();
space = dHashSpaceCreate(0);
dWorldSetGravity(bodyWorld, 0, 0, -0.981);
double lx = 0.2;
double ly = 0.2;
double lz = 0.1;
dMassSetBox(&m, 1, lx, ly, lz);
boxGeom = dCreateBox(space, lx, ly, lz);
boxBody = dBodyCreate(bodyWorld);
dBodySetMass(boxBody, &m);
dGeomSetBody(boxGeom, boxBody);
// *********************************
// END SETTING UP AN OPENDE ENVIRONMENT
setPlanningParameters();
}
void createInvisibleHead( float* pos )
{
dMatrix3 head_orientation;
dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);
//position and orientation
head.Body = dBodyCreate(World);
dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
dBodySetRotation(head.Body, head_orientation);
dBodySetLinearVel(head.Body, 0, 0, 0);
dBodySetData(head.Body, (void *)0);
//mass
dMass head_mass;
dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
dBodySetMass(head.Body, &head_mass);
//geometry
head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
dGeomSetBody(head.Geom, head.Body);
//fixed joint
invis_box_joint = dJointCreateFixed(World, jointgroup);
dJointAttach(invis_box_joint, body.Body, head.Body);
dJointSetFixed(invis_box_joint);
}
void TrackedVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
//dMassAdjust(&this->vehicleMass, 2.40);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);
this->leftTrack->create();
this->rightTrack->create();
dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
dRigidBodyArraySetPosition(leftTrack->bodyArray, -wheelBase/2, -(w - trackWidth)/2, 0);
dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2, (w - trackWidth)/2, 0);
this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
dJointSetFixed(this->leftTrackJoint);
dJointSetFixed(this->rightTrackJoint);
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}
void Robots::construirChassi(dWorldID world)
{
for (int i=0; i < 2; i++)
{
// Cria objeto e geometria
this->body[i] = dBodyCreate(world);
this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);
// Define a posição do objeto
dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);
// Se o robô for do segundo time, deve ser rotacionado em 180 graus
if ((this->id == 3) || (this->id == 4) || (this->id == 5))
{
dQuaternion q;
dQFromAxisAndAngle(q,0,0,1,M_PI);
dBodySetQuaternion(this->body[i],q);
}
// Define a massa do objeto
dMass m;
dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT); // O segundo bloco é mais curto
dMassAdjust(&m,CMASS*(1+i*2)); // O segundo bloco é mais pesado
dBodySetMass(this->body[i],&m);
// Associa o objeto à sua geometria
dGeomSetBody(this->box[i],this->body[i]);
}
// O chassis é composto por dois blocos que são fixos entre si
dJointID fixed = dJointCreateFixed(world,0);
dJointAttach(fixed,this->body[1],this->body[0]);
dJointSetFixed(fixed);
}
void PhysicsActor::postLoad(){
dMass m;
if (type==CUBESHAPE)
{
geom = dCreateBox(space,shape.x,shape.y,shape.z);
dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z);
}
if (type==CAPSULESHAPE)
{
geom = dCreateCapsule(space,shape.x,shape.y);
dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y); //the '3' means align on z-axis and density is shape.z
generateCapsuleList();
}
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
//initialise position
if (base){
Matrix4f bGlobal= base->baseMatrix * renderer->inverseCameraMatrix;
dBodySetPosition(body,bGlobal.data[12] + originalMatrix.data[12] + transformMatrix.data[12],bGlobal.data[13] + originalMatrix.data[13] + transformMatrix.data[13],bGlobal.data[14] + originalMatrix.data[14] + transformMatrix.data[14]);
}else{
dBodySetPosition(body,originalMatrix.data[12] + transformMatrix.data[12],originalMatrix.data[13] + transformMatrix.data[13],originalMatrix.data[14] + transformMatrix.data[14]);
}
dBodySetDamping(body, linearDamp, angleDamp);
bInit=true;
}
GameObject::GameObject(GameWorld& gw, const ObjectPrototype& proto, double x, double y, double z, int id) :
m_meshName(proto.m_meshName), m_sceneEntity(nullptr), m_sceneNode(nullptr), m_lockRotation(proto.m_lockRotation), m_isKinematic(proto.m_isKinematic),
m_maxTurn(proto.m_maxTurnAngle), m_maxForward(proto.m_maxForward), m_maxBackward(proto.m_maxBackward),
m_hitPoints(proto.m_maxHitPoints), m_collisionAccum(0), m_totalDamage(0), m_hasAgent(proto.m_hasAgent), m_gw(gw), m_id(id), m_oldGS(nullptr), m_newGS(nullptr)
{
m_sceneEntity = gw.GetScene()->createEntity(gw.GetMesh(m_meshName));
m_sceneNode = gw.GetScene()->getRootSceneNode()->createChildSceneNode();
m_sceneNode->attachObject(m_sceneEntity);
Ogre::Vector3 size = m_sceneEntity->getBoundingBox().getSize();
m_body = dBodyCreate(gw.GetPhysicsWorld());
m_geom = dCreateBox(gw.GetPhysicsSpace(), size.x, size.y, size.z);
dMassSetBox(&m_mass, proto.m_density, size.x, size.y, size.z);
dBodySetMass(m_body, &m_mass);
dGeomSetBody(m_geom, m_body);
dBodySetPosition(m_body, x, y, z);
// automagically disable things when the body is still for long enough
dBodySetAutoDisableFlag(m_body, 1);
dBodySetAutoDisableLinearThreshold(m_body, 0.1f);
dBodySetAutoDisableAngularThreshold(m_body, 0.25f);
dBodySetAutoDisableSteps(m_body, 1);
// improve simulation accuracy
dBodySetDamping(m_body, 0.0f, 0.1f);
if (proto.m_registerCollisions) {
gw.RegisterForCollisions(this);
}
if (proto.m_isKinematic) {
dBodySetKinematic(m_body);
}
}
void CProtoHapticDoc::UpdateDynamics()
{
for(int i= 0; i<m_shapeCount; i++) {
dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(),
m_shapes[i]->getSizeY(),
m_shapes[i]->getSizeZ());
dGeomSetPosition (m_geoms[i], m_shapes[i]->getLocationX(),
m_shapes[i]->getLocationY(),
m_shapes[i]->getLocationZ());
dGeomSetRotation (m_geoms[i], dBodyGetRotation(bodies[i]));
dBodySetPosition (bodies[i], m_shapes[i]->getLocationX(),
m_shapes[i]->getLocationY(),
m_shapes[i]->getLocationZ());
float *rotation= m_shapes[i]->getRotation();
const dReal rot[12]=
{ rotation[0], rotation[4], rotation[8], rotation[12],
rotation[1], rotation[5], rotation[9], rotation[13],
rotation[2], rotation[6], rotation[10], rotation[14] };
dBodySetRotation (bodies[i], rot);
dMass mass;
dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(),
m_shapes[i]->getSizeY(),
m_shapes[i]->getSizeZ());
dBodySetMass (bodies[i], &mass);
}
}
void TSRODERigidBody::AddBoxGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, const TSRVector3& _vExtents, 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 = dCreateBox( 0, _vExtents.x, _vExtents.y, _vExtents.z );
dMassSetBox( &currMass, _fDensity, _vExtents.x,_vExtents.y,_vExtents.z);
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 constructWorldForTest (dReal gravity, int bodycount,
/* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z,
/* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z,
/* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z,
/* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z,
/* rotation angles */ dReal a1, dReal a2)
{
// create world
world = dWorldCreate();
dWorldSetERP (world,0.2);
dWorldSetCFM (world,1e-6);
dWorldSetGravity (world,0,0,gravity);
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0], pos1x, pos1y, pos1z);
dQuaternion q;
dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1);
dBodySetQuaternion (body[0],q);
if (bodycount==2) {
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1], pos2x, pos2y, pos2z);
dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2);
dBodySetQuaternion (body[1],q);
}
else body[1] = 0;
}
void CRigidBox::setDensity(F32 density){
if(!mBodyID)
return;
dMass m;
dMassSetBox(&m, TO_WORLD(density), TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y), TO_PHYSICS(mDimentions.z));
dBodySetMass(mBodyID, &m);
}
void CBoxGeom::get_mass(dMass& m)
{
Fvector& hside=m_box.m_halfsize;
dMassSetBox(&m,1.f,hside.x*2.f,hside.y*2.f,hside.z*2.f);
dMatrix3 DMatx;
PHDynamicData::FMX33toDMX(m_box.m_rotate,DMatx);
dMassRotate(&m,DMatx);
}
//===========================================================================
void cODEGenericBody::createDynamicBox(const double a_lengthX,
const double a_lengthY,
const double a_lengthZ,
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 box
m_ode_geom = dCreateBox(m_ODEWorld->m_ode_space, a_lengthX, a_lengthY, a_lengthZ);
// 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)
{
dMassSetBox(&m_ode_mass, 1.0, a_lengthX, a_lengthY, a_lengthZ);
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 = a_lengthX;
m_paramDynColModel1 = a_lengthY;
m_paramDynColModel2 = a_lengthZ;
m_posOffsetDynColModel = a_offsetPos;
m_rotOffsetDynColModel = a_offsetRot;
}
void OscPrismODE::on_density()
{
ODEObject *ode_object = static_cast<ODEObject*>(special());
dMassSetBox(&ode_object->mass(), m_density.m_value,
m_size.x, m_size.y, m_size.z);
dBodySetMass(ode_object->body(), &ode_object->mass());
m_mass.m_value = ode_object->mass().mass;
}
void Plane::setMass(double mass, bool density){
if(body){
dMass m;
dMassSetBox(&m,mass,1000,1000,0.01); // fake the mass of the plane with a thin box
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
void SBoxParts::set(dWorldID w, dSpaceID space)
{
Size &sz = m_cmpnt.size();
/*
const dReal hx = sz.x();
const dReal hy = sz.y();
const dReal hz = sz.z();
*/
dReal hx = sz.x();
dReal hy = sz.y();
dReal hz = sz.z();
// konao
DUMP(("[SBoxParts::set] ODE geom created (hx, hy, hz)=(%f, %f, %f) [%s:%d]\n", hx, hy, hz, __FILE__, __LINE__));
if (hz == 0) hz = 0.001;
if (hy == 0) hy = 0.001;
if (hx == 0) hx = 0.001;
dGeomID geom = dCreateBox(0, hx, hy, hz);
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);
// x-axis and z-axis is swapped between ODE/SIGVerse
dMassSetBox(&m, DENSITY, hz, hy, hx); //TODO: mass of cube should be configurable
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
// Gap between ODE shape and body
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz);
// Initial orientation
dReal offq[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()};
dGeomSetOffsetQuaternion(geom, offq);
//dMassAdjust(&m, 1.0);
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
void Box::setMass(double mass, bool density){
if(body){
dMass m;
osg::Vec3 dim = osgbox->getDim();
dMassSetBox(&m, mass, dim.x() , dim.y() , dim.z());
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
IoObject *IoODEMass_setBoxDensity(IoODEMass *self, IoObject *locals, IoMessage *m)
{
const double density = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double lx = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double ly = IoMessage_locals_doubleArgAt_(m, locals, 2);
const double lz = IoMessage_locals_doubleArgAt_(m, locals, 3);
dMassSetBox(DATA(self), density, lx, ly, lz);
return self;
}
void LinkBoxDesign::create(dWorldID world, dSpaceID space, dGeomID * geom,
dBodyID * body)
{
body[0] = dBodyCreate(world);
geom[0] = dCreateBox(space, sides[XX], sides[YY], sides[ZZ]);
dGeomSetBody(geom[0], body[0]);
dMass m;
dMassSetBox(&m, 1, sides[XX], sides[YY], sides[ZZ]);
dMassAdjust(&m, LINK_MASS);
dBodySetMass(body[0], &m);
}
void cubeD_D::SetSize(float x, float y, float z) {
h_m = x;
w_m = y;
d_m = z;
dMass mass;
dMassSetBox(&mass, density, h_m, w_m, d_m);
dBodySetMass(boxBody_m, &mass);
dGeomBoxSetLengths(boxGeom_m, h_m, w_m, d_m);
}
int main (int argc, char **argv)
{
int i;
dReal k;
dMass m;
/* setup pointers to drawstuff callback functions */
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
/* create world */
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);
dWorldSetGravity (world,0,0,-0.5);
dCreatePlane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i] = dBodyCreate (world);
k = i*SIDE;
dBodySetPosition (body[i],k,k,k+0.4);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dBodySetMass (body[i],&m);
sphere[i] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[i],body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[i],body[i+1]);
k = (i+0.5)*SIDE;
dJointSetBallAnchor (joint[i],k,k,k+0.4);
}
/* run simulation */
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
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 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;
}
void LaserBeam::embody(dBodyID myBodySelf)
{
dMass m;
float myMass = 1.0f;
dBodySetPosition(myBodySelf, pos[0], pos[1], pos[2]);
dMassSetBox(&m, 1,Gunshot::width, Gunshot::height, Gunshot::length);
dMassAdjust(&m, myMass*1.0f);
dBodySetMass(myBodySelf,&m);
me = myBodySelf;
}
//set mass by sides and density for box objects
void ODE_Particle::setMass(dReal density, dReal side1,dReal side2,dReal side3)
{
dMass m;
dMassSetZero(&m);
if (getShapeType() == dBoxClass)
{
dMassSetBox (&m,density,side1,side2,side3);
dBodySetMass(body,&m);
}
else
{
printf("ERROR in ODE_Particle.cpp: Setting Mass using density for non box object");
exit(0);
}
}
void Balaenidae::embody(dBodyID myBodySelf)
{
dMass m;
float myMass = 250.0f;
float radius = 2.64f;
float length = 7.0f;
dBodySetPosition(myBodySelf, pos[0], pos[1], pos[2]);
//dMassSetBox(&m,1,1.0f, 4, 5.0f);
dMassSetBox(&m, 1,100.0f, 40, 500.0f);
//dMassSetSphere(&m,1,radius);
dMassAdjust(&m, myMass*1.0f);
dBodySetMass(myBodySelf,&m);
me = myBodySelf;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dQuaternion q;
dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
dBodySetQuaternion (body[0],q);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
dBodySetQuaternion (body[1],q);
hinge = dJointCreateHinge (world,0);
dJointAttach (hinge,body[0],body[1]);
dJointSetHingeAnchor (hinge,0,0,1);
dJointSetHingeAxis (hinge,1,-1,1.41421356);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 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;
}
}
/*
=================================================================================
createUniversalLeg
Use parameters to create leg body/geom and attach to body with universal joint
**Warning**
mass is not set
=================================================================================
*/
void createUniversalSquareLeg(ODEObject &leg,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal sides[3],
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;
dMassSetBox(&legMass, .5, sides[0], sides[1], sides[2]);
dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateBox(Space, sides[0], sides[1], sides[2]);
dGeomSetBody(leg.Geom, leg.Body);
//universal joint
joint = dJointCreateUniversal(World, jointgroup);
//attach and anchor
dJointAttach(joint, body.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);
}
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);
};
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
