summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/demo/demo_transmission.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_transmission.cpp')
-rw-r--r--libs/ode-0.16.1/ode/demo/demo_transmission.cpp414
1 files changed, 414 insertions, 0 deletions
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 <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;
+}
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 02:30:04 +0000