first commit
This commit is contained in:
258
thirdparty/ode-0.16.5/ode/demo/demo_gyroscopic.cpp
vendored
Normal file
258
thirdparty/ode-0.16.5/ode/demo/demo_gyroscopic.cpp
vendored
Normal file
@@ -0,0 +1,258 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* 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 _MSC_VER
|
||||
#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 dsDrawCapsule dsDrawCapsuleD
|
||||
#define dsDrawConvex dsDrawConvexD
|
||||
#endif
|
||||
|
||||
bool write_world = false;
|
||||
bool show_contacts = false;
|
||||
dWorld * world;
|
||||
dBody *top1, *top2;
|
||||
dSpace *space;
|
||||
dJointGroup contactgroup;
|
||||
|
||||
const dReal pinradius = 0.05f;
|
||||
const dReal pinlength = 1.5f;
|
||||
const dReal topradius = 1.0f;
|
||||
const dReal toplength = 0.25f;
|
||||
const dReal topmass = 1.0f;
|
||||
|
||||
#define MAX_CONTACTS 4
|
||||
|
||||
static void nearCallback (void *, dGeomID o1, dGeomID o2)
|
||||
{
|
||||
// for drawing the contact points
|
||||
dMatrix3 RI;
|
||||
dRSetIdentity (RI);
|
||||
const dReal ss[3] = {0.02,0.02,0.02};
|
||||
|
||||
int i;
|
||||
dBodyID b1 = dGeomGetBody(o1);
|
||||
dBodyID b2 = dGeomGetBody(o2);
|
||||
|
||||
dContact contact[MAX_CONTACTS];
|
||||
int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
|
||||
sizeof(dContact));
|
||||
|
||||
for (i=0; i<numc; i++) {
|
||||
contact[i].surface.mode = dContactApprox1;
|
||||
contact[i].surface.mu = 2;
|
||||
|
||||
dJointID c = dJointCreateContact (*world,contactgroup,contact+i);
|
||||
dJointAttach (c,b1,b2);
|
||||
if (show_contacts)
|
||||
dsDrawBox (contact[i].geom.pos, RI, ss);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// start simulation - set viewpoint
|
||||
|
||||
static void start()
|
||||
{
|
||||
static float xyz[3] = {4.777f, -2.084f, 2.18f};
|
||||
static float hpr[3] = {153.0f, -14.5f, 0.0f};
|
||||
dsSetViewpoint (xyz,hpr);
|
||||
printf ("Orange top approximates conservation of angular momentum\n");
|
||||
printf ("Green top uses conservation of angular velocity\n");
|
||||
printf ("---\n");
|
||||
printf ("SPACE to reset\n");
|
||||
printf ("A to tilt the tops.\n");
|
||||
printf ("T to toggle showing the contact points.\n");
|
||||
printf ("1 to save the current state to 'state.dif'.\n");
|
||||
}
|
||||
|
||||
|
||||
char locase (char c)
|
||||
{
|
||||
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
|
||||
else return c;
|
||||
}
|
||||
|
||||
|
||||
// called when a key pressed
|
||||
static void reset();
|
||||
static void tilt();
|
||||
|
||||
static void command (int cmd)
|
||||
{
|
||||
cmd = locase (cmd);
|
||||
if (cmd == ' ')
|
||||
{
|
||||
reset();
|
||||
}
|
||||
else if (cmd == 'a') {
|
||||
tilt();
|
||||
}
|
||||
else if (cmd == 't') {
|
||||
show_contacts = !show_contacts;
|
||||
}
|
||||
else if (cmd == '1') {
|
||||
write_world = true;
|
||||
}
|
||||
}
|
||||
|
||||
// simulation loop
|
||||
|
||||
static void simLoop (int pause)
|
||||
{
|
||||
dsSetColor (0,0,2);
|
||||
space->collide(0,&nearCallback);
|
||||
if (!pause)
|
||||
//world->quickStep(0.02);
|
||||
world->step(0.02);
|
||||
|
||||
if (write_world) {
|
||||
FILE *f = fopen ("state.dif","wt");
|
||||
if (f) {
|
||||
dWorldExportDIF (*world,f,"X");
|
||||
fclose (f);
|
||||
}
|
||||
write_world = false;
|
||||
}
|
||||
|
||||
// remove all contact joints
|
||||
dJointGroupEmpty (contactgroup);
|
||||
|
||||
dsSetTexture (DS_WOOD);
|
||||
|
||||
dsSetColor (1,0.5f,0);
|
||||
dsDrawCylinder(top1->getPosition(),
|
||||
top1->getRotation(),
|
||||
toplength, topradius);
|
||||
dsDrawCapsule(top1->getPosition(),
|
||||
top1->getRotation(),
|
||||
pinlength, pinradius);
|
||||
|
||||
dsSetColor (0.5f,1,0);
|
||||
dsDrawCylinder(top2->getPosition(),
|
||||
top2->getRotation(),
|
||||
toplength, topradius);
|
||||
dsDrawCapsule(top2->getPosition(),
|
||||
top2->getRotation(),
|
||||
pinlength, pinradius);
|
||||
|
||||
}
|
||||
|
||||
|
||||
static void reset()
|
||||
{
|
||||
dMatrix3 R;
|
||||
dRSetIdentity(R);
|
||||
|
||||
top1->setRotation(R);
|
||||
top2->setRotation(R);
|
||||
|
||||
top1->setPosition(0.8f, -2, 2);
|
||||
top2->setPosition(0.8f, 2, 2);
|
||||
|
||||
top1->setAngularVel(1,0,7);
|
||||
top2->setAngularVel(1,0,7);
|
||||
|
||||
top1->setLinearVel(0,0.2f,0);
|
||||
top2->setLinearVel(0,0.2f,0);
|
||||
}
|
||||
|
||||
static void tilt()
|
||||
{
|
||||
top1->addTorque(0, 10, 0);
|
||||
top2->addTorque(0, 10, 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;
|
||||
|
||||
|
||||
// create world
|
||||
dInitODE();
|
||||
world = new dWorld();
|
||||
world->setGravity(0,0,-0.5f);
|
||||
world->setCFM(1e-5f);
|
||||
world->setLinearDamping(0.00001f);
|
||||
world->setAngularDamping(0.0001f);
|
||||
|
||||
space = new dSimpleSpace(0);
|
||||
|
||||
dPlane *floor = new dPlane(*space, 0,0,1,0);
|
||||
|
||||
top1 = new dBody(*world);
|
||||
top2 = new dBody(*world);
|
||||
|
||||
dMass m;
|
||||
m.setCylinderTotal(1, 3, topradius, toplength);
|
||||
top1->setMass(m);
|
||||
top2->setMass(m);
|
||||
|
||||
dGeom *g1, *g2, *pin1, *pin2;
|
||||
g1 = new dCylinder(*space, topradius, toplength);
|
||||
g1->setBody(*top1);
|
||||
g2 = new dCylinder(*space, topradius, toplength);
|
||||
g2->setBody(*top2);
|
||||
|
||||
pin1 = new dCapsule(*space, pinradius, pinlength);
|
||||
pin1->setBody(*top1);
|
||||
pin2 = new dCapsule(*space, pinradius, pinlength);
|
||||
pin2->setBody(*top2);
|
||||
|
||||
top2->setGyroscopicMode(false);
|
||||
|
||||
reset();
|
||||
|
||||
// run simulation
|
||||
dsSimulationLoop (argc,argv,512,384,&fn);
|
||||
|
||||
delete g1;
|
||||
delete g2;
|
||||
delete pin1;
|
||||
delete pin2;
|
||||
delete floor;
|
||||
contactgroup.empty();
|
||||
delete top1;
|
||||
delete top2;
|
||||
delete space;
|
||||
delete world;
|
||||
dCloseODE();
|
||||
}
|
||||
Reference in New Issue
Block a user