//! A piston requires a fixed anchor point and an axis
OscPistonODE::OscPistonODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase* parent,
OscObject *object1, OscObject *object2,
double x, double y, double z,
double ax, double ay, double az)
: OscPiston(name, parent, object1, object2, x, y, z, ax, ay, az)
{
m_response = new OscResponse("response",this);
// create the constraint for object1
cVector3d anchor(x,y,z);
cVector3d axis(ax,ay,az);
dJointID odeJoint = dJointCreatePiston(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetPistonAnchor(odeJoint, anchor.x, anchor.y, anchor.z);
dJointSetPistonAxis(odeJoint, axis.x, axis.y, axis.z);
printf("Piston joint created between %s and %s at anchor (%f,%f,%f), axis (%f,%f,%f)\n",
object1->c_name(), object2?object2->c_name():"world", x,y,z,ax,ay,az);
}
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 setPositionBodies (int val)
{
const dVector3 POS1 = {0,0,1.5,0};
const dVector3 POS2 = {0,0,1.5,0};
const dVector3 ANCHOR = {0,0,1.5,0};
for (int i=0; i<3; ++i)
{
pos1[i] = POS1[i];
pos2[i] = POS2[i];
anchor[i] = ANCHOR[i];
}
if (body[BODY1])
{
dBodySetLinearVel (body[BODY1], 0,0,0);
dBodySetAngularVel (body[BODY1], 0,0,0);
}
if (body[BODY2])
{
dBodySetLinearVel (body[BODY2], 0,0,0);
dBodySetAngularVel (body[BODY2], 0,0,0);
}
switch (val)
{
case 3:
pos1[Z] += -0.5;
anchor[Z] -= 0.25;
break;
case 2:
pos1[Z] -= 0.5;
anchor[Z] -= 0.5;
break;
case 1:
pos1[Z] += -0.5;
break;
default: // This is also case 0
// Nothing to be done
break;
}
const dMatrix3 R =
{
1,0,0,0,
0,1,0,0,
0,0,1,0
};
if (body[BODY1])
{
dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]);
dBodySetRotation (body[BODY1], R);
}
if (body[BODY2])
{
dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]);
dBodySetRotation (body[BODY2], R);
}
if (joint)
{
joint->attach (body[BODY1], body[BODY2]);
if (joint->getType() == dJointTypePiston)
dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]);
}
}
void PhysicsPistonJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
_JointID = dJointCreatePiston(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreatePiston(getWorld()->getWorldID(), 0);
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetPistonParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetPistonParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetPistonParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetPistonParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetPistonParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetPistonParam(_JointID,dParamStopERP));
}
if(!(whichField & HiStop2FieldMask))
{
setHiStop2(dJointGetPistonParam(_JointID,dParamHiStop2));
}
if(!(whichField & LoStop2FieldMask))
{
setLoStop2(dJointGetPistonParam(_JointID,dParamLoStop2));
}
if(!(whichField & Bounce2FieldMask))
{
setBounce2(dJointGetPistonParam(_JointID,dParamBounce2));
}
if(!(whichField & CFM2FieldMask))
{
setCFM2(dJointGetPistonParam(_JointID,dParamCFM2));
}
if(!(whichField & StopCFM2FieldMask))
{
setStopCFM2(dJointGetPistonParam(_JointID,dParamStopCFM2));
}
if(!(whichField & StopERP2FieldMask))
{
setStopERP2(dJointGetPistonParam(_JointID,dParamStopERP2));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & AnchorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonAnchor(_JointID, getAnchor().x(), getAnchor().y(), getAnchor().z());
}
if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamStopCFM, getStopCFM());
}
if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamHiStop2, getHiStop2());
}
if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamLoStop2, getLoStop2());
}
if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamBounce2, getBounce2());
}
if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamCFM2, getCFM2());
}
if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamStopERP2, getStopERP2());
}
if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetPistonParam(_JointID, dParamStopCFM2, getStopCFM2());
}
}
