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;
}
/*** ロボットアームの生成 ***/
void makeArm()
{
dMass mass; // 質量パラメータ
dReal x[NUM] = {0.00, 0.00, 0.00, 0.00}; // 重心 x
dReal y[NUM] = {0.00, 0.00, 0.00, 0.00}; // 重心 y
dReal z[NUM] = {0.05, 0.50, 1.50, 2.50}; // 重心 z
dReal length[NUM] = {0.10, 0.90, 1.00, 1.00}; // 長さ
dReal weight[NUM] = {9.00, 2.00, 2.00, 2.00}; // 質量
dReal r[NUM] = {0.20, 0.04, 0.04, 0.04}; // 半径
dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00}; // 関節中心点 x
dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00}; // 関節中心点 y
dReal c_z[NUM] = {0.00, 0.10, 1.00, 2.00}; // 関節中心点 z
dReal axis_x[NUM] = {0, 0, 0, 0}; // 関節回転軸 x
dReal axis_y[NUM] = {0, 0, 1, 1}; // 関節回転軸 y
dReal axis_z[NUM] = {1, 1, 0, 0}; // 関節回転軸 z
// リンクの生成
for (int i = 0; i < NUM; 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);
}
// ジョイントの生成とリンクへの取り付け
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]);
}
}
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);
}
/*** 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;
}
static void soy_joints_hinge_real_setup (soyjointsJoint* base, soyatomsPosition* anchor, soyatomsAxis* axis1, soyatomsAxis* axis2) {
soyjointsHinge * self;
struct dxJoint* _tmp0_ = NULL;
soyatomsPosition* _tmp1_ = NULL;
gfloat _tmp2_ = 0.0F;
gfloat _tmp3_ = 0.0F;
soyatomsPosition* _tmp4_ = NULL;
gfloat _tmp5_ = 0.0F;
gfloat _tmp6_ = 0.0F;
soyatomsPosition* _tmp7_ = NULL;
gfloat _tmp8_ = 0.0F;
gfloat _tmp9_ = 0.0F;
struct dxJoint* _tmp10_ = NULL;
soyatomsAxis* _tmp11_ = NULL;
gfloat _tmp12_ = 0.0F;
gfloat _tmp13_ = 0.0F;
soyatomsAxis* _tmp14_ = NULL;
gfloat _tmp15_ = 0.0F;
gfloat _tmp16_ = 0.0F;
soyatomsAxis* _tmp17_ = NULL;
gfloat _tmp18_ = 0.0F;
gfloat _tmp19_ = 0.0F;
#line 33 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
self = (soyjointsHinge*) base;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp0_ = ((soyjointsJoint*) self)->joint;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp1_ = anchor;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp2_ = soy_atoms_position_get_x (_tmp1_);
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp3_ = _tmp2_;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp4_ = anchor;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp5_ = soy_atoms_position_get_y (_tmp4_);
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp6_ = _tmp5_;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp7_ = anchor;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp8_ = soy_atoms_position_get_z (_tmp7_);
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp9_ = _tmp8_;
#line 35 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
dJointSetHingeAnchor ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_);
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp10_ = ((soyjointsJoint*) self)->joint;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp11_ = axis1;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp12_ = soy_atoms_axis_get_x (_tmp11_);
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp13_ = _tmp12_;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp14_ = axis1;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp15_ = soy_atoms_axis_get_y (_tmp14_);
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp16_ = _tmp15_;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp17_ = axis1;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp18_ = soy_atoms_axis_get_z (_tmp17_);
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp19_ = _tmp18_;
#line 38 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
dJointSetHingeAxis ((struct dxJoint*) _tmp10_, (dReal) _tmp13_, (dReal) _tmp16_, (dReal) _tmp19_);
#line 319 "Hinge.c"
}
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 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;
}
void CManipulator::CreateJoint (SPoint &myptAnchor,SJointPara paraJoint,double Border,dBodyID &mybodyLastJoint,dBodyID &mybodyJoint,dGeomID &mygeomLink,dJointID &myJoint,dWorldID myworld,dSpaceID myspace)
{
SPoint ptBody;
if(!mybodyLastJoint)//如果是第一个关节，上个body=NULL
{
if(paraJoint.typeJoint==CYLINDER)
mygeomLink=dCreateCylinder(myspace,paraJoint.paraManipulatorCylinder.r,paraJoint.paraManipulatorCylinder.length);
else
mygeomLink=dCreateBox(myspace,paraJoint.paraManipulatorBox.x,paraJoint.paraManipulatorBox.y,paraJoint.paraManipulatorBox.z);
mybodyJoint=dBodyCreate(myworld);
dGeomSetBody(mygeomLink,mybodyJoint);
dBodySetMass(mybodyJoint,¶Joint.massBody);
ptBody.x=0;
if(paraJoint.typeJoint==CYLINDER)
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
dBodySetPosition(mybodyJoint,ptBody.x,ptBody.y,ptBody.z);
dMatrix3 rot;
if(paraJoint.typeJoint==CYLINDER)
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorCylinder.angle);
else
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorBox.angle);
dBodySetRotation(mybodyJoint,rot);
myJoint=dJointCreateHinge(myworld,0);//create Joint
dJointAttach(myJoint,0,mybodyJoint);
dJointSetHingeAnchor(myJoint,myptAnchor.x,myptAnchor.y,myptAnchor.z);
if(paraJoint.modeAxis==XX)
dJointSetHingeAxis(myJoint,1,0,0);
else if(paraJoint.modeAxis==YY)
dJointSetHingeAxis(myJoint,0,1,0);
else
dJointSetHingeAxis(myJoint,0,0,1);
if(paraJoint.typeJoint==CYLINDER)
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
}
else
{
if(paraJoint.typeJoint==CYLINDER)
mygeomLink=dCreateCylinder(myspace,paraJoint.paraManipulatorCylinder.r,paraJoint.paraManipulatorCylinder.length);
else
mygeomLink=dCreateBox(myspace,paraJoint.paraManipulatorBox.x,paraJoint.paraManipulatorBox.y,paraJoint.paraManipulatorBox.z);
mybodyJoint=dBodyCreate(myworld);
dGeomSetBody(mygeomLink,mybodyJoint);
dBodySetMass(mybodyJoint,¶Joint.massBody);
ptBody.x=0;
if(paraJoint.typeJoint==CYLINDER)
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
ptBody.y=myptAnchor.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
ptBody.z=myptAnchor.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
dBodySetPosition(mybodyJoint,ptBody.x,ptBody.y,ptBody.z);
dMatrix3 rot;
if(paraJoint.typeJoint==CYLINDER)
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorCylinder.angle);
else
dRFromAxisAndAngle(rot,1,0,0,paraJoint.paraManipulatorBox.angle);
dBodySetRotation(mybodyJoint,rot);
myJoint=dJointCreateHinge(myworld,0);//create Joint
dJointAttach(myJoint,mybodyLastJoint,mybodyJoint);
dJointSetHingeAnchor(myJoint,myptAnchor.x,myptAnchor.y,myptAnchor.z);
if(paraJoint.modeAxis==XX)
dJointSetHingeAxis(myJoint,1,0,0);
else if(paraJoint.modeAxis==YY)
dJointSetHingeAxis(myJoint,0,-1,0);
else
dJointSetHingeAxis(myJoint,0,0,1);
if(paraJoint.typeJoint==CYLINDER)
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorCylinder.length*sin(paraJoint.paraManipulatorCylinder.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorCylinder.length*cos(paraJoint.paraManipulatorCylinder.angle)/2.0;
}
else
{
myptAnchor.y=ptBody.y+paraJoint.paraManipulatorBox.z*sin(paraJoint.paraManipulatorBox.angle)/2.0;
myptAnchor.z=ptBody.z+paraJoint.paraManipulatorBox.z*cos(paraJoint.paraManipulatorBox.angle)/2.0;
}
}
mybodyLastJoint=mybodyJoint;
return;
}
State* init() {
State* state = new State();
dInitODE();
SDL_Init(SDL_INIT_EVERYTHING);
state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL);
SDL_WM_SetCaption("Physics", NULL);
SDL_Flip(state->screen);
SDL_ShowCursor(SDL_DISABLE);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
GLfloat light_ambient[] = { 1, 1, 1, 1 };
glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
GLfloat lightpos[] = {0, 4, 0, 1};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
glShadeModel(GL_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glClearColor(0, 0, 0, 1);
state->posx = 0;//21;
state->posy = 4;//8;
state->posz = 5;//21;
state->rotx = 0;
state->roty = 0;//-40;
state->rotz = 0;
state->wkey = false;
state->akey = false;
state->skey = false;
state->dkey = false;
state->gkey = false;
state->simSpeed = 60;
state->carcam = false;
state->carbodydrawable = new Drawable("objs/carbody.obj");
state->carwheeldrawable = new Drawable("objs/carwheel.obj");
state->map = new Drawable("objs/jump2.obj");
state->cube = new Drawable("objs/cube.obj");
// state->monkey = new Drawable("objs/monkey.obj");
state->world = dWorldCreate();
dWorldSetGravity(state->world, 0, -9.8, 0);
state->worldSpace = dHashSpaceCreate(0);
const double carHeight = 0.65;
const double carZ = 90;
const double carX = 0;
const float speed = -1000;
const float force = 200;
state->carbodybody = dBodyCreate(state->world);
dBodySetPosition(state->carbodybody, carX, carHeight, carZ);
dMass bodymass;
dMassSetBoxTotal(&bodymass, 100, 2, 4, 1);
dBodySetMass(state->carbodybody, &bodymass);
dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4);
dGeomSetBody(carbodygeom, state->carbodybody);
state->flcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2);
const dMatrix3 m = { 0, 0, 1, 0
, 0, 1, 0, 0
, 0, 0, 1, 0 };
dBodySetRotation(state->flcarwheelbody, m);
dMass wheelmass;
dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2);
dBodySetMass(state->flcarwheelbody, &wheelmass);
dJointID joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->flcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(flcarwheelgeom, state->flcarwheelbody);
dJointID motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->flcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->frcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2);
dBodySetRotation(state->frcarwheelbody, m);
dBodySetMass(state->frcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->frcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(frcarwheelgeom, state->frcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->frcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->blcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->blcarwheelbody, m);
dBodySetMass(state->blcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->blcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(blcarwheelgeom, state->blcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->blcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->brcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->brcarwheelbody, m);
dBodySetMass(state->brcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->brcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(brcarwheelgeom, state->brcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->brcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->var = new double[3];
state->var = dBodyGetPosition(state->carbodybody);
// cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl;
//TODO check if translation matrix from dBody can be used.
dSpaceID cubespace = dHashSpaceCreate(state->worldSpace);
for(int i = 0; i < NUM_CUBES/10; i++) {
for(int k = 0; k < 10; k++) {
state->cubebody[i*10+k] = dBodyCreate(state->world);
dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1);
dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70);
dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2);
dGeomSetBody(cubegeom, state->cubebody[i*10+k]);
}
}
{
int indexSize = state->map->vertices.size()/3;
unsigned int* index = new unsigned int[indexSize];
for(int i = 0; i < indexSize; i++)
index[i] = i;
dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12);
dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
dGeomSetPosition(mapGeom, 0, 0, 0);
}
// state->monkeyBody = dBodyCreate(state->world);
// {
// int indexSize = state->monkey->vertices.size()/3;
// unsigned int* index = new unsigned int[indexSize];
// for(int i = 0; i < indexSize; i++)
// index[i] = i;
// dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
// dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12);
// dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
// dGeomSetPosition(monkeyGeom, 0, 2, 0);
// dGeomSetBody(monkeyGeom, state->monkeyBody);
// }
state->physicsContactgroup = dJointGroupCreate(0);
return state;
}
dJointID ODE_Dynamixel::createJoint(dBodyID body1, dBodyID body2) {
if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get(), -1)) {
Core::log("ODE_Dynamixel: Invalid axes for ODE_Dynamixel.", true);
return 0;
}
Vector3D anchor = mJointAxisPoint1->get();
Vector3D axis = mJointAxisPoint2->get() - mJointAxisPoint1->get();
dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
dJointAttach(joint, body1, body2);
dJointSetAMotorMode(joint, dAMotorEuler);
dJointSetAMotorParam(joint, dParamVel, 0.0);
dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get());
dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());
axis.normalize();
Vector3D perpedicular;
if(axis.getY() != 0.0 || axis.getX() != 0.0) {
perpedicular.set(-axis.getY(), axis.getX(), 0);
}
else {
perpedicular.set(0, -axis.getZ(), axis.getY());
}
perpedicular.normalize();
// If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added.
if(body1 == 0) {
dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ());
}
else {
dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ());
}
if(body2 == 0) {
dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(),
-perpedicular.getZ());
}
else {
dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(),
perpedicular.getZ());
}
mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
dJointAttach(mHingeJoint, body1, body2);
dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ());
if(body1 == 0) {
double tmp = mMinAngle;
mMinAngle = -1.0 * mMaxAngle;
mMaxAngle = -1.0 * tmp;
}
dJointSetHingeParam(mHingeJoint, dParamLoStop, mMinAngle);
dJointSetHingeParam(mHingeJoint, dParamHiStop, mMaxAngle);
dJointSetHingeParam(mHingeJoint, dParamVel, 0.0);
return joint;
}
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
}
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());
}
}
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;
}
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);*/
}
/* --------------------------------
** 全方向移動ロボットの生成, 描画
----------------------------------- */
void MakeOmni()
{
/* ローカル変数の定義 */
dMass mass;
double r, w, d, m_w, x, y, z, lx, ly, lz, m_b;
/* ロボットの個数分だけループ */
for (int i=0; i< OMNI_NUM; i++)
{
/* 車輪 */
r = wheelSize[i][0];
w = wheelSize[i][1];
d = wheelSize[i][2];
m_w = wheelM[i];
/* 土台 */
x = basePos[i][0];
y = basePos[i][1];
z = basePos[i][2];
lx = baseSize[i][0];
ly = baseSize[i][1];
lz = baseSize[i][2];
m_b = baseM[i];
/* 土台の生成 */
base[i].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, m_b, lx, ly, lz);
dBodySetMass(base[i].body,&mass);
base[i].geom = dCreateBox(space, lx, ly, lz);
dGeomSetBody(base[i].geom, base[i].body);
dBodySetPosition(base[i].body, x, y, z); /* 座標 */
/* 車輪の生成 */
double wx[WHEEL_NUM] = {lx/2+w/2+d, - (lx/2+w/2+d), 0, 0};
double wy[WHEEL_NUM] = {0, 0, ly/2+w/2+d, -(ly/2+w/2+d)};
double wz[WHEEL_NUM] = {z, z, z, z};
double jx[WHEEL_NUM] = {lx/2, -lx/2, 0, 0};
double jy[WHEEL_NUM] = {0, 0, ly/2, -ly/2};
double jz[WHEEL_NUM] = {z, z, z, z};
for (int j = 0; j < WHEEL_NUM; j++)
{
wheel[i][j].body = dBodyCreate(world);
dMatrix3 R;
if (j >= 2)
{
dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
dBodySetRotation(wheel[i][j].body,R);
}
else
{
dRFromAxisAndAngle(R, 0, 1, 0, M_PI/2.0);
dBodySetRotation(wheel[i][j].body, R);
}
dMassSetZero(&mass);
if (j < 2)
{
dMassSetCylinderTotal(&mass, m_w, 1, r, w);
}
else
{
dMassSetCylinderTotal(&mass, m_w, 2, r, w);
}
dBodySetMass(wheel[i][j].body,&mass);
wheel[i][j].geom = dCreateCylinder(space, r, w);
dGeomSetBody(wheel[i][j].geom, wheel[i][j].body);
dBodySetPosition(wheel[i][j].body, wx[j], wy[j], wz[j]);
wheel[i][j].joint = dJointCreateHinge(world, 0);
dJointAttach(wheel[i][j].joint, base[i].body, wheel[0][j].body);
if (j < 2)
{
dJointSetHingeAxis(wheel[i][j].joint, 1, 0, 0);
}
else
{
dJointSetHingeAxis(wheel[i][j].joint, 0, -1, 0);
}
dJointSetHingeAnchor(wheel[i][j].joint, jx[j], jy[j], jz[j]);
}
}
}
