From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/demo/demo_transmission.cpp | 414 +++++++++++++++++++++++++ 1 file changed, 414 insertions(+) create mode 100644 libs/ode-0.16.1/ode/demo/demo_transmission.cpp (limited to 'libs/ode-0.16.1/ode/demo/demo_transmission.cpp') diff --git a/libs/ode-0.16.1/ode/demo/demo_transmission.cpp b/libs/ode-0.16.1/ode/demo/demo_transmission.cpp new file mode 100644 index 0000000..50cb820 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_transmission.cpp @@ -0,0 +1,414 @@ +/************************************************************************* + * * + * 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 +#include +#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 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; +} -- cgit v1.2.1