bool CDynamics3DEntity::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_check_only) {
/* Move the body to the new position */
dBodySetPosition(m_tBody, c_position.GetX(), c_position.GetY(), c_position.GetZ());
/* Rotate the body to the new orientation */
dQuaternion tQuat = { c_orientation.GetW(),
c_orientation.GetX(),
c_orientation.GetY(),
c_orientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
/* Check for collisions */
bool bCollisions = m_cEngine.IsEntityColliding(m_tEntitySpace);
if(bCollisions || b_check_only) {
/*
* Undo the changes if there is a collision or
* if the move was just a check
*/
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ());
const CQuaternion& cOrientation = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat2 = { cOrientation.GetW(),
cOrientation.GetX(),
cOrientation.GetY(),
cOrientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat2);
return !bCollisions;
}
else {
/* Set the new position and orientation */
GetEmbodiedEntity().SetPosition(c_position);
GetEmbodiedEntity().SetOrientation(c_orientation);
return true;
}
}
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 SParts::setQuaternion(dReal q0, dReal q1, dReal q2, dReal q3)
{
if (m_odeobj) {
dReal q[] = { q0, q1, q2, q3, };
dBodySetQuaternion(m_odeobj->body(), q);
}
}
void ompl::control::OpenDEStateSpace::writeState(const base::State *state) const
{
const auto *s = state->as<StateType>();
for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
{
unsigned int _i4 = i * 4;
double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]);
double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]);
double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
dBodySetAngularVel(env_->stateBodies_[i], s_ang[0], s_ang[1], s_ang[2]);
const base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
dQuaternion q;
q[0] = s_rot.w;
q[1] = s_rot.x;
q[2] = s_rot.y;
q[3] = s_rot.z;
dBodySetQuaternion(env_->stateBodies_[i], q);
}
}
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 dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
{
dAASSERT (g && quat);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->offset_posr) {
g->recomputePosr();
// move body such that body+offset = rotation
dxPosR new_final_posr;
dxPosR new_body_posr;
dQtoR (quat, new_final_posr.R);
memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
dBodySetRotation(g->body, new_body_posr.R);
dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
}
if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetQuaternion (g->body,quat);
}
else {
dQtoR (quat, g->final_posr->R);
dGeomMoved (g);
}
}
void RigidBody::setQuaternion(const ngl::Quaternion &_q)
{
// according to the ODE headers this is in the format
// quaternion (qs,qx,qy,qz) but stored as an array of 4 floats
dQuaternion q={_q.getS(),_q.getX(),_q.getY(),_q.getZ()};
dBodySetQuaternion(m_id,q);
}
/**
* 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
/**
* 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
static void reset_state(void)
{
float sx=-4, sy=-4, sz=2;
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
dBodySetPosition (boxbody, sx, sy+1, sz);
dBodySetLinearVel (boxbody, 0,0,0);
dBodySetAngularVel (boxbody, 0,0,0);
dBodySetQuaternion (boxbody, q);
#endif
#ifdef CYL
dBodySetPosition (cylbody, sx, sy, sz);
dBodySetLinearVel (cylbody, 0,0,0);
dBodySetAngularVel (cylbody, 0,0,0);
dBodySetQuaternion (cylbody, q);
#endif
}
/* sets position and rotation of a body based on position and quaternion values */
void body_set_position_and_quaternion (dBodyID b, t_real x, t_real y, t_real z, t_real q1, t_real q2, t_real q3, t_real q4) {
dQuaternion q;
dBodySetPosition (b, x, y, z);
q [0] = q1;
q [1] = q2;
q [2] = q3;
q [3] = q4;
dBodySetQuaternion (b, q);
}
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;
}
static void reset_ball(void)
{
float sx=0.0f, sy=3.40f, sz=7.15;
dQuaternion q;
dQSetIdentity(q);
dBodySetPosition (sphbody, sx, sy, sz);
dBodySetQuaternion(sphbody, q);
dBodySetLinearVel (sphbody, 0,0,0);
dBodySetAngularVel (sphbody, 0,0,0);
}
//旋转刚体
void Rigid::rotate( const Ogre::Vector3& axis,const Ogre::Real angle ){
mNode->rotate( axis,Ogre::Radian(angle),Ogre::Node::TS_WORLD );
const Ogre::Quaternion& q = mNode->getOrientation();
dQuaternion dQ;
dQ[0] = q.w;
dQ[1] = q.x;
dQ[2] = q.y;
dQ[3] = q.z;
dBodySetQuaternion(mBodyID,dQ);
_visual2physic();
}
void Rigid::_visual2physic()
{
const Ogre::Vector3& v3 = mNode->getPosition();
dBodySetPosition(mBodyID,v3.x,v3.y,v3.z);
const Ogre::Quaternion& q = mNode->getOrientation();
dQuaternion dq;
dq[0] = q.w;
dq[1] = q.x;
dq[2] = q.y;
dq[3] = q.z;
dBodySetQuaternion(mBodyID,dq);
}
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 dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
{
dAASSERT (g && quat);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetQuaternion (g->body,quat);
}
else {
dQtoR (quat, g->R);
dGeomMoved (g);
}
}
bool ODERigidObject::ReadState(File& f)
{
Vector3 w,v;
dReal pos[3];
dReal q[4];
if(!ReadArrayFile(f,pos,3)) return false;
if(!ReadArrayFile(f,q,4)) return false;
if(!ReadFile(f,w)) return false;
if(!ReadFile(f,v)) return false;
dBodySetPosition(bodyID,pos[0],pos[1],pos[2]);
dBodySetQuaternion(bodyID,q);
SetVelocity(w,v);
return true;
}
static void reset_ball(void)
{
float sx=0.0f, sy=3.40f, sz=6.80f;
#if defined(_MSC_VER) && defined(dDOUBLE)
sy -= 0.01; // Cheat, to make it score under win32/double
#endif
dQuaternion q;
dQSetIdentity(q);
dBodySetPosition (sphbody, sx, sy, sz);
dBodySetQuaternion(sphbody, q);
dBodySetLinearVel (sphbody, 0,0,0);
dBodySetAngularVel (sphbody, 0,0,0);
}
PMoveable::PMoveable(Kinematic &kinematic, float mass, GeomInfo *info)
: PObject(info), kinematic(kinematic)
{
dQuaternion q;
body = dBodyCreate(Physics::getInstance().getOdeWorld());
info->createMass(&this->mass, mass);
dBodySetMass(body, &this->mass);
dGeomSetBody(geom, body);
kinematicToOde();
dQFromAxisAndAngle(q, 0, 1, 0, kinematic.orientation);
dBodySetQuaternion(body, q);
}
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;
}
}
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 CPHIsland::Repair()
{
if(!m_flags.is_active()) return;
dBodyID body;
for (body = firstbody; body; body = (dxBody *) body->next)
{
if(!dV_valid(dBodyGetAngularVel(body)))
dBodySetAngularVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetLinearVel(body)))
dBodySetLinearVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetPosition(body)))
dBodySetPosition(body,0.f,0.f,0.f);
if(!dQ_valid(dBodyGetQuaternion(body)))
{
dQuaternion q={1.f,0.f,0.f,0.f};//dQSetIdentity(q);
dBodySetQuaternion(body,q);
}
}
}
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 Simulator::SetCurrentState(const double s[])
{
/*
* State for each body consists of position, orientation (stored as a quaternion),
* linear velocity, and angular velocity. In the current robot model,
* the bodies are vehicle chassis and the vehicle wheels.
*/
const int n = (int) m_robotBodies.size();
for (int i = 0; i < n; ++i)
{
const int offset = i * 13;
const dReal q[] = {s[offset + 3], s[offset + 4], s[offset + 5], s[offset + 6]};
dBodySetPosition (m_robotBodies[i], s[offset], s[offset + 1], s[offset + 2]);
dBodySetQuaternion(m_robotBodies[i], q);
dBodySetLinearVel (m_robotBodies[i], s[offset + 7], s[offset + 8], s[offset + 9]);
dBodySetAngularVel(m_robotBodies[i], s[offset + 10], s[offset + 11], s[offset + 12]);
}
dRandSetSeed((long unsigned int) (s[n * 13]));
}
void Mesh::setPose(const Pose& pose){
if(body){
osg::Vec3 pos = pose.getTrans();
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dBodySetQuaternion(body, quat);
}else if(geom){ // okay there is just a geom no body
osg::Vec3 pos = pose.getTrans();
dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dGeomSetQuaternion(geom, quat);
} else
poseWithoutBodyAndGeom = Pose(pose);
update(); // update the scenegraph stuff
}
void CDynamics3DEntity::Reset() {
/* Clear force and torque on the body */
dBodySetForce(m_tBody, 0.0f, 0.0f, 0.0f);
dBodySetTorque(m_tBody, 0.0f, 0.0f, 0.0f);
/* Clear speeds */
dBodySetLinearVel(m_tBody, 0.0f, 0.0f, 0.0f);
dBodySetAngularVel(m_tBody, 0.0f, 0.0f, 0.0f);
/* Reset position */
const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
dBodySetPosition(m_tBody,
cPosition.GetX(),
cPosition.GetY(),
cPosition.GetZ());
/* Reset orientation */
const CQuaternion& cQuaternion = GetEmbodiedEntity().GetInitOrientation();
dQuaternion tQuat = {
cQuaternion.GetW(),
cQuaternion.GetX(),
cQuaternion.GetY(),
cQuaternion.GetZ()
};
dBodySetQuaternion(m_tBody, tQuat);
}
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/textures";
// create world
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0,0,1);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dQuaternion q;
dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI);
dBodySetPosition (body[1],0.2,0.2,1.2);
dBodySetQuaternion (body[1],q);
slider = dJointCreateSlider (world,0);
dJointAttach (slider,body[0],body[1]);
dJointSetSliderAxis (slider,1,1,1);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
return 0;
}
void Primitive::setPose(const Pose& pose){
if(body){
osg::Vec3 pos = pose.getTrans();
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dBodySetQuaternion(body, quat);
}else if(geom){ // okay there is just a geom no body
osg::Vec3 pos = pose.getTrans();
dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dGeomSetQuaternion(geom, quat);
} else {
assert(0 && "Call setPose only after initialization");
}
update(); // update the scenegraph stuff
}
void Robots::construirRodas(dWorldID world)
{
for (int i=0; i < 2; i++)
{
// Cria objeto e geometria
this->wheel[i] = dBodyCreate(world);
this->cylinder[i] = dCreateCylinder(0,RADIUS,wTHICK);
// Define a posição e orientação do objeto
dQuaternion q;
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
dBodySetQuaternion(this->wheel[i],q);
dBodySetPosition(this->wheel[i],this->pegarX(),this->pegarY()+(1-2*i)*((WIDTH/2)+(wTHICK/2)),STARTZ);
// Define a massa do objeto
dMass m;
dMassSetCylinder(&m,1,3,RADIUS,wTHICK);
dMassAdjust(&m,WMASS);
dBodySetMass(this->wheel[i],&m);
// Associa o objeto à sua geometria
dGeomSetBody(this->cylinder[i],this->wheel[i]);
}
}