Fixed a bug affecting dWorldStep and disabled joints; now the step code knows about...
[ode:mainlinemirror.git] / ode / demo / demo_kinematic.cpp
1 #include <iostream>
2 #include <set>
3 #include <algorithm>
4 #include <functional>
5 #include <ode/ode.h>
6 #include <drawstuff/drawstuff.h>
7 #include "texturepath.h"
8
9 #ifdef dDOUBLE
10 #define dsDrawBox dsDrawBoxD
11 #define dsDrawCylinder dsDrawCylinderD
12 #endif
13
14
15 using namespace std;
16
17 dWorld *world;
18 dSpace *space;
19 dPlane *ground;
20 dBody *kbody;
21 dBox *kbox;
22 dJointGroup joints;
23 dCylinder *kpole;
24 dBody *matraca;
25 dBox *matraca_geom;
26 dHingeJoint *hinge;
27
28 struct Box {
29     dBody body;
30     dBox geom;
31     Box() :
32         body(*world),
33         geom(*space, 0.2, 0.2, 0.2)
34     {
35         dMass mass;
36         mass.setBox(10, 0.2, 0.2, 0.2);
37         body.setMass(mass);
38         geom.setData(this);
39         geom.setBody(body);
40     }
41     void draw() const
42     {
43         dVector3 lengths;
44         geom.getLengths(lengths);
45         dsSetTexture(DS_WOOD);
46         dsSetColor(0,1,0);
47         dsDrawBox(geom.getPosition(), geom.getRotation(), lengths);
48     }
49 };
50
51 set<Box*> boxes;
52 set<Box*> to_remove;
53
54 void dropBox()
55 {
56     Box *box = new Box();
57     
58     dReal px = (rand() / float(RAND_MAX)) * 2 - 1;
59     dReal py = (rand() / float(RAND_MAX)) * 2 - 1;
60     dReal pz = 2.5;
61     box->body.setPosition(px, py, pz);
62     
63     boxes.insert(box);
64 }
65
66 void queueRemoval(dGeomID g)
67 {
68     Box *b = (Box*)dGeomGetData(g);
69     to_remove.insert(b);
70 }
71
72 void removeQueued()
73 {
74     while (!to_remove.empty()) {
75         Box *b = *to_remove.begin();
76         to_remove.erase(b);
77         boxes.erase(b);
78         delete b;
79     }
80 }
81
82
83 void nearCallback(void *data, dGeomID g1, dGeomID g2)
84 {
85     if (g1 == ground->id()) {
86         queueRemoval(g2);
87         return;
88     }
89     if (g2 == ground->id()) {
90         queueRemoval(g1);
91         return;
92     }
93
94     dBodyID b1 = dGeomGetBody(g1);
95     dBodyID b2 = dGeomGetBody(g2);
96     
97     if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
98         return;
99
100     const int MAX_CONTACTS = 10;
101     dContact contact[MAX_CONTACTS];
102     int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
103     for (int i=0; i<n; ++i) {
104         contact[i].surface.mode = 0;
105         contact[i].surface.mu = 1;
106         dJointID j = dJointCreateContact (*world, joints.id(), contact+i);
107         dJointAttach(j, b1, b2);
108     }
109 }
110
111
112 void
113 simLoop(int pause)
114 {
115     if (!pause) {
116         const dReal timestep = 0.02;
117
118         // this does a hard-coded circular motion animation
119         static float t=0;
120         t += timestep/4;
121         if (t > 2*M_PI)
122             t = 0;
123         dReal px = cos(t);
124         dReal py = sin(t);
125         dReal vx = -sin(t)/4;
126         dReal vy = cos(t)/4;
127         kbody->setPosition(px, py, .5);
128         kbody->setLinearVel(vx, vy, 0);
129         // end of hard-coded animation
130         
131         space->collide(0, nearCallback);
132         removeQueued();
133         
134         world->quickStep(timestep);
135         joints.clear();
136     }
137
138     dVector3 lengths;
139
140     // the moving platform
141     kbox->getLengths(lengths);
142     dsSetTexture(DS_WOOD);
143     dsSetColor(.3, .3, 1);
144     dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths);
145     dReal length, radius;
146     kpole->getParams(&radius, &length);
147     dsSetTexture(DS_CHECKERED);
148     dsSetColor(1, 1, 0);
149     dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius);
150     
151     // the matraca
152     matraca_geom->getLengths(lengths);
153     dsSetColor(1,0,0);
154     dsSetTexture(DS_WOOD);
155     dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths);
156
157     // and the boxes
158     for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw));
159 }
160
161 void command(int c)
162 {
163     switch (c) {
164         case ' ':
165             dropBox();
166             break;
167     }
168 }
169
170 int main(int argc, char **argv)
171 {
172     dInitODE();
173
174     // setup pointers to drawstuff callback functions
175     dsFunctions fn;
176     fn.version = DS_VERSION;
177     fn.start = 0;
178     fn.step = &simLoop;
179     fn.command = &command;
180     fn.stop = 0;
181     fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
182     
183     cout << endl << "*** Press SPACE to drop boxes **" << endl;
184     
185     space = new dSimpleSpace();
186     ground = new dPlane(*space, 0, 0, 1, 0);
187     
188     world = new dWorld;
189     world->setGravity(0, 0, -.5);
190     
191     kbody = new dBody(*world);
192     kbody->setKinematic();
193     const dReal kx = 1, ky = 0, kz = .5;
194     kbody->setPosition(kx, ky, kz);
195     kbox = new dBox(*space, 3, 3, .5);
196     kbox->setBody(*kbody);
197     kpole = new dCylinder(*space, .125, 1.5);
198     kpole->setBody(*kbody);
199     dGeomSetOffsetPosition(kpole->id(), 0, 0, 0.8);
200     
201     matraca = new dBody(*world);
202     matraca->setPosition(kx+0, ky+1, kz+1);
203     matraca_geom = new dBox(*space, 0.5, 2, 0.75);
204     matraca_geom->setBody(*matraca);
205     dMass mass;
206     mass.setBox(1, 0.5, 2, 0.75);
207     matraca->setMass(mass);
208     
209     hinge = new dHingeJoint(*world);
210     hinge->attach(*kbody, *matraca);
211     hinge->setAnchor(kx, ky, kz+1);
212     hinge->setAxis(0, 0, 1);
213     
214     dsSimulationLoop (argc, argv, 640, 480, &fn);
215     
216     dCloseODE();
217 }