From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/demo/Makefile.am | 75 + libs/ode-0.16.1/ode/demo/Makefile.in | 1133 +++++++++++ libs/ode-0.16.1/ode/demo/basket_geom.h | 599 ++++++ libs/ode-0.16.1/ode/demo/bunny_geom.h | 1366 +++++++++++++ libs/ode-0.16.1/ode/demo/convex_bunny_geom.h | 468 +++++ libs/ode-0.16.1/ode/demo/convex_prism.h | 28 + libs/ode-0.16.1/ode/demo/demo_I.cpp | 253 +++ libs/ode-0.16.1/ode/demo/demo_basket.cpp | 276 +++ libs/ode-0.16.1/ode/demo/demo_boxstack.cpp | 619 ++++++ libs/ode-0.16.1/ode/demo/demo_buggy.cpp | 308 +++ libs/ode-0.16.1/ode/demo/demo_cards.cpp | 237 +++ libs/ode-0.16.1/ode/demo/demo_chain1.c | 171 ++ libs/ode-0.16.1/ode/demo/demo_chain2.cpp | 165 ++ libs/ode-0.16.1/ode/demo/demo_collision.cpp | 1463 ++++++++++++++ libs/ode-0.16.1/ode/demo/demo_convex.cpp | 307 +++ libs/ode-0.16.1/ode/demo/demo_crash.cpp | 652 +++++++ libs/ode-0.16.1/ode/demo/demo_cyl.cpp | 321 +++ libs/ode-0.16.1/ode/demo/demo_cylvssphere.cpp | 240 +++ libs/ode-0.16.1/ode/demo/demo_dball.cpp | 194 ++ libs/ode-0.16.1/ode/demo/demo_dhinge.cpp | 217 +++ libs/ode-0.16.1/ode/demo/demo_feedback.cpp | 312 +++ libs/ode-0.16.1/ode/demo/demo_friction.cpp | 205 ++ libs/ode-0.16.1/ode/demo/demo_gyro2.cpp | 210 ++ libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp | 258 +++ libs/ode-0.16.1/ode/demo/demo_heightfield.cpp | 714 +++++++ libs/ode-0.16.1/ode/demo/demo_hinge.cpp | 165 ++ libs/ode-0.16.1/ode/demo/demo_jointPR.cpp | 434 +++++ libs/ode-0.16.1/ode/demo/demo_jointPU.cpp | 735 +++++++ libs/ode-0.16.1/ode/demo/demo_joints.cpp | 1090 +++++++++++ libs/ode-0.16.1/ode/demo/demo_kinematic.cpp | 239 +++ libs/ode-0.16.1/ode/demo/demo_motion.cpp | 527 +++++ libs/ode-0.16.1/ode/demo/demo_motor.cpp | 209 ++ libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp | 415 ++++ libs/ode-0.16.1/ode/demo/demo_moving_trimesh.cpp | 677 +++++++ libs/ode-0.16.1/ode/demo/demo_ode.cpp | 1380 +++++++++++++ libs/ode-0.16.1/ode/demo/demo_piston.cpp | 813 ++++++++ libs/ode-0.16.1/ode/demo/demo_plane2d.cpp | 304 +++ libs/ode-0.16.1/ode/demo/demo_rfriction.cpp | 258 +++ libs/ode-0.16.1/ode/demo/demo_slider.cpp | 171 ++ libs/ode-0.16.1/ode/demo/demo_space.cpp | 232 +++ libs/ode-0.16.1/ode/demo/demo_space_stress.cpp | 449 +++++ libs/ode-0.16.1/ode/demo/demo_step.cpp | 192 ++ libs/ode-0.16.1/ode/demo/demo_tracks.cpp | 498 +++++ libs/ode-0.16.1/ode/demo/demo_transmission.cpp | 414 ++++ libs/ode-0.16.1/ode/demo/demo_trimesh.cpp | 605 ++++++ libs/ode-0.16.1/ode/demo/halton235_geom.h | 2271 ++++++++++++++++++++++ libs/ode-0.16.1/ode/demo/icosahedron_geom.h | 216 ++ libs/ode-0.16.1/ode/demo/texturepath.h | 26 + libs/ode-0.16.1/ode/demo/world_geom3.h | 9 + 49 files changed, 23120 insertions(+) create mode 100644 libs/ode-0.16.1/ode/demo/Makefile.am create mode 100644 libs/ode-0.16.1/ode/demo/Makefile.in create mode 100644 libs/ode-0.16.1/ode/demo/basket_geom.h create mode 100644 libs/ode-0.16.1/ode/demo/bunny_geom.h create mode 100644 libs/ode-0.16.1/ode/demo/convex_bunny_geom.h create mode 100644 libs/ode-0.16.1/ode/demo/convex_prism.h create mode 100644 libs/ode-0.16.1/ode/demo/demo_I.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_basket.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_boxstack.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_buggy.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_cards.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_chain1.c create mode 100644 libs/ode-0.16.1/ode/demo/demo_chain2.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_collision.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_convex.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_crash.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_cyl.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_cylvssphere.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_dball.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_dhinge.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_feedback.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_friction.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_gyro2.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_heightfield.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_hinge.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_jointPR.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_jointPU.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_joints.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_kinematic.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_motion.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_motor.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_moving_trimesh.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_ode.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_piston.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_plane2d.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_rfriction.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_slider.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_space.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_space_stress.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_step.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_tracks.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_transmission.cpp create mode 100644 libs/ode-0.16.1/ode/demo/demo_trimesh.cpp create mode 100644 libs/ode-0.16.1/ode/demo/halton235_geom.h create mode 100644 libs/ode-0.16.1/ode/demo/icosahedron_geom.h create mode 100644 libs/ode-0.16.1/ode/demo/texturepath.h create mode 100644 libs/ode-0.16.1/ode/demo/world_geom3.h (limited to 'libs/ode-0.16.1/ode/demo') diff --git a/libs/ode-0.16.1/ode/demo/Makefile.am b/libs/ode-0.16.1/ode/demo/Makefile.am new file mode 100644 index 0000000..5d02b87 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/Makefile.am @@ -0,0 +1,75 @@ +AM_CPPFLAGS = -I$(top_srcdir)/include \ + -I$(top_builddir)/include \ + -DDRAWSTUFF_TEXTURE_PATH="\"$(abs_top_srcdir)/drawstuff/textures\"" + +if X11 +AM_LDFLAGS = $(X_PRE_LIBS) $(X_LIBS) $(X_EXTRA_LIBS) +endif + +# On Windows, GL_LIBS must go after libdrawstuff.la. +LDADD = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la @GL_LIBS@ + +noinst_HEADERS = basket_geom.h bunny_geom.h convex_bunny_geom.h convex_prism.h \ + icosahedron_geom.h halton235_geom.h texturepath.h world_geom3.h + +AM_DEFAULT_SOURCE_EXT = .cpp + +noinst_PROGRAMS = \ + demo_boxstack \ + demo_buggy \ + demo_cards \ + demo_chain1 \ + demo_chain2 \ + demo_collision \ + demo_convex \ + demo_crash \ + demo_cylvssphere \ + demo_dball \ + demo_dhinge \ + demo_transmission \ + demo_feedback \ + demo_friction \ + demo_gyroscopic \ + demo_gyro2 \ + demo_heightfield \ + demo_hinge \ + demo_I \ + demo_jointPR \ + demo_joints \ + demo_jointPU \ + demo_kinematic \ + demo_motion \ + demo_motor \ + demo_ode \ + demo_piston \ + demo_plane2d \ + demo_rfriction \ + demo_slider \ + demo_space \ + demo_space_stress \ + demo_step \ + demo_tracks + +demo_chain1_SOURCES = demo_chain1.c +demo_chain1_LDADD = $(LDADD) -lstdc++ + + +if TRIMESH +noinst_PROGRAMS += \ + demo_basket \ + demo_cyl \ + demo_moving_trimesh \ + demo_moving_convex \ + demo_trimesh + +AM_CPPFLAGS += -DdTRIMESH_ENABLED +endif + + + +if WIN32 +resources.o: $(top_srcdir)/drawstuff/src/resources.rc $(top_srcdir)/drawstuff/src/resource.h + @WINDRES@ $(top_srcdir)/drawstuff/src/resources.rc -o resources.o +LDADD += resources.o +endif diff --git a/libs/ode-0.16.1/ode/demo/Makefile.in b/libs/ode-0.16.1/ode/demo/Makefile.in new file mode 100644 index 0000000..368a5d1 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/Makefile.in @@ -0,0 +1,1133 @@ +# Makefile.in generated by automake 1.15 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994-2014 Free Software Foundation, Inc. + +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ + + +VPATH = @srcdir@ +am__is_gnu_make = { \ + if test -z '$(MAKELEVEL)'; then \ + false; \ + elif test -n '$(MAKE_HOST)'; then \ + true; \ + elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \ + true; \ + else \ + false; \ + fi; \ +} +am__make_running_with_option = \ + case $${target_option-} in \ + ?) ;; \ + *) echo "am__make_running_with_option: internal error: invalid" \ + "target option '$${target_option-}' specified" >&2; \ + exit 1;; \ + esac; \ + has_opt=no; \ + sane_makeflags=$$MAKEFLAGS; \ + if $(am__is_gnu_make); then \ + sane_makeflags=$$MFLAGS; \ + else \ + case $$MAKEFLAGS in \ + *\\[\ \ ]*) \ + bs=\\; \ + sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \ + | sed "s/$$bs$$bs[$$bs $$bs ]*//g"`;; \ + esac; \ + fi; \ + skip_next=no; \ + strip_trailopt () \ + { \ + flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \ + }; \ + for flg in $$sane_makeflags; do \ + test $$skip_next = yes && { skip_next=no; continue; }; \ + case $$flg in \ + *=*|--*) continue;; \ + -*I) strip_trailopt 'I'; skip_next=yes;; \ + -*I?*) strip_trailopt 'I';; \ + -*O) strip_trailopt 'O'; skip_next=yes;; \ + -*O?*) strip_trailopt 'O';; \ + -*l) strip_trailopt 'l'; skip_next=yes;; \ + -*l?*) strip_trailopt 'l';; \ + -[dEDm]) skip_next=yes;; \ + -[JT]) skip_next=yes;; \ + esac; \ + case $$flg in \ + *$$target_option*) has_opt=yes; break;; \ + esac; \ + done; \ + test $$has_opt = yes +am__make_dryrun = (target_option=n; $(am__make_running_with_option)) +am__make_keepgoing = (target_option=k; $(am__make_running_with_option)) +pkgdatadir = $(datadir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkglibexecdir = $(libexecdir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +build_triplet = @build@ +host_triplet = @host@ +noinst_PROGRAMS = demo_boxstack$(EXEEXT) demo_buggy$(EXEEXT) \ + demo_cards$(EXEEXT) demo_chain1$(EXEEXT) demo_chain2$(EXEEXT) \ + demo_collision$(EXEEXT) demo_convex$(EXEEXT) \ + demo_crash$(EXEEXT) demo_cylvssphere$(EXEEXT) \ + demo_dball$(EXEEXT) demo_dhinge$(EXEEXT) \ + demo_transmission$(EXEEXT) demo_feedback$(EXEEXT) \ + demo_friction$(EXEEXT) demo_gyroscopic$(EXEEXT) \ + demo_gyro2$(EXEEXT) demo_heightfield$(EXEEXT) \ + demo_hinge$(EXEEXT) demo_I$(EXEEXT) demo_jointPR$(EXEEXT) \ + demo_joints$(EXEEXT) demo_jointPU$(EXEEXT) \ + demo_kinematic$(EXEEXT) demo_motion$(EXEEXT) \ + demo_motor$(EXEEXT) demo_ode$(EXEEXT) demo_piston$(EXEEXT) \ + demo_plane2d$(EXEEXT) demo_rfriction$(EXEEXT) \ + demo_slider$(EXEEXT) demo_space$(EXEEXT) \ + demo_space_stress$(EXEEXT) demo_step$(EXEEXT) \ + demo_tracks$(EXEEXT) $(am__EXEEXT_1) +@TRIMESH_TRUE@am__append_1 = \ +@TRIMESH_TRUE@ demo_basket \ +@TRIMESH_TRUE@ demo_cyl \ +@TRIMESH_TRUE@ demo_moving_trimesh \ +@TRIMESH_TRUE@ demo_moving_convex \ +@TRIMESH_TRUE@ demo_trimesh + +@TRIMESH_TRUE@am__append_2 = -DdTRIMESH_ENABLED +@WIN32_TRUE@am__append_3 = resources.o +subdir = ode/demo +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \ + $(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \ + $(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \ + $(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +DIST_COMMON = $(srcdir)/Makefile.am $(noinst_HEADERS) \ + $(am__DIST_COMMON) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/ode/src/config.h +CONFIG_CLEAN_FILES = +CONFIG_CLEAN_VPATH_FILES = +@TRIMESH_TRUE@am__EXEEXT_1 = demo_basket$(EXEEXT) demo_cyl$(EXEEXT) \ +@TRIMESH_TRUE@ demo_moving_trimesh$(EXEEXT) \ +@TRIMESH_TRUE@ demo_moving_convex$(EXEEXT) \ +@TRIMESH_TRUE@ demo_trimesh$(EXEEXT) +PROGRAMS = $(noinst_PROGRAMS) +demo_I_SOURCES = demo_I.cpp +demo_I_OBJECTS = demo_I.$(OBJEXT) +demo_I_LDADD = $(LDADD) +demo_I_DEPENDENCIES = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +AM_V_lt = $(am__v_lt_@AM_V@) +am__v_lt_ = $(am__v_lt_@AM_DEFAULT_V@) +am__v_lt_0 = --silent +am__v_lt_1 = +demo_basket_SOURCES = demo_basket.cpp +demo_basket_OBJECTS = demo_basket.$(OBJEXT) +demo_basket_LDADD = $(LDADD) +demo_basket_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_boxstack_SOURCES = demo_boxstack.cpp +demo_boxstack_OBJECTS = demo_boxstack.$(OBJEXT) +demo_boxstack_LDADD = $(LDADD) +demo_boxstack_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_buggy_SOURCES = demo_buggy.cpp +demo_buggy_OBJECTS = demo_buggy.$(OBJEXT) +demo_buggy_LDADD = $(LDADD) +demo_buggy_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_cards_SOURCES = demo_cards.cpp +demo_cards_OBJECTS = demo_cards.$(OBJEXT) +demo_cards_LDADD = $(LDADD) +demo_cards_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +am_demo_chain1_OBJECTS = demo_chain1.$(OBJEXT) +demo_chain1_OBJECTS = $(am_demo_chain1_OBJECTS) +am__DEPENDENCIES_1 = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_chain1_DEPENDENCIES = $(am__DEPENDENCIES_1) +demo_chain2_SOURCES = demo_chain2.cpp +demo_chain2_OBJECTS = demo_chain2.$(OBJEXT) +demo_chain2_LDADD = $(LDADD) +demo_chain2_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_collision_SOURCES = demo_collision.cpp +demo_collision_OBJECTS = demo_collision.$(OBJEXT) +demo_collision_LDADD = $(LDADD) +demo_collision_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_convex_SOURCES = demo_convex.cpp +demo_convex_OBJECTS = demo_convex.$(OBJEXT) +demo_convex_LDADD = $(LDADD) +demo_convex_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_crash_SOURCES = demo_crash.cpp +demo_crash_OBJECTS = demo_crash.$(OBJEXT) +demo_crash_LDADD = $(LDADD) +demo_crash_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_cyl_SOURCES = demo_cyl.cpp +demo_cyl_OBJECTS = demo_cyl.$(OBJEXT) +demo_cyl_LDADD = $(LDADD) +demo_cyl_DEPENDENCIES = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_cylvssphere_SOURCES = demo_cylvssphere.cpp +demo_cylvssphere_OBJECTS = demo_cylvssphere.$(OBJEXT) +demo_cylvssphere_LDADD = $(LDADD) +demo_cylvssphere_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_dball_SOURCES = demo_dball.cpp +demo_dball_OBJECTS = demo_dball.$(OBJEXT) +demo_dball_LDADD = $(LDADD) +demo_dball_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_dhinge_SOURCES = demo_dhinge.cpp +demo_dhinge_OBJECTS = demo_dhinge.$(OBJEXT) +demo_dhinge_LDADD = $(LDADD) +demo_dhinge_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_feedback_SOURCES = demo_feedback.cpp +demo_feedback_OBJECTS = demo_feedback.$(OBJEXT) +demo_feedback_LDADD = $(LDADD) +demo_feedback_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_friction_SOURCES = demo_friction.cpp +demo_friction_OBJECTS = demo_friction.$(OBJEXT) +demo_friction_LDADD = $(LDADD) +demo_friction_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_gyro2_SOURCES = demo_gyro2.cpp +demo_gyro2_OBJECTS = demo_gyro2.$(OBJEXT) +demo_gyro2_LDADD = $(LDADD) +demo_gyro2_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_gyroscopic_SOURCES = demo_gyroscopic.cpp +demo_gyroscopic_OBJECTS = demo_gyroscopic.$(OBJEXT) +demo_gyroscopic_LDADD = $(LDADD) +demo_gyroscopic_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_heightfield_SOURCES = demo_heightfield.cpp +demo_heightfield_OBJECTS = demo_heightfield.$(OBJEXT) +demo_heightfield_LDADD = $(LDADD) +demo_heightfield_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_hinge_SOURCES = demo_hinge.cpp +demo_hinge_OBJECTS = demo_hinge.$(OBJEXT) +demo_hinge_LDADD = $(LDADD) +demo_hinge_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_jointPR_SOURCES = demo_jointPR.cpp +demo_jointPR_OBJECTS = demo_jointPR.$(OBJEXT) +demo_jointPR_LDADD = $(LDADD) +demo_jointPR_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_jointPU_SOURCES = demo_jointPU.cpp +demo_jointPU_OBJECTS = demo_jointPU.$(OBJEXT) +demo_jointPU_LDADD = $(LDADD) +demo_jointPU_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_joints_SOURCES = demo_joints.cpp +demo_joints_OBJECTS = demo_joints.$(OBJEXT) +demo_joints_LDADD = $(LDADD) +demo_joints_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_kinematic_SOURCES = demo_kinematic.cpp +demo_kinematic_OBJECTS = demo_kinematic.$(OBJEXT) +demo_kinematic_LDADD = $(LDADD) +demo_kinematic_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_motion_SOURCES = demo_motion.cpp +demo_motion_OBJECTS = demo_motion.$(OBJEXT) +demo_motion_LDADD = $(LDADD) +demo_motion_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_motor_SOURCES = demo_motor.cpp +demo_motor_OBJECTS = demo_motor.$(OBJEXT) +demo_motor_LDADD = $(LDADD) +demo_motor_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_moving_convex_SOURCES = demo_moving_convex.cpp +demo_moving_convex_OBJECTS = demo_moving_convex.$(OBJEXT) +demo_moving_convex_LDADD = $(LDADD) +demo_moving_convex_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_moving_trimesh_SOURCES = demo_moving_trimesh.cpp +demo_moving_trimesh_OBJECTS = demo_moving_trimesh.$(OBJEXT) +demo_moving_trimesh_LDADD = $(LDADD) +demo_moving_trimesh_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_ode_SOURCES = demo_ode.cpp +demo_ode_OBJECTS = demo_ode.$(OBJEXT) +demo_ode_LDADD = $(LDADD) +demo_ode_DEPENDENCIES = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_piston_SOURCES = demo_piston.cpp +demo_piston_OBJECTS = demo_piston.$(OBJEXT) +demo_piston_LDADD = $(LDADD) +demo_piston_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_plane2d_SOURCES = demo_plane2d.cpp +demo_plane2d_OBJECTS = demo_plane2d.$(OBJEXT) +demo_plane2d_LDADD = $(LDADD) +demo_plane2d_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_rfriction_SOURCES = demo_rfriction.cpp +demo_rfriction_OBJECTS = demo_rfriction.$(OBJEXT) +demo_rfriction_LDADD = $(LDADD) +demo_rfriction_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_slider_SOURCES = demo_slider.cpp +demo_slider_OBJECTS = demo_slider.$(OBJEXT) +demo_slider_LDADD = $(LDADD) +demo_slider_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_space_SOURCES = demo_space.cpp +demo_space_OBJECTS = demo_space.$(OBJEXT) +demo_space_LDADD = $(LDADD) +demo_space_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_space_stress_SOURCES = demo_space_stress.cpp +demo_space_stress_OBJECTS = demo_space_stress.$(OBJEXT) +demo_space_stress_LDADD = $(LDADD) +demo_space_stress_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_step_SOURCES = demo_step.cpp +demo_step_OBJECTS = demo_step.$(OBJEXT) +demo_step_LDADD = $(LDADD) +demo_step_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_tracks_SOURCES = demo_tracks.cpp +demo_tracks_OBJECTS = demo_tracks.$(OBJEXT) +demo_tracks_LDADD = $(LDADD) +demo_tracks_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_transmission_SOURCES = demo_transmission.cpp +demo_transmission_OBJECTS = demo_transmission.$(OBJEXT) +demo_transmission_LDADD = $(LDADD) +demo_transmission_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +demo_trimesh_SOURCES = demo_trimesh.cpp +demo_trimesh_OBJECTS = demo_trimesh.$(OBJEXT) +demo_trimesh_LDADD = $(LDADD) +demo_trimesh_DEPENDENCIES = \ + $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la $(am__append_3) +AM_V_P = $(am__v_P_@AM_V@) +am__v_P_ = $(am__v_P_@AM_DEFAULT_V@) +am__v_P_0 = false +am__v_P_1 = : +AM_V_GEN = $(am__v_GEN_@AM_V@) +am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@) +am__v_GEN_0 = @echo " GEN " $@; +am__v_GEN_1 = +AM_V_at = $(am__v_at_@AM_V@) +am__v_at_ = $(am__v_at_@AM_DEFAULT_V@) +am__v_at_0 = @ +am__v_at_1 = +DEFAULT_INCLUDES = -I.@am__isrc@ -I$(top_builddir)/ode/src +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +am__mv = mv -f +COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \ + $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) +LTCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CC) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CFLAGS) $(CFLAGS) +AM_V_CC = $(am__v_CC_@AM_V@) +am__v_CC_ = $(am__v_CC_@AM_DEFAULT_V@) +am__v_CC_0 = @echo " CC " $@; +am__v_CC_1 = +CCLD = $(CC) +LINK = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CCLD) $(AM_CFLAGS) $(CFLAGS) \ + $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CCLD = $(am__v_CCLD_@AM_V@) +am__v_CCLD_ = $(am__v_CCLD_@AM_DEFAULT_V@) +am__v_CCLD_0 = @echo " CCLD " $@; +am__v_CCLD_1 = +CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +LTCXXCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=compile $(CXX) $(DEFS) \ + $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \ + $(AM_CXXFLAGS) $(CXXFLAGS) +AM_V_CXX = $(am__v_CXX_@AM_V@) +am__v_CXX_ = $(am__v_CXX_@AM_DEFAULT_V@) +am__v_CXX_0 = @echo " CXX " $@; +am__v_CXX_1 = +CXXLD = $(CXX) +CXXLINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \ + $(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \ + $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@ +AM_V_CXXLD = $(am__v_CXXLD_@AM_V@) +am__v_CXXLD_ = $(am__v_CXXLD_@AM_DEFAULT_V@) +am__v_CXXLD_0 = @echo " CXXLD " $@; +am__v_CXXLD_1 = +SOURCES = demo_I.cpp demo_basket.cpp demo_boxstack.cpp demo_buggy.cpp \ + demo_cards.cpp $(demo_chain1_SOURCES) demo_chain2.cpp \ + demo_collision.cpp demo_convex.cpp demo_crash.cpp demo_cyl.cpp \ + demo_cylvssphere.cpp demo_dball.cpp demo_dhinge.cpp \ + demo_feedback.cpp demo_friction.cpp demo_gyro2.cpp \ + demo_gyroscopic.cpp demo_heightfield.cpp demo_hinge.cpp \ + demo_jointPR.cpp demo_jointPU.cpp demo_joints.cpp \ + demo_kinematic.cpp demo_motion.cpp demo_motor.cpp \ + demo_moving_convex.cpp demo_moving_trimesh.cpp demo_ode.cpp \ + demo_piston.cpp demo_plane2d.cpp demo_rfriction.cpp \ + demo_slider.cpp demo_space.cpp demo_space_stress.cpp \ + demo_step.cpp demo_tracks.cpp demo_transmission.cpp \ + demo_trimesh.cpp +DIST_SOURCES = demo_I.cpp demo_basket.cpp demo_boxstack.cpp \ + demo_buggy.cpp demo_cards.cpp $(demo_chain1_SOURCES) \ + demo_chain2.cpp demo_collision.cpp demo_convex.cpp \ + demo_crash.cpp demo_cyl.cpp demo_cylvssphere.cpp \ + demo_dball.cpp demo_dhinge.cpp demo_feedback.cpp \ + demo_friction.cpp demo_gyro2.cpp demo_gyroscopic.cpp \ + demo_heightfield.cpp demo_hinge.cpp demo_jointPR.cpp \ + demo_jointPU.cpp demo_joints.cpp demo_kinematic.cpp \ + demo_motion.cpp demo_motor.cpp demo_moving_convex.cpp \ + demo_moving_trimesh.cpp demo_ode.cpp demo_piston.cpp \ + demo_plane2d.cpp demo_rfriction.cpp demo_slider.cpp \ + demo_space.cpp demo_space_stress.cpp demo_step.cpp \ + demo_tracks.cpp demo_transmission.cpp demo_trimesh.cpp +am__can_run_installinfo = \ + case $$AM_UPDATE_INFO_DIR in \ + n|no|NO) false;; \ + *) (install-info --version) >/dev/null 2>&1;; \ + esac +HEADERS = $(noinst_HEADERS) +am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) $(LISP) +# Read a list of newline-separated strings from the standard input, +# and print each of them once, without duplicates. Input order is +# *not* preserved. +am__uniquify_input = $(AWK) '\ + BEGIN { nonempty = 0; } \ + { items[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in items) print i; }; } \ +' +# Make sure the list of sources is unique. This is necessary because, +# e.g., the same source file might be shared among _SOURCES variables +# for different programs/libraries. +am__define_uniq_tagged_files = \ + list='$(am__tagged_files)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | $(am__uniquify_input)` +ETAGS = etags +CTAGS = ctags +am__DIST_COMMON = $(srcdir)/Makefile.in $(top_srcdir)/depcomp +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@ +AR = @AR@ +AS = @AS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CCD_CFLAGS = @CCD_CFLAGS@ +CCD_LIBS = @CCD_LIBS@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXCPP = @CXXCPP@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DLLTOOL = @DLLTOOL@ +DOXYGEN = @DOXYGEN@ +DSYMUTIL = @DSYMUTIL@ +DUMPBIN = @DUMPBIN@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ +FGREP = @FGREP@ +GL_LIBS = @GL_LIBS@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LD = @LD@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LIBSTDCXX = @LIBSTDCXX@ +LIBTOOL = @LIBTOOL@ +LIPO = @LIPO@ +LN_S = @LN_S@ +LTLIBOBJS = @LTLIBOBJS@ +LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@ +MAKEINFO = @MAKEINFO@ +MANIFEST_TOOL = @MANIFEST_TOOL@ +MKDIR_P = @MKDIR_P@ +NM = @NM@ +NMEDIT = @NMEDIT@ +OBJDUMP = @OBJDUMP@ +OBJEXT = @OBJEXT@ +ODE_PRECISION = @ODE_PRECISION@ +ODE_VERSION = @ODE_VERSION@ +ODE_VERSION_INFO = @ODE_VERSION_INFO@ +OTOOL = @OTOOL@ +OTOOL64 = @OTOOL64@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_URL = @PACKAGE_URL@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +PKG_CONFIG = @PKG_CONFIG@ +PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@ +PKG_CONFIG_PATH = @PKG_CONFIG_PATH@ +RANLIB = @RANLIB@ +SED = @SED@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +X11_CFLAGS = @X11_CFLAGS@ +X11_LIBS = @X11_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_AR = @ac_ct_AR@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +ac_ct_DUMPBIN = @ac_ct_DUMPBIN@ +ac_ct_WINDRES = @ac_ct_WINDRES@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build = @build@ +build_alias = @build_alias@ +build_cpu = @build_cpu@ +build_os = @build_os@ +build_vendor = @build_vendor@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host = @host@ +host_alias = @host_alias@ +host_cpu = @host_cpu@ +host_os = @host_os@ +host_vendor = @host_vendor@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +runstatedir = @runstatedir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +subdirs = @subdirs@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_build_prefix = @top_build_prefix@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include \ + -DDRAWSTUFF_TEXTURE_PATH="\"$(abs_top_srcdir)/drawstuff/textures\"" \ + $(am__append_2) +@X11_TRUE@AM_LDFLAGS = $(X_PRE_LIBS) $(X_LIBS) $(X_EXTRA_LIBS) + +# On Windows, GL_LIBS must go after libdrawstuff.la. +LDADD = $(top_builddir)/drawstuff/src/libdrawstuff.la \ + $(top_builddir)/ode/src/libode.la @GL_LIBS@ $(am__append_3) +noinst_HEADERS = basket_geom.h bunny_geom.h convex_bunny_geom.h convex_prism.h \ + icosahedron_geom.h halton235_geom.h texturepath.h world_geom3.h + +AM_DEFAULT_SOURCE_EXT = .cpp +demo_chain1_SOURCES = demo_chain1.c +demo_chain1_LDADD = $(LDADD) -lstdc++ +all: all-am + +.SUFFIXES: +.SUFFIXES: .c .cpp .lo .o .obj +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \ + && { if test -f $@; then exit 0; else break; fi; }; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/demo/Makefile'; \ + $(am__cd) $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/demo/Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(am__aclocal_m4_deps): + +clean-noinstPROGRAMS: + @list='$(noinst_PROGRAMS)'; test -n "$$list" || exit 0; \ + echo " rm -f" $$list; \ + rm -f $$list || exit $$?; \ + test -n "$(EXEEXT)" || exit 0; \ + list=`for p in $$list; do echo "$$p"; done | sed 's/$(EXEEXT)$$//'`; \ + echo " rm -f" $$list; \ + rm -f $$list + +demo_I$(EXEEXT): $(demo_I_OBJECTS) $(demo_I_DEPENDENCIES) $(EXTRA_demo_I_DEPENDENCIES) + @rm -f demo_I$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_I_OBJECTS) $(demo_I_LDADD) $(LIBS) + +demo_basket$(EXEEXT): $(demo_basket_OBJECTS) $(demo_basket_DEPENDENCIES) $(EXTRA_demo_basket_DEPENDENCIES) + @rm -f demo_basket$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_basket_OBJECTS) $(demo_basket_LDADD) $(LIBS) + +demo_boxstack$(EXEEXT): $(demo_boxstack_OBJECTS) $(demo_boxstack_DEPENDENCIES) $(EXTRA_demo_boxstack_DEPENDENCIES) + @rm -f demo_boxstack$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_boxstack_OBJECTS) $(demo_boxstack_LDADD) $(LIBS) + +demo_buggy$(EXEEXT): $(demo_buggy_OBJECTS) $(demo_buggy_DEPENDENCIES) $(EXTRA_demo_buggy_DEPENDENCIES) + @rm -f demo_buggy$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_buggy_OBJECTS) $(demo_buggy_LDADD) $(LIBS) + +demo_cards$(EXEEXT): $(demo_cards_OBJECTS) $(demo_cards_DEPENDENCIES) $(EXTRA_demo_cards_DEPENDENCIES) + @rm -f demo_cards$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_cards_OBJECTS) $(demo_cards_LDADD) $(LIBS) + +demo_chain1$(EXEEXT): $(demo_chain1_OBJECTS) $(demo_chain1_DEPENDENCIES) $(EXTRA_demo_chain1_DEPENDENCIES) + @rm -f demo_chain1$(EXEEXT) + $(AM_V_CCLD)$(LINK) $(demo_chain1_OBJECTS) $(demo_chain1_LDADD) $(LIBS) + +demo_chain2$(EXEEXT): $(demo_chain2_OBJECTS) $(demo_chain2_DEPENDENCIES) $(EXTRA_demo_chain2_DEPENDENCIES) + @rm -f demo_chain2$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_chain2_OBJECTS) $(demo_chain2_LDADD) $(LIBS) + +demo_collision$(EXEEXT): $(demo_collision_OBJECTS) $(demo_collision_DEPENDENCIES) $(EXTRA_demo_collision_DEPENDENCIES) + @rm -f demo_collision$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_collision_OBJECTS) $(demo_collision_LDADD) $(LIBS) + +demo_convex$(EXEEXT): $(demo_convex_OBJECTS) $(demo_convex_DEPENDENCIES) $(EXTRA_demo_convex_DEPENDENCIES) + @rm -f demo_convex$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_convex_OBJECTS) $(demo_convex_LDADD) $(LIBS) + +demo_crash$(EXEEXT): $(demo_crash_OBJECTS) $(demo_crash_DEPENDENCIES) $(EXTRA_demo_crash_DEPENDENCIES) + @rm -f demo_crash$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_crash_OBJECTS) $(demo_crash_LDADD) $(LIBS) + +demo_cyl$(EXEEXT): $(demo_cyl_OBJECTS) $(demo_cyl_DEPENDENCIES) $(EXTRA_demo_cyl_DEPENDENCIES) + @rm -f demo_cyl$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_cyl_OBJECTS) $(demo_cyl_LDADD) $(LIBS) + +demo_cylvssphere$(EXEEXT): $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_DEPENDENCIES) $(EXTRA_demo_cylvssphere_DEPENDENCIES) + @rm -f demo_cylvssphere$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_LDADD) $(LIBS) + +demo_dball$(EXEEXT): $(demo_dball_OBJECTS) $(demo_dball_DEPENDENCIES) $(EXTRA_demo_dball_DEPENDENCIES) + @rm -f demo_dball$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_dball_OBJECTS) $(demo_dball_LDADD) $(LIBS) + +demo_dhinge$(EXEEXT): $(demo_dhinge_OBJECTS) $(demo_dhinge_DEPENDENCIES) $(EXTRA_demo_dhinge_DEPENDENCIES) + @rm -f demo_dhinge$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_dhinge_OBJECTS) $(demo_dhinge_LDADD) $(LIBS) + +demo_feedback$(EXEEXT): $(demo_feedback_OBJECTS) $(demo_feedback_DEPENDENCIES) $(EXTRA_demo_feedback_DEPENDENCIES) + @rm -f demo_feedback$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_feedback_OBJECTS) $(demo_feedback_LDADD) $(LIBS) + +demo_friction$(EXEEXT): $(demo_friction_OBJECTS) $(demo_friction_DEPENDENCIES) $(EXTRA_demo_friction_DEPENDENCIES) + @rm -f demo_friction$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_friction_OBJECTS) $(demo_friction_LDADD) $(LIBS) + +demo_gyro2$(EXEEXT): $(demo_gyro2_OBJECTS) $(demo_gyro2_DEPENDENCIES) $(EXTRA_demo_gyro2_DEPENDENCIES) + @rm -f demo_gyro2$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_gyro2_OBJECTS) $(demo_gyro2_LDADD) $(LIBS) + +demo_gyroscopic$(EXEEXT): $(demo_gyroscopic_OBJECTS) $(demo_gyroscopic_DEPENDENCIES) $(EXTRA_demo_gyroscopic_DEPENDENCIES) + @rm -f demo_gyroscopic$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_gyroscopic_OBJECTS) $(demo_gyroscopic_LDADD) $(LIBS) + +demo_heightfield$(EXEEXT): $(demo_heightfield_OBJECTS) $(demo_heightfield_DEPENDENCIES) $(EXTRA_demo_heightfield_DEPENDENCIES) + @rm -f demo_heightfield$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_heightfield_OBJECTS) $(demo_heightfield_LDADD) $(LIBS) + +demo_hinge$(EXEEXT): $(demo_hinge_OBJECTS) $(demo_hinge_DEPENDENCIES) $(EXTRA_demo_hinge_DEPENDENCIES) + @rm -f demo_hinge$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_hinge_OBJECTS) $(demo_hinge_LDADD) $(LIBS) + +demo_jointPR$(EXEEXT): $(demo_jointPR_OBJECTS) $(demo_jointPR_DEPENDENCIES) $(EXTRA_demo_jointPR_DEPENDENCIES) + @rm -f demo_jointPR$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_jointPR_OBJECTS) $(demo_jointPR_LDADD) $(LIBS) + +demo_jointPU$(EXEEXT): $(demo_jointPU_OBJECTS) $(demo_jointPU_DEPENDENCIES) $(EXTRA_demo_jointPU_DEPENDENCIES) + @rm -f demo_jointPU$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_jointPU_OBJECTS) $(demo_jointPU_LDADD) $(LIBS) + +demo_joints$(EXEEXT): $(demo_joints_OBJECTS) $(demo_joints_DEPENDENCIES) $(EXTRA_demo_joints_DEPENDENCIES) + @rm -f demo_joints$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_joints_OBJECTS) $(demo_joints_LDADD) $(LIBS) + +demo_kinematic$(EXEEXT): $(demo_kinematic_OBJECTS) $(demo_kinematic_DEPENDENCIES) $(EXTRA_demo_kinematic_DEPENDENCIES) + @rm -f demo_kinematic$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_kinematic_OBJECTS) $(demo_kinematic_LDADD) $(LIBS) + +demo_motion$(EXEEXT): $(demo_motion_OBJECTS) $(demo_motion_DEPENDENCIES) $(EXTRA_demo_motion_DEPENDENCIES) + @rm -f demo_motion$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_motion_OBJECTS) $(demo_motion_LDADD) $(LIBS) + +demo_motor$(EXEEXT): $(demo_motor_OBJECTS) $(demo_motor_DEPENDENCIES) $(EXTRA_demo_motor_DEPENDENCIES) + @rm -f demo_motor$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_motor_OBJECTS) $(demo_motor_LDADD) $(LIBS) + +demo_moving_convex$(EXEEXT): $(demo_moving_convex_OBJECTS) $(demo_moving_convex_DEPENDENCIES) $(EXTRA_demo_moving_convex_DEPENDENCIES) + @rm -f demo_moving_convex$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_moving_convex_OBJECTS) $(demo_moving_convex_LDADD) $(LIBS) + +demo_moving_trimesh$(EXEEXT): $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_DEPENDENCIES) $(EXTRA_demo_moving_trimesh_DEPENDENCIES) + @rm -f demo_moving_trimesh$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_LDADD) $(LIBS) + +demo_ode$(EXEEXT): $(demo_ode_OBJECTS) $(demo_ode_DEPENDENCIES) $(EXTRA_demo_ode_DEPENDENCIES) + @rm -f demo_ode$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_ode_OBJECTS) $(demo_ode_LDADD) $(LIBS) + +demo_piston$(EXEEXT): $(demo_piston_OBJECTS) $(demo_piston_DEPENDENCIES) $(EXTRA_demo_piston_DEPENDENCIES) + @rm -f demo_piston$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_piston_OBJECTS) $(demo_piston_LDADD) $(LIBS) + +demo_plane2d$(EXEEXT): $(demo_plane2d_OBJECTS) $(demo_plane2d_DEPENDENCIES) $(EXTRA_demo_plane2d_DEPENDENCIES) + @rm -f demo_plane2d$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_plane2d_OBJECTS) $(demo_plane2d_LDADD) $(LIBS) + +demo_rfriction$(EXEEXT): $(demo_rfriction_OBJECTS) $(demo_rfriction_DEPENDENCIES) $(EXTRA_demo_rfriction_DEPENDENCIES) + @rm -f demo_rfriction$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_rfriction_OBJECTS) $(demo_rfriction_LDADD) $(LIBS) + +demo_slider$(EXEEXT): $(demo_slider_OBJECTS) $(demo_slider_DEPENDENCIES) $(EXTRA_demo_slider_DEPENDENCIES) + @rm -f demo_slider$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_slider_OBJECTS) $(demo_slider_LDADD) $(LIBS) + +demo_space$(EXEEXT): $(demo_space_OBJECTS) $(demo_space_DEPENDENCIES) $(EXTRA_demo_space_DEPENDENCIES) + @rm -f demo_space$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_space_OBJECTS) $(demo_space_LDADD) $(LIBS) + +demo_space_stress$(EXEEXT): $(demo_space_stress_OBJECTS) $(demo_space_stress_DEPENDENCIES) $(EXTRA_demo_space_stress_DEPENDENCIES) + @rm -f demo_space_stress$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_space_stress_OBJECTS) $(demo_space_stress_LDADD) $(LIBS) + +demo_step$(EXEEXT): $(demo_step_OBJECTS) $(demo_step_DEPENDENCIES) $(EXTRA_demo_step_DEPENDENCIES) + @rm -f demo_step$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_step_OBJECTS) $(demo_step_LDADD) $(LIBS) + +demo_tracks$(EXEEXT): $(demo_tracks_OBJECTS) $(demo_tracks_DEPENDENCIES) $(EXTRA_demo_tracks_DEPENDENCIES) + @rm -f demo_tracks$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_tracks_OBJECTS) $(demo_tracks_LDADD) $(LIBS) + +demo_transmission$(EXEEXT): $(demo_transmission_OBJECTS) $(demo_transmission_DEPENDENCIES) $(EXTRA_demo_transmission_DEPENDENCIES) + @rm -f demo_transmission$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_transmission_OBJECTS) $(demo_transmission_LDADD) $(LIBS) + +demo_trimesh$(EXEEXT): $(demo_trimesh_OBJECTS) $(demo_trimesh_DEPENDENCIES) $(EXTRA_demo_trimesh_DEPENDENCIES) + @rm -f demo_trimesh$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(demo_trimesh_OBJECTS) $(demo_trimesh_LDADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_I.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_basket.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_boxstack.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_buggy.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cards.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_chain1.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_chain2.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_collision.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_convex.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_crash.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cyl.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_cylvssphere.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_dball.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_dhinge.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_feedback.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_friction.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_gyro2.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_gyroscopic.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_heightfield.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_hinge.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_jointPR.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_jointPU.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_joints.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_kinematic.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_motion.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_motor.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_moving_convex.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_moving_trimesh.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_ode.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_piston.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_plane2d.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_rfriction.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_slider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_space.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_space_stress.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_step.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_tracks.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_transmission.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_trimesh.Po@am__quote@ + +.c.o: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(COMPILE) -c -o $@ $< + +.c.obj: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(COMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.c.lo: +@am__fastdepCC_TRUE@ $(AM_V_CC)$(LTCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCC_FALSE@ $(AM_V_CC)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(AM_V_CC@am__nodep@)$(LTCOMPILE) -c -o $@ $< + +.cpp.o: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ $< + +.cpp.obj: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +.cpp.lo: +@am__fastdepCXX_TRUE@ $(AM_V_CXX)$(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ $(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ $(AM_V_CXX)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(AM_V_CXX@am__nodep@)$(LTCXXCOMPILE) -c -o $@ $< + +mostlyclean-libtool: + -rm -f *.lo + +clean-libtool: + -rm -rf .libs _libs + +ID: $(am__tagged_files) + $(am__define_uniq_tagged_files); mkid -fID $$unique +tags: tags-am +TAGS: tags + +tags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + set x; \ + here=`pwd`; \ + $(am__define_uniq_tagged_files); \ + shift; \ + if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + if test $$# -gt 0; then \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + "$$@" $$unique; \ + else \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$unique; \ + fi; \ + fi +ctags: ctags-am + +CTAGS: ctags +ctags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files) + $(am__define_uniq_tagged_files); \ + test -z "$(CTAGS_ARGS)$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && $(am__cd) $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) "$$here" +cscopelist: cscopelist-am + +cscopelist-am: $(am__tagged_files) + list='$(am__tagged_files)'; \ + case "$(srcdir)" in \ + [\\/]* | ?:[\\/]*) sdir="$(srcdir)" ;; \ + *) sdir=$(subdir)/$(srcdir) ;; \ + esac; \ + for i in $$list; do \ + if test -f "$$i"; then \ + echo "$(subdir)/$$i"; \ + else \ + echo "$$sdir/$$i"; \ + fi; \ + done >> $(top_builddir)/cscope.files + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d "$(distdir)/$$file"; then \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \ + find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \ + fi; \ + cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \ + else \ + test -f "$(distdir)/$$file" \ + || cp -p $$d/$$file "$(distdir)/$$file" \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-am +all-am: Makefile $(PROGRAMS) $(HEADERS) +installdirs: +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + if test -z '$(STRIP)'; then \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + install; \ + else \ + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \ + fi +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + -test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-am + +clean-am: clean-generic clean-libtool clean-noinstPROGRAMS \ + mostlyclean-am + +distclean: distclean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +distclean-am: clean-am distclean-compile distclean-generic \ + distclean-tags + +dvi: dvi-am + +dvi-am: + +html: html-am + +html-am: + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +install-dvi-am: + +install-exec-am: + +install-html: install-html-am + +install-html-am: + +install-info: install-info-am + +install-info-am: + +install-man: + +install-pdf: install-pdf-am + +install-pdf-am: + +install-ps: install-ps-am + +install-ps-am: + +installcheck-am: + +maintainer-clean: maintainer-clean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-am + +mostlyclean-am: mostlyclean-compile mostlyclean-generic \ + mostlyclean-libtool + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: + +.MAKE: install-am install-strip + +.PHONY: CTAGS GTAGS TAGS all all-am check check-am clean clean-generic \ + clean-libtool clean-noinstPROGRAMS cscopelist-am ctags \ + ctags-am distclean distclean-compile distclean-generic \ + distclean-libtool distclean-tags distdir dvi dvi-am html \ + html-am info info-am install install-am install-data \ + install-data-am install-dvi install-dvi-am install-exec \ + install-exec-am install-html install-html-am install-info \ + install-info-am install-man install-pdf install-pdf-am \ + install-ps install-ps-am install-strip installcheck \ + installcheck-am installdirs maintainer-clean \ + maintainer-clean-generic mostlyclean mostlyclean-compile \ + mostlyclean-generic mostlyclean-libtool pdf pdf-am ps ps-am \ + tags tags-am uninstall uninstall-am + +.PRECIOUS: Makefile + + +@WIN32_TRUE@resources.o: $(top_srcdir)/drawstuff/src/resources.rc $(top_srcdir)/drawstuff/src/resource.h +@WIN32_TRUE@ @WINDRES@ $(top_srcdir)/drawstuff/src/resources.rc -o resources.o + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/libs/ode-0.16.1/ode/demo/basket_geom.h b/libs/ode-0.16.1/ode/demo/basket_geom.h new file mode 100644 index 0000000..ec88327 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/basket_geom.h @@ -0,0 +1,599 @@ + +static float world_normals[] = { + 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,-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, +}; + +static float world_vertices[] = { + -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,4,-0.100000f, + 0.066000f,-2.060000f,2, + 0.066000f,-1.940000f,2, + -0.066000f,-2.060000f,2, + -0.066000f,-1.940000f,2, + -4,4,0.100000f, + -4,0,0.100000f, + 0.360000f,3.244444f,1.466974f, + 0.360000f,3.422222f,2.266974f, + -0.360000f,3.422222f,2.266974f, + -0.360000f,3.244444f,1.466974f, + 0.066000f,-2.060000f,0.100000f, + -0.066000f,-2.060000f,0.100000f, + 0.066000f,-1.940000f,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.066000f,-1.940000f,1.950000f, + -0.066000f,-1.840000f,1.950000f, + 0.066000f,-1.840000f,1.950000f, + -0.066000f,-1.840000f,2, + 0.066000f,-1.840000f,2, + -0.171600f,-1.740000f,2, + -0.171600f,-1.740000f,1.950000f, + 0.171600f,-1.740000f,1.950000f, + 0.171600f,-1.740000f,2, + -0.188760f,-1.640000f,2, + -0.188760f,-1.640000f,1.950000f, + 0.188760f,-1.640000f,1.950000f, + 0.188760f,-1.640000f,2, + -0.132132f,-1.540000f,2, + -0.132132f,-1.540000f,1.950000f, + 0.132132f,-1.540000f,1.950000f, + 0.132132f,-1.540000f,2, + 0.173397f,-1.642679f,1.950000f, + 0.121808f,-1.551577f,1.950000f, + 0.157950f,-1.732697f,1.950000f, + 0.060149f,-1.825311f,1.950000f, + -0.060149f,-1.825311f,1.950000f, + -0.157950f,-1.732697f,1.950000f, + -0.173397f,-1.642679f,1.950000f, + -0.121808f,-1.551577f,1.950000f, + -0.049868f,-1.521079f,1.950000f, + 0.049868f,-1.521079f,1.950000f, + -0.173397f,-1.642679f,2, + -0.121808f,-1.551577f,2, + -0.157950f,-1.732697f,2, + -0.060149f,-1.825311f,2, + 0.060149f,-1.825311f,2, + 0.157950f,-1.732697f,2, + 0.173397f,-1.642679f,2, + 0.121808f,-1.551577f,2, + 0.049868f,-1.521079f,2, + -0.049868f,-1.521079f,2, + -0.360000f,3.600000f,0.100000f, + 0.360000f,3.600000f,0.100000f, + -0.360000f,0.400000f,0.100000f, + 0.360000f,0.400000f,0.100000f, + 0.360000f,2.888889f,1.023752f, + 0.360000f,3.066667f,1.166974f, + -0.360000f,3.066667f,1.166974f, + -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.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,1.822222f,0.939976f, + 0.360000f,1.822222f,0.939976f, + 0.360000f,2,0.939976f, + -0.360000f,2,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.111111f,0.957571f, + 0.360000f,1.288889f,0.939976f, + -0.360000f,1.288889f,0.939976f, + -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.577778f,1.372130f, + -0.360000f,0.577778f,1.372130f, + -0.360000f,3.600000f,3.900000f, + 0.360000f,3.600000f,3.900000f, + 0.360000f,0.400000f,1.743932f, + -0.360000f,0.400000f,1.743932f, +}; + +static dTriIndex world_indices[] = { + 0, + 1, + 2, + 0, + 2, + 3, + 4, + 1, + 5, + 4, + 5, + 6, + 4, + 2, + 1, + 0, + 7, + 5, + 0, + 5, + 1, + 8, + 9, + 10, + 9, + 11, + 10, + 12, + 6, + 5, + 5, + 7, + 12, + 3, + 13, + 0, + 13, + 12, + 7, + 13, + 7, + 0, + 14, + 15, + 16, + 16, + 17, + 14, + 2, + 18, + 19, + 19, + 3, + 2, + 4, + 20, + 2, + 20, + 18, + 2, + 21, + 20, + 4, + 4, + 13, + 21, + 19, + 21, + 13, + 13, + 3, + 19, + 8, + 10, + 19, + 19, + 18, + 8, + 22, + 9, + 8, + 8, + 18, + 22, + 18, + 20, + 22, + 23, + 24, + 25, + 25, + 26, + 23, + 19, + 10, + 27, + 19, + 27, + 21, + 10, + 11, + 27, + 21, + 27, + 22, + 21, + 22, + 20, + 27, + 28, + 22, + 28, + 29, + 22, + 11, + 30, + 28, + 28, + 27, + 11, + 9, + 31, + 11, + 31, + 30, + 11, + 22, + 29, + 31, + 22, + 31, + 9, + 30, + 32, + 28, + 32, + 33, + 28, + 29, + 34, + 35, + 29, + 35, + 31, + 32, + 36, + 37, + 37, + 33, + 32, + 34, + 38, + 39, + 34, + 39, + 35, + 36, + 40, + 41, + 41, + 37, + 36, + 38, + 42, + 43, + 38, + 43, + 39, + 40, + 23, + 26, + 26, + 41, + 40, + 42, + 25, + 24, + 42, + 24, + 43, + 38, + 44, + 45, + 45, + 42, + 38, + 34, + 46, + 44, + 34, + 44, + 38, + 34, + 29, + 47, + 34, + 47, + 46, + 28, + 48, + 29, + 48, + 47, + 29, + 33, + 49, + 48, + 33, + 48, + 28, + 50, + 49, + 33, + 33, + 37, + 50, + 51, + 50, + 37, + 37, + 41, + 51, + 26, + 52, + 51, + 26, + 51, + 41, + 53, + 52, + 26, + 26, + 25, + 53, + 25, + 42, + 45, + 25, + 45, + 53, + 36, + 54, + 55, + 55, + 40, + 36, + 32, + 56, + 54, + 54, + 36, + 32, + 30, + 57, + 32, + 57, + 56, + 32, + 31, + 58, + 30, + 58, + 57, + 30, + 35, + 59, + 58, + 35, + 58, + 31, + 60, + 59, + 35, + 35, + 39, + 60, + 61, + 60, + 39, + 39, + 43, + 61, + 24, + 62, + 61, + 24, + 61, + 43, + 63, + 62, + 24, + 24, + 23, + 63, + 55, + 63, + 23, + 23, + 40, + 55, + 54, + 56, + 49, + 49, + 50, + 54, + 56, + 57, + 48, + 48, + 49, + 56, + 57, + 58, + 47, + 47, + 48, + 57, + 47, + 58, + 59, + 59, + 46, + 47, + 59, + 60, + 44, + 44, + 46, + 59, + 60, + 61, + 45, + 45, + 44, + 60, + 61, + 62, + 53, + 53, + 45, + 61, + 62, + 63, + 52, + 52, + 53, + 62, + 63, + 55, + 51, + 51, + 52, + 63, + 55, + 54, + 50, + 50, + 51, + 55, + 64, + 65, + 6, + 6, + 12, + 64, + 66, + 64, + 12, + 12, + 13, + 66, + 4, + 67, + 66, + 66, + 13, + 4, + 6, + 65, + 4, + 65, + 67, + 4, + 68, + 69, + 70, + 70, + 71, + 68, + 72, + 73, + 74, + 74, + 75, + 72, + 76, + 77, + 78, + 78, + 79, + 76, + 80, + 81, + 82, + 82, + 83, + 80, + 84, + 85, + 86, + 86, + 87, + 84, + 88, + 89, + 90, + 90, + 91, + 88, + 92, + 93, + 94, + 94, + 95, + 92, + 93, + 92, + 96, + 92, + 97, + 96, + 98, + 16, + 15, + 98, + 15, + 99, + 14, + 17, + 69, + 17, + 70, + 69, + 68, + 71, + 73, + 71, + 74, + 73, + 72, + 75, + 79, + 72, + 79, + 78, + 77, + 76, + 83, + 77, + 83, + 82, + 81, + 80, + 87, + 81, + 87, + 86, + 85, + 84, + 90, + 85, + 90, + 89, + 88, + 91, + 94, + 91, + 95, + 94, + 100, + 96, + 97, + 97, + 101, + 100, +}; + diff --git a/libs/ode-0.16.1/ode/demo/bunny_geom.h b/libs/ode-0.16.1/ode/demo/bunny_geom.h new file mode 100644 index 0000000..78f8eb0 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/bunny_geom.h @@ -0,0 +1,1366 @@ +// Bunny mesh ripped from Opcode +const int VertexCount = 453; +const int IndexCount = 902 * 3; + + +float Vertices[VertexCount * 3] = { + -0.334392f, 0.133007f, 0.062259f, + -0.350189f, 0.150354f, -0.147769f, + -0.234201f, 0.343811f, -0.174307f, + -0.200259f, 0.285207f, 0.093749f, + 0.003520f, 0.475208f, -0.159365f, + 0.001856f, 0.419203f, 0.098582f, + -0.252802f, 0.093666f, 0.237538f, + -0.162901f, 0.237984f, 0.206905f, + 0.000865f, 0.318141f, 0.235370f, + -0.414624f, 0.164083f, -0.278254f, + -0.262213f, 0.357334f, -0.293246f, + 0.004628f, 0.482694f, -0.338626f, + -0.402162f, 0.133528f, -0.443247f, + -0.243781f, 0.324275f, -0.436763f, + 0.005293f, 0.437592f, -0.458332f, + -0.339884f, -0.041150f, -0.668211f, + -0.248382f, 0.255825f, -0.627493f, + 0.006261f, 0.376103f, -0.631506f, + -0.216201f, -0.126776f, -0.886936f, + -0.171075f, 0.011544f, -0.881386f, + -0.181074f, 0.098223f, -0.814779f, + -0.119891f, 0.218786f, -0.760153f, + -0.078895f, 0.276780f, -0.739281f, + 0.006801f, 0.310959f, -0.735661f, + -0.168842f, 0.102387f, -0.920381f, + -0.104072f, 0.177278f, -0.952530f, + -0.129704f, 0.211848f, -0.836678f, + -0.099875f, 0.310931f, -0.799381f, + 0.007237f, 0.361687f, -0.794439f, + -0.077913f, 0.258753f, -0.921640f, + 0.007957f, 0.282241f, -0.931680f, + -0.252222f, -0.550401f, -0.557810f, + -0.267633f, -0.603419f, -0.655209f, + -0.446838f, -0.118517f, -0.466159f, + -0.459488f, -0.093017f, -0.311341f, + -0.370645f, -0.100108f, -0.159454f, + -0.371984f, -0.091991f, -0.011044f, + -0.328945f, -0.098269f, 0.088659f, + -0.282452f, -0.018862f, 0.311501f, + -0.352403f, -0.131341f, 0.144902f, + -0.364126f, -0.200299f, 0.202388f, + -0.283965f, -0.231869f, 0.023668f, + -0.298943f, -0.155218f, 0.369716f, + -0.293787f, -0.121856f, 0.419097f, + -0.290163f, -0.290797f, 0.107824f, + -0.264165f, -0.272849f, 0.036347f, + -0.228567f, -0.372573f, 0.290309f, + -0.190431f, -0.286997f, 0.421917f, + -0.191039f, -0.240973f, 0.507118f, + -0.287272f, -0.276431f, -0.065444f, + -0.295675f, -0.280818f, -0.174200f, + -0.399537f, -0.313131f, -0.376167f, + -0.392666f, -0.488581f, -0.427494f, + -0.331669f, -0.570185f, -0.466054f, + -0.282290f, -0.618140f, -0.589220f, + -0.374238f, -0.594882f, -0.323298f, + -0.381071f, -0.629723f, -0.350777f, + -0.382112f, -0.624060f, -0.221577f, + -0.272701f, -0.566522f, 0.259157f, + -0.256702f, -0.663406f, 0.286079f, + -0.280948f, -0.428359f, 0.055790f, + -0.184974f, -0.508894f, 0.326265f, + -0.279971f, -0.526918f, 0.395319f, + -0.282599f, -0.663393f, 0.412411f, + -0.188329f, -0.475093f, 0.417954f, + -0.263384f, -0.663396f, 0.466604f, + -0.209063f, -0.663393f, 0.509344f, + -0.002044f, -0.319624f, 0.553078f, + -0.001266f, -0.371260f, 0.413296f, + -0.219753f, -0.339762f, -0.040921f, + -0.256986f, -0.282511f, -0.006349f, + -0.271706f, -0.260881f, 0.001764f, + -0.091191f, -0.419184f, -0.045912f, + -0.114944f, -0.429752f, -0.124739f, + -0.113970f, -0.382987f, -0.188540f, + -0.243012f, -0.464942f, -0.242850f, + -0.314815f, -0.505402f, -0.324768f, + 0.002774f, -0.437526f, -0.262766f, + -0.072625f, -0.417748f, -0.221440f, + -0.160112f, -0.476932f, -0.293450f, + 0.003859f, -0.453425f, -0.443916f, + -0.120363f, -0.581567f, -0.438689f, + -0.091499f, -0.584191f, -0.294511f, + -0.116469f, -0.599861f, -0.188308f, + -0.208032f, -0.513640f, -0.134649f, + -0.235749f, -0.610017f, -0.040939f, + -0.344916f, -0.622487f, -0.085380f, + -0.336401f, -0.531864f, -0.212298f, + 0.001961f, -0.459550f, -0.135547f, + -0.058296f, -0.430536f, -0.043440f, + 0.001378f, -0.449511f, -0.037762f, + -0.130135f, -0.510222f, 0.079144f, + 0.000142f, -0.477549f, 0.157064f, + -0.114284f, -0.453206f, 0.304397f, + -0.000592f, -0.443558f, 0.285401f, + -0.056215f, -0.663402f, 0.326073f, + -0.026248f, -0.568010f, 0.273318f, + -0.049261f, -0.531064f, 0.389854f, + -0.127096f, -0.663398f, 0.479316f, + -0.058384f, -0.663401f, 0.372891f, + -0.303961f, 0.054199f, 0.625921f, + -0.268594f, 0.193403f, 0.502766f, + -0.277159f, 0.126123f, 0.443289f, + -0.287605f, -0.005722f, 0.531844f, + -0.231396f, -0.121289f, 0.587387f, + -0.253475f, -0.081797f, 0.756541f, + -0.195164f, -0.137969f, 0.728011f, + -0.167673f, -0.156573f, 0.609388f, + -0.145917f, -0.169029f, 0.697600f, + -0.077776f, -0.214247f, 0.622586f, + -0.076873f, -0.214971f, 0.696301f, + -0.002341f, -0.233135f, 0.622859f, + -0.002730f, -0.213526f, 0.691267f, + -0.003136f, -0.192628f, 0.762731f, + -0.056136f, -0.201222f, 0.763806f, + -0.114589f, -0.166192f, 0.770723f, + -0.155145f, -0.129632f, 0.791738f, + -0.183611f, -0.058705f, 0.847012f, + -0.165562f, 0.001980f, 0.833386f, + -0.220084f, 0.019914f, 0.768935f, + -0.255730f, 0.090306f, 0.670782f, + -0.255594f, 0.113833f, 0.663389f, + -0.226380f, 0.212655f, 0.617740f, + -0.003367f, -0.195342f, 0.799680f, + -0.029743f, -0.210508f, 0.827180f, + -0.003818f, -0.194783f, 0.873636f, + -0.004116f, -0.157907f, 0.931268f, + -0.031280f, -0.184555f, 0.889476f, + -0.059885f, -0.184448f, 0.841330f, + -0.135333f, -0.164332f, 0.878200f, + -0.085574f, -0.170948f, 0.925547f, + -0.163833f, -0.094170f, 0.897114f, + -0.138444f, -0.104250f, 0.945975f, + -0.083497f, -0.084934f, 0.979607f, + -0.004433f, -0.146642f, 0.985872f, + -0.150715f, 0.032650f, 0.884111f, + -0.135892f, -0.035520f, 0.945455f, + -0.070612f, 0.036849f, 0.975733f, + -0.004458f, -0.042526f, 1.015670f, + -0.004249f, 0.046042f, 1.003240f, + -0.086969f, 0.133224f, 0.947633f, + -0.003873f, 0.161605f, 0.970499f, + -0.125544f, 0.140012f, 0.917678f, + -0.125651f, 0.250246f, 0.857602f, + -0.003127f, 0.284070f, 0.878870f, + -0.159174f, 0.125726f, 0.888878f, + -0.183807f, 0.196970f, 0.844480f, + -0.159890f, 0.291736f, 0.732480f, + -0.199495f, 0.207230f, 0.779864f, + -0.206182f, 0.164608f, 0.693257f, + -0.186315f, 0.160689f, 0.817193f, + -0.192827f, 0.166706f, 0.782271f, + -0.175112f, 0.110008f, 0.860621f, + -0.161022f, 0.057420f, 0.855111f, + -0.172319f, 0.036155f, 0.816189f, + -0.190318f, 0.064083f, 0.760605f, + -0.195072f, 0.129179f, 0.731104f, + -0.203126f, 0.410287f, 0.680536f, + -0.216677f, 0.309274f, 0.642272f, + -0.241515f, 0.311485f, 0.587832f, + -0.002209f, 0.366663f, 0.749413f, + -0.088230f, 0.396265f, 0.678635f, + -0.170147f, 0.109517f, 0.840784f, + -0.160521f, 0.067766f, 0.830650f, + -0.181546f, 0.139805f, 0.812146f, + -0.180495f, 0.148568f, 0.776087f, + -0.180255f, 0.129125f, 0.744192f, + -0.186298f, 0.078308f, 0.769352f, + -0.167622f, 0.060539f, 0.806675f, + -0.189876f, 0.102760f, 0.802582f, + -0.108340f, 0.455446f, 0.657174f, + -0.241585f, 0.527592f, 0.669296f, + -0.265676f, 0.513366f, 0.634594f, + -0.203073f, 0.478550f, 0.581526f, + -0.266772f, 0.642330f, 0.602061f, + -0.216961f, 0.564846f, 0.535435f, + -0.202210f, 0.525495f, 0.475944f, + -0.193888f, 0.467925f, 0.520606f, + -0.265837f, 0.757267f, 0.500933f, + -0.240306f, 0.653440f, 0.463215f, + -0.309239f, 0.776868f, 0.304726f, + -0.271009f, 0.683094f, 0.382018f, + -0.312111f, 0.671099f, 0.286687f, + -0.268791f, 0.624342f, 0.377231f, + -0.302457f, 0.533996f, 0.360289f, + -0.263656f, 0.529310f, 0.412564f, + -0.282311f, 0.415167f, 0.447666f, + -0.239201f, 0.442096f, 0.495604f, + -0.220043f, 0.569026f, 0.445877f, + -0.001263f, 0.395631f, 0.602029f, + -0.057345f, 0.442535f, 0.572224f, + -0.088927f, 0.506333f, 0.529106f, + -0.125738f, 0.535076f, 0.612913f, + -0.126251f, 0.577170f, 0.483159f, + -0.149594f, 0.611520f, 0.557731f, + -0.163188f, 0.660791f, 0.491080f, + -0.172482f, 0.663387f, 0.415416f, + -0.160464f, 0.591710f, 0.370659f, + -0.156445f, 0.536396f, 0.378302f, + -0.136496f, 0.444358f, 0.425226f, + -0.095564f, 0.373768f, 0.473659f, + -0.104146f, 0.315912f, 0.498104f, + -0.000496f, 0.384194f, 0.473817f, + -0.000183f, 0.297770f, 0.401486f, + -0.129042f, 0.270145f, 0.434495f, + 0.000100f, 0.272963f, 0.349138f, + -0.113060f, 0.236984f, 0.385554f, + 0.007260f, 0.016311f, -0.883396f, + 0.007865f, 0.122104f, -0.956137f, + -0.032842f, 0.115282f, -0.953252f, + -0.089115f, 0.108449f, -0.950317f, + -0.047440f, 0.014729f, -0.882756f, + -0.104458f, 0.013137f, -0.882070f, + -0.086439f, -0.584866f, -0.608343f, + -0.115026f, -0.662605f, -0.436732f, + -0.071683f, -0.665372f, -0.606385f, + -0.257884f, -0.665381f, -0.658052f, + -0.272542f, -0.665381f, -0.592063f, + -0.371322f, -0.665382f, -0.353620f, + -0.372362f, -0.665381f, -0.224420f, + -0.335166f, -0.665380f, -0.078623f, + -0.225999f, -0.665375f, -0.038981f, + -0.106719f, -0.665374f, -0.186351f, + -0.081749f, -0.665372f, -0.292554f, + 0.006943f, -0.091505f, -0.858354f, + 0.006117f, -0.280985f, -0.769967f, + 0.004495f, -0.502360f, -0.559799f, + -0.198638f, -0.302135f, -0.845816f, + -0.237395f, -0.542544f, -0.587188f, + -0.270001f, -0.279489f, -0.669861f, + -0.134547f, -0.119852f, -0.959004f, + -0.052088f, -0.122463f, -0.944549f, + -0.124463f, -0.293508f, -0.899566f, + -0.047616f, -0.289643f, -0.879292f, + -0.168595f, -0.529132f, -0.654931f, + -0.099793f, -0.515719f, -0.645873f, + -0.186168f, -0.605282f, -0.724690f, + -0.112970f, -0.583097f, -0.707469f, + -0.108152f, -0.665375f, -0.700408f, + -0.183019f, -0.665378f, -0.717630f, + -0.349529f, -0.334459f, -0.511985f, + -0.141182f, -0.437705f, -0.798194f, + -0.212670f, -0.448725f, -0.737447f, + -0.261111f, -0.414945f, -0.613835f, + -0.077364f, -0.431480f, -0.778113f, + 0.005174f, -0.425277f, -0.651592f, + 0.089236f, -0.431732f, -0.777093f, + 0.271006f, -0.415749f, -0.610577f, + 0.223981f, -0.449384f, -0.734774f, + 0.153275f, -0.438150f, -0.796391f, + 0.358414f, -0.335529f, -0.507649f, + 0.193434f, -0.665946f, -0.715325f, + 0.118363f, -0.665717f, -0.699021f, + 0.123515f, -0.583454f, -0.706020f, + 0.196851f, -0.605860f, -0.722345f, + 0.109788f, -0.516035f, -0.644590f, + 0.178656f, -0.529656f, -0.652804f, + 0.061157f, -0.289807f, -0.878626f, + 0.138234f, -0.293905f, -0.897958f, + 0.066933f, -0.122643f, -0.943820f, + 0.149571f, -0.120281f, -0.957264f, + 0.280989f, -0.280321f, -0.666487f, + 0.246581f, -0.543275f, -0.584224f, + 0.211720f, -0.302754f, -0.843303f, + 0.086966f, -0.665627f, -0.291520f, + 0.110634f, -0.665702f, -0.185021f, + 0.228099f, -0.666061f, -0.036201f, + 0.337743f, -0.666396f, -0.074503f, + 0.376722f, -0.666513f, -0.219833f, + 0.377265f, -0.666513f, -0.349036f, + 0.281411f, -0.666217f, -0.588670f, + 0.267564f, -0.666174f, -0.654834f, + 0.080745f, -0.665602f, -0.605452f, + 0.122016f, -0.662963f, -0.435280f, + 0.095767f, -0.585141f, -0.607228f, + 0.118944f, 0.012799f, -0.880702f, + 0.061944f, 0.014564f, -0.882086f, + 0.104725f, 0.108156f, -0.949130f, + 0.048513f, 0.115159f, -0.952753f, + 0.112696f, 0.236643f, 0.386937f, + 0.128177f, 0.269757f, 0.436071f, + 0.102643f, 0.315600f, 0.499370f, + 0.094535f, 0.373481f, 0.474824f, + 0.136270f, 0.443946f, 0.426895f, + 0.157071f, 0.535923f, 0.380222f, + 0.161350f, 0.591224f, 0.372630f, + 0.173035f, 0.662865f, 0.417531f, + 0.162808f, 0.660299f, 0.493077f, + 0.148250f, 0.611070f, 0.559555f, + 0.125719f, 0.576790f, 0.484702f, + 0.123489f, 0.534699f, 0.614440f, + 0.087621f, 0.506066f, 0.530188f, + 0.055321f, 0.442365f, 0.572915f, + 0.219936f, 0.568361f, 0.448571f, + 0.238099f, 0.441375f, 0.498528f, + 0.281711f, 0.414315f, 0.451121f, + 0.263833f, 0.528513f, 0.415794f, + 0.303284f, 0.533081f, 0.363998f, + 0.269687f, 0.623528f, 0.380528f, + 0.314255f, 0.670153f, 0.290524f, + 0.272023f, 0.682273f, 0.385343f, + 0.311480f, 0.775931f, 0.308527f, + 0.240239f, 0.652714f, 0.466159f, + 0.265619f, 0.756464f, 0.504187f, + 0.192562f, 0.467341f, 0.522972f, + 0.201605f, 0.524885f, 0.478417f, + 0.215743f, 0.564193f, 0.538084f, + 0.264969f, 0.641527f, 0.605317f, + 0.201031f, 0.477940f, 0.584002f, + 0.263086f, 0.512567f, 0.637832f, + 0.238615f, 0.526867f, 0.672237f, + 0.105309f, 0.455123f, 0.658482f, + 0.183993f, 0.102195f, 0.804872f, + 0.161563f, 0.060042f, 0.808692f, + 0.180748f, 0.077754f, 0.771600f, + 0.175168f, 0.128588f, 0.746368f, + 0.175075f, 0.148030f, 0.778264f, + 0.175658f, 0.139265f, 0.814333f, + 0.154191f, 0.067291f, 0.832578f, + 0.163818f, 0.109013f, 0.842830f, + 0.084760f, 0.396004f, 0.679695f, + 0.238888f, 0.310760f, 0.590775f, + 0.213380f, 0.308625f, 0.644905f, + 0.199666f, 0.409678f, 0.683003f, + 0.190143f, 0.128597f, 0.733463f, + 0.184833f, 0.063516f, 0.762902f, + 0.166070f, 0.035644f, 0.818261f, + 0.154361f, 0.056943f, 0.857042f, + 0.168542f, 0.109489f, 0.862725f, + 0.187387f, 0.166131f, 0.784599f, + 0.180428f, 0.160135f, 0.819438f, + 0.201823f, 0.163991f, 0.695756f, + 0.194206f, 0.206635f, 0.782275f, + 0.155438f, 0.291260f, 0.734412f, + 0.177696f, 0.196424f, 0.846693f, + 0.152305f, 0.125256f, 0.890786f, + 0.119546f, 0.249876f, 0.859104f, + 0.118369f, 0.139643f, 0.919173f, + 0.079410f, 0.132973f, 0.948652f, + 0.062419f, 0.036648f, 0.976547f, + 0.127847f, -0.035919f, 0.947070f, + 0.143624f, 0.032206f, 0.885913f, + 0.074888f, -0.085173f, 0.980577f, + 0.130184f, -0.104656f, 0.947620f, + 0.156201f, -0.094653f, 0.899074f, + 0.077366f, -0.171194f, 0.926545f, + 0.127722f, -0.164729f, 0.879810f, + 0.052670f, -0.184618f, 0.842019f, + 0.023477f, -0.184638f, 0.889811f, + 0.022626f, -0.210587f, 0.827500f, + 0.223089f, 0.211976f, 0.620493f, + 0.251444f, 0.113067f, 0.666494f, + 0.251419f, 0.089540f, 0.673887f, + 0.214360f, 0.019258f, 0.771595f, + 0.158999f, 0.001490f, 0.835374f, + 0.176696f, -0.059249f, 0.849218f, + 0.148696f, -0.130091f, 0.793599f, + 0.108290f, -0.166528f, 0.772088f, + 0.049820f, -0.201382f, 0.764454f, + 0.071341f, -0.215195f, 0.697209f, + 0.073148f, -0.214475f, 0.623510f, + 0.140502f, -0.169461f, 0.699354f, + 0.163374f, -0.157073f, 0.611416f, + 0.189466f, -0.138550f, 0.730366f, + 0.247593f, -0.082554f, 0.759610f, + 0.227468f, -0.121982f, 0.590197f, + 0.284702f, -0.006586f, 0.535347f, + 0.275741f, 0.125287f, 0.446676f, + 0.266650f, 0.192594f, 0.506044f, + 0.300086f, 0.053287f, 0.629620f, + 0.055450f, -0.663935f, 0.375065f, + 0.122854f, -0.664138f, 0.482323f, + 0.046520f, -0.531571f, 0.391918f, + 0.024824f, -0.568450f, 0.275106f, + 0.053855f, -0.663931f, 0.328224f, + 0.112829f, -0.453549f, 0.305788f, + 0.131265f, -0.510617f, 0.080746f, + 0.061174f, -0.430716f, -0.042710f, + 0.341019f, -0.532887f, -0.208150f, + 0.347705f, -0.623533f, -0.081139f, + 0.238040f, -0.610732f, -0.038037f, + 0.211764f, -0.514274f, -0.132078f, + 0.120605f, -0.600219f, -0.186856f, + 0.096985f, -0.584476f, -0.293357f, + 0.127621f, -0.581941f, -0.437170f, + 0.165902f, -0.477425f, -0.291453f, + 0.077720f, -0.417975f, -0.220519f, + 0.320892f, -0.506363f, -0.320874f, + 0.248214f, -0.465684f, -0.239842f, + 0.118764f, -0.383338f, -0.187114f, + 0.118816f, -0.430106f, -0.123307f, + 0.094131f, -0.419464f, -0.044777f, + 0.274526f, -0.261706f, 0.005110f, + 0.259842f, -0.283292f, -0.003185f, + 0.222861f, -0.340431f, -0.038210f, + 0.204445f, -0.664380f, 0.513353f, + 0.259286f, -0.664547f, 0.471281f, + 0.185402f, -0.476020f, 0.421718f, + 0.279163f, -0.664604f, 0.417328f, + 0.277157f, -0.528122f, 0.400208f, + 0.183069f, -0.509812f, 0.329995f, + 0.282599f, -0.429210f, 0.059242f, + 0.254816f, -0.664541f, 0.290687f, + 0.271436f, -0.567707f, 0.263966f, + 0.386561f, -0.625221f, -0.216870f, + 0.387086f, -0.630883f, -0.346073f, + 0.380021f, -0.596021f, -0.318679f, + 0.291269f, -0.619007f, -0.585707f, + 0.339280f, -0.571198f, -0.461946f, + 0.400045f, -0.489778f, -0.422640f, + 0.406817f, -0.314349f, -0.371230f, + 0.300588f, -0.281718f, -0.170549f, + 0.290866f, -0.277304f, -0.061905f, + 0.187735f, -0.241545f, 0.509437f, + 0.188032f, -0.287569f, 0.424234f, + 0.227520f, -0.373262f, 0.293102f, + 0.266526f, -0.273650f, 0.039597f, + 0.291592f, -0.291676f, 0.111386f, + 0.291914f, -0.122741f, 0.422683f, + 0.297574f, -0.156119f, 0.373368f, + 0.286603f, -0.232731f, 0.027162f, + 0.364663f, -0.201399f, 0.206850f, + 0.353855f, -0.132408f, 0.149228f, + 0.282208f, -0.019715f, 0.314960f, + 0.331187f, -0.099266f, 0.092701f, + 0.375463f, -0.093120f, -0.006467f, + 0.375917f, -0.101236f, -0.154882f, + 0.466635f, -0.094416f, -0.305669f, + 0.455805f, -0.119881f, -0.460632f, + 0.277465f, -0.604242f, -0.651871f, + 0.261022f, -0.551176f, -0.554667f, + 0.093627f, 0.258494f, -0.920589f, + 0.114248f, 0.310608f, -0.798070f, + 0.144232f, 0.211434f, -0.835001f, + 0.119916f, 0.176940f, -0.951159f, + 0.184061f, 0.101854f, -0.918220f, + 0.092431f, 0.276521f, -0.738231f, + 0.133504f, 0.218403f, -0.758602f, + 0.194987f, 0.097655f, -0.812476f, + 0.185542f, 0.011005f, -0.879202f, + 0.230315f, -0.127450f, -0.884202f, + 0.260471f, 0.255056f, -0.624378f, + 0.351567f, -0.042194f, -0.663976f, + 0.253742f, 0.323524f, -0.433716f, + 0.411612f, 0.132299f, -0.438264f, + 0.270513f, 0.356530f, -0.289984f, + 0.422146f, 0.162819f, -0.273130f, + 0.164724f, 0.237490f, 0.208912f, + 0.253806f, 0.092900f, 0.240640f, + 0.203608f, 0.284597f, 0.096223f, + 0.241006f, 0.343093f, -0.171396f, + 0.356076f, 0.149288f, -0.143443f, + 0.337656f, 0.131992f, 0.066374f +}; + +dTriIndex Indices[IndexCount / 3][3] = { + {126,134,133}, + {342,138,134}, + {133,134,138}, + {126,342,134}, + {312,316,317}, + {169,163,162}, + {312,317,319}, + {312,319,318}, + {169,162,164}, + {169,168,163}, + {312,314,315}, + {169,164,165}, + {169,167,168}, + {312,315,316}, + {312,313,314}, + {169,165,166}, + {169,166,167}, + {312,318,313}, + {308,304,305}, + {308,305,306}, + {179,181,188}, + {177,173,175}, + {177,175,176}, + {302,293,300}, + {322,294,304}, + {188,176,175}, + {188,175,179}, + {158,177,187}, + {305,293,302}, + {305,302,306}, + {322,304,308}, + {188,181,183}, + {158,173,177}, + {293,298,300}, + {304,294,296}, + {304,296,305}, + {185,176,188}, + {185,188,183}, + {187,177,176}, + {187,176,185}, + {305,296,298}, + {305,298,293}, + {436,432, 28}, + {436, 28, 23}, + {434,278,431}, + { 30,208,209}, + { 30,209, 29}, + { 19, 20, 24}, + {208,207,211}, + {208,211,209}, + { 19,210,212}, + {433,434,431}, + {433,431,432}, + {433,432,436}, + {436,437,433}, + {277,275,276}, + {277,276,278}, + {209,210, 25}, + { 21, 26, 24}, + { 21, 24, 20}, + { 25, 26, 27}, + { 25, 27, 29}, + {435,439,277}, + {439,275,277}, + {432,431, 30}, + {432, 30, 28}, + {433,437,438}, + {433,438,435}, + {434,277,278}, + { 24, 25,210}, + { 24, 26, 25}, + { 29, 27, 28}, + { 29, 28, 30}, + { 19, 24,210}, + {208, 30,431}, + {208,431,278}, + {435,434,433}, + {435,277,434}, + { 25, 29,209}, + { 27, 22, 23}, + { 27, 23, 28}, + { 26, 22, 27}, + { 26, 21, 22}, + {212,210,209}, + {212,209,211}, + {207,208,278}, + {207,278,276}, + {439,435,438}, + { 12, 9, 10}, + { 12, 10, 13}, + { 2, 3, 5}, + { 2, 5, 4}, + { 16, 13, 14}, + { 16, 14, 17}, + { 22, 21, 16}, + { 13, 10, 11}, + { 13, 11, 14}, + { 1, 0, 3}, + { 1, 3, 2}, + { 15, 12, 16}, + { 19, 18, 15}, + { 19, 15, 16}, + { 19, 16, 20}, + { 9, 1, 2}, + { 9, 2, 10}, + { 3, 7, 8}, + { 3, 8, 5}, + { 16, 17, 23}, + { 16, 23, 22}, + { 21, 20, 16}, + { 10, 2, 4}, + { 10, 4, 11}, + { 0, 6, 7}, + { 0, 7, 3}, + { 12, 13, 16}, + {451,446,445}, + {451,445,450}, + {442,440,439}, + {442,439,438}, + {442,438,441}, + {421,420,422}, + {412,411,426}, + {412,426,425}, + {408,405,407}, + {413, 67, 68}, + {413, 68,414}, + {391,390,412}, + { 80,384,386}, + {404,406,378}, + {390,391,377}, + {390,377, 88}, + {400,415,375}, + {398,396,395}, + {398,395,371}, + {398,371,370}, + {112,359,358}, + {112,358,113}, + {351,352,369}, + {125,349,348}, + {345,343,342}, + {342,340,339}, + {341,335,337}, + {328,341,327}, + {331,323,333}, + {331,322,323}, + {327,318,319}, + {327,319,328}, + {315,314,324}, + {302,300,301}, + {302,301,303}, + {320,311,292}, + {285,284,289}, + {310,307,288}, + {310,288,290}, + {321,350,281}, + {321,281,282}, + {423,448,367}, + {272,273,384}, + {272,384,274}, + {264,265,382}, + {264,382,383}, + {440,442,261}, + {440,261,263}, + {252,253,254}, + {252,254,251}, + {262,256,249}, + {262,249,248}, + {228,243,242}, + {228, 31,243}, + {213,215,238}, + {213,238,237}, + { 19,212,230}, + {224,225,233}, + {224,233,231}, + {217,218, 56}, + {217, 56, 54}, + {217,216,239}, + {217,239,238}, + {217,238,215}, + {218,217,215}, + {218,215,214}, + { 6,102,206}, + {186,199,200}, + {197,182,180}, + {170,171,157}, + {201,200,189}, + {170,190,191}, + {170,191,192}, + {175,174,178}, + {175,178,179}, + {168,167,155}, + {122,149,158}, + {122,158,159}, + {135,153,154}, + {135,154,118}, + {143,140,141}, + {143,141,144}, + {132,133,136}, + {130,126,133}, + {124,125,127}, + {122,101,100}, + {122,100,121}, + {110,108,107}, + {110,107,109}, + { 98, 99, 97}, + { 98, 97, 64}, + { 98, 64, 66}, + { 87, 55, 57}, + { 83, 82, 79}, + { 83, 79, 84}, + { 78, 74, 50}, + { 49, 71, 41}, + { 49, 41, 37}, + { 49, 37, 36}, + { 58, 44, 60}, + { 60, 59, 58}, + { 51, 34, 33}, + { 39, 40, 42}, + { 39, 42, 38}, + {243,240, 33}, + {243, 33,229}, + { 39, 38, 6}, + { 44, 46, 40}, + { 55, 56, 57}, + { 64, 62, 65}, + { 64, 65, 66}, + { 41, 71, 45}, + { 75, 50, 51}, + { 81, 79, 82}, + { 77, 88, 73}, + { 93, 92, 94}, + { 68, 47, 46}, + { 96, 97, 99}, + { 96, 99, 95}, + {110,109,111}, + {111,112,110}, + {114,113,123}, + {114,123,124}, + {132,131,129}, + {133,137,136}, + {135,142,145}, + {145,152,135}, + {149,147,157}, + {157,158,149}, + {164,150,151}, + {153,163,168}, + {153,168,154}, + {185,183,182}, + {185,182,184}, + {161,189,190}, + {200,199,191}, + {200,191,190}, + {180,178,195}, + {180,195,196}, + {102,101,204}, + {102,204,206}, + { 43, 48,104}, + { 43,104,103}, + {216,217, 54}, + {216, 54, 32}, + {207,224,231}, + {230,212,211}, + {230,211,231}, + {227,232,241}, + {227,241,242}, + {235,234,241}, + {235,241,244}, + {430,248,247}, + {272,274,253}, + {272,253,252}, + {439,260,275}, + {225,224,259}, + {225,259,257}, + {269,270,407}, + {269,407,405}, + {270,269,273}, + {270,273,272}, + {273,269,268}, + {273,268,267}, + {273,267,266}, + {273,266,265}, + {273,265,264}, + {448,279,367}, + {281,350,368}, + {285,286,301}, + {290,323,310}, + {290,311,323}, + {282,281,189}, + {292,311,290}, + {292,290,291}, + {307,306,302}, + {307,302,303}, + {316,315,324}, + {316,324,329}, + {331,351,350}, + {330,334,335}, + {330,335,328}, + {341,337,338}, + {344,355,354}, + {346,345,348}, + {346,348,347}, + {364,369,352}, + {364,352,353}, + {365,363,361}, + {365,361,362}, + {376,401,402}, + {373,372,397}, + {373,397,400}, + {376, 92,377}, + {381,378,387}, + {381,387,385}, + {386, 77, 80}, + {390,389,412}, + {416,417,401}, + {403,417,415}, + {408,429,430}, + {419,423,418}, + {427,428,444}, + {427,444,446}, + {437,436,441}, + {450,445, 11}, + {450, 11, 4}, + {447,449, 5}, + {447, 5, 8}, + {441,438,437}, + {425,426,451}, + {425,451,452}, + {417,421,415}, + {408,407,429}, + {399,403,400}, + {399,400,397}, + {394,393,416}, + {389,411,412}, + {386,383,385}, + {408,387,378}, + {408,378,406}, + {377,391,376}, + { 94,375,415}, + {372,373,374}, + {372,374,370}, + {359,111,360}, + {359,112,111}, + {113,358,349}, + {113,349,123}, + {346,343,345}, + {343,340,342}, + {338,336,144}, + {338,144,141}, + {327,341,354}, + {327,354,326}, + {331,350,321}, + {331,321,322}, + {314,313,326}, + {314,326,325}, + {300,298,299}, + {300,299,301}, + {288,287,289}, + {189,292,282}, + {287,288,303}, + {284,285,297}, + {368,280,281}, + {448,447,279}, + {274,226,255}, + {267,268,404}, + {267,404,379}, + {429,262,430}, + {439,440,260}, + {257,258,249}, + {257,249,246}, + {430,262,248}, + {234,228,242}, + {234,242,241}, + {237,238,239}, + {237,239,236}, + { 15, 18,227}, + { 15,227,229}, + {222,223, 82}, + {222, 82, 83}, + {214,215,213}, + {214,213, 81}, + { 38,102, 6}, + {122,159,200}, + {122,200,201}, + {174,171,192}, + {174,192,194}, + {197,193,198}, + {190,170,161}, + {181,179,178}, + {181,178,180}, + {166,156,155}, + {163,153,152}, + {163,152,162}, + {120,156,149}, + {120,149,121}, + {152,153,135}, + {140,143,142}, + {135,131,132}, + {135,132,136}, + {130,129,128}, + {130,128,127}, + {100,105,119}, + {100,119,120}, + {106,104,107}, + {106,107,108}, + { 91, 95, 59}, + { 93, 94, 68}, + { 91, 89, 92}, + { 76, 53, 55}, + { 76, 55, 87}, + { 81, 78, 79}, + { 74, 73, 49}, + { 69, 60, 45}, + { 58, 62, 64}, + { 58, 64, 61}, + { 53, 31, 32}, + { 32, 54, 53}, + { 42, 43, 38}, + { 35, 36, 0}, + { 35, 0, 1}, + { 34, 35, 1}, + { 34, 1, 9}, + { 44, 40, 41}, + { 44, 41, 45}, + { 33,240, 51}, + { 63, 62, 58}, + { 63, 58, 59}, + { 45, 71, 70}, + { 76, 75, 51}, + { 76, 51, 52}, + { 86, 85, 84}, + { 86, 84, 87}, + { 89, 72, 73}, + { 89, 73, 88}, + { 91, 92, 96}, + { 91, 96, 95}, + { 72, 91, 60}, + { 72, 60, 69}, + {104,106,105}, + {119,105,117}, + {119,117,118}, + {124,127,128}, + {117,116,129}, + {117,129,131}, + {118,117,131}, + {135,140,142}, + {146,150,152}, + {146,152,145}, + {149,122,121}, + {166,165,151}, + {166,151,156}, + {158,172,173}, + {161,160,189}, + {199,198,193}, + {199,193,191}, + {204,201,202}, + {178,174,194}, + {200,159,186}, + {109, 48, 67}, + { 48,107,104}, + {216, 32,236}, + {216,236,239}, + {223,214, 81}, + {223, 81, 82}, + { 33, 12, 15}, + { 32,228,234}, + { 32,234,236}, + {240, 31, 52}, + {256,255,246}, + {256,246,249}, + {258,263,248}, + {258,248,249}, + {275,260,259}, + {275,259,276}, + {207,276,259}, + {270,271,429}, + {270,429,407}, + {413,418,366}, + {413,366,365}, + {368,367,279}, + {368,279,280}, + {303,301,286}, + {303,286,287}, + {283,282,292}, + {283,292,291}, + {320,292,189}, + {298,296,297}, + {298,297,299}, + {318,327,326}, + {318,326,313}, + {329,330,317}, + {336,333,320}, + {326,354,353}, + {334,332,333}, + {334,333,336}, + {342,339,139}, + {342,139,138}, + {345,342,126}, + {347,357,356}, + {369,368,351}, + {363,356,357}, + {363,357,361}, + {366,367,368}, + {366,368,369}, + {375,373,400}, + { 92, 90,377}, + {409,387,408}, + {386,385,387}, + {386,387,388}, + {412,394,391}, + {396,398,399}, + {408,406,405}, + {415,421,419}, + {415,419,414}, + {425,452,448}, + {425,448,424}, + {444,441,443}, + {448,452,449}, + {448,449,447}, + {446,444,443}, + {446,443,445}, + {250,247,261}, + {250,261,428}, + {421,422,423}, + {421,423,419}, + {427,410,250}, + {417,403,401}, + {403,402,401}, + {420,392,412}, + {420,412,425}, + {420,425,424}, + {386,411,389}, + {383,382,381}, + {383,381,385}, + {378,379,404}, + {372,371,395}, + {372,395,397}, + {371,372,370}, + {361,359,360}, + {361,360,362}, + {368,350,351}, + {349,347,348}, + {356,355,344}, + {356,344,346}, + {344,341,340}, + {344,340,343}, + {338,337,336}, + {328,335,341}, + {324,352,351}, + {324,351,331}, + {320,144,336}, + {314,325,324}, + {322,308,309}, + {310,309,307}, + {287,286,289}, + {203,280,279}, + {203,279,205}, + {297,295,283}, + {297,283,284}, + {447,205,279}, + {274,384, 80}, + {274, 80,226}, + {266,267,379}, + {266,379,380}, + {225,257,246}, + {225,246,245}, + {256,254,253}, + {256,253,255}, + {430,247,250}, + {226,235,244}, + {226,244,245}, + {232,233,244}, + {232,244,241}, + {230, 18, 19}, + { 32, 31,228}, + {219,220, 86}, + {219, 86, 57}, + {226,213,235}, + {206, 7, 6}, + {122,201,101}, + {201,204,101}, + {180,196,197}, + {170,192,171}, + {200,190,189}, + {194,193,195}, + {183,181,180}, + {183,180,182}, + {155,154,168}, + {149,156,151}, + {149,151,148}, + {155,156,120}, + {145,142,143}, + {145,143,146}, + {136,137,140}, + {133,132,130}, + {128,129,116}, + {100,120,121}, + {110,112,113}, + {110,113,114}, + { 66, 65, 63}, + { 66, 63, 99}, + { 66, 99, 98}, + { 96, 46, 61}, + { 89, 88, 90}, + { 86, 87, 57}, + { 80, 78, 81}, + { 72, 69, 49}, + { 67, 48, 47}, + { 67, 47, 68}, + { 56, 55, 53}, + { 50, 49, 36}, + { 50, 36, 35}, + { 40, 39, 41}, + {242,243,229}, + {242,229,227}, + { 6, 37, 39}, + { 42, 47, 48}, + { 42, 48, 43}, + { 61, 46, 44}, + { 45, 70, 69}, + { 69, 70, 71}, + { 69, 71, 49}, + { 74, 78, 77}, + { 83, 84, 85}, + { 73, 74, 77}, + { 93, 96, 92}, + { 68, 46, 93}, + { 95, 99, 63}, + { 95, 63, 59}, + {115,108,110}, + {115,110,114}, + {125,126,127}, + {129,130,132}, + {137,133,138}, + {137,138,139}, + {148,146,143}, + {148,143,147}, + {119,118,154}, + {161,147,143}, + {165,164,151}, + {158,157,171}, + {158,171,172}, + {159,158,187}, + {159,187,186}, + {194,192,191}, + {194,191,193}, + {189,202,201}, + {182,197,184}, + {205, 8, 7}, + { 48,109,107}, + {218,219, 57}, + {218, 57, 56}, + {207,231,211}, + {232,230,231}, + {232,231,233}, + { 53, 52, 31}, + {388,411,386}, + {409,430,250}, + {262,429,254}, + {262,254,256}, + {442,444,428}, + {273,264,383}, + {273,383,384}, + {429,271,251}, + {429,251,254}, + {413,365,362}, + { 67,413,360}, + {282,283,295}, + {285,301,299}, + {202,281,280}, + {284,283,291}, + {284,291,289}, + {320,189,160}, + {308,306,307}, + {307,309,308}, + {319,317,330}, + {319,330,328}, + {353,352,324}, + {332,331,333}, + {340,341,338}, + {354,341,344}, + {349,358,357}, + {349,357,347}, + {364,355,356}, + {364,356,363}, + {364,365,366}, + {364,366,369}, + {374,376,402}, + {375, 92,373}, + { 77,389,390}, + {382,380,381}, + {389, 77,386}, + {393,394,412}, + {393,412,392}, + {401,394,416}, + {415,400,403}, + {411,410,427}, + {411,427,426}, + {422,420,424}, + {247,248,263}, + {247,263,261}, + {445,443, 14}, + {445, 14, 11}, + {449,450, 4}, + {449, 4, 5}, + {443,441, 17}, + {443, 17, 14}, + {436, 23, 17}, + {436, 17,441}, + {424,448,422}, + {448,423,422}, + {414,419,418}, + {414,418,413}, + {406,404,405}, + {399,397,395}, + {399,395,396}, + {420,416,392}, + {388,410,411}, + {386,384,383}, + {390, 88, 77}, + {375, 94, 92}, + {415,414, 68}, + {415, 68, 94}, + {370,374,402}, + {370,402,398}, + {361,357,358}, + {361,358,359}, + {125,348,126}, + {346,344,343}, + {340,338,339}, + {337,335,334}, + {337,334,336}, + {325,353,324}, + {324,331,332}, + {324,332,329}, + {323,322,309}, + {323,309,310}, + {294,295,297}, + {294,297,296}, + {289,286,285}, + {202,280,203}, + {288,307,303}, + {282,295,321}, + { 67,360,111}, + {418,423,367}, + {418,367,366}, + {272,252,251}, + {272,251,271}, + {272,271,270}, + {255,253,274}, + {265,266,380}, + {265,380,382}, + {442,428,261}, + {440,263,258}, + {440,258,260}, + {409,250,410}, + {255,226,245}, + {255,245,246}, + { 31,240,243}, + {236,234,235}, + {236,235,237}, + {233,225,245}, + {233,245,244}, + {220,221, 85}, + {220, 85, 86}, + { 81,213,226}, + { 81,226, 80}, + { 7,206,205}, + {186,184,198}, + {186,198,199}, + {204,203,205}, + {204,205,206}, + {195,193,196}, + {171,174,172}, + {173,174,175}, + {173,172,174}, + {155,167,166}, + {160,161,143}, + {160,143,144}, + {119,154,155}, + {148,151,150}, + {148,150,146}, + {140,137,139}, + {140,139,141}, + {127,126,130}, + {114,124,128}, + {114,128,115}, + {117,105,106}, + {117,106,116}, + {104,105,100}, + {104,100,103}, + { 59, 60, 91}, + { 97, 96, 61}, + { 97, 61, 64}, + { 91, 72, 89}, + { 87, 84, 79}, + { 87, 79, 76}, + { 78, 80, 77}, + { 49, 50, 74}, + { 60, 44, 45}, + { 61, 44, 58}, + { 51, 50, 35}, + { 51, 35, 34}, + { 39, 37, 41}, + { 33, 34, 9}, + { 33, 9, 12}, + { 0, 36, 37}, + { 0, 37, 6}, + { 40, 46, 47}, + { 40, 47, 42}, + { 53, 54, 56}, + { 65, 62, 63}, + { 72, 49, 73}, + { 79, 78, 75}, + { 79, 75, 76}, + { 52, 53, 76}, + { 92, 89, 90}, + { 96, 93, 46}, + {102,103,100}, + {102,100,101}, + {116,106,108}, + {116,108,115}, + {123,125,124}, + {116,115,128}, + {118,131,135}, + {140,135,136}, + {148,147,149}, + {120,119,155}, + {164,162,152}, + {164,152,150}, + {157,147,161}, + {157,161,170}, + {186,187,185}, + {186,185,184}, + {193,197,196}, + {202,203,204}, + {194,195,178}, + {198,184,197}, + { 67,111,109}, + { 38, 43,103}, + { 38,103,102}, + {214,223,222}, + {214,222,221}, + {214,221,220}, + {214,220,219}, + {214,219,218}, + {213,237,235}, + {221,222, 83}, + {221, 83, 85}, + { 15,229, 33}, + {227, 18,230}, + {227,230,232}, + { 52, 51,240}, + { 75, 78, 50}, + {408,430,409}, + {260,258,257}, + {260,257,259}, + {224,207,259}, + {268,269,405}, + {268,405,404}, + {413,362,360}, + {447, 8,205}, + {299,297,285}, + {189,281,202}, + {290,288,289}, + {290,289,291}, + {322,321,295}, + {322,295,294}, + {333,323,311}, + {333,311,320}, + {317,316,329}, + {320,160,144}, + {353,325,326}, + {329,332,334}, + {329,334,330}, + {339,338,141}, + {339,141,139}, + {348,345,126}, + {347,356,346}, + {123,349,125}, + {364,353,354}, + {364,354,355}, + {365,364,363}, + {376,391,394}, + {376,394,401}, + { 92,376,374}, + { 92,374,373}, + {377, 90, 88}, + {380,379,378}, + {380,378,381}, + {388,387,409}, + {388,409,410}, + {416,393,392}, + {399,398,402}, + {399,402,403}, + {250,428,427}, + {421,417,416}, + {421,416,420}, + {426,427,446}, + {426,446,451}, + {444,442,441}, + {452,451,450}, + {452,450,449} +}; + diff --git a/libs/ode-0.16.1/ode/demo/convex_bunny_geom.h b/libs/ode-0.16.1/ode/demo/convex_bunny_geom.h new file mode 100644 index 0000000..09fe6de --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/convex_bunny_geom.h @@ -0,0 +1,468 @@ +const unsigned int convexBunnyPlaneCount = 176; +dReal convexBunnyPlanes[] = +{ + 0.986167, -0.0612533, -0.154021, 0.399481, + 0.982735, -0.0691036, -0.171628, 0.409884, + -0.984387, -0.0582774, -0.166089, 0.403079, + 0.985044, -0.172279, 0.00302105, 0.437531, + 0.976915, -0.184361, 0.107929, 0.465334, + 0.951478, -0.281619, 0.124019, 0.475223, + 0.798136, -0.502214, 0.332805, 0.535555, + 0.949728, -0.211128, -0.231176, 0.528156, + -0.000894561, -0.995066, -0.0992088, 0.80299, + -0.000896015, -0.995066, -0.0992131, 0.802991, + -0.0035709, -0.935822, 0.352454, 0.618504, + -0.291551, -0.828515, 0.478081, 0.681579, + -0.978715, -0.181408, 0.0959605, 0.468907, + -0.985523, -0.169302, -0.00904739, 0.441129, + -0.953768, -0.278749, 0.11236, 0.478704, + 0.372745, -0.827499, 0.419888, 0.630174, + 0.976911, -0.18316, 0.109991, 0.466086, + 0.817827, -0.387738, 0.425227, 0.569153, + -0.978732, -0.180211, 0.0980152, 0.469656, + 0.662794, -0.0277654, 0.748287, 0.803459, + 0.00359857, -0.660581, -0.750746, 0.877267, + 0.00359952, -0.660579, -0.750748, 0.877266, + -0.947456, -0.208272, -0.242797, 0.531628, + -0.980764, -0.0661375, -0.18365, 0.413468, + -0.940835, -0.0698657, -0.331585, 0.494827, + 0.983751, 0.0529367, -0.171556, 0.403533, + 0.981839, 0.0996302, -0.16145, 0.410144, + 0.977938, 0.0857834, -0.190468, 0.411823, + 0.959636, 0.106068, -0.260476, 0.44898, + 0.85334, -0.0495414, -0.518996, 0.60489, + -0.803667, -0.499793, 0.322995, 0.538478, + -0.689742, -0.615484, 0.381361, 0.574754, + -0.380353, -0.826364, 0.415277, 0.63155, + -0.39985, -0.817283, 0.414933, 0.630682, + -0.380309, -0.826285, 0.415473, 0.631677, + -0.981411, 0.0559147, -0.183592, 0.407121, + -0.979526, 0.101914, -0.173615, 0.413635, + -0.975381, 0.0881975, -0.202119, 0.415257, + 0.988445, 0.140182, 0.0576755, 0.485174, + 0.876515, 0.0408992, 0.479634, 0.620093, + 0.848907, -0.0996824, 0.519057, 0.631268, + 0.895754, -0.195088, 0.399457, 0.563346, + 0.861448, -0.256989, 0.438023, 0.574909, + 0.775672, -0.447481, 0.445076, 0.586422, + 0.683157, -0.617557, 0.389768, 0.572247, + 0.391755, -0.818722, 0.419789, 0.629264, + 0.28317, -0.829384, 0.481599, 0.680529, + 0.3727, -0.827422, 0.420079, 0.630298, + -0.824143, -0.385255, 0.415171, 0.57215, + -0.782413, -0.445128, 0.435536, 0.589267, + 0.00152876, 0.999994, -0.00308275, 0.665646, + 0.00242466, 0.999989, -0.0038994, 0.665879, + -0.979892, 0.121321, -0.158406, 0.420287, + 0.767537, -0.190214, -0.612132, 0.695479, + 0.372649, -0.42747, -0.823652, 0.869878, + 0.537245, -0.335515, -0.77382, 0.82472, + 0.0263648, -0.598975, -0.800334, 0.873623, + 0.00393345, -0.60959, -0.792707, 0.869865, + -0.0183706, -0.598907, -0.800608, 0.873704, + 0.00875728, 0.676014, -0.736836, 0.825597, + 0.852333, -0.0355955, -0.521786, 0.607886, + 0.392036, 0.534934, -0.748434, 0.818042, + 0.847696, -0.122973, -0.516033, 0.615814, + 0.884763, -0.0760716, -0.45979, 0.565893, + 0.9446, -0.0727144, -0.320069, 0.491401, + 0.904971, -0.0675211, -0.420081, 0.541673, + -0.899959, -0.0647928, -0.431134, 0.544968, + -0.955972, 0.108494, -0.272667, 0.452769, + -0.363823, -0.426358, -0.828161, 0.871222, + -0.528689, -0.333936, -0.780368, 0.826685, + 0.982068, 0.118277, -0.146811, 0.416542, + 0.98951, 0.144455, 0.00164104, 0.468616, + 0.50797, -0.0708041, 0.85846, 0.883126, + 0.748614, -0.431275, 0.503565, 0.634026, + 0.214863, -0.405791, 0.888351, 0.94048, + -0.901162, -0.192379, 0.388455, 0.566627, + -0.867521, -0.254376, 0.427435, 0.578065, + -0.226957, -0.405135, 0.885639, 0.941284, + -0.756029, -0.429007, 0.494341, 0.636765, + -0.00629553, -0.188362, 0.982079, 0.968197, + 0.0165684, 0.999846, -0.00581961, 0.670373, + 0.00313267, 0.999987, -0.00405124, 0.666103, + 0.545069, 0.472158, -0.692796, 0.780052, + 0.932011, 0.148856, -0.330451, 0.498417, + 0.844043, 0.227673, -0.485547, 0.599903, + -0.019033, 0.999801, -0.00590978, 0.672252, + -0.959662, 0.239262, -0.147656, 0.488538, + 0.00151234, 0.999999, -0.000399466, 0.665855, + -0.988649, 0.143168, 0.0455675, 0.488784, + -0.989015, 0.147444, -0.01048, 0.472227, + -0.972439, 0.232727, -0.01415, 0.518344, + 0.587681, -0.160147, -0.793085, 0.823999, + 0.640479, -0.269179, -0.719256, 0.778142, + 0.541109, -0.332896, -0.772257, 0.823226, + 0.546185, -0.14771, -0.824538, 0.84854, + 0.528519, -0.026044, -0.848522, 0.873136, + 0.447231, -0.0756684, -0.891212, 0.903953, + 0.490619, -0.0795123, -0.867739, 0.884255, + 0.279393, 0.264257, -0.923097, 0.950045, + 0.374653, 0.39486, -0.83888, 0.886593, + 0.00050174, 0.999994, -0.00331563, 0.665976, + -0.0103777, 0.999934, -0.00487936, 0.669494, + -0.927571, 0.150741, -0.341889, 0.501807, + -0.267268, 0.265084, -0.926444, 0.951043, + -0.845984, -0.0330207, -0.532184, 0.610986, + -0.518165, -0.0244575, -0.854931, 0.875047, + -0.83753, 0.228953, -0.496108, 0.603116, + -0.535666, 0.472122, -0.700116, 0.78259, + -0.381083, 0.534649, -0.754272, 0.820328, + -0.363157, 0.395975, -0.843398, 0.88794, + -0.326829, -0.256634, -0.909572, 0.922946, + 0.394875, -0.128601, -0.90969, 0.920236, + 0.337169, -0.257642, -0.905504, 0.921733, + 0.398433, -0.193767, -0.896496, 0.910015, + -0.536477, -0.146072, -0.831177, 0.850523, + -0.436512, -0.0743406, -0.896622, 0.905564, + -0.480187, -0.0780522, -0.873687, 0.886029, + -0.384093, -0.127432, -0.914458, 0.921656, + -0.388009, -0.192572, -0.901313, 0.911451, + 0.977045, 0.15796, 0.14294, 0.521934, + 0.930035, 0.231515, 0.28537, 0.600301, + -0.855499, -0.0971121, 0.508616, 0.634376, + -0.875419, 0.136849, 0.463589, 0.62897, + -0.882196, 0.0435387, 0.468864, 0.623303, + 0.0204398, -0.0238739, 0.999506, 0.958419, + -0.0062197, -0.0777937, 0.99695, 0.962769, + 0.907123, 0.250746, 0.338015, 0.623205, + 0.902358, 0.173321, 0.394601, 0.607696, + 0.870085, 0.134211, 0.474278, 0.625782, + 0.0015108, 0.999999, 6.34978e-06, 0.665945, + 0.00150567, 0.999999, 0.000568537, 0.666143, + 0.00150738, 0.999999, 0.000565012, 0.666141, + -0.963954, 0.266039, -0.00405118, 0.539571, + 0.0015136, 0.999999, -0.000393102, 0.665856, + 0.00151117, 0.999999, 4.32104e-06, 0.665944, + 0.0272711, 0.999604, -0.00696439, 0.673709, + 0.962047, 0.236383, -0.136341, 0.484917, + 0.973236, 0.229796, -0.00223071, 0.514797, + 0.964728, 0.263133, 0.00776342, 0.536054, + -0.906596, 0.176056, 0.383521, 0.610999, + -0.978237, 0.160918, 0.130987, 0.525513, + -0.910434, 0.253486, 0.326887, 0.626522, + -0.932759, 0.234321, 0.27396, 0.603697, + 0.800621, -0.0921333, -0.592045, 0.665278, + 0.679569, -0.15358, -0.717356, 0.765121, + 0.684928, -0.199829, -0.700672, 0.756959, + -0.532604, -0.33128, -0.778837, 0.82519, + -0.578395, -0.158384, -0.800234, 0.826134, + -0.671209, -0.151537, -0.725613, 0.767576, + -0.00530287, 0.323551, 0.946196, 0.94547, + -0.719766, 0.229227, 0.655281, 0.774388, + -0.604194, 0.29171, 0.741522, 0.841564, + -0.544989, 0.302925, 0.781808, 0.866398, + -0.518662, -0.0692529, 0.85217, 0.884999, + -0.671987, -0.0257512, 0.740115, 0.805898, + -0.00613626, -0.00823685, 0.999947, 0.957141, + -0.032747, -0.0237908, 0.99918, 0.958516, + -0.00530611, 0.323546, 0.946198, 0.945471, + -0.545061, 0.302657, 0.781861, 0.866377, + -0.639902, 0.23832, 0.730568, 0.823722, + 0.00149706, 0.999997, 0.00193434, 0.667038, + 0.00149731, 0.999997, 0.00193252, 0.667037, + -0.0048341, 0.44008, 0.897946, 0.936327, + -0.00483143, 0.440078, 0.897947, 0.936327, + -0.632454, -0.267241, -0.727039, 0.780455, + -0.67691, -0.197765, -0.709, 0.759436, + -0.841667, -0.120432, -0.526396, 0.618913, + -0.760706, -0.187792, -0.621337, 0.698143, + -0.87929, -0.0734062, -0.470597, 0.569116, + -0.847068, -0.0469762, -0.529405, 0.607991, + -0.793572, -0.0897399, -0.601823, 0.668201, + 0.536355, 0.301024, 0.788485, 0.864403, + 0.536284, 0.301291, 0.788431, 0.864424, + 0.595945, 0.289897, 0.748872, 0.839373, + 0.631626, 0.236397, 0.738353, 0.8214, + 0.712378, 0.227061, 0.664049, 0.771772, +}; +const unsigned int convexBunnyPointCount = 105; +dReal convexBunnyPoints[] = +{ + -0.459488, -0.093017, -0.311341, + 0.466635, -0.094416, -0.305669, + -0.309239, 0.776868, 0.304726, + -0.004458, -0.042526, 1.01567, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.007957, 0.282241, -0.93168, + 0.204445, -0.66438, 0.513353, + -0.303961, 0.054199, 0.625921, + 0.265619, 0.756464, 0.504187, + -0.402162, 0.133528, -0.443247, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.266772, 0.64233, 0.602061, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.411612, 0.132299, -0.438264, + 0.31148, 0.775931, 0.308527, + 0.300086, 0.053287, 0.62962, + -0.414624, 0.164083, -0.278254, + -0.248382, 0.255825, -0.627493, + -0.216201, -0.126776, -0.886936, + 0.267564, -0.666174, -0.654834, + -0.135892, -0.03552, 0.945455, + -0.265837, 0.757267, 0.500933, + -0.003873, 0.161605, 0.970499, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.282599, -0.663393, 0.412411, + 0.007237, 0.361687, -0.794439, + 0.093627, 0.258494, -0.920589, + 0.422146, 0.162819, -0.27313, + 0.279163, -0.664604, 0.417328, + 0.263086, 0.512567, 0.637832, + -0.099875, 0.310931, -0.799381, + -0.446838, -0.118517, -0.466159, + -0.168842, 0.102387, -0.920381, + 0.455805, -0.119881, -0.460632, + 0.337743, -0.666396, -0.074503, + -0.134547, -0.119852, -0.959004, + -0.183807, 0.19697, 0.84448, + 0.264969, 0.641527, 0.605317, + -0.209063, -0.663393, 0.509344, + -0.364126, -0.200299, 0.202388, + -0.253475, -0.081797, 0.756541, + 0.260471, 0.255056, -0.624378, + 0.114248, 0.310608, -0.79807, + 0.364663, -0.201399, 0.20685, + 0.127847, -0.035919, 0.94707, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.381071, -0.629723, -0.350777, + -0.339884, -0.04115, -0.668211, + -0.077913, 0.258753, -0.92164, + 0.184061, 0.101854, -0.91822, + -0.335166, -0.66538, -0.078623, + 0.386561, -0.625221, -0.21687, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.241585, 0.527592, 0.669296, + -0.086969, 0.133224, 0.947633, + -0.003127, 0.28407, 0.87887, + -0.004433, -0.146642, 0.985872, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.138444, -0.10425, 0.945975, + -0.265676, 0.513366, 0.634594, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.247593, -0.082554, 0.75961, + 0.07941, 0.132973, 0.948652, + 0.238615, 0.526867, 0.672237, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.382112, -0.62406, -0.221577, + -0.104072, 0.177278, -0.95253, + 0.351567, -0.042194, -0.663976, + 0.138234, -0.293905, -0.897958, + 0.119916, 0.17694, -0.951159, + -0.371322, -0.665382, -0.35362, + -0.263384, -0.663396, 0.466604, + 0.376722, -0.666513, -0.219833, + 0.387086, -0.630883, -0.346073, + -0.125544, 0.140012, 0.917678, + -0.070612, 0.036849, 0.975733, + -0.083497, -0.084934, 0.979607, + 0.259286, -0.664547, 0.471281, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.074888, -0.085173, 0.980577, + 0.152305, 0.125256, 0.890786, + 0.130184, -0.104656, 0.94762, + -0.004249, 0.046042, 1.00324, + 0.062419, 0.036648, 0.976547, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.392666, -0.488581, -0.427494, + 0.230315, -0.12745, -0.884202, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + 0.193434, -0.665946, -0.715325, + 0.007865, 0.122104, -0.956137, + 8.40779e-45, 3.00321e-39, 2.8026e-44, + -0.257884, -0.665381, -0.658052, + 0.377265, -0.666513, -0.349036, + -0.372362, -0.665381, -0.22442, + 0.400045, -0.489778, -0.42264, + -0.159174, 0.125726, 0.888878, + 0.118369, 0.139643, 0.919173, + -0.124463, -0.293508, -0.899566, + 0.21172, -0.302754, -0.843303, + 0.149571, -0.120281, -0.957264, + -0.183019, -0.665378, -0.71763, + 0.177696, 0.196424, 0.846693, + -0.198638, -0.302135, -0.845816, +}; +unsigned int convexBunnyPolygons[] = +{ + 3, 7, 2, 0, + 3, 2, 7, 11, + 3, 1, 15, 16, + 3, 0, 2, 17, + 3, 17, 9, 0, + 3, 2, 9, 17, + 3, 18, 9, 2, + 3, 2, 11, 22, + 3, 22, 15, 2, + 3, 8, 15, 22, + 3, 2, 15, 26, + 3, 5, 26, 27, + 3, 1, 14, 28, + 3, 28, 15, 1, + 3, 14, 15, 28, + 3, 2, 26, 31, + 3, 0, 9, 32, + 3, 9, 18, 33, + 3, 34, 14, 1, + 3, 19, 33, 36, + 3, 8, 22, 38, + 3, 38, 22, 11, + 3, 38, 15, 8, + 3, 38, 16, 15, + 3, 38, 30, 16, + 3, 40, 7, 0, + 3, 0, 25, 40, + 3, 40, 25, 7, + 3, 7, 25, 41, + 3, 21, 37, 41, + 3, 42, 15, 14, + 3, 42, 27, 15, + 3, 43, 26, 15, + 3, 15, 27, 43, + 3, 43, 27, 26, + 3, 1, 16, 44, + 3, 44, 29, 1, + 3, 16, 29, 44, + 3, 0, 32, 47, + 3, 19, 32, 48, + 3, 48, 33, 19, + 3, 48, 32, 9, + 3, 9, 33, 48, + 3, 49, 33, 18, + 3, 49, 18, 2, + 3, 2, 31, 49, + 3, 49, 26, 5, + 3, 49, 31, 26, + 3, 50, 42, 14, + 3, 27, 42, 50, + 3, 51, 35, 6, + 3, 6, 39, 51, + 3, 1, 29, 52, + 3, 11, 37, 54, + 3, 55, 23, 11, + 3, 11, 54, 55, + 3, 11, 23, 56, + 3, 56, 38, 11, + 3, 23, 38, 56, + 3, 57, 39, 6, + 3, 21, 41, 59, + 3, 39, 57, 59, + 3, 60, 37, 11, + 3, 60, 41, 37, + 3, 60, 11, 7, + 3, 7, 41, 60, + 3, 16, 30, 62, + 3, 62, 29, 16, + 3, 63, 38, 23, + 3, 38, 63, 64, + 3, 67, 25, 0, + 3, 0, 47, 67, + 3, 68, 36, 33, + 3, 33, 49, 68, + 3, 68, 49, 5, + 3, 14, 34, 69, + 3, 69, 50, 14, + 3, 5, 27, 71, + 3, 27, 50, 71, + 3, 71, 68, 5, + 3, 25, 51, 73, + 3, 73, 51, 39, + 3, 39, 59, 73, + 3, 73, 41, 25, + 3, 73, 59, 41, + 3, 29, 35, 74, + 3, 74, 52, 29, + 3, 35, 51, 74, + 3, 75, 34, 1, + 3, 1, 52, 75, + 3, 52, 74, 75, + 3, 21, 55, 76, + 3, 76, 54, 37, + 3, 76, 55, 54, + 3, 77, 55, 21, + 3, 21, 59, 78, + 3, 3, 77, 78, + 3, 78, 77, 21, + 3, 78, 57, 3, + 3, 78, 59, 57, + 3, 6, 35, 79, + 3, 79, 35, 29, + 3, 29, 62, 79, + 3, 3, 57, 81, + 3, 83, 62, 45, + 3, 45, 81, 83, + 3, 83, 79, 62, + 3, 6, 79, 83, + 3, 83, 57, 6, + 3, 83, 81, 57, + 3, 84, 63, 23, + 3, 84, 77, 3, + 3, 23, 55, 84, + 3, 55, 77, 84, + 3, 45, 63, 85, + 3, 3, 81, 85, + 3, 85, 81, 45, + 3, 85, 84, 3, + 3, 63, 84, 85, + 3, 87, 47, 32, + 3, 87, 72, 47, + 3, 50, 69, 88, + 3, 88, 34, 20, + 3, 88, 69, 34, + 3, 36, 68, 91, + 3, 68, 71, 91, + 3, 72, 87, 93, + 3, 93, 87, 32, + 3, 93, 32, 19, + 3, 94, 74, 72, + 3, 94, 93, 20, + 3, 72, 93, 94, + 3, 94, 75, 74, + 3, 95, 74, 51, + 3, 72, 74, 95, + 3, 95, 51, 25, + 3, 25, 67, 95, + 3, 95, 67, 47, + 3, 47, 72, 95, + 3, 20, 34, 96, + 3, 34, 75, 96, + 3, 96, 94, 20, + 3, 75, 94, 96, + 3, 97, 37, 21, + 3, 21, 76, 97, + 3, 97, 76, 37, + 3, 98, 64, 63, + 3, 98, 63, 45, + 3, 45, 82, 98, + 3, 36, 70, 99, + 3, 100, 88, 20, + 3, 20, 90, 100, + 3, 100, 90, 70, + 3, 101, 71, 50, + 3, 50, 88, 101, + 3, 36, 91, 101, + 3, 101, 91, 71, + 3, 101, 70, 36, + 3, 101, 100, 70, + 3, 88, 100, 101, + 3, 102, 90, 20, + 3, 20, 93, 102, + 3, 70, 90, 102, + 3, 102, 99, 70, + 3, 64, 98, 103, + 3, 103, 98, 82, + 3, 30, 38, 103, + 3, 38, 64, 103, + 3, 103, 62, 30, + 3, 45, 62, 103, + 3, 103, 82, 45, + 3, 36, 99, 104, + 3, 99, 102, 104, + 3, 104, 102, 93, + 3, 19, 36, 104, + 3, 104, 93, 19, +}; diff --git a/libs/ode-0.16.1/ode/demo/convex_prism.h b/libs/ode-0.16.1/ode/demo/convex_prism.h new file mode 100644 index 0000000..1ee7fcb --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/convex_prism.h @@ -0,0 +1,28 @@ +unsigned int prism_pointcount = 8; +unsigned int prism_planecount = 6; +dReal prism_points[24]={ + 10.0, 1.0,-1.0, + 10.0,-1.0,-1.0, +-10.0,-1.0,-1.0, +-10.0, 1.0,-1.0, + 10.0, 1.0, 1.0, + 10.0,-1.0, 1.0, +-10.0,-1.0, 1.0, +-10.0, 1.0, 1.0 +}; +unsigned int prism_polygons[]={ +4,0,1,2,3, +4,4,7,6,5, +4,0,4,5,1, +4,1,5,6,2, +4,2,6,7,3, +4,4,0,3,7, +}; +dReal prism_planes[]={ +0.0,0.0,-1.0,1.0, +0.0,0.0,1.0,1.0, +1.0,0.0,0.0,10.0, +0.0,-1.0,0.0,1.0, +-1.0,0.0,-0.0,10.0, +0.0,1.0,0.0,1.0, +}; diff --git a/libs/ode-0.16.1/ode/demo/demo_I.cpp b/libs/ode-0.16.1/ode/demo/demo_I.cpp new file mode 100644 index 0000000..156a4ad --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_I.cpp @@ -0,0 +1,253 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +test that the rotational physics is correct. + +an "anchor body" has a number of other randomly positioned bodies +("particles") attached to it by ball-and-socket joints, giving it some +random effective inertia tensor. the effective inertia matrix is calculated, +and then this inertia is assigned to another "test" body. a random torque is +applied to both bodies and the difference in angular velocity and orientation +is observed after a number of iterations. + +typical errors for each test cycle are about 1e-5 ... 1e-4. + +*/ + + +#include +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10 // number of particles +#define SIDE 0.1 // visual size of the particles + + +// dynamics objects an globals + +static dWorldID world=0; +static dBodyID anchor_body,particle[NUM],test_body; +static dJointID particle_joint[NUM]; +static dReal torque[3]; +static int iteration; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.5572f,-1.8886f,1.5700f}; + static float hpr[3] = {118.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// compute the mass parameters of a particle set. q = particle positions, +// pm = particle masses + +#define _I(i,j) I[(i)*4+(j)] + +void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM]) +{ + int i,j; + dMassSetZero (m); + for (i=0; imass += pm[i]; + for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j]; + m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]); + m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]); + m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]); + m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]); + m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]); + m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]); + } + for (j=0; j<3; j++) m->c[j] /= m->mass; + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); +} + + +void reset_test() +{ + int i; + dMass m,anchor_m; + dReal q[NUM][3], pm[NUM]; // particle positions and masses + dReal pos1[3] = {1,0,1}; // point of reference (POR) + dReal pos2[3] = {-1,0,1}; // point of reference (POR) + + // make random particle positions (relative to POR) and masses + for (i=0; i= 100) { + // measure the difference between the anchor and test bodies + const dReal *w1 = dBodyGetAngularVel (anchor_body); + const dReal *w2 = dBodyGetAngularVel (test_body); + const dReal *q1 = dBodyGetQuaternion (anchor_body); + const dReal *q2 = dBodyGetQuaternion (test_body); + dReal maxdiff = dMaxDifference (w1,w2,1,3); + printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n", + maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]); + maxdiff = dMaxDifference (q1,q2,1,4); + printf ("q-error = %.4e\n",maxdiff); + reset_test(); + } + } + + dReal sides[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE}; + dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE}; + dsSetColor (1,1,1); + dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body), + sides3); + dsSetColor (1,0,0); + dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2); + dsSetColor (1,1,0); + for (int i=0; i +#ifdef HAVE_UNISTD_H +#include +#endif +#include +#include +#include "texturepath.h" +#include "basket_geom.h" // this is our world mesh + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// some constants + +#define RADIUS 0.14 + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; + +static dBodyID sphbody; +static dGeomID sphgeom; + +static dJointGroupID contactgroup; +static dGeomID world_mesh; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + +// fprintf(stderr,"testing geoms %p %p\n", o1, o2); + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#include "icosahedron_geom.h" + + +//<---- Convex Object +dReal planes[]= // planes for a cube, these should coincide with the face array + { + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ + }; +const unsigned int planecount=6; + +dReal points[]= // points for a cube + { + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 + }; +const unsigned int pointcount=8; +unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 8 // maximum number of contact points per body +#define MAX_FEEDBACKNUM 20 +#define GRAVITY REAL(0.5) + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; +static int show_body = 0; + +struct MyFeedback { + dJointFeedback fb; + bool first; +}; +static int doFeedback=0; +static MyFeedback feedbacks[MAX_FEEDBACKNUM]; +static int fbnum=0; + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) + return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command(int cmd) +{ + dsizeint i; + int j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase(cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') { + if (num < NUM) { + // new object to be created + i = num; + num++; + } else { + // recycle existing object + i = nextobj++; + nextobj %= num; // wrap-around if needed + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + obj[i].body = 0; + + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) { + dGeomDestroy(obj[i].geom[k]); + obj[i].geom[k] = 0; + } + } + + obj[i].body = dBodyCreate(world); + + for (k=0; k<3; k++) + sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition(obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); + dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } else { + // higher than highest body position + dReal maxheight = 0; + for (k=0; k maxheight) + maxheight = pos[2]; + } + dBodySetPosition(obj[i].body, 0,0,maxheight+1); + dRSetIdentity(R); + //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); + } + + dBodySetRotation(obj[i].body,R); + + if (cmd == 'b') { + + dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]); + + } else if (cmd == 'c') { + + sides[0] *= 0.5; + dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + + } else if (cmd == 'v') { + + dMassSetBox(&m,DENSITY,0.25,0.25,0.25); +#if 0 + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); +#else + obj[i].geom[0] = dCreateConvex(space, + Sphere_planes, + Sphere_planecount, + Sphere_points, + Sphere_pointcount, + Sphere_polygons); +#endif + + } else if (cmd == 'y') { + + dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]); + + } else if (cmd == 's') { + + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + } else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j= num) + selected = 0; + if (selected == -1) + selected = 0; + + } else if (cmd == 'd' && selected >= 0 && selected < num) { + + dBodyDisable(obj[selected].body); + + } else if (cmd == 'e' && selected >= 0 && selected < num) { + + dBodyEnable(obj[selected].body); + + } else if (cmd == 'a') { + + show_aabb = !show_aabb; + + } else if (cmd == 't') { + + show_contacts = !show_contacts; + + } else if (cmd == 'r') { + + random_pos = !random_pos; + } else if (cmd == '1') { + + write_world = 1; + + } else if (cmd == 'p'&& selected >= 0) { + + const dReal* pos = dGeomGetPosition(obj[selected].geom[0]); + const dReal* rot = dGeomGetRotation(obj[selected].geom[0]); + printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]); + printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n", + rot[0],rot[1],rot[2],rot[3], + rot[4],rot[5],rot[6],rot[7], + rot[8],rot[9],rot[10],rot[11]); + + } else if (cmd == 'f' && selected >= 0 && selected < num) { + + if (dBodyIsEnabled(obj[selected].body)) + doFeedback = 1; + + } +} + + +// draw a geom + +void drawGeom(dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + int i; + + if (!g) + return; + if (!pos) + pos = dGeomGetPosition(g); + if (!R) + R = dGeomGetRotation(g); + + int type = dGeomGetClass(g); + if (type == dBoxClass) { + + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox(pos,R,sides); + + } else if (type == dSphereClass) { + + dsDrawSphere(pos,R,dGeomSphereGetRadius(g)); + + } else if (type == dCapsuleClass) { + + dReal radius,length; + dGeomCapsuleGetParams(g,&radius,&length); + dsDrawCapsule(pos,R,length,radius); + + } else if (type == dConvexClass) { + +#if 0 + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); +#else + dsDrawConvex(pos,R, + Sphere_planes, + Sphere_planecount, + Sphere_points, + Sphere_pointcount, + Sphere_polygons); +#endif + + } else if (type == dCylinderClass) { + + dReal radius,length; + dGeomCylinderGetParams(g,&radius,&length); + dsDrawCylinder(pos,R,length,radius); + + } + + if (show_body) { + dBodyID body = dGeomGetBody(g); + if (body) { + const dReal *bodypos = dBodyGetPosition(body); + const dReal *bodyr = dBodyGetRotation(body); + dReal bodySides[3] = { 0.1, 0.1, 0.1 }; + dsSetColorAlpha(0,1,0,1); + dsDrawBox(bodypos,bodyr,bodySides); + } + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB(g,aabb); + dVector3 bbpos; + for (i=0; i<3; i++) + bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (i=0; i<3; i++) + bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha(1,0,0,0.5); + dsDrawBox(bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void simLoop(int pause) +{ + dSpaceCollide(space, 0, &nearCallback); + + if (!pause) + dWorldQuickStep(world, 0.02); + + if (write_world) { + FILE *f = fopen("state.dif","wt"); + if (f) { + dWorldExportDIF(world,f,"X"); + fclose (f); + } + write_world = 0; + } + + + if (doFeedback) { + if (fbnum>MAX_FEEDBACKNUM) + printf("joint feedback buffer overflow!\n"); + else { + dVector3 sum = {0, 0, 0}; + printf("\n"); + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define LENGTH 0.7 // chassis length +#define WIDTH 0.5 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.18 // wheel radius +#define STARTZ 0.5 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + +static const dVector3 yunit = { 0, 1, 0 }, zunit = { 0, 0, 1 }; + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dBodyID body[4]; +static dJointID joint[3]; // joint[0] is the front wheel +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[3]; +static dGeomID ground_box; + + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + // only collide things with the ground + int g1 = (o1 == ground || o1 == ground_box); + int g2 = (o2 == ground || o2 == ground_box); + if (!(g1 ^ g2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[0],dParamVel,v); + dJointSetHinge2Param (joint[0],dParamFMax,0.2); + dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[0],dParamHiStop,0.75); + dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + } + + dsSetColor (0,1,1); + dsSetTexture (DS_WOOD); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]),0.02f,RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + +} + + +int main (int argc, char **argv) +{ + int i; + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + ground = dCreatePlane (space,0,0,1,0); + + // chassis body + body[0] = dBodyCreate (world); + dBodySetPosition (body[0],0,0,STARTZ); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS); + dBodySetMass (body[0],&m); + box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[0],body[0]); + + // wheel bodies + for (i=1; i<=3; i++) { + body[i] = dBodyCreate (world); + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); + dBodySetQuaternion (body[i],q); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (body[i],&m); + sphere[i-1] = dCreateSphere (0,RADIUS); + dGeomSetBody (sphere[i-1],body[i]); + } + dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); + + // front and back wheel hinges + for (i=0; i<3; i++) { + joint[i] = dJointCreateHinge2 (world,0); + dJointAttach (joint[i],body[0],body[i+1]); + const dReal *a = dBodyGetPosition (body[i+1]); + dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); + dJointSetHinge2Axes (joint[i], zunit, yunit); + } + + // set joint suspension + for (i=0; i<3; i++) { + dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); + dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); + } + + // lock back wheels along the steering axis + for (i=1; i<3; i++) { + // set stops to make sure wheels always stay in alignment + dJointSetHinge2Param (joint[i],dParamLoStop,0); + dJointSetHinge2Param (joint[i],dParamHiStop,0); + // the following alternative method is no good as the wheels may get out + // of alignment: + // dJointSetHinge2Param (joint[i],dParamVel,0); + // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity); + } + + // create car space and add it to the top level space + car_space = dSimpleSpaceCreate (space); + dSpaceSetCleanup (car_space,0); + dSpaceAdd (car_space,box[0]); + dSpaceAdd (car_space,sphere[0]); + dSpaceAdd (car_space,sphere[1]); + dSpaceAdd (car_space,sphere[2]); + + // environment + ground_box = dCreateBox (space,2,1.5,1); + dMatrix3 R; + dRFromAxisAndAngle (R,0,1,0,-0.15); + dGeomSetPosition (ground_box,2,0,-0.34); + dGeomSetRotation (ground_box,R); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dGeomDestroy (box[0]); + dGeomDestroy (sphere[0]); + dGeomDestroy (sphere[1]); + dGeomDestroy (sphere[2]); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_cards.cpp b/libs/ode-0.16.1/ode/demo/demo_cards.cpp new file mode 100644 index 0000000..17284ba --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_cards.cpp @@ -0,0 +1,237 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include +#include +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + +static int levels = 5; +static int ncards = 0; + +static dSpaceID space; +static dWorldID world; +static dJointGroupID contactgroup; + +struct Card { + dBodyID body; + dGeomID geom; + static const dReal sides[3]; + + Card() + { + body = dBodyCreate(world); + geom = dCreateBox(space, sides[0], sides[1], sides[2]); + dGeomSetBody(geom, body); + dGeomSetData(geom, this); + dMass mass; + mass.setBox(1, sides[0], sides[1], sides[2]); + dBodySetMass(body, &mass); + } + + ~Card() + { + dBodyDestroy(body); + dGeomDestroy(geom); + } + + void draw() const + { + dsDrawBox(dBodyGetPosition(body), + dBodyGetRotation(body), sides); + } +}; +static const dReal cwidth=.5, cthikness=.02, clength=1; +const dReal Card::sides[3] = { cwidth, cthikness, clength }; + + +std::vector cards; + +int getncards(int levels) +{ + return (3*levels*levels + levels) / 2; +} + +void place_cards() +{ + ncards = getncards(levels); + // destroy removed cards (if any) + int oldcards = cards.size(); + for (int i=ncards; ibody, + 0, + -n*hstep + hstep*i, + height + ); + if (i%2) + dBodySetRotation(cards[c]->body, left); + else + dBodySetRotation(cards[c]->body, right); + } + + if (n==1) // top of the house + break; + + // horizontal cards + for (int i=0; ibody, + 0, + -(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i, + height + vstep/2); + dBodySetRotation(cards[c]->body, hrot); + } + } + +} + + +void start() +{ + puts("Controls:"); + puts(" SPACE - reposition cards"); + puts(" - - one less level"); + puts(" = - one more level"); +} + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + const int MAX_CONTACTS = 8; + dContact contact[MAX_CONTACTS]; + + int numc = dCollide (o1, o2, MAX_CONTACTS, + &contact[0].geom, + sizeof(dContact)); + + for (int i=0; idraw(); + } + +} + +void command(int c) +{ + switch (c) { + case '=': + levels++; + place_cards(); + break; + case '-': + levels--; + if (levels <= 0) + levels++; + place_cards(); + break; + case ' ': + place_cards(); + break; + } +} + +int main(int argc, char **argv) +{ + dInitODE(); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + + world = dWorldCreate(); + dWorldSetGravity(world, 0, 0, -0.5); + dWorldSetQuickStepNumIterations(world, 50); // <-- increase for more stability + + space = dSimpleSpaceCreate(0); + contactgroup = dJointGroupCreate(0); + dGeomID ground = dCreatePlane(space, 0, 0, 1, 0); + + place_cards(); + + // run simulation + dsSimulationLoop (argc, argv, 640, 480, &fn); + + levels = 0; + place_cards(); + + dJointGroupDestroy(contactgroup); + dWorldDestroy(world); + dGeomDestroy(ground); + dSpaceDestroy(space); + + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_chain1.c b/libs/ode-0.16.1/ode/demo/demo_chain1.c new file mode 100644 index 0000000..a6d7b38 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_chain1.c @@ -0,0 +1,171 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* exercise the C interface */ + +#include +#include "ode/ode.h" +#include "drawstuff/drawstuff.h" +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) /* for VC++, no precision loss complaints */ +#endif + +/* select correct drawing functions */ + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +/* some constants */ + +#define NUM 10 /* number of boxes */ +#define SIDE (0.2) /* side length of a box */ +#define MASS (1.0) /* mass of a box */ +#define RADIUS (0.1732f) /* sphere radius */ + + +/* dynamics and collision objects */ + +static dWorldID world; +static dSpaceID space; +static dBodyID body[NUM]; +static dJointID joint[NUM-1]; +static dJointGroupID contactgroup; +static dGeomID sphere[NUM]; + + +/* this is called by dSpaceCollide when two objects in space are + * potentially colliding. + */ + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + /* exit without doing anything if the two bodies are connected by a joint */ + dBodyID b1,b2; + dContact contact; + (void)data; + + b1 = dGeomGetBody(o1); + b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected (b1,b2)) return; + + contact.surface.mode = 0; + contact.surface.mu = 0.1; + contact.surface.mu2 = 0; + if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { + dJointID c = dJointCreateContact (world,contactgroup,&contact); + dJointAttach (c,b1,b2); + } +} + + +/* start simulation - set viewpoint */ + +static void start() +{ + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + + dAllocateODEDataForThread(dAllocateMaskAll); + dsSetViewpoint (xyz,hpr); +} + + +/* simulation loop */ + +static void simLoop (int pause) +{ + int i; + if (!pause) { + static double angle = 0; + angle += 0.05; + dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0)); + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + /* remove all contact joints */ + dJointGroupEmpty (contactgroup); + } + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10 // number of boxes +#define SIDE (0.2) // side length of a box +#define MASS (1.0) // mass of a box +#define RADIUS (0.1732f) // sphere radius + +//using namespace ode; + +// dynamics and collision objects + +static dWorld world; +static dSimpleSpace space (0); +static dBody body[NUM]; +static dBallJoint joint[NUM-1]; +static dJointGroup contactgroup; +static dBox box[NUM]; + + +// this is called by space.collide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected (b1,b2)) return; + + // @@@ it's still more convenient to use the C interface here. + + dContact contact; + contact.surface.mode = 0; + contact.surface.mu = dInfinity; + if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { + dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact); + dJointAttach (c,b1,b2); + } +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + static double angle = 0; + angle += 0.05; + body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0)); + + space.collide (0,&nearCallback); + world.step (0.05); + + // remove all contact joints + contactgroup.empty(); + } + + dReal sides[3] = {SIDE,SIDE,SIDE}; + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawCylinder dsDrawCylinderD +#endif + +//**************************************************************************** +// test infrastructure, including constants and macros + +#define TEST_REPS1 1000 // run each test this many times (first batch) +#define TEST_REPS2 10000 // run each test this many times (second batch) +const dReal tol = 1e-8; // tolerance used for numerical checks +#define MAX_TESTS 1000 // maximum number of test slots +#define Z_OFFSET 2 // z offset for drawing (to get above ground) + +//using namespace ode; + +// test function. returns 1 if the test passed or 0 if it failed +typedef int test_function_t(); + +struct TestSlot { + int number; // number of test + const char *name; // name of test + int failcount; + test_function_t *test_fn; + int last_failed_line; +}; +TestSlot testslot[MAX_TESTS]; + + +// globals used by the test functions +int graphical_test=0; // show graphical results of this test, 0=none +int current_test; // currently execiting test +int draw_all_objects_called; + + +#define MAKE_TEST(number,function) \ + if (testslot[number].name) dDebug (0,"test number already used"); \ + if (number <= 0 || number >= MAX_TESTS) dDebug (0,"bad test number"); \ + testslot[number].name = # function; \ + testslot[number].test_fn = function; + +#define FAILED() { if (graphical_test==0) { \ + testslot[current_test].last_failed_line=__LINE__; return 0; } } +#define PASSED() { return 1; } + +//**************************************************************************** +// globals + +/* int dBoxBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2, + dVector3 normal, dReal *depth, int *code, + int maxc, dContactGeom *contact, int skip); */ + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta); + +//**************************************************************************** +// draw all objects in a space, and draw all the collision contact points + +void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,j,n; + const int N = 100; + dContactGeom contact[N]; + + if (dGeomGetClass (o2) == dRayClass) { + n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom)); + } + else { + n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom)); + } + if (n > 0) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.01,0.01,0.01}; + for (i=0; i tol) FAILED(); + + // ********** test point on surface has depth 0 + + for (j=0; j<3; j++) q[j] = dRandReal()-0.5; + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = q[j]*r + p[j]; + if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED(); + + // ********** test point at random depth + + d = (dRandReal()*2-1) * r; + for (j=0; j<3; j++) q[j] = dRandReal()-0.5; + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j]; + if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED(); + + PASSED(); +} + + +int test_box_point_depth() +{ + int i,j; + dVector3 s,p,q,q2; // s = box sides + dMatrix3 R; + dReal ss,d; // ss = smallest side + + dSimpleSpace space(0); + dGeomID box = dCreateBox (0,1,1,1); + dSpaceAdd (space,box); + + // ********** make a random box + + for (j=0; j<3; j++) s[j] = dRandReal() + 0.1; + dGeomBoxSetLengths (box,s[0],s[1],s[2]); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (box,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (box,R); + + // ********** test center point has depth of smallest side + + ss = 1e9; + for (j=0; j<3; j++) if (s[j] < ss) ss = s[j]; + if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol) + FAILED(); + + // ********** test point on surface has depth 0 + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED(); + + // ********** test points outside box have -ve depth + + for (j=0; j<3; j++) { + q[j] = 0.5*s[j] + dRandReal() + 0.01; + if (dRandReal() > 0.5) q[j] = -q[j]; + } + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED(); + + // ********** test points inside box have +ve depth + + for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5); + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED(); + + // ********** test random depth of point aligned along axis (up to ss deep) + + i = dRandInt (3); + for (j=0; j<3; j++) q[j] = 0; + d = (dRandReal()*(ss*0.5+1)-1); + q[i] = s[i]*0.5 - d; + if (dRandReal() > 0.5) q[i] = -q[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED(); + + PASSED(); +} + + +int test_ccylinder_point_depth() +{ + int j; + dVector3 p,a; + dMatrix3 R; + dReal r,l,beta,x,y,d; + + dSimpleSpace space(0); + dGeomID ccyl = dCreateCapsule (0,1,1); + dSpaceAdd (space,ccyl); + + // ********** make a random ccyl + + r = dRandReal()*0.5 + 0.01; + l = dRandReal()*1 + 0.01; + dGeomCapsuleSetParams (ccyl,r,l); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (ccyl,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ccyl,R); + + // ********** test point on axis has depth of 'radius' + + beta = dRandReal()-0.5; + for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2]; + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol) + FAILED(); + + // ********** test point on surface (excluding caps) has depth 0 + + beta = dRandReal()*2*M_PI; + x = r*sin(beta); + y = r*cos(beta); + beta = dRandReal()-0.5; + 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]; + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED(); + + // ********** test point on surface of caps has depth 0 + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + if (dCalcVectorDot3_14(a,R+2) > 0) { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2]; + } + else { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2]; + } + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED(); + + // ********** test point inside ccyl has positive depth + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + beta = dRandReal()-0.5; + for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2]; + if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED(); + + // ********** test point depth (1) + + d = (dRandReal()*2-1) * r; + beta = dRandReal()*2*M_PI; + x = (r-d)*sin(beta); + y = (r-d)*cos(beta); + beta = dRandReal()-0.5; + 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]; + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol) + FAILED(); + + // ********** test point depth (2) + + d = (dRandReal()*2-1) * r; + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + if (dCalcVectorDot3_14(a,R+2) > 0) { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2]; + } + else { + for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2]; + } + if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol) + FAILED(); + + PASSED(); +} + + +int test_plane_point_depth() +{ + int j; + dVector3 n,p,q,a,b; // n = plane normal + dReal d; + + dSimpleSpace space(0); + dGeomID plane = dCreatePlane (0,0,0,1,0); + dSpaceAdd (space,plane); + + // ********** make a random plane + + for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; + dNormalize3 (n); + d = dRandReal() - 0.5; + dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); + dPlaneSpace (n,p,q); + + // ********** test point on plane has depth 0 + + a[0] = dRandReal() - 0.5; + a[1] = dRandReal() - 0.5; + a[2] = 0; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED(); + + // ********** test arbitrary depth point + + a[0] = dRandReal() - 0.5; + a[1] = dRandReal() - 0.5; + a[2] = dRandReal() - 0.5; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol) + FAILED(); + + // ********** test depth-1 point + + a[0] = dRandReal() - 0.5; + a[1] = dRandReal() - 0.5; + a[2] = -1; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED(); + + PASSED(); +} + +//**************************************************************************** +// ray tests + +int test_ray_and_sphere() +{ + int j; + dContactGeom contact; + dVector3 p,q,q2,n,v1; + dMatrix3 R; + dReal r,k; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID sphere = dCreateSphere (0,1); + dSpaceAdd (space,ray); + dSpaceAdd (space,sphere); + + // ********** make a random sphere of radius r at position p + + r = dRandReal()+0.1; + dGeomSphereSetRadius (sphere,r); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (sphere,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (sphere,R); + + // ********** test zero length ray just inside sphere + + dGeomRaySetLength (ray,0); + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j]; + dGeomSetPosition (ray,q[0],q[1],q[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test zero length ray just outside that sphere + + dGeomRaySetLength (ray,0); + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j]; + dGeomSetPosition (ray,q[0],q[1],q[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally contained inside the sphere + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + k = dRandReal(); + for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j]; + dMakeRandomVector (q2,3,1.0); + dNormalize3 (q2); + k = dRandReal(); + for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j]; + for (j=0; j<3; j++) n[j] = q2[j] - q[j]; + dNormalize3 (n); + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2)); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally outside the sphere + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + do { + dMakeRandomVector (n,3,1.0); + dNormalize3 (n); + } + while (dCalcVectorDot3(n,q) < 0); // make sure normal goes away from sphere + for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j]; + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,100); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just above surface + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + for (j=0; j<3; j++) n[j] = -q[j]; + for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j]; + dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,0.99*r); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just below surface + + dGeomRaySetLength (ray,1.01*r); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + for (j=0; j<3; j++) q2[j] = r * q[j] + p[j]; + if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED(); + + // ********** test contact point distance for random rays + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + k = dRandReal()+0.5; + for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j]; + dMakeRandomVector (n,3,1.0); + dNormalize3 (n); + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,100); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) { + k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere)); + if (dFabs(k - r) > tol) FAILED(); + // also check normal signs + if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED(); + // also check depth of contact point + if (dFabs (dGeomSpherePointDepth + (sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + draw_all_objects (space); + } + + // ********** test tangential grazing - miss + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + dPlaneSpace (q,n,v1); + for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j]; + for (j=0; j<3; j++) q[j] -= n[j]; + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,2); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test tangential grazing - hit + + dMakeRandomVector (q,3,1.0); + dNormalize3 (q); + dPlaneSpace (q,n,v1); + for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j]; + for (j=0; j<3; j++) q[j] -= n[j]; + dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,2); + if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + PASSED(); +} + + +int test_ray_and_box() +{ + int i,j; + dContactGeom contact; + dVector3 s,p,q,n,q2,q3,q4; // s = box sides + dMatrix3 R; + dReal k; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID box = dCreateBox (0,1,1,1); + dSpaceAdd (space,ray); + dSpaceAdd (space,box); + + // ********** make a random box + + for (j=0; j<3; j++) s[j] = dRandReal() + 0.1; + dGeomBoxSetLengths (box,s[0],s[1],s[2]); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (box,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (box,R); + + // ********** test zero length ray just inside box + + dGeomRaySetLength (ray,0); + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + dGeomSetPosition (ray,q2[0],q2[1],q2[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test zero length ray just outside box + + dGeomRaySetLength (ray,0); + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + dGeomSetPosition (ray,q2[0],q2[1],q2[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally contained inside the box + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j]; + dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1); + for (j=0; j<3; j++) q4[j] += p[j]; + for (j=0; j<3; j++) n[j] = q4[j] - q2[j]; + dNormalize3 (n); + dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4)); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray totally outside the box + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q3[j] = q2[j] + p[j]; + dNormalize3 (q2); + dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just above surface + + for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j]; + i = dRandInt (3); + if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j]; + k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]); + for (j=0; j<3; j++) q2[j] = -q2[j]; + dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]); + dGeomRaySetLength (ray,k*0.99); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray from outside to just below surface + + dGeomRaySetLength (ray,k*1.01); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + // ********** test contact point position for random rays + + for (j=0; j<3; j++) q[j] = dRandReal()*s[j]; + dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1); + for (j=0; j<3; j++) q2[j] += p[j]; + for (j=0; j<3; j++) q3[j] = dRandReal()-0.5; + dNormalize3 (q3); + dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) { + // check depth of contact point + if (dFabs (dGeomBoxPointDepth + (box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + // check position of contact point + for (j=0; j<3; j++) contact.pos[j] -= p[j]; + dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1); + if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol && + dFabs(dFabs (q[1]) - 0.5*s[1]) > tol && + dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) { + FAILED(); + } + // also check normal signs + if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED(); + + draw_all_objects (space); + } + + PASSED(); +} + + +int test_ray_and_ccylinder() +{ + int j; + dContactGeom contact; + dVector3 p,a,b,n; + dMatrix3 R; + dReal r,l,k,x,y; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID ccyl = dCreateCapsule (0,1,1); + dSpaceAdd (space,ray); + dSpaceAdd (space,ccyl); + + // ********** make a random capped cylinder + + r = dRandReal()*0.5 + 0.01; + l = dRandReal()*1 + 0.01; + dGeomCapsuleSetParams (ccyl,r,l); + dMakeRandomVector (p,3,1.0); + dGeomSetPosition (ccyl,p[0],p[1],p[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ccyl,R); + + // ********** test ray completely within ccyl + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + k = (dRandReal()-0.5)*l; + for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2]; + for (j=0; j<3; j++) b[j] = dRandReal()-0.5; + dNormalize3 (b); + k = (dRandReal()-0.5)*l; + for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2]; + dGeomRaySetLength (ray,dCalcPointsDistance3(a,b)); + for (j=0; j<3; j++) b[j] -= a[j]; + dNormalize3 (b); + dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray outside ccyl that just misses (between caps) + + k = dRandReal()*2*M_PI; + x = sin(k); + y = cos(k); + for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1]; + k = (dRandReal()-0.5)*l; + for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j]; + dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]); + dGeomRaySetLength (ray,r*0.99); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray outside ccyl that just hits (between caps) + + dGeomRaySetLength (ray,r*1.01); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + // check depth of contact point + if (dFabs (dGeomCapsulePointDepth + (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + // ********** test ray outside ccyl that just misses (caps) + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + dNormalize3 (a); + if (dCalcVectorDot3_14(a,R+2) < 0) { + for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2]; + } + else { + for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2]; + } + dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]); + dGeomRaySetLength (ray,r*0.99); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray outside ccyl that just hits (caps) + + dGeomRaySetLength (ray,r*1.01); + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + // check depth of contact point + if (dFabs (dGeomCapsulePointDepth + (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + // ********** test random rays + + for (j=0; j<3; j++) a[j] = dRandReal()-0.5; + for (j=0; j<3; j++) n[j] = dRandReal()-0.5; + dNormalize3 (n); + dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]); + dGeomRaySetLength (ray,10); + + if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) { + // check depth of contact point + if (dFabs (dGeomCapsulePointDepth + (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + // check normal signs + if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED(); + + draw_all_objects (space); + } + + PASSED(); +} + +/* + Test rays within the cylinder + -completely inside + -exiting through side + -exiting through cap + -exiting through corner + Test rays outside the cylinder +*/ +int test_ray_and_cylinder() +{ + dVector3 a,b; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay(space,4); + + // The first thing that happens is the ray is + // rotated into cylinder coordinates. We'll trust that's + // done right. The major axis is in the z-dir. + + + // Random tests + /*b[0]=4*dRandReal()-2; + b[1]=4*dRandReal()-2; + b[2]=4*dRandReal()-2; + a[0]=2*dRandReal()-1; + a[1]=2*dRandReal()-1; + a[2]=2*dRandReal()-1;*/ + + // Inside out + b[0]=dRandReal()-0.5; + b[1]=dRandReal()-0.5; + b[2]=dRandReal()-0.5; + a[0]=2*dRandReal()-1; + a[1]=2*dRandReal()-1; + a[2]=2*dRandReal()-1; + + // Outside in + /*b[0]=4*dRandReal()-2; + b[1]=4*dRandReal()-2; + b[2]=4*dRandReal()-2; + a[0]=-b[0]; + a[1]=-b[1]; + a[2]=-b[2];*/ + + + dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]); + // This is just for visual inspection right now. + //if (dCollide (ray,cyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + draw_all_objects (space); + + PASSED(); +} + + +int test_ray_and_plane() +{ + int j; + dContactGeom contact; + dVector3 n,p,q,a,b,g,h; // n,d = plane parameters + dMatrix3 R; + dReal d; + + dSimpleSpace space(0); + dGeomID ray = dCreateRay (0,0); + dGeomID plane = dCreatePlane (0,0,0,1,0); + dSpaceAdd (space,ray); + dSpaceAdd (space,plane); + + // ********** make a random plane + + for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; + dNormalize3 (n); + d = dRandReal() - 0.5; + dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); + dPlaneSpace (n,p,q); + + // ********** test finite length ray below plane + + dGeomRaySetLength (ray,0.09); + a[0] = dRandReal()-0.5; + a[1] = dRandReal()-0.5; + a[2] = -dRandReal()*0.5 - 0.1; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + dGeomSetPosition (ray,b[0],b[1],b[2]); + dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, + dRandReal()*2-1,dRandReal()*10-5); + dGeomSetRotation (ray,R); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray above plane + + a[0] = dRandReal()-0.5; + a[1] = dRandReal()-0.5; + a[2] = dRandReal()*0.5 + 0.01; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + g[0] = dRandReal()-0.5; + g[1] = dRandReal()-0.5; + g[2] = dRandReal() + 0.01; + for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j]; + dNormalize3 (h); + dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test finite length ray that intersects plane + + a[0] = dRandReal()-0.5; + a[1] = dRandReal()-0.5; + a[2] = dRandReal()-0.5; + for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; + g[0] = dRandReal()-0.5; + g[1] = dRandReal()-0.5; + g[2] = dRandReal()-0.5; + for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j]; + dNormalize3 (h); + dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); + dGeomRaySetLength (ray,10); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) { + // test that contact is on plane surface + if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED(); + // also check normal signs + if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED(); + // also check contact point depth + if (dFabs (dGeomPlanePointDepth + (plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) + FAILED(); + + draw_all_objects (space); + } + + // ********** test ray that just misses + + for (j=0; j<3; j++) b[j] = (1+d)*n[j]; + for (j=0; j<3; j++) h[j] = -n[j]; + dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); + dGeomRaySetLength (ray,0.99); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); + + // ********** test ray that just hits + + dGeomRaySetLength (ray,1.01); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + + // ********** test polarity with typical ground plane + + dGeomPlaneSetParams (plane,0,0,1,0); + for (j=0; j<3; j++) a[j] = 0.1; + for (j=0; j<3; j++) b[j] = 0; + a[2] = 1; + b[2] = -1; + dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); + dGeomRaySetLength (ray,2); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + if (dFabs (contact.depth - 1) > tol) FAILED(); + a[2] = -1; + b[2] = 1; + dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); + if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); + if (dFabs (contact.depth - 1) > tol) FAILED(); + + PASSED(); +} + +//**************************************************************************** +// a really inefficient, but hopefully correct implementation of +// dBoxTouchesBox(), that does 144 edge-face tests. + +// return 1 if edge v1 -> v2 hits the rectangle described by p1,p2,p3 + +static int edgeIntersectsRect (dVector3 v1, dVector3 v2, + dVector3 p1, dVector3 p2, dVector3 p3) +{ + int k; + dVector3 u1, u2, n, tmp; + + for (k=0; k < 3; k++) u1[k] = p3[k] - p1[k]; + for (k=0; k < 3; k++) u2[k] = p2[k] - p1[k]; + + dReal d1 = dSqrt(dCalcVectorDot3(u1, u1)); + dReal d2 = dSqrt(dCalcVectorDot3(u2, u2)); + dNormalize3(u1); + dNormalize3(u2); + + dReal error; +#ifdef dSINGLE + const dReal uEpsilon = 1e-5, pEpsilon = 1e-6, tmpEpsilon = 1.5e-4; +#else + const dReal uEpsilon = 1e-6, pEpsilon = 1e-8, tmpEpsilon = 1e-6; +#endif + + error = dFabs(dCalcVectorDot3(u1, u2)); + if (error > uEpsilon) dDebug(0, "bad u1/u2"); + + dCalcVectorCross3(n, u1, u2); + + for (k=0; k < 3; k++) tmp[k] = v2[k] - v1[k]; + + dReal d = -dCalcVectorDot3(n, p1); + + error = dFabs(dCalcVectorDot3(n, p1) + d); + if (error > pEpsilon) dDebug(0, "bad n wrt p1"); + + error = dFabs(dCalcVectorDot3(n, p2) + d); + if (error > pEpsilon) dDebug(0, "bad n wrt p2"); + + error = dFabs(dCalcVectorDot3(n, p3) + d); + if (error > pEpsilon) dDebug(0, "bad n wrt p3"); + + dReal alpha = -(d + dCalcVectorDot3(n, v1)) / dCalcVectorDot3(n, tmp); + for (k=0; k < 3; k++) tmp[k] = v1[k] + alpha * (v2[k] - v1[k]); + + error = dFabs(dCalcVectorDot3(n, tmp) + d); + if (error > tmpEpsilon) dDebug(0, "bad tmp"); + + if (alpha < 0) return 0; + if (alpha > 1) return 0; + + for (k=0; k < 3; k++) tmp[k] -= p1[k]; + dReal a1 = dCalcVectorDot3(u1, tmp); + dReal a2 = dCalcVectorDot3(u2, tmp); + if (a1 < 0 || a2 < 0 || a1 > d1 || a2 > d2) return 0; + + return 1; +} + + +// return 1 if box 1 is completely inside box 2 + +static int box1inside2 (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + for (int i=-1; i<=1; i+=2) { + for (int j=-1; j<=1; j+=2) { + for (int k=-1; k<=1; k+=2) { + dVector3 v,vv; + v[0] = i*0.5*side1[0]; + v[1] = j*0.5*side1[1]; + v[2] = k*0.5*side1[2]; + dMultiply0_331 (vv,R1,v); + vv[0] += p1[0] - p2[0]; + vv[1] += p1[1] - p2[1]; + vv[2] += p1[2] - p2[2]; + for (int axis=0; axis < 3; axis++) { + dReal z = dCalcVectorDot3_14(vv,R2+axis); + if (z < (-side2[axis]*0.5) || z > (side2[axis]*0.5)) return 0; + } + } + } + } + return 1; +} + + +// test if any edge from box 1 hits a face from box 2 + +static int testBoxesTouch2 (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + int j,k,j1,j2; + + // for 6 faces from box 2 + for (int fd=0; fd<3; fd++) { // direction for face + + for (int fo=0; fo<2; fo++) { // offset of face + // get four points on the face. first get 2 indexes that are not fd + int k1=0,k2=0; + if (fd==0) { k1 = 1; k2 = 2; } + if (fd==1) { k1 = 0; k2 = 2; } + if (fd==2) { k1 = 0; k2 = 1; } + dVector3 fp[4],tmp; + k=0; + for (j1=-1; j1<=1; j1+=2) { + for (j2=-1; j2<=1; j2+=2) { + fp[k][k1] = j1; + fp[k][k2] = j2; + fp[k][fd] = fo*2-1; + k++; + } + } + for (j=0; j<4; j++) { + for (k=0; k<3; k++) fp[j][k] *= 0.5*side2[k]; + dMultiply0_331 (tmp,R2,fp[j]); + for (k=0; k<3; k++) fp[j][k] = tmp[k] + p2[k]; + } + + // for 8 vertices + dReal v1[3]; + for (v1[0]=-1; v1[0] <= 1; v1[0] += 2) { + for (v1[1]=-1; v1[1] <= 1; v1[1] += 2) { + for (v1[2]=-1; v1[2] <= 1; v1[2] += 2) { + // for all possible +ve leading edges from those vertices + for (int ei=0; ei < 3; ei ++) { + if (v1[ei] < 0) { + // get vertex1 -> vertex2 = an edge from box 1 + dVector3 vv1,vv2; + for (k=0; k<3; k++) vv1[k] = v1[k] * 0.5*side1[k]; + for (k=0; k<3; k++) vv2[k] = (v1[k] + (k==ei)*2)*0.5*side1[k]; + dVector3 vertex1,vertex2; + dMultiply0_331 (vertex1,R1,vv1); + dMultiply0_331 (vertex2,R1,vv2); + for (k=0; k<3; k++) vertex1[k] += p1[k]; + for (k=0; k<3; k++) vertex2[k] += p1[k]; + + // see if vertex1 -> vertex2 interesects face + if (edgeIntersectsRect (vertex1,vertex2,fp[0],fp[1],fp[2])) + return 1; + } + } + } + } + } + } + } + + if (box1inside2 (p1,R1,side1,p2,R2,side2)) return 1; + if (box1inside2 (p2,R2,side2,p1,R1,side1)) return 1; + + return 0; +} + +//**************************************************************************** +// dBoxTouchesBox() test + +int test_dBoxTouchesBox() +{ + int k,bt1,bt2; + dVector3 p1,p2,side1,side2; + dMatrix3 R1,R2; + + dSimpleSpace space(0); + dGeomID box1 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box1); + dGeomID box2 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box2); + + dMakeRandomVector (p1,3,0.5); + dMakeRandomVector (p2,3,0.5); + for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01; + for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01; + dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]); + dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]); + dGeomSetPosition (box1,p1[0],p1[1],p1[2]); + dGeomSetRotation (box1,R1); + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + dGeomSetRotation (box2,R2); + draw_all_objects (space); + + int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2); + int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1); + bt1 = t1 || t2; + bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2); + + if (bt1 != bt2) FAILED(); + + /* + // some more debugging info if necessary + if (bt1 && bt2) printf ("agree - boxes touch\n"); + if (!bt1 && !bt2) printf ("agree - boxes don't touch\n"); + if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox " + "says no\n"); + if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox " + "says yes\n"); + */ + + PASSED(); +} + +//**************************************************************************** +// test box-box collision + +int test_dBoxBox() +{ + int k,bt; + dVector3 p1,p2,side1,side2,normal,normal2; + dMatrix3 R1,R2; + dReal depth,depth2; + int code; + dContactGeom contact[48]; + + dSimpleSpace space(0); + dGeomID box1 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box1); + dGeomID box2 = dCreateBox (0,1,1,1); + dSpaceAdd (space,box2); + + dMakeRandomVector (p1,3,0.5); + dMakeRandomVector (p2,3,0.5); + for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01; + for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01; + + dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + // dRSetIdentity (R1); // we can also try this + // dRSetIdentity (R2); + + dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]); + dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]); + dGeomSetPosition (box1,p1[0],p1[1],p1[2]); + dGeomSetRotation (box1,R1); + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + dGeomSetRotation (box2,R2); + + code = 0; + depth = 0; + bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact, + sizeof(dContactGeom)); + if (bt==1) { + p2[0] += normal[0] * 0.96 * depth; + p2[1] += normal[1] * 0.96 * depth; + p2[2] += normal[2] * 0.96 * depth; + bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact, + sizeof(dContactGeom)); + + /* + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + draw_all_objects (space); + */ + + if (bt != 1) { + FAILED(); + dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + draw_all_objects (space); + } + + p2[0] += normal[0] * 0.08 * depth; + p2[1] += normal[1] * 0.08 * depth; + p2[2] += normal[2] * 0.08 * depth; + bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact, + sizeof(dContactGeom)); + if (bt != 0) FAILED(); + + // dGeomSetPosition (box2,p2[0],p2[1],p2[2]); + // draw_all_objects (space); + } + + // printf ("code=%2d depth=%.4f ",code,depth); + + PASSED(); +} + +//**************************************************************************** +// graphics + +int space_pressed = 0; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.4807,-1.8023,2.7600}; + static float hpr[3] = {141.5000,-18.5000,0.0000}; + dsSetViewpoint (xyz,hpr); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == ' ') space_pressed = 1; +} + + +// simulation loop + +static void simLoop (int) +{ + do { + draw_all_objects_called = 0; + unsigned long seed = dRandGetSeed(); + testslot[graphical_test].test_fn(); + if (draw_all_objects_called) { + if (space_pressed) space_pressed = 0; else dRandSetSeed (seed); + } + } + while (!draw_all_objects_called); +} + +//**************************************************************************** +// do all the tests + +void do_tests (int argc, char **argv) +{ + int i,j; + + // process command line arguments + if (argc >= 2) { + graphical_test = atoi (argv[1]); + } + + if (graphical_test) { + // do one test gaphically and interactively + + if (graphical_test < 1 || graphical_test >= MAX_TESTS || + !testslot[graphical_test].name) { + dError (0,"invalid test number"); + } + + printf ("performing test: %s\n",testslot[graphical_test].name); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dsSetSphereQuality (3); + dsSetCapsuleQuality (8); + dsSimulationLoop (argc,argv,1280,900,&fn); + } + else { + // do all tests noninteractively + + for (i=0; ifailcount = 0; + int total_reps=0; + for (int batch=0; batch<2; batch++) { + int reps = (batch==0) ? TEST_REPS1 : TEST_REPS2; + total_reps += reps; + printf ("testing batch %d (%d reps)...\n",batch+1,reps); + + // run tests + for (j=0; jnumber; + if (ts[i]->test_fn() != 1) ts[i]->failcount++; + } + } + + // check for failures + int total_fail_count=0; + for (i=0; ifailcount; + if (total_fail_count) break; + } + + // print results + for (i=0; inumber,ts[i]->name); + if (ts[i]->failcount) { + printf ("FAILED (%.2f%%) at line %d\n", + double(ts[i]->failcount)/double(total_reps)*100.0, + ts[i]->last_failed_line); + } + else { + printf ("ok\n"); + } + } + } +} + +//**************************************************************************** + +int main (int argc, char **argv) +{ + // setup all tests + + memset (testslot,0,sizeof(testslot)); + dInitODE2(0); + + MAKE_TEST(1,test_sphere_point_depth); + MAKE_TEST(2,test_box_point_depth); + MAKE_TEST(3,test_ccylinder_point_depth); + MAKE_TEST(4,test_plane_point_depth); + + MAKE_TEST(10,test_ray_and_sphere); + MAKE_TEST(11,test_ray_and_box); + MAKE_TEST(12,test_ray_and_ccylinder); + MAKE_TEST(13,test_ray_and_plane); + MAKE_TEST(14,test_ray_and_cylinder); + + MAKE_TEST(100,test_dBoxTouchesBox); + MAKE_TEST(101,test_dBoxBox); + + do_tests (argc,argv); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_convex.cpp b/libs/ode-0.16.1/ode/demo/demo_convex.cpp new file mode 100644 index 0000000..eea5c6e --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_convex.cpp @@ -0,0 +1,307 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Convex demo. +// Serves as a test for the convex geometry. +// By Bram Stolk. + +#include +#ifdef HAVE_UNISTD_H +# include +#endif +#include +#include +#include "texturepath.h" + +#include "halton235_geom.h" + +#ifdef dDOUBLE +# define dsDrawConvex dsDrawConvexD +# define dsDrawLine dsDrawLineD +#endif + + +#ifdef _MSC_VER +# pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +// Height at which we drop the composite block. +const dReal H=4.20; + +static dWorldID world; +static dSpaceID space; + +static dBodyID mbody; + +static dBodyID hbody[ halton_numc ]; +static dGeomID hgeom[ halton_numc ]; + +static dJointGroupID contactgroup; + +static bool drawpos=false; +static bool solidkernel=false; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback(void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i 8 ? 8 : nrofsteps; + + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define LENGTH 3.5 // chassis length +#define WIDTH 2.5 // chassis width +#define HEIGHT 1.0 // chassis height +#define RADIUS 0.5 // wheel radius +#define STARTZ 1.0 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 1 // wheel mass +#define COMOFFSET -5 // center of mass offset +#define WALLMASS 1 // wall box mass +#define BALLMASS 1 // ball mass +#define FMAX 25 // car engine fmax +#define ROWS 1 // rows of cars +#define COLS 1 // columns of cars +#define ITERS 20 // number of iterations +#define WBOXSIZE 1.0 // size of wall boxes +#define WALLWIDTH 12 // width of wall +#define WALLHEIGHT 10 // height of wall +#define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled +#define DISABLE_STEPS 10 // number of steps a box has to have been disable-able before it will be disabled +#define CANNON_X -10 // x position of cannon +#define CANNON_Y 5 // y position of cannon +#define CANNON_BALL_MASS 10 // mass of the cannon ball +#define CANNON_BALL_RADIUS 0.5 + +static const dVector3 xunit = { 1, 0, 0 }, yunit = { 0, 1, 0 }, zpunit = { 0, 0, 1 }, zmunit = { 0, 0, -1 }; + +//#define BOX +#define CARS +#define WALL +//#define BALLS +//#define BALLSTACK +//#define ONEBALL +//#define CENTIPEDE +#define CANNON + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dThreadingImplementationID threading; +static dThreadingThreadPoolID pool; +static dBodyID body[10000]; +static int bodies; +static dJointID joint[100000]; +static int joints; +static dJointGroupID contactgroup; +static dGeomID ground; +static dGeomID box[10000]; +static int boxes; +static dGeomID sphere[10000]; +static int spheres; +static dGeomID wall_boxes[10000]; +static dBodyID wall_bodies[10000]; +static dGeomID cannon_ball_geom; +static dBodyID cannon_ball_body; +static int wb_stepsdis[10000]; +static int wb; +static bool doFast; +static dBodyID b; +static dMass m; + + +// things that the user controls + +static dReal turn = 0, speed = 0; // user commands +static dReal cannon_angle=0,cannon_elevation=-1.2; + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected(b1, b2)) + return; + + const int N = 4; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i -20; x-=RADIUS*2) + { + body[bodies] = dBodyCreate (world); + dBodySetPosition(body[bodies], x, y, STARTZ); + dMassSetSphere(&m, 1, RADIUS); + dMassAdjust(&m, WMASS); + dBodySetMass(body[bodies], &m); + sphere[spheres] = dCreateSphere (space, RADIUS); + dGeomSetBody (sphere[spheres++], body[bodies]); + + joint[joints] = dJointCreateHinge2 (world,0); + if (x == -17) + dJointAttach (joint[joints],b,body[bodies]); + else + dJointAttach (joint[joints],body[bodies-2],body[bodies]); + const dReal *a = dBodyGetPosition (body[bodies++]); + dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]); + dJointSetHinge2Axes (joint[joints], zpunit, xunit); + dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); + dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); + dJointSetHinge2Param (joint[joints],dParamLoStop,0); + dJointSetHinge2Param (joint[joints],dParamHiStop,0); + dJointSetHinge2Param (joint[joints],dParamVel2,-10.0); + dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); + + body[bodies] = dBodyCreate (world); + dBodySetPosition(body[bodies], -30 - x, y, STARTZ); + dMassSetSphere(&m, 1, RADIUS); + dMassAdjust(&m, WMASS); + dBodySetMass(body[bodies], &m); + sphere[spheres] = dCreateSphere (space, RADIUS); + dGeomSetBody (sphere[spheres++], body[bodies]); + + joint[joints] = dJointCreateHinge2 (world,0); + if (x == -17) + dJointAttach (joint[joints],b,body[bodies]); + else + dJointAttach (joint[joints],body[bodies-2],body[bodies]); + const dReal *b = dBodyGetPosition (body[bodies++]); + dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]); + dJointSetHinge2Axes (joint[joints], zpunit, xunit); + dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); + dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); + dJointSetHinge2Param (joint[joints],dParamLoStop,0); + dJointSetHinge2Param (joint[joints],dParamHiStop,0); + dJointSetHinge2Param (joint[joints],dParamVel2,10.0); + dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); + } + if (lastb) + { + dJointID j = dJointCreateFixed(world,0); + dJointAttach (j, b, lastb); + dJointSetFixed(j); + } + lastb = b; + } +#endif +#ifdef BOX + body[bodies] = dBodyCreate (world); + dBodySetPosition (body[bodies],0,0,HEIGHT/2); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m, 1); + dBodySetMass (body[bodies],&m); + box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[boxes++],body[bodies++]); +#endif +#ifdef CANNON + cannon_ball_body = dBodyCreate (world); + cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS); + dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS); + dBodySetMass (cannon_ball_body,&m); + dGeomSetBody (cannon_ball_geom,cannon_ball_body); + dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS); +#endif +} + +// called when a key pressed + +static void command (int cmd) +{ + switch (cmd) { + case 'a': case 'A': + speed += 0.3; + break; + case 'z': case 'Z': + speed -= 0.3; + break; + case ',': + turn += 0.1; + if (turn > 0.3) + turn = 0.3; + break; + case '.': + turn -= 0.1; + if (turn < -0.3) + turn = -0.3; + break; + case ' ': + speed = 0; + turn = 0; + break; + case 'f': case 'F': + doFast = !doFast; + break; + case 'r': case 'R': + shutdownSimulation(); + setupSimulation(); + break; + case '[': + cannon_angle += 0.1; + break; + case ']': + cannon_angle -= 0.1; + break; + case '1': + cannon_elevation += 0.1; + break; + case '2': + cannon_elevation -= 0.1; + break; + case 'x': case 'X': { + dMatrix3 R2,R3,R4; + dRFromAxisAndAngle (R2,0,0,1,cannon_angle); + dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); + dMultiply0 (R4,R2,R3,3,3,3); + dReal cpos[3] = {CANNON_X,CANNON_Y,1}; + for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2]; + dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]); + dReal force = 10; + dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]); + dBodySetAngularVel (cannon_ball_body,0,0,0); + break; + } + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + int i, j; + + dsSetTexture (DS_WOOD); + + if (!pause) { +#ifdef BOX + dBodyAddForce(body[bodies-1],lspeed,0,0); +#endif + for (j = 0; j < joints; j++) + { + dReal curturn = dJointGetHinge2Angle1 (joint[j]); + //dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0); + dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0); + dJointSetHinge2Param(joint[j],dParamFMax,dInfinity); + dJointSetHinge2Param(joint[j],dParamVel2,speed); + dJointSetHinge2Param(joint[j],dParamFMax2,FMAX); + dBodyEnable(dJointGetBody(joint[j],0)); + dBodyEnable(dJointGetBody(joint[j],1)); + } + if (doFast) + { + dSpaceCollide (space,0,&nearCallback); + dWorldQuickStep (world,0.05); + dJointGroupEmpty (contactgroup); + } + else + { + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + dJointGroupEmpty (contactgroup); + } + + for (i = 0; i < wb; i++) + { + b = dGeomGetBody(wall_boxes[i]); + if (dBodyIsEnabled(b)) + { + bool disable = true; + const dReal *lvel = dBodyGetLinearVel(b); + dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2]; + if (lspeed > DISABLE_THRESHOLD) + disable = false; + const dReal *avel = dBodyGetAngularVel(b); + dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2]; + if (aspeed > DISABLE_THRESHOLD) + disable = false; + + if (disable) + wb_stepsdis[i]++; + else + wb_stepsdis[i] = 0; + + if (wb_stepsdis[i] > DISABLE_STEPS) + { + dBodyDisable(b); + dsSetColor(0.5,0.5,1); + } + else + dsSetColor(1,1,1); + + } + else + dsSetColor(0.4,0.4,0.4); + dVector3 ss; + dGeomBoxGetLengths (wall_boxes[i], ss); + dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); + } + } + else + { + for (i = 0; i < wb; i++) + { + b = dGeomGetBody(wall_boxes[i]); + if (dBodyIsEnabled(b)) + dsSetColor(1,1,1); + else + dsSetColor(0.4,0.4,0.4); + dVector3 ss; + dGeomBoxGetLengths (wall_boxes[i], ss); + dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); + } + } + + dsSetColor (0,1,1); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + for (i = 0; i < boxes; i++) + dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides); + dsSetColor (1,1,1); + for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]), + dGeomGetRotation(sphere[i]),RADIUS); + + // draw the cannon + dsSetColor (1,1,0); + dMatrix3 R2,R3,R4; + dRFromAxisAndAngle (R2,0,0,1,cannon_angle); + dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); + dMultiply0 (R4,R2,R3,3,3,3); + dReal cpos[3] = {CANNON_X,CANNON_Y,1}; + dReal csides[3] = {2,2,2}; + dsDrawBox (cpos,R2,csides); + for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2]; + dsDrawCylinder (cpos,R4,3,0.5); + + // draw the cannon ball + dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body), + CANNON_BALL_RADIUS); +} + +int main (int argc, char **argv) +{ + doFast = true; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + + bodies = 0; + joints = 0; + boxes = 0; + spheres = 0; + + setupSimulation(); + + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(8, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1); + dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dWorldSetStepThreadingImplementation(world, NULL, NULL); + dThreadingFreeImplementation(threading); + + shutdownSimulation(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_cyl.cpp b/libs/ode-0.16.1/ode/demo/demo_cyl.cpp new file mode 100644 index 0000000..368a0c1 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_cyl.cpp @@ -0,0 +1,321 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Test for non-capped cylinder, by Bram Stolk +#include +#include +#ifdef HAVE_UNISTD_H +#include +#endif +#include +#include +#include "texturepath.h" + +#include "world_geom3.h" // this is our world mesh + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +#define BOX +#define CYL + +// some constants + +#define RADIUS 0.22 // wheel radius +#define WMASS 0.2 // wheel mass +#define WHEELW 0.2 // wheel width +#define BOXSZ 0.4 // box size +//#define CYL_GEOM_OFFSET // rotate cylinder using geom offset + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +#ifdef BOX +static dBodyID boxbody; +static dGeomID boxgeom; +#endif +#ifdef CYL +static dBodyID cylbody; +static dGeomID cylgeom; +#endif +static dJointGroupID contactgroup; +static dGeomID world_mesh; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + +// fprintf(stderr,"testing geoms %p %p\n", o1, o2); + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i +#include +#ifdef HAVE_UNISTD_H +#include +#endif +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; + +static dBodyID cylbody; +static dGeomID cylgeom; + +static dBodyID sphbody; +static dGeomID sphgeom; + +static dJointGroupID contactgroup; + +static bool show_contacts = true; + +#define CYLRADIUS 0.6 +#define CYLLENGTH 2.0 +#define SPHERERADIUS 0.5 + + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + +dWorldID world; +dSpaceID space; +dBodyID body1; +dBodyID body2; +dJointID joint1, joint2; + +void start() +{ + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetDamping(world, 1e-4, 1e-5); +// dWorldSetERP(world, 1); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetPosition(body1, 0, 0, 3); + dBodySetPosition(body2, 0, 0, 1); + + + dGeomID g; + dMass mass; + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body1); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body1, &mass); + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body2); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body2, &mass); + + joint1 = dJointCreateDBall(world, 0); + dJointAttach(joint1, body1, 0); + dJointSetDBallAnchor1(joint1, 0, 0, 3.5); + dJointSetDBallAnchor2(joint1, 0, 0, 4.5); + + joint2 = dJointCreateDBall(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDBallAnchor1(joint2, 0, 0, 2.5); + dJointSetDBallAnchor2(joint2, 0, 0, 1.5); + + + // initial camera position + static float xyz[3] = {3.8966, -2.0614, 4.0300}; + static float hpr[3] = {153.5, -16.5, 0}; + dsSetViewpoint (xyz,hpr); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dSphereClass: + dsSetColorAlpha(0, 0.75, 0.5, 1); + dsSetTexture (DS_CHECKERED); + dsDrawSphere(pos, rot, dGeomSphereGetRadius(g)); + break; + case dBoxClass: + { + dVector3 lengths; + dsSetColorAlpha(1, 1, 0, 1); + dsSetTexture (DS_WOOD); + dGeomBoxGetLengths(g, lengths); + dsDrawBox(pos, rot, lengths); + break; + } + + default: + {} + } +} + +void simLoop(int pause) +{ + if (!pause) { + + static dReal t = 0; + + const dReal step = 0.005; + const unsigned nsteps = 4; + + for (unsigned i=0; i +#include +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + +dWorldID world; +dSpaceID space; +dBodyID body1; +dBodyID body2; +dJointID joint1, joint2; +bool applyForce = false; + +void start() +{ + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetDamping(world, 1e-4, 1e-5); +// dWorldSetERP(world, 1); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetPosition(body1, 0, 0, 3); + dBodySetPosition(body2, 0, 0, 1); + + + dGeomID g; + dMass mass; + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body1); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body1, &mass); + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body2); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body2, &mass); + +#if 1 + joint1 = dJointCreateDHinge(world, 0); + dJointAttach(joint1, body1, 0); + dJointSetDHingeAxis(joint1, 0, 1, 0); + dJointSetDHingeAnchor1(joint1, 0, 0, 3.5); + dJointSetDHingeAnchor2(joint1, 0, 0, 4.5); +#endif + +#if 1 + joint2 = dJointCreateDHinge(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDHingeAxis(joint2, 1, 0, 0); + dJointSetDHingeAnchor1(joint2, 0, 0, 2.5); + dJointSetDHingeAnchor2(joint2, 0, 0, 1.5); +#else + joint2 = dJointCreateDBall(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDBallAnchor1(joint2, 0, 0, 2.5); + dJointSetDBallAnchor2(joint2, 0, 0, 1.5); +#endif + + //dBodyAddForce(body1, 20, 0, 0); + + + // initial camera position + static float xyz[3] = {3.8966, -2.0614, 4.0300}; + static float hpr[3] = {153.5, -16.5, 0}; + dsSetViewpoint (xyz,hpr); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dBoxClass: + { + dVector3 lengths; + if (applyForce) + dsSetColor(1, .5, 0); + else + dsSetColor(1, 1, 0); + dsSetTexture (DS_WOOD); + dGeomBoxGetLengths(g, lengths); + dsDrawBox(pos, rot, lengths); + break; + } + + default: + {} + } +} + + +void simLoop(int pause) +{ + if (!pause) { + + static dReal t = 0; + + const dReal step = 0.005; + const unsigned nsteps = 2; + + for (unsigned i=0; i 2.; + + if (applyForce) { + dReal f = 0.3 * sin(t*1.2); + dBodyAddForceAtRelPos(body1, + f, 0, 0, + 0, 0, -0.5); // at the lower end + + dReal g = 0.3 * sin(t*0.7); + dBodyAddForceAtRelPos(body2, + 0, g, 0, + 0, 0, -0.5); // at the lower end + } + + t += step; + if (t > 20.) + t = 0.; + + dWorldQuickStep(world, step); + } + } + + // now we draw everything + unsigned ngeoms = dSpaceGetNumGeoms(space); + for (unsigned i=0; i +#include +#ifdef HAVE_UNISTD_H +#include +#endif +#include +#include +#include "texturepath.h" + + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#endif + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; + +static const int STACKCNT=10; // nr of weights on bridge +static const int SEGMCNT=16; // nr of segments in bridge +static const float SEGMDIM[3] = { 0.9, 4, 0.1 }; + +static dGeomID groundgeom; +static dBodyID segbodies[SEGMCNT]; +static dGeomID seggeoms[SEGMCNT]; +static dBodyID stackbodies[STACKCNT]; +static dGeomID stackgeoms[STACKCNT]; +static dJointID hinges[SEGMCNT-1]; +static dJointID sliders[2]; +static dJointFeedback jfeedbacks[SEGMCNT-1]; +static dReal colours[SEGMCNT]; +static int stress[SEGMCNT-1]; + +static dJointGroupID contactgroup; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + assert(o1); + assert(o2); + + if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) + { + fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); + // colliding a space with something + dSpaceCollide2(o1,o2,data,&nearCallback); + // Note we do not want to test intersections within a space, + // only between spaces. + return; + } + + const int N = 32; + dContact contact[N]; + int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); + if (n > 0) + { + for (int i=0; i forcelimit || l1 > forcelimit) + stress[i]++; + else + stress[i]=0; + if (stress[i]>4) + { + // Low-pass filter the noisy feedback data. + // Only after 4 consecutive timesteps with excessive load, snap. + fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i); + dJointAttach (hinges[i], 0, 0); + } + } + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + int i; + + double simstep = 0.002; // 2ms simulation steps + double dt = dsElapsedTime(); + int nrofsteps = (int) ceilf(dt/simstep); + for (i=0; i1.0) v=1.0; + if (v<0.5) + { + r=2*v; + g=1.0; + } + else + { + r=1.0; + g=2*(1.0-v); + } + dsSetColor (r,g,b); + drawGeom(seggeoms[i]); + } + dsSetColor (1,1,1); + for (i=0; i MU * body_mass * GRAVITY + (j+1)*FORCE > MU * (i+1)*MASS * GRAVITY + (j+1) > (i+1) * (MU*MASS*GRAVITY/FORCE) + (j+1) > (i+1) * k + +this should be independent of the number of contact points, as N contact +points will each have 1/N'th the normal force but the pushing force will +have to overcome N contacts. the constants are chosen so that k=1. +thus you should see a triangle made of half the bodies in the array start to +slide. + +*/ + + +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define LENGTH 0.2 // box length & width +#define HEIGHT 0.05 // box height +#define MASS 0.2 // mass of box[i][j] = (i+1) * MASS +#define FORCE 0.05 // force applied to box[i][j] = (j+1) * FORCE +#define MU 0.5 // the global mu to use +#define GRAVITY 0.5 // the global gravity to use +#define N1 10 // number of different forces to try +#define N2 10 // number of different masses to try + + +// dynamics and collision objects + +static dWorldID world; +static dSpaceID space; +static dBodyID body[N1][N2]; +static dJointGroupID contactgroup; +static dGeomID ground; +static dGeomID box[N1][N2]; + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + + // only collide things with the ground + int g1 = (o1 == ground); + int g2 = (o2 == ground); + if (!(g1 ^ g2)) return; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact[3]; // up to 3 contacts per box + for (i=0; i<3; i++) { + contact[i].surface.mode = dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = MU; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) { + for (i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + +// dynamics and collision objects +static dWorldID world = 0; + +static const dReal dt = 1/REAL(60.0); // 60 fps +// Water density if units are meters and kg +static const dReal density = 1000; + +// A long skinny thing +static dVector3 sides = {2,.5,.25}; +// Initial angular velocity +static dVector3 omega = {5,1,2}; +static dVector3 torque = {0,10,0}; +static dBodyID noGyroBody; +static dBodyID expGyroBody; +static dBodyID impGyroBody; + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {0,-4.0f,3.0f}; + static float hpr[3] = {90.0000,-15.0000,0.0000}; + dsSetViewpoint (xyz,hpr); + printf ("Press:\n" + "\t'a' to apply a torque\n" + "\t'r' to reset simulation.\n"); +} + +/** + Delete the bodies, etc. +*/ +static void clear() +{ + if (world) dWorldDestroy (world); + world = 0; +} + + + +/** + Cleanup if necessary and rebuild the + world. +*/ +static void reset() +{ + clear(); + + // create world + world = dWorldCreate(); + + // Calculate mass for a box; + dMass boxMass; + dMassSetBox(&boxMass,density,sides[0],sides[1],sides[2]); + + noGyroBody = dBodyCreate(world);// Conservation of ang-velocity + expGyroBody = dBodyCreate(world);// Explicit conservation of ang-momentum + impGyroBody = dBodyCreate(world);// Implicit conservation of ang-momentum + + dBodySetMass( noGyroBody , &boxMass ); + dBodySetMass( expGyroBody, &boxMass ); + dBodySetMass( impGyroBody, &boxMass ); + + // Try to avoid collisions. + dReal sep = dCalcVectorLength3(sides); + dBodySetPosition( noGyroBody , -sep, 0, sep); + dBodySetPosition( expGyroBody, 0, 0, sep); + dBodySetPosition( impGyroBody, sep, 0, sep); + + // Set the initial angular velocity + dBodySetAngularVel( noGyroBody , omega[0], omega[1], omega[2]); + dBodySetAngularVel( expGyroBody, omega[0], omega[1], omega[2]); + dBodySetAngularVel( impGyroBody, omega[0], omega[1], omega[2]); + + dBodySetGyroscopicMode( noGyroBody, 0); + // We compute this ourselves using the math + // that was in the old stepper. + dBodySetGyroscopicMode(expGyroBody, 0); + // Keep things from crashing by limiting + // the angular speed of the explicit body. + // Note that this isn't necessary for + // the other two bodies. + dBodySetMaxAngularSpeed( expGyroBody, 40 ); +} + +static void command (int cmd) +{ + switch (cmd) { + case 'a': case 'A': + dBodyAddTorque( noGyroBody, torque[0], torque[1], torque[2]); + dBodyAddTorque(expGyroBody, torque[0], torque[1], torque[2]); + dBodyAddTorque(impGyroBody, torque[0], torque[1], torque[2]); + break; + case 'r': case 'R': + reset(); + break; + } + +} + +/** + This is the explicit computation of + gyroscopic forces. +*/ +static void expStep(dBodyID body) +{ + // Explicit computation + dMatrix3 I,tmp; + dMass m; + dBodyGetMass(body,&m); + const dReal* R = dBodyGetRotation(body); + // compute inertia tensor in global frame + dMultiply2_333 (tmp,m.I,R); + dMultiply0_333 (I,R,tmp); + // compute explicit rotational force + // we treat 'tmp'like a vector, but that's okay. + const dReal* w = dBodyGetAngularVel(body); + dMultiply0_331 (tmp,I,w); + dVector3 tau; + dCalcVectorCross3(tau,tmp,w); + dBodyAddTorque(body,tau[0],tau[1],tau[2]); +} + + +// simulation loop +static void simLoop (int pause) +{ + if (!pause) { + expStep(expGyroBody); + dWorldStep (world,dt); + } + + dsSetTexture (DS_WOOD); + dsSetColor(1,0,0); + dsDrawBox(dBodyGetPosition(noGyroBody ),dBodyGetRotation(noGyroBody ),sides); + dsSetColor(1,1,0); + dsDrawBox(dBodyGetPosition(expGyroBody),dBodyGetRotation(expGyroBody),sides); + dsSetColor(0,1,0); + dsDrawBox(dBodyGetPosition(impGyroBody),dBodyGetRotation(impGyroBody),sides); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + reset(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + clear(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp b/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp new file mode 100644 index 0000000..5b6a532 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp @@ -0,0 +1,258 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + +bool write_world = false; +bool show_contacts = false; +dWorld * world; +dBody *top1, *top2; +dSpace *space; +dJointGroup contactgroup; + +const dReal pinradius = 0.05f; +const dReal pinlength = 1.5f; +const dReal topradius = 1.0f; +const dReal toplength = 0.25f; +const dReal topmass = 1.0f; + +#define MAX_CONTACTS 4 + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // for drawing the contact points + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + + int i; + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact[MAX_CONTACTS]; + int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact)); + + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed +static void reset(); +static void tilt(); + +static void command (int cmd) +{ + cmd = locase (cmd); + if (cmd == ' ') + { + reset(); + } + else if (cmd == 'a') { + tilt(); + } + else if (cmd == 't') { + show_contacts = !show_contacts; + } + else if (cmd == '1') { + write_world = true; + } +} + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + space->collide(0,&nearCallback); + if (!pause) + //world->quickStep(0.02); + world->step(0.02); + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (*world,f,"X"); + fclose (f); + } + write_world = false; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetTexture (DS_WOOD); + + dsSetColor (1,0.5f,0); + dsDrawCylinder(top1->getPosition(), + top1->getRotation(), + toplength, topradius); + dsDrawCapsule(top1->getPosition(), + top1->getRotation(), + pinlength, pinradius); + + dsSetColor (0.5f,1,0); + dsDrawCylinder(top2->getPosition(), + top2->getRotation(), + toplength, topradius); + dsDrawCapsule(top2->getPosition(), + top2->getRotation(), + pinlength, pinradius); + +} + + +static void reset() +{ + dMatrix3 R; + dRSetIdentity(R); + + top1->setRotation(R); + top2->setRotation(R); + + top1->setPosition(0.8f, -2, 2); + top2->setPosition(0.8f, 2, 2); + + top1->setAngularVel(1,0,7); + top2->setAngularVel(1,0,7); + + top1->setLinearVel(0,0.2f,0); + top2->setLinearVel(0,0.2f,0); +} + +static void tilt() +{ + top1->addTorque(0, 10, 0); + top2->addTorque(0, 10, 0); +} + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + + // create world + dInitODE(); + world = new dWorld(); + world->setGravity(0,0,-0.5f); + world->setCFM(1e-5f); + world->setLinearDamping(0.00001f); + world->setAngularDamping(0.0001f); + + space = new dSimpleSpace(0); + + dPlane *floor = new dPlane(*space, 0,0,1,0); + + top1 = new dBody(*world); + top2 = new dBody(*world); + + dMass m; + m.setCylinderTotal(1, 3, topradius, toplength); + top1->setMass(m); + top2->setMass(m); + + dGeom *g1, *g2, *pin1, *pin2; + g1 = new dCylinder(*space, topradius, toplength); + g1->setBody(*top1); + g2 = new dCylinder(*space, topradius, toplength); + g2->setBody(*top2); + + pin1 = new dCapsule(*space, pinradius, pinlength); + pin1->setBody(*top1); + pin2 = new dCapsule(*space, pinradius, pinlength); + pin2->setBody(*top2); + + top2->setGyroscopicMode(false); + + reset(); + + // run simulation + dsSimulationLoop (argc,argv,512,384,&fn); + + delete g1; + delete g2; + delete pin1; + delete pin2; + delete floor; + contactgroup.empty(); + delete top1; + delete top2; + delete space; + delete world; + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_heightfield.cpp b/libs/ode-0.16.1/ode/demo/demo_heightfield.cpp new file mode 100644 index 0000000..d68cb6a --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_heightfield.cpp @@ -0,0 +1,714 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include +#include "texturepath.h" +#include "bunny_geom.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +#define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians + +int g_allow_trimesh; + +// Our heightfield geom +dGeomID gheight; + + + +// Heightfield dimensions + +#define HFIELD_WSTEP 15 // Vertex count along edge >= 2 +#define HFIELD_DSTEP 31 + +#define HFIELD_WIDTH REAL( 4.0 ) +#define HFIELD_DEPTH REAL( 8.0 ) + +#define HFIELD_WSAMP ( HFIELD_WIDTH / ( HFIELD_WSTEP-1 ) ) +#define HFIELD_DSAMP ( HFIELD_DEPTH / ( HFIELD_DSTEP-1 ) ) + + + +//<---- Convex Object +dReal planes[]= // planes for a cube + { + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ + }; +const unsigned int planecount=6; + +dReal points[]= // points for a cube + { + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 + }; +const unsigned int pointcount=8; +unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#define dsDrawTriangle dsDrawTriangleD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 64 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body + + // Trimesh only - double buffered matrices for 'last transform' setup + dReal matrix_dblbuff[ 16 * 2 ]; + int last_matrix_index; +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; + + + + +//============================ + +dGeomID TriMesh1; +dGeomID TriMesh2; +//static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data + +//============================ + + +dReal heightfield_callback( void*, int x, int z ) +{ + dReal fx = ( ((dReal)x) - ( HFIELD_WSTEP-1 )/2 ) / (dReal)( HFIELD_WSTEP-1 ); + dReal fz = ( ((dReal)z) - ( HFIELD_DSTEP-1 )/2 ) / (dReal)( HFIELD_DSTEP-1 ); + + // Create an interesting 'hump' shape + dReal h = REAL( 1.0 ) + ( REAL( -16.0 ) * ( fx*fx*fx + fz*fz*fz ) ); + + return h; +} + + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) + return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command(int cmd) +{ + dsizeint i; + int j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase (cmd); + + + // + // Geom Creation + // + + if ( cmd == 'b' || cmd == 's' || cmd == 'c' || ( cmd == 'm' && g_allow_trimesh ) || + cmd == 'x' || cmd == 'y' || cmd == 'v' ) { + + if ( num < NUM ) { + i = num; + num++; + } else { + i = nextobj++; + nextobj %= num; + + // destroy the body and geoms for slot i + dBodyDestroy(obj[i].body); + obj[i].body = 0; + + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) { + dGeomDestroy(obj[i].geom[k]); + obj[i].geom[k] = 0; + } + } + + obj[i].body = dBodyCreate(world); + for (k=0; k<3; k++) + sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition(obj[i].body, + (dRandReal()-0.5)*HFIELD_WIDTH*0.75, + (dRandReal()-0.5)*HFIELD_DEPTH*0.75, + dRandReal() + 2 ); + dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } else { + dReal maxheight = 0; + for (k=0; k maxheight) + maxheight = pos[2]; + } + dBodySetPosition(obj[i].body, 0,maxheight+1,0); + dRFromAxisAndAngle(R,0,0,1,dRandReal()*10.0-5.0); + } + + dBodySetRotation(obj[i].body,R); + + if (cmd == 'b') { + + dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]); + + } else if (cmd == 'c') { + + sides[0] *= 0.5; + dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule(space,sides[0],sides[1]); + + } else if (cmd == 'v') { + + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); + + } else if (cmd == 'y') { + + dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]); + + } else if (cmd == 's') { + + sides[0] *= 0.5; + dMassSetSphere(&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere(space,sides[0]); + + } else if (cmd == 'm' && g_allow_trimesh) { + + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, + &Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); + + obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); + + dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); + printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); + dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); + dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); + + } else if (cmd == 'x') { + + setBody = 1; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j= num) + selected = 0; + if (selected < -1) + selected = 0; + + } else if (cmd == 'd' && selected >= 0 && selected < num) { + + dBodyDisable(obj[selected].body); + + } else if (cmd == 'e' && selected >= 0 && selected < num) { + + dBodyEnable(obj[selected].body); + + } else if (cmd == 'a') { + + show_aabb = !show_aabb; + + } else if (cmd == 't') { + + show_contacts = !show_contacts; + + } else if (cmd == 'r') { + + random_pos = !random_pos; + + } else if (cmd == '1') { + + write_world = 1; + + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!g) + return; + if (!pos) + pos = dGeomGetPosition(g); + if (!R) + R = dGeomGetRotation(g); + + int type = dGeomGetClass(g); + if (type == dBoxClass) { + + dVector3 sides; + dGeomBoxGetLengths(g,sides); + dsDrawBox(pos,R,sides); + + } else if (type == dSphereClass) { + + dsDrawSphere(pos,R,dGeomSphereGetRadius(g)); + + } else if (type == dCapsuleClass) { + + dReal radius,length; + dGeomCapsuleGetParams(g,&radius,&length); + dsDrawCapsule(pos,R,length,radius); + + } else if (type == dConvexClass) { + + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + + } else if (type == dCylinderClass) { + + dReal radius,length; + dGeomCylinderGetParams(g,&radius,&length); + dsDrawCylinder(pos,R,length,radius); + + } else if (type == dTriMeshClass) { + + dTriIndex* Indices = (dTriIndex*)::Indices; + + // assume all trimeshes are drawn as bunnies + for (int ii = 0; ii < IndexCount / 3; ii++) { + const dReal v[9] = { // explicit conversion from float to dReal + Vertices[Indices[ii * 3 + 0] * 3 + 0], + Vertices[Indices[ii * 3 + 0] * 3 + 1], + Vertices[Indices[ii * 3 + 0] * 3 + 2], + Vertices[Indices[ii * 3 + 1] * 3 + 0], + Vertices[Indices[ii * 3 + 1] * 3 + 1], + Vertices[Indices[ii * 3 + 1] * 3 + 2], + Vertices[Indices[ii * 3 + 2] * 3 + 0], + Vertices[Indices[ii * 3 + 2] * 3 + 1], + Vertices[Indices[ii * 3 + 2] * 3 + 2] + }; + dsDrawTriangle(pos, R, &v[0], &v[3], &v[6], 1); + } + + } else if (type == dHeightfieldClass) { + + // Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode. + int ox = (int) ( -HFIELD_WIDTH/2 ); + int oz = (int) ( -HFIELD_DEPTH/2 ); + + // for ( int tx = -1; tx < 2; ++tx ) + // for ( int tz = -1; tz < 2; ++tz ) + dsSetColorAlpha (0.5,1,0.5,0.5); + dsSetTexture( DS_WOOD ); + + for ( int i = 0; i < HFIELD_WSTEP - 1; ++i ) + for ( int j = 0; j < HFIELD_DSTEP - 1; ++j ) { + dReal a[3], b[3], c[3], d[3]; + + a[ 0 ] = ox + ( i ) * HFIELD_WSAMP; + a[ 1 ] = heightfield_callback( NULL, i, j ); + a[ 2 ] = oz + ( j ) * HFIELD_DSAMP; + + b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP; + b[ 1 ] = heightfield_callback( NULL, i + 1, j ); + b[ 2 ] = oz + ( j ) * HFIELD_DSAMP; + + c[ 0 ] = ox + ( i ) * HFIELD_WSAMP; + c[ 1 ] = heightfield_callback( NULL, i, j + 1 ); + c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP; + + d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP; + d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 ); + d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP; + + dsDrawTriangle( pos, R, a, c, b, 1 ); + dsDrawTriangle( pos, R, b, c, d, 1 ); + } + + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB(g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) + bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int i=0; i<3; i++) + bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity(RI); + dsSetColorAlpha(1,0,0,0.5); + dsDrawBox(bbpos,RI,bbsides); + } + +} + +// simulation loop + +static void simLoop (int pause) +{ + int i,j; + + dSpaceCollide(space,0,&nearCallback); + + if (!pause) + dWorldQuickStep(world,0.05); + + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF(world,f,"X"); + fclose (f); + } + write_world = 0; + } + + // remove all contact joints + dJointGroupEmpty(contactgroup); + + + + // + // Draw Heightfield + // + + drawGeom(gheight, 0, 0, 0); + + + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define SIDE (0.5f) // side length of a box +#define MASS (1.0) // mass of a box + + +// dynamics and collision objects +static dWorldID world; +static dBodyID body[2]; +static dJointID hinge; + + +// state set by keyboard commands +static int occasional_error = 0; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press 'e' to start/stop occasional error.\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == 'e' || cmd == 'E') { + occasional_error ^= 1; + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + const dReal kd = -0.3; // angular damping constant + if (!pause) { + // add an oscillating torque to body 0, and also damp its rotational motion + static dReal a=0; + const dReal *w = dBodyGetAngularVel (body[0]); + dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a)); + dWorldStep (world,0.05); + a += 0.01; + + // occasionally re-orient one of the bodies to create a deliberate error. + if (occasional_error) { + static int count = 0; + if ((count % 20)==0) { + // randomly adjust orientation of body[0] + const dReal *R1; + dMatrix3 R2,R3; + R1 = dBodyGetRotation (body[0]); + dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, + dRandReal()-0.5,dRandReal()-0.5); + dMultiply0 (R3,R1,R2,3,3,3); + dBodySetRotation (body[0],R3); + + // randomly adjust position of body[0] + const dReal *pos = dBodyGetPosition (body[0]); + dBodySetPosition (body[0], + pos[0]+0.2*(dRandReal()-0.5), + pos[1]+0.2*(dRandReal()-0.5), + pos[2]+0.2*(dRandReal()-0.5)); + } + count++; + } + } + + dReal sides1[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {SIDE,SIDE,SIDE*0.8f}; + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + dQuaternion q; + dQFromAxisAndAngle (q,1,1,0,0.25*M_PI); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1); + dBodySetQuaternion (body[0],q); + + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1); + dBodySetQuaternion (body[1],q); + + hinge = dJointCreateHinge (world,0); + dJointAttach (hinge,body[0],body[1]); + dJointSetHingeAnchor (hinge,0,0,1); + dJointSetHingeAxis (hinge,1,-1,1.41421356); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp new file mode 100644 index 0000000..b760af1 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp @@ -0,0 +1,434 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +This file try to demonstrate how the PR joint is working. + +The axisP is draw in red and the axisR is in green + +*/ + + +#include +#include +#include +#include +#include "texturepath.h" + + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// physics parameters +#define BOX1_LENGTH 2 // Size along the X axis +#define BOX1_WIDTH 1 // Size along the Y axis +#define BOX1_HEIGHT 0.4 // Size along the Z axis (up) since gravity is (0,0,-10) +#define BOX2_LENGTH 0.2 +#define BOX2_WIDTH 0.1 +#define BOX2_HEIGHT 0.4 +#define Mass1 10 +#define Mass2 0.1 + + +#define PRISMATIC_ONLY 1 +#define ROTOIDE_ONLY 2 +int flag = 0; + + +//camera view +static float xyz[3] = {2.0f,-3.5f,2.0000f}; +static float hpr[3] = {90.000f,-25.5000f,0.0000f}; +//world,space,body & geom +static dWorldID world; +static dSpaceID space; +static dSpaceID box1_space; +static dBodyID box1_body[1]; +static dBodyID box2_body[1]; +static dJointID joint[1]; +static dJointGroupID contactgroup; +static dGeomID ground; +static dGeomID box1[1]; +static dGeomID box2[1]; + + +//collision detection +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) + { + for (i=0; i= 2 ) + { + for (int i=1; i < argc; ++i) + { + if ( 0 == strcmp("-h", argv[i]) || 0 == strcmp("--help", argv[i]) ) + Help(argv); + + if (!flag && (0 == strcmp("-p", argv[i]) ||0 == strcmp("--prismatic-only", argv[i])) ) + flag = PRISMATIC_ONLY; + + if (!flag && (0 == strcmp("-r", argv[i]) || 0 == strcmp("--rotoide-only", argv[i])) ) + flag = ROTOIDE_ONLY; + + if (0 == strcmp("-t", argv[i]) || 0 == strcmp("--texture-path", argv[i])) + { + int j = i+1; + if ( j >= argc || // Check if we have enough arguments + argv[j][0] == '\0' || // We should have a path here + argv[j][0] == '-' ) // We should have a path not a command line + Help(argv); + else + fn.path_to_textures = argv[++i]; // Increase i since we use this argument + } + } + } + + dInitODE2(0); + + // create world + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-10); + ground = dCreatePlane (space,0,0,1,0); + + //create two boxes + dMass m; + box1_body[0] = dBodyCreate (world); + dMassSetBox (&m,1,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT); + dMassAdjust (&m,Mass1); + dBodySetMass (box1_body[0],&m); + box1[0] = dCreateBox (0,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT); + dGeomSetBody (box1[0],box1_body[0]); + + box2_body[0] = dBodyCreate (world); + dMassSetBox (&m,10,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT); + dMassAdjust (&m,Mass2); + dBodySetMass (box2_body[0],&m); + box2[0] = dCreateBox (0,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT); + dGeomSetBody (box2[0],box2_body[0]); + + //set the initial positions of body1 and body2 + dMatrix3 R; + dRSetIdentity(R); + dBodySetPosition (box1_body[0],0,0,BOX1_HEIGHT/2.0); + dBodySetRotation (box1_body[0], R); + + dBodySetPosition (box2_body[0], + 2.1, + 0.0, + BOX2_HEIGHT/2.0); + dBodySetRotation (box2_body[0], R); + + + //set PR joint + joint[0] = dJointCreatePR(world,0); + dJointAttach (joint[0],box1_body[0],box2_body[0]); + switch (flag) + { + case PRISMATIC_ONLY: + dJointSetPRAnchor (joint[0], + 2.1, + 0.0, + BOX2_HEIGHT/2.0); + dJointSetPRParam (joint[0],dParamLoStop, -0.5); + dJointSetPRParam (joint[0],dParamHiStop, 1.5); + break; + + case ROTOIDE_ONLY: + dJointSetPRAnchor (joint[0], + 0.0, + 0.0, + BOX2_HEIGHT/2.0); + dJointSetPRParam (joint[0],dParamLoStop, 0.0); + dJointSetPRParam (joint[0],dParamHiStop, 0.0); + break; + + default: + dJointSetPRAnchor (joint[0], + 1.1, + 0.0, + BOX2_HEIGHT/2.0); + dJointSetPRParam (joint[0],dParamLoStop, -0.5); + dJointSetPRParam (joint[0],dParamHiStop, 1.5); + break; + } + + dJointSetPRAxis1(joint[0],1,0,0); + dJointSetPRAxis2(joint[0],0,0,1); +// We position the 2 body +// The position of the rotoide joint is on the second body so it can rotate on itself +// and move along the X axis. +// With this anchor +// - A force in X will move only the body 2 inside the low and hi limit +// of the prismatic +// - A force in Y will make the 2 bodies to rotate around on the plane + + box1_space = dSimpleSpaceCreate (space); + dSpaceSetCleanup (box1_space,0); + dSpaceAdd(box1_space,box1[0]); + + // run simulation + dsSimulationLoop (argc,argv,400,300,&fn); + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp new file mode 100644 index 0000000..6ec3093 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp @@ -0,0 +1,735 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + This program demonstrates how the PU joint works. + A PU joint is a combination of a Universal joint and a Slider joint. + It is a universal joint with a slider between the anchor point and + body 1. + + + The upper yellow body is fixed to the world + The lower yellow body is attached to the upper body by a PU joint + The green object is one aprt of the slider. + The purple object is the second part of the slider. + The red object represent the axis1 of the universal part. + The blue object represent the axis2 of the universal part. + The gray object represent the anchor2 of the PU joint. +*/ + + +#include +#include +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + +enum IDX_CYL_DIM +{ + RADIUS, + LENGTH, + NUM_CYL_DIM +}; + + +const dVector3 boxDim = {1,1,1}; +const dVector3 extDim = {0.2,0.2,1.2}; +const dVector3 ancDim = {0.2,0.2,0.5}; +const dReal axDim[NUM_CYL_DIM] = {0.1,1.0}; + + +int type = dJointTypePU; + + +const dReal VEL_INC = 0.01; // Velocity increment + +// physics parameters +const dReal PI = 3.14159265358979323846264338327950288419716939937510; + + +const dReal INT_EXT_RATIO = 0.8; + +#define X 0 +#define Y 1 +#define Z 2 + +enum INDEX +{ + W = 0, + D, + EXT, + INT, + AXIS1, + AXIS2, + ANCHOR, + GROUND, + NUM_PARTS, + ALL = NUM_PARTS, + // INDEX for catBits + JOINT, + LAST_INDEX_CNT +}; + +const int catBits[LAST_INDEX_CNT] = + { + 0x0001, ///< W Cylinder category + 0x0002, ///< D Cylinder category + 0x0004, ///< EXT sliderr category + 0x0008, ///< INT slider category + 0x0010, ///< AXIS1 universal category + 0x0020, ///< AXIS2 universal category + 0x0040, ///< ANCHOR category + 0x0080, ///< Ground category + ~0L, ///< All categories + 0x0004 | 0x0008 | 0x0010 | 0x0020 ///< JOINT category + }; + +#define Mass1 10 +#define Mass2 8 + + +//camera view +static float xyz[3] = {6.0f,0.0f,6.0000f}; +static float hpr[3] = {-180.000f,-25.5000f,0.0000f}; + + +//world,space,body & geom +static dWorldID world; +static dSpaceID space; +static dJointGroupID contactgroup; +static dBodyID body[NUM_PARTS]; +static dGeomID geom[NUM_PARTS]; + +static dJoint *joint; + + + +const dReal BOX_SIDES[3] = {1.0,1.0,1.0}; +const dReal OBS_SIDES[3] = {0.4,0.4,0.4}; +const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2}; + + +//collision detection +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) ); + if (n > 0) { + for (i=0; igetParam (dParamVel3) - VEL_INC; + joint->setParam (dParamVel3, vel); + joint->setParam (dParamFMax3, 2); + std::cout<<"Velocity = "<' : { + dReal vel = joint->getParam (dParamVel3) + VEL_INC; + joint->setParam (dParamVel3, vel); + joint->setParam (dParamFMax3, 2); + std::cout<<"Velocity = "<getParam (dParamFMax) ) { + aLimit = dInfinity; + lLimit = dInfinity; + fmax = 0; + } + else { + aLimit = 0.25*PI; + lLimit = 0.5*axDim[LENGTH]; + fmax = 0.02; + } + + joint->setParam (dParamFMax1, fmax); + joint->setParam (dParamFMax2, fmax); + joint->setParam (dParamFMax3, fmax); + + switch (joint->getType() ) { + case dJointTypePR : { + dPRJoint *pr = reinterpret_cast (joint); + pr->setParam (dParamLoStop, -lLimit); + pr->setParam (dParamHiStop, -lLimit); + pr->setParam (dParamLoStop2, aLimit); + pr->setParam (dParamHiStop2, -aLimit); + } + break; + case dJointTypePU : { + dPUJoint *pu = reinterpret_cast (joint); + pu->setParam (dParamLoStop1, -aLimit); + pu->setParam (dParamHiStop1, aLimit); + pu->setParam (dParamLoStop2, -aLimit); + pu->setParam (dParamHiStop2, aLimit); + pu->setParam (dParamLoStop3, -lLimit); + pu->setParam (dParamHiStop3, lLimit); + } + break; + default: {} // keep the compiler happy + } + } + + break; + + case 'g': case 'G' : { + dVector3 g; + dWorldGetGravity(world, g); + if ( g[2]< -0.1 ) + dWorldSetGravity(world, 0, 0, 0); + else + dWorldSetGravity(world, 0, 0, -0.5); + + } + +case 'p' :case 'P' : { + switch (joint->getType() ) { + case dJointTypeSlider : { + dSliderJoint *sj = reinterpret_cast (joint); + std::cout<<"Position ="<getPosition() <<"\n"; + } + break; + case dJointTypePU : { + dPUJoint *pu = reinterpret_cast (joint); + std::cout<<"Position ="<getPosition() <<"\n"; + std::cout<<"Position Rate="<getPositionRate() <<"\n"; + std::cout<<"Angle1 ="<getAngle1() <<"\n"; + std::cout<<"Angle1 Rate="<getAngle1Rate() <<"\n"; + std::cout<<"Angle2 ="<getAngle2() <<"\n"; + std::cout<<"Angle2 Rate="<getAngle2Rate() <<"\n"; + } + break; + default: {} // keep the compiler happy + } + } + break; + } +} + +static void drawBox (dGeomID id, int R, int G, int B) +{ + if (!id) + return; + + const dReal *pos = dGeomGetPosition (id); + const dReal *rot = dGeomGetRotation (id); + dsSetColor (R,G,B); + + dVector3 l; + dGeomBoxGetLengths (id, l); + dsDrawBox (pos, rot, l); +} + + +// simulation loop +static void simLoop (int pause) +{ + static bool todo = false; + if ( todo ) { // DEBUG + static int cnt = 0; + ++cnt; + + if (cnt == 5) + command ( 'q' ); + if (cnt == 10) + dsStop(); + } + + + + + if (!pause) { + double simstep = 0.01; // 10ms simulation steps + double dt = dsElapsedTime(); + + int nrofsteps = (int) ceilf (dt/simstep); + if (!nrofsteps) + nrofsteps = 1; + + for (int i=0; i (joint); + ang1 = pu->getAngle1(); + ang2 = pu->getAngle2(); + pu->getAxis1 (axisR1); + pu->getAxis2 (axisR2); + pu->getAxisP (axisP); + + dJointGetPUAnchor (pu->id(), anchorPos); + } + else if ( dJointTypePR == type ) { + dPRJoint *pr = dynamic_cast (joint); + pr->getAxis1 (axisP); + pr->getAxis2 (axisR1); + + dJointGetPRAnchor (pr->id(), anchorPos); + } + + + // Draw the axisR + if ( geom[INT] ) { + dsSetColor (1,0,1); + dVector3 l; + dGeomBoxGetLengths (geom[INT], l); + + const dReal *rotBox = dGeomGetRotation (geom[W]); + + dVector3 pos; + for (int i=0; i<3; ++i) + pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i]; + dsDrawBox (pos, rotBox, l); + } + + dsSetTexture (DS_CHECKERED); + if ( geom[AXIS1] ) { + dQuaternion q, qAng; + dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); + dGeomGetQuaternion (geom[AXIS1], q); + + dQuaternion qq; + dQMultiply1 (qq, qAng, q); + dMatrix3 R; + dQtoR (qq,R); + + + dGeomCylinderGetParams (geom[AXIS1], &radius, &length); + dsSetColor (1,0,0); + dsDrawCylinder (anchorPos, R, length, radius); + } + + if ( dJointTypePU == type && geom[AXIS2] ) { + //dPUJoint *pu = dynamic_cast (joint); + + dQuaternion q, qAng, qq, qq1; + dGeomGetQuaternion (geom[AXIS2], q); + + dQFromAxisAndAngle (qAng, 0, 1, 0, ang2); + dQMultiply1 (qq, qAng, q); + + + dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); + + dQMultiply1 (qq1, qAng, qq); + + + dMatrix3 R; + dQtoR (qq1,R); + + + dGeomCylinderGetParams (geom[AXIS2], &radius, &length); + dsSetColor (0,0,1); + dsDrawCylinder (anchorPos, R, length, radius); + } + + dsSetTexture (DS_WOOD); + + // Draw the anchor + if ( geom[ANCHOR] ) { + dsSetColor (1,1,1); + dVector3 l; + dGeomBoxGetLengths (geom[ANCHOR], l); + + const dReal *rotBox = dGeomGetRotation (geom[D]); + const dReal *posBox = dGeomGetPosition (geom[D]); + + dVector3 e; + for (int i=0; i<3; ++i) + e[i] = posBox[i] - anchorPos[i]; + dNormalize3 (e); + + dVector3 pos; + for (int i=0; i<3; ++i) + pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i]; + dsDrawBox (pos, rotBox, l); + } + + drawBox (geom[D], 1,1,0); + } +} + + +void Help (char **argv) +{ + printf ("%s ", argv[0]); + printf (" -h | --help : print this help\n"); + printf (" -p | --PRJoint : Use a PR joint instead of PU joint\n"); + printf (" -t | --texture-path path : Path to the texture.\n"); + printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH); + printf ("--------------------------------------------------\n"); + printf ("Hit any key to continue:"); + getchar(); + + exit (0); +} + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + if (argc >= 2 ) { + for (int i=1; i < argc; ++i) { + if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) + Help (argv); + + if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) ) + type = dJointTypePR; + + if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) { + int j = i+1; + if ( j >= argc || // Check if we have enough arguments + argv[j][0] == '\0' || // We should have a path here + argv[j][0] == '-' ) // We should have a path not a command line + Help (argv); + else + fn.path_to_textures = argv[++i]; // Increase i since we use this argument + } + } + } + + dInitODE2(0); + + world = dWorldCreate(); + dWorldSetERP (world, 0.8); + + space = dSimpleSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + geom[GROUND] = dCreatePlane (space, 0,0,1,0); + dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); + dGeomSetCollideBits (geom[GROUND], catBits[ALL]); + + dMass m; + + // Create the body attached to the World + body[W] = dBodyCreate (world); + // Main axis of cylinder is along X=1 + m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]); + m.adjust (Mass1); + geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]); + dGeomSetBody (geom[W], body[W]); + dGeomSetCategoryBits (geom[W], catBits[W]); + dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) ); + dBodySetMass (body[W], &m); + + + + + + // Create the dandling body + body[D] = dBodyCreate (world); + // Main axis of capsule is along X=1 + m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]); + m.adjust (Mass1); + geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]); + dGeomSetBody (geom[D], body[D]); + dGeomSetCategoryBits (geom[D], catBits[D]); + dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) ); + dBodySetMass (body[D], &m); + + + // Create the external part of the slider joint + geom[EXT] = dCreateBox (0, extDim[X], extDim[Y], extDim[Z]); + dGeomSetCategoryBits (geom[EXT], catBits[EXT]); + dGeomSetCollideBits (geom[EXT], + catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); + + // Create the internal part of the slider joint + geom[INT] = dCreateBox (0, INT_EXT_RATIO*extDim[X], + INT_EXT_RATIO*extDim[Y], + INT_EXT_RATIO*extDim[Z]); + dGeomSetCategoryBits (geom[INT], catBits[INT]); + dGeomSetCollideBits (geom[INT], + catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); + + + dMatrix3 R; + // Create the first axis of the universal joi9nt + //Rotation of 90deg around y + geom[AXIS1] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]); + dRFromAxisAndAngle(R, 0,1,0, 0.5*PI); + dGeomSetRotation(geom[AXIS1], R); + dGeomSetCategoryBits(geom[AXIS1], catBits[AXIS1]); + dGeomSetCollideBits(geom[AXIS1], + catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]); + + + // Create the second axis of the universal joint + geom[AXIS2] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]); + //Rotation of 90deg around y + dRFromAxisAndAngle(R, 1,0,0, 0.5*PI); + dGeomSetRotation(geom[AXIS2], R); + dGeomSetCategoryBits(geom[AXIS2], catBits[AXIS2]); + dGeomSetCollideBits(geom[AXIS2], + catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]); + + // Create the anchor + geom[ANCHOR] = dCreateBox (0, ancDim[X], ancDim[Y], ancDim[Z]); + dGeomSetCategoryBits(geom[ANCHOR], catBits[ANCHOR]); + dGeomSetCollideBits(geom[ANCHOR], + catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); + + + + if (body[W]) { + dBodySetPosition(body[W], 0, 0, 5); + } + + + if (geom[EXT]) { + dGeomSetPosition(geom[EXT], 0,0,3.8); + } + if (geom[INT]) { + dGeomSetPosition(geom[INT], 0,0,2.6); + } + if (geom[AXIS1]) { + dGeomSetPosition(geom[AXIS1], 0,0,2.5); + } + if (geom[AXIS2]) { + dGeomSetPosition(geom[AXIS2], 0,0,2.5); + } + + if (geom[ANCHOR]) { + dGeomSetPosition(geom[ANCHOR], 0,0,2.25); + } + + if (body[D]) { + dBodySetPosition(body[D], 0,0,1.5); + } + + + + // Attache the upper box to the world + dJointID fixed = dJointCreateFixed (world,0); + dJointAttach (fixed , NULL, body[W]); + dJointSetFixed (fixed ); + + if (type == dJointTypePR) { + dPRJoint *pr = new dPRJoint (world, 0); + pr->attach (body[W], body[D]); + pr->setAxis1 (0, 0, -1); + pr->setAxis2 (1, 0, 0); + joint = pr; + + dJointSetPRAnchor (pr->id(), 0, 0, 2.5); + } + else { + dPUJoint *pu = new dPUJoint (world, 0); + pu->attach (body[W], body[D]); + pu->setAxis1 (1, 0, 0); + pu->setAxis2 (0, 1, 0); + pu->setAxisP (0, 0, -1); + joint = pu; + + dJointSetPUAnchor (pu->id(), 0, 0, 2.5); + } + + + // run simulation + dsSimulationLoop (argc,argv,400,300,&fn); + + delete joint; + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + diff --git a/libs/ode-0.16.1/ode/demo/demo_joints.cpp b/libs/ode-0.16.1/ode/demo/demo_joints.cpp new file mode 100644 index 0000000..d545369 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_joints.cpp @@ -0,0 +1,1090 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +perform tests on all the joint types. +this should be done using the double precision version of the library. + +usage: + test_joints [-nXXX] [-g] [-i] [-e] [path_to_textures] + +if a test number is given then that specific test is performed, otherwise +all the tests are performed. the tests are numbered `xxyy', where xx +corresponds to the joint type and yy is the sub-test number. not every +number maps to an actual test. + +flags: + i: the test is interactive. + g: turn off graphical display (can't use this with `i'). + e: turn on occasional error perturbations + n: performe test XXX +some tests compute and display error values. these values are scaled so +<1 is good and >1 is bad. other tests just show graphical results which +you must verify visually. + +*/ + +#include +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define NUM_JOINTS 10 // number of joints to test (the `xx' value) +#define SIDE (0.5f) // side length of a box - don't change this +#define MASS (1.0) // mass of a box +#define STEPSIZE 0.05 + +static const dVector3 xunit = { 1, 0, 0 }, zunit = { 0, 0, 1 }; + + +// dynamics objects +static dWorldID world; +static dBodyID body[2]; +static dJointID joint; + + +// data from the command line arguments +static int cmd_test_num = -1; +static int cmd_interactive = 0; +static int cmd_graphics = 1; +static char *cmd_path_to_textures = NULL; +static int cmd_occasional_error = 0; // perturb occasionally + + +// info about the current test +struct TestInfo; +static int test_num = 0; // number of the current test +static int iteration = 0; +static int max_iterations = 0; +static dReal max_error = 0; + +//**************************************************************************** +// utility stuff + +static dReal length (dVector3 a) +{ + return dSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]); +} + + +// get the max difference between a 3x3 matrix and the identity + +dReal cmpIdentity (const dMatrix3 A) +{ + dMatrix3 I; + dSetZero (I,12); + I[0] = 1; + I[5] = 1; + I[10] = 1; + return dMaxDifference (A,I,3,3); +} + +//**************************************************************************** +// test world construction and utilities + +void constructWorldForTest (dReal gravity, int bodycount, + /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z, + /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z, + /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z, + /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z, + /* rotation angles */ dReal a1, dReal a2) +{ + // create world + world = dWorldCreate(); + dWorldSetERP (world,0.2); + dWorldSetCFM (world,1e-6); + dWorldSetGravity (world,0,0,gravity); + + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0], pos1x, pos1y, pos1z); + dQuaternion q; + dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1); + dBodySetQuaternion (body[0],q); + + if (bodycount==2) { + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dBodySetPosition (body[1], pos2x, pos2y, pos2z); + dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2); + dBodySetQuaternion (body[1],q); + } + else body[1] = 0; +} + + +// add an oscillating torque to body 0 + +void addOscillatingTorque (dReal tscale) +{ + static dReal a=0; + dBodyAddTorque (body[0],tscale*cos(2*a),tscale*cos(2.7183*a), + tscale*cos(1.5708*a)); + a += 0.01; +} + + +void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z) +{ + static dReal a=0; + dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y, + tscale * cos(a) * z); + a += 0.02; +} + + +// damp the rotational motion of body 0 a bit + +void dampRotationalMotion (dReal kd) +{ + const dReal *w = dBodyGetAngularVel (body[0]); + dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]); +} + + +// add a spring force to keep the bodies together, otherwise they may fly +// apart with some joints. + +void addSpringForce (dReal ks) +{ + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),ks*(p2[2]-p1[2])); + dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),ks*(p1[2]-p2[2])); +} + + +// add an oscillating Force to body 0 + +void addOscillatingForce (dReal fscale) +{ + static dReal a=0; + dBodyAddForce (body[0],fscale*cos(2*a),fscale*cos(2.7183*a), + fscale*cos(1.5708*a)); + a += 0.01; +} + +//**************************************************************************** +// stuff specific to the tests +// +// 0xx : fixed +// 1xx : ball and socket +// 2xx : hinge +// 3xx : slider +// 4xx : hinge 2 +// 5xx : contact +// 6xx : amotor +// 7xx : universal joint +// 8xx : PR joint (Prismatic and Rotoide) + +// setup for the given test. return 0 if there is no such test + +int setupTest (int n) +{ + switch (n) { + + // ********** fixed joint + + case 0: { // 2 body + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, + 0.25*M_PI,0.25*M_PI); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetFixed (joint); + return 1; + } + + case 1: { // 1 body to static env + constructWorldForTest (0,1, + 0.5*SIDE,0.5*SIDE,1, 0,0,0, + 1,0,0, 1,0,0, + 0,0); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],0); + dJointSetFixed (joint); + return 1; + } + + case 2: { // 2 body with relative rotation + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, + 0.25*M_PI,-0.25*M_PI); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetFixed (joint); + return 1; + } + + case 3: { // 1 body to static env with relative rotation + constructWorldForTest (0,1, + 0.5*SIDE,0.5*SIDE,1, 0,0,0, + 1,0,0, 1,0,0, + 0.25*M_PI,0); + joint = dJointCreateFixed (world,0); + dJointAttach (joint,body[0],0); + dJointSetFixed (joint); + return 1; + } + + // ********** hinge joint + + case 200: // 2 body + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,1,-1,1.41421356); + return 1; + + case 220: // hinge angle polarity test + case 221: // hinge angle rate test + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,0,0,1); + max_iterations = 50; + return 1; + + case 230: // hinge motor rate (and polarity) test + case 231: // ...with stops + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,0,0,1); + dJointSetHingeParam (joint,dParamFMax,1); + if (n==231) { + dJointSetHingeParam (joint,dParamLoStop,-0.5); + dJointSetHingeParam (joint,dParamHiStop,0.5); + } + return 1; + + case 250: // limit bounce test (gravity down) + case 251: { // ...gravity up + constructWorldForTest ((n==251) ? 0.1 : -0.1, 2, + 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHingeAnchor (joint,0,0,1); + dJointSetHingeAxis (joint,0,1,0); + dJointSetHingeParam (joint,dParamLoStop,-0.9); + dJointSetHingeParam (joint,dParamHiStop,0.7854); + dJointSetHingeParam (joint,dParamBounce,0.5); + // anchor 2nd body with a fixed joint + dJointID j = dJointCreateFixed (world,0); + dJointAttach (j,body[1],0); + dJointSetFixed (j); + return 1; + } + + // ********** slider + + case 300: // 2 body + constructWorldForTest (0,2, + 0,0,1, 0.2,0.2,1.2, + 0,0,1, -1,1,0, 0,0.25*M_PI); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,1,1,1); + return 1; + + case 320: // slider angle polarity test + case 321: // slider angle rate test + constructWorldForTest (0,2, + 0,0,1, 0,0,1.2, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,0,0,1); + max_iterations = 50; + return 1; + + case 330: // slider motor rate (and polarity) test + case 331: // ...with stops + constructWorldForTest (0, 2, + 0,0,1, 0,0,1.2, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,0,0,1); + dJointSetSliderParam (joint,dParamFMax,100); + if (n==331) { + dJointSetSliderParam (joint,dParamLoStop,-0.4); + dJointSetSliderParam (joint,dParamHiStop,0.4); + } + return 1; + + case 350: // limit bounce tests + case 351: { + constructWorldForTest ((n==351) ? 0.1 : -0.1, 2, + 0,0,1, 0,0,1.2, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateSlider (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetSliderAxis (joint,0,0,1); + dJointSetSliderParam (joint,dParamLoStop,-0.5); + dJointSetSliderParam (joint,dParamHiStop,0.5); + dJointSetSliderParam (joint,dParamBounce,0.5); + // anchor 2nd body with a fixed joint + dJointID j = dJointCreateFixed (world,0); + dJointAttach (j,body[1],0); + dJointSetFixed (j); + return 1; + } + + // ********** hinge-2 joint + + case 420: // hinge-2 steering angle polarity test + case 421: // hinge-2 steering angle rate test + constructWorldForTest (0,2, + 0.5*SIDE,0,1, -0.5*SIDE,0,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge2 (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); + dJointSetHinge2Axes (joint, zunit, xunit); + max_iterations = 50; + return 1; + + case 430: // hinge 2 steering motor rate (+polarity) test + case 431: // ...with stops + case 432: // hinge 2 wheel motor rate (+polarity) test + constructWorldForTest (0,2, + 0.5*SIDE,0,1, -0.5*SIDE,0,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateHinge2 (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); + dJointSetHinge2Axes (joint, zunit, xunit); + dJointSetHinge2Param (joint,dParamFMax,1); + dJointSetHinge2Param (joint,dParamFMax2,1); + if (n==431) { + dJointSetHinge2Param (joint,dParamLoStop,-0.5); + dJointSetHinge2Param (joint,dParamHiStop,0.5); + } + return 1; + + // ********** angular motor joint + + case 600: // test euler angle calculations + constructWorldForTest (0,2, + -SIDE*0.5,0,1, SIDE*0.5,0,1, + 0,0,1, 0,0,1, 0,0); + joint = dJointCreateAMotor (world,0); + dJointAttach (joint,body[0],body[1]); + + dJointSetAMotorNumAxes (joint,3); + dJointSetAMotorAxis (joint,0,1, 0,0,1); + dJointSetAMotorAxis (joint,2,2, 1,0,0); + dJointSetAMotorMode (joint,dAMotorEuler); + max_iterations = 200; + return 1; + + // ********** universal joint + + case 700: // 2 body + case 701: + case 702: + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); + joint = dJointCreateUniversal (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetUniversalAnchor (joint,0,0,1); + dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356); + dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356); + return 1; + + case 720: // universal transmit torque test + case 721: + case 722: + case 730: // universal torque about axis 1 + case 731: + case 732: + case 740: // universal torque about axis 2 + case 741: + case 742: + constructWorldForTest (0,2, + 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, + 1,0,0, 1,0,0, 0,0); + joint = dJointCreateUniversal (world,0); + dJointAttach (joint,body[0],body[1]); + dJointSetUniversalAnchor (joint,0,0,1); + dJointSetUniversalAxis1 (joint,0,0,1); + dJointSetUniversalAxis2 (joint, 1, -1,0); + max_iterations = 100; + return 1; + + // Joint PR (Prismatic and Rotoide) + case 800: // 2 body + case 801: // 2 bodies with spring force and prismatic fixed + case 802: // 2 bodies with torque on body1 and prismatic fixed + constructWorldForTest (0, 2, + -1.0, 0.0, 1.0, + 1.0, 0.0, 1.0, + 1,0,0, 1,0,0, + 0, 0); + joint = dJointCreatePR (world, 0); + dJointAttach (joint, body[0], body[1]); + dJointSetPRAnchor (joint,-0.5, 0.0, 1.0); + dJointSetPRAxis1 (joint, 0, 1, 0); + dJointSetPRAxis2 (joint, 1, 0, 0); + dJointSetPRParam (joint,dParamLoStop,-0.5); + dJointSetPRParam (joint,dParamHiStop,0.5); + dJointSetPRParam (joint,dParamLoStop2,0); + dJointSetPRParam (joint,dParamHiStop2,0); + return 1; + case 803: // 2 bodies with spring force and prismatic NOT fixed + case 804: // 2 bodies with torque force and prismatic NOT fixed + case 805: // 2 bodies with force only on first body + constructWorldForTest (0, 2, + -1.0, 0.0, 1.0, + 1.0, 0.0, 1.0, + 1,0,0, 1,0,0, + 0, 0); + joint = dJointCreatePR (world, 0); + dJointAttach (joint, body[0], body[1]); + dJointSetPRAnchor (joint,-0.5, 0.0, 1.0); + dJointSetPRAxis1 (joint, 0, 1, 0); + dJointSetPRAxis2 (joint, 1, 0, 0); + dJointSetPRParam (joint,dParamLoStop,-0.5); + dJointSetPRParam (joint,dParamHiStop,0.5); + dJointSetPRParam (joint,dParamLoStop2,-0.5); + dJointSetPRParam (joint,dParamHiStop2,0.5); + return 1; + } + return 0; +} + + +// do stuff specific to this test each iteration. you can check some +// invariants for the test -- the return value is some scaled error measurement +// that must be less than 1. +// return a dInfinity if error is not measured for this n. + +dReal doStuffAndGetError (int n) +{ + switch (n) { + + // ********** fixed joint + + case 0: { // 2 body + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + // check the orientations are the same + const dReal *R1 = dBodyGetRotation (body[0]); + const dReal *R2 = dBodyGetRotation (body[1]); + dReal err1 = dMaxDifference (R1,R2,3,3); + // check the body offset is correct + dVector3 p,pp; + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; + dMultiply1_331 (pp,R1,p); + pp[0] += 0.5; + pp[1] += 0.5; + return (err1 + length (pp)) * 300; + } + + case 1: { // 1 body to static env + addOscillatingTorque (0.1); + + // check the orientation is the identity + dReal err1 = cmpIdentity (dBodyGetRotation (body[0])); + + // check the body offset is correct + dVector3 p; + const dReal *p1 = dBodyGetPosition (body[0]); + for (int i=0; i<3; i++) p[i] = p1[i]; + p[0] -= 0.25; + p[1] -= 0.25; + p[2] -= 1; + return (err1 + length (p)) * 1e6; + } + + case 2: { // 2 body + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + // check the body offset is correct + // Should really check body rotation too. Oh well. + const dReal *R1 = dBodyGetRotation (body[0]); + dVector3 p,pp; + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; + dMultiply1_331 (pp,R1,p); + pp[0] += 0.5; + pp[1] += 0.5; + return length(pp) * 300; + } + + case 3: { // 1 body to static env with relative rotation + addOscillatingTorque (0.1); + + // check the body offset is correct + dVector3 p; + const dReal *p1 = dBodyGetPosition (body[0]); + for (int i=0; i<3; i++) p[i] = p1[i]; + p[0] -= 0.25; + p[1] -= 0.25; + p[2] -= 1; + return length (p) * 1e6; + } + + + // ********** hinge joint + + case 200: // 2 body + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + return dInfinity; + + case 220: // hinge angle polarity test + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + if (iteration == 40) { + dReal a = dJointGetHingeAngle (joint); + if (a > 0.5 && a < 1) return 0; else return 10; + } + return 0; + + case 221: { // hinge angle rate test + static dReal last_angle = 0; + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + dReal a = dJointGetHingeAngle (joint); + dReal r = dJointGetHingeAngleRate (joint); + dReal er = (a-last_angle)/STEPSIZE; // estimated rate + last_angle = a; + return fabs(r-er) * 4e4; + } + + case 230: // hinge motor rate (and polarity) test + case 231: { // ...with stops + static dReal a = 0; + dReal r = dJointGetHingeAngleRate (joint); + dReal err = fabs (cos(a) - r); + if (a==0) err = 0; + a += 0.03; + dJointSetHingeParam (joint,dParamVel,cos(a)); + if (n==231) return dInfinity; + return err * 1e6; + } + + // ********** slider joint + + case 300: // 2 body + addOscillatingTorque (0.05); + dampRotationalMotion (0.1); + addSpringForce (0.5); + return dInfinity; + + case 320: // slider angle polarity test + dBodyAddForce (body[0],0,0,0.1); + dBodyAddForce (body[1],0,0,-0.1); + if (iteration == 40) { + dReal a = dJointGetSliderPosition (joint); + if (a > 0.2 && a < 0.5) + return 0; + else + return 10; // Failed + } + return 0; + + case 321: { // slider angle rate test + static dReal last_pos = 0; + dBodyAddForce (body[0],0,0,0.1); + dBodyAddForce (body[1],0,0,-0.1); + dReal p = dJointGetSliderPosition (joint); + dReal r = dJointGetSliderPositionRate (joint); + dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact) + last_pos = p; + return fabs(r-er) * 1e9; + } + + case 330: // slider motor rate (and polarity) test + case 331: { // ...with stops + static dReal a = 0; + dReal r = dJointGetSliderPositionRate (joint); + dReal err = fabs (0.7*cos(a) - r); + if (a < 0.04) err = 0; + a += 0.03; + dJointSetSliderParam (joint,dParamVel,0.7*cos(a)); + if (n==331) return dInfinity; + return err * 1e6; + } + + // ********** hinge-2 joint + + case 420: // hinge-2 steering angle polarity test + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + if (iteration == 40) { + dReal a = dJointGetHinge2Angle1 (joint); + if (a > 0.5 && a < 0.6) return 0; else return 10; + } + return 0; + + case 421: { // hinge-2 steering angle rate test + static dReal last_angle = 0; + dBodyAddTorque (body[0],0,0,0.01); + dBodyAddTorque (body[1],0,0,-0.01); + dReal a = dJointGetHinge2Angle1 (joint); + dReal r = dJointGetHinge2Angle1Rate (joint); + dReal er = (a-last_angle)/STEPSIZE; // estimated rate + last_angle = a; + return fabs(r-er)*2e4; + } + + case 430: // hinge 2 steering motor rate (+polarity) test + case 431: { // ...with stops + static dReal a = 0; + dReal r = dJointGetHinge2Angle1Rate (joint); + dReal err = fabs (cos(a) - r); + if (a==0) err = 0; + a += 0.03; + dJointSetHinge2Param (joint,dParamVel,cos(a)); + if (n==431) return dInfinity; + return err * 1e6; + } + + case 432: { // hinge 2 wheel motor rate (+polarity) test + static dReal a = 0; + dReal r = dJointGetHinge2Angle2Rate (joint); + dReal err = fabs (cos(a) - r); + if (a==0) err = 0; + a += 0.03; + dJointSetHinge2Param (joint,dParamVel2,cos(a)); + return err * 1e6; + } + + // ********** angular motor joint + + case 600: { // test euler angle calculations + // desired euler angles from last iteration + static dReal a1,a2,a3; + + // find actual euler angles + dReal aa1 = dJointGetAMotorAngle (joint,0); + dReal aa2 = dJointGetAMotorAngle (joint,1); + dReal aa3 = dJointGetAMotorAngle (joint,2); + // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3); + + dReal err = dInfinity; + if (iteration > 0) { + err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3); + err *= 1e10; + } + + // get random base rotation for both bodies + dMatrix3 Rbase; + dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), + 3*(dRandReal()-0.5), 3*(dRandReal()-0.5)); + dBodySetRotation (body[0],Rbase); + + // rotate body 2 by random euler angles w.r.t. body 1 + a1 = 3.14 * 2 * (dRandReal()-0.5); + a2 = 1.57 * 2 * (dRandReal()-0.5); + a3 = 3.14 * 2 * (dRandReal()-0.5); + dMatrix3 R1,R2,R3,Rtmp1,Rtmp2; + dRFromAxisAndAngle (R1,0,0,1,-a1); + dRFromAxisAndAngle (R2,0,1,0,a2); + dRFromAxisAndAngle (R3,1,0,0,-a3); + dMultiply0 (Rtmp1,R2,R3,3,3,3); + dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3); + dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3); + dBodySetRotation (body[1],Rtmp1); + // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3); + + return err; + } + + // ********** universal joint + + case 700: { // 2 body: joint constraint + dVector3 ax1, ax2; + + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 701: { // 2 body: angle 1 rate + static dReal last_angle = 0; + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + // I'm not sure why the error is so large here. + return fabs(r - er) * 1e1; + } + + case 702: { // 2 body: angle 2 rate + static dReal last_angle = 0; + addOscillatingTorque (0.1); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + // I'm not sure why the error is so large here. + return fabs(r - er) * 1e1; + } + + case 720: { // universal transmit torque test: constraint error + dVector3 ax1, ax2; + addOscillatingTorqueAbout (0.1, 1, 1, 0); + dampRotationalMotion (0.1); + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 721: { // universal transmit torque test: angle1 rate + static dReal last_angle = 0; + addOscillatingTorqueAbout (0.1, 1, 1, 0); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 722: { // universal transmit torque test: angle2 rate + static dReal last_angle = 0; + addOscillatingTorqueAbout (0.1, 1, 1, 0); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 730:{ + dVector3 ax1, ax2; + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); + dampRotationalMotion (0.1); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 731:{ + dVector3 ax1; + static dReal last_angle = 0; + dJointGetUniversalAxis1(joint, ax1); + addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 2e3; + } + + case 732:{ + dVector3 ax1; + static dReal last_angle = 0; + dJointGetUniversalAxis1(joint, ax1); + addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 740:{ + dVector3 ax1, ax2; + dJointGetUniversalAxis1(joint, ax1); + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); + dampRotationalMotion (0.1); + return fabs(10*dCalcVectorDot3(ax1, ax2)); + } + + case 741:{ + dVector3 ax2; + static dReal last_angle = 0; + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle1(joint); + dReal r = dJointGetUniversalAngle1Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e10; + } + + case 742:{ + dVector3 ax2; + static dReal last_angle = 0; + dJointGetUniversalAxis2(joint, ax2); + addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); + dampRotationalMotion (0.1); + dReal a = dJointGetUniversalAngle2(joint); + dReal r = dJointGetUniversalAngle2Rate(joint); + dReal diff = a - last_angle; + if (diff > M_PI) diff -= 2*M_PI; + if (diff < -M_PI) diff += 2*M_PI; + dReal er = diff / STEPSIZE; // estimated rate + last_angle = a; + return fabs(r - er) * 1e4; + } + + // ********** slider joint + case 801: + case 803: + addSpringForce (0.25); + return dInfinity; + + case 802: + case 804: { + static dReal a = 0; + dBodyAddTorque (body[0], 0, 0.01*cos(1.5708*a), 0); + a += 0.01; + return dInfinity; + } + + case 805: + addOscillatingForce (0.1); + return dInfinity; + } + + + return dInfinity; +} + +//**************************************************************************** +// simulation stuff common to all the tests + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +// simulation loop + +static void simLoop (int pause) +{ + // stop after a given number of iterations, as long as we are not in + // interactive mode + if (cmd_graphics && !cmd_interactive && + (iteration >= max_iterations)) { + dsStop(); + return; + } + iteration++; + + if (!pause) { + // do stuff for this test and check to see if the joint is behaving well + dReal error = doStuffAndGetError (test_num); + if (error > max_error) max_error = error; + if (cmd_interactive && error < dInfinity) { + printf ("scaled error = %.4e\n",error); + } + + // take a step + dWorldStep (world,STEPSIZE); + + // occasionally re-orient the first body to create a deliberate error. + if (cmd_occasional_error) { + static int count = 0; + if ((count % 20)==0) { + // randomly adjust orientation of body[0] + const dReal *R1; + dMatrix3 R2,R3; + R1 = dBodyGetRotation (body[0]); + dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, + dRandReal()-0.5,dRandReal()-0.5); + dMultiply0 (R3,R1,R2,3,3,3); + dBodySetRotation (body[0],R3); + + // randomly adjust position of body[0] + const dReal *pos = dBodyGetPosition (body[0]); + dBodySetPosition (body[0], + pos[0]+0.2*(dRandReal()-0.5), + pos[1]+0.2*(dRandReal()-0.5), + pos[2]+0.2*(dRandReal()-0.5)); + } + count++; + } + } + + if (cmd_graphics) { + dReal sides1[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f}; + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + if (body[1]) { + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); + } + } +} + +//**************************************************************************** +// conduct a specific test, and report the results + +void doTest (int argc, char **argv, int n, int fatal_if_bad_n) +{ + test_num = n; + iteration = 0; + max_iterations = 300; + max_error = 0; + + if (! setupTest (n)) { + if (fatal_if_bad_n) dError (0,"bad test number"); + return; + } + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = 0; + fn.stop = 0; + if (cmd_path_to_textures) + fn.path_to_textures = cmd_path_to_textures; + else + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // run simulation + if (cmd_graphics) { + dsSimulationLoop (argc,argv,352,288,&fn); + } + else { + for (int i=0; i < max_iterations; i++) simLoop (0); + } + dWorldDestroy (world); + body[0] = 0; + body[1] = 0; + joint = 0; + + // print results + printf ("test %d: ",n); + if (max_error == dInfinity) printf ("error not computed\n"); + else { + printf ("max scaled error = %.4e",max_error); + if (max_error < 1) printf (" - passed\n"); + else printf (" - FAILED\n"); + } +} + +//**************************************************************************** +// main + +int main (int argc, char **argv) +{ + int i; + dInitODE2(0); + + // process the command line args. anything that starts with `-' is assumed + // to be a drawstuff argument. + for (i=1; i +#include +#include +#include +#include +#include +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#endif + + +using namespace std; + +dWorld *world; +dSpace *space; +dPlane *ground; +dBody *kbody; +dBox *kbox; +dJointGroup joints; +dCylinder *kpole; +dBody *matraca; +dBox *matraca_geom; +dHingeJoint *hinge; + +struct Box { + dBody body; + dBox geom; + Box() : + body(*world), + geom(*space, 0.2, 0.2, 0.2) + { + dMass mass; + mass.setBox(10, 0.2, 0.2, 0.2); + body.setMass(mass); + geom.setData(this); + geom.setBody(body); + } + void draw() const + { + dVector3 lengths; + geom.getLengths(lengths); + dsSetTexture(DS_WOOD); + dsSetColor(0,1,0); + dsDrawBox(geom.getPosition(), geom.getRotation(), lengths); + } +}; + +set boxes; +set to_remove; + +void dropBox() +{ + Box *box = new Box(); + + dReal px = (rand() / float(RAND_MAX)) * 2 - 1; + dReal py = (rand() / float(RAND_MAX)) * 2 - 1; + dReal pz = 2.5; + box->body.setPosition(px, py, pz); + + boxes.insert(box); +} + +void queueRemoval(dGeomID g) +{ + Box *b = (Box*)dGeomGetData(g); + to_remove.insert(b); +} + +void removeQueued() +{ + while (!to_remove.empty()) { + Box *b = *to_remove.begin(); + to_remove.erase(b); + boxes.erase(b); + delete b; + } +} + + +void nearCallback(void *, dGeomID g1, dGeomID g2) +{ + if (g1 == ground->id()) { + queueRemoval(g2); + return; + } + if (g2 == ground->id()) { + queueRemoval(g1); + return; + } + + dBodyID b1 = dGeomGetBody(g1); + dBodyID b2 = dGeomGetBody(g2); + + if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) + return; + + const int MAX_CONTACTS = 10; + dContact contact[MAX_CONTACTS]; + int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); + for (int i=0; i 2*M_PI) + t = 0; + dVector3 next_pos = { dCos(t), dSin(t), REAL(0.5)}; + dVector3 vel; + // vel = (next_pos - cur_pos) / timestep + dSubtractVectors3(vel, next_pos, kbody->getPosition()); + dScaleVector3(vel, 1/timestep); + kbody->setLinearVel(vel); + // end of hard-coded animation + + space->collide(0, nearCallback); + removeQueued(); + + world->quickStep(timestep); + joints.clear(); + } + + dVector3 lengths; + + // the moving platform + kbox->getLengths(lengths); + dsSetTexture(DS_WOOD); + dsSetColor(.3, .3, 1); + dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths); + dReal length, radius; + kpole->getParams(&radius, &length); + dsSetTexture(DS_CHECKERED); + dsSetColor(1, 1, 0); + dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius); + + // the matraca + matraca_geom->getLengths(lengths); + dsSetColor(1,0,0); + dsSetTexture(DS_WOOD); + dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths); + + // and the boxes + for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw)); +} + +void command(int c) +{ + switch (c) { + case ' ': + dropBox(); + break; + } +} + +int main(int argc, char **argv) +{ + dInitODE(); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = 0; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + cout << endl << "*** Press SPACE to drop boxes **" << endl; + + space = new dSimpleSpace(); + ground = new dPlane(*space, 0, 0, 1, 0); + + world = new dWorld; + world->setGravity(0, 0, -.5); + + kbody = new dBody(*world); + kbody->setKinematic(); + const dReal kx = 1, ky = 0, kz = .5; + kbody->setPosition(kx, ky, kz); + kbox = new dBox(*space, 3, 3, .5); + kbox->setBody(*kbody); + kpole = new dCylinder(*space, .125, 1.5); + kpole->setBody(*kbody); + dGeomSetOffsetPosition(kpole->id(), 0, 0, 0.8); + + matraca = new dBody(*world); + matraca->setPosition(kx+0, ky+1, kz+1); + matraca_geom = new dBox(*space, 0.5, 2, 0.75); + matraca_geom->setBody(*matraca); + dMass mass; + mass.setBox(1, 0.5, 2, 0.75); + matraca->setMass(mass); + + hinge = new dHingeJoint(*world); + hinge->attach(*kbody, *matraca); + hinge->setAnchor(kx, ky, kz+1); + hinge->setAxis(0, 0, 1); + + dsSimulationLoop (argc, argv, 640, 480, &fn); + + dCloseODE(); +} diff --git a/libs/ode-0.16.1/ode/demo/demo_motion.cpp b/libs/ode-0.16.1/ode/demo/demo_motion.cpp new file mode 100644 index 0000000..a83887d --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_motion.cpp @@ -0,0 +1,527 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + This demo shows how to use dContactMotionN in a lifting platform. +*/ +//#include // for usleep() +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 8 // maximum number of contact points per body +#define USE_GEOM_OFFSET 1 + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; +static int show_body = 0; + +static dGeomID platform, ground; + +dVector3 platpos = {0, 0, 0}; +int mov_type = 2; +dReal mov_time = 0; + + +const dReal mov1_speed = 0.2; + +dVector3 mov2_vel = { 0.2, 0.1, 0.25}; + + + + +/**************************************************************** + * Movement 1: move platform up, reset every 80 units of time. * + * This is the simplest case * + ****************************************************************/ +static void moveplat_1(dReal stepsize) +{ + mov_time += stepsize; + if (mov_time > 80) + mov_time = 0; + + platpos[0] = platpos[1] = 0; + // the platform moves up (Z) at constant speed: mov1_speed + platpos[2] = mov1_speed * mov_time; +} + +// Generate contact info for movement 1 +static void contactplat_1(dContact &contact) +{ + contact.surface.mode |= dContactMotionN; + contact.surface.motionN = mov1_speed; +} + + + +/**************************************************************** + * Movement 2: move platform along direction mov2_vel, reset * + * every 80 units of time. * + * This is the most general case: the geom moves along * + * an arbitrary direction. * + ****************************************************************/ +static void moveplat_2(dReal stepsize) +{ + mov_time += stepsize; + if (mov_time > 80) + mov_time = 0; + + // the platform moves at constant speed: mov2_speed + platpos[0] = mov2_vel[0] * mov_time; + platpos[1] = mov2_vel[1] * mov_time; + platpos[2] = mov2_vel[2] * mov_time; +} + +// Generate contact info for movement 1 +static void contactplat_2(dContact &contact) +{ + /* + For arbitrary contact directions we need to project the moving + geom's velocity against the contact normal and fdir1, fdir2 + (obtained with dPlaneSpace()). Assuming moving geom=g2 + (so the contact joint is in the moving geom's reference frame): + motion1 = dCalcVectorDot3(fdir1, vel); + motion2 = dCalcVectorDot3(fdir2, vel); + motionN = dCalcVectorDot3(normal, vel); + + For geom=g1 just negate motionN and motion2. fdir1 is an arbitrary + vector, so there's no need to negate motion1. + + */ + contact.surface.mode |= + dContactMotionN | // velocity along normal + dContactMotion1 | dContactMotion2 | // and along the contact plane + dContactFDir1; // don't forget to set the direction 1 + + + // This is a convenience function: given a vector, it finds other 2 perpendicular vectors + dVector3 motiondir1, motiondir2; + dPlaneSpace(contact.geom.normal, motiondir1, motiondir2); + for (int i=0; i<3; ++i) + contact.fdir1[i] = motiondir1[i]; + + + dReal inv = 1; + if (contact.geom.g1 == platform) + inv = -1; + + contact.surface.motion1 = dCalcVectorDot3(mov2_vel, motiondir1); + contact.surface.motion2 = inv * dCalcVectorDot3(mov2_vel, motiondir2); + contact.surface.motionN = inv * dCalcVectorDot3(mov2_vel, contact.geom.normal); + +} + + + + + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + dMatrix3 RI; + static const dReal ss[3] = {0.02,0.02,0.02}; + + dContact contact[MAX_CONTACTS]; + int numc = dCollide (o1, o2, MAX_CONTACTS, + &contact[0].geom, sizeof(dContact)); + + if (numc) + dRSetIdentity(RI); + + bool isplatform = (o1 == platform) || (o2 == platform); + + for (int i=0; i< numc; i++) { + contact[i].surface.mode = dContactBounce; + contact[i].surface.mu = 1; + contact[i].surface.bounce = 0.25; + contact[i].surface.bounce_vel = 0.01; + + if (isplatform) { + switch (mov_type) { + case 1: + contactplat_1(contact[i]); + break; + case 2: + contactplat_2(contact[i]); + break; + } + } + + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c, dGeomGetBody(o1), dGeomGetBody(o2)); + if (show_contacts) + dsDrawBox (contact[i].geom.pos, RI, ss); + } +} + + +// start simulation - set viewpoint + +static float xyz[3] = {2.1106f,-1.3007,2.f}; +static float hpr[3] = {150.f,-13.5000f,0.0000f}; + +static void start() +{ + //dAllocateODEDataForThread(dAllocateMaskAll); + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" c for capsule.\n"); + printf (" y for cylinder.\n"); + printf ("Press m to change the movement type\n"); + printf ("Press space to reset the platform\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); + printf ("To save the current state to 'state.dif', press 1.\n"); +} + + +char locase (char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + dsizeint i; + int k; + dReal sides[3]; + dMass m; + int setBody; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y') + { + setBody = 0; + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + if (obj[i].body) { + dBodyDestroy (obj[i].body); + } + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) { + dGeomDestroy (obj[i].geom[k]); + } + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) + { + dBodySetPosition (obj[i].body, + dRandReal()*2-1 + platpos[0], + dRandReal()*2-1 + platpos[1], + dRandReal()+2 + platpos[2]); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else + { + dBodySetPosition (obj[i].body, + platpos[0], + platpos[1], + platpos[2]+2); + dRSetIdentity (R); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*) i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } + else if (cmd == 'y') { + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + + if (!setBody) + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) { + dGeomSetBody (obj[i].geom[k],obj[i].body); + } + } + + dBodySetMass (obj[i].body,&m); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } + else if (cmd == '1') { + write_world = 1; + } + else if (cmd == ' ') { + mov_time = 0; + } + else if (cmd == 'm') { + mov_type = mov_type==1 ? 2 : 1; + mov_time = 0; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + int i; + + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + + if (show_body) { + dBodyID body = dGeomGetBody(g); + if (body) { + const dReal *bodypos = dBodyGetPosition (body); + const dReal *bodyr = dBodyGetRotation (body); + dReal bodySides[3] = { 0.1, 0.1, 0.1 }; + dsSetColorAlpha(0,1,0,1); + dsDrawBox(bodypos,bodyr,bodySides); + } + } + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void updatecam() +{ + xyz[0] = platpos[0] + 3.3; + xyz[1] = platpos[1] - 1.8; + xyz[2] = platpos[2] + 2; + dsSetViewpoint (xyz, hpr); +} + +static void simLoop (int pause) +{ + const dReal stepsize = 0.02; + + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + if (!pause) { + + if (mov_type == 1) + moveplat_1(stepsize); + else + moveplat_2(stepsize); + + dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]); + updatecam(); + dWorldQuickStep (world,stepsize); + //dWorldStep (world,stepsize); + } + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (world,f,"X"); + fclose (f); + } + write_world = 0; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define SIDE (0.5f) // side length of a box +#define MASS (1.0) // mass of a box + + +// dynamics and collision objects +static dWorldID world; +static dBodyID body[2]; +static dGeomID geom[2]; +static dJointID lmotor[2]; +static dJointID amotor[2]; +static dSpaceID space; +static dJointGroupID contactgroup; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press 'q,a,z' to control one axis of lmotor connectiong two bodies. (q is +,a is 0, z is -)\n"); + printf ("Press 'w,e,r' to control one axis of lmotor connectiong first body with world. (w is +,e is 0, r is -)\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == 'q' || cmd == 'Q') { + dJointSetLMotorParam(lmotor[0],dParamVel,0); + dJointSetLMotorParam(lmotor[0],dParamVel2,0); + dJointSetLMotorParam(lmotor[0],dParamVel3,0.1); + } else if (cmd == 'a' || cmd == 'A') { + dJointSetLMotorParam(lmotor[0],dParamVel,0); + dJointSetLMotorParam(lmotor[0],dParamVel2,0); + dJointSetLMotorParam(lmotor[0],dParamVel3,0); + } else if (cmd == 'z' || cmd == 'Z') { + dJointSetLMotorParam(lmotor[0],dParamVel,0); + dJointSetLMotorParam(lmotor[0],dParamVel2,0); + dJointSetLMotorParam(lmotor[0],dParamVel3,-0.1); + } else if (cmd == 'w' || cmd == 'W') { + dJointSetLMotorParam(lmotor[1],dParamVel,0.1); + dJointSetLMotorParam(lmotor[1],dParamVel2,0); + dJointSetLMotorParam(lmotor[1],dParamVel3,0); + } else if (cmd == 'e' || cmd == 'E') { + dJointSetLMotorParam(lmotor[1],dParamVel,0); + dJointSetLMotorParam(lmotor[1],dParamVel2,0); + dJointSetLMotorParam(lmotor[1],dParamVel3,0); + } else if (cmd == 'r' || cmd == 'R') { + dJointSetLMotorParam(lmotor[1],dParamVel,-0.1); + dJointSetLMotorParam(lmotor[1],dParamVel2,0); + dJointSetLMotorParam(lmotor[1],dParamVel3,0); + } + +} + + + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact; + contact.surface.mode = 0; + contact.surface.mu = dInfinity; + if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { + dJointID c = dJointCreateContact (world,contactgroup,&contact); + dJointAttach (c,b1,b2); + } +} + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + dSpaceCollide(space,0,&nearCallback); + dWorldQuickStep (world,0.05); + dJointGroupEmpty(contactgroup); + } + + dReal sides1[3]; + dGeomBoxGetLengths(geom[0], sides1); + dReal sides2[3]; + dGeomBoxGetLengths(geom[1], sides2); + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + contactgroup = dJointGroupCreate(0); + world = dWorldCreate(); + space = dSimpleSpaceCreate(0); + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0],0,0,1); + geom[0] = dCreateBox(space,SIDE,SIDE,SIDE); + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dBodySetPosition (body[1],0,0,2); + geom[1] = dCreateBox(space,SIDE,SIDE,SIDE); + + dGeomSetBody(geom[0],body[0]); + dGeomSetBody(geom[1],body[1]); + + lmotor[0] = dJointCreateLMotor (world,0); + dJointAttach (lmotor[0],body[0],body[1]); + lmotor[1] = dJointCreateLMotor (world,0); + dJointAttach (lmotor[1],body[0],0); + amotor[0] = dJointCreateAMotor(world,0); + dJointAttach(amotor[0], body[0],body[1]); + amotor[1] = dJointCreateAMotor(world,0); + dJointAttach(amotor[1], body[0], 0); + + for (int i=0; i<2; i++) { + dJointSetAMotorNumAxes(amotor[i], 3); + dJointSetAMotorAxis(amotor[i],0,1,1,0,0); + dJointSetAMotorAxis(amotor[i],1,1,0,1,0); + dJointSetAMotorAxis(amotor[i],2,1,0,0,1); + dJointSetAMotorParam(amotor[i],dParamFMax,0.00001); + dJointSetAMotorParam(amotor[i],dParamFMax2,0.00001); + dJointSetAMotorParam(amotor[i],dParamFMax3,0.00001); + + dJointSetAMotorParam(amotor[i],dParamVel,0); + dJointSetAMotorParam(amotor[i],dParamVel2,0); + dJointSetAMotorParam(amotor[i],dParamVel3,0); + + dJointSetLMotorNumAxes(lmotor[i],3); + dJointSetLMotorAxis(lmotor[i],0,1,1,0,0); + dJointSetLMotorAxis(lmotor[i],1,1,0,1,0); + dJointSetLMotorAxis(lmotor[i],2,1,0,0,1); + + dJointSetLMotorParam(lmotor[i],dParamFMax,0.0001); + dJointSetLMotorParam(lmotor[i],dParamFMax2,0.0001); + dJointSetLMotorParam(lmotor[i],dParamFMax3,0.0001); + } + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy(contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp b/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp new file mode 100644 index 0000000..2f03900 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_moving_convex.cpp @@ -0,0 +1,415 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include +#include "texturepath.h" +#include "bunny_geom.h" +#include "convex_bunny_geom.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawLine dsDrawLineD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 200 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 64 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject +{ + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? + +typedef dReal dVector3R[3]; + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback( void *, dGeomID o1, dGeomID o2 ) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody( o1 ); + dBodyID b2 = dGeomGetBody( o2 ); + if ( b1 && b2 && dAreConnectedExcluding( b1,b2,dJointTypeContact ) ) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for ( i=0; i= 'A' && c <= 'Z' ) return c - ( 'a'-'A' ); + else return c; +} + + +// called when a key pressed +static void command( int cmd ) +{ + int i,k; + dReal sides[3]; + dMass m; + + cmd = locase( cmd ); + if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' || cmd == 'y') + { + if ( num < NUM ) + { + i = num; + num++; + } + else + { + i = nextobj; + nextobj++; + if ( nextobj >= num ) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy( obj[i].body ); + for ( k=0; k < GPB; k++ ) + { + if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] ); + } + memset( &obj[i],0,sizeof( obj[i] ) ); + } + + obj[i].body = dBodyCreate( world ); + for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if ( random_pos ) + { + dBodySetPosition( obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 ); + dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 ); + } + else + { + dReal maxheight = 0; + for ( k=0; k maxheight ) maxheight = pos[2]; + } + dBodySetPosition( obj[i].body, 0,0,maxheight+1 ); + dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 ); + } + dBodySetRotation( obj[i].body,R ); + dBodySetData( obj[i].body,( void* )( dsizeint )i ); + + if ( cmd == 'b' ) + { + dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] ); + obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] ); + } + else if ( cmd == 'c' ) + { + sides[0] *= 0.5; + dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] ); + obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] ); + } + else if (cmd == 'y') { + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if ( cmd == 's' ) + { + sides[0] *= 0.5; + dMassSetSphere( &m,DENSITY,sides[0] ); + obj[i].geom[0] = dCreateSphere( space,sides[0] ); + } + else if ( cmd == 'v' ) + { + obj[i].geom[0] = dCreateConvex( space, + convexBunnyPlanes, + convexBunnyPlaneCount, + convexBunnyPoints, + convexBunnyPointCount, + convexBunnyPolygons ); + + /// Use equivalent TriMesh to set mass + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount, + ( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) ); + dGeomTriMeshDataPreprocess2( new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL ); + + dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 ); + + dMassSetTrimesh( &m, DENSITY, triMesh ); + + dGeomDestroy( triMesh ); + dGeomTriMeshDataDestroy( new_tmdata ); + + printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] ); + dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] ); + dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] ); + } + + for ( k=0; k < GPB; k++ ) + { + if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body ); + } + + dBodySetMass( obj[i].body,&m ); + } + + if ( cmd == ' ' ) + { + selected++; + if ( selected >= num ) selected = 0; + if ( selected < 0 ) selected = 0; + } + else if ( cmd == 'd' && selected >= 0 && selected < num ) + { + dBodyDisable( obj[selected].body ); + } + else if ( cmd == 'e' && selected >= 0 && selected < num ) + { + dBodyEnable( obj[selected].body ); + } + else if ( cmd == 'a' ) + { + show_aabb ^= 1; + } + else if ( cmd == 't' ) + { + show_contacts ^= 1; + } + else if ( cmd == 'r' ) + { + random_pos ^= 1; + } +} + +// draw a geom +void drawGeom( dGeomID g, const dReal *pos, const dReal *R, int show_aabb ) +{ + if ( !g ) return; + if ( !pos ) pos = dGeomGetPosition( g ); + if ( !R ) R = dGeomGetRotation( g ); + + int type = dGeomGetClass( g ); + if ( type == dBoxClass ) + { + dVector3 sides; + dGeomBoxGetLengths( g,sides ); + dsDrawBox( pos,R,sides ); + } + else if ( type == dSphereClass ) + { + dsDrawSphere( pos,R,dGeomSphereGetRadius( g ) ); + } + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + else if ( type == dCapsuleClass ) + { + dReal radius,length; + dGeomCapsuleGetParams( g,&radius,&length ); + dsDrawCapsule( pos,R,length,radius ); + } + else if ( type == dConvexClass ) + { + dsDrawConvex( pos,R, + convexBunnyPlanes, + convexBunnyPlaneCount, + convexBunnyPoints, + convexBunnyPointCount, + convexBunnyPolygons ); + } + + if ( show_aabb ) + { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB( g,aabb ); + dVector3 bbpos; + for ( int i=0; i<3; i++ ) bbpos[i] = 0.5*( aabb[i*2] + aabb[i*2+1] ); + dVector3 bbsides; + for ( int j=0; j<3; j++ ) bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity( RI ); + dsSetColorAlpha( 1,0,0,0.5 ); + dsDrawBox( bbpos,RI,bbsides ); + } +} + +// simulation loop + +static void simLoop( int pause ) +{ + dsSetColor( 0,0,2 ); + dSpaceCollide( space,0,&nearCallback ); + + if ( !pause ) dWorldQuickStep( world,0.05 ); + + for ( int j = 0; j < dSpaceGetNumGeoms( space ); j++ ) + { + dSpaceGetGeom( space, j ); + } + + // remove all contact joints + dJointGroupEmpty( contactgroup ); + + dsSetColor( 1,1,0 ); + dsSetTexture( DS_WOOD ); + for ( int i=0; i +#include +#include "texturepath.h" +#include "bunny_geom.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +//<---- Convex Object +static const dReal planes[] = // planes for a cube +{ + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ +}; +static const unsigned int planecount=6; + +static const dReal points[] = // points for a cube +{ + 0.25f,0.25f,0.25f, + -0.25f,0.25f,0.25f, + + 0.25f,-0.25f,0.25f, + -0.25f,-0.25f,0.25f, + + 0.25f,0.25f,-0.25f, + -0.25f,0.25f,-0.25f, + + 0.25f,-0.25f,-0.25f, + -0.25f,-0.25f,-0.25f, +}; +static const unsigned int pointcount=8; + +static const unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +int tmTriangles[] = +{ + 0,2,6, + 0,6,4, + 1,0,4, + 1,4,5, + 0,1,3, + 0,3,2, + 3,1,5, + 3,5,7, + 2,3,7, + 2,7,6, + 5,4,6, + 5,6,7 +}; + +float tmVertices[] = +{ + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 +}; + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawLine dsDrawLineD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 200 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 64 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body + + // Trimesh only - double buffered matrices for 'last transform' setup + dReal matrix_dblbuff[ 16 * 2 ]; + int last_matrix_index; +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? + +typedef dReal dVector3R[3]; + +dGeomID TriMesh1; +dGeomID TriMesh2; +static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') { + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition (obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else { + dReal maxheight = 0; + for (k=0; k maxheight) maxheight = pos[2]; + } + dBodySetPosition (obj[i].body, 0,0,maxheight+1); + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*)(dsizeint)i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } else if (cmd == 'v') { + + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex(space, + planes, + planecount, + points, + pointcount, + polygons); + } + else if (cmd == 'y') { + sides[1] *= 0.5; + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + else if (cmd == 'm') { + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, + (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); + dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); + + + obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); + + // remember the mesh's dTriMeshDataID on its userdata for convenience. + dGeomSetData(obj[i].geom[0], new_tmdata); + + dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); + printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); + dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); + dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); + } + else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } else if (type == dConvexClass) { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +// set previous transformation matrix for trimesh +void setCurrentTransform(dGeomID geom) +{ + const dReal* Pos = dGeomGetPosition(geom); + const dReal* Rot = dGeomGetRotation(geom); + + const dReal Transform[16] = + { + Rot[0], Rot[4], Rot[8], 0, + Rot[1], Rot[5], Rot[9], 0, + Rot[2], Rot[6], Rot[10], 0, + Pos[0], Pos[1], Pos[2], 1 + }; + + dGeomTriMeshSetLastTransform( geom, *(dMatrix4*)(&Transform) ); + +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + + +#if 1 + // What is this for??? - Bram + if (!pause) + { + for (int i=0; i +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +//**************************************************************************** +// matrix sizes + +#define dALIGN_SIZE(buf_size, alignment) (((buf_size) + (alignment - 1)) & (int)(~(alignment - 1))) // Casting the mask to int ensures sign-extension to larger integer sizes +#define dALIGN_PTR(buf_ptr, alignment) ((void *)(((duintptr)(buf_ptr) + ((alignment) - 1)) & (int)(~(alignment - 1)))) // Casting the mask to int ensures sign-extension to larger integer sizes + +#define MSIZE 21 +#define MSIZE4 dALIGN_SIZE(MSIZE, 4) // MSIZE rounded up to 4 + + + +//**************************************************************************** +// matrix accessors + +#define _A(i,j) A[(i)*4+(j)] +#define _I(i,j) I[(i)*4+(j)] +#define _R(i,j) R[(i)*4+(j)] + +//**************************************************************************** +// tolerances + +#ifdef dDOUBLE +const double tol = 1e-10; +#endif + +#ifdef dSINGLE +const double tol = 1e-5; +#endif + +//**************************************************************************** +// misc messages and error handling + +#ifdef __GNUC__ +#define HEADER printf ("%s()\n", __FUNCTION__); +#else +#define HEADER printf ("%s:%d\n",__FILE__,__LINE__); +#endif + +static jmp_buf jump_buffer; + + +void myMessageFunction (int num, const char *msg, va_list ap) +{ + printf ("(Message %d: ",num); + vprintf (msg,ap); + printf (")"); + dSetMessageHandler (0); + longjmp (jump_buffer,1); +} + + +#define TRAP_MESSAGE(do,ifnomsg,ifmsg) \ + dSetMessageHandler (&myMessageFunction); \ + if (setjmp (jump_buffer)) { \ + dSetMessageHandler (0); \ + ifmsg ; \ + } \ + else { \ + dSetMessageHandler (&myMessageFunction); \ + do ; \ + ifnomsg ; \ + } \ + dSetMessageHandler (0); + +//**************************************************************************** +// utility stuff + +// compare two numbers, within a threshhold, return 1 if approx equal + +int cmp (dReal a, dReal b) +{ + return (fabs(a-b) < tol); +} + +//**************************************************************************** +// matrix utility stuff + +// compare a 3x3 matrix with the identity + +int cmpIdentityMat3 (dMatrix3 A) +{ + return + (cmp(_A(0,0),1.0) && cmp(_A(0,1),0.0) && cmp(_A(0,2),0.0) && + cmp(_A(1,0),0.0) && cmp(_A(1,1),1.0) && cmp(_A(1,2),0.0) && + cmp(_A(2,0),0.0) && cmp(_A(2,1),0.0) && cmp(_A(2,2),1.0)); +} + + +// transpose a 3x3 matrix in-line + +void transpose3x3 (dMatrix3 A) +{ + dReal tmp; + tmp=A[4]; A[4]=A[1]; A[1]=tmp; + tmp=A[8]; A[8]=A[2]; A[2]=tmp; + tmp=A[9]; A[9]=A[6]; A[6]=tmp; +} + +//**************************************************************************** +// test miscellaneous math functions + +void testRandomNumberGenerator() +{ + HEADER; + if (dTestRand()) printf ("\tpassed\n"); + else printf ("\tFAILED\n"); +} + + +void testInfinity() +{ + HEADER; + if (1e10 < dInfinity && -1e10 > -dInfinity && -dInfinity < dInfinity) + printf ("\tpassed\n"); + else printf ("\tFAILED\n"); +} + + +void testPad() +{ + HEADER; + char s[100]; + s[0]=0; + for (int i=0; i<=16; i++) sprintf (s+strlen(s),"%d ",dPAD(i)); + printf ("\t%s\n", strcmp(s,"0 1 4 4 4 8 8 8 8 12 12 12 12 16 16 16 16 ") ? + "FAILED" : "passed"); +} + + +void testCrossProduct() +{ + HEADER; + + dVector3 a1,a2,b,c; + dMatrix3 B; + dMakeRandomVector (b,3,1.0); + dMakeRandomVector (c,3,1.0); + + dCalcVectorCross3(a1,b,c); + + dSetZero (B,12); + dSetCrossMatrixPlus(B,b,4); + dMultiply0 (a2,B,c,3,3,1); + + dReal diff = dMaxDifference(a1,a2,3,1); + printf ("\t%s\n", diff > tol ? "FAILED" : "passed"); +} + + +void testSetZero() +{ + HEADER; + dReal a[100]; + dMakeRandomVector (a,100,1.0); + dSetZero (a,100); + for (int i=0; i<100; i++) if (a[i] != 0.0) { + printf ("\tFAILED\n"); + return; + } + printf ("\tpassed\n"); +} + + +void testNormalize3() +{ + HEADER; + int i,j,bad=0; + dVector3 n1,n2; + for (i=0; i<1000; i++) { + dMakeRandomVector (n1,3,1.0); + for (j=0; j<3; j++) n2[j]=n1[j]; + dNormalize3 (n2); + if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1; + if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2; + if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4; + if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8; + if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16; + if (bad) { + printf ("\tFAILED (code=%x)\n",bad); + return; + } + } + printf ("\tpassed\n"); +} + + +/* +void testReorthonormalize() +{ +HEADER; +dMatrix3 R,I; +dMakeRandomMatrix (R,3,3,1.0); +for (int i=0; i<30; i++) dReorthonormalize (R); +dMultiply2 (I,R,R,3,3,3); +printf ("\t%s\n",cmpIdentityMat3 (I) ? "passed" : "FAILED"); +} +*/ + + +void testPlaneSpace() +{ + HEADER; + dVector3 n,p,q; + int bad = 0; + for (int i=0; i<1000; i++) { + dMakeRandomVector (n,3,1.0); + dNormalize3 (n); + dPlaneSpace (n,p,q); + if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1; + if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1; + if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1; + if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1; + if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1; + } + printf ("\t%s\n", bad ? "FAILED" : "passed"); +} + +//**************************************************************************** +// test matrix functions + +void testMatrixMultiply() +{ + // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4 + dReal A[8],B[12],A2[12],B2[16],C[8]; + int i; + + HEADER; + dSetZero (A,8); + for (i=0; i<3; i++) A[i] = i+2; + for (i=0; i<3; i++) A[i+4] = i+3+2; + for (i=0; i<12; i++) B[i] = i+8; + dSetZero (A2,12); + for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3]; + dSetZero (B2,16); + for (i=0; i<12; i++) B2[i+i/3] = B[i]; + + dMultiply0 (C,A,B,2,3,4); + if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 || + C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278) + printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n"); + + dMultiply1 (C,A2,B,2,3,4); + if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 || + C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241) + printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n"); + + dMultiply2 (C,A,B2,2,3,4); + if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 || + C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326) + printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n"); +} + + +void testSmallMatrixMultiply() +{ + dMatrix3 A,B,C,A2; + dVector3 a,a2,x; + + HEADER; + dMakeRandomMatrix (A,3,3,1.0); + dMakeRandomMatrix (B,3,3,1.0); + dMakeRandomMatrix (C,3,3,1.0); + dMakeRandomMatrix (x,3,1,1.0); + + // dMultiply0_331() + dMultiply0_331 (a,B,x); + dMultiply0 (a2,B,x,3,3,1); + printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" : + "passed"); + + // dMultiply1_331() + dMultiply1_331 (a,B,x); + dMultiply1 (a2,B,x,3,3,1); + printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" : + "passed"); + + // dMultiply0_133 + dMultiply0_133 (a,x,B); + dMultiply0 (a2,x,B,1,3,3); + printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" : + "passed"); + + // dMultiply0_333() + dMultiply0_333 (A,B,C); + dMultiply0 (A2,B,C,3,3,3); + printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : + "passed"); + + // dMultiply1_333() + dMultiply1_333 (A,B,C); + dMultiply1 (A2,B,C,3,3,3); + printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : + "passed"); + + // dMultiply2_333() + dMultiply2_333 (A,B,C); + dMultiply2 (A2,B,C,3,3,3); + printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" : + "passed"); +} + + +void testCholeskyFactorization() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *B = (dReal *)dAlloc(matrixSize), *C = (dReal *)dAlloc(matrixSize), diff; + + HEADER; + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,B,MSIZE4*MSIZE*sizeof(dReal)); + if (dFactorCholesky (B,MSIZE)) printf ("\tpassed (1)\n"); + else printf ("\tFAILED (1)\n"); + dClearUpperTriangle (B,MSIZE); + dMultiply2 (C,B,B,MSIZE,MSIZE,MSIZE); + diff = dMaxDifference(A,C,MSIZE,MSIZE); + printf ("\tmaximum difference = %.6e - %s (2)\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(C, matrixSize); + dFree(B, matrixSize); + dFree(A, matrixSize); +} + + +void testCholeskySolve() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize); + dReal *b = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + // get A,L = PD matrix + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + + // get b,x = right hand side + dMakeRandomMatrix (b,MSIZE,1,1.0); + memcpy (x,b,MSIZE*sizeof(dReal)); + + // factor L + if (dFactorCholesky (L,MSIZE)) printf ("\tpassed (1)\n"); + else printf ("\tFAILED (1)\n"); + dClearUpperTriangle (L,MSIZE); + + // solve A*x = b + dSolveCholesky (L,x,MSIZE); + + // compute A*x and compare it with b + dMultiply2 (btest,A,x,MSIZE,MSIZE,1); + diff = dMaxDifference(b,btest,MSIZE,1); + printf ("\tmaximum difference = %.6e - %s (2)\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(btest, vectorSize); + dFree(x, vectorSize); + dFree(b, vectorSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testInvertPDMatrix() +{ + int i,j,ok; + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *Ainv = (dReal *)dAlloc(matrixSize), *I = (dReal *)dAlloc(matrixSize); + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal)); + dSetZero (Ainv,MSIZE4*MSIZE); + + if (dInvertPDMatrix (A,Ainv,MSIZE)) + printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n"); + dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE); + + // compare with identity + ok = 1; + for (i=0; i tol ? "FAILED" : "passed"); + + dFree(d, vectorSize); + dFree(ATEST, matrixSize); + dFree(DL, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testCoopLDLTFactorization() +{ + int i,j; + + const dsizeint COOP_MSIZE = MSIZE * 51, COOP_MSIZE4 = dALIGN_SIZE(COOP_MSIZE, 4); + + dsizeint matrixSize = sizeof(dReal) * COOP_MSIZE4 * COOP_MSIZE, vectorSize = sizeof(dReal) * COOP_MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), *DL = (dReal *)dAlloc(matrixSize), + *ATEST = (dReal *)dAlloc(matrixSize), *d = (dReal *)dAlloc(vectorSize), diff; + + const unsigned threadCountMaximum = 8; + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dCooperativeID cooperative = dCooperativeCreate(dThreadingImplementationGetFunctions(threading), threading); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(threadCountMaximum, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + + dResourceRequirementsID requirements = dResourceRequirementsCreate(cooperative); + dEstimateCooperativelyFactorLDLTResourceRequirements(requirements, threadCountMaximum, COOP_MSIZE); + dResourceContainerID resources = dResourceContainerAcquire(requirements); + + HEADER; + + for (int pass = 0; pass != 4; ++pass) + { + dTimerStart ("Factoring LDLT"); + + const unsigned allowedThreads = 4; + const unsigned PASS_MSIZE = COOP_MSIZE - pass, PASS_MSIZE4 = dALIGN_SIZE(PASS_MSIZE, 4); + + dTimerNow ("Preparing data"); + dMakeRandomMatrix (L, PASS_MSIZE, PASS_MSIZE, 1.0); + dMultiply2 (A, L, L, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE); + memcpy (L, A, sizeof(dReal) * PASS_MSIZE4 * PASS_MSIZE); + + dTimerNow ("Factoring multi threaded"); + dCooperativelyFactorLDLT (resources, allowedThreads, L, d, PASS_MSIZE, PASS_MSIZE4); + + dTimerNow ("Verifying"); + dClearUpperTriangle (L, PASS_MSIZE); + for (i = 0; i < PASS_MSIZE; i++) L[i * PASS_MSIZE4 + i] = 1.0; + + dSetZero (DL, PASS_MSIZE4 * PASS_MSIZE); + for (i = 0; i < PASS_MSIZE; i++) { + for (j = 0; j < PASS_MSIZE; j++) DL[i * PASS_MSIZE4 + j] = L[i * PASS_MSIZE4 + j] / d[j]; + } + + dMultiply2 (ATEST, L, DL, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE); + diff = dMaxDifference(A, ATEST, PASS_MSIZE, PASS_MSIZE); + printf ("\tN=%u: maximum difference = %.6e - %s\n", PASS_MSIZE, diff, diff > 1e2 * tol ? "FAILED" : "passed"); + + dTimerEnd(); + dTimerReport(stdout, 0); + } + + dResourceContainerDestroy(resources); + dResourceRequirementsDestroy(requirements); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dCooperativeDestroy(cooperative); + dThreadingFreeImplementation(threading); + + dFree(d, vectorSize); + dFree(ATEST, matrixSize); + dFree(DL, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testSolveLDLT() +{ + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), + *b = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + + dMakeRandomMatrix (b,MSIZE,1,1.0); + memcpy (x,b,MSIZE*sizeof(dReal)); + + dFactorLDLT (L,d,MSIZE,MSIZE4); + dSolveLDLT (L,d,x,MSIZE,MSIZE4); + + dMultiply2 (btest,A,x,MSIZE,MSIZE,1); + diff = dMaxDifference(b,btest,MSIZE,1); + printf ("\tmaximum difference = %.6e - %s\n",diff, + diff > tol ? "FAILED" : "passed"); + + dFree(btest, vectorSize); + dFree(b, vectorSize); + dFree(x, vectorSize); + dFree(d, vectorSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + +void testCoopSolveLDLT() +{ + const dsizeint COOP_MSIZE = MSIZE * 51, COOP_MSIZE4 = dALIGN_SIZE(COOP_MSIZE, 4); + + dsizeint matrixSize = sizeof(dReal) * COOP_MSIZE4 * COOP_MSIZE, vectorSize = sizeof(dReal) * COOP_MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), + *b = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff; + + const unsigned threadCountMaximum = 8; + dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation(); + dCooperativeID cooperative = dCooperativeCreate(dThreadingImplementationGetFunctions(threading), threading); + dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(threadCountMaximum, 0, dAllocateFlagBasicData, NULL); + dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); + + dResourceRequirementsID requirements = dResourceRequirementsCreate(cooperative); + dEstimateCooperativelySolveLDLTResourceRequirements(requirements, threadCountMaximum, COOP_MSIZE); + dResourceContainerID resources = dResourceContainerAcquire(requirements); + + HEADER; + + for (int pass = 0; pass != 4; ++pass) + { + dTimerStart ("Solving LDLT"); + + const unsigned allowedThreads = 4; + const unsigned PASS_MSIZE = COOP_MSIZE - pass, PASS_MSIZE4 = dALIGN_SIZE(PASS_MSIZE, 4); + + dTimerNow ("Preparing data"); + dMakeRandomMatrix (b, PASS_MSIZE, 1, 1.0); + + dMakeRandomMatrix (L, PASS_MSIZE, PASS_MSIZE, 1.0); + dMultiply2 (A, L, L, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE); + + memcpy (x, b, PASS_MSIZE * sizeof(dReal)); + memcpy (L, A, sizeof(dReal) * PASS_MSIZE4 * PASS_MSIZE); + + dTimerNow ("Factoring"); + dFactorLDLT (L, d, PASS_MSIZE, PASS_MSIZE4); + + dTimerNow ("Solving multi-threaded"); + dCooperativelySolveLDLT(resources, allowedThreads, L, d, x, PASS_MSIZE, PASS_MSIZE4); + + dTimerNow ("Verifying solution"); + dMultiply2 (btest, A, x, PASS_MSIZE, PASS_MSIZE, 1); + diff = dMaxDifference(b, btest, PASS_MSIZE, 1); + printf ("\tN=%u: maximum difference = %.6e - %s\n", PASS_MSIZE, diff, diff > 1e2 * tol ? "FAILED" : "passed"); + + dTimerEnd(); + dTimerReport(stdout, 0); + } + + dResourceContainerDestroy(resources); + dResourceRequirementsDestroy(requirements); + + dThreadingImplementationShutdownProcessing(threading); + dThreadingFreeThreadPool(pool); + dCooperativeDestroy(cooperative); + dThreadingFreeImplementation(threading); + + dFree(btest, vectorSize); + dFree(b, vectorSize); + dFree(x, vectorSize); + dFree(d, vectorSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testLDLTAddTL() +{ + int i,j; + dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *DL = (dReal *)dAlloc(matrixSize), *ATEST = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *a = (dReal *)dAlloc(vectorSize), diff; + + HEADER; + + dMakeRandomMatrix (A,MSIZE,MSIZE,1.0); + dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE); + memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal)); + dFactorLDLT (L,d,MSIZE,MSIZE4); + + // delete first row and column of factorization + for (i=0; i tol ? "FAILED" : "passed"); + + dFree(a, vectorSize); + dFree(d, vectorSize); + dFree(ATEST, matrixSize); + dFree(DL, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + + +void testLDLTRemove() +{ + int i,j,r; + dsizeint intVectorSize = sizeof(int) * MSIZE, matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE; + int *p = (int *)dAlloc(intVectorSize); + dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), + *L2 = (dReal *)dAlloc(matrixSize), *DL2 = (dReal *)dAlloc(matrixSize), + *Atest1 = (dReal *)dAlloc(matrixSize), *Atest2 = (dReal *)dAlloc(matrixSize), + *d = (dReal *)dAlloc(vectorSize), *d2 = (dReal *)dAlloc(vectorSize), diff, maxdiff; + + HEADER; + + // make array of A row pointers + dReal *Arows[MSIZE]; + for (i=0; i= r) ii--; + if (jj >= r) jj--; + if (A[i*MSIZE4+j] != Atest1[ii*MSIZE4+jj]) bad = 1; + } + } + } + if (bad) printf ("\trow/col removal FAILED for row %d\n",r); + + // zero out last row/column of Atest1 + for (i=0; i tol ? "FAILED" : "passed"); + + dFree(d2, vectorSize); + dFree(d, vectorSize); + dFree(Atest2, matrixSize); + dFree(Atest1, matrixSize); + dFree(DL2, matrixSize); + dFree(L2, matrixSize); + dFree(L, matrixSize); + dFree(A, matrixSize); +} + +//**************************************************************************** +// test mass stuff + +#define NUMP 10 // number of particles + + +void printMassParams (dMass *m) +{ + printf ("mass = %.4f\n",m->mass); + printf ("com = (%.4f,%.4f,%.4f)\n",m->c[0],m->c[1],m->c[2]); + printf ("I = [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n", + m->_I(0,0),m->_I(0,1),m->_I(0,2), + m->_I(1,0),m->_I(1,1),m->_I(1,2), + m->_I(2,0),m->_I(2,1),m->_I(2,2)); +} + + +void compareMassParams (dMass *m1, dMass *m2, const char *msg) +{ + int i,j,ok = 1; + if (!(cmp(m1->mass,m2->mass) && cmp(m1->c[0],m2->c[0]) && + cmp(m1->c[1],m2->c[1]) && cmp(m1->c[2],m2->c[2]))) + ok = 0; + for (i=0; i<3; i++) for (j=0; j<3; j++) + if (cmp (m1->_I(i,j),m2->_I(i,j))==0) ok = 0; + if (ok) printf ("\tpassed (%s)\n",msg); else printf ("\tFAILED (%s)\n",msg); +} + + +// compute the mass parameters of a particle set + +void computeMassParams (dMass *m, dReal q[NUMP][3], dReal pm[NUMP]) +{ + int i,j; + dMassSetZero (m); + for (i=0; imass += pm[i]; + for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j]; + m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]); + m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]); + m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]); + m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]); + m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]); + m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]); + } + for (j=0; j<3; j++) m->c[j] /= m->mass; + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); +} + + +void testMassFunctions() +{ + dMass m; + int i,j; + dReal q[NUMP][3]; // particle positions + dReal pm[NUMP]; // particle masses + dMass m1,m2; + dMatrix3 R; + + HEADER; + + printf ("\t"); + dMassSetZero (&m); + TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6), + printf (" FAILED (1)\n"), printf (" passed (1)\n")); + + printf ("\t"); + dMassSetZero (&m); + TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4), + printf ("passed (2)\n") , printf (" FAILED (2)\n")); + if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) && + m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 && + m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 && + m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4) + printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n"); + + dMassSetZero (&m); + dMassSetSphere (&m,1.4, 0.86); + if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && + cmp(m._I(0,0),1.10349124669826) && + cmp(m._I(1,1),1.10349124669826) && + cmp(m._I(2,2),1.10349124669826) && + m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && + m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) + printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n"); + + dMassSetZero (&m); + dMassSetCapsule (&m,1.3,1,0.76,1.53); + if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && + cmp(m._I(0,0),1.59461986077384) && + cmp(m._I(1,1),4.21878433864904) && + cmp(m._I(2,2),4.21878433864904) && + m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && + m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) + printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n"); + + dMassSetZero (&m); + dMassSetBox (&m,0.27,3,4,5); + if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && + cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) && + m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && + m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) + printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n"); + + // test dMassAdjust? + + // make random particles and compute the mass, COM and inertia, then + // translate and repeat. + for (i=0; i Q -> R works + dReal maxdiff=0; + for (i=0; i<100; i++) { + makeRandomRotation (R); + dRtoQ (R,q); + dQtoR (q,R2); + dReal diff = dMaxDifference (R,R2,3,3); + if (diff > maxdiff) maxdiff = diff; + } + printf ("\tmaximum difference = %e - %s (3)\n",maxdiff, + (maxdiff > tol) ? "FAILED" : "passed"); +} + + +void testQuaternionMultiply() +{ + HEADER; + dMatrix3 RA,RB,RC,Rtest; + dQuaternion qa,qb,qc; + dReal diff,maxdiff=0; + + for (int i=0; i<100; i++) { + makeRandomRotation (RB); + makeRandomRotation (RC); + dRtoQ (RB,qb); + dRtoQ (RC,qc); + + dMultiply0 (RA,RB,RC,3,3,3); + dQMultiply0 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + + dMultiply1 (RA,RB,RC,3,3,3); + dQMultiply1 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + + dMultiply2 (RA,RB,RC,3,3,3); + dQMultiply2 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + + dMultiply0 (RA,RC,RB,3,3,3); + transpose3x3 (RA); + dQMultiply3 (qa,qb,qc); + dQtoR (qa,Rtest); + diff = dMaxDifference (Rtest,RA,3,3); + if (diff > maxdiff) maxdiff = diff; + } + printf ("\tmaximum difference = %e - %s\n",maxdiff, + (maxdiff > tol) ? "FAILED" : "passed"); +} + + +void testRotationFunctions() +{ + dMatrix3 R1; + HEADER; + + printf ("\tdRSetIdentity - "); + dMakeRandomMatrix (R1,3,3,1.0); + dRSetIdentity (R1); + if (cmpIdentityMat3(R1)) printf ("passed\n"); else printf ("FAILED\n"); + + printf ("\tdRFromAxisAndAngle - "); + + printf ("\n"); + printf ("\tdRFromEulerAngles - "); + + printf ("\n"); + printf ("\tdRFrom2Axes - "); + + printf ("\n"); +} + +//**************************************************************************** + +#include + +template +class simplevector +{ +private: + int n; + int max; + T* data; + +public: + simplevector() { initialize(); } + ~simplevector() { finalize(); } + T& operator[](int i) { assert(i>=0 && i=0 && i mat; + int afterfirst,index; + +public: + dMatrixComparison(); + ~dMatrixComparison(); + + dReal nextMatrix (dReal *A, int n, int m, int lower_tri, const char *name, ...); + // add a new n*m matrix A to the sequence. the name of the matrix is given + // by the printf-style arguments (name,...). if this is the first sequence + // then this object will simply record the matrices and return 0. + // if this the second or subsequent sequence then this object will compare + // the matrices with the first sequence, and report any differences. + // the matrix error will be returned. if `lower_tri' is 1 then only the + // lower triangle of the matrix (including the diagonal) will be compared + // (the matrix must be square). + + void end(); + // end a sequence. + + void reset(); + // restarts the object, so the next sequence will be the first sequence. + + void dump(); + // print out info about all the matrices in the sequence +}; + +struct dMatrixComparison::dMatInfo { + int n,m; // size of matrix + char name[128]; // name of the matrix + dReal *data; // matrix data + int size; // size of `data' +}; + + + +dMatrixComparison::dMatrixComparison() +{ + afterfirst = 0; + index = 0; +} + + +dMatrixComparison::~dMatrixComparison() +{ + reset(); +} + + +dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri, + const char *name, ...) +{ + if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix"); + int num = n*dPAD(m); + + if (afterfirst==0) { + dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo)); + mi->n = n; + mi->m = m; + mi->size = num * sizeof(dReal); + mi->data = (dReal*) dAlloc (mi->size); + memcpy (mi->data,A,mi->size); + + va_list ap; + va_start (ap,name); + vsprintf (mi->name,name,ap); + va_end (ap); + if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long"); + + mat.push_back(mi); + return 0; + } + else { + if (lower_tri && n != m) + dDebug (0,"dMatrixComparison, lower triangular matrix must be square"); + if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices"); + dMatInfo *mp = mat[index]; + index++; + + dMatInfo mi; + va_list ap; + va_start (ap,name); + vsprintf (mi.name,name,ap); + va_end (ap); + if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long"); + + if (strcmp(mp->name,mi.name) != 0) + dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")", + mp->name,mi.name); + if (mp->n != n || mp->m != m) + dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)", + mp->n,mp->m,n,m); + + dReal maxdiff; + if (lower_tri) { + maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n); + } + else { + maxdiff = dMaxDifference (A,mp->data,n,m); + } + if (maxdiff > tol) + dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", " + "error=%.4e)",n,m,mi.name,maxdiff); + return maxdiff; + } +} + + +void dMatrixComparison::end() +{ + if (mat.size() <= 0) dDebug (0,"no matrices in sequence"); + afterfirst = 1; + index = 0; +} + + +void dMatrixComparison::reset() +{ + for (int i=0; idata,mat[i]->size); + dFree (mat[i],sizeof(dMatInfo)); + } + mat.clear(); + afterfirst = 0; + index = 0; +} + + +void dMatrixComparison::dump() +{ + for (int i=0; iname,mat[i]->n,mat[i]->m); +} + +//**************************************************************************** +// unit test + +#include + +// static jmp_buf jump_buffer; + +static void myDebug (int /*num*/, const char* /*msg*/, va_list /*ap*/) +{ + // printf ("(Error %d: ",num); + // vprintf (msg,ap); + // printf (")\n"); + longjmp (jump_buffer,1); +} + + +extern "C" void dTestMatrixComparison() +{ + volatile int i; + printf ("dTestMatrixComparison()\n"); + dMessageFunction *orig_debug = dGetDebugHandler(); + + dMatrixComparison mc; + dReal A[50*50]; + + // make first sequence + unsigned long seed = dRandGetSeed(); + for (i=1; i<49; i++) { + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + } + mc.end(); + + //mc.dump(); + + // test identical sequence + dSetDebugHandler (&myDebug); + dRandSetSeed (seed); + if (setjmp (jump_buffer)) { + printf ("\tFAILED (1)\n"); + } + else { + for (i=1; i<49; i++) { + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + } + mc.end(); + printf ("\tpassed (1)\n"); + } + dSetDebugHandler (orig_debug); + + // test broken sequences (with matrix error) + dRandSetSeed (seed); + volatile int passcount = 0; + for (i=1; i<49; i++) { + if (setjmp (jump_buffer)) { + passcount++; + } + else { + dSetDebugHandler (&myDebug); + dMakeRandomMatrix (A,i,i+1,1.0); + A[(i-1)*dPAD(i+1)+i] += REAL(0.01); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + dSetDebugHandler (orig_debug); + } + } + mc.end(); + printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED"); + + // test broken sequences (with name error) + dRandSetSeed (seed); + passcount = 0; + for (i=1; i<49; i++) { + if (setjmp (jump_buffer)) { + passcount++; + } + else { + dSetDebugHandler (&myDebug); + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"B%d",i); + dSetDebugHandler (orig_debug); + } + } + mc.end(); + printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED"); + + // test identical sequence again + dSetDebugHandler (&myDebug); + dRandSetSeed (seed); + if (setjmp (jump_buffer)) { + printf ("\tFAILED (4)\n"); + } + else { + for (i=1; i<49; i++) { + dMakeRandomMatrix (A,i,i+1,1.0); + mc.nextMatrix (A,i,i+1,0,"A%d",i); + } + mc.end(); + printf ("\tpassed (4)\n"); + } + dSetDebugHandler (orig_debug); +} + +//**************************************************************************** + +// internal unit tests +extern "C" void dTestDataStructures(); +extern "C" void dTestMatrixComparison(); +extern "C" int dTestSolveLCP(); + + +int main() +{ + dInitODE(); + testRandomNumberGenerator(); + testInfinity(); + testPad(); + testCrossProduct(); + testSetZero(); + testNormalize3(); + //testReorthonormalize(); ... not any more + testPlaneSpace(); + testMatrixMultiply(); + testSmallMatrixMultiply(); + testCholeskyFactorization(); + testCholeskySolve(); + testInvertPDMatrix(); + testIsPositiveDefinite(); + testFastLDLTFactorization(); + testCoopLDLTFactorization(); + testSolveLDLT(); + testCoopSolveLDLT(); + testLDLTAddTL(); + testLDLTRemove(); + testMassFunctions(); + testRtoQandQtoR(); + testQuaternionMultiply(); + testRotationFunctions(); + dTestMatrixComparison(); + dTestSolveLCP(); + // dTestDataStructures(); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_piston.cpp b/libs/ode-0.16.1/ode/demo/demo_piston.cpp new file mode 100644 index 0000000..8a0453a --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_piston.cpp @@ -0,0 +1,813 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + * Created by: Remi Ricard * + * (remi.ricard@simlog.com or papaDoc@videotron.ca) * + * Creation date: 2007/05/04 * + *************************************************************************/ + +/* + This program demonstrates how the Piston joint works. + + A Piston joint enables the sliding of a body with respect to another body + and the 2 bodies are free to rotate about the sliding axis. + + - The yellow body is fixed to the world. + - The yellow body and the blue body are attached by a Piston joint with + the axis along the x direction. + - The purple object is a geometry obstacle. + - The red line is the representation of the prismatic axis + - The orange line is the representation of the rotoide axis + - The light blue ball is the anchor position + + N.B. Many command options are available type -h to print them. +*/ + +#include +#include +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawSphere dsDrawSphereD +#endif + + +const dReal VEL_INC = 0.01; // Velocity increment + +// physics parameters +const dReal PI = 3.14159265358979323846264338327950288419716939937510; +const dReal BODY1_LENGTH = 1.5; // Size along the X axis + +const dReal RADIUS = 0.2; +const dReal AXIS_RADIUS = 0.01; + + +#define X 0 +#define Y 1 +#define Z 2 + +enum INDEX +{ + BODY1 = 0, + BODY2, + RECT, + BOX, + OBS, + GROUND, + NUM_PARTS, + ALL = NUM_PARTS +}; + +const int catBits[NUM_PARTS+1] = +{ + 0x0001, ///< Ext Cylinder category + 0x0002, ///< Int Cylinder category + 0x0004, ///< Int_Rect Cylinder category + 0x0008, ///< Box category + 0x0010, ///< Obstacle category + 0x0020, ///< Ground category + ~0L ///< All categories +}; + +#define Mass1 10 +#define Mass2 8 + + +//camera view +static float xyz[3] = {2.0f,-3.5f,2.0000f}; +static float hpr[3] = {90.000f,-25.5000f,0.0000f}; + + +//world,space,body & geom +static dWorldID world; +static dSpaceID space; +static dJointGroupID contactgroup; +static dBodyID body[NUM_PARTS]; +static dGeomID geom[NUM_PARTS]; + +// Default Positions and anchor of the 2 bodies +dVector3 pos1; +dVector3 pos2; +dVector3 anchor; + +static dJoint *joint; + + +const dReal BODY2_SIDES[3] = {0.4, 0.4, 0.4}; +const dReal OBS_SIDES[3] = {1,1,1}; +const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2}; + + +int type = dJointTypePiston; + +//#pragma message("tc to be changed to 0") + +int tc = 0; // The test case choice; + + +//collision detection +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i,n; + + dBodyID b1 = dGeomGetBody (o1); + dBodyID b2 = dGeomGetBody (o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return; + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) ); + if (n > 0) + { + for (i=0; iattach (body[BODY1], body[BODY2]); + if (joint->getType() == dJointTypePiston) + dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]); + } + +} + + +// function to update camera position at each step. +void update() +{ +// static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w"); + +// static int cnt = 0; +// char str[24]; +// sprintf(str, "%06d",cnt++); + +// dWorldExportDIF(world, file, str); +} + + +// called when a key pressed +static void command (int cmd) +{ + switch (cmd) + { + case 'h' : + case 'H' : + case '?' : + printKeyBoardShortCut(); + break; + + // Force + case 'q' : + case 'Q' : + dBodyAddForce (body[BODY1],4,0,0); + break; + case 'w' : + case 'W' : + dBodyAddForce (body[BODY1],-4,0,0); + break; + + case 'a' : + case 'A' : + dBodyAddForce (body[BODY1],0,40,0); + break; + case 's' : + case 'S' : + dBodyAddForce (body[BODY1],0,-40,0); + break; + + case 'z' : + case 'Z' : + dBodyAddForce (body[BODY1],0,0,4); + break; + case 'x' : + case 'X' : + dBodyAddForce (body[BODY1],0,0,-4); + break; + + // Torque + case 'e': + case 'E': + dBodyAddTorque (body[BODY1],0.1,0,0); + break; + case 'r': + case 'R': + dBodyAddTorque (body[BODY1],-0.1,0,0); + break; + + case 'd': + case 'D': + dBodyAddTorque (body[BODY1],0, 0.1,0); + break; + case 'f': + case 'F': + dBodyAddTorque (body[BODY1],0,-0.1,0); + break; + + case 'c': + case 'C': + dBodyAddTorque (body[BODY1],0.1,0,0); + break; + case 'v': + case 'V': + dBodyAddTorque (body[BODY1],-0.1,0,0); + break; + + case 't': + case 'T': + if (joint->getType() == dJointTypePiston) + dJointAddPistonForce (joint->id(),1); + else + dJointAddSliderForce (joint->id(),1); + break; + case 'y': + case 'Y': + if (joint->getType() == dJointTypePiston) + dJointAddPistonForce (joint->id(),-1); + else + dJointAddSliderForce (joint->id(),-1); + break; + + + case '8' : + dJointAttach(joint->id(), body[0], 0); + break; + case '9' : + dJointAttach(joint->id(), 0, body[0]); + break; + + case 'i': + case 'I' : + joint->setParam (dParamLoStop, 0); + joint->setParam (dParamHiStop, 0); + break; + + case 'o': + case 'O' : + joint->setParam (dParamLoStop2, 0); + joint->setParam (dParamHiStop2, 0); + break; + + case 'k': + case 'K': + joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0); + joint->setParam (dParamHiStop2, 45.0*3.14159267/180.0); + break; + case 'l': + case 'L': + joint->setParam (dParamLoStop2, -dInfinity); + joint->setParam (dParamHiStop2, dInfinity); + break; + + // Velocity of joint + case ',': + case '<' : + { + dReal vel = joint->getParam (dParamVel) - VEL_INC; + joint->setParam (dParamVel, vel); + std::cout<<"Velocity = "<' : + { + dReal vel = joint->getParam (dParamVel) + VEL_INC; + joint->setParam (dParamVel, vel); + std::cout<<"Velocity = "<getType() ) + { + case dJointTypeSlider : + { + dSliderJoint *sj = reinterpret_cast (joint); + std::cout<<"Position ="<getPosition() <<"\n"; + } + break; + case dJointTypePiston : + { + dPistonJoint *rj = reinterpret_cast (joint); + std::cout<<"Position ="<getPosition() <<"\n"; + } + break; + default: + {} // keep the compiler happy + } + } + break; + + case '+' : + (++tc) %= 4; + setPositionBodies (tc); + break; + case '-' : + (--tc) %= 4; + setPositionBodies (tc); + break; + + + } +} + +static void drawBox (dGeomID id, int R, int G, int B) +{ + if (!id) + return; + + const dReal *pos = dGeomGetPosition (id); + const dReal *rot = dGeomGetRotation (id); + dsSetColor (R,G,B); + + dVector3 l; + dGeomBoxGetLengths (id, l); + dsDrawBox (pos, rot, l); +} + + +// simulation loop +static void simLoop (int pause) +{ + const dReal *rot; + dVector3 ax; + dReal l=0; + + switch (joint->getType() ) + { + case dJointTypeSlider : + ( (dSliderJoint *) joint)->getAxis (ax); + l = ( (dSliderJoint *) joint)->getPosition(); + break; + case dJointTypePiston : + ( (dPistonJoint *) joint)->getAxis (ax); + l = ( (dPistonJoint *) joint)->getPosition(); + break; + default: + {} // keep the compiler happy + } + + + if (!pause) + { + double simstep = 0.01; // 1ms simulation steps + double dt = dsElapsedTime(); + + int nrofsteps = (int) ceilf (dt/simstep); + if (!nrofsteps) + nrofsteps = 1; + + for (int i=0; igetType() == dJointTypePiston ) + { + dVector3 anchor; + dJointGetPistonAnchor(joint->id(), anchor); + + // Draw the rotoide axis + rot = dGeomGetRotation (geom[BODY2]); + dsSetColor (1,0.5,0); + dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS); + + + dsSetColor (0,1,1); + rot = dGeomGetRotation (geom[BODY1]); + dsDrawSphere (anchor, rot, 1.5*RADIUS); + } + + } +} + + +void Help (char **argv) +{ + printf ("%s ", argv[0]); + printf (" -h | --help : print this help\n"); + printf (" -s | --slider : Set the joint as a slider\n"); + printf (" -p | --piston : Set the joint as a Piston. (Default joint)\n"); + printf (" -1 | --offset1 : Create an offset between the 2 bodies\n"); + printf (" Offset one of the body by z=-0.5 and keep the anchor\n"); + printf (" point in the middle of the fixed body\n"); + printf (" -2 | --offset2 : Create an offset between the 2 bodies\n"); + printf (" Offset one of the body by z=-0.5 and set the anchor\n"); + printf (" point in the middle of the movable body\n"); + printf (" -3 | --offset3 : Create an offset between the 2 bodies\n"); + printf (" Offset one of the body by z=-0.5 and set the anchor\n"); + printf (" point in the middle of the 2 bodies\n"); + printf (" -t | --texture-path path : Path to the texture.\n"); + printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH); + printf (" -n | --notFixed : In free space with no gravity mode"); + printf ("-notex : Don't use texture\n"); + printf ("-noshadow : No shadow\n"); + printf ("-noshadows : No shadows\n"); + printf ("-pause : Initial pause\n"); + printf ("--------------------------------------------------\n"); + printf ("Hit any key to continue:"); + getchar(); + + exit (0); +} + +int main (int argc, char **argv) +{ + dInitODE2(0); + bool fixed = true; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dVector3 offset; + dSetZero (offset, 4); + + // Default test case + + if (argc >= 2 ) + { + for (int i=1; i < argc; ++i) + { + //static int tata = 0; + + if (1) + { + if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) + Help (argv); + + if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) ) + type = dJointTypeSlider; + + if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) + { + int j = i+1; + if ( j >= argc || // Check if we have enough arguments + argv[j][0] == '\0' || // We should have a path here + argv[j][0] == '-' ) // We should have a path not a command line + Help (argv); + else + fn.path_to_textures = argv[++i]; // Increase i since we use this argument + } + } + + + if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) ) + tc = 1; + + if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) ) + tc = 2; + + if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) ) + tc = 3; + + if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) ) + fixed = false; + } + } + + world = dWorldCreate(); + dWorldSetERP (world, 0.8); + + space = dSimpleSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + geom[GROUND] = dCreatePlane (space, 0,0,1,0); + dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); + dGeomSetCollideBits (geom[GROUND], catBits[ALL]); + + dMass m; + dMatrix3 R; + + + // Create the Obstacle + geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]); + dGeomSetCategoryBits (geom[OBS], catBits[OBS]); + dGeomSetCollideBits (geom[OBS], catBits[ALL]); + //Rotation of 45deg around y + dRFromAxisAndAngle (R, 1,1,0, -0.25*PI); + dGeomSetRotation (geom[OBS], R); + dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5); + + + //Rotation of 90deg around y + // Will orient the Z axis along X + dRFromAxisAndAngle (R, 0,1,0, -0.5*PI); + + + // Create Body2 (Wiil be attached to the world) + body[BODY2] = dBodyCreate (world); + // Main axis of cylinder is along X=1 + dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); + dMassAdjust (&m, Mass1); + geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); + dGeomSetBody (geom[BODY2], body[BODY2]); + dGeomSetOffsetRotation (geom[BODY2], R); + dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]); + dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) ); + dBodySetMass (body[BODY2], &m); + + + // Create Body 1 (Slider on the prismatic axis) + body[BODY1] = dBodyCreate (world); + // Main axis of capsule is along X=1 + dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH); + dMassAdjust (&m, Mass1); + geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH); + dGeomSetBody (geom[BODY1], body[BODY1]); + dGeomSetOffsetRotation (geom[BODY1], R); + dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]); + dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]); + + dMass mRect; + dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); + dMassAdd (&m, &mRect); + // TODO: translate m? + geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); + dGeomSetBody (geom[RECT], body[BODY1]); + dGeomSetOffsetPosition (geom[RECT], + (BODY1_LENGTH-RECT_SIDES[0]) /2.0, + 0.0, + -RADIUS -RECT_SIDES[2]/2.0); + dGeomSetCategoryBits (geom[RECT], catBits[RECT]); + dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) ); + + dBodySetMass (body[BODY1], &m); + + + + setPositionBodies (tc); + + + if ( fixed ) + { + // Attache external cylinder to the world + dJointID fixed = dJointCreateFixed (world,0); + dJointAttach (fixed , NULL, body[BODY2]); + dJointSetFixed (fixed ); + dWorldSetGravity (world,0,0,-0.8); + } + else + { + dWorldSetGravity (world,0,0,0); + } + + + + + // The static is here only to help debugging + switch (type) + { + case dJointTypeSlider : + { + dSliderJoint *sj = new dSliderJoint (world, 0); + sj->attach (body[BODY1], body[BODY2]); + sj->setAxis (1, 0, 0); + joint = sj; + } + break; + + case dJointTypePiston : // fall through default + default: + { + dPistonJoint *pj = new dPistonJoint (world, 0); + pj->attach (body[BODY1], body[BODY2]); + pj->setAxis (1, 0, 0); + + dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]); + + joint = pj; + } + break; + }; + + + // run simulation + dsSimulationLoop (argc,argv,400,300,&fn); + + delete joint; + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} + + + + diff --git a/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp b/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp new file mode 100644 index 0000000..559f9ae --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp @@ -0,0 +1,304 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// Test my Plane2D constraint. +// Uses ode-0.35 collision API. + +# include +# include +# include +# include +# include +#include "texturepath.h" + + +# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX))) + +# define N_BODIES 40 +# define STAGE_SIZE 8.0 // in m + +# define TIME_STEP 0.01 +# define K_SPRING 10.0 +# define K_DAMP 10.0 + +//using namespace ode; + +struct GlobalVars +{ + dWorld dyn_world; + dBody dyn_bodies[N_BODIES]; + dReal bodies_sides[N_BODIES][3]; + + dSpaceID coll_space_id; + dJointID plane2d_joint_ids[N_BODIES]; + dJointGroup coll_contacts; +}; + +static GlobalVars *g_globals_ptr = NULL; + + + +static void cb_start () +/*************************/ +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.65f*STAGE_SIZE}; + static float hpr[3] = { 90.0f, -90.0f, 0 }; + + dsSetViewpoint (xyz, hpr); +} + + + +static void cb_near_collision (void *, dGeomID o1, dGeomID o2) +/********************************************************************/ +{ + dBodyID b1 = dGeomGetBody (o1); + dBodyID b2 = dGeomGetBody (o2); + dContact contact; + + + // exit without doing anything if the two bodies are static + if (b1 == 0 && b2 == 0) + return; + + // exit without doing anything if the two bodies are connected by a joint + if (b1 && b2 && dAreConnected (b1, b2)) + { + /* MTRAP; */ + return; + } + + contact.surface.mode = 0; + contact.surface.mu = 0; // frictionless + + if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom))) + { + dJointID c = dJointCreateContact (g_globals_ptr->dyn_world.id(), + g_globals_ptr->coll_contacts.id (), &contact); + dJointAttach (c, b1, b2); + } +} + + +static void track_to_pos (dBody &body, dJointID joint_id, + dReal target_x, dReal target_y) +/************************************************************************/ +{ + dReal curr_x = body.getPosition()[0]; + dReal curr_y = body.getPosition()[1]; + + dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x)); + dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y)); +} + + + +static void cb_sim_step (int pause) +/*************************************/ +{ + if (! pause) + { + static dReal angle = 0; + + angle += REAL( 0.01 ); + + track_to_pos (g_globals_ptr->dyn_bodies[0], g_globals_ptr->plane2d_joint_ids[0], + dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ), + dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) )); + + /* double f0 = 0.001; */ + /* for (int b = 0; b < N_BODIES; b ++) */ + /* { */ + /* double p = 1 + b / (double) N_BODIES; */ + /* double q = 2 - b / (double) N_BODIES; */ + /* g_globals_ptr->dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */ + /* } */ + /* g_globals_ptr->dyn_bodies[0].addTorque (0, 0, 0.1); */ + + const int n = 10; + for (int i = 0; i < n; i ++) + { + dSpaceCollide (g_globals_ptr->coll_space_id, 0, &cb_near_collision); + g_globals_ptr->dyn_world.step (dReal(TIME_STEP/n)); + g_globals_ptr->coll_contacts.empty (); + } + } + +# if 1 /* [ */ + { + // @@@ hack Plane2D constraint error reduction here: + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id()); + const dReal *quat_ptr; + dReal quat[4], + quat_len; + + + quat_ptr = dBodyGetQuaternion (g_globals_ptr->dyn_bodies[b].id()); + quat[0] = quat_ptr[0]; + quat[1] = 0; + quat[2] = 0; + quat[3] = quat_ptr[3]; + quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]); + quat[0] /= quat_len; + quat[3] /= quat_len; + dBodySetQuaternion (g_globals_ptr->dyn_bodies[b].id(), quat); + dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(), 0, 0, rot[2]); + } + } +# endif /* ] */ + + +# if 0 /* [ */ + { + // @@@ friction + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *vel = dBodyGetLinearVel (g_globals_ptr->dyn_bodies[b].id()), + *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id()); + dReal s = 1.00; + dReal t = 0.99; + + dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]); + dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]); + } + } +# endif /* ] */ + + + { + // ode drawstuff + + dsSetTexture (DS_WOOD); + for (int b = 0; b < N_BODIES; b ++) + { + if (b == 0) + dsSetColor (1.0, 0.5, 1.0); + else + dsSetColor (0, 0.5, 1.0); +#ifdef dDOUBLE + dsDrawBoxD (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]); +#else + dsDrawBox (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]); +#endif + } + } +} + + + +extern int main +/******************/ +( + int argc, + char **argv +) +{ + int b; + dsFunctions drawstuff_functions; + + + dInitODE2(0); + + g_globals_ptr = new GlobalVars(); + + // dynamic world + + dReal cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP; + dReal err_reduct;// = TIME_STEP * K_SPRING * cf_mixing; + err_reduct = REAL( 0.5 ); + cf_mixing = REAL( 0.001 ); + dWorldSetERP (g_globals_ptr->dyn_world.id (), err_reduct); + dWorldSetCFM (g_globals_ptr->dyn_world.id (), cf_mixing); + g_globals_ptr->dyn_world.setGravity (0, 0.0, -1.0); + + g_globals_ptr->coll_space_id = dSimpleSpaceCreate (0); + + // dynamic bodies + for (b = 0; b < N_BODIES; b ++) + { + int l = (int) (1 + sqrt ((double) N_BODIES)); + dReal x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE ); + dReal y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE ); + dReal z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48(); + + g_globals_ptr->bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); + g_globals_ptr->bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); + g_globals_ptr->bodies_sides[b][2] = z; + + g_globals_ptr->dyn_bodies[b].create (g_globals_ptr->dyn_world); + g_globals_ptr->dyn_bodies[b].setPosition (x, y, z/2); + g_globals_ptr->dyn_bodies[b].setData ((void*) (dsizeint)b); + dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id (), + dReal( 3 * (drand48 () - 0.5) ), + dReal( 3 * (drand48 () - 0.5) ), 0); + + dMass m; + m.setBox (1, g_globals_ptr->bodies_sides[b][0],g_globals_ptr->bodies_sides[b][1],g_globals_ptr->bodies_sides[b][2]); + m.adjust (REAL(0.1) * g_globals_ptr->bodies_sides[b][0] * g_globals_ptr->bodies_sides[b][1]); + g_globals_ptr->dyn_bodies[b].setMass (&m); + + g_globals_ptr->plane2d_joint_ids[b] = dJointCreatePlane2D (g_globals_ptr->dyn_world.id (), 0); + dJointAttach (g_globals_ptr->plane2d_joint_ids[b], g_globals_ptr->dyn_bodies[b].id (), 0); + } + + dJointSetPlane2DXParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10); + dJointSetPlane2DYParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10); + + + // collision geoms and joints + dCreatePlane (g_globals_ptr->coll_space_id, 1, 0, 0, 0); + dCreatePlane (g_globals_ptr->coll_space_id, -1, 0, 0, -STAGE_SIZE); + dCreatePlane (g_globals_ptr->coll_space_id, 0, 1, 0, 0); + dCreatePlane (g_globals_ptr->coll_space_id, 0, -1, 0, -STAGE_SIZE); + + for (b = 0; b < N_BODIES; b ++) + { + dGeomID coll_box_id; + coll_box_id = dCreateBox (g_globals_ptr->coll_space_id, + g_globals_ptr->bodies_sides[b][0], g_globals_ptr->bodies_sides[b][1], g_globals_ptr->bodies_sides[b][2]); + dGeomSetBody (coll_box_id, g_globals_ptr->dyn_bodies[b].id ()); + } + + g_globals_ptr->coll_contacts.create (); + + { + // simulation loop (by drawstuff lib) + drawstuff_functions.version = DS_VERSION; + drawstuff_functions.start = &cb_start; + drawstuff_functions.step = &cb_sim_step; + drawstuff_functions.command = 0; + drawstuff_functions.stop = 0; + drawstuff_functions.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions); + } + + delete g_globals_ptr; + g_globals_ptr = NULL; + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp b/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp new file mode 100644 index 0000000..61d1115 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp @@ -0,0 +1,258 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* +Angular friction demo: + +A bunch of ramps of different pitch. +A bunch of spheres with rolling friction. +*/ + + +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants +#define GRAVITY 10 // the global gravity to use +#define RAMP_COUNT 8 + +static const dReal rampX = 6.0f; +static const dReal rampY = 0.5f; +static const dReal rampZ = 0.25f; +static const dReal sphereRadius = 0.25f; +static const dReal maxRamp = M_PI/4.0f; // Needs to be less than pi/2 +static const dReal rampInc = maxRamp/RAMP_COUNT; + +// dynamics and collision objects +static dWorldID world = 0; +static dSpaceID space = 0; +static dJointGroupID contactgroup = 0; +static dGeomID ground; + +static dReal mu = REAL(0.37); // the global mu to use +static dReal rho = REAL(0.1); // the global rho to use +static dReal omega = REAL(25.0); + +static dGeomID rampGeom[RAMP_COUNT]; +static dBodyID sphereBody[RAMP_COUNT]; +static dGeomID sphereGeom[RAMP_COUNT]; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + if (b1==0 && b2==0) return; + + dContact contact[3]; + for (int ii=0; ii<3; ii++) { + contact[ii].surface.mode = dContactApprox1 | dContactRolling; + contact[ii].surface.mu = mu; + contact[ii].surface.rho = rho; + } + if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) { + for (i=0; i1) rho=1; + break; + case 'n': case 'N': + mu-=0.02; + if (mu<0) mu=0; + break; + case 'm': case 'M': + mu+=0.02; + if (mu>1) mu=1; + break; + case 'r': case 'R': + reset(); + break; + case ']': + omega+=1; + break; + case '[': + omega-=1; + break; + } +} + +// simulation loop + +static void simLoop (int pause) +{ + if (!pause) { + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.017); // 60 fps + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + // Render ramps and spheres + dsSetTexture (DS_WOOD); + for (int ii=0;ii +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#endif + + +// some constants +#define SIDE (0.5f) // side length of a box +#define MASS (1.0) // mass of a box + + +// dynamics and collision objects +static dWorldID world; +static dBodyID body[2]; +static dJointID slider; + + +// state set by keyboard commands +static int occasional_error = 0; + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; + static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("Press 'e' to start/stop occasional error.\n"); +} + + +// called when a key pressed + +static void command (int cmd) +{ + if (cmd == 'e' || cmd == 'E') { + occasional_error ^= 1; + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + const dReal kd = -0.3; // angular damping constant + const dReal ks = 0.5; // spring constant + if (!pause) { + // add an oscillating torque to body 0, and also damp its rotational motion + static dReal a=0; + const dReal *w = dBodyGetAngularVel (body[0]); + dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a)); + a += 0.01; + + // add a spring force to keep the bodies together, otherwise they will + // fly apart along the slider axis. + const dReal *p1 = dBodyGetPosition (body[0]); + const dReal *p2 = dBodyGetPosition (body[1]); + dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]), + ks*(p2[2]-p1[2])); + dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]), + ks*(p1[2]-p2[2])); + + // occasionally re-orient one of the bodies to create a deliberate error. + if (occasional_error) { + static int count = 0; + if ((count % 20)==0) { + // randomly adjust orientation of body[0] + const dReal *R1; + dMatrix3 R2,R3; + R1 = dBodyGetRotation (body[0]); + dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, + dRandReal()-0.5,dRandReal()-0.5); + dMultiply0 (R3,R1,R2,3,3,3); + dBodySetRotation (body[0],R3); + + // randomly adjust position of body[0] + const dReal *pos = dBodyGetPosition (body[0]); + dBodySetPosition (body[0], + pos[0]+0.2*(dRandReal()-0.5), + pos[1]+0.2*(dRandReal()-0.5), + pos[2]+0.2*(dRandReal()-0.5)); + } + count++; + } + + dWorldStep (world,0.05); + } + + dReal sides1[3] = {SIDE,SIDE,SIDE}; + dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f}; + dsSetTexture (DS_WOOD); + dsSetColor (1,1,0); + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); + dsSetColor (0,1,1); + dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE2(0); + world = dWorldCreate(); + dMass m; + dMassSetBox (&m,1,SIDE,SIDE,SIDE); + dMassAdjust (&m,MASS); + + body[0] = dBodyCreate (world); + dBodySetMass (body[0],&m); + dBodySetPosition (body[0],0,0,1); + body[1] = dBodyCreate (world); + dBodySetMass (body[1],&m); + dQuaternion q; + dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI); + dBodySetPosition (body[1],0.2,0.2,1.2); + dBodySetQuaternion (body[1],q); + + slider = dJointCreateSlider (world,0); + dJointAttach (slider,body[0],body[1]); + dJointSetSliderAxis (slider,1,1,1); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_space.cpp b/libs/ode-0.16.1/ode/demo/demo_space.cpp new file mode 100644 index 0000000..6f871f6 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_space.cpp @@ -0,0 +1,232 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +testing procedure: + * create a bunch of random boxes + * test for intersections directly, put results in n^2 array + * get space to report collisions: + - all correct collisions reported + - no pair reported more than once + - no incorrect collisions reported + +*/ + + +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 20 // number of boxes to test + + +// collision objects and globals + +static dSpaceID space; +static dGeomID geom[NUM]; +static dReal bounds[NUM][6]; +static dsizeint good_matrix[NUM][NUM]; // correct collision matrix +static dsizeint test_matrix[NUM][NUM]; // testing collision matrix +static dsizeint hits[NUM]; // number of collisions a box has +static unsigned long seed=37; + + +static void init_test() +{ + int i,j; + const dReal scale = 0.5; + + // set random boxes + dRandSetSeed (seed); + for (i=0; i < NUM; i++) { + bounds[i][0] = dRandReal()*2-1; + bounds[i][1] = bounds[i][0] + dRandReal()*scale; + bounds[i][2] = dRandReal()*2-1; + bounds[i][3] = bounds[i][2] + dRandReal()*scale; + bounds[i][4] = dRandReal()*2; + bounds[i][5] = bounds[i][4] + dRandReal()*scale; + + if (geom[i]) dGeomDestroy (geom[i]); + geom[i] = dCreateBox (space, + bounds[i][1] - bounds[i][0], + bounds[i][3] - bounds[i][2], + bounds[i][5] - bounds[i][4]); + dGeomSetPosition (geom[i], + (bounds[i][0] + bounds[i][1])*0.5, + (bounds[i][2] + bounds[i][3])*0.5, + (bounds[i][4] + bounds[i][5])*0.5); + dGeomSetData (geom[i],(void*)(dsizeint)(i)); + } + + // compute all intersections and put the results in "good_matrix" + for (i=0; i < NUM; i++) { + for (j=0; j < NUM; j++) good_matrix[i][j] = 0; + } + for (i=0; i < NUM; i++) hits[i] = 0; + + for (i=0; i < NUM; i++) { + for (j=i+1; j < NUM; j++) { + dReal *bounds1 = &bounds[i][0]; + dReal *bounds2 = &bounds[j][0]; + if (bounds1[0] > bounds2[1] || + bounds1[1] < bounds2[0] || + bounds1[2] > bounds2[3] || + bounds1[3] < bounds2[2] || + bounds1[4] > bounds2[5] || + bounds1[5] < bounds2[4]) continue; + good_matrix[i][j] = 1; + good_matrix[j][i] = 1; + hits[i]++; + hits[j]++; + } + } +} + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + dsizeint i,j; + i = (dsizeint) dGeomGetData (o1); + j = (dsizeint) dGeomGetData (o2); + if (i==j) + printf ("collision (%d,%d) is between the same object\n",(int)i,(int)j); + if (!good_matrix[i][j] || !good_matrix[j][i]) + printf ("collision (%d,%d) is incorrect\n",(int)i,(int)j); + if (test_matrix[i][j] || test_matrix[j][i]) + printf ("collision (%d,%d) reported more than once\n",(int)i,(int)j); + test_matrix[i][j] = 1; + test_matrix[j][i] = 1; +} + + +// start simulation - set viewpoint + +static void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); +} + + +static void command (int cmd) +{ + if (cmd == ' ') { + seed++; + init_test(); + } +} + + +// simulation loop + +static void simLoop (int) +{ + int i,j; + + for (i=0; i < NUM; i++) { + for (j=0; j < NUM; j++) test_matrix[i][j] = 0; + } + dSpaceCollide (space,0,&nearCallback); + for (i=0; i < NUM; i++) { + for (j=i+1; j < NUM; j++) { + if (good_matrix[i][j] && !test_matrix[i][j]) { + printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed); + } + } + } + + seed++; + init_test(); + + for (i=0; i 0) dsSetColor (1,0,0); + else dsSetColor (1,1,0); + dsDrawBox (pos,R,side); + } +} + + +int main (int argc, char **argv) +{ + int i; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + dInitODE2(0); + + // test the simple space: + // space = dSimpleSpaceCreate(); + + // test the hash space: + // space = dHashSpaceCreate (0); + // dHashSpaceSetLevels (space,-10,10); + + // test the quadtree space + dVector3 Center = {0, 0, 0, 0}; + dVector3 Extents = {10, 0, 10, 0}; + space = dQuadTreeSpaceCreate(0, Center, Extents, 7); + + for (i=0; i < NUM; i++) geom[i] = 0; + init_test(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dSpaceDestroy (space); + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_space_stress.cpp b/libs/ode-0.16.1/ode/demo/demo_space_stress.cpp new file mode 100644 index 0000000..dcbd9d7 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_space_stress.cpp @@ -0,0 +1,449 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include + +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10000 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 4 // maximum number of contact points per body +#define WORLD_SIZE 20 +#define WORLD_HEIGHT 20 + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space = NULL; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int draw_geom = 1; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase(cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y') { + if (num < NUM) { + // new object to be created + i = num; + num++; + } else { + // recycle existing object + i = nextobj++; + nextobj %= num; // wrap-around if needed + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + obj[i].body = 0; + + for (k=0; k < GPB; k++) + if (obj[i].geom[k]) { + dGeomDestroy(obj[i].geom[k]); + obj[i].geom[k] = 0; + } + } + + obj[i].body = dBodyCreate(world); + + for (k=0; k<3; k++) + sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition(obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); + dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } else { + // higher than highest body position + dReal maxheight = 0; + for (k=0; k maxheight) + maxheight = pos[2]; + } + dBodySetPosition(obj[i].body, 0,0,maxheight+1); + dRSetIdentity(R); + //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); + } + + dBodySetRotation(obj[i].body,R); + + if (cmd == 'b') { + + dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]); + + } else if (cmd == 'c') { + + sides[0] *= 0.5; + dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + + } else if (cmd == 'y') { + + dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]); + + } else if (cmd == 's') { + + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + } else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } + else if (cmd == 'o') { + draw_geom ^= 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!draw_geom){ + return; + } + + if (!g) return; + if (!pos) pos = dGeomGetPosition(g); + if (!R) R = dGeomGetRotation(g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths(g,sides); + dsDrawBox(pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere(pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams(g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams(g,&radius,&length); + dsDrawCylinder(pos,R,length,radius); + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB(g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) + bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int j=0; j<3; j++) + bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity(RI); + dsSetColorAlpha(1,0,0,0.5); + dsDrawBox(bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + //if (!pause) dWorldStep (world,0.05); + if (!pause) dWorldQuickStep (world,0.05); + //if (!pause) dWorldStepFast (world,0.05, 1); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#endif + + +// some constants + +#define NUM 10 // number of bodies +#define NUMJ 9 // number of joints +#define SIDE (0.2) // side length of a box +#define MASS (1.0) // mass of a box +#define RADIUS (0.1732f) // sphere radius + + + +// dynamics and collision objects + +static dWorldID world=0; +static dBodyID body[NUM]; +static dJointID joint[NUMJ]; + + +// create the test system + +void createTest() +{ + int i,j; + if (world) dWorldDestroy (world); + + world = dWorldCreate(); + + // create random bodies + for (i=0; i +#include +#include +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawLine dsDrawLineD +#endif + + + +const dReal ball_radius = 0.4; +const dReal balls_sep = 2; // separation between the balls + +/* Choose one test case + */ +#define TEST_CASE 0 + +#if TEST_CASE == 0 +const dReal track_len = 10; +const dReal track_height = 1; +const dReal track_width = 0.1; +const dReal track_gauge = 1; +const dReal track_elevation = 2; +const dReal track_angle = 80 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#elif TEST_CASE == 1 +const dReal track_len = 10; +const dReal track_height = 1; +const dReal track_width = 0.1; +const dReal track_gauge = 1.9*ball_radius; +const dReal track_elevation = 2; +const dReal track_angle = 0 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#elif TEST_CASE == 2 +const dReal track_len = 10; +const dReal track_height = 1; +const dReal track_width = 0.1; +const dReal track_gauge = 1.9*ball_radius; +const dReal track_elevation = 2; +const dReal track_angle = 15 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#elif TEST_CASE == 3 +const dReal track_len = 10; +const dReal track_height = .7; +const dReal track_width = 0.1; +const dReal track_gauge = track_height*1.1; +const dReal track_elevation = 2; +const dReal track_angle = 90 * M_PI/180.; +const dReal track_incl = 10 * M_PI/180.; +#else +#error "TEST_CAST to a valid value!" +#endif + + + +dWorldID world; +dSpaceID space; +dJointGroupID contact_group; +dGeomID ground; +dGeomID ball1_geom, ball2_geom; +dTriMeshDataID mesh_data; +dGeomID mesh_geom; + +dBodyID ball1_body, ball2_body; + +const unsigned n_box_verts = 8; +dVector3 box_verts[n_box_verts] = { + {-track_len/2, -track_width/2, track_height/2}, // 0 + { track_len/2, -track_width/2, track_height/2}, // 1 + { track_len/2, track_width/2, track_height/2}, // 2 + {-track_len/2, track_width/2, track_height/2}, // 3 + { track_len/2, -track_width/2, -track_height/2}, // 4 + {-track_len/2, -track_width/2, -track_height/2}, // 5 + {-track_len/2, track_width/2, -track_height/2}, // 6 + { track_len/2, track_width/2, -track_height/2} // 7 +}; + +const unsigned n_box_faces = 12; +dTriIndex box_faces[n_box_faces * 3] = { + 0, 1, 2, + 0, 2, 3, + 1, 4, 7, + 1, 7, 2, + 4, 5, 6, + 4, 6, 7, + 5, 0, 3, + 5, 3, 6, + 3, 2, 7, + 3, 7, 6, + 0, 5, 4, + 0, 4, 1 +}; + + +const unsigned n_track_verts = n_box_verts * 2; +const unsigned n_track_faces = n_box_faces * 2; + +dVector3 track_verts[n_track_verts]; +dTriIndex track_faces[n_track_faces * 3]; + + + +void resetBall(dBodyID b, unsigned idx) +{ + dBodySetPosition(b, + 0.5*track_len*cos(track_incl) // Z + - 0.5*track_height*sin(track_incl) + - ball_radius, // X + balls_sep*idx, // Y + track_elevation + ball_radius// Z + + 0.5*track_len*sin(track_incl) + + 0.5*track_height*cos(track_incl)); + dMatrix3 r = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0}; + dBodySetRotation(b, r); + dBodySetLinearVel(b, 0, 0, 0); + dBodySetAngularVel(b, 0, 0, 0); + +} + + +void resetSim() +{ + resetBall(ball1_body, 0); + resetBall(ball2_body, 1); +} + + +void start() +{ + dAllocateODEDataForThread(dAllocateMaskAll); + + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + contact_group = dJointGroupCreate(0); + + space = dSimpleSpaceCreate (0); + + + // first, the ground plane + // it has to coincide with the plane we have in drawstuff + ground = dCreatePlane(space, 0, 0, 1, 0); + + + // now a ball + dMass m; + dMassSetSphere(&m, 0.1, ball_radius); + + ball1_geom = dCreateSphere(space, ball_radius); + ball1_body = dBodyCreate(world); + dGeomSetBody(ball1_geom, ball1_body); + dBodySetMass(ball1_body, &m); + + ball2_geom = dCreateSphere(space, ball_radius); + ball2_body = dBodyCreate(world); + dGeomSetBody(ball2_geom, ball2_body); + dBodySetMass(ball2_body, &m); + + + + + // tracks made out of boxes + dGeomID trk; + dMatrix3 r1, r2, r3; + dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation}; + dMatrix3 s1, s2, s3; + dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation}; + + dRFromAxisAndAngle(r1, 1, 0, 0, track_angle); + dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl); + dMultiply0_333(r3, r2, r1); + + dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle); + dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl); + dMultiply0_333(s3, s2, s1); + + trk = dCreateBox(space, track_len, track_width, track_height); + dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]); + dGeomSetRotation(trk, r3); + + trk = dCreateBox(space, track_len, track_width, track_height); + dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]); + dGeomSetRotation(trk, s3); + + + + + + // tracks made out of trimesh + for (unsigned i=0; i 0.99 // about 8 degrees of difference + && + dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) { + // they are too close + closest_point = j; + //clog << "found close points: " << j << " and " << i << endl; + break; + } + } + + if (closest_point != i) { + // we discard one of the points + if (contacts[i].geom.depth > contacts[closest_point].geom.depth) + // the new point is deeper, copy it over closest_point + contacts[closest_point] = contacts[i]; + } else + contacts[new_n++] = contacts[i]; // the point is preserved + } + //clog << "reduced from " << n << " to " << new_n << endl; + n = new_n; + + for (int i=0; i +#include +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawLine dsDrawLineD +#define dsDrawSphere dsDrawSphereD +#endif + +dReal theta = M_PI / 4; +dReal ratio = 1, speed = 5, rho_1 = 1, rho_2 = 1, backlash = 0.1; +int mode = 0; + +dWorldID world; +dSpaceID space; +dBodyID body1, body2; +dGeomID geom1, geom2; +dJointID hinge1, hinge2, transmission; +dJointFeedback feedback; + +void setup() { + dMatrix3 R; + + switch (mode) { + case 0: + // Parallel axes. + + dBodySetPosition(body1, 1, 0, 1); + dBodySetPosition(body2, -1, 0, 1); + + dRSetIdentity (R); + dBodySetRotation (body1, R); + dBodySetRotation (body2, R); + + dJointSetHingeAnchor(hinge2, -1, 0, 1); + dJointSetHingeAxis(hinge2, 0, 0, 1); + + dJointSetHingeAnchor(hinge1, 1, 0, 1); + dJointSetHingeAxis(hinge1, 0, 0, 1); + + dJointSetTransmissionMode(transmission, dTransmissionParallelAxes); + dJointSetTransmissionRatio(transmission, ratio); + dJointSetTransmissionAnchor1(transmission, 1, 0, 1); + dJointSetTransmissionAnchor2(transmission, -1, 0, 1); + dJointSetTransmissionAxis(transmission, 0, 0, 1); + + break; + case 1: + // Intersecting axes. + + dBodySetPosition(body1, 1, 0, 1); + dBodySetPosition(body2, -1, 0, 2); + + dRSetIdentity (R); + dBodySetRotation (body1, R); + + dRFromZAxis (R, cos(theta), 0, sin(theta)); + dBodySetRotation (body2, R); + + dJointSetHingeAnchor(hinge2, -1, 0, 2); + dJointSetHingeAxis(hinge2, cos(theta), 0, sin(theta)); + + dJointSetHingeAnchor(hinge1, 1, 0, 1); + dJointSetHingeAxis(hinge1, 0, 0, 1); + + dJointSetTransmissionMode(transmission, dTransmissionIntersectingAxes); + dJointSetTransmissionAnchor1(transmission, 1, 0, 1); + dJointSetTransmissionAnchor2(transmission, -1, 0, 2); + dJointSetTransmissionAxis1(transmission, 0, 0, -1); + dJointSetTransmissionAxis2(transmission, cos(theta), 0, sin(theta)); + + break; + case 2: + // Chain. + + dBodySetPosition(body1, 2, 0, 1); + dBodySetPosition(body2, -2, 0, 1); + + dRSetIdentity (R); + dBodySetRotation (body1, R); + dBodySetRotation (body2, R); + + dJointSetHingeAnchor(hinge2, -2, 0, 1); + dJointSetHingeAxis(hinge2, 0, 0, 1); + + dJointSetHingeAnchor(hinge1, 2, 0, 1); + dJointSetHingeAxis(hinge1, 0, 0, 1); + + dJointSetTransmissionMode(transmission, dTransmissionChainDrive); + dJointSetTransmissionAnchor1(transmission, 2, 0, 1); + dJointSetTransmissionAnchor2(transmission, -2, 0, 1); + dJointSetTransmissionRadius1(transmission, rho_1); + dJointSetTransmissionRadius2(transmission, rho_2); + dJointSetTransmissionAxis(transmission, 0, 0, 1); + + break; + } + + dJointSetTransmissionBacklash(transmission, backlash); + + dJointSetHingeParam(hinge2, dParamVel, speed); + dJointSetHingeParam(hinge2, dParamFMax, 50); + + dJointSetHingeParam(hinge1, dParamVel, 0); + dJointSetHingeParam(hinge1, dParamFMax, 2); + + dBodySetLinearVel(body1, 0, 0, 0); + dBodySetLinearVel(body2, 0, 0, 0); + dBodySetAngularVel(body1, 0, 0, 0); + dBodySetAngularVel(body2, 0, 0, 0); +} + +void start() +{ + dMass mass; + + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetERP(world, 0.2); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetFiniteRotationMode(body1, 1); + dBodySetFiniteRotationMode(body2, 1); + + geom1 = dCreateCylinder(space, 0.2, 0.5); + dGeomSetBody(geom1, body1); + dMassSetCylinder(&mass, 100, 3, 0.2, 0.5); + dBodySetMass(body1, &mass); + + geom2 = dCreateCylinder(space, 0.2, 0.5); + dGeomSetBody(geom2, body2); + dMassSetCylinder(&mass, 100, 3, 0.2, 0.5); + dBodySetMass(body2, &mass); + + hinge1 = dJointCreateHinge(world, 0); + dJointAttach(hinge1, body1, 0); + + hinge2 = dJointCreateHinge(world, 0); + dJointAttach(hinge2, body2, 0); + + transmission = dJointCreateTransmission(world, 0); + dJointAttach(transmission, body1, body2); + dJointSetFeedback(transmission, &feedback); + + setup(); + + // initial camera position + static float xyz[3] = {1.15,-2.78,4.1}; + static float hpr[3] = {105,-45.5,0}; + dsSetViewpoint (xyz,hpr); + + fprintf (stderr, + "The green wheel is driving the red one. To control it use the following:\n" + " '[' : decrease wheel ratio\n" + " ']' : increase wheel ratio\n" + " ',' : decrease driving wheel speed\n" + " '.' : increase driving wheel speed\n" + " '-' : decrease backlash\n" + " '=' : increase backlash\n" + " '1' : switch to parallel axes gears mode\n" + " '2' : switch to intersecting axes gears mode\n" + " '3' : switch to chain (or belt) mode\n" +); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dCylinderClass: + { + dReal length, radius; + + if (g == geom1) { + dsSetColorAlpha(1, 0, 0, 1); + } else { + dsSetColorAlpha(0, 1, 0, 1); + } + + dsSetTexture (DS_WOOD); + dGeomCylinderGetParams(g, &radius, &length); + dsDrawCylinder(pos, rot, length, radius); + break; + } + + default: + { + abort(); + } + } +} + +void simLoop(int pause) +{ + if (!pause) { + + const dReal step = 0.003; + const unsigned nsteps = 4; + + for (unsigned i=0; i 0.125) { + ratio *= 0.5; + + fprintf (stderr, "Gear ratio set to %.3f.\n", ratio); + } + break; + case dTransmissionIntersectingAxes: + if (theta > 0.1) { + theta -= 0.1; + + fprintf (stderr, "Gear angle set to %.3f deg.\n", + theta / M_PI * 180); + } + break; + case dTransmissionChainDrive: + if (rho_2 > 0.125) { + rho_2 /= 2; + + fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1); + } + break; + } + + setup(); + } else if (cmd == ']') { + switch(mode) { + case dTransmissionParallelAxes: + if (ratio < 8) { + ratio *= 2; + + fprintf (stderr, "Gear ratio set to %.3f.\n", ratio); + } + break; + case dTransmissionIntersectingAxes: + if (theta < 0.9) { + theta += 0.1; + + fprintf (stderr, "Gear angle set to %.3f deg.\n", + theta / M_PI * 180); + } + break; + case dTransmissionChainDrive: + if (rho_2 < 2) { + rho_2 *= 2; + + fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1); + } + break; + } + + setup(); + } else if (cmd == '.') { + speed += 5; + + fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed); + + dJointSetHingeParam(hinge2, dParamVel, speed); + } else if (cmd == ',') { + speed -= 5; + + fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed); + + dJointSetHingeParam(hinge2, dParamVel, speed); + } else if (cmd == '/') { + if (dJointGetHingeParam(hinge2, dParamFMax) > 0) { + dJointSetHingeParam(hinge2, dParamFMax, 0); + } else { + dJointSetHingeParam(hinge2, dParamFMax, 50); + } + + } else if (cmd == '-') { + backlash -= 0.1; + + fprintf (stderr, "Backlash set to %g m.\n", backlash); + + dJointSetTransmissionBacklash(transmission, backlash); + } else if (cmd == '=') { + backlash += 0.1; + + fprintf (stderr, "Backlash set to %g m.\n", backlash); + + dJointSetTransmissionBacklash(transmission, backlash); + } else if (cmd == '1') { + mode = dTransmissionParallelAxes; + setup(); + } else if (cmd == '2') { + mode = dTransmissionIntersectingAxes; + setup(); + } else if (cmd == '3') { + mode = dTransmissionChainDrive; + setup(); + } +} + +int main(int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = stop; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + + // run demo + dsSimulationLoop (argc, argv, 800, 600, &fn); + + dCloseODE(); + return 0; +} diff --git a/libs/ode-0.16.1/ode/demo/demo_trimesh.cpp b/libs/ode-0.16.1/ode/demo/demo_trimesh.cpp new file mode 100644 index 0000000..1c53334 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_trimesh.cpp @@ -0,0 +1,605 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh test by Erwin de Vries + +#include +#include +#include "texturepath.h" + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +//<---- Convex Object +static const dReal planes[] = // planes for a cube +{ + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ +}; +static const unsigned int planecount=6; + +static const dReal points[] = // points for a cube +{ + 0.25f,0.25f,0.25f, + -0.25f,0.25f,0.25f, + + 0.25f,-0.25f,0.25f, + -0.25f,-0.25f,0.25f, + + 0.25f,0.25f,-0.25f, + -0.25f,0.25f,-0.25f, + + 0.25f,-0.25f,-0.25f, + -0.25f,-0.25f,-0.25f, +}; +static const unsigned int pointcount=8; + +static const unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawLine dsDrawLineD +#define dsDrawTriangle dsDrawTriangleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 200 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 40 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? + +#define VertexCount 5 +#define IndexCount 12 + +static dVector3 Size; +static float Vertices[VertexCount][3]; +static dTriIndex Indices[IndexCount]; + +static dGeomID TriMesh; +static dGeomID Ray; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,j,k; + dReal sides[3]; + dMass m; + bool setBody = false; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' + /* || cmd == 'l' */) { + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition (obj[i].body, + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else { + dReal maxheight = 0; + for (k=0; k maxheight) maxheight = pos[2]; + } + dBodySetPosition (obj[i].body, 0,0,maxheight+1); + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*)(dsizeint)i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } +/* + // cylinder option not yet implemented + else if (cmd == 'l') { + sides[1] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } +*/ + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + else if (cmd == 'x') { + + setBody = true; + // start accumulating masses for the composite geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for composite geometries + dMatrix3 drot[GPB]; + + // set random delta positions + for (j=0; j= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } else if (type == dConvexClass) { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } +/* + // cylinder option not yet implemented + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } +*/ + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + if (!pause) dWorldStep (world,0.05); + //if (!pause) dWorldStepFast (world,0.05, 1); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i +/* + This is a description of a convex icosahedron, to test + the convex collision detection. +*/ +unsigned int Sphere_pointcount = 42; +unsigned int Sphere_planecount = 80; +dReal Sphere_points[126]={ +0.000000,0.000000,-0.300000, +0.217080,-0.157716,-0.134164, +-0.082915,-0.255192,-0.134164, +-0.268327,0.000000,-0.134164, +-0.082915,0.255192,-0.134164, +0.217080,0.157716,-0.134164, +0.082915,-0.255192,0.134164, +-0.217080,-0.157716,0.134164, +-0.217080,0.157716,0.134164, +0.082915,0.255192,0.134164, +0.268327,0.000000,0.134164, +0.000000,0.000000,0.300000, +0.127597,-0.092703,-0.255196, +-0.048737,-0.149999,-0.255196, +0.078861,-0.242703,-0.157721, +0.127597,0.092703,-0.255196, +0.255194,0.000000,-0.157721, +-0.157719,0.000000,-0.255195, +-0.206457,-0.149999,-0.157721, +-0.048737,0.149999,-0.255196, +-0.206457,0.149999,-0.157721, +0.078861,0.242703,-0.157721, +0.285317,0.092704,0.000000, +0.285317,-0.092704,0.000000, +0.176336,-0.242705,0.000000, +0.000000,-0.300000,0.000000, +-0.176336,-0.242705,0.000000, +-0.285317,-0.092704,0.000000, +-0.285317,0.092704,0.000000, +-0.176336,0.242705,0.000000, +0.000000,0.300000,0.000000, +0.176336,0.242705,0.000000, +0.206457,-0.149999,0.157721, +-0.078861,-0.242703,0.157721, +-0.255194,0.000000,0.157721, +-0.078861,0.242703,0.157721, +0.206457,0.149999,0.157721, +0.157719,0.000000,0.255195, +0.048737,-0.149999,0.255196, +-0.127597,-0.092703,0.255196, +-0.127597,0.092703,0.255196, +0.048737,0.149999,0.255196 +}; +unsigned int Sphere_polygons[]={ +3,14,12,1, +3,12,14,13, +3,2,13,14, +3,13,0,12, +3,16,1,12, +3,12,15,16, +3,5,16,15, +3,12,0,15, +3,18,13,2, +3,13,18,17, +3,3,17,18, +3,17,0,13, +3,20,17,3, +3,17,20,19, +3,4,19,20, +3,19,0,17, +3,21,19,4, +3,19,21,15, +3,5,15,21, +3,15,0,19, +3,23,1,16, +3,16,22,23, +3,10,23,22, +3,22,16,5, +3,25,2,14, +3,14,24,25, +3,6,25,24, +3,24,14,1, +3,27,3,18, +3,18,26,27, +3,7,27,26, +3,26,18,2, +3,29,4,20, +3,20,28,29, +3,8,29,28, +3,28,20,3, +3,31,5,21, +3,21,30,31, +3,9,31,30, +3,30,21,4, +3,32,23,10, +3,23,32,24, +3,6,24,32, +3,24,1,23, +3,33,25,6, +3,25,33,26, +3,7,26,33, +3,26,2,25, +3,34,27,7, +3,27,34,28, +3,8,28,34, +3,28,3,27, +3,35,29,8, +3,29,35,30, +3,9,30,35, +3,30,4,29, +3,36,31,9, +3,31,36,22, +3,10,22,36, +3,22,5,31, +3,38,6,32, +3,32,37,38, +3,11,38,37, +3,37,32,10, +3,39,7,33, +3,33,38,39, +3,11,39,38, +3,38,33,6, +3,40,8,34, +3,34,39,40, +3,11,40,39, +3,39,34,7, +3,41,9,35, +3,35,40,41, +3,11,41,40, +3,40,35,8, +3,37,10,36, +3,36,41,37, +3,11,37,41, +3,41,36,9, +}; +dReal Sphere_planes[]={ +0.471317,-0.583121,-0.661687,0.283056, +0.187594,-0.577345,-0.794658,0.280252, +-0.038547,-0.748789,-0.661687,0.283056, +0.102381,-0.315090,-0.943523,0.283057, +0.700228,-0.268049,-0.661688,0.283056, +0.607060,0.000000,-0.794656,0.280252, +0.700228,0.268049,-0.661688,0.283056, +0.331305,0.000000,-0.943524,0.283057, +-0.408939,-0.628443,-0.661686,0.283056, +-0.491119,-0.356821,-0.794657,0.280252, +-0.724044,-0.194735,-0.661694,0.283057, +-0.268034,-0.194737,-0.943523,0.283057, +-0.724044,0.194735,-0.661694,0.283057, +-0.491119,0.356821,-0.794657,0.280252, +-0.408939,0.628443,-0.661686,0.283056, +-0.268034,0.194737,-0.943523,0.283057, +-0.038547,0.748789,-0.661687,0.283056, +0.187594,0.577345,-0.794658,0.280252, +0.471317,0.583121,-0.661687,0.283056, +0.102381,0.315090,-0.943523,0.283057, +0.904981,-0.268049,-0.330393,0.283056, +0.982246,0.000000,-0.187599,0.280252, +0.992077,0.000000,0.125631,0.283057, +0.904981,0.268049,-0.330393,0.283056, +0.024726,-0.943519,-0.330396,0.283056, +0.303531,-0.934171,-0.187598,0.280251, +0.306568,-0.943519,0.125651,0.283056, +0.534590,-0.777851,-0.330395,0.283056, +-0.889698,-0.315092,-0.330386,0.283056, +-0.794656,-0.577348,-0.187595,0.280251, +-0.802607,-0.583125,0.125648,0.283055, +-0.574584,-0.748793,-0.330397,0.283055, +-0.574584,0.748793,-0.330397,0.283055, +-0.794656,0.577348,-0.187595,0.280251, +-0.802607,0.583125,0.125648,0.283055, +-0.889698,0.315092,-0.330386,0.283056, +0.534590,0.777851,-0.330395,0.283056, +0.303531,0.934171,-0.187598,0.280251, +0.306568,0.943519,0.125651,0.283056, +0.024726,0.943519,-0.330396,0.283056, +0.889698,-0.315092,0.330386,0.283056, +0.794656,-0.577348,0.187595,0.280251, +0.574584,-0.748793,0.330397,0.283055, +0.802607,-0.583125,-0.125648,0.283055, +-0.024726,-0.943519,0.330396,0.283055, +-0.303531,-0.934171,0.187598,0.280251, +-0.534590,-0.777851,0.330395,0.283056, +-0.306568,-0.943519,-0.125651,0.283056, +-0.904981,-0.268049,0.330393,0.283056, +-0.982246,0.000000,0.187599,0.280252, +-0.904981,0.268049,0.330393,0.283056, +-0.992077,0.000000,-0.125631,0.283057, +-0.534590,0.777851,0.330395,0.283056, +-0.303531,0.934171,0.187598,0.280251, +-0.024726,0.943519,0.330396,0.283055, +-0.306568,0.943519,-0.125651,0.283056, +0.574584,0.748793,0.330397,0.283055, +0.794656,0.577348,0.187595,0.280251, +0.889698,0.315092,0.330386,0.283056, +0.802607,0.583125,-0.125648,0.283055, +0.408939,-0.628443,0.661686,0.283056, +0.491119,-0.356821,0.794657,0.280252, +0.268034,-0.194737,0.943523,0.283057, +0.724044,-0.194735,0.661694,0.283057, +-0.471317,-0.583121,0.661687,0.283056, +-0.187594,-0.577345,0.794658,0.280252, +-0.102381,-0.315090,0.943523,0.283057, +0.038547,-0.748789,0.661687,0.283056, +-0.700228,0.268049,0.661688,0.283056, +-0.607060,0.000000,0.794656,0.280252, +-0.331305,0.000000,0.943524,0.283057, +-0.700228,-0.268049,0.661688,0.283056, +0.038547,0.748789,0.661687,0.283056, +-0.187594,0.577345,0.794658,0.280252, +-0.102381,0.315090,0.943523,0.283057, +-0.471317,0.583121,0.661687,0.283056, +0.724044,0.194735,0.661694,0.283057, +0.491119,0.356821,0.794657,0.280252, +0.268034,0.194737,0.943523,0.283057, +0.408939,0.628443,0.661686,0.283056, +}; + diff --git a/libs/ode-0.16.1/ode/demo/texturepath.h b/libs/ode-0.16.1/ode/demo/texturepath.h new file mode 100644 index 0000000..5815138 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/texturepath.h @@ -0,0 +1,26 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef DRAWSTUFF_TEXTURE_PATH +#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures" +#endif + diff --git a/libs/ode-0.16.1/ode/demo/world_geom3.h b/libs/ode-0.16.1/ode/demo/world_geom3.h new file mode 100644 index 0000000..27cd69e --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/world_geom3.h @@ -0,0 +1,9 @@ +// mesh for a world model, to be used with test_cyl.cpp + +static 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}; + +static 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}; + + +static dTriIndex 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}; + -- cgit v1.2.1