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
dInitODE();
world = dWorldCreate();
#if 1
space = dHashSpaceCreate (0);
#elif 0
dVector3 center = {0,0,0}, extents = { 100, 100, 100};
space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
space = dSimpleSpaceCreate(0);
#endif
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactSurfaceLayer (world,0.001);
ground = dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// create lift platform
platform = dCreateBox(space, 4, 4, 1);
dGeomSetCategoryBits(ground, 1ul);
dGeomSetCategoryBits(platform, 2ul);
dGeomSetCollideBits(ground, ~2ul);
dGeomSetCollideBits(platform, ~1ul);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
// Universal method for all specific ODE geom types, which add the
// geom to the collide space, using an ODE proxy geom to offset the
// geom by the provided transformation matrix. The geom will also
// be attached to the rigid body, if any is set.
void CShape::AttachGeom(dGeomID GeomId, dSpaceID SpaceID)
{
n_assert(GeomId);
n_assert(!IsAttached());
// set the geom's local Transform
const vector3& Pos = Transform.pos_component();
dGeomSetPosition(GeomId, Pos.x, Pos.y, Pos.z);
dMatrix3 ODERotation;
CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
dGeomSetRotation(GeomId, ODERotation);
// if attached to rigid body, create a geom Transform "proxy" object && attach it to the rigid body
// else directly set Transform and rotation
if (pRigidBody)
{
ODEGeomID = dCreateGeomTransform(0);
dGeomTransformSetCleanup(ODEGeomID, 1);
dGeomTransformSetGeom(ODEGeomID, GeomId);
dGeomSetBody(ODEGeomID, pRigidBody->GetODEBodyID());
}
else ODEGeomID = GeomId;
dGeomSetCategoryBits(ODEGeomID, CatBits);
dGeomSetCollideBits(ODEGeomID, CollBits);
dGeomSetData(ODEGeomID, this);
AttachToSpace(SpaceID);
}
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 Primitive::attachGeomAndSetColliderFlags(){
if(mode & Body){
// geom is assigned to body and is set into category Dyn
dGeomSetBody (geom, body);
dGeomSetCategoryBits (geom, Dyn);
dGeomSetCollideBits (geom, ~0x0); // collides with everything
} else {
// geom is static, so it is member of the static category and will collide not with other statics
dGeomSetCategoryBits (geom, Stat);
dGeomSetCollideBits (geom, ~Stat);
}
if(mode & _Child){ // in case of a child object it is always dynamic
dGeomSetCategoryBits (geom, Dyn);
dGeomSetCollideBits (geom, ~0x0); // collides with everything
}
dGeomSetData(geom, (void*)this); // set primitive as geom data
}
void ODESimulator::AddObject(const RigidObject& object)
{
objects.push_back(new ODERigidObject(object));
objects.back()->Create(worldID,envSpaceID);
dGeomSetData(objects.back()->geom(),(void*)(objects.size()-1));
dGeomSetCategoryBits(objects.back()->geom(),0x2);
dGeomSetCollideBits(objects.back()->geom(),0xffffffff);
}
void ODESimulator::AddRobot(Robot& robot)
{
robots.push_back(new ODERobot(robot));
//For some reason, self collisions don't work with hash spaces
robots.back()->Create(worldID);
//robotStances.resize(robots.size());
for(size_t i=0;i<robot.links.size();i++)
if(robots.back()->triMesh(i) && robots.back()->geom(i)) {
if(robots.back()->robot.parents[i] == -1) { //treat as part of the terrain
dGeomSetCategoryBits(robots.back()->geom(i),0x1);
dGeomSetCollideBits(robots.back()->geom(i),0xffffffff ^ 0x1);
}
else {
dGeomSetCategoryBits(robots.back()->geom(i),0x4);
dGeomSetCollideBits(robots.back()->geom(i),0xffffffff);
}
}
}
void set_phys_geom_attr_i(dGeomID geom, int p, int i)
{
switch (p)
{
case GEOM_ATTR_CATEGORY: dGeomSetCategoryBits(geom, i); break;
case GEOM_ATTR_COLLIDER: dGeomSetCollideBits (geom, i); break;
case GEOM_ATTR_RESPONSE: get_data(geom)->response = i; break;
case GEOM_ATTR_CALLBACK: get_data(geom)->callback = i; break;
}
}
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]);
}
}
}
void Solid::createGeometry(const dSpaceID& i_space)
{
/// need to get the correct geometry from volume.
std::vector< ::Ogre::Vector3> vertices ;
std::vector<unsigned long> indices ;
Ogre::Vector3 scale(1,1,1) ;
m_vertices = NULL ;
m_indices = NULL ;
InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#0") ;
getObject()
->getTrait<Model::Solid>()
->getMesh().getMeshInformation(vertices,indices,scale) ;
if (vertices.size()>0 && indices.size() > 0)
{
m_vertices = new dVector3[vertices.size()];
m_indices = new dTriIndex[indices.size()];
for(unsigned int vertex_index = 0 ;
vertex_index < vertices.size() ;
++vertex_index)
{
m_vertices[vertex_index][0] = (dReal)(vertices[vertex_index].x) ;
m_vertices[vertex_index][1] = (dReal)(vertices[vertex_index].y) ;
m_vertices[vertex_index][2] = (dReal)(vertices[vertex_index].z) ;
m_vertices[vertex_index][3] = 0 ;
}
for(unsigned int index = 0 ;
index < indices.size() ;
++index)
{
m_indices[index] = (int)indices[index] ;
}
m_data = dGeomTriMeshDataCreate() ;
dGeomTriMeshDataBuildSimple(m_data,
(dReal*)m_vertices,(int)vertices.size(),
m_indices,(int)indices.size()) ;
m_geometry1 = dCreateTriMesh(i_space,m_data,0,0,0);
dGeomSetData(m_geometry1,m_data) ;
dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Solid) ;
createApproximatedGeometry(i_space) ;
InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#5") ;
}
}
void ODESimulator::AddEnvironment(const Environment& env)
{
envs.push_back(&env);
envMeshes.resize(envMeshes.size()+1);
envMeshes.back() = new ODETriMesh;
envMeshes.back()->Create(env.mesh,envSpaceID);
envMeshes.back()->surf() = settings.defaultEnvSurface;
envMeshes.back()->SetPadding(settings.defaultEnvPadding);
if(!env.kFriction.empty())
envMeshes.back()->surf().kFriction = env.kFriction[0];
//the index of the environment is encoded as -1-index
dGeomSetData(envMeshes.back()->geom(),(void*)(-(int)envs.size()));
dGeomSetCategoryBits(envMeshes.back()->geom(),0x1);
dGeomSetCollideBits(envMeshes.back()->geom(),0xffffffff ^ 0x1);
}
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 LaserBeam::createGeometry(const dSpaceID& space)
{
/// build a ray
Model::LaserBeam* beam = getTrait<Model::LaserBeam>() ;
Model::Mobile* mobile = getTrait<Model::Mobile>() ;
float collision_beam_length =
std::max(mobile->getSpeed().MeterPerSecond().length()*getControlerSet()->getTimeStep(),
beam->getLength().Meter()) ;
m_ray = dCreateRay(0,2*collision_beam_length) ;
/// we use -z as forward @see Model::Orientation
dGeomRaySet(m_ray,0,0,0,0,0,-1) ;
dGeomRaySetParams(m_ray,false,false) ;
dGeomRaySetClosestHit(m_ray,true) ;
/// a geometry transform allows to have a local z axis inversion
m_geometry1 = dCreateGeomTransform(space) ;
dGeomTransformSetGeom(m_geometry1,m_ray) ;
dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Laser) ;
}
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;
if (argc >= 2 ) {
for (int i=1; i < argc; ++i) {
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
type = dJointTypePR;
if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
}
dInitODE2(0);
world.setERP (0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
// Create the body attached to the World
body[W].create (world);
// Main axis of cylinder is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[W], body[W]);
dGeomSetCategoryBits (geom[W], catBits[W]);
dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
body[W].setMass(m);
// Create the dandling body
body[D].create(world);
// Main axis of capsule is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[D], body[D]);
dGeomSetCategoryBits (geom[D], catBits[D]);
dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
body[D].setMass(&m);
// Create the external part of the slider joint
geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
dGeomSetCollideBits (geom[EXT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
// Create the internal part of the slider joint
geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
INT_EXT_RATIO*extDim[Y],
INT_EXT_RATIO*extDim[Z]);
dGeomSetCategoryBits (geom[INT], catBits[INT]);
dGeomSetCollideBits (geom[INT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
dMatrix3 R;
dGeomID id;
// Create the first axis of the universal joi9nt
geom[AXIS1] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
dGeomSetRotation (geom[AXIS1], R);
dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
dGeomSetCollideBits (geom[AXIS1],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS1];
dGeomTransformSetGeom (geom[AXIS1], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the second axis of the universal joint
geom[AXIS2] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
dGeomSetRotation (geom[AXIS2], R);
dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
dGeomSetCollideBits (geom[AXIS2],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS2];
dGeomTransformSetGeom (geom[AXIS2], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the anchor
geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
dGeomSetCollideBits (geom[ANCHOR],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
if (body[W]) {
body[W].setPosition(0, 0, 5);
}
if (geom[EXT]) {
dGeomSetPosition (geom[EXT], 0,0,3.8);
}
if (geom[INT]) {
dGeomSetPosition (geom[INT], 0,0,2.6);
}
if (geom[AXIS1]) {
dGeomSetPosition (geom[AXIS1], 0,0,2.5);
}
if (geom[AXIS2]) {
dGeomSetPosition (geom[AXIS2], 0,0,2.5);
}
if (geom[ANCHOR]) {
dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
}
if (body[D]) {
body[D].setPosition(0,0,1.5);
}
// Attache the upper box to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[W]);
dJointSetFixed (fixed );
if (type == dJointTypePR) {
dPRJoint *pr = new dPRJoint (world, 0);
pr->attach (body[W], body[D]);
pr->setAxis1 (0, 0, -1);
pr->setAxis2 (1, 0, 0);
joint = pr;
dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
}
else {
dPUJoint *pu = new dPUJoint (world, 0);
pu->attach (body[W], body[D]);
pu->setAxis1 (1, 0, 0);
pu->setAxis2 (0, 1, 0);
pu->setAxisP (0, 0, -1);
joint = pu;
dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
}
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void PhysicsGeom::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & BodyFieldMask)
{
if(getBody() != NULL)
{
dGeomSetBody(_GeomID, getBody()->getBodyID());
}
else
{
dGeomSetBody(_GeomID, 0);
}
}
if(whichField & PositionFieldMask)
{
dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z());
}
if(whichField & RotationFieldMask)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetRotation(_GeomID, rotation);
}
if(whichField & QuaternionFieldMask)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dGeomSetQuaternion(_GeomID,q);
}
if(whichField & OffsetPositionFieldMask &&
getBody() != NULL)
{
dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z());
}
if(whichField & OffsetRotationFieldMask &&
getBody() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getOffsetRotation()[0];
Vec4f v2 = getOffsetRotation()[1];
Vec4f v3 = getOffsetRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetOffsetRotation(_GeomID, rotation);
}
if(whichField & OffsetQuaternionFieldMask && getBody() != NULL)
{
dQuaternion q;
q[0]=getOffsetQuaternion().w();
q[1]=getOffsetQuaternion().x();
q[2]=getOffsetQuaternion().y();
q[3]=getOffsetQuaternion().z();
dGeomSetOffsetQuaternion(_GeomID,q);
}
if(whichField & CategoryBitsFieldMask)
{
dGeomSetCategoryBits(_GeomID, getCategoryBits());
}
if(whichField & CollideBitsFieldMask)
{
dGeomSetCollideBits(_GeomID, getCollideBits());
}
if(whichField & SpaceFieldMask)
{
dSpaceID CurSpace(dGeomGetSpace(_GeomID));
if(CurSpace != 0 &&
(getSpace() == NULL ||
CurSpace != getSpace()->getSpaceID()))
{
dSpaceRemove(CurSpace,_GeomID);
}
if(getSpace() != NULL)
{
dSpaceAdd(getSpace()->getSpaceID(), _GeomID);
}
}
if(whichField & EnableFieldMask)
{
if(getEnable())
{
dGeomEnable(_GeomID);
}
else
{
dGeomDisable(_GeomID);
}
}
}
void ODEGeom::setGeomCollideBits (unsigned long bits)
{
dGeomSetCollideBits(geom,bits);
}
void Solid::createApproximatedGeometry(const dSpaceID& space)
{
m_geometry2 = dCreateSphere(space,getObject()->getTrait<Model::Solid>()
->getRadius().Meter()) ;
dGeomSetCollideBits(m_geometry2,(unsigned long)Collideable::ApproximatedSolid) ;
}
int main (int argc, char **argv)
{
dInitODE2(0);
bool fixed = true;
// 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;
dVector3 offset;
dSetZero (offset, 4);
// Default test case
if (argc >= 2 )
{
for (int i=1; i < argc; ++i)
{
//static int tata = 0;
if (1)
{
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
type = dJointTypeSlider;
if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
{
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
tc = 1;
if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
tc = 2;
if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
tc = 3;
if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
fixed = false;
}
}
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
dMatrix3 R;
// Create the Obstacle
geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
dGeomSetCollideBits (geom[OBS], catBits[ALL]);
//Rotation of 45deg around y
dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
dGeomSetRotation (geom[OBS], R);
dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
//Rotation of 90deg around y
// Will orient the Z axis along X
dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
// Create Body2 (Wiil be attached to the world)
body[BODY2] = dBodyCreate (world);
// Main axis of cylinder is along X=1
dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dMassAdjust (&m, Mass1);
geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dGeomSetBody (geom[BODY2], body[BODY2]);
dGeomSetOffsetRotation (geom[BODY2], R);
dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY2], &m);
// Create Body 1 (Slider on the prismatic axis)
body[BODY1] = dBodyCreate (world);
// Main axis of capsule is along X=1
dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
dMassAdjust (&m, Mass1);
geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
dGeomSetBody (geom[BODY1], body[BODY1]);
dGeomSetOffsetRotation (geom[BODY1], R);
dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
dMass mRect;
dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dMassAdd (&m, &mRect);
// TODO: translate m?
geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dGeomSetBody (geom[RECT], body[BODY1]);
dGeomSetOffsetPosition (geom[RECT],
(BODY1_LENGTH-RECT_SIDES[0]) /2.0,
0.0,
-RADIUS -RECT_SIDES[2]/2.0);
dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY1], &m);
setPositionBodies (tc);
if ( fixed )
{
// Attache external cylinder to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[BODY2]);
dJointSetFixed (fixed );
dWorldSetGravity (world,0,0,-0.8);
}
else
{
dWorldSetGravity (world,0,0,0);
}
// The static is here only to help debugging
switch (type)
{
case dJointTypeSlider :
{
dSliderJoint *sj = new dSliderJoint (world, 0);
sj->attach (body[BODY1], body[BODY2]);
sj->setAxis (1, 0, 0);
joint = sj;
}
break;
case dJointTypePiston : // fall through default
default:
{
dPistonJoint *pj = new dPistonJoint (world, 0);
pj->attach (body[BODY1], body[BODY2]);
pj->setAxis (1, 0, 0);
dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
joint = pj;
}
break;
};
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void ODE_Particle::setCollideBits (unsigned long bits)
{
dGeomSetCollideBits(geom,bits);
}
