void ComponentMovement::createMotorJoints() {
ASSERT(physicsEngine, "Null pointer: physicsEngine");
dBodyID body = getBodyID();
ASSERT(body, "Cannot create motor joints: No physics body available");
amotor = dJointCreateAMotor(physicsEngine->getWorld(), 0);
dJointAttach(amotor, body, 0);
dJointSetAMotorNumAxes(amotor, 3);
dJointSetAMotorAxis (amotor, 0, 1, 1, 0, 0);
dJointSetAMotorAxis (amotor, 1, 1, 0, 1, 0);
dJointSetAMotorAxis (amotor, 2, 1, 0, 0, 1);
dJointSetAMotorParam(amotor, dParamFMax, maxForce);
dJointSetAMotorParam(amotor, dParamFMax2, maxForce);
dJointSetAMotorParam(amotor, dParamFMax3, maxForce);
dJointSetAMotorParam(amotor, dParamVel, 0);
dJointSetAMotorParam(amotor, dParamVel2, 0);
dJointSetAMotorParam(amotor, dParamVel3, 0);
lmotor = dJointCreateLMotor(physicsEngine->getWorld(), 0);
dJointAttach(lmotor, body, 0);
dJointSetLMotorNumAxes(lmotor, 2);
dJointSetLMotorAxis (lmotor, 0, 1, 1, 0, 0);
dJointSetLMotorAxis (lmotor, 1, 1, 0, 1, 0);
dJointSetLMotorParam(lmotor, dParamFMax, maxForce);
dJointSetLMotorParam(lmotor, dParamFMax2, maxForce);
dJointSetLMotorParam(lmotor, dParamVel, 0);
dJointSetLMotorParam(lmotor, dParamVel2, 0);
}
osaODEServoMotor::osaODEServoMotor(dWorldID world,
dBodyID body1,
dBodyID body2,
const vctFixedSizeVector<double,3>& axis,
double vwmax,
double ftmax,
dJointType motortype ) :
vwmax( fabs( vwmax ) ),
ftmax( fabs( ftmax ) ) {
if( motortype == dJointTypeHinge ){
motorid = dJointCreateAMotor( world, 0 ); // create the motor
dJointAttach( MotorID(), body1, body2 ); // attach the joint
dJointSetAMotorMode( MotorID(), dAMotorUser ); // motor is in user mode
dJointSetAMotorNumAxes( MotorID(), 1 ); // only 1 axis
dJointSetAMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );
SetVelocity( 0.0 ); // idle the motor
}
if( motortype == dJointTypeSlider ){
motorid = dJointCreateLMotor( world, 0 ); // create the motor
dJointAttach( MotorID(), body1, body2 ); // attach the joint
dJointSetLMotorNumAxes( MotorID(), 1 ); // 1 axis
dJointSetLMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );
SetVelocity( 0.0 ); // idle the motor
}
}
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);
contactgroup = dJointGroupCreate(0);
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
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);
geom[0] = dCreateBox(space,SIDE,SIDE,SIDE);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],0,0,2);
geom[1] = dCreateBox(space,SIDE,SIDE,SIDE);
dGeomSetBody(geom[0],body[0]);
dGeomSetBody(geom[1],body[1]);
lmotor[0] = dJointCreateLMotor (world,0);
dJointAttach (lmotor[0],body[0],body[1]);
lmotor[1] = dJointCreateLMotor (world,0);
dJointAttach (lmotor[1],body[0],0);
amotor[0] = dJointCreateAMotor(world,0);
dJointAttach(amotor[0], body[0],body[1]);
amotor[1] = dJointCreateAMotor(world,0);
dJointAttach(amotor[1], body[0], 0);
for (int i=0; i<2; i++) {
dJointSetAMotorNumAxes(amotor[i], 3);
dJointSetAMotorAxis(amotor[i],0,1,1,0,0);
dJointSetAMotorAxis(amotor[i],1,1,0,1,0);
dJointSetAMotorAxis(amotor[i],2,1,0,0,1);
dJointSetAMotorParam(amotor[i],dParamFMax,0.00001);
dJointSetAMotorParam(amotor[i],dParamFMax2,0.00001);
dJointSetAMotorParam(amotor[i],dParamFMax3,0.00001);
dJointSetAMotorParam(amotor[i],dParamVel,0);
dJointSetAMotorParam(amotor[i],dParamVel2,0);
dJointSetAMotorParam(amotor[i],dParamVel3,0);
dJointSetLMotorNumAxes(lmotor[i],3);
dJointSetLMotorAxis(lmotor[i],0,1,1,0,0);
dJointSetLMotorAxis(lmotor[i],1,1,0,1,0);
dJointSetLMotorAxis(lmotor[i],2,1,0,0,1);
dJointSetLMotorParam(lmotor[i],dParamFMax,0.0001);
dJointSetLMotorParam(lmotor[i],dParamFMax2,0.0001);
dJointSetLMotorParam(lmotor[i],dParamFMax3,0.0001);
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy(contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void PhysicsLMotorJoint::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 = dJointCreateLMotor(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateLMotor(getWorld()->getWorldID(), 0);
if(!(whichField & VelFieldMask))
{
setVel(dJointGetLMotorParam(_JointID,dParamVel));
}
if(!(whichField & FMaxFieldMask))
{
setFMax(dJointGetLMotorParam(_JointID,dParamFMax));
}
if(!(whichField & FudgeFactorFieldMask))
{
setFudgeFactor(dJointGetLMotorParam(_JointID,dParamFudgeFactor));
}
if(!(whichField & Vel2FieldMask))
{
setVel2(dJointGetLMotorParam(_JointID,dParamVel2));
}
if(!(whichField & FMax2FieldMask))
{
setFMax2(dJointGetLMotorParam(_JointID,dParamFMax2));
}
if(!(whichField & FudgeFactor2FieldMask))
{
setFudgeFactor2(dJointGetLMotorParam(_JointID,dParamFudgeFactor2));
}
if(!(whichField & Vel3FieldMask))
{
setVel3(dJointGetLMotorParam(_JointID,dParamVel3));
}
if(!(whichField & FMax3FieldMask))
{
setFMax3(dJointGetLMotorParam(_JointID,dParamFMax3));
}
if(!(whichField & FudgeFactor3FieldMask))
{
setFudgeFactor3(dJointGetLMotorParam(_JointID,dParamFudgeFactor3));
}
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetLMotorParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetLMotorParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetLMotorParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetLMotorParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetLMotorParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetLMotorParam(_JointID,dParamStopERP));
}
if(!(whichField & HiStop2FieldMask))
{
setHiStop2(dJointGetLMotorParam(_JointID,dParamHiStop2));
}
if(!(whichField & LoStop2FieldMask))
{
setLoStop2(dJointGetLMotorParam(_JointID,dParamLoStop2));
}
if(!(whichField & Bounce2FieldMask))
{
setBounce2(dJointGetLMotorParam(_JointID,dParamBounce2));
}
if(!(whichField & CFM2FieldMask))
{
setCFM2(dJointGetLMotorParam(_JointID,dParamCFM2));
}
if(!(whichField & StopCFM2FieldMask))
{
setStopCFM2(dJointGetLMotorParam(_JointID,dParamStopCFM2));
}
if(!(whichField & StopERP2FieldMask))
{
setStopERP2(dJointGetLMotorParam(_JointID,dParamStopERP2));
}
if(!(whichField & HiStop3FieldMask))
{
setHiStop3(dJointGetLMotorParam(_JointID,dParamHiStop3));
}
if(!(whichField & LoStop3FieldMask))
{
setLoStop3(dJointGetLMotorParam(_JointID,dParamLoStop3));
}
if(!(whichField & Bounce3FieldMask))
{
setBounce3(dJointGetLMotorParam(_JointID,dParamBounce3));
}
if(!(whichField & CFM3FieldMask))
{
setCFM3(dJointGetLMotorParam(_JointID,dParamCFM3));
}
if(!(whichField & StopCFM3FieldMask))
{
setStopCFM3(dJointGetLMotorParam(_JointID,dParamStopCFM3));
}
if(!(whichField & StopERP3FieldMask))
{
setStopERP3(dJointGetLMotorParam(_JointID,dParamStopERP3));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & NumAxesFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetLMotorNumAxes(_JointID,getNumAxes());
}
if((whichField & Axis1FieldMask) ||
(whichField & Axis1ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetLMotorAxis(_JointID,0, getAxis1ReferenceFrame(), getAxis1().x(), getAxis1().y(), getAxis1().z());
}
if((whichField & Axis2FieldMask) ||
(whichField & Axis2ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetLMotorAxis(_JointID,1, getAxis2ReferenceFrame(), getAxis2().x(), getAxis2().y(), getAxis2().z());
}
if((whichField & Axis3FieldMask) ||
(whichField & Axis3ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetLMotorAxis(_JointID,2, getAxis3ReferenceFrame(), getAxis3().x(), getAxis3().y(), getAxis3().z());
}
if((whichField & VelFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamVel, getVel());
}
if((whichField & FMaxFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamFMax, getFMax());
}
if((whichField & FudgeFactorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamFudgeFactor, getFudgeFactor());
}
if((whichField & Vel2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamVel2, getVel2());
}
if((whichField & FMax2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamFMax2, getFMax2());
}
if((whichField & FudgeFactor2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamFudgeFactor2, getFudgeFactor2());
}
if((whichField & Vel3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamVel3, getVel3());
}
if((whichField & FMax3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamFMax3, getFMax3());
}
if((whichField & FudgeFactor3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamFudgeFactor3, getFudgeFactor3());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamStopCFM, getStopCFM());
}
if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamHiStop2, getHiStop2());
}
if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamLoStop2, getLoStop2());
}
if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamBounce2, getBounce2());
}
if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamCFM2, getCFM2());
}
if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamStopERP2, getStopERP2());
}
if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamStopCFM2, getStopCFM2());
}
if((whichField & HiStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamHiStop3, getHiStop3());
}
if((whichField & LoStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamLoStop3, getLoStop3());
}
if((whichField & Bounce3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamBounce3, getBounce3());
}
if((whichField & CFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamCFM3, getCFM3());
}
if((whichField & StopERP3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamStopERP3, getStopERP3());
}
if((whichField & StopCFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetLMotorParam(_JointID, dParamStopCFM3, getStopCFM3());
}
}
