www.pudn.com > RCApp-src.zip > mod_buggy.cpp
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001 Russell L. Smith. *
* 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 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. *
* *
* 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 GNU *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library (see the file LICENSE.TXT); if not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA. *
* *
*************************************************************************/
/*
buggy with suspension.
this also shows you how to use geom groups.
*/
#include
#include
#include "SDL.h"
#include
#include
#ifdef MSVC
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif
// some constants
#define LENGTH 1.05 // chassis length
#define WIDTH .6 // chassis width
#define HEIGHT .45 // chassis height
#define RADIUS 0.18 // wheeldInfinity radius
#define STARTZ 1.0 // starting height of chassis
#define CMASS 1.5 // chassis mass
#define WMASS 1.0 // wheel mass
#define MU .6
#define SUSPHEIGHT .08
#define SUSPERP .8
#define SUSPCFM .4
#define VCONS 1.5
#define MAXV 60.0
#define MAXF .12 // .125 was good too
#define ETUVETO
#define TAKAVETO
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID body[5];
static dJointID joint[4]; // joint[0] is the front wheel
static dJointGroupID contactgroup;
static dGeomID ground,geom_group;
static dGeomID box[1];
static dGeomID sphere[4];
static dGeomID ground_box;
// things that the user controls
static dReal speed=0,steer=0; // user commands
extern SDL_Joystick *joystick;
short xax, yax;
float kaasu, kaanto;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i,n;
// only collide things with the ground
int g1 = (o1 == ground || o1 == ground_box);
int g2 = (o2 == ground || o2 == ground_box);
if (!(g1 ^ g2)) return;
const int N = 11;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
for (i=0; iMAXF?MAXF:tv1;
maxvee = butt ? -kaasu*MAXV : kaasu*MAXV;
// printf("maxvee: %f\tvauhti: %f\n", maxvee, vauhti);
// kaanto = vauhti<-4.0 ? kaanto/(vauhti/(-4.0)) : kaanto;
if (kaanto>0) {
dJointSetHinge2Param (joint[3],dParamVel,kaanto);
dJointSetHinge2Param (joint[3],dParamFMax,.5);
dJointSetHinge2Param (joint[3],dParamLoStop,kaanto);
dJointSetHinge2Param (joint[3],dParamHiStop,kaanto);
dJointSetHinge2Param (joint[3],dParamFudgeFactor,0.1);
r1 = LENGTH/tan((double)kaanto);
beeta = atan(LENGTH/(r1+WIDTH));
dJointSetHinge2Param (joint[0],dParamVel,beeta);
dJointSetHinge2Param (joint[0],dParamFMax,.5);
dJointSetHinge2Param (joint[0],dParamLoStop,beeta);
dJointSetHinge2Param (joint[0],dParamHiStop,beeta);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
v2 = tv1*(r1+WIDTH)/r1;
v2 = VCONS*v2/vauhti;
v2 = v2>MAXF?MAXF:v2;
#ifdef TAKAVETO
dJointSetHinge2Param (joint[1],dParamVel2, maxvee);
dJointSetHinge2Param (joint[1],dParamFMax2, tv1);
dJointSetHinge2Param (joint[2],dParamVel2, maxvee);
dJointSetHinge2Param (joint[2],dParamFMax2, v2);
#endif
#ifdef ETUVETO
dJointSetHinge2Param (joint[3],dParamVel2, maxvee);
dJointSetHinge2Param (joint[3],dParamFMax2, tv1);
dJointSetHinge2Param (joint[0],dParamVel2, maxvee);
dJointSetHinge2Param (joint[0],dParamFMax2, v2);
#endif
} else {
dJointSetHinge2Param (joint[0],dParamVel,kaanto);
dJointSetHinge2Param (joint[0],dParamFMax,.5);
dJointSetHinge2Param (joint[0],dParamLoStop,kaanto);
dJointSetHinge2Param (joint[0],dParamHiStop,kaanto);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
r1 = LENGTH/tan((double)kaanto);
beeta = -atan(LENGTH/(-r1+WIDTH));
dJointSetHinge2Param (joint[3],dParamVel,beeta);
dJointSetHinge2Param (joint[3],dParamFMax,.5);
dJointSetHinge2Param (joint[3],dParamLoStop,beeta);
dJointSetHinge2Param (joint[3],dParamHiStop,beeta);
dJointSetHinge2Param (joint[3],dParamFudgeFactor,0.1);
v2 = tv1*(-r1+WIDTH)/r1;
v2 = VCONS*v2/vauhti;
v2 = v2>MAXF?MAXF:v2;
#ifdef TAKAVETO
dJointSetHinge2Param (joint[1],dParamVel2, maxvee);
dJointSetHinge2Param (joint[1],dParamFMax2, v2);
dJointSetHinge2Param (joint[2],dParamVel2, maxvee);
dJointSetHinge2Param (joint[2],dParamFMax2, tv1);
#endif
#ifdef ETUVETO
dJointSetHinge2Param (joint[0],dParamVel2, maxvee);
dJointSetHinge2Param (joint[0],dParamFMax2, v2);
dJointSetHinge2Param (joint[3],dParamVel2, maxvee);
dJointSetHinge2Param (joint[3],dParamFMax2, tv1);
#endif
}
// disable braking in front wheels
#ifndef ETUVETO
dJointSetHinge2Param (joint[3],dParamVel2, .0);
dJointSetHinge2Param (joint[3],dParamFMax2, .0);
dJointSetHinge2Param (joint[0],dParamVel2, .0);
dJointSetHinge2Param (joint[0],dParamFMax2, .0);
#endif
} else { //jarrutys
kaasu /= 6.0;
if (kaanto>0) {
dJointSetHinge2Param (joint[3],dParamVel,kaanto);
dJointSetHinge2Param (joint[3],dParamFMax,.5);
dJointSetHinge2Param (joint[3],dParamLoStop,kaanto);
dJointSetHinge2Param (joint[3],dParamHiStop,kaanto);
dJointSetHinge2Param (joint[3],dParamFudgeFactor,0.1);
r1 = LENGTH/tan((double)kaanto);
beeta = atan(LENGTH/(r1+WIDTH));
dJointSetHinge2Param (joint[0],dParamVel,beeta);
dJointSetHinge2Param (joint[0],dParamFMax,.5);
dJointSetHinge2Param (joint[0],dParamLoStop,beeta);
dJointSetHinge2Param (joint[0],dParamHiStop,beeta);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
dJointSetHinge2Param (joint[2],dParamVel2, .0);
dJointSetHinge2Param (joint[2],dParamFMax2, kaasu);
dJointSetHinge2Param (joint[1],dParamVel2, .0);
dJointSetHinge2Param (joint[1],dParamFMax2, kaasu);
dJointSetHinge2Param (joint[3],dParamVel2, .0);
dJointSetHinge2Param (joint[3],dParamFMax2, kaasu);
dJointSetHinge2Param (joint[0],dParamVel2, .0);
dJointSetHinge2Param (joint[0],dParamFMax2, kaasu);
} else {
dJointSetHinge2Param (joint[0],dParamVel,kaanto);
dJointSetHinge2Param (joint[0],dParamFMax,.5);
dJointSetHinge2Param (joint[0],dParamLoStop,kaanto);
dJointSetHinge2Param (joint[0],dParamHiStop,kaanto);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
r1 = LENGTH/tan((double)kaanto);
beeta = -atan(LENGTH/(-r1+WIDTH));
dJointSetHinge2Param (joint[3],dParamVel,beeta);
dJointSetHinge2Param (joint[3],dParamFMax,.5);
dJointSetHinge2Param (joint[3],dParamLoStop,beeta);
dJointSetHinge2Param (joint[3],dParamHiStop,beeta);
dJointSetHinge2Param (joint[3],dParamFudgeFactor,0.1);
dJointSetHinge2Param (joint[2],dParamVel2, .0);
dJointSetHinge2Param (joint[2],dParamFMax2, kaasu);
dJointSetHinge2Param (joint[1],dParamVel2, .0);
dJointSetHinge2Param (joint[1],dParamFMax2, kaasu);
dJointSetHinge2Param (joint[3],dParamVel2, .0);
dJointSetHinge2Param (joint[3],dParamFMax2, kaasu);
dJointSetHinge2Param (joint[0],dParamVel2, .0);
dJointSetHinge2Param (joint[0],dParamFMax2, kaasu);
}
}
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
// }
dsSetColor (0,1,1);
dsSetTexture (DS_WOOD);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
dsSetColor (1,1,1);
for (i=1; i<=4; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),0.04f,RADIUS);
dVector3 ss;
dGeomBoxGetLengths (ground_box,ss);
dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
/*
printf ("%.10f %.10f %.10f %.10f\n",
dJointGetHingeAngle (joint[1]),
dJointGetHingeAngle (joint[2]),
dJointGetHingeAngleRate (joint[1]),
dJointGetHingeAngleRate (joint[2]));
*/
}
int main (int argc, char **argv)
{
int i;
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/textures";
// create world
world = dWorldCreate();
space = dHashSpaceCreate();
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
ground = dCreatePlane (space,0,0,1,0);
// chassis body
body[0] = dBodyCreate (world);
dBodySetPosition (body[0],0,0,STARTZ-RADIUS+SUSPHEIGHT);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[0],&m);
box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[0],body[0]);
// wheel bodies
for (i=1; i<=4; i++) {
body[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[i],&m);
sphere[i-1] = dCreateSphere (0,RADIUS);
dGeomSetBody (sphere[i-1],body[i]);
}
dBodySetPosition (body[1],0.5*LENGTH, WIDTH*.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[4],0.5*LENGTH,-WIDTH*.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front wheel hinge
/*
joint[0] = dJointCreateHinge2 (world,0);
dJointAttach (joint[0],body[0],body[1]);
const dReal *a = dBodyGetPosition (body[1]);
dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[0],0,0,1);
dJointSetHinge2Axis2 (joint[0],0,1,0);
*/
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[i],body[0],body[i+1]);
const dReal *a = dBodyGetPosition (body[i+1]);
dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[i],0,0,1);
dJointSetHinge2Axis2 (joint[i],0,1,0);
}
// set joint suspension
for (i=0; i<4; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,SUSPERP); // .4
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,SUSPCFM); // .8
}
// lock back wheels along the steering axis
for (i=1; i<3; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i],dParamLoStop,0);
dJointSetHinge2Param (joint[i],dParamHiStop,0);
// the following alternative method is no good as the wheels may get out
// of alignment:
// dJointSetHinge2Param (joint[i],dParamVel,0);
// dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}
// create geometry group and add it to the space
geom_group = dCreateGeomGroup (space);
dGeomGroupAdd (geom_group,box[0]);
dGeomGroupAdd (geom_group,sphere[0]);
dGeomGroupAdd (geom_group,sphere[1]);
dGeomGroupAdd (geom_group,sphere[2]);
dGeomGroupAdd (geom_group,sphere[3]);
// environment
ground_box = dCreateBox (space,6,2,.8);
dMatrix3 R;
dRFromAxisAndAngle (R,0,3,0,-0.45);
dGeomSetPosition (ground_box,2,0,-.3);
dGeomSetRotation (ground_box,R);
// run simulation
dsSimulationLoop (argc,argv,560,440,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dGeomDestroy (box[0]);
dGeomDestroy (sphere[0]);
dGeomDestroy (sphere[1]);
dGeomDestroy (sphere[2]);
dGeomDestroy (sphere[3]);
SDL_Quit();
return 0;
}