// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
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;
}
WheelPiece::WheelPiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
dBodyCreate(world);
body = dBodyCreate(world);
radius = .35;
thickness = .25;
activationDirection = 0;
geom = dCreateCylinder(space, radius, thickness);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setCylinder(.5, 1, radius, thickness);
dBodySetMass(body, &mass);
const dMatrix3 rotationMatrix = {1,0,0,0,
0,1,0,0,
0,0,1,0
};
dBodySetRotation(body, rotationMatrix);
dBodySetPosition(body,x,y,z);
attachmentOffset = thickness + .25;
color[0] = .5;
color[1] = 1;
color[2] = 1;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.stop = 0;
fn.command = 0;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE ();
// create world
world = dWorldCreate ();
space = dHashSpaceCreate (0);
dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// create object
sphere0 = dBodyCreate (world);
sphere0_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere0,&m);
dGeomSetBody (sphere0_geom,sphere0);
sphere1 = dBodyCreate (world);
sphere1_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere1,&m);
dGeomSetBody (sphere1_geom,sphere1);
sphere2 = dBodyCreate (world);
sphere2_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere2,&m);
dGeomSetBody (sphere2_geom,sphere2);
// set initial position
dBodySetPosition (sphere0,0,0,4);
dBodySetPosition (sphere1,5,0,4);
dBodySetPosition (sphere2,-2,0,4);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// clean up
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
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 cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot)
{
math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight);
rotation.LoadIdentity();
rotation.SetFromAngles(math::DegreesToRadians(rot));
const math::cMat4 m = rotation.GetMatrix();
dMatrix3 r;
r[0] = m[0]; r[1] = m[4]; r[2] = m[8]; r[3] = 0;
r[4] = m[1]; r[5] = m[5]; r[6] = m[9]; r[7] = 0;
r[8] = m[2]; r[9] = m[6]; r[10] = m[10]; r[11] = 0;
position = pos;
dGeomSetPosition(geom, position.x, position.y, position.z);
dGeomSetRotation(geom, r);
if (bBody) {
body = dBodyCreate(pWorld->GetWorld());
dBodySetPosition(body, position.x, position.y, position.z);
dBodySetRotation(body, r);
dBodySetAutoDisableFlag(body, 1);
dGeomSetBody(geom, body);
pWorld->AddPhysicsObject(shared_from_this());
}
}
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 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 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);
}
/*
=================================================================================
createFixedLeg
Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius,
dReal length)
{
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);
//fixed joint
joint = dJointCreateFixed(World, jointgroup);
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetFixed(joint);
}
void Balaenidae::embody(dWorldID world, dSpaceID space)
{
me = dBodyCreate(world);
embody(me);
geom = dCreateBox( space, 100.0f, 40, 500.0f); // scale 50
dGeomSetBody(geom, me);
}
CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
body = dBodyCreate(world);
geom = dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]);
dBodySetMass(body, &mass);
const dMatrix3 rotationMatrix = {1,0,0,0,
0,1,0,0,
0,0,1,0};
dBodySetRotation(body, rotationMatrix);
dBodySetPosition(body,x,y,z);
for(int i = 0 ; i < 6 ; i++)
attachedPieces[i] = NULL; // initialize attached piece array to all null pointers
color[0] = 1;
color[1] = 1;
color[2] = 1;
}
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);
}
int main(int argc, char** argv)
{
init_gfx();
init_2d();
world = dWorldCreate();
dBodyID ball = dBodyCreate(world);
dBodySetPosition(ball, 0, 0, 0);
for (;;) {
Uint8* chars = SDL_GetKeyState(NULL);
if (chars[SDLK_LEFT]) {
dBodyAddForce(ball, -force, 0, 0);
}
if (chars[SDLK_RIGHT]) {
dBodyAddForce(ball, +force, 0, 0);
}
if (chars[SDLK_UP]) {
dBodyAddForce(ball, 0, +force, 0);
}
if (chars[SDLK_DOWN]) {
dBodyAddForce(ball, 0, -force, 0);
}
const dReal* pos = dBodyGetPosition(ball);
glTranslatef(pos[0], pos[1], pos[2]);
draw_circle();
dWorldQuickStep(world, 0.01);
step();
}
}
void ODERigidObject::Create(dWorldID worldID,dSpaceID space,bool useBoundaryLayer)
{
Clear();
spaceID = space;
bodyID = dBodyCreate(worldID);
dMass mass;
mass.mass = obj.mass;
//NOTE: in ODE, COM must be zero vector! -- we take care of this by shifting geometry
//CopyVector3(mass.c,obj.com);
mass.c[0] = mass.c[1] = mass.c[2] = 0; mass.c[3] = 1.0;
CopyMatrix(mass.I,obj.inertia);
int res=dMassCheck(&mass);
if(res != 1) {
fprintf(stderr,"Uh... mass is not considered to be valid by ODE?\n");
std::cerr<<"Inertia: "<<obj.inertia<<std::endl;
}
dBodySetMass(bodyID,&mass);
geometry = new ODEGeometry;
geometry->Create(&obj.geometry,spaceID,-obj.com,useBoundaryLayer);
dGeomSetBody(geometry->geom(),bodyID);
dGeomSetData(geometry->geom(),(void*)-1);
geometry->SetPadding(defaultPadding);
geometry->surf().kRestitution = obj.kRestitution;
geometry->surf().kFriction = obj.kFriction;
geometry->surf().kStiffness = obj.kStiffness;
geometry->surf().kDamping = obj.kDamping;
SetTransform(obj.T);
}
App::App(dWorldID world, dSpaceID space, AppDesignID app_design,
BladeDesignID blade_design, dBodyID main_body)
: MyObject(world, space), ebMain(main_body)
{
// body count: 9 = blade + 2 * ( piston(2) + piston(2) )
// geom count: 10 = body_num + 1
// joint count: 12 = 2 * (A,B,C,D,E + piston )
Init(9, 12, 10); // b j g
bBlade = dBodyCreate(world);
gBlade = blade_design->createGeom(space);
int i;
for (i = 0; i < 2; ++i) {
app_design->setSide(i); // tell it which side i want
half[i] = new AppHalf(app_design);
// connect the half to the body and to the blade
dBodyID B = half[i]->getBBody();
dBodyID B_star = half[i]->getBstarBody();
dBodyID E = half[i]->getEBody();
BJoint = dCreate
}
return;
}
ODEObject::ODEObject(OscObject *obj, dGeomID odeGeom, dWorldID odeWorld, dSpaceID odeSpace)
: m_odeWorld(odeWorld), m_odeSpace(odeSpace)
{
m_object = obj;
m_odeGeom = odeGeom;
m_odeBody = NULL;
m_odeBody = dBodyCreate(m_odeWorld);
assert(m_odeGeom!=NULL);
dBodySetPosition(m_odeBody, 0, 0, 0);
dGeomSetPosition(m_odeGeom, 0, 0, 0);
// note: owners must override this by setting the density. can't
// do it here because obj->m_pSpecial is not yet
// initialized.
dMassSetSphere(&m_odeMass, 1, 1);
dBodySetMass(m_odeBody, &m_odeMass);
dGeomSetBody(m_odeGeom, m_odeBody);
dGeomSetData(m_odeGeom, obj);
if (!obj) return;
obj->m_rotation.setSetCallback(ODEObject::on_set_rotation, this);
obj->m_position.setSetCallback(ODEObject::on_set_position, this);
obj->m_velocity.setSetCallback(ODEObject::on_set_velocity, this);
obj->m_accel.setSetCallback(ODEObject::on_set_accel, this);
obj->m_force.setSetCallback(ODEObject::on_set_force, this);
obj->addHandler("push", "ffffff", ODEObject::push_handler);
}
PhysicsActor::PhysicsActor(){
name="PhysicsActor";
drawType=DRAW_CUBE;
world=renderer->physicsWorld;
space=renderer->collisionSpace;
body = dBodyCreate(renderer->physicsWorld);
geom=0;
joint=NULL;
oldBase=NULL;
shape=Vector3f(0.1,0.3,0.1);
mass=1.0;
type=CAPSULESHAPE;
//drawType=DRAW_SPECIAL;
bJointedToBase=false;
bSkeletalPart=false;
bFixToWorld=false;
bInit=false;
angleDamp=0.59;
linearDamp=0.004;
registerProperties();
menuType.push_back("12AssignButton");
menuProp.push_back("NULL");
menuIcon.push_back("icon_base");
menuName.push_back("make joint");
}
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);
}
}
RigidBody::RigidBody(const dWorldID &_world, const ngl::Vec3 &_pos)
{
m_world=_world;
m_id=dBodyCreate(m_world);
dBodySetPosition(m_id,_pos.m_x,_pos.m_y,_pos.m_z);
enable();
}
int main(int argc, char** argv)
{
init_gfx();
init_2d();
// create world and set gravity
world = dWorldCreate();
dWorldSetGravity(world, 0, -5, 0);
// create a body for our ball
dBodyID body = dBodyCreate(world);
dBodySetPosition(body, 0, 0, 0);
for (;;) { // infinite loop
// get the position and move there in OpenGL
const dReal* pos = dBodyGetPosition(body);
glTranslatef(pos[0], pos[1], pos[2]);
draw_circle();
// integrate all bodies
dWorldQuickStep(world, 0.01);
// redraw screen
step();
}
}
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 cubeD_D::SetboxBandG() {
boxBody_m = dBodyCreate (gODEWorld);
boxGeom_m = dCreateBox (gODESpace, 1,1,1);
}
/**
* This method initializes the grabbing of an object.
* It creates an ODE-object with the current position and
* orientation and stores the offset to the grabbed object
* for correct update of the Entity-transformation
* in the update method. It also creates all joints and
* linked objects by calling the attachJoints-method.
* @param entity Entity that should be grabbed
**/
void JointInteraction::grabEntity(Entity* entity, TransformationData offset)
{
unsigned entityID;
unsigned typeBasedID;
Entity* ent;
dQuaternion quat;
std::map< int, ODEObject*>::iterator it;
// TransformationPipe* linkedObjectPipe;
// unsigned short type, id;
std::vector<unsigned> entityIds;
unsigned userId;
JointInteractionBeginEvent* evt;
isGrabbing = true;
// // store position and orientation difference between user and object
deltaTrans = offset;
// Create ODE-representation of grabbed object by simply creating an object with
// user's position and orientation
ODEObject* odeObj = new ODEObject;
odeObj->body = dBodyCreate(world);
odeObj->entity = entity;
dBodySetPosition(odeObj->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
convert(quat, userTrans.orientation);
dBodySetQuaternion(odeObj->body, quat);
// attach all joints to object
entityID = entity->getEnvironmentBasedId();
linkedObjMap[entityID] = odeObj;
attachJoints(entityID);
grabbedObject = odeObj;
entityIds.push_back(entity->getTypeBasedId());
// open Pipes for all linked objects
for (it = linkedObjMap.begin(); it != linkedObjMap.end(); ++it)
{
if (it->second == grabbedObject)
continue;
ent = WorldDatabase::getEntityWithEnvironmentId(it->first);
assert(ent->getEnvironmentBasedId() != entityID);
typeBasedID = ent->getTypeBasedId();
entityIds.push_back(typeBasedID);
/*
split(typeBasedID, type, id);
// TODO: open pipes via Event
linkedObjectPipe = TransformationManager::openPipe(JOINT_INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 0, 0, type, id, 0, false);
linkedObjPipes[it->first] = linkedObjectPipe;
*/
} // for
userId = UserDatabase::getLocalUserId();
evt = new JointInteractionBeginEvent(userId, entityIds);
EventManager::sendEvent(evt, EventManager::EXECUTE_GLOBAL);
} // grabEntity
//------------------------------------------------------------------------------
OdeRigidBody * OdeSimulator::instantiate(const OdeRigidBody * blueprint)
{
OdeRigidBody * ret = blueprint->instantiate(dBodyCreate(world_id_), this);
body_.push_back(ret);
return ret;
}
dBodyID PhysicsBody::getBodyID(void)
{
if(_BodyID == 0 && getWorld() != NULL)
{
_BodyID = dBodyCreate(getWorld()->getWorldID());
}
return _BodyID;
}
void Rigid::_init()
{
mBodyID = dBodyCreate(RigidManager::getSingleton().getWorldID());
//默认质量密度
mMassDensity = 1;
if(mGeom)
dGeomSetBody(mGeom->getGeomID(), mBodyID);
}
void createPart(const dWorldID & _world , dPart & part, dReal * pos3){
dMass mass;
part.body = dBodyCreate(_world);
dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position
dMassSetZero(&mass); // Set mass parameter to zero
dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length); // Calculate mass parameter
dBodySetMass(part.body, &mass); // Set mass
}
/**
* This method returns the ODE-representation of the
* Entity with the passed ID. It searches
* in the map of connected objects for a matching
* ODEObject. If no object is found the method
* creates one and stores it in the map.
* @param entityID ID of the EntityTransform
* @return ODE representation of the EntityTransform
**/
dBodyID JointInteraction::getBodyWithID(int entityID)
{
if (entityID == 0)
return 0;
// TransformationPipe* objectPipe;
Entity* entity;
TransformationData trans;
gmtl::Vec3f pos;
gmtl::Quatf ori;
gmtl::AxisAnglef axAng;
dQuaternion quat;
ODEObject* object = linkedObjMap[entityID];
if (object == NULL)
{
entity = getEntityWithID(entityID);
if (entity == NULL)
{
printd(ERROR, "JointInteraction::getBodyWithID(): ERROR: FOUND BODY WITHOUT MATCHING ENTITY OBJECT!!!\n");
return 0;
} // if
object = new ODEObject;
object->body = dBodyCreate(world);
object->entity = entity;
trans = entity->getEnvironmentTransformation();
pos = trans.position;
ori = trans.orientation;
// get current object position and orientation
// pos[0] = entity->getXTrans();
// pos[1] = entity->getYTrans();
// pos[2] = entity->getZTrans();
// axAng.set(entity->getRotAngle(), entity->getXRot(), entity->getYRot(), entity->getZRot());
// gmtl::set(ori, axAng);
convert(quat, ori);
dBodySetPosition(object->body, pos[0], pos[1], pos[2]);
dBodySetQuaternion(object->body, quat);
linkedObjMap[entityID] = object;
// TODO: open and close pipes via events
// open TransformationPipe for handling transformations of connected entities
// split(entity->getTypeBasedId(), type, id);
// objectPipe = TransformationManager::openPipe(JOINT_INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 0, 0, type, id, 0, 0);
// linkedObjPipes[entityID] = objectPipe;
// ingnore request if Entity is not movable
if (!entity->getEntityType()->isFixed())
attachJoints(entityID);
} // if
if (object->entity->getEntityType()->isFixed())
return 0;
return object->body;
} // getBodyWithID
/*** ロボットアームの生成 ***/
void makeArm()
{
dMass mass; // 質量パラメータ
dReal x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 x
dReal y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 y
dReal z[NUM] = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0}; // 重心 z
dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50}; // 長さ
dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50}; // 質量
dReal r[NUM-1] = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; // 半径
dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 x
dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 y
dReal c_z[NUM] = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9}; // 関節中心点 z
dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0}; // 関節回転軸 x
dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0}; // 関節回転軸 y
dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1}; // 関節回転軸 z
// リンクの生成
for (int i = 0; i < NUM-1; i++) {
rlink[i].body = dBodyCreate(world);
dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
dBodySetMass(rlink[i].body, &mass);
rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
dGeomSetBody(rlink[i].geom,rlink[i].body);
}
rlink[NUM-1].body = dBodyCreate(world);
dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
dBodySetMass(rlink[NUM-1].body, &mass);
rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);
// ジョイントの生成とリンクへの取り付け
joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント
dJointAttach(joint[0], rlink[0].body, 0);
dJointSetFixed(joint[0]);
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
}
}
