void PhysicsBody::adjustMass( Real32 newMass )
{
dMass mass;
getMassStruct(mass);
dMassAdjust(&mass, newMass);
setMassStruct(mass);
}
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 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;
}
void CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16 flags)
{
VERIFY(ref_obj);
R_ASSERT(_valid( start_pos ) );
R_ASSERT( _valid( start_size ) );
m_body = dBodyCreate (0) ;
dMass m;
dMassSetSphere(&m,1.f,100000.f);
dMassAdjust(&m,1.f);
dBodySetMass(m_body,&m);
switch(_type)
{
case etBox:
m_geom = dCreateBox (0,start_size.x,start_size.y,start_size.z) ;
break;
case etSphere:
m_geom = dCreateSphere (0,start_size.x);
break;
};
dGeomCreateUserData (m_geom) ;
dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback) ;
dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj) ;
dGeomSetBody (m_geom,m_body) ;
dBodySetPosition (m_body,start_pos.x,start_pos.y,start_pos.z) ;
Island() .AddBody (m_body) ;
dBodyEnable (m_body) ;
m_safe_state .create(m_body) ;
spatial_register () ;
m_flags.set(flags,TRUE);
}
void dMassSetTrimeshTotal( dMass *m, dReal total_mass, dGeomID g)
{
dAASSERT( m );
dUASSERT( g && g->type == dTriMeshClass, "argument not a trimesh" );
dMassSetTrimesh( m, 1.0, g );
dMassAdjust( m, total_mass );
}
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 Cylinder::computeMassProperties(dMass& m)
{
if(capped)
dMassSetCapsule(&m, 1, 3, dReal(height), dReal(height));
else
dMassSetCylinder(&m, 1, 3, dReal(height), dReal(height));
dMassAdjust(&m, dReal(mass));
}
void PCylinder::setMass(float mass)
{
m_mass = mass;
dMass m;
dMassSetCylinder (&m,1,1,m_radius,m_length);
dMassAdjust (&m,m_mass);
dBodySetMass (body,&m);
}
void CODEGeom::get_mass(dMass& m,const Fvector& ref_point, float density)
{
get_mass(m);
dMassAdjust(&m,density*volume());
Fvector l;
l.sub(local_center(),ref_point);
dMassTranslate(&m,l.x,l.y,l.z);
}
void VStateSphereMass::Apply()
{
VState::Parent pParent = GetParent();
dMassSetSphere (&m_Mass, GetDensity(), m_fRadius);
dMassAdjust (&m_Mass, GetMass());
dBodySetMass (pParent->GetBodyID(),&m_Mass);
}
void Sphere::setMass(double mass, bool density){
if(body){
dMass m;
dMassSetSphere(&m, mass, osgsphere->getRadius());
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
void Capsule::setMass(double mass, bool density){
if(mass){
dMass m;
dMassSetCapsule(&m, mass, 3 , osgcapsule->getRadius(), osgcapsule->getHeight());
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
SphereGeom::SphereGeom(Body* in_pBody, Space* in_pSpace, double radius)
: Geom(in_pSpace)
{
m_pBody = in_pBody;
dMassSetSphere(&m_mass,1,radius);
dMassAdjust(&m_mass,1);
m_id = dCreateSphere(m_pSpace->id(),radius);
finishGeom();
}
void 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 Cylinder::setMass(double mass, bool density){
if(body){
dMass m;
dMassSetCylinder(&m, mass, 3 , osgcylinder->getRadius(), osgcylinder->getHeight());
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
//===========================================================================
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 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 CPHCapture::CreateBody()
{
m_body= dBodyCreate(0);
m_island.AddBody(m_body);
dMass m;
dMassSetSphere(&m,1.f,1000000.f);
dMassAdjust(&m,100000.f);
dBodySetMass(m_body,&m);
dBodySetGravityMode(m_body,0);
}
void ode::newLine(float xp, float yp)
{
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
//dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
lineStrips[lines] = dCreateSphere (space,RADIUS);
dGeomSetBody (lineStrips[lines++],b);
}
void Mesh::setMass(double mass, bool density){
if(body){
// we should use the bouding box here
dMass m;
dMassSetSphere(&m, mass, osgmesh->getRadius()); // we use a sphere
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
void 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
}
}
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 SParts::setMass(double mass)
{
m_mass = mass;
if(m_odeobj != NULL) {
dMass m;
dBodyGetMass(m_odeobj->body(), &m);
dMassAdjust(&m, m_mass);
dBodyID body = m_odeobj->body();
dBodySetMass(body, &m);
}
}
void FixBody(dBodyID body,float ext_param,float mass_param)
{
dMass m;
dMassSetSphere(&m,1.f,ext_param);
dMassAdjust(&m,mass_param);
dBodySetMass(body,&m);
dBodySetGravityMode(body,0);
dBodySetLinearVel(body,0,0,0);
dBodySetAngularVel(body,0,0,0);
dBodySetForce(body,0,0,0);
dBodySetTorque(body,0,0,0);
}
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;
}
void ode::newBall(int xp, int yp)
{
dReal z=1,y=1,x=1;
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
//===========================================================================
void cODEGenericBody::setMass(double a_mass)
{
// check if ODE body defined
if (m_ode_body == NULL) { return; }
// store value
m_mass = a_mass;
// adjust mass
dMassAdjust(&m_ode_mass, a_mass);
dBodySetMass(m_ode_body,&m_ode_mass);
}
//===========================================================================
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;
}
WheelItem::WheelItem(dWorldID world,dSpaceID space,dQuaternion q,dReal radius,dReal mass)
{
body = dBodyCreate(world);
geom = dCreateSphere(space,radius);
dBodySetQuaternion(body,q);
dMass m;
dMassSetSphere(&m,1,radius);
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
}
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;
}
