Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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