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;
}
