1 | // Test my Plane2D constraint. |
---|
2 | // Uses ode-0.35 collision API. |
---|
3 | |
---|
4 | # include <stdio.h> |
---|
5 | # include <stdlib.h> |
---|
6 | # include <math.h> |
---|
7 | # include <ode/ode.h> |
---|
8 | # include <drawstuff/drawstuff.h> |
---|
9 | |
---|
10 | # define drand48() ((double) (((double) rand()) / ((double) RAND_MAX))) |
---|
11 | |
---|
12 | # define N_BODIES 40 |
---|
13 | # define STAGE_SIZE 8.0 // in m |
---|
14 | |
---|
15 | # define TIME_STEP 0.01 |
---|
16 | # define K_SPRING 10.0 |
---|
17 | # define K_DAMP 10.0 |
---|
18 | |
---|
19 | |
---|
20 | static dWorld dyn_world; |
---|
21 | static dBody dyn_bodies[N_BODIES]; |
---|
22 | static dReal bodies_sides[N_BODIES][3]; |
---|
23 | |
---|
24 | static dSpaceID coll_space_id; |
---|
25 | static dJointID plane2d_joint_ids[N_BODIES]; |
---|
26 | static dJointGroup |
---|
27 | coll_contacts; |
---|
28 | |
---|
29 | |
---|
30 | |
---|
31 | static void cb_start () |
---|
32 | /*************************/ |
---|
33 | { |
---|
34 | static float xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.65f*STAGE_SIZE}; |
---|
35 | static float hpr[3] = { 90.0f, -90.0f, 0 }; |
---|
36 | |
---|
37 | dsSetViewpoint (xyz, hpr); |
---|
38 | } |
---|
39 | |
---|
40 | |
---|
41 | |
---|
42 | static void cb_near_collision (void *data, dGeomID o1, dGeomID o2) |
---|
43 | /********************************************************************/ |
---|
44 | { |
---|
45 | dBodyID b1 = dGeomGetBody (o1); |
---|
46 | dBodyID b2 = dGeomGetBody (o2); |
---|
47 | dContact contact; |
---|
48 | |
---|
49 | |
---|
50 | // exit without doing anything if the two bodies are static |
---|
51 | if (b1 == 0 && b2 == 0) |
---|
52 | return; |
---|
53 | |
---|
54 | // exit without doing anything if the two bodies are connected by a joint |
---|
55 | if (b1 && b2 && dAreConnected (b1, b2)) |
---|
56 | { |
---|
57 | /* MTRAP; */ |
---|
58 | return; |
---|
59 | } |
---|
60 | |
---|
61 | contact.surface.mode = 0; |
---|
62 | contact.surface.mu = 0; // frictionless |
---|
63 | |
---|
64 | if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom))) |
---|
65 | { |
---|
66 | dJointID c = dJointCreateContact (dyn_world.id(), |
---|
67 | coll_contacts.id (), &contact); |
---|
68 | dJointAttach (c, b1, b2); |
---|
69 | } |
---|
70 | } |
---|
71 | |
---|
72 | |
---|
73 | static void track_to_pos (dBody &body, dJointID joint_id, |
---|
74 | dReal target_x, dReal target_y) |
---|
75 | /************************************************************************/ |
---|
76 | { |
---|
77 | dReal curr_x = body.getPosition()[0]; |
---|
78 | dReal curr_y = body.getPosition()[1]; |
---|
79 | |
---|
80 | dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x)); |
---|
81 | dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y)); |
---|
82 | } |
---|
83 | |
---|
84 | |
---|
85 | |
---|
86 | static void cb_sim_step (int pause) |
---|
87 | /*************************************/ |
---|
88 | { |
---|
89 | if (! pause) |
---|
90 | { |
---|
91 | static dReal angle = 0; |
---|
92 | |
---|
93 | angle += REAL( 0.01 ); |
---|
94 | |
---|
95 | track_to_pos (dyn_bodies[0], plane2d_joint_ids[0], |
---|
96 | dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ), |
---|
97 | dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) )); |
---|
98 | |
---|
99 | /* double f0 = 0.001; */ |
---|
100 | /* for (int b = 0; b < N_BODIES; b ++) */ |
---|
101 | /* { */ |
---|
102 | /* double p = 1 + b / (double) N_BODIES; */ |
---|
103 | /* double q = 2 - b / (double) N_BODIES; */ |
---|
104 | /* dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */ |
---|
105 | /* } */ |
---|
106 | /* dyn_bodies[0].addTorque (0, 0, 0.1); */ |
---|
107 | |
---|
108 | const int n = 10; |
---|
109 | for (int i = 0; i < n; i ++) |
---|
110 | { |
---|
111 | dSpaceCollide (coll_space_id, 0, &cb_near_collision); |
---|
112 | dyn_world.step (dReal(TIME_STEP/n)); |
---|
113 | coll_contacts.empty (); |
---|
114 | } |
---|
115 | } |
---|
116 | |
---|
117 | # if 1 /* [ */ |
---|
118 | { |
---|
119 | // @@@ hack Plane2D constraint error reduction here: |
---|
120 | for (int b = 0; b < N_BODIES; b ++) |
---|
121 | { |
---|
122 | const dReal *rot = dBodyGetAngularVel (dyn_bodies[b].id()); |
---|
123 | const dReal *quat_ptr; |
---|
124 | dReal quat[4], |
---|
125 | quat_len; |
---|
126 | |
---|
127 | |
---|
128 | quat_ptr = dBodyGetQuaternion (dyn_bodies[b].id()); |
---|
129 | quat[0] = quat_ptr[0]; |
---|
130 | quat[1] = 0; |
---|
131 | quat[2] = 0; |
---|
132 | quat[3] = quat_ptr[3]; |
---|
133 | quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]); |
---|
134 | quat[0] /= quat_len; |
---|
135 | quat[3] /= quat_len; |
---|
136 | dBodySetQuaternion (dyn_bodies[b].id(), quat); |
---|
137 | dBodySetAngularVel (dyn_bodies[b].id(), 0, 0, rot[2]); |
---|
138 | } |
---|
139 | } |
---|
140 | # endif /* ] */ |
---|
141 | |
---|
142 | |
---|
143 | # if 0 /* [ */ |
---|
144 | { |
---|
145 | // @@@ friction |
---|
146 | for (int b = 0; b < N_BODIES; b ++) |
---|
147 | { |
---|
148 | const dReal *vel = dBodyGetLinearVel (dyn_bodies[b].id()), |
---|
149 | *rot = dBodyGetAngularVel (dyn_bodies[b].id()); |
---|
150 | dReal s = 1.00; |
---|
151 | dReal t = 0.99; |
---|
152 | |
---|
153 | dBodySetLinearVel (dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]); |
---|
154 | dBodySetAngularVel (dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]); |
---|
155 | } |
---|
156 | } |
---|
157 | # endif /* ] */ |
---|
158 | |
---|
159 | |
---|
160 | { |
---|
161 | // ode drawstuff |
---|
162 | |
---|
163 | dsSetTexture (DS_WOOD); |
---|
164 | for (int b = 0; b < N_BODIES; b ++) |
---|
165 | { |
---|
166 | if (b == 0) |
---|
167 | dsSetColor (1.0, 0.5, 1.0); |
---|
168 | else |
---|
169 | dsSetColor (0, 0.5, 1.0); |
---|
170 | #ifdef dDOUBLE |
---|
171 | dsDrawBoxD (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]); |
---|
172 | #else |
---|
173 | dsDrawBox (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]); |
---|
174 | #endif |
---|
175 | } |
---|
176 | } |
---|
177 | } |
---|
178 | |
---|
179 | |
---|
180 | |
---|
181 | extern int main |
---|
182 | /******************/ |
---|
183 | ( |
---|
184 | int argc, |
---|
185 | char **argv |
---|
186 | ) |
---|
187 | { |
---|
188 | int b; |
---|
189 | dsFunctions drawstuff_functions; |
---|
190 | |
---|
191 | |
---|
192 | dInitODE(); |
---|
193 | |
---|
194 | // dynamic world |
---|
195 | |
---|
196 | dReal cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP; |
---|
197 | dReal err_reduct;// = TIME_STEP * K_SPRING * cf_mixing; |
---|
198 | err_reduct = REAL( 0.5 ); |
---|
199 | cf_mixing = REAL( 0.001 ); |
---|
200 | dWorldSetERP (dyn_world.id (), err_reduct); |
---|
201 | dWorldSetCFM (dyn_world.id (), cf_mixing); |
---|
202 | dyn_world.setGravity (0, 0.0, -1.0); |
---|
203 | |
---|
204 | coll_space_id = dSimpleSpaceCreate (0); |
---|
205 | |
---|
206 | // dynamic bodies |
---|
207 | for (b = 0; b < N_BODIES; b ++) |
---|
208 | { |
---|
209 | int l = (int) (1 + sqrt ((double) N_BODIES)); |
---|
210 | dReal x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE ); |
---|
211 | dReal y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE ); |
---|
212 | dReal z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48(); |
---|
213 | |
---|
214 | bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); |
---|
215 | bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); |
---|
216 | bodies_sides[b][2] = z; |
---|
217 | |
---|
218 | dyn_bodies[b].create (dyn_world); |
---|
219 | dyn_bodies[b].setPosition (x, y, z/2); |
---|
220 | dyn_bodies[b].setData ((void*) (size_t)b); |
---|
221 | dBodySetLinearVel (dyn_bodies[b].id (), |
---|
222 | dReal( 3 * (drand48 () - 0.5) ), |
---|
223 | dReal( 3 * (drand48 () - 0.5) ), 0); |
---|
224 | |
---|
225 | dMass m; |
---|
226 | m.setBox (1, bodies_sides[b][0],bodies_sides[b][1],bodies_sides[b][2]); |
---|
227 | m.adjust (REAL(0.1) * bodies_sides[b][0] * bodies_sides[b][1]); |
---|
228 | dyn_bodies[b].setMass (&m); |
---|
229 | |
---|
230 | plane2d_joint_ids[b] = dJointCreatePlane2D (dyn_world.id (), 0); |
---|
231 | dJointAttach (plane2d_joint_ids[b], dyn_bodies[b].id (), 0); |
---|
232 | } |
---|
233 | |
---|
234 | dJointSetPlane2DXParam (plane2d_joint_ids[0], dParamFMax, 10); |
---|
235 | dJointSetPlane2DYParam (plane2d_joint_ids[0], dParamFMax, 10); |
---|
236 | |
---|
237 | |
---|
238 | // collision geoms and joints |
---|
239 | dCreatePlane (coll_space_id, 1, 0, 0, 0); |
---|
240 | dCreatePlane (coll_space_id, -1, 0, 0, -STAGE_SIZE); |
---|
241 | dCreatePlane (coll_space_id, 0, 1, 0, 0); |
---|
242 | dCreatePlane (coll_space_id, 0, -1, 0, -STAGE_SIZE); |
---|
243 | |
---|
244 | for (b = 0; b < N_BODIES; b ++) |
---|
245 | { |
---|
246 | dGeomID coll_box_id; |
---|
247 | coll_box_id = dCreateBox (coll_space_id, |
---|
248 | bodies_sides[b][0], bodies_sides[b][1], bodies_sides[b][2]); |
---|
249 | dGeomSetBody (coll_box_id, dyn_bodies[b].id ()); |
---|
250 | } |
---|
251 | |
---|
252 | coll_contacts.create (0); |
---|
253 | |
---|
254 | { |
---|
255 | // simulation loop (by drawstuff lib) |
---|
256 | drawstuff_functions.version = DS_VERSION; |
---|
257 | drawstuff_functions.start = &cb_start; |
---|
258 | drawstuff_functions.step = &cb_sim_step; |
---|
259 | drawstuff_functions.command = 0; |
---|
260 | drawstuff_functions.stop = 0; |
---|
261 | drawstuff_functions.path_to_textures = "../../drawstuff/textures"; |
---|
262 | |
---|
263 | dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions); |
---|
264 | } |
---|
265 | |
---|
266 | dCloseODE(); |
---|
267 | return 0; |
---|
268 | } |
---|