void TrackedVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
//dMassAdjust(&this->vehicleMass, 2.40);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);
this->leftTrack->create();
this->rightTrack->create();
dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
dRigidBodyArraySetPosition(leftTrack->bodyArray, -wheelBase/2, -(w - trackWidth)/2, 0);
dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2, (w - trackWidth)/2, 0);
this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
dJointSetFixed(this->leftTrackJoint);
dJointSetFixed(this->rightTrackJoint);
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}
/*
=================================================================================
createFixedLeg
Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius,
dReal length)
{
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);
//fixed joint
joint = dJointCreateFixed(World, jointgroup);
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetFixed(joint);
}
void createInvisibleHead( float* pos )
{
dMatrix3 head_orientation;
dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);
//position and orientation
head.Body = dBodyCreate(World);
dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
dBodySetRotation(head.Body, head_orientation);
dBodySetLinearVel(head.Body, 0, 0, 0);
dBodySetData(head.Body, (void *)0);
//mass
dMass head_mass;
dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
dBodySetMass(head.Body, &head_mass);
//geometry
head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
dGeomSetBody(head.Geom, head.Body);
//fixed joint
invis_box_joint = dJointCreateFixed(World, jointgroup);
dJointAttach(invis_box_joint, body.Body, head.Body);
dJointSetFixed(invis_box_joint);
}
OscFixedODE::OscFixedODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase* parent,
OscObject *object1, OscObject *object2)
: OscFixed(name, parent, object1, object2)
{
if (object2) {
dJointID odeJoint = dJointCreateFixed(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetFixed(odeJoint);
}
else {
ODEObject *o = NULL;
if (object1)
o = dynamic_cast<ODEObject*>(object1->special());
if (o)
o->disconnectBody();
m_pSpecial = new ODEConstraint(this, NULL, odeWorld, odeSpace,
object1, object2);
}
printf("[%s] Fixed joint created between %s and %s.\n",
simulation()->type_str(),
object1->c_name(), object2?object2->c_name():"world");
}
/**
* This method is used for activating or deactivating
* the Joint. If the Joint was inactive and should be
* activated the fixed ODE-joint is removed and the
* virtual attachJoint-method is called.
* If the Joint was active and should be deactivated
* the virtual detachJoint-method is called and then
* a fixed ODE-joint is created and attached to the bodies.
* @param isActive 0 .. not active
* anything else .. active
**/
void Joint::setActive(int isActive)
{
dBodyID body1, body2;
if (!active && isActive && joint)
{
dJointDestroy(joint);
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
attachJoint(body1, body2);
} // if
else if (active && !isActive && joint)
{
// first detach joint e.g. for saving axis angles
detachJoint();
dJointDestroy(joint);
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
joint = dJointCreateFixed(world, 0);
dJointAttach(joint, body1, body2);
dJointSetFixed(joint);
} // else if
if (isActive)
this->active = true;
else
this->active = false;
} // setActive
void Joint::build(const Vector3d &v, const Rotation &r, bool dynamics)
{
if (m_bodyNum != BODY_NUM) { return; }
assert(m_world);
m_joint = createJoint(m_bodies[0], m_bodies[1]);
Parts *parts = (Parts*) dBodyGetData(m_bodies[1]);
assert(parts);
double x, y, z;
parts->givePosition(x, y, z);
m_rotv.set(x, y, z);
m_rotv -= m_anchor;
Vector3d av = m_anchor;
av.rotate(r);
av += v;
applyAnchor(av.x(), av.y(), av.z());
if (m_fixed) {
dJointSetFixed(m_joint);
}
if (dynamics) {
m_jfb = new dJointFeedback;
dJointSetFeedback(m_joint, m_jfb);
}
}
void Robots::construirChassi(dWorldID world)
{
for (int i=0; i < 2; i++)
{
// Cria objeto e geometria
this->body[i] = dBodyCreate(world);
this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);
// Define a posição do objeto
dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);
// Se o robô for do segundo time, deve ser rotacionado em 180 graus
if ((this->id == 3) || (this->id == 4) || (this->id == 5))
{
dQuaternion q;
dQFromAxisAndAngle(q,0,0,1,M_PI);
dBodySetQuaternion(this->body[i],q);
}
// Define a massa do objeto
dMass m;
dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT); // O segundo bloco é mais curto
dMassAdjust(&m,CMASS*(1+i*2)); // O segundo bloco é mais pesado
dBodySetMass(this->body[i],&m);
// Associa o objeto à sua geometria
dGeomSetBody(this->box[i],this->body[i]);
}
// O chassis é composto por dois blocos que são fixos entre si
dJointID fixed = dJointCreateFixed(world,0);
dJointAttach(fixed,this->body[1],this->body[0]);
dJointSetFixed(fixed);
}
void Spikey::stick(Body* b) {
if (hinge_) dJointDestroy(hinge_);
hinge_ = dJointCreateFixed(LEVEL->world, 0);
dJointAttach(hinge_, body_.body_id(), b ? b->body_id() : 0);
dJointSetFixed(hinge_);
state_ = STUCK;
}
/*** ロボットアームの生成 ***/
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]);
}
}
sODEJoint* sODEJoint::AddFixedJoint( int id, dBodyID body1, dBodyID body2 )
{
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 = dJointCreateFixed( g_ODEWorld, 0 );
dJointAttach( pJoint->oJoint, body1, body2 );
dJointSetFixed( pJoint->oJoint );
pJoint->iID = id;
return pJoint;
}
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;
}
/**
* This method is called if the Joint should be attached.
* It gets the ODE-bodies from the JointInteraction-class and
* then either attaches the Joint by calling the virtual
* method attachJoint or attaches a fixed ODE-joint if
* the joint is not active.
**/
void Joint::attach()
{
if (alreadyAttached)
return;
alreadyAttached = true;
dBodyID body1, body2;
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
if (active)
attachJoint(body1, body2);
else
{
joint = dJointCreateFixed(world, 0);
dJointAttach(joint, body1, body2);
dJointSetFixed(joint);
} // else
jointInteraction->attachedJoints.push_back(this);
} // attachJoint
/* ------------------------
* 箱の生成,描画
------------------------ */
void MakeBox()
{
/* ローカル変数の定義 */
dMass mass;
for(int i = 0; i < BOX_NUM; i++)
{
/* ボディを生成 */
box[i].body = dBodyCreate(world);
dMassSetZero(&mass);
/* 質量を設定 */
dMassSetBoxTotal(&mass, boxM[i], boxSize[i][0], boxSize[i][1], boxSize[i][2]);
dBodySetMass(box[i].body, &mass);
/* ジオメトリ生成 */
box[i].geom = dCreateBox(space, boxSize[i][0], boxSize[i][1], boxSize[i][2]);
/* ジオメトリをセット */
dGeomSetBody(box[i].geom, box[i].body);
dBodySetPosition(box[i].body, boxPos[i][0], boxPos[i][1], boxPos[i][2]);
/* 箱と地面の結合 */
fixed[i] = dJointCreateFixed(world, 0);
dJointAttach(fixed[i], box[i].body, 0);
dJointSetFixed(fixed[i]);
}
}
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;
}
IoObject *IoODEFixed_fix(IoODEFixed *self, IoObject *locals, IoMessage *m)
{
IoODEJoint_assertValidJoint(self, locals, m);
dJointSetFixed(JOINTID);
return self;
}
//Fixed
void JointFixed::SetFixed(void)
{
dJointSetFixed(this->_id);
}
/**
* 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 Hinge2Joint::attachJoint(dBodyID obj1, dBodyID obj2)
{
TransformationData entityTrans;
gmtl::Vec3f newAnchor, newAxis1, newAxis2, scaleVec;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
joint = dJointCreateHinge2(world, 0);
// Attaching the Joint is done after the position and
// orientation calculation!
// dJointAttach(joint, obj1, obj2);
newAxis1 = axis1;
newAxis2 = axis2;
newAnchor = anchor;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
// scaleVec[0] = mainEntity->getXScale();
// scaleVec[1] = mainEntity->getYScale();
// scaleVec[2] = mainEntity->getZScale();
scaleVec = entityTrans.scale;
// scale Anchor-offset by mainEntity-scale value
newAnchor[0] *= scaleVec[0];
newAnchor[1] *= scaleVec[1];
newAnchor[2] *= scaleVec[2];
// scale Axes by mainEntity-scale value because of possible distortion
newAxis1[0] *= scaleVec[0];
newAxis1[1] *= scaleVec[1];
newAxis1[2] *= scaleVec[2];
gmtl::normalize(newAxis1);
newAxis2[0] *= scaleVec[0];
newAxis2[1] *= scaleVec[1];
newAxis2[2] *= scaleVec[2];
gmtl::normalize(newAxis2);
// get the Rotation of the mainEntity
// axAng[0] = mainEntity->getRotAngle();
// axAng[1] = mainEntity->getXRot();
// axAng[2] = mainEntity->getYRot();
// axAng[3] = mainEntity->getZRot();
// gmtl::set(entityRot, axAng);
entityRot = entityTrans.orientation;
// rotate Axes by mainEntity-rotation
newAxis1 *= entityRot;
newAxis2 *= entityRot;
// rotate Anchor-offset by mainEntity-rotation
newAnchor *= entityRot;
// transform new Anchor to world coordinates
/* newAnchor[0] += mainEntity->getXTrans();
newAnchor[1] += mainEntity->getYTrans();
newAnchor[2] += mainEntity->getZTrans();*/
newAnchor += entityTrans.position;
} // if
// create a helper body when necessary to avoid the
// Segmentation Fault that is thrown by ODE (v0.5)
// when attaching a Hinge2Joint to the static environment
if (obj1 == 0 || obj2 == 0)
{
helperBody = dBodyCreate(world);
dBodySetPosition(helperBody, newAnchor[0], newAnchor[1], newAnchor[2]);
helperJoint = dJointCreateFixed(world, 0);
dJointAttach(helperJoint, helperBody, 0);
dJointSetFixed(helperJoint);
if (obj1 == 0)
obj1 = helperBody;
else
obj2 = helperBody;
usedHelperJoint = true;
} // if
dJointAttach(joint, obj1, obj2);
dJointSetHinge2Anchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
dJointSetHinge2Axis1(joint, newAxis1[0], newAxis1[1], newAxis1[2]);
dJointSetHinge2Axis2(joint, newAxis2[0], newAxis2[1], newAxis2[2]);
} // attachJoint
int main (int argc, char **argv)
{
dInitODE2(0);
bool fixed = true;
// 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;
dVector3 offset;
dSetZero (offset, 4);
// Default test case
if (argc >= 2 )
{
for (int i=1; i < argc; ++i)
{
//static int tata = 0;
if (1)
{
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
type = dJointTypeSlider;
if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
{
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
tc = 1;
if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
tc = 2;
if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
tc = 3;
if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
fixed = false;
}
}
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
dMatrix3 R;
// Create the Obstacle
geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
dGeomSetCollideBits (geom[OBS], catBits[ALL]);
//Rotation of 45deg around y
dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
dGeomSetRotation (geom[OBS], R);
dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
//Rotation of 90deg around y
// Will orient the Z axis along X
dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
// Create Body2 (Wiil be attached to the world)
body[BODY2] = dBodyCreate (world);
// Main axis of cylinder is along X=1
dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dMassAdjust (&m, Mass1);
geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dGeomSetBody (geom[BODY2], body[BODY2]);
dGeomSetOffsetRotation (geom[BODY2], R);
dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY2], &m);
// Create Body 1 (Slider on the prismatic axis)
body[BODY1] = dBodyCreate (world);
// Main axis of capsule is along X=1
dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
dMassAdjust (&m, Mass1);
geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
dGeomSetBody (geom[BODY1], body[BODY1]);
dGeomSetOffsetRotation (geom[BODY1], R);
dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
dMass mRect;
dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dMassAdd (&m, &mRect);
// TODO: translate m?
geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dGeomSetBody (geom[RECT], body[BODY1]);
dGeomSetOffsetPosition (geom[RECT],
(BODY1_LENGTH-RECT_SIDES[0]) /2.0,
0.0,
-RADIUS -RECT_SIDES[2]/2.0);
dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY1], &m);
setPositionBodies (tc);
if ( fixed )
{
// Attache external cylinder to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[BODY2]);
dJointSetFixed (fixed );
dWorldSetGravity (world,0,0,-0.8);
}
else
{
dWorldSetGravity (world,0,0,0);
}
// The static is here only to help debugging
switch (type)
{
case dJointTypeSlider :
{
dSliderJoint *sj = new dSliderJoint (world, 0);
sj->attach (body[BODY1], body[BODY2]);
sj->setAxis (1, 0, 0);
joint = sj;
}
break;
case dJointTypePiston : // fall through default
default:
{
dPistonJoint *pj = new dPistonJoint (world, 0);
pj->attach (body[BODY1], body[BODY2]);
pj->setAxis (1, 0, 0);
dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
joint = pj;
}
break;
};
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
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;
if (argc >= 2 ) {
for (int i=1; i < argc; ++i) {
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
type = dJointTypePR;
if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
}
dInitODE2(0);
world.setERP (0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
// Create the body attached to the World
body[W].create (world);
// Main axis of cylinder is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[W], body[W]);
dGeomSetCategoryBits (geom[W], catBits[W]);
dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
body[W].setMass(m);
// Create the dandling body
body[D].create(world);
// Main axis of capsule is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[D], body[D]);
dGeomSetCategoryBits (geom[D], catBits[D]);
dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
body[D].setMass(&m);
// Create the external part of the slider joint
geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
dGeomSetCollideBits (geom[EXT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
// Create the internal part of the slider joint
geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
INT_EXT_RATIO*extDim[Y],
INT_EXT_RATIO*extDim[Z]);
dGeomSetCategoryBits (geom[INT], catBits[INT]);
dGeomSetCollideBits (geom[INT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
dMatrix3 R;
dGeomID id;
// Create the first axis of the universal joi9nt
geom[AXIS1] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
dGeomSetRotation (geom[AXIS1], R);
dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
dGeomSetCollideBits (geom[AXIS1],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS1];
dGeomTransformSetGeom (geom[AXIS1], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the second axis of the universal joint
geom[AXIS2] = dCreateGeomTransform (space);
//Rotation of 90deg around y
dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
dGeomSetRotation (geom[AXIS2], R);
dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
dGeomSetCollideBits (geom[AXIS2],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
id = geom[AXIS2];
dGeomTransformSetGeom (geom[AXIS2], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );
// Create the anchor
geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
dGeomSetCollideBits (geom[ANCHOR],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
if (body[W]) {
body[W].setPosition(0, 0, 5);
}
if (geom[EXT]) {
dGeomSetPosition (geom[EXT], 0,0,3.8);
}
if (geom[INT]) {
dGeomSetPosition (geom[INT], 0,0,2.6);
}
if (geom[AXIS1]) {
dGeomSetPosition (geom[AXIS1], 0,0,2.5);
}
if (geom[AXIS2]) {
dGeomSetPosition (geom[AXIS2], 0,0,2.5);
}
if (geom[ANCHOR]) {
dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
}
if (body[D]) {
body[D].setPosition(0,0,1.5);
}
// Attache the upper box to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[W]);
dJointSetFixed (fixed );
if (type == dJointTypePR) {
dPRJoint *pr = new dPRJoint (world, 0);
pr->attach (body[W], body[D]);
pr->setAxis1 (0, 0, -1);
pr->setAxis2 (1, 0, 0);
joint = pr;
dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
}
else {
dPUJoint *pu = new dPUJoint (world, 0);
pu->attach (body[W], body[D]);
pu->setAxis1 (1, 0, 0);
pu->setAxis2 (0, 1, 0);
pu->setAxisP (0, 0, -1);
joint = pu;
dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
}
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
{
int i;
dMass m;
// chassis body
body[bodyI] = dBodyCreate (world);
dBodySetPosition (body[bodyI],x,y,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (body[bodyI],&m);
box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxI],body[bodyI]);
// wheel bodies
for (i=1; i<=4; i++) {
body[bodyI+i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[bodyI+i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[bodyI+i],&m);
sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
}
dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[jointI+i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1));
dJointSetHinge2Axis2 (joint[jointI+i],0,1,0);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
}
//center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
dBodyID b = dBodyCreate (world);
dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (b,&m);
dJointID j = dJointCreateFixed(world, 0);
dJointAttach(j, body[bodyI], b);
dJointSetFixed(j);
//box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
//dGeomSetBody (box[boxI+1],b);
bodyI += 5;
jointI += 4;
boxI += 1;
sphereI += 4;
}
void CODEFixedJoint::setParams()
{
dJointSetFixed(mID);
}
// ロボットの生成
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);
}
/**
* Creates the ode joint objects and includes it into the ode-engine.
* @param body1
* @param body2
* @return
*/
dJointID ODE_FixedJoint::createJoint(dBodyID body1, dBodyID body2) {
dJointID newJoint = dJointCreateFixed(mWorldID, mGeneralJointGroup);
dJointAttach(newJoint, body1, body2);
dJointSetFixed(newJoint);
return newJoint;
}
