737 lines
20 KiB
C++
737 lines
20 KiB
C++
|
/*************************************************************************
|
||
|
* *
|
||
|
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||
|
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||
|
* *
|
||
|
* This library is free software; you can redistribute it and/or *
|
||
|
* modify it under the terms of EITHER: *
|
||
|
* (1) The GNU Lesser General Public License as published by the Free *
|
||
|
* Software Foundation; either version 2.1 of the License, or (at *
|
||
|
* your option) any later version. The text of the GNU Lesser *
|
||
|
* General Public License is included with this library in the *
|
||
|
* file LICENSE.TXT. *
|
||
|
* (2) The BSD-style license that is included with this library in *
|
||
|
* the file LICENSE-BSD.TXT. *
|
||
|
* *
|
||
|
* This library is distributed in the hope that it will be useful, *
|
||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||
|
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||
|
* *
|
||
|
*************************************************************************/
|
||
|
|
||
|
/*
|
||
|
This program demonstrates how the PU joint works.
|
||
|
A PU joint is a combination of a Universal joint and a Slider joint.
|
||
|
It is a universal joint with a slider between the anchor point and
|
||
|
body 1.
|
||
|
|
||
|
|
||
|
The upper yellow body is fixed to the world
|
||
|
The lower yellow body is attached to the upper body by a PU joint
|
||
|
The green object is one aprt of the slider.
|
||
|
The purple object is the second part of the slider.
|
||
|
The red object represent the axis1 of the universal part.
|
||
|
The blue object represent the axis2 of the universal part.
|
||
|
The gray object represent the anchor2 of the PU joint.
|
||
|
*/
|
||
|
|
||
|
|
||
|
#include <ode/ode.h>
|
||
|
#include <drawstuff/drawstuff.h>
|
||
|
#include <iostream>
|
||
|
#include <math.h>
|
||
|
#include "texturepath.h"
|
||
|
|
||
|
#ifdef _MSC_VER
|
||
|
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
|
||
|
#endif
|
||
|
// select correct drawing functions
|
||
|
#ifdef dDOUBLE
|
||
|
#define dsDrawBox dsDrawBoxD
|
||
|
#define dsDrawCylinder dsDrawCylinderD
|
||
|
#define dsDrawCapsule dsDrawCapsuleD
|
||
|
#endif
|
||
|
|
||
|
enum IDX_CYL_DIM
|
||
|
{
|
||
|
RADIUS,
|
||
|
LENGTH,
|
||
|
NUM_CYL_DIM
|
||
|
};
|
||
|
|
||
|
|
||
|
const dVector3 boxDim = {1,1,1};
|
||
|
const dVector3 extDim = {0.2,0.2,1.2};
|
||
|
const dVector3 ancDim = {0.2,0.2,0.5};
|
||
|
const dReal axDim[NUM_CYL_DIM] = {0.1,1.0};
|
||
|
|
||
|
|
||
|
int type = dJointTypePU;
|
||
|
|
||
|
|
||
|
const dReal VEL_INC = 0.01; // Velocity increment
|
||
|
|
||
|
// physics parameters
|
||
|
const dReal PI = 3.14159265358979323846264338327950288419716939937510;
|
||
|
|
||
|
|
||
|
const dReal INT_EXT_RATIO = 0.8;
|
||
|
|
||
|
#define X 0
|
||
|
#define Y 1
|
||
|
#define Z 2
|
||
|
|
||
|
enum INDEX
|
||
|
{
|
||
|
W = 0,
|
||
|
D,
|
||
|
EXT,
|
||
|
INT,
|
||
|
AXIS1,
|
||
|
AXIS2,
|
||
|
ANCHOR,
|
||
|
GROUND,
|
||
|
NUM_PARTS,
|
||
|
ALL = NUM_PARTS,
|
||
|
// INDEX for catBits
|
||
|
JOINT,
|
||
|
LAST_INDEX_CNT
|
||
|
};
|
||
|
|
||
|
const int catBits[LAST_INDEX_CNT] =
|
||
|
{
|
||
|
0x0001, ///< W Cylinder category
|
||
|
0x0002, ///< D Cylinder category
|
||
|
0x0004, ///< EXT sliderr category
|
||
|
0x0008, ///< INT slider category
|
||
|
0x0010, ///< AXIS1 universal category
|
||
|
0x0020, ///< AXIS2 universal category
|
||
|
0x0040, ///< ANCHOR category
|
||
|
0x0080, ///< Ground category
|
||
|
~0L, ///< All categories
|
||
|
0x0004 | 0x0008 | 0x0010 | 0x0020 ///< JOINT category
|
||
|
};
|
||
|
|
||
|
#define Mass1 10
|
||
|
#define Mass2 8
|
||
|
|
||
|
|
||
|
//camera view
|
||
|
static float xyz[3] = {6.0f,0.0f,6.0000f};
|
||
|
static float hpr[3] = {-180.000f,-25.5000f,0.0000f};
|
||
|
|
||
|
|
||
|
//world,space,body & geom
|
||
|
static dWorldID world;
|
||
|
static dSpaceID space;
|
||
|
static dJointGroupID contactgroup;
|
||
|
static dBodyID body[NUM_PARTS];
|
||
|
static dGeomID geom[NUM_PARTS];
|
||
|
|
||
|
static dJoint *joint;
|
||
|
|
||
|
|
||
|
|
||
|
const dReal BOX_SIDES[3] = {1.0,1.0,1.0};
|
||
|
const dReal OBS_SIDES[3] = {0.4,0.4,0.4};
|
||
|
const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};
|
||
|
|
||
|
|
||
|
//collision detection
|
||
|
static void nearCallback (void *, dGeomID o1, dGeomID o2)
|
||
|
{
|
||
|
int i,n;
|
||
|
|
||
|
const int N = 10;
|
||
|
dContact contact[N];
|
||
|
n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) );
|
||
|
if (n > 0) {
|
||
|
for (i=0; i<n; i++) {
|
||
|
contact[i].surface.mode = (dContactSlip1 | dContactSlip2 |
|
||
|
dContactSoftERP | dContactSoftCFM |
|
||
|
dContactApprox1);
|
||
|
contact[i].surface.mu = 0.1;
|
||
|
contact[i].surface.slip1 = 0.02;
|
||
|
contact[i].surface.slip2 = 0.02;
|
||
|
contact[i].surface.soft_erp = 0.1;
|
||
|
contact[i].surface.soft_cfm = 0.0001;
|
||
|
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
|
||
|
dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
static void printKeyBoardShortCut()
|
||
|
{
|
||
|
printf ("Press 'h' for this help.\n");
|
||
|
printf ("Press 'q' to add force on BLUE body along positive x direction.\n");
|
||
|
printf ("Press 'w' to add force on BLUE body along negative x direction.\n");
|
||
|
|
||
|
printf ("Press 'a' to add force on BLUE body along positive y direction.\n");
|
||
|
printf ("Press 's' to add force on BLUE body along negative y direction.\n");
|
||
|
|
||
|
printf ("Press 'z' to add force on BLUE body along positive z direction.\n");
|
||
|
printf ("Press 'x' to add force on BLUE body along negative z direction.\n");
|
||
|
|
||
|
printf ("Press 'e' to add torque on BLUE body around positive x direction \n");
|
||
|
printf ("Press 'r' to add torque on BLUE body around negative x direction \n");
|
||
|
|
||
|
printf ("Press 'd' to add torque on BLUE body around positive y direction \n");
|
||
|
printf ("Press 'f' to add torque on BLUE body around negative y direction \n");
|
||
|
|
||
|
printf ("Press 'c' to add torque on BLUE body around positive z direction \n");
|
||
|
printf ("Press 'v' to add torque on BLUE body around negative z direction \n");
|
||
|
|
||
|
printf ("Press '.' to increase joint velocity along the prismatic direction.\n");
|
||
|
printf ("Press ',' to decrease joint velocity along the prismatic direction.\n");
|
||
|
|
||
|
printf ("Press 'l' Toggle ON/OFF the limits on all the axis\n");
|
||
|
printf ("Press 'g' Toggle ON/OFF the gravity\n");
|
||
|
|
||
|
|
||
|
printf ("Press 'p' to print the position, angle and rates of the joint.\n");
|
||
|
}
|
||
|
|
||
|
|
||
|
// start simulation - set viewpoint
|
||
|
static void start()
|
||
|
{
|
||
|
dAllocateODEDataForThread(dAllocateMaskAll);
|
||
|
|
||
|
dsSetViewpoint (xyz,hpr);
|
||
|
printf ("This program demonstrates how the PU joint works.\n");
|
||
|
printf ("A PU joint is a combination of a Universal joint and a Slider joint.\n");
|
||
|
printf ("It is a universal joint with a slider between the anchor point and \n");
|
||
|
printf ("body 1.\n\n");
|
||
|
printf ("The upper yellow body is fixed to the world\n");
|
||
|
printf ("The lower yellow body is attached to the upper body by a PU joint\n");
|
||
|
printf ("The green object is one aprt of the slider.\n");
|
||
|
printf ("The purple object is the second part of the slider.\n");
|
||
|
printf ("The red object represent the axis1 of the universal part. \n");
|
||
|
printf ("The blue object represent the axis2 of the universal part. \n");
|
||
|
printf ("The gray object represent the anchor2 of the PU joint. \n");
|
||
|
printKeyBoardShortCut();
|
||
|
}
|
||
|
|
||
|
// function to update camera position at each step.
|
||
|
void update()
|
||
|
{
|
||
|
// static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w");
|
||
|
|
||
|
// static int cnt = 0;
|
||
|
// char str[24];
|
||
|
// sprintf(str, "%06d",cnt++);
|
||
|
|
||
|
// dWorldExportDIF(world, file, str);
|
||
|
}
|
||
|
|
||
|
|
||
|
// called when a key pressed
|
||
|
static void command (int cmd)
|
||
|
{
|
||
|
switch (cmd) {
|
||
|
case 'h' : case 'H' : case '?' :
|
||
|
printKeyBoardShortCut();
|
||
|
break;
|
||
|
|
||
|
// Force
|
||
|
case 'q' : case 'Q' :
|
||
|
dBodyAddForce(body[D],40,0,0);
|
||
|
break;
|
||
|
case 'w' : case 'W' :
|
||
|
dBodyAddForce(body[D],-40,0,0);
|
||
|
break;
|
||
|
|
||
|
case 'a' : case 'A' :
|
||
|
dBodyAddForce(body[D],0,40,0);
|
||
|
break;
|
||
|
case 's' : case 'S' :
|
||
|
dBodyAddForce(body[D],0,-40,0);
|
||
|
break;
|
||
|
|
||
|
case 'z' : case 'Z' :
|
||
|
dBodyAddForce(body[D],0,0,40);
|
||
|
break;
|
||
|
case 'x' : case 'X' :
|
||
|
dBodyAddForce(body[D],0,0,-40);
|
||
|
break;
|
||
|
|
||
|
// Torque
|
||
|
case 'e': case 'E':
|
||
|
dBodyAddTorque(body[D],0.1,0,0);
|
||
|
break;
|
||
|
case 'r': case 'R':
|
||
|
dBodyAddTorque(body[D],-0.1,0,0);
|
||
|
break;
|
||
|
|
||
|
case 'd': case 'D':
|
||
|
dBodyAddTorque(body[D],0, 0.1,0);
|
||
|
break;
|
||
|
case 'f': case 'F':
|
||
|
dBodyAddTorque(body[D],0,-0.1,0);
|
||
|
break;
|
||
|
|
||
|
case 'c': case 'C':
|
||
|
dBodyAddTorque(body[D],0,0,0.1);
|
||
|
break;
|
||
|
case 'v': case 'V':
|
||
|
dBodyAddTorque(body[D],0,0,0.1);
|
||
|
break;
|
||
|
|
||
|
// Velocity of joint
|
||
|
case ',': case '<' : {
|
||
|
dReal vel = joint->getParam (dParamVel3) - VEL_INC;
|
||
|
joint->setParam (dParamVel3, vel);
|
||
|
joint->setParam (dParamFMax3, 2);
|
||
|
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
|
||
|
}
|
||
|
break;
|
||
|
|
||
|
case '.': case '>' : {
|
||
|
dReal vel = joint->getParam (dParamVel3) + VEL_INC;
|
||
|
joint->setParam (dParamVel3, vel);
|
||
|
joint->setParam (dParamFMax3, 2);
|
||
|
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
|
||
|
}
|
||
|
break;
|
||
|
|
||
|
case 'l': case 'L' : {
|
||
|
dReal aLimit, lLimit, fmax;
|
||
|
if ( joint->getParam (dParamFMax) ) {
|
||
|
aLimit = dInfinity;
|
||
|
lLimit = dInfinity;
|
||
|
fmax = 0;
|
||
|
}
|
||
|
else {
|
||
|
aLimit = 0.25*PI;
|
||
|
lLimit = 0.5*axDim[LENGTH];
|
||
|
fmax = 0.02;
|
||
|
}
|
||
|
|
||
|
joint->setParam (dParamFMax1, fmax);
|
||
|
joint->setParam (dParamFMax2, fmax);
|
||
|
joint->setParam (dParamFMax3, fmax);
|
||
|
|
||
|
switch (joint->getType() ) {
|
||
|
case dJointTypePR : {
|
||
|
dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint);
|
||
|
pr->setParam (dParamLoStop, -lLimit);
|
||
|
pr->setParam (dParamHiStop, -lLimit);
|
||
|
pr->setParam (dParamLoStop2, aLimit);
|
||
|
pr->setParam (dParamHiStop2, -aLimit);
|
||
|
}
|
||
|
break;
|
||
|
case dJointTypePU : {
|
||
|
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
|
||
|
pu->setParam (dParamLoStop1, -aLimit);
|
||
|
pu->setParam (dParamHiStop1, aLimit);
|
||
|
pu->setParam (dParamLoStop2, -aLimit);
|
||
|
pu->setParam (dParamHiStop2, aLimit);
|
||
|
pu->setParam (dParamLoStop3, -lLimit);
|
||
|
pu->setParam (dParamHiStop3, lLimit);
|
||
|
}
|
||
|
break;
|
||
|
default: {} // keep the compiler happy
|
||
|
}
|
||
|
}
|
||
|
|
||
|
break;
|
||
|
|
||
|
case 'g': case 'G' : {
|
||
|
dVector3 g;
|
||
|
dWorldGetGravity(world, g);
|
||
|
if ( g[2]< -0.1 )
|
||
|
dWorldSetGravity(world, 0, 0, 0);
|
||
|
else
|
||
|
dWorldSetGravity(world, 0, 0, -0.5);
|
||
|
|
||
|
}
|
||
|
break;
|
||
|
|
||
|
case 'p' :case 'P' : {
|
||
|
switch (joint->getType() ) {
|
||
|
case dJointTypeSlider : {
|
||
|
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
|
||
|
std::cout<<"Position ="<<sj->getPosition() <<"\n";
|
||
|
}
|
||
|
break;
|
||
|
case dJointTypePU : {
|
||
|
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
|
||
|
std::cout<<"Position ="<<pu->getPosition() <<"\n";
|
||
|
std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n";
|
||
|
std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n";
|
||
|
std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n";
|
||
|
std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n";
|
||
|
std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n";
|
||
|
}
|
||
|
break;
|
||
|
default: {} // keep the compiler happy
|
||
|
}
|
||
|
}
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
static void drawBox (dGeomID id, int R, int G, int B)
|
||
|
{
|
||
|
if (!id)
|
||
|
return;
|
||
|
|
||
|
const dReal *pos = dGeomGetPosition (id);
|
||
|
const dReal *rot = dGeomGetRotation (id);
|
||
|
dsSetColor (R,G,B);
|
||
|
|
||
|
dVector3 l;
|
||
|
dGeomBoxGetLengths (id, l);
|
||
|
dsDrawBox (pos, rot, l);
|
||
|
}
|
||
|
|
||
|
|
||
|
// simulation loop
|
||
|
static void simLoop (int pause)
|
||
|
{
|
||
|
static bool todo = false;
|
||
|
if ( todo ) { // DEBUG
|
||
|
static int cnt = 0;
|
||
|
++cnt;
|
||
|
|
||
|
if (cnt == 5)
|
||
|
command ( 'q' );
|
||
|
if (cnt == 10)
|
||
|
dsStop();
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
if (!pause) {
|
||
|
double simstep = 0.01; // 10ms simulation steps
|
||
|
double dt = dsElapsedTime();
|
||
|
|
||
|
int nrofsteps = (int) ceilf (dt/simstep);
|
||
|
if (!nrofsteps)
|
||
|
nrofsteps = 1;
|
||
|
|
||
|
for (int i=0; i<nrofsteps && !pause; i++) {
|
||
|
dSpaceCollide (space,0,&nearCallback);
|
||
|
dWorldStep (world, simstep);
|
||
|
|
||
|
dJointGroupEmpty (contactgroup);
|
||
|
}
|
||
|
|
||
|
update();
|
||
|
|
||
|
|
||
|
dReal radius, length;
|
||
|
|
||
|
dsSetTexture (DS_WOOD);
|
||
|
|
||
|
drawBox (geom[W], 1,1,0);
|
||
|
|
||
|
|
||
|
drawBox (geom[EXT], 0,1,0);
|
||
|
|
||
|
dVector3 anchorPos;
|
||
|
|
||
|
|
||
|
|
||
|
dReal ang1 = 0;
|
||
|
dReal ang2 = 0;
|
||
|
dVector3 axisP, axisR1, axisR2;
|
||
|
|
||
|
if ( dJointTypePU == type ) {
|
||
|
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
|
||
|
ang1 = pu->getAngle1();
|
||
|
ang2 = pu->getAngle2();
|
||
|
pu->getAxis1 (axisR1);
|
||
|
pu->getAxis2 (axisR2);
|
||
|
pu->getAxisP (axisP);
|
||
|
|
||
|
dJointGetPUAnchor (pu->id(), anchorPos);
|
||
|
}
|
||
|
else if ( dJointTypePR == type ) {
|
||
|
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
|
||
|
pr->getAxis1 (axisP);
|
||
|
pr->getAxis2 (axisR1);
|
||
|
|
||
|
dJointGetPRAnchor (pr->id(), anchorPos);
|
||
|
}
|
||
|
|
||
|
|
||
|
// Draw the axisR
|
||
|
if ( geom[INT] ) {
|
||
|
dsSetColor (1,0,1);
|
||
|
dVector3 l;
|
||
|
dGeomBoxGetLengths (geom[INT], l);
|
||
|
|
||
|
const dReal *rotBox = dGeomGetRotation (geom[W]);
|
||
|
|
||
|
dVector3 pos;
|
||
|
for (int i=0; i<3; ++i)
|
||
|
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
|
||
|
dsDrawBox (pos, rotBox, l);
|
||
|
}
|
||
|
|
||
|
dsSetTexture (DS_CHECKERED);
|
||
|
if ( geom[AXIS1] ) {
|
||
|
dQuaternion q, qAng;
|
||
|
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
|
||
|
dGeomGetQuaternion (geom[AXIS1], q);
|
||
|
|
||
|
dQuaternion qq;
|
||
|
dQMultiply1 (qq, qAng, q);
|
||
|
dMatrix3 R;
|
||
|
dQtoR (qq,R);
|
||
|
|
||
|
|
||
|
dGeomCylinderGetParams (geom[AXIS1], &radius, &length);
|
||
|
dsSetColor (1,0,0);
|
||
|
dsDrawCylinder (anchorPos, R, length, radius);
|
||
|
}
|
||
|
|
||
|
if ( dJointTypePU == type && geom[AXIS2] ) {
|
||
|
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
|
||
|
|
||
|
dQuaternion q, qAng, qq, qq1;
|
||
|
dGeomGetQuaternion (geom[AXIS2], q);
|
||
|
|
||
|
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
|
||
|
dQMultiply1 (qq, qAng, q);
|
||
|
|
||
|
|
||
|
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
|
||
|
|
||
|
dQMultiply1 (qq1, qAng, qq);
|
||
|
|
||
|
|
||
|
dMatrix3 R;
|
||
|
dQtoR (qq1,R);
|
||
|
|
||
|
|
||
|
dGeomCylinderGetParams (geom[AXIS2], &radius, &length);
|
||
|
dsSetColor (0,0,1);
|
||
|
dsDrawCylinder (anchorPos, R, length, radius);
|
||
|
}
|
||
|
|
||
|
dsSetTexture (DS_WOOD);
|
||
|
|
||
|
// Draw the anchor
|
||
|
if ( geom[ANCHOR] ) {
|
||
|
dsSetColor (1,1,1);
|
||
|
dVector3 l;
|
||
|
dGeomBoxGetLengths (geom[ANCHOR], l);
|
||
|
|
||
|
const dReal *rotBox = dGeomGetRotation (geom[D]);
|
||
|
const dReal *posBox = dGeomGetPosition (geom[D]);
|
||
|
|
||
|
dVector3 e;
|
||
|
for (int i=0; i<3; ++i)
|
||
|
e[i] = posBox[i] - anchorPos[i];
|
||
|
dNormalize3 (e);
|
||
|
|
||
|
dVector3 pos;
|
||
|
for (int i=0; i<3; ++i)
|
||
|
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
|
||
|
dsDrawBox (pos, rotBox, l);
|
||
|
}
|
||
|
|
||
|
drawBox (geom[D], 1,1,0);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
void Help (char **argv)
|
||
|
{
|
||
|
printf ("%s ", argv[0]);
|
||
|
printf (" -h | --help : print this help\n");
|
||
|
printf (" -p | --PRJoint : Use a PR joint instead of PU joint\n");
|
||
|
printf (" -t | --texture-path path : Path to the texture.\n");
|
||
|
printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
|
||
|
printf ("--------------------------------------------------\n");
|
||
|
printf ("Hit any key to continue:");
|
||
|
getchar();
|
||
|
|
||
|
exit (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 >= argc || // Check if we have enough arguments
|
||
|
argv[j][0] == '\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 = 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;
|
||
|
|
||
|
// Create the body attached to the World
|
||
|
body[W] = dBodyCreate (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]) );
|
||
|
dBodySetMass (body[W], &m);
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
// Create the dandling body
|
||
|
body[D] = dBodyCreate (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]) );
|
||
|
dBodySetMass (body[D], &m);
|
||
|
|
||
|
|
||
|
// Create the external part of the slider joint
|
||
|
geom[EXT] = dCreateBox (0, 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 (0, 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;
|
||
|
// Create the first axis of the universal joi9nt
|
||
|
//Rotation of 90deg around y
|
||
|
geom[AXIS1] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]);
|
||
|
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]);
|
||
|
|
||
|
|
||
|
// Create the second axis of the universal joint
|
||
|
geom[AXIS2] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]);
|
||
|
//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]);
|
||
|
|
||
|
// Create the anchor
|
||
|
geom[ANCHOR] = dCreateBox (0, 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]) {
|
||
|
dBodySetPosition(body[W], 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]) {
|
||
|
dBodySetPosition(body[D], 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;
|
||
|
}
|
||
|
|