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_jointPU.cpp | 735 ++++++++++++++++++++++++++++++ 1 file changed, 735 insertions(+) create mode 100644 libs/ode-0.16.1/ode/demo/demo_jointPU.cpp (limited to 'libs/ode-0.16.1/ode/demo/demo_jointPU.cpp') diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp new file mode 100644 index 0000000..6ec3093 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp @@ -0,0 +1,735 @@ +/************************************************************************* + * * + * 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 +#include +#include +#include +#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; igetParam (dParamVel3) - VEL_INC; + joint->setParam (dParamVel3, vel); + joint->setParam (dParamFMax3, 2); + std::cout<<"Velocity = "<' : { + dReal vel = joint->getParam (dParamVel3) + VEL_INC; + joint->setParam (dParamVel3, vel); + joint->setParam (dParamFMax3, 2); + std::cout<<"Velocity = "<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 (joint); + pr->setParam (dParamLoStop, -lLimit); + pr->setParam (dParamHiStop, -lLimit); + pr->setParam (dParamLoStop2, aLimit); + pr->setParam (dParamHiStop2, -aLimit); + } + break; + case dJointTypePU : { + dPUJoint *pu = reinterpret_cast (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); + + } + +case 'p' :case 'P' : { + switch (joint->getType() ) { + case dJointTypeSlider : { + dSliderJoint *sj = reinterpret_cast (joint); + std::cout<<"Position ="<getPosition() <<"\n"; + } + break; + case dJointTypePU : { + dPUJoint *pu = reinterpret_cast (joint); + std::cout<<"Position ="<getPosition() <<"\n"; + std::cout<<"Position Rate="<getPositionRate() <<"\n"; + std::cout<<"Angle1 ="<getAngle1() <<"\n"; + std::cout<<"Angle1 Rate="<getAngle1Rate() <<"\n"; + std::cout<<"Angle2 ="<getAngle2() <<"\n"; + std::cout<<"Angle2 Rate="<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 (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 (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 (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; +} + -- cgit v1.2.1