void BallJoint::act(bool initial)
{
if(!initial)
{
for(unsigned int i=0; i < this->motors.size(); i++)
{
int numAxes = this->motors[i]->getNumberOfParsedAxes();
if(numAxes > 0)
{
if(this->motors[i]->getMotorType() == VELOCITY)
{
double desiredVelocity = this->motors[i]->getDesiredVelocity();
dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel, dReal(desiredVelocity));
}
if(numAxes > 1)
{
if(this->motors[i]->getMotorType() == VELOCITY)
{
double desiredVelocity = this->motors[i]->getDesiredVelocity();
dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel2, dReal(desiredVelocity));
}
if(numAxes > 2)
{
if(this->motors[i]->getMotorType() == VELOCITY)
{
double desiredVelocity = this->motors[i]->getDesiredVelocity();
dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel3, dReal(desiredVelocity));
}
}
}
}
}
}
}
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 osaODEServoMotor::SetVelocity( double qd ){
if( dJointGetType( MotorID() ) == dJointTypeAMotor ){
dJointSetAMotorParam( MotorID(), dParamVel, qd );
dJointSetAMotorParam( MotorID(), dParamFMax, ftmax );
}
if( dJointGetType( MotorID() ) == dJointTypeLMotor ){
dJointSetLMotorParam( MotorID(), dParamVel, qd/1000.0 );
dJointSetLMotorParam( MotorID(), dParamFMax, ftmax );
}
}
// 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;
}
void ODE_Dynamixel::valueChanged(Value *value) {
Dynamixel::valueChanged(value);
if((value == mDesiredMotorTorqueValue || value == mFMaxFactor) && mJoint != 0) {
dJointSetAMotorParam(mJoint, dParamFMax, mFMaxFactor->get()
* Math::max(0.0, mDesiredMotorTorqueValue->get()));
}
}
void ODE_Dynamixel::updateActuators() {
if(mJoint != 0) {
mCurrentPosition = dJointGetAMotorAngle(mJoint, 0);
calculateMotorFrictionAndVelocity();
dJointSetHingeParam(mHingeJoint, dParamFMax, mCalculatedFriction);
dJointSetAMotorParam(mJoint, dParamVel, mCalculatedVelocity);
}
}
void ODE_PID_PassiveActuatorModel::updateInputValues()
{
if(mJoint != 0) {
double desiredAngle = 0.0;
PassiveActuatorAdapter *owner = dynamic_cast<PassiveActuatorAdapter*>(mOwner);
if(owner != 0) {
desiredAngle = (owner->getReferenceAngle() + owner->getReferenceOffsetAngle())
* owner->getGearRatio() + owner->getOffsetAngle();
}
double currentAngle = dJointGetAMotorAngle(mJoint, 0);
mPID_Controller.update(currentAngle, desiredAngle * Math::PI / 180.0);
dJointSetAMotorParam(mJoint, dParamFMax, mMaxForce->get());
dJointSetHingeParam(mHingeJoint, dParamFMax, mFriction->get());
dJointSetAMotorParam(mJoint, dParamVel, mPID_Controller.getVelocity());
}
}
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);
}
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;
}
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());
}
}
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;
}
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;
}
void Robot::Wheel::step()
{
dJointSetAMotorParam(motor,dParamVel,speed);
dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax);
}
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;
}