Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/ode/demo/demo_heightfield.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 16 years ago

[Physik] add ode-0.9
File size: 56.6 KB
Line 
1/*************************************************************************
2 *                                                                       *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
4 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
5 *                                                                       *
6 * This library is free software; you can redistribute it and/or         *
7 * modify it under the terms of EITHER:                                  *
8 *   (1) The GNU Lesser General Public License as published by the Free  *
9 *       Software Foundation; either version 2.1 of the License, or (at  *
10 *       your option) any later version. The text of the GNU Lesser      *
11 *       General Public License is included with this library in the     *
12 *       file LICENSE.TXT.                                               *
13 *   (2) The BSD-style license that is included with this library in     *
14 *       the file LICENSE-BSD.TXT.                                       *
15 *                                                                       *
16 * This library is distributed in the hope that it will be useful,       *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
20 *                                                                       *
21 *************************************************************************/
22
23#include <ode/ode.h>
24#include <drawstuff/drawstuff.h>
25
26#ifdef _MSC_VER
27#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
28#endif
29
30#define DEGTORAD                        0.01745329251994329577f                         //!< PI / 180.0, convert degrees to radians
31
32
33// Our heightfield geom
34dGeomID gheight;
35
36
37
38// Heightfield dimensions
39
40#define HFIELD_WSTEP                    15                      // Vertex count along edge >= 2
41#define HFIELD_DSTEP                    31
42
43#define HFIELD_WIDTH                    REAL( 4.0 )
44#define HFIELD_DEPTH                    REAL( 8.0 )
45
46#define HFIELD_WSAMP                    ( HFIELD_WIDTH / ( HFIELD_WSTEP-1 ) )
47#define HFIELD_DSAMP                    ( HFIELD_DEPTH / ( HFIELD_DSTEP-1 ) )
48
49#ifdef dDOUBLE
50#define dsDrawBox dsDrawBoxD
51#define dsDrawSphere dsDrawSphereD
52#define dsDrawCylinder dsDrawCylinderD
53#define dsDrawCapsule dsDrawCapsuleD
54#define dsDrawConvex dsDrawConvexD
55#define dsDrawTriangle dsDrawTriangleD
56#endif
57
58
59
60//<---- Convex Object
61dReal planes[]= // planes for a cube
62  {
63    1.0f ,0.0f ,0.0f ,0.25f,
64    0.0f ,1.0f ,0.0f ,0.25f,
65    0.0f ,0.0f ,1.0f ,0.25f,
66    0.0f ,0.0f ,-1.0f,0.25f,
67    0.0f ,-1.0f,0.0f ,0.25f,
68    -1.0f,0.0f ,0.0f ,0.25f
69    /*
70    1.0f ,0.0f ,0.0f ,2.0f,
71    0.0f ,1.0f ,0.0f ,1.0f,
72    0.0f ,0.0f ,1.0f ,1.0f,
73    0.0f ,0.0f ,-1.0f,1.0f,
74    0.0f ,-1.0f,0.0f ,1.0f,
75    -1.0f,0.0f ,0.0f ,0.0f
76    */
77  };
78const unsigned int planecount=6;
79
80dReal points[]= // points for a cube
81  {
82    0.25f,0.25f,0.25f,  //  point 0
83    -0.25f,0.25f,0.25f, //  point 1
84
85    0.25f,-0.25f,0.25f, //  point 2
86    -0.25f,-0.25f,0.25f,//  point 3
87
88    0.25f,0.25f,-0.25f, //  point 4
89    -0.25f,0.25f,-0.25f,//  point 5
90
91    0.25f,-0.25f,-0.25f,//  point 6
92    -0.25f,-0.25f,-0.25f,// point 7
93  };
94const unsigned int pointcount=8;
95unsigned int polygons[] = //Polygons for a cube (6 squares)
96  {
97    4,0,2,6,4, // positive X
98    4,1,0,4,5, // positive Y
99    4,0,1,3,2, // positive Z
100    4,3,1,5,7, // negative X
101    4,2,3,7,6, // negative Y
102    4,5,4,6,7, // negative Z
103  };
104//----> Convex Object
105
106// select correct drawing functions
107
108#ifdef dDOUBLE
109#define dsDrawBox dsDrawBoxD
110#define dsDrawSphere dsDrawSphereD
111#define dsDrawCylinder dsDrawCylinderD
112#define dsDrawCapsule dsDrawCapsuleD
113#define dsDrawConvex dsDrawConvexD
114#endif
115
116
117// some constants
118
119#define NUM 100                 // max number of objects
120#define DENSITY (5.0)           // density of all objects
121#define GPB 3                   // maximum number of geometries per body
122#define MAX_CONTACTS 64         // maximum number of contact points per body
123
124
125// dynamics and collision objects
126
127struct MyObject {
128  dBodyID body;                 // the body
129  dGeomID geom[GPB];            // geometries representing this body
130
131  // Trimesh only - double buffered matrices for 'last transform' setup
132  dReal matrix_dblbuff[ 16 * 2 ];
133  int last_matrix_index;
134};
135
136static int num=0;               // number of objects in simulation
137static int nextobj=0;           // next object to recycle if num==NUM
138static dWorldID world;
139static dSpaceID space;
140static MyObject obj[NUM];
141static dJointGroupID contactgroup;
142static int selected = -1;       // selected object
143static int show_aabb = 0;       // show geom AABBs?
144static int show_contacts = 0;   // show contact points?
145static int random_pos = 1;      // drop objects from random position?
146static int write_world = 0;
147
148
149
150
151//============================
152
153// Bunny mesh ripped from Opcode
154const int VertexCount = 453;
155const int IndexCount = 902 * 3;
156
157typedef dReal dVector3R[3];
158
159dGeomID TriMesh1;
160dGeomID TriMesh2;
161static dTriMeshDataID TriData1, TriData2;  // reusable static trimesh data
162
163static float Vertices[VertexCount * 3] = {
164        REAL(-0.334392), REAL(0.133007), REAL(0.062259),
165        REAL(-0.350189), REAL(0.150354), REAL(-0.147769),
166        REAL(-0.234201), REAL(0.343811), REAL(-0.174307),
167        REAL(-0.200259), REAL(0.285207), REAL(0.093749),
168        REAL(0.003520), REAL(0.475208), REAL(-0.159365),
169        REAL(0.001856), REAL(0.419203), REAL(0.098582),
170        REAL(-0.252802), REAL(0.093666), REAL(0.237538),
171        REAL(-0.162901), REAL(0.237984), REAL(0.206905),
172        REAL(0.000865), REAL(0.318141), REAL(0.235370),
173        REAL(-0.414624), REAL(0.164083), REAL(-0.278254),
174        REAL(-0.262213), REAL(0.357334), REAL(-0.293246),
175        REAL(0.004628), REAL(0.482694), REAL(-0.338626),
176        REAL(-0.402162), REAL(0.133528), REAL(-0.443247),
177        REAL(-0.243781), REAL(0.324275), REAL(-0.436763),
178        REAL(0.005293), REAL(0.437592), REAL(-0.458332),
179        REAL(-0.339884), REAL(-0.041150), REAL(-0.668211),
180        REAL(-0.248382), REAL(0.255825), REAL(-0.627493),
181        REAL(0.006261), REAL(0.376103), REAL(-0.631506),
182        REAL(-0.216201), REAL(-0.126776), REAL(-0.886936),
183        REAL(-0.171075), REAL(0.011544), REAL(-0.881386),
184        REAL(-0.181074), REAL(0.098223), REAL(-0.814779),
185        REAL(-0.119891), REAL(0.218786), REAL(-0.760153),
186        REAL(-0.078895), REAL(0.276780), REAL(-0.739281),
187        REAL(0.006801), REAL(0.310959), REAL(-0.735661),
188        REAL(-0.168842), REAL(0.102387), REAL(-0.920381),
189        REAL(-0.104072), REAL(0.177278), REAL(-0.952530),
190        REAL(-0.129704), REAL(0.211848), REAL(-0.836678),
191        REAL(-0.099875), REAL(0.310931), REAL(-0.799381),
192        REAL(0.007237), REAL(0.361687), REAL(-0.794439),
193        REAL(-0.077913), REAL(0.258753), REAL(-0.921640),
194        REAL(0.007957), REAL(0.282241), REAL(-0.931680),
195        REAL(-0.252222), REAL(-0.550401), REAL(-0.557810),
196        REAL(-0.267633), REAL(-0.603419), REAL(-0.655209),
197        REAL(-0.446838), REAL(-0.118517), REAL(-0.466159),
198        REAL(-0.459488), REAL(-0.093017), REAL(-0.311341),
199        REAL(-0.370645), REAL(-0.100108), REAL(-0.159454),
200        REAL(-0.371984), REAL(-0.091991), REAL(-0.011044),
201        REAL(-0.328945), REAL(-0.098269), REAL(0.088659),
202        REAL(-0.282452), REAL(-0.018862), REAL(0.311501),
203        REAL(-0.352403), REAL(-0.131341), REAL(0.144902),
204        REAL(-0.364126), REAL(-0.200299), REAL(0.202388),
205        REAL(-0.283965), REAL(-0.231869), REAL(0.023668),
206        REAL(-0.298943), REAL(-0.155218), REAL(0.369716),
207        REAL(-0.293787), REAL(-0.121856), REAL(0.419097),
208        REAL(-0.290163), REAL(-0.290797), REAL(0.107824),
209        REAL(-0.264165), REAL(-0.272849), REAL(0.036347),
210        REAL(-0.228567), REAL(-0.372573), REAL(0.290309),
211        REAL(-0.190431), REAL(-0.286997), REAL(0.421917),
212        REAL(-0.191039), REAL(-0.240973), REAL(0.507118),
213        REAL(-0.287272), REAL(-0.276431), REAL(-0.065444),
214        REAL(-0.295675), REAL(-0.280818), REAL(-0.174200),
215        REAL(-0.399537), REAL(-0.313131), REAL(-0.376167),
216        REAL(-0.392666), REAL(-0.488581), REAL(-0.427494),
217        REAL(-0.331669), REAL(-0.570185), REAL(-0.466054),
218        REAL(-0.282290), REAL(-0.618140), REAL(-0.589220),
219        REAL(-0.374238), REAL(-0.594882), REAL(-0.323298),
220        REAL(-0.381071), REAL(-0.629723), REAL(-0.350777),
221        REAL(-0.382112), REAL(-0.624060), REAL(-0.221577),
222        REAL(-0.272701), REAL(-0.566522), REAL(0.259157),
223        REAL(-0.256702), REAL(-0.663406), REAL(0.286079),
224        REAL(-0.280948), REAL(-0.428359), REAL(0.055790),
225        REAL(-0.184974), REAL(-0.508894), REAL(0.326265),
226        REAL(-0.279971), REAL(-0.526918), REAL(0.395319),
227        REAL(-0.282599), REAL(-0.663393), REAL(0.412411),
228        REAL(-0.188329), REAL(-0.475093), REAL(0.417954),
229        REAL(-0.263384), REAL(-0.663396), REAL(0.466604),
230        REAL(-0.209063), REAL(-0.663393), REAL(0.509344),
231        REAL(-0.002044), REAL(-0.319624), REAL(0.553078),
232        REAL(-0.001266), REAL(-0.371260), REAL(0.413296),
233        REAL(-0.219753), REAL(-0.339762), REAL(-0.040921),
234        REAL(-0.256986), REAL(-0.282511), REAL(-0.006349),
235        REAL(-0.271706), REAL(-0.260881), REAL(0.001764),
236        REAL(-0.091191), REAL(-0.419184), REAL(-0.045912),
237        REAL(-0.114944), REAL(-0.429752), REAL(-0.124739),
238        REAL(-0.113970), REAL(-0.382987), REAL(-0.188540),
239        REAL(-0.243012), REAL(-0.464942), REAL(-0.242850),
240        REAL(-0.314815), REAL(-0.505402), REAL(-0.324768),
241        REAL(0.002774), REAL(-0.437526), REAL(-0.262766),
242        REAL(-0.072625), REAL(-0.417748), REAL(-0.221440),
243        REAL(-0.160112), REAL(-0.476932), REAL(-0.293450),
244        REAL(0.003859), REAL(-0.453425), REAL(-0.443916),
245        REAL(-0.120363), REAL(-0.581567), REAL(-0.438689),
246        REAL(-0.091499), REAL(-0.584191), REAL(-0.294511),
247        REAL(-0.116469), REAL(-0.599861), REAL(-0.188308),
248        REAL(-0.208032), REAL(-0.513640), REAL(-0.134649),
249        REAL(-0.235749), REAL(-0.610017), REAL(-0.040939),
250        REAL(-0.344916), REAL(-0.622487), REAL(-0.085380),
251        REAL(-0.336401), REAL(-0.531864), REAL(-0.212298),
252        REAL(0.001961), REAL(-0.459550), REAL(-0.135547),
253        REAL(-0.058296), REAL(-0.430536), REAL(-0.043440),
254        REAL(0.001378), REAL(-0.449511), REAL(-0.037762),
255        REAL(-0.130135), REAL(-0.510222), REAL(0.079144),
256        REAL(0.000142), REAL(-0.477549), REAL(0.157064),
257        REAL(-0.114284), REAL(-0.453206), REAL(0.304397),
258        REAL(-0.000592), REAL(-0.443558), REAL(0.285401),
259        REAL(-0.056215), REAL(-0.663402), REAL(0.326073),
260        REAL(-0.026248), REAL(-0.568010), REAL(0.273318),
261        REAL(-0.049261), REAL(-0.531064), REAL(0.389854),
262        REAL(-0.127096), REAL(-0.663398), REAL(0.479316),
263        REAL(-0.058384), REAL(-0.663401), REAL(0.372891),
264        REAL(-0.303961), REAL(0.054199), REAL(0.625921),
265        REAL(-0.268594), REAL(0.193403), REAL(0.502766),
266        REAL(-0.277159), REAL(0.126123), REAL(0.443289),
267        REAL(-0.287605), REAL(-0.005722), REAL(0.531844),
268        REAL(-0.231396), REAL(-0.121289), REAL(0.587387),
269        REAL(-0.253475), REAL(-0.081797), REAL(0.756541),
270        REAL(-0.195164), REAL(-0.137969), REAL(0.728011),
271        REAL(-0.167673), REAL(-0.156573), REAL(0.609388),
272        REAL(-0.145917), REAL(-0.169029), REAL(0.697600),
273        REAL(-0.077776), REAL(-0.214247), REAL(0.622586),
274        REAL(-0.076873), REAL(-0.214971), REAL(0.696301),
275        REAL(-0.002341), REAL(-0.233135), REAL(0.622859),
276        REAL(-0.002730), REAL(-0.213526), REAL(0.691267),
277        REAL(-0.003136), REAL(-0.192628), REAL(0.762731),
278        REAL(-0.056136), REAL(-0.201222), REAL(0.763806),
279        REAL(-0.114589), REAL(-0.166192), REAL(0.770723),
280        REAL(-0.155145), REAL(-0.129632), REAL(0.791738),
281        REAL(-0.183611), REAL(-0.058705), REAL(0.847012),
282        REAL(-0.165562), REAL(0.001980), REAL(0.833386),
283        REAL(-0.220084), REAL(0.019914), REAL(0.768935),
284        REAL(-0.255730), REAL(0.090306), REAL(0.670782),
285        REAL(-0.255594), REAL(0.113833), REAL(0.663389),
286        REAL(-0.226380), REAL(0.212655), REAL(0.617740),
287        REAL(-0.003367), REAL(-0.195342), REAL(0.799680),
288        REAL(-0.029743), REAL(-0.210508), REAL(0.827180),
289        REAL(-0.003818), REAL(-0.194783), REAL(0.873636),
290        REAL(-0.004116), REAL(-0.157907), REAL(0.931268),
291        REAL(-0.031280), REAL(-0.184555), REAL(0.889476),
292        REAL(-0.059885), REAL(-0.184448), REAL(0.841330),
293        REAL(-0.135333), REAL(-0.164332), REAL(0.878200),
294        REAL(-0.085574), REAL(-0.170948), REAL(0.925547),
295        REAL(-0.163833), REAL(-0.094170), REAL(0.897114),
296        REAL(-0.138444), REAL(-0.104250), REAL(0.945975),
297        REAL(-0.083497), REAL(-0.084934), REAL(0.979607),
298        REAL(-0.004433), REAL(-0.146642), REAL(0.985872),
299        REAL(-0.150715), REAL(0.032650), REAL(0.884111),
300        REAL(-0.135892), REAL(-0.035520), REAL(0.945455),
301        REAL(-0.070612), REAL(0.036849), REAL(0.975733),
302        REAL(-0.004458), REAL(-0.042526), REAL(1.015670),
303        REAL(-0.004249), REAL(0.046042), REAL(1.003240),
304        REAL(-0.086969), REAL(0.133224), REAL(0.947633),
305        REAL(-0.003873), REAL(0.161605), REAL(0.970499),
306        REAL(-0.125544), REAL(0.140012), REAL(0.917678),
307        REAL(-0.125651), REAL(0.250246), REAL(0.857602),
308        REAL(-0.003127), REAL(0.284070), REAL(0.878870),
309        REAL(-0.159174), REAL(0.125726), REAL(0.888878),
310        REAL(-0.183807), REAL(0.196970), REAL(0.844480),
311        REAL(-0.159890), REAL(0.291736), REAL(0.732480),
312        REAL(-0.199495), REAL(0.207230), REAL(0.779864),
313        REAL(-0.206182), REAL(0.164608), REAL(0.693257),
314        REAL(-0.186315), REAL(0.160689), REAL(0.817193),
315        REAL(-0.192827), REAL(0.166706), REAL(0.782271),
316        REAL(-0.175112), REAL(0.110008), REAL(0.860621),
317        REAL(-0.161022), REAL(0.057420), REAL(0.855111),
318        REAL(-0.172319), REAL(0.036155), REAL(0.816189),
319        REAL(-0.190318), REAL(0.064083), REAL(0.760605),
320        REAL(-0.195072), REAL(0.129179), REAL(0.731104),
321        REAL(-0.203126), REAL(0.410287), REAL(0.680536),
322        REAL(-0.216677), REAL(0.309274), REAL(0.642272),
323        REAL(-0.241515), REAL(0.311485), REAL(0.587832),
324        REAL(-0.002209), REAL(0.366663), REAL(0.749413),
325        REAL(-0.088230), REAL(0.396265), REAL(0.678635),
326        REAL(-0.170147), REAL(0.109517), REAL(0.840784),
327        REAL(-0.160521), REAL(0.067766), REAL(0.830650),
328        REAL(-0.181546), REAL(0.139805), REAL(0.812146),
329        REAL(-0.180495), REAL(0.148568), REAL(0.776087),
330        REAL(-0.180255), REAL(0.129125), REAL(0.744192),
331        REAL(-0.186298), REAL(0.078308), REAL(0.769352),
332        REAL(-0.167622), REAL(0.060539), REAL(0.806675),
333        REAL(-0.189876), REAL(0.102760), REAL(0.802582),
334        REAL(-0.108340), REAL(0.455446), REAL(0.657174),
335        REAL(-0.241585), REAL(0.527592), REAL(0.669296),
336        REAL(-0.265676), REAL(0.513366), REAL(0.634594),
337        REAL(-0.203073), REAL(0.478550), REAL(0.581526),
338        REAL(-0.266772), REAL(0.642330), REAL(0.602061),
339        REAL(-0.216961), REAL(0.564846), REAL(0.535435),
340        REAL(-0.202210), REAL(0.525495), REAL(0.475944),
341        REAL(-0.193888), REAL(0.467925), REAL(0.520606),
342        REAL(-0.265837), REAL(0.757267), REAL(0.500933),
343        REAL(-0.240306), REAL(0.653440), REAL(0.463215),
344        REAL(-0.309239), REAL(0.776868), REAL(0.304726),
345        REAL(-0.271009), REAL(0.683094), REAL(0.382018),
346        REAL(-0.312111), REAL(0.671099), REAL(0.286687),
347        REAL(-0.268791), REAL(0.624342), REAL(0.377231),
348        REAL(-0.302457), REAL(0.533996), REAL(0.360289),
349        REAL(-0.263656), REAL(0.529310), REAL(0.412564),
350        REAL(-0.282311), REAL(0.415167), REAL(0.447666),
351        REAL(-0.239201), REAL(0.442096), REAL(0.495604),
352        REAL(-0.220043), REAL(0.569026), REAL(0.445877),
353        REAL(-0.001263), REAL(0.395631), REAL(0.602029),
354        REAL(-0.057345), REAL(0.442535), REAL(0.572224),
355        REAL(-0.088927), REAL(0.506333), REAL(0.529106),
356        REAL(-0.125738), REAL(0.535076), REAL(0.612913),
357        REAL(-0.126251), REAL(0.577170), REAL(0.483159),
358        REAL(-0.149594), REAL(0.611520), REAL(0.557731),
359        REAL(-0.163188), REAL(0.660791), REAL(0.491080),
360        REAL(-0.172482), REAL(0.663387), REAL(0.415416),
361        REAL(-0.160464), REAL(0.591710), REAL(0.370659),
362        REAL(-0.156445), REAL(0.536396), REAL(0.378302),
363        REAL(-0.136496), REAL(0.444358), REAL(0.425226),
364        REAL(-0.095564), REAL(0.373768), REAL(0.473659),
365        REAL(-0.104146), REAL(0.315912), REAL(0.498104),
366        REAL(-0.000496), REAL(0.384194), REAL(0.473817),
367        REAL(-0.000183), REAL(0.297770), REAL(0.401486),
368        REAL(-0.129042), REAL(0.270145), REAL(0.434495),
369        REAL(0.000100), REAL(0.272963), REAL(0.349138),
370        REAL(-0.113060), REAL(0.236984), REAL(0.385554),
371        REAL(0.007260), REAL(0.016311), REAL(-0.883396),
372        REAL(0.007865), REAL(0.122104), REAL(-0.956137),
373        REAL(-0.032842), REAL(0.115282), REAL(-0.953252),
374        REAL(-0.089115), REAL(0.108449), REAL(-0.950317),
375        REAL(-0.047440), REAL(0.014729), REAL(-0.882756),
376        REAL(-0.104458), REAL(0.013137), REAL(-0.882070),
377        REAL(-0.086439), REAL(-0.584866), REAL(-0.608343),
378        REAL(-0.115026), REAL(-0.662605), REAL(-0.436732),
379        REAL(-0.071683), REAL(-0.665372), REAL(-0.606385),
380        REAL(-0.257884), REAL(-0.665381), REAL(-0.658052),
381        REAL(-0.272542), REAL(-0.665381), REAL(-0.592063),
382        REAL(-0.371322), REAL(-0.665382), REAL(-0.353620),
383        REAL(-0.372362), REAL(-0.665381), REAL(-0.224420),
384        REAL(-0.335166), REAL(-0.665380), REAL(-0.078623),
385        REAL(-0.225999), REAL(-0.665375), REAL(-0.038981),
386        REAL(-0.106719), REAL(-0.665374), REAL(-0.186351),
387        REAL(-0.081749), REAL(-0.665372), REAL(-0.292554),
388        REAL(0.006943), REAL(-0.091505), REAL(-0.858354),
389        REAL(0.006117), REAL(-0.280985), REAL(-0.769967),
390        REAL(0.004495), REAL(-0.502360), REAL(-0.559799),
391        REAL(-0.198638), REAL(-0.302135), REAL(-0.845816),
392        REAL(-0.237395), REAL(-0.542544), REAL(-0.587188),
393        REAL(-0.270001), REAL(-0.279489), REAL(-0.669861),
394        REAL(-0.134547), REAL(-0.119852), REAL(-0.959004),
395        REAL(-0.052088), REAL(-0.122463), REAL(-0.944549),
396        REAL(-0.124463), REAL(-0.293508), REAL(-0.899566),
397        REAL(-0.047616), REAL(-0.289643), REAL(-0.879292),
398        REAL(-0.168595), REAL(-0.529132), REAL(-0.654931),
399        REAL(-0.099793), REAL(-0.515719), REAL(-0.645873),
400        REAL(-0.186168), REAL(-0.605282), REAL(-0.724690),
401        REAL(-0.112970), REAL(-0.583097), REAL(-0.707469),
402        REAL(-0.108152), REAL(-0.665375), REAL(-0.700408),
403        REAL(-0.183019), REAL(-0.665378), REAL(-0.717630),
404        REAL(-0.349529), REAL(-0.334459), REAL(-0.511985),
405        REAL(-0.141182), REAL(-0.437705), REAL(-0.798194),
406        REAL(-0.212670), REAL(-0.448725), REAL(-0.737447),
407        REAL(-0.261111), REAL(-0.414945), REAL(-0.613835),
408        REAL(-0.077364), REAL(-0.431480), REAL(-0.778113),
409        REAL(0.005174), REAL(-0.425277), REAL(-0.651592),
410        REAL(0.089236), REAL(-0.431732), REAL(-0.777093),
411        REAL(0.271006), REAL(-0.415749), REAL(-0.610577),
412        REAL(0.223981), REAL(-0.449384), REAL(-0.734774),
413        REAL(0.153275), REAL(-0.438150), REAL(-0.796391),
414        REAL(0.358414), REAL(-0.335529), REAL(-0.507649),
415        REAL(0.193434), REAL(-0.665946), REAL(-0.715325),
416        REAL(0.118363), REAL(-0.665717), REAL(-0.699021),
417        REAL(0.123515), REAL(-0.583454), REAL(-0.706020),
418        REAL(0.196851), REAL(-0.605860), REAL(-0.722345),
419        REAL(0.109788), REAL(-0.516035), REAL(-0.644590),
420        REAL(0.178656), REAL(-0.529656), REAL(-0.652804),
421        REAL(0.061157), REAL(-0.289807), REAL(-0.878626),
422        REAL(0.138234), REAL(-0.293905), REAL(-0.897958),
423        REAL(0.066933), REAL(-0.122643), REAL(-0.943820),
424        REAL(0.149571), REAL(-0.120281), REAL(-0.957264),
425        REAL(0.280989), REAL(-0.280321), REAL(-0.666487),
426        REAL(0.246581), REAL(-0.543275), REAL(-0.584224),
427        REAL(0.211720), REAL(-0.302754), REAL(-0.843303),
428        REAL(0.086966), REAL(-0.665627), REAL(-0.291520),
429        REAL(0.110634), REAL(-0.665702), REAL(-0.185021),
430        REAL(0.228099), REAL(-0.666061), REAL(-0.036201),
431        REAL(0.337743), REAL(-0.666396), REAL(-0.074503),
432        REAL(0.376722), REAL(-0.666513), REAL(-0.219833),
433        REAL(0.377265), REAL(-0.666513), REAL(-0.349036),
434        REAL(0.281411), REAL(-0.666217), REAL(-0.588670),
435        REAL(0.267564), REAL(-0.666174), REAL(-0.654834),
436        REAL(0.080745), REAL(-0.665602), REAL(-0.605452),
437        REAL(0.122016), REAL(-0.662963), REAL(-0.435280),
438        REAL(0.095767), REAL(-0.585141), REAL(-0.607228),
439        REAL(0.118944), REAL(0.012799), REAL(-0.880702),
440        REAL(0.061944), REAL(0.014564), REAL(-0.882086),
441        REAL(0.104725), REAL(0.108156), REAL(-0.949130),
442        REAL(0.048513), REAL(0.115159), REAL(-0.952753),
443        REAL(0.112696), REAL(0.236643), REAL(0.386937),
444        REAL(0.128177), REAL(0.269757), REAL(0.436071),
445        REAL(0.102643), REAL(0.315600), REAL(0.499370),
446        REAL(0.094535), REAL(0.373481), REAL(0.474824),
447        REAL(0.136270), REAL(0.443946), REAL(0.426895),
448        REAL(0.157071), REAL(0.535923), REAL(0.380222),
449        REAL(0.161350), REAL(0.591224), REAL(0.372630),
450        REAL(0.173035), REAL(0.662865), REAL(0.417531),
451        REAL(0.162808), REAL(0.660299), REAL(0.493077),
452        REAL(0.148250), REAL(0.611070), REAL(0.559555),
453        REAL(0.125719), REAL(0.576790), REAL(0.484702),
454        REAL(0.123489), REAL(0.534699), REAL(0.614440),
455        REAL(0.087621), REAL(0.506066), REAL(0.530188),
456        REAL(0.055321), REAL(0.442365), REAL(0.572915),
457        REAL(0.219936), REAL(0.568361), REAL(0.448571),
458        REAL(0.238099), REAL(0.441375), REAL(0.498528),
459        REAL(0.281711), REAL(0.414315), REAL(0.451121),
460        REAL(0.263833), REAL(0.528513), REAL(0.415794),
461        REAL(0.303284), REAL(0.533081), REAL(0.363998),
462        REAL(0.269687), REAL(0.623528), REAL(0.380528),
463        REAL(0.314255), REAL(0.670153), REAL(0.290524),
464        REAL(0.272023), REAL(0.682273), REAL(0.385343),
465        REAL(0.311480), REAL(0.775931), REAL(0.308527),
466        REAL(0.240239), REAL(0.652714), REAL(0.466159),
467        REAL(0.265619), REAL(0.756464), REAL(0.504187),
468        REAL(0.192562), REAL(0.467341), REAL(0.522972),
469        REAL(0.201605), REAL(0.524885), REAL(0.478417),
470        REAL(0.215743), REAL(0.564193), REAL(0.538084),
471        REAL(0.264969), REAL(0.641527), REAL(0.605317),
472        REAL(0.201031), REAL(0.477940), REAL(0.584002),
473        REAL(0.263086), REAL(0.512567), REAL(0.637832),
474        REAL(0.238615), REAL(0.526867), REAL(0.672237),
475        REAL(0.105309), REAL(0.455123), REAL(0.658482),
476        REAL(0.183993), REAL(0.102195), REAL(0.804872),
477        REAL(0.161563), REAL(0.060042), REAL(0.808692),
478        REAL(0.180748), REAL(0.077754), REAL(0.771600),
479        REAL(0.175168), REAL(0.128588), REAL(0.746368),
480        REAL(0.175075), REAL(0.148030), REAL(0.778264),
481        REAL(0.175658), REAL(0.139265), REAL(0.814333),
482        REAL(0.154191), REAL(0.067291), REAL(0.832578),
483        REAL(0.163818), REAL(0.109013), REAL(0.842830),
484        REAL(0.084760), REAL(0.396004), REAL(0.679695),
485        REAL(0.238888), REAL(0.310760), REAL(0.590775),
486        REAL(0.213380), REAL(0.308625), REAL(0.644905),
487        REAL(0.199666), REAL(0.409678), REAL(0.683003),
488        REAL(0.190143), REAL(0.128597), REAL(0.733463),
489        REAL(0.184833), REAL(0.063516), REAL(0.762902),
490        REAL(0.166070), REAL(0.035644), REAL(0.818261),
491        REAL(0.154361), REAL(0.056943), REAL(0.857042),
492        REAL(0.168542), REAL(0.109489), REAL(0.862725),
493        REAL(0.187387), REAL(0.166131), REAL(0.784599),
494        REAL(0.180428), REAL(0.160135), REAL(0.819438),
495        REAL(0.201823), REAL(0.163991), REAL(0.695756),
496        REAL(0.194206), REAL(0.206635), REAL(0.782275),
497        REAL(0.155438), REAL(0.291260), REAL(0.734412),
498        REAL(0.177696), REAL(0.196424), REAL(0.846693),
499        REAL(0.152305), REAL(0.125256), REAL(0.890786),
500        REAL(0.119546), REAL(0.249876), REAL(0.859104),
501        REAL(0.118369), REAL(0.139643), REAL(0.919173),
502        REAL(0.079410), REAL(0.132973), REAL(0.948652),
503        REAL(0.062419), REAL(0.036648), REAL(0.976547),
504        REAL(0.127847), REAL(-0.035919), REAL(0.947070),
505        REAL(0.143624), REAL(0.032206), REAL(0.885913),
506        REAL(0.074888), REAL(-0.085173), REAL(0.980577),
507        REAL(0.130184), REAL(-0.104656), REAL(0.947620),
508        REAL(0.156201), REAL(-0.094653), REAL(0.899074),
509        REAL(0.077366), REAL(-0.171194), REAL(0.926545),
510        REAL(0.127722), REAL(-0.164729), REAL(0.879810),
511        REAL(0.052670), REAL(-0.184618), REAL(0.842019),
512        REAL(0.023477), REAL(-0.184638), REAL(0.889811),
513        REAL(0.022626), REAL(-0.210587), REAL(0.827500),
514        REAL(0.223089), REAL(0.211976), REAL(0.620493),
515        REAL(0.251444), REAL(0.113067), REAL(0.666494),
516        REAL(0.251419), REAL(0.089540), REAL(0.673887),
517        REAL(0.214360), REAL(0.019258), REAL(0.771595),
518        REAL(0.158999), REAL(0.001490), REAL(0.835374),
519        REAL(0.176696), REAL(-0.059249), REAL(0.849218),
520        REAL(0.148696), REAL(-0.130091), REAL(0.793599),
521        REAL(0.108290), REAL(-0.166528), REAL(0.772088),
522        REAL(0.049820), REAL(-0.201382), REAL(0.764454),
523        REAL(0.071341), REAL(-0.215195), REAL(0.697209),
524        REAL(0.073148), REAL(-0.214475), REAL(0.623510),
525        REAL(0.140502), REAL(-0.169461), REAL(0.699354),
526        REAL(0.163374), REAL(-0.157073), REAL(0.611416),
527        REAL(0.189466), REAL(-0.138550), REAL(0.730366),
528        REAL(0.247593), REAL(-0.082554), REAL(0.759610),
529        REAL(0.227468), REAL(-0.121982), REAL(0.590197),
530        REAL(0.284702), REAL(-0.006586), REAL(0.535347),
531        REAL(0.275741), REAL(0.125287), REAL(0.446676),
532        REAL(0.266650), REAL(0.192594), REAL(0.506044),
533        REAL(0.300086), REAL(0.053287), REAL(0.629620),
534        REAL(0.055450), REAL(-0.663935), REAL(0.375065),
535        REAL(0.122854), REAL(-0.664138), REAL(0.482323),
536        REAL(0.046520), REAL(-0.531571), REAL(0.391918),
537        REAL(0.024824), REAL(-0.568450), REAL(0.275106),
538        REAL(0.053855), REAL(-0.663931), REAL(0.328224),
539        REAL(0.112829), REAL(-0.453549), REAL(0.305788),
540        REAL(0.131265), REAL(-0.510617), REAL(0.080746),
541        REAL(0.061174), REAL(-0.430716), REAL(-0.042710),
542        REAL(0.341019), REAL(-0.532887), REAL(-0.208150),
543        REAL(0.347705), REAL(-0.623533), REAL(-0.081139),
544        REAL(0.238040), REAL(-0.610732), REAL(-0.038037),
545        REAL(0.211764), REAL(-0.514274), REAL(-0.132078),
546        REAL(0.120605), REAL(-0.600219), REAL(-0.186856),
547        REAL(0.096985), REAL(-0.584476), REAL(-0.293357),
548        REAL(0.127621), REAL(-0.581941), REAL(-0.437170),
549        REAL(0.165902), REAL(-0.477425), REAL(-0.291453),
550        REAL(0.077720), REAL(-0.417975), REAL(-0.220519),
551        REAL(0.320892), REAL(-0.506363), REAL(-0.320874),
552        REAL(0.248214), REAL(-0.465684), REAL(-0.239842),
553        REAL(0.118764), REAL(-0.383338), REAL(-0.187114),
554        REAL(0.118816), REAL(-0.430106), REAL(-0.123307),
555        REAL(0.094131), REAL(-0.419464), REAL(-0.044777),
556        REAL(0.274526), REAL(-0.261706), REAL(0.005110),
557        REAL(0.259842), REAL(-0.283292), REAL(-0.003185),
558        REAL(0.222861), REAL(-0.340431), REAL(-0.038210),
559        REAL(0.204445), REAL(-0.664380), REAL(0.513353),
560        REAL(0.259286), REAL(-0.664547), REAL(0.471281),
561        REAL(0.185402), REAL(-0.476020), REAL(0.421718),
562        REAL(0.279163), REAL(-0.664604), REAL(0.417328),
563        REAL(0.277157), REAL(-0.528122), REAL(0.400208),
564        REAL(0.183069), REAL(-0.509812), REAL(0.329995),
565        REAL(0.282599), REAL(-0.429210), REAL(0.059242),
566        REAL(0.254816), REAL(-0.664541), REAL(0.290687),
567        REAL(0.271436), REAL(-0.567707), REAL(0.263966),
568        REAL(0.386561), REAL(-0.625221), REAL(-0.216870),
569        REAL(0.387086), REAL(-0.630883), REAL(-0.346073),
570        REAL(0.380021), REAL(-0.596021), REAL(-0.318679),
571        REAL(0.291269), REAL(-0.619007), REAL(-0.585707),
572        REAL(0.339280), REAL(-0.571198), REAL(-0.461946),
573        REAL(0.400045), REAL(-0.489778), REAL(-0.422640),
574        REAL(0.406817), REAL(-0.314349), REAL(-0.371230),
575        REAL(0.300588), REAL(-0.281718), REAL(-0.170549),
576        REAL(0.290866), REAL(-0.277304), REAL(-0.061905),
577        REAL(0.187735), REAL(-0.241545), REAL(0.509437),
578        REAL(0.188032), REAL(-0.287569), REAL(0.424234),
579        REAL(0.227520), REAL(-0.373262), REAL(0.293102),
580        REAL(0.266526), REAL(-0.273650), REAL(0.039597),
581        REAL(0.291592), REAL(-0.291676), REAL(0.111386),
582        REAL(0.291914), REAL(-0.122741), REAL(0.422683),
583        REAL(0.297574), REAL(-0.156119), REAL(0.373368),
584        REAL(0.286603), REAL(-0.232731), REAL(0.027162),
585        REAL(0.364663), REAL(-0.201399), REAL(0.206850),
586        REAL(0.353855), REAL(-0.132408), REAL(0.149228),
587        REAL(0.282208), REAL(-0.019715), REAL(0.314960),
588        REAL(0.331187), REAL(-0.099266), REAL(0.092701),
589        REAL(0.375463), REAL(-0.093120), REAL(-0.006467),
590        REAL(0.375917), REAL(-0.101236), REAL(-0.154882),
591        REAL(0.466635), REAL(-0.094416), REAL(-0.305669),
592        REAL(0.455805), REAL(-0.119881), REAL(-0.460632),
593        REAL(0.277465), REAL(-0.604242), REAL(-0.651871),
594        REAL(0.261022), REAL(-0.551176), REAL(-0.554667),
595        REAL(0.093627), REAL(0.258494), REAL(-0.920589),
596        REAL(0.114248), REAL(0.310608), REAL(-0.798070),
597        REAL(0.144232), REAL(0.211434), REAL(-0.835001),
598        REAL(0.119916), REAL(0.176940), REAL(-0.951159),
599        REAL(0.184061), REAL(0.101854), REAL(-0.918220),
600        REAL(0.092431), REAL(0.276521), REAL(-0.738231),
601        REAL(0.133504), REAL(0.218403), REAL(-0.758602),
602        REAL(0.194987), REAL(0.097655), REAL(-0.812476),
603        REAL(0.185542), REAL(0.011005), REAL(-0.879202),
604        REAL(0.230315), REAL(-0.127450), REAL(-0.884202),
605        REAL(0.260471), REAL(0.255056), REAL(-0.624378),
606        REAL(0.351567), REAL(-0.042194), REAL(-0.663976),
607        REAL(0.253742), REAL(0.323524), REAL(-0.433716),
608        REAL(0.411612), REAL(0.132299), REAL(-0.438264),
609        REAL(0.270513), REAL(0.356530), REAL(-0.289984),
610        REAL(0.422146), REAL(0.162819), REAL(-0.273130),
611        REAL(0.164724), REAL(0.237490), REAL(0.208912),
612        REAL(0.253806), REAL(0.092900), REAL(0.240640),
613        REAL(0.203608), REAL(0.284597), REAL(0.096223),
614        REAL(0.241006), REAL(0.343093), REAL(-0.171396),
615        REAL(0.356076), REAL(0.149288), REAL(-0.143443),
616        REAL(0.337656), REAL(0.131992), REAL(0.066374)
617};
618
619int Indices[IndexCount / 3][3] = {
620        {126,134,133},
621        {342,138,134},
622        {133,134,138},
623        {126,342,134},
624        {312,316,317},
625        {169,163,162},
626        {312,317,319},
627        {312,319,318},
628        {169,162,164},
629        {169,168,163},
630        {312,314,315},
631        {169,164,165},
632        {169,167,168},
633        {312,315,316},
634        {312,313,314},
635        {169,165,166},
636        {169,166,167},
637        {312,318,313},
638        {308,304,305},
639        {308,305,306},
640        {179,181,188},
641        {177,173,175},
642        {177,175,176},
643        {302,293,300},
644        {322,294,304},
645        {188,176,175},
646        {188,175,179},
647        {158,177,187},
648        {305,293,302},
649        {305,302,306},
650        {322,304,308},
651        {188,181,183},
652        {158,173,177},
653        {293,298,300},
654        {304,294,296},
655        {304,296,305},
656        {185,176,188},
657        {185,188,183},
658        {187,177,176},
659        {187,176,185},
660        {305,296,298},
661        {305,298,293},
662        {436,432, 28},
663        {436, 28, 23},
664        {434,278,431},
665        { 30,208,209},
666        { 30,209, 29},
667        { 19, 20, 24},
668        {208,207,211},
669        {208,211,209},
670        { 19,210,212},
671        {433,434,431},
672        {433,431,432},
673        {433,432,436},
674        {436,437,433},
675        {277,275,276},
676        {277,276,278},
677        {209,210, 25},
678        { 21, 26, 24},
679        { 21, 24, 20},
680        { 25, 26, 27},
681        { 25, 27, 29},
682        {435,439,277},
683        {439,275,277},
684        {432,431, 30},
685        {432, 30, 28},
686        {433,437,438},
687        {433,438,435},
688        {434,277,278},
689        { 24, 25,210},
690        { 24, 26, 25},
691        { 29, 27, 28},
692        { 29, 28, 30},
693        { 19, 24,210},
694        {208, 30,431},
695        {208,431,278},
696        {435,434,433},
697        {435,277,434},
698        { 25, 29,209},
699        { 27, 22, 23},
700        { 27, 23, 28},
701        { 26, 22, 27},
702        { 26, 21, 22},
703        {212,210,209},
704        {212,209,211},
705        {207,208,278},
706        {207,278,276},
707        {439,435,438},
708        { 12,  9, 10},
709        { 12, 10, 13},
710        {  2,  3,  5},
711        {  2,  5,  4},
712        { 16, 13, 14},
713        { 16, 14, 17},
714        { 22, 21, 16},
715        { 13, 10, 11},
716        { 13, 11, 14},
717        {  1,  0,  3},
718        {  1,  3,  2},
719        { 15, 12, 16},
720        { 19, 18, 15},
721        { 19, 15, 16},
722        { 19, 16, 20},
723        {  9,  1,  2},
724        {  9,  2, 10},
725        {  3,  7,  8},
726        {  3,  8,  5},
727        { 16, 17, 23},
728        { 16, 23, 22},
729        { 21, 20, 16},
730        { 10,  2,  4},
731        { 10,  4, 11},
732        {  0,  6,  7},
733        {  0,  7,  3},
734        { 12, 13, 16},
735        {451,446,445},
736        {451,445,450},
737        {442,440,439},
738        {442,439,438},
739        {442,438,441},
740        {421,420,422},
741        {412,411,426},
742        {412,426,425},
743        {408,405,407},
744        {413, 67, 68},
745        {413, 68,414},
746        {391,390,412},
747        { 80,384,386},
748        {404,406,378},
749        {390,391,377},
750        {390,377, 88},
751        {400,415,375},
752        {398,396,395},
753        {398,395,371},
754        {398,371,370},
755        {112,359,358},
756        {112,358,113},
757        {351,352,369},
758        {125,349,348},
759        {345,343,342},
760        {342,340,339},
761        {341,335,337},
762        {328,341,327},
763        {331,323,333},
764        {331,322,323},
765        {327,318,319},
766        {327,319,328},
767        {315,314,324},
768        {302,300,301},
769        {302,301,303},
770        {320,311,292},
771        {285,284,289},
772        {310,307,288},
773        {310,288,290},
774        {321,350,281},
775        {321,281,282},
776        {423,448,367},
777        {272,273,384},
778        {272,384,274},
779        {264,265,382},
780        {264,382,383},
781        {440,442,261},
782        {440,261,263},
783        {252,253,254},
784        {252,254,251},
785        {262,256,249},
786        {262,249,248},
787        {228,243,242},
788        {228, 31,243},
789        {213,215,238},
790        {213,238,237},
791        { 19,212,230},
792        {224,225,233},
793        {224,233,231},
794        {217,218, 56},
795        {217, 56, 54},
796        {217,216,239},
797        {217,239,238},
798        {217,238,215},
799        {218,217,215},
800        {218,215,214},
801        {  6,102,206},
802        {186,199,200},
803        {197,182,180},
804        {170,171,157},
805        {201,200,189},
806        {170,190,191},
807        {170,191,192},
808        {175,174,178},
809        {175,178,179},
810        {168,167,155},
811        {122,149,158},
812        {122,158,159},
813        {135,153,154},
814        {135,154,118},
815        {143,140,141},
816        {143,141,144},
817        {132,133,136},
818        {130,126,133},
819        {124,125,127},
820        {122,101,100},
821        {122,100,121},
822        {110,108,107},
823        {110,107,109},
824        { 98, 99, 97},
825        { 98, 97, 64},
826        { 98, 64, 66},
827        { 87, 55, 57},
828        { 83, 82, 79},
829        { 83, 79, 84},
830        { 78, 74, 50},
831        { 49, 71, 41},
832        { 49, 41, 37},
833        { 49, 37, 36},
834        { 58, 44, 60},
835        { 60, 59, 58},
836        { 51, 34, 33},
837        { 39, 40, 42},
838        { 39, 42, 38},
839        {243,240, 33},
840        {243, 33,229},
841        { 39, 38,  6},
842        { 44, 46, 40},
843        { 55, 56, 57},
844        { 64, 62, 65},
845        { 64, 65, 66},
846        { 41, 71, 45},
847        { 75, 50, 51},
848        { 81, 79, 82},
849        { 77, 88, 73},
850        { 93, 92, 94},
851        { 68, 47, 46},
852        { 96, 97, 99},
853        { 96, 99, 95},
854        {110,109,111},
855        {111,112,110},
856        {114,113,123},
857        {114,123,124},
858        {132,131,129},
859        {133,137,136},
860        {135,142,145},
861        {145,152,135},
862        {149,147,157},
863        {157,158,149},
864        {164,150,151},
865        {153,163,168},
866        {153,168,154},
867        {185,183,182},
868        {185,182,184},
869        {161,189,190},
870        {200,199,191},
871        {200,191,190},
872        {180,178,195},
873        {180,195,196},
874        {102,101,204},
875        {102,204,206},
876        { 43, 48,104},
877        { 43,104,103},
878        {216,217, 54},
879        {216, 54, 32},
880        {207,224,231},
881        {230,212,211},
882        {230,211,231},
883        {227,232,241},
884        {227,241,242},
885        {235,234,241},
886        {235,241,244},
887        {430,248,247},
888        {272,274,253},
889        {272,253,252},
890        {439,260,275},
891        {225,224,259},
892        {225,259,257},
893        {269,270,407},
894        {269,407,405},
895        {270,269,273},
896        {270,273,272},
897        {273,269,268},
898        {273,268,267},
899        {273,267,266},
900        {273,266,265},
901        {273,265,264},
902        {448,279,367},
903        {281,350,368},
904        {285,286,301},
905        {290,323,310},
906        {290,311,323},
907        {282,281,189},
908        {292,311,290},
909        {292,290,291},
910        {307,306,302},
911        {307,302,303},
912        {316,315,324},
913        {316,324,329},
914        {331,351,350},
915        {330,334,335},
916        {330,335,328},
917        {341,337,338},
918        {344,355,354},
919        {346,345,348},
920        {346,348,347},
921        {364,369,352},
922        {364,352,353},
923        {365,363,361},
924        {365,361,362},
925        {376,401,402},
926        {373,372,397},
927        {373,397,400},
928        {376, 92,377},
929        {381,378,387},
930        {381,387,385},
931        {386, 77, 80},
932        {390,389,412},
933        {416,417,401},
934        {403,417,415},
935        {408,429,430},
936        {419,423,418},
937        {427,428,444},
938        {427,444,446},
939        {437,436,441},
940        {450,445, 11},
941        {450, 11,  4},
942        {447,449,  5},
943        {447,  5,  8},
944        {441,438,437},
945        {425,426,451},
946        {425,451,452},
947        {417,421,415},
948        {408,407,429},
949        {399,403,400},
950        {399,400,397},
951        {394,393,416},
952        {389,411,412},
953        {386,383,385},
954        {408,387,378},
955        {408,378,406},
956        {377,391,376},
957        { 94,375,415},
958        {372,373,374},
959        {372,374,370},
960        {359,111,360},
961        {359,112,111},
962        {113,358,349},
963        {113,349,123},
964        {346,343,345},
965        {343,340,342},
966        {338,336,144},
967        {338,144,141},
968        {327,341,354},
969        {327,354,326},
970        {331,350,321},
971        {331,321,322},
972        {314,313,326},
973        {314,326,325},
974        {300,298,299},
975        {300,299,301},
976        {288,287,289},
977        {189,292,282},
978        {287,288,303},
979        {284,285,297},
980        {368,280,281},
981        {448,447,279},
982        {274,226,255},
983        {267,268,404},
984        {267,404,379},
985        {429,262,430},
986        {439,440,260},
987        {257,258,249},
988        {257,249,246},
989        {430,262,248},
990        {234,228,242},
991        {234,242,241},
992        {237,238,239},
993        {237,239,236},
994        { 15, 18,227},
995        { 15,227,229},
996        {222,223, 82},
997        {222, 82, 83},
998        {214,215,213},
999        {214,213, 81},
1000        { 38,102,  6},
1001        {122,159,200},
1002        {122,200,201},
1003        {174,171,192},
1004        {174,192,194},
1005        {197,193,198},
1006        {190,170,161},
1007        {181,179,178},
1008        {181,178,180},
1009        {166,156,155},
1010        {163,153,152},
1011        {163,152,162},
1012        {120,156,149},
1013        {120,149,121},
1014        {152,153,135},
1015        {140,143,142},
1016        {135,131,132},
1017        {135,132,136},
1018        {130,129,128},
1019        {130,128,127},
1020        {100,105,119},
1021        {100,119,120},
1022        {106,104,107},
1023        {106,107,108},
1024        { 91, 95, 59},
1025        { 93, 94, 68},
1026        { 91, 89, 92},
1027        { 76, 53, 55},
1028        { 76, 55, 87},
1029        { 81, 78, 79},
1030        { 74, 73, 49},
1031        { 69, 60, 45},
1032        { 58, 62, 64},
1033        { 58, 64, 61},
1034        { 53, 31, 32},
1035        { 32, 54, 53},
1036        { 42, 43, 38},
1037        { 35, 36,  0},
1038        { 35,  0,  1},
1039        { 34, 35,  1},
1040        { 34,  1,  9},
1041        { 44, 40, 41},
1042        { 44, 41, 45},
1043        { 33,240, 51},
1044        { 63, 62, 58},
1045        { 63, 58, 59},
1046        { 45, 71, 70},
1047        { 76, 75, 51},
1048        { 76, 51, 52},
1049        { 86, 85, 84},
1050        { 86, 84, 87},
1051        { 89, 72, 73},
1052        { 89, 73, 88},
1053        { 91, 92, 96},
1054        { 91, 96, 95},
1055        { 72, 91, 60},
1056        { 72, 60, 69},
1057        {104,106,105},
1058        {119,105,117},
1059        {119,117,118},
1060        {124,127,128},
1061        {117,116,129},
1062        {117,129,131},
1063        {118,117,131},
1064        {135,140,142},
1065        {146,150,152},
1066        {146,152,145},
1067        {149,122,121},
1068        {166,165,151},
1069        {166,151,156},
1070        {158,172,173},
1071        {161,160,189},
1072        {199,198,193},
1073        {199,193,191},
1074        {204,201,202},
1075        {178,174,194},
1076        {200,159,186},
1077        {109, 48, 67},
1078        { 48,107,104},
1079        {216, 32,236},
1080        {216,236,239},
1081        {223,214, 81},
1082        {223, 81, 82},
1083        { 33, 12, 15},
1084        { 32,228,234},
1085        { 32,234,236},
1086        {240, 31, 52},
1087        {256,255,246},
1088        {256,246,249},
1089        {258,263,248},
1090        {258,248,249},
1091        {275,260,259},
1092        {275,259,276},
1093        {207,276,259},
1094        {270,271,429},
1095        {270,429,407},
1096        {413,418,366},
1097        {413,366,365},
1098        {368,367,279},
1099        {368,279,280},
1100        {303,301,286},
1101        {303,286,287},
1102        {283,282,292},
1103        {283,292,291},
1104        {320,292,189},
1105        {298,296,297},
1106        {298,297,299},
1107        {318,327,326},
1108        {318,326,313},
1109        {329,330,317},
1110        {336,333,320},
1111        {326,354,353},
1112        {334,332,333},
1113        {334,333,336},
1114        {342,339,139},
1115        {342,139,138},
1116        {345,342,126},
1117        {347,357,356},
1118        {369,368,351},
1119        {363,356,357},
1120        {363,357,361},
1121        {366,367,368},
1122        {366,368,369},
1123        {375,373,400},
1124        { 92, 90,377},
1125        {409,387,408},
1126        {386,385,387},
1127        {386,387,388},
1128        {412,394,391},
1129        {396,398,399},
1130        {408,406,405},
1131        {415,421,419},
1132        {415,419,414},
1133        {425,452,448},
1134        {425,448,424},
1135        {444,441,443},
1136        {448,452,449},
1137        {448,449,447},
1138        {446,444,443},
1139        {446,443,445},
1140        {250,247,261},
1141        {250,261,428},
1142        {421,422,423},
1143        {421,423,419},
1144        {427,410,250},
1145        {417,403,401},
1146        {403,402,401},
1147        {420,392,412},
1148        {420,412,425},
1149        {420,425,424},
1150        {386,411,389},
1151        {383,382,381},
1152        {383,381,385},
1153        {378,379,404},
1154        {372,371,395},
1155        {372,395,397},
1156        {371,372,370},
1157        {361,359,360},
1158        {361,360,362},
1159        {368,350,351},
1160        {349,347,348},
1161        {356,355,344},
1162        {356,344,346},
1163        {344,341,340},
1164        {344,340,343},
1165        {338,337,336},
1166        {328,335,341},
1167        {324,352,351},
1168        {324,351,331},
1169        {320,144,336},
1170        {314,325,324},
1171        {322,308,309},
1172        {310,309,307},
1173        {287,286,289},
1174        {203,280,279},
1175        {203,279,205},
1176        {297,295,283},
1177        {297,283,284},
1178        {447,205,279},
1179        {274,384, 80},
1180        {274, 80,226},
1181        {266,267,379},
1182        {266,379,380},
1183        {225,257,246},
1184        {225,246,245},
1185        {256,254,253},
1186        {256,253,255},
1187        {430,247,250},
1188        {226,235,244},
1189        {226,244,245},
1190        {232,233,244},
1191        {232,244,241},
1192        {230, 18, 19},
1193        { 32, 31,228},
1194        {219,220, 86},
1195        {219, 86, 57},
1196        {226,213,235},
1197        {206,  7,  6},
1198        {122,201,101},
1199        {201,204,101},
1200        {180,196,197},
1201        {170,192,171},
1202        {200,190,189},
1203        {194,193,195},
1204        {183,181,180},
1205        {183,180,182},
1206        {155,154,168},
1207        {149,156,151},
1208        {149,151,148},
1209        {155,156,120},
1210        {145,142,143},
1211        {145,143,146},
1212        {136,137,140},
1213        {133,132,130},
1214        {128,129,116},
1215        {100,120,121},
1216        {110,112,113},
1217        {110,113,114},
1218        { 66, 65, 63},
1219        { 66, 63, 99},
1220        { 66, 99, 98},
1221        { 96, 46, 61},
1222        { 89, 88, 90},
1223        { 86, 87, 57},
1224        { 80, 78, 81},
1225        { 72, 69, 49},
1226        { 67, 48, 47},
1227        { 67, 47, 68},
1228        { 56, 55, 53},
1229        { 50, 49, 36},
1230        { 50, 36, 35},
1231        { 40, 39, 41},
1232        {242,243,229},
1233        {242,229,227},
1234        {  6, 37, 39},
1235        { 42, 47, 48},
1236        { 42, 48, 43},
1237        { 61, 46, 44},
1238        { 45, 70, 69},
1239        { 69, 70, 71},
1240        { 69, 71, 49},
1241        { 74, 78, 77},
1242        { 83, 84, 85},
1243        { 73, 74, 77},
1244        { 93, 96, 92},
1245        { 68, 46, 93},
1246        { 95, 99, 63},
1247        { 95, 63, 59},
1248        {115,108,110},
1249        {115,110,114},
1250        {125,126,127},
1251        {129,130,132},
1252        {137,133,138},
1253        {137,138,139},
1254        {148,146,143},
1255        {148,143,147},
1256        {119,118,154},
1257        {161,147,143},
1258        {165,164,151},
1259        {158,157,171},
1260        {158,171,172},
1261        {159,158,187},
1262        {159,187,186},
1263        {194,192,191},
1264        {194,191,193},
1265        {189,202,201},
1266        {182,197,184},
1267        {205,  8,  7},
1268        { 48,109,107},
1269        {218,219, 57},
1270        {218, 57, 56},
1271        {207,231,211},
1272        {232,230,231},
1273        {232,231,233},
1274        { 53, 52, 31},
1275        {388,411,386},
1276        {409,430,250},
1277        {262,429,254},
1278        {262,254,256},
1279        {442,444,428},
1280        {273,264,383},
1281        {273,383,384},
1282        {429,271,251},
1283        {429,251,254},
1284        {413,365,362},
1285        { 67,413,360},
1286        {282,283,295},
1287        {285,301,299},
1288        {202,281,280},
1289        {284,283,291},
1290        {284,291,289},
1291        {320,189,160},
1292        {308,306,307},
1293        {307,309,308},
1294        {319,317,330},
1295        {319,330,328},
1296        {353,352,324},
1297        {332,331,333},
1298        {340,341,338},
1299        {354,341,344},
1300        {349,358,357},
1301        {349,357,347},
1302        {364,355,356},
1303        {364,356,363},
1304        {364,365,366},
1305        {364,366,369},
1306        {374,376,402},
1307        {375, 92,373},
1308        { 77,389,390},
1309        {382,380,381},
1310        {389, 77,386},
1311        {393,394,412},
1312        {393,412,392},
1313        {401,394,416},
1314        {415,400,403},
1315        {411,410,427},
1316        {411,427,426},
1317        {422,420,424},
1318        {247,248,263},
1319        {247,263,261},
1320        {445,443, 14},
1321        {445, 14, 11},
1322        {449,450,  4},
1323        {449,  4,  5},
1324        {443,441, 17},
1325        {443, 17, 14},
1326        {436, 23, 17},
1327        {436, 17,441},
1328        {424,448,422},
1329        {448,423,422},
1330        {414,419,418},
1331        {414,418,413},
1332        {406,404,405},
1333        {399,397,395},
1334        {399,395,396},
1335        {420,416,392},
1336        {388,410,411},
1337        {386,384,383},
1338        {390, 88, 77},
1339        {375, 94, 92},
1340        {415,414, 68},
1341        {415, 68, 94},
1342        {370,374,402},
1343        {370,402,398},
1344        {361,357,358},
1345        {361,358,359},
1346        {125,348,126},
1347        {346,344,343},
1348        {340,338,339},
1349        {337,335,334},
1350        {337,334,336},
1351        {325,353,324},
1352        {324,331,332},
1353        {324,332,329},
1354        {323,322,309},
1355        {323,309,310},
1356        {294,295,297},
1357        {294,297,296},
1358        {289,286,285},
1359        {202,280,203},
1360        {288,307,303},
1361        {282,295,321},
1362        { 67,360,111},
1363        {418,423,367},
1364        {418,367,366},
1365        {272,252,251},
1366        {272,251,271},
1367        {272,271,270},
1368        {255,253,274},
1369        {265,266,380},
1370        {265,380,382},
1371        {442,428,261},
1372        {440,263,258},
1373        {440,258,260},
1374        {409,250,410},
1375        {255,226,245},
1376        {255,245,246},
1377        { 31,240,243},
1378        {236,234,235},
1379        {236,235,237},
1380        {233,225,245},
1381        {233,245,244},
1382        {220,221, 85},
1383        {220, 85, 86},
1384        { 81,213,226},
1385        { 81,226, 80},
1386        {  7,206,205},
1387        {186,184,198},
1388        {186,198,199},
1389        {204,203,205},
1390        {204,205,206},
1391        {195,193,196},
1392        {171,174,172},
1393        {173,174,175},
1394        {173,172,174},
1395        {155,167,166},
1396        {160,161,143},
1397        {160,143,144},
1398        {119,154,155},
1399        {148,151,150},
1400        {148,150,146},
1401        {140,137,139},
1402        {140,139,141},
1403        {127,126,130},
1404        {114,124,128},
1405        {114,128,115},
1406        {117,105,106},
1407        {117,106,116},
1408        {104,105,100},
1409        {104,100,103},
1410        { 59, 60, 91},
1411        { 97, 96, 61},
1412        { 97, 61, 64},
1413        { 91, 72, 89},
1414        { 87, 84, 79},
1415        { 87, 79, 76},
1416        { 78, 80, 77},
1417        { 49, 50, 74},
1418        { 60, 44, 45},
1419        { 61, 44, 58},
1420        { 51, 50, 35},
1421        { 51, 35, 34},
1422        { 39, 37, 41},
1423        { 33, 34,  9},
1424        { 33,  9, 12},
1425        {  0, 36, 37},
1426        {  0, 37,  6},
1427        { 40, 46, 47},
1428        { 40, 47, 42},
1429        { 53, 54, 56},
1430        { 65, 62, 63},
1431        { 72, 49, 73},
1432        { 79, 78, 75},
1433        { 79, 75, 76},
1434        { 52, 53, 76},
1435        { 92, 89, 90},
1436        { 96, 93, 46},
1437        {102,103,100},
1438        {102,100,101},
1439        {116,106,108},
1440        {116,108,115},
1441        {123,125,124},
1442        {116,115,128},
1443        {118,131,135},
1444        {140,135,136},
1445        {148,147,149},
1446        {120,119,155},
1447        {164,162,152},
1448        {164,152,150},
1449        {157,147,161},
1450        {157,161,170},
1451        {186,187,185},
1452        {186,185,184},
1453        {193,197,196},
1454        {202,203,204},
1455        {194,195,178},
1456        {198,184,197},
1457        { 67,111,109},
1458        { 38, 43,103},
1459        { 38,103,102},
1460        {214,223,222},
1461        {214,222,221},
1462        {214,221,220},
1463        {214,220,219},
1464        {214,219,218},
1465        {213,237,235},
1466        {221,222, 83},
1467        {221, 83, 85},
1468        { 15,229, 33},
1469        {227, 18,230},
1470        {227,230,232},
1471        { 52, 51,240},
1472        { 75, 78, 50},
1473        {408,430,409},
1474        {260,258,257},
1475        {260,257,259},
1476        {224,207,259},
1477        {268,269,405},
1478        {268,405,404},
1479        {413,362,360},
1480        {447,  8,205},
1481        {299,297,285},
1482        {189,281,202},
1483        {290,288,289},
1484        {290,289,291},
1485        {322,321,295},
1486        {322,295,294},
1487        {333,323,311},
1488        {333,311,320},
1489        {317,316,329},
1490        {320,160,144},
1491        {353,325,326},
1492        {329,332,334},
1493        {329,334,330},
1494        {339,338,141},
1495        {339,141,139},
1496        {348,345,126},
1497        {347,356,346},
1498        {123,349,125},
1499        {364,353,354},
1500        {364,354,355},
1501        {365,364,363},
1502        {376,391,394},
1503        {376,394,401},
1504        { 92,376,374},
1505        { 92,374,373},
1506        {377, 90, 88},
1507        {380,379,378},
1508        {380,378,381},
1509        {388,387,409},
1510        {388,409,410},
1511        {416,393,392},
1512        {399,398,402},
1513        {399,402,403},
1514        {250,428,427},
1515        {421,417,416},
1516        {421,416,420},
1517        {426,427,446},
1518        {426,446,451},
1519        {444,442,441},
1520        {452,451,450},
1521        {452,450,449}
1522};
1523
1524//============================
1525
1526
1527dReal heightfield_callback( void* pUserData, int x, int z )
1528{
1529        dIASSERT( x < HFIELD_WSTEP );
1530        dIASSERT( z < HFIELD_DSTEP );
1531
1532        dReal fx = ( ((dReal)x) - ( HFIELD_WSTEP-1 )/2 ) / (dReal)( HFIELD_WSTEP-1 );
1533        dReal fz = ( ((dReal)z) - ( HFIELD_DSTEP-1 )/2 ) / (dReal)( HFIELD_DSTEP-1 );
1534
1535        // Create an interesting 'hump' shape
1536        dReal h = REAL( 1.0 ) + ( REAL( -16.0 ) * ( fx*fx*fx + fz*fz*fz ) );
1537
1538        return h;
1539}
1540
1541
1542
1543
1544
1545// this is called by dSpaceCollide when two objects in space are
1546// potentially colliding.
1547
1548static void nearCallback (void *data, dGeomID o1, dGeomID o2)
1549{
1550  int i;
1551  // if (o1->body && o2->body) return;
1552
1553  // exit without doing anything if the two bodies are connected by a joint
1554  dBodyID b1 = dGeomGetBody(o1);
1555  dBodyID b2 = dGeomGetBody(o2);
1556  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
1557
1558  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
1559  for (i=0; i<MAX_CONTACTS; i++) {
1560    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
1561    contact[i].surface.mu = dInfinity;
1562    contact[i].surface.mu2 = 0;
1563    contact[i].surface.bounce = 0.1;
1564    contact[i].surface.bounce_vel = 0.1;
1565    contact[i].surface.soft_cfm = 0.01;
1566  }
1567  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
1568                           sizeof(dContact))) {
1569    dMatrix3 RI;
1570    dRSetIdentity (RI);
1571    const dReal ss[3] = {0.02,0.02,0.02};
1572    for (i=0; i<numc; i++) {
1573      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
1574      dJointAttach (c,b1,b2);
1575      if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
1576    }
1577  }
1578}
1579
1580
1581// start simulation - set viewpoint
1582
1583static void start()
1584{
1585  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
1586  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
1587  dsSetViewpoint (xyz,hpr);
1588  printf ("To drop another object, press:\n");
1589  printf ("   b for box.\n");
1590  printf ("   s for sphere.\n");
1591  printf ("   c for capsule.\n");
1592  printf ("   y for cylinder.\n");
1593  printf ("   v for a convex object.\n");
1594  printf ("   x for a composite object.\n");
1595  printf ("   m for a trimesh.\n");
1596  printf ("To select an object, press space.\n");
1597  printf ("To disable the selected object, press d.\n");
1598  printf ("To enable the selected object, press e.\n");
1599  printf ("To toggle showing the geom AABBs, press a.\n");
1600  printf ("To toggle showing the contact points, press t.\n");
1601  printf ("To toggle dropping from random position/orientation, press r.\n");
1602  printf ("To save the current state to 'state.dif', press 1.\n");
1603}
1604
1605
1606char locase (char c)
1607{
1608  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
1609  else return c;
1610}
1611
1612
1613// called when a key pressed
1614
1615static void command (int cmd)
1616{
1617        size_t i;
1618        int j,k;
1619        dReal sides[3];
1620        dMass m;
1621
1622        cmd = locase (cmd);
1623
1624
1625        //
1626        // Geom Creation
1627        //
1628
1629        if ( cmd == 'b' || cmd == 's' || cmd == 'c' || 
1630                 cmd == 'x' || cmd == 'y' || cmd == 'm' || cmd == 'v' )
1631        {
1632                if ( num < NUM )
1633                {
1634                        i = num;
1635                        num++;
1636                }
1637                else
1638                {
1639                        i = nextobj;
1640                        nextobj++;
1641                        if (nextobj >= num) nextobj = 0;
1642
1643                        // destroy the body and geoms for slot i
1644                        dBodyDestroy (obj[i].body);
1645                        for (k=0; k < GPB; k++)
1646                        {
1647                                if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
1648                        }
1649                        memset (&obj[i],0,sizeof(obj[i]));
1650                }
1651
1652                obj[i].body = dBodyCreate (world);
1653                for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
1654
1655                dMatrix3 R;
1656                if (random_pos) {
1657                        dBodySetPosition (obj[i].body,
1658                                (dRandReal()-0.5)*HFIELD_WIDTH*0.75,
1659                                (dRandReal()-0.5)*HFIELD_DEPTH*0.75,
1660                                dRandReal() + 2 );
1661                        dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1662                                dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1663                }
1664                else {
1665                        dReal maxheight = 0;
1666                        for (k=0; k<num; k++) {
1667                                const dReal *pos = dBodyGetPosition (obj[k].body);
1668                                if (pos[2] > maxheight) maxheight = pos[2];
1669                        }
1670                        dBodySetPosition (obj[i].body, 0,maxheight+1,0);
1671                        dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
1672                }
1673                dBodySetRotation (obj[i].body,R);
1674                dBodySetData (obj[i].body,(void*) i);
1675
1676                if (cmd == 'b')
1677                {
1678                        dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
1679                        obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
1680                }
1681                else if (cmd == 'c')
1682                {
1683                        sides[0] *= 0.5;
1684                        dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
1685                        obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
1686                }
1687                //<---- Convex Object
1688                else if (cmd == 'v')
1689                {
1690                        dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
1691                        obj[i].geom[0] = dCreateConvex (space,
1692                                planes,
1693                                planecount,
1694                                points,
1695                                pointcount,
1696                                polygons);
1697                }
1698                //----> Convex Object
1699                else if (cmd == 'y')
1700                {
1701                        dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
1702                        obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
1703                }
1704                else if (cmd == 's')
1705                {
1706                        sides[0] *= 0.5;
1707                        dMassSetSphere (&m,DENSITY,sides[0]);
1708                        obj[i].geom[0] = dCreateSphere (space,sides[0]);
1709                }
1710#ifdef dTRIMESH_ENABLED
1711                else if (cmd == 'm')
1712                {
1713                        dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
1714                        dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (int*)&Indices[0], IndexCount, 3 * sizeof(int));
1715
1716                        obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
1717
1718                        // remember the mesh's dTriMeshDataID on its userdata for convenience.
1719                        dGeomSetData(obj[i].geom[0], new_tmdata);
1720
1721                        dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
1722                }
1723#endif
1724                else if (cmd == 'x')
1725                {
1726                        dGeomID g2[GPB];                // encapsulated geometries
1727                        dReal dpos[GPB][3];     // delta-positions for encapsulated geometries
1728
1729                        // start accumulating masses for the encapsulated geometries
1730                        dMass m2;
1731                        dMassSetZero (&m);
1732
1733                        // set random delta positions
1734                        for (j=0; j<GPB; j++) {
1735                                for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
1736                        }
1737
1738                        for (k=0; k<GPB; k++) {
1739                                obj[i].geom[k] = dCreateGeomTransform (space);
1740                                dGeomTransformSetCleanup (obj[i].geom[k],1);
1741                                if (k==0) {
1742                                        dReal radius = dRandReal()*0.25+0.05;
1743                                        g2[k] = dCreateSphere (0,radius);
1744                                        dMassSetSphere (&m2,DENSITY,radius);
1745                                }
1746                                else if (k==1) {
1747                                        g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
1748                                        dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
1749                                }
1750                                else {
1751                                        dReal radius = dRandReal()*0.1+0.05;
1752                                        dReal length = dRandReal()*1.0+0.1;
1753                                        g2[k] = dCreateCapsule (0,radius,length);
1754                                        dMassSetCapsule (&m2,DENSITY,3,radius,length);
1755                                }
1756                                dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
1757
1758                                // set the transformation (adjust the mass too)
1759                                dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
1760                                dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
1761                                dMatrix3 Rtx;
1762                                dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1763                                        dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1764                                dGeomSetRotation (g2[k],Rtx);
1765                                dMassRotate (&m2,Rtx);
1766
1767                                // add to the total mass
1768                                dMassAdd (&m,&m2);
1769                        }
1770
1771                        // move all encapsulated objects so that the center of mass is (0,0,0)
1772                        for (k=0; k<2; k++) {
1773                                dGeomSetPosition (g2[k],
1774                                        dpos[k][0]-m.c[0],
1775                                        dpos[k][1]-m.c[1],
1776                                        dpos[k][2]-m.c[2]);
1777                        }
1778                        dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
1779                }
1780
1781                for (k=0; k < GPB; k++)
1782                {
1783                        if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
1784                }
1785
1786                dBodySetMass (obj[i].body,&m);
1787        }
1788
1789
1790        //
1791        // Control Commands
1792        //
1793
1794        if (cmd == ' ') {
1795                selected++;
1796                if (selected >= num) selected = 0;
1797                if (selected < 0) selected = 0;
1798        }
1799        else if (cmd == 'd' && selected >= 0 && selected < num) {
1800                dBodyDisable (obj[selected].body);
1801        }
1802        else if (cmd == 'e' && selected >= 0 && selected < num) {
1803                dBodyEnable (obj[selected].body);
1804        }
1805        else if (cmd == 'a') {
1806                show_aabb ^= 1;
1807        }
1808        else if (cmd == 't') {
1809                show_contacts ^= 1;
1810        }
1811        else if (cmd == 'r') {
1812                random_pos ^= 1;
1813        }
1814        else if (cmd == '1') {
1815                write_world = 1;
1816        }
1817}
1818
1819
1820// draw a geom
1821
1822void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
1823{
1824  int i;
1825
1826  if (!g) return;
1827  if (!pos) pos = dGeomGetPosition (g);
1828  if (!R) R = dGeomGetRotation (g);
1829
1830  int type = dGeomGetClass (g);
1831  if (type == dBoxClass) {
1832    dVector3 sides;
1833    dGeomBoxGetLengths (g,sides);
1834    dsDrawBox (pos,R,sides);
1835  }
1836  else if (type == dSphereClass) {
1837    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
1838  }
1839  else if (type == dCapsuleClass) {
1840    dReal radius,length;
1841    dGeomCapsuleGetParams (g,&radius,&length);
1842    dsDrawCapsule (pos,R,length,radius);
1843  }
1844  //<---- Convex Object
1845  else if (type == dConvexClass)
1846    {
1847      //dVector3 sides={0.50,0.50,0.50};
1848      dsDrawConvex(pos,R,planes,
1849                   planecount,
1850                   points,
1851                   pointcount,
1852                   polygons);
1853    }
1854  //----> Convex Object
1855  else if (type == dCylinderClass) {
1856    dReal radius,length;
1857    dGeomCylinderGetParams (g,&radius,&length);
1858    dsDrawCylinder (pos,R,length,radius);
1859  }
1860  else if (type == dGeomTransformClass) {
1861    dGeomID g2 = dGeomTransformGetGeom (g);
1862    const dReal *pos2 = dGeomGetPosition (g2);
1863    const dReal *R2 = dGeomGetRotation (g2);
1864    dVector3 actual_pos;
1865    dMatrix3 actual_R;
1866    dMULTIPLY0_331 (actual_pos,R,pos2);
1867    actual_pos[0] += pos[0];
1868    actual_pos[1] += pos[1];
1869    actual_pos[2] += pos[2];
1870    dMULTIPLY0_333 (actual_R,R,R2);
1871    drawGeom (g2,actual_pos,actual_R,0);
1872  }
1873
1874  if (show_aabb) {
1875    // draw the bounding box for this geom
1876    dReal aabb[6];
1877    dGeomGetAABB (g,aabb);
1878    dVector3 bbpos;
1879    for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1880    dVector3 bbsides;
1881    for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1882    dMatrix3 RI;
1883    dRSetIdentity (RI);
1884    dsSetColorAlpha (1,0,0,0.5);
1885    dsDrawBox (bbpos,RI,bbsides);
1886  }
1887
1888}
1889
1890// simulation loop
1891
1892static void simLoop (int pause)
1893{
1894  int i,j;
1895 
1896  dsSetColor (0,0,2);
1897 
1898  dSpaceCollide (space,0,&nearCallback);
1899 
1900  //if (!pause) dWorldStep (world,0.05);
1901  //if (!pause) dWorldQuickStep (world,0.05);
1902  if (!pause) dWorldStepFast1 (world,0.05, 5);
1903
1904
1905  if (write_world) {
1906    FILE *f = fopen ("state.dif","wt");
1907    if (f) {
1908      dWorldExportDIF (world,f,"X");
1909      fclose (f);
1910    }
1911    write_world = 0;
1912  }
1913
1914  // remove all contact joints
1915  dJointGroupEmpty (contactgroup);
1916
1917
1918
1919        const dReal* pReal = dGeomGetPosition( gheight );
1920
1921        const dReal* RReal = dGeomGetRotation( gheight );
1922
1923        //
1924        // Draw Heightfield
1925        //
1926
1927        // Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
1928        int ox = (int) ( -HFIELD_WIDTH/2 );
1929        int oz = (int) ( -HFIELD_DEPTH/2 );
1930
1931//      for ( int tx = -1; tx < 2; ++tx )
1932//      for ( int tz = -1; tz < 2; ++tz )
1933        {
1934                dsSetColorAlpha (0.5,1,0.5,0.5);
1935                dsSetTexture( DS_WOOD );
1936
1937                for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
1938                for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
1939                {
1940                        dReal a[3], b[3], c[3], d[3];
1941
1942                        a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
1943                        a[ 1 ] = heightfield_callback( NULL, i, j );
1944                        a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
1945
1946                        b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
1947                        b[ 1 ] = heightfield_callback( NULL, i + 1, j );
1948                        b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
1949
1950                        c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
1951                        c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
1952                        c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
1953
1954                        d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
1955                        d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
1956                        d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
1957
1958                        dsDrawTriangle( pReal, RReal, a, c, b, 1 );
1959                        dsDrawTriangle( pReal, RReal, b, c, d, 1 );
1960                }
1961        }
1962
1963
1964
1965
1966
1967        dsSetColor (1,1,0);
1968        dsSetTexture (DS_WOOD);
1969        for (i=0; i<num; i++)
1970        {
1971                for (j=0; j < GPB; j++)
1972                {
1973                        if (i==selected)
1974                        {
1975                                dsSetColor (0,0.7,1);
1976                        }
1977                        else if (! dBodyIsEnabled (obj[i].body))
1978                        {
1979                                dsSetColor (1,0.8,0);
1980                        }
1981                        else 
1982                        {
1983                                dsSetColor (1,1,0);
1984                        }
1985
1986
1987                        if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
1988                        {
1989                                int* Indices = (int*)::Indices;
1990
1991                                // assume all trimeshes are drawn as bunnies
1992                                const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
1993                                const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
1994
1995                                for (int ii = 0; ii < IndexCount / 3; ii++)
1996                                {
1997                                        const dReal v[9] = { // explicit conversion from float to dReal
1998                                                Vertices[Indices[ii * 3 + 0] * 3 + 0],
1999                                                        Vertices[Indices[ii * 3 + 0] * 3 + 1],
2000                                                        Vertices[Indices[ii * 3 + 0] * 3 + 2],
2001                                                        Vertices[Indices[ii * 3 + 1] * 3 + 0],
2002                                                        Vertices[Indices[ii * 3 + 1] * 3 + 1],
2003                                                        Vertices[Indices[ii * 3 + 1] * 3 + 2],
2004                                                        Vertices[Indices[ii * 3 + 2] * 3 + 0],
2005                                                        Vertices[Indices[ii * 3 + 2] * 3 + 1],
2006                                                        Vertices[Indices[ii * 3 + 2] * 3 + 2]
2007                                        };
2008                                        dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
2009                                }
2010
2011                                // tell the tri-tri collider the current transform of the trimesh --
2012                                // this is fairly important for good results.
2013
2014                                // Fill in the (4x4) matrix.
2015                                dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
2016
2017                                p_matrix[ 0 ] = Rot[ 0 ];       p_matrix[ 1 ] = Rot[ 1 ];       p_matrix[ 2 ] = Rot[ 2 ];       p_matrix[ 3 ] = 0;
2018                                p_matrix[ 4 ] = Rot[ 4 ];       p_matrix[ 5 ] = Rot[ 5 ];       p_matrix[ 6 ] = Rot[ 6 ];       p_matrix[ 7 ] = 0;
2019                                p_matrix[ 8 ] = Rot[ 8 ];       p_matrix[ 9 ] = Rot[ 9 ];       p_matrix[10 ] = Rot[10 ];       p_matrix[11 ] = 0;
2020                                p_matrix[12 ] = Pos[ 0 ];       p_matrix[13 ] = Pos[ 1 ];       p_matrix[14 ] = Pos[ 2 ];       p_matrix[15 ] = 1;
2021
2022                                // Flip to other matrix.
2023                                obj[i].last_matrix_index = !obj[i].last_matrix_index;
2024
2025                                // Apply the 'other' matrix which is the oldest.
2026#ifdef dTRIMESH_ENABLED
2027                                dGeomTriMeshSetLastTransform( obj[i].geom[j], 
2028                                        *(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
2029#endif
2030                        }
2031                        else
2032                        {
2033                                drawGeom (obj[i].geom[j],0,0,show_aabb);
2034                        }
2035                }
2036        }
2037
2038        if ( show_aabb )
2039        {
2040                // draw the bounding box for this geom
2041                dReal aabb[6];
2042                dGeomGetAABB (gheight,aabb);
2043                dVector3 bbpos;
2044                for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
2045                dVector3 bbsides;
2046                for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
2047                dMatrix3 RI;
2048                dRSetIdentity (RI);
2049                dsSetColorAlpha (1,0,0,0.5);
2050                dsDrawBox (bbpos,RI,bbsides);
2051        }
2052}
2053
2054
2055int main (int argc, char **argv)
2056{
2057        // setup pointers to drawstuff callback functions
2058        dsFunctions fn;
2059        fn.version = DS_VERSION;
2060        fn.start = &start;
2061        fn.step = &simLoop;
2062        fn.command = &command;
2063        fn.stop = 0;
2064        fn.path_to_textures = "../../drawstuff/textures";
2065        if(argc==2)
2066        {
2067                fn.path_to_textures = argv[1];
2068        }
2069
2070        // create world
2071        dInitODE();
2072        world = dWorldCreate();
2073        space = dHashSpaceCreate (0);
2074        contactgroup = dJointGroupCreate (0);
2075        dWorldSetGravity (world,0,0,-0.05);
2076        dWorldSetCFM (world,1e-5);
2077        dWorldSetAutoDisableFlag (world,1);
2078        dWorldSetContactMaxCorrectingVel (world,0.1);
2079        dWorldSetContactSurfaceLayer (world,0.001);
2080        memset (obj,0,sizeof(obj));
2081
2082#if 1
2083
2084  dWorldSetAutoDisableAverageSamplesCount( world, 1 );
2085
2086#endif
2087
2088        // base plane to catch overspill
2089        dCreatePlane( space, 0, 0, 1, 0 );
2090
2091
2092        // our heightfield floor
2093
2094        dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
2095
2096        // Create an finite heightfield.
2097        dGeomHeightfieldDataBuildCallback( heightid, NULL, heightfield_callback,
2098                HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
2099                REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );
2100
2101        // Give some very bounds which, while conservative,
2102        // makes AABB computation more accurate than +/-INF.
2103        dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );
2104
2105        gheight = dCreateHeightfield( space, heightid, 1 );
2106
2107        dVector3 pos;
2108        pos[ 0 ] = 0;
2109        pos[ 1 ] = 0;
2110        pos[ 2 ] = 0;
2111
2112        // Rotate so Z is up, not Y (which is the default orientation)
2113        dMatrix3 R;
2114        dRSetIdentity( R );
2115        dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
2116
2117        // Place it.
2118        dGeomSetRotation( gheight, R );
2119        dGeomSetPosition( gheight, pos[0], pos[1], pos[2] );
2120
2121
2122
2123
2124        // run simulation
2125        dsSimulationLoop (argc,argv,352,288,&fn);
2126
2127        dJointGroupDestroy (contactgroup);
2128        dSpaceDestroy (space);
2129        dWorldDestroy (world);
2130        dCloseODE();
2131        return 0;
2132}
Note: See TracBrowser for help on using the repository browser.