void Hinge::createPhysics()
{
ASSERT(axis);
//
axis->create();
//
::PhysicalObject::createPhysics();
// find bodies to connect
Body* parentBody = dynamic_cast<Body*>(parent);
ASSERT(!parentBody || parentBody->body);
ASSERT(!children.empty());
Body* childBody = dynamic_cast<Body*>(*children.begin());
ASSERT(childBody);
ASSERT(childBody->body);
// create joint
joint = dJointCreateHinge(Simulation::simulation->physicalWorld, 0);
dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
//set hinge joint parameter
dJointSetHingeAnchor(joint, pose.translation.x, pose.translation.y, pose.translation.z);
Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
dJointSetHingeAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
if(axis->cfm != -1.f)
dJointSetHingeParam(joint, dParamCFM, dReal(axis->cfm));
if(axis->deflection)
{
const Axis::Deflection& deflection = *axis->deflection;
float minHingeLimit = deflection.min;
float maxHingeLimit = deflection.max;
if(minHingeLimit > maxHingeLimit)
minHingeLimit = maxHingeLimit;
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
float internalTolerance = (maxHingeLimit - minHingeLimit) * 0.1f;
if(dynamic_cast<ServoMotor*>(axis->motor))
{
minHingeLimit -= internalTolerance;
maxHingeLimit += internalTolerance;
}
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
dJointSetHingeParam(joint, dParamHiStop, dReal(maxHingeLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
if(deflection.stopCFM != -1.f)
dJointSetHingeParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
if(deflection.stopERP != -1.f)
dJointSetHingeParam(joint, dParamStopERP, dReal(deflection.stopERP));
}
// create motor
if(axis->motor)
axis->motor->create(this);
OpenGLTools::convertTransformation(rotation, translation, transformation);
}
/*** 車輪の生成 ***/
void makeWheel()
{
dMass mass;
dReal r = 0.1;
dReal w = 0.024;
dReal m = 0.15;
dReal d = 0.01;
dReal tmp_z = Z;
dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0};
dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)};
dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0};
dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2};
dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
for (int i = 0; i < WHEEL_NUM; i++)
{
wheel[i].v = 0.0;
// make body, geom and set geom to body.
// The position and orientation of geom is abondoned.
wheel[i].body = dBodyCreate(world);
wheel[i].geom = dCreateCylinder(space,r, w);
dGeomSetBody(wheel[i].geom,wheel[i].body);
// set configure of body
dMassSetZero(&mass);
if (i < 2) {
dMassSetCylinderTotal(&mass,m, 1, r, w);
} else {
dMassSetCylinderTotal(&mass,m, 2, r, w);
}
dBodySetMass(wheel[i].body,&mass);
// set position and orientation of body.
dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]);
dMatrix3 R;
if (i >= 2) {
dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
} else {
dRFromAxisAndAngle(R,0,1,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
}
// make joint.
wheel[i].joint = dJointCreateHinge(world,0);
dJointAttach(wheel[i].joint,base.body,wheel[i].body);
if (i < 2) {
dJointSetHingeAxis(wheel[i].joint, 1, 0, 0);
} else {
dJointSetHingeAxis(wheel[i].joint, 0, -1, 0);
}
dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]);
}
}
/**
* This method is called if the joint should be attached.
* It creates the ODE-joint, calculates the current anchor-position
* and axis-orientation and attaches the Joint.
* @param obj1 first ODE-object to attach with
* @param obj2 second ODE-object to attach with
**/
void HingeJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
gmtl::Vec3f newAnchor, newAxis, scaleVec;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
TransformationData entityTrans;
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, obj1, obj2);
newAnchor = anchor;
newAxis = axis;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
scaleVec = entityTrans.scale;
// scale Anchor-offset by mainEntity-scale value
newAnchor[0] *= scaleVec[0];
newAnchor[1] *= scaleVec[1];
newAnchor[2] *= scaleVec[2];
// scale Axis by mainEntity-scale value because of possible distortion
newAxis[0] *= scaleVec[0];
newAxis[1] *= scaleVec[1];
newAxis[2] *= scaleVec[2];
gmtl::normalize(newAxis);
// get the Rotation of the mainEntity
entityRot = entityTrans.orientation;
// rotate Axis by mainEntity-rotation
newAxis *= entityRot;
// rotate Anchor-offset by mainEntity-rotation
newAnchor *= entityRot;
// transform new Anchor to world coordinates
newAnchor += entityTrans.position;
} // if
dJointSetHingeAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
dJointSetHingeAxis(joint, newAxis[0], newAxis[1], newAxis[2]);
// ODE parameters for minimizing failure in Hinge Joint
// It should not be correct this way but it works better than without it ;-)
dJointSetHingeParam(joint, dParamCFM, 0);
dJointSetHingeParam(joint, dParamStopERP, 0);
dJointSetHingeParam(joint, dParamStopCFM, 0);
oldAngle = 0;
// set the maximum and minimum Joint-angles (if set).
if (anglesSet)
setODEAngles();
} // attachJoint
static void soy_joints_hinge_real_create (soyjointsJoint* base) {
soyjointsHinge * self;
struct dxWorld* _tmp0_;
struct dxJoint* _tmp1_;
self = (soyjointsHinge*) base;
_tmp0_ = soy_scenes__world;
_tmp1_ = dJointCreateHinge (_tmp0_, NULL);
_dJointDestroy0 (((soyjointsJoint*) self)->joint);
((soyjointsJoint*) self)->joint = _tmp1_;
}
bool ActiveHingeModel::initModel() {
// Create the 4 components of the hinge
hingeRoot_ = this->createBody(B_SLOT_A_ID);
dBodyID frame = this->createBody(B_FRAME_ID);
dBodyID servo = this->createBody(B_SERVO_ID);
hingeTail_ = this->createBody(B_SLOT_B_ID);
// Set the masses for the various boxes
float separation = 0;//inMm(0.1);
this->createBoxGeom(hingeRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
dReal xFrame = separation + FRAME_LENGTH / 2 + SLOT_THICKNESS / 2;
this->createBoxGeom(frame, MASS_FRAME, osg::Vec3(xFrame,
SERVO_POSITION_OFFSET, 0),
FRAME_LENGTH, FRAME_WIDTH, FRAME_HEIGHT);
dReal xServo = xFrame + (FRAME_ROTATION_OFFSET - (FRAME_LENGTH / 2))
+ SERVO_ROTATION_OFFSET - SERVO_LENGTH/2;
this->createBoxGeom(servo, MASS_SERVO, osg::Vec3(xServo,
SERVO_POSITION_OFFSET, 0),
SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT);
dReal xTail = xServo + SERVO_LENGTH / 2 + separation + SLOT_THICKNESS / 2;
this->createBoxGeom(hingeTail_, MASS_SLOT, osg::Vec3(xTail, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
// Create joints to hold pieces in position
// root <-> connectionPartA
this->fixBodies(hingeRoot_, frame, osg::Vec3(1, 0, 0));
// connectionPartA <(hinge)> connectionPArtB
dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint, frame, servo);
dJointSetHingeAxis(joint, 0, 1, 0);
dJointSetHingeAnchor(joint,
SLOT_THICKNESS / 2 + separation + FRAME_ROTATION_OFFSET, 0, 0);
// connectionPartB <-> tail
this->fixBodies(servo, hingeTail_, osg::Vec3(1, 0, 0));
// Create servo
this->motor_.reset(
new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
ServoMotor::DEFAULT_GAIN,
ioPair(this->getId(),0)));
return true;
}
static dJointID create_phys_joint(int t)
{
switch (t)
{
case dJointTypeBall: return dJointCreateBall (world, 0);
case dJointTypeHinge: return dJointCreateHinge (world, 0);
case dJointTypeSlider: return dJointCreateSlider (world, 0);
case dJointTypeUniversal: return dJointCreateUniversal(world, 0);
case dJointTypeHinge2: return dJointCreateHinge2 (world, 0);
default: return 0;
}
}
void JOINT::Create_In_Simulator(dWorldID world, OBJECT *firstObject, OBJECT *secondObject) {
joint = dJointCreateHinge(world,0);
dJointAttach( joint , firstObject->Get_Body() , secondObject->Get_Body() );
dJointSetHingeAnchor(joint,x,y,z);
dJointSetHingeAxis(joint,normalX,normalY,normalZ);
dJointSetHingeParam(joint,dParamLoStop,lowStop);
dJointSetHingeParam(joint,dParamHiStop,highStop);
}
IoODEHinge *IoODEHinge_rawClone(IoODEHinge *proto)
{
IoObject *self = IoODEJoint_rawClone(proto);
if(DATA(proto)->jointGroup)
{
IoODEJointGroup *jointGroup = DATA(proto)->jointGroup;
JOINTGROUP = jointGroup;
IoODEJointGroup_addJoint(jointGroup, self);
JOINTID = dJointCreateHinge(WORLDID, JOINTGROUPID);
}
return self;
}
AppHalf::AppHalf(dWorldID world, dSpaceID space, AppDesignID app_design,
dBodyID main_body, dBodyID blade)
: ebMain(main_body), ebBlade(blade)
{
// NOTE: app_design must have a side called before this, so that A..E and K..Q are one of left or right
// create bodies with geometries
// create links and connect them
jA = dJointCreateHinge(world, 0);
dJointSetHingeAxis(jA, 0, 1, 0); // is -1 or 1 a matter? only for angle and anglerate sign, i think.
dReal *A = app_design->getA();
dJointSetHingeAnchor(jA, A[0], A[1], A[2]);
dJointConnect(jA, ebMain, bBeam);
}
static void soy_joints_hinge_real_create (soyjointsJoint* base) {
soyjointsHinge * self;
struct dxWorld* _tmp0_ = NULL;
struct dxJoint* _tmp1_ = NULL;
#line 29 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
self = (soyjointsHinge*) base;
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp0_ = soy_scenes__world;
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp1_ = dJointCreateHinge (_tmp0_, NULL);
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_dJointDestroy0 (((soyjointsJoint*) self)->joint);
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
((soyjointsJoint*) self)->joint = _tmp1_;
#line 247 "Hinge.c"
}
/*** ロボットアームの生成 ***/
void makeArm()
{
dMass mass; // 質量パラメータ
dReal x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 x
dReal y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 y
dReal z[NUM] = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0}; // 重心 z
dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50}; // 長さ
dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50}; // 質量
dReal r[NUM-1] = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; // 半径
dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 x
dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 y
dReal c_z[NUM] = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9}; // 関節中心点 z
dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0}; // 関節回転軸 x
dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0}; // 関節回転軸 y
dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1}; // 関節回転軸 z
// リンクの生成
for (int i = 0; i < NUM-1; i++) {
rlink[i].body = dBodyCreate(world);
dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
dBodySetMass(rlink[i].body, &mass);
rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
dGeomSetBody(rlink[i].geom,rlink[i].body);
}
rlink[NUM-1].body = dBodyCreate(world);
dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
dBodySetMass(rlink[NUM-1].body, &mass);
rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);
// ジョイントの生成とリンクへの取り付け
joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント
dJointAttach(joint[0], rlink[0].body, 0);
dJointSetFixed(joint[0]);
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
}
}
/**
* Creates a new ODE_HingeJoint.
*
* @param body1 the first body to connect the joint to.
* @param body2 the second body to connect the joint to.
* @return the new ODE_HingeJoint.
*/
dJointID ODE_HingeJoint::createJoint(dBodyID body1, dBodyID body2) {
if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get())) {
Core::log("Invalid axes for ODE_HingeJoint.");
return 0;
}
//if one of the bodyIDs is null, the joint is connected to a static object.
dJointID newJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
dJointAttach(newJoint, body1, body2);
Vector3D anchor = mJointAxisPoint1->get() ;
Vector3D axis = mJointAxisPoint2->get() ;
axis.set(mJointAxisPoint2->getX() - mJointAxisPoint1->getX(), mJointAxisPoint2->getY() - mJointAxisPoint1->getY(), mJointAxisPoint2->getZ() - mJointAxisPoint1->getZ());
dJointSetHingeAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetHingeAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
return newJoint;
}
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);
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dQuaternion q;
dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
dBodySetQuaternion (body[0],q);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
dBodySetQuaternion (body[1],q);
hinge = dJointCreateHinge (world,0);
dJointAttach (hinge,body[0],body[1]);
dJointSetHingeAnchor (hinge,0,0,1);
dJointSetHingeAxis (hinge,1,-1,1.41421356);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void createHingeLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID &joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius, dReal length,
dReal maxAngle, dReal minAngle,
dReal anchorXPos, dReal anchorYPos, dReal anchorZPos)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
//dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//hinge joint
joint = dJointCreateHinge(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetHingeAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetHingeAxis(joint, 0, 1, 0);
//Max and min angles
dJointSetHingeParam(joint, dParamHiStop, maxAngle);
dJointSetHingeParam(joint, dParamLoStop, minAngle);
}
sODEJoint* sODEJoint::AddHingeJoint( int id, dBodyID body1, dBodyID body2, float ax, float ay, float az, float x, float y, float z )
{
sODEJoint *pJoint = new sODEJoint( );
//modify prev and next pointers of any involved joints, make new joint the new head
pJoint->pNextJoint = pODEJointList;
if ( pODEJointList ) pODEJointList->pPrevJoint = pJoint;
pODEJointList = pJoint;
//create the ODE hinge joint
pJoint->oJoint = dJointCreateHinge( g_ODEWorld, 0 );
dJointAttach( pJoint->oJoint, body1, body2 );
dJointSetHingeAnchor( pJoint->oJoint, x,y,z );
dJointSetHingeAxis( pJoint->oJoint, ax,ay,az );
pJoint->iID = id;
return pJoint;
}
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
int main(int argc, char *argv[]) {
dsFunctions fn;
double x[NUM] = {0.00}, y[NUM] = {0.00}; // Center of gravity
double z[NUM] = { 0.05, 0.50, 1.50, 2.55};
double m[NUM] = {10.00, 2.00, 2.00, 2.00}; // mass
double anchor_x[NUM] = {0.00}, anchor_y[NUM] = {0.00};// anchors of joints
double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00};
double axis_x[NUM] = { 0.00, 0.00, 0.00, 0.00}; // axises of joints
double axis_y[NUM] = { 0.00, 0.00, 1.00, 1.00};
double axis_z[NUM] = { 1.00, 1.00, 0.00, 0.00};
fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop;
fn.command = &command;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE(); // Initialize ODE
world = dWorldCreate(); // Create a world
dWorldSetGravity(world, 0, 0, -9.8);
for (int i = 0; i < NUM; i++) {
dMass mass;
link[i] = dBodyCreate(world);
dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position
dMassSetZero(&mass); // Set mass parameter to zero
dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]); // Calculate mass parameter
dBodySetMass(link[i], &mass); // Set mass
}
joint[0] = dJointCreateFixed(world, 0); // A fixed joint
dJointAttach(joint[0], link[0], 0); // Attach the joint between the ground and the base
dJointSetFixed(joint[0]); // Set the fixed joint
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint
dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint
dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]);
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]);
}
dsSimulationLoop(argc, argv, 640, 570, &fn); // Simulation loop
dCloseODE();
return 0;
}
void createLeg( const dWorldID & _world ){
// read leg parameters there
lower_part.mass = 0.5;
lower_part.radius = 0.1;
lower_part.length = 0.5;
dReal lower_part_pos[3] = {0.0, 0.0, lower_part.length};
upper_part.mass = 0.5;
upper_part.radius = 0.1;
upper_part.length = 0.5;
dReal upper_part_pos[3] = {0.0, 0.0, upper_part.length + lower_part.length};
createPart(_world,lower_part, lower_part_pos);
createPart(_world,upper_part, upper_part_pos);
jointHinge = dJointCreateHinge(_world, 0); // Create a hinge joint
dJointAttach(jointHinge, lower_part.body, upper_part.body ); // Attach the joint
dJointSetHingeAnchor(jointHinge, 1.0,0.0,0.0);//anchor_x[j], anchor_y[j],anchor_z[j]);
}
bool ActiveWheelModel::initModel() {
// Create the 4 components of the hinge
wheelRoot_ = this->createBody(B_SLOT_ID);
dBodyID servo = this->createBody(B_SERVO_ID);
dBodyID wheel = this->createBody(B_WHEEL_ID);
// Set the masses for the various boxes
dMass mass;
this->createBoxGeom(wheelRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
dReal zServo = 0;//-SLOT_WIDTH / 2 + SERVO_Z_OFFSET + SERVO_HEIGHT / 2;
this->createBoxGeom(servo, MASS_SERVO, osg::Vec3(X_SERVO, 0, zServo),
SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT);
this->createCylinderGeom(wheel, MASS_WHEEL, osg::Vec3(X_WHEEL, 0, 0), 1,
getRadius(), WHEEL_THICKNESS);
// Create joints to hold pieces in position
// slot <slider> hinge
this->fixBodies(wheelRoot_, servo, osg::Vec3(1, 0, 0));
// servo <(hinge)> wheel
dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint, servo, wheel);
dJointSetHingeAxis(joint, 1, 0, 0);
dJointSetHingeAnchor(joint, X_WHEEL, 0, 0);
// Create servo
this->motor_.reset(
new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE,
ioPair(this->getId(),0)));
return true;
}
Else::AcrobotArticulatedBody
::AcrobotArticulatedBody( dWorldID argWorldID,
dSpaceID argSpaceID,
double argScale )
: ArticulatedBody( argWorldID, argSpaceID ),
myRadius( 0.1 * argScale ),
myLength( argScale ),
myJointGroupID( 0 )
{
// create body
gmtl::Vec3d dims( 2*myRadius, 2*myRadius, 2*myRadius + myLength );
const double density = 1.0;
const int zDirection = 3;
const double extraSpace = 0.1*myLength;
dMass mass;
dBodyID body = dBodyCreate( myWorldID );
dBodySetData( body, this );
// create cylinder aligned to z-axis
dGeomID geom = dCreateCapsule( mySpaceID, myRadius, myLength );
dGeomSetBody( geom, body );
dBodySetPosition( body, 0, 0, extraSpace + 0.5*myLength + myRadius );
dMassSetCappedCylinder( &mass, density, zDirection, myRadius, myLength );
dBodySetMass( body, &mass );
dJointID joint = dJointCreateHinge( myWorldID, myJointGroupID );
dJointAttach( joint, NULL, body );
dJointSetHingeAnchor( joint, 0, 0, extraSpace + myLength + myRadius );
dVector3 axis;
axis[0] = 0; axis[1] = 1; axis[2] = 0;
dJointSetHingeAxis( joint, axis[0], axis[1], axis[2] );
myBodies[0] = body;
myJoints[0] = joint;
myBodyDims[0] = dims;
myGeoms.push_back( geom );
}
//! A hinge requires a fixed anchor point and an axis
OscHingeODE::OscHingeODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase* parent,
OscObject *object1, OscObject *object2,
double x, double y, double z, double ax, double ay, double az)
: OscHinge(name, parent, object1, object2, x, y, z, ax, ay, az)
{
m_response = new OscResponse("response",this);
// create the constraint for object1
cVector3d anchor(x,y,z);
cVector3d axis(ax,ay,az);
dJointID odeJoint = dJointCreateHinge(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetHingeAnchor(odeJoint, anchor.x, anchor.y, anchor.z);
dJointSetHingeAxis(odeJoint, axis.x, axis.y, axis.z);
printf("Hinge joint created between %s and %s at anchor (%f,%f,%f), axis (%f,%f,%f)\n",
object1->c_name(), object2?object2->c_name():"world", x,y,z,ax,ay,az);
}
void makeRobot_Nleg()
{
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
dReal torso_m = 50.0; // Mass of body
// torso_m[segment] = 10.0;
dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments
//for four legs
// dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
// {-cx1,-cx1,-cx1}};
dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
{0,0,0}};
dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
{-cy1,-cy1,-cy1}};
dReal z[num_legs][num_links] = { // Position of each link (z coordinate)
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
dReal r[num_links] = { r1, r2, r3}; // radius of leg segment
dReal length[num_links] = { l1, l2, l3}; // Length of leg segment
dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment
// //for one leg
// dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
// dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
// dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
//for four legs
dReal axis_x[num_legs][num_links];
dReal axis_y[num_legs][num_links];
dReal axis_z[num_legs][num_links];
for(int i = 0; i < num_legs; ++i) {
axis_x[i][0] = 0.0;
axis_x[i][1] = 1.0;
axis_x[i][2] = 0.0;
axis_y[i][0] = 1.0;
axis_y[i][1] = 0.0;
axis_y[i][2] = 1.0;
axis_z[i][0] = 0.0;
axis_z[i][1] = 0.0;
axis_z[i][2] = 0.0;
}
// For mation of the body
dMass mass;
torso[segment].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
dBodySetMass(torso[segment].body,&mass);
torso[segment].geom = dCreateBox(space,lx, ly, lz);
dGeomSetBody(torso[segment].geom, torso[segment].body);
dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ);
// Formation of leg
dMatrix3 R; // Revolution queue
dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
THETA[segment][i][j] = 0;
leg[segment][i][j].body = dBodyCreate(world);
if (j == 0)
dBodySetRotation(leg[segment][i][j].body,R);
dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
dBodySetMass(leg[segment][i][j].body, &mass);
//if(i==1 and j==2) //to set the length of one leg differently
//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
//else
leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
}
}
// Formation of joints (and connecting them up)
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[segment][i][j].joint = dJointCreateHinge(world, 0);
if (j == 0){
dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop, .50*M_PI);
}
else
dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
}
}
}
#ifdef USE_NLEG_ROBOT
// link torsos
for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
switch (hingeType) {
case 1: //Hinge Joint, X axis (back-breaker)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
break;
case 2: //Hinge Joint, Y axis (???)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
break;
case 3: //Hinge Joint, Z axis (snake-like)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
break;
case 4: // Slider, Y axis (??)
torso[segment].joint = dJointCreateSlider(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
break;
case 5: // Slider, X axis (extendo)
torso[segment].joint = dJointCreateSlider(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);
break;
case 6: //Universal Joint
torso[segment].joint = dJointCreateUniversal(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);
break;
case 7: //Ball Joint
torso[segment].joint = dJointCreateBall(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
break;
case 8: // Fixed
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(torso[segment].joint, dParamHiStop, 0.00*M_PI);
break;
default:
assert(false); // not a valid hinge type
break;
}
}
#endif
}
Car::Car(dWorldID world, dSpaceID space, CarDesignID n_car_design, AppDesignID n_app_design) :MyObject(world, space), car_design(n_car_design),
app_design(n_app_design)
{
int i, j, ind;
body_obj = 0;
for (i = 0; i < 2; ++i)
chain_obj[i] = 0;
for (i = 0; i < 6; ++i)
wheel_obj[i] = 0;
max_f = 0; // must be set with setMaxF
TrackDesignID track_design = car_design->left_track_design;
LinkDesignID link_design = track_design->link_design;
WheelDesignID wheel_design = car_design->wheel_design;
body_mass = car_design->getBodyMass();
// create body
createBody(car_design->getChassisPosition());
/*
for ( int i = 0 ; i < 6 ; ++i ) {
CreateToothedWheel(world, space,
wheel_obj[i], sprocket_teeth, R, t_sides, teeth_h_fuzz);
}
*/
// create sprocket wheels in back wheels position
for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
wheel_obj[ind] = wheel_design->create(world, space);
sprocket[j] = wheel_obj[ind]->body[0];
const dReal *sprocket_pos =
track_design->getBackWheelPos();
PVEC(sprocket_pos);
dReal y = sprocket_pos[YY];
if (j == 1)
y += car_design->getTrackToTrack();
PEXP(y);
dBodySetPosition(sprocket[j], sprocket_pos[XX], y,
sprocket_pos[ZZ]);
}
// create sprockets and set their position
// create front wheels and set their position
for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
wheel_obj[ind] = wheel_design->create(world, space);
front[j] = wheel_obj[ind]->body[0];
const dReal *front_pos = track_design->getFrontWheelPos();
PVEC(front_pos);
dReal y = front_pos[YY];
if (j == 1)
y += car_design->getTrackToTrack();
PEXP(y);
dBodySetPosition(front[j], front_pos[XX], y,
front_pos[ZZ]);
}
// create chains
chain_obj[0] =
new Chain(world, space, track_design, link_design);
track_design->moveDesign(0, car_design->getTrackToTrack(), 0); // instead of recreating design
chain_obj[1] = new Chain(world, space, track_design, link_design);
// create axle joints for sprockets
for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
axle[ind] = dJointCreateHinge(world, 0);
dBodyID second = (body_obj ? body_obj->body[0] : 0);
dJointAttach(axle[ind], sprocket[j], second);
const dReal *sp_v = dBodyGetPosition(sprocket[j]);
const dReal *rot = dBodyGetRotation(sprocket[j]);
dJointSetHingeAnchor(axle[ind], sp_v[XX], sp_v[YY],
sp_v[ZZ]);
dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
}
/*
for ( j = 0, ind = FIRST_BACK ; j < 2 ; ++j, ++ind ) {
axle[ind] = dJointCreateHinge(world, 0); // no joint group (0==NULL)
dJointAttach(axle[ind], back[j], 0); // attach to world
const dReal* v = dBodyGetPosition(back[j]);
dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
dJointSetHingeAxis(axle[ind], y_axis[XX], y_axis[YY], y_axis[ZZ]); // axis is up (positive z)
}
*/
// create front wheels joints
for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
axle[ind] = dJointCreateHinge(world, 0);
dBodyID second = (body_obj ? body_obj->body[0] : 0);
dJointAttach(axle[ind], front[j], second);
const dReal *v = dBodyGetPosition(front[j]);
const dReal *rot = dBodyGetRotation(sprocket[j]);
dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
} if (body_obj)
body_obj->show_forces = 1;
// create appendage
if (body_obj && app_design) {
//app_obj = new App(world, space, app_design, body_obj->body[0]);
app_obj = 0;
};
// lets see what we have, height wise
if (chain_obj[0] != 0) {
const dReal *pos =
dBodyGetPosition(chain_obj[0]->body[0]);
std::cout << "debug: first link position, z wise\n";
PEXP(pos[ZZ]);
}
#ifdef ARE_YOU_NUTS_ABOUT_SPRINGS
// now create the suspension - it is between the wheels and the body.
Init(0, 2, 0);
// initially lets make some springs between the wheels and the body.
for (int i = 0 ; i < 2 ; ++i) {
dBody a_wheel = wheel_obj[i + FIRST_SPROCKET]->body[0];
};
#endif
}
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
// chassis pin joints
const dReal *pos;
snow->jChassisUp= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body);
dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0);
pos= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jChassisUp,pos[0]
,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]);
const dReal *pos1;
snow->jChassisLow= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body);
dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0);
pos1= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jChassisLow, pos1[0]
, pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]);
// rotor ball joint
/*const dReal *p2;
jRotorUp= dJointCreateBall(world,0);
dJointAttach(jRotorUp, uplink.body, hlink.body);
p2= dBodyGetPosition(uplink.body);
dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]);
const dReal *p3;
jRotorLow= dJointCreateBall(world,0);
dJointAttach(jRotorLow, lowlink.body,hlink.body);
p3= dBodyGetPosition(lowlink.body);
dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/
const dReal *p2;
snow->jRotorUp= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body);
p2= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jRotorUp, p2[0]
,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]);
dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0);
const dReal *p3;
snow->jRotorLow= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body);
p3= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jRotorLow, p3[0]
,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]);
dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0);
// rotor hinge joint
const dReal *pw= dBodyGetPosition(wnow->body);
snow->jRotorMid= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body);
dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0);
dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]);
// strut joint
const dReal *ps1, *ps2;
dReal angle= -shiftSign*strutAngle;
ps1= dBodyGetPosition(snow->upstrut.body);
ps2= dBodyGetPosition(snow->lowstrut.body);
snow->jStrutUp= dJointCreateBall(world,0);
dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body);
//dJointSetFixed(snow->jStrutUp);
dJointSetBallAnchor(snow->jStrutUp, ps1[0]
,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle))
,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle)));
snow->jStrutLow= dJointCreateBall(world,0);
dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body);
dJointSetBallAnchor(snow->jStrutLow, ps2[0]
,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle))
,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle)));
// struct sliding joint
snow->jStrutMid= dJointCreateSlider(world,0);
dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body);
dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle)));
// set joint feedback
wnow->feedback= new dJointFeedback;
dJointSetFeedback(snow->jStrutMid,wnow->feedback);
// suspension slider
/*snow->jLowSpring= dJointCreateLMotor(world,0);
dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body);
dJointSetLMotorNumAxes(snow->jLowSpring, 1);
dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.0);*/
}
bool ActiveCardanModel::initModel() {
// Create the 4 components of the hinge
cardanRoot_ = this->createBody(B_SLOT_A_ID);
dBodyID connectionPartA = this->createBody(B_CONNECTION_A_ID);
dBodyID crossPartA = this->createBody(B_CROSS_PART_A_ID);
dBodyID crossPartB = this->createBody(B_CROSS_PART_B_ID);
dBodyID connectionPartB = this->createBody(B_CONNECTION_B_ID);
cardanTail_ = this->createBody(B_SLOT_B_ID);
float separation = inMm(0.1);
this->createBoxGeom(cardanRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
dReal xPartA = SLOT_THICKNESS / 2 + separation
+ CONNNECTION_PART_LENGTH / 2;
this->createBoxGeom(connectionPartA, MASS_SERVO, osg::Vec3(xPartA, 0, 0),
CONNNECTION_PART_LENGTH, CONNNECTION_PART_WIDTH,
CONNECTION_PART_HEIGHT);
dReal xCrossPartA = xPartA + CONNECTION_PART_OFFSET;
this->createBoxGeom(crossPartA, MASS_CROSS / 3,
osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_WIDTH,
CROSS_HEIGHT);
this->createBoxGeom(crossPartB, MASS_CROSS / 3,
osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_HEIGHT,
CROSS_WIDTH);
dReal xPartB = xCrossPartA + CONNECTION_PART_OFFSET;
this->createBoxGeom(connectionPartB, MASS_SERVO, osg::Vec3(xPartB, 0, 0),
CONNNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT,
CONNNECTION_PART_WIDTH);
dReal xTail = xPartB + CONNNECTION_PART_LENGTH / 2 + separation
+ SLOT_THICKNESS / 2;
this->createBoxGeom(cardanTail_, MASS_SLOT, osg::Vec3(xTail, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
// Cross Geometries
dBodyID crossPartAedge1 = this->createBody(B_CROSS_PART_A_EDGE_1_ID);
dBodyID crossPartAedge2 = this->createBody(B_CROSS_PART_A_EDGE_2_ID);
dBodyID crossPartBedge1 = this->createBody(B_CROSS_PART_B_EDGE_1_ID);
dBodyID crossPartBedge2 = this->createBody(B_CROSS_PART_B_EDGE_2_ID);
this->createBoxGeom(crossPartAedge1, MASS_CROSS / 12,
osg::Vec3(xCrossPartA - (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2), 0,
CROSS_HEIGHT / 2 + (CROSS_THICKNESS / 2)),
CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS);
this->createBoxGeom(crossPartAedge2, MASS_CROSS / 12,
osg::Vec3(xCrossPartA - (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2), 0,
-CROSS_HEIGHT / 2 - (CROSS_THICKNESS / 2)),
CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS);
this->createBoxGeom(crossPartBedge1, MASS_CROSS / 12,
osg::Vec3(xCrossPartA + (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2),
CROSS_HEIGHT / 2 - (CROSS_THICKNESS / 2), 0),
CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH);
this->createBoxGeom(crossPartBedge2, MASS_CROSS / 12,
osg::Vec3(xCrossPartA + (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2),
-CROSS_HEIGHT / 2 + (CROSS_THICKNESS / 2), 0),
CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH);
// Create joints to hold pieces in position
// root <-> connectionPartA
this->fixBodies(cardanRoot_, connectionPartA, osg::Vec3(1, 0, 0));
// connectionPartA <(hinge)> crossPartA
dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint, connectionPartA, crossPartA);
dJointSetHingeAxis(joint, 0, 0, 1);
dJointSetHingeAnchor(joint,
xPartA
+ ((CONNNECTION_PART_LENGTH / 2)
- (CONNNECTION_PART_LENGTH
- CONNNECTION_PART_ROTATION_OFFSET)), 0, 0);
// crossPartA <-> crossPartB
this->fixBodies(crossPartA, crossPartB, osg::Vec3(1, 0, 0));
// crossPartB <(hinge)> connectionPartB
dJointID joint2 = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint2, crossPartB, connectionPartB);
dJointSetHingeAxis(joint2, 0, 1, 0);
dJointSetHingeAnchor(joint2,
xPartB
- ((CONNNECTION_PART_LENGTH / 2)
- (CONNNECTION_PART_LENGTH
- CONNNECTION_PART_ROTATION_OFFSET)), 0, 0);
// connectionPartB <-> tail
this->fixBodies(connectionPartB, cardanTail_, osg::Vec3(1, 0, 0));
// Fix cross Parts
this->fixBodies(crossPartA, crossPartAedge1, osg::Vec3(1, 0, 0));
this->fixBodies(crossPartA, crossPartAedge2, osg::Vec3(1, 0, 0));
this->fixBodies(crossPartB, crossPartBedge1, osg::Vec3(1, 0, 0));
this->fixBodies(crossPartB, crossPartBedge2, osg::Vec3(1, 0, 0));
// Create motors
motor1_.reset(
new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
ServoMotor::DEFAULT_GAIN,
ioPair(this->getId(),0)));
motor2_.reset(
new ServoMotor(joint2, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
ServoMotor::DEFAULT_GAIN,
ioPair(this->getId(),1)));
return true;
}
int main (int argc, char **argv)
{
dMass m;
// 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);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 20);
int i;
for (i=0; i<SEGMCNT; i++)
{
segbodies[i] = dBodyCreate (world);
dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dBodySetMass (segbodies[i], &m);
seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dGeomSetBody (seggeoms[i], segbodies[i]);
dSpaceAdd (space, seggeoms[i]);
}
for (i=0; i<SEGMCNT-1; i++)
{
hinges[i] = dJointCreateHinge (world,0);
dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
dJointSetHingeAxis (hinges[i], 0,1,0);
dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
// NOTE:
// Here we tell ODE where to put the feedback on the forces for this hinge
dJointSetFeedback (hinges[i], jfeedbacks+i);
stress[i]=0;
}
for (i=0; i<STACKCNT; i++)
{
stackbodies[i] = dBodyCreate(world);
dMassSetBox (&m, 2.0, 2, 2, 0.6);
dBodySetMass(stackbodies[i],&m);
stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
dGeomSetBody(stackgeoms[i], stackbodies[i]);
dBodySetPosition(stackbodies[i], 0,0,8+2*i);
dSpaceAdd(space, stackgeoms[i]);
}
sliders[0] = dJointCreateSlider (world,0);
dJointAttach(sliders[0], segbodies[0], 0);
dJointSetSliderAxis (sliders[0], 1,0,0);
dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
sliders[1] = dJointCreateSlider (world,0);
dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
dJointSetSliderAxis (sliders[1], 1,0,0);
dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
groundgeom = dCreatePlane(space, 0,0,1,0);
for (i=0; i<SEGMCNT; i++)
colours[i]=0.0;
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy seggeoms, then space, then the world.
for (i=0; i<SEGMCNT; i++)
dGeomDestroy (seggeoms[i]);
for (i=0; i<STACKCNT; i++)
dGeomDestroy (stackgeoms[i]);
dSpaceDestroy(space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void PhysicsHingeJoint::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 = dJointCreateHinge(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateHinge(getWorld()->getWorldID(), 0);
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetHingeParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetHingeParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetHingeParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetHingeParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetHingeParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetHingeParam(_JointID,dParamStopERP));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & AnchorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeAnchor(_JointID, getAnchor().x(), getAnchor().y(), getAnchor().z());
}
if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamStopCFM, getStopCFM());
}
}
/*** Formation of robot ***/
void makeRobot()
{
dReal torso_m = 10.0; // Mass of body
dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments
//for four legs
dReal x[num_legs][num_links] = {{ cx1, cx1, cx1},{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
{-cx1,-cx1,-cx1},{ cx1, cx1, cx1}};
dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},{ cy1, cy1, cy1},// Position of each link (y coordinate)
{-cy1,-cy1,-cy1},{-cy1,-cy1,-cy1}};
dReal z[num_legs][num_links] = { // Position of each link (z coordinate)
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}};
dReal r[num_links] = { r1, r2, r3}; // radius of leg segment
dReal length[num_links] = { l1, l2, l3}; // Length of leg segment
dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment
// //for one leg
// dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
// dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
// dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
//for four legs
dReal axis_x[num_legs][num_links] = {{ 0,1, 0},{ 0,1,0},{ 0, 1, 0},{ 0, 1, 0}};
dReal axis_y[num_legs][num_links] = {{ 1,0, 1},{ 1,0,1},{ 1, 0, 1},{ 1, 0, 1}};
dReal axis_z[num_legs][num_links] = {{ 0,0, 0},{ 0,0,0},{ 0, 0, 0},{ 0, 0, 0}};
// For mation of the body
dMass mass;
torso[0].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
dBodySetMass(torso[0].body,&mass);
torso[0].geom = dCreateBox(space, lx, ly, lz);
dGeomSetBody(torso[0].geom, torso[0].body);
dBodySetPosition(torso[0].body, SX, SY, SZ);
// Formation of leg
dMatrix3 R; // Revolution queue
dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[0][i][j].body = dBodyCreate(world);
if (j == 0) dBodySetRotation(leg[0][i][j].body,R);
dBodySetPosition(leg[0][i][j].body, SX+x[i][j], SY+y[i][j], SZ+z[i][j]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
dBodySetMass(leg[0][i][j].body, &mass);
//if(i==1 and j==2) //to set the length of one leg differently
//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
//else
leg[0][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
dGeomSetBody(leg[0][i][j].geom,leg[0][i][j].body);
}
}
// Formation of joints (and connecting them up)
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[0][i][j].joint = dJointCreateHinge(world, 0);
if (j == 0){
dJointAttach(leg[0][i][j].joint, torso[0].body, leg[0][i][j].body); //connects hip to the environment
dJointSetHingeParam(leg[0][i][j].joint, dParamLoStop, -.5*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(leg[0][i][j].joint, dParamHiStop, .5*M_PI);
}
else
dJointAttach(leg[0][i][j].joint, leg[0][i][j-1].body, leg[0][i][j].body);
dJointSetHingeAnchor(leg[0][i][j].joint, SX+c_x[i][j], SY+c_y[i][j],SZ+c_z[i][j]);
dJointSetHingeAxis(leg[0][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
}
}
}
// ロボットの生成
void create() {
// SHIELDの生成（空間に固定）
rod[0].body = dBodyCreate(world);
dBodySetPosition(rod[0].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
dMassSetZero(&mass);
dMassSetCylinderTotal(&mass, SHIELD_WEIGHT, 2, SHIELD_RADIUS, SHIELD_LENGTH);
dBodySetMass(rod[0].body, &mass);
rod[0].geom = dCreateCylinder(space, SHIELD_RADIUS, SHIELD_LENGTH);
dGeomSetBody(rod[0].geom, rod[0].body);
dRFromAxisAndAngle(R, 1, 0, 0, 0.5 * M_PI); // x軸に90度回転
dBodySetRotation(rod[0].body, R);
rod_joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント
dJointAttach(rod_joint[0], rod[0].body, 0);
dJointSetFixed(rod_joint[0]);
// RODの生成（回転ジョイントy軸に回転軸）
rod[1].body = dBodyCreate(world);
dBodySetPosition(rod[1].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, ROD_WEIGHT, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
dBodySetMass(rod[1].body, &mass);
rod[1].geom = dCreateBox(space, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
dGeomSetBody(rod[1].geom, rod[1].body);
dRFromAxisAndAngle(R, 0, 0, 1, 0.25 * M_PI); // z軸に45度回転
dBodySetRotation(rod[1].body, R);
rod_joint[1] = dJointCreateHinge(world, 0); // ヒンジジョイント
dJointAttach(rod_joint[1], rod[1].body, rod[0].body);
dJointSetHingeAnchor(rod_joint[1], SHIELD_X, SHIELD_Y, SHIELD_Z);
dJointSetHingeAxis(rod_joint[1], 0, 1, 0);// y軸ジョイント
// BODYの生成（たてておくだけ）
rod[2].body = dBodyCreate(world);
dBodySetPosition(rod[2].body, BODY_X, BODY_Y, BODY_Z);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, BODY_WEIGHT, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
dBodySetMass(rod[2].body, &mass);
rod[2].geom = dCreateBox(space, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
dGeomSetBody(rod[2].geom, rod[2].body);
// BULLETの生成（CANNON中心に初期座標）
bullet.body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetSphereTotal(&mass, BULLET_WEIGHT, BULLET_RADIUS);
dBodySetMass(bullet.body,&mass);
dBodySetPosition(bullet.body, CANNON_X, CANNON_Y, CANNON_Z);
bullet.geom = dCreateSphere(space, BULLET_RADIUS);
dGeomSetBody(bullet.geom, bullet.body);
// TARGETの生成
// target.body = dBodyCreate(world);
// dMassSetZero(&mass);
// dMassSetSphereTotal(&mass, 0.0001, BULLET_RADIUS);
// dBodySetMass(target.body,&mass);
// dBodySetPosition(target.body, SHIELD_X, SHIELD_Y, SHIELD_Z);
// target.geom = dCreateSphere(space, BULLET_RADIUS);
// dGeomSetBody(target.geom, target.body);
}
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;
}
