aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/demo')
-rw-r--r--libraries/ode-0.9/ode/demo/Makefile.am254
-rw-r--r--libraries/ode-0.9/ode/demo/Makefile.in953
-rw-r--r--libraries/ode-0.9/ode/demo/basket_geom.h13
-rw-r--r--libraries/ode-0.9/ode/demo/demo_I.cpp254
-rw-r--r--libraries/ode-0.9/ode/demo/demo_basket.cpp278
-rw-r--r--libraries/ode-0.9/ode/demo/demo_boxstack.cpp577
-rw-r--r--libraries/ode-0.9/ode/demo/demo_buggy.cpp325
-rw-r--r--libraries/ode-0.9/ode/demo/demo_chain1.c171
-rw-r--r--libraries/ode-0.9/ode/demo/demo_chain2.cpp165
-rw-r--r--libraries/ode-0.9/ode/demo/demo_collision.cpp1371
-rw-r--r--libraries/ode-0.9/ode/demo/demo_convex_cd.cpp195
-rw-r--r--libraries/ode-0.9/ode/demo/demo_crash.cpp635
-rw-r--r--libraries/ode-0.9/ode/demo/demo_cyl.cpp318
-rw-r--r--libraries/ode-0.9/ode/demo/demo_cylvssphere.cpp238
-rw-r--r--libraries/ode-0.9/ode/demo/demo_feedback.cpp314
-rw-r--r--libraries/ode-0.9/ode/demo/demo_friction.cpp206
-rw-r--r--libraries/ode-0.9/ode/demo/demo_heightfield.cpp2132
-rw-r--r--libraries/ode-0.9/ode/demo/demo_hinge.cpp166
-rw-r--r--libraries/ode-0.9/ode/demo/demo_jointPR.cpp377
-rw-r--r--libraries/ode-0.9/ode/demo/demo_joints.cpp1092
-rw-r--r--libraries/ode-0.9/ode/demo/demo_motor.cpp210
-rw-r--r--libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp1944
-rw-r--r--libraries/ode-0.9/ode/demo/demo_ode.cpp1128
-rw-r--r--libraries/ode-0.9/ode/demo/demo_plane2d.cpp268
-rw-r--r--libraries/ode-0.9/ode/demo/demo_slider.cpp172
-rw-r--r--libraries/ode-0.9/ode/demo/demo_space.cpp233
-rw-r--r--libraries/ode-0.9/ode/demo/demo_space_stress.cpp435
-rw-r--r--libraries/ode-0.9/ode/demo/demo_step.cpp193
-rw-r--r--libraries/ode-0.9/ode/demo/demo_trimesh.cpp537
-rw-r--r--libraries/ode-0.9/ode/demo/world_geom3.h9
30 files changed, 15163 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/demo/Makefile.am b/libraries/ode-0.9/ode/demo/Makefile.am
new file mode 100644
index 0000000..361ad53
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/Makefile.am
@@ -0,0 +1,254 @@
1AM_CXXFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
2AM_CFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
3
4noinst_PROGRAMS=demo_collision \
5 demo_slider \
6 demo_feedback \
7 demo_crash \
8 demo_space \
9 demo_I \
10 demo_friction \
11 demo_space_stress \
12 demo_boxstack demo_hinge \
13 demo_step \
14 demo_buggy \
15 demo_joints \
16 demo_motor \
17 demo_chain1 \
18 demo_chain2 \
19 demo_cylvssphere \
20 demo_ode \
21 demo_plane2d \
22 demo_heightfield \
23 demo_convex_cd \
24 demo_jointPR
25if TRIMESH
26noinst_PROGRAMS+= demo_trimesh demo_moving_trimesh demo_basket demo_cyl
27endif
28demo_collision_SOURCES= demo_collision.cpp
29demo_slider_SOURCES= demo_slider.cpp
30demo_feedback_SOURCES= demo_feedback.cpp
31demo_crash_SOURCES= demo_crash.cpp
32demo_space_SOURCES= demo_space.cpp
33demo_I_SOURCES= demo_I.cpp
34demo_friction_SOURCES= demo_friction.cpp
35demo_space_stress_SOURCES= demo_space_stress.cpp
36demo_boxstack_SOURCES= demo_boxstack.cpp
37demo_hinge_SOURCES= demo_hinge.cpp
38demo_step_SOURCES= demo_step.cpp
39demo_buggy_SOURCES= demo_buggy.cpp
40demo_cyl_SOURCES= demo_cyl.cpp world_geom3.h
41demo_cylvssphere_SOURCES= demo_cylvssphere.cpp
42demo_joints_SOURCES= demo_joints.cpp
43demo_jointPR_SOURCES= demo_jointPR.cpp
44demo_motor_SOURCES= demo_motor.cpp
45demo_chain1_SOURCES= demo_chain1.c
46demo_chain2_SOURCES= demo_chain2.cpp
47demo_ode_SOURCES= demo_ode.cpp
48demo_plane2d_SOURCES= demo_plane2d.cpp
49demo_heightfield_SOURCES= demo_heightfield.cpp
50demo_convex_cd_SOURCES= demo_convex_cd.cpp
51demo_collision_LDFLAGS= -L$(top_builddir)/drawstuff/src \
52 -L$(top_builddir)/ode/src @LDFLAGS@
53demo_slider_LDFLAGS= -L$(top_builddir)/drawstuff/src \
54 -L$(top_builddir)/ode/src @LDFLAGS@
55demo_feedback_LDFLAGS= -L$(top_builddir)/drawstuff/src \
56 -L$(top_builddir)/ode/src @LDFLAGS@
57demo_crash_LDFLAGS= -L$(top_builddir)/drawstuff/src \
58 -L$(top_builddir)/ode/src @LDFLAGS@
59demo_space_LDFLAGS= -L$(top_builddir)/drawstuff/src \
60 -L$(top_builddir)/ode/src @LDFLAGS@
61demo_I_LDFLAGS= -L$(top_builddir)/drawstuff/src \
62 -L$(top_builddir)/ode/src @LDFLAGS@
63demo_friction_LDFLAGS= -L$(top_builddir)/drawstuff/src \
64 -L$(top_builddir)/ode/src @LDFLAGS@
65demo_space_stress_LDFLAGS= -L$(top_builddir)/drawstuff/src \
66 -L$(top_builddir)/ode/src @LDFLAGS@
67demo_boxstack_LDFLAGS= -L$(top_builddir)/drawstuff/src \
68 -L$(top_builddir)/ode/src @LDFLAGS@
69demo_hinge_LDFLAGS= -L$(top_builddir)/drawstuff/src \
70 -L$(top_builddir)/ode/src @LDFLAGS@
71demo_step_LDFLAGS= -L$(top_builddir)/drawstuff/src \
72 -L$(top_builddir)/ode/src @LDFLAGS@
73demo_buggy_LDFLAGS= -L$(top_builddir)/drawstuff/src \
74 -L$(top_builddir)/ode/src @LDFLAGS@
75demo_cyl_LDFLAGS= -L$(top_builddir)/drawstuff/src \
76 -L$(top_builddir)/ode/src @LDFLAGS@
77demo_cylvssphere_LDFLAGS= -L$(top_builddir)/drawstuff/src \
78 -L$(top_builddir)/ode/src @LDFLAGS@
79demo_joints_LDFLAGS= -L$(top_builddir)/drawstuff/src \
80 -L$(top_builddir)/ode/src @LDFLAGS@
81demo_jointPR_LDFLAGS= -L$(top_builddir)/drawstuff/src \
82 -L$(top_builddir)/ode/src @LDFLAGS@
83demo_motor_LDFLAGS= -L$(top_builddir)/drawstuff/src \
84 -L$(top_builddir)/ode/src @LDFLAGS@
85demo_chain1_LDFLAGS= -L$(top_builddir)/drawstuff/src \
86 -L$(top_builddir)/ode/src @LDFLAGS@
87demo_chain2_LDFLAGS= -L$(top_builddir)/drawstuff/src \
88 -L$(top_builddir)/ode/src @LDFLAGS@
89demo_ode_LDFLAGS= -L$(top_builddir)/drawstuff/src \
90 -L$(top_builddir)/ode/src @LDFLAGS@
91demo_plane2d_LDFLAGS= -L$(top_builddir)/drawstuff/src \
92 -L$(top_builddir)/ode/src @LDFLAGS@
93demo_heightfield_LDFLAGS= -L$(top_builddir)/drawstuff/src \
94 -L$(top_builddir)/ode/src @LDFLAGS@
95demo_convex_cd_LDFLAGS= -L$(top_builddir)/drawstuff/src \
96 -L$(top_builddir)/ode/src @LDFLAGS@
97
98demo_collision_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
99 $(top_builddir)/drawstuff/src/libdrawstuff.a
100demo_slider_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
101 $(top_builddir)/drawstuff/src/libdrawstuff.a
102demo_feedback_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
103 $(top_builddir)/drawstuff/src/libdrawstuff.a
104demo_crash_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
105 $(top_builddir)/drawstuff/src/libdrawstuff.a
106demo_space_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
107 $(top_builddir)/drawstuff/src/libdrawstuff.a
108demo_I_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
109 $(top_builddir)/drawstuff/src/libdrawstuff.a
110demo_friction_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
111 $(top_builddir)/drawstuff/src/libdrawstuff.a
112demo_space_stress_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
113 $(top_builddir)/drawstuff/src/libdrawstuff.a
114demo_boxstack_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
115 $(top_builddir)/drawstuff/src/libdrawstuff.a
116demo_hinge_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
117 $(top_builddir)/drawstuff/src/libdrawstuff.a
118demo_step_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
119 $(top_builddir)/drawstuff/src/libdrawstuff.a
120demo_buggy_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
121 $(top_builddir)/drawstuff/src/libdrawstuff.a
122demo_cyl_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
123 $(top_builddir)/drawstuff/src/libdrawstuff.a
124demo_cylvssphere_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
125 $(top_builddir)/drawstuff/src/libdrawstuff.a
126demo_joints_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
127 $(top_builddir)/drawstuff/src/libdrawstuff.a
128demo_jointPR_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
129 $(top_builddir)/drawstuff/src/libdrawstuff.a
130demo_motor_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
131 $(top_builddir)/drawstuff/src/libdrawstuff.a
132demo_chain1_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
133 $(top_builddir)/drawstuff/src/libdrawstuff.a
134demo_chain2_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
135 $(top_builddir)/drawstuff/src/libdrawstuff.a
136demo_ode_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
137 $(top_builddir)/drawstuff/src/libdrawstuff.a
138demo_plane2d_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
139 $(top_builddir)/drawstuff/src/libdrawstuff.a
140demo_heightfield_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
141 $(top_builddir)/drawstuff/src/libdrawstuff.a
142demo_convex_cd_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
143 $(top_builddir)/drawstuff/src/libdrawstuff.a
144
145if TRIMESH
146demo_trimesh_SOURCES= demo_trimesh.cpp
147demo_moving_trimesh_SOURCES= demo_moving_trimesh.cpp
148demo_basket_SOURCES= demo_basket.cpp
149demo_trimesh_LDFLAGS= -L$(top_builddir)/drawstuff/src \
150 -L$(top_builddir)/ode/src @LDFLAGS@ \
151 @GL_LIBS@ @LIBS@
152demo_moving_trimesh_LDFLAGS= -L$(top_builddir)/drawstuff/src \
153 -L$(top_builddir)/ode/src @LDFLAGS@ \
154 @GL_LIBS@ @LIBS@
155demo_basket_LDFLAGS= -L$(top_builddir)/drawstuff/src \
156 -L$(top_builddir)/ode/src @LDFLAGS@ \
157 @GL_LIBS@ @LIBS@
158demo_trimesh_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
159 $(top_builddir)/drawstuff/src/libdrawstuff.a
160demo_moving_trimesh_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
161 $(top_builddir)/drawstuff/src/libdrawstuff.a
162
163demo_basket_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \
164 $(top_builddir)/drawstuff/src/libdrawstuff.a
165
166endif
167
168demo_ode_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
169demo_plane2d_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
170demo_heightfield_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
171demo_chain2_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
172demo_chain1_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
173demo_joints_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
174demo_jointPR_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
175demo_motor_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
176demo_buggy_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
177demo_cyl_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
178demo_cylvssphere_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
179demo_step_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
180demo_hinge_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
181demo_boxstack_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
182demo_space_stress_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
183demo_friction_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
184demo_I_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
185demo_space_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
186demo_crash_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
187demo_slider_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
188demo_feedback_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
189demo_collision_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
190demo_convex_cd_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
191
192
193if TRIMESH
194demo_trimesh_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
195demo_moving_trimesh_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
196demo_basket_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
197endif
198
199
200if WIN32
201resources.o: ../../drawstuff/src/resources.rc ../../drawstuff/src/resource.h
202 @WINDRES@ ../../drawstuff/src/resources.rc -o resources.o
203demo_ode_LDADD+= resources.o
204demo_heightfield_LDADD+= resources.o
205demo_chain2_LDADD+= resources.o
206demo_chain1_LDADD+= resources.o
207demo_joints_LDADD+= resources.o
208demo_jointPR_LDADD+= resources.o
209demo_motor_LDADD+= resources.o
210demo_buggy_LDADD+= resources.o
211demo_cyl_LDADD+= resources.o
212demo_cylvssphere_LDADD+= resources.o
213demo_step_LDADD+= resources.o
214demo_hinge_LDADD+= resources.o
215demo_boxstack_LDADD+= resources.o
216demo_space_stress_LDADD+= resources.o
217demo_friction_LDADD+= resources.o
218demo_I_LDADD+= resources.o
219demo_space_LDADD+= resources.o
220demo_crash_LDADD+= resources.o
221demo_slider_LDADD+= resources.o
222demo_feedback_LDADD+= resources.o
223demo_collision_LDADD+= resources.o
224demo_convex_cd_LDADD+= resources.o
225demo_ode_DEPENDENCIES+= resources.o
226demo_chain2_DEPENDENCIES+= resources.o
227demo_chain1_DEPENDENCIES+= resources.o
228demo_joints_DEPENDENCIES+= resources.o
229demo_jointPR_DEPENDENCIES+= resources.o
230demo_motor_DEPENDENCIES+= resources.o
231demo_buggy_DEPENDENCIES+= resources.o
232demo_cyl_DEPENDENCIES+= resources.o
233demo_cylvssphere_DEPENDENCIES+= resources.o
234demo_step_DEPENDENCIES+= resources.o
235demo_hinge_DEPENDENCIES+= resources.o
236demo_boxstack_DEPENDENCIES+= resources.o
237demo_space_stress_DEPENDENCIES+= resources.o
238demo_friction_DEPENDENCIES+= resources.o
239demo_I_DEPENDENCIES+= resources.o
240demo_space_DEPENDENCIES+= resources.o
241demo_crash_DEPENDENCIES+= resources.o
242demo_slider_DEPENDENCIES+= resources.o
243demo_feedback_DEPENDENCIES+= resources.o
244demo_collision_DEPENDENCIES+= resources.o
245demo_convex_cd_DEPENDENCIES+= resources.o
246
247if TRIMESH
248demo_trimesh_LDADD+= resources.o
249demo_moving_trimesh_LDADD+= resources.o
250demo_trimesh_DEPENDENCIES+= resources.o
251demo_moving_trimesh_DEPENDENCIES+= resources.o
252demo_basket_DEPENDENCIES+= resources.o
253endif
254endif
diff --git a/libraries/ode-0.9/ode/demo/Makefile.in b/libraries/ode-0.9/ode/demo/Makefile.in
new file mode 100644
index 0000000..8e73f18
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/Makefile.in
@@ -0,0 +1,953 @@
1# Makefile.in generated by automake 1.10 from Makefile.am.
2# @configure_input@
3
4# Copyright (C) 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002,
5# 2003, 2004, 2005, 2006 Free Software Foundation, Inc.
6# This Makefile.in is free software; the Free Software Foundation
7# gives unlimited permission to copy and/or distribute it,
8# with or without modifications, as long as this notice is preserved.
9
10# This program is distributed in the hope that it will be useful,
11# but WITHOUT ANY WARRANTY, to the extent permitted by law; without
12# even the implied warranty of MERCHANTABILITY or FITNESS FOR A
13# PARTICULAR PURPOSE.
14
15@SET_MAKE@
16
17VPATH = @srcdir@
18pkgdatadir = $(datadir)/@PACKAGE@
19pkglibdir = $(libdir)/@PACKAGE@
20pkgincludedir = $(includedir)/@PACKAGE@
21am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd
22install_sh_DATA = $(install_sh) -c -m 644
23install_sh_PROGRAM = $(install_sh) -c
24install_sh_SCRIPT = $(install_sh) -c
25INSTALL_HEADER = $(INSTALL_DATA)
26transform = $(program_transform_name)
27NORMAL_INSTALL = :
28PRE_INSTALL = :
29POST_INSTALL = :
30NORMAL_UNINSTALL = :
31PRE_UNINSTALL = :
32POST_UNINSTALL = :
33build_triplet = @build@
34host_triplet = @host@
35target_triplet = @target@
36noinst_PROGRAMS = demo_collision$(EXEEXT) demo_slider$(EXEEXT) \
37 demo_feedback$(EXEEXT) demo_crash$(EXEEXT) demo_space$(EXEEXT) \
38 demo_I$(EXEEXT) demo_friction$(EXEEXT) \
39 demo_space_stress$(EXEEXT) demo_boxstack$(EXEEXT) \
40 demo_hinge$(EXEEXT) demo_step$(EXEEXT) demo_buggy$(EXEEXT) \
41 demo_joints$(EXEEXT) demo_motor$(EXEEXT) demo_chain1$(EXEEXT) \
42 demo_chain2$(EXEEXT) demo_cylvssphere$(EXEEXT) \
43 demo_ode$(EXEEXT) demo_plane2d$(EXEEXT) \
44 demo_heightfield$(EXEEXT) demo_convex_cd$(EXEEXT) \
45 demo_jointPR$(EXEEXT) $(am__EXEEXT_1)
46@TRIMESH_TRUE@am__append_1 = demo_trimesh demo_moving_trimesh demo_basket demo_cyl
47@WIN32_TRUE@am__append_2 = resources.o
48@WIN32_TRUE@am__append_3 = resources.o
49@WIN32_TRUE@am__append_4 = resources.o
50@WIN32_TRUE@am__append_5 = resources.o
51@WIN32_TRUE@am__append_6 = resources.o
52@WIN32_TRUE@am__append_7 = resources.o
53@WIN32_TRUE@am__append_8 = resources.o
54@WIN32_TRUE@am__append_9 = resources.o
55@WIN32_TRUE@am__append_10 = resources.o
56@WIN32_TRUE@am__append_11 = resources.o
57@WIN32_TRUE@am__append_12 = resources.o
58@WIN32_TRUE@am__append_13 = resources.o
59@WIN32_TRUE@am__append_14 = resources.o
60@WIN32_TRUE@am__append_15 = resources.o
61@WIN32_TRUE@am__append_16 = resources.o
62@WIN32_TRUE@am__append_17 = resources.o
63@WIN32_TRUE@am__append_18 = resources.o
64@WIN32_TRUE@am__append_19 = resources.o
65@WIN32_TRUE@am__append_20 = resources.o
66@WIN32_TRUE@am__append_21 = resources.o
67@WIN32_TRUE@am__append_22 = resources.o
68@WIN32_TRUE@am__append_23 = resources.o
69@WIN32_TRUE@am__append_24 = resources.o
70@WIN32_TRUE@am__append_25 = resources.o
71@WIN32_TRUE@am__append_26 = resources.o
72@WIN32_TRUE@am__append_27 = resources.o
73@WIN32_TRUE@am__append_28 = resources.o
74@WIN32_TRUE@am__append_29 = resources.o
75@WIN32_TRUE@am__append_30 = resources.o
76@WIN32_TRUE@am__append_31 = resources.o
77@WIN32_TRUE@am__append_32 = resources.o
78@WIN32_TRUE@am__append_33 = resources.o
79@WIN32_TRUE@am__append_34 = resources.o
80@WIN32_TRUE@am__append_35 = resources.o
81@WIN32_TRUE@am__append_36 = resources.o
82@WIN32_TRUE@am__append_37 = resources.o
83@WIN32_TRUE@am__append_38 = resources.o
84@WIN32_TRUE@am__append_39 = resources.o
85@WIN32_TRUE@am__append_40 = resources.o
86@WIN32_TRUE@am__append_41 = resources.o
87@WIN32_TRUE@am__append_42 = resources.o
88@WIN32_TRUE@am__append_43 = resources.o
89@WIN32_TRUE@am__append_44 = resources.o
90@TRIMESH_TRUE@@WIN32_TRUE@am__append_45 = resources.o
91@TRIMESH_TRUE@@WIN32_TRUE@am__append_46 = resources.o
92@TRIMESH_TRUE@@WIN32_TRUE@am__append_47 = resources.o
93@TRIMESH_TRUE@@WIN32_TRUE@am__append_48 = resources.o
94@TRIMESH_TRUE@@WIN32_TRUE@am__append_49 = resources.o
95subdir = ode/demo
96DIST_COMMON = $(srcdir)/Makefile.am $(srcdir)/Makefile.in
97ACLOCAL_M4 = $(top_srcdir)/aclocal.m4
98am__aclocal_m4_deps = $(top_srcdir)/configure.in
99am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \
100 $(ACLOCAL_M4)
101mkinstalldirs = $(install_sh) -d
102CONFIG_HEADER = $(top_builddir)/include/ode/config.h
103CONFIG_CLEAN_FILES =
104@TRIMESH_TRUE@am__EXEEXT_1 = demo_trimesh$(EXEEXT) \
105@TRIMESH_TRUE@ demo_moving_trimesh$(EXEEXT) \
106@TRIMESH_TRUE@ demo_basket$(EXEEXT) demo_cyl$(EXEEXT)
107PROGRAMS = $(noinst_PROGRAMS)
108am_demo_I_OBJECTS = demo_I.$(OBJEXT)
109demo_I_OBJECTS = $(am_demo_I_OBJECTS)
110demo_I_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) $(demo_I_LDFLAGS) \
111 $(LDFLAGS) -o $@
112am__demo_basket_SOURCES_DIST = demo_basket.cpp
113@TRIMESH_TRUE@am_demo_basket_OBJECTS = demo_basket.$(OBJEXT)
114demo_basket_OBJECTS = $(am_demo_basket_OBJECTS)
115demo_basket_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
116 $(demo_basket_LDFLAGS) $(LDFLAGS) -o $@
117am_demo_boxstack_OBJECTS = demo_boxstack.$(OBJEXT)
118demo_boxstack_OBJECTS = $(am_demo_boxstack_OBJECTS)
119demo_boxstack_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
120 $(demo_boxstack_LDFLAGS) $(LDFLAGS) -o $@
121am_demo_buggy_OBJECTS = demo_buggy.$(OBJEXT)
122demo_buggy_OBJECTS = $(am_demo_buggy_OBJECTS)
123demo_buggy_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
124 $(demo_buggy_LDFLAGS) $(LDFLAGS) -o $@
125am_demo_chain1_OBJECTS = demo_chain1.$(OBJEXT)
126demo_chain1_OBJECTS = $(am_demo_chain1_OBJECTS)
127demo_chain1_LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) \
128 $(demo_chain1_LDFLAGS) $(LDFLAGS) -o $@
129am_demo_chain2_OBJECTS = demo_chain2.$(OBJEXT)
130demo_chain2_OBJECTS = $(am_demo_chain2_OBJECTS)
131demo_chain2_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
132 $(demo_chain2_LDFLAGS) $(LDFLAGS) -o $@
133am_demo_collision_OBJECTS = demo_collision.$(OBJEXT)
134demo_collision_OBJECTS = $(am_demo_collision_OBJECTS)
135demo_collision_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
136 $(demo_collision_LDFLAGS) $(LDFLAGS) -o $@
137am_demo_convex_cd_OBJECTS = demo_convex_cd.$(OBJEXT)
138demo_convex_cd_OBJECTS = $(am_demo_convex_cd_OBJECTS)
139demo_convex_cd_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
140 $(demo_convex_cd_LDFLAGS) $(LDFLAGS) -o $@
141am_demo_crash_OBJECTS = demo_crash.$(OBJEXT)
142demo_crash_OBJECTS = $(am_demo_crash_OBJECTS)
143demo_crash_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
144 $(demo_crash_LDFLAGS) $(LDFLAGS) -o $@
145am_demo_cyl_OBJECTS = demo_cyl.$(OBJEXT)
146demo_cyl_OBJECTS = $(am_demo_cyl_OBJECTS)
147demo_cyl_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
148 $(demo_cyl_LDFLAGS) $(LDFLAGS) -o $@
149am_demo_cylvssphere_OBJECTS = demo_cylvssphere.$(OBJEXT)
150demo_cylvssphere_OBJECTS = $(am_demo_cylvssphere_OBJECTS)
151demo_cylvssphere_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
152 $(demo_cylvssphere_LDFLAGS) $(LDFLAGS) -o $@
153am_demo_feedback_OBJECTS = demo_feedback.$(OBJEXT)
154demo_feedback_OBJECTS = $(am_demo_feedback_OBJECTS)
155demo_feedback_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
156 $(demo_feedback_LDFLAGS) $(LDFLAGS) -o $@
157am_demo_friction_OBJECTS = demo_friction.$(OBJEXT)
158demo_friction_OBJECTS = $(am_demo_friction_OBJECTS)
159demo_friction_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
160 $(demo_friction_LDFLAGS) $(LDFLAGS) -o $@
161am_demo_heightfield_OBJECTS = demo_heightfield.$(OBJEXT)
162demo_heightfield_OBJECTS = $(am_demo_heightfield_OBJECTS)
163demo_heightfield_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
164 $(demo_heightfield_LDFLAGS) $(LDFLAGS) -o $@
165am_demo_hinge_OBJECTS = demo_hinge.$(OBJEXT)
166demo_hinge_OBJECTS = $(am_demo_hinge_OBJECTS)
167demo_hinge_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
168 $(demo_hinge_LDFLAGS) $(LDFLAGS) -o $@
169am_demo_jointPR_OBJECTS = demo_jointPR.$(OBJEXT)
170demo_jointPR_OBJECTS = $(am_demo_jointPR_OBJECTS)
171demo_jointPR_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
172 $(demo_jointPR_LDFLAGS) $(LDFLAGS) -o $@
173am_demo_joints_OBJECTS = demo_joints.$(OBJEXT)
174demo_joints_OBJECTS = $(am_demo_joints_OBJECTS)
175demo_joints_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
176 $(demo_joints_LDFLAGS) $(LDFLAGS) -o $@
177am_demo_motor_OBJECTS = demo_motor.$(OBJEXT)
178demo_motor_OBJECTS = $(am_demo_motor_OBJECTS)
179demo_motor_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
180 $(demo_motor_LDFLAGS) $(LDFLAGS) -o $@
181am__demo_moving_trimesh_SOURCES_DIST = demo_moving_trimesh.cpp
182@TRIMESH_TRUE@am_demo_moving_trimesh_OBJECTS = \
183@TRIMESH_TRUE@ demo_moving_trimesh.$(OBJEXT)
184demo_moving_trimesh_OBJECTS = $(am_demo_moving_trimesh_OBJECTS)
185demo_moving_trimesh_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
186 $(demo_moving_trimesh_LDFLAGS) $(LDFLAGS) -o $@
187am_demo_ode_OBJECTS = demo_ode.$(OBJEXT)
188demo_ode_OBJECTS = $(am_demo_ode_OBJECTS)
189demo_ode_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
190 $(demo_ode_LDFLAGS) $(LDFLAGS) -o $@
191am_demo_plane2d_OBJECTS = demo_plane2d.$(OBJEXT)
192demo_plane2d_OBJECTS = $(am_demo_plane2d_OBJECTS)
193demo_plane2d_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
194 $(demo_plane2d_LDFLAGS) $(LDFLAGS) -o $@
195am_demo_slider_OBJECTS = demo_slider.$(OBJEXT)
196demo_slider_OBJECTS = $(am_demo_slider_OBJECTS)
197demo_slider_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
198 $(demo_slider_LDFLAGS) $(LDFLAGS) -o $@
199am_demo_space_OBJECTS = demo_space.$(OBJEXT)
200demo_space_OBJECTS = $(am_demo_space_OBJECTS)
201demo_space_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
202 $(demo_space_LDFLAGS) $(LDFLAGS) -o $@
203am_demo_space_stress_OBJECTS = demo_space_stress.$(OBJEXT)
204demo_space_stress_OBJECTS = $(am_demo_space_stress_OBJECTS)
205demo_space_stress_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
206 $(demo_space_stress_LDFLAGS) $(LDFLAGS) -o $@
207am_demo_step_OBJECTS = demo_step.$(OBJEXT)
208demo_step_OBJECTS = $(am_demo_step_OBJECTS)
209demo_step_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
210 $(demo_step_LDFLAGS) $(LDFLAGS) -o $@
211am__demo_trimesh_SOURCES_DIST = demo_trimesh.cpp
212@TRIMESH_TRUE@am_demo_trimesh_OBJECTS = demo_trimesh.$(OBJEXT)
213demo_trimesh_OBJECTS = $(am_demo_trimesh_OBJECTS)
214demo_trimesh_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \
215 $(demo_trimesh_LDFLAGS) $(LDFLAGS) -o $@
216DEFAULT_INCLUDES = -I. -I$(top_builddir)/include/ode@am__isrc@
217depcomp = $(SHELL) $(top_srcdir)/depcomp
218am__depfiles_maybe = depfiles
219COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \
220 $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS)
221CCLD = $(CC)
222LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@
223CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \
224 $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS)
225CXXLD = $(CXX)
226CXXLINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) \
227 -o $@
228SOURCES = $(demo_I_SOURCES) $(demo_basket_SOURCES) \
229 $(demo_boxstack_SOURCES) $(demo_buggy_SOURCES) \
230 $(demo_chain1_SOURCES) $(demo_chain2_SOURCES) \
231 $(demo_collision_SOURCES) $(demo_convex_cd_SOURCES) \
232 $(demo_crash_SOURCES) $(demo_cyl_SOURCES) \
233 $(demo_cylvssphere_SOURCES) $(demo_feedback_SOURCES) \
234 $(demo_friction_SOURCES) $(demo_heightfield_SOURCES) \
235 $(demo_hinge_SOURCES) $(demo_jointPR_SOURCES) \
236 $(demo_joints_SOURCES) $(demo_motor_SOURCES) \
237 $(demo_moving_trimesh_SOURCES) $(demo_ode_SOURCES) \
238 $(demo_plane2d_SOURCES) $(demo_slider_SOURCES) \
239 $(demo_space_SOURCES) $(demo_space_stress_SOURCES) \
240 $(demo_step_SOURCES) $(demo_trimesh_SOURCES)
241DIST_SOURCES = $(demo_I_SOURCES) $(am__demo_basket_SOURCES_DIST) \
242 $(demo_boxstack_SOURCES) $(demo_buggy_SOURCES) \
243 $(demo_chain1_SOURCES) $(demo_chain2_SOURCES) \
244 $(demo_collision_SOURCES) $(demo_convex_cd_SOURCES) \
245 $(demo_crash_SOURCES) $(demo_cyl_SOURCES) \
246 $(demo_cylvssphere_SOURCES) $(demo_feedback_SOURCES) \
247 $(demo_friction_SOURCES) $(demo_heightfield_SOURCES) \
248 $(demo_hinge_SOURCES) $(demo_jointPR_SOURCES) \
249 $(demo_joints_SOURCES) $(demo_motor_SOURCES) \
250 $(am__demo_moving_trimesh_SOURCES_DIST) $(demo_ode_SOURCES) \
251 $(demo_plane2d_SOURCES) $(demo_slider_SOURCES) \
252 $(demo_space_SOURCES) $(demo_space_stress_SOURCES) \
253 $(demo_step_SOURCES) $(am__demo_trimesh_SOURCES_DIST)
254ETAGS = etags
255CTAGS = ctags
256DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST)
257ACLOCAL = @ACLOCAL@
258ALLOCA = @ALLOCA@
259AMTAR = @AMTAR@
260ARCHFLAGS = @ARCHFLAGS@
261AUTOCONF = @AUTOCONF@
262AUTOHEADER = @AUTOHEADER@
263AUTOMAKE = @AUTOMAKE@
264AWK = @AWK@
265CC = @CC@
266CCDEPMODE = @CCDEPMODE@
267CFLAGS = @CFLAGS@
268CPP = @CPP@
269CPPFLAGS = @CPPFLAGS@
270CXX = @CXX@
271CXXDEPMODE = @CXXDEPMODE@
272CXXFLAGS = @CXXFLAGS@
273CYGPATH_W = @CYGPATH_W@
274DEFS = @DEFS@
275DEPDIR = @DEPDIR@
276DRAWSTUFF = @DRAWSTUFF@
277ECHO_C = @ECHO_C@
278ECHO_N = @ECHO_N@
279ECHO_T = @ECHO_T@
280EGREP = @EGREP@
281EXEEXT = @EXEEXT@
282GL_LIBS = @GL_LIBS@
283GREP = @GREP@
284INSTALL = @INSTALL@
285INSTALL_DATA = @INSTALL_DATA@
286INSTALL_PROGRAM = @INSTALL_PROGRAM@
287INSTALL_SCRIPT = @INSTALL_SCRIPT@
288INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@
289LDFLAGS = @LDFLAGS@
290LIBOBJS = @LIBOBJS@
291LIBS = @LIBS@
292LTLIBOBJS = @LTLIBOBJS@
293MAKEINFO = @MAKEINFO@
294MKDIR_P = @MKDIR_P@
295OBJEXT = @OBJEXT@
296ODE_AGE = @ODE_AGE@
297ODE_CURRENT = @ODE_CURRENT@
298ODE_RELEASE = @ODE_RELEASE@
299ODE_REVISION = @ODE_REVISION@
300ODE_SONAME = @ODE_SONAME@
301PACKAGE = @PACKAGE@
302PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@
303PACKAGE_NAME = @PACKAGE_NAME@
304PACKAGE_STRING = @PACKAGE_STRING@
305PACKAGE_TARNAME = @PACKAGE_TARNAME@
306PACKAGE_VERSION = @PACKAGE_VERSION@
307PATH_SEPARATOR = @PATH_SEPARATOR@
308RANLIB = @RANLIB@
309SET_MAKE = @SET_MAKE@
310SHARED_LDFLAGS = @SHARED_LDFLAGS@
311SHELL = @SHELL@
312STRIP = @STRIP@
313TOPDIR = @TOPDIR@
314VERSION = @VERSION@
315WINDRES = @WINDRES@
316XMKMF = @XMKMF@
317X_CFLAGS = @X_CFLAGS@
318X_EXTRA_LIBS = @X_EXTRA_LIBS@
319X_LIBS = @X_LIBS@
320X_PRE_LIBS = @X_PRE_LIBS@
321abs_builddir = @abs_builddir@
322abs_srcdir = @abs_srcdir@
323abs_top_builddir = @abs_top_builddir@
324abs_top_srcdir = @abs_top_srcdir@
325ac_ct_CC = @ac_ct_CC@
326ac_ct_CXX = @ac_ct_CXX@
327ac_ct_WINDRES = @ac_ct_WINDRES@
328am__include = @am__include@
329am__leading_dot = @am__leading_dot@
330am__quote = @am__quote@
331am__tar = @am__tar@
332am__untar = @am__untar@
333bindir = @bindir@
334build = @build@
335build_alias = @build_alias@
336build_cpu = @build_cpu@
337build_os = @build_os@
338build_vendor = @build_vendor@
339builddir = @builddir@
340datadir = @datadir@
341datarootdir = @datarootdir@
342docdir = @docdir@
343dvidir = @dvidir@
344exec_prefix = @exec_prefix@
345host = @host@
346host_alias = @host_alias@
347host_cpu = @host_cpu@
348host_os = @host_os@
349host_vendor = @host_vendor@
350htmldir = @htmldir@
351includedir = @includedir@
352infodir = @infodir@
353install_sh = @install_sh@
354libdir = @libdir@
355libexecdir = @libexecdir@
356localedir = @localedir@
357localstatedir = @localstatedir@
358mandir = @mandir@
359mkdir_p = @mkdir_p@
360oldincludedir = @oldincludedir@
361pdfdir = @pdfdir@
362prefix = @prefix@
363program_transform_name = @program_transform_name@
364psdir = @psdir@
365sbindir = @sbindir@
366sharedstatedir = @sharedstatedir@
367so_ext = @so_ext@
368srcdir = @srcdir@
369sysconfdir = @sysconfdir@
370target = @target@
371target_alias = @target_alias@
372target_cpu = @target_cpu@
373target_os = @target_os@
374target_vendor = @target_vendor@
375top_builddir = @top_builddir@
376top_srcdir = @top_srcdir@
377AM_CXXFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
378AM_CFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
379demo_collision_SOURCES = demo_collision.cpp
380demo_slider_SOURCES = demo_slider.cpp
381demo_feedback_SOURCES = demo_feedback.cpp
382demo_crash_SOURCES = demo_crash.cpp
383demo_space_SOURCES = demo_space.cpp
384demo_I_SOURCES = demo_I.cpp
385demo_friction_SOURCES = demo_friction.cpp
386demo_space_stress_SOURCES = demo_space_stress.cpp
387demo_boxstack_SOURCES = demo_boxstack.cpp
388demo_hinge_SOURCES = demo_hinge.cpp
389demo_step_SOURCES = demo_step.cpp
390demo_buggy_SOURCES = demo_buggy.cpp
391demo_cyl_SOURCES = demo_cyl.cpp world_geom3.h
392demo_cylvssphere_SOURCES = demo_cylvssphere.cpp
393demo_joints_SOURCES = demo_joints.cpp
394demo_jointPR_SOURCES = demo_jointPR.cpp
395demo_motor_SOURCES = demo_motor.cpp
396demo_chain1_SOURCES = demo_chain1.c
397demo_chain2_SOURCES = demo_chain2.cpp
398demo_ode_SOURCES = demo_ode.cpp
399demo_plane2d_SOURCES = demo_plane2d.cpp
400demo_heightfield_SOURCES = demo_heightfield.cpp
401demo_convex_cd_SOURCES = demo_convex_cd.cpp
402demo_collision_LDFLAGS = -L$(top_builddir)/drawstuff/src \
403 -L$(top_builddir)/ode/src @LDFLAGS@
404
405demo_slider_LDFLAGS = -L$(top_builddir)/drawstuff/src \
406 -L$(top_builddir)/ode/src @LDFLAGS@
407
408demo_feedback_LDFLAGS = -L$(top_builddir)/drawstuff/src \
409 -L$(top_builddir)/ode/src @LDFLAGS@
410
411demo_crash_LDFLAGS = -L$(top_builddir)/drawstuff/src \
412 -L$(top_builddir)/ode/src @LDFLAGS@
413
414demo_space_LDFLAGS = -L$(top_builddir)/drawstuff/src \
415 -L$(top_builddir)/ode/src @LDFLAGS@
416
417demo_I_LDFLAGS = -L$(top_builddir)/drawstuff/src \
418 -L$(top_builddir)/ode/src @LDFLAGS@
419
420demo_friction_LDFLAGS = -L$(top_builddir)/drawstuff/src \
421 -L$(top_builddir)/ode/src @LDFLAGS@
422
423demo_space_stress_LDFLAGS = -L$(top_builddir)/drawstuff/src \
424 -L$(top_builddir)/ode/src @LDFLAGS@
425
426demo_boxstack_LDFLAGS = -L$(top_builddir)/drawstuff/src \
427 -L$(top_builddir)/ode/src @LDFLAGS@
428
429demo_hinge_LDFLAGS = -L$(top_builddir)/drawstuff/src \
430 -L$(top_builddir)/ode/src @LDFLAGS@
431
432demo_step_LDFLAGS = -L$(top_builddir)/drawstuff/src \
433 -L$(top_builddir)/ode/src @LDFLAGS@
434
435demo_buggy_LDFLAGS = -L$(top_builddir)/drawstuff/src \
436 -L$(top_builddir)/ode/src @LDFLAGS@
437
438demo_cyl_LDFLAGS = -L$(top_builddir)/drawstuff/src \
439 -L$(top_builddir)/ode/src @LDFLAGS@
440
441demo_cylvssphere_LDFLAGS = -L$(top_builddir)/drawstuff/src \
442 -L$(top_builddir)/ode/src @LDFLAGS@
443
444demo_joints_LDFLAGS = -L$(top_builddir)/drawstuff/src \
445 -L$(top_builddir)/ode/src @LDFLAGS@
446
447demo_jointPR_LDFLAGS = -L$(top_builddir)/drawstuff/src \
448 -L$(top_builddir)/ode/src @LDFLAGS@
449
450demo_motor_LDFLAGS = -L$(top_builddir)/drawstuff/src \
451 -L$(top_builddir)/ode/src @LDFLAGS@
452
453demo_chain1_LDFLAGS = -L$(top_builddir)/drawstuff/src \
454 -L$(top_builddir)/ode/src @LDFLAGS@
455
456demo_chain2_LDFLAGS = -L$(top_builddir)/drawstuff/src \
457 -L$(top_builddir)/ode/src @LDFLAGS@
458
459demo_ode_LDFLAGS = -L$(top_builddir)/drawstuff/src \
460 -L$(top_builddir)/ode/src @LDFLAGS@
461
462demo_plane2d_LDFLAGS = -L$(top_builddir)/drawstuff/src \
463 -L$(top_builddir)/ode/src @LDFLAGS@
464
465demo_heightfield_LDFLAGS = -L$(top_builddir)/drawstuff/src \
466 -L$(top_builddir)/ode/src @LDFLAGS@
467
468demo_convex_cd_LDFLAGS = -L$(top_builddir)/drawstuff/src \
469 -L$(top_builddir)/ode/src @LDFLAGS@
470
471demo_collision_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
472 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_43)
473demo_slider_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
474 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_41)
475demo_feedback_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
476 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_42)
477demo_crash_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
478 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_40)
479demo_space_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
480 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_39)
481demo_I_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
482 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_38)
483demo_friction_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
484 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_37)
485demo_space_stress_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
486 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_36)
487demo_boxstack_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
488 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_35)
489demo_hinge_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
490 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_34)
491demo_step_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
492 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_33)
493demo_buggy_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
494 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_30)
495demo_cyl_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
496 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_31)
497demo_cylvssphere_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
498 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_32)
499demo_joints_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
500 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_27)
501demo_jointPR_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
502 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_28)
503demo_motor_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
504 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_29)
505demo_chain1_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
506 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_26)
507demo_chain2_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
508 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_25)
509demo_ode_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
510 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_24)
511demo_plane2d_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
512 $(top_builddir)/drawstuff/src/libdrawstuff.a
513
514demo_heightfield_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
515 $(top_builddir)/drawstuff/src/libdrawstuff.a
516
517demo_convex_cd_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \
518 $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_44)
519@TRIMESH_TRUE@demo_trimesh_SOURCES = demo_trimesh.cpp
520@TRIMESH_TRUE@demo_moving_trimesh_SOURCES = demo_moving_trimesh.cpp
521@TRIMESH_TRUE@demo_basket_SOURCES = demo_basket.cpp
522@TRIMESH_TRUE@demo_trimesh_LDFLAGS = -L$(top_builddir)/drawstuff/src \
523@TRIMESH_TRUE@ -L$(top_builddir)/ode/src @LDFLAGS@ \
524@TRIMESH_TRUE@ @GL_LIBS@ @LIBS@
525
526@TRIMESH_TRUE@demo_moving_trimesh_LDFLAGS = -L$(top_builddir)/drawstuff/src \
527@TRIMESH_TRUE@ -L$(top_builddir)/ode/src @LDFLAGS@ \
528@TRIMESH_TRUE@ @GL_LIBS@ @LIBS@
529
530@TRIMESH_TRUE@demo_basket_LDFLAGS = -L$(top_builddir)/drawstuff/src \
531@TRIMESH_TRUE@ -L$(top_builddir)/ode/src @LDFLAGS@ \
532@TRIMESH_TRUE@ @GL_LIBS@ @LIBS@
533
534@TRIMESH_TRUE@demo_trimesh_DEPENDENCIES = \
535@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a \
536@TRIMESH_TRUE@ $(top_builddir)/drawstuff/src/libdrawstuff.a \
537@TRIMESH_TRUE@ $(am__append_47)
538@TRIMESH_TRUE@demo_moving_trimesh_DEPENDENCIES = \
539@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a \
540@TRIMESH_TRUE@ $(top_builddir)/drawstuff/src/libdrawstuff.a \
541@TRIMESH_TRUE@ $(am__append_48)
542@TRIMESH_TRUE@demo_basket_DEPENDENCIES = \
543@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a \
544@TRIMESH_TRUE@ $(top_builddir)/drawstuff/src/libdrawstuff.a \
545@TRIMESH_TRUE@ $(am__append_49)
546demo_ode_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
547 @GL_LIBS@ @LIBS@ $(am__append_2)
548demo_plane2d_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
549demo_heightfield_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
550 @GL_LIBS@ @LIBS@ $(am__append_3)
551demo_chain2_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
552 @GL_LIBS@ @LIBS@ $(am__append_4)
553demo_chain1_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
554 @GL_LIBS@ @LIBS@ $(am__append_5)
555demo_joints_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
556 @GL_LIBS@ @LIBS@ $(am__append_6)
557demo_jointPR_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
558 @GL_LIBS@ @LIBS@ $(am__append_7)
559demo_motor_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
560 @GL_LIBS@ @LIBS@ $(am__append_8)
561demo_buggy_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
562 @GL_LIBS@ @LIBS@ $(am__append_9)
563demo_cyl_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
564 @GL_LIBS@ @LIBS@ $(am__append_10)
565demo_cylvssphere_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
566 @GL_LIBS@ @LIBS@ $(am__append_11)
567demo_step_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
568 @GL_LIBS@ @LIBS@ $(am__append_12)
569demo_hinge_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
570 @GL_LIBS@ @LIBS@ $(am__append_13)
571demo_boxstack_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
572 @GL_LIBS@ @LIBS@ $(am__append_14)
573demo_space_stress_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
574 @GL_LIBS@ @LIBS@ $(am__append_15)
575demo_friction_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
576 @GL_LIBS@ @LIBS@ $(am__append_16)
577demo_I_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ \
578 @LIBS@ $(am__append_17)
579demo_space_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
580 @GL_LIBS@ @LIBS@ $(am__append_18)
581demo_crash_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
582 @GL_LIBS@ @LIBS@ $(am__append_19)
583demo_slider_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
584 @GL_LIBS@ @LIBS@ $(am__append_20)
585demo_feedback_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
586 @GL_LIBS@ @LIBS@ $(am__append_21)
587demo_collision_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
588 @GL_LIBS@ @LIBS@ $(am__append_22)
589demo_convex_cd_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \
590 @GL_LIBS@ @LIBS@ $(am__append_23)
591@TRIMESH_TRUE@demo_trimesh_LDADD = -ldrawstuff \
592@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a @GL_LIBS@ \
593@TRIMESH_TRUE@ @LIBS@ $(am__append_45)
594@TRIMESH_TRUE@demo_moving_trimesh_LDADD = -ldrawstuff \
595@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a @GL_LIBS@ \
596@TRIMESH_TRUE@ @LIBS@ $(am__append_46)
597@TRIMESH_TRUE@demo_basket_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@
598all: all-am
599
600.SUFFIXES:
601.SUFFIXES: .c .cpp .o .obj
602$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps)
603 @for dep in $?; do \
604 case '$(am__configure_deps)' in \
605 *$$dep*) \
606 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh \
607 && exit 0; \
608 exit 1;; \
609 esac; \
610 done; \
611 echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/demo/Makefile'; \
612 cd $(top_srcdir) && \
613 $(AUTOMAKE) --foreign ode/demo/Makefile
614.PRECIOUS: Makefile
615Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status
616 @case '$?' in \
617 *config.status*) \
618 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \
619 *) \
620 echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \
621 cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \
622 esac;
623
624$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES)
625 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
626
627$(top_srcdir)/configure: $(am__configure_deps)
628 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
629$(ACLOCAL_M4): $(am__aclocal_m4_deps)
630 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
631
632clean-noinstPROGRAMS:
633 -test -z "$(noinst_PROGRAMS)" || rm -f $(noinst_PROGRAMS)
634demo_I$(EXEEXT): $(demo_I_OBJECTS) $(demo_I_DEPENDENCIES)
635 @rm -f demo_I$(EXEEXT)
636 $(demo_I_LINK) $(demo_I_OBJECTS) $(demo_I_LDADD) $(LIBS)
637demo_basket$(EXEEXT): $(demo_basket_OBJECTS) $(demo_basket_DEPENDENCIES)
638 @rm -f demo_basket$(EXEEXT)
639 $(demo_basket_LINK) $(demo_basket_OBJECTS) $(demo_basket_LDADD) $(LIBS)
640demo_boxstack$(EXEEXT): $(demo_boxstack_OBJECTS) $(demo_boxstack_DEPENDENCIES)
641 @rm -f demo_boxstack$(EXEEXT)
642 $(demo_boxstack_LINK) $(demo_boxstack_OBJECTS) $(demo_boxstack_LDADD) $(LIBS)
643demo_buggy$(EXEEXT): $(demo_buggy_OBJECTS) $(demo_buggy_DEPENDENCIES)
644 @rm -f demo_buggy$(EXEEXT)
645 $(demo_buggy_LINK) $(demo_buggy_OBJECTS) $(demo_buggy_LDADD) $(LIBS)
646demo_chain1$(EXEEXT): $(demo_chain1_OBJECTS) $(demo_chain1_DEPENDENCIES)
647 @rm -f demo_chain1$(EXEEXT)
648 $(demo_chain1_LINK) $(demo_chain1_OBJECTS) $(demo_chain1_LDADD) $(LIBS)
649demo_chain2$(EXEEXT): $(demo_chain2_OBJECTS) $(demo_chain2_DEPENDENCIES)
650 @rm -f demo_chain2$(EXEEXT)
651 $(demo_chain2_LINK) $(demo_chain2_OBJECTS) $(demo_chain2_LDADD) $(LIBS)
652demo_collision$(EXEEXT): $(demo_collision_OBJECTS) $(demo_collision_DEPENDENCIES)
653 @rm -f demo_collision$(EXEEXT)
654 $(demo_collision_LINK) $(demo_collision_OBJECTS) $(demo_collision_LDADD) $(LIBS)
655demo_convex_cd$(EXEEXT): $(demo_convex_cd_OBJECTS) $(demo_convex_cd_DEPENDENCIES)
656 @rm -f demo_convex_cd$(EXEEXT)
657 $(demo_convex_cd_LINK) $(demo_convex_cd_OBJECTS) $(demo_convex_cd_LDADD) $(LIBS)
658demo_crash$(EXEEXT): $(demo_crash_OBJECTS) $(demo_crash_DEPENDENCIES)
659 @rm -f demo_crash$(EXEEXT)
660 $(demo_crash_LINK) $(demo_crash_OBJECTS) $(demo_crash_LDADD) $(LIBS)
661demo_cyl$(EXEEXT): $(demo_cyl_OBJECTS) $(demo_cyl_DEPENDENCIES)
662 @rm -f demo_cyl$(EXEEXT)
663 $(demo_cyl_LINK) $(demo_cyl_OBJECTS) $(demo_cyl_LDADD) $(LIBS)
664demo_cylvssphere$(EXEEXT): $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_DEPENDENCIES)
665 @rm -f demo_cylvssphere$(EXEEXT)
666 $(demo_cylvssphere_LINK) $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_LDADD) $(LIBS)
667demo_feedback$(EXEEXT): $(demo_feedback_OBJECTS) $(demo_feedback_DEPENDENCIES)
668 @rm -f demo_feedback$(EXEEXT)
669 $(demo_feedback_LINK) $(demo_feedback_OBJECTS) $(demo_feedback_LDADD) $(LIBS)
670demo_friction$(EXEEXT): $(demo_friction_OBJECTS) $(demo_friction_DEPENDENCIES)
671 @rm -f demo_friction$(EXEEXT)
672 $(demo_friction_LINK) $(demo_friction_OBJECTS) $(demo_friction_LDADD) $(LIBS)
673demo_heightfield$(EXEEXT): $(demo_heightfield_OBJECTS) $(demo_heightfield_DEPENDENCIES)
674 @rm -f demo_heightfield$(EXEEXT)
675 $(demo_heightfield_LINK) $(demo_heightfield_OBJECTS) $(demo_heightfield_LDADD) $(LIBS)
676demo_hinge$(EXEEXT): $(demo_hinge_OBJECTS) $(demo_hinge_DEPENDENCIES)
677 @rm -f demo_hinge$(EXEEXT)
678 $(demo_hinge_LINK) $(demo_hinge_OBJECTS) $(demo_hinge_LDADD) $(LIBS)
679demo_jointPR$(EXEEXT): $(demo_jointPR_OBJECTS) $(demo_jointPR_DEPENDENCIES)
680 @rm -f demo_jointPR$(EXEEXT)
681 $(demo_jointPR_LINK) $(demo_jointPR_OBJECTS) $(demo_jointPR_LDADD) $(LIBS)
682demo_joints$(EXEEXT): $(demo_joints_OBJECTS) $(demo_joints_DEPENDENCIES)
683 @rm -f demo_joints$(EXEEXT)
684 $(demo_joints_LINK) $(demo_joints_OBJECTS) $(demo_joints_LDADD) $(LIBS)
685demo_motor$(EXEEXT): $(demo_motor_OBJECTS) $(demo_motor_DEPENDENCIES)
686 @rm -f demo_motor$(EXEEXT)
687 $(demo_motor_LINK) $(demo_motor_OBJECTS) $(demo_motor_LDADD) $(LIBS)
688demo_moving_trimesh$(EXEEXT): $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_DEPENDENCIES)
689 @rm -f demo_moving_trimesh$(EXEEXT)
690 $(demo_moving_trimesh_LINK) $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_LDADD) $(LIBS)
691demo_ode$(EXEEXT): $(demo_ode_OBJECTS) $(demo_ode_DEPENDENCIES)
692 @rm -f demo_ode$(EXEEXT)
693 $(demo_ode_LINK) $(demo_ode_OBJECTS) $(demo_ode_LDADD) $(LIBS)
694demo_plane2d$(EXEEXT): $(demo_plane2d_OBJECTS) $(demo_plane2d_DEPENDENCIES)
695 @rm -f demo_plane2d$(EXEEXT)
696 $(demo_plane2d_LINK) $(demo_plane2d_OBJECTS) $(demo_plane2d_LDADD) $(LIBS)
697demo_slider$(EXEEXT): $(demo_slider_OBJECTS) $(demo_slider_DEPENDENCIES)
698 @rm -f demo_slider$(EXEEXT)
699 $(demo_slider_LINK) $(demo_slider_OBJECTS) $(demo_slider_LDADD) $(LIBS)
700demo_space$(EXEEXT): $(demo_space_OBJECTS) $(demo_space_DEPENDENCIES)
701 @rm -f demo_space$(EXEEXT)
702 $(demo_space_LINK) $(demo_space_OBJECTS) $(demo_space_LDADD) $(LIBS)
703demo_space_stress$(EXEEXT): $(demo_space_stress_OBJECTS) $(demo_space_stress_DEPENDENCIES)
704 @rm -f demo_space_stress$(EXEEXT)
705 $(demo_space_stress_LINK) $(demo_space_stress_OBJECTS) $(demo_space_stress_LDADD) $(LIBS)
706demo_step$(EXEEXT): $(demo_step_OBJECTS) $(demo_step_DEPENDENCIES)
707 @rm -f demo_step$(EXEEXT)
708 $(demo_step_LINK) $(demo_step_OBJECTS) $(demo_step_LDADD) $(LIBS)
709demo_trimesh$(EXEEXT): $(demo_trimesh_OBJECTS) $(demo_trimesh_DEPENDENCIES)
710 @rm -f demo_trimesh$(EXEEXT)
711 $(demo_trimesh_LINK) $(demo_trimesh_OBJECTS) $(demo_trimesh_LDADD) $(LIBS)
712
713mostlyclean-compile:
714 -rm -f *.$(OBJEXT)
715
716distclean-compile:
717 -rm -f *.tab.c
718
719@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_I.Po@am__quote@
720@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_basket.Po@am__quote@
721@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_boxstack.Po@am__quote@
722@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_buggy.Po@am__quote@
723@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_chain1.Po@am__quote@
724@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_chain2.Po@am__quote@
725@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_collision.Po@am__quote@
726@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_convex_cd.Po@am__quote@
727@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_crash.Po@am__quote@
728@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cyl.Po@am__quote@
729@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cylvssphere.Po@am__quote@
730@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_feedback.Po@am__quote@
731@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_friction.Po@am__quote@
732@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_heightfield.Po@am__quote@
733@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_hinge.Po@am__quote@
734@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_jointPR.Po@am__quote@
735@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_joints.Po@am__quote@
736@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_motor.Po@am__quote@
737@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_moving_trimesh.Po@am__quote@
738@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_ode.Po@am__quote@
739@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_plane2d.Po@am__quote@
740@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_slider.Po@am__quote@
741@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_space.Po@am__quote@
742@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_space_stress.Po@am__quote@
743@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_step.Po@am__quote@
744@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_trimesh.Po@am__quote@
745
746.c.o:
747@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
748@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
749@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
750@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
751@am__fastdepCC_FALSE@ $(COMPILE) -c $<
752
753.c.obj:
754@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'`
755@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
756@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
757@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
758@am__fastdepCC_FALSE@ $(COMPILE) -c `$(CYGPATH_W) '$<'`
759
760.cpp.o:
761@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
762@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
763@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
764@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
765@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ $<
766
767.cpp.obj:
768@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'`
769@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
770@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
771@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
772@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'`
773
774ID: $(HEADERS) $(SOURCES) $(LISP) $(TAGS_FILES)
775 list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \
776 unique=`for i in $$list; do \
777 if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
778 done | \
779 $(AWK) ' { files[$$0] = 1; } \
780 END { for (i in files) print i; }'`; \
781 mkid -fID $$unique
782tags: TAGS
783
784TAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \
785 $(TAGS_FILES) $(LISP)
786 tags=; \
787 here=`pwd`; \
788 list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \
789 unique=`for i in $$list; do \
790 if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
791 done | \
792 $(AWK) ' { files[$$0] = 1; } \
793 END { for (i in files) print i; }'`; \
794 if test -z "$(ETAGS_ARGS)$$tags$$unique"; then :; else \
795 test -n "$$unique" || unique=$$empty_fix; \
796 $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \
797 $$tags $$unique; \
798 fi
799ctags: CTAGS
800CTAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \
801 $(TAGS_FILES) $(LISP)
802 tags=; \
803 here=`pwd`; \
804 list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \
805 unique=`for i in $$list; do \
806 if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
807 done | \
808 $(AWK) ' { files[$$0] = 1; } \
809 END { for (i in files) print i; }'`; \
810 test -z "$(CTAGS_ARGS)$$tags$$unique" \
811 || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \
812 $$tags $$unique
813
814GTAGS:
815 here=`$(am__cd) $(top_builddir) && pwd` \
816 && cd $(top_srcdir) \
817 && gtags -i $(GTAGS_ARGS) $$here
818
819distclean-tags:
820 -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags
821
822distdir: $(DISTFILES)
823 @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \
824 topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \
825 list='$(DISTFILES)'; \
826 dist_files=`for file in $$list; do echo $$file; done | \
827 sed -e "s|^$$srcdirstrip/||;t" \
828 -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \
829 case $$dist_files in \
830 */*) $(MKDIR_P) `echo "$$dist_files" | \
831 sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \
832 sort -u` ;; \
833 esac; \
834 for file in $$dist_files; do \
835 if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \
836 if test -d $$d/$$file; then \
837 dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \
838 if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \
839 cp -pR $(srcdir)/$$file $(distdir)$$dir || exit 1; \
840 fi; \
841 cp -pR $$d/$$file $(distdir)$$dir || exit 1; \
842 else \
843 test -f $(distdir)/$$file \
844 || cp -p $$d/$$file $(distdir)/$$file \
845 || exit 1; \
846 fi; \
847 done
848check-am: all-am
849check: check-am
850all-am: Makefile $(PROGRAMS)
851installdirs:
852install: install-am
853install-exec: install-exec-am
854install-data: install-data-am
855uninstall: uninstall-am
856
857install-am: all-am
858 @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am
859
860installcheck: installcheck-am
861install-strip:
862 $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \
863 install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \
864 `test -z '$(STRIP)' || \
865 echo "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'"` install
866mostlyclean-generic:
867
868clean-generic:
869
870distclean-generic:
871 -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES)
872
873maintainer-clean-generic:
874 @echo "This command is intended for maintainers to use"
875 @echo "it deletes files that may require special tools to rebuild."
876clean: clean-am
877
878clean-am: clean-generic clean-noinstPROGRAMS mostlyclean-am
879
880distclean: distclean-am
881 -rm -rf ./$(DEPDIR)
882 -rm -f Makefile
883distclean-am: clean-am distclean-compile distclean-generic \
884 distclean-tags
885
886dvi: dvi-am
887
888dvi-am:
889
890html: html-am
891
892info: info-am
893
894info-am:
895
896install-data-am:
897
898install-dvi: install-dvi-am
899
900install-exec-am:
901
902install-html: install-html-am
903
904install-info: install-info-am
905
906install-man:
907
908install-pdf: install-pdf-am
909
910install-ps: install-ps-am
911
912installcheck-am:
913
914maintainer-clean: maintainer-clean-am
915 -rm -rf ./$(DEPDIR)
916 -rm -f Makefile
917maintainer-clean-am: distclean-am maintainer-clean-generic
918
919mostlyclean: mostlyclean-am
920
921mostlyclean-am: mostlyclean-compile mostlyclean-generic
922
923pdf: pdf-am
924
925pdf-am:
926
927ps: ps-am
928
929ps-am:
930
931uninstall-am:
932
933.MAKE: install-am install-strip
934
935.PHONY: CTAGS GTAGS all all-am check check-am clean clean-generic \
936 clean-noinstPROGRAMS ctags distclean distclean-compile \
937 distclean-generic distclean-tags distdir dvi dvi-am html \
938 html-am info info-am install install-am install-data \
939 install-data-am install-dvi install-dvi-am install-exec \
940 install-exec-am install-html install-html-am install-info \
941 install-info-am install-man install-pdf install-pdf-am \
942 install-ps install-ps-am install-strip installcheck \
943 installcheck-am installdirs maintainer-clean \
944 maintainer-clean-generic mostlyclean mostlyclean-compile \
945 mostlyclean-generic pdf pdf-am ps ps-am tags uninstall \
946 uninstall-am
947
948
949@WIN32_TRUE@resources.o: ../../drawstuff/src/resources.rc ../../drawstuff/src/resource.h
950@WIN32_TRUE@ @WINDRES@ ../../drawstuff/src/resources.rc -o resources.o
951# Tell versions [3.59,3.63) of GNU make to not export all variables.
952# Otherwise a system limit (for SysV at least) may be exceeded.
953.NOEXPORT:
diff --git a/libraries/ode-0.9/ode/demo/basket_geom.h b/libraries/ode-0.9/ode/demo/basket_geom.h
new file mode 100644
index 0000000..7796ca8
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/basket_geom.h
@@ -0,0 +1,13 @@
1
2static float world_normals[] = {
30,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,-0,0,1,0,0,1,0,0,1,0,0,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,-0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,1,0,-0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,0,-0.948064f,0.318080f,0,-0.989482f,0.144655f,0,-0.983494f,0.180939f,0,-0.983494f,0.180939f,0,-0.908999f,0.416798f,0,-0.948064f,0.318080f,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-1,0,-0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,-0.132460f,0.991188f,0,0.264920f,0.964270f,0,0.132460f,0.991188f,0,0.132460f,0.991188f,0,-0.264920f,0.964270f,0,-0.132460f,0.991188f,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,-0.687592f,-0.726097f,-0,-0.881727f,-0.471761f,0,-0.687592f,-0.726097f,-0,-0.881727f,-0.471761f,0,-0.881727f,-0.471761f,0,-0.687592f,-0.726097f,-0,0.687592f,-0.726097f,0,0.928375f,-0.371644f,0,0.824321f,-0.566123f,0,0.687592f,-0.726097f,0,0.824321f,-0.566123f,0,0.687592f,-0.726097f,0,-0.881727f,-0.471761f,0,-0.985594f,-0.169128f,0,-0.985594f,-0.169128f,0,-0.985594f,-0.169128f,0,-0.881727f,-0.471761f,0,-0.881727f,-0.471761f,0,0.928375f,-0.371644f,0,0.985594f,-0.169128f,0,0.985594f,-0.169128f,0,0.928375f,-0.371644f,0,0.985594f,-0.169128f,0,0.824321f,-0.566123f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,-0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,0,0.870167f,0.492758f,-0,-0.390313f,0.920682f,0,-0.132460f,0.991188f,0,-0.264920f,0.964270f,0,-0.264920f,0.964270f,0,-0.390313f,0.920682f,0,-0.390313f,0.920682f,0,0.390313f,0.920682f,0,0.132460f,0.991188f,0,0.264920f,0.964270f,0,0.390313f,0.920682f,0,0.264920f,0.964270f,0,0.390313f,0.920682f,-0,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,-1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0.985594f,0.169128f,0,0.824321f,0.566123f,0,0.928375f,0.371644f,0,0.928375f,0.371644f,0,0.985594f,0.169128f,0,0.985594f,0.169128f,0,0.824321f,0.566123f,0,0.687592f,0.726097f,0,0.687592f,0.726097f,0,0.687592f,0.726097f,0,0.928375f,0.371644f,0,0.824321f,0.566123f,0,0,1,0,-0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-0.687592f,0.726097f,0,-0.687592f,0.726097f,0,-0.881727f,0.471761f,0,-0.881727f,0.471761f,0,-0.881727f,0.471761f,0,-0.687592f,0.726097f,0,-0.881727f,0.471761f,0,-0.985594f,0.169128f,0,-0.985594f,0.169128f,0,-0.985594f,0.169128f,0,-0.881727f,0.471761f,0,-0.881727f,0.471761f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.870166f,-0.492758f,0,-0.390314f,-0.920682f,0,-0.132460f,-0.991188f,0,-0.264921f,-0.964270f,0,-0.264921f,-0.964270f,0,-0.390314f,-0.920682f,0,-0.390314f,-0.920682f,0,-0.132460f,-0.991188f,0,0.264921f,-0.964270f,0,0.132460f,-0.991188f,0,0.132460f,-0.991188f,0,-0.264921f,-0.964270f,0,-0.132460f,-0.991188f,0,0.264921f,-0.964270f,0,0.390314f,-0.920682f,0,0.390314f,-0.920682f,0,0.390314f,-0.920682f,0,0.132460f,-0.991188f,0,0.264921f,-0.964270f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0.870166f,-0.492758f,0,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,-0.527606f,0.849489f,0,-0.793893f,0.608057f,0,-0.715135f,0.698986f,0,-0.715135f,0.698986f,0,-0.418249f,0.908332f,0,-0.527606f,0.849489f,0,-0.075284f,0.997162f,0,-0.253577f,0.967315f,0,-0.202069f,0.979371f,0,-0.202069f,0.979371f,0,-0.075284f,0.997162f,0,-0.075284f,0.997162f,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0.160137f,0.987095f,0,0.049305f,0.998784f,0,0.049305f,0.998784f,0,0.049305f,0.998784f,0,0.221401f,0.975183f,0,0.160137f,0.987095f,0,0.696124f,0.717921f,0,0.696124f,0.717921f,0,0.433340f,0.901230f,0,0.433340f,0.901230f,0,0.433340f,0.901230f,0,0.696124f,0.717921f,0,0.696124f,0.717921f,0,0.696124f,0.717921f,0,0.838308f,0.545197f,0,0.696124f,0.717921f,0,0.872167f,0.489208f,0,0.838308f,0.545197f,0,-0.994126f,0.108225f,0,-0.983494f,0.180939f,0,-0.989482f,0.144655f,0,-0.994126f,0.108225f,0,-0.989482f,0.144655f,0,-0.994126f,0.108225f,0,-0.948064f,0.318080f,0,-0.908999f,0.416798f,0,-0.793893f,0.608057f,0,-0.908999f,0.416798f,0,-0.715135f,0.698986f,0,-0.793893f,0.608057f,0,-0.527606f,0.849489f,0,-0.418249f,0.908332f,0,-0.253577f,0.967315f,0,-0.418249f,0.908332f,0,-0.202069f,0.979371f,0,-0.253577f,0.967315f,0,-0.075284f,0.997162f,0,-0.075284f,0.997162f,0,0,1,0,-0.075284f,0.997162f,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0,1,0,0.049305f,0.998784f,0,0,1,0,0.049305f,0.998784f,0,0.049305f,0.998784f,0,0.160137f,0.987095f,0,0.221401f,0.975183f,0,0.433340f,0.901230f,0,0.221401f,0.975183f,0,0.433340f,0.901230f,0,0.433340f,0.901230f,0,0.902172f,0.431376f,0,0.838308f,0.545197f,0,0.872167f,0.489208f,0,0.872167f,0.489208f,0,0.902172f,0.431376f,0,0.902172f,0.431376f,
4};
5
6static float world_vertices[] = {
7-4,-4,-0.100000f,4,-4,-0.100000f,4,-4,0.100000f,-4,-4,-0.100000f,4,-4,0.100000f,-4,-4,0.100000f,4,0,0.100000f,4,-4,-0.100000f,4,4,-0.100000f,4,0,0.100000f,4,4,-0.100000f,4,4,0.100000f,4,0,0.100000f,4,-4,0.100000f,4,-4,-0.100000f,-4,-4,-0.100000f,-4,4,-0.100000f,4,4,-0.100000f,-4,-4,-0.100000f,4,4,-0.100000f,4,-4,-0.100000f,0.066000f,-2.060000f,2,0.066000f,-1.940000f,2,-0.066000f,-2.060000f,2,0.066000f,-1.940000f,2,-0.066000f,-1.940000f,2,-0.066000f,-2.060000f,2,-4,4,0.100000f,4,4,0.100000f,4,4,-0.100000f,4,4,-0.100000f,-4,4,-0.100000f,-4,4,0.100000f,-4,-4,0.100000f,-4,0,0.100000f,-4,-4,-0.100000f,-4,0,0.100000f,-4,4,0.100000f,-4,4,-0.100000f,-4,0,0.100000f,-4,4,-0.100000f,-4,-4,-0.100000f,0.360000f,3.244444f,1.466974f,0.360000f,3.422222f,2.266974f,-0.360000f,3.422222f,2.266974f,-0.360000f,3.422222f,2.266974f,-0.360000f,3.244444f,1.466974f,0.360000f,3.244444f,1.466974f,4,-4,0.100000f,0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-4,-4,0.100000f,4,-4,0.100000f,4,0,0.100000f,0.066000f,-1.940000f,0.100000f,4,-4,0.100000f,0.066000f,-1.940000f,0.100000f,0.066000f,-2.060000f,0.100000f,4,-4,0.100000f,-0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,0.100000f,4,0,0.100000f,4,0,0.100000f,-4,0,0.100000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-1.940000f,0.100000f,-4,0,0.100000f,-4,0,0.100000f,-4,-4,0.100000f,-0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,2,-0.066000f,-2.060000f,2,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,2,0.066000f,-2.060000f,2,0.066000f,-2.060000f,2,0.066000f,-2.060000f,0.100000f,0.066000f,-1.940000f,1.950000f,0.066000f,-2.060000f,0.100000f,0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,1.950000f,-0.052853f,-1.506390f,2,0.052853f,-1.506390f,2,0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,2,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,2,-0.066000f,-1.940000f,1.950000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-2.060000f,2,-0.066000f,-1.940000f,2,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,0.100000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,2,-0.066000f,-1.840000f,2,-0.066000f,-1.840000f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,2,0.066000f,-1.940000f,2,0.066000f,-1.840000f,2,-0.066000f,-1.940000f,2,0.066000f,-1.840000f,2,-0.066000f,-1.840000f,2,-0.066000f,-1.940000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.840000f,2,0.066000f,-1.940000f,2,-0.066000f,-1.840000f,2,-0.171600f,-1.740000f,2,-0.066000f,-1.840000f,1.950000f,-0.171600f,-1.740000f,2,-0.171600f,-1.740000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.171600f,-1.740000f,2,0.066000f,-1.840000f,1.950000f,0.171600f,-1.740000f,2,0.066000f,-1.840000f,2,-0.171600f,-1.740000f,2,-0.188760f,-1.640000f,2,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.171600f,-1.740000f,2,0.171600f,-1.740000f,1.950000f,0.188760f,-1.640000f,1.950000f,0.188760f,-1.640000f,2,0.171600f,-1.740000f,1.950000f,0.188760f,-1.640000f,2,0.171600f,-1.740000f,2,-0.188760f,-1.640000f,2,-0.132132f,-1.540000f,2,-0.132132f,-1.540000f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,2,0.188760f,-1.640000f,1.950000f,0.132132f,-1.540000f,1.950000f,0.132132f,-1.540000f,2,0.188760f,-1.640000f,1.950000f,0.132132f,-1.540000f,2,0.188760f,-1.640000f,2,-0.132132f,-1.540000f,2,-0.052853f,-1.506390f,2,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.132132f,-1.540000f,2,0.132132f,-1.540000f,1.950000f,0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,2,0.132132f,-1.540000f,1.950000f,0.052853f,-1.506390f,2,0.132132f,-1.540000f,2,0.188760f,-1.640000f,1.950000f,0.173397f,-1.642679f,1.950000f,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,1.950000f,0.132132f,-1.540000f,1.950000f,0.188760f,-1.640000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.157950f,-1.732697f,1.950000f,0.173397f,-1.642679f,1.950000f,0.171600f,-1.740000f,1.950000f,0.173397f,-1.642679f,1.950000f,0.188760f,-1.640000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.060149f,-1.825311f,1.950000f,0.171600f,-1.740000f,1.950000f,0.060149f,-1.825311f,1.950000f,0.157950f,-1.732697f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.060149f,-1.825311f,1.950000f,0.066000f,-1.840000f,1.950000f,-0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,1.950000f,0.066000f,-1.840000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.132132f,-1.540000f,1.950000f,0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,1.950000f,0.049868f,-1.521079f,1.950000f,0.052853f,-1.506390f,1.950000f,0.132132f,-1.540000f,1.950000f,0.121808f,-1.551577f,1.950000f,0.052853f,-1.506390f,1.950000f,0.121808f,-1.551577f,1.950000f,0.049868f,-1.521079f,1.950000f,-0.188760f,-1.640000f,2,-0.173397f,-1.642679f,2,-0.121808f,-1.551577f,2,-0.121808f,-1.551577f,2,-0.132132f,-1.540000f,2,-0.188760f,-1.640000f,2,-0.171600f,-1.740000f,2,-0.157950f,-1.732697f,2,-0.173397f,-1.642679f,2,-0.173397f,-1.642679f,2,-0.188760f,-1.640000f,2,-0.171600f,-1.740000f,2,-0.066000f,-1.840000f,2,-0.060149f,-1.825311f,2,-0.171600f,-1.740000f,2,-0.060149f,-1.825311f,2,-0.157950f,-1.732697f,2,-0.171600f,-1.740000f,2,0.066000f,-1.840000f,2,0.060149f,-1.825311f,2,-0.066000f,-1.840000f,2,0.060149f,-1.825311f,2,-0.060149f,-1.825311f,2,-0.066000f,-1.840000f,2,0.171600f,-1.740000f,2,0.157950f,-1.732697f,2,0.060149f,-1.825311f,2,0.171600f,-1.740000f,2,0.060149f,-1.825311f,2,0.066000f,-1.840000f,2,0.173397f,-1.642679f,2,0.157950f,-1.732697f,2,0.171600f,-1.740000f,2,0.171600f,-1.740000f,2,0.188760f,-1.640000f,2,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.173397f,-1.642679f,2,0.188760f,-1.640000f,2,0.188760f,-1.640000f,2,0.132132f,-1.540000f,2,0.121808f,-1.551577f,2,0.052853f,-1.506390f,2,0.049868f,-1.521079f,2,0.121808f,-1.551577f,2,0.052853f,-1.506390f,2,0.121808f,-1.551577f,2,0.132132f,-1.540000f,2,-0.049868f,-1.521079f,2,0.049868f,-1.521079f,2,0.052853f,-1.506390f,2,0.052853f,-1.506390f,2,-0.052853f,-1.506390f,2,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.049868f,-1.521079f,2,-0.052853f,-1.506390f,2,-0.052853f,-1.506390f,2,-0.132132f,-1.540000f,2,-0.121808f,-1.551577f,2,-0.173397f,-1.642679f,2,-0.157950f,-1.732697f,2,-0.157950f,-1.732697f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.173397f,-1.642679f,2,-0.157950f,-1.732697f,2,-0.060149f,-1.825311f,2,-0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.157950f,-1.732697f,2,-0.060149f,-1.825311f,2,0.060149f,-1.825311f,2,0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,2,0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,2,0.157950f,-1.732697f,2,0.157950f,-1.732697f,2,0.157950f,-1.732697f,1.950000f,0.060149f,-1.825311f,1.950000f,0.157950f,-1.732697f,2,0.173397f,-1.642679f,2,0.173397f,-1.642679f,1.950000f,0.173397f,-1.642679f,1.950000f,0.157950f,-1.732697f,1.950000f,0.157950f,-1.732697f,2,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,1.950000f,0.173397f,-1.642679f,1.950000f,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.049868f,-1.521079f,2,0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,1.950000f,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,2,0.049868f,-1.521079f,2,-0.049868f,-1.521079f,2,-0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,2,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.121808f,-1.551577f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.173397f,-1.642679f,2,-0.173397f,-1.642679f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.121808f,-1.551577f,2,-0.360000f,3.600000f,0.100000f,0.360000f,3.600000f,0.100000f,4,4,0.100000f,4,4,0.100000f,-4,4,0.100000f,-0.360000f,3.600000f,0.100000f,-0.360000f,0.400000f,0.100000f,-0.360000f,3.600000f,0.100000f,-4,4,0.100000f,-4,4,0.100000f,-4,0,0.100000f,-0.360000f,0.400000f,0.100000f,4,0,0.100000f,0.360000f,0.400000f,0.100000f,-0.360000f,0.400000f,0.100000f,-0.360000f,0.400000f,0.100000f,-4,0,0.100000f,4,0,0.100000f,4,4,0.100000f,0.360000f,3.600000f,0.100000f,4,0,0.100000f,0.360000f,3.600000f,0.100000f,0.360000f,0.400000f,0.100000f,4,0,0.100000f,0.360000f,2.888889f,1.023752f,0.360000f,3.066667f,1.166974f,-0.360000f,3.066667f,1.166974f,-0.360000f,3.066667f,1.166974f,-0.360000f,2.888889f,1.023752f,0.360000f,2.888889f,1.023752f,0.360000f,2.533333f,0.939976f,0.360000f,2.711111f,0.966974f,-0.360000f,2.711111f,0.966974f,-0.360000f,2.711111f,0.966974f,-0.360000f,2.533333f,0.939976f,0.360000f,2.533333f,0.939976f,-0.360000f,2.177778f,0.939976f,0.360000f,2.177778f,0.939976f,0.360000f,2.355556f,0.939976f,0.360000f,2.355556f,0.939976f,-0.360000f,2.355556f,0.939976f,-0.360000f,2.177778f,0.939976f,-0.360000f,1.822222f,0.939976f,0.360000f,1.822222f,0.939976f,0.360000f,2,0.939976f,0.360000f,2,0.939976f,-0.360000f,2,0.939976f,-0.360000f,1.822222f,0.939976f,-0.360000f,1.466667f,0.939976f,0.360000f,1.466667f,0.939976f,0.360000f,1.644444f,0.939976f,0.360000f,1.644444f,0.939976f,-0.360000f,1.644444f,0.939976f,-0.360000f,1.466667f,0.939976f,0.360000f,1.111111f,0.957571f,0.360000f,1.288889f,0.939976f,-0.360000f,1.288889f,0.939976f,-0.360000f,1.288889f,0.939976f,-0.360000f,1.111111f,0.957571f,0.360000f,1.111111f,0.957571f,-0.360000f,0.755556f,1.134246f,0.360000f,0.755556f,1.134246f,0.360000f,0.933333f,1.009739f,0.360000f,0.933333f,1.009739f,-0.360000f,0.933333f,1.009739f,-0.360000f,0.755556f,1.134246f,0.360000f,0.755556f,1.134246f,-0.360000f,0.755556f,1.134246f,0.360000f,0.577778f,1.372130f,-0.360000f,0.755556f,1.134246f,-0.360000f,0.577778f,1.372130f,0.360000f,0.577778f,1.372130f,-0.360000f,3.600000f,3.900000f,-0.360000f,3.422222f,2.266974f,0.360000f,3.422222f,2.266974f,-0.360000f,3.600000f,3.900000f,0.360000f,3.422222f,2.266974f,0.360000f,3.600000f,3.900000f,0.360000f,3.244444f,1.466974f,-0.360000f,3.244444f,1.466974f,0.360000f,3.066667f,1.166974f,-0.360000f,3.244444f,1.466974f,-0.360000f,3.066667f,1.166974f,0.360000f,3.066667f,1.166974f,0.360000f,2.888889f,1.023752f,-0.360000f,2.888889f,1.023752f,0.360000f,2.711111f,0.966974f,-0.360000f,2.888889f,1.023752f,-0.360000f,2.711111f,0.966974f,0.360000f,2.711111f,0.966974f,0.360000f,2.533333f,0.939976f,-0.360000f,2.533333f,0.939976f,-0.360000f,2.355556f,0.939976f,0.360000f,2.533333f,0.939976f,-0.360000f,2.355556f,0.939976f,0.360000f,2.355556f,0.939976f,0.360000f,2.177778f,0.939976f,-0.360000f,2.177778f,0.939976f,-0.360000f,2,0.939976f,0.360000f,2.177778f,0.939976f,-0.360000f,2,0.939976f,0.360000f,2,0.939976f,0.360000f,1.822222f,0.939976f,-0.360000f,1.822222f,0.939976f,-0.360000f,1.644444f,0.939976f,0.360000f,1.822222f,0.939976f,-0.360000f,1.644444f,0.939976f,0.360000f,1.644444f,0.939976f,0.360000f,1.466667f,0.939976f,-0.360000f,1.466667f,0.939976f,-0.360000f,1.288889f,0.939976f,0.360000f,1.466667f,0.939976f,-0.360000f,1.288889f,0.939976f,0.360000f,1.288889f,0.939976f,0.360000f,1.111111f,0.957571f,-0.360000f,1.111111f,0.957571f,0.360000f,0.933333f,1.009739f,-0.360000f,1.111111f,0.957571f,-0.360000f,0.933333f,1.009739f,0.360000f,0.933333f,1.009739f,0.360000f,0.400000f,1.743932f,0.360000f,0.577778f,1.372130f,-0.360000f,0.577778f,1.372130f,-0.360000f,0.577778f,1.372130f,-0.360000f,0.400000f,1.743932f,0.360000f,0.400000f,1.743932f,
8};
9
10static int world_indices[] = {
110,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223,224,225,226,227,228,229,230,231,232,233,234,235,236,237,238,239,240,241,242,243,244,245,246,247,248,249,250,251,252,253,254,255,256,257,258,259,260,261,262,263,264,265,266,267,268,269,270,271,272,273,274,275,276,277,278,279,280,281,282,283,284,285,286,287,288,289,290,291,292,293,294,295,296,297,298,299,300,301,302,303,304,305,306,307,308,309,310,311,312,313,314,315,316,317,318,319,320,321,322,323,324,325,326,327,328,329,330,331,332,333,334,335,336,337,338,339,340,341,342,343,344,345,346,347,348,349,350,351,352,353,354,355,356,357,358,359,360,361,362,363,364,365,366,367,368,369,370,371,372,373,374,375,376,377,378,379,380,381,382,383,384,385,386,387,388,389,390,391,392,393,394,395,396,397,398,399,400,401,402,403,404,405,406,407,408,409,410,411,412,413,414,415,416,417,418,419,420,421,422,423,424,425,426,427,428,429,430,431,432,433,434,435,436,437,438,439,440,441,442,443,444,445,446,447,448,449,450,451,452,453,454,455,456,457,458,459,460,461,462,463,464,465,466,467,468,469,470,471,472,473,474,475,476,477,478,479,480,481,482,483,484,485,
12};
13
diff --git a/libraries/ode-0.9/ode/demo/demo_I.cpp b/libraries/ode-0.9/ode/demo/demo_I.cpp
new file mode 100644
index 0000000..b4f8d6e
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_I.cpp
@@ -0,0 +1,254 @@
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/*
24
25test that the rotational physics is correct.
26
27an "anchor body" has a number of other randomly positioned bodies
28("particles") attached to it by ball-and-socket joints, giving it some
29random effective inertia tensor. the effective inertia matrix is calculated,
30and then this inertia is assigned to another "test" body. a random torque is
31applied to both bodies and the difference in angular velocity and orientation
32is observed after a number of iterations.
33
34typical errors for each test cycle are about 1e-5 ... 1e-4.
35
36*/
37
38
39#include <time.h>
40#include <ode/ode.h>
41#include <drawstuff/drawstuff.h>
42
43#ifdef _MSC_VER
44#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
45#endif
46
47// select correct drawing functions
48
49#ifdef dDOUBLE
50#define dsDrawBox dsDrawBoxD
51#define dsDrawSphere dsDrawSphereD
52#define dsDrawCylinder dsDrawCylinderD
53#define dsDrawCapsule dsDrawCapsuleD
54#endif
55
56
57// some constants
58
59#define NUM 10 // number of particles
60#define SIDE 0.1 // visual size of the particles
61
62
63// dynamics objects an globals
64
65static dWorldID world=0;
66static dBodyID anchor_body,particle[NUM],test_body;
67static dJointID particle_joint[NUM];
68static dReal torque[3];
69static int iteration;
70
71
72// start simulation - set viewpoint
73
74static void start()
75{
76 static float xyz[3] = {1.5572f,-1.8886f,1.5700f};
77 static float hpr[3] = {118.5000f,-17.0000f,0.0000f};
78 dsSetViewpoint (xyz,hpr);
79}
80
81
82// compute the mass parameters of a particle set. q = particle positions,
83// pm = particle masses
84
85#define _I(i,j) I[(i)*4+(j)]
86
87void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM])
88{
89 int i,j;
90 dMassSetZero (m);
91 for (i=0; i<NUM; i++) {
92 m->mass += pm[i];
93 for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
94 m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
95 m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
96 m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
97 m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
98 m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
99 m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
100 }
101 for (j=0; j<3; j++) m->c[j] /= m->mass;
102 m->_I(1,0) = m->_I(0,1);
103 m->_I(2,0) = m->_I(0,2);
104 m->_I(2,1) = m->_I(1,2);
105}
106
107
108void reset_test()
109{
110 int i;
111 dMass m,anchor_m;
112 dReal q[NUM][3], pm[NUM]; // particle positions and masses
113 dReal pos1[3] = {1,0,1}; // point of reference (POR)
114 dReal pos2[3] = {-1,0,1}; // point of reference (POR)
115
116 // make random particle positions (relative to POR) and masses
117 for (i=0; i<NUM; i++) {
118 pm[i] = dRandReal()+0.1;
119 q[i][0] = dRandReal()-0.5;
120 q[i][1] = dRandReal()-0.5;
121 q[i][2] = dRandReal()-0.5;
122 }
123
124 // adjust particle positions so centor of mass = POR
125 computeMassParams (&m,q,pm);
126 for (i=0; i<NUM; i++) {
127 q[i][0] -= m.c[0];
128 q[i][1] -= m.c[1];
129 q[i][2] -= m.c[2];
130 }
131
132 if (world) dWorldDestroy (world);
133 world = dWorldCreate();
134
135 anchor_body = dBodyCreate (world);
136 dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
137 dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
138 dMassAdjust (&anchor_m,0.1);
139 dBodySetMass (anchor_body,&anchor_m);
140
141 for (i=0; i<NUM; i++) {
142 particle[i] = dBodyCreate (world);
143 dBodySetPosition (particle[i],
144 pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
145 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
146 dMassAdjust (&m,pm[i]);
147 dBodySetMass (particle[i],&m);
148 }
149
150 for (i=0; i < NUM; i++) {
151 particle_joint[i] = dJointCreateBall (world,0);
152 dJointAttach (particle_joint[i],anchor_body,particle[i]);
153 const dReal *p = dBodyGetPosition (particle[i]);
154 dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
155 }
156
157 // make test_body with the same mass and inertia of the anchor_body plus
158 // all the particles
159
160 test_body = dBodyCreate (world);
161 dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
162 computeMassParams (&m,q,pm);
163 m.mass += anchor_m.mass;
164 for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
165 dBodySetMass (test_body,&m);
166
167 // rotate the test and anchor bodies by a random amount
168 dQuaternion qrot;
169 for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
170 dNormalize4 (qrot);
171 dBodySetQuaternion (anchor_body,qrot);
172 dBodySetQuaternion (test_body,qrot);
173 dMatrix3 R;
174 dQtoR (qrot,R);
175 for (i=0; i<NUM; i++) {
176 dVector3 v;
177 dMultiply0 (v,R,&q[i][0],3,3,1);
178 dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
179 }
180
181 // set random torque
182 for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
183
184
185 iteration=0;
186}
187
188
189// simulation loop
190
191static void simLoop (int pause)
192{
193 if (!pause) {
194 dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]);
195 dBodyAddTorque (test_body,torque[0],torque[1],torque[2]);
196 dWorldStep (world,0.03);
197
198 iteration++;
199 if (iteration >= 100) {
200 // measure the difference between the anchor and test bodies
201 const dReal *w1 = dBodyGetAngularVel (anchor_body);
202 const dReal *w2 = dBodyGetAngularVel (test_body);
203 const dReal *q1 = dBodyGetQuaternion (anchor_body);
204 const dReal *q2 = dBodyGetQuaternion (test_body);
205 dReal maxdiff = dMaxDifference (w1,w2,1,3);
206 printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n",
207 maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]);
208 maxdiff = dMaxDifference (q1,q2,1,4);
209 printf ("q-error = %.4e\n",maxdiff);
210 reset_test();
211 }
212 }
213
214 dReal sides[3] = {SIDE,SIDE,SIDE};
215 dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE};
216 dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE};
217 dsSetColor (1,1,1);
218 dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body),
219 sides3);
220 dsSetColor (1,0,0);
221 dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2);
222 dsSetColor (1,1,0);
223 for (int i=0; i<NUM; i++)
224 dsDrawBox (dBodyGetPosition (particle[i]),
225 dBodyGetRotation (particle[i]), sides);
226}
227
228
229int main (int argc, char **argv)
230{
231 // setup pointers to drawstuff callback functions
232 dsFunctions fn;
233 fn.version = DS_VERSION;
234 fn.start = &start;
235 fn.step = &simLoop;
236 fn.command = 0;
237 fn.stop = 0;
238 fn.path_to_textures = "../../drawstuff/textures";
239 if(argc==2)
240 {
241 fn.path_to_textures = argv[1];
242 }
243
244 dInitODE();
245 dRandSetSeed (time(0));
246 reset_test();
247
248 // run simulation
249 dsSimulationLoop (argc,argv,352,288,&fn);
250
251 dWorldDestroy (world);
252 dCloseODE();
253 return 0;
254}
diff --git a/libraries/ode-0.9/ode/demo/demo_basket.cpp b/libraries/ode-0.9/ode/demo/demo_basket.cpp
new file mode 100644
index 0000000..35fb49a
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_basket.cpp
@@ -0,0 +1,278 @@
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// Basket ball demo.
24// Serves as a test for the sphere vs trimesh collider
25// By Bram Stolk.
26// Press the spacebar to reset the position of the ball.
27
28#include <ode/config.h>
29#include <assert.h>
30#ifdef HAVE_UNISTD_H
31#include <unistd.h>
32#endif
33#include <ode/ode.h>
34#include <drawstuff/drawstuff.h>
35
36#include "basket_geom.h" // this is our world mesh
37
38#ifdef _MSC_VER
39#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
40#endif
41
42// some constants
43
44#define RADIUS 0.14
45
46// dynamics and collision objects (chassis, 3 wheels, environment)
47
48static dWorldID world;
49static dSpaceID space;
50
51static dBodyID sphbody;
52static dGeomID sphgeom;
53
54static dJointGroupID contactgroup;
55static dGeomID world_mesh;
56
57
58// this is called by dSpaceCollide when two objects in space are
59// potentially colliding.
60
61static void nearCallback (void *data, dGeomID o1, dGeomID o2)
62{
63 assert(o1);
64 assert(o2);
65
66 if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
67 {
68 fprintf(stderr,"testing space %p %p\n", o1,o2);
69 // colliding a space with something
70 dSpaceCollide2(o1,o2,data,&nearCallback);
71 // Note we do not want to test intersections within a space,
72 // only between spaces.
73 return;
74 }
75
76// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
77
78 const int N = 32;
79 dContact contact[N];
80 int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
81 if (n > 0)
82 {
83 for (int i=0; i<n; i++)
84 {
85 // Paranoia <-- not working for some people, temporarily removed for 0.6
86 //dIASSERT(dVALIDVEC3(contact[i].geom.pos));
87 //dIASSERT(dVALIDVEC3(contact[i].geom.normal));
88 //dIASSERT(!dIsNan(contact[i].geom.depth));
89 contact[i].surface.slip1 = 0.7;
90 contact[i].surface.slip2 = 0.7;
91 contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
92 contact[i].surface.mu = 50.0; // was: dInfinity
93 contact[i].surface.soft_erp = 0.96;
94 contact[i].surface.soft_cfm = 0.04;
95 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
96 dJointAttach (c,
97 dGeomGetBody(contact[i].geom.g1),
98 dGeomGetBody(contact[i].geom.g2));
99 }
100 }
101}
102
103
104// start simulation - set viewpoint
105
106static void start()
107{
108 static float xyz[3] = {-8,0,5};
109 static float hpr[3] = {0.0f,-29.5000f,0.0000f};
110 dsSetViewpoint (xyz,hpr);
111}
112
113
114
115static void reset_ball(void)
116{
117 float sx=0.0f, sy=3.40f, sz=7.05;
118
119#if defined(_MSC_VER) && defined(dDOUBLE)
120 sy -= 0.01; // Cheat, to make it score under win32/double
121#endif
122
123 dQuaternion q;
124 dQSetIdentity(q);
125 dBodySetPosition (sphbody, sx, sy, sz);
126 dBodySetQuaternion(sphbody, q);
127 dBodySetLinearVel (sphbody, 0,0,0);
128 dBodySetAngularVel (sphbody, 0,0,0);
129}
130
131
132// called when a key pressed
133
134static void command (int cmd)
135{
136 switch (cmd)
137 {
138 case ' ':
139 reset_ball();
140 break;
141 }
142}
143
144
145// simulation loop
146
147static void simLoop (int pause)
148{
149 double simstep = 0.001; // 1ms simulation steps
150 double dt = dsElapsedTime();
151
152 int nrofsteps = (int) ceilf(dt/simstep);
153// fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);
154
155 for (int i=0; i<nrofsteps && !pause; i++)
156 {
157 dSpaceCollide (space,0,&nearCallback);
158 dWorldQuickStep (world, simstep);
159 dJointGroupEmpty (contactgroup);
160 }
161
162 dsSetColor (1,1,1);
163 const dReal *SPos = dBodyGetPosition(sphbody);
164 const dReal *SRot = dBodyGetRotation(sphbody);
165 float spos[3] = {SPos[0], SPos[1], SPos[2]};
166 float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
167 dsDrawSphere
168 (
169 spos,
170 srot,
171 RADIUS
172 );
173
174 // draw world trimesh
175 dsSetColor(0.4,0.7,0.9);
176 dsSetTexture (DS_NONE);
177
178 const dReal* Pos = dGeomGetPosition(world_mesh);
179 //dIASSERT(dVALIDVEC3(Pos));
180 float pos[3] = { Pos[0], Pos[1], Pos[2] };
181
182 const dReal* Rot = dGeomGetRotation(world_mesh);
183 //dIASSERT(dVALIDMAT3(Rot));
184 float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
185
186 int numi = sizeof(world_indices) / sizeof(int);
187
188 for (int i=0; i<numi/3; i++)
189 {
190 int i0 = world_indices[i*3+0];
191 int i1 = world_indices[i*3+1];
192 int i2 = world_indices[i*3+2];
193 float *v0 = world_vertices+i0*3;
194 float *v1 = world_vertices+i1*3;
195 float *v2 = world_vertices+i2*3;
196 dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
197 }
198}
199
200
201int main (int argc, char **argv)
202{
203 dMass m;
204 dMatrix3 R;
205
206 // setup pointers to drawstuff callback functions
207 dsFunctions fn;
208 fn.version = DS_VERSION;
209 fn.start = &start;
210 fn.step = &simLoop;
211 fn.command = &command;
212 fn.stop = 0;
213 fn.path_to_textures = "../../drawstuff/textures";
214 if(argc==2)
215 fn.path_to_textures = argv[1];
216
217 // create world
218 dInitODE();
219 world = dWorldCreate();
220 space = dHashSpaceCreate (0);
221
222 contactgroup = dJointGroupCreate (0);
223 dWorldSetGravity (world,0,0,-9.8);
224 dWorldSetQuickStepNumIterations (world, 64);
225
226 // Create a static world using a triangle mesh that we can collide with.
227 int numv = sizeof(world_vertices)/(3*sizeof(float));
228 int numi = sizeof(world_indices)/ sizeof(int);
229 printf("numv=%d, numi=%d\n", numv, numi);
230 dTriMeshDataID Data = dGeomTriMeshDataCreate();
231
232// fprintf(stderr,"Building Single Precision Mesh\n");
233
234 dGeomTriMeshDataBuildSingle
235 (
236 Data,
237 world_vertices,
238 3 * sizeof(float),
239 numv,
240 world_indices,
241 numi,
242 3 * sizeof(int)
243 );
244
245 world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
246 dGeomTriMeshEnableTC(world_mesh, dSphereClass, false);
247 dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);
248 dGeomSetPosition(world_mesh, 0, 0, 0.5);
249 dRSetIdentity(R);
250 //dIASSERT(dVALIDMAT3(R));
251
252 dGeomSetRotation (world_mesh, R);
253
254 float sx=0.0, sy=3.40, sz=6.80;
255 sphbody = dBodyCreate (world);
256 dMassSetSphere (&m,1,RADIUS);
257 dBodySetMass (sphbody,&m);
258 sphgeom = dCreateSphere(0, RADIUS);
259 dGeomSetBody (sphgeom,sphbody);
260 reset_ball();
261 dSpaceAdd (space, sphgeom);
262
263 // run simulation
264 dsSimulationLoop (argc,argv,352,288,&fn);
265
266 // Causes segm violation? Why?
267 // (because dWorldDestroy() destroys body connected to geom; must call first!)
268 dGeomDestroy(sphgeom);
269 dGeomDestroy (world_mesh);
270
271 dJointGroupEmpty (contactgroup);
272 dJointGroupDestroy (contactgroup);
273 dSpaceDestroy (space);
274 dWorldDestroy (world);
275 dCloseODE();
276 return 0;
277}
278
diff --git a/libraries/ode-0.9/ode/demo/demo_boxstack.cpp b/libraries/ode-0.9/ode/demo/demo_boxstack.cpp
new file mode 100644
index 0000000..8d237b2
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_boxstack.cpp
@@ -0,0 +1,577 @@
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
31//<---- Convex Object
32dReal planes[]= // planes for a cube, these should coincide with the face array
33 {
34 1.0f ,0.0f ,0.0f ,0.25f,
35 0.0f ,1.0f ,0.0f ,0.25f,
36 0.0f ,0.0f ,1.0f ,0.25f,
37 -1.0f,0.0f ,0.0f ,0.25f,
38 0.0f ,-1.0f,0.0f ,0.25f,
39 0.0f ,0.0f ,-1.0f,0.25f
40 /*
41 1.0f ,0.0f ,0.0f ,2.0f,
42 0.0f ,1.0f ,0.0f ,1.0f,
43 0.0f ,0.0f ,1.0f ,1.0f,
44 0.0f ,0.0f ,-1.0f,1.0f,
45 0.0f ,-1.0f,0.0f ,1.0f,
46 -1.0f,0.0f ,0.0f ,0.0f
47 */
48 };
49const unsigned int planecount=6;
50
51dReal points[]= // points for a cube
52 {
53 0.25f,0.25f,0.25f, // point 0
54 -0.25f,0.25f,0.25f, // point 1
55
56 0.25f,-0.25f,0.25f, // point 2
57 -0.25f,-0.25f,0.25f,// point 3
58
59 0.25f,0.25f,-0.25f, // point 4
60 -0.25f,0.25f,-0.25f,// point 5
61
62 0.25f,-0.25f,-0.25f,// point 6
63 -0.25f,-0.25f,-0.25f,// point 7
64 };
65const unsigned int pointcount=8;
66unsigned int polygons[] = //Polygons for a cube (6 squares)
67 {
68 4,0,2,6,4, // positive X
69 4,1,0,4,5, // positive Y
70 4,0,1,3,2, // positive Z
71 4,3,1,5,7, // negative X
72 4,2,3,7,6, // negative Y
73 4,5,4,6,7, // negative Z
74 };
75//----> Convex Object
76
77// select correct drawing functions
78
79#ifdef dDOUBLE
80#define dsDrawBox dsDrawBoxD
81#define dsDrawSphere dsDrawSphereD
82#define dsDrawCylinder dsDrawCylinderD
83#define dsDrawCapsule dsDrawCapsuleD
84#define dsDrawConvex dsDrawConvexD
85#endif
86
87
88// some constants
89
90#define NUM 100 // max number of objects
91#define DENSITY (5.0) // density of all objects
92#define GPB 3 // maximum number of geometries per body
93#define MAX_CONTACTS 8 // maximum number of contact points per body
94#define USE_GEOM_OFFSET 1
95
96// dynamics and collision objects
97
98struct MyObject {
99 dBodyID body; // the body
100 dGeomID geom[GPB]; // geometries representing this body
101};
102
103static int num=0; // number of objects in simulation
104static int nextobj=0; // next object to recycle if num==NUM
105static dWorldID world;
106static dSpaceID space;
107static MyObject obj[NUM];
108static dJointGroupID contactgroup;
109static int selected = -1; // selected object
110static int show_aabb = 0; // show geom AABBs?
111static int show_contacts = 0; // show contact points?
112static int random_pos = 1; // drop objects from random position?
113static int write_world = 0;
114static int show_body = 1;
115
116// this is called by dSpaceCollide when two objects in space are
117// potentially colliding.
118
119static void nearCallback (void *data, dGeomID o1, dGeomID o2)
120{
121 int i;
122 // if (o1->body && o2->body) return;
123
124 // exit without doing anything if the two bodies are connected by a joint
125 dBodyID b1 = dGeomGetBody(o1);
126 dBodyID b2 = dGeomGetBody(o2);
127 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
128
129 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
130 for (i=0; i<MAX_CONTACTS; i++) {
131 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
132 contact[i].surface.mu = dInfinity;
133 contact[i].surface.mu2 = 0;
134 contact[i].surface.bounce = 0.1;
135 contact[i].surface.bounce_vel = 0.1;
136 contact[i].surface.soft_cfm = 0.01;
137 }
138 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
139 sizeof(dContact))) {
140 dMatrix3 RI;
141 dRSetIdentity (RI);
142 const dReal ss[3] = {0.02,0.02,0.02};
143 for (i=0; i<numc; i++) {
144 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
145 dJointAttach (c,b1,b2);
146 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
147 }
148 }
149}
150
151
152// start simulation - set viewpoint
153
154static void start()
155{
156 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
157 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
158 dsSetViewpoint (xyz,hpr);
159 printf ("To drop another object, press:\n");
160 printf (" b for box.\n");
161 printf (" s for sphere.\n");
162 printf (" c for capsule.\n");
163 printf (" y for cylinder.\n");
164 printf (" v for a convex object.\n");
165 printf (" x for a composite object.\n");
166 printf ("To select an object, press space.\n");
167 printf ("To disable the selected object, press d.\n");
168 printf ("To enable the selected object, press e.\n");
169 printf ("To toggle showing the geom AABBs, press a.\n");
170 printf ("To toggle showing the contact points, press t.\n");
171 printf ("To toggle dropping from random position/orientation, press r.\n");
172 printf ("To save the current state to 'state.dif', press 1.\n");
173}
174
175
176char locase (char c)
177{
178 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
179 else return c;
180}
181
182
183// called when a key pressed
184
185static void command (int cmd)
186{
187 size_t i;
188 int j,k;
189 dReal sides[3];
190 dMass m;
191 int setBody;
192
193 cmd = locase (cmd);
194 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
195 {
196 setBody = 0;
197 if (num < NUM) {
198 i = num;
199 num++;
200 }
201 else {
202 i = nextobj;
203 nextobj++;
204 if (nextobj >= num) nextobj = 0;
205
206 // destroy the body and geoms for slot i
207 dBodyDestroy (obj[i].body);
208 for (k=0; k < GPB; k++) {
209 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
210 }
211 memset (&obj[i],0,sizeof(obj[i]));
212 }
213
214 obj[i].body = dBodyCreate (world);
215 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
216
217 dMatrix3 R;
218 if (random_pos)
219 {
220 dBodySetPosition (obj[i].body,
221 dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
222 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
223 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
224 }
225 else
226 {
227 dReal maxheight = 0;
228 for (k=0; k<num; k++)
229 {
230 const dReal *pos = dBodyGetPosition (obj[k].body);
231 if (pos[2] > maxheight) maxheight = pos[2];
232 }
233 dBodySetPosition (obj[i].body, 0,0,maxheight+1);
234 dRSetIdentity (R);
235 //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
236 }
237 dBodySetRotation (obj[i].body,R);
238 dBodySetData (obj[i].body,(void*) i);
239
240 if (cmd == 'b') {
241 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
242 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
243 }
244 else if (cmd == 'c') {
245 sides[0] *= 0.5;
246 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
247 obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
248 }
249 //<---- Convex Object
250 else if (cmd == 'v')
251 {
252 dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
253 obj[i].geom[0] = dCreateConvex (space,
254 planes,
255 planecount,
256 points,
257 pointcount,
258 polygons);
259 }
260 //----> Convex Object
261 else if (cmd == 'y') {
262 dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
263 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
264 }
265 else if (cmd == 's') {
266 sides[0] *= 0.5;
267 dMassSetSphere (&m,DENSITY,sides[0]);
268 obj[i].geom[0] = dCreateSphere (space,sides[0]);
269 }
270 else if (cmd == 'x' && USE_GEOM_OFFSET) {
271 setBody = 1;
272 // start accumulating masses for the encapsulated geometries
273 dMass m2;
274 dMassSetZero (&m);
275
276 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
277 dMatrix3 drot[GPB];
278
279 // set random delta positions
280 for (j=0; j<GPB; j++) {
281 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
282 }
283
284 for (k=0; k<GPB; k++) {
285 if (k==0) {
286 dReal radius = dRandReal()*0.25+0.05;
287 obj[i].geom[k] = dCreateSphere (space,radius);
288 dMassSetSphere (&m2,DENSITY,radius);
289 }
290 else if (k==1) {
291 obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
292 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
293 }
294 else {
295 dReal radius = dRandReal()*0.1+0.05;
296 dReal length = dRandReal()*1.0+0.1;
297 obj[i].geom[k] = dCreateCapsule (space,radius,length);
298 dMassSetCapsule (&m2,DENSITY,3,radius,length);
299 }
300
301 dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
302 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
303 dMassRotate (&m2,drot[k]);
304
305 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
306
307 // add to the total mass
308 dMassAdd (&m,&m2);
309
310 }
311 for (k=0; k<GPB; k++) {
312 dGeomSetBody (obj[i].geom[k],obj[i].body);
313 dGeomSetOffsetPosition (obj[i].geom[k],
314 dpos[k][0]-m.c[0],
315 dpos[k][1]-m.c[1],
316 dpos[k][2]-m.c[2]);
317 dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
318 }
319 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
320 dBodySetMass (obj[i].body,&m);
321
322 }
323 else if (cmd == 'x') {
324 dGeomID g2[GPB]; // encapsulated geometries
325 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
326
327 // start accumulating masses for the encapsulated geometries
328 dMass m2;
329 dMassSetZero (&m);
330
331 // set random delta positions
332 for (j=0; j<GPB; j++) {
333 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
334 }
335
336 for (k=0; k<GPB; k++) {
337 obj[i].geom[k] = dCreateGeomTransform (space);
338 dGeomTransformSetCleanup (obj[i].geom[k],1);
339 if (k==0) {
340 dReal radius = dRandReal()*0.25+0.05;
341 g2[k] = dCreateSphere (0,radius);
342 dMassSetSphere (&m2,DENSITY,radius);
343 }
344 else if (k==1) {
345 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
346 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
347 }
348 else {
349 dReal radius = dRandReal()*0.1+0.05;
350 dReal length = dRandReal()*1.0+0.1;
351 g2[k] = dCreateCapsule (0,radius,length);
352 dMassSetCapsule (&m2,DENSITY,3,radius,length);
353 }
354 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
355
356 // set the transformation (adjust the mass too)
357 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
358 dMatrix3 Rtx;
359 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
360 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
361 dGeomSetRotation (g2[k],Rtx);
362 dMassRotate (&m2,Rtx);
363
364 // Translation *after* rotation
365 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
366
367 // add to the total mass
368 dMassAdd (&m,&m2);
369 }
370
371 // move all encapsulated objects so that the center of mass is (0,0,0)
372 for (k=0; k<GPB; k++) {
373 dGeomSetPosition (g2[k],
374 dpos[k][0]-m.c[0],
375 dpos[k][1]-m.c[1],
376 dpos[k][2]-m.c[2]);
377 }
378 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
379 }
380
381 if (!setBody)
382 for (k=0; k < GPB; k++) {
383 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
384 }
385
386 dBodySetMass (obj[i].body,&m);
387 }
388
389 if (cmd == ' ') {
390 selected++;
391 if (selected >= num) selected = 0;
392 if (selected < 0) selected = 0;
393 }
394 else if (cmd == 'd' && selected >= 0 && selected < num) {
395 dBodyDisable (obj[selected].body);
396 }
397 else if (cmd == 'e' && selected >= 0 && selected < num) {
398 dBodyEnable (obj[selected].body);
399 }
400 else if (cmd == 'a') {
401 show_aabb ^= 1;
402 }
403 else if (cmd == 't') {
404 show_contacts ^= 1;
405 }
406 else if (cmd == 'r') {
407 random_pos ^= 1;
408 }
409 else if (cmd == '1') {
410 write_world = 1;
411 }
412}
413
414
415// draw a geom
416
417void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
418{
419 int i;
420
421 if (!g) return;
422 if (!pos) pos = dGeomGetPosition (g);
423 if (!R) R = dGeomGetRotation (g);
424
425 int type = dGeomGetClass (g);
426 if (type == dBoxClass) {
427 dVector3 sides;
428 dGeomBoxGetLengths (g,sides);
429 dsDrawBox (pos,R,sides);
430 }
431 else if (type == dSphereClass) {
432 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
433 }
434 else if (type == dCapsuleClass) {
435 dReal radius,length;
436 dGeomCapsuleGetParams (g,&radius,&length);
437 dsDrawCapsule (pos,R,length,radius);
438 }
439 //<---- Convex Object
440 else if (type == dConvexClass)
441 {
442 //dVector3 sides={0.50,0.50,0.50};
443 dsDrawConvex(pos,R,planes,
444 planecount,
445 points,
446 pointcount,
447 polygons);
448 }
449 //----> Convex Object
450 else if (type == dCylinderClass) {
451 dReal radius,length;
452 dGeomCylinderGetParams (g,&radius,&length);
453 dsDrawCylinder (pos,R,length,radius);
454 }
455 else if (type == dGeomTransformClass) {
456 dGeomID g2 = dGeomTransformGetGeom (g);
457 const dReal *pos2 = dGeomGetPosition (g2);
458 const dReal *R2 = dGeomGetRotation (g2);
459 dVector3 actual_pos;
460 dMatrix3 actual_R;
461 dMULTIPLY0_331 (actual_pos,R,pos2);
462 actual_pos[0] += pos[0];
463 actual_pos[1] += pos[1];
464 actual_pos[2] += pos[2];
465 dMULTIPLY0_333 (actual_R,R,R2);
466 drawGeom (g2,actual_pos,actual_R,0);
467 }
468 if (show_body) {
469 dBodyID body = dGeomGetBody(g);
470 if (body) {
471 const dReal *bodypos = dBodyGetPosition (body);
472 const dReal *bodyr = dBodyGetRotation (body);
473 dReal bodySides[3] = { 0.1, 0.1, 0.1 };
474 dsSetColorAlpha(0,1,0,1);
475 dsDrawBox(bodypos,bodyr,bodySides);
476 }
477 }
478 if (show_aabb) {
479 // draw the bounding box for this geom
480 dReal aabb[6];
481 dGeomGetAABB (g,aabb);
482 dVector3 bbpos;
483 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
484 dVector3 bbsides;
485 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
486 dMatrix3 RI;
487 dRSetIdentity (RI);
488 dsSetColorAlpha (1,0,0,0.5);
489 dsDrawBox (bbpos,RI,bbsides);
490 }
491}
492
493
494// simulation loop
495
496static void simLoop (int pause)
497{
498 dsSetColor (0,0,2);
499 dSpaceCollide (space,0,&nearCallback);
500 if (!pause) dWorldQuickStep (world,0.02);
501
502 if (write_world) {
503 FILE *f = fopen ("state.dif","wt");
504 if (f) {
505 dWorldExportDIF (world,f,"X");
506 fclose (f);
507 }
508 write_world = 0;
509 }
510
511 // remove all contact joints
512 dJointGroupEmpty (contactgroup);
513
514 dsSetColor (1,1,0);
515 dsSetTexture (DS_WOOD);
516 for (int i=0; i<num; i++) {
517 for (int j=0; j < GPB; j++) {
518 if (i==selected) {
519 dsSetColor (0,0.7,1);
520 }
521 else if (! dBodyIsEnabled (obj[i].body)) {
522 dsSetColor (1,0.8,0);
523 }
524 else {
525 dsSetColor (1,1,0);
526 }
527 drawGeom (obj[i].geom[j],0,0,show_aabb);
528 }
529 }
530}
531
532
533int main (int argc, char **argv)
534{
535 // setup pointers to drawstuff callback functions
536 dsFunctions fn;
537 fn.version = DS_VERSION;
538 fn.start = &start;
539 fn.step = &simLoop;
540 fn.command = &command;
541 fn.stop = 0;
542 fn.path_to_textures = "../../drawstuff/textures";
543 if(argc==2)
544 {
545 fn.path_to_textures = argv[1];
546 }
547
548 // create world
549 dInitODE();
550 world = dWorldCreate();
551 space = dHashSpaceCreate (0);
552 contactgroup = dJointGroupCreate (0);
553 dWorldSetGravity (world,0,0,-0.5);
554 dWorldSetCFM (world,1e-5);
555 dWorldSetAutoDisableFlag (world,1);
556
557#if 1
558
559 dWorldSetAutoDisableAverageSamplesCount( world, 10 );
560
561#endif
562
563
564 dWorldSetContactMaxCorrectingVel (world,0.1);
565 dWorldSetContactSurfaceLayer (world,0.001);
566 dCreatePlane (space,0,0,1,0);
567 memset (obj,0,sizeof(obj));
568
569 // run simulation
570 dsSimulationLoop (argc,argv,352,288,&fn);
571
572 dJointGroupDestroy (contactgroup);
573 dSpaceDestroy (space);
574 dWorldDestroy (world);
575 dCloseODE();
576 return 0;
577}
diff --git a/libraries/ode-0.9/ode/demo/demo_buggy.cpp b/libraries/ode-0.9/ode/demo/demo_buggy.cpp
new file mode 100644
index 0000000..a482e02
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_buggy.cpp
@@ -0,0 +1,325 @@
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/*
24
25buggy with suspension.
26this also shows you how to use geom groups.
27
28*/
29
30
31#include <ode/ode.h>
32#include <drawstuff/drawstuff.h>
33
34#ifdef _MSC_VER
35#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
36#endif
37
38// select correct drawing functions
39
40#ifdef dDOUBLE
41#define dsDrawBox dsDrawBoxD
42#define dsDrawSphere dsDrawSphereD
43#define dsDrawCylinder dsDrawCylinderD
44#define dsDrawCapsule dsDrawCapsuleD
45#endif
46
47
48// some constants
49
50#define LENGTH 0.7 // chassis length
51#define WIDTH 0.5 // chassis width
52#define HEIGHT 0.2 // chassis height
53#define RADIUS 0.18 // wheel radius
54#define STARTZ 0.5 // starting height of chassis
55#define CMASS 1 // chassis mass
56#define WMASS 0.2 // wheel mass
57
58
59// dynamics and collision objects (chassis, 3 wheels, environment)
60
61static dWorldID world;
62static dSpaceID space;
63static dBodyID body[4];
64static dJointID joint[3]; // joint[0] is the front wheel
65static dJointGroupID contactgroup;
66static dGeomID ground;
67static dSpaceID car_space;
68static dGeomID box[1];
69static dGeomID sphere[3];
70static dGeomID ground_box;
71
72
73// things that the user controls
74
75static dReal speed=0,steer=0; // user commands
76
77
78
79// this is called by dSpaceCollide when two objects in space are
80// potentially colliding.
81
82static void nearCallback (void *data, dGeomID o1, dGeomID o2)
83{
84 int i,n;
85
86 // only collide things with the ground
87 int g1 = (o1 == ground || o1 == ground_box);
88 int g2 = (o2 == ground || o2 == ground_box);
89 if (!(g1 ^ g2)) return;
90
91 const int N = 10;
92 dContact contact[N];
93 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
94 if (n > 0) {
95 for (i=0; i<n; i++) {
96 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
97 dContactSoftERP | dContactSoftCFM | dContactApprox1;
98 contact[i].surface.mu = dInfinity;
99 contact[i].surface.slip1 = 0.1;
100 contact[i].surface.slip2 = 0.1;
101 contact[i].surface.soft_erp = 0.5;
102 contact[i].surface.soft_cfm = 0.3;
103 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
104 dJointAttach (c,
105 dGeomGetBody(contact[i].geom.g1),
106 dGeomGetBody(contact[i].geom.g2));
107 }
108 }
109}
110
111
112// start simulation - set viewpoint
113
114static void start()
115{
116 static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
117 static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
118 dsSetViewpoint (xyz,hpr);
119 printf ("Press:\t'a' to increase speed.\n"
120 "\t'z' to decrease speed.\n"
121 "\t',' to steer left.\n"
122 "\t'.' to steer right.\n"
123 "\t' ' to reset speed and steering.\n"
124 "\t'1' to save the current state to 'state.dif'.\n");
125}
126
127
128// called when a key pressed
129
130static void command (int cmd)
131{
132 switch (cmd) {
133 case 'a': case 'A':
134 speed += 0.3;
135 break;
136 case 'z': case 'Z':
137 speed -= 0.3;
138 break;
139 case ',':
140 steer -= 0.5;
141 break;
142 case '.':
143 steer += 0.5;
144 break;
145 case ' ':
146 speed = 0;
147 steer = 0;
148 break;
149 case '1': {
150 FILE *f = fopen ("state.dif","wt");
151 if (f) {
152 dWorldExportDIF (world,f,"");
153 fclose (f);
154 }
155 }
156 }
157}
158
159
160// simulation loop
161
162static void simLoop (int pause)
163{
164 int i;
165 if (!pause) {
166 // motor
167 dJointSetHinge2Param (joint[0],dParamVel2,-speed);
168 dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
169
170 // steering
171 dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
172 if (v > 0.1) v = 0.1;
173 if (v < -0.1) v = -0.1;
174 v *= 10.0;
175 dJointSetHinge2Param (joint[0],dParamVel,v);
176 dJointSetHinge2Param (joint[0],dParamFMax,0.2);
177 dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
178 dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
179 dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
180
181 dSpaceCollide (space,0,&nearCallback);
182 dWorldStep (world,0.05);
183
184 // remove all contact joints
185 dJointGroupEmpty (contactgroup);
186 }
187
188 dsSetColor (0,1,1);
189 dsSetTexture (DS_WOOD);
190 dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
191 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
192 dsSetColor (1,1,1);
193 for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
194 dBodyGetRotation(body[i]),0.02f,RADIUS);
195
196 dVector3 ss;
197 dGeomBoxGetLengths (ground_box,ss);
198 dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
199
200 /*
201 printf ("%.10f %.10f %.10f %.10f\n",
202 dJointGetHingeAngle (joint[1]),
203 dJointGetHingeAngle (joint[2]),
204 dJointGetHingeAngleRate (joint[1]),
205 dJointGetHingeAngleRate (joint[2]));
206 */
207}
208
209
210int main (int argc, char **argv)
211{
212 int i;
213 dMass m;
214
215 // setup pointers to drawstuff callback functions
216 dsFunctions fn;
217 fn.version = DS_VERSION;
218 fn.start = &start;
219 fn.step = &simLoop;
220 fn.command = &command;
221 fn.stop = 0;
222 fn.path_to_textures = "../../drawstuff/textures";
223 if(argc==2)
224 {
225 fn.path_to_textures = argv[1];
226 }
227
228 // create world
229 dInitODE();
230 world = dWorldCreate();
231 space = dHashSpaceCreate (0);
232 contactgroup = dJointGroupCreate (0);
233 dWorldSetGravity (world,0,0,-0.5);
234 ground = dCreatePlane (space,0,0,1,0);
235
236 // chassis body
237 body[0] = dBodyCreate (world);
238 dBodySetPosition (body[0],0,0,STARTZ);
239 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
240 dMassAdjust (&m,CMASS);
241 dBodySetMass (body[0],&m);
242 box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
243 dGeomSetBody (box[0],body[0]);
244
245 // wheel bodies
246 for (i=1; i<=3; i++) {
247 body[i] = dBodyCreate (world);
248 dQuaternion q;
249 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
250 dBodySetQuaternion (body[i],q);
251 dMassSetSphere (&m,1,RADIUS);
252 dMassAdjust (&m,WMASS);
253 dBodySetMass (body[i],&m);
254 sphere[i-1] = dCreateSphere (0,RADIUS);
255 dGeomSetBody (sphere[i-1],body[i]);
256 }
257 dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
258 dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
259 dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
260
261 // front wheel hinge
262 /*
263 joint[0] = dJointCreateHinge2 (world,0);
264 dJointAttach (joint[0],body[0],body[1]);
265 const dReal *a = dBodyGetPosition (body[1]);
266 dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
267 dJointSetHinge2Axis1 (joint[0],0,0,1);
268 dJointSetHinge2Axis2 (joint[0],0,1,0);
269 */
270
271 // front and back wheel hinges
272 for (i=0; i<3; i++) {
273 joint[i] = dJointCreateHinge2 (world,0);
274 dJointAttach (joint[i],body[0],body[i+1]);
275 const dReal *a = dBodyGetPosition (body[i+1]);
276 dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
277 dJointSetHinge2Axis1 (joint[i],0,0,1);
278 dJointSetHinge2Axis2 (joint[i],0,1,0);
279 }
280
281 // set joint suspension
282 for (i=0; i<3; i++) {
283 dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
284 dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
285 }
286
287 // lock back wheels along the steering axis
288 for (i=1; i<3; i++) {
289 // set stops to make sure wheels always stay in alignment
290 dJointSetHinge2Param (joint[i],dParamLoStop,0);
291 dJointSetHinge2Param (joint[i],dParamHiStop,0);
292 // the following alternative method is no good as the wheels may get out
293 // of alignment:
294 // dJointSetHinge2Param (joint[i],dParamVel,0);
295 // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
296 }
297
298 // create car space and add it to the top level space
299 car_space = dSimpleSpaceCreate (space);
300 dSpaceSetCleanup (car_space,0);
301 dSpaceAdd (car_space,box[0]);
302 dSpaceAdd (car_space,sphere[0]);
303 dSpaceAdd (car_space,sphere[1]);
304 dSpaceAdd (car_space,sphere[2]);
305
306 // environment
307 ground_box = dCreateBox (space,2,1.5,1);
308 dMatrix3 R;
309 dRFromAxisAndAngle (R,0,1,0,-0.15);
310 dGeomSetPosition (ground_box,2,0,-0.34);
311 dGeomSetRotation (ground_box,R);
312
313 // run simulation
314 dsSimulationLoop (argc,argv,352,288,&fn);
315
316 dGeomDestroy (box[0]);
317 dGeomDestroy (sphere[0]);
318 dGeomDestroy (sphere[1]);
319 dGeomDestroy (sphere[2]);
320 dJointGroupDestroy (contactgroup);
321 dSpaceDestroy (space);
322 dWorldDestroy (world);
323 dCloseODE();
324 return 0;
325}
diff --git a/libraries/ode-0.9/ode/demo/demo_chain1.c b/libraries/ode-0.9/ode/demo/demo_chain1.c
new file mode 100644
index 0000000..e20c5dc
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_chain1.c
@@ -0,0 +1,171 @@
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/* exercise the C interface */
24
25#include <stdio.h>
26#include "ode/ode.h"
27#include "drawstuff/drawstuff.h"
28
29#ifdef _MSC_VER
30#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
31#endif
32
33/* select correct drawing functions */
34
35#ifdef dDOUBLE
36#define dsDrawBox dsDrawBoxD
37#define dsDrawSphere dsDrawSphereD
38#define dsDrawCylinder dsDrawCylinderD
39#define dsDrawCapsule dsDrawCapsuleD
40#endif
41
42
43/* some constants */
44
45#define NUM 10 /* number of boxes */
46#define SIDE (0.2) /* side length of a box */
47#define MASS (1.0) /* mass of a box */
48#define RADIUS (0.1732f) /* sphere radius */
49
50
51/* dynamics and collision objects */
52
53static dWorldID world;
54static dSpaceID space;
55static dBodyID body[NUM];
56static dJointID joint[NUM-1];
57static dJointGroupID contactgroup;
58static dGeomID sphere[NUM];
59
60
61/* this is called by dSpaceCollide when two objects in space are
62 * potentially colliding.
63 */
64
65static void nearCallback (void *data, dGeomID o1, dGeomID o2)
66{
67 /* exit without doing anything if the two bodies are connected by a joint */
68 dBodyID b1,b2;
69 dContact contact;
70
71 b1 = dGeomGetBody(o1);
72 b2 = dGeomGetBody(o2);
73 if (b1 && b2 && dAreConnected (b1,b2)) return;
74
75 contact.surface.mode = 0;
76 contact.surface.mu = 0.1;
77 contact.surface.mu2 = 0;
78 if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
79 dJointID c = dJointCreateContact (world,contactgroup,&contact);
80 dJointAttach (c,b1,b2);
81 }
82}
83
84
85/* start simulation - set viewpoint */
86
87static void start()
88{
89 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
90 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
91 dsSetViewpoint (xyz,hpr);
92}
93
94
95/* simulation loop */
96
97static void simLoop (int pause)
98{
99 int i;
100 if (!pause) {
101 static double angle = 0;
102 angle += 0.05;
103 dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0));
104
105 dSpaceCollide (space,0,&nearCallback);
106 dWorldStep (world,0.05);
107
108 /* remove all contact joints */
109 dJointGroupEmpty (contactgroup);
110 }
111
112 dsSetColor (1,1,0);
113 dsSetTexture (DS_WOOD);
114 for (i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]),
115 dBodyGetRotation(body[i]),RADIUS);
116}
117
118
119int main (int argc, char **argv)
120{
121 int i;
122 dReal k;
123 dMass m;
124
125 /* setup pointers to drawstuff callback functions */
126 dsFunctions fn;
127 fn.version = DS_VERSION;
128 fn.start = &start;
129 fn.step = &simLoop;
130 fn.command = 0;
131 fn.stop = 0;
132 fn.path_to_textures = "../../drawstuff/textures";
133 if(argc==2)
134 {
135 fn.path_to_textures = argv[1];
136 }
137
138 /* create world */
139 dInitODE();
140 world = dWorldCreate();
141 space = dHashSpaceCreate (0);
142 contactgroup = dJointGroupCreate (1000000);
143 dWorldSetGravity (world,0,0,-0.5);
144 dCreatePlane (space,0,0,1,0);
145
146 for (i=0; i<NUM; i++) {
147 body[i] = dBodyCreate (world);
148 k = i*SIDE;
149 dBodySetPosition (body[i],k,k,k+0.4);
150 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
151 dMassAdjust (&m,MASS);
152 dBodySetMass (body[i],&m);
153 sphere[i] = dCreateSphere (space,RADIUS);
154 dGeomSetBody (sphere[i],body[i]);
155 }
156 for (i=0; i<(NUM-1); i++) {
157 joint[i] = dJointCreateBall (world,0);
158 dJointAttach (joint[i],body[i],body[i+1]);
159 k = (i+0.5)*SIDE;
160 dJointSetBallAnchor (joint[i],k,k,k+0.4);
161 }
162
163 /* run simulation */
164 dsSimulationLoop (argc,argv,352,288,&fn);
165
166 dJointGroupDestroy (contactgroup);
167 dSpaceDestroy (space);
168 dWorldDestroy (world);
169 dCloseODE();
170 return 0;
171}
diff --git a/libraries/ode-0.9/ode/demo/demo_chain2.cpp b/libraries/ode-0.9/ode/demo/demo_chain2.cpp
new file mode 100644
index 0000000..cf35971
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_chain2.cpp
@@ -0,0 +1,165 @@
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/* exercise the C++ interface */
24
25#include <ode/ode.h>
26#include <drawstuff/drawstuff.h>
27
28#ifdef _MSC_VER
29#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
30#endif
31
32// select correct drawing functions
33
34#ifdef dDOUBLE
35#define dsDrawBox dsDrawBoxD
36#define dsDrawSphere dsDrawSphereD
37#define dsDrawCylinder dsDrawCylinderD
38#define dsDrawCapsule dsDrawCapsuleD
39#endif
40
41
42// some constants
43
44#define NUM 10 // number of boxes
45#define SIDE (0.2) // side length of a box
46#define MASS (1.0) // mass of a box
47#define RADIUS (0.1732f) // sphere radius
48
49
50// dynamics and collision objects
51
52static dWorld world;
53static dSimpleSpace space (0);
54static dBody body[NUM];
55static dBallJoint joint[NUM-1];
56static dJointGroup contactgroup;
57static dBox box[NUM];
58
59
60// this is called by space.collide when two objects in space are
61// potentially colliding.
62
63static void nearCallback (void *data, dGeomID o1, dGeomID o2)
64{
65 // exit without doing anything if the two bodies are connected by a joint
66 dBodyID b1 = dGeomGetBody(o1);
67 dBodyID b2 = dGeomGetBody(o2);
68 if (b1 && b2 && dAreConnected (b1,b2)) return;
69
70 // @@@ it's still more convenient to use the C interface here.
71
72 dContact contact;
73 contact.surface.mode = 0;
74 contact.surface.mu = dInfinity;
75 if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
76 dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
77 dJointAttach (c,b1,b2);
78 }
79}
80
81
82// start simulation - set viewpoint
83
84static void start()
85{
86 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
87 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
88 dsSetViewpoint (xyz,hpr);
89}
90
91
92// simulation loop
93
94static void simLoop (int pause)
95{
96 if (!pause) {
97 static double angle = 0;
98 angle += 0.05;
99 body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0));
100
101 space.collide (0,&nearCallback);
102 world.step (0.05);
103
104 // remove all contact joints
105 contactgroup.empty();
106 }
107
108 dReal sides[3] = {SIDE,SIDE,SIDE};
109 dsSetColor (1,1,0);
110 dsSetTexture (DS_WOOD);
111 for (int i=0; i<NUM; i++)
112 dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
113}
114
115
116int main (int argc, char **argv)
117{
118 // setup pointers to drawstuff callback functions
119 dsFunctions fn;
120 fn.version = DS_VERSION;
121 fn.start = &start;
122 fn.step = &simLoop;
123 fn.command = 0;
124 fn.stop = 0;
125 fn.path_to_textures = "../../drawstuff/textures";
126 if(argc==2)
127 {
128 fn.path_to_textures = argv[1];
129 }
130
131 // create world
132 dInitODE();
133
134 int i;
135 contactgroup.create (0);
136 world.setGravity (0,0,-0.5);
137 dWorldSetCFM (world.id(),1e-5);
138 dPlane plane (space,0,0,1,0);
139
140 for (i=0; i<NUM; i++) {
141 body[i].create (world);
142 dReal k = i*SIDE;
143 body[i].setPosition (k,k,k+0.4);
144 dMass m;
145 m.setBox (1,SIDE,SIDE,SIDE);
146 m.adjust (MASS);
147 body[i].setMass (&m);
148 body[i].setData ((void*)(size_t)i);
149
150 box[i].create (space,SIDE,SIDE,SIDE);
151 box[i].setBody (body[i]);
152 }
153 for (i=0; i<(NUM-1); i++) {
154 joint[i].create (world);
155 joint[i].attach (body[i],body[i+1]);
156 dReal k = (i+0.5)*SIDE;
157 joint[i].setAnchor (k,k,k+0.4);
158 }
159
160 // run simulation
161 dsSimulationLoop (argc,argv,352,288,&fn);
162
163 dCloseODE();
164 return 0;
165}
diff --git a/libraries/ode-0.9/ode/demo/demo_collision.cpp b/libraries/ode-0.9/ode/demo/demo_collision.cpp
new file mode 100644
index 0000000..8f591e9
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_collision.cpp
@@ -0,0 +1,1371 @@
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/*
24
25collision tests. if this program is run without any arguments it will
26perform all the tests multiple times, with different random data for each
27test. if this program is given a test number it will run that test
28graphically/interactively, in which case the space bar can be used to
29change the random test conditions.
30
31*/
32
33
34#include <ode/ode.h>
35#include <drawstuff/drawstuff.h>
36
37#ifdef _MSC_VER
38#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
39#endif
40
41// select correct drawing functions
42#ifdef dDOUBLE
43#define dsDrawSphere dsDrawSphereD
44#define dsDrawBox dsDrawBoxD
45#define dsDrawLine dsDrawLineD
46#define dsDrawCapsule dsDrawCapsuleD
47#endif
48
49//****************************************************************************
50// test infrastructure, including constants and macros
51
52#define TEST_REPS1 1000 // run each test this many times (first batch)
53#define TEST_REPS2 10000 // run each test this many times (second batch)
54const dReal tol = 1e-8; // tolerance used for numerical checks
55#define MAX_TESTS 1000 // maximum number of test slots
56#define Z_OFFSET 2 // z offset for drawing (to get above ground)
57
58
59// test function. returns 1 if the test passed or 0 if it failed
60typedef int test_function_t();
61
62struct TestSlot {
63 int number; // number of test
64 char *name; // name of test
65 int failcount;
66 test_function_t *test_fn;
67 int last_failed_line;
68};
69TestSlot testslot[MAX_TESTS];
70
71
72// globals used by the test functions
73int graphical_test=0; // show graphical results of this test, 0=none
74int current_test; // currently execiting test
75int draw_all_objects_called;
76
77
78#define MAKE_TEST(number,function) \
79 if (testslot[number].name) dDebug (0,"test number already used"); \
80 if (number <= 0 || number >= MAX_TESTS) dDebug (0,"bad test number"); \
81 testslot[number].name = # function; \
82 testslot[number].test_fn = function;
83
84#define FAILED() { if (graphical_test==0) { \
85 testslot[current_test].last_failed_line=__LINE__; return 0; } }
86#define PASSED() { return 1; }
87
88//****************************************************************************
89// globals
90
91/* int dBoxBox (const dVector3 p1, const dMatrix3 R1,
92 const dVector3 side1, const dVector3 p2,
93 const dMatrix3 R2, const dVector3 side2,
94 dVector3 normal, dReal *depth, int *code,
95 int maxc, dContactGeom *contact, int skip); */
96
97void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
98 const dVector3 pb, const dVector3 ub,
99 dReal *alpha, dReal *beta);
100
101//****************************************************************************
102// draw all objects in a space, and draw all the collision contact points
103
104void nearCallback (void *data, dGeomID o1, dGeomID o2)
105{
106 int i,j,n;
107 const int N = 100;
108 dContactGeom contact[N];
109
110 if (dGeomGetClass (o2) == dRayClass) {
111 n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom));
112 }
113 else {
114 n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom));
115 }
116 if (n > 0) {
117 dMatrix3 RI;
118 dRSetIdentity (RI);
119 const dReal ss[3] = {0.01,0.01,0.01};
120 for (i=0; i<n; i++) {
121 contact[i].pos[2] += Z_OFFSET;
122 dsDrawBox (contact[i].pos,RI,ss);
123 dVector3 n;
124 for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j];
125 dsDrawLine (contact[i].pos,n);
126 }
127 }
128}
129
130
131void draw_all_objects (dSpaceID space)
132{
133 int i, j;
134
135 draw_all_objects_called = 1;
136 if (!graphical_test) return;
137 int n = dSpaceGetNumGeoms (space);
138
139 // draw all contact points
140 dsSetColor (0,1,1);
141 dSpaceCollide (space,0,&nearCallback);
142
143 // draw all rays
144 for (i=0; i<n; i++) {
145 dGeomID g = dSpaceGetGeom (space,i);
146 if (dGeomGetClass (g) == dRayClass) {
147 dsSetColor (1,1,1);
148 dVector3 origin,dir;
149 dGeomRayGet (g,origin,dir);
150 origin[2] += Z_OFFSET;
151 dReal length = dGeomRayGetLength (g);
152 for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j];
153 dsDrawLine (origin,dir);
154 dsSetColor (0,0,1);
155 dsDrawSphere (origin,dGeomGetRotation(g),0.01);
156 }
157 }
158
159 // draw all other objects
160 for (i=0; i<n; i++) {
161 dGeomID g = dSpaceGetGeom (space,i);
162 dVector3 pos;
163 if (dGeomGetClass (g) != dPlaneClass) {
164 memcpy (pos,dGeomGetPosition(g),sizeof(pos));
165 pos[2] += Z_OFFSET;
166 }
167
168 switch (dGeomGetClass (g)) {
169
170 case dSphereClass: {
171 dsSetColorAlpha (1,0,0,0.8);
172 dReal radius = dGeomSphereGetRadius (g);
173 dsDrawSphere (pos,dGeomGetRotation(g),radius);
174 break;
175 }
176
177 case dBoxClass: {
178 dsSetColorAlpha (1,1,0,0.8);
179 dVector3 sides;
180 dGeomBoxGetLengths (g,sides);
181 dsDrawBox (pos,dGeomGetRotation(g),sides);
182 break;
183 }
184
185 case dCapsuleClass: {
186 dsSetColorAlpha (0,1,0,0.8);
187 dReal radius,length;
188 dGeomCapsuleGetParams (g,&radius,&length);
189 dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
190 break;
191 }
192
193 case dPlaneClass: {
194 dVector4 n;
195 dMatrix3 R,sides;
196 dVector3 pos2;
197 dGeomPlaneGetParams (g,n);
198 dRFromZAxis (R,n[0],n[1],n[2]);
199 for (j=0; j<3; j++) pos[j] = n[j]*n[3];
200 pos[2] += Z_OFFSET;
201 sides[0] = 2;
202 sides[1] = 2;
203 sides[2] = 0.001;
204 dsSetColor (1,0,1);
205 for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
206 dsDrawLine (pos,pos2);
207 dsSetColorAlpha (1,0,1,0.8);
208 dsDrawBox (pos,R,sides);
209 break;
210 }
211
212 }
213 }
214}
215
216//****************************************************************************
217// point depth tests
218
219int test_sphere_point_depth()
220{
221 int j;
222 dVector3 p,q;
223 dMatrix3 R;
224 dReal r,d;
225
226 dSimpleSpace space(0);
227 dGeomID sphere = dCreateSphere (0,1);
228 dSpaceAdd (space,sphere);
229
230 // ********** make a random sphere of radius r at position p
231
232 r = dRandReal()+0.1;
233 dGeomSphereSetRadius (sphere,r);
234 dMakeRandomVector (p,3,1.0);
235 dGeomSetPosition (sphere,p[0],p[1],p[2]);
236 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
237 dRandReal()*2-1,dRandReal()*10-5);
238 dGeomSetRotation (sphere,R);
239
240 // ********** test center point has depth r
241
242 if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();
243
244 // ********** test point on surface has depth 0
245
246 for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
247 dNormalize3 (q);
248 for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
249 if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();
250
251 // ********** test point at random depth
252
253 d = (dRandReal()*2-1) * r;
254 for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
255 dNormalize3 (q);
256 for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
257 if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();
258
259 PASSED();
260}
261
262
263int test_box_point_depth()
264{
265 int i,j;
266 dVector3 s,p,q,q2; // s = box sides
267 dMatrix3 R;
268 dReal ss,d; // ss = smallest side
269
270 dSimpleSpace space(0);
271 dGeomID box = dCreateBox (0,1,1,1);
272 dSpaceAdd (space,box);
273
274 // ********** make a random box
275
276 for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
277 dGeomBoxSetLengths (box,s[0],s[1],s[2]);
278 dMakeRandomVector (p,3,1.0);
279 dGeomSetPosition (box,p[0],p[1],p[2]);
280 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
281 dRandReal()*2-1,dRandReal()*10-5);
282 dGeomSetRotation (box,R);
283
284 // ********** test center point has depth of smallest side
285
286 ss = 1e9;
287 for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
288 if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
289 FAILED();
290
291 // ********** test point on surface has depth 0
292
293 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
294 i = dRandInt (3);
295 if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
296 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
297 for (j=0; j<3; j++) q2[j] += p[j];
298 if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();
299
300 // ********** test points outside box have -ve depth
301
302 for (j=0; j<3; j++) {
303 q[j] = 0.5*s[j] + dRandReal() + 0.01;
304 if (dRandReal() > 0.5) q[j] = -q[j];
305 }
306 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
307 for (j=0; j<3; j++) q2[j] += p[j];
308 if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();
309
310 // ********** test points inside box have +ve depth
311
312 for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
313 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
314 for (j=0; j<3; j++) q2[j] += p[j];
315 if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();
316
317 // ********** test random depth of point aligned along axis (up to ss deep)
318
319 i = dRandInt (3);
320 for (j=0; j<3; j++) q[j] = 0;
321 d = (dRandReal()*(ss*0.5+1)-1);
322 q[i] = s[i]*0.5 - d;
323 if (dRandReal() > 0.5) q[i] = -q[i];
324 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
325 for (j=0; j<3; j++) q2[j] += p[j];
326 if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();
327
328 PASSED();
329}
330
331
332int test_ccylinder_point_depth()
333{
334 int j;
335 dVector3 p,a;
336 dMatrix3 R;
337 dReal r,l,beta,x,y,d;
338
339 dSimpleSpace space(0);
340 dGeomID ccyl = dCreateCapsule (0,1,1);
341 dSpaceAdd (space,ccyl);
342
343 // ********** make a random ccyl
344
345 r = dRandReal()*0.5 + 0.01;
346 l = dRandReal()*1 + 0.01;
347 dGeomCapsuleSetParams (ccyl,r,l);
348 dMakeRandomVector (p,3,1.0);
349 dGeomSetPosition (ccyl,p[0],p[1],p[2]);
350 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
351 dRandReal()*2-1,dRandReal()*10-5);
352 dGeomSetRotation (ccyl,R);
353
354 // ********** test point on axis has depth of 'radius'
355
356 beta = dRandReal()-0.5;
357 for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
358 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
359 FAILED();
360
361 // ********** test point on surface (excluding caps) has depth 0
362
363 beta = dRandReal()*2*M_PI;
364 x = r*sin(beta);
365 y = r*cos(beta);
366 beta = dRandReal()-0.5;
367 for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
368 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
369
370 // ********** test point on surface of caps has depth 0
371
372 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
373 dNormalize3 (a);
374 if (dDOT14(a,R+2) > 0) {
375 for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
376 }
377 else {
378 for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
379 }
380 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
381
382 // ********** test point inside ccyl has positive depth
383
384 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
385 dNormalize3 (a);
386 beta = dRandReal()-0.5;
387 for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
388 if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();
389
390 // ********** test point depth (1)
391
392 d = (dRandReal()*2-1) * r;
393 beta = dRandReal()*2*M_PI;
394 x = (r-d)*sin(beta);
395 y = (r-d)*cos(beta);
396 beta = dRandReal()-0.5;
397 for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
398 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
399 FAILED();
400
401 // ********** test point depth (2)
402
403 d = (dRandReal()*2-1) * r;
404 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
405 dNormalize3 (a);
406 if (dDOT14(a,R+2) > 0) {
407 for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
408 }
409 else {
410 for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
411 }
412 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
413 FAILED();
414
415 PASSED();
416}
417
418
419int test_plane_point_depth()
420{
421 int j;
422 dVector3 n,p,q,a,b; // n = plane normal
423 dReal d;
424
425 dSimpleSpace space(0);
426 dGeomID plane = dCreatePlane (0,0,0,1,0);
427 dSpaceAdd (space,plane);
428
429 // ********** make a random plane
430
431 for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
432 dNormalize3 (n);
433 d = dRandReal() - 0.5;
434 dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
435 dPlaneSpace (n,p,q);
436
437 // ********** test point on plane has depth 0
438
439 a[0] = dRandReal() - 0.5;
440 a[1] = dRandReal() - 0.5;
441 a[2] = 0;
442 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
443 if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
444
445 // ********** test arbitrary depth point
446
447 a[0] = dRandReal() - 0.5;
448 a[1] = dRandReal() - 0.5;
449 a[2] = dRandReal() - 0.5;
450 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
451 if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
452 FAILED();
453
454 // ********** test depth-1 point
455
456 a[0] = dRandReal() - 0.5;
457 a[1] = dRandReal() - 0.5;
458 a[2] = -1;
459 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
460 if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
461
462 PASSED();
463}
464
465//****************************************************************************
466// ray tests
467
468int test_ray_and_sphere()
469{
470 int j;
471 dContactGeom contact;
472 dVector3 p,q,q2,n,v1;
473 dMatrix3 R;
474 dReal r,k;
475
476 dSimpleSpace space(0);
477 dGeomID ray = dCreateRay (0,0);
478 dGeomID sphere = dCreateSphere (0,1);
479 dSpaceAdd (space,ray);
480 dSpaceAdd (space,sphere);
481
482 // ********** make a random sphere of radius r at position p
483
484 r = dRandReal()+0.1;
485 dGeomSphereSetRadius (sphere,r);
486 dMakeRandomVector (p,3,1.0);
487 dGeomSetPosition (sphere,p[0],p[1],p[2]);
488 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
489 dRandReal()*2-1,dRandReal()*10-5);
490 dGeomSetRotation (sphere,R);
491
492 // ********** test zero length ray just inside sphere
493
494 dGeomRaySetLength (ray,0);
495 dMakeRandomVector (q,3,1.0);
496 dNormalize3 (q);
497 for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
498 dGeomSetPosition (ray,q[0],q[1],q[2]);
499 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
500 dRandReal()*2-1,dRandReal()*10-5);
501 dGeomSetRotation (ray,R);
502 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
503
504 // ********** test zero length ray just outside that sphere
505
506 dGeomRaySetLength (ray,0);
507 dMakeRandomVector (q,3,1.0);
508 dNormalize3 (q);
509 for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
510 dGeomSetPosition (ray,q[0],q[1],q[2]);
511 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
512 dRandReal()*2-1,dRandReal()*10-5);
513 dGeomSetRotation (ray,R);
514 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
515
516 // ********** test finite length ray totally contained inside the sphere
517
518 dMakeRandomVector (q,3,1.0);
519 dNormalize3 (q);
520 k = dRandReal();
521 for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
522 dMakeRandomVector (q2,3,1.0);
523 dNormalize3 (q2);
524 k = dRandReal();
525 for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
526 for (j=0; j<3; j++) n[j] = q2[j] - q[j];
527 dNormalize3 (n);
528 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
529 dGeomRaySetLength (ray,dDISTANCE (q,q2));
530 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
531
532 // ********** test finite length ray totally outside the sphere
533
534 dMakeRandomVector (q,3,1.0);
535 dNormalize3 (q);
536 do {
537 dMakeRandomVector (n,3,1.0);
538 dNormalize3 (n);
539 }
540 while (dDOT(n,q) < 0); // make sure normal goes away from sphere
541 for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
542 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
543 dGeomRaySetLength (ray,100);
544 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
545
546 // ********** test ray from outside to just above surface
547
548 dMakeRandomVector (q,3,1.0);
549 dNormalize3 (q);
550 for (j=0; j<3; j++) n[j] = -q[j];
551 for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
552 dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
553 dGeomRaySetLength (ray,0.99*r);
554 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
555
556 // ********** test ray from outside to just below surface
557
558 dGeomRaySetLength (ray,1.01*r);
559 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
560 for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
561 if (dDISTANCE (contact.pos,q2) > tol) FAILED();
562
563 // ********** test contact point distance for random rays
564
565 dMakeRandomVector (q,3,1.0);
566 dNormalize3 (q);
567 k = dRandReal()+0.5;
568 for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
569 dMakeRandomVector (n,3,1.0);
570 dNormalize3 (n);
571 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
572 dGeomRaySetLength (ray,100);
573 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
574 k = dDISTANCE (contact.pos,dGeomGetPosition(sphere));
575 if (dFabs(k - r) > tol) FAILED();
576 // also check normal signs
577 if (dDOT (n,contact.normal) > 0) FAILED();
578 // also check depth of contact point
579 if (dFabs (dGeomSpherePointDepth
580 (sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
581 FAILED();
582
583 draw_all_objects (space);
584 }
585
586 // ********** test tangential grazing - miss
587
588 dMakeRandomVector (q,3,1.0);
589 dNormalize3 (q);
590 dPlaneSpace (q,n,v1);
591 for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
592 for (j=0; j<3; j++) q[j] -= n[j];
593 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
594 dGeomRaySetLength (ray,2);
595 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
596
597 // ********** test tangential grazing - hit
598
599 dMakeRandomVector (q,3,1.0);
600 dNormalize3 (q);
601 dPlaneSpace (q,n,v1);
602 for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
603 for (j=0; j<3; j++) q[j] -= n[j];
604 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
605 dGeomRaySetLength (ray,2);
606 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
607
608 PASSED();
609}
610
611
612int test_ray_and_box()
613{
614 int i,j;
615 dContactGeom contact;
616 dVector3 s,p,q,n,q2,q3,q4; // s = box sides
617 dMatrix3 R;
618 dReal k;
619
620 dSimpleSpace space(0);
621 dGeomID ray = dCreateRay (0,0);
622 dGeomID box = dCreateBox (0,1,1,1);
623 dSpaceAdd (space,ray);
624 dSpaceAdd (space,box);
625
626 // ********** make a random box
627
628 for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
629 dGeomBoxSetLengths (box,s[0],s[1],s[2]);
630 dMakeRandomVector (p,3,1.0);
631 dGeomSetPosition (box,p[0],p[1],p[2]);
632 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
633 dRandReal()*2-1,dRandReal()*10-5);
634 dGeomSetRotation (box,R);
635
636 // ********** test zero length ray just inside box
637
638 dGeomRaySetLength (ray,0);
639 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
640 i = dRandInt (3);
641 if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
642 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
643 for (j=0; j<3; j++) q2[j] += p[j];
644 dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
645 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
646 dRandReal()*2-1,dRandReal()*10-5);
647 dGeomSetRotation (ray,R);
648 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
649
650 // ********** test zero length ray just outside box
651
652 dGeomRaySetLength (ray,0);
653 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
654 i = dRandInt (3);
655 if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
656 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
657 for (j=0; j<3; j++) q2[j] += p[j];
658 dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
659 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
660 dRandReal()*2-1,dRandReal()*10-5);
661 dGeomSetRotation (ray,R);
662 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
663
664 // ********** test finite length ray totally contained inside the box
665
666 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
667 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
668 for (j=0; j<3; j++) q2[j] += p[j];
669 for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
670 dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
671 for (j=0; j<3; j++) q4[j] += p[j];
672 for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
673 dNormalize3 (n);
674 dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
675 dGeomRaySetLength (ray,dDISTANCE(q2,q4));
676 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
677
678 // ********** test finite length ray totally outside the box
679
680 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
681 i = dRandInt (3);
682 if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
683 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
684 for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
685 dNormalize3 (q2);
686 dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
687 dGeomRaySetLength (ray,10);
688 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
689
690 // ********** test ray from outside to just above surface
691
692 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
693 i = dRandInt (3);
694 if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
695 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
696 for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
697 k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
698 for (j=0; j<3; j++) q2[j] = -q2[j];
699 dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
700 dGeomRaySetLength (ray,k*0.99);
701 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
702
703 // ********** test ray from outside to just below surface
704
705 dGeomRaySetLength (ray,k*1.01);
706 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
707
708 // ********** test contact point position for random rays
709
710 for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
711 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
712 for (j=0; j<3; j++) q2[j] += p[j];
713 for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
714 dNormalize3 (q3);
715 dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
716 dGeomRaySetLength (ray,10);
717 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
718 // check depth of contact point
719 if (dFabs (dGeomBoxPointDepth
720 (box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
721 FAILED();
722 // check position of contact point
723 for (j=0; j<3; j++) contact.pos[j] -= p[j];
724 dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
725 if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
726 dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
727 dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
728 FAILED();
729 }
730 // also check normal signs
731 if (dDOT (q3,contact.normal) > 0) FAILED();
732
733 draw_all_objects (space);
734 }
735
736 PASSED();
737}
738
739
740int test_ray_and_ccylinder()
741{
742 int j;
743 dContactGeom contact;
744 dVector3 p,a,b,n;
745 dMatrix3 R;
746 dReal r,l,k,x,y;
747
748 dSimpleSpace space(0);
749 dGeomID ray = dCreateRay (0,0);
750 dGeomID ccyl = dCreateCapsule (0,1,1);
751 dSpaceAdd (space,ray);
752 dSpaceAdd (space,ccyl);
753
754 // ********** make a random capped cylinder
755
756 r = dRandReal()*0.5 + 0.01;
757 l = dRandReal()*1 + 0.01;
758 dGeomCapsuleSetParams (ccyl,r,l);
759 dMakeRandomVector (p,3,1.0);
760 dGeomSetPosition (ccyl,p[0],p[1],p[2]);
761 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
762 dRandReal()*2-1,dRandReal()*10-5);
763 dGeomSetRotation (ccyl,R);
764
765 // ********** test ray completely within ccyl
766
767 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
768 dNormalize3 (a);
769 k = (dRandReal()-0.5)*l;
770 for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
771 for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
772 dNormalize3 (b);
773 k = (dRandReal()-0.5)*l;
774 for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
775 dGeomRaySetLength (ray,dDISTANCE(a,b));
776 for (j=0; j<3; j++) b[j] -= a[j];
777 dNormalize3 (b);
778 dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
779 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
780
781 // ********** test ray outside ccyl that just misses (between caps)
782
783 k = dRandReal()*2*M_PI;
784 x = sin(k);
785 y = cos(k);
786 for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
787 k = (dRandReal()-0.5)*l;
788 for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
789 dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
790 dGeomRaySetLength (ray,r*0.99);
791 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
792
793 // ********** test ray outside ccyl that just hits (between caps)
794
795 dGeomRaySetLength (ray,r*1.01);
796 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
797 // check depth of contact point
798 if (dFabs (dGeomCapsulePointDepth
799 (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
800 FAILED();
801
802 // ********** test ray outside ccyl that just misses (caps)
803
804 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
805 dNormalize3 (a);
806 if (dDOT14(a,R+2) < 0) {
807 for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
808 }
809 else {
810 for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
811 }
812 dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
813 dGeomRaySetLength (ray,r*0.99);
814 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
815
816 // ********** test ray outside ccyl that just hits (caps)
817
818 dGeomRaySetLength (ray,r*1.01);
819 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
820 // check depth of contact point
821 if (dFabs (dGeomCapsulePointDepth
822 (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
823 FAILED();
824
825 // ********** test random rays
826
827 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
828 for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
829 dNormalize3 (n);
830 dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
831 dGeomRaySetLength (ray,10);
832
833 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
834 // check depth of contact point
835 if (dFabs (dGeomCapsulePointDepth
836 (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
837 FAILED();
838
839 // check normal signs
840 if (dDOT (n,contact.normal) > 0) FAILED();
841
842 draw_all_objects (space);
843 }
844
845 PASSED();
846}
847
848
849int test_ray_and_plane()
850{
851 int j;
852 dContactGeom contact;
853 dVector3 n,p,q,a,b,g,h; // n,d = plane parameters
854 dMatrix3 R;
855 dReal d;
856
857 dSimpleSpace space(0);
858 dGeomID ray = dCreateRay (0,0);
859 dGeomID plane = dCreatePlane (0,0,0,1,0);
860 dSpaceAdd (space,ray);
861 dSpaceAdd (space,plane);
862
863 // ********** make a random plane
864
865 for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
866 dNormalize3 (n);
867 d = dRandReal() - 0.5;
868 dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
869 dPlaneSpace (n,p,q);
870
871 // ********** test finite length ray below plane
872
873 dGeomRaySetLength (ray,0.09);
874 a[0] = dRandReal()-0.5;
875 a[1] = dRandReal()-0.5;
876 a[2] = -dRandReal()*0.5 - 0.1;
877 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
878 dGeomSetPosition (ray,b[0],b[1],b[2]);
879 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
880 dRandReal()*2-1,dRandReal()*10-5);
881 dGeomSetRotation (ray,R);
882 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
883
884 // ********** test finite length ray above plane
885
886 a[0] = dRandReal()-0.5;
887 a[1] = dRandReal()-0.5;
888 a[2] = dRandReal()*0.5 + 0.01;
889 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
890 g[0] = dRandReal()-0.5;
891 g[1] = dRandReal()-0.5;
892 g[2] = dRandReal() + 0.01;
893 for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
894 dNormalize3 (h);
895 dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
896 dGeomRaySetLength (ray,10);
897 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
898
899 // ********** test finite length ray that intersects plane
900
901 a[0] = dRandReal()-0.5;
902 a[1] = dRandReal()-0.5;
903 a[2] = dRandReal()-0.5;
904 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
905 g[0] = dRandReal()-0.5;
906 g[1] = dRandReal()-0.5;
907 g[2] = dRandReal()-0.5;
908 for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
909 dNormalize3 (h);
910 dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
911 dGeomRaySetLength (ray,10);
912 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
913 // test that contact is on plane surface
914 if (dFabs (dDOT(contact.pos,n) - d) > tol) FAILED();
915 // also check normal signs
916 if (dDOT (h,contact.normal) > 0) FAILED();
917 // also check contact point depth
918 if (dFabs (dGeomPlanePointDepth
919 (plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
920 FAILED();
921
922 draw_all_objects (space);
923 }
924
925 // ********** test ray that just misses
926
927 for (j=0; j<3; j++) b[j] = (1+d)*n[j];
928 for (j=0; j<3; j++) h[j] = -n[j];
929 dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
930 dGeomRaySetLength (ray,0.99);
931 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
932
933 // ********** test ray that just hits
934
935 dGeomRaySetLength (ray,1.01);
936 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
937
938 // ********** test polarity with typical ground plane
939
940 dGeomPlaneSetParams (plane,0,0,1,0);
941 for (j=0; j<3; j++) a[j] = 0.1;
942 for (j=0; j<3; j++) b[j] = 0;
943 a[2] = 1;
944 b[2] = -1;
945 dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
946 dGeomRaySetLength (ray,2);
947 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
948 if (dFabs (contact.depth - 1) > tol) FAILED();
949 a[2] = -1;
950 b[2] = 1;
951 dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
952 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
953 if (dFabs (contact.depth - 1) > tol) FAILED();
954
955 PASSED();
956}
957
958//****************************************************************************
959// a really inefficient, but hopefully correct implementation of
960// dBoxTouchesBox(), that does 144 edge-face tests.
961
962// return 1 if edge v1 -> v2 hits the rectangle described by p1,p2,p3
963
964static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
965 dVector3 p1, dVector3 p2, dVector3 p3)
966{
967 int k;
968 dVector3 u1,u2,n,tmp;
969 for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
970 for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
971 dReal d1 = dSqrt(dDOT(u1,u1));
972 dReal d2 = dSqrt(dDOT(u2,u2));
973 dNormalize3 (u1);
974 dNormalize3 (u2);
975 if (dFabs(dDOT(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
976 dCROSS (n,=,u1,u2);
977 for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
978 dReal d = -dDOT(n,p1);
979 if (dFabs(dDOT(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
980 if (dFabs(dDOT(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
981 if (dFabs(dDOT(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
982 dReal alpha = -(d+dDOT(n,v1))/dDOT(n,tmp);
983 for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
984 if (dFabs(dDOT(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
985 if (alpha < 0) return 0;
986 if (alpha > 1) return 0;
987 for (k=0; k<3; k++) tmp[k] -= p1[k];
988 dReal a1 = dDOT(u1,tmp);
989 dReal a2 = dDOT(u2,tmp);
990 if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
991 return 1;
992}
993
994
995// return 1 if box 1 is completely inside box 2
996
997static int box1inside2 (const dVector3 p1, const dMatrix3 R1,
998 const dVector3 side1, const dVector3 p2,
999 const dMatrix3 R2, const dVector3 side2)
1000{
1001 for (int i=-1; i<=1; i+=2) {
1002 for (int j=-1; j<=1; j+=2) {
1003 for (int k=-1; k<=1; k+=2) {
1004 dVector3 v,vv;
1005 v[0] = i*0.5*side1[0];
1006 v[1] = j*0.5*side1[1];
1007 v[2] = k*0.5*side1[2];
1008 dMULTIPLY0_331 (vv,R1,v);
1009 vv[0] += p1[0] - p2[0];
1010 vv[1] += p1[1] - p2[1];
1011 vv[2] += p1[2] - p2[2];
1012 for (int axis=0; axis < 3; axis++) {
1013 dReal z = dDOT14(vv,R2+axis);
1014 if (z < (-side2[axis]*0.5) || z > (side2[axis]*0.5)) return 0;
1015 }
1016 }
1017 }
1018 }
1019 return 1;
1020}
1021
1022
1023// test if any edge from box 1 hits a face from box 2
1024
1025static int testBoxesTouch2 (const dVector3 p1, const dMatrix3 R1,
1026 const dVector3 side1, const dVector3 p2,
1027 const dMatrix3 R2, const dVector3 side2)
1028{
1029 int j,k,j1,j2;
1030
1031 // for 6 faces from box 2
1032 for (int fd=0; fd<3; fd++) { // direction for face
1033
1034 for (int fo=0; fo<2; fo++) { // offset of face
1035 // get four points on the face. first get 2 indexes that are not fd
1036 int k1=0,k2=0;
1037 if (fd==0) { k1 = 1; k2 = 2; }
1038 if (fd==1) { k1 = 0; k2 = 2; }
1039 if (fd==2) { k1 = 0; k2 = 1; }
1040 dVector3 fp[4],tmp;
1041 k=0;
1042 for (j1=-1; j1<=1; j1+=2) {
1043 for (j2=-1; j2<=1; j2+=2) {
1044 fp[k][k1] = j1;
1045 fp[k][k2] = j2;
1046 fp[k][fd] = fo*2-1;
1047 k++;
1048 }
1049 }
1050 for (j=0; j<4; j++) {
1051 for (k=0; k<3; k++) fp[j][k] *= 0.5*side2[k];
1052 dMULTIPLY0_331 (tmp,R2,fp[j]);
1053 for (k=0; k<3; k++) fp[j][k] = tmp[k] + p2[k];
1054 }
1055
1056 // for 8 vertices
1057 dReal v1[3];
1058 for (v1[0]=-1; v1[0] <= 1; v1[0] += 2) {
1059 for (v1[1]=-1; v1[1] <= 1; v1[1] += 2) {
1060 for (v1[2]=-1; v1[2] <= 1; v1[2] += 2) {
1061 // for all possible +ve leading edges from those vertices
1062 for (int ei=0; ei < 3; ei ++) {
1063 if (v1[ei] < 0) {
1064 // get vertex1 -> vertex2 = an edge from box 1
1065 dVector3 vv1,vv2;
1066 for (k=0; k<3; k++) vv1[k] = v1[k] * 0.5*side1[k];
1067 for (k=0; k<3; k++) vv2[k] = (v1[k] + (k==ei)*2)*0.5*side1[k];
1068 dVector3 vertex1,vertex2;
1069 dMULTIPLY0_331 (vertex1,R1,vv1);
1070 dMULTIPLY0_331 (vertex2,R1,vv2);
1071 for (k=0; k<3; k++) vertex1[k] += p1[k];
1072 for (k=0; k<3; k++) vertex2[k] += p1[k];
1073
1074 // see if vertex1 -> vertex2 interesects face
1075 if (edgeIntersectsRect (vertex1,vertex2,fp[0],fp[1],fp[2]))
1076 return 1;
1077 }
1078 }
1079 }
1080 }
1081 }
1082 }
1083 }
1084
1085 if (box1inside2 (p1,R1,side1,p2,R2,side2)) return 1;
1086 if (box1inside2 (p2,R2,side2,p1,R1,side1)) return 1;
1087
1088 return 0;
1089}
1090
1091//****************************************************************************
1092// dBoxTouchesBox() test
1093
1094int test_dBoxTouchesBox()
1095{
1096 int k,bt1,bt2;
1097 dVector3 p1,p2,side1,side2;
1098 dMatrix3 R1,R2;
1099
1100 dSimpleSpace space(0);
1101 dGeomID box1 = dCreateBox (0,1,1,1);
1102 dSpaceAdd (space,box1);
1103 dGeomID box2 = dCreateBox (0,1,1,1);
1104 dSpaceAdd (space,box2);
1105
1106 dMakeRandomVector (p1,3,0.5);
1107 dMakeRandomVector (p2,3,0.5);
1108 for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
1109 for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
1110 dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1111 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1112 dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1113 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1114
1115 dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
1116 dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
1117 dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
1118 dGeomSetRotation (box1,R1);
1119 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1120 dGeomSetRotation (box2,R2);
1121 draw_all_objects (space);
1122
1123 int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
1124 int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
1125 bt1 = t1 || t2;
1126 bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);
1127
1128 if (bt1 != bt2) FAILED();
1129
1130 /*
1131 // some more debugging info if necessary
1132 if (bt1 && bt2) printf ("agree - boxes touch\n");
1133 if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
1134 if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
1135 "says no\n");
1136 if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
1137 "says yes\n");
1138 */
1139
1140 PASSED();
1141}
1142
1143//****************************************************************************
1144// test box-box collision
1145
1146int test_dBoxBox()
1147{
1148 int k,bt;
1149 dVector3 p1,p2,side1,side2,normal,normal2;
1150 dMatrix3 R1,R2;
1151 dReal depth,depth2;
1152 int code;
1153 dContactGeom contact[48];
1154
1155 dSimpleSpace space(0);
1156 dGeomID box1 = dCreateBox (0,1,1,1);
1157 dSpaceAdd (space,box1);
1158 dGeomID box2 = dCreateBox (0,1,1,1);
1159 dSpaceAdd (space,box2);
1160
1161 dMakeRandomVector (p1,3,0.5);
1162 dMakeRandomVector (p2,3,0.5);
1163 for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
1164 for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
1165
1166 dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1167 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1168 dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1169 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1170
1171 // dRSetIdentity (R1); // we can also try this
1172 // dRSetIdentity (R2);
1173
1174 dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
1175 dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
1176 dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
1177 dGeomSetRotation (box1,R1);
1178 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1179 dGeomSetRotation (box2,R2);
1180
1181 code = 0;
1182 depth = 0;
1183 bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact,
1184 sizeof(dContactGeom));
1185 if (bt==1) {
1186 p2[0] += normal[0] * 0.96 * depth;
1187 p2[1] += normal[1] * 0.96 * depth;
1188 p2[2] += normal[2] * 0.96 * depth;
1189 bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
1190 sizeof(dContactGeom));
1191
1192 /*
1193 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1194 draw_all_objects (space);
1195 */
1196
1197 if (bt != 1) {
1198 FAILED();
1199 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1200 draw_all_objects (space);
1201 }
1202
1203 p2[0] += normal[0] * 0.08 * depth;
1204 p2[1] += normal[1] * 0.08 * depth;
1205 p2[2] += normal[2] * 0.08 * depth;
1206 bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
1207 sizeof(dContactGeom));
1208 if (bt != 0) FAILED();
1209
1210 // dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1211 // draw_all_objects (space);
1212 }
1213
1214 // printf ("code=%2d depth=%.4f ",code,depth);
1215
1216 PASSED();
1217}
1218
1219//****************************************************************************
1220// graphics
1221
1222int space_pressed = 0;
1223
1224
1225// start simulation - set viewpoint
1226
1227static void start()
1228{
1229 static float xyz[3] = {2.4807,-1.8023,2.7600};
1230 static float hpr[3] = {141.5000,-18.5000,0.0000};
1231 dsSetViewpoint (xyz,hpr);
1232}
1233
1234
1235// called when a key pressed
1236
1237static void command (int cmd)
1238{
1239 if (cmd == ' ') space_pressed = 1;
1240}
1241
1242
1243// simulation loop
1244
1245static void simLoop (int pause)
1246{
1247 do {
1248 draw_all_objects_called = 0;
1249 unsigned long seed = dRandGetSeed();
1250 testslot[graphical_test].test_fn();
1251 if (draw_all_objects_called) {
1252 if (space_pressed) space_pressed = 0; else dRandSetSeed (seed);
1253 }
1254 }
1255 while (!draw_all_objects_called);
1256}
1257
1258//****************************************************************************
1259// do all the tests
1260
1261void do_tests (int argc, char **argv)
1262{
1263 int i,j;
1264
1265 // process command line arguments
1266 if (argc >= 2) {
1267 graphical_test = atoi (argv[1]);
1268 }
1269
1270 if (graphical_test) {
1271 // do one test gaphically and interactively
1272
1273 if (graphical_test < 1 || graphical_test >= MAX_TESTS ||
1274 !testslot[graphical_test].name) {
1275 dError (0,"invalid test number");
1276 }
1277
1278 printf ("performing test: %s\n",testslot[graphical_test].name);
1279
1280 // setup pointers to drawstuff callback functions
1281 dsFunctions fn;
1282 fn.version = DS_VERSION;
1283 fn.start = &start;
1284 fn.step = &simLoop;
1285 fn.command = &command;
1286 fn.stop = 0;
1287 fn.path_to_textures = "../../drawstuff/textures";
1288
1289 dsSetSphereQuality (3);
1290 dsSetCapsuleQuality (8);
1291 dsSimulationLoop (argc,argv,1280,900,&fn);
1292 }
1293 else {
1294 // do all tests noninteractively
1295
1296 for (i=0; i<MAX_TESTS; i++) testslot[i].number = i;
1297
1298 // first put the active tests into a separate array
1299 int n=0;
1300 for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) n++;
1301 TestSlot **ts = (TestSlot**) alloca (n * sizeof(TestSlot*));
1302 j = 0;
1303 for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) ts[j++] = testslot+i;
1304 if (j != n) dDebug (0,"internal");
1305
1306 // do two test batches. the first test batch has far fewer reps and will
1307 // catch problems quickly. if all tests in the first batch passes, the
1308 // second batch is run.
1309
1310 for (i=0; i<n; i++) ts[i]->failcount = 0;
1311 int total_reps=0;
1312 for (int batch=0; batch<2; batch++) {
1313 int reps = (batch==0) ? TEST_REPS1 : TEST_REPS2;
1314 total_reps += reps;
1315 printf ("testing batch %d (%d reps)...\n",batch+1,reps);
1316
1317 // run tests
1318 for (j=0; j<reps; j++) {
1319 for (i=0; i<n; i++) {
1320 current_test = ts[i]->number;
1321 if (ts[i]->test_fn() != 1) ts[i]->failcount++;
1322 }
1323 }
1324
1325 // check for failures
1326 int total_fail_count=0;
1327 for (i=0; i<n; i++) total_fail_count += ts[i]->failcount;
1328 if (total_fail_count) break;
1329 }
1330
1331 // print results
1332 for (i=0; i<n; i++) {
1333 printf ("%3d: %-30s: ",ts[i]->number,ts[i]->name);
1334 if (ts[i]->failcount) {
1335 printf ("FAILED (%.2f%%) at line %d\n",
1336 double(ts[i]->failcount)/double(total_reps)*100.0,
1337 ts[i]->last_failed_line);
1338 }
1339 else {
1340 printf ("ok\n");
1341 }
1342 }
1343 }
1344}
1345
1346//****************************************************************************
1347
1348int main (int argc, char **argv)
1349{
1350 // setup all tests
1351
1352 memset (testslot,0,sizeof(testslot));
1353 dInitODE();
1354
1355 MAKE_TEST(1,test_sphere_point_depth);
1356 MAKE_TEST(2,test_box_point_depth);
1357 MAKE_TEST(3,test_ccylinder_point_depth);
1358 MAKE_TEST(4,test_plane_point_depth);
1359
1360 MAKE_TEST(10,test_ray_and_sphere);
1361 MAKE_TEST(11,test_ray_and_box);
1362 MAKE_TEST(12,test_ray_and_ccylinder);
1363 MAKE_TEST(13,test_ray_and_plane);
1364
1365 MAKE_TEST(100,test_dBoxTouchesBox);
1366 MAKE_TEST(101,test_dBoxBox);
1367
1368 do_tests (argc,argv);
1369 dCloseODE();
1370 return 0;
1371}
diff --git a/libraries/ode-0.9/ode/demo/demo_convex_cd.cpp b/libraries/ode-0.9/ode/demo/demo_convex_cd.cpp
new file mode 100644
index 0000000..e1764f3
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_convex_cd.cpp
@@ -0,0 +1,195 @@
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 <stdio.h>
24#include <math.h>
25#include <ode/ode.h>
26#include <drawstuff/drawstuff.h>
27#ifdef _MSC_VER
28#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
29#endif
30
31
32#ifndef M_PI
33#define M_PI (3.14159265358979323846)
34#endif
35
36//<---- Convex Object
37dReal planes[]= // planes for a cube
38 {
39 1.0f ,0.0f ,0.0f ,0.25f,
40 0.0f ,1.0f ,0.0f ,0.25f,
41 0.0f ,0.0f ,1.0f ,0.25f,
42 -1.0f,0.0f ,0.0f ,0.25f,
43 0.0f ,-1.0f,0.0f ,0.25f,
44 0.0f ,0.0f ,-1.0f,0.25f
45 /*
46 1.0f ,0.0f ,0.0f ,2.0f,
47 0.0f ,1.0f ,0.0f ,1.0f,
48 0.0f ,0.0f ,1.0f ,1.0f,
49 0.0f ,0.0f ,-1.0f,1.0f,
50 0.0f ,-1.0f,0.0f ,1.0f,
51 -1.0f,0.0f ,0.0f ,0.0f
52 */
53 };
54const unsigned int planecount=6;
55
56dReal points[]= // points for a cube
57 {
58 0.25f,0.25f,0.25f, // point 0
59 -0.25f,0.25f,0.25f, // point 1
60
61 0.25f,-0.25f,0.25f, // point 2
62 -0.25f,-0.25f,0.25f,// point 3
63
64 0.25f,0.25f,-0.25f, // point 4
65 -0.25f,0.25f,-0.25f,// point 5
66
67 0.25f,-0.25f,-0.25f,// point 6
68 -0.25f,-0.25f,-0.25f,// point 7
69 };
70const unsigned int pointcount=8;
71unsigned int polygons[] = //Polygons for a cube (6 squares)
72 {
73 4,0,2,6,4, // positive X
74 4,1,0,4,5, // positive Y
75 4,0,1,3,2, // positive Z
76 4,3,1,5,7, // negative X
77 4,2,3,7,6, // negative Y
78 4,5,4,6,7, // negative Z
79 };
80//----> Convex Object
81
82#ifdef dDOUBLE
83#define dsDrawConvex dsDrawConvexD
84#define dsDrawBox dsDrawBoxD
85#endif
86
87dGeomID geoms[2];
88dSpaceID space;
89dWorldID world;
90dJointGroupID contactgroup;
91
92void start()
93{
94 // adjust the starting viewpoint a bit
95 float xyz[3],hpr[3];
96 dsGetViewpoint (xyz,hpr);
97 hpr[0] += 7;
98 dsSetViewpoint (xyz,hpr);
99 geoms[0]=dCreateConvex (space,
100 planes,
101 planecount,
102 points,
103 pointcount,
104 polygons);
105 dGeomSetPosition (geoms[0],0,0,0.25);
106 geoms[1]=dCreateConvex (space,
107 planes,
108 planecount,
109 points,
110 pointcount,
111 polygons);
112 dGeomSetPosition (geoms[1],0.25,0.25,0.70);
113
114}
115
116int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
117 dContactGeom *contact, int skip);
118void simLoop (int pause)
119{
120 static bool DumpInfo=true;
121 const dReal ss[3] = {0.02,0.02,0.02};
122 dContactGeom contacts[8];
123 int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
124 //fprintf(stdout,"Contact Count %d\n",contactcount);
125 const dReal* pos;
126 const dReal* R;
127 dsSetTexture (DS_WOOD);
128 pos = dGeomGetPosition (geoms[0]);
129 R = dGeomGetRotation (geoms[0]);
130 dsSetColor (0.6f,0.6f,1);
131 dsDrawConvex(pos,R,planes,
132 planecount,
133 points,
134 pointcount,
135 polygons);
136 pos = dGeomGetPosition (geoms[1]);
137 R = dGeomGetRotation (geoms[1]);
138 dsSetColor (0.4f,1,1);
139 dsDrawConvex(pos,R,planes,
140 planecount,
141 points,
142 pointcount,
143 polygons);
144 /*if (show_contacts) */
145 dMatrix3 RI;
146 dRSetIdentity (RI);
147 dsSetColor (1.0f,0,0);
148 for(int i=0;i<contactcount;++i)
149 {
150 if(DumpInfo)
151 {
152 //DumpInfo=false;
153 fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f\n",
154 i,
155 contacts[i].normal[0],
156 contacts[i].normal[1],
157 contacts[i].normal[2],
158 contacts[i].depth);
159 }
160 dsDrawBox (contacts[i].pos,RI,ss);
161 }
162 if(DumpInfo)
163 DumpInfo=false;
164
165}
166
167
168void command (int cmd)
169{
170 dsPrint ("received command %d (`%c')\n",cmd,cmd);
171}
172
173
174int main (int argc, char **argv)
175{
176 // setup pointers to callback functions
177 dsFunctions fn;
178 fn.version = DS_VERSION;
179 fn.start = &start;
180 fn.step = &simLoop;
181 fn.command = command;
182 fn.stop = 0;
183 fn.path_to_textures = "../../drawstuff/textures"; // uses default
184 world = dWorldCreate();
185 space = dHashSpaceCreate (0);
186 contactgroup = dJointGroupCreate (0);
187
188 // run simulation
189 dsSimulationLoop (argc,argv,400,400,&fn);
190 dJointGroupDestroy (contactgroup);
191 dSpaceDestroy (space);
192 dWorldDestroy (world);
193
194 return 0;
195}
diff --git a/libraries/ode-0.9/ode/demo/demo_crash.cpp b/libraries/ode-0.9/ode/demo/demo_crash.cpp
new file mode 100644
index 0000000..29ec01f
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_crash.cpp
@@ -0,0 +1,635 @@
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// This is a demo of the QuickStep and StepFast methods,
24// originally by David Whittaker.
25
26#include <ode/ode.h>
27#include <drawstuff/drawstuff.h>
28
29#ifdef _MSC_VER
30#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
31#endif
32
33// select correct drawing functions
34
35#ifdef dDOUBLE
36#define dsDrawBox dsDrawBoxD
37#define dsDrawSphere dsDrawSphereD
38#define dsDrawCylinder dsDrawCylinderD
39#define dsDrawCapsule dsDrawCapsuleD
40#endif
41
42
43// select the method you want to test here (only uncomment *one* line)
44#define QUICKSTEP 1
45//#define STEPFAST 1
46
47// some constants
48
49#define LENGTH 3.5 // chassis length
50#define WIDTH 2.5 // chassis width
51#define HEIGHT 1.0 // chassis height
52#define RADIUS 0.5 // wheel radius
53#define STARTZ 1.0 // starting height of chassis
54#define CMASS 1 // chassis mass
55#define WMASS 1 // wheel mass
56#define COMOFFSET -5 // center of mass offset
57#define WALLMASS 1 // wall box mass
58#define BALLMASS 1 // ball mass
59#define FMAX 25 // car engine fmax
60#define ROWS 1 // rows of cars
61#define COLS 1 // columns of cars
62#define ITERS 20 // number of iterations
63#define WBOXSIZE 1.0 // size of wall boxes
64#define WALLWIDTH 12 // width of wall
65#define WALLHEIGHT 10 // height of wall
66#define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled
67#define DISABLE_STEPS 10 // number of steps a box has to have been disable-able before it will be disabled
68#define CANNON_X -10 // x position of cannon
69#define CANNON_Y 5 // y position of cannon
70#define CANNON_BALL_MASS 10 // mass of the cannon ball
71#define CANNON_BALL_RADIUS 0.5
72
73//#define BOX
74#define CARS
75#define WALL
76//#define BALLS
77//#define BALLSTACK
78//#define ONEBALL
79//#define CENTIPEDE
80#define CANNON
81
82// dynamics and collision objects (chassis, 3 wheels, environment)
83
84static dWorldID world;
85static dSpaceID space;
86static dBodyID body[10000];
87static int bodies;
88static dJointID joint[100000];
89static int joints;
90static dJointGroupID contactgroup;
91static dGeomID ground;
92static dGeomID box[10000];
93static int boxes;
94static dGeomID sphere[10000];
95static int spheres;
96static dGeomID wall_boxes[10000];
97static dBodyID wall_bodies[10000];
98static dGeomID cannon_ball_geom;
99static dBodyID cannon_ball_body;
100static int wb_stepsdis[10000];
101static int wb;
102static bool doFast;
103static dBodyID b;
104static dMass m;
105
106
107// things that the user controls
108
109static dReal turn = 0, speed = 0; // user commands
110static dReal cannon_angle=0,cannon_elevation=-1.2;
111
112
113
114// this is called by dSpaceCollide when two objects in space are
115// potentially colliding.
116
117static void nearCallback (void *data, dGeomID o1, dGeomID o2)
118{
119 int i,n;
120
121 dBodyID b1 = dGeomGetBody(o1);
122 dBodyID b2 = dGeomGetBody(o2);
123 if (b1 && b2 && dAreConnected(b1, b2))
124 return;
125
126 const int N = 4;
127 dContact contact[N];
128 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
129 if (n > 0) {
130 for (i=0; i<n; i++) {
131 contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
132 if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
133 contact[i].surface.mu = 20;
134 else
135 contact[i].surface.mu = 0.5;
136 contact[i].surface.slip1 = 0.0;
137 contact[i].surface.slip2 = 0.0;
138 contact[i].surface.soft_erp = 0.8;
139 contact[i].surface.soft_cfm = 0.01;
140 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
141 dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
142 }
143 }
144}
145
146
147// start simulation - set viewpoint
148
149static void start()
150{
151 static float xyz[3] = {3.8548f,9.0843f,7.5900f};
152 static float hpr[3] = {-145.5f,-22.5f,0.25f};
153 dsSetViewpoint (xyz,hpr);
154 printf ("Press:\t'a' to increase speed.\n"
155 "\t'z' to decrease speed.\n"
156 "\t',' to steer left.\n"
157 "\t'.' to steer right.\n"
158 "\t' ' to reset speed and steering.\n"
159 "\t'[' to turn the cannon left.\n"
160 "\t']' to turn the cannon right.\n"
161 "\t'1' to raise the cannon.\n"
162 "\t'2' to lower the cannon.\n"
163 "\t'x' to shoot from the cannon.\n"
164 "\t'f' to toggle fast step mode.\n"
165 "\t'+' to increase AutoEnableDepth.\n"
166 "\t'-' to decrease AutoEnableDepth.\n"
167 "\t'r' to reset simulation.\n");
168}
169
170
171void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
172{
173 int i;
174 dMass m;
175
176 // chassis body
177 body[bodyI] = dBodyCreate (world);
178 dBodySetPosition (body[bodyI],x,y,STARTZ);
179 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
180 dMassAdjust (&m,CMASS/2.0);
181 dBodySetMass (body[bodyI],&m);
182 box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
183 dGeomSetBody (box[boxI],body[bodyI]);
184
185 // wheel bodies
186 for (i=1; i<=4; i++) {
187 body[bodyI+i] = dBodyCreate (world);
188 dQuaternion q;
189 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
190 dBodySetQuaternion (body[bodyI+i],q);
191 dMassSetSphere (&m,1,RADIUS);
192 dMassAdjust (&m,WMASS);
193 dBodySetMass (body[bodyI+i],&m);
194 sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
195 dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
196 }
197 dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
198 dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
199 dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
200 dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
201
202 // front and back wheel hinges
203 for (i=0; i<4; i++) {
204 joint[jointI+i] = dJointCreateHinge2 (world,0);
205 dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
206 const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
207 dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
208 dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1));
209 dJointSetHinge2Axis2 (joint[jointI+i],0,1,0);
210 dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
211 dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
212 dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
213 dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
214 }
215
216 //center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
217 dBodyID b = dBodyCreate (world);
218 dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
219 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
220 dMassAdjust (&m,CMASS/2.0);
221 dBodySetMass (b,&m);
222 dJointID j = dJointCreateFixed(world, 0);
223 dJointAttach(j, body[bodyI], b);
224 dJointSetFixed(j);
225 //box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
226 //dGeomSetBody (box[boxI+1],b);
227
228 bodyI += 5;
229 jointI += 4;
230 boxI += 1;
231 sphereI += 4;
232}
233
234
235void resetSimulation()
236{
237 int i;
238 i = 0;
239 // destroy world if it exists
240 if (bodies)
241 {
242 dJointGroupDestroy (contactgroup);
243 dSpaceDestroy (space);
244 dWorldDestroy (world);
245 }
246
247 for (i = 0; i < 1000; i++)
248 wb_stepsdis[i] = 0;
249
250 // recreate world
251
252 world = dWorldCreate();
253 space = dHashSpaceCreate (0);
254 contactgroup = dJointGroupCreate (0);
255 dWorldSetGravity (world,0,0,-1.5);
256 dWorldSetCFM (world, 1e-5);
257 dWorldSetERP (world, 0.8);
258 dWorldSetQuickStepNumIterations (world,ITERS);
259 ground = dCreatePlane (space,0,0,1,0);
260
261 bodies = 0;
262 joints = 0;
263 boxes = 0;
264 spheres = 0;
265 wb = 0;
266
267#ifdef CARS
268 for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
269 for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
270 makeCar(x, y, bodies, joints, boxes, spheres);
271#endif
272#ifdef WALL
273 bool offset = false;
274 for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
275 {
276 offset = !offset;
277 for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
278 {
279 wall_bodies[wb] = dBodyCreate (world);
280 dBodySetPosition (wall_bodies[wb],-20,y,z);
281 dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
282 dMassAdjust (&m, WALLMASS);
283 dBodySetMass (wall_bodies[wb],&m);
284 wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
285 dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
286 //dBodyDisable(wall_bodies[wb++]);
287 wb++;
288 }
289 }
290 dMessage(0,"wall boxes: %i", wb);
291#endif
292#ifdef BALLS
293 for (dReal x = -7; x <= -4; x+=1)
294 for (dReal y = -1.5; y <= 1.5; y+=1)
295 for (dReal z = 1; z <= 4; z+=1)
296 {
297 b = dBodyCreate (world);
298 dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
299 dMassSetSphere (&m,1,RADIUS);
300 dMassAdjust (&m, BALLMASS);
301 dBodySetMass (b,&m);
302 sphere[spheres] = dCreateSphere (space,RADIUS);
303 dGeomSetBody (sphere[spheres++],b);
304 }
305#endif
306#ifdef ONEBALL
307 b = dBodyCreate (world);
308 dBodySetPosition (b,0,0,2);
309 dMassSetSphere (&m,1,RADIUS);
310 dMassAdjust (&m, 1);
311 dBodySetMass (b,&m);
312 sphere[spheres] = dCreateSphere (space,RADIUS);
313 dGeomSetBody (sphere[spheres++],b);
314#endif
315#ifdef BALLSTACK
316 for (dReal z = 1; z <= 6; z+=1)
317 {
318 b = dBodyCreate (world);
319 dBodySetPosition (b,0,0,z*RADIUS*2);
320 dMassSetSphere (&m,1,RADIUS);
321 dMassAdjust (&m, 0.1);
322 dBodySetMass (b,&m);
323 sphere[spheres] = dCreateSphere (space,RADIUS);
324 dGeomSetBody (sphere[spheres++],b);
325 }
326#endif
327#ifdef CENTIPEDE
328 dBodyID lastb = 0;
329 for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
330 {
331 // chassis body
332
333 b = body[bodies] = dBodyCreate (world);
334 dBodySetPosition (body[bodies],-15,y,STARTZ);
335 dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
336 dMassAdjust (&m,CMASS);
337 dBodySetMass (body[bodies],&m);
338 box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
339 dGeomSetBody (box[boxes++],body[bodies++]);
340
341 for (dReal x = -17; x > -20; x-=RADIUS*2)
342 {
343 body[bodies] = dBodyCreate (world);
344 dBodySetPosition(body[bodies], x, y, STARTZ);
345 dMassSetSphere(&m, 1, RADIUS);
346 dMassAdjust(&m, WMASS);
347 dBodySetMass(body[bodies], &m);
348 sphere[spheres] = dCreateSphere (space, RADIUS);
349 dGeomSetBody (sphere[spheres++], body[bodies]);
350
351 joint[joints] = dJointCreateHinge2 (world,0);
352 if (x == -17)
353 dJointAttach (joint[joints],b,body[bodies]);
354 else
355 dJointAttach (joint[joints],body[bodies-2],body[bodies]);
356 const dReal *a = dBodyGetPosition (body[bodies++]);
357 dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
358 dJointSetHinge2Axis1 (joint[joints],0,0,1);
359 dJointSetHinge2Axis2 (joint[joints],1,0,0);
360 dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
361 dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
362 dJointSetHinge2Param (joint[joints],dParamLoStop,0);
363 dJointSetHinge2Param (joint[joints],dParamHiStop,0);
364 dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
365 dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
366
367 body[bodies] = dBodyCreate (world);
368 dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
369 dMassSetSphere(&m, 1, RADIUS);
370 dMassAdjust(&m, WMASS);
371 dBodySetMass(body[bodies], &m);
372 sphere[spheres] = dCreateSphere (space, RADIUS);
373 dGeomSetBody (sphere[spheres++], body[bodies]);
374
375 joint[joints] = dJointCreateHinge2 (world,0);
376 if (x == -17)
377 dJointAttach (joint[joints],b,body[bodies]);
378 else
379 dJointAttach (joint[joints],body[bodies-2],body[bodies]);
380 const dReal *b = dBodyGetPosition (body[bodies++]);
381 dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
382 dJointSetHinge2Axis1 (joint[joints],0,0,1);
383 dJointSetHinge2Axis2 (joint[joints],1,0,0);
384 dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
385 dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
386 dJointSetHinge2Param (joint[joints],dParamLoStop,0);
387 dJointSetHinge2Param (joint[joints],dParamHiStop,0);
388 dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
389 dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
390 }
391 if (lastb)
392 {
393 dJointID j = dJointCreateFixed(world,0);
394 dJointAttach (j, b, lastb);
395 dJointSetFixed(j);
396 }
397 lastb = b;
398 }
399#endif
400#ifdef BOX
401 body[bodies] = dBodyCreate (world);
402 dBodySetPosition (body[bodies],0,0,HEIGHT/2);
403 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
404 dMassAdjust (&m, 1);
405 dBodySetMass (body[bodies],&m);
406 box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
407 dGeomSetBody (box[boxes++],body[bodies++]);
408#endif
409#ifdef CANNON
410 cannon_ball_body = dBodyCreate (world);
411 cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
412 dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
413 dBodySetMass (cannon_ball_body,&m);
414 dGeomSetBody (cannon_ball_geom,cannon_ball_body);
415 dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
416#endif
417}
418
419// called when a key pressed
420
421static void command (int cmd)
422{
423 switch (cmd) {
424 case 'a': case 'A':
425 speed += 0.3;
426 break;
427 case 'z': case 'Z':
428 speed -= 0.3;
429 break;
430 case ',':
431 turn += 0.1;
432 if (turn > 0.3)
433 turn = 0.3;
434 break;
435 case '.':
436 turn -= 0.1;
437 if (turn < -0.3)
438 turn = -0.3;
439 break;
440 case ' ':
441 speed = 0;
442 turn = 0;
443 break;
444 case 'f': case 'F':
445 doFast = !doFast;
446 break;
447 case '+':
448 dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1);
449 break;
450 case '-':
451 dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1);
452 break;
453 case 'r': case 'R':
454 resetSimulation();
455 break;
456 case '[':
457 cannon_angle += 0.1;
458 break;
459 case ']':
460 cannon_angle -= 0.1;
461 break;
462 case '1':
463 cannon_elevation += 0.1;
464 break;
465 case '2':
466 cannon_elevation -= 0.1;
467 break;
468 case 'x': case 'X': {
469 dMatrix3 R2,R3,R4;
470 dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
471 dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
472 dMultiply0 (R4,R2,R3,3,3,3);
473 dReal cpos[3] = {CANNON_X,CANNON_Y,1};
474 for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2];
475 dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]);
476 dReal force = 10;
477 dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]);
478 dBodySetAngularVel (cannon_ball_body,0,0,0);
479 break;
480 }
481 }
482}
483
484
485// simulation loop
486
487static void simLoop (int pause)
488{
489 int i, j;
490
491 dsSetTexture (DS_WOOD);
492
493 if (!pause) {
494#ifdef BOX
495 dBodyAddForce(body[bodies-1],lspeed,0,0);
496#endif
497 for (j = 0; j < joints; j++)
498 {
499 dReal curturn = dJointGetHinge2Angle1 (joint[j]);
500 //dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
501 dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
502 dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
503 dJointSetHinge2Param(joint[j],dParamVel2,speed);
504 dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
505 dBodyEnable(dJointGetBody(joint[j],0));
506 dBodyEnable(dJointGetBody(joint[j],1));
507 }
508 if (doFast)
509 {
510 dSpaceCollide (space,0,&nearCallback);
511#if defined(QUICKSTEP)
512 dWorldQuickStep (world,0.05);
513#elif defined(STEPFAST)
514 dWorldStepFast1 (world,0.05,ITERS);
515#endif
516 dJointGroupEmpty (contactgroup);
517 }
518 else
519 {
520 dSpaceCollide (space,0,&nearCallback);
521 dWorldStep (world,0.05);
522 dJointGroupEmpty (contactgroup);
523 }
524
525 for (i = 0; i < wb; i++)
526 {
527 b = dGeomGetBody(wall_boxes[i]);
528 if (dBodyIsEnabled(b))
529 {
530 bool disable = true;
531 const dReal *lvel = dBodyGetLinearVel(b);
532 dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
533 if (lspeed > DISABLE_THRESHOLD)
534 disable = false;
535 const dReal *avel = dBodyGetAngularVel(b);
536 dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
537 if (aspeed > DISABLE_THRESHOLD)
538 disable = false;
539
540 if (disable)
541 wb_stepsdis[i]++;
542 else
543 wb_stepsdis[i] = 0;
544
545 if (wb_stepsdis[i] > DISABLE_STEPS)
546 {
547 dBodyDisable(b);
548 dsSetColor(0.5,0.5,1);
549 }
550 else
551 dsSetColor(1,1,1);
552
553 }
554 else
555 dsSetColor(0.4,0.4,0.4);
556 dVector3 ss;
557 dGeomBoxGetLengths (wall_boxes[i], ss);
558 dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
559 }
560 }
561 else
562 {
563 for (i = 0; i < wb; i++)
564 {
565 b = dGeomGetBody(wall_boxes[i]);
566 if (dBodyIsEnabled(b))
567 dsSetColor(1,1,1);
568 else
569 dsSetColor(0.4,0.4,0.4);
570 dVector3 ss;
571 dGeomBoxGetLengths (wall_boxes[i], ss);
572 dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
573 }
574 }
575
576 dsSetColor (0,1,1);
577 dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
578 for (i = 0; i < boxes; i++)
579 dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
580 dsSetColor (1,1,1);
581 for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
582 dGeomGetRotation(sphere[i]),RADIUS);
583
584 // draw the cannon
585 dsSetColor (1,1,0);
586 dMatrix3 R2,R3,R4;
587 dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
588 dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
589 dMultiply0 (R4,R2,R3,3,3,3);
590 dReal cpos[3] = {CANNON_X,CANNON_Y,1};
591 dReal csides[3] = {2,2,2};
592 dsDrawBox (cpos,R2,csides);
593 for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
594 dsDrawCylinder (cpos,R4,3,0.5);
595
596 // draw the cannon ball
597 dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
598 CANNON_BALL_RADIUS);
599}
600
601int main (int argc, char **argv)
602{
603 doFast = true;
604
605 // setup pointers to drawstuff callback functions
606 dsFunctions fn;
607 fn.version = DS_VERSION;
608 fn.start = &start;
609 fn.step = &simLoop;
610 fn.command = &command;
611 fn.stop = 0;
612 fn.path_to_textures = "../../drawstuff/textures";
613 if(argc==2)
614 {
615 fn.path_to_textures = argv[1];
616 }
617
618 dInitODE();
619
620 bodies = 0;
621 joints = 0;
622 boxes = 0;
623 spheres = 0;
624
625 resetSimulation();
626
627 // run simulation
628 dsSimulationLoop (argc,argv,352,288,&fn);
629
630 dJointGroupDestroy (contactgroup);
631 dSpaceDestroy (space);
632 dWorldDestroy (world);
633 dCloseODE();
634 return 0;
635}
diff --git a/libraries/ode-0.9/ode/demo/demo_cyl.cpp b/libraries/ode-0.9/ode/demo/demo_cyl.cpp
new file mode 100644
index 0000000..3692234
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_cyl.cpp
@@ -0,0 +1,318 @@
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// Test for non-capped cylinder, by Bram Stolk
24#include <ode/config.h>
25#include <assert.h>
26#ifdef HAVE_UNISTD_H
27#include <unistd.h>
28#endif
29#include <ode/ode.h>
30#include <drawstuff/drawstuff.h>
31
32#include "world_geom3.h" // this is our world mesh
33
34#ifdef _MSC_VER
35#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
36#endif
37
38#define BOX
39#define CYL
40
41// some constants
42
43#define RADIUS 0.22 // wheel radius
44#define WMASS 0.2 // wheel mass
45#define WHEELW 0.2 // wheel width
46#define BOXSZ 0.4 // box size
47//#define CYL_GEOM_OFFSET // rotate cylinder using geom offset
48
49// dynamics and collision objects (chassis, 3 wheels, environment)
50
51static dWorldID world;
52static dSpaceID space;
53#ifdef BOX
54static dBodyID boxbody;
55static dGeomID boxgeom;
56#endif
57#ifdef CYL
58static dBodyID cylbody;
59static dGeomID cylgeom;
60#endif
61static dJointGroupID contactgroup;
62static dGeomID world_mesh;
63
64
65// this is called by dSpaceCollide when two objects in space are
66// potentially colliding.
67
68static void nearCallback (void *data, dGeomID o1, dGeomID o2)
69{
70 assert(o1);
71 assert(o2);
72
73 if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
74 {
75 fprintf(stderr,"testing space %p %p\n", o1,o2);
76 // colliding a space with something
77 dSpaceCollide2(o1,o2,data,&nearCallback);
78 // Note we do not want to test intersections within a space,
79 // only between spaces.
80 return;
81 }
82
83// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
84
85 const int N = 32;
86 dContact contact[N];
87 int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
88 if (n > 0)
89 {
90 for (int i=0; i<n; i++)
91 {
92 contact[i].surface.slip1 = 0.7;
93 contact[i].surface.slip2 = 0.7;
94 contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
95 contact[i].surface.mu = 50.0; // was: dInfinity
96 contact[i].surface.soft_erp = 0.96;
97 contact[i].surface.soft_cfm = 0.04;
98 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
99 dJointAttach (c,
100 dGeomGetBody(contact[i].geom.g1),
101 dGeomGetBody(contact[i].geom.g2));
102 }
103 }
104}
105
106
107// start simulation - set viewpoint
108
109static void start()
110{
111 static float xyz[3] = {-8,-9,3};
112 static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
113 dsSetViewpoint (xyz,hpr);
114}
115
116
117
118static void reset_state(void)
119{
120 float sx=-4, sy=-4, sz=2;
121 dQuaternion q;
122 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
123#ifdef BOX
124 dBodySetPosition (boxbody, sx, sy+1, sz);
125 dBodySetLinearVel (boxbody, 0,0,0);
126 dBodySetAngularVel (boxbody, 0,0,0);
127 dBodySetQuaternion (boxbody, q);
128#endif
129#ifdef CYL
130 dBodySetPosition (cylbody, sx, sy, sz);
131 dBodySetLinearVel (cylbody, 0,0,0);
132 dBodySetAngularVel (cylbody, 0,0,0);
133 dBodySetQuaternion (cylbody, q);
134#endif
135}
136
137
138// called when a key pressed
139
140static void command (int cmd)
141{
142 switch (cmd)
143 {
144 case ' ':
145 reset_state();
146 break;
147 }
148}
149
150
151
152// simulation loop
153
154static void simLoop (int pause)
155{
156 double simstep = 0.005; // 5ms simulation steps
157 double dt = dsElapsedTime();
158 int nrofsteps = (int) ceilf(dt/simstep);
159 for (int i=0; i<nrofsteps && !pause; i++)
160 {
161 dSpaceCollide (space,0,&nearCallback);
162 dWorldQuickStep (world, simstep);
163 dJointGroupEmpty (contactgroup);
164 }
165
166 dsSetColor (1,1,1);
167#ifdef BOX
168 const dReal *BPos = dBodyGetPosition(boxbody);
169 const dReal *BRot = dBodyGetRotation(boxbody);
170 float bpos[3] = {BPos[0], BPos[1], BPos[2]};
171 float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] };
172 float sides[3] = {BOXSZ, BOXSZ, BOXSZ};
173 dsDrawBox
174 (
175 bpos,
176 brot,
177 sides
178 ); // single precision
179#endif
180#ifdef CYL
181 const dReal *CPos = dGeomGetPosition(cylgeom);
182 const dReal *CRot = dGeomGetRotation(cylgeom);
183 float cpos[3] = {CPos[0], CPos[1], CPos[2]};
184 float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
185 dsDrawCylinder
186 (
187// dBodyGetPosition(cylbody),
188// dBodyGetRotation(cylbody),
189 cpos,
190 crot,
191 WHEELW,
192 RADIUS
193 ); // single precision
194#endif
195
196 // draw world trimesh
197 dsSetColor(0.7,0.7,0.4);
198 dsSetTexture (DS_NONE);
199
200 const dReal* Pos = dGeomGetPosition(world_mesh);
201 float pos[3] = { Pos[0], Pos[1], Pos[2] };
202
203 const dReal* Rot = dGeomGetRotation(world_mesh);
204 float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
205
206 int numi = sizeof(world_indices) / sizeof(int);
207
208 for (int i=0; i<numi/3; i++)
209 {
210 int i0 = world_indices[i*3+0];
211 int i1 = world_indices[i*3+1];
212 int i2 = world_indices[i*3+2];
213 float *v0 = world_vertices+i0*3;
214 float *v1 = world_vertices+i1*3;
215 float *v2 = world_vertices+i2*3;
216 dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
217 }
218}
219
220
221int main (int argc, char **argv)
222{
223 dMass m;
224 dMatrix3 R;
225
226 // setup pointers to drawstuff callback functions
227 dsFunctions fn;
228 fn.version = DS_VERSION;
229 fn.start = &start;
230 fn.step = &simLoop;
231 fn.command = &command;
232 fn.stop = 0;
233 fn.path_to_textures = "../../drawstuff/textures";
234 if(argc==2)
235 {
236 fn.path_to_textures = argv[1];
237 }
238
239 // create world
240 dInitODE();
241 world = dWorldCreate();
242 space = dHashSpaceCreate (0);
243 contactgroup = dJointGroupCreate (0);
244 dWorldSetGravity (world,0,0,-9.8);
245 dWorldSetQuickStepNumIterations (world, 12);
246
247
248 // Create a static world using a triangle mesh that we can collide with.
249 int numv = sizeof(world_vertices)/(3*sizeof(float));
250 int numi = sizeof(world_indices)/ sizeof(int);
251 printf("numv=%d, numi=%d\n", numv, numi);
252 dTriMeshDataID Data = dGeomTriMeshDataCreate();
253
254 dGeomTriMeshDataBuildSingle
255 (
256 Data,
257 world_vertices,
258 3 * sizeof(float),
259 numv,
260 world_indices,
261 numi,
262 3 * sizeof(int)
263 );
264
265 world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
266 dGeomSetPosition(world_mesh, 0, 0, 0.5);
267 dRFromAxisAndAngle (R, 0,1,0, 0.0);
268 dGeomSetRotation (world_mesh, R);
269
270
271#ifdef BOX
272 boxbody = dBodyCreate (world);
273 dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
274 dMassAdjust (&m, 1);
275 dBodySetMass (boxbody,&m);
276 boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
277 dGeomSetBody (boxgeom,boxbody);
278 dSpaceAdd (space, boxgeom);
279#endif
280#ifdef CYL
281 cylbody = dBodyCreate (world);
282 dMassSetSphere (&m,1,RADIUS);
283 dMassAdjust (&m,WMASS);
284 dBodySetMass (cylbody,&m);
285 cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
286 dGeomSetBody (cylgeom,cylbody);
287
288 #if defined(CYL_GEOM_OFFSET)
289 dMatrix3 mat;
290 dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0);
291 dGeomSetOffsetRotation(cylgeom,mat);
292 #endif
293
294 dSpaceAdd (space, cylgeom);
295#endif
296 reset_state();
297
298 // run simulation
299 dsSimulationLoop (argc,argv,352,288,&fn);
300
301 dJointGroupEmpty (contactgroup);
302 dJointGroupDestroy (contactgroup);
303
304 // First destroy geoms, then space, then the world.
305#ifdef CYL
306 dGeomDestroy (cylgeom);
307#endif
308#ifdef BOX
309 dGeomDestroy (boxgeom);
310#endif
311 dGeomDestroy (world_mesh);
312
313 dSpaceDestroy (space);
314 dWorldDestroy (world);
315 dCloseODE();
316 return 0;
317}
318
diff --git a/libraries/ode-0.9/ode/demo/demo_cylvssphere.cpp b/libraries/ode-0.9/ode/demo/demo_cylvssphere.cpp
new file mode 100644
index 0000000..1fb98ad
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_cylvssphere.cpp
@@ -0,0 +1,238 @@
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// Test for cylinder vs sphere, by Bram Stolk
24
25#include <ode/config.h>
26#include <assert.h>
27#ifdef HAVE_UNISTD_H
28#include <unistd.h>
29#endif
30#include <ode/ode.h>
31#include <drawstuff/drawstuff.h>
32
33#ifdef _MSC_VER
34#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
35#endif
36
37
38// dynamics and collision objects (chassis, 3 wheels, environment)
39
40static dWorldID world;
41static dSpaceID space;
42
43static dBodyID cylbody;
44static dGeomID cylgeom;
45
46static dBodyID sphbody;
47static dGeomID sphgeom;
48
49static dJointGroupID contactgroup;
50static dGeomID world_mesh;
51
52static bool show_contacts = true;
53
54#define CYLRADIUS 0.6
55#define CYLLENGTH 2.0
56#define SPHERERADIUS 0.5
57
58
59#ifdef dDOUBLE
60#define dsDrawBox dsDrawBoxD
61#define dsDrawLine dsDrawLineD
62#endif
63
64
65
66// this is called by dSpaceCollide when two objects in space are
67// potentially colliding.
68
69static void nearCallback (void *data, dGeomID o1, dGeomID o2)
70{
71 assert(o1);
72 assert(o2);
73
74 if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
75 {
76 fprintf(stderr,"testing space %p %p\n", o1,o2);
77 // colliding a space with something
78 dSpaceCollide2(o1,o2,data,&nearCallback);
79 // Note we do not want to test intersections within a space,
80 // only between spaces.
81 return;
82 }
83
84 const int N = 32;
85 dContact contact[N];
86 int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
87 if (n > 0)
88 {
89 for (int i=0; i<n; i++)
90 {
91 contact[i].surface.mode = 0;
92 contact[i].surface.mu = 50.0; // was: dInfinity
93 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
94 dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
95 if (show_contacts)
96 {
97 dMatrix3 RI;
98 dRSetIdentity (RI);
99 const dReal ss[3] = {0.12,0.12,0.12};
100 dsSetColorAlpha (0,0,1,0.5);
101 dsDrawBox (contact[i].geom.pos,RI,ss);
102 dReal *pos = contact[i].geom.pos;
103 dReal depth = contact[i].geom.depth;
104 dReal *norm = contact[i].geom.normal;
105 dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]};
106 dsSetColorAlpha (1,1,1,1);
107 dsDrawLine (contact[i].geom.pos, endp);
108 }
109 }
110 }
111}
112
113
114// start simulation - set viewpoint
115
116static void start()
117{
118 static float xyz[3] = {-8,-9,3};
119 static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
120 dsSetViewpoint (xyz,hpr);
121}
122
123
124// called when a key pressed
125
126static void command (int cmd)
127{
128 switch (cmd)
129 {
130 case ' ':
131 break;
132 }
133}
134
135
136
137// simulation loop
138
139static void simLoop (int pause)
140{
141 dSpaceCollide (space,0,&nearCallback);
142 if (!pause)
143 {
144 dWorldQuickStep (world, 0.01); // 100 Hz
145 }
146 dJointGroupEmpty (contactgroup);
147
148 dsSetColorAlpha (1,1,0,0.5);
149
150 const dReal *CPos = dBodyGetPosition(cylbody);
151 const dReal *CRot = dBodyGetRotation(cylbody);
152 float cpos[3] = {CPos[0], CPos[1], CPos[2]};
153 float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
154 dsDrawCylinder
155 (
156 cpos,
157 crot,
158 CYLLENGTH,
159 CYLRADIUS
160 ); // single precision
161
162 const dReal *SPos = dBodyGetPosition(sphbody);
163 const dReal *SRot = dBodyGetRotation(sphbody);
164 float spos[3] = {SPos[0], SPos[1], SPos[2]};
165 float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
166 dsDrawSphere
167 (
168 spos,
169 srot,
170 SPHERERADIUS
171 ); // single precision
172}
173
174
175int main (int argc, char **argv)
176{
177 dMass m;
178
179 // setup pointers to drawstuff callback functions
180 dsFunctions fn;
181 fn.version = DS_VERSION;
182 fn.start = &start;
183 fn.step = &simLoop;
184 fn.command = &command;
185 fn.stop = 0;
186 fn.path_to_textures = "../../drawstuff/textures";
187 if(argc==2)
188 fn.path_to_textures = argv[1];
189
190 // create world
191 dInitODE();
192 world = dWorldCreate();
193 space = dHashSpaceCreate (0);
194 contactgroup = dJointGroupCreate (0);
195 dWorldSetGravity (world,0,0,-9.8);
196 dWorldSetQuickStepNumIterations (world, 32);
197
198 dCreatePlane (space,0,0,1, 0.0);
199
200 cylbody = dBodyCreate (world);
201 dQuaternion q;
202#if 0
203 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
204#else
205// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0);
206 dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);
207#endif
208 dBodySetQuaternion (cylbody,q);
209 dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH);
210 dBodySetMass (cylbody,&m);
211 cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH);
212 dGeomSetBody (cylgeom,cylbody);
213 dBodySetPosition (cylbody, 0, 0, 3);
214 dSpaceAdd (space, cylgeom);
215
216 sphbody = dBodyCreate (world);
217 dMassSetSphere (&m,1,SPHERERADIUS);
218 dBodySetMass (sphbody,&m);
219 sphgeom = dCreateSphere(0, SPHERERADIUS);
220 dGeomSetBody (sphgeom,sphbody);
221 dBodySetPosition (sphbody, 0, 0, 5.5);
222 dSpaceAdd (space, sphgeom);
223
224 // run simulation
225 dsSimulationLoop (argc,argv,352,288,&fn);
226
227 dJointGroupEmpty (contactgroup);
228 dJointGroupDestroy (contactgroup);
229
230 dGeomDestroy(sphgeom);
231 dGeomDestroy (cylgeom);
232
233 dSpaceDestroy (space);
234 dWorldDestroy (world);
235 dCloseODE();
236 return 0;
237}
238
diff --git a/libraries/ode-0.9/ode/demo/demo_feedback.cpp b/libraries/ode-0.9/ode/demo/demo_feedback.cpp
new file mode 100644
index 0000000..fd49278
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_feedback.cpp
@@ -0,0 +1,314 @@
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// Test for breaking joints, by Bram Stolk
24
25#include <ode/config.h>
26#include <assert.h>
27#ifdef HAVE_UNISTD_H
28#include <unistd.h>
29#endif
30#include <ode/ode.h>
31#include <drawstuff/drawstuff.h>
32
33
34#ifdef _MSC_VER
35#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
36#endif
37
38#ifdef dDOUBLE
39#define dsDrawBox dsDrawBoxD
40#define dsDrawCylinder dsDrawCylinderD
41#endif
42
43
44// dynamics and collision objects (chassis, 3 wheels, environment)
45
46static dWorldID world;
47static dSpaceID space;
48
49static const int STACKCNT=10; // nr of weights on bridge
50static const int SEGMCNT=16; // nr of segments in bridge
51static const float SEGMDIM[3] = { 0.9, 4, 0.1 };
52
53static dGeomID groundgeom;
54static dBodyID segbodies[SEGMCNT];
55static dGeomID seggeoms[SEGMCNT];
56static dBodyID stackbodies[STACKCNT];
57static dGeomID stackgeoms[STACKCNT];
58static dJointID hinges[SEGMCNT-1];
59static dJointID sliders[2];
60static dJointFeedback jfeedbacks[SEGMCNT-1];
61static dReal colours[SEGMCNT];
62static int stress[SEGMCNT-1];
63
64static dJointGroupID contactgroup;
65
66
67// this is called by dSpaceCollide when two objects in space are
68// potentially colliding.
69
70static void nearCallback (void *data, dGeomID o1, dGeomID o2)
71{
72 assert(o1);
73 assert(o2);
74
75 if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
76 {
77 fprintf(stderr,"testing space %p %p\n", o1,o2);
78 // colliding a space with something
79 dSpaceCollide2(o1,o2,data,&nearCallback);
80 // Note we do not want to test intersections within a space,
81 // only between spaces.
82 return;
83 }
84
85 const int N = 32;
86 dContact contact[N];
87 int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
88 if (n > 0)
89 {
90 for (int i=0; i<n; i++)
91 {
92 contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
93 contact[i].surface.mu = 100.0;
94 contact[i].surface.soft_erp = 0.96;
95 contact[i].surface.soft_cfm = 0.02;
96 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
97 dJointAttach (c,
98 dGeomGetBody(contact[i].geom.g1),
99 dGeomGetBody(contact[i].geom.g2));
100 }
101 }
102}
103
104
105// start simulation - set viewpoint
106
107static void start()
108{
109 static float xyz[3] = { -6, 8, 6};
110 static float hpr[3] = { -65.0f, -27.0f, 0.0f};
111 dsSetViewpoint (xyz,hpr);
112}
113
114
115
116// called when a key pressed
117
118static void command (int cmd)
119{
120}
121
122
123
124void drawGeom (dGeomID g)
125{
126 const dReal *pos = dGeomGetPosition(g);
127 const dReal *R = dGeomGetRotation(g);
128
129 int type = dGeomGetClass (g);
130 if (type == dBoxClass)
131 {
132 dVector3 sides;
133 dGeomBoxGetLengths (g, sides);
134 dsDrawBox (pos,R,sides);
135 }
136 if (type == dCylinderClass)
137 {
138 dReal r,l;
139 dGeomCylinderGetParams(g, &r, &l);
140 dsDrawCylinder (pos, R, l, r);
141 }
142}
143
144
145static void inspectJoints(void)
146{
147 const dReal forcelimit = 2000.0;
148 int i;
149 for (i=0; i<SEGMCNT-1; i++)
150 {
151 if (dJointGetBody(hinges[i], 0))
152 {
153 // This joint has not snapped already... inspect it.
154 dReal l0 = dLENGTH(jfeedbacks[i].f1);
155 dReal l1 = dLENGTH(jfeedbacks[i].f2);
156 colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit;
157 colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit;
158 if (l0 > forcelimit || l1 > forcelimit)
159 stress[i]++;
160 else
161 stress[i]=0;
162 if (stress[i]>4)
163 {
164 // Low-pass filter the noisy feedback data.
165 // Only after 4 consecutive timesteps with excessive load, snap.
166 fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i);
167 dJointAttach (hinges[i], 0, 0);
168 }
169 }
170 }
171}
172
173
174// simulation loop
175
176static void simLoop (int pause)
177{
178 int i;
179
180 double simstep = 0.005; // 5ms simulation steps
181 double dt = dsElapsedTime();
182 int nrofsteps = (int) ceilf(dt/simstep);
183 for (i=0; i<nrofsteps && !pause; i++)
184 {
185 dSpaceCollide (space,0,&nearCallback);
186 dWorldQuickStep (world, simstep);
187 dJointGroupEmpty (contactgroup);
188 inspectJoints();
189 }
190
191 for (i=0; i<SEGMCNT; i++)
192 {
193 float r=0,g=0,b=0.2;
194 float v = colours[i];
195 if (v>1.0) v=1.0;
196 if (v<0.5)
197 {
198 r=2*v;
199 g=1.0;
200 }
201 else
202 {
203 r=1.0;
204 g=2*(1.0-v);
205 }
206 dsSetColor (r,g,b);
207 drawGeom(seggeoms[i]);
208 }
209 dsSetColor (1,1,1);
210 for (i=0; i<STACKCNT; i++)
211 drawGeom(stackgeoms[i]);
212}
213
214
215
216int main (int argc, char **argv)
217{
218 dMass m;
219
220 // setup pointers to drawstuff callback functions
221 dsFunctions fn;
222 fn.version = DS_VERSION;
223 fn.start = &start;
224 fn.step = &simLoop;
225 fn.command = &command;
226 fn.stop = 0;
227 fn.path_to_textures = "../../drawstuff/textures";
228 if(argc==2)
229 {
230 fn.path_to_textures = argv[1];
231 }
232
233 // create world
234 dInitODE();
235 world = dWorldCreate();
236 space = dHashSpaceCreate (0);
237 contactgroup = dJointGroupCreate (0);
238 dWorldSetGravity (world,0,0,-9.8);
239 dWorldSetQuickStepNumIterations (world, 20);
240
241 int i;
242 for (i=0; i<SEGMCNT; i++)
243 {
244 segbodies[i] = dBodyCreate (world);
245 dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
246 dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
247 dBodySetMass (segbodies[i], &m);
248 seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
249 dGeomSetBody (seggeoms[i], segbodies[i]);
250 dSpaceAdd (space, seggeoms[i]);
251 }
252
253 for (i=0; i<SEGMCNT-1; i++)
254 {
255 hinges[i] = dJointCreateHinge (world,0);
256 dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
257 dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
258 dJointSetHingeAxis (hinges[i], 0,1,0);
259 dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
260 // NOTE:
261 // Here we tell ODE where to put the feedback on the forces for this hinge
262 dJointSetFeedback (hinges[i], jfeedbacks+i);
263 stress[i]=0;
264 }
265
266 for (i=0; i<STACKCNT; i++)
267 {
268 stackbodies[i] = dBodyCreate(world);
269 dMassSetBox (&m, 2.0, 2, 2, 0.6);
270 dBodySetMass(stackbodies[i],&m);
271
272 stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
273 dGeomSetBody(stackgeoms[i], stackbodies[i]);
274 dBodySetPosition(stackbodies[i], 0,0,8+2*i);
275 dSpaceAdd(space, stackgeoms[i]);
276 }
277
278 sliders[0] = dJointCreateSlider (world,0);
279 dJointAttach(sliders[0], segbodies[0], 0);
280 dJointSetSliderAxis (sliders[0], 1,0,0);
281 dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
282 dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
283 dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
284
285 sliders[1] = dJointCreateSlider (world,0);
286 dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
287 dJointSetSliderAxis (sliders[1], 1,0,0);
288 dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
289 dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
290 dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
291
292 groundgeom = dCreatePlane(space, 0,0,1,0);
293
294 for (i=0; i<SEGMCNT; i++)
295 colours[i]=0.0;
296
297 // run simulation
298 dsSimulationLoop (argc,argv,352,288,&fn);
299
300 dJointGroupEmpty (contactgroup);
301 dJointGroupDestroy (contactgroup);
302
303 // First destroy seggeoms, then space, then the world.
304 for (i=0; i<SEGMCNT; i++)
305 dGeomDestroy (seggeoms[i]);
306 for (i=0; i<STACKCNT; i++)
307 dGeomDestroy (stackgeoms[i]);
308
309 dSpaceDestroy(space);
310 dWorldDestroy (world);
311 dCloseODE();
312 return 0;
313}
314
diff --git a/libraries/ode-0.9/ode/demo/demo_friction.cpp b/libraries/ode-0.9/ode/demo/demo_friction.cpp
new file mode 100644
index 0000000..36ba2d4
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_friction.cpp
@@ -0,0 +1,206 @@
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/*
24
25test the Coulomb friction approximation.
26
27a 10x10 array of boxes is made, each of which rests on the ground.
28a horizantal force is applied to each box to try and get it to slide.
29box[i][j] has a mass (i+1)*MASS and a force (j+1)*FORCE. by the Coloumb
30friction model, the box should only slide if the force is greater than MU
31times the contact normal force, i.e.
32
33 f > MU * body_mass * GRAVITY
34 (j+1)*FORCE > MU * (i+1)*MASS * GRAVITY
35 (j+1) > (i+1) * (MU*MASS*GRAVITY/FORCE)
36 (j+1) > (i+1) * k
37
38this should be independent of the number of contact points, as N contact
39points will each have 1/N'th the normal force but the pushing force will
40have to overcome N contacts. the constants are chosen so that k=1.
41thus you should see a triangle made of half the bodies in the array start to
42slide.
43
44*/
45
46
47#include <ode/ode.h>
48#include <drawstuff/drawstuff.h>
49
50#ifdef _MSC_VER
51#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
52#endif
53
54// select correct drawing functions
55
56#ifdef dDOUBLE
57#define dsDrawBox dsDrawBoxD
58#define dsDrawSphere dsDrawSphereD
59#define dsDrawCylinder dsDrawCylinderD
60#define dsDrawCapsule dsDrawCapsuleD
61#endif
62
63
64// some constants
65
66#define LENGTH 0.2 // box length & width
67#define HEIGHT 0.05 // box height
68#define MASS 0.2 // mass of box[i][j] = (i+1) * MASS
69#define FORCE 0.05 // force applied to box[i][j] = (j+1) * FORCE
70#define MU 0.5 // the global mu to use
71#define GRAVITY 0.5 // the global gravity to use
72#define N1 10 // number of different forces to try
73#define N2 10 // number of different masses to try
74
75
76// dynamics and collision objects
77
78static dWorldID world;
79static dSpaceID space;
80static dBodyID body[N1][N2];
81static dJointGroupID contactgroup;
82static dGeomID ground;
83static dGeomID box[N1][N2];
84
85
86
87// this is called by dSpaceCollide when two objects in space are
88// potentially colliding.
89
90static void nearCallback (void *data, dGeomID o1, dGeomID o2)
91{
92 int i;
93
94 // only collide things with the ground
95 int g1 = (o1 == ground);
96 int g2 = (o2 == ground);
97 if (!(g1 ^ g2)) return;
98
99 dBodyID b1 = dGeomGetBody(o1);
100 dBodyID b2 = dGeomGetBody(o2);
101
102 dContact contact[3]; // up to 3 contacts per box
103 for (i=0; i<3; i++) {
104 contact[i].surface.mode = dContactSoftCFM | dContactApprox1;
105 contact[i].surface.mu = MU;
106 contact[i].surface.soft_cfm = 0.01;
107 }
108 if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
109 for (i=0; i<numc; i++) {
110 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
111 dJointAttach (c,b1,b2);
112 }
113 }
114}
115
116
117// start simulation - set viewpoint
118
119static void start()
120{
121 static float xyz[3] = {1.7772,-0.7924,2.7600};
122 static float hpr[3] = {90.0000,-54.0000,0.0000};
123 dsSetViewpoint (xyz,hpr);
124}
125
126
127// simulation loop
128
129static void simLoop (int pause)
130{
131 int i;
132 if (!pause) {
133 // apply forces to all bodies
134 for (i=0; i<N1; i++) {
135 for (int j=0; j<N2; j++) {
136 dBodyAddForce (body[i][j],FORCE*(i+1),0,0);
137 }
138 }
139
140 dSpaceCollide (space,0,&nearCallback);
141 dWorldStep (world,0.05);
142
143 // remove all contact joints
144 dJointGroupEmpty (contactgroup);
145 }
146
147 dsSetColor (1,0,1);
148 dReal sides[3] = {LENGTH,LENGTH,HEIGHT};
149 for (i=0; i<N1; i++) {
150 for (int j=0; j<N2; j++) {
151 dsDrawBox (dGeomGetPosition(box[i][j]),dGeomGetRotation(box[i][j]),
152 sides);
153 }
154 }
155}
156
157
158int main (int argc, char **argv)
159{
160 int i,j;
161 dMass m;
162
163 // setup pointers to drawstuff callback functions
164 dsFunctions fn;
165 fn.version = DS_VERSION;
166 fn.start = &start;
167 fn.step = &simLoop;
168 fn.command = 0;
169 fn.stop = 0;
170 fn.path_to_textures = "../../drawstuff/textures";
171 if(argc==2)
172 {
173 fn.path_to_textures = argv[1];
174 }
175
176 // create world
177 dInitODE();
178 world = dWorldCreate();
179 space = dHashSpaceCreate (0);
180 contactgroup = dJointGroupCreate (0);
181 dWorldSetGravity (world,0,0,-GRAVITY);
182 ground = dCreatePlane (space,0,0,1,0);
183
184 // bodies
185 for (i=0; i<N1; i++) {
186 for (j=0; j<N2; j++) {
187 body[i][j] = dBodyCreate (world);
188 dMassSetBox (&m,1,LENGTH,LENGTH,HEIGHT);
189 dMassAdjust (&m,MASS*(j+1));
190 dBodySetMass (body[i][j],&m);
191 dBodySetPosition (body[i][j],i*2*LENGTH,j*2*LENGTH,HEIGHT*0.5);
192
193 box[i][j] = dCreateBox (space,LENGTH,LENGTH,HEIGHT);
194 dGeomSetBody (box[i][j],body[i][j]);
195 }
196 }
197
198 // run simulation
199 dsSimulationLoop (argc,argv,352,288,&fn);
200
201 dJointGroupDestroy (contactgroup);
202 dSpaceDestroy (space);
203 dWorldDestroy (world);
204 dCloseODE();
205 return 0;
206}
diff --git a/libraries/ode-0.9/ode/demo/demo_heightfield.cpp b/libraries/ode-0.9/ode/demo/demo_heightfield.cpp
new file mode 100644
index 0000000..83156ab
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_heightfield.cpp
@@ -0,0 +1,2132 @@
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}
diff --git a/libraries/ode-0.9/ode/demo/demo_hinge.cpp b/libraries/ode-0.9/ode/demo/demo_hinge.cpp
new file mode 100644
index 0000000..58e8f9e
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_hinge.cpp
@@ -0,0 +1,166 @@
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// select correct drawing functions
31#ifdef dDOUBLE
32#define dsDrawBox dsDrawBoxD
33#endif
34
35
36// some constants
37#define SIDE (0.5f) // side length of a box
38#define MASS (1.0) // mass of a box
39
40
41// dynamics and collision objects
42static dWorldID world;
43static dBodyID body[2];
44static dJointID hinge;
45
46
47// state set by keyboard commands
48static int occasional_error = 0;
49
50
51// start simulation - set viewpoint
52
53static void start()
54{
55 static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
56 static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
57 dsSetViewpoint (xyz,hpr);
58 printf ("Press 'e' to start/stop occasional error.\n");
59}
60
61
62// called when a key pressed
63
64static void command (int cmd)
65{
66 if (cmd == 'e' || cmd == 'E') {
67 occasional_error ^= 1;
68 }
69}
70
71
72// simulation loop
73
74static void simLoop (int pause)
75{
76 const dReal kd = -0.3; // angular damping constant
77 if (!pause) {
78 // add an oscillating torque to body 0, and also damp its rotational motion
79 static dReal a=0;
80 const dReal *w = dBodyGetAngularVel (body[0]);
81 dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
82 dWorldStep (world,0.05);
83 a += 0.01;
84
85 // occasionally re-orient one of the bodies to create a deliberate error.
86 if (occasional_error) {
87 static int count = 0;
88 if ((count % 20)==0) {
89 // randomly adjust orientation of body[0]
90 const dReal *R1;
91 dMatrix3 R2,R3;
92 R1 = dBodyGetRotation (body[0]);
93 dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
94 dRandReal()-0.5,dRandReal()-0.5);
95 dMultiply0 (R3,R1,R2,3,3,3);
96 dBodySetRotation (body[0],R3);
97
98 // randomly adjust position of body[0]
99 const dReal *pos = dBodyGetPosition (body[0]);
100 dBodySetPosition (body[0],
101 pos[0]+0.2*(dRandReal()-0.5),
102 pos[1]+0.2*(dRandReal()-0.5),
103 pos[2]+0.2*(dRandReal()-0.5));
104 }
105 count++;
106 }
107 }
108
109 dReal sides1[3] = {SIDE,SIDE,SIDE};
110 dReal sides2[3] = {SIDE,SIDE,SIDE*0.8f};
111 dsSetTexture (DS_WOOD);
112 dsSetColor (1,1,0);
113 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
114 dsSetColor (0,1,1);
115 dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
116}
117
118
119int main (int argc, char **argv)
120{
121 // setup pointers to drawstuff callback functions
122 dsFunctions fn;
123 fn.version = DS_VERSION;
124 fn.start = &start;
125 fn.step = &simLoop;
126 fn.command = &command;
127 fn.stop = 0;
128 fn.path_to_textures = "../../drawstuff/textures";
129 if(argc==2)
130 {
131 fn.path_to_textures = argv[1];
132 }
133
134 // create world
135 dInitODE();
136 world = dWorldCreate();
137
138 dMass m;
139 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
140 dMassAdjust (&m,MASS);
141
142 dQuaternion q;
143 dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
144
145 body[0] = dBodyCreate (world);
146 dBodySetMass (body[0],&m);
147 dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
148 dBodySetQuaternion (body[0],q);
149
150 body[1] = dBodyCreate (world);
151 dBodySetMass (body[1],&m);
152 dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
153 dBodySetQuaternion (body[1],q);
154
155 hinge = dJointCreateHinge (world,0);
156 dJointAttach (hinge,body[0],body[1]);
157 dJointSetHingeAnchor (hinge,0,0,1);
158 dJointSetHingeAxis (hinge,1,-1,1.41421356);
159
160 // run simulation
161 dsSimulationLoop (argc,argv,352,288,&fn);
162
163 dWorldDestroy (world);
164 dCloseODE();
165 return 0;
166}
diff --git a/libraries/ode-0.9/ode/demo/demo_jointPR.cpp b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
new file mode 100644
index 0000000..e0d002d
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
@@ -0,0 +1,377 @@
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/*
24
25This file try to demonstrate how the PR joint is working.
26
27The axisP is draw in red and the axisR is in green
28
29*/
30
31
32#include <ode/ode.h>
33#include <drawstuff/drawstuff.h>
34#include <iostream>
35#include <math.h>
36
37
38#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures"
39
40
41#ifdef _MSC_VER
42#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
43#endif
44// select correct drawing functions
45#ifdef dDOUBLE
46#define dsDrawBox dsDrawBoxD
47#endif
48
49// physics parameters
50#define BOX1_LENGTH 2 // Size along the X axis
51#define BOX1_WIDTH 1 // Size along the Y axis
52#define BOX1_HEIGHT 0.4 // Size along the Z axis (up) since gravity is (0,0,-10)
53#define BOX2_LENGTH 0.2
54#define BOX2_WIDTH 0.1
55#define BOX2_HEIGHT 0.4
56#define Mass1 10
57#define Mass2 0.1
58
59
60#define PRISMATIC_ONLY 1
61#define ROTOIDE_ONLY 2
62int flag = 0;
63
64
65//camera view
66static float xyz[3] = {2.0f,-3.5f,2.0000f};
67static float hpr[3] = {90.000f,-25.5000f,0.0000f};
68//world,space,body & geom
69static dWorldID world;
70static dSpaceID space;
71static dSpaceID box1_space;
72static dSpaceID box2_space;
73static dBodyID box1_body[1];
74static dBodyID box2_body[1];
75static dJointID joint[1];
76static dJointGroupID contactgroup;
77static dGeomID ground;
78static dGeomID box1[1];
79static dGeomID box2[1];
80
81
82//collision detection
83static void nearCallback (void *data, dGeomID o1, dGeomID o2)
84{
85 int i,n;
86
87 dBodyID b1 = dGeomGetBody(o1);
88 dBodyID b2 = dGeomGetBody(o2);
89 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
90 const int N = 10;
91 dContact contact[N];
92 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
93 if (n > 0)
94 {
95 for (i=0; i<n; i++)
96 {
97 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
98 dContactSoftERP | dContactSoftCFM | dContactApprox1;
99 contact[i].surface.mu = 0.1;
100 contact[i].surface.slip1 = 0.02;
101 contact[i].surface.slip2 = 0.02;
102 contact[i].surface.soft_erp = 0.1;
103 contact[i].surface.soft_cfm = 0.0001;
104 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
105 dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
106 }
107 }
108}
109
110
111// start simulation - set viewpoint
112static void start()
113{
114 dsSetViewpoint (xyz,hpr);
115 printf ("Press 'd' to add force along positive x direction.\nPress 'a' to add force along negative x direction.\n");
116 printf ("Press 'w' to add force along positive y direction.\nPress 's' to add force along negative y direction.\n");
117 printf ("Press 'e' to add torque around positive z direction.\nPress 'q' to add torque around negative z direction.\n");
118 printf ("Press 'o' to add force around positive x direction \n");
119}
120
121// function to update camera position at each step.
122void update()
123{
124// const dReal *a =(dBodyGetPosition (box1_body[0]));
125// float dx=a[0];
126// float dy=a[1];
127// float dz=a[2];
128// xyz[0]=dx;
129// xyz[1]=dy-5;
130// xyz[2]=dz+2;
131// hpr[1]=-22.5000f;
132// dsSetViewpoint (xyz,hpr);
133}
134
135
136// called when a key pressed
137static void command (int cmd)
138{
139 switch(cmd)
140 {
141 case 'w': case 'W':
142 dBodyAddForce(box2_body[0],0,500,0);
143 std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
144 break;
145 case 's': case 'S':
146 dBodyAddForce(box2_body[0],0,-500,0);
147 std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
148 break;
149 case 'd': case 'D':
150 dBodyAddForce(box2_body[0],500,0,0);
151 std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
152 break;
153 case 'a': case 'A':
154 dBodyAddForce(box2_body[0],-500,0,0);
155 std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
156 break;
157 case 'e': case 'E':
158 dBodyAddRelTorque(box2_body[0],0,0,200);
159 break;
160 case 'q': case 'Q':
161 dBodyAddRelTorque(box2_body[0],0,0,-200);
162 break;
163 case 'o': case 'O':
164 dBodyAddForce(box1_body[0],10000,0,0);
165 break;
166 }
167}
168
169
170// simulation loop
171static void simLoop (int pause)
172{
173 if (!pause)
174 {
175 //draw 2 boxes
176 dVector3 ss;
177 dsSetTexture (DS_WOOD);
178
179 const dReal *posBox2 = dGeomGetPosition(box2[0]);
180 const dReal *rotBox2 = dGeomGetRotation(box2[0]);
181 dsSetColor (1,1,0);
182 dGeomBoxGetLengths (box2[0],ss);
183 dsDrawBox (posBox2, rotBox2, ss);
184
185 const dReal *posBox1 = dGeomGetPosition(box1[0]);
186 const dReal *rotBox1 = dGeomGetRotation(box1[0]);
187 dsSetColor (1,1,2);
188 dGeomBoxGetLengths (box1[0], ss);
189 dsDrawBox (posBox1, rotBox1, ss);
190
191 dVector3 anchorPos;
192 dJointGetPRAnchor (joint[0], anchorPos);
193
194 // Draw the axisP
195 if (ROTOIDE_ONLY != flag )
196 {
197 dsSetColor (1,0,0);
198 dVector3 sizeP = {0, 0.1, 0.1};
199 for (int i=0; i<3; ++i)
200 sizeP[0] += (anchorPos[i] - posBox1[i])*(anchorPos[i] - posBox1[i]);
201 sizeP[0] = sqrt(sizeP[0]);
202 dVector3 posAxisP;
203 for (int i=0; i<3; ++i)
204 posAxisP[i] = posBox1[i] + (anchorPos[i] - posBox1[i])/2.0;
205 dsDrawBox (posAxisP, rotBox1, sizeP);
206 }
207
208
209 // Draw the axisR
210 if (PRISMATIC_ONLY != flag )
211 {
212 dsSetColor (0,1,0);
213 dVector3 sizeR = {0, 0.1, 0.1};
214 for (int i=0; i<3; ++i)
215 sizeR[0] += (anchorPos[i] - posBox2[i])*(anchorPos[i] - posBox2[i]);
216 sizeR[0] = sqrt(sizeR[0]);
217 dVector3 posAxisR;
218 for (int i=0; i<3; ++i)
219 posAxisR[i] = posBox2[i] + (anchorPos[i] - posBox2[i])/2.0;
220 dsDrawBox (posAxisR, rotBox2, sizeR);
221 }
222
223 dSpaceCollide (space,0,&nearCallback);
224 dWorldQuickStep (world,0.0001);
225 update();
226 dJointGroupEmpty (contactgroup);
227 }
228}
229
230
231void Help(char **argv)
232{
233 printf("%s ", argv[0]);
234 printf(" -h | --help : print this help\n");
235 printf(" -b | --both : Display how the complete joint works\n");
236 printf(" Default behavior\n");
237 printf(" -p | --prismatic-only : Display how the prismatic part works\n");
238 printf(" The anchor pts is set at the center of body 2\n");
239 printf(" -r | --rotoide-only : Display how the rotoide part works\n");
240 printf(" The anchor pts is set at the center of body 1\n");
241 printf(" -t | --texture-path path : Path to the texture.\n");
242 printf(" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
243 printf("--------------------------------------------------\n");
244 printf("Hit any key to continue:");
245 getchar();
246
247 exit(0);
248}
249
250int main (int argc, char **argv)
251{
252 // setup pointers to drawstuff callback functions
253 dsFunctions fn;
254 fn.version = DS_VERSION;
255 fn.start = &start;
256 fn.step = &simLoop;
257 fn.command = &command;
258 fn.stop = 0;
259 fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
260
261 if (argc >= 2 )
262 {
263 for (int i=1; i < argc; ++i)
264 {
265 if( 0 == strcmp("-h", argv[i]) || 0 == strcmp("--help", argv[i]) )
266 Help(argv);
267
268 if(!flag && (0 == strcmp("-p", argv[i]) ||0 == strcmp("--prismatic-only", argv[i])) )
269 flag = PRISMATIC_ONLY;
270
271 if(!flag && (0 == strcmp("-r", argv[i]) || 0 == strcmp("--rotoide-only", argv[i])) )
272 flag = ROTOIDE_ONLY;
273
274 if(0 == strcmp("-t", argv[i]) || 0 == strcmp("--texture-path", argv[i]))
275 {
276 int j = i+1;
277 if ( j+1 > argc || // Check if we have enough arguments
278 argv[j] == '\0' || // We should have a path here
279 argv[j][0] == '-' ) // We should have a path not a command line
280 Help(argv);
281 else
282 fn.path_to_textures = argv[++i]; // Increase i since we use this argument
283 }
284 }
285 }
286
287 // create world
288 world = dWorldCreate();
289 space = dHashSpaceCreate (0);
290 contactgroup = dJointGroupCreate (0);
291 dWorldSetGravity (world,0,0,-10);
292 ground = dCreatePlane (space,0,0,1,0);
293
294 //create two boxes
295 dMass m;
296 box1_body[0] = dBodyCreate (world);
297 dMassSetBox (&m,1,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
298 dMassAdjust (&m,Mass1);
299 dBodySetMass (box1_body[0],&m);
300 box1[0] = dCreateBox (0,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
301 dGeomSetBody (box1[0],box1_body[0]);
302
303 box2_body[0] = dBodyCreate (world);
304 dMassSetBox (&m,10,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
305 dMassAdjust (&m,Mass2);
306 dBodySetMass (box2_body[0],&m);
307 box2[0] = dCreateBox (0,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
308 dGeomSetBody (box2[0],box2_body[0]);
309
310 //set the initial positions of body1 and body2
311 dMatrix3 R;
312 dRSetIdentity(R);
313 dBodySetPosition (box1_body[0],0,0,BOX1_HEIGHT/2.0);
314 dBodySetRotation (box1_body[0], R);
315
316 dBodySetPosition (box2_body[0],
317 2.1,
318 0.0,
319 BOX2_HEIGHT/2.0);
320 dBodySetRotation (box2_body[0], R);
321
322
323 //set PR joint
324 joint[0] = dJointCreatePR(world,0);
325 dJointAttach (joint[0],box1_body[0],box2_body[0]);
326 switch (flag)
327 {
328 case PRISMATIC_ONLY:
329 dJointSetPRAnchor (joint[0],
330 2.1,
331 0.0,
332 BOX2_HEIGHT/2.0);
333 dJointSetPRParam (joint[0],dParamLoStop, -0.5);
334 dJointSetPRParam (joint[0],dParamHiStop, 1.5);
335 break;
336
337 case ROTOIDE_ONLY:
338 dJointSetPRAnchor (joint[0],
339 0.0,
340 0.0,
341 BOX2_HEIGHT/2.0);
342 dJointSetPRParam (joint[0],dParamLoStop, 0.0);
343 dJointSetPRParam (joint[0],dParamHiStop, 0.0);
344 break;
345
346 default:
347 dJointSetPRAnchor (joint[0],
348 1.1,
349 0.0,
350 BOX2_HEIGHT/2.0);
351 dJointSetPRParam (joint[0],dParamLoStop, -0.5);
352 dJointSetPRParam (joint[0],dParamHiStop, 1.5);
353 break;
354 }
355
356 dJointSetPRAxis1(joint[0],1,0,0);
357 dJointSetPRAxis2(joint[0],0,0,1);
358// We position the 2 body
359// The position of the rotoide joint is on the second body so it can rotate on itself
360// and move along the X axis.
361// With this anchor
362// - A force in X will move only the body 2 inside the low and hi limit
363// of the prismatic
364// - A force in Y will make the 2 bodies to rotate around on the plane
365
366 box1_space = dSimpleSpaceCreate (space);
367 dSpaceSetCleanup (box1_space,0);
368 dSpaceAdd(box1_space,box1[0]);
369
370 // run simulation
371 dsSimulationLoop (argc,argv,400,300,&fn);
372 dJointGroupDestroy (contactgroup);
373 dSpaceDestroy (space);
374 dWorldDestroy (world);
375 return 0;
376}
377
diff --git a/libraries/ode-0.9/ode/demo/demo_joints.cpp b/libraries/ode-0.9/ode/demo/demo_joints.cpp
new file mode 100644
index 0000000..2a83c2f
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_joints.cpp
@@ -0,0 +1,1092 @@
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/*
24
25perform tests on all the joint types.
26this should be done using the double precision version of the library.
27
28usage:
29 test_joints [-nXXX] [-g] [-i] [-e] [path_to_textures]
30
31if a test number is given then that specific test is performed, otherwise
32all the tests are performed. the tests are numbered `xxyy', where xx
33corresponds to the joint type and yy is the sub-test number. not every
34number maps to an actual test.
35
36flags:
37 i: the test is interactive.
38 g: turn off graphical display (can't use this with `i').
39 e: turn on occasional error perturbations
40 n: performe test XXX
41some tests compute and display error values. these values are scaled so
42<1 is good and >1 is bad. other tests just show graphical results which
43you must verify visually.
44
45*/
46
47#include <ctype.h>
48#include <ode/ode.h>
49#include <drawstuff/drawstuff.h>
50
51#ifdef _MSC_VER
52#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
53#endif
54
55// select correct drawing functions
56#ifdef dDOUBLE
57#define dsDrawBox dsDrawBoxD
58#endif
59
60
61// some constants
62#define NUM_JOINTS 10 // number of joints to test (the `xx' value)
63#define SIDE (0.5f) // side length of a box - don't change this
64#define MASS (1.0) // mass of a box
65#define STEPSIZE 0.05
66
67
68// dynamics objects
69static dWorldID world;
70static dBodyID body[2];
71static dJointID joint;
72
73
74// data from the command line arguments
75static int cmd_test_num = -1;
76static int cmd_interactive = 0;
77static int cmd_graphics = 1;
78static char *cmd_path_to_textures = NULL;
79static int cmd_occasional_error = 0; // perturb occasionally
80
81
82// info about the current test
83struct TestInfo;
84static int test_num = 0; // number of the current test
85static int iteration = 0;
86static int max_iterations = 0;
87static dReal max_error = 0;
88
89//****************************************************************************
90// utility stuff
91
92static char loCase (char a)
93{
94 if (a >= 'A' && a <= 'Z') return a + ('a'-'A');
95 else return a;
96}
97
98
99static dReal length (dVector3 a)
100{
101 return dSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
102}
103
104
105// get the max difference between a 3x3 matrix and the identity
106
107dReal cmpIdentity (const dMatrix3 A)
108{
109 dMatrix3 I;
110 dSetZero (I,12);
111 I[0] = 1;
112 I[5] = 1;
113 I[10] = 1;
114 return dMaxDifference (A,I,3,3);
115}
116
117//****************************************************************************
118// test world construction and utilities
119
120void constructWorldForTest (dReal gravity, int bodycount,
121 /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z,
122 /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z,
123 /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z,
124 /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z,
125 /* rotation angles */ dReal a1, dReal a2)
126{
127 // create world
128 world = dWorldCreate();
129 dWorldSetERP (world,0.2);
130 dWorldSetCFM (world,1e-6);
131 dWorldSetGravity (world,0,0,gravity);
132
133 dMass m;
134 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
135 dMassAdjust (&m,MASS);
136
137 body[0] = dBodyCreate (world);
138 dBodySetMass (body[0],&m);
139 dBodySetPosition (body[0], pos1x, pos1y, pos1z);
140 dQuaternion q;
141 dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1);
142 dBodySetQuaternion (body[0],q);
143
144 if (bodycount==2) {
145 body[1] = dBodyCreate (world);
146 dBodySetMass (body[1],&m);
147 dBodySetPosition (body[1], pos2x, pos2y, pos2z);
148 dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2);
149 dBodySetQuaternion (body[1],q);
150 }
151 else body[1] = 0;
152}
153
154
155// add an oscillating torque to body 0
156
157void addOscillatingTorque (dReal tscale)
158{
159 static dReal a=0;
160 dBodyAddTorque (body[0],tscale*cos(2*a),tscale*cos(2.7183*a),
161 tscale*cos(1.5708*a));
162 a += 0.01;
163}
164
165
166void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z)
167{
168 static dReal a=0;
169 dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y,
170 tscale * cos(a) * z);
171 a += 0.02;
172}
173
174
175// damp the rotational motion of body 0 a bit
176
177void dampRotationalMotion (dReal kd)
178{
179 const dReal *w = dBodyGetAngularVel (body[0]);
180 dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]);
181}
182
183
184// add a spring force to keep the bodies together, otherwise they may fly
185// apart with some joints.
186
187void addSpringForce (dReal ks)
188{
189 const dReal *p1 = dBodyGetPosition (body[0]);
190 const dReal *p2 = dBodyGetPosition (body[1]);
191 dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),ks*(p2[2]-p1[2]));
192 dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),ks*(p1[2]-p2[2]));
193}
194
195
196// add an oscillating Force to body 0
197
198void addOscillatingForce (dReal fscale)
199{
200 static dReal a=0;
201 dBodyAddForce (body[0],fscale*cos(2*a),fscale*cos(2.7183*a),
202 fscale*cos(1.5708*a));
203 a += 0.01;
204}
205
206//****************************************************************************
207// stuff specific to the tests
208//
209// 0xx : fixed
210// 1xx : ball and socket
211// 2xx : hinge
212// 3xx : slider
213// 4xx : hinge 2
214// 5xx : contact
215// 6xx : amotor
216// 7xx : universal joint
217// 8xx : PR joint (Prismatic and Rotoide)
218
219// setup for the given test. return 0 if there is no such test
220
221int setupTest (int n)
222{
223 switch (n) {
224
225 // ********** fixed joint
226
227 case 0: { // 2 body
228 constructWorldForTest (0,2,
229 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
230 1,1,0, 1,1,0,
231 0.25*M_PI,0.25*M_PI);
232 joint = dJointCreateFixed (world,0);
233 dJointAttach (joint,body[0],body[1]);
234 dJointSetFixed (joint);
235 return 1;
236 }
237
238 case 1: { // 1 body to static env
239 constructWorldForTest (0,1,
240 0.5*SIDE,0.5*SIDE,1, 0,0,0,
241 1,0,0, 1,0,0,
242 0,0);
243 joint = dJointCreateFixed (world,0);
244 dJointAttach (joint,body[0],0);
245 dJointSetFixed (joint);
246 return 1;
247 }
248
249 case 2: { // 2 body with relative rotation
250 constructWorldForTest (0,2,
251 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
252 1,1,0, 1,1,0,
253 0.25*M_PI,-0.25*M_PI);
254 joint = dJointCreateFixed (world,0);
255 dJointAttach (joint,body[0],body[1]);
256 dJointSetFixed (joint);
257 return 1;
258 }
259
260 case 3: { // 1 body to static env with relative rotation
261 constructWorldForTest (0,1,
262 0.5*SIDE,0.5*SIDE,1, 0,0,0,
263 1,0,0, 1,0,0,
264 0.25*M_PI,0);
265 joint = dJointCreateFixed (world,0);
266 dJointAttach (joint,body[0],0);
267 dJointSetFixed (joint);
268 return 1;
269 }
270
271 // ********** hinge joint
272
273 case 200: // 2 body
274 constructWorldForTest (0,2,
275 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
276 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
277 joint = dJointCreateHinge (world,0);
278 dJointAttach (joint,body[0],body[1]);
279 dJointSetHingeAnchor (joint,0,0,1);
280 dJointSetHingeAxis (joint,1,-1,1.41421356);
281 return 1;
282
283 case 220: // hinge angle polarity test
284 case 221: // hinge angle rate test
285 constructWorldForTest (0,2,
286 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
287 1,0,0, 1,0,0, 0,0);
288 joint = dJointCreateHinge (world,0);
289 dJointAttach (joint,body[0],body[1]);
290 dJointSetHingeAnchor (joint,0,0,1);
291 dJointSetHingeAxis (joint,0,0,1);
292 max_iterations = 50;
293 return 1;
294
295 case 230: // hinge motor rate (and polarity) test
296 case 231: // ...with stops
297 constructWorldForTest (0,2,
298 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
299 1,0,0, 1,0,0, 0,0);
300 joint = dJointCreateHinge (world,0);
301 dJointAttach (joint,body[0],body[1]);
302 dJointSetHingeAnchor (joint,0,0,1);
303 dJointSetHingeAxis (joint,0,0,1);
304 dJointSetHingeParam (joint,dParamFMax,1);
305 if (n==231) {
306 dJointSetHingeParam (joint,dParamLoStop,-0.5);
307 dJointSetHingeParam (joint,dParamHiStop,0.5);
308 }
309 return 1;
310
311 case 250: // limit bounce test (gravity down)
312 case 251: { // ...gravity up
313 constructWorldForTest ((n==251) ? 0.1 : -0.1, 2,
314 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE,
315 1,0,0, 1,0,0, 0,0);
316 joint = dJointCreateHinge (world,0);
317 dJointAttach (joint,body[0],body[1]);
318 dJointSetHingeAnchor (joint,0,0,1);
319 dJointSetHingeAxis (joint,0,1,0);
320 dJointSetHingeParam (joint,dParamLoStop,-0.9);
321 dJointSetHingeParam (joint,dParamHiStop,0.7854);
322 dJointSetHingeParam (joint,dParamBounce,0.5);
323 // anchor 2nd body with a fixed joint
324 dJointID j = dJointCreateFixed (world,0);
325 dJointAttach (j,body[1],0);
326 dJointSetFixed (j);
327 return 1;
328 }
329
330 // ********** slider
331
332 case 300: // 2 body
333 constructWorldForTest (0,2,
334 0,0,1, 0.2,0.2,1.2,
335 0,0,1, -1,1,0, 0,0.25*M_PI);
336 joint = dJointCreateSlider (world,0);
337 dJointAttach (joint,body[0],body[1]);
338 dJointSetSliderAxis (joint,1,1,1);
339 return 1;
340
341 case 320: // slider angle polarity test
342 case 321: // slider angle rate test
343 constructWorldForTest (0,2,
344 0,0,1, 0,0,1.2,
345 1,0,0, 1,0,0, 0,0);
346 joint = dJointCreateSlider (world,0);
347 dJointAttach (joint,body[0],body[1]);
348 dJointSetSliderAxis (joint,0,0,1);
349 max_iterations = 50;
350 return 1;
351
352 case 330: // slider motor rate (and polarity) test
353 case 331: // ...with stops
354 constructWorldForTest (0, 2,
355 0,0,1, 0,0,1.2,
356 1,0,0, 1,0,0, 0,0);
357 joint = dJointCreateSlider (world,0);
358 dJointAttach (joint,body[0],body[1]);
359 dJointSetSliderAxis (joint,0,0,1);
360 dJointSetSliderParam (joint,dParamFMax,100);
361 if (n==331) {
362 dJointSetSliderParam (joint,dParamLoStop,-0.4);
363 dJointSetSliderParam (joint,dParamHiStop,0.4);
364 }
365 return 1;
366
367 case 350: // limit bounce tests
368 case 351: {
369 constructWorldForTest ((n==351) ? 0.1 : -0.1, 2,
370 0,0,1, 0,0,1.2,
371 1,0,0, 1,0,0, 0,0);
372 joint = dJointCreateSlider (world,0);
373 dJointAttach (joint,body[0],body[1]);
374 dJointSetSliderAxis (joint,0,0,1);
375 dJointSetSliderParam (joint,dParamLoStop,-0.5);
376 dJointSetSliderParam (joint,dParamHiStop,0.5);
377 dJointSetSliderParam (joint,dParamBounce,0.5);
378 // anchor 2nd body with a fixed joint
379 dJointID j = dJointCreateFixed (world,0);
380 dJointAttach (j,body[1],0);
381 dJointSetFixed (j);
382 return 1;
383 }
384
385 // ********** hinge-2 joint
386
387 case 420: // hinge-2 steering angle polarity test
388 case 421: // hinge-2 steering angle rate test
389 constructWorldForTest (0,2,
390 0.5*SIDE,0,1, -0.5*SIDE,0,1,
391 1,0,0, 1,0,0, 0,0);
392 joint = dJointCreateHinge2 (world,0);
393 dJointAttach (joint,body[0],body[1]);
394 dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
395 dJointSetHinge2Axis1 (joint,0,0,1);
396 dJointSetHinge2Axis2 (joint,1,0,0);
397 max_iterations = 50;
398 return 1;
399
400 case 430: // hinge 2 steering motor rate (+polarity) test
401 case 431: // ...with stops
402 case 432: // hinge 2 wheel motor rate (+polarity) test
403 constructWorldForTest (0,2,
404 0.5*SIDE,0,1, -0.5*SIDE,0,1,
405 1,0,0, 1,0,0, 0,0);
406 joint = dJointCreateHinge2 (world,0);
407 dJointAttach (joint,body[0],body[1]);
408 dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
409 dJointSetHinge2Axis1 (joint,0,0,1);
410 dJointSetHinge2Axis2 (joint,1,0,0);
411 dJointSetHinge2Param (joint,dParamFMax,1);
412 dJointSetHinge2Param (joint,dParamFMax2,1);
413 if (n==431) {
414 dJointSetHinge2Param (joint,dParamLoStop,-0.5);
415 dJointSetHinge2Param (joint,dParamHiStop,0.5);
416 }
417 return 1;
418
419 // ********** angular motor joint
420
421 case 600: // test euler angle calculations
422 constructWorldForTest (0,2,
423 -SIDE*0.5,0,1, SIDE*0.5,0,1,
424 0,0,1, 0,0,1, 0,0);
425 joint = dJointCreateAMotor (world,0);
426 dJointAttach (joint,body[0],body[1]);
427
428 dJointSetAMotorNumAxes (joint,3);
429 dJointSetAMotorAxis (joint,0,1, 0,0,1);
430 dJointSetAMotorAxis (joint,2,2, 1,0,0);
431 dJointSetAMotorMode (joint,dAMotorEuler);
432 max_iterations = 200;
433 return 1;
434
435 // ********** universal joint
436
437 case 700: // 2 body
438 case 701:
439 case 702:
440 constructWorldForTest (0,2,
441 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
442 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
443 joint = dJointCreateUniversal (world,0);
444 dJointAttach (joint,body[0],body[1]);
445 dJointSetUniversalAnchor (joint,0,0,1);
446 dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
447 dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
448 return 1;
449
450 case 720: // universal transmit torque test
451 case 721:
452 case 722:
453 case 730: // universal torque about axis 1
454 case 731:
455 case 732:
456 case 740: // universal torque about axis 2
457 case 741:
458 case 742:
459 constructWorldForTest (0,2,
460 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
461 1,0,0, 1,0,0, 0,0);
462 joint = dJointCreateUniversal (world,0);
463 dJointAttach (joint,body[0],body[1]);
464 dJointSetUniversalAnchor (joint,0,0,1);
465 dJointSetUniversalAxis1 (joint,0,0,1);
466 dJointSetUniversalAxis2 (joint, 1, -1,0);
467 max_iterations = 100;
468 return 1;
469
470 // Joint PR (Prismatic and Rotoide)
471 case 800: // 2 body
472 case 801: // 2 bodies with spring force and prismatic fixed
473 case 802: // 2 bodies with torque on body1 and prismatic fixed
474 constructWorldForTest (0, 2,
475 -1.0, 0.0, 1.0,
476 1.0, 0.0, 1.0,
477 1,0,0, 1,0,0,
478 0, 0);
479 joint = dJointCreatePR (world, 0);
480 dJointAttach (joint, body[0], body[1]);
481 dJointSetPRAnchor (joint,-0.5, 0.0, 1.0);
482 dJointSetPRAxis1 (joint, 0, 1, 0);
483 dJointSetPRAxis2 (joint, 1, 0, 0);
484 dJointSetPRParam (joint,dParamLoStop,-0.5);
485 dJointSetPRParam (joint,dParamHiStop,0.5);
486 dJointSetPRParam (joint,dParamLoStop2,0);
487 dJointSetPRParam (joint,dParamHiStop2,0);
488 return 1;
489 case 803: // 2 bodies with spring force and prismatic NOT fixed
490 case 804: // 2 bodies with torque force and prismatic NOT fixed
491 case 805: // 2 bodies with force only on first body
492 constructWorldForTest (0, 2,
493 -1.0, 0.0, 1.0,
494 1.0, 0.0, 1.0,
495 1,0,0, 1,0,0,
496 0, 0);
497 joint = dJointCreatePR (world, 0);
498 dJointAttach (joint, body[0], body[1]);
499 dJointSetPRAnchor (joint,-0.5, 0.0, 1.0);
500 dJointSetPRAxis1 (joint, 0, 1, 0);
501 dJointSetPRAxis2 (joint, 1, 0, 0);
502 dJointSetPRParam (joint,dParamLoStop,-0.5);
503 dJointSetPRParam (joint,dParamHiStop,0.5);
504 dJointSetPRParam (joint,dParamLoStop2,-0.5);
505 dJointSetPRParam (joint,dParamHiStop2,0.5);
506 return 1;
507 }
508 return 0;
509}
510
511
512// do stuff specific to this test each iteration. you can check some
513// invariants for the test -- the return value is some scaled error measurement
514// that must be less than 1.
515// return a dInfinity if error is not measured for this n.
516
517dReal doStuffAndGetError (int n)
518{
519 switch (n) {
520
521 // ********** fixed joint
522
523 case 0: { // 2 body
524 addOscillatingTorque (0.1);
525 dampRotationalMotion (0.1);
526 // check the orientations are the same
527 const dReal *R1 = dBodyGetRotation (body[0]);
528 const dReal *R2 = dBodyGetRotation (body[1]);
529 dReal err1 = dMaxDifference (R1,R2,3,3);
530 // check the body offset is correct
531 dVector3 p,pp;
532 const dReal *p1 = dBodyGetPosition (body[0]);
533 const dReal *p2 = dBodyGetPosition (body[1]);
534 for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
535 dMULTIPLY1_331 (pp,R1,p);
536 pp[0] += 0.5;
537 pp[1] += 0.5;
538 return (err1 + length (pp)) * 300;
539 }
540
541 case 1: { // 1 body to static env
542 addOscillatingTorque (0.1);
543
544 // check the orientation is the identity
545 dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));
546
547 // check the body offset is correct
548 dVector3 p;
549 const dReal *p1 = dBodyGetPosition (body[0]);
550 for (int i=0; i<3; i++) p[i] = p1[i];
551 p[0] -= 0.25;
552 p[1] -= 0.25;
553 p[2] -= 1;
554 return (err1 + length (p)) * 1e6;
555 }
556
557 case 2: { // 2 body
558 addOscillatingTorque (0.1);
559 dampRotationalMotion (0.1);
560 // check the body offset is correct
561 // Should really check body rotation too. Oh well.
562 const dReal *R1 = dBodyGetRotation (body[0]);
563 dVector3 p,pp;
564 const dReal *p1 = dBodyGetPosition (body[0]);
565 const dReal *p2 = dBodyGetPosition (body[1]);
566 for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
567 dMULTIPLY1_331 (pp,R1,p);
568 pp[0] += 0.5;
569 pp[1] += 0.5;
570 return length(pp) * 300;
571 }
572
573 case 3: { // 1 body to static env with relative rotation
574 addOscillatingTorque (0.1);
575
576 // check the body offset is correct
577 dVector3 p;
578 const dReal *p1 = dBodyGetPosition (body[0]);
579 for (int i=0; i<3; i++) p[i] = p1[i];
580 p[0] -= 0.25;
581 p[1] -= 0.25;
582 p[2] -= 1;
583 return length (p) * 1e6;
584 }
585
586
587 // ********** hinge joint
588
589 case 200: // 2 body
590 addOscillatingTorque (0.1);
591 dampRotationalMotion (0.1);
592 return dInfinity;
593
594 case 220: // hinge angle polarity test
595 dBodyAddTorque (body[0],0,0,0.01);
596 dBodyAddTorque (body[1],0,0,-0.01);
597 if (iteration == 40) {
598 dReal a = dJointGetHingeAngle (joint);
599 if (a > 0.5 && a < 1) return 0; else return 10;
600 }
601 return 0;
602
603 case 221: { // hinge angle rate test
604 static dReal last_angle = 0;
605 dBodyAddTorque (body[0],0,0,0.01);
606 dBodyAddTorque (body[1],0,0,-0.01);
607 dReal a = dJointGetHingeAngle (joint);
608 dReal r = dJointGetHingeAngleRate (joint);
609 dReal er = (a-last_angle)/STEPSIZE; // estimated rate
610 last_angle = a;
611 return fabs(r-er) * 4e4;
612 }
613
614 case 230: // hinge motor rate (and polarity) test
615 case 231: { // ...with stops
616 static dReal a = 0;
617 dReal r = dJointGetHingeAngleRate (joint);
618 dReal err = fabs (cos(a) - r);
619 if (a==0) err = 0;
620 a += 0.03;
621 dJointSetHingeParam (joint,dParamVel,cos(a));
622 if (n==231) return dInfinity;
623 return err * 1e6;
624 }
625
626 // ********** slider joint
627
628 case 300: // 2 body
629 addOscillatingTorque (0.05);
630 dampRotationalMotion (0.1);
631 addSpringForce (0.5);
632 return dInfinity;
633
634 case 320: // slider angle polarity test
635 dBodyAddForce (body[0],0,0,0.1);
636 dBodyAddForce (body[1],0,0,-0.1);
637 if (iteration == 40) {
638 dReal a = dJointGetSliderPosition (joint);
639 if (a > 0.2 && a < 0.5) return 0; else return 10;
640 return a;
641 }
642 return 0;
643
644 case 321: { // slider angle rate test
645 static dReal last_pos = 0;
646 dBodyAddForce (body[0],0,0,0.1);
647 dBodyAddForce (body[1],0,0,-0.1);
648 dReal p = dJointGetSliderPosition (joint);
649 dReal r = dJointGetSliderPositionRate (joint);
650 dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact)
651 last_pos = p;
652 return fabs(r-er) * 1e9;
653 }
654
655 case 330: // slider motor rate (and polarity) test
656 case 331: { // ...with stops
657 static dReal a = 0;
658 dReal r = dJointGetSliderPositionRate (joint);
659 dReal err = fabs (0.7*cos(a) - r);
660 if (a < 0.04) err = 0;
661 a += 0.03;
662 dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
663 if (n==331) return dInfinity;
664 return err * 1e6;
665 }
666
667 // ********** hinge-2 joint
668
669 case 420: // hinge-2 steering angle polarity test
670 dBodyAddTorque (body[0],0,0,0.01);
671 dBodyAddTorque (body[1],0,0,-0.01);
672 if (iteration == 40) {
673 dReal a = dJointGetHinge2Angle1 (joint);
674 if (a > 0.5 && a < 0.6) return 0; else return 10;
675 }
676 return 0;
677
678 case 421: { // hinge-2 steering angle rate test
679 static dReal last_angle = 0;
680 dBodyAddTorque (body[0],0,0,0.01);
681 dBodyAddTorque (body[1],0,0,-0.01);
682 dReal a = dJointGetHinge2Angle1 (joint);
683 dReal r = dJointGetHinge2Angle1Rate (joint);
684 dReal er = (a-last_angle)/STEPSIZE; // estimated rate
685 last_angle = a;
686 return fabs(r-er)*2e4;
687 }
688
689 case 430: // hinge 2 steering motor rate (+polarity) test
690 case 431: { // ...with stops
691 static dReal a = 0;
692 dReal r = dJointGetHinge2Angle1Rate (joint);
693 dReal err = fabs (cos(a) - r);
694 if (a==0) err = 0;
695 a += 0.03;
696 dJointSetHinge2Param (joint,dParamVel,cos(a));
697 if (n==431) return dInfinity;
698 return err * 1e6;
699 }
700
701 case 432: { // hinge 2 wheel motor rate (+polarity) test
702 static dReal a = 0;
703 dReal r = dJointGetHinge2Angle2Rate (joint);
704 dReal err = fabs (cos(a) - r);
705 if (a==0) err = 0;
706 a += 0.03;
707 dJointSetHinge2Param (joint,dParamVel2,cos(a));
708 return err * 1e6;
709 }
710
711 // ********** angular motor joint
712
713 case 600: { // test euler angle calculations
714 // desired euler angles from last iteration
715 static dReal a1,a2,a3;
716
717 // find actual euler angles
718 dReal aa1 = dJointGetAMotorAngle (joint,0);
719 dReal aa2 = dJointGetAMotorAngle (joint,1);
720 dReal aa3 = dJointGetAMotorAngle (joint,2);
721 // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);
722
723 dReal err = dInfinity;
724 if (iteration > 0) {
725 err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
726 err *= 1e10;
727 }
728
729 // get random base rotation for both bodies
730 dMatrix3 Rbase;
731 dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
732 3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
733 dBodySetRotation (body[0],Rbase);
734
735 // rotate body 2 by random euler angles w.r.t. body 1
736 a1 = 3.14 * 2 * (dRandReal()-0.5);
737 a2 = 1.57 * 2 * (dRandReal()-0.5);
738 a3 = 3.14 * 2 * (dRandReal()-0.5);
739 dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
740 dRFromAxisAndAngle (R1,0,0,1,-a1);
741 dRFromAxisAndAngle (R2,0,1,0,a2);
742 dRFromAxisAndAngle (R3,1,0,0,-a3);
743 dMultiply0 (Rtmp1,R2,R3,3,3,3);
744 dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
745 dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
746 dBodySetRotation (body[1],Rtmp1);
747 // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
748
749 return err;
750 }
751
752 // ********** universal joint
753
754 case 700: { // 2 body: joint constraint
755 dVector3 ax1, ax2;
756
757 addOscillatingTorque (0.1);
758 dampRotationalMotion (0.1);
759 dJointGetUniversalAxis1(joint, ax1);
760 dJointGetUniversalAxis2(joint, ax2);
761 return fabs(10*dDOT(ax1, ax2));
762 }
763
764 case 701: { // 2 body: angle 1 rate
765 static dReal last_angle = 0;
766 addOscillatingTorque (0.1);
767 dampRotationalMotion (0.1);
768 dReal a = dJointGetUniversalAngle1(joint);
769 dReal r = dJointGetUniversalAngle1Rate(joint);
770 dReal diff = a - last_angle;
771 if (diff > M_PI) diff -= 2*M_PI;
772 if (diff < -M_PI) diff += 2*M_PI;
773 dReal er = diff / STEPSIZE; // estimated rate
774 last_angle = a;
775 // I'm not sure why the error is so large here.
776 return fabs(r - er) * 1e1;
777 }
778
779 case 702: { // 2 body: angle 2 rate
780 static dReal last_angle = 0;
781 addOscillatingTorque (0.1);
782 dampRotationalMotion (0.1);
783 dReal a = dJointGetUniversalAngle2(joint);
784 dReal r = dJointGetUniversalAngle2Rate(joint);
785 dReal diff = a - last_angle;
786 if (diff > M_PI) diff -= 2*M_PI;
787 if (diff < -M_PI) diff += 2*M_PI;
788 dReal er = diff / STEPSIZE; // estimated rate
789 last_angle = a;
790 // I'm not sure why the error is so large here.
791 return fabs(r - er) * 1e1;
792 }
793
794 case 720: { // universal transmit torque test: constraint error
795 dVector3 ax1, ax2;
796 addOscillatingTorqueAbout (0.1, 1, 1, 0);
797 dampRotationalMotion (0.1);
798 dJointGetUniversalAxis1(joint, ax1);
799 dJointGetUniversalAxis2(joint, ax2);
800 return fabs(10*dDOT(ax1, ax2));
801 }
802
803 case 721: { // universal transmit torque test: angle1 rate
804 static dReal last_angle = 0;
805 addOscillatingTorqueAbout (0.1, 1, 1, 0);
806 dampRotationalMotion (0.1);
807 dReal a = dJointGetUniversalAngle1(joint);
808 dReal r = dJointGetUniversalAngle1Rate(joint);
809 dReal diff = a - last_angle;
810 if (diff > M_PI) diff -= 2*M_PI;
811 if (diff < -M_PI) diff += 2*M_PI;
812 dReal er = diff / STEPSIZE; // estimated rate
813 last_angle = a;
814 return fabs(r - er) * 1e10;
815 }
816
817 case 722: { // universal transmit torque test: angle2 rate
818 static dReal last_angle = 0;
819 addOscillatingTorqueAbout (0.1, 1, 1, 0);
820 dampRotationalMotion (0.1);
821 dReal a = dJointGetUniversalAngle2(joint);
822 dReal r = dJointGetUniversalAngle2Rate(joint);
823 dReal diff = a - last_angle;
824 if (diff > M_PI) diff -= 2*M_PI;
825 if (diff < -M_PI) diff += 2*M_PI;
826 dReal er = diff / STEPSIZE; // estimated rate
827 last_angle = a;
828 return fabs(r - er) * 1e10;
829 }
830
831 case 730:{
832 dVector3 ax1, ax2;
833 dJointGetUniversalAxis1(joint, ax1);
834 dJointGetUniversalAxis2(joint, ax2);
835 addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
836 dampRotationalMotion (0.1);
837 return fabs(10*dDOT(ax1, ax2));
838 }
839
840 case 731:{
841 dVector3 ax1;
842 static dReal last_angle = 0;
843 dJointGetUniversalAxis1(joint, ax1);
844 addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
845 dampRotationalMotion (0.1);
846 dReal a = dJointGetUniversalAngle1(joint);
847 dReal r = dJointGetUniversalAngle1Rate(joint);
848 dReal diff = a - last_angle;
849 if (diff > M_PI) diff -= 2*M_PI;
850 if (diff < -M_PI) diff += 2*M_PI;
851 dReal er = diff / STEPSIZE; // estimated rate
852 last_angle = a;
853 return fabs(r - er) * 2e3;
854 }
855
856 case 732:{
857 dVector3 ax1;
858 static dReal last_angle = 0;
859 dJointGetUniversalAxis1(joint, ax1);
860 addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
861 dampRotationalMotion (0.1);
862 dReal a = dJointGetUniversalAngle2(joint);
863 dReal r = dJointGetUniversalAngle2Rate(joint);
864 dReal diff = a - last_angle;
865 if (diff > M_PI) diff -= 2*M_PI;
866 if (diff < -M_PI) diff += 2*M_PI;
867 dReal er = diff / STEPSIZE; // estimated rate
868 last_angle = a;
869 return fabs(r - er) * 1e10;
870 }
871
872 case 740:{
873 dVector3 ax1, ax2;
874 dJointGetUniversalAxis1(joint, ax1);
875 dJointGetUniversalAxis2(joint, ax2);
876 addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
877 dampRotationalMotion (0.1);
878 return fabs(10*dDOT(ax1, ax2));
879 }
880
881 case 741:{
882 dVector3 ax2;
883 static dReal last_angle = 0;
884 dJointGetUniversalAxis2(joint, ax2);
885 addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
886 dampRotationalMotion (0.1);
887 dReal a = dJointGetUniversalAngle1(joint);
888 dReal r = dJointGetUniversalAngle1Rate(joint);
889 dReal diff = a - last_angle;
890 if (diff > M_PI) diff -= 2*M_PI;
891 if (diff < -M_PI) diff += 2*M_PI;
892 dReal er = diff / STEPSIZE; // estimated rate
893 last_angle = a;
894 return fabs(r - er) * 1e10;
895 }
896
897 case 742:{
898 dVector3 ax2;
899 static dReal last_angle = 0;
900 dJointGetUniversalAxis2(joint, ax2);
901 addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
902 dampRotationalMotion (0.1);
903 dReal a = dJointGetUniversalAngle2(joint);
904 dReal r = dJointGetUniversalAngle2Rate(joint);
905 dReal diff = a - last_angle;
906 if (diff > M_PI) diff -= 2*M_PI;
907 if (diff < -M_PI) diff += 2*M_PI;
908 dReal er = diff / STEPSIZE; // estimated rate
909 last_angle = a;
910 return fabs(r - er) * 1e4;
911 }
912
913 // ********** slider joint
914 case 801:
915 case 803:
916 addSpringForce (0.25);
917 return dInfinity;
918
919 case 802:
920 case 804: {
921 static dReal a = 0;
922 dBodyAddTorque (body[0], 0, 0.01*cos(1.5708*a), 0);
923 a += 0.01;
924 return dInfinity;
925 }
926
927 case 805:
928 addOscillatingForce (0.1);
929 return dInfinity;
930 }
931
932
933 return dInfinity;
934}
935
936//****************************************************************************
937// simulation stuff common to all the tests
938
939// start simulation - set viewpoint
940
941static void start()
942{
943 static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
944 static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
945 dsSetViewpoint (xyz,hpr);
946}
947
948
949// simulation loop
950
951static void simLoop (int pause)
952{
953 // stop after a given number of iterations, as long as we are not in
954 // interactive mode
955 if (cmd_graphics && !cmd_interactive &&
956 (iteration >= max_iterations)) {
957 dsStop();
958 return;
959 }
960 iteration++;
961
962 if (!pause) {
963 // do stuff for this test and check to see if the joint is behaving well
964 dReal error = doStuffAndGetError (test_num);
965 if (error > max_error) max_error = error;
966 if (cmd_interactive && error < dInfinity) {
967 printf ("scaled error = %.4e\n",error);
968 }
969
970 // take a step
971 dWorldStep (world,STEPSIZE);
972
973 // occasionally re-orient the first body to create a deliberate error.
974 if (cmd_occasional_error) {
975 static int count = 0;
976 if ((count % 20)==0) {
977 // randomly adjust orientation of body[0]
978 const dReal *R1;
979 dMatrix3 R2,R3;
980 R1 = dBodyGetRotation (body[0]);
981 dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
982 dRandReal()-0.5,dRandReal()-0.5);
983 dMultiply0 (R3,R1,R2,3,3,3);
984 dBodySetRotation (body[0],R3);
985
986 // randomly adjust position of body[0]
987 const dReal *pos = dBodyGetPosition (body[0]);
988 dBodySetPosition (body[0],
989 pos[0]+0.2*(dRandReal()-0.5),
990 pos[1]+0.2*(dRandReal()-0.5),
991 pos[2]+0.2*(dRandReal()-0.5));
992 }
993 count++;
994 }
995 }
996
997 if (cmd_graphics) {
998 dReal sides1[3] = {SIDE,SIDE,SIDE};
999 dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
1000 dsSetTexture (DS_WOOD);
1001 dsSetColor (1,1,0);
1002 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
1003 if (body[1]) {
1004 dsSetColor (0,1,1);
1005 dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
1006 }
1007 }
1008}
1009
1010//****************************************************************************
1011// conduct a specific test, and report the results
1012
1013void doTest (int argc, char **argv, int n, int fatal_if_bad_n)
1014{
1015 test_num = n;
1016 iteration = 0;
1017 max_iterations = 300;
1018 max_error = 0;
1019
1020 if (! setupTest (n)) {
1021 if (fatal_if_bad_n) dError (0,"bad test number");
1022 return;
1023 }
1024
1025 // setup pointers to drawstuff callback functions
1026 dsFunctions fn;
1027 fn.version = DS_VERSION;
1028 fn.start = &start;
1029 fn.step = &simLoop;
1030 fn.command = 0;
1031 fn.stop = 0;
1032 if (cmd_path_to_textures)
1033 fn.path_to_textures = cmd_path_to_textures;
1034 else
1035 fn.path_to_textures = "../../drawstuff/textures";
1036
1037 // run simulation
1038 if (cmd_graphics) {
1039 dsSimulationLoop (argc,argv,352,288,&fn);
1040 }
1041 else {
1042 for (int i=0; i < max_iterations; i++) simLoop (0);
1043 }
1044 dWorldDestroy (world);
1045 body[0] = 0;
1046 body[1] = 0;
1047 joint = 0;
1048
1049 // print results
1050 printf ("test %d: ",n);
1051 if (max_error == dInfinity) printf ("error not computed\n");
1052 else {
1053 printf ("max scaled error = %.4e",max_error);
1054 if (max_error < 1) printf (" - passed\n");
1055 else printf (" - FAILED\n");
1056 }
1057}
1058
1059//****************************************************************************
1060// main
1061
1062int main (int argc, char **argv)
1063{
1064 int i;
1065 dInitODE();
1066
1067 // process the command line args. anything that starts with `-' is assumed
1068 // to be a drawstuff argument.
1069 for (i=1; i<argc; i++) {
1070 if ( argv[i][0]=='-' && argv[i][1]=='i' && argv[i][2]==0) cmd_interactive = 1;
1071 else if ( argv[i][0]=='-' && argv[i][1]=='g' && argv[i][2]==0) cmd_graphics = 0;
1072 else if ( argv[i][0]=='-' && argv[i][1]=='e' && argv[i][2]==0) cmd_graphics = 0;
1073 else if ( argv[i][0]=='-' && argv[i][1]=='n' && isdigit(argv[i][2]) ) {
1074 char *endptr;
1075 long int n = strtol (&(argv[i][2]),&endptr,10);
1076 if (*endptr == 0) cmd_test_num = n;
1077 }
1078 else
1079 cmd_path_to_textures = argv[i];
1080 }
1081
1082 // do the tests
1083 if (cmd_test_num == -1) {
1084 for (i=0; i<NUM_JOINTS*100; i++) doTest (argc,argv,i,0);
1085 }
1086 else {
1087 doTest (argc,argv,cmd_test_num,1);
1088 }
1089
1090 dCloseODE();
1091 return 0;
1092}
diff --git a/libraries/ode-0.9/ode/demo/demo_motor.cpp b/libraries/ode-0.9/ode/demo/demo_motor.cpp
new file mode 100644
index 0000000..655da0c
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_motor.cpp
@@ -0,0 +1,210 @@
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// select correct drawing functions
31#ifdef dDOUBLE
32#define dsDrawBox dsDrawBoxD
33#endif
34
35
36// some constants
37#define SIDE (0.5f) // side length of a box
38#define MASS (1.0) // mass of a box
39
40
41// dynamics and collision objects
42static dWorldID world;
43static dBodyID body[2];
44static dGeomID geom[2];
45static dJointID lmotor[2];
46static dJointID amotor[2];
47static dSpaceID space;
48static dJointGroupID contactgroup;
49
50
51// start simulation - set viewpoint
52
53static void start()
54{
55 static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
56 static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
57 dsSetViewpoint (xyz,hpr);
58 printf ("Press 'q,a,z' to control one axis of lmotor connectiong two bodies. (q is +,a is 0, z is -)\n");
59 printf ("Press 'w,e,r' to control one axis of lmotor connectiong first body with world. (w is +,e is 0, r is -)\n");
60}
61
62
63// called when a key pressed
64
65static void command (int cmd)
66{
67 if (cmd == 'q' || cmd == 'Q') {
68 dJointSetLMotorParam(lmotor[0],dParamVel,0);
69 dJointSetLMotorParam(lmotor[0],dParamVel2,0);
70 dJointSetLMotorParam(lmotor[0],dParamVel3,0.1);
71 } else if (cmd == 'a' || cmd == 'A') {
72 dJointSetLMotorParam(lmotor[0],dParamVel,0);
73 dJointSetLMotorParam(lmotor[0],dParamVel2,0);
74 dJointSetLMotorParam(lmotor[0],dParamVel3,0);
75 } else if (cmd == 'z' || cmd == 'Z') {
76 dJointSetLMotorParam(lmotor[0],dParamVel,0);
77 dJointSetLMotorParam(lmotor[0],dParamVel2,0);
78 dJointSetLMotorParam(lmotor[0],dParamVel3,-0.1);
79 } else if (cmd == 'w' || cmd == 'W') {
80 dJointSetLMotorParam(lmotor[1],dParamVel,0.1);
81 dJointSetLMotorParam(lmotor[1],dParamVel2,0);
82 dJointSetLMotorParam(lmotor[1],dParamVel3,0);
83 } else if (cmd == 'e' || cmd == 'E') {
84 dJointSetLMotorParam(lmotor[1],dParamVel,0);
85 dJointSetLMotorParam(lmotor[1],dParamVel2,0);
86 dJointSetLMotorParam(lmotor[1],dParamVel3,0);
87 } else if (cmd == 'r' || cmd == 'R') {
88 dJointSetLMotorParam(lmotor[1],dParamVel,-0.1);
89 dJointSetLMotorParam(lmotor[1],dParamVel2,0);
90 dJointSetLMotorParam(lmotor[1],dParamVel3,0);
91 }
92
93}
94
95
96
97static void nearCallback (void *data, dGeomID o1, dGeomID o2)
98{
99 // exit without doing anything if the two bodies are connected by a joint
100 dBodyID b1 = dGeomGetBody(o1);
101 dBodyID b2 = dGeomGetBody(o2);
102
103 dContact contact;
104 contact.surface.mode = 0;
105 contact.surface.mu = dInfinity;
106 if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
107 dJointID c = dJointCreateContact (world,contactgroup,&contact);
108 dJointAttach (c,b1,b2);
109 }
110}
111
112// simulation loop
113
114static void simLoop (int pause)
115{
116 if (!pause) {
117 dSpaceCollide(space,0,&nearCallback);
118 dWorldQuickStep (world,0.05);
119 dJointGroupEmpty(contactgroup);
120 }
121
122 dReal sides1[3];
123 dGeomBoxGetLengths(geom[0], sides1);
124 dReal sides2[3];
125 dGeomBoxGetLengths(geom[1], sides2);
126 dsSetTexture (DS_WOOD);
127 dsSetColor (1,1,0);
128 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
129 dsSetColor (0,1,1);
130 dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
131}
132
133
134int main (int argc, char **argv)
135{
136 // setup pointers to drawstuff callback functions
137 dsFunctions fn;
138 fn.version = DS_VERSION;
139 fn.start = &start;
140 fn.step = &simLoop;
141 fn.command = &command;
142 fn.stop = 0;
143 fn.path_to_textures = "../../drawstuff/textures";
144 if(argc>=2)
145 {
146 fn.path_to_textures = argv[1];
147 }
148
149 // create world
150 dInitODE();
151 contactgroup = dJointGroupCreate(0);
152 world = dWorldCreate();
153 space = dSimpleSpaceCreate(0);
154 dMass m;
155 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
156 dMassAdjust (&m,MASS);
157
158 body[0] = dBodyCreate (world);
159 dBodySetMass (body[0],&m);
160 dBodySetPosition (body[0],0,0,1);
161 geom[0] = dCreateBox(space,SIDE,SIDE,SIDE);
162 body[1] = dBodyCreate (world);
163 dBodySetMass (body[1],&m);
164 dBodySetPosition (body[1],0,0,2);
165 geom[1] = dCreateBox(space,SIDE,SIDE,SIDE);
166
167 dGeomSetBody(geom[0],body[0]);
168 dGeomSetBody(geom[1],body[1]);
169
170 lmotor[0] = dJointCreateLMotor (world,0);
171 dJointAttach (lmotor[0],body[0],body[1]);
172 lmotor[1] = dJointCreateLMotor (world,0);
173 dJointAttach (lmotor[1],body[0],0);
174 amotor[0] = dJointCreateAMotor(world,0);
175 dJointAttach(amotor[0], body[0],body[1]);
176 amotor[1] = dJointCreateAMotor(world,0);
177 dJointAttach(amotor[1], body[0], 0);
178
179 for (int i=0; i<2; i++) {
180 dJointSetAMotorNumAxes(amotor[i], 3);
181 dJointSetAMotorAxis(amotor[i],0,1,1,0,0);
182 dJointSetAMotorAxis(amotor[i],1,1,0,1,0);
183 dJointSetAMotorAxis(amotor[i],2,1,0,0,1);
184 dJointSetAMotorParam(amotor[i],dParamFMax,0.00001);
185 dJointSetAMotorParam(amotor[i],dParamFMax2,0.00001);
186 dJointSetAMotorParam(amotor[i],dParamFMax3,0.00001);
187
188 dJointSetAMotorParam(amotor[i],dParamVel,0);
189 dJointSetAMotorParam(amotor[i],dParamVel2,0);
190 dJointSetAMotorParam(amotor[i],dParamVel3,0);
191
192 dJointSetLMotorNumAxes(lmotor[i],3);
193 dJointSetLMotorAxis(lmotor[i],0,1,1,0,0);
194 dJointSetLMotorAxis(lmotor[i],1,1,0,1,0);
195 dJointSetLMotorAxis(lmotor[i],2,1,0,0,1);
196
197 dJointSetLMotorParam(lmotor[i],dParamFMax,0.0001);
198 dJointSetLMotorParam(lmotor[i],dParamFMax2,0.0001);
199 dJointSetLMotorParam(lmotor[i],dParamFMax3,0.0001);
200 }
201
202 // run simulation
203 dsSimulationLoop (argc,argv,352,288,&fn);
204
205 dJointGroupDestroy(contactgroup);
206 dSpaceDestroy (space);
207 dWorldDestroy (world);
208 dCloseODE();
209 return 0;
210}
diff --git a/libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp b/libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp
new file mode 100644
index 0000000..3d03020
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp
@@ -0,0 +1,1944 @@
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}
diff --git a/libraries/ode-0.9/ode/demo/demo_ode.cpp b/libraries/ode-0.9/ode/demo/demo_ode.cpp
new file mode 100644
index 0000000..bd22ac1
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_ode.cpp
@@ -0,0 +1,1128 @@
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 <setjmp.h>
24#include <ode/ode.h>
25
26#ifdef _MSC_VER
27#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28#endif
29
30//****************************************************************************
31// matrix accessors
32
33#define _A(i,j) A[(i)*4+(j)]
34#define _I(i,j) I[(i)*4+(j)]
35#define _R(i,j) R[(i)*4+(j)]
36
37//****************************************************************************
38// tolerances
39
40#ifdef dDOUBLE
41const double tol = 1e-10;
42#endif
43
44#ifdef dSINGLE
45const double tol = 1e-5;
46#endif
47
48//****************************************************************************
49// misc messages and error handling
50
51#ifdef __GNUC__
52#define HEADER printf ("%s()\n", __FUNCTION__);
53#else
54#define HEADER printf ("%s:%d\n",__FILE__,__LINE__);
55#endif
56
57static jmp_buf jump_buffer;
58
59
60void myMessageFunction (int num, const char *msg, va_list ap)
61{
62 printf ("(Message %d: ",num);
63 vprintf (msg,ap);
64 printf (")");
65 dSetMessageHandler (0);
66 longjmp (jump_buffer,1);
67}
68
69
70#define TRAP_MESSAGE(do,ifnomsg,ifmsg) \
71 dSetMessageHandler (&myMessageFunction); \
72 if (setjmp (jump_buffer)) { \
73 dSetMessageHandler (0); \
74 ifmsg ; \
75 } \
76 else { \
77 dSetMessageHandler (&myMessageFunction); \
78 do ; \
79 ifnomsg ; \
80 } \
81 dSetMessageHandler (0);
82
83//****************************************************************************
84// utility stuff
85
86// compare two numbers, within a threshhold, return 1 if approx equal
87
88int cmp (dReal a, dReal b)
89{
90 return (fabs(a-b) < tol);
91}
92
93//****************************************************************************
94// matrix utility stuff
95
96// compare a 3x3 matrix with the identity
97
98int cmpIdentityMat3 (dMatrix3 A)
99{
100 return
101 (cmp(_A(0,0),1.0) && cmp(_A(0,1),0.0) && cmp(_A(0,2),0.0) &&
102 cmp(_A(1,0),0.0) && cmp(_A(1,1),1.0) && cmp(_A(1,2),0.0) &&
103 cmp(_A(2,0),0.0) && cmp(_A(2,1),0.0) && cmp(_A(2,2),1.0));
104}
105
106
107// transpose a 3x3 matrix in-line
108
109void transpose3x3 (dMatrix3 A)
110{
111 dReal tmp;
112 tmp=A[4]; A[4]=A[1]; A[1]=tmp;
113 tmp=A[8]; A[8]=A[2]; A[2]=tmp;
114 tmp=A[9]; A[9]=A[6]; A[6]=tmp;
115}
116
117//****************************************************************************
118// test miscellaneous math functions
119
120void testRandomNumberGenerator()
121{
122 HEADER;
123 if (dTestRand()) printf ("\tpassed\n");
124 else printf ("\tFAILED\n");
125}
126
127
128void testInfinity()
129{
130 HEADER;
131 if (1e10 < dInfinity && -1e10 > -dInfinity && -dInfinity < dInfinity)
132 printf ("\tpassed\n");
133 else printf ("\tFAILED\n");
134}
135
136
137void testPad()
138{
139 HEADER;
140 char s[100];
141 s[0]=0;
142 for (int i=0; i<=16; i++) sprintf (s+strlen(s),"%d ",dPAD(i));
143 printf ("\t%s\n", strcmp(s,"0 1 4 4 4 8 8 8 8 12 12 12 12 16 16 16 16 ") ?
144 "FAILED" : "passed");
145}
146
147
148void testCrossProduct()
149{
150 HEADER;
151
152 dVector3 a1,a2,b,c;
153 dMatrix3 B;
154 dMakeRandomVector (b,3,1.0);
155 dMakeRandomVector (c,3,1.0);
156
157 dCROSS (a1,=,b,c);
158
159 dSetZero (B,12);
160 dCROSSMAT (B,b,4,+,-);
161 dMultiply0 (a2,B,c,3,3,1);
162
163 dReal diff = dMaxDifference(a1,a2,3,1);
164 printf ("\t%s\n", diff > tol ? "FAILED" : "passed");
165}
166
167
168void testSetZero()
169{
170 HEADER;
171 dReal a[100];
172 dMakeRandomVector (a,100,1.0);
173 dSetZero (a,100);
174 for (int i=0; i<100; i++) if (a[i] != 0.0) {
175 printf ("\tFAILED\n");
176 return;
177 }
178 printf ("\tpassed\n");
179}
180
181
182void testNormalize3()
183{
184 HEADER;
185 int i,j,bad=0;
186 dVector3 n1,n2;
187 for (i=0; i<1000; i++) {
188 dMakeRandomVector (n1,3,1.0);
189 for (j=0; j<3; j++) n2[j]=n1[j];
190 dNormalize3 (n2);
191 if (dFabs(dDOT(n2,n2) - 1.0) > tol) bad |= 1;
192 if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
193 if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
194 if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
195 if (dFabs(dDOT(n2,n1) - dSqrt(dDOT(n1,n1))) > tol) bad |= 16;
196 if (bad) {
197 printf ("\tFAILED (code=%x)\n",bad);
198 return;
199 }
200 }
201 printf ("\tpassed\n");
202}
203
204
205/*
206void testReorthonormalize()
207{
208 HEADER;
209 dMatrix3 R,I;
210 dMakeRandomMatrix (R,3,3,1.0);
211 for (int i=0; i<30; i++) dReorthonormalize (R);
212 dMultiply2 (I,R,R,3,3,3);
213 printf ("\t%s\n",cmpIdentityMat3 (I) ? "passed" : "FAILED");
214}
215*/
216
217
218void testPlaneSpace()
219{
220 HEADER;
221 dVector3 n,p,q;
222 int bad = 0;
223 for (int i=0; i<1000; i++) {
224 dMakeRandomVector (n,3,1.0);
225 dNormalize3 (n);
226 dPlaneSpace (n,p,q);
227 if (fabs(dDOT(n,p)) > tol) bad = 1;
228 if (fabs(dDOT(n,q)) > tol) bad = 1;
229 if (fabs(dDOT(p,q)) > tol) bad = 1;
230 if (fabs(dDOT(p,p)-1) > tol) bad = 1;
231 if (fabs(dDOT(q,q)-1) > tol) bad = 1;
232 }
233 printf ("\t%s\n", bad ? "FAILED" : "passed");
234}
235
236//****************************************************************************
237// test matrix functions
238
239#define MSIZE 21
240#define MSIZE4 24 // MSIZE rounded up to 4
241
242
243void testMatrixMultiply()
244{
245 // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
246 dReal A[8],B[12],A2[12],B2[16],C[8];
247 int i;
248
249 HEADER;
250 dSetZero (A,8);
251 for (i=0; i<3; i++) A[i] = i+2;
252 for (i=0; i<3; i++) A[i+4] = i+3+2;
253 for (i=0; i<12; i++) B[i] = i+8;
254 dSetZero (A2,12);
255 for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
256 dSetZero (B2,16);
257 for (i=0; i<12; i++) B2[i+i/3] = B[i];
258
259 dMultiply0 (C,A,B,2,3,4);
260 if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
261 C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
262 printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");
263
264 dMultiply1 (C,A2,B,2,3,4);
265 if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
266 C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
267 printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");
268
269 dMultiply2 (C,A,B2,2,3,4);
270 if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
271 C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
272 printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
273}
274
275
276void testSmallMatrixMultiply()
277{
278 dMatrix3 A,B,C,A2;
279 dVector3 a,a2,x;
280
281 HEADER;
282 dMakeRandomMatrix (A,3,3,1.0);
283 dMakeRandomMatrix (B,3,3,1.0);
284 dMakeRandomMatrix (C,3,3,1.0);
285 dMakeRandomMatrix (x,3,1,1.0);
286
287 // dMULTIPLY0_331()
288 dMULTIPLY0_331 (a,B,x);
289 dMultiply0 (a2,B,x,3,3,1);
290 printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
291 "passed");
292
293 // dMULTIPLY1_331()
294 dMULTIPLY1_331 (a,B,x);
295 dMultiply1 (a2,B,x,3,3,1);
296 printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
297 "passed");
298
299 // dMULTIPLY0_133
300 dMULTIPLY0_133 (a,x,B);
301 dMultiply0 (a2,x,B,1,3,3);
302 printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
303 "passed");
304
305 // dMULTIPLY0_333()
306 dMULTIPLY0_333 (A,B,C);
307 dMultiply0 (A2,B,C,3,3,3);
308 printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
309 "passed");
310
311 // dMULTIPLY1_333()
312 dMULTIPLY1_333 (A,B,C);
313 dMultiply1 (A2,B,C,3,3,3);
314 printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
315 "passed");
316
317 // dMULTIPLY2_333()
318 dMULTIPLY2_333 (A,B,C);
319 dMultiply2 (A2,B,C,3,3,3);
320 printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
321 "passed");
322}
323
324
325void testCholeskyFactorization()
326{
327 dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE], C[MSIZE4*MSIZE], diff;
328 HEADER;
329 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
330 dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
331 memcpy (A,B,MSIZE4*MSIZE*sizeof(dReal));
332 if (dFactorCholesky (B,MSIZE)) printf ("\tpassed (1)\n");
333 else printf ("\tFAILED (1)\n");
334 dClearUpperTriangle (B,MSIZE);
335 dMultiply2 (C,B,B,MSIZE,MSIZE,MSIZE);
336 diff = dMaxDifference(A,C,MSIZE,MSIZE);
337 printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
338 diff > tol ? "FAILED" : "passed");
339}
340
341
342void testCholeskySolve()
343{
344 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], b[MSIZE],x[MSIZE],btest[MSIZE],diff;
345 HEADER;
346
347 // get A,L = PD matrix
348 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
349 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
350 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
351
352 // get b,x = right hand side
353 dMakeRandomMatrix (b,MSIZE,1,1.0);
354 memcpy (x,b,MSIZE*sizeof(dReal));
355
356 // factor L
357 if (dFactorCholesky (L,MSIZE)) printf ("\tpassed (1)\n");
358 else printf ("\tFAILED (1)\n");
359 dClearUpperTriangle (L,MSIZE);
360
361 // solve A*x = b
362 dSolveCholesky (L,x,MSIZE);
363
364 // compute A*x and compare it with b
365 dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
366 diff = dMaxDifference(b,btest,MSIZE,1);
367 printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
368 diff > tol ? "FAILED" : "passed");
369}
370
371
372void testInvertPDMatrix()
373{
374 int i,j,ok;
375 dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE];
376 HEADER;
377
378 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
379 dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
380 memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
381 dSetZero (Ainv,MSIZE4*MSIZE);
382
383 if (dInvertPDMatrix (A,Ainv,MSIZE))
384 printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
385 dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);
386
387 // compare with identity
388 ok = 1;
389 for (i=0; i<MSIZE; i++) {
390 for (j=0; j<MSIZE; j++) {
391 if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
392 }
393 }
394 for (i=0; i<MSIZE; i++) {
395 if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
396 }
397 if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");
398}
399
400
401void testIsPositiveDefinite()
402{
403 dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE];
404 HEADER;
405 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
406 dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
407 printf ("\t%s\n",dIsPositiveDefinite(A,MSIZE) ? "FAILED (1)":"passed (1)");
408 printf ("\t%s\n",dIsPositiveDefinite(B,MSIZE) ? "passed (2)":"FAILED (2)");
409}
410
411
412void testFastLDLTFactorization()
413{
414 int i,j;
415 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], DL[MSIZE4*MSIZE],
416 ATEST[MSIZE4*MSIZE], d[MSIZE], diff;
417 HEADER;
418 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
419 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
420 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
421
422 dFactorLDLT (L,d,MSIZE,MSIZE4);
423 dClearUpperTriangle (L,MSIZE);
424 for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
425
426 dSetZero (DL,MSIZE4*MSIZE);
427 for (i=0; i<MSIZE; i++) {
428 for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
429 }
430
431 dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
432 diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
433 printf ("\tmaximum difference = %.6e - %s\n",diff,
434 diff > tol ? "FAILED" : "passed");
435}
436
437
438void testSolveLDLT()
439{
440 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], x[MSIZE],
441 b[MSIZE], btest[MSIZE], diff;
442 HEADER;
443 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
444 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
445 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
446
447 dMakeRandomMatrix (b,MSIZE,1,1.0);
448 memcpy (x,b,MSIZE*sizeof(dReal));
449
450 dFactorLDLT (L,d,MSIZE,MSIZE4);
451 dSolveLDLT (L,d,x,MSIZE,MSIZE4);
452
453 dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
454 diff = dMaxDifference(b,btest,MSIZE,1);
455 printf ("\tmaximum difference = %.6e - %s\n",diff,
456 diff > tol ? "FAILED" : "passed");
457}
458
459
460void testLDLTAddTL()
461{
462 int i,j;
463 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], a[MSIZE],
464 DL[MSIZE4*MSIZE], ATEST[MSIZE4*MSIZE], diff;
465 HEADER;
466
467 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
468 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
469 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
470 dFactorLDLT (L,d,MSIZE,MSIZE4);
471
472 // delete first row and column of factorization
473 for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4];
474 a[0] += 1;
475 dLDLTAddTL (L,d,a,MSIZE,MSIZE4);
476 for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0;
477 d[0] = 1;
478
479 // get modified L*D*L'
480 dClearUpperTriangle (L,MSIZE);
481 for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
482 dSetZero (DL,MSIZE4*MSIZE);
483 for (i=0; i<MSIZE; i++) {
484 for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
485 }
486 dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
487
488 // compare it to A with its first row/column removed
489 for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0;
490 A[0] = 1;
491 diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
492 printf ("\tmaximum difference = %.6e - %s\n",diff,
493 diff > tol ? "FAILED" : "passed");
494}
495
496
497void testLDLTRemove()
498{
499 int i,j,r,p[MSIZE];
500 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE],
501 L2[MSIZE4*MSIZE], d2[MSIZE], DL2[MSIZE4*MSIZE],
502 Atest1[MSIZE4*MSIZE], Atest2[MSIZE4*MSIZE], diff, maxdiff;
503 HEADER;
504
505 // make array of A row pointers
506 dReal *Arows[MSIZE];
507 for (i=0; i<MSIZE; i++) Arows[i] = A+i*MSIZE4;
508
509 // fill permutation vector
510 for (i=0; i<MSIZE; i++) p[i]=i;
511
512 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
513 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
514 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
515 dFactorLDLT (L,d,MSIZE,MSIZE4);
516
517 maxdiff = 1e10;
518 for (r=0; r<MSIZE; r++) {
519 // get Atest1 = A with row/column r removed
520 memcpy (Atest1,A,MSIZE4*MSIZE*sizeof(dReal));
521 dRemoveRowCol (Atest1,MSIZE,MSIZE4,r);
522
523 // test that the row/column removal worked
524 int bad = 0;
525 for (i=0; i<MSIZE; i++) {
526 for (j=0; j<MSIZE; j++) {
527 if (i != r && j != r) {
528 int ii = i;
529 int jj = j;
530 if (ii >= r) ii--;
531 if (jj >= r) jj--;
532 if (A[i*MSIZE4+j] != Atest1[ii*MSIZE4+jj]) bad = 1;
533 }
534 }
535 }
536 if (bad) printf ("\trow/col removal FAILED for row %d\n",r);
537
538 // zero out last row/column of Atest1
539 for (i=0; i<MSIZE; i++) {
540 Atest1[(MSIZE-1)*MSIZE4+i] = 0;
541 Atest1[i*MSIZE4+MSIZE-1] = 0;
542 }
543
544 // get L2*D2*L2' = adjusted factorization to remove that row
545 memcpy (L2,L,MSIZE4*MSIZE*sizeof(dReal));
546 memcpy (d2,d,MSIZE*sizeof(dReal));
547 dLDLTRemove (/*A*/ Arows,p,L2,d2,MSIZE,MSIZE,r,MSIZE4);
548
549 // get Atest2 = L2*D2*L2'
550 dClearUpperTriangle (L2,MSIZE);
551 for (i=0; i<(MSIZE-1); i++) L2[i*MSIZE4+i] = 1.0;
552 for (i=0; i<MSIZE; i++) L2[(MSIZE-1)*MSIZE4+i] = 0;
553 d2[MSIZE-1] = 1;
554 dSetZero (DL2,MSIZE4*MSIZE);
555 for (i=0; i<(MSIZE-1); i++) {
556 for (j=0; j<MSIZE-1; j++) DL2[i*MSIZE4+j] = L2[i*MSIZE4+j] / d2[j];
557 }
558
559 dMultiply2 (Atest2,L2,DL2,MSIZE,MSIZE,MSIZE);
560
561 diff = dMaxDifference(Atest1,Atest2,MSIZE,MSIZE);
562 if (diff < maxdiff) maxdiff = diff;
563
564 /*
565 dPrintMatrix (Atest1,MSIZE,MSIZE);
566 printf ("\n");
567 dPrintMatrix (Atest2,MSIZE,MSIZE);
568 printf ("\n");
569 */
570 }
571 printf ("\tmaximum difference = %.6e - %s\n",maxdiff,
572 maxdiff > tol ? "FAILED" : "passed");
573}
574
575//****************************************************************************
576// test mass stuff
577
578#define NUMP 10 // number of particles
579
580
581void printMassParams (dMass *m)
582{
583 printf ("mass = %.4f\n",m->mass);
584 printf ("com = (%.4f,%.4f,%.4f)\n",m->c[0],m->c[1],m->c[2]);
585 printf ("I = [ %10.4f %10.4f %10.4f ]\n"
586 " [ %10.4f %10.4f %10.4f ]\n"
587 " [ %10.4f %10.4f %10.4f ]\n",
588 m->_I(0,0),m->_I(0,1),m->_I(0,2),
589 m->_I(1,0),m->_I(1,1),m->_I(1,2),
590 m->_I(2,0),m->_I(2,1),m->_I(2,2));
591}
592
593
594void compareMassParams (dMass *m1, dMass *m2, char *msg)
595{
596 int i,j,ok = 1;
597 if (!(cmp(m1->mass,m2->mass) && cmp(m1->c[0],m2->c[0]) &&
598 cmp(m1->c[1],m2->c[1]) && cmp(m1->c[2],m2->c[2])))
599 ok = 0;
600 for (i=0; i<3; i++) for (j=0; j<3; j++)
601 if (cmp (m1->_I(i,j),m2->_I(i,j))==0) ok = 0;
602 if (ok) printf ("\tpassed (%s)\n",msg); else printf ("\tFAILED (%s)\n",msg);
603}
604
605
606// compute the mass parameters of a particle set
607
608void computeMassParams (dMass *m, dReal q[NUMP][3], dReal pm[NUMP])
609{
610 int i,j;
611 dMassSetZero (m);
612 for (i=0; i<NUMP; i++) {
613 m->mass += pm[i];
614 for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
615 m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
616 m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
617 m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
618 m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
619 m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
620 m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
621 }
622 for (j=0; j<3; j++) m->c[j] /= m->mass;
623 m->_I(1,0) = m->_I(0,1);
624 m->_I(2,0) = m->_I(0,2);
625 m->_I(2,1) = m->_I(1,2);
626}
627
628
629void testMassFunctions()
630{
631 dMass m;
632 int i,j;
633 dReal q[NUMP][3]; // particle positions
634 dReal pm[NUMP]; // particle masses
635 dMass m1,m2;
636 dMatrix3 R;
637
638 HEADER;
639
640 printf ("\t");
641 dMassSetZero (&m);
642 TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
643 printf (" FAILED (1)\n"), printf (" passed (1)\n"));
644
645 printf ("\t");
646 dMassSetZero (&m);
647 TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
648 printf (" passed (2)\n") , printf (" FAILED (2)\n"));
649 if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
650 m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
651 m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
652 m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
653 printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");
654
655 dMassSetZero (&m);
656 dMassSetSphere (&m,1.4, 0.86);
657 if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
658 cmp(m._I(0,0),1.10349124669826) &&
659 cmp(m._I(1,1),1.10349124669826) &&
660 cmp(m._I(2,2),1.10349124669826) &&
661 m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
662 m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
663 printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");
664
665 dMassSetZero (&m);
666 dMassSetCapsule (&m,1.3,1,0.76,1.53);
667 if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
668 cmp(m._I(0,0),1.59461986077384) &&
669 cmp(m._I(1,1),4.57537403079093) &&
670 cmp(m._I(2,2),4.57537403079093) &&
671 m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
672 m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
673 printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");
674
675 dMassSetZero (&m);
676 dMassSetBox (&m,0.27,3,4,5);
677 if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
678 cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
679 m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
680 m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
681 printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");
682
683 // test dMassAdjust?
684
685 // make random particles and compute the mass, COM and inertia, then
686 // translate and repeat.
687 for (i=0; i<NUMP; i++) {
688 pm[i] = dRandReal()+0.5;
689 for (j=0; j<3; j++) {
690 q[i][j] = 2.0*(dRandReal()-0.5);
691 }
692 }
693 computeMassParams (&m1,q,pm);
694 memcpy (&m2,&m1,sizeof(dMass));
695 dMassTranslate (&m2,1,2,-3);
696 for (i=0; i<NUMP; i++) {
697 q[i][0] += 1;
698 q[i][1] += 2;
699 q[i][2] -= 3;
700 }
701 computeMassParams (&m1,q,pm);
702 compareMassParams (&m1,&m2,"7");
703
704 // rotate the masses
705 _R(0,0) = -0.87919618797635;
706 _R(0,1) = 0.15278881840384;
707 _R(0,2) = -0.45129772879842;
708 _R(1,0) = -0.47307856232664;
709 _R(1,1) = -0.39258064912909;
710 _R(1,2) = 0.78871864932708;
711 _R(2,0) = -0.05666336483842;
712 _R(2,1) = 0.90693771059546;
713 _R(2,2) = 0.41743652473765;
714 dMassRotate (&m2,R);
715 for (i=0; i<NUMP; i++) {
716 dReal a[3];
717 dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
718 q[i][0] = a[0];
719 q[i][1] = a[1];
720 q[i][2] = a[2];
721 }
722 computeMassParams (&m1,q,pm);
723 compareMassParams (&m1,&m2,"8");
724}
725
726//****************************************************************************
727// test rotation stuff
728
729void makeRandomRotation (dMatrix3 R)
730{
731 dReal *u1 = R, *u2=R+4, *u3=R+8;
732 dMakeRandomVector (u1,3,1.0);
733 dNormalize3 (u1);
734 dMakeRandomVector (u2,3,1.0);
735 dReal d = dDOT(u1,u2);
736 u2[0] -= d*u1[0];
737 u2[1] -= d*u1[1];
738 u2[2] -= d*u1[2];
739 dNormalize3 (u2);
740 dCROSS (u3,=,u1,u2);
741}
742
743
744void testRtoQandQtoR()
745{
746 HEADER;
747 dMatrix3 R,I,R2;
748 dQuaternion q;
749 int i;
750
751 // test makeRandomRotation()
752 makeRandomRotation (R);
753 dMultiply2 (I,R,R,3,3,3);
754 printf ("\tmakeRandomRotation() - %s (1)\n",
755 cmpIdentityMat3(I) ? "passed" : "FAILED");
756
757 // test QtoR() on random normalized quaternions
758 int ok = 1;
759 for (i=0; i<100; i++) {
760 dMakeRandomVector (q,4,1.0);
761 dNormalize4 (q);
762 dQtoR (q,R);
763 dMultiply2 (I,R,R,3,3,3);
764 if (cmpIdentityMat3(I)==0) ok = 0;
765 }
766 printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");
767
768 // test R -> Q -> R works
769 dReal maxdiff=0;
770 for (i=0; i<100; i++) {
771 makeRandomRotation (R);
772 dRtoQ (R,q);
773 dQtoR (q,R2);
774 dReal diff = dMaxDifference (R,R2,3,3);
775 if (diff > maxdiff) maxdiff = diff;
776 }
777 printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
778 (maxdiff > tol) ? "FAILED" : "passed");
779}
780
781
782void testQuaternionMultiply()
783{
784 HEADER;
785 dMatrix3 RA,RB,RC,Rtest;
786 dQuaternion qa,qb,qc;
787 dReal diff,maxdiff=0;
788
789 for (int i=0; i<100; i++) {
790 makeRandomRotation (RB);
791 makeRandomRotation (RC);
792 dRtoQ (RB,qb);
793 dRtoQ (RC,qc);
794
795 dMultiply0 (RA,RB,RC,3,3,3);
796 dQMultiply0 (qa,qb,qc);
797 dQtoR (qa,Rtest);
798 diff = dMaxDifference (Rtest,RA,3,3);
799 if (diff > maxdiff) maxdiff = diff;
800
801 dMultiply1 (RA,RB,RC,3,3,3);
802 dQMultiply1 (qa,qb,qc);
803 dQtoR (qa,Rtest);
804 diff = dMaxDifference (Rtest,RA,3,3);
805 if (diff > maxdiff) maxdiff = diff;
806
807 dMultiply2 (RA,RB,RC,3,3,3);
808 dQMultiply2 (qa,qb,qc);
809 dQtoR (qa,Rtest);
810 diff = dMaxDifference (Rtest,RA,3,3);
811 if (diff > maxdiff) maxdiff = diff;
812
813 dMultiply0 (RA,RC,RB,3,3,3);
814 transpose3x3 (RA);
815 dQMultiply3 (qa,qb,qc);
816 dQtoR (qa,Rtest);
817 diff = dMaxDifference (Rtest,RA,3,3);
818 if (diff > maxdiff) maxdiff = diff;
819 }
820 printf ("\tmaximum difference = %e - %s\n",maxdiff,
821 (maxdiff > tol) ? "FAILED" : "passed");
822}
823
824
825void testRotationFunctions()
826{
827 dMatrix3 R1;
828 HEADER;
829
830 printf ("\tdRSetIdentity - ");
831 dMakeRandomMatrix (R1,3,3,1.0);
832 dRSetIdentity (R1);
833 if (cmpIdentityMat3(R1)) printf ("passed\n"); else printf ("FAILED\n");
834
835 printf ("\tdRFromAxisAndAngle - ");
836
837 printf ("\n");
838 printf ("\tdRFromEulerAngles - ");
839
840 printf ("\n");
841 printf ("\tdRFrom2Axes - ");
842
843 printf ("\n");
844}
845
846//****************************************************************************
847
848#include "../src/array.h"
849#include "../src/array.cpp"
850
851// matrix header on the stack
852
853class dMatrixComparison {
854 struct dMatInfo;
855 dArray<dMatInfo*> mat;
856 int afterfirst,index;
857
858public:
859 dMatrixComparison();
860 ~dMatrixComparison();
861
862 dReal nextMatrix (dReal *A, int n, int m, int lower_tri, char *name, ...);
863 // add a new n*m matrix A to the sequence. the name of the matrix is given
864 // by the printf-style arguments (name,...). if this is the first sequence
865 // then this object will simply record the matrices and return 0.
866 // if this the second or subsequent sequence then this object will compare
867 // the matrices with the first sequence, and report any differences.
868 // the matrix error will be returned. if `lower_tri' is 1 then only the
869 // lower triangle of the matrix (including the diagonal) will be compared
870 // (the matrix must be square).
871
872 void end();
873 // end a sequence.
874
875 void reset();
876 // restarts the object, so the next sequence will be the first sequence.
877
878 void dump();
879 // print out info about all the matrices in the sequence
880};
881
882struct dMatrixComparison::dMatInfo {
883 int n,m; // size of matrix
884 char name[128]; // name of the matrix
885 dReal *data; // matrix data
886 int size; // size of `data'
887};
888
889
890
891dMatrixComparison::dMatrixComparison()
892{
893 afterfirst = 0;
894 index = 0;
895}
896
897
898dMatrixComparison::~dMatrixComparison()
899{
900 reset();
901}
902
903
904dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
905 char *name, ...)
906{
907 if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
908 int num = n*dPAD(m);
909
910 if (afterfirst==0) {
911 dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
912 mi->n = n;
913 mi->m = m;
914 mi->size = num * sizeof(dReal);
915 mi->data = (dReal*) dAlloc (mi->size);
916 memcpy (mi->data,A,mi->size);
917
918 va_list ap;
919 va_start (ap,name);
920 vsprintf (mi->name,name,ap);
921 if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
922
923 mat.push (mi);
924 return 0;
925 }
926 else {
927 if (lower_tri && n != m)
928 dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
929 if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
930 dMatInfo *mp = mat[index];
931 index++;
932
933 dMatInfo mi;
934 va_list ap;
935 va_start (ap,name);
936 vsprintf (mi.name,name,ap);
937 if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
938
939 if (strcmp(mp->name,mi.name) != 0)
940 dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
941 mp->name,mi.name);
942 if (mp->n != n || mp->m != m)
943 dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
944 mp->n,mp->m,n,m);
945
946 dReal maxdiff;
947 if (lower_tri) {
948 maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
949 }
950 else {
951 maxdiff = dMaxDifference (A,mp->data,n,m);
952 }
953 if (maxdiff > tol)
954 dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
955 "error=%.4e)",n,m,mi.name,maxdiff);
956 return maxdiff;
957 }
958}
959
960
961void dMatrixComparison::end()
962{
963 if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
964 afterfirst = 1;
965 index = 0;
966}
967
968
969void dMatrixComparison::reset()
970{
971 for (int i=0; i<mat.size(); i++) {
972 dFree (mat[i]->data,mat[i]->size);
973 dFree (mat[i],sizeof(dMatInfo));
974 }
975 mat.setSize (0);
976 afterfirst = 0;
977 index = 0;
978}
979
980
981void dMatrixComparison::dump()
982{
983 for (int i=0; i<mat.size(); i++)
984 printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
985}
986
987//****************************************************************************
988// unit test
989
990#include <setjmp.h>
991
992// static jmp_buf jump_buffer;
993
994static void myDebug (int num, const char *msg, va_list ap)
995{
996 // printf ("(Error %d: ",num);
997 // vprintf (msg,ap);
998 // printf (")\n");
999 longjmp (jump_buffer,1);
1000}
1001
1002
1003extern "C" void dTestMatrixComparison()
1004{
1005 volatile int i;
1006 printf ("dTestMatrixComparison()\n");
1007 dMessageFunction *orig_debug = dGetDebugHandler();
1008
1009 dMatrixComparison mc;
1010 dReal A[50*50];
1011
1012 // make first sequence
1013 unsigned long seed = dRandGetSeed();
1014 for (i=1; i<49; i++) {
1015 dMakeRandomMatrix (A,i,i+1,1.0);
1016 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1017 }
1018 mc.end();
1019
1020 //mc.dump();
1021
1022 // test identical sequence
1023 dSetDebugHandler (&myDebug);
1024 dRandSetSeed (seed);
1025 if (setjmp (jump_buffer)) {
1026 printf ("\tFAILED (1)\n");
1027 }
1028 else {
1029 for (i=1; i<49; i++) {
1030 dMakeRandomMatrix (A,i,i+1,1.0);
1031 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1032 }
1033 mc.end();
1034 printf ("\tpassed (1)\n");
1035 }
1036 dSetDebugHandler (orig_debug);
1037
1038 // test broken sequences (with matrix error)
1039 dRandSetSeed (seed);
1040 volatile int passcount = 0;
1041 for (i=1; i<49; i++) {
1042 if (setjmp (jump_buffer)) {
1043 passcount++;
1044 }
1045 else {
1046 dSetDebugHandler (&myDebug);
1047 dMakeRandomMatrix (A,i,i+1,1.0);
1048 A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
1049 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1050 dSetDebugHandler (orig_debug);
1051 }
1052 }
1053 mc.end();
1054 printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
1055
1056 // test broken sequences (with name error)
1057 dRandSetSeed (seed);
1058 passcount = 0;
1059 for (i=1; i<49; i++) {
1060 if (setjmp (jump_buffer)) {
1061 passcount++;
1062 }
1063 else {
1064 dSetDebugHandler (&myDebug);
1065 dMakeRandomMatrix (A,i,i+1,1.0);
1066 mc.nextMatrix (A,i,i+1,0,"B%d",i);
1067 dSetDebugHandler (orig_debug);
1068 }
1069 }
1070 mc.end();
1071 printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
1072
1073 // test identical sequence again
1074 dSetDebugHandler (&myDebug);
1075 dRandSetSeed (seed);
1076 if (setjmp (jump_buffer)) {
1077 printf ("\tFAILED (4)\n");
1078 }
1079 else {
1080 for (i=1; i<49; i++) {
1081 dMakeRandomMatrix (A,i,i+1,1.0);
1082 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1083 }
1084 mc.end();
1085 printf ("\tpassed (4)\n");
1086 }
1087 dSetDebugHandler (orig_debug);
1088}
1089
1090//****************************************************************************
1091
1092// internal unit tests
1093extern "C" void dTestDataStructures();
1094extern "C" void dTestMatrixComparison();
1095extern "C" void dTestSolveLCP();
1096
1097
1098int main()
1099{
1100 dInitODE();
1101 testRandomNumberGenerator();
1102 testInfinity();
1103 testPad();
1104 testCrossProduct();
1105 testSetZero();
1106 testNormalize3();
1107 //testReorthonormalize(); ... not any more
1108 testPlaneSpace();
1109 testMatrixMultiply();
1110 testSmallMatrixMultiply();
1111 testCholeskyFactorization();
1112 testCholeskySolve();
1113 testInvertPDMatrix();
1114 testIsPositiveDefinite();
1115 testFastLDLTFactorization();
1116 testSolveLDLT();
1117 testLDLTAddTL();
1118 testLDLTRemove();
1119 testMassFunctions();
1120 testRtoQandQtoR();
1121 testQuaternionMultiply();
1122 testRotationFunctions();
1123 dTestMatrixComparison();
1124 dTestSolveLCP();
1125 // dTestDataStructures();
1126 dCloseODE();
1127 return 0;
1128}
diff --git a/libraries/ode-0.9/ode/demo/demo_plane2d.cpp b/libraries/ode-0.9/ode/demo/demo_plane2d.cpp
new file mode 100644
index 0000000..3ba4df8
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_plane2d.cpp
@@ -0,0 +1,268 @@
1// Test my Plane2D constraint.
2// Uses ode-0.35 collision API.
3
4# include <stdio.h>
5# include <stdlib.h>
6# include <math.h>
7# include <ode/ode.h>
8# include <drawstuff/drawstuff.h>
9
10# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX)))
11
12# define N_BODIES 40
13# define STAGE_SIZE 8.0 // in m
14
15# define TIME_STEP 0.01
16# define K_SPRING 10.0
17# define K_DAMP 10.0
18
19
20static dWorld dyn_world;
21static dBody dyn_bodies[N_BODIES];
22static dReal bodies_sides[N_BODIES][3];
23
24static dSpaceID coll_space_id;
25static dJointID plane2d_joint_ids[N_BODIES];
26static dJointGroup
27 coll_contacts;
28
29
30
31static void cb_start ()
32/*************************/
33{
34 static float xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.65f*STAGE_SIZE};
35 static float hpr[3] = { 90.0f, -90.0f, 0 };
36
37 dsSetViewpoint (xyz, hpr);
38}
39
40
41
42static void cb_near_collision (void *data, dGeomID o1, dGeomID o2)
43/********************************************************************/
44{
45 dBodyID b1 = dGeomGetBody (o1);
46 dBodyID b2 = dGeomGetBody (o2);
47 dContact contact;
48
49
50 // exit without doing anything if the two bodies are static
51 if (b1 == 0 && b2 == 0)
52 return;
53
54 // exit without doing anything if the two bodies are connected by a joint
55 if (b1 && b2 && dAreConnected (b1, b2))
56 {
57 /* MTRAP; */
58 return;
59 }
60
61 contact.surface.mode = 0;
62 contact.surface.mu = 0; // frictionless
63
64 if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom)))
65 {
66 dJointID c = dJointCreateContact (dyn_world.id(),
67 coll_contacts.id (), &contact);
68 dJointAttach (c, b1, b2);
69 }
70}
71
72
73static void track_to_pos (dBody &body, dJointID joint_id,
74 dReal target_x, dReal target_y)
75/************************************************************************/
76{
77 dReal curr_x = body.getPosition()[0];
78 dReal curr_y = body.getPosition()[1];
79
80 dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x));
81 dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y));
82}
83
84
85
86static void cb_sim_step (int pause)
87/*************************************/
88{
89 if (! pause)
90 {
91 static dReal angle = 0;
92
93 angle += REAL( 0.01 );
94
95 track_to_pos (dyn_bodies[0], plane2d_joint_ids[0],
96 dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ),
97 dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) ));
98
99 /* double f0 = 0.001; */
100 /* for (int b = 0; b < N_BODIES; b ++) */
101 /* { */
102 /* double p = 1 + b / (double) N_BODIES; */
103 /* double q = 2 - b / (double) N_BODIES; */
104 /* dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */
105 /* } */
106 /* dyn_bodies[0].addTorque (0, 0, 0.1); */
107
108 const int n = 10;
109 for (int i = 0; i < n; i ++)
110 {
111 dSpaceCollide (coll_space_id, 0, &cb_near_collision);
112 dyn_world.step (dReal(TIME_STEP/n));
113 coll_contacts.empty ();
114 }
115 }
116
117# if 1 /* [ */
118 {
119 // @@@ hack Plane2D constraint error reduction here:
120 for (int b = 0; b < N_BODIES; b ++)
121 {
122 const dReal *rot = dBodyGetAngularVel (dyn_bodies[b].id());
123 const dReal *quat_ptr;
124 dReal quat[4],
125 quat_len;
126
127
128 quat_ptr = dBodyGetQuaternion (dyn_bodies[b].id());
129 quat[0] = quat_ptr[0];
130 quat[1] = 0;
131 quat[2] = 0;
132 quat[3] = quat_ptr[3];
133 quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]);
134 quat[0] /= quat_len;
135 quat[3] /= quat_len;
136 dBodySetQuaternion (dyn_bodies[b].id(), quat);
137 dBodySetAngularVel (dyn_bodies[b].id(), 0, 0, rot[2]);
138 }
139 }
140# endif /* ] */
141
142
143# if 0 /* [ */
144 {
145 // @@@ friction
146 for (int b = 0; b < N_BODIES; b ++)
147 {
148 const dReal *vel = dBodyGetLinearVel (dyn_bodies[b].id()),
149 *rot = dBodyGetAngularVel (dyn_bodies[b].id());
150 dReal s = 1.00;
151 dReal t = 0.99;
152
153 dBodySetLinearVel (dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]);
154 dBodySetAngularVel (dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]);
155 }
156 }
157# endif /* ] */
158
159
160 {
161 // ode drawstuff
162
163 dsSetTexture (DS_WOOD);
164 for (int b = 0; b < N_BODIES; b ++)
165 {
166 if (b == 0)
167 dsSetColor (1.0, 0.5, 1.0);
168 else
169 dsSetColor (0, 0.5, 1.0);
170#ifdef dDOUBLE
171 dsDrawBoxD (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]);
172#else
173 dsDrawBox (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]);
174#endif
175 }
176 }
177}
178
179
180
181extern int main
182/******************/
183(
184 int argc,
185 char **argv
186)
187{
188 int b;
189 dsFunctions drawstuff_functions;
190
191
192 dInitODE();
193
194 // dynamic world
195
196 dReal cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP;
197 dReal err_reduct;// = TIME_STEP * K_SPRING * cf_mixing;
198 err_reduct = REAL( 0.5 );
199 cf_mixing = REAL( 0.001 );
200 dWorldSetERP (dyn_world.id (), err_reduct);
201 dWorldSetCFM (dyn_world.id (), cf_mixing);
202 dyn_world.setGravity (0, 0.0, -1.0);
203
204 coll_space_id = dSimpleSpaceCreate (0);
205
206 // dynamic bodies
207 for (b = 0; b < N_BODIES; b ++)
208 {
209 int l = (int) (1 + sqrt ((double) N_BODIES));
210 dReal x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE );
211 dReal y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE );
212 dReal z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48();
213
214 bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
215 bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
216 bodies_sides[b][2] = z;
217
218 dyn_bodies[b].create (dyn_world);
219 dyn_bodies[b].setPosition (x, y, z/2);
220 dyn_bodies[b].setData ((void*) (size_t)b);
221 dBodySetLinearVel (dyn_bodies[b].id (),
222 dReal( 3 * (drand48 () - 0.5) ),
223 dReal( 3 * (drand48 () - 0.5) ), 0);
224
225 dMass m;
226 m.setBox (1, bodies_sides[b][0],bodies_sides[b][1],bodies_sides[b][2]);
227 m.adjust (REAL(0.1) * bodies_sides[b][0] * bodies_sides[b][1]);
228 dyn_bodies[b].setMass (&m);
229
230 plane2d_joint_ids[b] = dJointCreatePlane2D (dyn_world.id (), 0);
231 dJointAttach (plane2d_joint_ids[b], dyn_bodies[b].id (), 0);
232 }
233
234 dJointSetPlane2DXParam (plane2d_joint_ids[0], dParamFMax, 10);
235 dJointSetPlane2DYParam (plane2d_joint_ids[0], dParamFMax, 10);
236
237
238 // collision geoms and joints
239 dCreatePlane (coll_space_id, 1, 0, 0, 0);
240 dCreatePlane (coll_space_id, -1, 0, 0, -STAGE_SIZE);
241 dCreatePlane (coll_space_id, 0, 1, 0, 0);
242 dCreatePlane (coll_space_id, 0, -1, 0, -STAGE_SIZE);
243
244 for (b = 0; b < N_BODIES; b ++)
245 {
246 dGeomID coll_box_id;
247 coll_box_id = dCreateBox (coll_space_id,
248 bodies_sides[b][0], bodies_sides[b][1], bodies_sides[b][2]);
249 dGeomSetBody (coll_box_id, dyn_bodies[b].id ());
250 }
251
252 coll_contacts.create (0);
253
254 {
255 // simulation loop (by drawstuff lib)
256 drawstuff_functions.version = DS_VERSION;
257 drawstuff_functions.start = &cb_start;
258 drawstuff_functions.step = &cb_sim_step;
259 drawstuff_functions.command = 0;
260 drawstuff_functions.stop = 0;
261 drawstuff_functions.path_to_textures = "../../drawstuff/textures";
262
263 dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions);
264 }
265
266 dCloseODE();
267 return 0;
268}
diff --git a/libraries/ode-0.9/ode/demo/demo_slider.cpp b/libraries/ode-0.9/ode/demo/demo_slider.cpp
new file mode 100644
index 0000000..5de08b0
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_slider.cpp
@@ -0,0 +1,172 @@
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// select correct drawing functions
31#ifdef dDOUBLE
32#define dsDrawBox dsDrawBoxD
33#endif
34
35
36// some constants
37#define SIDE (0.5f) // side length of a box
38#define MASS (1.0) // mass of a box
39
40
41// dynamics and collision objects
42static dWorldID world;
43static dBodyID body[2];
44static dJointID slider;
45
46
47// state set by keyboard commands
48static int occasional_error = 0;
49
50
51// start simulation - set viewpoint
52
53static void start()
54{
55 static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
56 static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
57 dsSetViewpoint (xyz,hpr);
58 printf ("Press 'e' to start/stop occasional error.\n");
59}
60
61
62// called when a key pressed
63
64static void command (int cmd)
65{
66 if (cmd == 'e' || cmd == 'E') {
67 occasional_error ^= 1;
68 }
69}
70
71
72// simulation loop
73
74static void simLoop (int pause)
75{
76 const dReal kd = -0.3; // angular damping constant
77 const dReal ks = 0.5; // spring constant
78 if (!pause) {
79 // add an oscillating torque to body 0, and also damp its rotational motion
80 static dReal a=0;
81 const dReal *w = dBodyGetAngularVel (body[0]);
82 dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
83 a += 0.01;
84
85 // add a spring force to keep the bodies together, otherwise they will
86 // fly apart along the slider axis.
87 const dReal *p1 = dBodyGetPosition (body[0]);
88 const dReal *p2 = dBodyGetPosition (body[1]);
89 dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
90 ks*(p2[2]-p1[2]));
91 dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
92 ks*(p1[2]-p2[2]));
93
94 // occasionally re-orient one of the bodies to create a deliberate error.
95 if (occasional_error) {
96 static int count = 0;
97 if ((count % 20)==0) {
98 // randomly adjust orientation of body[0]
99 const dReal *R1;
100 dMatrix3 R2,R3;
101 R1 = dBodyGetRotation (body[0]);
102 dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
103 dRandReal()-0.5,dRandReal()-0.5);
104 dMultiply0 (R3,R1,R2,3,3,3);
105 dBodySetRotation (body[0],R3);
106
107 // randomly adjust position of body[0]
108 const dReal *pos = dBodyGetPosition (body[0]);
109 dBodySetPosition (body[0],
110 pos[0]+0.2*(dRandReal()-0.5),
111 pos[1]+0.2*(dRandReal()-0.5),
112 pos[2]+0.2*(dRandReal()-0.5));
113 }
114 count++;
115 }
116
117 dWorldStep (world,0.05);
118 }
119
120 dReal sides1[3] = {SIDE,SIDE,SIDE};
121 dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
122 dsSetTexture (DS_WOOD);
123 dsSetColor (1,1,0);
124 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
125 dsSetColor (0,1,1);
126 dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
127}
128
129
130int main (int argc, char **argv)
131{
132 // setup pointers to drawstuff callback functions
133 dsFunctions fn;
134 fn.version = DS_VERSION;
135 fn.start = &start;
136 fn.step = &simLoop;
137 fn.command = &command;
138 fn.stop = 0;
139 fn.path_to_textures = "../../drawstuff/textures";
140 if(argc==2)
141 {
142 fn.path_to_textures = argv[1];
143 }
144
145 // create world
146 dInitODE();
147 world = dWorldCreate();
148 dMass m;
149 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
150 dMassAdjust (&m,MASS);
151
152 body[0] = dBodyCreate (world);
153 dBodySetMass (body[0],&m);
154 dBodySetPosition (body[0],0,0,1);
155 body[1] = dBodyCreate (world);
156 dBodySetMass (body[1],&m);
157 dQuaternion q;
158 dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI);
159 dBodySetPosition (body[1],0.2,0.2,1.2);
160 dBodySetQuaternion (body[1],q);
161
162 slider = dJointCreateSlider (world,0);
163 dJointAttach (slider,body[0],body[1]);
164 dJointSetSliderAxis (slider,1,1,1);
165
166 // run simulation
167 dsSimulationLoop (argc,argv,352,288,&fn);
168
169 dWorldDestroy (world);
170 dCloseODE();
171 return 0;
172}
diff --git a/libraries/ode-0.9/ode/demo/demo_space.cpp b/libraries/ode-0.9/ode/demo/demo_space.cpp
new file mode 100644
index 0000000..73101b5
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_space.cpp
@@ -0,0 +1,233 @@
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/*
24
25testing procedure:
26 * create a bunch of random boxes
27 * test for intersections directly, put results in n^2 array
28 * get space to report collisions:
29 - all correct collisions reported
30 - no pair reported more than once
31 - no incorrect collisions reported
32
33*/
34
35
36#include <ode/ode.h>
37#include <drawstuff/drawstuff.h>
38
39#ifdef _MSC_VER
40#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
41#endif
42
43// select correct drawing functions
44
45#ifdef dDOUBLE
46#define dsDrawBox dsDrawBoxD
47#define dsDrawSphere dsDrawSphereD
48#define dsDrawCylinder dsDrawCylinderD
49#define dsDrawCapsule dsDrawCapsuleD
50#endif
51
52
53// some constants
54
55#define NUM 20 // number of boxes to test
56
57
58// collision objects and globals
59
60static dSpaceID space;
61static dGeomID geom[NUM];
62static dReal bounds[NUM][6];
63static size_t good_matrix[NUM][NUM]; // correct collision matrix
64static size_t test_matrix[NUM][NUM]; // testing collision matrix
65static size_t hits[NUM]; // number of collisions a box has
66static unsigned long seed=37;
67
68
69static void init_test()
70{
71 int i,j;
72 const dReal scale = 0.5;
73
74 // set random boxes
75 dRandSetSeed (seed);
76 for (i=0; i < NUM; i++) {
77 bounds[i][0] = dRandReal()*2-1;
78 bounds[i][1] = bounds[i][0] + dRandReal()*scale;
79 bounds[i][2] = dRandReal()*2-1;
80 bounds[i][3] = bounds[i][2] + dRandReal()*scale;
81 bounds[i][4] = dRandReal()*2;
82 bounds[i][5] = bounds[i][4] + dRandReal()*scale;
83
84 if (geom[i]) dGeomDestroy (geom[i]);
85 geom[i] = dCreateBox (space,
86 bounds[i][1] - bounds[i][0],
87 bounds[i][3] - bounds[i][2],
88 bounds[i][5] - bounds[i][4]);
89 dGeomSetPosition (geom[i],
90 (bounds[i][0] + bounds[i][1])*0.5,
91 (bounds[i][2] + bounds[i][3])*0.5,
92 (bounds[i][4] + bounds[i][5])*0.5);
93 dGeomSetData (geom[i],(void*)(size_t)(i));
94 }
95
96 // compute all intersections and put the results in "good_matrix"
97 for (i=0; i < NUM; i++) {
98 for (j=0; j < NUM; j++) good_matrix[i][j] = 0;
99 }
100 for (i=0; i < NUM; i++) hits[i] = 0;
101
102 for (i=0; i < NUM; i++) {
103 for (j=i+1; j < NUM; j++) {
104 dReal *bounds1 = &bounds[i][0];
105 dReal *bounds2 = &bounds[j][0];
106 if (bounds1[0] > bounds2[1] ||
107 bounds1[1] < bounds2[0] ||
108 bounds1[2] > bounds2[3] ||
109 bounds1[3] < bounds2[2] ||
110 bounds1[4] > bounds2[5] ||
111 bounds1[5] < bounds2[4]) continue;
112 good_matrix[i][j] = 1;
113 good_matrix[j][i] = 1;
114 hits[i]++;
115 hits[j]++;
116 }
117 }
118}
119
120
121// this is called by dSpaceCollide when two objects in space are
122// potentially colliding.
123
124static void nearCallback (void *data, dGeomID o1, dGeomID o2)
125{
126 size_t i,j;
127 i = (size_t) dGeomGetData (o1);
128 j = (size_t) dGeomGetData (o2);
129 if (i==j)
130 printf ("collision (%d,%d) is between the same object\n",i,j);
131 if (!good_matrix[i][j] || !good_matrix[j][i])
132 printf ("collision (%d,%d) is incorrect\n",i,j);
133 if (test_matrix[i][j] || test_matrix[j][i])
134 printf ("collision (%d,%d) reported more than once\n",i,j);
135 test_matrix[i][j] = 1;
136 test_matrix[j][i] = 1;
137}
138
139
140// start simulation - set viewpoint
141
142static void start()
143{
144 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
145 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
146 dsSetViewpoint (xyz,hpr);
147}
148
149
150static void command (int cmd)
151{
152 if (cmd == ' ') {
153 seed++;
154 init_test();
155 }
156}
157
158
159// simulation loop
160
161static void simLoop (int pause)
162{
163 int i,j;
164
165 for (i=0; i < NUM; i++) {
166 for (j=0; j < NUM; j++) test_matrix[i][j] = 0;
167 }
168 dSpaceCollide (space,0,&nearCallback);
169 for (i=0; i < NUM; i++) {
170 for (j=i+1; j < NUM; j++) {
171 if (good_matrix[i][j] && !test_matrix[i][j]) {
172 printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed);
173 }
174 }
175 }
176
177 seed++;
178 init_test();
179
180 for (i=0; i<NUM; i++) {
181 dVector3 pos,side;
182 dMatrix3 R;
183 dRSetIdentity (R);
184 for (j=0; j<3; j++) pos[j] = (bounds[i][j*2+1] + bounds[i][j*2]) * 0.5;
185 for (j=0; j<3; j++) side[j] = bounds[i][j*2+1] - bounds[i][j*2];
186 if (hits[i] > 0) dsSetColor (1,0,0);
187 else dsSetColor (1,1,0);
188 dsDrawBox (pos,R,side);
189 }
190}
191
192
193int main (int argc, char **argv)
194{
195 int i;
196
197 // setup pointers to drawstuff callback functions
198 dsFunctions fn;
199 fn.version = DS_VERSION;
200 fn.start = &start;
201 fn.step = &simLoop;
202 fn.command = &command;
203 fn.stop = 0;
204 fn.path_to_textures = "../../drawstuff/textures";
205 if(argc==2)
206 {
207 fn.path_to_textures = argv[1];
208 }
209
210 dInitODE();
211
212 // test the simple space:
213 // space = dSimpleSpaceCreate();
214
215 // test the hash space:
216 // space = dHashSpaceCreate (0);
217 // dHashSpaceSetLevels (space,-10,10);
218
219 // test the quadtree space
220 dVector3 Center = {0, 0, 0, 0};
221 dVector3 Extents = {10, 0, 10, 0};
222 space = dQuadTreeSpaceCreate(0, Center, Extents, 7);
223
224 for (i=0; i < NUM; i++) geom[i] = 0;
225 init_test();
226
227 // run simulation
228 dsSimulationLoop (argc,argv,352,288,&fn);
229
230 dSpaceDestroy (space);
231 dCloseODE();
232 return 0;
233}
diff --git a/libraries/ode-0.9/ode/demo/demo_space_stress.cpp b/libraries/ode-0.9/ode/demo/demo_space_stress.cpp
new file mode 100644
index 0000000..e1be369
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_space_stress.cpp
@@ -0,0 +1,435 @@
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#endif
38
39
40// some constants
41
42#define NUM 10000 // max number of objects
43#define DENSITY (5.0) // density of all objects
44#define GPB 3 // maximum number of geometries per body
45#define MAX_CONTACTS 4 // maximum number of contact points per body
46#define WORLD_SIZE 100
47
48
49// dynamics and collision objects
50
51struct MyObject {
52 dBodyID body; // the body
53 dGeomID geom[GPB]; // geometries representing this body
54};
55
56static int num=0; // number of objects in simulation
57static int nextobj=0; // next object to recycle if num==NUM
58static dWorldID world;
59static dSpaceID space;
60static MyObject obj[NUM];
61static dJointGroupID contactgroup;
62static int selected = -1; // selected object
63static int show_aabb = 0; // show geom AABBs?
64static int show_contacts = 0; // show contact points?
65static int random_pos = 1; // drop objects from random position?
66static int draw_geom = 1;
67
68
69// this is called by dSpaceCollide when two objects in space are
70// potentially colliding.
71
72static void nearCallback (void *data, dGeomID o1, dGeomID o2)
73{
74 int i;
75 // if (o1->body && o2->body) return;
76
77 // exit without doing anything if the two bodies are connected by a joint
78 dBodyID b1 = dGeomGetBody(o1);
79 dBodyID b2 = dGeomGetBody(o2);
80 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
81
82 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
83 for (i=0; i<MAX_CONTACTS; i++) {
84 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
85 contact[i].surface.mu = dInfinity;
86 contact[i].surface.mu2 = 0;
87 contact[i].surface.bounce = 0.1;
88 contact[i].surface.bounce_vel = 0.1;
89 contact[i].surface.soft_cfm = 0.01;
90 }
91 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
92 sizeof(dContact))) {
93 dMatrix3 RI;
94 dRSetIdentity (RI);
95 const dReal ss[3] = {0.02,0.02,0.02};
96 for (i=0; i<numc; i++) {
97 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
98 dJointAttach (c,b1,b2);
99 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
100 }
101 }
102}
103
104
105// start simulation - set viewpoint
106
107static void start()
108{
109 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
110 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
111 dsSetViewpoint (xyz,hpr);
112 printf ("To drop another object, press:\n");
113 printf (" o to disable rendering.\n");
114 printf (" b for box.\n");
115 printf (" s for sphere.\n");
116 printf (" c for cylinder.\n");
117 printf (" x for a composite object.\n");
118 printf ("To select an object, press space.\n");
119 printf ("To disable the selected object, press d.\n");
120 printf ("To enable the selected object, press e.\n");
121 printf ("To toggle showing the geom AABBs, press a.\n");
122 printf ("To toggle showing the contact points, press t.\n");
123 printf ("To toggle dropping from random position/orientation, press r.\n");
124}
125
126
127char locase (char c)
128{
129 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
130 else return c;
131}
132
133
134// called when a key pressed
135
136static void command (int cmd)
137{
138 int i,j,k;
139 dReal sides[3];
140 dMass m;
141
142 cmd = locase (cmd);
143 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
144 /* || cmd == 'l' */) {
145 if (num < NUM) {
146 i = num;
147 num++;
148 }
149 else {
150 i = nextobj;
151 nextobj++;
152 if (nextobj >= num) nextobj = 0;
153
154 // destroy the body and geoms for slot i
155 dBodyDestroy (obj[i].body);
156 for (k=0; k < GPB; k++) {
157 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
158 }
159 memset (&obj[i],0,sizeof(obj[i]));
160 }
161
162 obj[i].body = dBodyCreate (world);
163 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
164
165 dMatrix3 R;
166 if (random_pos) {
167 dBodySetPosition (obj[i].body,
168 dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()+1);
169 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
170 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
171 }
172 else {
173 dReal maxheight = 0;
174 for (k=0; k<num; k++) {
175 const dReal *pos = dBodyGetPosition (obj[k].body);
176 if (pos[2] > maxheight) maxheight = pos[2];
177 }
178 dBodySetPosition (obj[i].body, 0,0,maxheight+1);
179 dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
180 }
181 dBodySetRotation (obj[i].body,R);
182 dBodySetData (obj[i].body,(void*)(size_t)i);
183
184 if (cmd == 'b') {
185 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
186 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
187 }
188 else if (cmd == 'c') {
189 sides[0] *= 0.5;
190 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
191 obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
192 }
193/*
194 // cylinder option not yet implemented
195 else if (cmd == 'l') {
196 sides[1] *= 0.5;
197 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
198 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
199 }
200*/
201 else if (cmd == 's') {
202 sides[0] *= 0.5;
203 dMassSetSphere (&m,DENSITY,sides[0]);
204 obj[i].geom[0] = dCreateSphere (space,sides[0]);
205 }
206 else if (cmd == 'x') {
207 dGeomID g2[GPB]; // encapsulated geometries
208 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
209
210 // start accumulating masses for the encapsulated geometries
211 dMass m2;
212 dMassSetZero (&m);
213
214 // set random delta positions
215 for (j=0; j<GPB; j++) {
216 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
217 }
218
219 for (k=0; k<GPB; k++) {
220 obj[i].geom[k] = dCreateGeomTransform (space);
221 dGeomTransformSetCleanup (obj[i].geom[k],1);
222 if (k==0) {
223 dReal radius = dRandReal()*0.25+0.05;
224 g2[k] = dCreateSphere (0,radius);
225 dMassSetSphere (&m2,DENSITY,radius);
226 }
227 else if (k==1) {
228 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
229 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
230 }
231 else {
232 dReal radius = dRandReal()*0.1+0.05;
233 dReal length = dRandReal()*1.0+0.1;
234 g2[k] = dCreateCapsule (0,radius,length);
235 dMassSetCapsule (&m2,DENSITY,3,radius,length);
236 }
237 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
238
239 // set the transformation (adjust the mass too)
240 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
241 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
242 dMatrix3 Rtx;
243 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
244 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
245 dGeomSetRotation (g2[k],Rtx);
246 dMassRotate (&m2,Rtx);
247
248 // add to the total mass
249 dMassAdd (&m,&m2);
250 }
251
252 // move all encapsulated objects so that the center of mass is (0,0,0)
253 for (k=0; k<2; k++) {
254 dGeomSetPosition (g2[k],
255 dpos[k][0]-m.c[0],
256 dpos[k][1]-m.c[1],
257 dpos[k][2]-m.c[2]);
258 }
259 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
260 }
261
262 for (k=0; k < GPB; k++) {
263 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
264 }
265
266 dBodySetMass (obj[i].body,&m);
267 }
268
269 if (cmd == ' ') {
270 selected++;
271 if (selected >= num) selected = 0;
272 if (selected < 0) selected = 0;
273 }
274 else if (cmd == 'd' && selected >= 0 && selected < num) {
275 dBodyDisable (obj[selected].body);
276 }
277 else if (cmd == 'e' && selected >= 0 && selected < num) {
278 dBodyEnable (obj[selected].body);
279 }
280 else if (cmd == 'a') {
281 show_aabb ^= 1;
282 }
283 else if (cmd == 't') {
284 show_contacts ^= 1;
285 }
286 else if (cmd == 'r') {
287 random_pos ^= 1;
288 }
289 else if (cmd == 'o') {
290 draw_geom ^= 1;
291 }
292}
293
294
295// draw a geom
296
297void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
298{
299 if (!draw_geom){
300 return;
301 }
302
303 if (!g) return;
304 if (!pos) pos = dGeomGetPosition (g);
305 if (!R) R = dGeomGetRotation (g);
306
307 int type = dGeomGetClass (g);
308 if (type == dBoxClass) {
309 dVector3 sides;
310 dGeomBoxGetLengths (g,sides);
311 dsDrawBox (pos,R,sides);
312 }
313 else if (type == dSphereClass) {
314 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
315 }
316 else if (type == dCapsuleClass) {
317 dReal radius,length;
318 dGeomCapsuleGetParams (g,&radius,&length);
319 dsDrawCapsule (pos,R,length,radius);
320 }
321/*
322 // cylinder option not yet implemented
323 else if (type == dCylinderClass) {
324 dReal radius,length;
325 dGeomCylinderGetParams (g,&radius,&length);
326 dsDrawCylinder (pos,R,length,radius);
327 }
328*/
329 else if (type == dGeomTransformClass) {
330 dGeomID g2 = dGeomTransformGetGeom (g);
331 const dReal *pos2 = dGeomGetPosition (g2);
332 const dReal *R2 = dGeomGetRotation (g2);
333 dVector3 actual_pos;
334 dMatrix3 actual_R;
335 dMULTIPLY0_331 (actual_pos,R,pos2);
336 actual_pos[0] += pos[0];
337 actual_pos[1] += pos[1];
338 actual_pos[2] += pos[2];
339 dMULTIPLY0_333 (actual_R,R,R2);
340 drawGeom (g2,actual_pos,actual_R,0);
341 }
342
343 if (show_aabb) {
344 // draw the bounding box for this geom
345 dReal aabb[6];
346 dGeomGetAABB (g,aabb);
347 dVector3 bbpos;
348 for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
349 dVector3 bbsides;
350 for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
351 dMatrix3 RI;
352 dRSetIdentity (RI);
353 dsSetColorAlpha (1,0,0,0.5);
354 dsDrawBox (bbpos,RI,bbsides);
355 }
356}
357
358
359// simulation loop
360
361static void simLoop (int pause)
362{
363 dsSetColor (0,0,2);
364 dSpaceCollide (space,0,&nearCallback);
365 //if (!pause) dWorldStep (world,0.05);
366 //if (!pause) dWorldStepFast (world,0.05, 1);
367
368 // remove all contact joints
369 dJointGroupEmpty (contactgroup);
370
371 dsSetColor (1,1,0);
372 dsSetTexture (DS_WOOD);
373 for (int i=0; i<num; i++) {
374 for (int j=0; j < GPB; j++) {
375 if (i==selected) {
376 dsSetColor (0,0.7,1);
377 }
378 else if (! dBodyIsEnabled (obj[i].body)) {
379 dsSetColor (1,0,0);
380 }
381 else {
382 dsSetColor (1,1,0);
383 }
384 drawGeom (obj[i].geom[j],0,0,show_aabb);
385 }
386 }
387}
388
389
390int main (int argc, char **argv)
391{
392 // setup pointers to drawstuff callback functions
393 dsFunctions fn;
394 fn.version = DS_VERSION;
395 fn.start = &start;
396 fn.step = &simLoop;
397 fn.command = &command;
398 fn.stop = 0;
399 fn.path_to_textures = "../../drawstuff/textures";
400 if(argc==2)
401 {
402 fn.path_to_textures = argv[1];
403 }
404
405 // create world
406 dInitODE();
407 world = dWorldCreate();
408
409
410 dVector3 Center = {0, 0, 0, 0};
411 dVector3 Extents = {WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, 0};
412
413 //space = dSimpleSpaceCreate(0);
414 //space = dHashSpaceCreate (0);
415 space = dQuadTreeSpaceCreate (0, Center, Extents, 6);
416
417 contactgroup = dJointGroupCreate (0);
418 dWorldSetGravity (world,0,0,-0.5);
419 dWorldSetCFM (world,1e-5);
420 dCreatePlane (space,0,0,1,0);
421 memset (obj,0,sizeof(obj));
422
423 for (int i = 0; i < NUM; i++){
424 command('s');
425 }
426
427 // run simulation
428 dsSimulationLoop (argc,argv,352,288,&fn);
429
430 dJointGroupDestroy (contactgroup);
431 dSpaceDestroy (space);
432 dWorldDestroy (world);
433 dCloseODE();
434 return 0;
435}
diff --git a/libraries/ode-0.9/ode/demo/demo_step.cpp b/libraries/ode-0.9/ode/demo/demo_step.cpp
new file mode 100644
index 0000000..df2385c
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_step.cpp
@@ -0,0 +1,193 @@
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// test the step function by comparing the output of the fast and the slow
24// version, for various systems. currently you have to define COMPARE_METHODS
25// in step.cpp for this to work properly.
26//
27// @@@ report MAX error
28
29#include <time.h>
30#include <ode/ode.h>
31#include <drawstuff/drawstuff.h>
32
33#ifdef _MSC_VER
34#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
35#endif
36
37// select correct drawing functions
38
39#ifdef dDOUBLE
40#define dsDrawBox dsDrawBoxD
41#define dsDrawSphere dsDrawSphereD
42#define dsDrawCylinder dsDrawCylinderD
43#define dsDrawCapsule dsDrawCapsuleD
44#endif
45
46
47// some constants
48
49#define NUM 10 // number of bodies
50#define NUMJ 9 // number of joints
51#define SIDE (0.2) // side length of a box
52#define MASS (1.0) // mass of a box
53#define RADIUS (0.1732f) // sphere radius
54
55
56
57// dynamics and collision objects
58
59static dWorldID world=0;
60static dBodyID body[NUM];
61static dJointID joint[NUMJ];
62
63
64// create the test system
65
66void createTest()
67{
68 int i,j;
69 if (world) dWorldDestroy (world);
70
71 world = dWorldCreate();
72
73 // create random bodies
74 for (i=0; i<NUM; i++) {
75 // create bodies at random position and orientation
76 body[i] = dBodyCreate (world);
77 dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1,
78 dRandReal()*2+RADIUS);
79 dReal q[4];
80 for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
81 dBodySetQuaternion (body[i],q);
82
83 // set random velocity
84 dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
85 dRandReal()*2-1);
86 dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
87 dRandReal()*2-1);
88
89 // set random mass (random diagonal mass rotated by a random amount)
90 dMass m;
91 dMatrix3 R;
92 dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1);
93 dMassAdjust (&m,dRandReal()+1);
94 for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
95 dQtoR (q,R);
96 dMassRotate (&m,R);
97 dBodySetMass (body[i],&m);
98 }
99
100 // create ball-n-socket joints at random positions, linking random bodies
101 // (but make sure not to link the same pair of bodies twice)
102 char linked[NUM*NUM];
103 for (i=0; i<NUM*NUM; i++) linked[i] = 0;
104 for (i=0; i<NUMJ; i++) {
105 int b1,b2;
106 do {
107 b1 = dRandInt (NUM);
108 b2 = dRandInt (NUM);
109 } while (linked[b1*NUM + b2] || b1==b2);
110 linked[b1*NUM + b2] = 1;
111 linked[b2*NUM + b1] = 1;
112 joint[i] = dJointCreateBall (world,0);
113 dJointAttach (joint[i],body[b1],body[b2]);
114 dJointSetBallAnchor (joint[i],dRandReal()*2-1,
115 dRandReal()*2-1,dRandReal()*2+RADIUS);
116 }
117
118 for (i=0; i<NUM; i++) {
119 // move bodies a bit to get some joint error
120 const dReal *pos = dBodyGetPosition (body[i]);
121 dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1,
122 pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1);
123 }
124}
125
126
127// start simulation - set viewpoint
128
129static void start()
130{
131 static float xyz[3] = {2.6117f,-1.4433f,2.3700f};
132 static float hpr[3] = {151.5000f,-30.5000f,0.0000f};
133 dsSetViewpoint (xyz,hpr);
134}
135
136
137// simulation loop
138
139static void simLoop (int pause)
140{
141 if (!pause) {
142 // add random forces and torques to all bodies
143 int i;
144 const dReal scale1 = 5;
145 const dReal scale2 = 5;
146 for (i=0; i<NUM; i++) {
147 dBodyAddForce (body[i],
148 scale1*(dRandReal()*2-1),
149 scale1*(dRandReal()*2-1),
150 scale1*(dRandReal()*2-1));
151 dBodyAddTorque (body[i],
152 scale2*(dRandReal()*2-1),
153 scale2*(dRandReal()*2-1),
154 scale2*(dRandReal()*2-1));
155 }
156
157 dWorldStep (world,0.05);
158 createTest();
159 }
160
161 // float sides[3] = {SIDE,SIDE,SIDE};
162 dsSetColor (1,1,0);
163 for (int i=0; i<NUM; i++)
164 dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
165}
166
167
168int main (int argc, char **argv)
169{
170 // setup pointers to drawstuff callback functions
171 dsFunctions fn;
172 fn.version = DS_VERSION;
173 fn.start = &start;
174 fn.step = &simLoop;
175 fn.command = 0;
176 fn.stop = 0;
177 fn.path_to_textures = "../../drawstuff/textures";
178 if(argc==2)
179 {
180 fn.path_to_textures = argv[1];
181 }
182
183 dInitODE();
184 dRandSetSeed (time(0));
185 createTest();
186
187 // run simulation
188 dsSimulationLoop (argc,argv,352,288,&fn);
189
190 dWorldDestroy (world);
191 dCloseODE();
192 return 0;
193}
diff --git a/libraries/ode-0.9/ode/demo/demo_trimesh.cpp b/libraries/ode-0.9/ode/demo/demo_trimesh.cpp
new file mode 100644
index 0000000..f87ff0b
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_trimesh.cpp
@@ -0,0 +1,537 @@
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// TriMesh test by Erwin de Vries
24
25#include <ode/ode.h>
26#include <drawstuff/drawstuff.h>
27
28#ifdef _MSC_VER
29#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
30#endif
31
32// select correct drawing functions
33
34#ifdef dDOUBLE
35#define dsDrawBox dsDrawBoxD
36#define dsDrawSphere dsDrawSphereD
37#define dsDrawCylinder dsDrawCylinderD
38#define dsDrawCapsule dsDrawCapsuleD
39#define dsDrawLine dsDrawLineD
40#define dsDrawTriangle dsDrawTriangleD
41#endif
42
43
44// some constants
45
46#define NUM 200 // max number of objects
47#define DENSITY (5.0) // density of all objects
48#define GPB 3 // maximum number of geometries per body
49#define MAX_CONTACTS 40 // maximum number of contact points per body
50
51
52// dynamics and collision objects
53
54struct MyObject {
55 dBodyID body; // the body
56 dGeomID geom[GPB]; // geometries representing this body
57};
58
59static int num=0; // number of objects in simulation
60static int nextobj=0; // next object to recycle if num==NUM
61static dWorldID world;
62static dSpaceID space;
63static MyObject obj[NUM];
64static dJointGroupID contactgroup;
65static int selected = -1; // selected object
66static int show_aabb = 0; // show geom AABBs?
67static int show_contacts = 0; // show contact points?
68static int random_pos = 1; // drop objects from random position?
69
70#define VertexCount 5
71#define IndexCount 12
72
73static dVector3 Size;
74static dVector3 Vertices[VertexCount];
75static int Indices[IndexCount];
76
77static dGeomID TriMesh;
78static dGeomID Ray;
79
80
81// this is called by dSpaceCollide when two objects in space are
82// potentially colliding.
83
84static void nearCallback (void *data, dGeomID o1, dGeomID o2)
85{
86 int i;
87 // if (o1->body && o2->body) return;
88
89 // exit without doing anything if the two bodies are connected by a joint
90 dBodyID b1 = dGeomGetBody(o1);
91 dBodyID b2 = dGeomGetBody(o2);
92 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
93
94 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
95 for (i=0; i<MAX_CONTACTS; i++) {
96 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
97 contact[i].surface.mu = dInfinity;
98 contact[i].surface.mu2 = 0;
99 contact[i].surface.bounce = 0.1;
100 contact[i].surface.bounce_vel = 0.1;
101 contact[i].surface.soft_cfm = 0.01;
102 }
103 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
104 sizeof(dContact))) {
105 dMatrix3 RI;
106 dRSetIdentity (RI);
107 const dReal ss[3] = {0.02,0.02,0.02};
108 for (i=0; i<numc; i++) {
109 if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
110 dMatrix3 Rotation;
111 dRSetIdentity(Rotation);
112 dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
113
114 dVector3 End;
115 End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth);
116 End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth);
117 End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth);
118 End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth);
119
120 dsDrawLine(contact[i].geom.pos, End);
121 continue;
122 }
123
124 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
125 dJointAttach (c,b1,b2);
126 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
127 }
128 }
129}
130
131
132// start simulation - set viewpoint
133
134static void start()
135{
136 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
137 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
138 dsSetViewpoint (xyz,hpr);
139 printf ("To drop another object, press:\n");
140 printf (" b for box.\n");
141 printf (" s for sphere.\n");
142 printf (" c for cylinder.\n");
143 printf (" x for a composite object.\n");
144 printf ("To select an object, press space.\n");
145 printf ("To disable the selected object, press d.\n");
146 printf ("To enable the selected object, press e.\n");
147 printf ("To toggle showing the geom AABBs, press a.\n");
148 printf ("To toggle showing the contact points, press t.\n");
149 printf ("To toggle dropping from random position/orientation, press r.\n");
150}
151
152
153char locase (char c)
154{
155 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
156 else return c;
157}
158
159
160// called when a key pressed
161
162static void command (int cmd)
163{
164 int i,j,k;
165 dReal sides[3];
166 dMass m;
167
168 cmd = locase (cmd);
169 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
170 /* || cmd == 'l' */) {
171 if (num < NUM) {
172 i = num;
173 num++;
174 }
175 else {
176 i = nextobj;
177 nextobj++;
178 if (nextobj >= num) nextobj = 0;
179
180 // destroy the body and geoms for slot i
181 dBodyDestroy (obj[i].body);
182 for (k=0; k < GPB; k++) {
183 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
184 }
185 memset (&obj[i],0,sizeof(obj[i]));
186 }
187
188 obj[i].body = dBodyCreate (world);
189 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
190
191 dMatrix3 R;
192 if (random_pos) {
193 dBodySetPosition (obj[i].body,
194 dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
195 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
196 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
197 }
198 else {
199 dReal maxheight = 0;
200 for (k=0; k<num; k++) {
201 const dReal *pos = dBodyGetPosition (obj[k].body);
202 if (pos[2] > maxheight) maxheight = pos[2];
203 }
204 dBodySetPosition (obj[i].body, 0,0,maxheight+1);
205 dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
206 }
207 dBodySetRotation (obj[i].body,R);
208 dBodySetData (obj[i].body,(void*)(size_t)i);
209
210 if (cmd == 'b') {
211 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
212 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
213 }
214 else if (cmd == 'c') {
215 sides[0] *= 0.5;
216 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
217 obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
218 }
219/*
220 // cylinder option not yet implemented
221 else if (cmd == 'l') {
222 sides[1] *= 0.5;
223 dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
224 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
225 }
226*/
227 else if (cmd == 's') {
228 sides[0] *= 0.5;
229 dMassSetSphere (&m,DENSITY,sides[0]);
230 obj[i].geom[0] = dCreateSphere (space,sides[0]);
231 }
232 else if (cmd == 'x') {
233 dGeomID g2[GPB]; // encapsulated geometries
234 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
235
236 // start accumulating masses for the encapsulated geometries
237 dMass m2;
238 dMassSetZero (&m);
239
240 // set random delta positions
241 for (j=0; j<GPB; j++) {
242 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
243 }
244
245 for (k=0; k<GPB; k++) {
246 obj[i].geom[k] = dCreateGeomTransform (space);
247 dGeomTransformSetCleanup (obj[i].geom[k],1);
248 if (k==0) {
249 dReal radius = dRandReal()*0.25+0.05;
250 g2[k] = dCreateSphere (0,radius);
251 dMassSetSphere (&m2,DENSITY,radius);
252 }
253 else if (k==1) {
254 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
255 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
256 }
257 else {
258 dReal radius = dRandReal()*0.1+0.05;
259 dReal length = dRandReal()*1.0+0.1;
260 g2[k] = dCreateCapsule (0,radius,length);
261 dMassSetCapsule (&m2,DENSITY,3,radius,length);
262 }
263 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
264
265 // set the transformation (adjust the mass too)
266 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
267 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
268 dMatrix3 Rtx;
269 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
270 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
271 dGeomSetRotation (g2[k],Rtx);
272 dMassRotate (&m2,Rtx);
273
274 // add to the total mass
275 dMassAdd (&m,&m2);
276 }
277
278 // move all encapsulated objects so that the center of mass is (0,0,0)
279 for (k=0; k<2; k++) {
280 dGeomSetPosition (g2[k],
281 dpos[k][0]-m.c[0],
282 dpos[k][1]-m.c[1],
283 dpos[k][2]-m.c[2]);
284 }
285 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
286 }
287
288 for (k=0; k < GPB; k++) {
289 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
290 }
291
292 dBodySetMass (obj[i].body,&m);
293 }
294
295 if (cmd == ' ') {
296 selected++;
297 if (selected >= num) selected = 0;
298 if (selected < 0) selected = 0;
299 }
300 else if (cmd == 'd' && selected >= 0 && selected < num) {
301 dBodyDisable (obj[selected].body);
302 }
303 else if (cmd == 'e' && selected >= 0 && selected < num) {
304 dBodyEnable (obj[selected].body);
305 }
306 else if (cmd == 'a') {
307 show_aabb ^= 1;
308 }
309 else if (cmd == 't') {
310 show_contacts ^= 1;
311 }
312 else if (cmd == 'r') {
313 random_pos ^= 1;
314 }
315}
316
317
318// draw a geom
319
320void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
321{
322 if (!g) return;
323 if (!pos) pos = dGeomGetPosition (g);
324 if (!R) R = dGeomGetRotation (g);
325
326 int type = dGeomGetClass (g);
327 if (type == dBoxClass) {
328 dVector3 sides;
329 dGeomBoxGetLengths (g,sides);
330 dsDrawBox (pos,R,sides);
331 }
332 else if (type == dSphereClass) {
333 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
334 }
335 else if (type == dCapsuleClass) {
336 dReal radius,length;
337 dGeomCapsuleGetParams (g,&radius,&length);
338 dsDrawCapsule (pos,R,length,radius);
339 }
340/*
341 // cylinder option not yet implemented
342 else if (type == dCylinderClass) {
343 dReal radius,length;
344 dGeomCylinderGetParams (g,&radius,&length);
345 dsDrawCylinder (pos,R,length,radius);
346 }
347*/
348 else if (type == dGeomTransformClass) {
349 dGeomID g2 = dGeomTransformGetGeom (g);
350 const dReal *pos2 = dGeomGetPosition (g2);
351 const dReal *R2 = dGeomGetRotation (g2);
352 dVector3 actual_pos;
353 dMatrix3 actual_R;
354 dMULTIPLY0_331 (actual_pos,R,pos2);
355 actual_pos[0] += pos[0];
356 actual_pos[1] += pos[1];
357 actual_pos[2] += pos[2];
358 dMULTIPLY0_333 (actual_R,R,R2);
359 drawGeom (g2,actual_pos,actual_R,0);
360 }
361
362 if (show_aabb) {
363 // draw the bounding box for this geom
364 dReal aabb[6];
365 dGeomGetAABB (g,aabb);
366 dVector3 bbpos;
367 for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
368 dVector3 bbsides;
369 for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
370 dMatrix3 RI;
371 dRSetIdentity (RI);
372 dsSetColorAlpha (1,0,0,0.5);
373 dsDrawBox (bbpos,RI,bbsides);
374 }
375}
376
377
378// simulation loop
379
380static void simLoop (int pause)
381{
382 dsSetColor (0,0,2);
383 dSpaceCollide (space,0,&nearCallback);
384 if (!pause) dWorldStep (world,0.05);
385 //if (!pause) dWorldStepFast (world,0.05, 1);
386
387 // remove all contact joints
388 dJointGroupEmpty (contactgroup);
389
390 dsSetColor (1,1,0);
391 dsSetTexture (DS_WOOD);
392 for (int i=0; i<num; i++) {
393 for (int j=0; j < GPB; j++) {
394 if (i==selected) {
395 dsSetColor (0,0.7,1);
396 }
397 else if (! dBodyIsEnabled (obj[i].body)) {
398 dsSetColor (1,0,0);
399 }
400 else {
401 dsSetColor (1,1,0);
402 }
403 drawGeom (obj[i].geom[j],0,0,show_aabb);
404 }
405 }
406
407 /*{
408 for (int i = 1; i < IndexCount; i++) {
409 dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
410 }
411 }*/
412
413 {const dReal* Pos = dGeomGetPosition(TriMesh);
414 const dReal* Rot = dGeomGetRotation(TriMesh);
415
416 {for (int i = 0; i < IndexCount / 3; i++){
417 const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
418 const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
419 const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
420 dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
421 }}}
422
423 if (Ray){
424 dVector3 Origin, Direction;
425 dGeomRayGet(Ray, Origin, Direction);
426
427 dReal Length = dGeomRayGetLength(Ray);
428
429 dVector3 End;
430 End[0] = Origin[0] + (Direction[0] * Length);
431 End[1] = Origin[1] + (Direction[1] * Length);
432 End[2] = Origin[2] + (Direction[2] * Length);
433 End[3] = Origin[3] + (Direction[3] * Length);
434
435 dsDrawLine(Origin, End);
436 }
437}
438
439
440int main (int argc, char **argv)
441{
442 // setup pointers to drawstuff callback functions
443 dsFunctions fn;
444 fn.version = DS_VERSION;
445 fn.start = &start;
446 fn.step = &simLoop;
447 fn.command = &command;
448 fn.stop = 0;
449 fn.path_to_textures = "../../drawstuff/textures";
450 if(argc==2)
451 {
452 fn.path_to_textures = argv[1];
453 }
454
455 // create world
456 dInitODE();
457 world = dWorldCreate();
458
459 space = dSimpleSpaceCreate(0);
460 contactgroup = dJointGroupCreate (0);
461 dWorldSetGravity (world,0,0,-0.5);
462 dWorldSetCFM (world,1e-5);
463 //dCreatePlane (space,0,0,1,0);
464 memset (obj,0,sizeof(obj));
465
466 Size[0] = 5.0f;
467 Size[1] = 5.0f;
468 Size[2] = 2.5f;
469
470 Vertices[0][0] = -Size[0];
471 Vertices[0][1] = -Size[1];
472 Vertices[0][2] = Size[2];
473
474 Vertices[1][0] = Size[0];
475 Vertices[1][1] = -Size[1];
476 Vertices[1][2] = Size[2];
477
478 Vertices[2][0] = Size[0];
479 Vertices[2][1] = Size[1];
480 Vertices[2][2] = Size[2];
481
482 Vertices[3][0] = -Size[0];
483 Vertices[3][1] = Size[1];
484 Vertices[3][2] = Size[2];
485
486 Vertices[4][0] = 0;
487 Vertices[4][1] = 0;
488 Vertices[4][2] = 0;
489
490 Indices[0] = 0;
491 Indices[1] = 1;
492 Indices[2] = 4;
493
494 Indices[3] = 1;
495 Indices[4] = 2;
496 Indices[5] = 4;
497
498 Indices[6] = 2;
499 Indices[7] = 3;
500 Indices[8] = 4;
501
502 Indices[9] = 3;
503 Indices[10] = 0;
504 Indices[11] = 4;
505
506 dTriMeshDataID Data = dGeomTriMeshDataCreate();
507
508 dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount);
509
510 TriMesh = dCreateTriMesh(space, Data, 0, 0, 0);
511
512 //dGeomSetPosition(TriMesh, 0, 0, 1.0);
513
514 Ray = dCreateRay(space, 0.9);
515 dVector3 Origin, Direction;
516 Origin[0] = 0.0;
517 Origin[1] = 0;
518 Origin[2] = 0.5;
519 Origin[3] = 0;
520
521 Direction[0] = 0;
522 Direction[1] = 1.1f;
523 Direction[2] = -1;
524 Direction[3] = 0;
525 dNormalize3(Direction);
526
527 dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
528
529 // run simulation
530 dsSimulationLoop (argc,argv,352,288,&fn);
531
532 dJointGroupDestroy (contactgroup);
533 dSpaceDestroy (space);
534 dWorldDestroy (world);
535 dCloseODE();
536 return 0;
537}
diff --git a/libraries/ode-0.9/ode/demo/world_geom3.h b/libraries/ode-0.9/ode/demo/world_geom3.h
new file mode 100644
index 0000000..0b94a5e
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/world_geom3.h
@@ -0,0 +1,9 @@
1// mesh for a world model, to be used with test_cyl.cpp
2
3static float world_vertices[] = {10.000000f,-10.000000f,1.000000f,-10.000000f,-10.000000f,1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,10.000000f,-10.000000f,-1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,-10.000000f,-1.000000f,10.000000f,-10.000000f,-1.000000f,10.000000f,10.000000f,-1.000000f,10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,-1.000000f,10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,10.000000f,-1.000000f,10.000000f,10.000000f,-1.000000f,0.000000f,9.000000f,-0.000000f,0.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,0.000000f,0.000000f,9.000000f,-0.000000f,9.000000f,-9.000000f,0.000000f,9.000000f,9.000000f,-0.000000f,10.000000f,10.000000f,-1.000000f,-10.000000f,10.000000f,-1.000000f,-10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,-1.000000f,-10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,-10.000000f,-10.000000f,-1.000000f,-10.000000f,-10.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-10.000000f,10.000000f,-1.000000f,-10.000000f,-10.000000f,-1.000000f,9.000000f,-9.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,-10.000000f,-10.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,9.000000f,9.000000f,1.000000f,9.000000f,-9.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,-10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,9.000000f,9.000000f,1.000000f,10.000000f,10.000000f,1.000000f,10.000000f,10.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,-10.000000f,10.000000f,1.000000f,-10.000000f,-10.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,0.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,1.000000f,0.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,1.000000f,9.000000f,-9.000000f,1.000000f,0.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,1.000000f,9.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,0.000000f,9.000000f,-9.000000f,1.000000f,9.000000f,9.000000f,1.000000f,9.000000f,9.000000f,1.000000f,9.000000f,9.000000f,-0.000000f,9.000000f,-9.000000f,0.000000f,0.000000f,9.000000f,-0.000000f,9.000000f,9.000000f,-0.000000f,9.000000f,9.000000f,1.000000f,0.000000f,9.000000f,-0.000000f,9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,1.000000f,0.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,1.000000f,-9.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,1.000000f,-9.000000f,-9.000000f,1.000000f,-9.000000f,-9.000000f,0.000000f,-9.000000f,-9.000000f,0.000000f,-9.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,1.000000f,-2.997000f,-1.748874f,0.000000f,-2.997000f,-2.001000f,0.000000f,0.000000f,-9.000000f,0.000000f,-2.997000f,-1.748874f,0.000000f,0.000000f,-9.000000f,0.000000f,-2.997000f,1.748874f,-0.000000f,-2.997000f,-2.001000f,0.000000f,-2.997000f,-6.003000f,0.002697f,0.000000f,-9.000000f,0.000000f,0.000000f,9.000000f,-0.000000f,-2.997000f,2.001000f,-0.000000f,-2.997000f,1.748874f,-0.000000f,0.000000f,9.000000f,-0.000000f,-2.997000f,1.748874f,-0.000000f,0.000000f,-9.000000f,0.000000f,-2.997000f,2.001000f,-0.000000f,0.000000f,9.000000f,-0.000000f,-2.997000f,6.003000f,0.002697f,-6.003000f,6.003000f,0.002697f,-2.997000f,6.003000f,0.002697f,0.000000f,9.000000f,-0.000000f,0.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,-0.000000f,-6.003000f,6.003000f,0.002697f,-6.003000f,1.748874f,-0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,2.001000f,-0.000000f,-6.003000f,6.003000f,0.002697f,-9.000000f,9.000000f,-0.000000f,-9.000000f,9.000000f,-0.000000f,-6.003000f,1.748874f,-0.000000f,-6.003000f,2.001000f,-0.000000f,-9.000000f,9.000000f,-0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,1.748874f,-0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,-6.003000f,0.002697f,-6.003000f,-2.001000f,0.000000f,-9.000000f,-9.000000f,0.000000f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-6.003000f,0.002697f,-9.000000f,-9.000000f,0.000000f,0.000000f,-9.000000f,0.000000f,-6.003000f,-6.003000f,0.002697f,0.000000f,-9.000000f,0.000000f,-2.997000f,-6.003000f,0.002697f,-2.997000f,1.748874f,1.237951f,-2.997000f,1.748874f,-0.000000f,-2.997000f,2.001000f,-0.000000f,-2.997000f,1.748874f,1.237951f,-2.997000f,2.001000f,-0.000000f,-2.997000f,2.001000f,1.515748f,-6.003000f,-2.001000f,1.515748f,-6.003000f,-6.003000f,0.002697f,-2.997000f,-6.003000f,0.002697f,-6.003000f,-2.001000f,1.515748f,-2.997000f,-6.003000f,0.002697f,-2.997000f,-2.001000f,1.515748f,-2.997000f,2.001000f,1.515748f,-2.997000f,6.003000f,0.002697f,-6.003000f,6.003000f,0.002697f,-6.003000f,6.003000f,0.002697f,-6.003000f,2.001000f,1.515748f,-2.997000f,2.001000f,1.515748f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-6.003000f,0.002697f,-6.003000f,-2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,6.003000f,0.002697f,-6.003000f,2.001000f,-0.000000f,-2.997000f,-2.001000f,1.515748f,-2.997000f,-6.003000f,0.002697f,-2.997000f,-2.001000f,0.000000f,-2.997000f,2.001000f,-0.000000f,-2.997000f,6.003000f,0.002697f,-2.997000f,2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-2.997000f,2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,-2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-2.997000f,-1.748874f,1.237951f,-2.997000f,1.748874f,1.237951f,-2.997000f,2.001000f,1.515748f,-2.997000f,-1.748874f,1.237951f,-2.997000f,2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-6.003000f,-1.748874f,1.237951f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-1.748874f,1.237951f,-6.003000f,-2.001000f,0.000000f,-6.003000f,-2.001000f,1.515748f,-2.997000f,-2.001000f,1.515748f,-2.997000f,-2.001000f,0.000000f,-2.997000f,-1.748874f,1.237951f,-2.997000f,-2.001000f,0.000000f,-2.997000f,-1.748874f,0.000000f,-2.997000f,-1.748874f,1.237951f,-6.003000f,1.748874f,1.237951f,-6.003000f,2.001000f,1.515748f,-6.003000f,2.001000f,-0.000000f,-6.003000f,1.748874f,1.237951f,-6.003000f,2.001000f,-0.000000f,-6.003000f,1.748874f,-0.000000f,-6.003000f,1.748874f,1.237951f,-6.003000f,-1.748874f,1.237951f,-6.003000f,-2.001000f,1.515748f,-6.003000f,1.748874f,1.237951f,-6.003000f,-2.001000f,1.515748f,-6.003000f,2.001000f,1.515748f,-6.003000f,1.748874f,1.237951f,-6.003000f,1.748874f,-0.000000f,-2.997000f,1.748874f,1.237951f,-6.003000f,1.748874f,-0.000000f,-2.997000f,1.748874f,-0.000000f,-2.997000f,1.748874f,1.237951f,-6.003000f,1.748874f,-0.000000f,-6.003000f,-1.748874f,0.000000f,-2.997000f,-1.748874f,0.000000f,-6.003000f,1.748874f,-0.000000f,-2.997000f,-1.748874f,0.000000f,-2.997000f,1.748874f,-0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f,-2.997000f,-1.748874f,0.000000f,-6.003000f,-1.748874f,0.000000f,-6.003000f,-1.748874f,1.237951f,-6.003000f,1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f,-6.003000f,1.748874f,1.237951f,-2.997000f,1.748874f,1.237951f,-2.997000f,-1.748874f,1.237951f};
4
5static float world_normals[] = {0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000225f,0.000161f,1.000000f,0.000225f,-0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,0.000225f,0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,-0.000000f,0.000000f,1.000000f,0.000787f,0.000337f,1.000000f,0.000225f,-0.000161f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000225f,-0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,0.000787f,0.000337f,1.000000f,0.000400f,-0.179805f,0.983702f,0.000225f,-0.000161f,1.000000f,0.000225f,0.000161f,1.000000f,0.000787f,-0.000337f,1.000000f,0.000000f,0.000000f,1.000000f,0.000225f,0.000161f,1.000000f,0.000000f,0.000000f,1.000000f,0.000225f,-0.000161f,1.000000f,0.000787f,-0.000337f,1.000000f,0.000225f,0.000161f,1.000000f,0.000532f,0.119686f,0.992812f,-0.000320f,0.143927f,0.989588f,0.000532f,0.119686f,0.992812f,0.000225f,0.000161f,1.000000f,0.000225f,0.000161f,1.000000f,-0.000393f,0.000056f,1.000000f,-0.000320f,0.143927f,0.989588f,-0.000000f,0.000000f,1.000000f,-0.000315f,-0.000045f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000787f,-0.000337f,1.000000f,-0.000320f,0.143927f,0.989588f,-0.000393f,0.000056f,1.000000f,-0.000393f,0.000056f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000787f,-0.000337f,1.000000f,-0.000393f,0.000056f,1.000000f,-0.000315f,-0.000045f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000315f,-0.000045f,1.000000f,-0.000398f,-0.089784f,0.995961f,-0.000787f,0.000337f,1.000000f,-0.000315f,-0.000045f,1.000000f,-0.000787f,0.000337f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000398f,-0.089784f,0.995961f,-0.000315f,-0.000045f,1.000000f,0.000225f,-0.000161f,1.000000f,-0.000398f,-0.089784f,0.995961f,0.000225f,-0.000161f,1.000000f,0.000400f,-0.179805f,0.983702f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,-0.239222f,0.970965f,-0.000398f,-0.089784f,0.995961f,0.000400f,-0.179805f,0.983702f,0.000000f,-0.239222f,0.970965f,0.000400f,-0.179805f,0.983702f,0.000000f,-0.119611f,0.992821f,0.000000f,0.239222f,0.970965f,0.000532f,0.119686f,0.992812f,-0.000320f,0.143927f,0.989588f,-0.000320f,0.143927f,0.989588f,0.000000f,0.119611f,0.992821f,0.000000f,0.239222f,0.970965f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,-0.119611f,0.992821f,0.000000f,0.239222f,0.970965f,0.000000f,0.119611f,0.992821f,0.000000f,0.119611f,0.992821f,0.000000f,-0.239222f,0.970965f,0.000000f,-0.119611f,0.992821f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,-0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,1.000000f,0.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f,0.000000f,0.000000f,-1.000000f};
6
7
8static int world_indices[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223,224,225,226,227};
9