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 } } }
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 } }
FixtureXEuler() { // body only allowed to rotate around X axis dJointAttach(joint, 0, body); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0); dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1); }
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; }
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; }
//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; }
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; }