From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- libraries/ode-0.9/ode/demo/Makefile.am | 254 +++ libraries/ode-0.9/ode/demo/Makefile.in | 953 +++++++++ libraries/ode-0.9/ode/demo/basket_geom.h | 13 + libraries/ode-0.9/ode/demo/demo_I.cpp | 254 +++ libraries/ode-0.9/ode/demo/demo_basket.cpp | 278 +++ libraries/ode-0.9/ode/demo/demo_boxstack.cpp | 577 ++++++ libraries/ode-0.9/ode/demo/demo_buggy.cpp | 325 +++ libraries/ode-0.9/ode/demo/demo_chain1.c | 171 ++ libraries/ode-0.9/ode/demo/demo_chain2.cpp | 165 ++ libraries/ode-0.9/ode/demo/demo_collision.cpp | 1371 +++++++++++++ libraries/ode-0.9/ode/demo/demo_convex_cd.cpp | 195 ++ libraries/ode-0.9/ode/demo/demo_crash.cpp | 635 ++++++ libraries/ode-0.9/ode/demo/demo_cyl.cpp | 318 +++ libraries/ode-0.9/ode/demo/demo_cylvssphere.cpp | 238 +++ libraries/ode-0.9/ode/demo/demo_feedback.cpp | 314 +++ libraries/ode-0.9/ode/demo/demo_friction.cpp | 206 ++ libraries/ode-0.9/ode/demo/demo_heightfield.cpp | 2132 ++++++++++++++++++++ libraries/ode-0.9/ode/demo/demo_hinge.cpp | 166 ++ libraries/ode-0.9/ode/demo/demo_jointPR.cpp | 377 ++++ libraries/ode-0.9/ode/demo/demo_joints.cpp | 1092 ++++++++++ libraries/ode-0.9/ode/demo/demo_motor.cpp | 210 ++ libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp | 1944 ++++++++++++++++++ libraries/ode-0.9/ode/demo/demo_ode.cpp | 1128 +++++++++++ libraries/ode-0.9/ode/demo/demo_plane2d.cpp | 268 +++ libraries/ode-0.9/ode/demo/demo_slider.cpp | 172 ++ libraries/ode-0.9/ode/demo/demo_space.cpp | 233 +++ libraries/ode-0.9/ode/demo/demo_space_stress.cpp | 435 ++++ libraries/ode-0.9/ode/demo/demo_step.cpp | 193 ++ libraries/ode-0.9/ode/demo/demo_trimesh.cpp | 537 +++++ libraries/ode-0.9/ode/demo/world_geom3.h | 9 + 30 files changed, 15163 insertions(+) create mode 100644 libraries/ode-0.9/ode/demo/Makefile.am create mode 100644 libraries/ode-0.9/ode/demo/Makefile.in create mode 100644 libraries/ode-0.9/ode/demo/basket_geom.h create mode 100644 libraries/ode-0.9/ode/demo/demo_I.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_basket.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_boxstack.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_buggy.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_chain1.c create mode 100644 libraries/ode-0.9/ode/demo/demo_chain2.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_collision.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_convex_cd.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_crash.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_cyl.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_cylvssphere.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_feedback.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_friction.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_heightfield.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_hinge.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_jointPR.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_joints.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_motor.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_ode.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_plane2d.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_slider.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_space.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_space_stress.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_step.cpp create mode 100644 libraries/ode-0.9/ode/demo/demo_trimesh.cpp create mode 100644 libraries/ode-0.9/ode/demo/world_geom3.h (limited to 'libraries/ode-0.9/ode/demo') diff --git a/libraries/ode-0.9/ode/demo/Makefile.am b/libraries/ode-0.9/ode/demo/Makefile.am new file mode 100644 index 0000000..361ad53 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/Makefile.am @@ -0,0 +1,254 @@ +AM_CXXFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include + +noinst_PROGRAMS=demo_collision \ + demo_slider \ + demo_feedback \ + demo_crash \ + demo_space \ + demo_I \ + demo_friction \ + demo_space_stress \ + demo_boxstack demo_hinge \ + demo_step \ + demo_buggy \ + demo_joints \ + demo_motor \ + demo_chain1 \ + demo_chain2 \ + demo_cylvssphere \ + demo_ode \ + demo_plane2d \ + demo_heightfield \ + demo_convex_cd \ + demo_jointPR +if TRIMESH +noinst_PROGRAMS+= demo_trimesh demo_moving_trimesh demo_basket demo_cyl +endif +demo_collision_SOURCES= demo_collision.cpp +demo_slider_SOURCES= demo_slider.cpp +demo_feedback_SOURCES= demo_feedback.cpp +demo_crash_SOURCES= demo_crash.cpp +demo_space_SOURCES= demo_space.cpp +demo_I_SOURCES= demo_I.cpp +demo_friction_SOURCES= demo_friction.cpp +demo_space_stress_SOURCES= demo_space_stress.cpp +demo_boxstack_SOURCES= demo_boxstack.cpp +demo_hinge_SOURCES= demo_hinge.cpp +demo_step_SOURCES= demo_step.cpp +demo_buggy_SOURCES= demo_buggy.cpp +demo_cyl_SOURCES= demo_cyl.cpp world_geom3.h +demo_cylvssphere_SOURCES= demo_cylvssphere.cpp +demo_joints_SOURCES= demo_joints.cpp +demo_jointPR_SOURCES= demo_jointPR.cpp +demo_motor_SOURCES= demo_motor.cpp +demo_chain1_SOURCES= demo_chain1.c +demo_chain2_SOURCES= demo_chain2.cpp +demo_ode_SOURCES= demo_ode.cpp +demo_plane2d_SOURCES= demo_plane2d.cpp +demo_heightfield_SOURCES= demo_heightfield.cpp +demo_convex_cd_SOURCES= demo_convex_cd.cpp +demo_collision_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_slider_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_feedback_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_crash_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_space_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_I_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_friction_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_space_stress_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_boxstack_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_hinge_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_step_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_buggy_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_cyl_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_cylvssphere_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_joints_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_jointPR_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_motor_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_chain1_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_chain2_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_ode_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_plane2d_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_heightfield_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ +demo_convex_cd_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_collision_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_slider_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_feedback_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_crash_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_space_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_I_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_friction_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_space_stress_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_boxstack_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_hinge_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_step_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_buggy_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_cyl_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_cylvssphere_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_joints_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_jointPR_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_motor_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_chain1_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_chain2_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_ode_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_plane2d_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_heightfield_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_convex_cd_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a + +if TRIMESH +demo_trimesh_SOURCES= demo_trimesh.cpp +demo_moving_trimesh_SOURCES= demo_moving_trimesh.cpp +demo_basket_SOURCES= demo_basket.cpp +demo_trimesh_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ \ + @GL_LIBS@ @LIBS@ +demo_moving_trimesh_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ \ + @GL_LIBS@ @LIBS@ +demo_basket_LDFLAGS= -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ \ + @GL_LIBS@ @LIBS@ +demo_trimesh_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a +demo_moving_trimesh_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a + +demo_basket_DEPENDENCIES= $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a + +endif + +demo_ode_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_plane2d_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_heightfield_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_chain2_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_chain1_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_joints_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_jointPR_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_motor_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_buggy_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_cyl_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_cylvssphere_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_step_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_hinge_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_boxstack_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_space_stress_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_friction_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_I_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_space_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_crash_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_slider_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_feedback_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_collision_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_convex_cd_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ + + +if TRIMESH +demo_trimesh_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_moving_trimesh_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_basket_LDADD= -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +endif + + +if WIN32 +resources.o: ../../drawstuff/src/resources.rc ../../drawstuff/src/resource.h + @WINDRES@ ../../drawstuff/src/resources.rc -o resources.o +demo_ode_LDADD+= resources.o +demo_heightfield_LDADD+= resources.o +demo_chain2_LDADD+= resources.o +demo_chain1_LDADD+= resources.o +demo_joints_LDADD+= resources.o +demo_jointPR_LDADD+= resources.o +demo_motor_LDADD+= resources.o +demo_buggy_LDADD+= resources.o +demo_cyl_LDADD+= resources.o +demo_cylvssphere_LDADD+= resources.o +demo_step_LDADD+= resources.o +demo_hinge_LDADD+= resources.o +demo_boxstack_LDADD+= resources.o +demo_space_stress_LDADD+= resources.o +demo_friction_LDADD+= resources.o +demo_I_LDADD+= resources.o +demo_space_LDADD+= resources.o +demo_crash_LDADD+= resources.o +demo_slider_LDADD+= resources.o +demo_feedback_LDADD+= resources.o +demo_collision_LDADD+= resources.o +demo_convex_cd_LDADD+= resources.o +demo_ode_DEPENDENCIES+= resources.o +demo_chain2_DEPENDENCIES+= resources.o +demo_chain1_DEPENDENCIES+= resources.o +demo_joints_DEPENDENCIES+= resources.o +demo_jointPR_DEPENDENCIES+= resources.o +demo_motor_DEPENDENCIES+= resources.o +demo_buggy_DEPENDENCIES+= resources.o +demo_cyl_DEPENDENCIES+= resources.o +demo_cylvssphere_DEPENDENCIES+= resources.o +demo_step_DEPENDENCIES+= resources.o +demo_hinge_DEPENDENCIES+= resources.o +demo_boxstack_DEPENDENCIES+= resources.o +demo_space_stress_DEPENDENCIES+= resources.o +demo_friction_DEPENDENCIES+= resources.o +demo_I_DEPENDENCIES+= resources.o +demo_space_DEPENDENCIES+= resources.o +demo_crash_DEPENDENCIES+= resources.o +demo_slider_DEPENDENCIES+= resources.o +demo_feedback_DEPENDENCIES+= resources.o +demo_collision_DEPENDENCIES+= resources.o +demo_convex_cd_DEPENDENCIES+= resources.o + +if TRIMESH +demo_trimesh_LDADD+= resources.o +demo_moving_trimesh_LDADD+= resources.o +demo_trimesh_DEPENDENCIES+= resources.o +demo_moving_trimesh_DEPENDENCIES+= resources.o +demo_basket_DEPENDENCIES+= resources.o +endif +endif diff --git a/libraries/ode-0.9/ode/demo/Makefile.in b/libraries/ode-0.9/ode/demo/Makefile.in new file mode 100644 index 0000000..8e73f18 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/Makefile.in @@ -0,0 +1,953 @@ +# Makefile.in generated by automake 1.10 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002, +# 2003, 2004, 2005, 2006 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@ +pkgdatadir = $(datadir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkgincludedir = $(includedir)/@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@ +target_triplet = @target@ +noinst_PROGRAMS = demo_collision$(EXEEXT) demo_slider$(EXEEXT) \ + demo_feedback$(EXEEXT) demo_crash$(EXEEXT) demo_space$(EXEEXT) \ + demo_I$(EXEEXT) demo_friction$(EXEEXT) \ + demo_space_stress$(EXEEXT) demo_boxstack$(EXEEXT) \ + demo_hinge$(EXEEXT) demo_step$(EXEEXT) demo_buggy$(EXEEXT) \ + demo_joints$(EXEEXT) demo_motor$(EXEEXT) demo_chain1$(EXEEXT) \ + demo_chain2$(EXEEXT) demo_cylvssphere$(EXEEXT) \ + demo_ode$(EXEEXT) demo_plane2d$(EXEEXT) \ + demo_heightfield$(EXEEXT) demo_convex_cd$(EXEEXT) \ + demo_jointPR$(EXEEXT) $(am__EXEEXT_1) +@TRIMESH_TRUE@am__append_1 = demo_trimesh demo_moving_trimesh demo_basket demo_cyl +@WIN32_TRUE@am__append_2 = resources.o +@WIN32_TRUE@am__append_3 = resources.o +@WIN32_TRUE@am__append_4 = resources.o +@WIN32_TRUE@am__append_5 = resources.o +@WIN32_TRUE@am__append_6 = resources.o +@WIN32_TRUE@am__append_7 = resources.o +@WIN32_TRUE@am__append_8 = resources.o +@WIN32_TRUE@am__append_9 = resources.o +@WIN32_TRUE@am__append_10 = resources.o +@WIN32_TRUE@am__append_11 = resources.o +@WIN32_TRUE@am__append_12 = resources.o +@WIN32_TRUE@am__append_13 = resources.o +@WIN32_TRUE@am__append_14 = resources.o +@WIN32_TRUE@am__append_15 = resources.o +@WIN32_TRUE@am__append_16 = resources.o +@WIN32_TRUE@am__append_17 = resources.o +@WIN32_TRUE@am__append_18 = resources.o +@WIN32_TRUE@am__append_19 = resources.o +@WIN32_TRUE@am__append_20 = resources.o +@WIN32_TRUE@am__append_21 = resources.o +@WIN32_TRUE@am__append_22 = resources.o +@WIN32_TRUE@am__append_23 = resources.o +@WIN32_TRUE@am__append_24 = resources.o +@WIN32_TRUE@am__append_25 = resources.o +@WIN32_TRUE@am__append_26 = resources.o +@WIN32_TRUE@am__append_27 = resources.o +@WIN32_TRUE@am__append_28 = resources.o +@WIN32_TRUE@am__append_29 = resources.o +@WIN32_TRUE@am__append_30 = resources.o +@WIN32_TRUE@am__append_31 = resources.o +@WIN32_TRUE@am__append_32 = resources.o +@WIN32_TRUE@am__append_33 = resources.o +@WIN32_TRUE@am__append_34 = resources.o +@WIN32_TRUE@am__append_35 = resources.o +@WIN32_TRUE@am__append_36 = resources.o +@WIN32_TRUE@am__append_37 = resources.o +@WIN32_TRUE@am__append_38 = resources.o +@WIN32_TRUE@am__append_39 = resources.o +@WIN32_TRUE@am__append_40 = resources.o +@WIN32_TRUE@am__append_41 = resources.o +@WIN32_TRUE@am__append_42 = resources.o +@WIN32_TRUE@am__append_43 = resources.o +@WIN32_TRUE@am__append_44 = resources.o +@TRIMESH_TRUE@@WIN32_TRUE@am__append_45 = resources.o +@TRIMESH_TRUE@@WIN32_TRUE@am__append_46 = resources.o +@TRIMESH_TRUE@@WIN32_TRUE@am__append_47 = resources.o +@TRIMESH_TRUE@@WIN32_TRUE@am__append_48 = resources.o +@TRIMESH_TRUE@@WIN32_TRUE@am__append_49 = resources.o +subdir = ode/demo +DIST_COMMON = $(srcdir)/Makefile.am $(srcdir)/Makefile.in +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/configure.in +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/include/ode/config.h +CONFIG_CLEAN_FILES = +@TRIMESH_TRUE@am__EXEEXT_1 = demo_trimesh$(EXEEXT) \ +@TRIMESH_TRUE@ demo_moving_trimesh$(EXEEXT) \ +@TRIMESH_TRUE@ demo_basket$(EXEEXT) demo_cyl$(EXEEXT) +PROGRAMS = $(noinst_PROGRAMS) +am_demo_I_OBJECTS = demo_I.$(OBJEXT) +demo_I_OBJECTS = $(am_demo_I_OBJECTS) +demo_I_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) $(demo_I_LDFLAGS) \ + $(LDFLAGS) -o $@ +am__demo_basket_SOURCES_DIST = demo_basket.cpp +@TRIMESH_TRUE@am_demo_basket_OBJECTS = demo_basket.$(OBJEXT) +demo_basket_OBJECTS = $(am_demo_basket_OBJECTS) +demo_basket_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_basket_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_boxstack_OBJECTS = demo_boxstack.$(OBJEXT) +demo_boxstack_OBJECTS = $(am_demo_boxstack_OBJECTS) +demo_boxstack_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_boxstack_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_buggy_OBJECTS = demo_buggy.$(OBJEXT) +demo_buggy_OBJECTS = $(am_demo_buggy_OBJECTS) +demo_buggy_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_buggy_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_chain1_OBJECTS = demo_chain1.$(OBJEXT) +demo_chain1_OBJECTS = $(am_demo_chain1_OBJECTS) +demo_chain1_LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) \ + $(demo_chain1_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_chain2_OBJECTS = demo_chain2.$(OBJEXT) +demo_chain2_OBJECTS = $(am_demo_chain2_OBJECTS) +demo_chain2_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_chain2_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_collision_OBJECTS = demo_collision.$(OBJEXT) +demo_collision_OBJECTS = $(am_demo_collision_OBJECTS) +demo_collision_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_collision_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_convex_cd_OBJECTS = demo_convex_cd.$(OBJEXT) +demo_convex_cd_OBJECTS = $(am_demo_convex_cd_OBJECTS) +demo_convex_cd_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_convex_cd_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_crash_OBJECTS = demo_crash.$(OBJEXT) +demo_crash_OBJECTS = $(am_demo_crash_OBJECTS) +demo_crash_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_crash_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_cyl_OBJECTS = demo_cyl.$(OBJEXT) +demo_cyl_OBJECTS = $(am_demo_cyl_OBJECTS) +demo_cyl_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_cyl_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_cylvssphere_OBJECTS = demo_cylvssphere.$(OBJEXT) +demo_cylvssphere_OBJECTS = $(am_demo_cylvssphere_OBJECTS) +demo_cylvssphere_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_cylvssphere_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_feedback_OBJECTS = demo_feedback.$(OBJEXT) +demo_feedback_OBJECTS = $(am_demo_feedback_OBJECTS) +demo_feedback_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_feedback_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_friction_OBJECTS = demo_friction.$(OBJEXT) +demo_friction_OBJECTS = $(am_demo_friction_OBJECTS) +demo_friction_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_friction_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_heightfield_OBJECTS = demo_heightfield.$(OBJEXT) +demo_heightfield_OBJECTS = $(am_demo_heightfield_OBJECTS) +demo_heightfield_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_heightfield_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_hinge_OBJECTS = demo_hinge.$(OBJEXT) +demo_hinge_OBJECTS = $(am_demo_hinge_OBJECTS) +demo_hinge_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_hinge_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_jointPR_OBJECTS = demo_jointPR.$(OBJEXT) +demo_jointPR_OBJECTS = $(am_demo_jointPR_OBJECTS) +demo_jointPR_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_jointPR_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_joints_OBJECTS = demo_joints.$(OBJEXT) +demo_joints_OBJECTS = $(am_demo_joints_OBJECTS) +demo_joints_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_joints_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_motor_OBJECTS = demo_motor.$(OBJEXT) +demo_motor_OBJECTS = $(am_demo_motor_OBJECTS) +demo_motor_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_motor_LDFLAGS) $(LDFLAGS) -o $@ +am__demo_moving_trimesh_SOURCES_DIST = demo_moving_trimesh.cpp +@TRIMESH_TRUE@am_demo_moving_trimesh_OBJECTS = \ +@TRIMESH_TRUE@ demo_moving_trimesh.$(OBJEXT) +demo_moving_trimesh_OBJECTS = $(am_demo_moving_trimesh_OBJECTS) +demo_moving_trimesh_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_moving_trimesh_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_ode_OBJECTS = demo_ode.$(OBJEXT) +demo_ode_OBJECTS = $(am_demo_ode_OBJECTS) +demo_ode_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_ode_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_plane2d_OBJECTS = demo_plane2d.$(OBJEXT) +demo_plane2d_OBJECTS = $(am_demo_plane2d_OBJECTS) +demo_plane2d_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_plane2d_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_slider_OBJECTS = demo_slider.$(OBJEXT) +demo_slider_OBJECTS = $(am_demo_slider_OBJECTS) +demo_slider_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_slider_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_space_OBJECTS = demo_space.$(OBJEXT) +demo_space_OBJECTS = $(am_demo_space_OBJECTS) +demo_space_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_space_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_space_stress_OBJECTS = demo_space_stress.$(OBJEXT) +demo_space_stress_OBJECTS = $(am_demo_space_stress_OBJECTS) +demo_space_stress_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_space_stress_LDFLAGS) $(LDFLAGS) -o $@ +am_demo_step_OBJECTS = demo_step.$(OBJEXT) +demo_step_OBJECTS = $(am_demo_step_OBJECTS) +demo_step_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_step_LDFLAGS) $(LDFLAGS) -o $@ +am__demo_trimesh_SOURCES_DIST = demo_trimesh.cpp +@TRIMESH_TRUE@am_demo_trimesh_OBJECTS = demo_trimesh.$(OBJEXT) +demo_trimesh_OBJECTS = $(am_demo_trimesh_OBJECTS) +demo_trimesh_LINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) \ + $(demo_trimesh_LDFLAGS) $(LDFLAGS) -o $@ +DEFAULT_INCLUDES = -I. -I$(top_builddir)/include/ode@am__isrc@ +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \ + $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) +CCLD = $(CC) +LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@ +CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \ + $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) +CXXLD = $(CXX) +CXXLINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) \ + -o $@ +SOURCES = $(demo_I_SOURCES) $(demo_basket_SOURCES) \ + $(demo_boxstack_SOURCES) $(demo_buggy_SOURCES) \ + $(demo_chain1_SOURCES) $(demo_chain2_SOURCES) \ + $(demo_collision_SOURCES) $(demo_convex_cd_SOURCES) \ + $(demo_crash_SOURCES) $(demo_cyl_SOURCES) \ + $(demo_cylvssphere_SOURCES) $(demo_feedback_SOURCES) \ + $(demo_friction_SOURCES) $(demo_heightfield_SOURCES) \ + $(demo_hinge_SOURCES) $(demo_jointPR_SOURCES) \ + $(demo_joints_SOURCES) $(demo_motor_SOURCES) \ + $(demo_moving_trimesh_SOURCES) $(demo_ode_SOURCES) \ + $(demo_plane2d_SOURCES) $(demo_slider_SOURCES) \ + $(demo_space_SOURCES) $(demo_space_stress_SOURCES) \ + $(demo_step_SOURCES) $(demo_trimesh_SOURCES) +DIST_SOURCES = $(demo_I_SOURCES) $(am__demo_basket_SOURCES_DIST) \ + $(demo_boxstack_SOURCES) $(demo_buggy_SOURCES) \ + $(demo_chain1_SOURCES) $(demo_chain2_SOURCES) \ + $(demo_collision_SOURCES) $(demo_convex_cd_SOURCES) \ + $(demo_crash_SOURCES) $(demo_cyl_SOURCES) \ + $(demo_cylvssphere_SOURCES) $(demo_feedback_SOURCES) \ + $(demo_friction_SOURCES) $(demo_heightfield_SOURCES) \ + $(demo_hinge_SOURCES) $(demo_jointPR_SOURCES) \ + $(demo_joints_SOURCES) $(demo_motor_SOURCES) \ + $(am__demo_moving_trimesh_SOURCES_DIST) $(demo_ode_SOURCES) \ + $(demo_plane2d_SOURCES) $(demo_slider_SOURCES) \ + $(demo_space_SOURCES) $(demo_space_stress_SOURCES) \ + $(demo_step_SOURCES) $(am__demo_trimesh_SOURCES_DIST) +ETAGS = etags +CTAGS = ctags +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +ALLOCA = @ALLOCA@ +AMTAR = @AMTAR@ +ARCHFLAGS = @ARCHFLAGS@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CXX = @CXX@ +CXXDEPMODE = @CXXDEPMODE@ +CXXFLAGS = @CXXFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +DRAWSTUFF = @DRAWSTUFF@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +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@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LTLIBOBJS = @LTLIBOBJS@ +MAKEINFO = @MAKEINFO@ +MKDIR_P = @MKDIR_P@ +OBJEXT = @OBJEXT@ +ODE_AGE = @ODE_AGE@ +ODE_CURRENT = @ODE_CURRENT@ +ODE_RELEASE = @ODE_RELEASE@ +ODE_REVISION = @ODE_REVISION@ +ODE_SONAME = @ODE_SONAME@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +RANLIB = @RANLIB@ +SET_MAKE = @SET_MAKE@ +SHARED_LDFLAGS = @SHARED_LDFLAGS@ +SHELL = @SHELL@ +STRIP = @STRIP@ +TOPDIR = @TOPDIR@ +VERSION = @VERSION@ +WINDRES = @WINDRES@ +XMKMF = @XMKMF@ +X_CFLAGS = @X_CFLAGS@ +X_EXTRA_LIBS = @X_EXTRA_LIBS@ +X_LIBS = @X_LIBS@ +X_PRE_LIBS = @X_PRE_LIBS@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_CC = @ac_ct_CC@ +ac_ct_CXX = @ac_ct_CXX@ +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@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +so_ext = @so_ext@ +srcdir = @srcdir@ +sysconfdir = @sysconfdir@ +target = @target@ +target_alias = @target_alias@ +target_cpu = @target_cpu@ +target_os = @target_os@ +target_vendor = @target_vendor@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +AM_CXXFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CFLAGS = @ARCHFLAGS@ @CXXFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +demo_collision_SOURCES = demo_collision.cpp +demo_slider_SOURCES = demo_slider.cpp +demo_feedback_SOURCES = demo_feedback.cpp +demo_crash_SOURCES = demo_crash.cpp +demo_space_SOURCES = demo_space.cpp +demo_I_SOURCES = demo_I.cpp +demo_friction_SOURCES = demo_friction.cpp +demo_space_stress_SOURCES = demo_space_stress.cpp +demo_boxstack_SOURCES = demo_boxstack.cpp +demo_hinge_SOURCES = demo_hinge.cpp +demo_step_SOURCES = demo_step.cpp +demo_buggy_SOURCES = demo_buggy.cpp +demo_cyl_SOURCES = demo_cyl.cpp world_geom3.h +demo_cylvssphere_SOURCES = demo_cylvssphere.cpp +demo_joints_SOURCES = demo_joints.cpp +demo_jointPR_SOURCES = demo_jointPR.cpp +demo_motor_SOURCES = demo_motor.cpp +demo_chain1_SOURCES = demo_chain1.c +demo_chain2_SOURCES = demo_chain2.cpp +demo_ode_SOURCES = demo_ode.cpp +demo_plane2d_SOURCES = demo_plane2d.cpp +demo_heightfield_SOURCES = demo_heightfield.cpp +demo_convex_cd_SOURCES = demo_convex_cd.cpp +demo_collision_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_slider_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_feedback_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_crash_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_space_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_I_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_friction_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_space_stress_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_boxstack_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_hinge_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_step_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_buggy_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_cyl_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_cylvssphere_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_joints_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_jointPR_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_motor_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_chain1_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_chain2_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_ode_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_plane2d_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_heightfield_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_convex_cd_LDFLAGS = -L$(top_builddir)/drawstuff/src \ + -L$(top_builddir)/ode/src @LDFLAGS@ + +demo_collision_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_43) +demo_slider_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_41) +demo_feedback_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_42) +demo_crash_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_40) +demo_space_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_39) +demo_I_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_38) +demo_friction_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_37) +demo_space_stress_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_36) +demo_boxstack_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_35) +demo_hinge_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_34) +demo_step_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_33) +demo_buggy_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_30) +demo_cyl_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_31) +demo_cylvssphere_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_32) +demo_joints_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_27) +demo_jointPR_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_28) +demo_motor_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_29) +demo_chain1_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_26) +demo_chain2_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_25) +demo_ode_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_24) +demo_plane2d_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a + +demo_heightfield_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a + +demo_convex_cd_DEPENDENCIES = $(top_builddir)/ode/src/libode.a \ + $(top_builddir)/drawstuff/src/libdrawstuff.a $(am__append_44) +@TRIMESH_TRUE@demo_trimesh_SOURCES = demo_trimesh.cpp +@TRIMESH_TRUE@demo_moving_trimesh_SOURCES = demo_moving_trimesh.cpp +@TRIMESH_TRUE@demo_basket_SOURCES = demo_basket.cpp +@TRIMESH_TRUE@demo_trimesh_LDFLAGS = -L$(top_builddir)/drawstuff/src \ +@TRIMESH_TRUE@ -L$(top_builddir)/ode/src @LDFLAGS@ \ +@TRIMESH_TRUE@ @GL_LIBS@ @LIBS@ + +@TRIMESH_TRUE@demo_moving_trimesh_LDFLAGS = -L$(top_builddir)/drawstuff/src \ +@TRIMESH_TRUE@ -L$(top_builddir)/ode/src @LDFLAGS@ \ +@TRIMESH_TRUE@ @GL_LIBS@ @LIBS@ + +@TRIMESH_TRUE@demo_basket_LDFLAGS = -L$(top_builddir)/drawstuff/src \ +@TRIMESH_TRUE@ -L$(top_builddir)/ode/src @LDFLAGS@ \ +@TRIMESH_TRUE@ @GL_LIBS@ @LIBS@ + +@TRIMESH_TRUE@demo_trimesh_DEPENDENCIES = \ +@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a \ +@TRIMESH_TRUE@ $(top_builddir)/drawstuff/src/libdrawstuff.a \ +@TRIMESH_TRUE@ $(am__append_47) +@TRIMESH_TRUE@demo_moving_trimesh_DEPENDENCIES = \ +@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a \ +@TRIMESH_TRUE@ $(top_builddir)/drawstuff/src/libdrawstuff.a \ +@TRIMESH_TRUE@ $(am__append_48) +@TRIMESH_TRUE@demo_basket_DEPENDENCIES = \ +@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a \ +@TRIMESH_TRUE@ $(top_builddir)/drawstuff/src/libdrawstuff.a \ +@TRIMESH_TRUE@ $(am__append_49) +demo_ode_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_2) +demo_plane2d_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +demo_heightfield_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_3) +demo_chain2_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_4) +demo_chain1_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_5) +demo_joints_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_6) +demo_jointPR_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_7) +demo_motor_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_8) +demo_buggy_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_9) +demo_cyl_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_10) +demo_cylvssphere_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_11) +demo_step_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_12) +demo_hinge_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_13) +demo_boxstack_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_14) +demo_space_stress_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_15) +demo_friction_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_16) +demo_I_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ \ + @LIBS@ $(am__append_17) +demo_space_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_18) +demo_crash_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_19) +demo_slider_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_20) +demo_feedback_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_21) +demo_collision_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_22) +demo_convex_cd_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a \ + @GL_LIBS@ @LIBS@ $(am__append_23) +@TRIMESH_TRUE@demo_trimesh_LDADD = -ldrawstuff \ +@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a @GL_LIBS@ \ +@TRIMESH_TRUE@ @LIBS@ $(am__append_45) +@TRIMESH_TRUE@demo_moving_trimesh_LDADD = -ldrawstuff \ +@TRIMESH_TRUE@ $(top_builddir)/ode/src/libode.a @GL_LIBS@ \ +@TRIMESH_TRUE@ @LIBS@ $(am__append_46) +@TRIMESH_TRUE@demo_basket_LDADD = -ldrawstuff $(top_builddir)/ode/src/libode.a @GL_LIBS@ @LIBS@ +all: all-am + +.SUFFIXES: +.SUFFIXES: .c .cpp .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 \ + && exit 0; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/demo/Makefile'; \ + cd $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/demo/Makefile +.PRECIOUS: 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 + +clean-noinstPROGRAMS: + -test -z "$(noinst_PROGRAMS)" || rm -f $(noinst_PROGRAMS) +demo_I$(EXEEXT): $(demo_I_OBJECTS) $(demo_I_DEPENDENCIES) + @rm -f demo_I$(EXEEXT) + $(demo_I_LINK) $(demo_I_OBJECTS) $(demo_I_LDADD) $(LIBS) +demo_basket$(EXEEXT): $(demo_basket_OBJECTS) $(demo_basket_DEPENDENCIES) + @rm -f demo_basket$(EXEEXT) + $(demo_basket_LINK) $(demo_basket_OBJECTS) $(demo_basket_LDADD) $(LIBS) +demo_boxstack$(EXEEXT): $(demo_boxstack_OBJECTS) $(demo_boxstack_DEPENDENCIES) + @rm -f demo_boxstack$(EXEEXT) + $(demo_boxstack_LINK) $(demo_boxstack_OBJECTS) $(demo_boxstack_LDADD) $(LIBS) +demo_buggy$(EXEEXT): $(demo_buggy_OBJECTS) $(demo_buggy_DEPENDENCIES) + @rm -f demo_buggy$(EXEEXT) + $(demo_buggy_LINK) $(demo_buggy_OBJECTS) $(demo_buggy_LDADD) $(LIBS) +demo_chain1$(EXEEXT): $(demo_chain1_OBJECTS) $(demo_chain1_DEPENDENCIES) + @rm -f demo_chain1$(EXEEXT) + $(demo_chain1_LINK) $(demo_chain1_OBJECTS) $(demo_chain1_LDADD) $(LIBS) +demo_chain2$(EXEEXT): $(demo_chain2_OBJECTS) $(demo_chain2_DEPENDENCIES) + @rm -f demo_chain2$(EXEEXT) + $(demo_chain2_LINK) $(demo_chain2_OBJECTS) $(demo_chain2_LDADD) $(LIBS) +demo_collision$(EXEEXT): $(demo_collision_OBJECTS) $(demo_collision_DEPENDENCIES) + @rm -f demo_collision$(EXEEXT) + $(demo_collision_LINK) $(demo_collision_OBJECTS) $(demo_collision_LDADD) $(LIBS) +demo_convex_cd$(EXEEXT): $(demo_convex_cd_OBJECTS) $(demo_convex_cd_DEPENDENCIES) + @rm -f demo_convex_cd$(EXEEXT) + $(demo_convex_cd_LINK) $(demo_convex_cd_OBJECTS) $(demo_convex_cd_LDADD) $(LIBS) +demo_crash$(EXEEXT): $(demo_crash_OBJECTS) $(demo_crash_DEPENDENCIES) + @rm -f demo_crash$(EXEEXT) + $(demo_crash_LINK) $(demo_crash_OBJECTS) $(demo_crash_LDADD) $(LIBS) +demo_cyl$(EXEEXT): $(demo_cyl_OBJECTS) $(demo_cyl_DEPENDENCIES) + @rm -f demo_cyl$(EXEEXT) + $(demo_cyl_LINK) $(demo_cyl_OBJECTS) $(demo_cyl_LDADD) $(LIBS) +demo_cylvssphere$(EXEEXT): $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_DEPENDENCIES) + @rm -f demo_cylvssphere$(EXEEXT) + $(demo_cylvssphere_LINK) $(demo_cylvssphere_OBJECTS) $(demo_cylvssphere_LDADD) $(LIBS) +demo_feedback$(EXEEXT): $(demo_feedback_OBJECTS) $(demo_feedback_DEPENDENCIES) + @rm -f demo_feedback$(EXEEXT) + $(demo_feedback_LINK) $(demo_feedback_OBJECTS) $(demo_feedback_LDADD) $(LIBS) +demo_friction$(EXEEXT): $(demo_friction_OBJECTS) $(demo_friction_DEPENDENCIES) + @rm -f demo_friction$(EXEEXT) + $(demo_friction_LINK) $(demo_friction_OBJECTS) $(demo_friction_LDADD) $(LIBS) +demo_heightfield$(EXEEXT): $(demo_heightfield_OBJECTS) $(demo_heightfield_DEPENDENCIES) + @rm -f demo_heightfield$(EXEEXT) + $(demo_heightfield_LINK) $(demo_heightfield_OBJECTS) $(demo_heightfield_LDADD) $(LIBS) +demo_hinge$(EXEEXT): $(demo_hinge_OBJECTS) $(demo_hinge_DEPENDENCIES) + @rm -f demo_hinge$(EXEEXT) + $(demo_hinge_LINK) $(demo_hinge_OBJECTS) $(demo_hinge_LDADD) $(LIBS) +demo_jointPR$(EXEEXT): $(demo_jointPR_OBJECTS) $(demo_jointPR_DEPENDENCIES) + @rm -f demo_jointPR$(EXEEXT) + $(demo_jointPR_LINK) $(demo_jointPR_OBJECTS) $(demo_jointPR_LDADD) $(LIBS) +demo_joints$(EXEEXT): $(demo_joints_OBJECTS) $(demo_joints_DEPENDENCIES) + @rm -f demo_joints$(EXEEXT) + $(demo_joints_LINK) $(demo_joints_OBJECTS) $(demo_joints_LDADD) $(LIBS) +demo_motor$(EXEEXT): $(demo_motor_OBJECTS) $(demo_motor_DEPENDENCIES) + @rm -f demo_motor$(EXEEXT) + $(demo_motor_LINK) $(demo_motor_OBJECTS) $(demo_motor_LDADD) $(LIBS) +demo_moving_trimesh$(EXEEXT): $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_DEPENDENCIES) + @rm -f demo_moving_trimesh$(EXEEXT) + $(demo_moving_trimesh_LINK) $(demo_moving_trimesh_OBJECTS) $(demo_moving_trimesh_LDADD) $(LIBS) +demo_ode$(EXEEXT): $(demo_ode_OBJECTS) $(demo_ode_DEPENDENCIES) + @rm -f demo_ode$(EXEEXT) + $(demo_ode_LINK) $(demo_ode_OBJECTS) $(demo_ode_LDADD) $(LIBS) +demo_plane2d$(EXEEXT): $(demo_plane2d_OBJECTS) $(demo_plane2d_DEPENDENCIES) + @rm -f demo_plane2d$(EXEEXT) + $(demo_plane2d_LINK) $(demo_plane2d_OBJECTS) $(demo_plane2d_LDADD) $(LIBS) +demo_slider$(EXEEXT): $(demo_slider_OBJECTS) $(demo_slider_DEPENDENCIES) + @rm -f demo_slider$(EXEEXT) + $(demo_slider_LINK) $(demo_slider_OBJECTS) $(demo_slider_LDADD) $(LIBS) +demo_space$(EXEEXT): $(demo_space_OBJECTS) $(demo_space_DEPENDENCIES) + @rm -f demo_space$(EXEEXT) + $(demo_space_LINK) $(demo_space_OBJECTS) $(demo_space_LDADD) $(LIBS) +demo_space_stress$(EXEEXT): $(demo_space_stress_OBJECTS) $(demo_space_stress_DEPENDENCIES) + @rm -f demo_space_stress$(EXEEXT) + $(demo_space_stress_LINK) $(demo_space_stress_OBJECTS) $(demo_space_stress_LDADD) $(LIBS) +demo_step$(EXEEXT): $(demo_step_OBJECTS) $(demo_step_DEPENDENCIES) + @rm -f demo_step$(EXEEXT) + $(demo_step_LINK) $(demo_step_OBJECTS) $(demo_step_LDADD) $(LIBS) +demo_trimesh$(EXEEXT): $(demo_trimesh_OBJECTS) $(demo_trimesh_DEPENDENCIES) + @rm -f demo_trimesh$(EXEEXT) + $(demo_trimesh_LINK) $(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_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_cd.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_feedback.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_friction.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_joints.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/demo_motor.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_plane2d.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_trimesh.Po@am__quote@ + +.c.o: +@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(COMPILE) -c $< + +.c.obj: +@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(COMPILE) -c `$(CYGPATH_W) '$<'` + +.cpp.o: +@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ $< + +.cpp.obj: +@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'` + +ID: $(HEADERS) $(SOURCES) $(LISP) $(TAGS_FILES) + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) ' { files[$$0] = 1; } \ + END { for (i in files) print i; }'`; \ + mkid -fID $$unique +tags: TAGS + +TAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \ + $(TAGS_FILES) $(LISP) + tags=; \ + here=`pwd`; \ + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) ' { files[$$0] = 1; } \ + END { for (i in files) print i; }'`; \ + if test -z "$(ETAGS_ARGS)$$tags$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$tags $$unique; \ + fi +ctags: CTAGS +CTAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \ + $(TAGS_FILES) $(LISP) + tags=; \ + here=`pwd`; \ + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) ' { files[$$0] = 1; } \ + END { for (i in files) print i; }'`; \ + test -z "$(CTAGS_ARGS)$$tags$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$tags $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && cd $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) $$here + +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 $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -pR $(srcdir)/$$file $(distdir)$$dir || exit 1; \ + fi; \ + cp -pR $$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) +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: + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + `test -z '$(STRIP)' || \ + echo "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'"` install +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_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-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 + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +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 + +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 + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: + +.MAKE: install-am install-strip + +.PHONY: CTAGS GTAGS all all-am check check-am clean clean-generic \ + clean-noinstPROGRAMS ctags distclean distclean-compile \ + distclean-generic 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 pdf pdf-am ps ps-am tags uninstall \ + uninstall-am + + +@WIN32_TRUE@resources.o: ../../drawstuff/src/resources.rc ../../drawstuff/src/resource.h +@WIN32_TRUE@ @WINDRES@ ../../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/libraries/ode-0.9/ode/demo/basket_geom.h b/libraries/ode-0.9/ode/demo/basket_geom.h new file mode 100644 index 0000000..7796ca8 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/basket_geom.h @@ -0,0 +1,13 @@ + +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,-4,0.100000f,-4,-4,0.100000f,4,0,0.100000f,4,-4,-0.100000f,4,4,-0.100000f,4,0,0.100000f,4,4,-0.100000f,4,4,0.100000f,4,0,0.100000f,4,-4,0.100000f,4,-4,-0.100000f,-4,-4,-0.100000f,-4,4,-0.100000f,4,4,-0.100000f,-4,-4,-0.100000f,4,4,-0.100000f,4,-4,-0.100000f,0.066000f,-2.060000f,2,0.066000f,-1.940000f,2,-0.066000f,-2.060000f,2,0.066000f,-1.940000f,2,-0.066000f,-1.940000f,2,-0.066000f,-2.060000f,2,-4,4,0.100000f,4,4,0.100000f,4,4,-0.100000f,4,4,-0.100000f,-4,4,-0.100000f,-4,4,0.100000f,-4,-4,0.100000f,-4,0,0.100000f,-4,-4,-0.100000f,-4,0,0.100000f,-4,4,0.100000f,-4,4,-0.100000f,-4,0,0.100000f,-4,4,-0.100000f,-4,-4,-0.100000f,0.360000f,3.244444f,1.466974f,0.360000f,3.422222f,2.266974f,-0.360000f,3.422222f,2.266974f,-0.360000f,3.422222f,2.266974f,-0.360000f,3.244444f,1.466974f,0.360000f,3.244444f,1.466974f,4,-4,0.100000f,0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-4,-4,0.100000f,4,-4,0.100000f,4,0,0.100000f,0.066000f,-1.940000f,0.100000f,4,-4,0.100000f,0.066000f,-1.940000f,0.100000f,0.066000f,-2.060000f,0.100000f,4,-4,0.100000f,-0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,0.100000f,4,0,0.100000f,4,0,0.100000f,-4,0,0.100000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-1.940000f,0.100000f,-4,0,0.100000f,-4,0,0.100000f,-4,-4,0.100000f,-0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,2,-0.066000f,-2.060000f,2,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,2,0.066000f,-2.060000f,2,0.066000f,-2.060000f,2,0.066000f,-2.060000f,0.100000f,0.066000f,-1.940000f,1.950000f,0.066000f,-2.060000f,0.100000f,0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,1.950000f,-0.052853f,-1.506390f,2,0.052853f,-1.506390f,2,0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,2,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,2,-0.066000f,-1.940000f,1.950000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-2.060000f,2,-0.066000f,-1.940000f,2,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,0.100000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,2,-0.066000f,-1.840000f,2,-0.066000f,-1.840000f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,2,0.066000f,-1.940000f,2,0.066000f,-1.840000f,2,-0.066000f,-1.940000f,2,0.066000f,-1.840000f,2,-0.066000f,-1.840000f,2,-0.066000f,-1.940000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.840000f,2,0.066000f,-1.940000f,2,-0.066000f,-1.840000f,2,-0.171600f,-1.740000f,2,-0.066000f,-1.840000f,1.950000f,-0.171600f,-1.740000f,2,-0.171600f,-1.740000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.171600f,-1.740000f,2,0.066000f,-1.840000f,1.950000f,0.171600f,-1.740000f,2,0.066000f,-1.840000f,2,-0.171600f,-1.740000f,2,-0.188760f,-1.640000f,2,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.171600f,-1.740000f,2,0.171600f,-1.740000f,1.950000f,0.188760f,-1.640000f,1.950000f,0.188760f,-1.640000f,2,0.171600f,-1.740000f,1.950000f,0.188760f,-1.640000f,2,0.171600f,-1.740000f,2,-0.188760f,-1.640000f,2,-0.132132f,-1.540000f,2,-0.132132f,-1.540000f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,2,0.188760f,-1.640000f,1.950000f,0.132132f,-1.540000f,1.950000f,0.132132f,-1.540000f,2,0.188760f,-1.640000f,1.950000f,0.132132f,-1.540000f,2,0.188760f,-1.640000f,2,-0.132132f,-1.540000f,2,-0.052853f,-1.506390f,2,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.132132f,-1.540000f,2,0.132132f,-1.540000f,1.950000f,0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,2,0.132132f,-1.540000f,1.950000f,0.052853f,-1.506390f,2,0.132132f,-1.540000f,2,0.188760f,-1.640000f,1.950000f,0.173397f,-1.642679f,1.950000f,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,1.950000f,0.132132f,-1.540000f,1.950000f,0.188760f,-1.640000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.157950f,-1.732697f,1.950000f,0.173397f,-1.642679f,1.950000f,0.171600f,-1.740000f,1.950000f,0.173397f,-1.642679f,1.950000f,0.188760f,-1.640000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.060149f,-1.825311f,1.950000f,0.171600f,-1.740000f,1.950000f,0.060149f,-1.825311f,1.950000f,0.157950f,-1.732697f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.060149f,-1.825311f,1.950000f,0.066000f,-1.840000f,1.950000f,-0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,1.950000f,0.066000f,-1.840000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.132132f,-1.540000f,1.950000f,0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,1.950000f,0.049868f,-1.521079f,1.950000f,0.052853f,-1.506390f,1.950000f,0.132132f,-1.540000f,1.950000f,0.121808f,-1.551577f,1.950000f,0.052853f,-1.506390f,1.950000f,0.121808f,-1.551577f,1.950000f,0.049868f,-1.521079f,1.950000f,-0.188760f,-1.640000f,2,-0.173397f,-1.642679f,2,-0.121808f,-1.551577f,2,-0.121808f,-1.551577f,2,-0.132132f,-1.540000f,2,-0.188760f,-1.640000f,2,-0.171600f,-1.740000f,2,-0.157950f,-1.732697f,2,-0.173397f,-1.642679f,2,-0.173397f,-1.642679f,2,-0.188760f,-1.640000f,2,-0.171600f,-1.740000f,2,-0.066000f,-1.840000f,2,-0.060149f,-1.825311f,2,-0.171600f,-1.740000f,2,-0.060149f,-1.825311f,2,-0.157950f,-1.732697f,2,-0.171600f,-1.740000f,2,0.066000f,-1.840000f,2,0.060149f,-1.825311f,2,-0.066000f,-1.840000f,2,0.060149f,-1.825311f,2,-0.060149f,-1.825311f,2,-0.066000f,-1.840000f,2,0.171600f,-1.740000f,2,0.157950f,-1.732697f,2,0.060149f,-1.825311f,2,0.171600f,-1.740000f,2,0.060149f,-1.825311f,2,0.066000f,-1.840000f,2,0.173397f,-1.642679f,2,0.157950f,-1.732697f,2,0.171600f,-1.740000f,2,0.171600f,-1.740000f,2,0.188760f,-1.640000f,2,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.173397f,-1.642679f,2,0.188760f,-1.640000f,2,0.188760f,-1.640000f,2,0.132132f,-1.540000f,2,0.121808f,-1.551577f,2,0.052853f,-1.506390f,2,0.049868f,-1.521079f,2,0.121808f,-1.551577f,2,0.052853f,-1.506390f,2,0.121808f,-1.551577f,2,0.132132f,-1.540000f,2,-0.049868f,-1.521079f,2,0.049868f,-1.521079f,2,0.052853f,-1.506390f,2,0.052853f,-1.506390f,2,-0.052853f,-1.506390f,2,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.049868f,-1.521079f,2,-0.052853f,-1.506390f,2,-0.052853f,-1.506390f,2,-0.132132f,-1.540000f,2,-0.121808f,-1.551577f,2,-0.173397f,-1.642679f,2,-0.157950f,-1.732697f,2,-0.157950f,-1.732697f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.173397f,-1.642679f,2,-0.157950f,-1.732697f,2,-0.060149f,-1.825311f,2,-0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.157950f,-1.732697f,2,-0.060149f,-1.825311f,2,0.060149f,-1.825311f,2,0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,2,0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,2,0.157950f,-1.732697f,2,0.157950f,-1.732697f,2,0.157950f,-1.732697f,1.950000f,0.060149f,-1.825311f,1.950000f,0.157950f,-1.732697f,2,0.173397f,-1.642679f,2,0.173397f,-1.642679f,1.950000f,0.173397f,-1.642679f,1.950000f,0.157950f,-1.732697f,1.950000f,0.157950f,-1.732697f,2,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,1.950000f,0.173397f,-1.642679f,1.950000f,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.049868f,-1.521079f,2,0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,1.950000f,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,2,0.049868f,-1.521079f,2,-0.049868f,-1.521079f,2,-0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,2,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.121808f,-1.551577f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.173397f,-1.642679f,2,-0.173397f,-1.642679f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.121808f,-1.551577f,2,-0.360000f,3.600000f,0.100000f,0.360000f,3.600000f,0.100000f,4,4,0.100000f,4,4,0.100000f,-4,4,0.100000f,-0.360000f,3.600000f,0.100000f,-0.360000f,0.400000f,0.100000f,-0.360000f,3.600000f,0.100000f,-4,4,0.100000f,-4,4,0.100000f,-4,0,0.100000f,-0.360000f,0.400000f,0.100000f,4,0,0.100000f,0.360000f,0.400000f,0.100000f,-0.360000f,0.400000f,0.100000f,-0.360000f,0.400000f,0.100000f,-4,0,0.100000f,4,0,0.100000f,4,4,0.100000f,0.360000f,3.600000f,0.100000f,4,0,0.100000f,0.360000f,3.600000f,0.100000f,0.360000f,0.400000f,0.100000f,4,0,0.100000f,0.360000f,2.888889f,1.023752f,0.360000f,3.066667f,1.166974f,-0.360000f,3.066667f,1.166974f,-0.360000f,3.066667f,1.166974f,-0.360000f,2.888889f,1.023752f,0.360000f,2.888889f,1.023752f,0.360000f,2.533333f,0.939976f,0.360000f,2.711111f,0.966974f,-0.360000f,2.711111f,0.966974f,-0.360000f,2.711111f,0.966974f,-0.360000f,2.533333f,0.939976f,0.360000f,2.533333f,0.939976f,-0.360000f,2.177778f,0.939976f,0.360000f,2.177778f,0.939976f,0.360000f,2.355556f,0.939976f,0.360000f,2.355556f,0.939976f,-0.360000f,2.355556f,0.939976f,-0.360000f,2.177778f,0.939976f,-0.360000f,1.822222f,0.939976f,0.360000f,1.822222f,0.939976f,0.360000f,2,0.939976f,0.360000f,2,0.939976f,-0.360000f,2,0.939976f,-0.360000f,1.822222f,0.939976f,-0.360000f,1.466667f,0.939976f,0.360000f,1.466667f,0.939976f,0.360000f,1.644444f,0.939976f,0.360000f,1.644444f,0.939976f,-0.360000f,1.644444f,0.939976f,-0.360000f,1.466667f,0.939976f,0.360000f,1.111111f,0.957571f,0.360000f,1.288889f,0.939976f,-0.360000f,1.288889f,0.939976f,-0.360000f,1.288889f,0.939976f,-0.360000f,1.111111f,0.957571f,0.360000f,1.111111f,0.957571f,-0.360000f,0.755556f,1.134246f,0.360000f,0.755556f,1.134246f,0.360000f,0.933333f,1.009739f,0.360000f,0.933333f,1.009739f,-0.360000f,0.933333f,1.009739f,-0.360000f,0.755556f,1.134246f,0.360000f,0.755556f,1.134246f,-0.360000f,0.755556f,1.134246f,0.360000f,0.577778f,1.372130f,-0.360000f,0.755556f,1.134246f,-0.360000f,0.577778f,1.372130f,0.360000f,0.577778f,1.372130f,-0.360000f,3.600000f,3.900000f,-0.360000f,3.422222f,2.266974f,0.360000f,3.422222f,2.266974f,-0.360000f,3.600000f,3.900000f,0.360000f,3.422222f,2.266974f,0.360000f,3.600000f,3.900000f,0.360000f,3.244444f,1.466974f,-0.360000f,3.244444f,1.466974f,0.360000f,3.066667f,1.166974f,-0.360000f,3.244444f,1.466974f,-0.360000f,3.066667f,1.166974f,0.360000f,3.066667f,1.166974f,0.360000f,2.888889f,1.023752f,-0.360000f,2.888889f,1.023752f,0.360000f,2.711111f,0.966974f,-0.360000f,2.888889f,1.023752f,-0.360000f,2.711111f,0.966974f,0.360000f,2.711111f,0.966974f,0.360000f,2.533333f,0.939976f,-0.360000f,2.533333f,0.939976f,-0.360000f,2.355556f,0.939976f,0.360000f,2.533333f,0.939976f,-0.360000f,2.355556f,0.939976f,0.360000f,2.355556f,0.939976f,0.360000f,2.177778f,0.939976f,-0.360000f,2.177778f,0.939976f,-0.360000f,2,0.939976f,0.360000f,2.177778f,0.939976f,-0.360000f,2,0.939976f,0.360000f,2,0.939976f,0.360000f,1.822222f,0.939976f,-0.360000f,1.822222f,0.939976f,-0.360000f,1.644444f,0.939976f,0.360000f,1.822222f,0.939976f,-0.360000f,1.644444f,0.939976f,0.360000f,1.644444f,0.939976f,0.360000f,1.466667f,0.939976f,-0.360000f,1.466667f,0.939976f,-0.360000f,1.288889f,0.939976f,0.360000f,1.466667f,0.939976f,-0.360000f,1.288889f,0.939976f,0.360000f,1.288889f,0.939976f,0.360000f,1.111111f,0.957571f,-0.360000f,1.111111f,0.957571f,0.360000f,0.933333f,1.009739f,-0.360000f,1.111111f,0.957571f,-0.360000f,0.933333f,1.009739f,0.360000f,0.933333f,1.009739f,0.360000f,0.400000f,1.743932f,0.360000f,0.577778f,1.372130f,-0.360000f,0.577778f,1.372130f,-0.360000f,0.577778f,1.372130f,-0.360000f,0.400000f,1.743932f,0.360000f,0.400000f,1.743932f, +}; + +static int world_indices[] = { +0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223,224,225,226,227,228,229,230,231,232,233,234,235,236,237,238,239,240,241,242,243,244,245,246,247,248,249,250,251,252,253,254,255,256,257,258,259,260,261,262,263,264,265,266,267,268,269,270,271,272,273,274,275,276,277,278,279,280,281,282,283,284,285,286,287,288,289,290,291,292,293,294,295,296,297,298,299,300,301,302,303,304,305,306,307,308,309,310,311,312,313,314,315,316,317,318,319,320,321,322,323,324,325,326,327,328,329,330,331,332,333,334,335,336,337,338,339,340,341,342,343,344,345,346,347,348,349,350,351,352,353,354,355,356,357,358,359,360,361,362,363,364,365,366,367,368,369,370,371,372,373,374,375,376,377,378,379,380,381,382,383,384,385,386,387,388,389,390,391,392,393,394,395,396,397,398,399,400,401,402,403,404,405,406,407,408,409,410,411,412,413,414,415,416,417,418,419,420,421,422,423,424,425,426,427,428,429,430,431,432,433,434,435,436,437,438,439,440,441,442,443,444,445,446,447,448,449,450,451,452,453,454,455,456,457,458,459,460,461,462,463,464,465,466,467,468,469,470,471,472,473,474,475,476,477,478,479,480,481,482,483,484,485, +}; + diff --git a/libraries/ode-0.9/ode/demo/demo_I.cpp b/libraries/ode-0.9/ode/demo/demo_I.cpp new file mode 100644 index 0000000..b4f8d6e --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_I.cpp @@ -0,0 +1,254 @@ +/************************************************************************* + * * + * 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 + +#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() +{ + 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 +#include +#ifdef HAVE_UNISTD_H +#include +#endif +#include +#include + +#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", o1,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 _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +//<---- 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 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 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 = 1; + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, 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) +{ + size_t i; + int j,k; + dReal sides[3]; + dMass m; + int setBody; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') + { + 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 + 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()+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,0,maxheight+1); + dRSetIdentity (R); + //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); + } + 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]); + } + //<---- Convex Object + else if (cmd == 'v') + { + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex (space, + planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + 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' && USE_GEOM_OFFSET) { + setBody = 1; + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + dReal dpos[GPB][3]; // delta-positions for encapsulated 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 == '1') { + write_world = 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); + } + //<---- Convex Object + else if (type == dConvexClass) + { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + else if (type == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R,0); + } + 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) +{ + dsSetColor (0,0,2); + 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; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i +#include + +#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 + + +// 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 *data, 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); + + /* + printf ("%.10f %.10f %.10f %.10f\n", + dJointGetHingeAngle (joint[1]), + dJointGetHingeAngle (joint[2]), + dJointGetHingeAngleRate (joint[1]), + dJointGetHingeAngleRate (joint[2])); + */ +} + + +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/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + dInitODE(); + 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 wheel hinge + /* + joint[0] = dJointCreateHinge2 (world,0); + dJointAttach (joint[0],body[0],body[1]); + const dReal *a = dBodyGetPosition (body[1]); + dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]); + dJointSetHinge2Axis1 (joint[0],0,0,1); + dJointSetHinge2Axis2 (joint[0],0,1,0); + */ + + // 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]); + dJointSetHinge2Axis1 (joint[i],0,0,1); + dJointSetHinge2Axis2 (joint[i],0,1,0); + } + + // 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/libraries/ode-0.9/ode/demo/demo_chain1.c b/libraries/ode-0.9/ode/demo/demo_chain1.c new file mode 100644 index 0000000..e20c5dc --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_chain1.c @@ -0,0 +1,171 @@ +/************************************************************************* + * * + * 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" + +#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; + + 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}; + 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 + +#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 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 *data, 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() +{ + 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 + +#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 +#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) + + +// 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 + 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 *data, 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 (dDOT14(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 (dDOT14(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,dDISTANCE (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 (dDOT(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 (dDISTANCE (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 = dDISTANCE (contact.pos,dGeomGetPosition(sphere)); + if (dFabs(k - r) > tol) FAILED(); + // also check normal signs + if (dDOT (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,dDISTANCE(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 (dDOT (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,dDISTANCE(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 (dDOT14(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 (dDOT (n,contact.normal) > 0) 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 (dDOT(contact.pos,n) - d) > tol) FAILED(); + // also check normal signs + if (dDOT (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(dDOT(u1,u1)); + dReal d2 = dSqrt(dDOT(u2,u2)); + dNormalize3 (u1); + dNormalize3 (u2); + if (dFabs(dDOT(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2"); + dCROSS (n,=,u1,u2); + for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k]; + dReal d = -dDOT(n,p1); + if (dFabs(dDOT(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1"); + if (dFabs(dDOT(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2"); + if (dFabs(dDOT(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3"); + dReal alpha = -(d+dDOT(n,v1))/dDOT(n,tmp); + for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]); + if (dFabs(dDOT(n,tmp)+d) > 1e-6) 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 = dDOT(u1,tmp); + dReal a2 = dDOT(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 = dDOT14(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() +{ + 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 pause) +{ + 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/textures"; + + 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)); + dInitODE(); + + 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(100,test_dBoxTouchesBox); + MAKE_TEST(101,test_dBoxBox); + + do_tests (argc,argv); + dCloseODE(); + return 0; +} diff --git a/libraries/ode-0.9/ode/demo/demo_convex_cd.cpp b/libraries/ode-0.9/ode/demo/demo_convex_cd.cpp new file mode 100644 index 0000000..e1764f3 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_convex_cd.cpp @@ -0,0 +1,195 @@ +/************************************************************************* + * * + * 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 +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + + +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +//<---- 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, + -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 + +#ifdef dDOUBLE +#define dsDrawConvex dsDrawConvexD +#define dsDrawBox dsDrawBoxD +#endif + +dGeomID geoms[2]; +dSpaceID space; +dWorldID world; +dJointGroupID contactgroup; + +void start() +{ + // adjust the starting viewpoint a bit + float xyz[3],hpr[3]; + dsGetViewpoint (xyz,hpr); + hpr[0] += 7; + dsSetViewpoint (xyz,hpr); + geoms[0]=dCreateConvex (space, + planes, + planecount, + points, + pointcount, + polygons); + dGeomSetPosition (geoms[0],0,0,0.25); + geoms[1]=dCreateConvex (space, + planes, + planecount, + points, + pointcount, + polygons); + dGeomSetPosition (geoms[1],0.25,0.25,0.70); + +} + +int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +void simLoop (int pause) +{ + static bool DumpInfo=true; + const dReal ss[3] = {0.02,0.02,0.02}; + dContactGeom contacts[8]; + int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom)); + //fprintf(stdout,"Contact Count %d\n",contactcount); + const dReal* pos; + const dReal* R; + dsSetTexture (DS_WOOD); + pos = dGeomGetPosition (geoms[0]); + R = dGeomGetRotation (geoms[0]); + dsSetColor (0.6f,0.6f,1); + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + pos = dGeomGetPosition (geoms[1]); + R = dGeomGetRotation (geoms[1]); + dsSetColor (0.4f,1,1); + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + /*if (show_contacts) */ + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColor (1.0f,0,0); + for(int i=0;i +#include + +#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 + + +// select the method you want to test here (only uncomment *one* line) +#define QUICKSTEP 1 +//#define STEPFAST 1 + +// 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 + +//#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 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 *data, 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]); + dJointSetHinge2Axis1 (joint[joints],0,0,1); + dJointSetHinge2Axis2 (joint[joints],1,0,0); + 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]); + dJointSetHinge2Axis1 (joint[joints],0,0,1); + dJointSetHinge2Axis2 (joint[joints],1,0,0); + 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 '+': + dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1); + break; + case '-': + dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1); + break; + case 'r': case 'R': + resetSimulation(); + 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); +#if defined(QUICKSTEP) + dWorldQuickStep (world,0.05); +#elif defined(STEPFAST) + dWorldStepFast1 (world,0.05,ITERS); +#endif + 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/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + dInitODE(); + + bodies = 0; + joints = 0; + boxes = 0; + spheres = 0; + + resetSimulation(); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dCloseODE(); + return 0; +} diff --git a/libraries/ode-0.9/ode/demo/demo_cyl.cpp b/libraries/ode-0.9/ode/demo/demo_cyl.cpp new file mode 100644 index 0000000..3692234 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_cyl.cpp @@ -0,0 +1,318 @@ +/************************************************************************* + * * + * 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 "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", o1,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 + +#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 dGeomID world_mesh; + +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", o1,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 +#ifdef HAVE_UNISTD_H +#include +#endif +#include +#include + + +#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", o1,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.005; // 5ms 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 + +#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 *data, 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 + +#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 + + +// 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 ) ) + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#define dsDrawTriangle dsDrawTriangleD +#endif + + + +//<---- 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 +#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; + + + + +//============================ + +// Bunny mesh ripped from Opcode +const int VertexCount = 453; +const int IndexCount = 902 * 3; + +typedef dReal dVector3R[3]; + +dGeomID TriMesh1; +dGeomID TriMesh2; +static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data + +static float Vertices[VertexCount * 3] = { + REAL(-0.334392), REAL(0.133007), REAL(0.062259), + REAL(-0.350189), REAL(0.150354), REAL(-0.147769), + REAL(-0.234201), REAL(0.343811), REAL(-0.174307), + REAL(-0.200259), REAL(0.285207), REAL(0.093749), + REAL(0.003520), REAL(0.475208), REAL(-0.159365), + REAL(0.001856), REAL(0.419203), REAL(0.098582), + REAL(-0.252802), REAL(0.093666), REAL(0.237538), + REAL(-0.162901), REAL(0.237984), REAL(0.206905), + REAL(0.000865), REAL(0.318141), REAL(0.235370), + REAL(-0.414624), REAL(0.164083), REAL(-0.278254), + REAL(-0.262213), REAL(0.357334), REAL(-0.293246), + REAL(0.004628), REAL(0.482694), REAL(-0.338626), + REAL(-0.402162), REAL(0.133528), REAL(-0.443247), + REAL(-0.243781), REAL(0.324275), REAL(-0.436763), + REAL(0.005293), REAL(0.437592), REAL(-0.458332), + REAL(-0.339884), REAL(-0.041150), REAL(-0.668211), + REAL(-0.248382), REAL(0.255825), REAL(-0.627493), + REAL(0.006261), REAL(0.376103), REAL(-0.631506), + REAL(-0.216201), REAL(-0.126776), REAL(-0.886936), + REAL(-0.171075), REAL(0.011544), REAL(-0.881386), + REAL(-0.181074), REAL(0.098223), REAL(-0.814779), + REAL(-0.119891), REAL(0.218786), REAL(-0.760153), + REAL(-0.078895), REAL(0.276780), REAL(-0.739281), + REAL(0.006801), REAL(0.310959), REAL(-0.735661), + REAL(-0.168842), REAL(0.102387), REAL(-0.920381), + REAL(-0.104072), REAL(0.177278), REAL(-0.952530), + REAL(-0.129704), REAL(0.211848), REAL(-0.836678), + REAL(-0.099875), REAL(0.310931), REAL(-0.799381), + REAL(0.007237), REAL(0.361687), REAL(-0.794439), + REAL(-0.077913), REAL(0.258753), REAL(-0.921640), + REAL(0.007957), REAL(0.282241), REAL(-0.931680), + REAL(-0.252222), REAL(-0.550401), REAL(-0.557810), + REAL(-0.267633), REAL(-0.603419), REAL(-0.655209), + REAL(-0.446838), REAL(-0.118517), REAL(-0.466159), + REAL(-0.459488), REAL(-0.093017), REAL(-0.311341), + REAL(-0.370645), REAL(-0.100108), REAL(-0.159454), + REAL(-0.371984), REAL(-0.091991), REAL(-0.011044), + REAL(-0.328945), REAL(-0.098269), REAL(0.088659), + REAL(-0.282452), REAL(-0.018862), REAL(0.311501), + REAL(-0.352403), REAL(-0.131341), REAL(0.144902), + REAL(-0.364126), REAL(-0.200299), REAL(0.202388), + REAL(-0.283965), REAL(-0.231869), REAL(0.023668), + REAL(-0.298943), REAL(-0.155218), REAL(0.369716), + REAL(-0.293787), REAL(-0.121856), REAL(0.419097), + REAL(-0.290163), REAL(-0.290797), REAL(0.107824), + REAL(-0.264165), REAL(-0.272849), REAL(0.036347), + REAL(-0.228567), REAL(-0.372573), REAL(0.290309), + REAL(-0.190431), REAL(-0.286997), REAL(0.421917), + REAL(-0.191039), REAL(-0.240973), REAL(0.507118), + REAL(-0.287272), REAL(-0.276431), REAL(-0.065444), + REAL(-0.295675), REAL(-0.280818), REAL(-0.174200), + REAL(-0.399537), REAL(-0.313131), REAL(-0.376167), + REAL(-0.392666), REAL(-0.488581), REAL(-0.427494), + REAL(-0.331669), REAL(-0.570185), REAL(-0.466054), + REAL(-0.282290), REAL(-0.618140), REAL(-0.589220), + REAL(-0.374238), REAL(-0.594882), REAL(-0.323298), + REAL(-0.381071), REAL(-0.629723), REAL(-0.350777), + REAL(-0.382112), REAL(-0.624060), REAL(-0.221577), + REAL(-0.272701), REAL(-0.566522), REAL(0.259157), + REAL(-0.256702), REAL(-0.663406), REAL(0.286079), + REAL(-0.280948), REAL(-0.428359), REAL(0.055790), + REAL(-0.184974), REAL(-0.508894), REAL(0.326265), + REAL(-0.279971), REAL(-0.526918), REAL(0.395319), + REAL(-0.282599), REAL(-0.663393), REAL(0.412411), + REAL(-0.188329), REAL(-0.475093), REAL(0.417954), + REAL(-0.263384), REAL(-0.663396), REAL(0.466604), + REAL(-0.209063), REAL(-0.663393), REAL(0.509344), + REAL(-0.002044), REAL(-0.319624), REAL(0.553078), + REAL(-0.001266), REAL(-0.371260), REAL(0.413296), + REAL(-0.219753), REAL(-0.339762), REAL(-0.040921), + REAL(-0.256986), REAL(-0.282511), REAL(-0.006349), + REAL(-0.271706), REAL(-0.260881), REAL(0.001764), + REAL(-0.091191), REAL(-0.419184), REAL(-0.045912), + REAL(-0.114944), REAL(-0.429752), REAL(-0.124739), + REAL(-0.113970), REAL(-0.382987), REAL(-0.188540), + REAL(-0.243012), REAL(-0.464942), REAL(-0.242850), + REAL(-0.314815), REAL(-0.505402), REAL(-0.324768), + REAL(0.002774), REAL(-0.437526), REAL(-0.262766), + REAL(-0.072625), REAL(-0.417748), REAL(-0.221440), + REAL(-0.160112), REAL(-0.476932), REAL(-0.293450), + REAL(0.003859), REAL(-0.453425), REAL(-0.443916), + REAL(-0.120363), REAL(-0.581567), REAL(-0.438689), + REAL(-0.091499), REAL(-0.584191), REAL(-0.294511), + REAL(-0.116469), REAL(-0.599861), REAL(-0.188308), + REAL(-0.208032), REAL(-0.513640), REAL(-0.134649), + REAL(-0.235749), REAL(-0.610017), REAL(-0.040939), + REAL(-0.344916), REAL(-0.622487), REAL(-0.085380), + REAL(-0.336401), REAL(-0.531864), REAL(-0.212298), + REAL(0.001961), REAL(-0.459550), REAL(-0.135547), + REAL(-0.058296), REAL(-0.430536), REAL(-0.043440), + REAL(0.001378), REAL(-0.449511), REAL(-0.037762), + REAL(-0.130135), REAL(-0.510222), REAL(0.079144), + REAL(0.000142), REAL(-0.477549), REAL(0.157064), + REAL(-0.114284), REAL(-0.453206), REAL(0.304397), + REAL(-0.000592), REAL(-0.443558), REAL(0.285401), + REAL(-0.056215), REAL(-0.663402), REAL(0.326073), + REAL(-0.026248), REAL(-0.568010), REAL(0.273318), + REAL(-0.049261), REAL(-0.531064), REAL(0.389854), + REAL(-0.127096), REAL(-0.663398), REAL(0.479316), + REAL(-0.058384), REAL(-0.663401), REAL(0.372891), + REAL(-0.303961), REAL(0.054199), REAL(0.625921), + REAL(-0.268594), REAL(0.193403), REAL(0.502766), + REAL(-0.277159), REAL(0.126123), REAL(0.443289), + REAL(-0.287605), REAL(-0.005722), REAL(0.531844), + REAL(-0.231396), REAL(-0.121289), REAL(0.587387), + REAL(-0.253475), REAL(-0.081797), REAL(0.756541), + REAL(-0.195164), REAL(-0.137969), REAL(0.728011), + REAL(-0.167673), REAL(-0.156573), REAL(0.609388), + REAL(-0.145917), REAL(-0.169029), REAL(0.697600), + REAL(-0.077776), REAL(-0.214247), REAL(0.622586), + REAL(-0.076873), REAL(-0.214971), REAL(0.696301), + REAL(-0.002341), REAL(-0.233135), REAL(0.622859), + REAL(-0.002730), REAL(-0.213526), REAL(0.691267), + REAL(-0.003136), REAL(-0.192628), REAL(0.762731), + REAL(-0.056136), REAL(-0.201222), REAL(0.763806), + REAL(-0.114589), REAL(-0.166192), REAL(0.770723), + REAL(-0.155145), REAL(-0.129632), REAL(0.791738), + REAL(-0.183611), REAL(-0.058705), REAL(0.847012), + REAL(-0.165562), REAL(0.001980), REAL(0.833386), + REAL(-0.220084), REAL(0.019914), REAL(0.768935), + REAL(-0.255730), REAL(0.090306), REAL(0.670782), + REAL(-0.255594), REAL(0.113833), REAL(0.663389), + REAL(-0.226380), REAL(0.212655), REAL(0.617740), + REAL(-0.003367), REAL(-0.195342), REAL(0.799680), + REAL(-0.029743), REAL(-0.210508), REAL(0.827180), + REAL(-0.003818), REAL(-0.194783), REAL(0.873636), + REAL(-0.004116), REAL(-0.157907), REAL(0.931268), + REAL(-0.031280), REAL(-0.184555), REAL(0.889476), + REAL(-0.059885), REAL(-0.184448), REAL(0.841330), + REAL(-0.135333), REAL(-0.164332), REAL(0.878200), + REAL(-0.085574), REAL(-0.170948), REAL(0.925547), + REAL(-0.163833), REAL(-0.094170), REAL(0.897114), + REAL(-0.138444), REAL(-0.104250), REAL(0.945975), + REAL(-0.083497), REAL(-0.084934), REAL(0.979607), + REAL(-0.004433), REAL(-0.146642), REAL(0.985872), + REAL(-0.150715), REAL(0.032650), REAL(0.884111), + REAL(-0.135892), REAL(-0.035520), REAL(0.945455), + REAL(-0.070612), REAL(0.036849), REAL(0.975733), + REAL(-0.004458), REAL(-0.042526), REAL(1.015670), + REAL(-0.004249), REAL(0.046042), REAL(1.003240), + REAL(-0.086969), REAL(0.133224), REAL(0.947633), + REAL(-0.003873), REAL(0.161605), REAL(0.970499), + REAL(-0.125544), REAL(0.140012), REAL(0.917678), + REAL(-0.125651), REAL(0.250246), REAL(0.857602), + REAL(-0.003127), REAL(0.284070), REAL(0.878870), + REAL(-0.159174), REAL(0.125726), REAL(0.888878), + REAL(-0.183807), REAL(0.196970), REAL(0.844480), + REAL(-0.159890), REAL(0.291736), REAL(0.732480), + REAL(-0.199495), REAL(0.207230), REAL(0.779864), + REAL(-0.206182), REAL(0.164608), REAL(0.693257), + REAL(-0.186315), REAL(0.160689), REAL(0.817193), + REAL(-0.192827), REAL(0.166706), REAL(0.782271), + REAL(-0.175112), REAL(0.110008), REAL(0.860621), + REAL(-0.161022), REAL(0.057420), REAL(0.855111), + REAL(-0.172319), REAL(0.036155), REAL(0.816189), + REAL(-0.190318), REAL(0.064083), REAL(0.760605), + REAL(-0.195072), REAL(0.129179), REAL(0.731104), + REAL(-0.203126), REAL(0.410287), REAL(0.680536), + REAL(-0.216677), REAL(0.309274), REAL(0.642272), + REAL(-0.241515), REAL(0.311485), REAL(0.587832), + REAL(-0.002209), REAL(0.366663), REAL(0.749413), + REAL(-0.088230), REAL(0.396265), REAL(0.678635), + REAL(-0.170147), REAL(0.109517), REAL(0.840784), + REAL(-0.160521), REAL(0.067766), REAL(0.830650), + REAL(-0.181546), REAL(0.139805), REAL(0.812146), + REAL(-0.180495), REAL(0.148568), REAL(0.776087), + REAL(-0.180255), REAL(0.129125), REAL(0.744192), + REAL(-0.186298), REAL(0.078308), REAL(0.769352), + REAL(-0.167622), REAL(0.060539), REAL(0.806675), + REAL(-0.189876), REAL(0.102760), REAL(0.802582), + REAL(-0.108340), REAL(0.455446), REAL(0.657174), + REAL(-0.241585), REAL(0.527592), REAL(0.669296), + REAL(-0.265676), REAL(0.513366), REAL(0.634594), + REAL(-0.203073), REAL(0.478550), REAL(0.581526), + REAL(-0.266772), REAL(0.642330), REAL(0.602061), + REAL(-0.216961), REAL(0.564846), REAL(0.535435), + REAL(-0.202210), REAL(0.525495), REAL(0.475944), + REAL(-0.193888), REAL(0.467925), REAL(0.520606), + REAL(-0.265837), REAL(0.757267), REAL(0.500933), + REAL(-0.240306), REAL(0.653440), REAL(0.463215), + REAL(-0.309239), REAL(0.776868), REAL(0.304726), + REAL(-0.271009), REAL(0.683094), REAL(0.382018), + REAL(-0.312111), REAL(0.671099), REAL(0.286687), + REAL(-0.268791), REAL(0.624342), REAL(0.377231), + REAL(-0.302457), REAL(0.533996), REAL(0.360289), + REAL(-0.263656), REAL(0.529310), REAL(0.412564), + REAL(-0.282311), REAL(0.415167), REAL(0.447666), + REAL(-0.239201), REAL(0.442096), REAL(0.495604), + REAL(-0.220043), REAL(0.569026), REAL(0.445877), + REAL(-0.001263), REAL(0.395631), REAL(0.602029), + REAL(-0.057345), REAL(0.442535), REAL(0.572224), + REAL(-0.088927), REAL(0.506333), REAL(0.529106), + REAL(-0.125738), REAL(0.535076), REAL(0.612913), + REAL(-0.126251), REAL(0.577170), REAL(0.483159), + REAL(-0.149594), REAL(0.611520), REAL(0.557731), + REAL(-0.163188), REAL(0.660791), REAL(0.491080), + REAL(-0.172482), REAL(0.663387), REAL(0.415416), + REAL(-0.160464), REAL(0.591710), REAL(0.370659), + REAL(-0.156445), REAL(0.536396), REAL(0.378302), + REAL(-0.136496), REAL(0.444358), REAL(0.425226), + REAL(-0.095564), REAL(0.373768), REAL(0.473659), + REAL(-0.104146), REAL(0.315912), REAL(0.498104), + REAL(-0.000496), REAL(0.384194), REAL(0.473817), + REAL(-0.000183), REAL(0.297770), REAL(0.401486), + REAL(-0.129042), REAL(0.270145), REAL(0.434495), + REAL(0.000100), REAL(0.272963), REAL(0.349138), + REAL(-0.113060), REAL(0.236984), REAL(0.385554), + REAL(0.007260), REAL(0.016311), REAL(-0.883396), + REAL(0.007865), REAL(0.122104), REAL(-0.956137), + REAL(-0.032842), REAL(0.115282), REAL(-0.953252), + REAL(-0.089115), REAL(0.108449), REAL(-0.950317), + REAL(-0.047440), REAL(0.014729), REAL(-0.882756), + REAL(-0.104458), REAL(0.013137), REAL(-0.882070), + REAL(-0.086439), REAL(-0.584866), REAL(-0.608343), + REAL(-0.115026), REAL(-0.662605), REAL(-0.436732), + REAL(-0.071683), REAL(-0.665372), REAL(-0.606385), + REAL(-0.257884), REAL(-0.665381), REAL(-0.658052), + REAL(-0.272542), REAL(-0.665381), REAL(-0.592063), + REAL(-0.371322), REAL(-0.665382), REAL(-0.353620), + REAL(-0.372362), REAL(-0.665381), REAL(-0.224420), + REAL(-0.335166), REAL(-0.665380), REAL(-0.078623), + REAL(-0.225999), REAL(-0.665375), REAL(-0.038981), + REAL(-0.106719), REAL(-0.665374), REAL(-0.186351), + REAL(-0.081749), REAL(-0.665372), REAL(-0.292554), + REAL(0.006943), REAL(-0.091505), REAL(-0.858354), + REAL(0.006117), REAL(-0.280985), REAL(-0.769967), + REAL(0.004495), REAL(-0.502360), REAL(-0.559799), + REAL(-0.198638), REAL(-0.302135), REAL(-0.845816), + REAL(-0.237395), REAL(-0.542544), REAL(-0.587188), + REAL(-0.270001), REAL(-0.279489), REAL(-0.669861), + REAL(-0.134547), REAL(-0.119852), REAL(-0.959004), + REAL(-0.052088), REAL(-0.122463), REAL(-0.944549), + REAL(-0.124463), REAL(-0.293508), REAL(-0.899566), + REAL(-0.047616), REAL(-0.289643), REAL(-0.879292), + REAL(-0.168595), REAL(-0.529132), REAL(-0.654931), + REAL(-0.099793), REAL(-0.515719), REAL(-0.645873), + REAL(-0.186168), REAL(-0.605282), REAL(-0.724690), + REAL(-0.112970), REAL(-0.583097), REAL(-0.707469), + REAL(-0.108152), REAL(-0.665375), REAL(-0.700408), + REAL(-0.183019), REAL(-0.665378), REAL(-0.717630), + REAL(-0.349529), REAL(-0.334459), REAL(-0.511985), + REAL(-0.141182), REAL(-0.437705), REAL(-0.798194), + REAL(-0.212670), REAL(-0.448725), REAL(-0.737447), + REAL(-0.261111), REAL(-0.414945), REAL(-0.613835), + REAL(-0.077364), REAL(-0.431480), REAL(-0.778113), + REAL(0.005174), REAL(-0.425277), REAL(-0.651592), + REAL(0.089236), REAL(-0.431732), REAL(-0.777093), + REAL(0.271006), REAL(-0.415749), REAL(-0.610577), + REAL(0.223981), REAL(-0.449384), REAL(-0.734774), + REAL(0.153275), REAL(-0.438150), REAL(-0.796391), + REAL(0.358414), REAL(-0.335529), REAL(-0.507649), + REAL(0.193434), REAL(-0.665946), REAL(-0.715325), + REAL(0.118363), REAL(-0.665717), REAL(-0.699021), + REAL(0.123515), REAL(-0.583454), REAL(-0.706020), + REAL(0.196851), REAL(-0.605860), REAL(-0.722345), + REAL(0.109788), REAL(-0.516035), REAL(-0.644590), + REAL(0.178656), REAL(-0.529656), REAL(-0.652804), + REAL(0.061157), REAL(-0.289807), REAL(-0.878626), + REAL(0.138234), REAL(-0.293905), REAL(-0.897958), + REAL(0.066933), REAL(-0.122643), REAL(-0.943820), + REAL(0.149571), REAL(-0.120281), REAL(-0.957264), + REAL(0.280989), REAL(-0.280321), REAL(-0.666487), + REAL(0.246581), REAL(-0.543275), REAL(-0.584224), + REAL(0.211720), REAL(-0.302754), REAL(-0.843303), + REAL(0.086966), REAL(-0.665627), REAL(-0.291520), + REAL(0.110634), REAL(-0.665702), REAL(-0.185021), + REAL(0.228099), REAL(-0.666061), REAL(-0.036201), + REAL(0.337743), REAL(-0.666396), REAL(-0.074503), + REAL(0.376722), REAL(-0.666513), REAL(-0.219833), + REAL(0.377265), REAL(-0.666513), REAL(-0.349036), + REAL(0.281411), REAL(-0.666217), REAL(-0.588670), + REAL(0.267564), REAL(-0.666174), REAL(-0.654834), + REAL(0.080745), REAL(-0.665602), REAL(-0.605452), + REAL(0.122016), REAL(-0.662963), REAL(-0.435280), + REAL(0.095767), REAL(-0.585141), REAL(-0.607228), + REAL(0.118944), REAL(0.012799), REAL(-0.880702), + REAL(0.061944), REAL(0.014564), REAL(-0.882086), + REAL(0.104725), REAL(0.108156), REAL(-0.949130), + REAL(0.048513), REAL(0.115159), REAL(-0.952753), + REAL(0.112696), REAL(0.236643), REAL(0.386937), + REAL(0.128177), REAL(0.269757), REAL(0.436071), + REAL(0.102643), REAL(0.315600), REAL(0.499370), + REAL(0.094535), REAL(0.373481), REAL(0.474824), + REAL(0.136270), REAL(0.443946), REAL(0.426895), + REAL(0.157071), REAL(0.535923), REAL(0.380222), + REAL(0.161350), REAL(0.591224), REAL(0.372630), + REAL(0.173035), REAL(0.662865), REAL(0.417531), + REAL(0.162808), REAL(0.660299), REAL(0.493077), + REAL(0.148250), REAL(0.611070), REAL(0.559555), + REAL(0.125719), REAL(0.576790), REAL(0.484702), + REAL(0.123489), REAL(0.534699), REAL(0.614440), + REAL(0.087621), REAL(0.506066), REAL(0.530188), + REAL(0.055321), REAL(0.442365), REAL(0.572915), + REAL(0.219936), REAL(0.568361), REAL(0.448571), + REAL(0.238099), REAL(0.441375), REAL(0.498528), + REAL(0.281711), REAL(0.414315), REAL(0.451121), + REAL(0.263833), REAL(0.528513), REAL(0.415794), + REAL(0.303284), REAL(0.533081), REAL(0.363998), + REAL(0.269687), REAL(0.623528), REAL(0.380528), + REAL(0.314255), REAL(0.670153), REAL(0.290524), + REAL(0.272023), REAL(0.682273), REAL(0.385343), + REAL(0.311480), REAL(0.775931), REAL(0.308527), + REAL(0.240239), REAL(0.652714), REAL(0.466159), + REAL(0.265619), REAL(0.756464), REAL(0.504187), + REAL(0.192562), REAL(0.467341), REAL(0.522972), + REAL(0.201605), REAL(0.524885), REAL(0.478417), + REAL(0.215743), REAL(0.564193), REAL(0.538084), + REAL(0.264969), REAL(0.641527), REAL(0.605317), + REAL(0.201031), REAL(0.477940), REAL(0.584002), + REAL(0.263086), REAL(0.512567), REAL(0.637832), + REAL(0.238615), REAL(0.526867), REAL(0.672237), + REAL(0.105309), REAL(0.455123), REAL(0.658482), + REAL(0.183993), REAL(0.102195), REAL(0.804872), + REAL(0.161563), REAL(0.060042), REAL(0.808692), + REAL(0.180748), REAL(0.077754), REAL(0.771600), + REAL(0.175168), REAL(0.128588), REAL(0.746368), + REAL(0.175075), REAL(0.148030), REAL(0.778264), + REAL(0.175658), REAL(0.139265), REAL(0.814333), + REAL(0.154191), REAL(0.067291), REAL(0.832578), + REAL(0.163818), REAL(0.109013), REAL(0.842830), + REAL(0.084760), REAL(0.396004), REAL(0.679695), + REAL(0.238888), REAL(0.310760), REAL(0.590775), + REAL(0.213380), REAL(0.308625), REAL(0.644905), + REAL(0.199666), REAL(0.409678), REAL(0.683003), + REAL(0.190143), REAL(0.128597), REAL(0.733463), + REAL(0.184833), REAL(0.063516), REAL(0.762902), + REAL(0.166070), REAL(0.035644), REAL(0.818261), + REAL(0.154361), REAL(0.056943), REAL(0.857042), + REAL(0.168542), REAL(0.109489), REAL(0.862725), + REAL(0.187387), REAL(0.166131), REAL(0.784599), + REAL(0.180428), REAL(0.160135), REAL(0.819438), + REAL(0.201823), REAL(0.163991), REAL(0.695756), + REAL(0.194206), REAL(0.206635), REAL(0.782275), + REAL(0.155438), REAL(0.291260), REAL(0.734412), + REAL(0.177696), REAL(0.196424), REAL(0.846693), + REAL(0.152305), REAL(0.125256), REAL(0.890786), + REAL(0.119546), REAL(0.249876), REAL(0.859104), + REAL(0.118369), REAL(0.139643), REAL(0.919173), + REAL(0.079410), REAL(0.132973), REAL(0.948652), + REAL(0.062419), REAL(0.036648), REAL(0.976547), + REAL(0.127847), REAL(-0.035919), REAL(0.947070), + REAL(0.143624), REAL(0.032206), REAL(0.885913), + REAL(0.074888), REAL(-0.085173), REAL(0.980577), + REAL(0.130184), REAL(-0.104656), REAL(0.947620), + REAL(0.156201), REAL(-0.094653), REAL(0.899074), + REAL(0.077366), REAL(-0.171194), REAL(0.926545), + REAL(0.127722), REAL(-0.164729), REAL(0.879810), + REAL(0.052670), REAL(-0.184618), REAL(0.842019), + REAL(0.023477), REAL(-0.184638), REAL(0.889811), + REAL(0.022626), REAL(-0.210587), REAL(0.827500), + REAL(0.223089), REAL(0.211976), REAL(0.620493), + REAL(0.251444), REAL(0.113067), REAL(0.666494), + REAL(0.251419), REAL(0.089540), REAL(0.673887), + REAL(0.214360), REAL(0.019258), REAL(0.771595), + REAL(0.158999), REAL(0.001490), REAL(0.835374), + REAL(0.176696), REAL(-0.059249), REAL(0.849218), + REAL(0.148696), REAL(-0.130091), REAL(0.793599), + REAL(0.108290), REAL(-0.166528), REAL(0.772088), + REAL(0.049820), REAL(-0.201382), REAL(0.764454), + REAL(0.071341), REAL(-0.215195), REAL(0.697209), + REAL(0.073148), REAL(-0.214475), REAL(0.623510), + REAL(0.140502), REAL(-0.169461), REAL(0.699354), + REAL(0.163374), REAL(-0.157073), REAL(0.611416), + REAL(0.189466), REAL(-0.138550), REAL(0.730366), + REAL(0.247593), REAL(-0.082554), REAL(0.759610), + REAL(0.227468), REAL(-0.121982), REAL(0.590197), + REAL(0.284702), REAL(-0.006586), REAL(0.535347), + REAL(0.275741), REAL(0.125287), REAL(0.446676), + REAL(0.266650), REAL(0.192594), REAL(0.506044), + REAL(0.300086), REAL(0.053287), REAL(0.629620), + REAL(0.055450), REAL(-0.663935), REAL(0.375065), + REAL(0.122854), REAL(-0.664138), REAL(0.482323), + REAL(0.046520), REAL(-0.531571), REAL(0.391918), + REAL(0.024824), REAL(-0.568450), REAL(0.275106), + REAL(0.053855), REAL(-0.663931), REAL(0.328224), + REAL(0.112829), REAL(-0.453549), REAL(0.305788), + REAL(0.131265), REAL(-0.510617), REAL(0.080746), + REAL(0.061174), REAL(-0.430716), REAL(-0.042710), + REAL(0.341019), REAL(-0.532887), REAL(-0.208150), + REAL(0.347705), REAL(-0.623533), REAL(-0.081139), + REAL(0.238040), REAL(-0.610732), REAL(-0.038037), + REAL(0.211764), REAL(-0.514274), REAL(-0.132078), + REAL(0.120605), REAL(-0.600219), REAL(-0.186856), + REAL(0.096985), REAL(-0.584476), REAL(-0.293357), + REAL(0.127621), REAL(-0.581941), REAL(-0.437170), + REAL(0.165902), REAL(-0.477425), REAL(-0.291453), + REAL(0.077720), REAL(-0.417975), REAL(-0.220519), + REAL(0.320892), REAL(-0.506363), REAL(-0.320874), + REAL(0.248214), REAL(-0.465684), REAL(-0.239842), + REAL(0.118764), REAL(-0.383338), REAL(-0.187114), + REAL(0.118816), REAL(-0.430106), REAL(-0.123307), + REAL(0.094131), REAL(-0.419464), REAL(-0.044777), + REAL(0.274526), REAL(-0.261706), REAL(0.005110), + REAL(0.259842), REAL(-0.283292), REAL(-0.003185), + REAL(0.222861), REAL(-0.340431), REAL(-0.038210), + REAL(0.204445), REAL(-0.664380), REAL(0.513353), + REAL(0.259286), REAL(-0.664547), REAL(0.471281), + REAL(0.185402), REAL(-0.476020), REAL(0.421718), + REAL(0.279163), REAL(-0.664604), REAL(0.417328), + REAL(0.277157), REAL(-0.528122), REAL(0.400208), + REAL(0.183069), REAL(-0.509812), REAL(0.329995), + REAL(0.282599), REAL(-0.429210), REAL(0.059242), + REAL(0.254816), REAL(-0.664541), REAL(0.290687), + REAL(0.271436), REAL(-0.567707), REAL(0.263966), + REAL(0.386561), REAL(-0.625221), REAL(-0.216870), + REAL(0.387086), REAL(-0.630883), REAL(-0.346073), + REAL(0.380021), REAL(-0.596021), REAL(-0.318679), + REAL(0.291269), REAL(-0.619007), REAL(-0.585707), + REAL(0.339280), REAL(-0.571198), REAL(-0.461946), + REAL(0.400045), REAL(-0.489778), REAL(-0.422640), + REAL(0.406817), REAL(-0.314349), REAL(-0.371230), + REAL(0.300588), REAL(-0.281718), REAL(-0.170549), + REAL(0.290866), REAL(-0.277304), REAL(-0.061905), + REAL(0.187735), REAL(-0.241545), REAL(0.509437), + REAL(0.188032), REAL(-0.287569), REAL(0.424234), + REAL(0.227520), REAL(-0.373262), REAL(0.293102), + REAL(0.266526), REAL(-0.273650), REAL(0.039597), + REAL(0.291592), REAL(-0.291676), REAL(0.111386), + REAL(0.291914), REAL(-0.122741), REAL(0.422683), + REAL(0.297574), REAL(-0.156119), REAL(0.373368), + REAL(0.286603), REAL(-0.232731), REAL(0.027162), + REAL(0.364663), REAL(-0.201399), REAL(0.206850), + REAL(0.353855), REAL(-0.132408), REAL(0.149228), + REAL(0.282208), REAL(-0.019715), REAL(0.314960), + REAL(0.331187), REAL(-0.099266), REAL(0.092701), + REAL(0.375463), REAL(-0.093120), REAL(-0.006467), + REAL(0.375917), REAL(-0.101236), REAL(-0.154882), + REAL(0.466635), REAL(-0.094416), REAL(-0.305669), + REAL(0.455805), REAL(-0.119881), REAL(-0.460632), + REAL(0.277465), REAL(-0.604242), REAL(-0.651871), + REAL(0.261022), REAL(-0.551176), REAL(-0.554667), + REAL(0.093627), REAL(0.258494), REAL(-0.920589), + REAL(0.114248), REAL(0.310608), REAL(-0.798070), + REAL(0.144232), REAL(0.211434), REAL(-0.835001), + REAL(0.119916), REAL(0.176940), REAL(-0.951159), + REAL(0.184061), REAL(0.101854), REAL(-0.918220), + REAL(0.092431), REAL(0.276521), REAL(-0.738231), + REAL(0.133504), REAL(0.218403), REAL(-0.758602), + REAL(0.194987), REAL(0.097655), REAL(-0.812476), + REAL(0.185542), REAL(0.011005), REAL(-0.879202), + REAL(0.230315), REAL(-0.127450), REAL(-0.884202), + REAL(0.260471), REAL(0.255056), REAL(-0.624378), + REAL(0.351567), REAL(-0.042194), REAL(-0.663976), + REAL(0.253742), REAL(0.323524), REAL(-0.433716), + REAL(0.411612), REAL(0.132299), REAL(-0.438264), + REAL(0.270513), REAL(0.356530), REAL(-0.289984), + REAL(0.422146), REAL(0.162819), REAL(-0.273130), + REAL(0.164724), REAL(0.237490), REAL(0.208912), + REAL(0.253806), REAL(0.092900), REAL(0.240640), + REAL(0.203608), REAL(0.284597), REAL(0.096223), + REAL(0.241006), REAL(0.343093), REAL(-0.171396), + REAL(0.356076), REAL(0.149288), REAL(-0.143443), + REAL(0.337656), REAL(0.131992), REAL(0.066374) +}; + +int 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} +}; + +//============================ + + +dReal heightfield_callback( void* pUserData, int x, int z ) +{ + dIASSERT( x < HFIELD_WSTEP ); + dIASSERT( z < HFIELD_DSTEP ); + + 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 *data, 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) +{ + size_t i; + int j,k; + dReal sides[3]; + dMass m; + + cmd = locase (cmd); + + + // + // Geom Creation + // + + if ( cmd == 'b' || cmd == 's' || cmd == 'c' || + cmd == 'x' || cmd == 'y' || cmd == 'm' || 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()-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); + 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]); + } + //<---- Convex Object + else if (cmd == 'v') + { + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex (space, + planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + 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]); + } +#ifdef dTRIMESH_ENABLED + else if (cmd == 'm') + { + dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (int*)&Indices[0], IndexCount, 3 * sizeof(int)); + + 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] ); + } +#endif + else if (cmd == 'x') + { + dGeomID g2[GPB]; // encapsulated geometries + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + // 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 == '1') { + write_world = 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); + } + //<---- Convex Object + else if (type == dConvexClass) + { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + else if (type == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R,0); + } + + 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) +{ + int i,j; + + dsSetColor (0,0,2); + + dSpaceCollide (space,0,&nearCallback); + + //if (!pause) dWorldStep (world,0.05); + //if (!pause) dWorldQuickStep (world,0.05); + if (!pause) dWorldStepFast1 (world,0.05, 5); + + + 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); + + + + const dReal* pReal = dGeomGetPosition( gheight ); + + const dReal* RReal = dGeomGetRotation( gheight ); + + // + // Draw Heightfield + // + + // 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( pReal, RReal, a, c, b, 1 ); + dsDrawTriangle( pReal, RReal, b, c, d, 1 ); + } + } + + + + + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (i=0; i +#include + +#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() +{ + 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/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + dInitODE(); + 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/libraries/ode-0.9/ode/demo/demo_jointPR.cpp b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp new file mode 100644 index 0000000..e0d002d --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp @@ -0,0 +1,377 @@ +/************************************************************************* + * * + * 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 + + +#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures" + + +#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 dSpaceID box2_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 *data, 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+1 > argc || // Check if we have enough arguments + argv[j] == '\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 + } + } + } + + // 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); + return 0; +} + diff --git a/libraries/ode-0.9/ode/demo/demo_joints.cpp b/libraries/ode-0.9/ode/demo/demo_joints.cpp new file mode 100644 index 0000000..2a83c2f --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_joints.cpp @@ -0,0 +1,1092 @@ +/************************************************************************* + * * + * 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 + +#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 + + +// 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 char loCase (char a) +{ + if (a >= 'A' && a <= 'Z') return a + ('a'-'A'); + else return a; +} + + +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); + dJointSetHinge2Axis1 (joint,0,0,1); + dJointSetHinge2Axis2 (joint,1,0,0); + 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); + dJointSetHinge2Axis1 (joint,0,0,1); + dJointSetHinge2Axis2 (joint,1,0,0); + 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; + return a; + } + 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*dDOT(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*dDOT(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*dDOT(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*dDOT(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() +{ + 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/textures"; + + // 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; + dInitODE(); + + // process the command line args. anything that starts with `-' is assumed + // to be a drawstuff argument. + for (i=1; i +#include + +#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() +{ + 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 *data, 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/textures"; + if(argc>=2) + { + fn.path_to_textures = argv[1]; + } + + // create world + dInitODE(); + 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/libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp b/libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp new file mode 100644 index 0000000..3d03020 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_moving_trimesh.cpp @@ -0,0 +1,1944 @@ +/************************************************************************* + * * + * 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 + +#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 +#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? + +// Bunny mesh ripped from Opcode +const int VertexCount = 453; +const int IndexCount = 902 * 3; + +typedef dReal dVector3R[3]; + +dGeomID TriMesh1; +dGeomID TriMesh2; +static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data + +float Vertices[VertexCount * 3] = { + REAL(-0.334392), REAL(0.133007), REAL(0.062259), + REAL(-0.350189), REAL(0.150354), REAL(-0.147769), + REAL(-0.234201), REAL(0.343811), REAL(-0.174307), + REAL(-0.200259), REAL(0.285207), REAL(0.093749), + REAL(0.003520), REAL(0.475208), REAL(-0.159365), + REAL(0.001856), REAL(0.419203), REAL(0.098582), + REAL(-0.252802), REAL(0.093666), REAL(0.237538), + REAL(-0.162901), REAL(0.237984), REAL(0.206905), + REAL(0.000865), REAL(0.318141), REAL(0.235370), + REAL(-0.414624), REAL(0.164083), REAL(-0.278254), + REAL(-0.262213), REAL(0.357334), REAL(-0.293246), + REAL(0.004628), REAL(0.482694), REAL(-0.338626), + REAL(-0.402162), REAL(0.133528), REAL(-0.443247), + REAL(-0.243781), REAL(0.324275), REAL(-0.436763), + REAL(0.005293), REAL(0.437592), REAL(-0.458332), + REAL(-0.339884), REAL(-0.041150), REAL(-0.668211), + REAL(-0.248382), REAL(0.255825), REAL(-0.627493), + REAL(0.006261), REAL(0.376103), REAL(-0.631506), + REAL(-0.216201), REAL(-0.126776), REAL(-0.886936), + REAL(-0.171075), REAL(0.011544), REAL(-0.881386), + REAL(-0.181074), REAL(0.098223), REAL(-0.814779), + REAL(-0.119891), REAL(0.218786), REAL(-0.760153), + REAL(-0.078895), REAL(0.276780), REAL(-0.739281), + REAL(0.006801), REAL(0.310959), REAL(-0.735661), + REAL(-0.168842), REAL(0.102387), REAL(-0.920381), + REAL(-0.104072), REAL(0.177278), REAL(-0.952530), + REAL(-0.129704), REAL(0.211848), REAL(-0.836678), + REAL(-0.099875), REAL(0.310931), REAL(-0.799381), + REAL(0.007237), REAL(0.361687), REAL(-0.794439), + REAL(-0.077913), REAL(0.258753), REAL(-0.921640), + REAL(0.007957), REAL(0.282241), REAL(-0.931680), + REAL(-0.252222), REAL(-0.550401), REAL(-0.557810), + REAL(-0.267633), REAL(-0.603419), REAL(-0.655209), + REAL(-0.446838), REAL(-0.118517), REAL(-0.466159), + REAL(-0.459488), REAL(-0.093017), REAL(-0.311341), + REAL(-0.370645), REAL(-0.100108), REAL(-0.159454), + REAL(-0.371984), REAL(-0.091991), REAL(-0.011044), + REAL(-0.328945), REAL(-0.098269), REAL(0.088659), + REAL(-0.282452), REAL(-0.018862), REAL(0.311501), + REAL(-0.352403), REAL(-0.131341), REAL(0.144902), + REAL(-0.364126), REAL(-0.200299), REAL(0.202388), + REAL(-0.283965), REAL(-0.231869), REAL(0.023668), + REAL(-0.298943), REAL(-0.155218), REAL(0.369716), + REAL(-0.293787), REAL(-0.121856), REAL(0.419097), + REAL(-0.290163), REAL(-0.290797), REAL(0.107824), + REAL(-0.264165), REAL(-0.272849), REAL(0.036347), + REAL(-0.228567), REAL(-0.372573), REAL(0.290309), + REAL(-0.190431), REAL(-0.286997), REAL(0.421917), + REAL(-0.191039), REAL(-0.240973), REAL(0.507118), + REAL(-0.287272), REAL(-0.276431), REAL(-0.065444), + REAL(-0.295675), REAL(-0.280818), REAL(-0.174200), + REAL(-0.399537), REAL(-0.313131), REAL(-0.376167), + REAL(-0.392666), REAL(-0.488581), REAL(-0.427494), + REAL(-0.331669), REAL(-0.570185), REAL(-0.466054), + REAL(-0.282290), REAL(-0.618140), REAL(-0.589220), + REAL(-0.374238), REAL(-0.594882), REAL(-0.323298), + REAL(-0.381071), REAL(-0.629723), REAL(-0.350777), + REAL(-0.382112), REAL(-0.624060), REAL(-0.221577), + REAL(-0.272701), REAL(-0.566522), REAL(0.259157), + REAL(-0.256702), REAL(-0.663406), REAL(0.286079), + REAL(-0.280948), REAL(-0.428359), REAL(0.055790), + REAL(-0.184974), REAL(-0.508894), REAL(0.326265), + REAL(-0.279971), REAL(-0.526918), REAL(0.395319), + REAL(-0.282599), REAL(-0.663393), REAL(0.412411), + REAL(-0.188329), REAL(-0.475093), REAL(0.417954), + REAL(-0.263384), REAL(-0.663396), REAL(0.466604), + REAL(-0.209063), REAL(-0.663393), REAL(0.509344), + REAL(-0.002044), REAL(-0.319624), REAL(0.553078), + REAL(-0.001266), REAL(-0.371260), REAL(0.413296), + REAL(-0.219753), REAL(-0.339762), REAL(-0.040921), + REAL(-0.256986), REAL(-0.282511), REAL(-0.006349), + REAL(-0.271706), REAL(-0.260881), REAL(0.001764), + REAL(-0.091191), REAL(-0.419184), REAL(-0.045912), + REAL(-0.114944), REAL(-0.429752), REAL(-0.124739), + REAL(-0.113970), REAL(-0.382987), REAL(-0.188540), + REAL(-0.243012), REAL(-0.464942), REAL(-0.242850), + REAL(-0.314815), REAL(-0.505402), REAL(-0.324768), + REAL(0.002774), REAL(-0.437526), REAL(-0.262766), + REAL(-0.072625), REAL(-0.417748), REAL(-0.221440), + REAL(-0.160112), REAL(-0.476932), REAL(-0.293450), + REAL(0.003859), REAL(-0.453425), REAL(-0.443916), + REAL(-0.120363), REAL(-0.581567), REAL(-0.438689), + REAL(-0.091499), REAL(-0.584191), REAL(-0.294511), + REAL(-0.116469), REAL(-0.599861), REAL(-0.188308), + REAL(-0.208032), REAL(-0.513640), REAL(-0.134649), + REAL(-0.235749), REAL(-0.610017), REAL(-0.040939), + REAL(-0.344916), REAL(-0.622487), REAL(-0.085380), + REAL(-0.336401), REAL(-0.531864), REAL(-0.212298), + REAL(0.001961), REAL(-0.459550), REAL(-0.135547), + REAL(-0.058296), REAL(-0.430536), REAL(-0.043440), + REAL(0.001378), REAL(-0.449511), REAL(-0.037762), + REAL(-0.130135), REAL(-0.510222), REAL(0.079144), + REAL(0.000142), REAL(-0.477549), REAL(0.157064), + REAL(-0.114284), REAL(-0.453206), REAL(0.304397), + REAL(-0.000592), REAL(-0.443558), REAL(0.285401), + REAL(-0.056215), REAL(-0.663402), REAL(0.326073), + REAL(-0.026248), REAL(-0.568010), REAL(0.273318), + REAL(-0.049261), REAL(-0.531064), REAL(0.389854), + REAL(-0.127096), REAL(-0.663398), REAL(0.479316), + REAL(-0.058384), REAL(-0.663401), REAL(0.372891), + REAL(-0.303961), REAL(0.054199), REAL(0.625921), + REAL(-0.268594), REAL(0.193403), REAL(0.502766), + REAL(-0.277159), REAL(0.126123), REAL(0.443289), + REAL(-0.287605), REAL(-0.005722), REAL(0.531844), + REAL(-0.231396), REAL(-0.121289), REAL(0.587387), + REAL(-0.253475), REAL(-0.081797), REAL(0.756541), + REAL(-0.195164), REAL(-0.137969), REAL(0.728011), + REAL(-0.167673), REAL(-0.156573), REAL(0.609388), + REAL(-0.145917), REAL(-0.169029), REAL(0.697600), + REAL(-0.077776), REAL(-0.214247), REAL(0.622586), + REAL(-0.076873), REAL(-0.214971), REAL(0.696301), + REAL(-0.002341), REAL(-0.233135), REAL(0.622859), + REAL(-0.002730), REAL(-0.213526), REAL(0.691267), + REAL(-0.003136), REAL(-0.192628), REAL(0.762731), + REAL(-0.056136), REAL(-0.201222), REAL(0.763806), + REAL(-0.114589), REAL(-0.166192), REAL(0.770723), + REAL(-0.155145), REAL(-0.129632), REAL(0.791738), + REAL(-0.183611), REAL(-0.058705), REAL(0.847012), + REAL(-0.165562), REAL(0.001980), REAL(0.833386), + REAL(-0.220084), REAL(0.019914), REAL(0.768935), + REAL(-0.255730), REAL(0.090306), REAL(0.670782), + REAL(-0.255594), REAL(0.113833), REAL(0.663389), + REAL(-0.226380), REAL(0.212655), REAL(0.617740), + REAL(-0.003367), REAL(-0.195342), REAL(0.799680), + REAL(-0.029743), REAL(-0.210508), REAL(0.827180), + REAL(-0.003818), REAL(-0.194783), REAL(0.873636), + REAL(-0.004116), REAL(-0.157907), REAL(0.931268), + REAL(-0.031280), REAL(-0.184555), REAL(0.889476), + REAL(-0.059885), REAL(-0.184448), REAL(0.841330), + REAL(-0.135333), REAL(-0.164332), REAL(0.878200), + REAL(-0.085574), REAL(-0.170948), REAL(0.925547), + REAL(-0.163833), REAL(-0.094170), REAL(0.897114), + REAL(-0.138444), REAL(-0.104250), REAL(0.945975), + REAL(-0.083497), REAL(-0.084934), REAL(0.979607), + REAL(-0.004433), REAL(-0.146642), REAL(0.985872), + REAL(-0.150715), REAL(0.032650), REAL(0.884111), + REAL(-0.135892), REAL(-0.035520), REAL(0.945455), + REAL(-0.070612), REAL(0.036849), REAL(0.975733), + REAL(-0.004458), REAL(-0.042526), REAL(1.015670), + REAL(-0.004249), REAL(0.046042), REAL(1.003240), + REAL(-0.086969), REAL(0.133224), REAL(0.947633), + REAL(-0.003873), REAL(0.161605), REAL(0.970499), + REAL(-0.125544), REAL(0.140012), REAL(0.917678), + REAL(-0.125651), REAL(0.250246), REAL(0.857602), + REAL(-0.003127), REAL(0.284070), REAL(0.878870), + REAL(-0.159174), REAL(0.125726), REAL(0.888878), + REAL(-0.183807), REAL(0.196970), REAL(0.844480), + REAL(-0.159890), REAL(0.291736), REAL(0.732480), + REAL(-0.199495), REAL(0.207230), REAL(0.779864), + REAL(-0.206182), REAL(0.164608), REAL(0.693257), + REAL(-0.186315), REAL(0.160689), REAL(0.817193), + REAL(-0.192827), REAL(0.166706), REAL(0.782271), + REAL(-0.175112), REAL(0.110008), REAL(0.860621), + REAL(-0.161022), REAL(0.057420), REAL(0.855111), + REAL(-0.172319), REAL(0.036155), REAL(0.816189), + REAL(-0.190318), REAL(0.064083), REAL(0.760605), + REAL(-0.195072), REAL(0.129179), REAL(0.731104), + REAL(-0.203126), REAL(0.410287), REAL(0.680536), + REAL(-0.216677), REAL(0.309274), REAL(0.642272), + REAL(-0.241515), REAL(0.311485), REAL(0.587832), + REAL(-0.002209), REAL(0.366663), REAL(0.749413), + REAL(-0.088230), REAL(0.396265), REAL(0.678635), + REAL(-0.170147), REAL(0.109517), REAL(0.840784), + REAL(-0.160521), REAL(0.067766), REAL(0.830650), + REAL(-0.181546), REAL(0.139805), REAL(0.812146), + REAL(-0.180495), REAL(0.148568), REAL(0.776087), + REAL(-0.180255), REAL(0.129125), REAL(0.744192), + REAL(-0.186298), REAL(0.078308), REAL(0.769352), + REAL(-0.167622), REAL(0.060539), REAL(0.806675), + REAL(-0.189876), REAL(0.102760), REAL(0.802582), + REAL(-0.108340), REAL(0.455446), REAL(0.657174), + REAL(-0.241585), REAL(0.527592), REAL(0.669296), + REAL(-0.265676), REAL(0.513366), REAL(0.634594), + REAL(-0.203073), REAL(0.478550), REAL(0.581526), + REAL(-0.266772), REAL(0.642330), REAL(0.602061), + REAL(-0.216961), REAL(0.564846), REAL(0.535435), + REAL(-0.202210), REAL(0.525495), REAL(0.475944), + REAL(-0.193888), REAL(0.467925), REAL(0.520606), + REAL(-0.265837), REAL(0.757267), REAL(0.500933), + REAL(-0.240306), REAL(0.653440), REAL(0.463215), + REAL(-0.309239), REAL(0.776868), REAL(0.304726), + REAL(-0.271009), REAL(0.683094), REAL(0.382018), + REAL(-0.312111), REAL(0.671099), REAL(0.286687), + REAL(-0.268791), REAL(0.624342), REAL(0.377231), + REAL(-0.302457), REAL(0.533996), REAL(0.360289), + REAL(-0.263656), REAL(0.529310), REAL(0.412564), + REAL(-0.282311), REAL(0.415167), REAL(0.447666), + REAL(-0.239201), REAL(0.442096), REAL(0.495604), + REAL(-0.220043), REAL(0.569026), REAL(0.445877), + REAL(-0.001263), REAL(0.395631), REAL(0.602029), + REAL(-0.057345), REAL(0.442535), REAL(0.572224), + REAL(-0.088927), REAL(0.506333), REAL(0.529106), + REAL(-0.125738), REAL(0.535076), REAL(0.612913), + REAL(-0.126251), REAL(0.577170), REAL(0.483159), + REAL(-0.149594), REAL(0.611520), REAL(0.557731), + REAL(-0.163188), REAL(0.660791), REAL(0.491080), + REAL(-0.172482), REAL(0.663387), REAL(0.415416), + REAL(-0.160464), REAL(0.591710), REAL(0.370659), + REAL(-0.156445), REAL(0.536396), REAL(0.378302), + REAL(-0.136496), REAL(0.444358), REAL(0.425226), + REAL(-0.095564), REAL(0.373768), REAL(0.473659), + REAL(-0.104146), REAL(0.315912), REAL(0.498104), + REAL(-0.000496), REAL(0.384194), REAL(0.473817), + REAL(-0.000183), REAL(0.297770), REAL(0.401486), + REAL(-0.129042), REAL(0.270145), REAL(0.434495), + REAL(0.000100), REAL(0.272963), REAL(0.349138), + REAL(-0.113060), REAL(0.236984), REAL(0.385554), + REAL(0.007260), REAL(0.016311), REAL(-0.883396), + REAL(0.007865), REAL(0.122104), REAL(-0.956137), + REAL(-0.032842), REAL(0.115282), REAL(-0.953252), + REAL(-0.089115), REAL(0.108449), REAL(-0.950317), + REAL(-0.047440), REAL(0.014729), REAL(-0.882756), + REAL(-0.104458), REAL(0.013137), REAL(-0.882070), + REAL(-0.086439), REAL(-0.584866), REAL(-0.608343), + REAL(-0.115026), REAL(-0.662605), REAL(-0.436732), + REAL(-0.071683), REAL(-0.665372), REAL(-0.606385), + REAL(-0.257884), REAL(-0.665381), REAL(-0.658052), + REAL(-0.272542), REAL(-0.665381), REAL(-0.592063), + REAL(-0.371322), REAL(-0.665382), REAL(-0.353620), + REAL(-0.372362), REAL(-0.665381), REAL(-0.224420), + REAL(-0.335166), REAL(-0.665380), REAL(-0.078623), + REAL(-0.225999), REAL(-0.665375), REAL(-0.038981), + REAL(-0.106719), REAL(-0.665374), REAL(-0.186351), + REAL(-0.081749), REAL(-0.665372), REAL(-0.292554), + REAL(0.006943), REAL(-0.091505), REAL(-0.858354), + REAL(0.006117), REAL(-0.280985), REAL(-0.769967), + REAL(0.004495), REAL(-0.502360), REAL(-0.559799), + REAL(-0.198638), REAL(-0.302135), REAL(-0.845816), + REAL(-0.237395), REAL(-0.542544), REAL(-0.587188), + REAL(-0.270001), REAL(-0.279489), REAL(-0.669861), + REAL(-0.134547), REAL(-0.119852), REAL(-0.959004), + REAL(-0.052088), REAL(-0.122463), REAL(-0.944549), + REAL(-0.124463), REAL(-0.293508), REAL(-0.899566), + REAL(-0.047616), REAL(-0.289643), REAL(-0.879292), + REAL(-0.168595), REAL(-0.529132), REAL(-0.654931), + REAL(-0.099793), REAL(-0.515719), REAL(-0.645873), + REAL(-0.186168), REAL(-0.605282), REAL(-0.724690), + REAL(-0.112970), REAL(-0.583097), REAL(-0.707469), + REAL(-0.108152), REAL(-0.665375), REAL(-0.700408), + REAL(-0.183019), REAL(-0.665378), REAL(-0.717630), + REAL(-0.349529), REAL(-0.334459), REAL(-0.511985), + REAL(-0.141182), REAL(-0.437705), REAL(-0.798194), + REAL(-0.212670), REAL(-0.448725), REAL(-0.737447), + REAL(-0.261111), REAL(-0.414945), REAL(-0.613835), + REAL(-0.077364), REAL(-0.431480), REAL(-0.778113), + REAL(0.005174), REAL(-0.425277), REAL(-0.651592), + REAL(0.089236), REAL(-0.431732), REAL(-0.777093), + REAL(0.271006), REAL(-0.415749), REAL(-0.610577), + REAL(0.223981), REAL(-0.449384), REAL(-0.734774), + REAL(0.153275), REAL(-0.438150), REAL(-0.796391), + REAL(0.358414), REAL(-0.335529), REAL(-0.507649), + REAL(0.193434), REAL(-0.665946), REAL(-0.715325), + REAL(0.118363), REAL(-0.665717), REAL(-0.699021), + REAL(0.123515), REAL(-0.583454), REAL(-0.706020), + REAL(0.196851), REAL(-0.605860), REAL(-0.722345), + REAL(0.109788), REAL(-0.516035), REAL(-0.644590), + REAL(0.178656), REAL(-0.529656), REAL(-0.652804), + REAL(0.061157), REAL(-0.289807), REAL(-0.878626), + REAL(0.138234), REAL(-0.293905), REAL(-0.897958), + REAL(0.066933), REAL(-0.122643), REAL(-0.943820), + REAL(0.149571), REAL(-0.120281), REAL(-0.957264), + REAL(0.280989), REAL(-0.280321), REAL(-0.666487), + REAL(0.246581), REAL(-0.543275), REAL(-0.584224), + REAL(0.211720), REAL(-0.302754), REAL(-0.843303), + REAL(0.086966), REAL(-0.665627), REAL(-0.291520), + REAL(0.110634), REAL(-0.665702), REAL(-0.185021), + REAL(0.228099), REAL(-0.666061), REAL(-0.036201), + REAL(0.337743), REAL(-0.666396), REAL(-0.074503), + REAL(0.376722), REAL(-0.666513), REAL(-0.219833), + REAL(0.377265), REAL(-0.666513), REAL(-0.349036), + REAL(0.281411), REAL(-0.666217), REAL(-0.588670), + REAL(0.267564), REAL(-0.666174), REAL(-0.654834), + REAL(0.080745), REAL(-0.665602), REAL(-0.605452), + REAL(0.122016), REAL(-0.662963), REAL(-0.435280), + REAL(0.095767), REAL(-0.585141), REAL(-0.607228), + REAL(0.118944), REAL(0.012799), REAL(-0.880702), + REAL(0.061944), REAL(0.014564), REAL(-0.882086), + REAL(0.104725), REAL(0.108156), REAL(-0.949130), + REAL(0.048513), REAL(0.115159), REAL(-0.952753), + REAL(0.112696), REAL(0.236643), REAL(0.386937), + REAL(0.128177), REAL(0.269757), REAL(0.436071), + REAL(0.102643), REAL(0.315600), REAL(0.499370), + REAL(0.094535), REAL(0.373481), REAL(0.474824), + REAL(0.136270), REAL(0.443946), REAL(0.426895), + REAL(0.157071), REAL(0.535923), REAL(0.380222), + REAL(0.161350), REAL(0.591224), REAL(0.372630), + REAL(0.173035), REAL(0.662865), REAL(0.417531), + REAL(0.162808), REAL(0.660299), REAL(0.493077), + REAL(0.148250), REAL(0.611070), REAL(0.559555), + REAL(0.125719), REAL(0.576790), REAL(0.484702), + REAL(0.123489), REAL(0.534699), REAL(0.614440), + REAL(0.087621), REAL(0.506066), REAL(0.530188), + REAL(0.055321), REAL(0.442365), REAL(0.572915), + REAL(0.219936), REAL(0.568361), REAL(0.448571), + REAL(0.238099), REAL(0.441375), REAL(0.498528), + REAL(0.281711), REAL(0.414315), REAL(0.451121), + REAL(0.263833), REAL(0.528513), REAL(0.415794), + REAL(0.303284), REAL(0.533081), REAL(0.363998), + REAL(0.269687), REAL(0.623528), REAL(0.380528), + REAL(0.314255), REAL(0.670153), REAL(0.290524), + REAL(0.272023), REAL(0.682273), REAL(0.385343), + REAL(0.311480), REAL(0.775931), REAL(0.308527), + REAL(0.240239), REAL(0.652714), REAL(0.466159), + REAL(0.265619), REAL(0.756464), REAL(0.504187), + REAL(0.192562), REAL(0.467341), REAL(0.522972), + REAL(0.201605), REAL(0.524885), REAL(0.478417), + REAL(0.215743), REAL(0.564193), REAL(0.538084), + REAL(0.264969), REAL(0.641527), REAL(0.605317), + REAL(0.201031), REAL(0.477940), REAL(0.584002), + REAL(0.263086), REAL(0.512567), REAL(0.637832), + REAL(0.238615), REAL(0.526867), REAL(0.672237), + REAL(0.105309), REAL(0.455123), REAL(0.658482), + REAL(0.183993), REAL(0.102195), REAL(0.804872), + REAL(0.161563), REAL(0.060042), REAL(0.808692), + REAL(0.180748), REAL(0.077754), REAL(0.771600), + REAL(0.175168), REAL(0.128588), REAL(0.746368), + REAL(0.175075), REAL(0.148030), REAL(0.778264), + REAL(0.175658), REAL(0.139265), REAL(0.814333), + REAL(0.154191), REAL(0.067291), REAL(0.832578), + REAL(0.163818), REAL(0.109013), REAL(0.842830), + REAL(0.084760), REAL(0.396004), REAL(0.679695), + REAL(0.238888), REAL(0.310760), REAL(0.590775), + REAL(0.213380), REAL(0.308625), REAL(0.644905), + REAL(0.199666), REAL(0.409678), REAL(0.683003), + REAL(0.190143), REAL(0.128597), REAL(0.733463), + REAL(0.184833), REAL(0.063516), REAL(0.762902), + REAL(0.166070), REAL(0.035644), REAL(0.818261), + REAL(0.154361), REAL(0.056943), REAL(0.857042), + REAL(0.168542), REAL(0.109489), REAL(0.862725), + REAL(0.187387), REAL(0.166131), REAL(0.784599), + REAL(0.180428), REAL(0.160135), REAL(0.819438), + REAL(0.201823), REAL(0.163991), REAL(0.695756), + REAL(0.194206), REAL(0.206635), REAL(0.782275), + REAL(0.155438), REAL(0.291260), REAL(0.734412), + REAL(0.177696), REAL(0.196424), REAL(0.846693), + REAL(0.152305), REAL(0.125256), REAL(0.890786), + REAL(0.119546), REAL(0.249876), REAL(0.859104), + REAL(0.118369), REAL(0.139643), REAL(0.919173), + REAL(0.079410), REAL(0.132973), REAL(0.948652), + REAL(0.062419), REAL(0.036648), REAL(0.976547), + REAL(0.127847), REAL(-0.035919), REAL(0.947070), + REAL(0.143624), REAL(0.032206), REAL(0.885913), + REAL(0.074888), REAL(-0.085173), REAL(0.980577), + REAL(0.130184), REAL(-0.104656), REAL(0.947620), + REAL(0.156201), REAL(-0.094653), REAL(0.899074), + REAL(0.077366), REAL(-0.171194), REAL(0.926545), + REAL(0.127722), REAL(-0.164729), REAL(0.879810), + REAL(0.052670), REAL(-0.184618), REAL(0.842019), + REAL(0.023477), REAL(-0.184638), REAL(0.889811), + REAL(0.022626), REAL(-0.210587), REAL(0.827500), + REAL(0.223089), REAL(0.211976), REAL(0.620493), + REAL(0.251444), REAL(0.113067), REAL(0.666494), + REAL(0.251419), REAL(0.089540), REAL(0.673887), + REAL(0.214360), REAL(0.019258), REAL(0.771595), + REAL(0.158999), REAL(0.001490), REAL(0.835374), + REAL(0.176696), REAL(-0.059249), REAL(0.849218), + REAL(0.148696), REAL(-0.130091), REAL(0.793599), + REAL(0.108290), REAL(-0.166528), REAL(0.772088), + REAL(0.049820), REAL(-0.201382), REAL(0.764454), + REAL(0.071341), REAL(-0.215195), REAL(0.697209), + REAL(0.073148), REAL(-0.214475), REAL(0.623510), + REAL(0.140502), REAL(-0.169461), REAL(0.699354), + REAL(0.163374), REAL(-0.157073), REAL(0.611416), + REAL(0.189466), REAL(-0.138550), REAL(0.730366), + REAL(0.247593), REAL(-0.082554), REAL(0.759610), + REAL(0.227468), REAL(-0.121982), REAL(0.590197), + REAL(0.284702), REAL(-0.006586), REAL(0.535347), + REAL(0.275741), REAL(0.125287), REAL(0.446676), + REAL(0.266650), REAL(0.192594), REAL(0.506044), + REAL(0.300086), REAL(0.053287), REAL(0.629620), + REAL(0.055450), REAL(-0.663935), REAL(0.375065), + REAL(0.122854), REAL(-0.664138), REAL(0.482323), + REAL(0.046520), REAL(-0.531571), REAL(0.391918), + REAL(0.024824), REAL(-0.568450), REAL(0.275106), + REAL(0.053855), REAL(-0.663931), REAL(0.328224), + REAL(0.112829), REAL(-0.453549), REAL(0.305788), + REAL(0.131265), REAL(-0.510617), REAL(0.080746), + REAL(0.061174), REAL(-0.430716), REAL(-0.042710), + REAL(0.341019), REAL(-0.532887), REAL(-0.208150), + REAL(0.347705), REAL(-0.623533), REAL(-0.081139), + REAL(0.238040), REAL(-0.610732), REAL(-0.038037), + REAL(0.211764), REAL(-0.514274), REAL(-0.132078), + REAL(0.120605), REAL(-0.600219), REAL(-0.186856), + REAL(0.096985), REAL(-0.584476), REAL(-0.293357), + REAL(0.127621), REAL(-0.581941), REAL(-0.437170), + REAL(0.165902), REAL(-0.477425), REAL(-0.291453), + REAL(0.077720), REAL(-0.417975), REAL(-0.220519), + REAL(0.320892), REAL(-0.506363), REAL(-0.320874), + REAL(0.248214), REAL(-0.465684), REAL(-0.239842), + REAL(0.118764), REAL(-0.383338), REAL(-0.187114), + REAL(0.118816), REAL(-0.430106), REAL(-0.123307), + REAL(0.094131), REAL(-0.419464), REAL(-0.044777), + REAL(0.274526), REAL(-0.261706), REAL(0.005110), + REAL(0.259842), REAL(-0.283292), REAL(-0.003185), + REAL(0.222861), REAL(-0.340431), REAL(-0.038210), + REAL(0.204445), REAL(-0.664380), REAL(0.513353), + REAL(0.259286), REAL(-0.664547), REAL(0.471281), + REAL(0.185402), REAL(-0.476020), REAL(0.421718), + REAL(0.279163), REAL(-0.664604), REAL(0.417328), + REAL(0.277157), REAL(-0.528122), REAL(0.400208), + REAL(0.183069), REAL(-0.509812), REAL(0.329995), + REAL(0.282599), REAL(-0.429210), REAL(0.059242), + REAL(0.254816), REAL(-0.664541), REAL(0.290687), + REAL(0.271436), REAL(-0.567707), REAL(0.263966), + REAL(0.386561), REAL(-0.625221), REAL(-0.216870), + REAL(0.387086), REAL(-0.630883), REAL(-0.346073), + REAL(0.380021), REAL(-0.596021), REAL(-0.318679), + REAL(0.291269), REAL(-0.619007), REAL(-0.585707), + REAL(0.339280), REAL(-0.571198), REAL(-0.461946), + REAL(0.400045), REAL(-0.489778), REAL(-0.422640), + REAL(0.406817), REAL(-0.314349), REAL(-0.371230), + REAL(0.300588), REAL(-0.281718), REAL(-0.170549), + REAL(0.290866), REAL(-0.277304), REAL(-0.061905), + REAL(0.187735), REAL(-0.241545), REAL(0.509437), + REAL(0.188032), REAL(-0.287569), REAL(0.424234), + REAL(0.227520), REAL(-0.373262), REAL(0.293102), + REAL(0.266526), REAL(-0.273650), REAL(0.039597), + REAL(0.291592), REAL(-0.291676), REAL(0.111386), + REAL(0.291914), REAL(-0.122741), REAL(0.422683), + REAL(0.297574), REAL(-0.156119), REAL(0.373368), + REAL(0.286603), REAL(-0.232731), REAL(0.027162), + REAL(0.364663), REAL(-0.201399), REAL(0.206850), + REAL(0.353855), REAL(-0.132408), REAL(0.149228), + REAL(0.282208), REAL(-0.019715), REAL(0.314960), + REAL(0.331187), REAL(-0.099266), REAL(0.092701), + REAL(0.375463), REAL(-0.093120), REAL(-0.006467), + REAL(0.375917), REAL(-0.101236), REAL(-0.154882), + REAL(0.466635), REAL(-0.094416), REAL(-0.305669), + REAL(0.455805), REAL(-0.119881), REAL(-0.460632), + REAL(0.277465), REAL(-0.604242), REAL(-0.651871), + REAL(0.261022), REAL(-0.551176), REAL(-0.554667), + REAL(0.093627), REAL(0.258494), REAL(-0.920589), + REAL(0.114248), REAL(0.310608), REAL(-0.798070), + REAL(0.144232), REAL(0.211434), REAL(-0.835001), + REAL(0.119916), REAL(0.176940), REAL(-0.951159), + REAL(0.184061), REAL(0.101854), REAL(-0.918220), + REAL(0.092431), REAL(0.276521), REAL(-0.738231), + REAL(0.133504), REAL(0.218403), REAL(-0.758602), + REAL(0.194987), REAL(0.097655), REAL(-0.812476), + REAL(0.185542), REAL(0.011005), REAL(-0.879202), + REAL(0.230315), REAL(-0.127450), REAL(-0.884202), + REAL(0.260471), REAL(0.255056), REAL(-0.624378), + REAL(0.351567), REAL(-0.042194), REAL(-0.663976), + REAL(0.253742), REAL(0.323524), REAL(-0.433716), + REAL(0.411612), REAL(0.132299), REAL(-0.438264), + REAL(0.270513), REAL(0.356530), REAL(-0.289984), + REAL(0.422146), REAL(0.162819), REAL(-0.273130), + REAL(0.164724), REAL(0.237490), REAL(0.208912), + REAL(0.253806), REAL(0.092900), REAL(0.240640), + REAL(0.203608), REAL(0.284597), REAL(0.096223), + REAL(0.241006), REAL(0.343093), REAL(-0.171396), + REAL(0.356076), REAL(0.149288), REAL(-0.143443), + REAL(0.337656), REAL(0.131992), REAL(0.066374) +}; + +int 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} +}; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, 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; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || 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*)(size_t)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') { + 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, (int*)&Indices[0], IndexCount, 3 * sizeof(int)); + + 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') { + dGeomID g2[GPB]; // encapsulated geometries + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + // 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 == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R,0); + } + + 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 0 + // 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 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); + + dCROSS (a1,=,b,c); + + dSetZero (B,12); + dCROSSMAT (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(dDOT(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(dDOT(n2,n1) - dSqrt(dDOT(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(dDOT(n,p)) > tol) bad = 1; + if (fabs(dDOT(n,q)) > tol) bad = 1; + if (fabs(dDOT(p,q)) > tol) bad = 1; + if (fabs(dDOT(p,p)-1) > tol) bad = 1; + if (fabs(dDOT(q,q)-1) > tol) bad = 1; + } + printf ("\t%s\n", bad ? "FAILED" : "passed"); +} + +//**************************************************************************** +// test matrix functions + +#define MSIZE 21 +#define MSIZE4 24 // MSIZE rounded up to 4 + + +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() +{ + dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE], C[MSIZE4*MSIZE], 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"); +} + + +void testCholeskySolve() +{ + dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], b[MSIZE],x[MSIZE],btest[MSIZE],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"); +} + + +void testInvertPDMatrix() +{ + int i,j,ok; + dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE]; + 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"); +} + + +void testSolveLDLT() +{ + dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], x[MSIZE], + b[MSIZE], btest[MSIZE], 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"); +} + + +void testLDLTAddTL() +{ + int i,j; + dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], a[MSIZE], + DL[MSIZE4*MSIZE], ATEST[MSIZE4*MSIZE], 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"); +} + + +void testLDLTRemove() +{ + int i,j,r,p[MSIZE]; + dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], + L2[MSIZE4*MSIZE], d2[MSIZE], DL2[MSIZE4*MSIZE], + Atest1[MSIZE4*MSIZE], Atest2[MSIZE4*MSIZE], 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"); +} + +//**************************************************************************** +// 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, 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.57537403079093) && + cmp(m._I(2,2),4.57537403079093) && + 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 "../src/array.h" +#include "../src/array.cpp" + +// matrix header on the stack + +class dMatrixComparison { + struct dMatInfo; + dArray mat; + int afterfirst,index; + +public: + dMatrixComparison(); + ~dMatrixComparison(); + + dReal nextMatrix (dReal *A, int n, int m, int lower_tri, 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, + 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); + if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long"); + + mat.push (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); + 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.setSize (0); + 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" void dTestSolveLCP(); + + +int main() +{ + dInitODE(); + testRandomNumberGenerator(); + testInfinity(); + testPad(); + testCrossProduct(); + testSetZero(); + testNormalize3(); + //testReorthonormalize(); ... not any more + testPlaneSpace(); + testMatrixMultiply(); + testSmallMatrixMultiply(); + testCholeskyFactorization(); + testCholeskySolve(); + testInvertPDMatrix(); + testIsPositiveDefinite(); + testFastLDLTFactorization(); + testSolveLDLT(); + testLDLTAddTL(); + testLDLTRemove(); + testMassFunctions(); + testRtoQandQtoR(); + testQuaternionMultiply(); + testRotationFunctions(); + dTestMatrixComparison(); + dTestSolveLCP(); + // dTestDataStructures(); + dCloseODE(); + return 0; +} diff --git a/libraries/ode-0.9/ode/demo/demo_plane2d.cpp b/libraries/ode-0.9/ode/demo/demo_plane2d.cpp new file mode 100644 index 0000000..3ba4df8 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_plane2d.cpp @@ -0,0 +1,268 @@ +// Test my Plane2D constraint. +// Uses ode-0.35 collision API. + +# include +# include +# include +# include +# include + +# 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 + + +static dWorld dyn_world; +static dBody dyn_bodies[N_BODIES]; +static dReal bodies_sides[N_BODIES][3]; + +static dSpaceID coll_space_id; +static dJointID plane2d_joint_ids[N_BODIES]; +static dJointGroup + coll_contacts; + + + +static void cb_start () +/*************************/ +{ + 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 *data, 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 (dyn_world.id(), + 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 (dyn_bodies[0], 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; */ + /* dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */ + /* } */ + /* dyn_bodies[0].addTorque (0, 0, 0.1); */ + + const int n = 10; + for (int i = 0; i < n; i ++) + { + dSpaceCollide (coll_space_id, 0, &cb_near_collision); + dyn_world.step (dReal(TIME_STEP/n)); + coll_contacts.empty (); + } + } + +# if 1 /* [ */ + { + // @@@ hack Plane2D constraint error reduction here: + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *rot = dBodyGetAngularVel (dyn_bodies[b].id()); + const dReal *quat_ptr; + dReal quat[4], + quat_len; + + + quat_ptr = dBodyGetQuaternion (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 (dyn_bodies[b].id(), quat); + dBodySetAngularVel (dyn_bodies[b].id(), 0, 0, rot[2]); + } + } +# endif /* ] */ + + +# if 0 /* [ */ + { + // @@@ friction + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *vel = dBodyGetLinearVel (dyn_bodies[b].id()), + *rot = dBodyGetAngularVel (dyn_bodies[b].id()); + dReal s = 1.00; + dReal t = 0.99; + + dBodySetLinearVel (dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]); + dBodySetAngularVel (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 (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]); +#else + dsDrawBox (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]); +#endif + } + } +} + + + +extern int main +/******************/ +( + int argc, + char **argv +) +{ + int b; + dsFunctions drawstuff_functions; + + + dInitODE(); + + // 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 (dyn_world.id (), err_reduct); + dWorldSetCFM (dyn_world.id (), cf_mixing); + dyn_world.setGravity (0, 0.0, -1.0); + + 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(); + + bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); + bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) ); + bodies_sides[b][2] = z; + + dyn_bodies[b].create (dyn_world); + dyn_bodies[b].setPosition (x, y, z/2); + dyn_bodies[b].setData ((void*) (size_t)b); + dBodySetLinearVel (dyn_bodies[b].id (), + dReal( 3 * (drand48 () - 0.5) ), + dReal( 3 * (drand48 () - 0.5) ), 0); + + dMass m; + m.setBox (1, bodies_sides[b][0],bodies_sides[b][1],bodies_sides[b][2]); + m.adjust (REAL(0.1) * bodies_sides[b][0] * bodies_sides[b][1]); + dyn_bodies[b].setMass (&m); + + plane2d_joint_ids[b] = dJointCreatePlane2D (dyn_world.id (), 0); + dJointAttach (plane2d_joint_ids[b], dyn_bodies[b].id (), 0); + } + + dJointSetPlane2DXParam (plane2d_joint_ids[0], dParamFMax, 10); + dJointSetPlane2DYParam (plane2d_joint_ids[0], dParamFMax, 10); + + + // collision geoms and joints + dCreatePlane (coll_space_id, 1, 0, 0, 0); + dCreatePlane (coll_space_id, -1, 0, 0, -STAGE_SIZE); + dCreatePlane (coll_space_id, 0, 1, 0, 0); + dCreatePlane (coll_space_id, 0, -1, 0, -STAGE_SIZE); + + for (b = 0; b < N_BODIES; b ++) + { + dGeomID coll_box_id; + coll_box_id = dCreateBox (coll_space_id, + bodies_sides[b][0], bodies_sides[b][1], bodies_sides[b][2]); + dGeomSetBody (coll_box_id, dyn_bodies[b].id ()); + } + + coll_contacts.create (0); + + { + // 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/textures"; + + dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions); + } + + dCloseODE(); + return 0; +} diff --git a/libraries/ode-0.9/ode/demo/demo_slider.cpp b/libraries/ode-0.9/ode/demo/demo_slider.cpp new file mode 100644 index 0000000..5de08b0 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_slider.cpp @@ -0,0 +1,172 @@ +/************************************************************************* + * * + * 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 + +#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() +{ + 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/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + dInitODE(); + 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/libraries/ode-0.9/ode/demo/demo_space.cpp b/libraries/ode-0.9/ode/demo/demo_space.cpp new file mode 100644 index 0000000..73101b5 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_space.cpp @@ -0,0 +1,233 @@ +/************************************************************************* + * * + * 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 + +#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 size_t good_matrix[NUM][NUM]; // correct collision matrix +static size_t test_matrix[NUM][NUM]; // testing collision matrix +static size_t 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*)(size_t)(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 *data, dGeomID o1, dGeomID o2) +{ + size_t i,j; + i = (size_t) dGeomGetData (o1); + j = (size_t) dGeomGetData (o2); + if (i==j) + printf ("collision (%d,%d) is between the same object\n",i,j); + if (!good_matrix[i][j] || !good_matrix[j][i]) + printf ("collision (%d,%d) is incorrect\n",i,j); + if (test_matrix[i][j] || test_matrix[j][i]) + printf ("collision (%d,%d) reported more than once\n",i,j); + test_matrix[i][j] = 1; + test_matrix[j][i] = 1; +} + + +// 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}; + dsSetViewpoint (xyz,hpr); +} + + +static void command (int cmd) +{ + if (cmd == ' ') { + seed++; + init_test(); + } +} + + +// simulation loop + +static void simLoop (int pause) +{ + 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/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + dInitODE(); + + // 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/libraries/ode-0.9/ode/demo/demo_space_stress.cpp b/libraries/ode-0.9/ode/demo/demo_space_stress.cpp new file mode 100644 index 0000000..e1be369 --- /dev/null +++ b/libraries/ode-0.9/ode/demo/demo_space_stress.cpp @@ -0,0 +1,435 @@ +/************************************************************************* + * * + * 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 + +#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 100 + + +// 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 draw_geom = 1; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, 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; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' + /* || 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()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),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*)(size_t)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') { + dGeomID g2[GPB]; // encapsulated geometries + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + // 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); + } +/* + // cylinder option not yet implemented + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } +*/ + else if (type == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R,0); + } + + 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 +#include +#include + +#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 + +#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 +#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 dVector3 Vertices[VertexCount]; +static int 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 *data, 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; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' + /* || 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*)(size_t)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') { + dGeomID g2[GPB]; // encapsulated geometries + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + // 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); + } +/* + // cylinder option not yet implemented + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } +*/ + else if (type == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R,0); + } + + 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