415 lines
11 KiB
C++
415 lines
11 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. *
|
|
* *
|
|
*************************************************************************/
|
|
|
|
#include <ode/ode.h>
|
|
#include <drawstuff/drawstuff.h>
|
|
#include "texturepath.h"
|
|
|
|
#ifdef dDOUBLE
|
|
#define dsDrawCylinder dsDrawCylinderD
|
|
#define dsDrawLine dsDrawLineD
|
|
#define dsDrawSphere dsDrawSphereD
|
|
#endif
|
|
|
|
dReal theta = M_PI / 4;
|
|
dReal ratio = 1, speed = 5, rho_1 = 1, rho_2 = 1, backlash = 0.1;
|
|
int mode = 0;
|
|
|
|
dWorldID world;
|
|
dSpaceID space;
|
|
dBodyID body1, body2;
|
|
dGeomID geom1, geom2;
|
|
dJointID hinge1, hinge2, transmission;
|
|
dJointFeedback feedback;
|
|
|
|
void setup() {
|
|
dMatrix3 R;
|
|
|
|
switch (mode) {
|
|
case 0:
|
|
// Parallel axes.
|
|
|
|
dBodySetPosition(body1, 1, 0, 1);
|
|
dBodySetPosition(body2, -1, 0, 1);
|
|
|
|
dRSetIdentity (R);
|
|
dBodySetRotation (body1, R);
|
|
dBodySetRotation (body2, R);
|
|
|
|
dJointSetHingeAnchor(hinge2, -1, 0, 1);
|
|
dJointSetHingeAxis(hinge2, 0, 0, 1);
|
|
|
|
dJointSetHingeAnchor(hinge1, 1, 0, 1);
|
|
dJointSetHingeAxis(hinge1, 0, 0, 1);
|
|
|
|
dJointSetTransmissionMode(transmission, dTransmissionParallelAxes);
|
|
dJointSetTransmissionRatio(transmission, ratio);
|
|
dJointSetTransmissionAnchor1(transmission, 1, 0, 1);
|
|
dJointSetTransmissionAnchor2(transmission, -1, 0, 1);
|
|
dJointSetTransmissionAxis(transmission, 0, 0, 1);
|
|
|
|
break;
|
|
case 1:
|
|
// Intersecting axes.
|
|
|
|
dBodySetPosition(body1, 1, 0, 1);
|
|
dBodySetPosition(body2, -1, 0, 2);
|
|
|
|
dRSetIdentity (R);
|
|
dBodySetRotation (body1, R);
|
|
|
|
dRFromZAxis (R, cos(theta), 0, sin(theta));
|
|
dBodySetRotation (body2, R);
|
|
|
|
dJointSetHingeAnchor(hinge2, -1, 0, 2);
|
|
dJointSetHingeAxis(hinge2, cos(theta), 0, sin(theta));
|
|
|
|
dJointSetHingeAnchor(hinge1, 1, 0, 1);
|
|
dJointSetHingeAxis(hinge1, 0, 0, 1);
|
|
|
|
dJointSetTransmissionMode(transmission, dTransmissionIntersectingAxes);
|
|
dJointSetTransmissionAnchor1(transmission, 1, 0, 1);
|
|
dJointSetTransmissionAnchor2(transmission, -1, 0, 2);
|
|
dJointSetTransmissionAxis1(transmission, 0, 0, -1);
|
|
dJointSetTransmissionAxis2(transmission, cos(theta), 0, sin(theta));
|
|
|
|
break;
|
|
case 2:
|
|
// Chain.
|
|
|
|
dBodySetPosition(body1, 2, 0, 1);
|
|
dBodySetPosition(body2, -2, 0, 1);
|
|
|
|
dRSetIdentity (R);
|
|
dBodySetRotation (body1, R);
|
|
dBodySetRotation (body2, R);
|
|
|
|
dJointSetHingeAnchor(hinge2, -2, 0, 1);
|
|
dJointSetHingeAxis(hinge2, 0, 0, 1);
|
|
|
|
dJointSetHingeAnchor(hinge1, 2, 0, 1);
|
|
dJointSetHingeAxis(hinge1, 0, 0, 1);
|
|
|
|
dJointSetTransmissionMode(transmission, dTransmissionChainDrive);
|
|
dJointSetTransmissionAnchor1(transmission, 2, 0, 1);
|
|
dJointSetTransmissionAnchor2(transmission, -2, 0, 1);
|
|
dJointSetTransmissionRadius1(transmission, rho_1);
|
|
dJointSetTransmissionRadius2(transmission, rho_2);
|
|
dJointSetTransmissionAxis(transmission, 0, 0, 1);
|
|
|
|
break;
|
|
}
|
|
|
|
dJointSetTransmissionBacklash(transmission, backlash);
|
|
|
|
dJointSetHingeParam(hinge2, dParamVel, speed);
|
|
dJointSetHingeParam(hinge2, dParamFMax, 50);
|
|
|
|
dJointSetHingeParam(hinge1, dParamVel, 0);
|
|
dJointSetHingeParam(hinge1, dParamFMax, 2);
|
|
|
|
dBodySetLinearVel(body1, 0, 0, 0);
|
|
dBodySetLinearVel(body2, 0, 0, 0);
|
|
dBodySetAngularVel(body1, 0, 0, 0);
|
|
dBodySetAngularVel(body2, 0, 0, 0);
|
|
}
|
|
|
|
void start()
|
|
{
|
|
dMass mass;
|
|
|
|
world = dWorldCreate();
|
|
dWorldSetGravity (world,0,0,-9.8);
|
|
|
|
dWorldSetERP(world, 0.2);
|
|
|
|
space = dSimpleSpaceCreate (0);
|
|
|
|
body1 = dBodyCreate(world);
|
|
body2 = dBodyCreate(world);
|
|
|
|
dBodySetFiniteRotationMode(body1, 1);
|
|
dBodySetFiniteRotationMode(body2, 1);
|
|
|
|
geom1 = dCreateCylinder(space, 0.2, 0.5);
|
|
dGeomSetBody(geom1, body1);
|
|
dMassSetCylinder(&mass, 100, 3, 0.2, 0.5);
|
|
dBodySetMass(body1, &mass);
|
|
|
|
geom2 = dCreateCylinder(space, 0.2, 0.5);
|
|
dGeomSetBody(geom2, body2);
|
|
dMassSetCylinder(&mass, 100, 3, 0.2, 0.5);
|
|
dBodySetMass(body2, &mass);
|
|
|
|
hinge1 = dJointCreateHinge(world, 0);
|
|
dJointAttach(hinge1, body1, 0);
|
|
|
|
hinge2 = dJointCreateHinge(world, 0);
|
|
dJointAttach(hinge2, body2, 0);
|
|
|
|
transmission = dJointCreateTransmission(world, 0);
|
|
dJointAttach(transmission, body1, body2);
|
|
dJointSetFeedback(transmission, &feedback);
|
|
|
|
setup();
|
|
|
|
// initial camera position
|
|
static float xyz[3] = {1.15,-2.78,4.1};
|
|
static float hpr[3] = {105,-45.5,0};
|
|
dsSetViewpoint (xyz,hpr);
|
|
|
|
fprintf (stderr,
|
|
"The green wheel is driving the red one. To control it use the following:\n"
|
|
" '[' : decrease wheel ratio\n"
|
|
" ']' : increase wheel ratio\n"
|
|
" ',' : decrease driving wheel speed\n"
|
|
" '.' : increase driving wheel speed\n"
|
|
" '-' : decrease backlash\n"
|
|
" '=' : increase backlash\n"
|
|
" '1' : switch to parallel axes gears mode\n"
|
|
" '2' : switch to intersecting axes gears mode\n"
|
|
" '3' : switch to chain (or belt) mode\n"
|
|
);
|
|
}
|
|
|
|
void stop()
|
|
{
|
|
dSpaceDestroy(space);
|
|
|
|
dWorldDestroy(world);
|
|
}
|
|
|
|
void drawGeom(dGeomID g)
|
|
{
|
|
int gclass = dGeomGetClass(g);
|
|
const dReal *pos = dGeomGetPosition(g);
|
|
const dReal *rot = dGeomGetRotation(g);
|
|
|
|
switch (gclass) {
|
|
case dCylinderClass:
|
|
{
|
|
dReal length, radius;
|
|
|
|
if (g == geom1) {
|
|
dsSetColorAlpha(1, 0, 0, 1);
|
|
} else {
|
|
dsSetColorAlpha(0, 1, 0, 1);
|
|
}
|
|
|
|
dsSetTexture (DS_WOOD);
|
|
dGeomCylinderGetParams(g, &radius, &length);
|
|
dsDrawCylinder(pos, rot, length, radius);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
abort();
|
|
}
|
|
}
|
|
}
|
|
|
|
void simLoop(int pause)
|
|
{
|
|
if (!pause) {
|
|
|
|
const dReal step = 0.003;
|
|
const unsigned nsteps = 4;
|
|
|
|
for (unsigned i=0; i<nsteps; ++i) {
|
|
dWorldQuickStep(world, step);
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
{
|
|
const dReal *omega_1, *omega_2;
|
|
|
|
omega_1 = dBodyGetAngularVel(body1);
|
|
omega_2 = dBodyGetAngularVel(body2);
|
|
|
|
printf ("T1: %f, %f, %f\n",
|
|
feedback.t1[0], feedback.t1[1], feedback.t1[2]);
|
|
|
|
printf ("T2: %f, %f, %f\n",
|
|
feedback.t2[0], feedback.t2[1], feedback.t2[2]);
|
|
|
|
printf ("F1: %f, %f, %f\n",
|
|
feedback.f1[0], feedback.f1[1], feedback.f1[2]);
|
|
|
|
printf ("F2: %f, %f, %f\n",
|
|
feedback.f2[0], feedback.f2[1], feedback.f2[2]);
|
|
}
|
|
#endif
|
|
|
|
// now we draw everything
|
|
unsigned ngeoms = dSpaceGetNumGeoms(space);
|
|
for (unsigned i=0; i<ngeoms; ++i) {
|
|
dGeomID g = dSpaceGetGeom(space, i);
|
|
|
|
drawGeom(g);
|
|
}
|
|
|
|
const dReal *R_1 = dGeomGetRotation(geom1);
|
|
const dReal *R_2 = dGeomGetRotation(geom2);
|
|
dVector3 c_1, c_2, a_1, a_2;
|
|
|
|
dJointGetTransmissionContactPoint1(transmission, c_1);
|
|
dJointGetTransmissionContactPoint2(transmission, c_2);
|
|
dJointGetTransmissionAnchor1(transmission, a_1);
|
|
dJointGetTransmissionAnchor2(transmission, a_2);
|
|
|
|
dsSetColorAlpha(1, 0, 0, 0.5);
|
|
dsDrawCylinder(a_1, R_1, 0.05, dCalcPointsDistance3(c_1, a_1));
|
|
dsSetColorAlpha(0, 1, 0, 0.5);
|
|
dsDrawCylinder(a_2, R_2, 0.05, dCalcPointsDistance3(c_2, a_2));
|
|
|
|
dsSetColorAlpha(1, 0, 0, 0.5);
|
|
dsDrawSphere (c_1, R_1, 0.05);
|
|
dsDrawSphere (c_2, R_1, 0.05);
|
|
|
|
dsSetColorAlpha(1, 1, 0, 0.5);
|
|
if (mode == dTransmissionChainDrive) {
|
|
dsDrawLine(c_1, c_2);
|
|
}
|
|
}
|
|
|
|
static void command (int cmd)
|
|
{
|
|
if (cmd == '[') {
|
|
switch(mode) {
|
|
case dTransmissionParallelAxes:
|
|
if (ratio > 0.125) {
|
|
ratio *= 0.5;
|
|
|
|
fprintf (stderr, "Gear ratio set to %.3f.\n", ratio);
|
|
}
|
|
break;
|
|
case dTransmissionIntersectingAxes:
|
|
if (theta > 0.1) {
|
|
theta -= 0.1;
|
|
|
|
fprintf (stderr, "Gear angle set to %.3f deg.\n",
|
|
theta / M_PI * 180);
|
|
}
|
|
break;
|
|
case dTransmissionChainDrive:
|
|
if (rho_2 > 0.125) {
|
|
rho_2 /= 2;
|
|
|
|
fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1);
|
|
}
|
|
break;
|
|
}
|
|
|
|
setup();
|
|
} else if (cmd == ']') {
|
|
switch(mode) {
|
|
case dTransmissionParallelAxes:
|
|
if (ratio < 8) {
|
|
ratio *= 2;
|
|
|
|
fprintf (stderr, "Gear ratio set to %.3f.\n", ratio);
|
|
}
|
|
break;
|
|
case dTransmissionIntersectingAxes:
|
|
if (theta < 0.9) {
|
|
theta += 0.1;
|
|
|
|
fprintf (stderr, "Gear angle set to %.3f deg.\n",
|
|
theta / M_PI * 180);
|
|
}
|
|
break;
|
|
case dTransmissionChainDrive:
|
|
if (rho_2 < 2) {
|
|
rho_2 *= 2;
|
|
|
|
fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1);
|
|
}
|
|
break;
|
|
}
|
|
|
|
setup();
|
|
} else if (cmd == '.') {
|
|
speed += 5;
|
|
|
|
fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed);
|
|
|
|
dJointSetHingeParam(hinge2, dParamVel, speed);
|
|
} else if (cmd == ',') {
|
|
speed -= 5;
|
|
|
|
fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed);
|
|
|
|
dJointSetHingeParam(hinge2, dParamVel, speed);
|
|
} else if (cmd == '/') {
|
|
if (dJointGetHingeParam(hinge2, dParamFMax) > 0) {
|
|
dJointSetHingeParam(hinge2, dParamFMax, 0);
|
|
} else {
|
|
dJointSetHingeParam(hinge2, dParamFMax, 50);
|
|
}
|
|
|
|
} else if (cmd == '-') {
|
|
backlash -= 0.1;
|
|
|
|
fprintf (stderr, "Backlash set to %g m.\n", backlash);
|
|
|
|
dJointSetTransmissionBacklash(transmission, backlash);
|
|
} else if (cmd == '=') {
|
|
backlash += 0.1;
|
|
|
|
fprintf (stderr, "Backlash set to %g m.\n", backlash);
|
|
|
|
dJointSetTransmissionBacklash(transmission, backlash);
|
|
} else if (cmd == '1') {
|
|
mode = dTransmissionParallelAxes;
|
|
setup();
|
|
} else if (cmd == '2') {
|
|
mode = dTransmissionIntersectingAxes;
|
|
setup();
|
|
} else if (cmd == '3') {
|
|
mode = dTransmissionChainDrive;
|
|
setup();
|
|
}
|
|
}
|
|
|
|
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 = stop;
|
|
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
|
|
|
|
// create world
|
|
dInitODE();
|
|
|
|
// run demo
|
|
dsSimulationLoop (argc, argv, 800, 600, &fn);
|
|
|
|
dCloseODE();
|
|
return 0;
|
|
}
|