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);
}
// ang2 = position angle
// ang = rotation angle
Robot::Wheel::Wheel(Robot* robot,int _id,float ang,float ang2,int wheeltexid)
{
id = _id;
rob = robot;
float rad = rob->cfg->robotSettings.RobotRadius - rob->cfg->robotSettings.WheelThickness / 2.0;
ang *= M_PI/180.0f;
ang2 *= M_PI/180.0f;
float x = rob->m_x;
float y = rob->m_y;
float z = rob->m_z;
float centerx = x+rad*cos(ang2);
float centery = y+rad*sin(ang2);
float centerz = z-rob->cfg->robotSettings.RobotHeight*0.5+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight;
cyl = new PCylinder(centerx,centery,centerz,rob->cfg->robotSettings.WheelRadius,rob->cfg->robotSettings.WheelThickness,rob->cfg->robotSettings.WheelMass,0.9,0.9,0.9,wheeltexid);
cyl->setRotation(-sin(ang),cos(ang),0,M_PI*0.5);
cyl->setBodyRotation(-sin(ang),cos(ang),0,M_PI*0.5,true); //set local rotation matrix
cyl->setBodyPosition(centerx-x,centery-y,centerz-z,true); //set local position vector
cyl->space = rob->space;
rob->w->addObject(cyl);
joint = dJointCreateHinge (rob->w->world,0);
dJointAttach (joint,rob->chassis->body,cyl->body);
const dReal *a = dBodyGetPosition (cyl->body);
dJointSetHingeAxis (joint,cos(ang),sin(ang),0);
dJointSetHingeAnchor (joint,a[0],a[1],a[2]);
motor = dJointCreateAMotor(rob->w->world,0);
dJointAttach(motor,rob->chassis->body,cyl->body);
dJointSetAMotorNumAxes(motor,1);
dJointSetAMotorAxis(motor,0,1,cos(ang),sin(ang),0);
dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax);
speed = 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
}
}
void BallJoint::createMotorPhysics()
{
for(unsigned int i=0; i<this->motors.size(); i++)
{
dJointID physMotor = dJointCreateAMotor(*(this->simulation->getPhysicalWorld()), 0);
this->motors[i]->setPhysicalMotor(physMotor);
dJointAttach(physMotor, dJointGetBody(this->physicalJoint, 0), dJointGetBody(this->physicalJoint, 1));
if(this->motors[i]->getKind() == "eulermotor")
dJointSetAMotorMode(physMotor, dAMotorEuler);
else
dJointSetAMotorMode(physMotor, dAMotorUser);
//user defined motor
if(this->motors[i]->getKind() == "userdefinedmotor")
{
unsigned short numOfAxes = this->motors[i]->getNumberOfParsedAxes();
dJointSetAMotorNumAxes(physMotor, numOfAxes);
dBodyID b1= dJointGetBody(this->physicalJoint, 0);
if(b1 != NULL)
{
for(int j=0; j < numOfAxes; j++)
{
MotorAxisDescription* ax = this->motors[i]->getAxis(j);
dJointSetAMotorAxis(physMotor, j, 1, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2]));
}
}
else
{
for(int j=0; j < numOfAxes; j++)
{
MotorAxisDescription* ax = this->motors[i]->getAxis(j);
dJointSetAMotorAxis(physMotor, j, 0, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2]));
}
}
//maxForce and maxVelocity are stored in each axis of the motor
dJointSetAMotorParam(physMotor, dParamFMax, dReal(this->motors[i]->getAxis(0)->maxForce));
if(numOfAxes > 1)
{
dJointSetAMotorParam(physMotor, dParamFMax2, dReal(this->motors[i]->getAxis(1)->maxForce));
if(numOfAxes > 2)
{
dJointSetAMotorParam(physMotor, dParamFMax3, dReal(this->motors[i]->getAxis(2)->maxForce));
}
}
//stops are useless for balljoint
}
}
}
void capsule::create_physical_body(
double x,
double y,
double z,
double radius,
double length,
double mass,
manager& mgr)
{
this->radius = radius;
this->length = length;
world_id = mgr.ode_world();
space_id = mgr.ode_space();
//set the body orientation
// dMatrix3 R;
//dRFromAxisAndAngle(R,1,0,0,M_PI/2);
//dBodySetRotation(body_id,R);
//create the geom
geom_id=dCreateCapsule(mgr.ode_space(),radius,length);
object::set_geom_data(geom_id); //must make sure to set the geom data for the collision callback!
dGeomSetPosition (geom_id, x, y, z);
if(mass > 0)
{
object::create_rigid_body(x, y, z, mgr);
set_mass(mass);
dGeomSetBody(geom_id,body_id);
//create an amotor to keep the body vertical
amotor_id=dJointCreateAMotor(mgr.ode_world(),0);
dJointAttach(amotor_id,body_id,0);
dJointSetAMotorMode(amotor_id,dAMotorEuler);
dJointSetAMotorNumAxes(amotor_id,3);
dJointSetAMotorAxis(amotor_id,0,0,1,0,0);
dJointSetAMotorAxis(amotor_id,1,0,0,1,0);
dJointSetAMotorAxis(amotor_id,2,0,0,0,1);
dJointSetAMotorAngle(amotor_id,0,0);
dJointSetAMotorAngle(amotor_id,1,0);
dJointSetAMotorAngle(amotor_id,2,0);
dJointSetAMotorParam(amotor_id,dParamLoStop,-0);
dJointSetAMotorParam(amotor_id,dParamLoStop3,-0);
dJointSetAMotorParam(amotor_id,dParamLoStop2,-0);
dJointSetAMotorParam(amotor_id,dParamHiStop,0);
dJointSetAMotorParam(amotor_id,dParamHiStop3,0);
dJointSetAMotorParam(amotor_id,dParamHiStop2,0);
}
}
void CPHCapture::PullingUpdate()
{
if(!m_taget_element->isActive()||inl_ph_world().Device().dwTimeGlobal-m_time_start>m_capture_time)
{
Release();
return;
}
Fvector dir;
Fvector capture_bone_position;
//CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
capture_bone_position.set(m_capture_bone->mTransform.c);
m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
m_taget_element->GetGlobalPositionDynamic(&dir);
dir.sub(capture_bone_position,dir);
float dist=dir.magnitude();
if(dist>m_pull_distance)
{
Release();
return;
}
dir.mul(1.f/dist);
if(dist<m_capture_distance)
{
m_back_force=0.f;
m_joint=dJointCreateBall(0,0);
m_island.AddJoint(m_joint);
m_ajoint=dJointCreateAMotor(0,0);
m_island.AddJoint(m_ajoint);
dJointSetAMotorMode (m_ajoint, dAMotorEuler);
dJointSetAMotorNumAxes (m_ajoint, 3);
CreateBody();
dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
VERIFY( smart_cast<CPHElement*>(m_taget_element) );
CPHElement * e = static_cast<CPHElement*>(m_taget_element);
dJointAttach(m_joint,m_body,e->get_body());
dJointAttach(m_ajoint,m_body,e->get_body());
dJointSetFeedback (m_joint, &m_joint_feedback);
dJointSetFeedback (m_ajoint, &m_joint_feedback);
dJointSetBallAnchor(m_joint,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
dJointSetAMotorAxis (m_ajoint, 0, 1, dir.x, dir.y, dir.z);
if(dir.x>EPS)
{
if(dir.y>EPS)
{
float mag=dir.x*dir.x+dir.y*dir.y;
dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.y/mag, dir.x/mag, 0.f);
}
else if(dir.z>EPS)
{
float mag=dir.x*dir.x+dir.z*dir.z;
dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.z/mag,0.f,dir.x/mag);
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 1.f,0.f,0.f);
}
}
else
{
if(dir.y>EPS)
{
if(dir.z>EPS)
{
float mag=dir.y*dir.y+dir.z*dir.z;
dJointSetAMotorAxis (m_ajoint, 2, 2,0.f,-dir.z/mag,dir.y/mag);
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,1.f,0.f);
}
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,0.f,1.f);
}
}
//float hi=-M_PI/2.f,lo=-hi;
//dJointSetAMotorParam(m_ajoint,dParamLoStop ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop ,hi);
//dJointSetAMotorParam(m_ajoint,dParamLoStop2 ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop2 ,hi);
//dJointSetAMotorParam(m_ajoint,dParamLoStop3 ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop3 ,hi);
dJointSetAMotorParam(m_ajoint,dParamFMax ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel ,0.f);
dJointSetAMotorParam(m_ajoint,dParamFMax2 ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel2 ,0.f);
dJointSetAMotorParam(m_ajoint,dParamFMax3 ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel3 ,0.f);
///////////////////////////////////
float sf=0.1f,df=10.f;
float erp=ERP(world_spring*sf,world_damping*df);
float cfm=CFM(world_spring*sf,world_damping*df);
dJointSetAMotorParam(m_ajoint,dParamStopERP ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM ,cfm);
dJointSetAMotorParam(m_ajoint,dParamStopERP2 ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM2 ,cfm);
dJointSetAMotorParam(m_ajoint,dParamStopERP3 ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM3 ,cfm);
/////////////////////////////////////////////////////////////////////
///dJointSetAMotorParam(m_joint1,dParamFudgeFactor ,0.1f);
//dJointSetAMotorParam(m_joint1,dParamFudgeFactor2 ,0.1f);
//dJointSetAMotorParam(m_joint1,dParamFudgeFactor3 ,0.1f);
/////////////////////////////////////////////////////////////////////////////
sf=0.1f,df=10.f;
erp=ERP(world_spring*sf,world_damping*df);
cfm=CFM(world_spring*sf,world_damping*df);
dJointSetAMotorParam(m_ajoint,dParamCFM ,cfm);
dJointSetAMotorParam(m_ajoint,dParamCFM2 ,cfm);
dJointSetAMotorParam(m_ajoint,dParamCFM3 ,cfm);
///////////////////////////
//dJointSetAMotorParam(m_ajoint,dParamLoStop ,0.f);
//dJointSetAMotorParam(m_ajoint,dParamHiStop ,0.f);
m_taget_element->set_LinearVel ( Fvector().set( 0 ,0, 0 ) );
m_taget_element->set_AngularVel( Fvector().set( 0 ,0, 0 ) );
m_taget_element->set_DynamicLimits();
//m_taget_object->PPhysicsShell()->set_JointResistance()
e_state=cstCaptured;
return;
}
m_taget_element->applyForce(dir,m_pull_force);
}
dJointID ODE_PID_PassiveActuatorModel::createJoint(dBodyID body1, dBodyID body2) {
Vector3DValue *jointAxisPoint1 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint1"));
Vector3DValue *jointAxisPoint2 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint2"));
DoubleValue *minAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MinAngle"));
DoubleValue *maxAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MaxAngle"));
if(jointAxisPoint1 == 0 || jointAxisPoint2 == 0 || minAngleValue == 0 || maxAngleValue == 0) {
Core::log("ODE_PID_PassiveActuatorModel: Could not find all required parameter values.");
return 0;
}
if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) {
Core::log("ODE_PID_PassiveActuatorModel: Invalid axis points " + jointAxisPoint1->getValueAsString() + " and " + jointAxisPoint2->getValueAsString() + ".");
return 0;
}
if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) {
Core::log("Invalid axes for ODE_PID_PassiveActuatorModel.");
return 0;
}
Vector3D anchor = jointAxisPoint1->get();
Vector3D axis = jointAxisPoint2->get() - jointAxisPoint1->get();
dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
dJointAttach(joint, body1, body2);
dJointSetAMotorMode(joint, dAMotorEuler);
dJointSetAMotorParam(joint, dParamVel, 0.0);
dJointSetAMotorParam(joint, dParamFMax, 1.0);
dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());
axis.normalize();
Vector3D perpedicular;
if(axis.getY() != 0.0 || axis.getX() != 0.0) {
perpedicular.set(-axis.getY(), axis.getX(), 0);
}
else {
perpedicular.set(0, -axis.getZ(), axis.getY());
}
perpedicular.normalize();
// If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added.
if(body1 == 0) {
dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ());
}
else {
dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ());
}
if(body2 == 0) {
dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(),
-perpedicular.getZ());
}
else {
dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(),
perpedicular.getZ());
}
mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
dJointAttach(mHingeJoint, body1, body2);
dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ());
double minAngle = (minAngleValue->get() * Math::PI) / 180.0;
double maxAngle = (maxAngleValue->get() * Math::PI) / 180.0;
if(body1 == 0) {
double tmp = minAngle;
minAngle = -1.0 * maxAngle;
maxAngle = -1.0 * tmp;
}
dJointSetHingeParam(mHingeJoint, dParamLoStop, minAngle);
dJointSetHingeParam(mHingeJoint, dParamHiStop, maxAngle);
dJointSetHingeParam(mHingeJoint, dParamVel, 0.0);
return joint;
}
State* init() {
State* state = new State();
dInitODE();
SDL_Init(SDL_INIT_EVERYTHING);
state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL);
SDL_WM_SetCaption("Physics", NULL);
SDL_Flip(state->screen);
SDL_ShowCursor(SDL_DISABLE);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
GLfloat light_ambient[] = { 1, 1, 1, 1 };
glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
GLfloat lightpos[] = {0, 4, 0, 1};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
glShadeModel(GL_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glClearColor(0, 0, 0, 1);
state->posx = 0;//21;
state->posy = 4;//8;
state->posz = 5;//21;
state->rotx = 0;
state->roty = 0;//-40;
state->rotz = 0;
state->wkey = false;
state->akey = false;
state->skey = false;
state->dkey = false;
state->gkey = false;
state->simSpeed = 60;
state->carcam = false;
state->carbodydrawable = new Drawable("objs/carbody.obj");
state->carwheeldrawable = new Drawable("objs/carwheel.obj");
state->map = new Drawable("objs/jump2.obj");
state->cube = new Drawable("objs/cube.obj");
// state->monkey = new Drawable("objs/monkey.obj");
state->world = dWorldCreate();
dWorldSetGravity(state->world, 0, -9.8, 0);
state->worldSpace = dHashSpaceCreate(0);
const double carHeight = 0.65;
const double carZ = 90;
const double carX = 0;
const float speed = -1000;
const float force = 200;
state->carbodybody = dBodyCreate(state->world);
dBodySetPosition(state->carbodybody, carX, carHeight, carZ);
dMass bodymass;
dMassSetBoxTotal(&bodymass, 100, 2, 4, 1);
dBodySetMass(state->carbodybody, &bodymass);
dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4);
dGeomSetBody(carbodygeom, state->carbodybody);
state->flcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2);
const dMatrix3 m = { 0, 0, 1, 0
, 0, 1, 0, 0
, 0, 0, 1, 0 };
dBodySetRotation(state->flcarwheelbody, m);
dMass wheelmass;
dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2);
dBodySetMass(state->flcarwheelbody, &wheelmass);
dJointID joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->flcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(flcarwheelgeom, state->flcarwheelbody);
dJointID motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->flcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->frcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2);
dBodySetRotation(state->frcarwheelbody, m);
dBodySetMass(state->frcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->frcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(frcarwheelgeom, state->frcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->frcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->blcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->blcarwheelbody, m);
dBodySetMass(state->blcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->blcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(blcarwheelgeom, state->blcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->blcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->brcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->brcarwheelbody, m);
dBodySetMass(state->brcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->brcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(brcarwheelgeom, state->brcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->brcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->var = new double[3];
state->var = dBodyGetPosition(state->carbodybody);
// cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl;
//TODO check if translation matrix from dBody can be used.
dSpaceID cubespace = dHashSpaceCreate(state->worldSpace);
for(int i = 0; i < NUM_CUBES/10; i++) {
for(int k = 0; k < 10; k++) {
state->cubebody[i*10+k] = dBodyCreate(state->world);
dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1);
dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70);
dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2);
dGeomSetBody(cubegeom, state->cubebody[i*10+k]);
}
}
{
int indexSize = state->map->vertices.size()/3;
unsigned int* index = new unsigned int[indexSize];
for(int i = 0; i < indexSize; i++)
index[i] = i;
dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12);
dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
dGeomSetPosition(mapGeom, 0, 0, 0);
}
// state->monkeyBody = dBodyCreate(state->world);
// {
// int indexSize = state->monkey->vertices.size()/3;
// unsigned int* index = new unsigned int[indexSize];
// for(int i = 0; i < indexSize; i++)
// index[i] = i;
// dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
// dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12);
// dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
// dGeomSetPosition(monkeyGeom, 0, 2, 0);
// dGeomSetBody(monkeyGeom, state->monkeyBody);
// }
state->physicsContactgroup = dJointGroupCreate(0);
return state;
}
int setupTest (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0,
0.25*M_PI,0.25*M_PI);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetFixed (joint);
return 1;
}
case 1: { // 1 body to static env
constructWorldForTest (0,1,
0.5*SIDE,0.5*SIDE,1, 0,0,0,
1,0,0, 1,0,0,
0,0);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],0);
dJointSetFixed (joint);
return 1;
}
case 2: { // 2 body with relative rotation
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0,
0.25*M_PI,-0.25*M_PI);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetFixed (joint);
return 1;
}
case 3: { // 1 body to static env with relative rotation
constructWorldForTest (0,1,
0.5*SIDE,0.5*SIDE,1, 0,0,0,
1,0,0, 1,0,0,
0.25*M_PI,0);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],0);
dJointSetFixed (joint);
return 1;
}
// ********** hinge joint
case 200: // 2 body
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,1,-1,1.41421356);
return 1;
case 220: // hinge angle polarity test
case 221: // hinge angle rate test
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,0,1);
max_iterations = 50;
return 1;
case 230: // hinge motor rate (and polarity) test
case 231: // ...with stops
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,0,1);
dJointSetHingeParam (joint,dParamFMax,1);
if (n==231) {
dJointSetHingeParam (joint,dParamLoStop,-0.5);
dJointSetHingeParam (joint,dParamHiStop,0.5);
}
return 1;
case 250: // limit bounce test (gravity down)
case 251: { // ...gravity up
constructWorldForTest ((n==251) ? 0.1 : -0.1, 2,
0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,1,0);
dJointSetHingeParam (joint,dParamLoStop,-0.9);
dJointSetHingeParam (joint,dParamHiStop,0.7854);
dJointSetHingeParam (joint,dParamBounce,0.5);
// anchor 2nd body with a fixed joint
dJointID j = dJointCreateFixed (world,0);
dJointAttach (j,body[1],0);
dJointSetFixed (j);
return 1;
}
// ********** slider
case 300: // 2 body
constructWorldForTest (0,2,
0,0,1, 0.2,0.2,1.2,
0,0,1, -1,1,0, 0,0.25*M_PI);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,1,1,1);
return 1;
case 320: // slider angle polarity test
case 321: // slider angle rate test
constructWorldForTest (0,2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
max_iterations = 50;
return 1;
case 330: // slider motor rate (and polarity) test
case 331: // ...with stops
constructWorldForTest (0, 2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
dJointSetSliderParam (joint,dParamFMax,100);
if (n==331) {
dJointSetSliderParam (joint,dParamLoStop,-0.4);
dJointSetSliderParam (joint,dParamHiStop,0.4);
}
return 1;
case 350: // limit bounce tests
case 351: {
constructWorldForTest ((n==351) ? 0.1 : -0.1, 2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
dJointSetSliderParam (joint,dParamLoStop,-0.5);
dJointSetSliderParam (joint,dParamHiStop,0.5);
dJointSetSliderParam (joint,dParamBounce,0.5);
// anchor 2nd body with a fixed joint
dJointID j = dJointCreateFixed (world,0);
dJointAttach (j,body[1],0);
dJointSetFixed (j);
return 1;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
case 421: // hinge-2 steering angle rate test
constructWorldForTest (0,2,
0.5*SIDE,0,1, -0.5*SIDE,0,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge2 (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
dJointSetHinge2Axis1 (joint,0,0,1);
dJointSetHinge2Axis2 (joint,1,0,0);
max_iterations = 50;
return 1;
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: // ...with stops
case 432: // hinge 2 wheel motor rate (+polarity) test
constructWorldForTest (0,2,
0.5*SIDE,0,1, -0.5*SIDE,0,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge2 (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
dJointSetHinge2Axis1 (joint,0,0,1);
dJointSetHinge2Axis2 (joint,1,0,0);
dJointSetHinge2Param (joint,dParamFMax,1);
dJointSetHinge2Param (joint,dParamFMax2,1);
if (n==431) {
dJointSetHinge2Param (joint,dParamLoStop,-0.5);
dJointSetHinge2Param (joint,dParamHiStop,0.5);
}
return 1;
// ********** angular motor joint
case 600: // test euler angle calculations
constructWorldForTest (0,2,
-SIDE*0.5,0,1, SIDE*0.5,0,1,
0,0,1, 0,0,1, 0,0);
joint = dJointCreateAMotor (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetAMotorNumAxes (joint,3);
dJointSetAMotorAxis (joint,0,1, 0,0,1);
dJointSetAMotorAxis (joint,2,2, 1,0,0);
dJointSetAMotorMode (joint,dAMotorEuler);
max_iterations = 200;
return 1;
// ********** universal joint
case 700: // 2 body
case 701:
case 702:
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
joint = dJointCreateUniversal (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetUniversalAnchor (joint,0,0,1);
dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
return 1;
case 720: // universal transmit torque test
case 721:
case 722:
case 730: // universal torque about axis 1
case 731:
case 732:
case 740: // universal torque about axis 2
case 741:
case 742:
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateUniversal (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetUniversalAnchor (joint,0,0,1);
dJointSetUniversalAxis1 (joint,0,0,1);
dJointSetUniversalAxis2 (joint, 1, -1,0);
max_iterations = 100;
return 1;
}
return 0;
}
dJointID ODE_Dynamixel::createJoint(dBodyID body1, dBodyID body2) {
if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get(), -1)) {
Core::log("ODE_Dynamixel: Invalid axes for ODE_Dynamixel.", true);
return 0;
}
Vector3D anchor = mJointAxisPoint1->get();
Vector3D axis = mJointAxisPoint2->get() - mJointAxisPoint1->get();
dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
dJointAttach(joint, body1, body2);
dJointSetAMotorMode(joint, dAMotorEuler);
dJointSetAMotorParam(joint, dParamVel, 0.0);
dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get());
dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());
axis.normalize();
Vector3D perpedicular;
if(axis.getY() != 0.0 || axis.getX() != 0.0) {
perpedicular.set(-axis.getY(), axis.getX(), 0);
}
else {
perpedicular.set(0, -axis.getZ(), axis.getY());
}
perpedicular.normalize();
// If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added.
if(body1 == 0) {
dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ());
}
else {
dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ());
}
if(body2 == 0) {
dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(),
-perpedicular.getZ());
}
else {
dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(),
perpedicular.getZ());
}
mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
dJointAttach(mHingeJoint, body1, body2);
dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ());
if(body1 == 0) {
double tmp = mMinAngle;
mMinAngle = -1.0 * mMaxAngle;
mMaxAngle = -1.0 * tmp;
}
dJointSetHingeParam(mHingeJoint, dParamLoStop, mMinAngle);
dJointSetHingeParam(mHingeJoint, dParamHiStop, mMaxAngle);
dJointSetHingeParam(mHingeJoint, dParamVel, 0.0);
return joint;
}
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;
}
//since this is just the base level construction, we'll just make a sphere
bool AvatarConstruction::Construct( char* descriptionFile, DynamicsSolver* solver, Screen3D& Screen,MeshManager& MM, Position& Location, ICollisionHandler* aCollisionHandler )
{
//deconstruct any old stuff
Deconstruct();
//save the solver pointer
mySolver = solver;
this->CollisionHandler = aCollisionHandler;
//create the body list
ObjectList = new DynamicsObject[1];
this->nObjects = 1;
//Create the geom group
GeomGroup = dSimpleSpaceCreate (solver->GetSpaceID(false));
//set up surface properties
ObjectList[0].SurfaceDesc.ContactSlip1 = 0.01f;
ObjectList[0].SurfaceDesc.ContactSlip2 = 0.01f;
//set some other properties
for(int i=0; i<nObjects; i++)
{
ObjectList[i].SurfaceDesc.Owner = &ObjectList[i];
ObjectList[i].SurfaceDesc.ParentConstruction = this;
ObjectList[i].SurfaceDesc.SoftCFM = .01f;
ObjectList[i].SurfaceDesc.SoftERP = .1f;
ObjectList[i].SurfaceDesc.mu = 10.0f;
ObjectList[i].isAvatar = true;
ObjectList[i].Owner = this;
}
//Create the actual body ( a sphere! )
ObjectList[0].CreateBody( solver );
dBodySetPosition ( ObjectList[0].Body, Location.x, Location.y, Location.z);
dMassSetSphere ( &ObjectList[0].Mass, 1.0, 6.0 );
dMassAdjust (&ObjectList[0].Mass, 1.0 );
dBodySetMass( ObjectList[0].Body, &ObjectList[0].Mass);
ObjectList[0].Geom = dCreateSphere (0,6.0);
dGeomSetData( ObjectList[0].Geom, &ObjectList[0].SurfaceDesc );
dGeomSetBody (ObjectList[0].Geom,ObjectList[0].Body);
dSpaceAdd (GeomGroup,ObjectList[0].Geom);
ObjectList[0].HasGeom = true;
//create the angular motor
this->nJoints = 1;
JointList = new dJointID[nJoints];
JointList[0] = dJointCreateAMotor( solver->GetWorldID(), 0 );
dJointSetAMotorMode ( JointList[0], dAMotorUser );
dJointSetAMotorNumAxes( JointList[0], 3 );
dJointSetAMotorAxis(JointList[0], 0, 0, 1, 0, 0); //x axis
dJointSetAMotorAxis(JointList[0], 1, 0, 0, 1, 0); //y axis
dJointSetAMotorAxis(JointList[0], 2, 0, 0, 0, 1); //z axis
dJointAttach( JointList[0], ObjectList[0].Body, NULL); //attach to world
//set some properties
LinearDisableEpsilon = .2;
AngularDisableEpsilon = .01f;
JumpDelay = 1000;
LastJumpTime = 0;
ForceEnable = false;
//create the mesh for drawing
D3DXCreateSphere( Screen.D3DDevice, 6.0f, 20, 20, &DrawMesh, NULL );
return true;
}
void PhysicsAMotorJoint::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 = dJointCreateAMotor(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
if(!(whichField & VelFieldMask))
{
setVel(dJointGetAMotorParam(_JointID,dParamVel));
}
if(!(whichField & FMaxFieldMask))
{
setFMax(dJointGetAMotorParam(_JointID,dParamFMax));
}
if(!(whichField & FudgeFactorFieldMask))
{
setFudgeFactor(dJointGetAMotorParam(_JointID,dParamFudgeFactor));
}
if(!(whichField & Vel2FieldMask))
{
setVel2(dJointGetAMotorParam(_JointID,dParamVel2));
}
if(!(whichField & FMax2FieldMask))
{
setFMax2(dJointGetAMotorParam(_JointID,dParamFMax2));
}
if(!(whichField & FudgeFactor2FieldMask))
{
setFudgeFactor2(dJointGetAMotorParam(_JointID,dParamFudgeFactor2));
}
if(!(whichField & Vel3FieldMask))
{
setVel3(dJointGetAMotorParam(_JointID,dParamVel3));
}
if(!(whichField & FMax3FieldMask))
{
setFMax3(dJointGetAMotorParam(_JointID,dParamFMax3));
}
if(!(whichField & FudgeFactor3FieldMask))
{
setFudgeFactor3(dJointGetAMotorParam(_JointID,dParamFudgeFactor3));
}
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetAMotorParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetAMotorParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetAMotorParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetAMotorParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetAMotorParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetAMotorParam(_JointID,dParamStopERP));
}
if(!(whichField & HiStop2FieldMask))
{
setHiStop2(dJointGetAMotorParam(_JointID,dParamHiStop2));
}
if(!(whichField & LoStop2FieldMask))
{
setLoStop2(dJointGetAMotorParam(_JointID,dParamLoStop2));
}
if(!(whichField & Bounce2FieldMask))
{
setBounce2(dJointGetAMotorParam(_JointID,dParamBounce2));
}
if(!(whichField & CFM2FieldMask))
{
setCFM2(dJointGetAMotorParam(_JointID,dParamCFM2));
}
if(!(whichField & StopCFM2FieldMask))
{
setStopCFM2(dJointGetAMotorParam(_JointID,dParamStopCFM2));
}
if(!(whichField & StopERP2FieldMask))
{
setStopERP2(dJointGetAMotorParam(_JointID,dParamStopERP2));
}
if(!(whichField & HiStop3FieldMask))
{
setHiStop3(dJointGetAMotorParam(_JointID,dParamHiStop3));
}
if(!(whichField & LoStop3FieldMask))
{
setLoStop3(dJointGetAMotorParam(_JointID,dParamLoStop3));
}
if(!(whichField & Bounce3FieldMask))
{
setBounce3(dJointGetAMotorParam(_JointID,dParamBounce3));
}
if(!(whichField & CFM3FieldMask))
{
setCFM3(dJointGetAMotorParam(_JointID,dParamCFM3));
}
if(!(whichField & StopCFM3FieldMask))
{
setStopCFM3(dJointGetAMotorParam(_JointID,dParamStopCFM3));
}
if(!(whichField & StopERP3FieldMask))
{
setStopERP3(dJointGetAMotorParam(_JointID,dParamStopERP3));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & NumAxesFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorNumAxes(_JointID,getNumAxes());
}
if((whichField & Axis1FieldMask) ||
(whichField & Axis1ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,0, getAxis1ReferenceFrame(), getAxis1().x(), getAxis1().y(), getAxis1().z());
}
if((whichField & Axis2FieldMask) ||
(whichField & Axis2ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,1, getAxis2ReferenceFrame(), getAxis2().x(), getAxis2().y(), getAxis2().z());
}
if((whichField & Axis3FieldMask) ||
(whichField & Axis3ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,2, getAxis3ReferenceFrame(), getAxis3().x(), getAxis3().y(), getAxis3().z());
}
if((whichField & VelFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel, getVel());
}
if((whichField & FMaxFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax, getFMax());
}
if((whichField & FudgeFactorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor, getFudgeFactor());
}
if((whichField & Vel2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel2, getVel2());
}
if((whichField & FMax2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax2, getFMax2());
}
if((whichField & FudgeFactor2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor2, getFudgeFactor2());
}
if((whichField & Vel3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel3, getVel3());
}
if((whichField & FMax3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax3, getFMax3());
}
if((whichField & FudgeFactor3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor3, getFudgeFactor3());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM, getStopCFM());
}
if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop2, getHiStop2());
}
if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop2, getLoStop2());
}
if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce2, getBounce2());
}
if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM2, getCFM2());
}
if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP2, getStopERP2());
}
if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM2, getStopCFM2());
}
if((whichField & HiStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop3, getHiStop3());
}
if((whichField & LoStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop3, getLoStop3());
}
if((whichField & Bounce3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce3, getBounce3());
}
if((whichField & CFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM3, getCFM3());
}
if((whichField & StopERP3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP3, getStopERP3());
}
if((whichField & StopCFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM3, getStopCFM3());
}
}
dJointID ODE_U_FrictionTorqueMotorModel::createJoint(dBodyID body1, dBodyID body2) {
if(mJointAxis1Point1 == 0 || mJointAxis1Point2 == 0
|| mJointAxis2Point1 == 0 || mJointAxis2Point2 == 0)
{
Core::log("ODE_U_FrictionTorqueMotorModel: Could not find all required parmaeter Values.");
return 0;
}
if(mJointAxis1Point1->get().equals(mJointAxis1Point2->get(), -1)
|| mJointAxis2Point1->get().equals(mJointAxis2Point2->get(), -1))
{
Core::log("Invalid axes for ODE_U_FrictionTorqueMotorModel: " + mJointAxis1Point1->getValueAsString() + " "
+ mJointAxis1Point2->getValueAsString() + " " + mJointAxis2Point1->getValueAsString() + " "
+ mJointAxis2Point2->getValueAsString());
return 0;
}
Vector3D anchor = mAnchor->get();
Vector3D axis1 = mJointAxis1Point2->get() - mJointAxis1Point1->get();
Vector3D axis2 = mJointAxis2Point2->get() - mJointAxis2Point1->get();
dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
dJointAttach(joint, body1, body2);
dJointSetAMotorMode(joint, dAMotorEuler);
dJointSetAMotorNumAxes(joint, 2);
dJointSetAMotorParam(joint, dParamVel, 0.0);
dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get());
dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());
axis1.normalize();
Vector3D perpedicular1;
if(axis1.getY() != 0.0 || axis1.getX() != 0.0) {
perpedicular1.set(-axis1.getY(), axis1.getX(), 0);
}
else {
perpedicular1.set(0, -axis1.getZ(), axis1.getY());
}
perpedicular1.normalize();
// If one of the bodies is static, the motor axis need to be defined in a different way.
// For different constellations of static and dynamic bodies, the turn direction
// of the motor changed, so this had to be added.
if(body1 == 0) {
dJointSetAMotorAxis(joint, 0, 0, -axis1.getX(), -axis1.getY(), -axis1.getZ());
}
else {
dJointSetAMotorAxis(joint, 0, 1, axis1.getX(), axis1.getY(), axis1.getZ());
}
if(body2 == 0) {
dJointSetAMotorAxis(joint, 2, 0, -perpedicular1.getX(), -perpedicular1.getY(),
-perpedicular1.getZ());
}
else {
dJointSetAMotorAxis(joint, 2, 2, perpedicular1.getX(), perpedicular1.getY(),
perpedicular1.getZ());
}
mUniversalJoint = dJointCreateUniversal(mWorldID, mGeneralJointGroup);
dJointAttach(mUniversalJoint, body1, body2);
dJointSetUniversalAnchor(mUniversalJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetUniversalAxis1(mUniversalJoint, axis1.getX(), axis1.getY(), axis1.getZ());
dJointSetUniversalAxis2(mUniversalJoint, axis2.getX(), axis2.getY(), axis2.getZ());
double minAngle1 = (mAxis1MinAngle->get() * Math::PI) / 180.0;
double minAngle2 = (mAxis2MinAngle->get() * Math::PI) / 180.0;
double maxAngle1 = (mAxis1MaxAngle->get() * Math::PI) / 180.0;
double maxAngle2 = (mAxis2MaxAngle->get() * Math::PI) / 180.0;
// if(body1 == 0) {
// double tmp =
// mMinAngle = -1.0 * mMaxAngle;
// mMaxAngle = -1.0 * tmp;
// }
dJointSetUniversalParam(mUniversalJoint, dParamLoStop, minAngle1);
dJointSetUniversalParam(mUniversalJoint, dParamHiStop, maxAngle1);
dJointSetUniversalParam(mUniversalJoint, dParamLoStop2, minAngle2);
dJointSetUniversalParam(mUniversalJoint, dParamHiStop2, maxAngle2);
dJointSetUniversalParam(mUniversalJoint, dParamVel, 0.0);
dJointSetUniversalParam(mUniversalJoint, dParamVel2, 0.0);
return joint;
}