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/src/Makefile.am | 207 + libraries/ode-0.9/ode/src/Makefile.in | 2290 +++++++++++ libraries/ode-0.9/ode/src/array.cpp | 80 + libraries/ode-0.9/ode/src/array.h | 135 + libraries/ode-0.9/ode/src/box.cpp | 847 ++++ libraries/ode-0.9/ode/src/capsule.cpp | 369 ++ .../ode-0.9/ode/src/collision_cylinder_box.cpp | 1007 +++++ .../ode-0.9/ode/src/collision_cylinder_plane.cpp | 254 ++ .../ode-0.9/ode/src/collision_cylinder_sphere.cpp | 264 ++ .../ode-0.9/ode/src/collision_cylinder_trimesh.cpp | 1145 ++++++ libraries/ode-0.9/ode/src/collision_kernel.cpp | 1103 ++++++ libraries/ode-0.9/ode/src/collision_kernel.h | 214 ++ .../ode-0.9/ode/src/collision_quadtreespace.cpp | 584 +++ libraries/ode-0.9/ode/src/collision_space.cpp | 790 ++++ .../ode-0.9/ode/src/collision_space_internal.h | 84 + libraries/ode-0.9/ode/src/collision_std.h | 172 + libraries/ode-0.9/ode/src/collision_transform.cpp | 232 ++ libraries/ode-0.9/ode/src/collision_transform.h | 40 + .../ode-0.9/ode/src/collision_trimesh_box.cpp | 1497 +++++++ .../ode/src/collision_trimesh_ccylinder.cpp | 1181 ++++++ .../ode-0.9/ode/src/collision_trimesh_distance.cpp | 1255 ++++++ .../ode-0.9/ode/src/collision_trimesh_gimpact.cpp | 456 +++ .../ode-0.9/ode/src/collision_trimesh_internal.h | 495 +++ .../ode-0.9/ode/src/collision_trimesh_opcode.cpp | 833 ++++ .../ode-0.9/ode/src/collision_trimesh_plane.cpp | 213 + .../ode-0.9/ode/src/collision_trimesh_ray.cpp | 198 + .../ode-0.9/ode/src/collision_trimesh_sphere.cpp | 573 +++ .../ode-0.9/ode/src/collision_trimesh_trimesh.cpp | 2033 ++++++++++ .../ode/src/collision_trimesh_trimesh_new.cpp | 1304 +++++++ libraries/ode-0.9/ode/src/collision_util.cpp | 612 +++ libraries/ode-0.9/ode/src/collision_util.h | 340 ++ libraries/ode-0.9/ode/src/convex.cpp | 1294 +++++++ libraries/ode-0.9/ode/src/cylinder.cpp | 100 + libraries/ode-0.9/ode/src/error.cpp | 172 + libraries/ode-0.9/ode/src/export-dif.cpp | 568 +++ libraries/ode-0.9/ode/src/fastdot.c | 30 + libraries/ode-0.9/ode/src/fastldlt.c | 381 ++ libraries/ode-0.9/ode/src/fastlsolve.c | 298 ++ libraries/ode-0.9/ode/src/fastltsolve.c | 199 + libraries/ode-0.9/ode/src/heightfield.cpp | 1812 +++++++++ libraries/ode-0.9/ode/src/heightfield.h | 225 ++ libraries/ode-0.9/ode/src/joint.cpp | 4065 ++++++++++++++++++++ libraries/ode-0.9/ode/src/joint.h | 346 ++ libraries/ode-0.9/ode/src/lcp.cpp | 2007 ++++++++++ libraries/ode-0.9/ode/src/lcp.h | 58 + libraries/ode-0.9/ode/src/mass.cpp | 529 +++ libraries/ode-0.9/ode/src/mat.cpp | 230 ++ libraries/ode-0.9/ode/src/mat.h | 71 + libraries/ode-0.9/ode/src/matrix.cpp | 358 ++ libraries/ode-0.9/ode/src/memory.cpp | 87 + libraries/ode-0.9/ode/src/misc.cpp | 169 + libraries/ode-0.9/ode/src/objects.h | 138 + libraries/ode-0.9/ode/src/obstack.cpp | 130 + libraries/ode-0.9/ode/src/obstack.h | 68 + libraries/ode-0.9/ode/src/ode.cpp | 1732 +++++++++ libraries/ode-0.9/ode/src/odemath.cpp | 178 + libraries/ode-0.9/ode/src/plane.cpp | 145 + libraries/ode-0.9/ode/src/quickstep.cpp | 880 +++++ libraries/ode-0.9/ode/src/quickstep.h | 33 + libraries/ode-0.9/ode/src/ray.cpp | 686 ++++ libraries/ode-0.9/ode/src/rotation.cpp | 316 ++ libraries/ode-0.9/ode/src/scrapbook.cpp | 485 +++ libraries/ode-0.9/ode/src/sphere.cpp | 241 ++ libraries/ode-0.9/ode/src/stack.cpp | 114 + libraries/ode-0.9/ode/src/stack.h | 138 + libraries/ode-0.9/ode/src/step.cpp | 1795 +++++++++ libraries/ode-0.9/ode/src/step.h | 36 + libraries/ode-0.9/ode/src/stepfast.cpp | 1139 ++++++ libraries/ode-0.9/ode/src/testing.cpp | 243 ++ libraries/ode-0.9/ode/src/testing.h | 65 + libraries/ode-0.9/ode/src/timer.cpp | 421 ++ libraries/ode-0.9/ode/src/util.cpp | 374 ++ libraries/ode-0.9/ode/src/util.h | 38 + 73 files changed, 43201 insertions(+) create mode 100644 libraries/ode-0.9/ode/src/Makefile.am create mode 100644 libraries/ode-0.9/ode/src/Makefile.in create mode 100644 libraries/ode-0.9/ode/src/array.cpp create mode 100644 libraries/ode-0.9/ode/src/array.h create mode 100644 libraries/ode-0.9/ode/src/box.cpp create mode 100644 libraries/ode-0.9/ode/src/capsule.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_box.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_kernel.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_kernel.h create mode 100644 libraries/ode-0.9/ode/src/collision_quadtreespace.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_space.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_space_internal.h create mode 100644 libraries/ode-0.9/ode/src/collision_std.h create mode 100644 libraries/ode-0.9/ode/src/collision_transform.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_transform.h create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_box.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_internal.h create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_util.cpp create mode 100644 libraries/ode-0.9/ode/src/collision_util.h create mode 100644 libraries/ode-0.9/ode/src/convex.cpp create mode 100644 libraries/ode-0.9/ode/src/cylinder.cpp create mode 100644 libraries/ode-0.9/ode/src/error.cpp create mode 100644 libraries/ode-0.9/ode/src/export-dif.cpp create mode 100644 libraries/ode-0.9/ode/src/fastdot.c create mode 100644 libraries/ode-0.9/ode/src/fastldlt.c create mode 100644 libraries/ode-0.9/ode/src/fastlsolve.c create mode 100644 libraries/ode-0.9/ode/src/fastltsolve.c create mode 100644 libraries/ode-0.9/ode/src/heightfield.cpp create mode 100644 libraries/ode-0.9/ode/src/heightfield.h create mode 100644 libraries/ode-0.9/ode/src/joint.cpp create mode 100644 libraries/ode-0.9/ode/src/joint.h create mode 100644 libraries/ode-0.9/ode/src/lcp.cpp create mode 100644 libraries/ode-0.9/ode/src/lcp.h create mode 100644 libraries/ode-0.9/ode/src/mass.cpp create mode 100644 libraries/ode-0.9/ode/src/mat.cpp create mode 100644 libraries/ode-0.9/ode/src/mat.h create mode 100644 libraries/ode-0.9/ode/src/matrix.cpp create mode 100644 libraries/ode-0.9/ode/src/memory.cpp create mode 100644 libraries/ode-0.9/ode/src/misc.cpp create mode 100644 libraries/ode-0.9/ode/src/objects.h create mode 100644 libraries/ode-0.9/ode/src/obstack.cpp create mode 100644 libraries/ode-0.9/ode/src/obstack.h create mode 100644 libraries/ode-0.9/ode/src/ode.cpp create mode 100644 libraries/ode-0.9/ode/src/odemath.cpp create mode 100644 libraries/ode-0.9/ode/src/plane.cpp create mode 100644 libraries/ode-0.9/ode/src/quickstep.cpp create mode 100644 libraries/ode-0.9/ode/src/quickstep.h create mode 100644 libraries/ode-0.9/ode/src/ray.cpp create mode 100644 libraries/ode-0.9/ode/src/rotation.cpp create mode 100644 libraries/ode-0.9/ode/src/scrapbook.cpp create mode 100644 libraries/ode-0.9/ode/src/sphere.cpp create mode 100644 libraries/ode-0.9/ode/src/stack.cpp create mode 100644 libraries/ode-0.9/ode/src/stack.h create mode 100644 libraries/ode-0.9/ode/src/step.cpp create mode 100644 libraries/ode-0.9/ode/src/step.h create mode 100755 libraries/ode-0.9/ode/src/stepfast.cpp create mode 100644 libraries/ode-0.9/ode/src/testing.cpp create mode 100644 libraries/ode-0.9/ode/src/testing.h create mode 100644 libraries/ode-0.9/ode/src/timer.cpp create mode 100644 libraries/ode-0.9/ode/src/util.cpp create mode 100644 libraries/ode-0.9/ode/src/util.h (limited to 'libraries/ode-0.9/ode/src') diff --git a/libraries/ode-0.9/ode/src/Makefile.am b/libraries/ode-0.9/ode/src/Makefile.am new file mode 100644 index 0000000..1b0cf19 --- /dev/null +++ b/libraries/ode-0.9/ode/src/Makefile.am @@ -0,0 +1,207 @@ +AM_CXXFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CPPFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +lib_LIBRARIES = libode.a +libode_a_CPPFLAGS = -O2 + +libode_a_CPPFLAGS += -fPIC + +# Fake an executable in order to get a shared library +# Note the elegant and cunning way to trick Autotools to install a program +# in a lib directory. --Rodrigo +traplibdir=$(libdir) +EXEEXT=@so_ext@ +traplib_PROGRAMS=libode +libode_SOURCES= +libode_DEPENDENCIES = libfast.a libode.a +libode_LDFLAGS= @SHARED_LDFLAGS@ +if USE_SONAME +libode_LDFLAGS+=-Wl,-soname,@ODE_SONAME@ +endif +libode_LDADD=$(libode_a_OBJECTS) $(libfast_a_OBJECTS) + +if OPCODE +libode_DEPENDENCIES+= libOPCODE.a +libode_LDADD+=$(libOPCODE_a_OBJECTS) +endif + + +if GIMPACT +libode_DEPENDENCIES+= libGIMPACT.a +libode_LDADD+=$(libGIMPACT_a_OBJECTS) +endif + + +# convenience library to simulate per object cflags +noinst_LIBRARIES= libfast.a +libfast_a_CFLAGS= -O1 +libfast_a_SOURCES= fastldlt.c fastltsolve.c fastdot.c fastlsolve.c + +libfast_a_CFLAGS += -fPIC + +libode_a_DEPENDENCIES = libfast.a +libode_a_LIBADD= $(libfast_a_OBJECTS) + +libode_a_SOURCES = objects.h \ + obstack.cpp \ + collision_util.cpp \ + obstack.h \ + array.cpp \ + collision_util.h \ + ode.cpp \ + array.h \ + error.cpp \ + odemath.cpp \ + collision_kernel.cpp \ + export-dif.cpp \ + quickstep.cpp \ + collision_kernel.h \ + quickstep.h \ + collision_quadtreespace.cpp \ + rotation.cpp \ + collision_space.cpp \ + collision_space_internal.h \ + collision_cylinder_box.cpp \ + collision_cylinder_sphere.cpp \ + collision_cylinder_plane.cpp \ + sphere.cpp \ + box.cpp \ + capsule.cpp \ + plane.cpp \ + ray.cpp \ + cylinder.cpp \ + convex.cpp \ + joint.cpp \ + stack.h \ + collision_std.h \ + joint.h \ + step.cpp \ + collision_transform.cpp \ + lcp.cpp \ + step.h \ + collision_transform.h \ + lcp.h \ + stepfast.cpp \ + mass.cpp \ + testing.cpp \ + mat.cpp \ + testing.h \ + mat.h \ + timer.cpp \ + matrix.cpp \ + util.cpp \ + memory.cpp \ + util.h \ + misc.cpp \ + heightfield.cpp \ + heightfield.h + + + +################################### +# G I M P A C T S T U F F +################################### + + +if GIMPACT +noinst_LIBRARIES+= libGIMPACT.a +libGIMPACT_a_CPPFLAGS= -O2 -fno-strict-aliasing -fPIC + +libode_a_SOURCES+= collision_trimesh_gimpact.cpp + +libGIMPACT_a_SOURCES = \ + @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp \ + @TOPDIR@/GIMPACT/src/gim_contact.cpp \ + @TOPDIR@/GIMPACT/src/gim_math.cpp \ + @TOPDIR@/GIMPACT/src/gim_memory.cpp \ + @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp \ + @TOPDIR@/GIMPACT/src/gimpact.cpp + +libode_a_DEPENDENCIES+=libGIMPACT.a +libode_a_LIBADD+= $(libGIMPACT_a_OBJECTS) +AM_CXXFLAGS += -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT +AM_CFLAGS += -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT + +libode_a_SOURCES+= collision_trimesh_trimesh.cpp \ + collision_trimesh_sphere.cpp \ + collision_trimesh_ray.cpp \ + collision_trimesh_opcode.cpp \ + collision_trimesh_box.cpp \ + collision_trimesh_ccylinder.cpp \ + collision_trimesh_distance.cpp \ + collision_trimesh_internal.h \ + collision_cylinder_trimesh.cpp \ + collision_trimesh_plane.cpp +endif + + + +################################# +# O P C O D E S T U F F +################################# + + +if OPCODE +noinst_LIBRARIES+= libOPCODE.a +libOPCODE_a_CPPFLAGS= -O2 -fno-strict-aliasing -fPIC + + +libOPCODE_a_SOURCES= @TOPDIR@/OPCODE/OPC_AABBCollider.cpp \ + @TOPDIR@/OPCODE/OPC_AABBTree.cpp \ + @TOPDIR@/OPCODE/OPC_BaseModel.cpp \ + @TOPDIR@/OPCODE/OPC_BoxPruning.cpp \ + @TOPDIR@/OPCODE/OPC_Collider.cpp \ + @TOPDIR@/OPCODE/OPC_Common.cpp \ + @TOPDIR@/OPCODE/OPC_HybridModel.cpp \ + @TOPDIR@/OPCODE/OPC_LSSCollider.cpp \ + @TOPDIR@/OPCODE/OPC_MeshInterface.cpp \ + @TOPDIR@/OPCODE/OPC_Model.cpp \ + @TOPDIR@/OPCODE/OPC_OBBCollider.cpp \ + @TOPDIR@/OPCODE/Opcode.cpp \ + @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp \ + @TOPDIR@/OPCODE/OPC_Picking.cpp \ + @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp \ + @TOPDIR@/OPCODE/OPC_RayCollider.cpp \ + @TOPDIR@/OPCODE/OPC_SphereCollider.cpp \ + @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp \ + @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp \ + @TOPDIR@/OPCODE/OPC_TreeCollider.cpp \ + @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp \ + @TOPDIR@/OPCODE/Ice/IceAABB.cpp \ + @TOPDIR@/OPCODE/Ice/IceContainer.cpp \ + @TOPDIR@/OPCODE/Ice/IceHPoint.cpp \ + @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp \ + @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp \ + @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp \ + @TOPDIR@/OPCODE/Ice/IceOBB.cpp \ + @TOPDIR@/OPCODE/Ice/IcePlane.cpp \ + @TOPDIR@/OPCODE/Ice/IcePoint.cpp \ + @TOPDIR@/OPCODE/Ice/IceRandom.cpp \ + @TOPDIR@/OPCODE/Ice/IceRay.cpp \ + @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp \ + @TOPDIR@/OPCODE/Ice/IceSegment.cpp \ + @TOPDIR@/OPCODE/Ice/IceTriangle.cpp \ + @TOPDIR@/OPCODE/Ice/IceUtils.cpp +libode_a_DEPENDENCIES+=libOPCODE.a + +libode_a_LIBADD+= $(libOPCODE_a_OBJECTS) +AM_CXXFLAGS += -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE +AM_CFLAGS += -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE +libode_a_SOURCES+= collision_trimesh_trimesh.cpp \ + collision_trimesh_sphere.cpp \ + collision_trimesh_ray.cpp \ + collision_trimesh_opcode.cpp \ + collision_trimesh_box.cpp \ + collision_trimesh_ccylinder.cpp \ + collision_trimesh_distance.cpp \ + collision_trimesh_internal.h \ + collision_cylinder_trimesh.cpp \ + collision_trimesh_plane.cpp +endif + + diff --git a/libraries/ode-0.9/ode/src/Makefile.in b/libraries/ode-0.9/ode/src/Makefile.in new file mode 100644 index 0000000..548f2e2 --- /dev/null +++ b/libraries/ode-0.9/ode/src/Makefile.in @@ -0,0 +1,2290 @@ +# 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@ +traplib_PROGRAMS = libode$(EXEEXT) +@USE_SONAME_TRUE@am__append_1 = -Wl,-soname,@ODE_SONAME@ +@OPCODE_TRUE@am__append_2 = libOPCODE.a +@OPCODE_TRUE@am__append_3 = $(libOPCODE_a_OBJECTS) +@GIMPACT_TRUE@am__append_4 = libGIMPACT.a +@GIMPACT_TRUE@am__append_5 = $(libGIMPACT_a_OBJECTS) + +################################### +# G I M P A C T S T U F F +################################### +@GIMPACT_TRUE@am__append_6 = libGIMPACT.a +@GIMPACT_TRUE@am__append_7 = collision_trimesh_gimpact.cpp \ +@GIMPACT_TRUE@ collision_trimesh_trimesh.cpp \ +@GIMPACT_TRUE@ collision_trimesh_sphere.cpp \ +@GIMPACT_TRUE@ collision_trimesh_ray.cpp \ +@GIMPACT_TRUE@ collision_trimesh_opcode.cpp \ +@GIMPACT_TRUE@ collision_trimesh_box.cpp \ +@GIMPACT_TRUE@ collision_trimesh_ccylinder.cpp \ +@GIMPACT_TRUE@ collision_trimesh_distance.cpp \ +@GIMPACT_TRUE@ collision_trimesh_internal.h \ +@GIMPACT_TRUE@ collision_cylinder_trimesh.cpp \ +@GIMPACT_TRUE@ collision_trimesh_plane.cpp +@GIMPACT_TRUE@am__append_8 = libGIMPACT.a +@GIMPACT_TRUE@am__append_9 = $(libGIMPACT_a_OBJECTS) +@GIMPACT_TRUE@am__append_10 = -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT +@GIMPACT_TRUE@am__append_11 = -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT + +################################# +# O P C O D E S T U F F +################################# +@OPCODE_TRUE@am__append_12 = libOPCODE.a +@OPCODE_TRUE@am__append_13 = libOPCODE.a +@OPCODE_TRUE@am__append_14 = $(libOPCODE_a_OBJECTS) +@OPCODE_TRUE@am__append_15 = -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE +@OPCODE_TRUE@am__append_16 = -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE +@OPCODE_TRUE@am__append_17 = collision_trimesh_trimesh.cpp \ +@OPCODE_TRUE@ collision_trimesh_sphere.cpp \ +@OPCODE_TRUE@ collision_trimesh_ray.cpp \ +@OPCODE_TRUE@ collision_trimesh_opcode.cpp \ +@OPCODE_TRUE@ collision_trimesh_box.cpp \ +@OPCODE_TRUE@ collision_trimesh_ccylinder.cpp \ +@OPCODE_TRUE@ collision_trimesh_distance.cpp \ +@OPCODE_TRUE@ collision_trimesh_internal.h \ +@OPCODE_TRUE@ collision_cylinder_trimesh.cpp \ +@OPCODE_TRUE@ collision_trimesh_plane.cpp + +subdir = ode/src +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 = +am__vpath_adj_setup = srcdirstrip=`echo "$(srcdir)" | sed 's|.|.|g'`; +am__vpath_adj = case $$p in \ + $(srcdir)/*) f=`echo "$$p" | sed "s|^$$srcdirstrip/||"`;; \ + *) f=$$p;; \ + esac; +am__strip_dir = `echo $$p | sed -e 's|^.*/||'`; +am__installdirs = "$(DESTDIR)$(libdir)" "$(DESTDIR)$(traplibdir)" +libLIBRARIES_INSTALL = $(INSTALL_DATA) +LIBRARIES = $(lib_LIBRARIES) $(noinst_LIBRARIES) +AR = ar +ARFLAGS = cru +libGIMPACT_a_AR = $(AR) $(ARFLAGS) +libGIMPACT_a_LIBADD = +am__libGIMPACT_a_SOURCES_DIST = \ + @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp \ + @TOPDIR@/GIMPACT/src/gim_contact.cpp \ + @TOPDIR@/GIMPACT/src/gim_math.cpp \ + @TOPDIR@/GIMPACT/src/gim_memory.cpp \ + @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp \ + @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp \ + @TOPDIR@/GIMPACT/src/gimpact.cpp +@GIMPACT_TRUE@am_libGIMPACT_a_OBJECTS = \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_boxpruning.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_contact.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_math.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_memory.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_tri_tri_overlap.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_capsule_collision.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_ray_collision.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_sphere_collision.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_trimesh_collision.$(OBJEXT) \ +@GIMPACT_TRUE@ libGIMPACT_a-gimpact.$(OBJEXT) +libGIMPACT_a_OBJECTS = $(am_libGIMPACT_a_OBJECTS) +libOPCODE_a_AR = $(AR) $(ARFLAGS) +libOPCODE_a_LIBADD = +am__libOPCODE_a_SOURCES_DIST = @TOPDIR@/OPCODE/OPC_AABBCollider.cpp \ + @TOPDIR@/OPCODE/OPC_AABBTree.cpp \ + @TOPDIR@/OPCODE/OPC_BaseModel.cpp \ + @TOPDIR@/OPCODE/OPC_BoxPruning.cpp \ + @TOPDIR@/OPCODE/OPC_Collider.cpp \ + @TOPDIR@/OPCODE/OPC_Common.cpp \ + @TOPDIR@/OPCODE/OPC_HybridModel.cpp \ + @TOPDIR@/OPCODE/OPC_LSSCollider.cpp \ + @TOPDIR@/OPCODE/OPC_MeshInterface.cpp \ + @TOPDIR@/OPCODE/OPC_Model.cpp \ + @TOPDIR@/OPCODE/OPC_OBBCollider.cpp @TOPDIR@/OPCODE/Opcode.cpp \ + @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp \ + @TOPDIR@/OPCODE/OPC_Picking.cpp \ + @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp \ + @TOPDIR@/OPCODE/OPC_RayCollider.cpp \ + @TOPDIR@/OPCODE/OPC_SphereCollider.cpp \ + @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp \ + @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp \ + @TOPDIR@/OPCODE/OPC_TreeCollider.cpp \ + @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp \ + @TOPDIR@/OPCODE/Ice/IceAABB.cpp \ + @TOPDIR@/OPCODE/Ice/IceContainer.cpp \ + @TOPDIR@/OPCODE/Ice/IceHPoint.cpp \ + @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp \ + @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp \ + @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp \ + @TOPDIR@/OPCODE/Ice/IceOBB.cpp \ + @TOPDIR@/OPCODE/Ice/IcePlane.cpp \ + @TOPDIR@/OPCODE/Ice/IcePoint.cpp \ + @TOPDIR@/OPCODE/Ice/IceRandom.cpp \ + @TOPDIR@/OPCODE/Ice/IceRay.cpp \ + @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp \ + @TOPDIR@/OPCODE/Ice/IceSegment.cpp \ + @TOPDIR@/OPCODE/Ice/IceTriangle.cpp \ + @TOPDIR@/OPCODE/Ice/IceUtils.cpp +@OPCODE_TRUE@am_libOPCODE_a_OBJECTS = \ +@OPCODE_TRUE@ libOPCODE_a-OPC_AABBCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_AABBTree.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_BaseModel.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_BoxPruning.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_Collider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_Common.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_HybridModel.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_LSSCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_MeshInterface.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_Model.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_OBBCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-Opcode.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_OptimizedTree.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_Picking.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_PlanesCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_RayCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_SphereCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_SweepAndPrune.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_TreeBuilders.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_TreeCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-OPC_VolumeCollider.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceAABB.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceContainer.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceHPoint.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceIndexedTriangle.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceMatrix3x3.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceMatrix4x4.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceOBB.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IcePlane.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IcePoint.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceRandom.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceRay.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceRevisitedRadix.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceSegment.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceTriangle.$(OBJEXT) \ +@OPCODE_TRUE@ libOPCODE_a-IceUtils.$(OBJEXT) +libOPCODE_a_OBJECTS = $(am_libOPCODE_a_OBJECTS) +libfast_a_AR = $(AR) $(ARFLAGS) +libfast_a_LIBADD = +am_libfast_a_OBJECTS = libfast_a-fastldlt.$(OBJEXT) \ + libfast_a-fastltsolve.$(OBJEXT) libfast_a-fastdot.$(OBJEXT) \ + libfast_a-fastlsolve.$(OBJEXT) +libfast_a_OBJECTS = $(am_libfast_a_OBJECTS) +libode_a_AR = $(AR) $(ARFLAGS) +am__libode_a_SOURCES_DIST = objects.h obstack.cpp collision_util.cpp \ + obstack.h array.cpp collision_util.h ode.cpp array.h error.cpp \ + odemath.cpp collision_kernel.cpp export-dif.cpp quickstep.cpp \ + collision_kernel.h quickstep.h collision_quadtreespace.cpp \ + rotation.cpp collision_space.cpp collision_space_internal.h \ + collision_cylinder_box.cpp collision_cylinder_sphere.cpp \ + collision_cylinder_plane.cpp sphere.cpp box.cpp capsule.cpp \ + plane.cpp ray.cpp cylinder.cpp convex.cpp joint.cpp stack.h \ + collision_std.h joint.h step.cpp collision_transform.cpp \ + lcp.cpp step.h collision_transform.h lcp.h stepfast.cpp \ + mass.cpp testing.cpp mat.cpp testing.h mat.h timer.cpp \ + matrix.cpp util.cpp memory.cpp util.h misc.cpp heightfield.cpp \ + heightfield.h collision_trimesh_gimpact.cpp \ + collision_trimesh_trimesh.cpp collision_trimesh_sphere.cpp \ + collision_trimesh_ray.cpp collision_trimesh_opcode.cpp \ + collision_trimesh_box.cpp collision_trimesh_ccylinder.cpp \ + collision_trimesh_distance.cpp collision_trimesh_internal.h \ + collision_cylinder_trimesh.cpp collision_trimesh_plane.cpp +@GIMPACT_TRUE@am__objects_1 = \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_gimpact.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_trimesh.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_sphere.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_ray.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_opcode.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_box.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_ccylinder.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_distance.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_cylinder_trimesh.$(OBJEXT) \ +@GIMPACT_TRUE@ libode_a-collision_trimesh_plane.$(OBJEXT) +@OPCODE_TRUE@am__objects_2 = \ +@OPCODE_TRUE@ libode_a-collision_trimesh_trimesh.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_sphere.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_ray.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_opcode.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_box.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_ccylinder.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_distance.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_cylinder_trimesh.$(OBJEXT) \ +@OPCODE_TRUE@ libode_a-collision_trimesh_plane.$(OBJEXT) +am_libode_a_OBJECTS = libode_a-obstack.$(OBJEXT) \ + libode_a-collision_util.$(OBJEXT) libode_a-array.$(OBJEXT) \ + libode_a-ode.$(OBJEXT) libode_a-error.$(OBJEXT) \ + libode_a-odemath.$(OBJEXT) libode_a-collision_kernel.$(OBJEXT) \ + libode_a-export-dif.$(OBJEXT) libode_a-quickstep.$(OBJEXT) \ + libode_a-collision_quadtreespace.$(OBJEXT) \ + libode_a-rotation.$(OBJEXT) libode_a-collision_space.$(OBJEXT) \ + libode_a-collision_cylinder_box.$(OBJEXT) \ + libode_a-collision_cylinder_sphere.$(OBJEXT) \ + libode_a-collision_cylinder_plane.$(OBJEXT) \ + libode_a-sphere.$(OBJEXT) libode_a-box.$(OBJEXT) \ + libode_a-capsule.$(OBJEXT) libode_a-plane.$(OBJEXT) \ + libode_a-ray.$(OBJEXT) libode_a-cylinder.$(OBJEXT) \ + libode_a-convex.$(OBJEXT) libode_a-joint.$(OBJEXT) \ + libode_a-step.$(OBJEXT) libode_a-collision_transform.$(OBJEXT) \ + libode_a-lcp.$(OBJEXT) libode_a-stepfast.$(OBJEXT) \ + libode_a-mass.$(OBJEXT) libode_a-testing.$(OBJEXT) \ + libode_a-mat.$(OBJEXT) libode_a-timer.$(OBJEXT) \ + libode_a-matrix.$(OBJEXT) libode_a-util.$(OBJEXT) \ + libode_a-memory.$(OBJEXT) libode_a-misc.$(OBJEXT) \ + libode_a-heightfield.$(OBJEXT) $(am__objects_1) \ + $(am__objects_2) +libode_a_OBJECTS = $(am_libode_a_OBJECTS) +traplibPROGRAMS_INSTALL = $(INSTALL_PROGRAM) +PROGRAMS = $(traplib_PROGRAMS) +am_libode_OBJECTS = +libode_OBJECTS = $(am_libode_OBJECTS) +libode_LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(libode_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 = $(libGIMPACT_a_SOURCES) $(libOPCODE_a_SOURCES) \ + $(libfast_a_SOURCES) $(libode_a_SOURCES) $(libode_SOURCES) +DIST_SOURCES = $(am__libGIMPACT_a_SOURCES_DIST) \ + $(am__libOPCODE_a_SOURCES_DIST) $(libfast_a_SOURCES) \ + $(am__libode_a_SOURCES_DIST) $(libode_SOURCES) +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 = @so_ext@ +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@ -I$(top_srcdir)/include \ + -I$(top_builddir)/include $(am__append_10) $(am__append_15) +AM_CPPFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include \ + -I$(top_builddir)/include $(am__append_11) $(am__append_16) +lib_LIBRARIES = libode.a +libode_a_CPPFLAGS = -O2 -fPIC + +# Fake an executable in order to get a shared library +# Note the elegant and cunning way to trick Autotools to install a program +# in a lib directory. --Rodrigo +traplibdir = $(libdir) +libode_SOURCES = +libode_DEPENDENCIES = libfast.a libode.a $(am__append_2) \ + $(am__append_4) +libode_LDFLAGS = @SHARED_LDFLAGS@ $(am__append_1) +libode_LDADD = $(libode_a_OBJECTS) $(libfast_a_OBJECTS) \ + $(am__append_3) $(am__append_5) + +# convenience library to simulate per object cflags +noinst_LIBRARIES = libfast.a $(am__append_6) $(am__append_12) +libfast_a_CFLAGS = -O1 -fPIC +libfast_a_SOURCES = fastldlt.c fastltsolve.c fastdot.c fastlsolve.c +libode_a_DEPENDENCIES = libfast.a $(am__append_8) $(am__append_13) +libode_a_LIBADD = $(libfast_a_OBJECTS) $(am__append_9) \ + $(am__append_14) +libode_a_SOURCES = objects.h obstack.cpp collision_util.cpp obstack.h \ + array.cpp collision_util.h ode.cpp array.h error.cpp \ + odemath.cpp collision_kernel.cpp export-dif.cpp quickstep.cpp \ + collision_kernel.h quickstep.h collision_quadtreespace.cpp \ + rotation.cpp collision_space.cpp collision_space_internal.h \ + collision_cylinder_box.cpp collision_cylinder_sphere.cpp \ + collision_cylinder_plane.cpp sphere.cpp box.cpp capsule.cpp \ + plane.cpp ray.cpp cylinder.cpp convex.cpp joint.cpp stack.h \ + collision_std.h joint.h step.cpp collision_transform.cpp \ + lcp.cpp step.h collision_transform.h lcp.h stepfast.cpp \ + mass.cpp testing.cpp mat.cpp testing.h mat.h timer.cpp \ + matrix.cpp util.cpp memory.cpp util.h misc.cpp heightfield.cpp \ + heightfield.h $(am__append_7) $(am__append_17) +@GIMPACT_TRUE@libGIMPACT_a_CPPFLAGS = -O2 -fno-strict-aliasing -fPIC +@GIMPACT_TRUE@libGIMPACT_a_SOURCES = \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_contact.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_math.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_memory.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp \ +@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gimpact.cpp + +@OPCODE_TRUE@libOPCODE_a_CPPFLAGS = -O2 -fno-strict-aliasing -fPIC +@OPCODE_TRUE@libOPCODE_a_SOURCES = @TOPDIR@/OPCODE/OPC_AABBCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_AABBTree.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_BaseModel.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_BoxPruning.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Collider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Common.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_HybridModel.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_LSSCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_MeshInterface.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Model.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_OBBCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Opcode.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Picking.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_RayCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_SphereCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_TreeCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceAABB.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceContainer.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceHPoint.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceOBB.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IcePlane.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IcePoint.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceRandom.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceRay.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceSegment.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceTriangle.cpp \ +@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceUtils.cpp + +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/src/Makefile'; \ + cd $(top_srcdir) && \ + $(AUTOMAKE) --foreign ode/src/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 +install-libLIBRARIES: $(lib_LIBRARIES) + @$(NORMAL_INSTALL) + test -z "$(libdir)" || $(MKDIR_P) "$(DESTDIR)$(libdir)" + @list='$(lib_LIBRARIES)'; for p in $$list; do \ + if test -f $$p; then \ + f=$(am__strip_dir) \ + echo " $(libLIBRARIES_INSTALL) '$$p' '$(DESTDIR)$(libdir)/$$f'"; \ + $(libLIBRARIES_INSTALL) "$$p" "$(DESTDIR)$(libdir)/$$f"; \ + else :; fi; \ + done + @$(POST_INSTALL) + @list='$(lib_LIBRARIES)'; for p in $$list; do \ + if test -f $$p; then \ + p=$(am__strip_dir) \ + echo " $(RANLIB) '$(DESTDIR)$(libdir)/$$p'"; \ + $(RANLIB) "$(DESTDIR)$(libdir)/$$p"; \ + else :; fi; \ + done + +uninstall-libLIBRARIES: + @$(NORMAL_UNINSTALL) + @list='$(lib_LIBRARIES)'; for p in $$list; do \ + p=$(am__strip_dir) \ + echo " rm -f '$(DESTDIR)$(libdir)/$$p'"; \ + rm -f "$(DESTDIR)$(libdir)/$$p"; \ + done + +clean-libLIBRARIES: + -test -z "$(lib_LIBRARIES)" || rm -f $(lib_LIBRARIES) + +clean-noinstLIBRARIES: + -test -z "$(noinst_LIBRARIES)" || rm -f $(noinst_LIBRARIES) +libGIMPACT.a: $(libGIMPACT_a_OBJECTS) $(libGIMPACT_a_DEPENDENCIES) + -rm -f libGIMPACT.a + $(libGIMPACT_a_AR) libGIMPACT.a $(libGIMPACT_a_OBJECTS) $(libGIMPACT_a_LIBADD) + $(RANLIB) libGIMPACT.a +libOPCODE.a: $(libOPCODE_a_OBJECTS) $(libOPCODE_a_DEPENDENCIES) + -rm -f libOPCODE.a + $(libOPCODE_a_AR) libOPCODE.a $(libOPCODE_a_OBJECTS) $(libOPCODE_a_LIBADD) + $(RANLIB) libOPCODE.a +libfast.a: $(libfast_a_OBJECTS) $(libfast_a_DEPENDENCIES) + -rm -f libfast.a + $(libfast_a_AR) libfast.a $(libfast_a_OBJECTS) $(libfast_a_LIBADD) + $(RANLIB) libfast.a +libode.a: $(libode_a_OBJECTS) $(libode_a_DEPENDENCIES) + -rm -f libode.a + $(libode_a_AR) libode.a $(libode_a_OBJECTS) $(libode_a_LIBADD) + $(RANLIB) libode.a +install-traplibPROGRAMS: $(traplib_PROGRAMS) + @$(NORMAL_INSTALL) + test -z "$(traplibdir)" || $(MKDIR_P) "$(DESTDIR)$(traplibdir)" + @list='$(traplib_PROGRAMS)'; for p in $$list; do \ + p1=`echo $$p|sed 's/$(EXEEXT)$$//'`; \ + if test -f $$p \ + ; then \ + f=`echo "$$p1" | sed 's,^.*/,,;$(transform);s/$$/$(EXEEXT)/'`; \ + echo " $(INSTALL_PROGRAM_ENV) $(traplibPROGRAMS_INSTALL) '$$p' '$(DESTDIR)$(traplibdir)/$$f'"; \ + $(INSTALL_PROGRAM_ENV) $(traplibPROGRAMS_INSTALL) "$$p" "$(DESTDIR)$(traplibdir)/$$f" || exit 1; \ + else :; fi; \ + done + +uninstall-traplibPROGRAMS: + @$(NORMAL_UNINSTALL) + @list='$(traplib_PROGRAMS)'; for p in $$list; do \ + f=`echo "$$p" | sed 's,^.*/,,;s/$(EXEEXT)$$//;$(transform);s/$$/$(EXEEXT)/'`; \ + echo " rm -f '$(DESTDIR)$(traplibdir)/$$f'"; \ + rm -f "$(DESTDIR)$(traplibdir)/$$f"; \ + done + +clean-traplibPROGRAMS: + -test -z "$(traplib_PROGRAMS)" || rm -f $(traplib_PROGRAMS) +libode$(EXEEXT): $(libode_OBJECTS) $(libode_DEPENDENCIES) + @rm -f libode$(EXEEXT) + $(libode_LINK) $(libode_OBJECTS) $(libode_LDADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_boxpruning.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_contact.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_math.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_memory.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gimpact.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceAABB.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceContainer.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceHPoint.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceMatrix3x3.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceMatrix4x4.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceOBB.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IcePlane.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IcePoint.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceRandom.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceRay.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceSegment.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceTriangle.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceUtils.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_AABBTree.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_BaseModel.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Collider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Common.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_HybridModel.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Model.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Picking.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_RayCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-Opcode.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastdot.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastldlt.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastlsolve.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastltsolve.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-array.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-box.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-capsule.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_box.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_plane.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_sphere.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_trimesh.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_kernel.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_quadtreespace.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_space.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_transform.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_box.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_ccylinder.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_distance.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_gimpact.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_opcode.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_plane.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_ray.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_sphere.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_trimesh.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_util.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-convex.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-cylinder.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-error.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-export-dif.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-heightfield.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-joint.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-lcp.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-mass.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-mat.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-matrix.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-memory.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-misc.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-obstack.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-ode.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-odemath.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-plane.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-quickstep.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-ray.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-rotation.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-sphere.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-step.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-stepfast.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-testing.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-timer.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-util.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) '$<'` + +libfast_a-fastldlt.o: fastldlt.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastldlt.o -MD -MP -MF $(DEPDIR)/libfast_a-fastldlt.Tpo -c -o libfast_a-fastldlt.o `test -f 'fastldlt.c' || echo '$(srcdir)/'`fastldlt.c +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastldlt.Tpo $(DEPDIR)/libfast_a-fastldlt.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastldlt.c' object='libfast_a-fastldlt.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastldlt.o `test -f 'fastldlt.c' || echo '$(srcdir)/'`fastldlt.c + +libfast_a-fastldlt.obj: fastldlt.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastldlt.obj -MD -MP -MF $(DEPDIR)/libfast_a-fastldlt.Tpo -c -o libfast_a-fastldlt.obj `if test -f 'fastldlt.c'; then $(CYGPATH_W) 'fastldlt.c'; else $(CYGPATH_W) '$(srcdir)/fastldlt.c'; fi` +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastldlt.Tpo $(DEPDIR)/libfast_a-fastldlt.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastldlt.c' object='libfast_a-fastldlt.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastldlt.obj `if test -f 'fastldlt.c'; then $(CYGPATH_W) 'fastldlt.c'; else $(CYGPATH_W) '$(srcdir)/fastldlt.c'; fi` + +libfast_a-fastltsolve.o: fastltsolve.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastltsolve.o -MD -MP -MF $(DEPDIR)/libfast_a-fastltsolve.Tpo -c -o libfast_a-fastltsolve.o `test -f 'fastltsolve.c' || echo '$(srcdir)/'`fastltsolve.c +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastltsolve.Tpo $(DEPDIR)/libfast_a-fastltsolve.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastltsolve.c' object='libfast_a-fastltsolve.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastltsolve.o `test -f 'fastltsolve.c' || echo '$(srcdir)/'`fastltsolve.c + +libfast_a-fastltsolve.obj: fastltsolve.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastltsolve.obj -MD -MP -MF $(DEPDIR)/libfast_a-fastltsolve.Tpo -c -o libfast_a-fastltsolve.obj `if test -f 'fastltsolve.c'; then $(CYGPATH_W) 'fastltsolve.c'; else $(CYGPATH_W) '$(srcdir)/fastltsolve.c'; fi` +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastltsolve.Tpo $(DEPDIR)/libfast_a-fastltsolve.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastltsolve.c' object='libfast_a-fastltsolve.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastltsolve.obj `if test -f 'fastltsolve.c'; then $(CYGPATH_W) 'fastltsolve.c'; else $(CYGPATH_W) '$(srcdir)/fastltsolve.c'; fi` + +libfast_a-fastdot.o: fastdot.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastdot.o -MD -MP -MF $(DEPDIR)/libfast_a-fastdot.Tpo -c -o libfast_a-fastdot.o `test -f 'fastdot.c' || echo '$(srcdir)/'`fastdot.c +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastdot.Tpo $(DEPDIR)/libfast_a-fastdot.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastdot.c' object='libfast_a-fastdot.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastdot.o `test -f 'fastdot.c' || echo '$(srcdir)/'`fastdot.c + +libfast_a-fastdot.obj: fastdot.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastdot.obj -MD -MP -MF $(DEPDIR)/libfast_a-fastdot.Tpo -c -o libfast_a-fastdot.obj `if test -f 'fastdot.c'; then $(CYGPATH_W) 'fastdot.c'; else $(CYGPATH_W) '$(srcdir)/fastdot.c'; fi` +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastdot.Tpo $(DEPDIR)/libfast_a-fastdot.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastdot.c' object='libfast_a-fastdot.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastdot.obj `if test -f 'fastdot.c'; then $(CYGPATH_W) 'fastdot.c'; else $(CYGPATH_W) '$(srcdir)/fastdot.c'; fi` + +libfast_a-fastlsolve.o: fastlsolve.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastlsolve.o -MD -MP -MF $(DEPDIR)/libfast_a-fastlsolve.Tpo -c -o libfast_a-fastlsolve.o `test -f 'fastlsolve.c' || echo '$(srcdir)/'`fastlsolve.c +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastlsolve.Tpo $(DEPDIR)/libfast_a-fastlsolve.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastlsolve.c' object='libfast_a-fastlsolve.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastlsolve.o `test -f 'fastlsolve.c' || echo '$(srcdir)/'`fastlsolve.c + +libfast_a-fastlsolve.obj: fastlsolve.c +@am__fastdepCC_TRUE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -MT libfast_a-fastlsolve.obj -MD -MP -MF $(DEPDIR)/libfast_a-fastlsolve.Tpo -c -o libfast_a-fastlsolve.obj `if test -f 'fastlsolve.c'; then $(CYGPATH_W) 'fastlsolve.c'; else $(CYGPATH_W) '$(srcdir)/fastlsolve.c'; fi` +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastlsolve.Tpo $(DEPDIR)/libfast_a-fastlsolve.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastlsolve.c' object='libfast_a-fastlsolve.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) $(libfast_a_CFLAGS) $(CFLAGS) -c -o libfast_a-fastlsolve.obj `if test -f 'fastlsolve.c'; then $(CYGPATH_W) 'fastlsolve.c'; else $(CYGPATH_W) '$(srcdir)/fastlsolve.c'; fi` + +.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) '$<'` + +libGIMPACT_a-gim_boxpruning.o: @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_boxpruning.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Tpo -c -o libGIMPACT_a-gim_boxpruning.o `test -f '@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Tpo $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp' object='libGIMPACT_a-gim_boxpruning.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_boxpruning.o `test -f '@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp + +libGIMPACT_a-gim_boxpruning.obj: @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_boxpruning.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Tpo -c -o libGIMPACT_a-gim_boxpruning.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Tpo $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp' object='libGIMPACT_a-gim_boxpruning.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_boxpruning.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp'; fi` + +libGIMPACT_a-gim_contact.o: @TOPDIR@/GIMPACT/src/gim_contact.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_contact.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_contact.Tpo -c -o libGIMPACT_a-gim_contact.o `test -f '@TOPDIR@/GIMPACT/src/gim_contact.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_contact.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_contact.Tpo $(DEPDIR)/libGIMPACT_a-gim_contact.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_contact.cpp' object='libGIMPACT_a-gim_contact.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_contact.o `test -f '@TOPDIR@/GIMPACT/src/gim_contact.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_contact.cpp + +libGIMPACT_a-gim_contact.obj: @TOPDIR@/GIMPACT/src/gim_contact.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_contact.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_contact.Tpo -c -o libGIMPACT_a-gim_contact.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_contact.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_contact.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_contact.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_contact.Tpo $(DEPDIR)/libGIMPACT_a-gim_contact.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_contact.cpp' object='libGIMPACT_a-gim_contact.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_contact.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_contact.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_contact.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_contact.cpp'; fi` + +libGIMPACT_a-gim_math.o: @TOPDIR@/GIMPACT/src/gim_math.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_math.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_math.Tpo -c -o libGIMPACT_a-gim_math.o `test -f '@TOPDIR@/GIMPACT/src/gim_math.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_math.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_math.Tpo $(DEPDIR)/libGIMPACT_a-gim_math.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_math.cpp' object='libGIMPACT_a-gim_math.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_math.o `test -f '@TOPDIR@/GIMPACT/src/gim_math.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_math.cpp + +libGIMPACT_a-gim_math.obj: @TOPDIR@/GIMPACT/src/gim_math.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_math.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_math.Tpo -c -o libGIMPACT_a-gim_math.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_math.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_math.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_math.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_math.Tpo $(DEPDIR)/libGIMPACT_a-gim_math.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_math.cpp' object='libGIMPACT_a-gim_math.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_math.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_math.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_math.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_math.cpp'; fi` + +libGIMPACT_a-gim_memory.o: @TOPDIR@/GIMPACT/src/gim_memory.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_memory.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_memory.Tpo -c -o libGIMPACT_a-gim_memory.o `test -f '@TOPDIR@/GIMPACT/src/gim_memory.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_memory.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_memory.Tpo $(DEPDIR)/libGIMPACT_a-gim_memory.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_memory.cpp' object='libGIMPACT_a-gim_memory.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_memory.o `test -f '@TOPDIR@/GIMPACT/src/gim_memory.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_memory.cpp + +libGIMPACT_a-gim_memory.obj: @TOPDIR@/GIMPACT/src/gim_memory.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_memory.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_memory.Tpo -c -o libGIMPACT_a-gim_memory.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_memory.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_memory.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_memory.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_memory.Tpo $(DEPDIR)/libGIMPACT_a-gim_memory.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_memory.cpp' object='libGIMPACT_a-gim_memory.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_memory.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_memory.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_memory.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_memory.cpp'; fi` + +libGIMPACT_a-gim_tri_tri_overlap.o: @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_tri_tri_overlap.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Tpo -c -o libGIMPACT_a-gim_tri_tri_overlap.o `test -f '@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Tpo $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp' object='libGIMPACT_a-gim_tri_tri_overlap.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_tri_tri_overlap.o `test -f '@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp + +libGIMPACT_a-gim_tri_tri_overlap.obj: @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_tri_tri_overlap.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Tpo -c -o libGIMPACT_a-gim_tri_tri_overlap.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Tpo $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp' object='libGIMPACT_a-gim_tri_tri_overlap.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_tri_tri_overlap.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp'; fi` + +libGIMPACT_a-gim_trimesh.o: @TOPDIR@/GIMPACT/src/gim_trimesh.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh.Tpo -c -o libGIMPACT_a-gim_trimesh.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh.cpp' object='libGIMPACT_a-gim_trimesh.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh.cpp + +libGIMPACT_a-gim_trimesh.obj: @TOPDIR@/GIMPACT/src/gim_trimesh.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh.Tpo -c -o libGIMPACT_a-gim_trimesh.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh.cpp' object='libGIMPACT_a-gim_trimesh.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh.cpp'; fi` + +libGIMPACT_a-gim_trimesh_capsule_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_capsule_collision.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_capsule_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp' object='libGIMPACT_a-gim_trimesh_capsule_collision.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_capsule_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp + +libGIMPACT_a-gim_trimesh_capsule_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_capsule_collision.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_capsule_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp' object='libGIMPACT_a-gim_trimesh_capsule_collision.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_capsule_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp'; fi` + +libGIMPACT_a-gim_trimesh_ray_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_ray_collision.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_ray_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp' object='libGIMPACT_a-gim_trimesh_ray_collision.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_ray_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp + +libGIMPACT_a-gim_trimesh_ray_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_ray_collision.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_ray_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp' object='libGIMPACT_a-gim_trimesh_ray_collision.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_ray_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp'; fi` + +libGIMPACT_a-gim_trimesh_sphere_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_sphere_collision.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_sphere_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp' object='libGIMPACT_a-gim_trimesh_sphere_collision.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_sphere_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp + +libGIMPACT_a-gim_trimesh_sphere_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_sphere_collision.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_sphere_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp' object='libGIMPACT_a-gim_trimesh_sphere_collision.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_sphere_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp'; fi` + +libGIMPACT_a-gim_trimesh_trimesh_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_trimesh_collision.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_trimesh_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp' object='libGIMPACT_a-gim_trimesh_trimesh_collision.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_trimesh_collision.o `test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp + +libGIMPACT_a-gim_trimesh_trimesh_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gim_trimesh_trimesh_collision.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Tpo -c -o libGIMPACT_a-gim_trimesh_trimesh_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp' object='libGIMPACT_a-gim_trimesh_trimesh_collision.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gim_trimesh_trimesh_collision.obj `if test -f '@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp'; fi` + +libGIMPACT_a-gimpact.o: @TOPDIR@/GIMPACT/src/gimpact.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gimpact.o -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gimpact.Tpo -c -o libGIMPACT_a-gimpact.o `test -f '@TOPDIR@/GIMPACT/src/gimpact.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gimpact.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gimpact.Tpo $(DEPDIR)/libGIMPACT_a-gimpact.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gimpact.cpp' object='libGIMPACT_a-gimpact.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gimpact.o `test -f '@TOPDIR@/GIMPACT/src/gimpact.cpp' || echo '$(srcdir)/'`@TOPDIR@/GIMPACT/src/gimpact.cpp + +libGIMPACT_a-gimpact.obj: @TOPDIR@/GIMPACT/src/gimpact.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libGIMPACT_a-gimpact.obj -MD -MP -MF $(DEPDIR)/libGIMPACT_a-gimpact.Tpo -c -o libGIMPACT_a-gimpact.obj `if test -f '@TOPDIR@/GIMPACT/src/gimpact.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gimpact.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gimpact.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gimpact.Tpo $(DEPDIR)/libGIMPACT_a-gimpact.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gimpact.cpp' object='libGIMPACT_a-gimpact.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libGIMPACT_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libGIMPACT_a-gimpact.obj `if test -f '@TOPDIR@/GIMPACT/src/gimpact.cpp'; then $(CYGPATH_W) '@TOPDIR@/GIMPACT/src/gimpact.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/GIMPACT/src/gimpact.cpp'; fi` + +libOPCODE_a-OPC_AABBCollider.o: @TOPDIR@/OPCODE/OPC_AABBCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_AABBCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Tpo -c -o libOPCODE_a-OPC_AABBCollider.o `test -f '@TOPDIR@/OPCODE/OPC_AABBCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_AABBCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBCollider.cpp' object='libOPCODE_a-OPC_AABBCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_AABBCollider.o `test -f '@TOPDIR@/OPCODE/OPC_AABBCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_AABBCollider.cpp + +libOPCODE_a-OPC_AABBCollider.obj: @TOPDIR@/OPCODE/OPC_AABBCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_AABBCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Tpo -c -o libOPCODE_a-OPC_AABBCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_AABBCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_AABBCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_AABBCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBCollider.cpp' object='libOPCODE_a-OPC_AABBCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_AABBCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_AABBCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_AABBCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_AABBCollider.cpp'; fi` + +libOPCODE_a-OPC_AABBTree.o: @TOPDIR@/OPCODE/OPC_AABBTree.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_AABBTree.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Tpo -c -o libOPCODE_a-OPC_AABBTree.o `test -f '@TOPDIR@/OPCODE/OPC_AABBTree.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_AABBTree.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBTree.cpp' object='libOPCODE_a-OPC_AABBTree.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_AABBTree.o `test -f '@TOPDIR@/OPCODE/OPC_AABBTree.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_AABBTree.cpp + +libOPCODE_a-OPC_AABBTree.obj: @TOPDIR@/OPCODE/OPC_AABBTree.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_AABBTree.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Tpo -c -o libOPCODE_a-OPC_AABBTree.obj `if test -f '@TOPDIR@/OPCODE/OPC_AABBTree.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_AABBTree.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_AABBTree.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBTree.cpp' object='libOPCODE_a-OPC_AABBTree.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_AABBTree.obj `if test -f '@TOPDIR@/OPCODE/OPC_AABBTree.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_AABBTree.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_AABBTree.cpp'; fi` + +libOPCODE_a-OPC_BaseModel.o: @TOPDIR@/OPCODE/OPC_BaseModel.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_BaseModel.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Tpo -c -o libOPCODE_a-OPC_BaseModel.o `test -f '@TOPDIR@/OPCODE/OPC_BaseModel.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_BaseModel.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BaseModel.cpp' object='libOPCODE_a-OPC_BaseModel.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_BaseModel.o `test -f '@TOPDIR@/OPCODE/OPC_BaseModel.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_BaseModel.cpp + +libOPCODE_a-OPC_BaseModel.obj: @TOPDIR@/OPCODE/OPC_BaseModel.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_BaseModel.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Tpo -c -o libOPCODE_a-OPC_BaseModel.obj `if test -f '@TOPDIR@/OPCODE/OPC_BaseModel.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_BaseModel.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_BaseModel.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BaseModel.cpp' object='libOPCODE_a-OPC_BaseModel.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_BaseModel.obj `if test -f '@TOPDIR@/OPCODE/OPC_BaseModel.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_BaseModel.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_BaseModel.cpp'; fi` + +libOPCODE_a-OPC_BoxPruning.o: @TOPDIR@/OPCODE/OPC_BoxPruning.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_BoxPruning.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Tpo -c -o libOPCODE_a-OPC_BoxPruning.o `test -f '@TOPDIR@/OPCODE/OPC_BoxPruning.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_BoxPruning.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Tpo $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BoxPruning.cpp' object='libOPCODE_a-OPC_BoxPruning.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_BoxPruning.o `test -f '@TOPDIR@/OPCODE/OPC_BoxPruning.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_BoxPruning.cpp + +libOPCODE_a-OPC_BoxPruning.obj: @TOPDIR@/OPCODE/OPC_BoxPruning.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_BoxPruning.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Tpo -c -o libOPCODE_a-OPC_BoxPruning.obj `if test -f '@TOPDIR@/OPCODE/OPC_BoxPruning.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_BoxPruning.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_BoxPruning.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Tpo $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BoxPruning.cpp' object='libOPCODE_a-OPC_BoxPruning.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_BoxPruning.obj `if test -f '@TOPDIR@/OPCODE/OPC_BoxPruning.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_BoxPruning.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_BoxPruning.cpp'; fi` + +libOPCODE_a-OPC_Collider.o: @TOPDIR@/OPCODE/OPC_Collider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Collider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Collider.Tpo -c -o libOPCODE_a-OPC_Collider.o `test -f '@TOPDIR@/OPCODE/OPC_Collider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Collider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Collider.Tpo $(DEPDIR)/libOPCODE_a-OPC_Collider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Collider.cpp' object='libOPCODE_a-OPC_Collider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Collider.o `test -f '@TOPDIR@/OPCODE/OPC_Collider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Collider.cpp + +libOPCODE_a-OPC_Collider.obj: @TOPDIR@/OPCODE/OPC_Collider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Collider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Collider.Tpo -c -o libOPCODE_a-OPC_Collider.obj `if test -f '@TOPDIR@/OPCODE/OPC_Collider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Collider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Collider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Collider.Tpo $(DEPDIR)/libOPCODE_a-OPC_Collider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Collider.cpp' object='libOPCODE_a-OPC_Collider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Collider.obj `if test -f '@TOPDIR@/OPCODE/OPC_Collider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Collider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Collider.cpp'; fi` + +libOPCODE_a-OPC_Common.o: @TOPDIR@/OPCODE/OPC_Common.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Common.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Common.Tpo -c -o libOPCODE_a-OPC_Common.o `test -f '@TOPDIR@/OPCODE/OPC_Common.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Common.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Common.Tpo $(DEPDIR)/libOPCODE_a-OPC_Common.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Common.cpp' object='libOPCODE_a-OPC_Common.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Common.o `test -f '@TOPDIR@/OPCODE/OPC_Common.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Common.cpp + +libOPCODE_a-OPC_Common.obj: @TOPDIR@/OPCODE/OPC_Common.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Common.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Common.Tpo -c -o libOPCODE_a-OPC_Common.obj `if test -f '@TOPDIR@/OPCODE/OPC_Common.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Common.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Common.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Common.Tpo $(DEPDIR)/libOPCODE_a-OPC_Common.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Common.cpp' object='libOPCODE_a-OPC_Common.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Common.obj `if test -f '@TOPDIR@/OPCODE/OPC_Common.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Common.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Common.cpp'; fi` + +libOPCODE_a-OPC_HybridModel.o: @TOPDIR@/OPCODE/OPC_HybridModel.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_HybridModel.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Tpo -c -o libOPCODE_a-OPC_HybridModel.o `test -f '@TOPDIR@/OPCODE/OPC_HybridModel.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_HybridModel.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_HybridModel.cpp' object='libOPCODE_a-OPC_HybridModel.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_HybridModel.o `test -f '@TOPDIR@/OPCODE/OPC_HybridModel.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_HybridModel.cpp + +libOPCODE_a-OPC_HybridModel.obj: @TOPDIR@/OPCODE/OPC_HybridModel.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_HybridModel.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Tpo -c -o libOPCODE_a-OPC_HybridModel.obj `if test -f '@TOPDIR@/OPCODE/OPC_HybridModel.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_HybridModel.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_HybridModel.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_HybridModel.cpp' object='libOPCODE_a-OPC_HybridModel.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_HybridModel.obj `if test -f '@TOPDIR@/OPCODE/OPC_HybridModel.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_HybridModel.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_HybridModel.cpp'; fi` + +libOPCODE_a-OPC_LSSCollider.o: @TOPDIR@/OPCODE/OPC_LSSCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_LSSCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Tpo -c -o libOPCODE_a-OPC_LSSCollider.o `test -f '@TOPDIR@/OPCODE/OPC_LSSCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_LSSCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_LSSCollider.cpp' object='libOPCODE_a-OPC_LSSCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_LSSCollider.o `test -f '@TOPDIR@/OPCODE/OPC_LSSCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_LSSCollider.cpp + +libOPCODE_a-OPC_LSSCollider.obj: @TOPDIR@/OPCODE/OPC_LSSCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_LSSCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Tpo -c -o libOPCODE_a-OPC_LSSCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_LSSCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_LSSCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_LSSCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_LSSCollider.cpp' object='libOPCODE_a-OPC_LSSCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_LSSCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_LSSCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_LSSCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_LSSCollider.cpp'; fi` + +libOPCODE_a-OPC_MeshInterface.o: @TOPDIR@/OPCODE/OPC_MeshInterface.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_MeshInterface.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Tpo -c -o libOPCODE_a-OPC_MeshInterface.o `test -f '@TOPDIR@/OPCODE/OPC_MeshInterface.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_MeshInterface.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Tpo $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_MeshInterface.cpp' object='libOPCODE_a-OPC_MeshInterface.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_MeshInterface.o `test -f '@TOPDIR@/OPCODE/OPC_MeshInterface.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_MeshInterface.cpp + +libOPCODE_a-OPC_MeshInterface.obj: @TOPDIR@/OPCODE/OPC_MeshInterface.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_MeshInterface.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Tpo -c -o libOPCODE_a-OPC_MeshInterface.obj `if test -f '@TOPDIR@/OPCODE/OPC_MeshInterface.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_MeshInterface.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_MeshInterface.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Tpo $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_MeshInterface.cpp' object='libOPCODE_a-OPC_MeshInterface.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_MeshInterface.obj `if test -f '@TOPDIR@/OPCODE/OPC_MeshInterface.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_MeshInterface.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_MeshInterface.cpp'; fi` + +libOPCODE_a-OPC_Model.o: @TOPDIR@/OPCODE/OPC_Model.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Model.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Model.Tpo -c -o libOPCODE_a-OPC_Model.o `test -f '@TOPDIR@/OPCODE/OPC_Model.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Model.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Model.Tpo $(DEPDIR)/libOPCODE_a-OPC_Model.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Model.cpp' object='libOPCODE_a-OPC_Model.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Model.o `test -f '@TOPDIR@/OPCODE/OPC_Model.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Model.cpp + +libOPCODE_a-OPC_Model.obj: @TOPDIR@/OPCODE/OPC_Model.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Model.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Model.Tpo -c -o libOPCODE_a-OPC_Model.obj `if test -f '@TOPDIR@/OPCODE/OPC_Model.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Model.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Model.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Model.Tpo $(DEPDIR)/libOPCODE_a-OPC_Model.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Model.cpp' object='libOPCODE_a-OPC_Model.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Model.obj `if test -f '@TOPDIR@/OPCODE/OPC_Model.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Model.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Model.cpp'; fi` + +libOPCODE_a-OPC_OBBCollider.o: @TOPDIR@/OPCODE/OPC_OBBCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_OBBCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Tpo -c -o libOPCODE_a-OPC_OBBCollider.o `test -f '@TOPDIR@/OPCODE/OPC_OBBCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_OBBCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OBBCollider.cpp' object='libOPCODE_a-OPC_OBBCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_OBBCollider.o `test -f '@TOPDIR@/OPCODE/OPC_OBBCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_OBBCollider.cpp + +libOPCODE_a-OPC_OBBCollider.obj: @TOPDIR@/OPCODE/OPC_OBBCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_OBBCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Tpo -c -o libOPCODE_a-OPC_OBBCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_OBBCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_OBBCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_OBBCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OBBCollider.cpp' object='libOPCODE_a-OPC_OBBCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_OBBCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_OBBCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_OBBCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_OBBCollider.cpp'; fi` + +libOPCODE_a-Opcode.o: @TOPDIR@/OPCODE/Opcode.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-Opcode.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-Opcode.Tpo -c -o libOPCODE_a-Opcode.o `test -f '@TOPDIR@/OPCODE/Opcode.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Opcode.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-Opcode.Tpo $(DEPDIR)/libOPCODE_a-Opcode.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Opcode.cpp' object='libOPCODE_a-Opcode.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-Opcode.o `test -f '@TOPDIR@/OPCODE/Opcode.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Opcode.cpp + +libOPCODE_a-Opcode.obj: @TOPDIR@/OPCODE/Opcode.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-Opcode.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-Opcode.Tpo -c -o libOPCODE_a-Opcode.obj `if test -f '@TOPDIR@/OPCODE/Opcode.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Opcode.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Opcode.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-Opcode.Tpo $(DEPDIR)/libOPCODE_a-Opcode.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Opcode.cpp' object='libOPCODE_a-Opcode.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-Opcode.obj `if test -f '@TOPDIR@/OPCODE/Opcode.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Opcode.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Opcode.cpp'; fi` + +libOPCODE_a-OPC_OptimizedTree.o: @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_OptimizedTree.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Tpo -c -o libOPCODE_a-OPC_OptimizedTree.o `test -f '@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp' object='libOPCODE_a-OPC_OptimizedTree.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_OptimizedTree.o `test -f '@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp + +libOPCODE_a-OPC_OptimizedTree.obj: @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_OptimizedTree.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Tpo -c -o libOPCODE_a-OPC_OptimizedTree.obj `if test -f '@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp' object='libOPCODE_a-OPC_OptimizedTree.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_OptimizedTree.obj `if test -f '@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp'; fi` + +libOPCODE_a-OPC_Picking.o: @TOPDIR@/OPCODE/OPC_Picking.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Picking.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Picking.Tpo -c -o libOPCODE_a-OPC_Picking.o `test -f '@TOPDIR@/OPCODE/OPC_Picking.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Picking.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Picking.Tpo $(DEPDIR)/libOPCODE_a-OPC_Picking.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Picking.cpp' object='libOPCODE_a-OPC_Picking.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Picking.o `test -f '@TOPDIR@/OPCODE/OPC_Picking.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_Picking.cpp + +libOPCODE_a-OPC_Picking.obj: @TOPDIR@/OPCODE/OPC_Picking.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_Picking.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_Picking.Tpo -c -o libOPCODE_a-OPC_Picking.obj `if test -f '@TOPDIR@/OPCODE/OPC_Picking.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Picking.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Picking.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Picking.Tpo $(DEPDIR)/libOPCODE_a-OPC_Picking.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Picking.cpp' object='libOPCODE_a-OPC_Picking.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_Picking.obj `if test -f '@TOPDIR@/OPCODE/OPC_Picking.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_Picking.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_Picking.cpp'; fi` + +libOPCODE_a-OPC_PlanesCollider.o: @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_PlanesCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Tpo -c -o libOPCODE_a-OPC_PlanesCollider.o `test -f '@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp' object='libOPCODE_a-OPC_PlanesCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_PlanesCollider.o `test -f '@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp + +libOPCODE_a-OPC_PlanesCollider.obj: @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_PlanesCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Tpo -c -o libOPCODE_a-OPC_PlanesCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp' object='libOPCODE_a-OPC_PlanesCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_PlanesCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp'; fi` + +libOPCODE_a-OPC_RayCollider.o: @TOPDIR@/OPCODE/OPC_RayCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_RayCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Tpo -c -o libOPCODE_a-OPC_RayCollider.o `test -f '@TOPDIR@/OPCODE/OPC_RayCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_RayCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_RayCollider.cpp' object='libOPCODE_a-OPC_RayCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_RayCollider.o `test -f '@TOPDIR@/OPCODE/OPC_RayCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_RayCollider.cpp + +libOPCODE_a-OPC_RayCollider.obj: @TOPDIR@/OPCODE/OPC_RayCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_RayCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Tpo -c -o libOPCODE_a-OPC_RayCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_RayCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_RayCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_RayCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_RayCollider.cpp' object='libOPCODE_a-OPC_RayCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_RayCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_RayCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_RayCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_RayCollider.cpp'; fi` + +libOPCODE_a-OPC_SphereCollider.o: @TOPDIR@/OPCODE/OPC_SphereCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_SphereCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Tpo -c -o libOPCODE_a-OPC_SphereCollider.o `test -f '@TOPDIR@/OPCODE/OPC_SphereCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_SphereCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SphereCollider.cpp' object='libOPCODE_a-OPC_SphereCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_SphereCollider.o `test -f '@TOPDIR@/OPCODE/OPC_SphereCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_SphereCollider.cpp + +libOPCODE_a-OPC_SphereCollider.obj: @TOPDIR@/OPCODE/OPC_SphereCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_SphereCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Tpo -c -o libOPCODE_a-OPC_SphereCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_SphereCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_SphereCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_SphereCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SphereCollider.cpp' object='libOPCODE_a-OPC_SphereCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_SphereCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_SphereCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_SphereCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_SphereCollider.cpp'; fi` + +libOPCODE_a-OPC_SweepAndPrune.o: @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_SweepAndPrune.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Tpo -c -o libOPCODE_a-OPC_SweepAndPrune.o `test -f '@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Tpo $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp' object='libOPCODE_a-OPC_SweepAndPrune.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_SweepAndPrune.o `test -f '@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp + +libOPCODE_a-OPC_SweepAndPrune.obj: @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_SweepAndPrune.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Tpo -c -o libOPCODE_a-OPC_SweepAndPrune.obj `if test -f '@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Tpo $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp' object='libOPCODE_a-OPC_SweepAndPrune.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_SweepAndPrune.obj `if test -f '@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp'; fi` + +libOPCODE_a-OPC_TreeBuilders.o: @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_TreeBuilders.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Tpo -c -o libOPCODE_a-OPC_TreeBuilders.o `test -f '@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp' object='libOPCODE_a-OPC_TreeBuilders.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_TreeBuilders.o `test -f '@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp + +libOPCODE_a-OPC_TreeBuilders.obj: @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_TreeBuilders.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Tpo -c -o libOPCODE_a-OPC_TreeBuilders.obj `if test -f '@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp' object='libOPCODE_a-OPC_TreeBuilders.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_TreeBuilders.obj `if test -f '@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp'; fi` + +libOPCODE_a-OPC_TreeCollider.o: @TOPDIR@/OPCODE/OPC_TreeCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_TreeCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Tpo -c -o libOPCODE_a-OPC_TreeCollider.o `test -f '@TOPDIR@/OPCODE/OPC_TreeCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_TreeCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeCollider.cpp' object='libOPCODE_a-OPC_TreeCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_TreeCollider.o `test -f '@TOPDIR@/OPCODE/OPC_TreeCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_TreeCollider.cpp + +libOPCODE_a-OPC_TreeCollider.obj: @TOPDIR@/OPCODE/OPC_TreeCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_TreeCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Tpo -c -o libOPCODE_a-OPC_TreeCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_TreeCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_TreeCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_TreeCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeCollider.cpp' object='libOPCODE_a-OPC_TreeCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_TreeCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_TreeCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_TreeCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_TreeCollider.cpp'; fi` + +libOPCODE_a-OPC_VolumeCollider.o: @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_VolumeCollider.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Tpo -c -o libOPCODE_a-OPC_VolumeCollider.o `test -f '@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp' object='libOPCODE_a-OPC_VolumeCollider.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_VolumeCollider.o `test -f '@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp + +libOPCODE_a-OPC_VolumeCollider.obj: @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-OPC_VolumeCollider.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Tpo -c -o libOPCODE_a-OPC_VolumeCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp' object='libOPCODE_a-OPC_VolumeCollider.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-OPC_VolumeCollider.obj `if test -f '@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp'; fi` + +libOPCODE_a-IceAABB.o: @TOPDIR@/OPCODE/Ice/IceAABB.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceAABB.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceAABB.Tpo -c -o libOPCODE_a-IceAABB.o `test -f '@TOPDIR@/OPCODE/Ice/IceAABB.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceAABB.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceAABB.Tpo $(DEPDIR)/libOPCODE_a-IceAABB.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceAABB.cpp' object='libOPCODE_a-IceAABB.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceAABB.o `test -f '@TOPDIR@/OPCODE/Ice/IceAABB.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceAABB.cpp + +libOPCODE_a-IceAABB.obj: @TOPDIR@/OPCODE/Ice/IceAABB.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceAABB.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceAABB.Tpo -c -o libOPCODE_a-IceAABB.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceAABB.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceAABB.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceAABB.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceAABB.Tpo $(DEPDIR)/libOPCODE_a-IceAABB.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceAABB.cpp' object='libOPCODE_a-IceAABB.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceAABB.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceAABB.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceAABB.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceAABB.cpp'; fi` + +libOPCODE_a-IceContainer.o: @TOPDIR@/OPCODE/Ice/IceContainer.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceContainer.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceContainer.Tpo -c -o libOPCODE_a-IceContainer.o `test -f '@TOPDIR@/OPCODE/Ice/IceContainer.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceContainer.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceContainer.Tpo $(DEPDIR)/libOPCODE_a-IceContainer.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceContainer.cpp' object='libOPCODE_a-IceContainer.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceContainer.o `test -f '@TOPDIR@/OPCODE/Ice/IceContainer.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceContainer.cpp + +libOPCODE_a-IceContainer.obj: @TOPDIR@/OPCODE/Ice/IceContainer.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceContainer.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceContainer.Tpo -c -o libOPCODE_a-IceContainer.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceContainer.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceContainer.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceContainer.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceContainer.Tpo $(DEPDIR)/libOPCODE_a-IceContainer.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceContainer.cpp' object='libOPCODE_a-IceContainer.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceContainer.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceContainer.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceContainer.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceContainer.cpp'; fi` + +libOPCODE_a-IceHPoint.o: @TOPDIR@/OPCODE/Ice/IceHPoint.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceHPoint.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceHPoint.Tpo -c -o libOPCODE_a-IceHPoint.o `test -f '@TOPDIR@/OPCODE/Ice/IceHPoint.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceHPoint.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceHPoint.Tpo $(DEPDIR)/libOPCODE_a-IceHPoint.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceHPoint.cpp' object='libOPCODE_a-IceHPoint.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceHPoint.o `test -f '@TOPDIR@/OPCODE/Ice/IceHPoint.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceHPoint.cpp + +libOPCODE_a-IceHPoint.obj: @TOPDIR@/OPCODE/Ice/IceHPoint.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceHPoint.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceHPoint.Tpo -c -o libOPCODE_a-IceHPoint.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceHPoint.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceHPoint.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceHPoint.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceHPoint.Tpo $(DEPDIR)/libOPCODE_a-IceHPoint.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceHPoint.cpp' object='libOPCODE_a-IceHPoint.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceHPoint.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceHPoint.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceHPoint.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceHPoint.cpp'; fi` + +libOPCODE_a-IceIndexedTriangle.o: @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceIndexedTriangle.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Tpo -c -o libOPCODE_a-IceIndexedTriangle.o `test -f '@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp' object='libOPCODE_a-IceIndexedTriangle.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceIndexedTriangle.o `test -f '@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp + +libOPCODE_a-IceIndexedTriangle.obj: @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceIndexedTriangle.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Tpo -c -o libOPCODE_a-IceIndexedTriangle.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp' object='libOPCODE_a-IceIndexedTriangle.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceIndexedTriangle.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp'; fi` + +libOPCODE_a-IceMatrix3x3.o: @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceMatrix3x3.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Tpo -c -o libOPCODE_a-IceMatrix3x3.o `test -f '@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp' object='libOPCODE_a-IceMatrix3x3.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceMatrix3x3.o `test -f '@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp + +libOPCODE_a-IceMatrix3x3.obj: @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceMatrix3x3.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Tpo -c -o libOPCODE_a-IceMatrix3x3.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp' object='libOPCODE_a-IceMatrix3x3.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceMatrix3x3.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp'; fi` + +libOPCODE_a-IceMatrix4x4.o: @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceMatrix4x4.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Tpo -c -o libOPCODE_a-IceMatrix4x4.o `test -f '@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp' object='libOPCODE_a-IceMatrix4x4.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceMatrix4x4.o `test -f '@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp + +libOPCODE_a-IceMatrix4x4.obj: @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceMatrix4x4.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Tpo -c -o libOPCODE_a-IceMatrix4x4.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp' object='libOPCODE_a-IceMatrix4x4.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceMatrix4x4.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp'; fi` + +libOPCODE_a-IceOBB.o: @TOPDIR@/OPCODE/Ice/IceOBB.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceOBB.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceOBB.Tpo -c -o libOPCODE_a-IceOBB.o `test -f '@TOPDIR@/OPCODE/Ice/IceOBB.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceOBB.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceOBB.Tpo $(DEPDIR)/libOPCODE_a-IceOBB.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceOBB.cpp' object='libOPCODE_a-IceOBB.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceOBB.o `test -f '@TOPDIR@/OPCODE/Ice/IceOBB.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceOBB.cpp + +libOPCODE_a-IceOBB.obj: @TOPDIR@/OPCODE/Ice/IceOBB.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceOBB.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceOBB.Tpo -c -o libOPCODE_a-IceOBB.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceOBB.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceOBB.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceOBB.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceOBB.Tpo $(DEPDIR)/libOPCODE_a-IceOBB.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceOBB.cpp' object='libOPCODE_a-IceOBB.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceOBB.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceOBB.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceOBB.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceOBB.cpp'; fi` + +libOPCODE_a-IcePlane.o: @TOPDIR@/OPCODE/Ice/IcePlane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IcePlane.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IcePlane.Tpo -c -o libOPCODE_a-IcePlane.o `test -f '@TOPDIR@/OPCODE/Ice/IcePlane.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IcePlane.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePlane.Tpo $(DEPDIR)/libOPCODE_a-IcePlane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePlane.cpp' object='libOPCODE_a-IcePlane.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IcePlane.o `test -f '@TOPDIR@/OPCODE/Ice/IcePlane.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IcePlane.cpp + +libOPCODE_a-IcePlane.obj: @TOPDIR@/OPCODE/Ice/IcePlane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IcePlane.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IcePlane.Tpo -c -o libOPCODE_a-IcePlane.obj `if test -f '@TOPDIR@/OPCODE/Ice/IcePlane.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IcePlane.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IcePlane.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePlane.Tpo $(DEPDIR)/libOPCODE_a-IcePlane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePlane.cpp' object='libOPCODE_a-IcePlane.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IcePlane.obj `if test -f '@TOPDIR@/OPCODE/Ice/IcePlane.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IcePlane.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IcePlane.cpp'; fi` + +libOPCODE_a-IcePoint.o: @TOPDIR@/OPCODE/Ice/IcePoint.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IcePoint.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IcePoint.Tpo -c -o libOPCODE_a-IcePoint.o `test -f '@TOPDIR@/OPCODE/Ice/IcePoint.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IcePoint.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePoint.Tpo $(DEPDIR)/libOPCODE_a-IcePoint.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePoint.cpp' object='libOPCODE_a-IcePoint.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IcePoint.o `test -f '@TOPDIR@/OPCODE/Ice/IcePoint.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IcePoint.cpp + +libOPCODE_a-IcePoint.obj: @TOPDIR@/OPCODE/Ice/IcePoint.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IcePoint.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IcePoint.Tpo -c -o libOPCODE_a-IcePoint.obj `if test -f '@TOPDIR@/OPCODE/Ice/IcePoint.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IcePoint.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IcePoint.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePoint.Tpo $(DEPDIR)/libOPCODE_a-IcePoint.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePoint.cpp' object='libOPCODE_a-IcePoint.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IcePoint.obj `if test -f '@TOPDIR@/OPCODE/Ice/IcePoint.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IcePoint.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IcePoint.cpp'; fi` + +libOPCODE_a-IceRandom.o: @TOPDIR@/OPCODE/Ice/IceRandom.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceRandom.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceRandom.Tpo -c -o libOPCODE_a-IceRandom.o `test -f '@TOPDIR@/OPCODE/Ice/IceRandom.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceRandom.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRandom.Tpo $(DEPDIR)/libOPCODE_a-IceRandom.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRandom.cpp' object='libOPCODE_a-IceRandom.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceRandom.o `test -f '@TOPDIR@/OPCODE/Ice/IceRandom.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceRandom.cpp + +libOPCODE_a-IceRandom.obj: @TOPDIR@/OPCODE/Ice/IceRandom.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceRandom.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceRandom.Tpo -c -o libOPCODE_a-IceRandom.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceRandom.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceRandom.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceRandom.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRandom.Tpo $(DEPDIR)/libOPCODE_a-IceRandom.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRandom.cpp' object='libOPCODE_a-IceRandom.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceRandom.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceRandom.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceRandom.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceRandom.cpp'; fi` + +libOPCODE_a-IceRay.o: @TOPDIR@/OPCODE/Ice/IceRay.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceRay.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceRay.Tpo -c -o libOPCODE_a-IceRay.o `test -f '@TOPDIR@/OPCODE/Ice/IceRay.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceRay.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRay.Tpo $(DEPDIR)/libOPCODE_a-IceRay.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRay.cpp' object='libOPCODE_a-IceRay.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceRay.o `test -f '@TOPDIR@/OPCODE/Ice/IceRay.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceRay.cpp + +libOPCODE_a-IceRay.obj: @TOPDIR@/OPCODE/Ice/IceRay.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceRay.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceRay.Tpo -c -o libOPCODE_a-IceRay.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceRay.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceRay.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceRay.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRay.Tpo $(DEPDIR)/libOPCODE_a-IceRay.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRay.cpp' object='libOPCODE_a-IceRay.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceRay.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceRay.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceRay.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceRay.cpp'; fi` + +libOPCODE_a-IceRevisitedRadix.o: @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceRevisitedRadix.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Tpo -c -o libOPCODE_a-IceRevisitedRadix.o `test -f '@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Tpo $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp' object='libOPCODE_a-IceRevisitedRadix.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceRevisitedRadix.o `test -f '@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp + +libOPCODE_a-IceRevisitedRadix.obj: @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceRevisitedRadix.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Tpo -c -o libOPCODE_a-IceRevisitedRadix.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Tpo $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp' object='libOPCODE_a-IceRevisitedRadix.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceRevisitedRadix.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp'; fi` + +libOPCODE_a-IceSegment.o: @TOPDIR@/OPCODE/Ice/IceSegment.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceSegment.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceSegment.Tpo -c -o libOPCODE_a-IceSegment.o `test -f '@TOPDIR@/OPCODE/Ice/IceSegment.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceSegment.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceSegment.Tpo $(DEPDIR)/libOPCODE_a-IceSegment.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceSegment.cpp' object='libOPCODE_a-IceSegment.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceSegment.o `test -f '@TOPDIR@/OPCODE/Ice/IceSegment.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceSegment.cpp + +libOPCODE_a-IceSegment.obj: @TOPDIR@/OPCODE/Ice/IceSegment.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceSegment.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceSegment.Tpo -c -o libOPCODE_a-IceSegment.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceSegment.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceSegment.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceSegment.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceSegment.Tpo $(DEPDIR)/libOPCODE_a-IceSegment.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceSegment.cpp' object='libOPCODE_a-IceSegment.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceSegment.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceSegment.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceSegment.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceSegment.cpp'; fi` + +libOPCODE_a-IceTriangle.o: @TOPDIR@/OPCODE/Ice/IceTriangle.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceTriangle.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceTriangle.Tpo -c -o libOPCODE_a-IceTriangle.o `test -f '@TOPDIR@/OPCODE/Ice/IceTriangle.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceTriangle.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceTriangle.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceTriangle.cpp' object='libOPCODE_a-IceTriangle.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceTriangle.o `test -f '@TOPDIR@/OPCODE/Ice/IceTriangle.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceTriangle.cpp + +libOPCODE_a-IceTriangle.obj: @TOPDIR@/OPCODE/Ice/IceTriangle.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceTriangle.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceTriangle.Tpo -c -o libOPCODE_a-IceTriangle.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceTriangle.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceTriangle.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceTriangle.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceTriangle.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceTriangle.cpp' object='libOPCODE_a-IceTriangle.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceTriangle.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceTriangle.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceTriangle.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceTriangle.cpp'; fi` + +libOPCODE_a-IceUtils.o: @TOPDIR@/OPCODE/Ice/IceUtils.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceUtils.o -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceUtils.Tpo -c -o libOPCODE_a-IceUtils.o `test -f '@TOPDIR@/OPCODE/Ice/IceUtils.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceUtils.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceUtils.Tpo $(DEPDIR)/libOPCODE_a-IceUtils.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceUtils.cpp' object='libOPCODE_a-IceUtils.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceUtils.o `test -f '@TOPDIR@/OPCODE/Ice/IceUtils.cpp' || echo '$(srcdir)/'`@TOPDIR@/OPCODE/Ice/IceUtils.cpp + +libOPCODE_a-IceUtils.obj: @TOPDIR@/OPCODE/Ice/IceUtils.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libOPCODE_a-IceUtils.obj -MD -MP -MF $(DEPDIR)/libOPCODE_a-IceUtils.Tpo -c -o libOPCODE_a-IceUtils.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceUtils.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceUtils.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceUtils.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceUtils.Tpo $(DEPDIR)/libOPCODE_a-IceUtils.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceUtils.cpp' object='libOPCODE_a-IceUtils.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libOPCODE_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libOPCODE_a-IceUtils.obj `if test -f '@TOPDIR@/OPCODE/Ice/IceUtils.cpp'; then $(CYGPATH_W) '@TOPDIR@/OPCODE/Ice/IceUtils.cpp'; else $(CYGPATH_W) '$(srcdir)/@TOPDIR@/OPCODE/Ice/IceUtils.cpp'; fi` + +libode_a-obstack.o: obstack.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-obstack.o -MD -MP -MF $(DEPDIR)/libode_a-obstack.Tpo -c -o libode_a-obstack.o `test -f 'obstack.cpp' || echo '$(srcdir)/'`obstack.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-obstack.Tpo $(DEPDIR)/libode_a-obstack.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='obstack.cpp' object='libode_a-obstack.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-obstack.o `test -f 'obstack.cpp' || echo '$(srcdir)/'`obstack.cpp + +libode_a-obstack.obj: obstack.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-obstack.obj -MD -MP -MF $(DEPDIR)/libode_a-obstack.Tpo -c -o libode_a-obstack.obj `if test -f 'obstack.cpp'; then $(CYGPATH_W) 'obstack.cpp'; else $(CYGPATH_W) '$(srcdir)/obstack.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-obstack.Tpo $(DEPDIR)/libode_a-obstack.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='obstack.cpp' object='libode_a-obstack.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-obstack.obj `if test -f 'obstack.cpp'; then $(CYGPATH_W) 'obstack.cpp'; else $(CYGPATH_W) '$(srcdir)/obstack.cpp'; fi` + +libode_a-collision_util.o: collision_util.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_util.o -MD -MP -MF $(DEPDIR)/libode_a-collision_util.Tpo -c -o libode_a-collision_util.o `test -f 'collision_util.cpp' || echo '$(srcdir)/'`collision_util.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_util.Tpo $(DEPDIR)/libode_a-collision_util.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_util.cpp' object='libode_a-collision_util.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_util.o `test -f 'collision_util.cpp' || echo '$(srcdir)/'`collision_util.cpp + +libode_a-collision_util.obj: collision_util.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_util.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_util.Tpo -c -o libode_a-collision_util.obj `if test -f 'collision_util.cpp'; then $(CYGPATH_W) 'collision_util.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_util.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_util.Tpo $(DEPDIR)/libode_a-collision_util.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_util.cpp' object='libode_a-collision_util.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_util.obj `if test -f 'collision_util.cpp'; then $(CYGPATH_W) 'collision_util.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_util.cpp'; fi` + +libode_a-array.o: array.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-array.o -MD -MP -MF $(DEPDIR)/libode_a-array.Tpo -c -o libode_a-array.o `test -f 'array.cpp' || echo '$(srcdir)/'`array.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-array.Tpo $(DEPDIR)/libode_a-array.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='array.cpp' object='libode_a-array.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-array.o `test -f 'array.cpp' || echo '$(srcdir)/'`array.cpp + +libode_a-array.obj: array.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-array.obj -MD -MP -MF $(DEPDIR)/libode_a-array.Tpo -c -o libode_a-array.obj `if test -f 'array.cpp'; then $(CYGPATH_W) 'array.cpp'; else $(CYGPATH_W) '$(srcdir)/array.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-array.Tpo $(DEPDIR)/libode_a-array.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='array.cpp' object='libode_a-array.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-array.obj `if test -f 'array.cpp'; then $(CYGPATH_W) 'array.cpp'; else $(CYGPATH_W) '$(srcdir)/array.cpp'; fi` + +libode_a-ode.o: ode.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-ode.o -MD -MP -MF $(DEPDIR)/libode_a-ode.Tpo -c -o libode_a-ode.o `test -f 'ode.cpp' || echo '$(srcdir)/'`ode.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ode.Tpo $(DEPDIR)/libode_a-ode.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ode.cpp' object='libode_a-ode.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-ode.o `test -f 'ode.cpp' || echo '$(srcdir)/'`ode.cpp + +libode_a-ode.obj: ode.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-ode.obj -MD -MP -MF $(DEPDIR)/libode_a-ode.Tpo -c -o libode_a-ode.obj `if test -f 'ode.cpp'; then $(CYGPATH_W) 'ode.cpp'; else $(CYGPATH_W) '$(srcdir)/ode.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ode.Tpo $(DEPDIR)/libode_a-ode.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ode.cpp' object='libode_a-ode.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-ode.obj `if test -f 'ode.cpp'; then $(CYGPATH_W) 'ode.cpp'; else $(CYGPATH_W) '$(srcdir)/ode.cpp'; fi` + +libode_a-error.o: error.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-error.o -MD -MP -MF $(DEPDIR)/libode_a-error.Tpo -c -o libode_a-error.o `test -f 'error.cpp' || echo '$(srcdir)/'`error.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-error.Tpo $(DEPDIR)/libode_a-error.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='error.cpp' object='libode_a-error.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-error.o `test -f 'error.cpp' || echo '$(srcdir)/'`error.cpp + +libode_a-error.obj: error.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-error.obj -MD -MP -MF $(DEPDIR)/libode_a-error.Tpo -c -o libode_a-error.obj `if test -f 'error.cpp'; then $(CYGPATH_W) 'error.cpp'; else $(CYGPATH_W) '$(srcdir)/error.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-error.Tpo $(DEPDIR)/libode_a-error.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='error.cpp' object='libode_a-error.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-error.obj `if test -f 'error.cpp'; then $(CYGPATH_W) 'error.cpp'; else $(CYGPATH_W) '$(srcdir)/error.cpp'; fi` + +libode_a-odemath.o: odemath.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-odemath.o -MD -MP -MF $(DEPDIR)/libode_a-odemath.Tpo -c -o libode_a-odemath.o `test -f 'odemath.cpp' || echo '$(srcdir)/'`odemath.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-odemath.Tpo $(DEPDIR)/libode_a-odemath.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='odemath.cpp' object='libode_a-odemath.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-odemath.o `test -f 'odemath.cpp' || echo '$(srcdir)/'`odemath.cpp + +libode_a-odemath.obj: odemath.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-odemath.obj -MD -MP -MF $(DEPDIR)/libode_a-odemath.Tpo -c -o libode_a-odemath.obj `if test -f 'odemath.cpp'; then $(CYGPATH_W) 'odemath.cpp'; else $(CYGPATH_W) '$(srcdir)/odemath.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-odemath.Tpo $(DEPDIR)/libode_a-odemath.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='odemath.cpp' object='libode_a-odemath.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-odemath.obj `if test -f 'odemath.cpp'; then $(CYGPATH_W) 'odemath.cpp'; else $(CYGPATH_W) '$(srcdir)/odemath.cpp'; fi` + +libode_a-collision_kernel.o: collision_kernel.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_kernel.o -MD -MP -MF $(DEPDIR)/libode_a-collision_kernel.Tpo -c -o libode_a-collision_kernel.o `test -f 'collision_kernel.cpp' || echo '$(srcdir)/'`collision_kernel.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_kernel.Tpo $(DEPDIR)/libode_a-collision_kernel.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_kernel.cpp' object='libode_a-collision_kernel.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_kernel.o `test -f 'collision_kernel.cpp' || echo '$(srcdir)/'`collision_kernel.cpp + +libode_a-collision_kernel.obj: collision_kernel.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_kernel.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_kernel.Tpo -c -o libode_a-collision_kernel.obj `if test -f 'collision_kernel.cpp'; then $(CYGPATH_W) 'collision_kernel.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_kernel.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_kernel.Tpo $(DEPDIR)/libode_a-collision_kernel.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_kernel.cpp' object='libode_a-collision_kernel.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_kernel.obj `if test -f 'collision_kernel.cpp'; then $(CYGPATH_W) 'collision_kernel.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_kernel.cpp'; fi` + +libode_a-export-dif.o: export-dif.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-export-dif.o -MD -MP -MF $(DEPDIR)/libode_a-export-dif.Tpo -c -o libode_a-export-dif.o `test -f 'export-dif.cpp' || echo '$(srcdir)/'`export-dif.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-export-dif.Tpo $(DEPDIR)/libode_a-export-dif.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='export-dif.cpp' object='libode_a-export-dif.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-export-dif.o `test -f 'export-dif.cpp' || echo '$(srcdir)/'`export-dif.cpp + +libode_a-export-dif.obj: export-dif.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-export-dif.obj -MD -MP -MF $(DEPDIR)/libode_a-export-dif.Tpo -c -o libode_a-export-dif.obj `if test -f 'export-dif.cpp'; then $(CYGPATH_W) 'export-dif.cpp'; else $(CYGPATH_W) '$(srcdir)/export-dif.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-export-dif.Tpo $(DEPDIR)/libode_a-export-dif.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='export-dif.cpp' object='libode_a-export-dif.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-export-dif.obj `if test -f 'export-dif.cpp'; then $(CYGPATH_W) 'export-dif.cpp'; else $(CYGPATH_W) '$(srcdir)/export-dif.cpp'; fi` + +libode_a-quickstep.o: quickstep.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-quickstep.o -MD -MP -MF $(DEPDIR)/libode_a-quickstep.Tpo -c -o libode_a-quickstep.o `test -f 'quickstep.cpp' || echo '$(srcdir)/'`quickstep.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-quickstep.Tpo $(DEPDIR)/libode_a-quickstep.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='quickstep.cpp' object='libode_a-quickstep.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-quickstep.o `test -f 'quickstep.cpp' || echo '$(srcdir)/'`quickstep.cpp + +libode_a-quickstep.obj: quickstep.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-quickstep.obj -MD -MP -MF $(DEPDIR)/libode_a-quickstep.Tpo -c -o libode_a-quickstep.obj `if test -f 'quickstep.cpp'; then $(CYGPATH_W) 'quickstep.cpp'; else $(CYGPATH_W) '$(srcdir)/quickstep.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-quickstep.Tpo $(DEPDIR)/libode_a-quickstep.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='quickstep.cpp' object='libode_a-quickstep.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-quickstep.obj `if test -f 'quickstep.cpp'; then $(CYGPATH_W) 'quickstep.cpp'; else $(CYGPATH_W) '$(srcdir)/quickstep.cpp'; fi` + +libode_a-collision_quadtreespace.o: collision_quadtreespace.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_quadtreespace.o -MD -MP -MF $(DEPDIR)/libode_a-collision_quadtreespace.Tpo -c -o libode_a-collision_quadtreespace.o `test -f 'collision_quadtreespace.cpp' || echo '$(srcdir)/'`collision_quadtreespace.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_quadtreespace.Tpo $(DEPDIR)/libode_a-collision_quadtreespace.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_quadtreespace.cpp' object='libode_a-collision_quadtreespace.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_quadtreespace.o `test -f 'collision_quadtreespace.cpp' || echo '$(srcdir)/'`collision_quadtreespace.cpp + +libode_a-collision_quadtreespace.obj: collision_quadtreespace.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_quadtreespace.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_quadtreespace.Tpo -c -o libode_a-collision_quadtreespace.obj `if test -f 'collision_quadtreespace.cpp'; then $(CYGPATH_W) 'collision_quadtreespace.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_quadtreespace.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_quadtreespace.Tpo $(DEPDIR)/libode_a-collision_quadtreespace.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_quadtreespace.cpp' object='libode_a-collision_quadtreespace.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_quadtreespace.obj `if test -f 'collision_quadtreespace.cpp'; then $(CYGPATH_W) 'collision_quadtreespace.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_quadtreespace.cpp'; fi` + +libode_a-rotation.o: rotation.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-rotation.o -MD -MP -MF $(DEPDIR)/libode_a-rotation.Tpo -c -o libode_a-rotation.o `test -f 'rotation.cpp' || echo '$(srcdir)/'`rotation.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-rotation.Tpo $(DEPDIR)/libode_a-rotation.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='rotation.cpp' object='libode_a-rotation.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-rotation.o `test -f 'rotation.cpp' || echo '$(srcdir)/'`rotation.cpp + +libode_a-rotation.obj: rotation.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-rotation.obj -MD -MP -MF $(DEPDIR)/libode_a-rotation.Tpo -c -o libode_a-rotation.obj `if test -f 'rotation.cpp'; then $(CYGPATH_W) 'rotation.cpp'; else $(CYGPATH_W) '$(srcdir)/rotation.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-rotation.Tpo $(DEPDIR)/libode_a-rotation.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='rotation.cpp' object='libode_a-rotation.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-rotation.obj `if test -f 'rotation.cpp'; then $(CYGPATH_W) 'rotation.cpp'; else $(CYGPATH_W) '$(srcdir)/rotation.cpp'; fi` + +libode_a-collision_space.o: collision_space.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_space.o -MD -MP -MF $(DEPDIR)/libode_a-collision_space.Tpo -c -o libode_a-collision_space.o `test -f 'collision_space.cpp' || echo '$(srcdir)/'`collision_space.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_space.Tpo $(DEPDIR)/libode_a-collision_space.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_space.cpp' object='libode_a-collision_space.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_space.o `test -f 'collision_space.cpp' || echo '$(srcdir)/'`collision_space.cpp + +libode_a-collision_space.obj: collision_space.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_space.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_space.Tpo -c -o libode_a-collision_space.obj `if test -f 'collision_space.cpp'; then $(CYGPATH_W) 'collision_space.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_space.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_space.Tpo $(DEPDIR)/libode_a-collision_space.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_space.cpp' object='libode_a-collision_space.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_space.obj `if test -f 'collision_space.cpp'; then $(CYGPATH_W) 'collision_space.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_space.cpp'; fi` + +libode_a-collision_cylinder_box.o: collision_cylinder_box.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_box.o -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_box.Tpo -c -o libode_a-collision_cylinder_box.o `test -f 'collision_cylinder_box.cpp' || echo '$(srcdir)/'`collision_cylinder_box.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_box.Tpo $(DEPDIR)/libode_a-collision_cylinder_box.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_box.cpp' object='libode_a-collision_cylinder_box.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_box.o `test -f 'collision_cylinder_box.cpp' || echo '$(srcdir)/'`collision_cylinder_box.cpp + +libode_a-collision_cylinder_box.obj: collision_cylinder_box.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_box.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_box.Tpo -c -o libode_a-collision_cylinder_box.obj `if test -f 'collision_cylinder_box.cpp'; then $(CYGPATH_W) 'collision_cylinder_box.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_box.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_box.Tpo $(DEPDIR)/libode_a-collision_cylinder_box.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_box.cpp' object='libode_a-collision_cylinder_box.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_box.obj `if test -f 'collision_cylinder_box.cpp'; then $(CYGPATH_W) 'collision_cylinder_box.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_box.cpp'; fi` + +libode_a-collision_cylinder_sphere.o: collision_cylinder_sphere.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_sphere.o -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_sphere.Tpo -c -o libode_a-collision_cylinder_sphere.o `test -f 'collision_cylinder_sphere.cpp' || echo '$(srcdir)/'`collision_cylinder_sphere.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_sphere.Tpo $(DEPDIR)/libode_a-collision_cylinder_sphere.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_sphere.cpp' object='libode_a-collision_cylinder_sphere.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_sphere.o `test -f 'collision_cylinder_sphere.cpp' || echo '$(srcdir)/'`collision_cylinder_sphere.cpp + +libode_a-collision_cylinder_sphere.obj: collision_cylinder_sphere.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_sphere.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_sphere.Tpo -c -o libode_a-collision_cylinder_sphere.obj `if test -f 'collision_cylinder_sphere.cpp'; then $(CYGPATH_W) 'collision_cylinder_sphere.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_sphere.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_sphere.Tpo $(DEPDIR)/libode_a-collision_cylinder_sphere.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_sphere.cpp' object='libode_a-collision_cylinder_sphere.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_sphere.obj `if test -f 'collision_cylinder_sphere.cpp'; then $(CYGPATH_W) 'collision_cylinder_sphere.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_sphere.cpp'; fi` + +libode_a-collision_cylinder_plane.o: collision_cylinder_plane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_plane.o -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_plane.Tpo -c -o libode_a-collision_cylinder_plane.o `test -f 'collision_cylinder_plane.cpp' || echo '$(srcdir)/'`collision_cylinder_plane.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_plane.Tpo $(DEPDIR)/libode_a-collision_cylinder_plane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_plane.cpp' object='libode_a-collision_cylinder_plane.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_plane.o `test -f 'collision_cylinder_plane.cpp' || echo '$(srcdir)/'`collision_cylinder_plane.cpp + +libode_a-collision_cylinder_plane.obj: collision_cylinder_plane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_plane.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_plane.Tpo -c -o libode_a-collision_cylinder_plane.obj `if test -f 'collision_cylinder_plane.cpp'; then $(CYGPATH_W) 'collision_cylinder_plane.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_plane.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_plane.Tpo $(DEPDIR)/libode_a-collision_cylinder_plane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_plane.cpp' object='libode_a-collision_cylinder_plane.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_plane.obj `if test -f 'collision_cylinder_plane.cpp'; then $(CYGPATH_W) 'collision_cylinder_plane.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_plane.cpp'; fi` + +libode_a-sphere.o: sphere.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-sphere.o -MD -MP -MF $(DEPDIR)/libode_a-sphere.Tpo -c -o libode_a-sphere.o `test -f 'sphere.cpp' || echo '$(srcdir)/'`sphere.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-sphere.Tpo $(DEPDIR)/libode_a-sphere.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='sphere.cpp' object='libode_a-sphere.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-sphere.o `test -f 'sphere.cpp' || echo '$(srcdir)/'`sphere.cpp + +libode_a-sphere.obj: sphere.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-sphere.obj -MD -MP -MF $(DEPDIR)/libode_a-sphere.Tpo -c -o libode_a-sphere.obj `if test -f 'sphere.cpp'; then $(CYGPATH_W) 'sphere.cpp'; else $(CYGPATH_W) '$(srcdir)/sphere.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-sphere.Tpo $(DEPDIR)/libode_a-sphere.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='sphere.cpp' object='libode_a-sphere.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-sphere.obj `if test -f 'sphere.cpp'; then $(CYGPATH_W) 'sphere.cpp'; else $(CYGPATH_W) '$(srcdir)/sphere.cpp'; fi` + +libode_a-box.o: box.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-box.o -MD -MP -MF $(DEPDIR)/libode_a-box.Tpo -c -o libode_a-box.o `test -f 'box.cpp' || echo '$(srcdir)/'`box.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-box.Tpo $(DEPDIR)/libode_a-box.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='box.cpp' object='libode_a-box.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-box.o `test -f 'box.cpp' || echo '$(srcdir)/'`box.cpp + +libode_a-box.obj: box.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-box.obj -MD -MP -MF $(DEPDIR)/libode_a-box.Tpo -c -o libode_a-box.obj `if test -f 'box.cpp'; then $(CYGPATH_W) 'box.cpp'; else $(CYGPATH_W) '$(srcdir)/box.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-box.Tpo $(DEPDIR)/libode_a-box.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='box.cpp' object='libode_a-box.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-box.obj `if test -f 'box.cpp'; then $(CYGPATH_W) 'box.cpp'; else $(CYGPATH_W) '$(srcdir)/box.cpp'; fi` + +libode_a-capsule.o: capsule.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-capsule.o -MD -MP -MF $(DEPDIR)/libode_a-capsule.Tpo -c -o libode_a-capsule.o `test -f 'capsule.cpp' || echo '$(srcdir)/'`capsule.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-capsule.Tpo $(DEPDIR)/libode_a-capsule.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='capsule.cpp' object='libode_a-capsule.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-capsule.o `test -f 'capsule.cpp' || echo '$(srcdir)/'`capsule.cpp + +libode_a-capsule.obj: capsule.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-capsule.obj -MD -MP -MF $(DEPDIR)/libode_a-capsule.Tpo -c -o libode_a-capsule.obj `if test -f 'capsule.cpp'; then $(CYGPATH_W) 'capsule.cpp'; else $(CYGPATH_W) '$(srcdir)/capsule.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-capsule.Tpo $(DEPDIR)/libode_a-capsule.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='capsule.cpp' object='libode_a-capsule.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-capsule.obj `if test -f 'capsule.cpp'; then $(CYGPATH_W) 'capsule.cpp'; else $(CYGPATH_W) '$(srcdir)/capsule.cpp'; fi` + +libode_a-plane.o: plane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-plane.o -MD -MP -MF $(DEPDIR)/libode_a-plane.Tpo -c -o libode_a-plane.o `test -f 'plane.cpp' || echo '$(srcdir)/'`plane.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-plane.Tpo $(DEPDIR)/libode_a-plane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='plane.cpp' object='libode_a-plane.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-plane.o `test -f 'plane.cpp' || echo '$(srcdir)/'`plane.cpp + +libode_a-plane.obj: plane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-plane.obj -MD -MP -MF $(DEPDIR)/libode_a-plane.Tpo -c -o libode_a-plane.obj `if test -f 'plane.cpp'; then $(CYGPATH_W) 'plane.cpp'; else $(CYGPATH_W) '$(srcdir)/plane.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-plane.Tpo $(DEPDIR)/libode_a-plane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='plane.cpp' object='libode_a-plane.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-plane.obj `if test -f 'plane.cpp'; then $(CYGPATH_W) 'plane.cpp'; else $(CYGPATH_W) '$(srcdir)/plane.cpp'; fi` + +libode_a-ray.o: ray.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-ray.o -MD -MP -MF $(DEPDIR)/libode_a-ray.Tpo -c -o libode_a-ray.o `test -f 'ray.cpp' || echo '$(srcdir)/'`ray.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ray.Tpo $(DEPDIR)/libode_a-ray.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ray.cpp' object='libode_a-ray.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-ray.o `test -f 'ray.cpp' || echo '$(srcdir)/'`ray.cpp + +libode_a-ray.obj: ray.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-ray.obj -MD -MP -MF $(DEPDIR)/libode_a-ray.Tpo -c -o libode_a-ray.obj `if test -f 'ray.cpp'; then $(CYGPATH_W) 'ray.cpp'; else $(CYGPATH_W) '$(srcdir)/ray.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ray.Tpo $(DEPDIR)/libode_a-ray.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ray.cpp' object='libode_a-ray.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-ray.obj `if test -f 'ray.cpp'; then $(CYGPATH_W) 'ray.cpp'; else $(CYGPATH_W) '$(srcdir)/ray.cpp'; fi` + +libode_a-cylinder.o: cylinder.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-cylinder.o -MD -MP -MF $(DEPDIR)/libode_a-cylinder.Tpo -c -o libode_a-cylinder.o `test -f 'cylinder.cpp' || echo '$(srcdir)/'`cylinder.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-cylinder.Tpo $(DEPDIR)/libode_a-cylinder.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='cylinder.cpp' object='libode_a-cylinder.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-cylinder.o `test -f 'cylinder.cpp' || echo '$(srcdir)/'`cylinder.cpp + +libode_a-cylinder.obj: cylinder.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-cylinder.obj -MD -MP -MF $(DEPDIR)/libode_a-cylinder.Tpo -c -o libode_a-cylinder.obj `if test -f 'cylinder.cpp'; then $(CYGPATH_W) 'cylinder.cpp'; else $(CYGPATH_W) '$(srcdir)/cylinder.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-cylinder.Tpo $(DEPDIR)/libode_a-cylinder.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='cylinder.cpp' object='libode_a-cylinder.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-cylinder.obj `if test -f 'cylinder.cpp'; then $(CYGPATH_W) 'cylinder.cpp'; else $(CYGPATH_W) '$(srcdir)/cylinder.cpp'; fi` + +libode_a-convex.o: convex.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-convex.o -MD -MP -MF $(DEPDIR)/libode_a-convex.Tpo -c -o libode_a-convex.o `test -f 'convex.cpp' || echo '$(srcdir)/'`convex.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-convex.Tpo $(DEPDIR)/libode_a-convex.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='convex.cpp' object='libode_a-convex.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-convex.o `test -f 'convex.cpp' || echo '$(srcdir)/'`convex.cpp + +libode_a-convex.obj: convex.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-convex.obj -MD -MP -MF $(DEPDIR)/libode_a-convex.Tpo -c -o libode_a-convex.obj `if test -f 'convex.cpp'; then $(CYGPATH_W) 'convex.cpp'; else $(CYGPATH_W) '$(srcdir)/convex.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-convex.Tpo $(DEPDIR)/libode_a-convex.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='convex.cpp' object='libode_a-convex.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-convex.obj `if test -f 'convex.cpp'; then $(CYGPATH_W) 'convex.cpp'; else $(CYGPATH_W) '$(srcdir)/convex.cpp'; fi` + +libode_a-joint.o: joint.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-joint.o -MD -MP -MF $(DEPDIR)/libode_a-joint.Tpo -c -o libode_a-joint.o `test -f 'joint.cpp' || echo '$(srcdir)/'`joint.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-joint.Tpo $(DEPDIR)/libode_a-joint.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='joint.cpp' object='libode_a-joint.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-joint.o `test -f 'joint.cpp' || echo '$(srcdir)/'`joint.cpp + +libode_a-joint.obj: joint.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-joint.obj -MD -MP -MF $(DEPDIR)/libode_a-joint.Tpo -c -o libode_a-joint.obj `if test -f 'joint.cpp'; then $(CYGPATH_W) 'joint.cpp'; else $(CYGPATH_W) '$(srcdir)/joint.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-joint.Tpo $(DEPDIR)/libode_a-joint.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='joint.cpp' object='libode_a-joint.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-joint.obj `if test -f 'joint.cpp'; then $(CYGPATH_W) 'joint.cpp'; else $(CYGPATH_W) '$(srcdir)/joint.cpp'; fi` + +libode_a-step.o: step.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-step.o -MD -MP -MF $(DEPDIR)/libode_a-step.Tpo -c -o libode_a-step.o `test -f 'step.cpp' || echo '$(srcdir)/'`step.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-step.Tpo $(DEPDIR)/libode_a-step.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='step.cpp' object='libode_a-step.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-step.o `test -f 'step.cpp' || echo '$(srcdir)/'`step.cpp + +libode_a-step.obj: step.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-step.obj -MD -MP -MF $(DEPDIR)/libode_a-step.Tpo -c -o libode_a-step.obj `if test -f 'step.cpp'; then $(CYGPATH_W) 'step.cpp'; else $(CYGPATH_W) '$(srcdir)/step.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-step.Tpo $(DEPDIR)/libode_a-step.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='step.cpp' object='libode_a-step.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-step.obj `if test -f 'step.cpp'; then $(CYGPATH_W) 'step.cpp'; else $(CYGPATH_W) '$(srcdir)/step.cpp'; fi` + +libode_a-collision_transform.o: collision_transform.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_transform.o -MD -MP -MF $(DEPDIR)/libode_a-collision_transform.Tpo -c -o libode_a-collision_transform.o `test -f 'collision_transform.cpp' || echo '$(srcdir)/'`collision_transform.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_transform.Tpo $(DEPDIR)/libode_a-collision_transform.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_transform.cpp' object='libode_a-collision_transform.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_transform.o `test -f 'collision_transform.cpp' || echo '$(srcdir)/'`collision_transform.cpp + +libode_a-collision_transform.obj: collision_transform.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_transform.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_transform.Tpo -c -o libode_a-collision_transform.obj `if test -f 'collision_transform.cpp'; then $(CYGPATH_W) 'collision_transform.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_transform.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_transform.Tpo $(DEPDIR)/libode_a-collision_transform.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_transform.cpp' object='libode_a-collision_transform.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_transform.obj `if test -f 'collision_transform.cpp'; then $(CYGPATH_W) 'collision_transform.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_transform.cpp'; fi` + +libode_a-lcp.o: lcp.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-lcp.o -MD -MP -MF $(DEPDIR)/libode_a-lcp.Tpo -c -o libode_a-lcp.o `test -f 'lcp.cpp' || echo '$(srcdir)/'`lcp.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-lcp.Tpo $(DEPDIR)/libode_a-lcp.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='lcp.cpp' object='libode_a-lcp.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-lcp.o `test -f 'lcp.cpp' || echo '$(srcdir)/'`lcp.cpp + +libode_a-lcp.obj: lcp.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-lcp.obj -MD -MP -MF $(DEPDIR)/libode_a-lcp.Tpo -c -o libode_a-lcp.obj `if test -f 'lcp.cpp'; then $(CYGPATH_W) 'lcp.cpp'; else $(CYGPATH_W) '$(srcdir)/lcp.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-lcp.Tpo $(DEPDIR)/libode_a-lcp.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='lcp.cpp' object='libode_a-lcp.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-lcp.obj `if test -f 'lcp.cpp'; then $(CYGPATH_W) 'lcp.cpp'; else $(CYGPATH_W) '$(srcdir)/lcp.cpp'; fi` + +libode_a-stepfast.o: stepfast.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-stepfast.o -MD -MP -MF $(DEPDIR)/libode_a-stepfast.Tpo -c -o libode_a-stepfast.o `test -f 'stepfast.cpp' || echo '$(srcdir)/'`stepfast.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-stepfast.Tpo $(DEPDIR)/libode_a-stepfast.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='stepfast.cpp' object='libode_a-stepfast.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-stepfast.o `test -f 'stepfast.cpp' || echo '$(srcdir)/'`stepfast.cpp + +libode_a-stepfast.obj: stepfast.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-stepfast.obj -MD -MP -MF $(DEPDIR)/libode_a-stepfast.Tpo -c -o libode_a-stepfast.obj `if test -f 'stepfast.cpp'; then $(CYGPATH_W) 'stepfast.cpp'; else $(CYGPATH_W) '$(srcdir)/stepfast.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-stepfast.Tpo $(DEPDIR)/libode_a-stepfast.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='stepfast.cpp' object='libode_a-stepfast.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-stepfast.obj `if test -f 'stepfast.cpp'; then $(CYGPATH_W) 'stepfast.cpp'; else $(CYGPATH_W) '$(srcdir)/stepfast.cpp'; fi` + +libode_a-mass.o: mass.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-mass.o -MD -MP -MF $(DEPDIR)/libode_a-mass.Tpo -c -o libode_a-mass.o `test -f 'mass.cpp' || echo '$(srcdir)/'`mass.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mass.Tpo $(DEPDIR)/libode_a-mass.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mass.cpp' object='libode_a-mass.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-mass.o `test -f 'mass.cpp' || echo '$(srcdir)/'`mass.cpp + +libode_a-mass.obj: mass.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-mass.obj -MD -MP -MF $(DEPDIR)/libode_a-mass.Tpo -c -o libode_a-mass.obj `if test -f 'mass.cpp'; then $(CYGPATH_W) 'mass.cpp'; else $(CYGPATH_W) '$(srcdir)/mass.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mass.Tpo $(DEPDIR)/libode_a-mass.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mass.cpp' object='libode_a-mass.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-mass.obj `if test -f 'mass.cpp'; then $(CYGPATH_W) 'mass.cpp'; else $(CYGPATH_W) '$(srcdir)/mass.cpp'; fi` + +libode_a-testing.o: testing.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-testing.o -MD -MP -MF $(DEPDIR)/libode_a-testing.Tpo -c -o libode_a-testing.o `test -f 'testing.cpp' || echo '$(srcdir)/'`testing.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-testing.Tpo $(DEPDIR)/libode_a-testing.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='testing.cpp' object='libode_a-testing.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-testing.o `test -f 'testing.cpp' || echo '$(srcdir)/'`testing.cpp + +libode_a-testing.obj: testing.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-testing.obj -MD -MP -MF $(DEPDIR)/libode_a-testing.Tpo -c -o libode_a-testing.obj `if test -f 'testing.cpp'; then $(CYGPATH_W) 'testing.cpp'; else $(CYGPATH_W) '$(srcdir)/testing.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-testing.Tpo $(DEPDIR)/libode_a-testing.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='testing.cpp' object='libode_a-testing.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-testing.obj `if test -f 'testing.cpp'; then $(CYGPATH_W) 'testing.cpp'; else $(CYGPATH_W) '$(srcdir)/testing.cpp'; fi` + +libode_a-mat.o: mat.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-mat.o -MD -MP -MF $(DEPDIR)/libode_a-mat.Tpo -c -o libode_a-mat.o `test -f 'mat.cpp' || echo '$(srcdir)/'`mat.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mat.Tpo $(DEPDIR)/libode_a-mat.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mat.cpp' object='libode_a-mat.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-mat.o `test -f 'mat.cpp' || echo '$(srcdir)/'`mat.cpp + +libode_a-mat.obj: mat.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-mat.obj -MD -MP -MF $(DEPDIR)/libode_a-mat.Tpo -c -o libode_a-mat.obj `if test -f 'mat.cpp'; then $(CYGPATH_W) 'mat.cpp'; else $(CYGPATH_W) '$(srcdir)/mat.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mat.Tpo $(DEPDIR)/libode_a-mat.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mat.cpp' object='libode_a-mat.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-mat.obj `if test -f 'mat.cpp'; then $(CYGPATH_W) 'mat.cpp'; else $(CYGPATH_W) '$(srcdir)/mat.cpp'; fi` + +libode_a-timer.o: timer.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-timer.o -MD -MP -MF $(DEPDIR)/libode_a-timer.Tpo -c -o libode_a-timer.o `test -f 'timer.cpp' || echo '$(srcdir)/'`timer.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-timer.Tpo $(DEPDIR)/libode_a-timer.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='timer.cpp' object='libode_a-timer.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-timer.o `test -f 'timer.cpp' || echo '$(srcdir)/'`timer.cpp + +libode_a-timer.obj: timer.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-timer.obj -MD -MP -MF $(DEPDIR)/libode_a-timer.Tpo -c -o libode_a-timer.obj `if test -f 'timer.cpp'; then $(CYGPATH_W) 'timer.cpp'; else $(CYGPATH_W) '$(srcdir)/timer.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-timer.Tpo $(DEPDIR)/libode_a-timer.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='timer.cpp' object='libode_a-timer.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-timer.obj `if test -f 'timer.cpp'; then $(CYGPATH_W) 'timer.cpp'; else $(CYGPATH_W) '$(srcdir)/timer.cpp'; fi` + +libode_a-matrix.o: matrix.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-matrix.o -MD -MP -MF $(DEPDIR)/libode_a-matrix.Tpo -c -o libode_a-matrix.o `test -f 'matrix.cpp' || echo '$(srcdir)/'`matrix.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-matrix.Tpo $(DEPDIR)/libode_a-matrix.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='matrix.cpp' object='libode_a-matrix.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-matrix.o `test -f 'matrix.cpp' || echo '$(srcdir)/'`matrix.cpp + +libode_a-matrix.obj: matrix.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-matrix.obj -MD -MP -MF $(DEPDIR)/libode_a-matrix.Tpo -c -o libode_a-matrix.obj `if test -f 'matrix.cpp'; then $(CYGPATH_W) 'matrix.cpp'; else $(CYGPATH_W) '$(srcdir)/matrix.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-matrix.Tpo $(DEPDIR)/libode_a-matrix.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='matrix.cpp' object='libode_a-matrix.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-matrix.obj `if test -f 'matrix.cpp'; then $(CYGPATH_W) 'matrix.cpp'; else $(CYGPATH_W) '$(srcdir)/matrix.cpp'; fi` + +libode_a-util.o: util.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-util.o -MD -MP -MF $(DEPDIR)/libode_a-util.Tpo -c -o libode_a-util.o `test -f 'util.cpp' || echo '$(srcdir)/'`util.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-util.Tpo $(DEPDIR)/libode_a-util.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='util.cpp' object='libode_a-util.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-util.o `test -f 'util.cpp' || echo '$(srcdir)/'`util.cpp + +libode_a-util.obj: util.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-util.obj -MD -MP -MF $(DEPDIR)/libode_a-util.Tpo -c -o libode_a-util.obj `if test -f 'util.cpp'; then $(CYGPATH_W) 'util.cpp'; else $(CYGPATH_W) '$(srcdir)/util.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-util.Tpo $(DEPDIR)/libode_a-util.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='util.cpp' object='libode_a-util.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-util.obj `if test -f 'util.cpp'; then $(CYGPATH_W) 'util.cpp'; else $(CYGPATH_W) '$(srcdir)/util.cpp'; fi` + +libode_a-memory.o: memory.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-memory.o -MD -MP -MF $(DEPDIR)/libode_a-memory.Tpo -c -o libode_a-memory.o `test -f 'memory.cpp' || echo '$(srcdir)/'`memory.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-memory.Tpo $(DEPDIR)/libode_a-memory.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='memory.cpp' object='libode_a-memory.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-memory.o `test -f 'memory.cpp' || echo '$(srcdir)/'`memory.cpp + +libode_a-memory.obj: memory.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-memory.obj -MD -MP -MF $(DEPDIR)/libode_a-memory.Tpo -c -o libode_a-memory.obj `if test -f 'memory.cpp'; then $(CYGPATH_W) 'memory.cpp'; else $(CYGPATH_W) '$(srcdir)/memory.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-memory.Tpo $(DEPDIR)/libode_a-memory.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='memory.cpp' object='libode_a-memory.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-memory.obj `if test -f 'memory.cpp'; then $(CYGPATH_W) 'memory.cpp'; else $(CYGPATH_W) '$(srcdir)/memory.cpp'; fi` + +libode_a-misc.o: misc.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-misc.o -MD -MP -MF $(DEPDIR)/libode_a-misc.Tpo -c -o libode_a-misc.o `test -f 'misc.cpp' || echo '$(srcdir)/'`misc.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-misc.Tpo $(DEPDIR)/libode_a-misc.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='misc.cpp' object='libode_a-misc.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-misc.o `test -f 'misc.cpp' || echo '$(srcdir)/'`misc.cpp + +libode_a-misc.obj: misc.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-misc.obj -MD -MP -MF $(DEPDIR)/libode_a-misc.Tpo -c -o libode_a-misc.obj `if test -f 'misc.cpp'; then $(CYGPATH_W) 'misc.cpp'; else $(CYGPATH_W) '$(srcdir)/misc.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-misc.Tpo $(DEPDIR)/libode_a-misc.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='misc.cpp' object='libode_a-misc.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-misc.obj `if test -f 'misc.cpp'; then $(CYGPATH_W) 'misc.cpp'; else $(CYGPATH_W) '$(srcdir)/misc.cpp'; fi` + +libode_a-heightfield.o: heightfield.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-heightfield.o -MD -MP -MF $(DEPDIR)/libode_a-heightfield.Tpo -c -o libode_a-heightfield.o `test -f 'heightfield.cpp' || echo '$(srcdir)/'`heightfield.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-heightfield.Tpo $(DEPDIR)/libode_a-heightfield.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='heightfield.cpp' object='libode_a-heightfield.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-heightfield.o `test -f 'heightfield.cpp' || echo '$(srcdir)/'`heightfield.cpp + +libode_a-heightfield.obj: heightfield.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-heightfield.obj -MD -MP -MF $(DEPDIR)/libode_a-heightfield.Tpo -c -o libode_a-heightfield.obj `if test -f 'heightfield.cpp'; then $(CYGPATH_W) 'heightfield.cpp'; else $(CYGPATH_W) '$(srcdir)/heightfield.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-heightfield.Tpo $(DEPDIR)/libode_a-heightfield.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='heightfield.cpp' object='libode_a-heightfield.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-heightfield.obj `if test -f 'heightfield.cpp'; then $(CYGPATH_W) 'heightfield.cpp'; else $(CYGPATH_W) '$(srcdir)/heightfield.cpp'; fi` + +libode_a-collision_trimesh_gimpact.o: collision_trimesh_gimpact.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_gimpact.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_gimpact.Tpo -c -o libode_a-collision_trimesh_gimpact.o `test -f 'collision_trimesh_gimpact.cpp' || echo '$(srcdir)/'`collision_trimesh_gimpact.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_gimpact.Tpo $(DEPDIR)/libode_a-collision_trimesh_gimpact.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_gimpact.cpp' object='libode_a-collision_trimesh_gimpact.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_gimpact.o `test -f 'collision_trimesh_gimpact.cpp' || echo '$(srcdir)/'`collision_trimesh_gimpact.cpp + +libode_a-collision_trimesh_gimpact.obj: collision_trimesh_gimpact.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_gimpact.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_gimpact.Tpo -c -o libode_a-collision_trimesh_gimpact.obj `if test -f 'collision_trimesh_gimpact.cpp'; then $(CYGPATH_W) 'collision_trimesh_gimpact.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_gimpact.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_gimpact.Tpo $(DEPDIR)/libode_a-collision_trimesh_gimpact.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_gimpact.cpp' object='libode_a-collision_trimesh_gimpact.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_gimpact.obj `if test -f 'collision_trimesh_gimpact.cpp'; then $(CYGPATH_W) 'collision_trimesh_gimpact.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_gimpact.cpp'; fi` + +libode_a-collision_trimesh_trimesh.o: collision_trimesh_trimesh.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_trimesh.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_trimesh.Tpo -c -o libode_a-collision_trimesh_trimesh.o `test -f 'collision_trimesh_trimesh.cpp' || echo '$(srcdir)/'`collision_trimesh_trimesh.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_trimesh.Tpo $(DEPDIR)/libode_a-collision_trimesh_trimesh.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_trimesh.cpp' object='libode_a-collision_trimesh_trimesh.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_trimesh.o `test -f 'collision_trimesh_trimesh.cpp' || echo '$(srcdir)/'`collision_trimesh_trimesh.cpp + +libode_a-collision_trimesh_trimesh.obj: collision_trimesh_trimesh.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_trimesh.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_trimesh.Tpo -c -o libode_a-collision_trimesh_trimesh.obj `if test -f 'collision_trimesh_trimesh.cpp'; then $(CYGPATH_W) 'collision_trimesh_trimesh.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_trimesh.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_trimesh.Tpo $(DEPDIR)/libode_a-collision_trimesh_trimesh.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_trimesh.cpp' object='libode_a-collision_trimesh_trimesh.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_trimesh.obj `if test -f 'collision_trimesh_trimesh.cpp'; then $(CYGPATH_W) 'collision_trimesh_trimesh.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_trimesh.cpp'; fi` + +libode_a-collision_trimesh_sphere.o: collision_trimesh_sphere.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_sphere.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_sphere.Tpo -c -o libode_a-collision_trimesh_sphere.o `test -f 'collision_trimesh_sphere.cpp' || echo '$(srcdir)/'`collision_trimesh_sphere.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_sphere.Tpo $(DEPDIR)/libode_a-collision_trimesh_sphere.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_sphere.cpp' object='libode_a-collision_trimesh_sphere.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_sphere.o `test -f 'collision_trimesh_sphere.cpp' || echo '$(srcdir)/'`collision_trimesh_sphere.cpp + +libode_a-collision_trimesh_sphere.obj: collision_trimesh_sphere.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_sphere.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_sphere.Tpo -c -o libode_a-collision_trimesh_sphere.obj `if test -f 'collision_trimesh_sphere.cpp'; then $(CYGPATH_W) 'collision_trimesh_sphere.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_sphere.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_sphere.Tpo $(DEPDIR)/libode_a-collision_trimesh_sphere.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_sphere.cpp' object='libode_a-collision_trimesh_sphere.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_sphere.obj `if test -f 'collision_trimesh_sphere.cpp'; then $(CYGPATH_W) 'collision_trimesh_sphere.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_sphere.cpp'; fi` + +libode_a-collision_trimesh_ray.o: collision_trimesh_ray.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_ray.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_ray.Tpo -c -o libode_a-collision_trimesh_ray.o `test -f 'collision_trimesh_ray.cpp' || echo '$(srcdir)/'`collision_trimesh_ray.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ray.Tpo $(DEPDIR)/libode_a-collision_trimesh_ray.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ray.cpp' object='libode_a-collision_trimesh_ray.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_ray.o `test -f 'collision_trimesh_ray.cpp' || echo '$(srcdir)/'`collision_trimesh_ray.cpp + +libode_a-collision_trimesh_ray.obj: collision_trimesh_ray.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_ray.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_ray.Tpo -c -o libode_a-collision_trimesh_ray.obj `if test -f 'collision_trimesh_ray.cpp'; then $(CYGPATH_W) 'collision_trimesh_ray.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_ray.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ray.Tpo $(DEPDIR)/libode_a-collision_trimesh_ray.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ray.cpp' object='libode_a-collision_trimesh_ray.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_ray.obj `if test -f 'collision_trimesh_ray.cpp'; then $(CYGPATH_W) 'collision_trimesh_ray.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_ray.cpp'; fi` + +libode_a-collision_trimesh_opcode.o: collision_trimesh_opcode.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_opcode.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_opcode.Tpo -c -o libode_a-collision_trimesh_opcode.o `test -f 'collision_trimesh_opcode.cpp' || echo '$(srcdir)/'`collision_trimesh_opcode.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_opcode.Tpo $(DEPDIR)/libode_a-collision_trimesh_opcode.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_opcode.cpp' object='libode_a-collision_trimesh_opcode.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_opcode.o `test -f 'collision_trimesh_opcode.cpp' || echo '$(srcdir)/'`collision_trimesh_opcode.cpp + +libode_a-collision_trimesh_opcode.obj: collision_trimesh_opcode.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_opcode.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_opcode.Tpo -c -o libode_a-collision_trimesh_opcode.obj `if test -f 'collision_trimesh_opcode.cpp'; then $(CYGPATH_W) 'collision_trimesh_opcode.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_opcode.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_opcode.Tpo $(DEPDIR)/libode_a-collision_trimesh_opcode.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_opcode.cpp' object='libode_a-collision_trimesh_opcode.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_opcode.obj `if test -f 'collision_trimesh_opcode.cpp'; then $(CYGPATH_W) 'collision_trimesh_opcode.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_opcode.cpp'; fi` + +libode_a-collision_trimesh_box.o: collision_trimesh_box.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_box.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_box.Tpo -c -o libode_a-collision_trimesh_box.o `test -f 'collision_trimesh_box.cpp' || echo '$(srcdir)/'`collision_trimesh_box.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_box.Tpo $(DEPDIR)/libode_a-collision_trimesh_box.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_box.cpp' object='libode_a-collision_trimesh_box.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_box.o `test -f 'collision_trimesh_box.cpp' || echo '$(srcdir)/'`collision_trimesh_box.cpp + +libode_a-collision_trimesh_box.obj: collision_trimesh_box.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_box.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_box.Tpo -c -o libode_a-collision_trimesh_box.obj `if test -f 'collision_trimesh_box.cpp'; then $(CYGPATH_W) 'collision_trimesh_box.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_box.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_box.Tpo $(DEPDIR)/libode_a-collision_trimesh_box.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_box.cpp' object='libode_a-collision_trimesh_box.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_box.obj `if test -f 'collision_trimesh_box.cpp'; then $(CYGPATH_W) 'collision_trimesh_box.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_box.cpp'; fi` + +libode_a-collision_trimesh_ccylinder.o: collision_trimesh_ccylinder.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_ccylinder.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Tpo -c -o libode_a-collision_trimesh_ccylinder.o `test -f 'collision_trimesh_ccylinder.cpp' || echo '$(srcdir)/'`collision_trimesh_ccylinder.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Tpo $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ccylinder.cpp' object='libode_a-collision_trimesh_ccylinder.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_ccylinder.o `test -f 'collision_trimesh_ccylinder.cpp' || echo '$(srcdir)/'`collision_trimesh_ccylinder.cpp + +libode_a-collision_trimesh_ccylinder.obj: collision_trimesh_ccylinder.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_ccylinder.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Tpo -c -o libode_a-collision_trimesh_ccylinder.obj `if test -f 'collision_trimesh_ccylinder.cpp'; then $(CYGPATH_W) 'collision_trimesh_ccylinder.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_ccylinder.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Tpo $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ccylinder.cpp' object='libode_a-collision_trimesh_ccylinder.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_ccylinder.obj `if test -f 'collision_trimesh_ccylinder.cpp'; then $(CYGPATH_W) 'collision_trimesh_ccylinder.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_ccylinder.cpp'; fi` + +libode_a-collision_trimesh_distance.o: collision_trimesh_distance.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_distance.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_distance.Tpo -c -o libode_a-collision_trimesh_distance.o `test -f 'collision_trimesh_distance.cpp' || echo '$(srcdir)/'`collision_trimesh_distance.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_distance.Tpo $(DEPDIR)/libode_a-collision_trimesh_distance.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_distance.cpp' object='libode_a-collision_trimesh_distance.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_distance.o `test -f 'collision_trimesh_distance.cpp' || echo '$(srcdir)/'`collision_trimesh_distance.cpp + +libode_a-collision_trimesh_distance.obj: collision_trimesh_distance.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_distance.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_distance.Tpo -c -o libode_a-collision_trimesh_distance.obj `if test -f 'collision_trimesh_distance.cpp'; then $(CYGPATH_W) 'collision_trimesh_distance.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_distance.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_distance.Tpo $(DEPDIR)/libode_a-collision_trimesh_distance.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_distance.cpp' object='libode_a-collision_trimesh_distance.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_distance.obj `if test -f 'collision_trimesh_distance.cpp'; then $(CYGPATH_W) 'collision_trimesh_distance.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_distance.cpp'; fi` + +libode_a-collision_cylinder_trimesh.o: collision_cylinder_trimesh.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_trimesh.o -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_trimesh.Tpo -c -o libode_a-collision_cylinder_trimesh.o `test -f 'collision_cylinder_trimesh.cpp' || echo '$(srcdir)/'`collision_cylinder_trimesh.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_trimesh.Tpo $(DEPDIR)/libode_a-collision_cylinder_trimesh.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_trimesh.cpp' object='libode_a-collision_cylinder_trimesh.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_trimesh.o `test -f 'collision_cylinder_trimesh.cpp' || echo '$(srcdir)/'`collision_cylinder_trimesh.cpp + +libode_a-collision_cylinder_trimesh.obj: collision_cylinder_trimesh.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_cylinder_trimesh.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_cylinder_trimesh.Tpo -c -o libode_a-collision_cylinder_trimesh.obj `if test -f 'collision_cylinder_trimesh.cpp'; then $(CYGPATH_W) 'collision_cylinder_trimesh.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_trimesh.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_trimesh.Tpo $(DEPDIR)/libode_a-collision_cylinder_trimesh.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_trimesh.cpp' object='libode_a-collision_cylinder_trimesh.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_cylinder_trimesh.obj `if test -f 'collision_cylinder_trimesh.cpp'; then $(CYGPATH_W) 'collision_cylinder_trimesh.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_cylinder_trimesh.cpp'; fi` + +libode_a-collision_trimesh_plane.o: collision_trimesh_plane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_plane.o -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_plane.Tpo -c -o libode_a-collision_trimesh_plane.o `test -f 'collision_trimesh_plane.cpp' || echo '$(srcdir)/'`collision_trimesh_plane.cpp +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_plane.Tpo $(DEPDIR)/libode_a-collision_trimesh_plane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_plane.cpp' object='libode_a-collision_trimesh_plane.o' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_plane.o `test -f 'collision_trimesh_plane.cpp' || echo '$(srcdir)/'`collision_trimesh_plane.cpp + +libode_a-collision_trimesh_plane.obj: collision_trimesh_plane.cpp +@am__fastdepCXX_TRUE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT libode_a-collision_trimesh_plane.obj -MD -MP -MF $(DEPDIR)/libode_a-collision_trimesh_plane.Tpo -c -o libode_a-collision_trimesh_plane.obj `if test -f 'collision_trimesh_plane.cpp'; then $(CYGPATH_W) 'collision_trimesh_plane.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_plane.cpp'; fi` +@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_plane.Tpo $(DEPDIR)/libode_a-collision_trimesh_plane.Po +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_plane.cpp' object='libode_a-collision_trimesh_plane.obj' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCXX_FALSE@ $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(libode_a_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o libode_a-collision_trimesh_plane.obj `if test -f 'collision_trimesh_plane.cpp'; then $(CYGPATH_W) 'collision_trimesh_plane.cpp'; else $(CYGPATH_W) '$(srcdir)/collision_trimesh_plane.cpp'; fi` + +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 $(LIBRARIES) $(PROGRAMS) +installdirs: + for dir in "$(DESTDIR)$(libdir)" "$(DESTDIR)$(traplibdir)"; do \ + test -z "$$dir" || $(MKDIR_P) "$$dir"; \ + done +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-libLIBRARIES clean-noinstLIBRARIES \ + clean-traplibPROGRAMS 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-traplibPROGRAMS + +install-dvi: install-dvi-am + +install-exec-am: install-libLIBRARIES + +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: uninstall-libLIBRARIES uninstall-traplibPROGRAMS + +.MAKE: install-am install-strip + +.PHONY: CTAGS GTAGS all all-am check check-am clean clean-generic \ + clean-libLIBRARIES clean-noinstLIBRARIES clean-traplibPROGRAMS \ + 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-libLIBRARIES install-man install-pdf install-pdf-am \ + install-ps install-ps-am install-strip install-traplibPROGRAMS \ + installcheck installcheck-am installdirs maintainer-clean \ + maintainer-clean-generic mostlyclean mostlyclean-compile \ + mostlyclean-generic pdf pdf-am ps ps-am tags uninstall \ + uninstall-am uninstall-libLIBRARIES uninstall-traplibPROGRAMS + +# 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/src/array.cpp b/libraries/ode-0.9/ode/src/array.cpp new file mode 100644 index 0000000..cbb1a6e --- /dev/null +++ b/libraries/ode-0.9/ode/src/array.cpp @@ -0,0 +1,80 @@ +/************************************************************************* + * * + * 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 "array.h" + + +static inline int roundUpToPowerOfTwo (int x) +{ + int i = 1; + while (i < x) i <<= 1; + return i; +} + + +void dArrayBase::_freeAll (int sizeofT) +{ + if (_data) { + if (_data == this+1) return; // if constructLocalArray() was called + dFree (_data,_anum * sizeofT); + } +} + + +void dArrayBase::_setSize (int newsize, int sizeofT) +{ + if (newsize < 0) return; + if (newsize > _anum) { + if (_data == this+1) { + // this is a no-no, because constructLocalArray() was called + dDebug (0,"setSize() out of space in LOCAL array"); + } + int newanum = roundUpToPowerOfTwo (newsize); + if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT); + else _data = dAlloc (newanum*sizeofT); + _anum = newanum; + } + _size = newsize; +} + + +void * dArrayBase::operator new (size_t size) +{ + return dAlloc (size); +} + + +void dArrayBase::operator delete (void *ptr, size_t size) +{ + dFree (ptr,size); +} + + +void dArrayBase::constructLocalArray (int __anum) +{ + _size = 0; + _anum = __anum; + _data = this+1; +} diff --git a/libraries/ode-0.9/ode/src/array.h b/libraries/ode-0.9/ode/src/array.h new file mode 100644 index 0000000..307206c --- /dev/null +++ b/libraries/ode-0.9/ode/src/array.h @@ -0,0 +1,135 @@ +/************************************************************************* + * * + * 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 comes from the `reuse' library. copy any changes back to the source. + * + * Variable sized array template. The array is always stored in a contiguous + * chunk. The array can be resized. A size increase will cause more memory + * to be allocated, and may result in relocation of the array memory. + * A size decrease has no effect on the memory allocation. + * + * Array elements with constructors or destructors are not supported! + * But if you must have such elements, here's what to know/do: + * - Bitwise copy is used when copying whole arrays. + * - When copying individual items (via push(), insert() etc) the `=' + * (equals) operator is used. Thus you should define this operator to do + * a bitwise copy. You should probably also define the copy constructor. + */ + + +#ifndef _ODE_ARRAY_H_ +#define _ODE_ARRAY_H_ + +#include + + +// this base class has no constructors or destructor, for your convenience. + +class dArrayBase { +protected: + int _size; // number of elements in `data' + int _anum; // allocated number of elements in `data' + void *_data; // array data + + void _freeAll (int sizeofT); + void _setSize (int newsize, int sizeofT); + // set the array size to `newsize', allocating more memory if necessary. + // if newsize>_anum and is a power of two then this is guaranteed to + // set _size and _anum to newsize. + +public: + // not: dArrayBase () { _size=0; _anum=0; _data=0; } + + int size() const { return _size; } + int allocatedSize() const { return _anum; } + void * operator new (size_t size); + void operator delete (void *ptr, size_t size); + + void constructor() { _size=0; _anum=0; _data=0; } + // if this structure is allocated with malloc() instead of new, you can + // call this to set it up. + + void constructLocalArray (int __anum); + // this helper function allows non-reallocating arrays to be constructed + // on the stack (or in the heap if necessary). this is something of a + // kludge and should be used with extreme care. this function acts like + // a constructor - it is called on uninitialized memory that will hold the + // Array structure and the data. __anum is the number of elements that + // are allocated. the memory MUST be allocated with size: + // sizeof(ArrayBase) + __anum*sizeof(T) + // arrays allocated this way will never try to reallocate or free the + // memory - that's your job. +}; + + +template class dArray : public dArrayBase { +public: + void equals (const dArray &x) { + setSize (x.size()); + memcpy (_data,x._data,x._size * sizeof(T)); + } + + dArray () { constructor(); } + dArray (const dArray &x) { constructor(); equals (x); } + ~dArray () { _freeAll(sizeof(T)); } + void setSize (int newsize) { _setSize (newsize,sizeof(T)); } + T *data() const { return (T*) _data; } + T & operator[] (int i) const { return ((T*)_data)[i]; } + void operator = (const dArray &x) { equals (x); } + + void push (const T item) { + if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); + memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T)); + } + + void swap (dArray &x) { + int tmp1; + void *tmp2; + tmp1=_size; _size=x._size; x._size=tmp1; + tmp1=_anum; _anum=x._anum; x._anum=tmp1; + tmp2=_data; _data=x._data; x._data=tmp2; + } + + // insert the item at the position `i'. if i<0 then add the item to the + // start, if i >= size then add the item to the end of the array. + void insert (int i, const T item) { + if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); + if (i >= (_size-1)) i = _size-1; // add to end + else { + if (i < 0) i=0; // add to start + int n = _size-1-i; + if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T)); + } + ((T*)_data)[i] = item; + } + + void remove (int i) { + if (i >= 0 && i < _size) { // passing this test guarantees size>0 + int n = _size-1-i; + if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T)); + _size--; + } + } +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/box.cpp b/libraries/ode-0.9/ode/src/box.cpp new file mode 100644 index 0000000..f328651 --- /dev/null +++ b/libraries/ode-0.9/ode/src/box.cpp @@ -0,0 +1,847 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// box public API + +dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1) +{ + dAASSERT (lx >= 0 && ly >= 0 && lz >= 0); + type = dBoxClass; + side[0] = lx; + side[1] = ly; + side[2] = lz; +} + + +void dxBox::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) + + dFabs (R[1] * side[1]) + dFabs (R[2] * side[2])); + dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) + + dFabs (R[5] * side[1]) + dFabs (R[6] * side[2])); + dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) + + dFabs (R[9] * side[1]) + dFabs (R[10] * side[2])); + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz) +{ + return new dxBox (space,lx,ly,lz); +} + + +void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dAASSERT (lx > 0 && ly > 0 && lz > 0); + dxBox *b = (dxBox*) g; + b->side[0] = lx; + b->side[1] = ly; + b->side[2] = lz; + dGeomMoved (g); +} + + +void dGeomBoxGetLengths (dGeomID g, dVector3 result) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dxBox *b = (dxBox*) g; + result[0] = b->side[0]; + result[1] = b->side[1]; + result[2] = b->side[2]; +} + + +dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + g->recomputePosr(); + dxBox *b = (dxBox*) g; + + // Set p = (x,y,z) relative to box center + // + // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2) + + dVector3 p,q; + + p[0] = x - b->final_posr->pos[0]; + p[1] = y - b->final_posr->pos[1]; + p[2] = z - b->final_posr->pos[2]; + + // Rotate p into box's coordinate frame, so we can + // treat the OBB as an AABB + + dMULTIPLY1_331 (q,b->final_posr->R,p); + + // Record distance from point to each successive box side, and see + // if the point is inside all six sides + + dReal dist[6]; + int i; + + bool inside = true; + + for (i=0; i < 3; i++) { + dReal side = b->side[i] * REAL(0.5); + + dist[i ] = side - q[i]; + dist[i+3] = side + q[i]; + + if ((dist[i] < 0) || (dist[i+3] < 0)) { + inside = false; + } + } + + // If point is inside the box, the depth is the smallest positive distance + // to any side + + if (inside) { + dReal smallest_dist = (dReal) (unsigned) -1; + + for (i=0; i < 6; i++) { + if (dist[i] < smallest_dist) smallest_dist = dist[i]; + } + + return smallest_dist; + } + + // Otherwise, if point is outside the box, the depth is the largest + // distance to any side. This is an approximation to the 'proper' + // solution (the proper solution may be larger in some cases). + + dReal largest_dist = 0; + + for (i=0; i < 6; i++) { + if (dist[i] > largest_dist) largest_dist = dist[i]; + } + + return -largest_dist; +} + +//**************************************************************************** +// box-box collision utility + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr; + dReal buffer[16]; + dReal *q = p; + dReal *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + dReal *pq = q; + dReal *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + dReal *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } + done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal)); + return nr; +} + + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints (int n, dReal p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + dReal a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = REAL(0.5)*(p[0] + p[2]); + cy = REAL(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + a = dRecip(REAL(3.0)*(a+q)); + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + dReal A[8]; + for (i=0; i M_PI) a -= 2*M_PI; + dReal maxdiff=1e9,diff; +#ifndef dNODEBUG + *iret = i0; // iret is not allowed to keep this value +#endif + for (i=0; i M_PI) diff = 2*M_PI - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#ifndef dNODEBUG + dIASSERT (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. + + +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 *return_code, + int flags, dContactGeom *contact, int skip) +{ + const dReal fudge_factor = REAL(1.05); + dVector3 p,pp,normalC; + const dReal *normalR = 0; + dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*REAL(0.5); + A[1] = side1[1]*REAL(0.5); + A[2] = side1[2]*REAL(0.5); + B[0] = side2[0]*REAL(0.5); + B[1] = side2[1]*REAL(0.5); + B[2] = side2[2]*REAL(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + + Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); + Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); + Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + + do { +#define TST(expr1,expr2,norm,cc) \ + expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ + s2 = dFabs(expr1_val) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1_val) < 0); \ + code = (cc); \ + if (flags & CONTACTS_UNIMPORTANT) break; \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ + s2 = dFabs(expr1_val) - (expr2); \ + if (s2 > 0) return 0; \ + l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1_val) < 0); \ + code = (cc); \ + if (flags & CONTACTS_UNIMPORTANT) break; \ + } \ + } + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); +#undef TST + } while (0); + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + dVector3 pa; + dReal sign; + for (i=0; i<3; i++) pa[i] = p1[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + dVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + dReal alpha,beta; + dVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + *return_code = code; + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + dVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331 (nr,Rb,normal2); + anr[0] = dFabs (nr[0]); + anr[1] = dFabs (nr[1]); + anr[2] = dFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + dVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + dReal quad[8]; // 2D coordinate of incident face (x,y pairs) + dReal c1,c2,m11,m12,m21,m22; + c1 = dDOT14 (center,Ra+code1); + c2 = dDOT14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44 (Ra+code1,Rb+a1); + m12 = dDOT44 (Ra+code1,Rb+a2); + m21 = dDOT44 (Ra+code2,Rb+a1); + m22 = dDOT44 (Ra+code2,Rb+a2); + { + dReal k1 = m11*Sb[a1]; + dReal k2 = m21*Sb[a1]; + dReal k3 = m12*Sb[a2]; + dReal k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + dReal rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + dReal ret[16]; + int n = intersectRectQuad (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + dReal point[3*8]; // penetrating contact points + dReal dep[8]; // depths for those points + dReal det1 = dRecip(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + if (cnum < 1) { + return 0; // this should not happen, yet does at times (demo_plane2d single precision). + } + + // we can't generate more contacts than we actually have + int maxc = flags & NUMC_MASK; + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function + + if (cnum <= maxc) { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) { + dContactGeom *con = CONTACT(contact,skip*j); + for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; + con->depth = dep[j]; + } + } + else { + dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + dReal maxdepth = dep[0]; + for (i=1; i maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { + dContactGeom *con = CONTACT(contact,skip*j); + for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + con->depth = dep[iret[j]]; + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + + + +int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dBoxClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dVector3 normal; + dReal depth; + int code; + dxBox *b1 = (dxBox*) o1; + dxBox *b2 = (dxBox*) o2; + int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side, + normal,&depth,&code,flags,contact,skip); + for (int i=0; inormal[0] = -normal[0]; + CONTACT(contact,i*skip)->normal[1] = -normal[1]; + CONTACT(contact,i*skip)->normal[2] = -normal[2]; + CONTACT(contact,i*skip)->g1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + return num; +} + + +int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dBoxClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxBox *box = (dxBox*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + int ret = 0; + + //@@@ problem: using 4-vector (plane->p) as 3-vector (normal). + const dReal *R = o1->final_posr->R; // rotation of box + const dReal *n = plane->p; // normal vector + + // project sides lengths along normal vector, get absolute values + dReal Q1 = dDOT14(n,R+0); + dReal Q2 = dDOT14(n,R+1); + dReal Q3 = dDOT14(n,R+2); + dReal A1 = box->side[0] * Q1; + dReal A2 = box->side[1] * Q2; + dReal A3 = box->side[2] * Q3; + dReal B1 = dFabs(A1); + dReal B2 = dFabs(A2); + dReal B3 = dFabs(A3); + + // early exit test + dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->final_posr->pos); + if (depth < 0) return 0; + + // find number of contacts requested + int maxc = flags & NUMC_MASK; + // if (maxc < 1) maxc = 1; // an assertion is made on entry + if (maxc > 3) maxc = 3; // not more than 3 contacts per box allowed + + // find deepest point + dVector3 p; + p[0] = o1->final_posr->pos[0]; + p[1] = o1->final_posr->pos[1]; + p[2] = o1->final_posr->pos[2]; +#define FOO(i,op) \ + p[0] op REAL(0.5)*box->side[i] * R[0+i]; \ + p[1] op REAL(0.5)*box->side[i] * R[4+i]; \ + p[2] op REAL(0.5)*box->side[i] * R[8+i]; +#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) } + BAR(0,1); + BAR(1,2); + BAR(2,3); +#undef FOO +#undef BAR + + // the deepest point is the first contact point + contact->pos[0] = p[0]; + contact->pos[1] = p[1]; + contact->pos[2] = p[2]; + contact->normal[0] = n[0]; + contact->normal[1] = n[1]; + contact->normal[2] = n[2]; + contact->depth = depth; + ret = 1; // ret is number of contact points found so far + if (maxc == 1) goto done; + + // get the second and third contact points by starting from `p' and going + // along the two sides with the smallest projected length. + +#define FOO(i,j,op) \ + CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \ + CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \ + CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j]; +#define BAR(ctact,side,sideinc) \ + depth -= B ## sideinc; \ + if (depth < 0) goto done; \ + if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \ + CONTACT(contact,ctact*skip)->depth = depth; \ + ret++; + + CONTACT(contact,skip)->normal[0] = n[0]; + CONTACT(contact,skip)->normal[1] = n[1]; + CONTACT(contact,skip)->normal[2] = n[2]; + if (maxc == 3) { + CONTACT(contact,2*skip)->normal[0] = n[0]; + CONTACT(contact,2*skip)->normal[1] = n[1]; + CONTACT(contact,2*skip)->normal[2] = n[2]; + } + + if (B1 < B2) { + if (B3 < B1) goto use_side_3; else { + BAR(1,0,1); // use side 1 + if (maxc == 2) goto done; + if (B2 < B3) goto contact2_2; else goto contact2_3; + } + } + else { + if (B3 < B2) { + use_side_3: // use side 3 + BAR(1,2,3); + if (maxc == 2) goto done; + if (B1 < B2) goto contact2_1; else goto contact2_2; + } + else { + BAR(1,1,2); // use side 2 + if (maxc == 2) goto done; + if (B1 < B3) goto contact2_1; else goto contact2_3; + } + } + + contact2_1: BAR(2,0,1); goto done; + contact2_2: BAR(2,1,2); goto done; + contact2_3: BAR(2,2,3); goto done; +#undef FOO +#undef BAR + + done: + for (int i=0; ig1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + return ret; +} diff --git a/libraries/ode-0.9/ode/src/capsule.cpp b/libraries/ode-0.9/ode/src/capsule.cpp new file mode 100644 index 0000000..5680baa --- /dev/null +++ b/libraries/ode-0.9/ode/src/capsule.cpp @@ -0,0 +1,369 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// capped cylinder public API + +dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) : + dxGeom (space,1) +{ + dAASSERT (_radius > 0 && _length > 0); + type = dCapsuleClass; + radius = _radius; + lz = _length; +} + + +void dxCapsule::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = dFabs(R[2] * lz) * REAL(0.5) + radius; + dReal yrange = dFabs(R[6] * lz) * REAL(0.5) + radius; + dReal zrange = dFabs(R[10] * lz) * REAL(0.5) + radius; + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length) +{ + return new dxCapsule (space,radius,length); +} + + +void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) +{ + dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); + dAASSERT (radius > 0 && length > 0); + dxCapsule *c = (dxCapsule*) g; + c->radius = radius; + c->lz = length; + dGeomMoved (g); +} + + +void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length) +{ + dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); + dxCapsule *c = (dxCapsule*) g; + *radius = c->radius; + *length = c->lz; +} + + +dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); + g->recomputePosr(); + dxCapsule *c = (dxCapsule*) g; + + const dReal* R = g->final_posr->R; + const dReal* pos = g->final_posr->pos; + + dVector3 a; + a[0] = x - pos[0]; + a[1] = y - pos[1]; + a[2] = z - pos[2]; + dReal beta = dDOT14(a,R+2); + dReal lz2 = c->lz*REAL(0.5); + if (beta < -lz2) beta = -lz2; + else if (beta > lz2) beta = lz2; + a[0] = c->final_posr->pos[0] + beta*R[0*4+2]; + a[1] = c->final_posr->pos[1] + beta*R[1*4+2]; + a[2] = c->final_posr->pos[2] + beta*R[2*4+2]; + return c->radius - + dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2])); +} + + + +int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxCapsule *ccyl = (dxCapsule*) o1; + dxSphere *sphere = (dxSphere*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // find the point on the cylinder axis that is closest to the sphere + dReal alpha = + o1->final_posr->R[2] * (o2->final_posr->pos[0] - o1->final_posr->pos[0]) + + o1->final_posr->R[6] * (o2->final_posr->pos[1] - o1->final_posr->pos[1]) + + o1->final_posr->R[10] * (o2->final_posr->pos[2] - o1->final_posr->pos[2]); + dReal lz2 = ccyl->lz * REAL(0.5); + if (alpha > lz2) alpha = lz2; + if (alpha < -lz2) alpha = -lz2; + + // collide the spheres + dVector3 p; + p[0] = o1->final_posr->pos[0] + alpha * o1->final_posr->R[2]; + p[1] = o1->final_posr->pos[1] + alpha * o1->final_posr->R[6]; + p[2] = o1->final_posr->pos[2] + alpha * o1->final_posr->R[10]; + return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact); +} + + +int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxCapsule *cyl = (dxCapsule*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // get p1,p2 = cylinder axis endpoints, get radius + dVector3 p1,p2; + dReal clen = cyl->lz * REAL(0.5); + p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2]; + p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6]; + p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10]; + p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2]; + p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6]; + p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10]; + dReal radius = cyl->radius; + + // copy out box center, rotation matrix, and side array + dReal *c = o2->final_posr->pos; + dReal *R = o2->final_posr->R; + const dReal *side = box->side; + + // get the closest point between the cylinder axis and the box + dVector3 pl,pb; + dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb); + + // generate contact point + return dCollideSpheres (pl,radius,pb,0,contact); +} + + +int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int i; + const dReal tolerance = REAL(1e-5); + + dxCapsule *cyl1 = (dxCapsule*) o1; + dxCapsule *cyl2 = (dxCapsule*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // copy out some variables, for convenience + dReal lz1 = cyl1->lz * REAL(0.5); + dReal lz2 = cyl2->lz * REAL(0.5); + dReal *pos1 = o1->final_posr->pos; + dReal *pos2 = o2->final_posr->pos; + dReal axis1[3],axis2[3]; + axis1[0] = o1->final_posr->R[2]; + axis1[1] = o1->final_posr->R[6]; + axis1[2] = o1->final_posr->R[10]; + axis2[0] = o2->final_posr->R[2]; + axis2[1] = o2->final_posr->R[6]; + axis2[2] = o2->final_posr->R[10]; + + // if the cylinder axes are close to parallel, we'll try to detect up to + // two contact points along the body of the cylinder. if we can't find any + // points then we'll fall back to the closest-points algorithm. note that + // we are not treating this special case for reasons of degeneracy, but + // because we want two contact points in some situations. the closet-points + // algorithm is robust in all casts, but it can return only one contact. + + dVector3 sphere1,sphere2; + dReal a1a2 = dDOT (axis1,axis2); + dReal det = REAL(1.0)-a1a2*a1a2; + if (det < tolerance) { + // the cylinder axes (almost) parallel, so we will generate up to two + // contacts. alpha1 and alpha2 (line position parameters) are related by: + // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) + // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) + // first compute where the two cylinders overlap in alpha1 space: + if (a1a2 < 0) { + axis2[0] = -axis2[0]; + axis2[1] = -axis2[1]; + axis2[2] = -axis2[2]; + } + dReal q[3]; + for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; + dReal k = dDOT (axis1,q); + dReal a1lo = -lz1; + dReal a1hi = lz1; + dReal a2lo = -lz2 - k; + dReal a2hi = lz2 - k; + dReal lo = (a1lo > a2lo) ? a1lo : a2lo; + dReal hi = (a1hi < a2hi) ? a1hi : a2hi; + if (lo <= hi) { + int num_contacts = flags & NUMC_MASK; + if (num_contacts >= 2 && lo < hi) { + // generate up to two contacts. if one of those contacts is + // not made, fall back on the one-contact strategy. + for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; + int n1 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + if (n1) { + for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; + dContactGeom *c2 = CONTACT(contact,skip); + int n2 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius, c2); + if (n2) { + c2->g1 = o1; + c2->g2 = o2; + return 2; + } + } + } + + // just one contact to generate, so put it in the middle of + // the range + dReal alpha1 = (lo + hi) * REAL(0.5); + dReal alpha2 = alpha1 + k; + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + return dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + } + } + + // use the closest point algorithm + dVector3 a1,a2,b1,b2; + a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1; + a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1; + a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1; + a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1; + a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1; + a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1; + b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2; + b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2; + b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2; + b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2; + b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2; + b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2; + + dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2); + return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); +} + + +int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCapsuleClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxCapsule *ccyl = (dxCapsule*) o1; + dxPlane *plane = (dxPlane*) o2; + + // collide the deepest capping sphere with the plane + dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0); + dVector3 p; + p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign; + p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign; + p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign; + + dReal k = dDOT (p,plane->p); + dReal depth = plane->p[3] - k + ccyl->radius; + if (depth < 0) return 0; + contact->normal[0] = plane->p[0]; + contact->normal[1] = plane->p[1]; + contact->normal[2] = plane->p[2]; + contact->pos[0] = p[0] - plane->p[0] * ccyl->radius; + contact->pos[1] = p[1] - plane->p[1] * ccyl->radius; + contact->pos[2] = p[2] - plane->p[2] * ccyl->radius; + contact->depth = depth; + + int ncontacts = 1; + if ((flags & NUMC_MASK) >= 2) { + // collide the other capping sphere with the plane + p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign; + p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign; + p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign; + + k = dDOT (p,plane->p); + depth = plane->p[3] - k + ccyl->radius; + if (depth >= 0) { + dContactGeom *c2 = CONTACT(contact,skip); + c2->normal[0] = plane->p[0]; + c2->normal[1] = plane->p[1]; + c2->normal[2] = plane->p[2]; + c2->pos[0] = p[0] - plane->p[0] * ccyl->radius; + c2->pos[1] = p[1] - plane->p[1] * ccyl->radius; + c2->pos[2] = p[2] - plane->p[2] * ccyl->radius; + c2->depth = depth; + ncontacts = 2; + } + } + + for (int i=0; i < ncontacts; i++) { + CONTACT(contact,i*skip)->g1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + return ncontacts; +} + diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp new file mode 100644 index 0000000..eb14c72 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp @@ -0,0 +1,1007 @@ +/************************************************************************* +* * +* 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. * +* * +*************************************************************************/ + +/* + * Cylinder-box collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +#include +#include +#include +#include +#include "collision_util.h" + +static const int MAX_CYLBOX_CLIP_POINTS = 16; +static const int nCYLINDER_AXIS = 2; +// Number of segment of cylinder base circle. +// Must be divisible by 4. +static const int nCYLINDER_SEGMENT = 8; + +#define MAX_FLOAT dInfinity + +// Data that passed through the collider's functions +typedef struct _sCylinderBoxData +{ + // cylinder parameters + dMatrix3 mCylinderRot; + dVector3 vCylinderPos; + dVector3 vCylinderAxis; + dReal fCylinderRadius; + dReal fCylinderSize; + dVector3 avCylinderNormals[nCYLINDER_SEGMENT]; + + // box parameters + + dMatrix3 mBoxRot; + dVector3 vBoxPos; + dVector3 vBoxHalfSize; + // box vertices array : 8 vertices + dVector3 avBoxVertices[8]; + + // global collider data + dVector3 vDiff; + dVector3 vNormal; + dReal fBestDepth; + dReal fBestrb; + dReal fBestrc; + int iBestAxis; + + // contact data + dVector3 vEp0, vEp1; + dReal fDepth0, fDepth1; + + // ODE stuff + dGeomID gBox; + dGeomID gCylinder; + dContactGeom* gContact; + int iFlags; + int iSkip; + int nContacts; + +} sCylinderBoxData; + + +// initialize collision data +void _cldInitCylinderBox(sCylinderBoxData& cData) +{ + // get cylinder position, orientation + const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); + dMatrix3Copy(pRotCyc,cData.mCylinderRot); + + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); + dVector3Copy(*pPosCyc,cData.vCylinderPos); + + dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); + + // get cylinder radius and size + dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); + + // get box position, orientation, size + const dReal* pRotBox = dGeomGetRotation(cData.gBox); + dMatrix3Copy(pRotBox,cData.mBoxRot); + const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox); + dVector3Copy(*pPosBox,cData.vBoxPos); + + dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize); + cData.vBoxHalfSize[0] *= REAL(0.5); + cData.vBoxHalfSize[1] *= REAL(0.5); + cData.vBoxHalfSize[2] *= REAL(0.5); + + // vertex 0 + cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2]; + + // vertex 1 + cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2]; + + // vertex 2 + cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2]; + + // vertex 3 + cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2]; + + // vertex 4 + cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2]; + + // vertex 5 + cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2]; + + // vertex 6 + cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2]; + + // vertex 7 + cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2]; + + // temp index + int i = 0; + dVector3 vTempBoxVertices[8]; + // transform vertices in absolute space + for(i=0; i < 8; i++) + { + dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]); + dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]); + } + + // find relative position + dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff); + cData.fBestDepth = MAX_FLOAT; + cData.vNormal[0] = REAL(0.0); + cData.vNormal[1] = REAL(0.0); + cData.vNormal[2] = REAL(0.0); + + // calculate basic angle for nCYLINDER_SEGMENT-gon + dReal fAngle = M_PI/nCYLINDER_SEGMENT; + + // calculate angle increment + dReal fAngleIncrement = fAngle * REAL(2.0); + + // calculate nCYLINDER_SEGMENT-gon points + for(i = 0; i < nCYLINDER_SEGMENT; i++) + { + cData.avCylinderNormals[i][0] = -dCos(fAngle); + cData.avCylinderNormals[i][1] = -dSin(fAngle); + cData.avCylinderNormals[i][2] = 0; + + fAngle += fAngleIncrement; + } + + cData.fBestrb = 0; + cData.fBestrc = 0; + cData.iBestAxis = 0; + cData.nContacts = 0; + +} + +// test for given separating axis +int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis ) +{ + // check length of input normal + dReal fL = dVector3Length(vInputNormal); + // if not long enough + if ( fL < REAL(1e-5) ) + { + // do nothing + return 1; + } + + // otherwise make it unit for sure + dNormalize3(vInputNormal); + + // project box and Cylinder on mAxis + dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal); + + dReal frc; + + if (fdot1 > REAL(1.0)) + { + fdot1 = REAL(1.0); + frc = dFabs(cData.fCylinderSize*REAL(0.5)); + } + + // project box and capsule on iAxis + frc = dFabs( fdot1 * (cData.fCylinderSize*REAL(0.5))) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + + dVector3 vTemp1; + dReal frb = REAL(0.0); + + dMat3GetCol(cData.mBoxRot,0,vTemp1); + frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0]; + + dMat3GetCol(cData.mBoxRot,1,vTemp1); + frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1]; + + dMat3GetCol(cData.mBoxRot,2,vTemp1); + frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2]; + + // project their distance on separating axis + dReal fd = dVector3Dot(cData.vDiff,vInputNormal); + + // if they do not overlap exit, we have no intersection + if ( dFabs(fd) > frc+frb ) + { + return 0; + } + + // get depth + dReal fDepth = - dFabs(fd) + (frc+frb); + + // get maximum depth + if ( fDepth < cData.fBestDepth ) + { + cData.fBestDepth = fDepth; + dVector3Copy(vInputNormal,cData.vNormal); + cData.iBestAxis = iAxis; + cData.fBestrb = frb; + cData.fBestrc = frc; + + // flip normal if interval is wrong faced + if (fd > 0) + { + dVector3Inv(cData.vNormal); + } + } + + return 1; +} + + +// check for separation between box edge and cylinder circle edge +int _cldTestEdgeCircleAxis( sCylinderBoxData& cData, + const dVector3 &vCenterPoint, + const dVector3 &vVx0, const dVector3 &vVx1, + int iAxis ) +{ + // calculate direction of edge + dVector3 vDirEdge; + dVector3Subtract(vVx1,vVx0,vDirEdge); + dNormalize3(vDirEdge); + // starting point of edge + dVector3 vEStart; + dVector3Copy(vVx0,vEStart);; + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2) < REAL(1e-5)) + { + // this can't be separating axis, because edge is parallel to circle plane + return 1; + } + + // find point of intersection between edge line and circle plane + dVector3 vTemp1; + dVector3Subtract(vCenterPoint,vEStart,vTemp1); + dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis); + dVector3 vpnt; + vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2); + vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2); + vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2); + + // find tangent vector on circle with same center (vCenterPoint) that + // touches point of intersection (vpnt) + dVector3 vTangent; + dVector3Subtract(vCenterPoint,vpnt,vTemp1); + dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent); + + // find vector orthogonal both to tangent and edge direction + dVector3 vAxis; + dVector3Cross(vTangent,vDirEdge,vAxis); + + // use that vector as separating axis + return _cldTestAxis( cData, vAxis, iAxis ); +} + +// Test separating axis for collision +int _cldTestSeparatingAxes(sCylinderBoxData& cData) +{ + // reset best axis + cData.fBestDepth = MAX_FLOAT; + cData.iBestAxis = 0; + cData.fBestrb = 0; + cData.fBestrc = 0; + cData.nContacts = 0; + + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = REAL(1e-6); + + // axis A0 + dMat3GetCol(cData.mBoxRot, 0 , vAxis); + if (!_cldTestAxis( cData, vAxis, 1 )) + { + return 0; + } + + // axis A1 + dMat3GetCol(cData.mBoxRot, 1 , vAxis); + if (!_cldTestAxis( cData, vAxis, 2 )) + { + return 0; + } + + // axis A2 + dMat3GetCol(cData.mBoxRot, 2 , vAxis); + if (!_cldTestAxis( cData, vAxis, 3 )) + { + return 0; + } + + // axis C - Cylinder Axis + //vAxis = vCylinderAxis; + dVector3Copy(cData.vCylinderAxis , vAxis); + if (!_cldTestAxis( cData, vAxis, 4 )) + { + return 0; + } + + // axis CxA0 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 )); + dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 5 )) + { + return 0; + } + } + + // axis CxA1 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 )); + dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 6 )) + { + return 0; + } + } + + // axis CxA2 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 )); + dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 7 )) + { + return 0; + } + } + + int i = 0; + dVector3 vTemp1; + dVector3 vTemp2; + // here we check box's vertices axis + for(i=0; i< 8; i++) + { + //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos)); + dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1); + dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2); + //vAxis = ( vCylinderAxis cross vAxis ); + dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 8 + i )) + { + return 0; + } + } + } + + // ************************************ + // this is defined for first 12 axes + // normal of plane that contains top circle of cylinder + // center of top circle of cylinder + dVector3 vcc; + vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + // ************************************ + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) + { + return 0; + } + + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) + { + return 0; + } + + // ************************************ + // this is defined for second 12 axes + // normal of plane that contains bottom circle of cylinder + // center of bottom circle of cylinder + // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5)); + vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + // ************************************ + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39)) + { + return 0; + } + + return 1; +} + +int _cldClipCylinderToBox(sCylinderBoxData& cData) +{ + dIASSERT(cData.nContacts != (cData.iFlags & NUMC_MASK)); + + // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal + dVector3 vN; + dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal); + vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1; + vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1; + vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1; + + // normalize that vector + dNormalize3(vN); + + // translate cylinder end points by the vector + dVector3 vCposTrans; + vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius; + vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius; + vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius; + + cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + // transform edge points in box space + cData.vEp0[0] -= cData.vBoxPos[0]; + cData.vEp0[1] -= cData.vBoxPos[1]; + cData.vEp0[2] -= cData.vBoxPos[2]; + + cData.vEp1[0] -= cData.vBoxPos[0]; + cData.vEp1[1] -= cData.vBoxPos[1]; + cData.vEp1[2] -= cData.vBoxPos[2]; + + dVector3 vTemp1; + // clip the edge to box + dVector4 plPlane; + // plane 0 +x + dMat3GetCol(cData.mBoxRot,0,vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 1 +y + dMat3GetCol(cData.mBoxRot,1,vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 2 +z + dMat3GetCol(cData.mBoxRot,2,vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 3 -x + dMat3GetCol(cData.mBoxRot,0,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 4 -y + dMat3GetCol(cData.mBoxRot,1,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 5 -z + dMat3GetCol(cData.mBoxRot,2,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // calculate depths for both contact points + cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal); + cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal); + + // clamp depths to 0 + if(cData.fDepth0<0) + { + cData.fDepth0 = REAL(0.0); + } + + if(cData.fDepth1<0) + { + cData.fDepth1 = REAL(0.0); + } + + // back transform edge points from box to absolute space + cData.vEp0[0] += cData.vBoxPos[0]; + cData.vEp0[1] += cData.vBoxPos[1]; + cData.vEp0[2] += cData.vBoxPos[2]; + + cData.vEp1[0] += cData.vBoxPos[0]; + cData.vEp1[1] += cData.vBoxPos[1]; + cData.vEp1[2] += cData.vBoxPos[2]; + + dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact0->depth = cData.fDepth0; + dVector3Copy(cData.vNormal,Contact0->normal); + dVector3Copy(cData.vEp0,Contact0->pos); + Contact0->g1 = cData.gCylinder; + Contact0->g2 = cData.gBox; + dVector3Inv(Contact0->normal); + cData.nContacts++; + + if (cData.nContacts != (cData.iFlags & NUMC_MASK)) + { + dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact1->depth = cData.fDepth1; + dVector3Copy(cData.vNormal,Contact1->normal); + dVector3Copy(cData.vEp1,Contact1->pos); + Contact1->g1 = cData.gCylinder; + Contact1->g2 = cData.gBox; + dVector3Inv(Contact1->normal); + cData.nContacts++; + } + + return 1; +} + + +void _cldClipBoxToCylinder(sCylinderBoxData& cData ) +{ + dIASSERT(cData.nContacts != (cData.iFlags & NUMC_MASK)); + + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + // check which circle from cylinder we take for clipping + if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) ) + { + // get top circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[0] = REAL(0.0); + vCylinderCircleNormal_Rel[1] = REAL(0.0); + vCylinderCircleNormal_Rel[2] = REAL(0.0); + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[0] = REAL(0.0); + vCylinderCircleNormal_Rel[1] = REAL(0.0); + vCylinderCircleNormal_Rel[2] = REAL(0.0); + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + // vNr is normal in Box frame, pointing from Cylinder to Box + dVector3 vNr; + dMatrix3 mBoxInv; + + // Find a way to use quaternion + dMatrix3Inv(cData.mBoxRot,mBoxInv); + dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr); + + dVector3 vAbsNormal; + + vAbsNormal[0] = dFabs( vNr[0] ); + vAbsNormal[1] = dFabs( vNr[1] ); + vAbsNormal[2] = dFabs( vNr[2] ); + + // find which face in box is closest to cylinder + int iB0, iB1, iB2; + + // Different from Croteam's code + if (vAbsNormal[1] > vAbsNormal[0]) + { + // 1 > 0 + if (vAbsNormal[0]> vAbsNormal[2]) + { + // 0 > 2 -> 1 > 0 >2 + iB0 = 1; iB1 = 0; iB2 = 2; + } + else + { + // 2 > 0-> Must compare 1 and 2 + if (vAbsNormal[1] > vAbsNormal[2]) + { + // 1 > 2 -> 1 > 2 > 0 + iB0 = 1; iB1 = 2; iB2 = 0; + } + else + { + // 2 > 1 -> 2 > 1 > 0; + iB0 = 2; iB1 = 1; iB2 = 0; + } + } + } + else + { + // 0 > 1 + if (vAbsNormal[1] > vAbsNormal[2]) + { + // 1 > 2 -> 0 > 1 > 2 + iB0 = 0; iB1 = 1; iB2 = 2; + } + else + { + // 2 > 1 -> Must compare 0 and 2 + if (vAbsNormal[0] > vAbsNormal[2]) + { + // 0 > 2 -> 0 > 2 > 1; + iB0 = 0; iB1 = 2; iB2 = 1; + } + else + { + // 2 > 0 -> 2 > 0 > 1; + iB0 = 2; iB1 = 0; iB2 = 1; + } + } + } + + dVector3 vCenter; + // find center of box polygon + dVector3 vTemp; + if (vNr[iB0] > 0) + { + dMat3GetCol(cData.mBoxRot,iB0,vTemp); + vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0]; + vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1]; + vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2]; + } + else + { + dMat3GetCol(cData.mBoxRot,iB0,vTemp); + vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0]; + vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1]; + vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2]; + } + + // find the vertices of box polygon + dVector3 avPoints[4]; + dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; + dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; + + int i=0; + for(i=0; i= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment % 2) + { + for( i=0; i REAL(0.0)) + { + // generate contacts + dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact0->depth = fTempDepth; + dVector3Copy(cData.vNormal,Contact0->normal); + dVector3Copy(vPoint,Contact0->pos); + Contact0->g1 = cData.gCylinder; + Contact0->g2 = cData.gBox; + dVector3Inv(Contact0->normal); + cData.nContacts++; + + if (cData.nContacts == (cData.iFlags & NUMC_MASK)) + { + break; + } + } + } + } + else + { + for( i=0; i REAL(0.0)) + { + // generate contacts + dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact0->depth = fTempDepth; + dVector3Copy(cData.vNormal,Contact0->normal); + dVector3Copy(vPoint,Contact0->pos); + Contact0->g1 = cData.gCylinder; + Contact0->g2 = cData.gBox; + dVector3Inv(Contact0->normal); + cData.nContacts++; + + if (cData.nContacts == (cData.iFlags & NUMC_MASK)) + { + break; + } + } + } + } +} + + +// Cylinder - Box by CroTeam +// Ported by Nguyen Binh +int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCylinderClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + sCylinderBoxData cData; + + // Assign ODE stuff + cData.gCylinder = o1; + cData.gBox = o2; + cData.iFlags = flags; + cData.iSkip = skip; + cData.gContact = contact; + + // initialize collider + _cldInitCylinderBox( cData ); + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes( cData ) ) + { + // if not found do nothing + return 0; + } + + // if best separation axis is not found + if ( cData.iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(0); + // do nothing + return 0; + } + + dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis); + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + // clip cylinder over box + if(!_cldClipCylinderToBox(cData)) + { + return 0; + } + } + else + { + _cldClipBoxToCylinder(cData); + } + + return cData.nContacts; +} + diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp new file mode 100644 index 0000000..f85701d --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp @@ -0,0 +1,254 @@ +/************************************************************************* +* * +* 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. * +* * +*************************************************************************/ + + +/* + * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de ) + * + * This testing basically comes down to testing the intersection + * of the cylinder caps (discs) with the plane. + * + */ + +#include +#include +#include +#include +#include + +#include "collision_kernel.h" // for dxGeom +#include "collision_util.h" + + +int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (Cylinder->type == dCylinderClass); + dIASSERT (Plane->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int GeomCount = 0; // count of used contactgeoms + +#ifdef dSINGLE + const dReal toleranz = REAL(0.0001); +#endif +#ifdef dDOUBLE + const dReal toleranz = REAL(0.0000001); +#endif + + // Get the properties of the cylinder (length+radius) + dReal radius, length; + dGeomCylinderGetParams(Cylinder, &radius, &length); + dVector3 &cylpos = Cylinder->final_posr->pos; + // and the plane + dVector4 planevec; + dGeomPlaneGetParams(Plane, planevec); + dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; + dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; + + dVector3 G1Pos1, G1Pos2, vDir1; + vDir1[0] = Cylinder->final_posr->R[2]; + vDir1[1] = Cylinder->final_posr->R[6]; + vDir1[2] = Cylinder->final_posr->R[10]; + + dReal s; + s = length * REAL(0.5); + G1Pos2[0] = vDir1[0] * s + cylpos[0]; + G1Pos2[1] = vDir1[1] * s + cylpos[1]; + G1Pos2[2] = vDir1[2] * s + cylpos[2]; + + G1Pos1[0] = vDir1[0] * -s + cylpos[0]; + G1Pos1[1] = vDir1[1] * -s + cylpos[1]; + G1Pos1[2] = vDir1[2] * -s + cylpos[2]; + + dVector3 C; + + // parallel-check + s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2]; + if(s < 0) + s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel + else + s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel + if(s < toleranz && s > (-toleranz)) + { + // discs are parallel to the plane + + // 1.compute if, and where contacts are + dVector3 P; + s = planevec[3] - dVector3Dot(planevec, G1Pos1); + dReal t; + t = planevec[3] - dVector3Dot(planevec, G1Pos2); + if(s >= t) // s == t does never happen, + { + if(s >= 0) + { + // 1. Disc + dVector3Copy(G1Pos1, P); + } + else + return GeomCount; // no contacts + } + else + { + if(t >= 0) + { + // 2. Disc + dVector3Copy(G1Pos2, P); + } + else + return GeomCount; // no contacts + } + + // 2. generate a coordinate-system on the disc + dVector3 V1, V2; + if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) + { + // not x-axis + V1[0] = vDir1[0] + REAL(1.0); // random value + V1[1] = vDir1[1]; + V1[2] = vDir1[2]; + } + else + { + // maybe x-axis + V1[0] = vDir1[0]; + V1[1] = vDir1[1] + REAL(1.0); // random value + V1[2] = vDir1[2]; + } + // V1 is now another direction than vDir1 + // Cross-product + dVector3Cross(V1, vDir1, V2); + // make unit V2 + t = dVector3Length(V2); + t = radius / t; + dVector3Scale(V2, t); + // cross again + dVector3Cross(V2, vDir1, V1); + // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' + // V1 = first axis + // V2 = second axis + + // 3. generate contactpoints + + // Potential contact 1 + dVector3Add(P, V1, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // Potential contact 2 + dVector3Subtract(P, V1, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // Potential contact 3 + dVector3Add(P, V2, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // Potential contact 4 + dVector3Subtract(P, V2, contact->pos); + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth > 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + } + else + { + dReal t = dVector3Dot(PlaneNormal, vDir1); + C[0] = vDir1[0] * t - PlaneNormal[0]; + C[1] = vDir1[1] * t - PlaneNormal[1]; + C[2] = vDir1[2] * t - PlaneNormal[2]; + s = dVector3Length(C); + // move C onto the circle + s = radius / s; + dVector3Scale(C, s); + + // deepest point of disc 1 + dVector3Add(C, G1Pos1, contact->pos); + + // depth of the deepest point + contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); + if(contact->depth >= 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + + // C is still computed + + // deepest point of disc 2 + dVector3Add(C, G1Pos2, contact->pos); + + // depth of the deepest point + contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; + if(contact->depth >= 0) + { + dVector3Copy(PlaneNormal, contact->normal); + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & NUMC_MASK)) + return GeomCount; // enough contactgeoms + contact = (dContactGeom *)((char *)contact + skip); + } + } + return GeomCount; +} diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp new file mode 100644 index 0000000..964c531 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp @@ -0,0 +1,264 @@ +/************************************************************************* +* * +* 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. * +* * +*************************************************************************/ + + +/******************************************************************* + * * + * cylinder-sphere collider by Christoph Beyer (boernerb@web.de) * + * * + * In Cylinder/Sphere-collisions, there are three possibilies: * + * 1. collision with the cylinder's nappe * + * 2. collision with one of the cylinder's disc * + * 3. collision with one of the disc's border * + * * + * This collider computes two distances (s, t) and based on them, * + * it decides, which collision we have. * + * This collider always generates 1 (or 0, if we have no collison) * + * contacts. * + * It is able to "separate" cylinder and sphere in all * + * configurations, but it never pays attention to velocity. * + * So, in extrem situations, "tunneling-effect" is possible. * + * * + *******************************************************************/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" // for dxGeom +#include "collision_util.h" + +int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (Cylinder->type == dCylinderClass); + dIASSERT (Sphere->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + unsigned char* pContactData = (unsigned char*)contact; + int GeomCount = 0; // count of used contacts + +#ifdef dSINGLE + const dReal toleranz = REAL(0.0001); +#endif +#ifdef dDOUBLE + const dReal toleranz = REAL(0.0000001); +#endif + + // get the data from the geoms + dReal radius, length; + dGeomCylinderGetParams(Cylinder, &radius, &length); + dVector3 &cylpos = Cylinder->final_posr->pos; + const dReal* pfRot1 = dGeomGetRotation(Cylinder); + + dReal radius2; + radius2 = dGeomSphereGetRadius(Sphere); + const dReal* SpherePos = dGeomGetPosition(Sphere); + + // G1Pos1 is the middle of the first disc + // G1Pos2 is the middle of the second disc + // vDir1 is the unit direction of the cylinderaxis + dVector3 G1Pos1, G1Pos2, vDir1; + vDir1[0] = Cylinder->final_posr->R[2]; + vDir1[1] = Cylinder->final_posr->R[6]; + vDir1[2] = Cylinder->final_posr->R[10]; + + dReal s; + s = length * REAL(0.5); // just a precomputed factor + G1Pos2[0] = vDir1[0] * s + cylpos[0]; + G1Pos2[1] = vDir1[1] * s + cylpos[1]; + G1Pos2[2] = vDir1[2] * s + cylpos[2]; + + G1Pos1[0] = vDir1[0] * -s + cylpos[0]; + G1Pos1[1] = vDir1[1] * -s + cylpos[1]; + G1Pos1[2] = vDir1[2] * -s + cylpos[2]; + + dVector3 C; + dReal t; + // Step 1: compute the two distances 's' and 't' + // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle + s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2]; + if(s < (-radius2) || s > (length + radius2) ) + { + // Sphere is too far away from the discs + // no collision + return 0; + } + + // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis + C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0]; + C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1]; + C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2]; + // t is the distance from the Sphere-middle to the cylinder-axis! + t = dVector3Length(C); + if(t > (radius + radius2) ) + { + // Sphere is too far away from the cylinder axis! + // no collision + return 0; + } + + // decide which kind of collision we have: + if(t > radius && (s < 0 || s > length) ) + { + // 3. collision + if(s <= 0) + { + contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) ); + if(contact->depth < 0) + { + // no collision! + return 0; + } + contact->pos[0] = C[0] / t * -radius + G1Pos1[0]; + contact->pos[1] = C[1] / t * -radius + G1Pos1[1]; + contact->pos[2] = C[2] / t * -radius + G1Pos1[2]; + contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth); + contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth); + contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth); + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + else + { + // now s is bigger than length here! + contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) ); + if(contact->depth < 0) + { + // no collision! + return 0; + } + contact->pos[0] = C[0] / t * -radius + G1Pos2[0]; + contact->pos[1] = C[1] / t * -radius + G1Pos2[1]; + contact->pos[2] = C[2] / t * -radius + G1Pos2[2]; + contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth); + contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth); + contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth); + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + else if( (radius - t) <= s && (radius - t) <= (length - s) ) + { + // 1. collsision + if(t > (radius2 + toleranz)) + { + // cylinder-axis is outside the sphere + contact->depth = (radius2 + radius) - t; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + else + { + C[0] /= t; + C[1] /= t; + C[2] /= t; + contact->pos[0] = C[0] * radius2 + SpherePos[0]; + contact->pos[1] = C[1] * radius2 + SpherePos[1]; + contact->pos[2] = C[2] * radius2 + SpherePos[2]; + contact->normal[0] = C[0]; + contact->normal[1] = C[1]; + contact->normal[2] = C[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + else + { + // cylinder-axis is outside of the sphere + contact->depth = (radius2 + radius) - t; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + else + { + contact->pos[0] = C[0] + SpherePos[0]; + contact->pos[1] = C[1] + SpherePos[1]; + contact->pos[2] = C[2] + SpherePos[2]; + contact->normal[0] = C[0] / t; + contact->normal[1] = C[1] / t; + contact->normal[2] = C[2] / t; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + } + else + { + // 2. collision + if(s <= (length * REAL(0.5)) ) + { + // collsision with the first disc + contact->depth = s + radius2; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + contact->pos[0] = radius2 * vDir1[0] + SpherePos[0]; + contact->pos[1] = radius2 * vDir1[1] + SpherePos[1]; + contact->pos[2] = radius2 * vDir1[2] + SpherePos[2]; + contact->normal[0] = vDir1[0]; + contact->normal[1] = vDir1[1]; + contact->normal[2] = vDir1[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + else + { + // collsision with the second disc + contact->depth = (radius2 + length - s); + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0]; + contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1]; + contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2]; + contact->normal[0] = -vDir1[0]; + contact->normal[1] = -vDir1[1]; + contact->normal[2] = -vDir1[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + return GeomCount; +} diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp new file mode 100644 index 0000000..342be04 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp @@ -0,0 +1,1145 @@ +/************************************************************************* +* * +* 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. * +* * +*************************************************************************/ + +/* + * Cylinder-trimesh collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + + +#include +#include +#include +#include +#include "collision_util.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#define MAX_REAL dInfinity +static const int nCYLINDER_AXIS = 2; +static const int nCYLINDER_CIRCLE_SEGMENTS = 8; +static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12; + +#define OPTIMIZE_CONTACTS 1 + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +typedef struct _sCylinderTrimeshColliderData +{ + // cylinder data + dMatrix3 mCylinderRot; + dQuaternion qCylinderRot; + dQuaternion qInvCylinderRot; + dVector3 vCylinderPos; + dVector3 vCylinderAxis; + dReal fCylinderRadius; + dReal fCylinderSize; + dVector3 avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; + + // mesh data + dQuaternion qTrimeshRot; + dQuaternion qInvTrimeshRot; + dMatrix3 mTrimeshRot; + dVector3 vTrimeshPos; + + // global collider data + dVector3 vBestPoint; + dReal fBestDepth; + dReal fBestCenter; + dReal fBestrt; + int iBestAxis; + dVector3 vContactNormal; + dVector3 vNormal; + dVector3 vE0; + dVector3 vE1; + dVector3 vE2; + + // ODE stuff + dGeomID gCylinder; + dxTriMesh* gTrimesh; + dContactGeom* gContact; + int iFlags; + int iSkip; + int nContacts;// = 0; + sLocalContactData* gLocalContacts; +} sCylinderTrimeshColliderData; + +// Short type name +typedef sCylinderTrimeshColliderData sData; + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + dVector3Subtract(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + dVector3Subtract(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +inline void _OptimizeLocalContacts(sData& cData) +{ + int nContacts = cData.nContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j])) + { + // If they are seem to be the same then filtered + // out the least penetrate one + if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i])) + { + cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + cData.gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} + +inline int _ProcessLocalContacts(sData& cData) +{ + if (cData.nContacts == 0) + { + return 0; + } + +#ifdef OPTIMIZE_CONTACTS + if (cData.nContacts > 1 && !(cData.iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(cData); + } +#endif + + int iContact = 0; + dContactGeom* Contact = 0; + + int nFinalContact = 0; + + for (iContact = 0; iContact < cData.nContacts; iContact ++) + { + if (1 == cData.gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip); + Contact->depth = cData.gLocalContacts[iContact].fDepth; + dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal); + dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos); + Contact->g1 = cData.gCylinder; + Contact->g2 = cData.gTrimesh; + Contact->side2 = cData.gLocalContacts[iContact].triIndex; + dVector3Inv(Contact->normal); + + nFinalContact++; + } + } + // debug + //if (nFinalContact != cData.nContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact); + //} + + return nFinalContact; +} + + +bool _cldTestAxis(sData& cData, + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3& vAxis, + int iAxis, + bool bNoFlip = false) +{ + + // calculate length of separating axis vector + dReal fL = dVector3Length(vAxis); + // if not long enough + if ( fL < REAL(1e-5) ) + { + // do nothing + return true; + } + + // otherwise normalize it + vAxis[0] /= fL; + vAxis[1] /= fL; + vAxis[2] /= fL; + + dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis); + // project capsule on vAxis + dReal frc; + + if (dFabs(fdot1) > REAL(1.0) ) + { +// fdot1 = REAL(1.0); + frc = dFabs(cData.fCylinderSize* REAL(0.5)); + } + else + { + frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1) + + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + } + + dVector3 vV0; + dVector3Subtract(v0,cData.vCylinderPos,vV0); + dVector3 vV1; + dVector3Subtract(v1,cData.vCylinderPos,vV1); + dVector3 vV2; + dVector3Subtract(v2,cData.vCylinderPos,vV2); + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dVector3Dot( vV0 , vAxis ); + afv[1] = dVector3Dot( vV1 , vAxis ); + afv[2] = dVector3Dot( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = -MAX_REAL; + + // for each vertex + for(int i = 0; i < 3; i++) + { + // find minimum + if (afv[i]fMax) + { + fMax = afv[i]; + } + } + + // find capsule's center of interval on axis + dReal fCenter = (fMin+fMax)* REAL(0.5); + // calculate triangles halfinterval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > (frc+fTriangleRadius) ) + { + // exit, we have no intersection + return false; + } + + // calculate depth + dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) ); + + // if greater then best found so far + if ( fDepth < cData.fBestDepth ) + { + // remember depth + cData.fBestDepth = fDepth; + cData.fBestCenter = fCenter; + cData.fBestrt = frc; + dVector3Copy(vAxis,cData.vContactNormal); + cData.iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if ( fCenter< REAL(0.0) && !bNoFlip) + { + dVector3Inv(cData.vContactNormal); + cData.fBestCenter = -fCenter; + } + } + + return true; +} + +// intersection test between edge and circle +bool _cldTestCircleToEdgeAxis(sData& cData, + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) +{ + // calculate direction of edge + dVector3 vkl; + dVector3Subtract( vVx1 , vVx0 , vkl); + dNormalize3(vkl); + // starting point of edge + dVector3 vol; + dVector3Copy(vVx0,vol); + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2) so save some cycles here + dVector3Subtract(v0 ,v2 , cData.vE2); + + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); + vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); + vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); + + dVector3 vCp1; + vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); + vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); + vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); + + // reset best axis + cData.iBestAxis = 0; + dVector3 vAxis; + + // axis cData.vNormal + //vAxis = -cData.vNormal; + vAxis[0] = -cData.vNormal[0]; + vAxis[1] = -cData.vNormal[1]; + vAxis[2] = -cData.vNormal[2]; + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true)) + { + return false; + } + + // axis CxE0 + // vAxis = ( cData.vCylinderAxis cross cData.vE0 ); + dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2)) + { + return false; + } + + // axis CxE1 + // vAxis = ( cData.vCylinderAxis cross cData.vE1 ); + dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3)) + { + return false; + } + + // axis CxE2 + // vAxis = ( cData.vCylinderAxis cross cData.vE2 ); + dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis); + if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4)) + { + return false; + } + + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; + _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11)) + { + return false; + } + + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; + _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12)) + { + return false; + } + + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; + _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13)) + { + return false; + } + + // test cylinder axis + // vAxis = cData.vCylinderAxis; + dVector3Copy(cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14)) + { + return false; + } + + // Test top and bottom circle ring of cylinder for separation + dVector3 vccATop; + vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); + vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); + vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); + + dVector3 vccABottom; + vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); + vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); + vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); + + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20)) + { + return false; + } + + return true; +} + +bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) +{ + // translate cylinder + dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal); + dVector3 vN2; + vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp; + vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp; + vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp; + + fTemp = dVector3Length(vN2); + if (fTemp < REAL(1e-5)) + { + return false; + } + + // Normalize it + vN2[0] /= fTemp; + vN2[1] /= fTemp; + vN2[2] /= fTemp; + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius; + vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius; + vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint0[1] = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint0[2] = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint1[1] = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint1[2] = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); + + // transform cylinder edge points into triangle space + vCEdgePoint0[0] -= v0[0]; + vCEdgePoint0[1] -= v0[1]; + vCEdgePoint0[2] -= v0[2]; + + vCEdgePoint1[0] -= v0[0]; + vCEdgePoint1[1] -= v0[1]; + vCEdgePoint1[2] -= v0[2]; + + dVector4 plPlane; + dVector3 vPlaneNormal; + + // triangle plane + //plPlane = Plane4f( -cData.vNormal, 0); + vPlaneNormal[0] = -cData.vNormal[0]; + vPlaneNormal[1] = -cData.vNormal[1]; + vPlaneNormal[2] = -cData.vNormal[2]; + dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 0 + //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), REAL(1e-5)); + dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 1 + //dVector3 vTemp = ( cData.vNormal cross cData.vE1 ); + dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal); + fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - REAL(1e-5); + //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-REAL(1e-5))); + dConstructPlane(vPlaneNormal,-fTemp,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 2 + // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), REAL(1e-5)); + dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += v0[0]; + vCEdgePoint0[1] += v0[1]; + vCEdgePoint0[2] += v0[2]; + + vCEdgePoint1[0] += v0[0]; + vCEdgePoint1[1] += v0[1]; + vCEdgePoint1[2] += v0[2]; + + // calculate depths for both contact points + dVector3 vTemp; + dVector3Subtract(vCEdgePoint0,cData.vCylinderPos, vTemp); + dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; + dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp); + dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; + + dReal fDepth0 = cData.fBestDepth - (fRestDepth0); + dReal fDepth1 = cData.fBestDepth - (fRestDepth1); + + // clamp depths to zero + if(fDepth0 < REAL(0.0) ) + { + fDepth0 = REAL(0.0); + } + + if(fDepth1= (cData.iFlags & NUMC_MASK)) + return true; + } + + // Generate contact 1 + { + // generate contacts + cData.gLocalContacts[cData.nContacts].fDepth = fDepth1; + dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); + dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos); + cData.gLocalContacts[cData.nContacts].nFlags = 1; + cData.nContacts++; + } + + return true; +} + +void _cldClipCylinderToTriangle(sData& cData,const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) +{ + int i = 0; + dVector3 avPoints[3]; + dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + + dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + + // setup array of triangle vertices + dVector3Copy(v0,avPoints[0]); + dVector3Copy(v1,avPoints[1]); + dVector3Copy(v2,avPoints[2]); + + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + dSetZero(vCylinderCircleNormal_Rel,4); + // check which circle from cylinder we take for clipping + if ( dVector3Dot(cData.vCylinderAxis , cData.vContactNormal) > REAL(0.0)) + { + // get top circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + dVector3 vTemp; + dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot); + // transform triangle points to space of cylinder circle + for(i=0; i<3; i++) + { + dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); + dQuatTransform(cData.qInvCylinderRot,vTemp,avPoints[i]); + } + + int iTmpCounter1 = 0; + int iTmpCounter2 = 0; + dVector4 plPlane; + + // plane of cylinder that contains circle for intersection + //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); + dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); + dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); + + // Body of base circle of Cylinder + int nCircleSegment = 0; + for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++) + { + dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane); + + if (0 == (nCircleSegment % 2)) + { + dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); + } + else + { + dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); + } + + dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment %2) + { + for( i=0; i REAL(0.0)) + { + cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; + dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); + dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); + cData.gLocalContacts[cData.nContacts].nFlags = 1; + cData.nContacts++; + if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) + return;; + } + } + } + else + { + for( i=0; i REAL(0.0)) + { + cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; + dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); + dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); + cData.gLocalContacts[cData.nContacts].nFlags = 1; + cData.nContacts++; + if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) + return;; + } + } + } +} + +void TestOneTriangleVsCylinder( sData& cData, + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + const bool bDoubleSided) +{ + + // calculate triangle normal + dVector3Subtract( v2 , v1 ,cData.vE1); + dVector3 vTemp; + dVector3Subtract( v0 , v1 ,vTemp); + dVector3Cross(cData.vE1 , vTemp , cData.vNormal ); + + dNormalize3( cData.vNormal); + + // create plane from triangle + //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); + dReal plDistance = -dVector3Dot(v0, cData.vNormal); + dVector4 plTrianglePlane; + dConstructPlane( cData.vNormal,plDistance,plTrianglePlane); + + // calculate sphere distance to plane + dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane); + + // Sphere must be over positive side of triangle + if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + dVector3 vPnt1; + dVector3 vPnt2; + + if (fDistanceCylinderCenterToPlane < REAL(0.0) ) + { + // flip it + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt2); + dVector3Copy(v2 , vPnt1); + } + else + { + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt1); + dVector3Copy(v2 , vPnt2); + } + + cData.fBestDepth = MAX_REAL; + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( cData.iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(false); + // do nothing + return; + } + + dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis ); + + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2)) + { + return; + } + } + else + { + _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2); + } + +} + +void _InitCylinderTrimeshData(sData& cData) +{ + // get cylinder information + // Rotation + const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); + dMatrix3Copy(pRotCyc,cData.mCylinderRot); + dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot); + + // Position + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); + dVector3Copy(*pPosCyc,cData.vCylinderPos); + // Cylinder axis + dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); + // get cylinder radius and size + dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); + + // get trimesh position and orientation + const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh); + dMatrix3Copy(pRotTris,cData.mTrimeshRot); + dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot); + + // Position + const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh); + dVector3Copy(*pPosTris,cData.vTrimeshPos); + + + // calculate basic angle for 8-gon + dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS; + // calculate angle increment + dReal fAngleIncrement = fAngle*REAL(2.0); + + // calculate plane normals + // axis dependant code + for(int i=0; i= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Main data holder + sData cData; + + // Assign ODE stuff + cData.gCylinder = o1; + cData.gTrimesh = (dxTriMesh*)o2; + cData.iFlags = flags; + cData.iSkip = skip; + cData.gContact = contact; + cData.nContacts = 0; + + _InitCylinderTrimeshData(cData); + + OBBCollider& Collider = cData.gTrimesh->_OBBCollider; + + Point cCenter(cData.vCylinderPos[0],cData.vCylinderPos[1],cData.vCylinderPos[2]); + + Point cExtents(cData.fCylinderRadius,cData.fCylinderRadius,cData.fCylinderRadius); + cExtents[nCYLINDER_AXIS] = cData.fCylinderSize * REAL(0.5); + + Matrix3x3 obbRot; + + // It is a potential issue to explicitly cast to float + // if custom width floating point type is introduced in OPCODE. + // It is necessary to make a typedef and cast to it + // (e.g. typedef float opc_float;) + // However I'm not sure in what header it should be added. + + obbRot[0][0] = /*(float)*/cData.mCylinderRot[0]; + obbRot[1][0] = /*(float)*/cData.mCylinderRot[1]; + obbRot[2][0] = /*(float)*/cData.mCylinderRot[2]; + + obbRot[0][1] = /*(float)*/cData.mCylinderRot[4]; + obbRot[1][1] = /*(float)*/cData.mCylinderRot[5]; + obbRot[2][1] = /*(float)*/cData.mCylinderRot[6]; + + obbRot[0][2] = /*(float)*/cData.mCylinderRot[8]; + obbRot[1][2] = /*(float)*/cData.mCylinderRot[9]; + obbRot[2][2] = /*(float)*/cData.mCylinderRot[10]; + + OBB obbCapsule(cCenter,cExtents,obbRot); + + Matrix4x4 CapsuleMatrix; + MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix); + + Matrix4x4 MeshMatrix; + MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix); + + // TC results + if (cData.gTrimesh->doBoxTC) + { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++) + { + if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder) + { + BoxTC = &cData.gTrimesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC) + { + cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1]; + BoxTC->Geom = cData.gCylinder; + BoxTC->FatCoeff = REAL(1.0); + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix); + } + else + { + Collider.SetTemporalCoherence(false); + Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix); + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + + if (TriCount != 0) + { + if (cData.gTrimesh->ArrayCallback != null) + { + cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK)); + + int ctContacts0 = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv); + + // test this triangle + TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false); + + // fill-in tri index for generated contacts + for (; ctContacts0= (cData.iFlags & NUMC_MASK)) + { + break; + } + } + } + + return _ProcessLocalContacts(cData); +} +#endif + +// GIMPACT version of cylinder to mesh collider +#if dTRIMESH_GIMPACT +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Main data holder + sData cData; + + // Assign ODE stuff + cData.gCylinder = o1; + cData.gTrimesh = (dxTriMesh*)o2; + cData.iFlags = flags; + cData.iSkip = skip; + cData.gContact = contact; + cData.nContacts = 0; + + _InitCylinderTrimeshData(cData); + +//*****at first , collide box aabb******// + + aabb3f test_aabb; + + test_aabb.minX = o1->aabb[0]; + test_aabb.maxX = o1->aabb[1]; + test_aabb.minY = o1->aabb[2]; + test_aabb.maxY = o1->aabb[3]; + test_aabb.minZ = o1->aabb[4]; + test_aabb.maxZ = o1->aabb[5]; + + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + gim_aabbset_box_collision(&test_aabb, &cData.gTrimesh->m_collision_trimesh.m_aabbset , &collision_result); + + if(collision_result.m_size==0) + { + GIM_DYNARRAY_DESTROY(collision_result); + return 0; + } +//*****Set globals for box collision******// + + int ctContacts0 = 0; + cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK)); + + GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result); + GIM_TRIMESH * ptrimesh = &cData.gTrimesh->m_collision_trimesh; + + gim_trimesh_locks_work_data(ptrimesh); + + + for(unsigned int i=0;i= (cData.iFlags & NUMC_MASK)) + { + break; + } + } + + gim_trimesh_unlocks_work_data(ptrimesh); + GIM_DYNARRAY_DESTROY(collision_result); + + return _ProcessLocalContacts(cData); +} +#endif + +#endif // dTRIMESH_ENABLED + + diff --git a/libraries/ode-0.9/ode/src/collision_kernel.cpp b/libraries/ode-0.9/ode/src/collision_kernel.cpp new file mode 100644 index 0000000..b885603 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_kernel.cpp @@ -0,0 +1,1103 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +core collision functions and data structures, plus part of the public API +for geometry objects + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_util.h" +#include "collision_std.h" +#include "collision_transform.h" +#include "collision_trimesh_internal.h" + +#if dTRIMESH_GIMPACT +#include +#endif + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// helper functions for dCollide()ing a space with another geom + +// this struct records the parameters passed to dCollideSpaceGeom() + +// Allocate and free posr - we cache a single posr to avoid thrashing +static dxPosR* s_cachedPosR = 0; + +dxPosR* dAllocPosr() +{ + dxPosR* retPosR; + if (s_cachedPosR) + { + retPosR = s_cachedPosR; + s_cachedPosR = 0; + } + else + { + retPosR = (dxPosR*) dAlloc (sizeof(dxPosR)); + } + return retPosR; +} + +void dFreePosr(dxPosR* oldPosR) +{ + if (oldPosR) + { + if (s_cachedPosR) + { + dFree(s_cachedPosR, sizeof(dxPosR)); + } + s_cachedPosR = oldPosR; + } +} + +void dClearPosrCache(void) +{ + if (s_cachedPosR) + { + dFree(s_cachedPosR, sizeof(dxPosR)); + s_cachedPosR = 0; + } +} + +struct SpaceGeomColliderData { + int flags; // space left in contacts array + dContactGeom *contact; + int skip; +}; + + +static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2) +{ + SpaceGeomColliderData *d = (SpaceGeomColliderData*) data; + if (d->flags & NUMC_MASK) { + int n = dCollide (o1,o2,d->flags,d->contact,d->skip); + d->contact = CONTACT (d->contact,d->skip*n); + d->flags -= n; + } +} + + +static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + SpaceGeomColliderData data; + data.flags = flags; + data.contact = contact; + data.skip = skip; + dSpaceCollide2 (o1,o2,&data,&space_geom_collider); + return (flags & NUMC_MASK) - (data.flags & NUMC_MASK); +} + +//**************************************************************************** +// dispatcher for the N^2 collider functions + +// function pointers and modes for n^2 class collider functions + +struct dColliderEntry { + dColliderFn *fn; // collider function, 0 = no function available + int reverse; // 1 = reverse o1 and o2 +}; +static dColliderEntry colliders[dGeomNumClasses][dGeomNumClasses]; +static int colliders_initialized = 0; + + +// setCollider() will refuse to write over a collider entry once it has +// been written. + +static void setCollider (int i, int j, dColliderFn *fn) +{ + if (colliders[i][j].fn == 0) { + colliders[i][j].fn = fn; + colliders[i][j].reverse = 0; + } + if (colliders[j][i].fn == 0) { + colliders[j][i].fn = fn; + colliders[j][i].reverse = 1; + } +} + + +static void setAllColliders (int i, dColliderFn *fn) +{ + for (int j=0; j Convex Collision + setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane); + setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex); + setCollider (dConvexClass,dBoxClass,&dCollideConvexBox); + setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule); + setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex); + setCollider (dRayClass,dConvexClass,&dCollideRayConvex); +//<-- Convex Collision + +//--> dHeightfield Collision + setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dCylinderClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dConvexClass,&dCollideHeightfield); +#if dTRIMESH_ENABLED + setCollider (dHeightfieldClass,dTriMeshClass,&dCollideHeightfield); +#endif +//<-- dHeightfield Collision + + setAllColliders (dGeomTransformClass,&dCollideTransform); +} + + +/* + * NOTE! + * If it is necessary to add special processing mode without contact generation + * use NULL contact parameter value as indicator, not zero in flags. + */ +int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, + int skip) +{ + dAASSERT(o1 && o2 && contact); + dUASSERT(colliders_initialized,"colliders array not initialized"); + dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number"); + dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number"); + // Even though comparison for greater or equal to one is used in all the + // other places, here it is more logical to check for greater than zero + // because function does not require any specific number of contact slots - + // it must be just a positive. + dUASSERT((flags & NUMC_MASK) > 0, "no contacts requested"); + + // Extra precaution for zero contact count in parameters + if ((flags & NUMC_MASK) == 0) return 0; + // no contacts if both geoms are the same + if (o1 == o2) return 0; + + // no contacts if both geoms on the same body, and the body is not 0 + if (o1->body == o2->body && o1->body) return 0; + + o1->recomputePosr(); + o2->recomputePosr(); + + dColliderEntry *ce = &colliders[o1->type][o2->type]; + int count = 0; + if (ce->fn) { + if (ce->reverse) { + count = (*ce->fn) (o2,o1,flags,contact,skip); + for (int i=0; inormal[0] = -c->normal[0]; + c->normal[1] = -c->normal[1]; + c->normal[2] = -c->normal[2]; + dxGeom *tmp = c->g1; + c->g1 = c->g2; + c->g2 = tmp; + int tmpint = c->side1; + c->side1 = c->side2; + c->side2 = tmpint; + } + } + else { + count = (*ce->fn) (o1,o2,flags,contact,skip); + } + } + return count; +} + +//**************************************************************************** +// dxGeom + +dxGeom::dxGeom (dSpaceID _space, int is_placeable) +{ + initColliders(); + + // setup body vars. invalid type of -1 must be changed by the constructor. + type = -1; + gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED; + if (is_placeable) gflags |= GEOM_PLACEABLE; + data = 0; + body = 0; + body_next = 0; + if (is_placeable) { + final_posr = dAllocPosr(); + dSetZero (final_posr->pos,4); + dRSetIdentity (final_posr->R); + } + else { + final_posr = 0; + } + offset_posr = 0; + + // setup space vars + next = 0; + tome = 0; + parent_space = 0; + dSetZero (aabb,6); + category_bits = ~0; + collide_bits = ~0; + + // put this geom in a space if required + if (_space) dSpaceAdd (_space,this); +} + + +dxGeom::~dxGeom() +{ + if (parent_space) dSpaceRemove (parent_space,this); + if ((gflags & GEOM_PLACEABLE) && (!body || (body && offset_posr))) + dFreePosr(final_posr); + if (offset_posr) dFreePosr(offset_posr); + bodyRemove(); +} + + +int dxGeom::AABBTest (dxGeom *o, dReal aabb[6]) +{ + return 1; +} + + +void dxGeom::bodyRemove() +{ + if (body) { + // delete this geom from body list + dxGeom **last = &body->geom, *g = body->geom; + while (g) { + if (g == this) { + *last = g->body_next; + break; + } + last = &g->body_next; + g = g->body_next; + } + body = 0; + body_next = 0; + } +} + +inline void myswap(dReal& a, dReal& b) { dReal t=b; b=a; a=t; } + + +inline void matrixInvert(const dMatrix3& inMat, dMatrix3& outMat) +{ + memcpy(outMat, inMat, sizeof(dMatrix3)); + // swap _12 and _21 + myswap(outMat[0 + 4*1], outMat[1 + 4*0]); + // swap _31 and _13 + myswap(outMat[2 + 4*0], outMat[0 + 4*2]); + // swap _23 and _32 + myswap(outMat[1 + 4*2], outMat[2 + 4*1]); +} + +void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr) +{ + dMatrix3 inv_offset; + matrixInvert(offset_posr.R, inv_offset); + + dMULTIPLY0_333(body_posr.R, final_posr.R, inv_offset); + dVector3 world_offset; + dMULTIPLY0_331(world_offset, body_posr.R, offset_posr.pos); + body_posr.pos[0] = final_posr.pos[0] - world_offset[0]; + body_posr.pos[1] = final_posr.pos[1] - world_offset[1]; + body_posr.pos[2] = final_posr.pos[2] - world_offset[2]; +} + +void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr) +{ + dMatrix3 inv_body; + matrixInvert(body_posr.R, inv_body); + + dMULTIPLY0_333(offset_posr.R, inv_body, world_posr.R); + dVector3 world_offset; + world_offset[0] = world_posr.pos[0] - body_posr.pos[0]; + world_offset[1] = world_posr.pos[1] - body_posr.pos[1]; + world_offset[2] = world_posr.pos[2] - body_posr.pos[2]; + dMULTIPLY0_331(offset_posr.pos, inv_body, world_offset); +} + +void dxGeom::computePosr() +{ + // should only be recalced if we need to - ie offset from a body + dIASSERT(offset_posr); + dIASSERT(body); + + dMULTIPLY0_331 (final_posr->pos,body->posr.R,offset_posr->pos); + final_posr->pos[0] += body->posr.pos[0]; + final_posr->pos[1] += body->posr.pos[1]; + final_posr->pos[2] += body->posr.pos[2]; + dMULTIPLY0_333 (final_posr->R,body->posr.R,offset_posr->R); +} + +//**************************************************************************** +// misc + +dxGeom *dGeomGetBodyNext (dxGeom *geom) +{ + return geom->body_next; +} + +//**************************************************************************** +// public API for geometry objects + +#define CHECK_NOT_LOCKED(space) \ + dUASSERT (!(space && space->lock_count), \ + "invalid operation for geom in locked space"); + + +void dGeomDestroy (dxGeom *g) +{ + dAASSERT (g); + delete g; +} + + +void dGeomSetData (dxGeom *g, void *data) +{ + dAASSERT (g); + g->data = data; +} + + +void *dGeomGetData (dxGeom *g) +{ + dAASSERT (g); + return g->data; +} + + +void dGeomSetBody (dxGeom *g, dxBody *b) +{ + dAASSERT (g); + dUASSERT (b == NULL || (g->gflags & GEOM_PLACEABLE),"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + + if (b) { + if (!g->body) dFreePosr(g->final_posr); + if (g->body != b) { + if (g->offset_posr) { + dFreePosr(g->offset_posr); + g->offset_posr = 0; + } + g->final_posr = &b->posr; + g->bodyRemove(); + g->bodyAdd (b); + } + dGeomMoved (g); + } + else { + if (g->body) { + if (g->offset_posr) + { + // if we're offset, we already have our own final position, make sure its updated + g->recomputePosr(); + dFreePosr(g->offset_posr); + g->offset_posr = 0; + } + else + { + g->final_posr = dAllocPosr(); + memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3)); + memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3)); + } + g->bodyRemove(); + } + // dGeomMoved() should not be called if the body is being set to 0, as the + // new position of the geom is set to the old position of the body, so the + // effective position of the geom remains unchanged. + } +} + + +dBodyID dGeomGetBody (dxGeom *g) +{ + dAASSERT (g); + return g->body; +} + + +void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->offset_posr) { + // move body such that body+offset = position + dVector3 world_offset; + dMULTIPLY0_331(world_offset, g->body->posr.R, g->offset_posr->pos); + dBodySetPosition(g->body, + x - world_offset[0], + y - world_offset[1], + z - world_offset[2]); + } + else if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetPosition (g->body,x,y,z); + } + else { + g->final_posr->pos[0] = x; + g->final_posr->pos[1] = y; + g->final_posr->pos[2] = z; + dGeomMoved (g); + } +} + + +void dGeomSetRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->offset_posr) { + g->recomputePosr(); + // move body such that body+offset = rotation + dxPosR new_final_posr; + dxPosR new_body_posr; + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + memcpy(new_final_posr.R, R, sizeof(dMatrix3)); + getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); + dBodySetRotation(g->body, new_body_posr.R); + dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); + } + else if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetRotation (g->body,R); + } + else { + memcpy (g->final_posr->R,R,sizeof(dMatrix3)); + dGeomMoved (g); + } +} + + +void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->offset_posr) { + g->recomputePosr(); + // move body such that body+offset = rotation + dxPosR new_final_posr; + dxPosR new_body_posr; + dQtoR (quat, new_final_posr.R); + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + + getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); + dBodySetRotation(g->body, new_body_posr.R); + dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); + } + if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetQuaternion (g->body,quat); + } + else { + dQtoR (quat, g->final_posr->R); + dGeomMoved (g); + } +} + + +const dReal * dGeomGetPosition (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + g->recomputePosr(); + return g->final_posr->pos; +} + + +void dGeomCopyPosition(dxGeom *g, dVector3 pos) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + g->recomputePosr(); + const dReal* src = g->final_posr->pos; + pos[0] = src[0]; + pos[1] = src[1]; + pos[2] = src[2]; +} + + +const dReal * dGeomGetRotation (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + g->recomputePosr(); + return g->final_posr->R; +} + + +void dGeomCopyRotation(dxGeom *g, dMatrix3 R) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + g->recomputePosr(); + const dReal* src = g->final_posr->R; + R[0] = src[0]; + R[1] = src[1]; + R[2] = src[2]; + R[4] = src[4]; + R[5] = src[5]; + R[6] = src[6]; + R[8] = src[8]; + R[9] = src[9]; + R[10] = src[10]; +} + + +void dGeomGetQuaternion (dxGeom *g, dQuaternion quat) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + if (g->body && !g->offset_posr) { + const dReal * body_quat = dBodyGetQuaternion (g->body); + quat[0] = body_quat[0]; + quat[1] = body_quat[1]; + quat[2] = body_quat[2]; + quat[3] = body_quat[3]; + } + else { + g->recomputePosr(); + dRtoQ (g->final_posr->R, quat); + } +} + + +void dGeomGetAABB (dxGeom *g, dReal aabb[6]) +{ + dAASSERT (g); + dAASSERT (aabb); + g->recomputeAABB(); + memcpy (aabb,g->aabb,6 * sizeof(dReal)); +} + + +int dGeomIsSpace (dxGeom *g) +{ + dAASSERT (g); + return IS_SPACE(g); +} + + +dSpaceID dGeomGetSpace (dxGeom *g) +{ + dAASSERT (g); + return g->parent_space; +} + + +int dGeomGetClass (dxGeom *g) +{ + dAASSERT (g); + return g->type; +} + + +void dGeomSetCategoryBits (dxGeom *g, unsigned long bits) +{ + dAASSERT (g); + CHECK_NOT_LOCKED (g->parent_space); + g->category_bits = bits; +} + + +void dGeomSetCollideBits (dxGeom *g, unsigned long bits) +{ + dAASSERT (g); + CHECK_NOT_LOCKED (g->parent_space); + g->collide_bits = bits; +} + + +unsigned long dGeomGetCategoryBits (dxGeom *g) +{ + dAASSERT (g); + return g->category_bits; +} + + +unsigned long dGeomGetCollideBits (dxGeom *g) +{ + dAASSERT (g); + return g->collide_bits; +} + + +void dGeomEnable (dxGeom *g) +{ + dAASSERT (g); + g->gflags |= GEOM_ENABLED; +} + +void dGeomDisable (dxGeom *g) +{ + dAASSERT (g); + g->gflags &= ~GEOM_ENABLED; +} + +int dGeomIsEnabled (dxGeom *g) +{ + dAASSERT (g); + return (g->gflags & GEOM_ENABLED) != 0; +} + + +//**************************************************************************** +// C interface that lets the user make new classes. this interface is a lot +// more cumbersome than C++ subclassing, which is what is used internally +// in ODE. this API is mainly to support legacy code. + +static int num_user_classes = 0; +static dGeomClass user_classes [dMaxUserClasses]; + + +struct dxUserGeom : public dxGeom { + void *user_data; + + dxUserGeom (int class_num); + ~dxUserGeom(); + void computeAABB(); + int AABBTest (dxGeom *o, dReal aabb[6]); +}; + + +dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1) +{ + type = class_num; + int size = user_classes[type-dFirstUserClass].bytes; + user_data = dAlloc (size); + memset (user_data,0,size); +} + + +dxUserGeom::~dxUserGeom() +{ + dGeomClass *c = &user_classes[type-dFirstUserClass]; + if (c->dtor) c->dtor (this); + dFree (user_data,c->bytes); +} + + +void dxUserGeom::computeAABB() +{ + user_classes[type-dFirstUserClass].aabb (this,aabb); +} + + +int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6]) +{ + dGeomClass *c = &user_classes[type-dFirstUserClass]; + if (c->aabb_test) return c->aabb_test (this,o,aabb); + else return 1; +} + + +static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + // this generic collider function is called the first time that a user class + // tries to collide against something. it will find out the correct collider + // function and then set the colliders array so that the correct function is + // called directly the next time around. + + int t1 = o1->type; // note that o1 is a user geom + int t2 = o2->type; // o2 *may* be a user geom + + // find the collider function to use. if o1 does not know how to collide with + // o2, then o2 might know how to collide with o1 (provided that it is a user + // geom). + dColliderFn *fn = user_classes[t1-dFirstUserClass].collider (t2); + int reverse = 0; + if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) { + fn = user_classes[t2-dFirstUserClass].collider (t1); + reverse = 1; + } + + // set the colliders array so that the correct function is called directly + // the next time around. note that fn can be 0 here if no collider was found, + // which means that dCollide() will always return 0 for this case. + colliders[t1][t2].fn = fn; + colliders[t1][t2].reverse = reverse; + colliders[t2][t1].fn = fn; + colliders[t2][t1].reverse = !reverse; + + // now call the collider function indirectly through dCollide(), so that + // contact reversing is properly handled. + return dCollide (o1,o2,flags,contact,skip); +} + + +int dCreateGeomClass (const dGeomClass *c) +{ + dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class"); + + if (num_user_classes >= dMaxUserClasses) { + dDebug (0,"too many user classes, you must increase the limit and " + "recompile ODE"); + } + user_classes[num_user_classes] = *c; + int class_number = num_user_classes + dFirstUserClass; + initColliders(); + setAllColliders (class_number,&dCollideUserGeomWithGeom); + + num_user_classes++; + return class_number; +} + + +void * dGeomGetClassData (dxGeom *g) +{ + dUASSERT (g && g->type >= dFirstUserClass && + g->type <= dLastUserClass,"not a custom class"); + dxUserGeom *user = (dxUserGeom*) g; + return user->user_data; +} + + +dGeomID dCreateGeom (int classnum) +{ + dUASSERT (classnum >= dFirstUserClass && + classnum <= dLastUserClass,"not a custom class"); + return new dxUserGeom (classnum); +} + + + +/* ************************************************************************ */ +/* geom offset from body */ + +void dGeomCreateOffset (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + if (g->offset_posr) + { + return; // already created + } + dIASSERT (g->final_posr == &g->body->posr); + + g->final_posr = dAllocPosr(); + g->offset_posr = dAllocPosr(); + dSetZero (g->offset_posr->pos,4); + dRSetIdentity (g->offset_posr->R); + + g->gflags |= GEOM_POSR_BAD; +} + +void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset(g); + } + g->offset_posr->pos[0] = x; + g->offset_posr->pos[1] = y; + g->offset_posr->pos[2] = z; + dGeomMoved (g); +} + +void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + memcpy (g->offset_posr->R,R,sizeof(dMatrix3)); + dGeomMoved (g); +} + +void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + dQtoR (quat, g->offset_posr->R); + dGeomMoved (g); +} + +void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset(g); + } + dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos); + dGeomMoved (g); +} + +void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + g->recomputePosr(); + + dxPosR new_final_posr; + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + memcpy(new_final_posr.R, R, sizeof(dMatrix3)); + + getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); + dGeomMoved (g); +} + +void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dUASSERT (g->body, "geom must be on a body"); + CHECK_NOT_LOCKED (g->parent_space); + if (!g->offset_posr) + { + dGeomCreateOffset (g); + } + + g->recomputePosr(); + + dxPosR new_final_posr; + memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); + dQtoR (quat, new_final_posr.R); + + getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); + dGeomMoved (g); +} + +void dGeomClearOffset(dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + if (g->offset_posr) + { + dIASSERT(g->body); + // no longer need an offset posr + dFreePosr(g->offset_posr); + g->offset_posr = 0; + // the geom will now share the position of the body + dFreePosr(g->final_posr); + g->final_posr = &g->body->posr; + // geom has moved + g->gflags &= ~GEOM_POSR_BAD; + dGeomMoved (g); + } +} + +int dGeomIsOffset(dxGeom *g) +{ + dAASSERT (g); + return ((0 != g->offset_posr) ? 1 : 0); +} + +static const dVector3 OFFSET_POSITION_ZERO = { 0.0f, 0.0f, 0.0f, 0.0f }; + +const dReal * dGeomGetOffsetPosition (dxGeom *g) +{ + dAASSERT (g); + if (g->offset_posr) + { + return g->offset_posr->pos; + } + return OFFSET_POSITION_ZERO; +} + +void dGeomCopyOffsetPosition (dxGeom *g, dVector3 pos) +{ + dAASSERT (g); + if (g->offset_posr) + { + const dReal* src = g->offset_posr->pos; + pos[0] = src[0]; + pos[1] = src[1]; + pos[2] = src[2]; + } + else + { + pos[0] = 0; + pos[1] = 0; + pos[2] = 0; + } +} + +static const dMatrix3 OFFSET_ROTATION_ZERO = +{ + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, +}; + +const dReal * dGeomGetOffsetRotation (dxGeom *g) +{ + dAASSERT (g); + if (g->offset_posr) + { + return g->offset_posr->R; + } + return OFFSET_ROTATION_ZERO; +} + +void dGeomCopyOffsetRotation (dxGeom *g, dMatrix3 R) +{ + dAASSERT (g); + if (g->offset_posr) + { + const dReal* src = g->final_posr->R; + R[0] = src[0]; + R[1] = src[1]; + R[2] = src[2]; + R[4] = src[4]; + R[5] = src[5]; + R[6] = src[6]; + R[8] = src[8]; + R[9] = src[9]; + R[10] = src[10]; + } + else + { + R[0] = OFFSET_ROTATION_ZERO[0]; + R[1] = OFFSET_ROTATION_ZERO[1]; + R[2] = OFFSET_ROTATION_ZERO[2]; + R[4] = OFFSET_ROTATION_ZERO[4]; + R[5] = OFFSET_ROTATION_ZERO[5]; + R[6] = OFFSET_ROTATION_ZERO[6]; + R[8] = OFFSET_ROTATION_ZERO[8]; + R[9] = OFFSET_ROTATION_ZERO[9]; + R[10] = OFFSET_ROTATION_ZERO[10]; + } +} + +void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result) +{ + dAASSERT (g); + if (g->offset_posr) + { + dRtoQ (g->offset_posr->R, result); + } + else + { + dSetZero (result,4); + result[0] = 1; + } +} + +//**************************************************************************** +// initialization and shutdown routines - allocate and initialize data, +// cleanup before exiting + +extern void opcode_collider_cleanup(); + +void dInitODE() +{ +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + gimpact_init(); +#endif +} + +void dCloseODE() +{ + colliders_initialized = 0; + num_user_classes = 0; + dClearPosrCache(); + +#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT + gimpact_terminate(); +#endif + +#if dTRIMESH_ENABLED && dTRIMESH_OPCODE + // Free up static allocations in opcode + opcode_collider_cleanup(); +#endif +} diff --git a/libraries/ode-0.9/ode/src/collision_kernel.h b/libraries/ode-0.9/ode/src/collision_kernel.h new file mode 100644 index 0000000..d5a2bc4 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_kernel.h @@ -0,0 +1,214 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +internal data structures and functions for collision detection. + +*/ + +#ifndef _ODE_COLLISION_KERNEL_H_ +#define _ODE_COLLISION_KERNEL_H_ + +#include +#include +#include +#include "objects.h" + +//**************************************************************************** +// constants and macros + +// mask for the number-of-contacts field in the dCollide() flags parameter +#define NUMC_MASK (0xffff) + +#define IS_SPACE(geom) \ + ((geom)->type >= dFirstSpaceClass && (geom)->type <= dLastSpaceClass) + +//**************************************************************************** +// geometry object base class + + +// geom flags. +// +// GEOM_DIRTY means that the space data structures for this geom are +// potentially not up to date. NOTE THAT all space parents of a dirty geom +// are themselves dirty. this is an invariant that must be enforced. +// +// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date. +// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might +// recalculate its own AABB but does not know how to update the space data +// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY. +// the valid combinations are: +// 0 +// GEOM_DIRTY +// GEOM_DIRTY|GEOM_AABB_BAD +// GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD + +enum { + GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown + GEOM_POSR_BAD = 2, // geom's final posr is not valid + GEOM_AABB_BAD = 4, // geom's AABB is not valid + GEOM_PLACEABLE = 8, // geom is placeable + GEOM_ENABLED = 16, // geom is enabled + + // Ray specific + RAY_FIRSTCONTACT = 0x10000, + RAY_BACKFACECULL = 0x20000, + RAY_CLOSEST_HIT = 0x40000 +}; + + +// geometry object base class. pos and R will either point to a separately +// allocated buffer (if body is 0 - pos points to the dxPosR object) or to +// the pos and R of the body (if body nonzero). +// a dGeomID is a pointer to this object. + +struct dxGeom : public dBase { + int type; // geom type number, set by subclass constructor + int gflags; // flags used by geom and space + void *data; // user-defined data pointer + dBodyID body; // dynamics body associated with this object (if any) + dxGeom *body_next; // next geom in body's linked list of associated geoms + dxPosR *final_posr; // final position of the geom in world coordinates + dxPosR *offset_posr; // offset from body in local coordinates + + // information used by spaces + dxGeom *next; // next geom in linked list of geoms + dxGeom **tome; // linked list backpointer + dxSpace *parent_space;// the space this geom is contained in, 0 if none + dReal aabb[6]; // cached AABB for this space + unsigned long category_bits,collide_bits; + + dxGeom (dSpaceID _space, int is_placeable); + virtual ~dxGeom(); + + + // calculate our new final position from our offset and body + void computePosr(); + + // recalculate our new final position if needed + void recomputePosr() + { + if (gflags & GEOM_POSR_BAD) { + computePosr(); + gflags &= ~GEOM_POSR_BAD; + } + } + + virtual void computeAABB()=0; + // compute the AABB for this object and put it in aabb. this function + // always performs a fresh computation, it does not inspect the + // GEOM_AABB_BAD flag. + + virtual int AABBTest (dxGeom *o, dReal aabb[6]); + // test whether the given AABB object intersects with this object, return + // 1=yes, 0=no. this is used as an early-exit test in the space collision + // functions. the default implementation returns 1, which is the correct + // behavior if no more detailed implementation can be provided. + + // utility functions + + // compute the AABB only if it is not current. this function manipulates + // the GEOM_AABB_BAD flag. + + void recomputeAABB() { + if (gflags & GEOM_AABB_BAD) { + // our aabb functions assume final_posr is up to date + recomputePosr(); + computeAABB(); + gflags &= ~GEOM_AABB_BAD; + } + } + + // add and remove this geom from a linked list maintained by a space. + + void spaceAdd (dxGeom **first_ptr) { + next = *first_ptr; + tome = first_ptr; + if (*first_ptr) (*first_ptr)->tome = &next; + *first_ptr = this; + } + void spaceRemove() { + if (next) next->tome = tome; + *tome = next; + } + + // add and remove this geom from a linked list maintained by a body. + + void bodyAdd (dxBody *b) { + body = b; + body_next = b->geom; + b->geom = this; + } + void bodyRemove(); +}; + +//**************************************************************************** +// the base space class +// +// the contained geoms are divided into two kinds: clean and dirty. +// the clean geoms have not moved since they were put in the list, +// and their AABBs are valid. the dirty geoms have changed position, and +// their AABBs are may not be valid. the two types are distinguished by the +// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list. + +struct dxSpace : public dxGeom { + int count; // number of geoms in this space + dxGeom *first; // first geom in list + int cleanup; // cleanup mode, 1=destroy geoms on exit + + // cached state for getGeom() + int current_index; // only valid if current_geom != 0 + dxGeom *current_geom; // if 0 then there is no information + + // locking stuff. the space is locked when it is currently traversing its + // internal data structures, e.g. in collide() and collide2(). operations + // that modify the contents of the space are not permitted when the space + // is locked. + int lock_count; + + dxSpace (dSpaceID _space); + ~dxSpace(); + + void computeAABB(); + + void setCleanup (int mode); + int getCleanup(); + int query (dxGeom *geom); + int getNumGeoms(); + virtual dxGeom *getGeom (int i); + + virtual void add (dxGeom *); + virtual void remove (dxGeom *); + virtual void dirty (dxGeom *); + + virtual void cleanGeoms()=0; + // turn all dirty geoms into clean geoms by computing their AABBs and any + // other space data structures that are required. this should clear the + // GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms. + + virtual void collide (void *data, dNearCallback *callback)=0; + virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0; +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp b/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp new file mode 100644 index 0000000..86a1833 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp @@ -0,0 +1,584 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// QuadTreeSpace by Erwin de Vries. + +#include +#include +#include +#include +#include "collision_kernel.h" + +#include "collision_space_internal.h" + + +#define AXIS0 0 +#define AXIS1 1 +#define UP 2 + +//#define DRAWBLOCKS + +const int SPLITAXIS = 2; +const int SPLITS = SPLITAXIS * SPLITAXIS; + +#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED + +class Block{ +public: + dReal MinX, MaxX; + dReal MinZ, MaxZ; + + dGeomID First; + int GeomCount; + + Block* Parent; + Block* Children; + + void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks); + + void Collide(void* UserData, dNearCallback* Callback); + void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback); + + void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback); + + void AddObject(dGeomID Object); + void DelObject(dGeomID Object); + void Traverse(dGeomID Object); + + bool Inside(const dReal* AABB); + + Block* GetBlock(const dReal* AABB); + Block* GetBlockChild(const dReal* AABB); +}; + + +#ifdef DRAWBLOCKS +#include "..\..\Include\drawstuff\\drawstuff.h" + +static void DrawBlock(Block* Block){ + dVector3 v[8]; + v[0][AXIS0] = Block->MinX; + v[0][UP] = REAL(-1.0); + v[0][AXIS1] = Block->MinZ; + + v[1][AXIS0] = Block->MinX; + v[1][UP] = REAL(-1.0); + v[1][AXIS1] = Block->MaxZ; + + v[2][AXIS0] = Block->MaxX; + v[2][UP] = REAL(-1.0); + v[2][AXIS1] = Block->MinZ; + + v[3][AXIS0] = Block->MaxX; + v[3][UP] = REAL(-1.0); + v[3][AXIS1] = Block->MaxZ; + + v[4][AXIS0] = Block->MinX; + v[4][UP] = REAL(1.0); + v[4][AXIS1] = Block->MinZ; + + v[5][AXIS0] = Block->MinX; + v[5][UP] = REAL(1.0); + v[5][AXIS1] = Block->MaxZ; + + v[6][AXIS0] = Block->MaxX; + v[6][UP] = REAL(1.0); + v[6][AXIS1] = Block->MinZ; + + v[7][AXIS0] = Block->MaxX; + v[7][UP] = REAL(1.0); + v[7][AXIS1] = Block->MaxZ; + + // Bottom + dsDrawLine(v[0], v[1]); + dsDrawLine(v[1], v[3]); + dsDrawLine(v[3], v[2]); + dsDrawLine(v[2], v[0]); + + // Top + dsDrawLine(v[4], v[5]); + dsDrawLine(v[5], v[7]); + dsDrawLine(v[7], v[6]); + dsDrawLine(v[6], v[4]); + + // Sides + dsDrawLine(v[0], v[4]); + dsDrawLine(v[1], v[5]); + dsDrawLine(v[2], v[6]); + dsDrawLine(v[3], v[7]); +} +#endif //DRAWBLOCKS + + +void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){ + GeomCount = 0; + First = 0; + + MinX = Center[AXIS0] - Extents[AXIS0]; + MaxX = Center[AXIS0] + Extents[AXIS0]; + + MinZ = Center[AXIS1] - Extents[AXIS1]; + MaxZ = Center[AXIS1] + Extents[AXIS1]; + + this->Parent = Parent; + if (Depth > 0){ + Children = Blocks; + Blocks += SPLITS; + + dVector3 ChildExtents; + ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS; + ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS; + ChildExtents[UP] = Extents[UP]; + + for (int i = 0; i < SPLITAXIS; i++){ + for (int j = 0; j < SPLITAXIS; j++){ + int Index = i * SPLITAXIS + j; + + dVector3 ChildCenter; + ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2); + ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2); + ChildCenter[UP] = Center[UP]; + + Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks); + } + } + } + else Children = 0; +} + +void Block::Collide(void* UserData, dNearCallback* Callback){ +#ifdef DRAWBLOCKS + DrawBlock(this); +#endif + // Collide the local list + dxGeom* g = First; + while (g){ + if (GEOM_ENABLED(g)){ + Collide(g, g->next, UserData, Callback); + } + g = g->next; + } + + // Recurse for children + if (Children){ + for (int i = 0; i < SPLITS; i++){ + if (Children[i].GeomCount <= 1){ // Early out + continue; + } + Children[i].Collide(UserData, Callback); + } + } +} + +void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){ +#ifdef DRAWBLOCKS + DrawBlock(this); +#endif + // Collide against local list + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next; + } + + // Collide against children + if (Children){ + for (int i = 0; i < SPLITS; i++){ + // Early out for empty blocks + if (Children[i].GeomCount == 0){ + continue; + } + + // Does the geom's AABB collide with the block? + // Dont do AABB tests for single geom blocks. + if (Children[i].GeomCount == 1 && Children[i].First){ + // + } + else if (true){ + if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX || + g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX || + g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ || + g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue; + } + Children[i].Collide(g1, Children[i].First, UserData, Callback); + } + } +} + +void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){ + // Collide against local list + dxGeom* g2 = First; + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next; + } +} + +void Block::AddObject(dGeomID Object){ + // Add the geom + Object->next = First; + First = Object; + Object->tome = (dxGeom**)this; + + // Now traverse upwards to tell that we have a geom + Block* Block = this; + do{ + Block->GeomCount++; + Block = Block->Parent; + } + while (Block); +} + +void Block::DelObject(dGeomID Object){ + // Del the geom + dxGeom* g = First; + dxGeom* Last = 0; + while (g){ + if (g == Object){ + if (Last){ + Last->next = g->next; + } + else First = g->next; + + break; + } + Last = g; + g = g->next; + } + + Object->tome = 0; + + // Now traverse upwards to tell that we have lost a geom + Block* Block = this; + do{ + Block->GeomCount--; + Block = Block->Parent; + } + while (Block); +} + +void Block::Traverse(dGeomID Object){ + Block* NewBlock = GetBlock(Object->aabb); + + if (NewBlock != this){ + // Remove the geom from the old block and add it to the new block. + // This could be more optimal, but the loss should be very small. + DelObject(Object); + NewBlock->AddObject(Object); + } +} + +bool Block::Inside(const dReal* AABB){ + return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ; +} + +Block* Block::GetBlock(const dReal* AABB){ + if (Inside(AABB)){ + return GetBlockChild(AABB); // Child or this will have a good block + } + else if (Parent){ + return Parent->GetBlock(AABB); // Parent has a good block + } + else return this; // We are at the root, so we have little choice +} + +Block* Block::GetBlockChild(const dReal* AABB){ + if (Children){ + for (int i = 0; i < SPLITS; i++){ + if (Children[i].Inside(AABB)){ + return Children[i].GetBlockChild(AABB); // Child will have good block + } + } + } + return this; // This is the best block +} + +//**************************************************************************** +// quadtree space + +struct dxQuadTreeSpace : public dxSpace{ + Block* Blocks; // Blocks[0] is the root + + dArray DirtyList; + + dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth); + ~dxQuadTreeSpace(); + + dxGeom* getGeom(int i); + + void add(dxGeom* g); + void remove(dxGeom* g); + void dirty(dxGeom* g); + + void computeAABB(); + + void cleanGeoms(); + void collide(void* UserData, dNearCallback* Callback); + void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback); + + // Temp data + Block* CurrentBlock; // Only used while enumerating + int* CurrentChild; // Only used while enumerating + int CurrentLevel; // Only used while enumerating + dxGeom* CurrentObject; // Only used while enumerating + int CurrentIndex; +}; + +dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){ + type = dQuadTreeSpaceClass; + + int BlockCount = 0; + for (int i = 0; i <= Depth; i++){ + BlockCount += (int)pow((dReal)SPLITS, i); + } + + Blocks = (Block*)dAlloc(BlockCount * sizeof(Block)); + Block* Blocks = this->Blocks + 1; // This pointer gets modified! + + this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks); + + CurrentBlock = 0; + CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int)); + CurrentLevel = 0; + CurrentObject = 0; + CurrentIndex = -1; + + // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + +dxQuadTreeSpace::~dxQuadTreeSpace(){ + int Depth = 0; + Block* Current = &Blocks[0]; + while (Current){ + Depth++; + Current = Current->Children; + } + + int BlockCount = 0; + for (int i = 0; i < Depth; i++){ + BlockCount += (int)pow((dReal)SPLITS, i); + } + + dFree(Blocks, BlockCount * sizeof(Block)); + dFree(CurrentChild, (Depth + 1) * sizeof(int)); +} + +dxGeom* dxQuadTreeSpace::getGeom(int Index){ + dUASSERT(Index >= 0 && Index < count, "index out of range"); + + //@@@ + dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented"); + + return 0; + + // This doesnt work + + /*if (CurrentIndex == Index){ + // Loop through all objects in the local list +CHILDRECURSE: + if (CurrentObject){ + dGeomID g = CurrentObject; + CurrentObject = CurrentObject->next; + CurrentIndex++; + +#ifdef DRAWBLOCKS + DrawBlock(CurrentBlock); +#endif //DRAWBLOCKS + return g; + } + else{ + // Now lets loop through our children. Starting at index 0. + if (CurrentBlock->Children){ + CurrentChild[CurrentLevel] = 0; +PARENTRECURSE: + for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){ + if (CurrentBlock->Children[i].GeomCount == 0){ + continue; + } + CurrentBlock = &CurrentBlock->Children[i]; + CurrentObject = CurrentBlock->First; + + i++; + + CurrentLevel++; + goto CHILDRECURSE; + } + } + } + + // Now lets go back to the parent so it can continue processing its other children. + if (CurrentBlock->Parent){ + CurrentBlock = CurrentBlock->Parent; + CurrentLevel--; + goto PARENTRECURSE; + } + } + else{ + CurrentBlock = &Blocks[0]; + CurrentLevel = 0; + CurrentObject = CurrentObject; + CurrentIndex = 0; + + // Other states are already set + CurrentObject = CurrentBlock->First; + } + + + if (current_geom && current_index == Index - 1){ + //current_geom = current_geom->next; // next + current_index = Index; + return current_geom; + } + else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow + getGeom(i); + }*/ + + return 0; +} + +void dxQuadTreeSpace::add(dxGeom* g){ + CHECK_NOT_LOCKED (this); + dAASSERT(g); + dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space"); + + g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + DirtyList.push(g); + + // add + g->parent_space = this; + Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block + count++; + + // enumerator has been invalidated + current_geom = 0; + + dGeomMoved(this); +} + +void dxQuadTreeSpace::remove(dxGeom* g){ + CHECK_NOT_LOCKED(this); + dAASSERT(g); + dUASSERT(g->parent_space == this,"object is not in this space"); + + // remove + ((Block*)g->tome)->DelObject(g); + count--; + + for (int i = 0; i < DirtyList.size(); i++){ + if (DirtyList[i] == g){ + DirtyList.remove(i); + // (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i + --i; + } + } + + // safeguard + g->next = 0; + g->tome = 0; + g->parent_space = 0; + + // enumerator has been invalidated + current_geom = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved(this); +} + +void dxQuadTreeSpace::dirty(dxGeom* g){ + DirtyList.push(g); +} + +void dxQuadTreeSpace::computeAABB(){ + // +} + +void dxQuadTreeSpace::cleanGeoms(){ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + + for (int i = 0; i < DirtyList.size(); i++){ + dxGeom* g = DirtyList[i]; + if (IS_SPACE(g)){ + ((dxSpace*)g)->cleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + + ((Block*)g->tome)->Traverse(g); + } + DirtyList.setSize(0); + + lock_count--; +} + +void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){ + dAASSERT(Callback); + + lock_count++; + cleanGeoms(); + + Blocks[0].Collide(UserData, Callback); + + lock_count--; +} + +void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){ + dAASSERT(g1 && Callback); + + lock_count++; + cleanGeoms(); + g1->recomputeAABB(); + + if (g1->parent_space == this){ + // The block the geom is in + Block* CurrentBlock = (Block*)g1->tome; + + // Collide against block and its children + CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback); + + // Collide against parents + while (true){ + CurrentBlock = CurrentBlock->Parent; + if (!CurrentBlock){ + break; + } + CurrentBlock->CollideLocal(g1, UserData, Callback); + } + } + else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback); + + lock_count--; +} + +dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){ + return new dxQuadTreeSpace(space, Center, Extents, Depth); +} diff --git a/libraries/ode-0.9/ode/src/collision_space.cpp b/libraries/ode-0.9/ode/src/collision_space.cpp new file mode 100644 index 0000000..1a8fc81 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_space.cpp @@ -0,0 +1,790 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +spaces + +*/ + +#include +#include +#include +#include +#include "collision_kernel.h" + +#include "collision_space_internal.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags +// and moving it to the front of the space's list. all the parents of a +// dirty geom also become dirty. + +void dGeomMoved (dxGeom *geom) +{ + dAASSERT (geom); + + // if geom is offset, mark it as needing a calculate + if (geom->offset_posr) { + geom->gflags |= GEOM_POSR_BAD; + } + + // from the bottom of the space heirarchy up, process all clean geoms + // turning them into dirty geoms. + dxSpace *parent = geom->parent_space; + + while (parent && (geom->gflags & GEOM_DIRTY)==0) { + CHECK_NOT_LOCKED (parent); + geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + parent->dirty (geom); + geom = parent; + parent = parent->parent_space; + } + + // all the remaining dirty geoms must have their AABB_BAD flags set, to + // ensure that their AABBs get recomputed + while (geom) { + geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + CHECK_NOT_LOCKED (geom->parent_space); + geom = geom->parent_space; + } +} + +#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED) + +//**************************************************************************** +// dxSpace + +dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0) +{ + count = 0; + first = 0; + cleanup = 1; + current_index = 0; + current_geom = 0; + lock_count = 0; +} + + +dxSpace::~dxSpace() +{ + CHECK_NOT_LOCKED (this); + if (cleanup) { + // note that destroying each geom will call remove() + dxGeom *g,*n; + for (g = first; g; g=n) { + n = g->next; + dGeomDestroy (g); + } + } + else { + dxGeom *g,*n; + for (g = first; g; g=n) { + n = g->next; + remove (g); + } + } +} + + +void dxSpace::computeAABB() +{ + if (first) { + int i; + dReal a[6]; + a[0] = dInfinity; + a[1] = -dInfinity; + a[2] = dInfinity; + a[3] = -dInfinity; + a[4] = dInfinity; + a[5] = -dInfinity; + for (dxGeom *g=first; g; g=g->next) { + g->recomputeAABB(); + for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i]; + for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i]; + } + memcpy(aabb,a,6*sizeof(dReal)); + } + else { + dSetZero (aabb,6); + } +} + + +void dxSpace::setCleanup (int mode) +{ + cleanup = (mode != 0); +} + + +int dxSpace::getCleanup() +{ + return cleanup; +} + + +int dxSpace::query (dxGeom *geom) +{ + dAASSERT (geom); + return (geom->parent_space == this); +} + + +int dxSpace::getNumGeoms() +{ + return count; +} + + +// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1 + +dxGeom *dxSpace::getGeom (int i) +{ + dUASSERT (i >= 0 && i < count,"index out of range"); + if (current_geom && current_index == i-1) { + current_geom = current_geom->next; + current_index = i; + return current_geom; + } + else { + dxGeom *g=first; + for (int j=0; jnext; else return 0; + } + current_geom = g; + current_index = i; + return g; + } +} + + +void dxSpace::add (dxGeom *geom) +{ + CHECK_NOT_LOCKED (this); + dAASSERT (geom); + dUASSERT (geom->parent_space == 0 && geom->next == 0, + "geom is already in a space"); + + // add + geom->parent_space = this; + geom->spaceAdd (&first); + count++; + + // enumerator has been invalidated + current_geom = 0; + + // new geoms are added to the front of the list and are always + // considered to be dirty. as a consequence, this space and all its + // parents are dirty too. + geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + dGeomMoved (this); +} + + +void dxSpace::remove (dxGeom *geom) +{ + CHECK_NOT_LOCKED (this); + dAASSERT (geom); + dUASSERT (geom->parent_space == this,"object is not in this space"); + + // remove + geom->spaceRemove(); + count--; + + // safeguard + geom->next = 0; + geom->tome = 0; + geom->parent_space = 0; + + // enumerator has been invalidated + current_geom = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved (this); +} + + +void dxSpace::dirty (dxGeom *geom) +{ + geom->spaceRemove(); + geom->spaceAdd (&first); +} + +//**************************************************************************** +// simple space - reports all n^2 object intersections + +struct dxSimpleSpace : public dxSpace { + dxSimpleSpace (dSpaceID _space); + void cleanGeoms(); + void collide (void *data, dNearCallback *callback); + void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + +dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space) +{ + type = dSimpleSpaceClass; +} + + +void dxSimpleSpace::cleanGeoms() +{ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) { + if (IS_SPACE(g)) { + ((dxSpace*)g)->cleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + } + lock_count--; +} + + +void dxSimpleSpace::collide (void *data, dNearCallback *callback) +{ + dAASSERT (callback); + + lock_count++; + cleanGeoms(); + + // intersect all bounding boxes + for (dxGeom *g1=first; g1; g1=g1->next) { + if (GEOM_ENABLED(g1)){ + for (dxGeom *g2=g1->next; g2; g2=g2->next) { + if (GEOM_ENABLED(g2)){ + collideAABBs (g1,g2,data,callback); + } + } + } + } + + lock_count--; +} + + +void dxSimpleSpace::collide2 (void *data, dxGeom *geom, + dNearCallback *callback) +{ + dAASSERT (geom && callback); + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + if (GEOM_ENABLED(g)){ + collideAABBs (g,geom,data,callback); + } + } + + lock_count--; +} + +//**************************************************************************** +// utility stuff for hash table space + +// kind of silly, but oh well... +#ifndef MAXINT +#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1)) +#endif + + +// prime[i] is the largest prime smaller than 2^i +#define NUM_PRIMES 31 +static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L, + 1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L, + 524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L, + 67108859L,134217689L,268435399L,536870909L,1073741789L}; + + +// an axis aligned bounding box in the hash table +struct dxAABB { + dxAABB *next; // next in the list of all AABBs + int level; // the level this is stored in (cell size = 2^level) + int dbounds[6]; // AABB bounds, discretized to cell size + dxGeom *geom; // corresponding geometry object (AABB stored here) + int index; // index of this AABB, starting from 0 +}; + + +// a hash table node that represents an AABB that intersects a particular cell +// at a particular level +struct Node { + Node *next; // next node in hash table collision list, 0 if none + int x,y,z; // cell position in space, discretized to cell size + dxAABB *aabb; // axis aligned bounding box that intersects this cell +}; + + +// return the `level' of an AABB. the AABB will be put into cells at this +// level - the cell size will be 2^level. the level is chosen to be the +// smallest value such that the AABB occupies no more than 8 cells, regardless +// of its placement. this means that: +// size/2 < q <= size +// where q is the maximum AABB dimension. + +static int findLevel (dReal bounds[6]) +{ + if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity || + bounds[2] <= -dInfinity || bounds[3] >= dInfinity || + bounds[4] <= -dInfinity || bounds[5] >= dInfinity) { + return MAXINT; + } + + // compute q + dReal q,q2; + q = bounds[1] - bounds[0]; // x bounds + q2 = bounds[3] - bounds[2]; // y bounds + if (q2 > q) q = q2; + q2 = bounds[5] - bounds[4]; // z bounds + if (q2 > q) q = q2; + + // find level such that 0.5 * 2^level < q <= 2^level + int level; + frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp) + return level; +} + + +// find a virtual memory address for a cell at the given level and x,y,z +// position. +// @@@ currently this is not very sophisticated, e.g. the scaling +// factors could be better designed to avoid collisions, and they should +// probably depend on the hash table physical size. + +static unsigned long getVirtualAddress (int level, int x, int y, int z) +{ + return level*1000 + x*100 + y*10 + z; +} + +//**************************************************************************** +// hash space + +struct dxHashSpace : public dxSpace { + int global_minlevel; // smallest hash table level to put AABBs in + int global_maxlevel; // objects that need a level larger than this will be + // put in a "big objects" list instead of a hash table + + dxHashSpace (dSpaceID _space); + void setLevels (int minlevel, int maxlevel); + void getLevels (int *minlevel, int *maxlevel); + void cleanGeoms(); + void collide (void *data, dNearCallback *callback); + void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + +dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space) +{ + type = dHashSpaceClass; + global_minlevel = -3; + global_maxlevel = 10; +} + + +void dxHashSpace::setLevels (int minlevel, int maxlevel) +{ + dAASSERT (minlevel <= maxlevel); + global_minlevel = minlevel; + global_maxlevel = maxlevel; +} + + +void dxHashSpace::getLevels (int *minlevel, int *maxlevel) +{ + if (minlevel) *minlevel = global_minlevel; + if (maxlevel) *maxlevel = global_maxlevel; +} + + +void dxHashSpace::cleanGeoms() +{ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) { + if (IS_SPACE(g)) { + ((dxSpace*)g)->cleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + } + lock_count--; +} + + +void dxHashSpace::collide (void *data, dNearCallback *callback) +{ + dAASSERT(this && callback); + dxGeom *geom; + dxAABB *aabb; + int i,maxlevel; + + // 0 or 1 geoms can't collide with anything + if (count < 2) return; + + lock_count++; + cleanGeoms(); + + // create a list of auxiliary information for all geom axis aligned bounding + // boxes. set the level for all AABBs. put AABBs larger than the space's + // global_maxlevel in the big_boxes list, check everything else against + // that list at the end. for AABBs that are not too big, record the maximum + // level that we need. + + int n = 0; // number of AABBs in main list + dxAABB *first_aabb = 0; // list of AABBs in hash table + dxAABB *big_boxes = 0; // list of AABBs too big for hash table + maxlevel = global_minlevel - 1; + for (geom = first; geom; geom=geom->next) { + if (!GEOM_ENABLED(geom)){ + continue; + } + dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB)); + aabb->geom = geom; + // compute level, but prevent cells from getting too small + int level = findLevel (geom->aabb); + if (level < global_minlevel) level = global_minlevel; + if (level <= global_maxlevel) { + // aabb goes in main list + aabb->next = first_aabb; + first_aabb = aabb; + aabb->level = level; + if (level > maxlevel) maxlevel = level; + // cellsize = 2^level + dReal cellsize = (dReal) ldexp (1.0,level); + // discretize AABB position to cell size + for (i=0; i < 6; i++) aabb->dbounds[i] = (int) + floor (geom->aabb[i]/cellsize); + // set AABB index + aabb->index = n; + n++; + } + else { + // aabb is too big, put it in the big_boxes list. we don't care about + // setting level, dbounds, index, or the maxlevel + aabb->next = big_boxes; + big_boxes = aabb; + } + } + + // for `n' objects, an n*n array of bits is used to record if those objects + // have been intersection-tested against each other yet. this array can + // grow large with high n, but oh well... + int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits + unsigned char *tested = (unsigned char *) ALLOCA (n * tested_rowsize); + memset (tested,0,n * tested_rowsize); + + // create a hash table to store all AABBs. each AABB may take up to 8 cells. + // we use chaining to resolve collisions, but we use a relatively large table + // to reduce the chance of collisions. + + // compute hash table size sz to be a prime > 8*n + for (i=0; i= (8*n)) break; + } + if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless + int sz = prime[i]; + + // allocate and initialize hash table node pointers + Node **table = (Node **) ALLOCA (sizeof(Node*) * sz); + for (i=0; inext) { + int *dbounds = aabb->dbounds; + for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) { + for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) { + for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz; + // add a new node to the hash table + Node *node = (Node*) ALLOCA (sizeof (Node)); + node->x = xi; + node->y = yi; + node->z = zi; + node->aabb = aabb; + node->next = table[hi]; + table[hi] = node; + } + } + } + } + + // now that all AABBs are loaded into the hash table, we do the actual + // collision detection. for all AABBs, check for other AABBs in the + // same cells for collisions, and then check for other AABBs in all + // intersecting higher level cells. + + int db[6]; // discrete bounds at current level + for (aabb=first_aabb; aabb; aabb=aabb->next) { + // we are searching for collisions with aabb + for (i=0; i<6; i++) db[i] = aabb->dbounds[i]; + for (int level = aabb->level; level <= maxlevel; level++) { + for (int xi = db[0]; xi <= db[1]; xi++) { + for (int yi = db[2]; yi <= db[3]; yi++) { + for (int zi = db[4]; zi <= db[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; + // search all nodes at this index + Node *node; + for (node = table[hi]; node; node=node->next) { + // node points to an AABB that may intersect aabb + if (node->aabb == aabb) continue; + if (node->aabb->level == level && + node->x == xi && node->y == yi && node->z == zi) { + // see if aabb and node->aabb have already been tested + // against each other + unsigned char mask; + if (aabb->index <= node->aabb->index) { + i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3); + mask = 1 << (node->aabb->index & 7); + } + else { + i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3); + mask = 1 << (aabb->index & 7); + } + dIASSERT (i >= 0 && i < (tested_rowsize*n)); + if ((tested[i] & mask)==0) { + collideAABBs (aabb->geom,node->aabb->geom,data,callback); + } + tested[i] |= mask; + } + } + } + } + } + // get the discrete bounds for the next level up + for (i=0; i<6; i++) db[i] >>= 1; + } + } + + // every AABB in the normal list must now be intersected against every + // AABB in the big_boxes list. so let's hope there are not too many objects + // in the big_boxes list. + for (aabb=first_aabb; aabb; aabb=aabb->next) { + for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) { + collideAABBs (aabb->geom,aabb2->geom,data,callback); + } + } + + // intersected all AABBs in the big_boxes list together + for (aabb=big_boxes; aabb; aabb=aabb->next) { + for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) { + collideAABBs (aabb->geom,aabb2->geom,data,callback); + } + } + + lock_count--; +} + + +void dxHashSpace::collide2 (void *data, dxGeom *geom, + dNearCallback *callback) +{ + dAASSERT (geom && callback); + + // this could take advantage of the hash structure to avoid + // O(n2) complexity, but it does not yet. + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + if (GEOM_ENABLED(g)) collideAABBs (g,geom,data,callback); + } + + lock_count--; +} + +//**************************************************************************** +// space functions + +dxSpace *dSimpleSpaceCreate (dxSpace *space) +{ + return new dxSimpleSpace (space); +} + + +dxSpace *dHashSpaceCreate (dxSpace *space) +{ + return new dxHashSpace (space); +} + + +void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel) +{ + dAASSERT (space); + dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel"); + dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space"); + dxHashSpace *hspace = (dxHashSpace*) space; + hspace->setLevels (minlevel,maxlevel); +} + + +void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel) +{ + dAASSERT (space); + dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space"); + dxHashSpace *hspace = (dxHashSpace*) space; + hspace->getLevels (minlevel,maxlevel); +} + + +void dSpaceDestroy (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + dGeomDestroy (space); +} + + +void dSpaceSetCleanup (dxSpace *space, int mode) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->setCleanup (mode); +} + + +int dSpaceGetCleanup (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getCleanup(); +} + + +void dSpaceAdd (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + CHECK_NOT_LOCKED (space); + space->add (g); +} + + +void dSpaceRemove (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + CHECK_NOT_LOCKED (space); + space->remove (g); +} + + +int dSpaceQuery (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->query (g); +} + +void dSpaceClean (dxSpace *space){ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + + space->cleanGeoms(); +} + +int dSpaceGetNumGeoms (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getNumGeoms(); +} + + +dGeomID dSpaceGetGeom (dxSpace *space, int i) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getGeom (i); +} + + +void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback) +{ + dAASSERT (space && callback); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->collide (data,callback); +} + + +void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data, + dNearCallback *callback) +{ + dAASSERT (g1 && g2 && callback); + dxSpace *s1,*s2; + + // see if either geom is a space + if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0; + if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0; + + // handle the four space/geom cases + if (s1) { + if (s2) { + // g1 and g2 are spaces. + if (s1==s2) { + // collide a space with itself --> interior collision + s1->collide (data,callback); + } + else { + // iterate through the space that has the fewest geoms, calling + // collide2 in the other space for each one. + if (s1->count < s2->count) { + for (dxGeom *g = s1->first; g; g=g->next) { + s2->collide2 (data,g,callback); + } + } + else { + for (dxGeom *g = s2->first; g; g=g->next) { + s1->collide2 (data,g,callback); + } + } + } + } + else { + // g1 is a space, g2 is a geom + s1->collide2 (data,g2,callback); + } + } + else { + if (s2) { + // g1 is a geom, g2 is a space + s2->collide2 (data,g1,callback); + } + else { + // g1 and g2 are geoms, call the callback directly + callback (data,g1,g2); + } + } +} diff --git a/libraries/ode-0.9/ode/src/collision_space_internal.h b/libraries/ode-0.9/ode/src/collision_space_internal.h new file mode 100644 index 0000000..004276a --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_space_internal.h @@ -0,0 +1,84 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +stuff common to all spaces + +*/ + +#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_ +#define _ODE_COLLISION_SPACE_INTERNAL_H_ + +#define ALLOCA(x) dALLOCA16(x) + +#define CHECK_NOT_LOCKED(space) \ + dUASSERT ((space)==0 || (space)->lock_count==0, \ + "invalid operation for locked space"); + + +// collide two geoms together. for the hash table space, this is +// called if the two AABBs inhabit the same hash table cells. +// this only calls the callback function if the AABBs actually +// intersect. if a geom has an AABB test function, that is called to +// provide a further refinement of the intersection. +// +// NOTE: this assumes that the geom AABBs are valid on entry +// and that both geoms are enabled. + +static void collideAABBs (dxGeom *g1, dxGeom *g2, + void *data, dNearCallback *callback) +{ + dIASSERT((g1->gflags & GEOM_AABB_BAD)==0); + dIASSERT((g2->gflags & GEOM_AABB_BAD)==0); + + // no contacts if both geoms on the same body, and the body is not 0 + if (g1->body == g2->body && g1->body) return; + + // test if the category and collide bitfields match + if ( ((g1->category_bits & g2->collide_bits) || + (g2->category_bits & g1->collide_bits)) == 0) { + return; + } + + // if the bounding boxes are disjoint then don't do anything + dReal *bounds1 = g1->aabb; + dReal *bounds2 = g2->aabb; + 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]) { + return; + } + + // check if either object is able to prove that it doesn't intersect the + // AABB of the other + if (g1->AABBTest (g2,bounds2) == 0) return; + if (g2->AABBTest (g1,bounds1) == 0) return; + + // the objects might actually intersect - call the space callback function + callback (data,g1,g2); +} + +#endif diff --git a/libraries/ode-0.9/ode/src/collision_std.h b/libraries/ode-0.9/ode/src/collision_std.h new file mode 100644 index 0000000..d203ad0 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_std.h @@ -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. * + * * + *************************************************************************/ + +/* + +the standard ODE geometry primitives. + +*/ + +#ifndef _ODE_COLLISION_STD_H_ +#define _ODE_COLLISION_STD_H_ + +#include +#include +#include "collision_kernel.h" + + +// primitive collision functions - these have the dColliderFn interface, i.e. +// the same interface as dCollide(). the first and second geom arguments must +// have the specified types. + +int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayCylinder (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); + +// Cylinder - Box/Sphere by (C) CroTeam +// Ported by Nguyen Binh +int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, + int flags, dContactGeom *contact, int skip); +int dCollideCylinderPlane(dxGeom *gCylinder, dxGeom *gPlane, + int flags, dContactGeom *contact, int skip); + +//--> Convex Collision +int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +//<-- Convex Collision + +// dHeightfield +int dCollideHeightfield( dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip ); + +//**************************************************************************** +// the basic geometry objects + +struct dxSphere : public dxGeom { + dReal radius; // sphere radius + dxSphere (dSpaceID space, dReal _radius); + void computeAABB(); +}; + + +struct dxBox : public dxGeom { + dVector3 side; // side lengths (x,y,z) + dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz); + void computeAABB(); +}; + + +struct dxCapsule : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCapsule (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + + +struct dxCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + + +struct dxPlane : public dxGeom { + dReal p[4]; + dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); + void computeAABB(); +}; + + +struct dxRay : public dxGeom { + dReal length; + dxRay (dSpaceID space, dReal _length); + void computeAABB(); +}; + +typedef std::pair edge; /*!< Used to descrive a convex hull edge, an edge is a pair or indices into the hull's points */ +struct dxConvex : public dxGeom +{ + + dReal *planes; /*!< An array of planes in the form: + normal X, normal Y, normal Z,Distance + */ + dReal *points; /*!< An array of points X,Y,Z */ + unsigned int *polygons; /*! An array of indices to the points of each polygon, it should be the number of vertices followed by that amount of indices to "points" in counter clockwise order*/ + unsigned int planecount; /*!< Amount of planes in planes */ + unsigned int pointcount;/*!< Amount of points in points */ + dReal saabb[6];/*!< Static AABB */ + std::set edges; + dxConvex(dSpaceID space, + dReal *planes, + unsigned int planecount, + dReal *points, + unsigned int pointcount, + unsigned int *polygons); + ~dxConvex() + { + //fprintf(stdout,"dxConvex Destroy\n"); + } + void computeAABB(); + private: + // For Internal Use Only + void FillEdges(); +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/collision_transform.cpp b/libraries/ode-0.9/ode/src/collision_transform.cpp new file mode 100644 index 0000000..85fd623 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_transform.cpp @@ -0,0 +1,232 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +geom transform + +*/ + +#include +#include +#include +#include +#include "collision_transform.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// dxGeomTransform class + +struct dxGeomTransform : public dxGeom { + dxGeom *obj; // object that is being transformed + int cleanup; // 1 to destroy obj when destroyed + int infomode; // 1 to put Tx geom in dContactGeom g1 + + // cached final object transform (body tx + relative tx). this is set by + // computeAABB(), and it is valid while the AABB is valid. + dxPosR transform_posr; + + dxGeomTransform (dSpaceID space); + ~dxGeomTransform(); + void computeAABB(); + void computeFinalTx(); +}; +/* +void RunMe() +{ + printf("sizeof body = %i\n", sizeof(dxBody)); + printf("sizeof geom = %i\n", sizeof(dxGeom)); + printf("sizeof geomtransform = %i\n", sizeof(dxGeomTransform)); + printf("sizeof posr = %i\n", sizeof(dxPosR)); +} +*/ + +dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1) +{ + type = dGeomTransformClass; + obj = 0; + cleanup = 0; + infomode = 0; + dSetZero (transform_posr.pos,4); + dRSetIdentity (transform_posr.R); +} + + +dxGeomTransform::~dxGeomTransform() +{ + if (obj && cleanup) delete obj; +} + + +void dxGeomTransform::computeAABB() +{ + if (!obj) { + dSetZero (aabb,6); + return; + } + + // backup the relative pos and R pointers of the encapsulated geom object + dxPosR* posr_bak = obj->final_posr; + + // compute temporary pos and R for the encapsulated geom object + computeFinalTx(); + obj->final_posr = &transform_posr; + + // compute the AABB + obj->computeAABB(); + memcpy (aabb,obj->aabb,6*sizeof(dReal)); + + // restore the pos and R + obj->final_posr = posr_bak; +} + + +// utility function for dCollideTransform() : compute final pos and R +// for the encapsulated geom object + +void dxGeomTransform::computeFinalTx() +{ + dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos); + transform_posr.pos[0] += final_posr->pos[0]; + transform_posr.pos[1] += final_posr->pos[1]; + transform_posr.pos[2] += final_posr->pos[2]; + dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R); +} + +//**************************************************************************** +// collider function: +// this collides a transformed geom with another geom. the other geom can +// also be a transformed geom, but this case is not handled specially. + +int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dGeomTransformClass); + + dxGeomTransform *tr = (dxGeomTransform*) o1; + if (!tr->obj) return 0; + dUASSERT (tr->obj->parent_space==0, + "GeomTransform encapsulated object must not be in a space"); + dUASSERT (tr->obj->body==0, + "GeomTransform encapsulated object must not be attached " + "to a body"); + + // backup the relative pos and R pointers of the encapsulated geom object, + // and the body pointer + dxPosR *posr_bak = tr->obj->final_posr; + dxBody *bodybak = tr->obj->body; + + // compute temporary pos and R for the encapsulated geom object. + // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag, + // because computeFinalTx() will have already been called in + // dxGeomTransform::computeAABB() + + if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx(); + tr->obj->final_posr = &tr->transform_posr; + tr->obj->body = o1->body; + + // do the collision + int n = dCollide (tr->obj,o2,flags,contact,skip); + + // if required, adjust the 'g1' values in the generated contacts so that + // thay indicated the GeomTransform object instead of the encapsulated + // object. + if (tr->infomode) { + for (int i=0; ig1 = o1; + } + } + + // restore the pos, R and body + tr->obj->final_posr = posr_bak; + tr->obj->body = bodybak; + return n; +} + +//**************************************************************************** +// public API + +dGeomID dCreateGeomTransform (dSpaceID space) +{ + return new dxGeomTransform (space); +} + + +void dGeomTransformSetGeom (dGeomID g, dGeomID obj) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + if (tr->obj && tr->cleanup) delete tr->obj; + tr->obj = obj; +} + + +dGeomID dGeomTransformGetGeom (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->obj; +} + + +void dGeomTransformSetCleanup (dGeomID g, int mode) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + tr->cleanup = mode; +} + + +int dGeomTransformGetCleanup (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->cleanup; +} + + +void dGeomTransformSetInfo (dGeomID g, int mode) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + tr->infomode = mode; +} + + +int dGeomTransformGetInfo (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->infomode; +} diff --git a/libraries/ode-0.9/ode/src/collision_transform.h b/libraries/ode-0.9/ode/src/collision_transform.h new file mode 100644 index 0000000..406a687 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_transform.h @@ -0,0 +1,40 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +geom transform + +*/ + +#ifndef _ODE_COLLISION_TRANSFORM_H_ +#define _ODE_COLLISION_TRANSFORM_H_ + +#include +#include "collision_kernel.h" + + +int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); + + +#endif diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp new file mode 100644 index 0000000..0194d02 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp @@ -0,0 +1,1497 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + + +/************************************************************************* + * * + * Triangle-box collider by Alen Ladavac and Vedran Klanac. * + * Ported to ODE by Oskari Nyman. * + * * + *************************************************************************/ + + +#include +#include +#include +#include +#include "collision_util.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_ENABLED + + +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxGeom* in_g1, dxGeom* in_g2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount); + + +// largest number, double or float +#if defined(dSINGLE) + #define MAXVALUE FLT_MAX +#else + #define MAXVALUE DBL_MAX +#endif + + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) do{ \ + (r)[0]=(a)[0] - (b)[0]; \ + (r)[1]=(a)[1] - (b)[1]; \ + (r)[2]=(a)[2] - (b)[2]; }while(0) + + +// dVector3 +// a=b +#define SET(a,b) do{ \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; }while(0) + + +// dMatrix3 +// a=b +#define SETM(a,b) do{ \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; \ + (a)[3]=(b)[3]; \ + (a)[4]=(b)[4]; \ + (a)[5]=(b)[5]; \ + (a)[6]=(b)[6]; \ + (a)[7]=(b)[7]; \ + (a)[8]=(b)[8]; \ + (a)[9]=(b)[9]; \ + (a)[10]=(b)[10]; \ + (a)[11]=(b)[11]; }while(0) + + +// dVector3 +// r=a+b +#define ADD(a,b,r) do{ \ + (r)[0]=(a)[0] + (b)[0]; \ + (r)[1]=(a)[1] + (b)[1]; \ + (r)[2]=(a)[2] + (b)[2]; }while(0) + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) do{ \ + (v)[0]=(m)[(a)+0]; \ + (v)[1]=(m)[(a)+4]; \ + (v)[2]=(m)[(a)+8]; }while(0) + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) \ + ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ) + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) do{ \ + plane[0]=normal[0];\ + plane[1]=normal[1];\ + plane[2]=normal[2];\ + plane[3]=d; }while(0) + + +// dVector3 +// length of vector a +#define LENGTHOF(a) \ + dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]) + + +// box data +static dMatrix3 mHullBoxRot; +static dVector3 vHullBoxPos; +static dVector3 vBoxHalfSize; + +// mesh data +static dVector3 vHullDstPos; + +// global collider data +static dVector3 vBestNormal; +static dReal fBestDepth; +static int iBestAxis = 0; +static int iExitAxis = 0; +static dVector3 vE0, vE1, vE2, vN; + +// global info for contact creation +static int iFlags; +static dContactGeom *ContactGeoms; +static int iStride; +static dxGeom *Geom1; +static dxGeom *Geom2; +static int ctContacts = 0; + + + +// Test normal of mesh face as separating axis for intersection +static bool _cldTestNormal( dReal fp0, dReal fR, dVector3 vNormal, int iAxis ) +{ + // calculate overlapping interval of box and triangle + dReal fDepth = fR+fp0; + + // if we do not overlap + if ( fDepth<0 ) { + // do nothing + return false; + } + + // calculate normal's length + dReal fLength = LENGTHOF(vNormal); + // if long enough + if ( fLength > 0.0f ) { + + dReal fOneOverLength = 1.0f/fLength; + // normalize depth + fDepth = fDepth*fOneOverLength; + + // get minimum depth + if (fDepth=0); + fBestDepth = fDepth; + } + + } + + return true; +} + + + + +// Test box axis as separating axis +static bool _cldTestFace( dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD, + dVector3 vNormal, int iAxis ) +{ + dReal fMin, fMax; + + // find min of triangle interval + if ( fp0 < fp1 ) { + if ( fp0 < fp2 ) { + fMin = fp0; + } else { + fMin = fp2; + } + } else { + if( fp1 < fp2 ) { + fMin = fp1; + } else { + fMin = fp2; + } + } + + // find max of triangle interval + if ( fp0 > fp1 ) { + if ( fp0 > fp2 ) { + fMax = fp0; + } else { + fMax = fp2; + } + } else { + if( fp1 > fp2 ) { + fMax = fp1; + } else { + fMax = fp2; + } + } + + // calculate minimum and maximum depth + dReal fDepthMin = fR - fMin; + dReal fDepthMax = fMax + fR; + + // if we dont't have overlapping interval + if ( fDepthMin < 0 || fDepthMax < 0 ) { + // do nothing + return false; + } + + dReal fDepth = 0; + + // if greater depth is on negative side + if ( fDepthMin > fDepthMax ) { + // use smaller depth (one from positive side) + fDepth = fDepthMax; + // flip normal direction + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + fD = -fD; + // if greater depth is on positive side + } else { + // use smaller depth (one from negative side) + fDepth = fDepthMin; + } + + + // if lower depth than best found so far + if (fDepth=0); + fBestDepth = fDepth; + } + + return true; +} + + + + + +// Test cross products of box axis and triangle edges as separating axis +static bool _cldTestEdge( dReal fp0, dReal fp1, dReal fR, dReal fD, + dVector3 vNormal, int iAxis ) +{ + dReal fMin, fMax; + + + // ===== Begin Patch by Francisco Leon, 2006/10/28 ===== + + // Fixed Null Normal. This prevents boxes passing + // through trimeshes at certain contact angles + + fMin = vNormal[0] * vNormal[0] + + vNormal[1] * vNormal[1] + + vNormal[2] * vNormal[2]; + + if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS + return true; + + // ===== Ending Patch by Francisco Leon ===== + + + // calculate min and max interval values + if ( fp0 < fp1 ) { + fMin = fp0; + fMax = fp1; + } else { + fMin = fp1; + fMax = fp0; + } + + // check if we overlapp + dReal fDepthMin = fR - fMin; + dReal fDepthMax = fMax + fR; + + // if we don't overlapp + if ( fDepthMin < 0 || fDepthMax < 0 ) { + // do nothing + return false; + } + + dReal fDepth; + + + // if greater depth is on negative side + if ( fDepthMin > fDepthMax ) { + // use smaller depth (one from positive side) + fDepth = fDepthMax; + // flip normal direction + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + fD = -fD; + // if greater depth is on positive side + } else { + // use smaller depth (one from negative side) + fDepth = fDepthMin; + } + + // calculate normal's length + dReal fLength = LENGTHOF(vNormal); + + // if long enough + if ( fLength > 0.0f ) { + + // normalize depth + dReal fOneOverLength = 1.0f/fLength; + fDepth = fDepth*fOneOverLength; + fD*=fOneOverLength; + + + // if lower depth than best found so far (favor face over edges) + if (fDepth*1.5f=0); + fBestDepth = fDepth; + } + } + + return true; +} + + + + + +// clip polygon with plane and generate new polygon points +static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1= 0 ) { + // emit point + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + +} + + + + +static bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { + // reset best axis + iBestAxis = 0; + iExitAxis = -1; + fBestDepth = MAXVALUE; + + // calculate edges + SUBTRACT(v1,v0,vE0); + SUBTRACT(v2,v0,vE1); + SUBTRACT(vE1,vE0,vE2); + + // calculate poly normal + dCROSS(vN,=,vE0,vE1); + + // extract box axes as vectors + dVector3 vA0,vA1,vA2; + GETCOL(mHullBoxRot,0,vA0); + GETCOL(mHullBoxRot,1,vA1); + GETCOL(mHullBoxRot,2,vA2); + + // box halfsizes + dReal fa0 = vBoxHalfSize[0]; + dReal fa1 = vBoxHalfSize[1]; + dReal fa2 = vBoxHalfSize[2]; + + // calculate relative position between box and triangle + dVector3 vD; + SUBTRACT(v0,vHullBoxPos,vD); + + // calculate length of face normal + dReal fNLen = LENGTHOF( vN ); + + dVector3 vL; + dReal fp0, fp1, fp2, fR, fD; + + // Test separating axes for intersection + // ************************************************ + // Axis 1 - Triangle Normal + SET(vL,vN); + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0; + fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) ); + + + if( !_cldTestNormal( fp0, fR, vL, 1) ) { + iExitAxis=1; + return false; + } + + // ************************************************ + + // Test Faces + // ************************************************ + // Axis 2 - Box X-Axis + SET(vL,vA0); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 + dDOT(vA0,vE0); + fp2 = fp0 + dDOT(vA0,vE1); + fR = fa0; + + + if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) { + iExitAxis=2; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 3 - Box Y-Axis + SET(vL,vA1); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 + dDOT(vA1,vE0); + fp2 = fp0 + dDOT(vA1,vE1); + fR = fa1; + + + if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) { + iExitAxis=3; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 4 - Box Z-Axis + SET(vL,vA2); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 + dDOT(vA2,vE0); + fp2 = fp0 + dDOT(vA2,vE1); + fR = fa2; + + + if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) { + iExitAxis=4; + return false; + } + + // ************************************************ + + // Test Edges + // ************************************************ + // Axis 5 - Box X-Axis cross Edge0 + dCROSS(vL,=,vA0,vE0); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0 + dDOT(vA0,vN); + fR = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0)); + + + if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) { + iExitAxis=5; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 6 - Box X-Axis cross Edge1 + dCROSS(vL,=,vA0,vE1); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA0,vN); + fp2 = fp0; + fR = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1)); + + + if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) { + iExitAxis=6; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 7 - Box X-Axis cross Edge2 + dCROSS(vL,=,vA0,vE2); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA0,vN); + fp2 = fp0 - dDOT(vA0,vN); + fR = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2)); + + + if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) { + iExitAxis=7; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 8 - Box Y-Axis cross Edge0 + dCROSS(vL,=,vA1,vE0); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0 + dDOT(vA1,vN); + fR = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0)); + + + if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) { + iExitAxis=8; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 9 - Box Y-Axis cross Edge1 + dCROSS(vL,=,vA1,vE1); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA1,vN); + fp2 = fp0; + fR = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1)); + + + if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) { + iExitAxis=9; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 10 - Box Y-Axis cross Edge2 + dCROSS(vL,=,vA1,vE2); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA1,vN); + fp2 = fp0 - dDOT(vA1,vN); + fR = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2)); + + + if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) { + iExitAxis=10; + return false; + } + + // ************************************************ + + // ************************************************ + // Axis 11 - Box Z-Axis cross Edge0 + dCROSS(vL,=,vA2,vE0); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0 + dDOT(vA2,vN); + fR = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0)); + + + if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) { + iExitAxis=11; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 12 - Box Z-Axis cross Edge1 + dCROSS(vL,=,vA2,vE1); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA2,vN); + fp2 = fp0; + fR = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1)); + + + if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) { + iExitAxis=12; + return false; + } + // ************************************************ + + // ************************************************ + // Axis 13 - Box Z-Axis cross Edge2 + dCROSS(vL,=,vA2,vE2); + fD = dDOT(vL,vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA2,vN); + fp2 = fp0 - dDOT(vA2,vN); + fR = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2)); + + + if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) { + iExitAxis=13; + return false; + } + + // ************************************************ + return true; +} + + + + + +// find two closest points on two lines +static bool _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1, + dVector3 vPoint2, dVector3 vLenVec2, + dReal &fvalue1, dReal &fvalue2) +{ + // calulate denominator + dVector3 vp; + SUBTRACT(vPoint2,vPoint1,vp); + dReal fuaub = dDOT(vLenVec1,vLenVec2); + dReal fq1 = dDOT(vLenVec1,vp); + dReal fq2 = -dDOT(vLenVec2,vp); + dReal fd = 1.0f - fuaub * fuaub; + + // if denominator is positive + if (fd > 0.0f) { + // calculate points of closest approach + fd = 1.0f/fd; + fvalue1 = (fq1 + fuaub*fq2)*fd; + fvalue2 = (fuaub*fq1 + fq2)*fd; + return true; + // otherwise + } else { + // lines are parallel + fvalue1 = 0.0f; + fvalue2 = 0.0f; + return false; + } + +} + + + + + +// clip and generate contacts +static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { + dIASSERT( !(iFlags & CONTACTS_UNIMPORTANT) || ctContacts < (iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results + + // if we have edge/edge intersection + if ( iBestAxis > 4 ) { + + dVector3 vub,vPb,vPa; + + SET(vPa,vHullBoxPos); + + // calculate point on box edge + for( int i=0; i<3; i++) { + dVector3 vRotCol; + GETCOL(mHullBoxRot,i,vRotCol); + dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f; + + vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0]; + vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1]; + vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2]; + } + + int iEdge = (iBestAxis-5)%3; + + // decide which edge is on triangle + if ( iEdge == 0 ) { + SET(vPb,v0); + SET(vub,vE0); + } else if ( iEdge == 1) { + SET(vPb,v2); + SET(vub,vE1); + } else { + SET(vPb,v1); + SET(vub,vE2); + } + + + // setup direction parameter for face edge + dNormalize3(vub); + + dReal fParam1, fParam2; + + // setup direction parameter for box edge + dVector3 vua; + int col=(iBestAxis-5)/3; + GETCOL(mHullBoxRot,col,vua); + + // find two closest points on both edges + _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 ); + vPa[0] += vua[0]*fParam1; + vPa[1] += vua[1]*fParam1; + vPa[2] += vua[2]*fParam1; + + vPb[0] += vub[0]*fParam2; + vPb[1] += vub[1]*fParam2; + vPb[2] += vub[2]*fParam2; + + // calculate collision point + dVector3 vPntTmp; + ADD(vPa,vPb,vPntTmp); + + vPntTmp[0]*=0.5f; + vPntTmp[1]*=0.5f; + vPntTmp[2]*=0.5f; + + // generate contact point between two closest points +#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else + dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); + Contact->depth = fBestDepth; + SET(Contact->normal,vBestNormal); + SET(Contact->pos,vPntTmp); + Contact->g1 = Geom1; + Contact->g2 = Geom2; + ctContacts++; +#endif + GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, + vPntTmp, vBestNormal, fBestDepth, ctContacts); + + + // if triangle is the referent face then clip box to triangle face + } else if ( iBestAxis == 1 ) { + + + dVector3 vNormal2; + vNormal2[0]=-vBestNormal[0]; + vNormal2[1]=-vBestNormal[1]; + vNormal2[2]=-vBestNormal[2]; + + + // vNr is normal in box frame, pointing from triangle to box + dMatrix3 mTransposed; + mTransposed[0*4+0]=mHullBoxRot[0*4+0]; + mTransposed[0*4+1]=mHullBoxRot[1*4+0]; + mTransposed[0*4+2]=mHullBoxRot[2*4+0]; + + mTransposed[1*4+0]=mHullBoxRot[0*4+1]; + mTransposed[1*4+1]=mHullBoxRot[1*4+1]; + mTransposed[1*4+2]=mHullBoxRot[2*4+1]; + + mTransposed[2*4+0]=mHullBoxRot[0*4+2]; + mTransposed[2*4+1]=mHullBoxRot[1*4+2]; + mTransposed[2*4+2]=mHullBoxRot[2*4+2]; + + dVector3 vNr; + vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2]; + vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2]; + vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2]; + + + dVector3 vAbsNormal; + vAbsNormal[0] = dFabs( vNr[0] ); + vAbsNormal[1] = dFabs( vNr[1] ); + vAbsNormal[2] = dFabs( vNr[2] ); + + // get closest face from box + int iB0, iB1, iB2; + if (vAbsNormal[1] > vAbsNormal[0]) { + if (vAbsNormal[1] > vAbsNormal[2]) { + iB1 = 0; iB0 = 1; iB2 = 2; + } else { + iB1 = 0; iB2 = 1; iB0 = 2; + } + } else { + + if (vAbsNormal[0] > vAbsNormal[2]) { + iB0 = 0; iB1 = 1; iB2 = 2; + } else { + iB1 = 0; iB2 = 1; iB0 = 2; + } + } + + // Here find center of box face we are going to project + dVector3 vCenter; + dVector3 vRotCol; + GETCOL(mHullBoxRot,iB0,vRotCol); + + if (vNr[iB0] > 0) { + vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0]; + vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1]; + vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2]; + } else { + vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0]; + vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1]; + vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2]; + } + + // Here find 4 corner points of box + dVector3 avPoints[4]; + + dVector3 vRotCol2; + GETCOL(mHullBoxRot,iB1,vRotCol); + GETCOL(mHullBoxRot,iB2,vRotCol2); + + for(int x=0;x<3;x++) { + avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]); + } + + + // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes) + dVector3 avTempArray1[9]; + dVector3 avTempArray2[9]; + dVector4 plPlane; + + int iTempCnt1=0; + int iTempCnt2=0; + + // zeroify vectors - necessary? + for(int i=0; i<9; i++) { + avTempArray1[i][0]=0; + avTempArray1[i][1]=0; + avTempArray1[i][2]=0; + + avTempArray2[i][0]=0; + avTempArray2[i][1]=0; + avTempArray2[i][2]=0; + } + + + // Normal plane + dVector3 vTemp; + vTemp[0]=-vN[0]; + vTemp[1]=-vN[1]; + vTemp[2]=-vN[2]; + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p0 + dVector3 vTemp2; + SUBTRACT(v1,v0,vTemp2); + dCROSS(vTemp,=,vN,vTemp2); + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p1 + SUBTRACT(v2,v1,vTemp2); + dCROSS(vTemp,=,vN,vTemp2); + dNormalize3(vTemp); + SUBTRACT(v0,v2,vTemp2); + CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp)); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p2 + SUBTRACT(v0,v2,vTemp2); + dCROSS(vTemp,=,vN,vTemp2); + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // END of clipping polygons + + + + // for each generated contact point + for ( int i=0; i 0) { + fTempDepth = 0; + } + + dVector3 vPntTmp; + ADD(avTempArray2[i],v0,vPntTmp); + +#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else + dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); + + Contact->depth = -fTempDepth; + SET(Contact->normal,vBestNormal); + SET(Contact->pos,vPntTmp); + Contact->g1 = Geom1; + Contact->g2 = Geom2; + ctContacts++; +#endif + GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, + vPntTmp, vBestNormal, -fTempDepth, ctContacts); + + if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + //dAASSERT(ctContacts>0); + + // if box face is the referent face, then clip triangle on box face + } else { // 2 <= if iBestAxis <= 4 + + // get normal of box face + dVector3 vNormal2; + SET(vNormal2,vBestNormal); + + // get indices of box axes in correct order + int iA0,iA1,iA2; + iA0 = iBestAxis-2; + if ( iA0 == 0 ) { + iA1 = 1; iA2 = 2; + } else if ( iA0 == 1 ) { + iA1 = 0; iA2 = 2; + } else { + iA1 = 0; iA2 = 1; + } + + dVector3 avPoints[3]; + // calculate triangle vertices in box frame + SUBTRACT(v0,vHullBoxPos,avPoints[0]); + SUBTRACT(v1,vHullBoxPos,avPoints[1]); + SUBTRACT(v2,vHullBoxPos,avPoints[2]); + + // CLIP Polygons + // define temp data for clipping + dVector3 avTempArray1[9]; + dVector3 avTempArray2[9]; + + int iTempCnt1, iTempCnt2; + + // zeroify vectors - necessary? + for(int i=0; i<9; i++) { + avTempArray1[i][0]=0; + avTempArray1[i][1]=0; + avTempArray1[i][2]=0; + + avTempArray2[i][0]=0; + avTempArray2[i][1]=0; + avTempArray2[i][2]=0; + } + + // clip triangle with 5 box planes (1 face plane, 4 edge planes) + + dVector4 plPlane; + + // Normal plane + dVector3 vTemp; + vTemp[0]=-vNormal2[0]; + vTemp[1]=-vNormal2[1]; + vTemp[2]=-vNormal2[2]; + CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA0]); + + _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p0 + GETCOL(mHullBoxRot,iA1,vTemp); + CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p1 + GETCOL(mHullBoxRot,iA1,vTemp); + vTemp[0]=-vTemp[0]; + vTemp[1]=-vTemp[1]; + vTemp[2]=-vTemp[2]; + CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p2 + GETCOL(mHullBoxRot,iA2,vTemp); + CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p3 + GETCOL(mHullBoxRot,iA2,vTemp); + vTemp[0]=-vTemp[0]; + vTemp[1]=-vTemp[1]; + vTemp[2]=-vTemp[2]; + CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // for each generated contact point + for ( int i=0; i 0) { + fTempDepth = 0; + } + + // generate contact data + dVector3 vPntTmp; + ADD(avTempArray1[i],vHullBoxPos,vPntTmp); + +#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else + dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); + + Contact->depth = -fTempDepth; + SET(Contact->normal,vBestNormal); + SET(Contact->pos,vPntTmp); + Contact->g1 = Geom1; + Contact->g2 = Geom2; + ctContacts++; +#endif + GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, + vPntTmp, vBestNormal, -fTempDepth, ctContacts); + + if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + //dAASSERT(ctContacts>0); + } + +} + + + + + +// test one mesh triangle on intersection with given box +static void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser) +{ + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(v0, v1, v2) ) { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( iBestAxis == 0 ) { + // this should not happen (we should already exit in that case) + //dMessage (0, "best separation axis not found"); + // do nothing + return; + } + + _cldClipping(v0, v1, v2); +} + + + + + +// OPCODE version of box to mesh collider +#if dTRIMESH_OPCODE +int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (BoxGeom->type == dBoxClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + + // get source hull position, orientation and half size + const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); + const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); + + // to global + SETM(mHullBoxRot,mRotBox); + SET(vHullBoxPos,vPosBox); + + dGeomBoxGetLengths(BoxGeom, vBoxHalfSize); + vBoxHalfSize[0] *= 0.5f; + vBoxHalfSize[1] *= 0.5f; + vBoxHalfSize[2] *= 0.5f; + + + + // get destination hull position and orientation + const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); + const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); + + // to global + SET(vHullDstPos,vPosMesh); + + + + // global info for contact creation + ctContacts = 0; + iStride=Stride; + iFlags=Flags; + ContactGeoms=Contacts; + Geom1=TriMesh; + Geom2=BoxGeom; + + + + // reset stuff + fBestDepth = MAXVALUE; + vBestNormal[0]=0; + vBestNormal[1]=0; + vBestNormal[2]=0; + + OBBCollider& Collider = TriMesh->_OBBCollider; + + + + + // Make OBB + OBB Box; + Box.mCenter.x = vPosBox[0]; + Box.mCenter.y = vPosBox[1]; + Box.mCenter.z = vPosBox[2]; + + // It is a potential issue to explicitly cast to float + // if custom width floating point type is introduced in OPCODE. + // It is necessary to make a typedef and cast to it + // (e.g. typedef float opc_float;) + // However I'm not sure in what header it should be added. + + Box.mExtents.x = /*(float)*/vBoxHalfSize[0]; + Box.mExtents.y = /*(float)*/vBoxHalfSize[1]; + Box.mExtents.z = /*(float)*/vBoxHalfSize[2]; + + Box.mRot.m[0][0] = /*(float)*/mRotBox[0]; + Box.mRot.m[1][0] = /*(float)*/mRotBox[1]; + Box.mRot.m[2][0] = /*(float)*/mRotBox[2]; + + Box.mRot.m[0][1] = /*(float)*/mRotBox[4]; + Box.mRot.m[1][1] = /*(float)*/mRotBox[5]; + Box.mRot.m[2][1] = /*(float)*/mRotBox[6]; + + Box.mRot.m[0][2] = /*(float)*/mRotBox[8]; + Box.mRot.m[1][2] = /*(float)*/mRotBox[9]; + Box.mRot.m[2][2] = /*(float)*/mRotBox[10]; + + Matrix4x4 amatrix; + Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix); + + Matrix4x4 InvBoxMatrix; + InvertPRMatrix(InvBoxMatrix, BoxMatrix); + + // TC results + if (TriMesh->doBoxTC) { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){ + if (TriMesh->BoxTCCache[i].Geom == BoxGeom){ + BoxTC = &TriMesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1]; + BoxTC->Geom = BoxGeom; + BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0 + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix)); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null, + &MakeMatrix(vPosMesh, mRotMesh, amatrix)); + } + + if (! Collider.GetContactStatus()) { + // no collision occurred + return 0; + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0){ + if (TriMesh->ArrayCallback != null){ + TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount); + } + + int ctContacts0 = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++){ + + + const int Triint = Triangles[i]; + if (!Callback(TriMesh, BoxGeom, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv); + + + // test this triangle + _cldTestOneTriangle(dv[0],dv[1],dv[2]); + + // fill-in tri index for generated contacts + for (; ctContacts0side1 = Triint; + + /* + NOTE by Oleh_Derevenko: + The function continues checking triangles after maximal number + of contacts is reached because it selects maximal penetration depths. + See also comments in GenerateContact() + */ + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) + break; + } + } + + + return ctContacts; +} +#endif + +// GIMPACT version of box to mesh collider +#if dTRIMESH_GIMPACT +int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (BoxGeom->type == dBoxClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + + // get source hull position, orientation and half size + const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); + const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); + + // to global + SETM(mHullBoxRot,mRotBox); + SET(vHullBoxPos,vPosBox); + + dGeomBoxGetLengths(BoxGeom, vBoxHalfSize); + vBoxHalfSize[0] *= 0.5f; + vBoxHalfSize[1] *= 0.5f; + vBoxHalfSize[2] *= 0.5f; + + // get destination hull position and orientation + /*const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); + const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); + + // to global + SET(vHullDstPos,vPosMesh);*/ + + // global info for contact creation + ctContacts = 0; + iStride=Stride; + iFlags=Flags; + ContactGeoms=Contacts; + Geom1=TriMesh; + Geom2=BoxGeom; + + + // reset stuff + fBestDepth = MAXVALUE; + vBestNormal[0]=0; + vBestNormal[1]=0; + vBestNormal[2]=0; + + +//*****at first , collide box aabb******// + + GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh; + aabb3f test_aabb; + + test_aabb.minX = BoxGeom->aabb[0]; + test_aabb.maxX = BoxGeom->aabb[1]; + test_aabb.minY = BoxGeom->aabb[2]; + test_aabb.maxY = BoxGeom->aabb[3]; + test_aabb.minZ = BoxGeom->aabb[4]; + test_aabb.maxZ = BoxGeom->aabb[5]; + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result); + + if(collision_result.m_size==0) + { + GIM_DYNARRAY_DESTROY(collision_result); + return 0; + } +//*****Set globals for box collision******// + + //collide triangles + + GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result); + gim_trimesh_locks_work_data(ptrimesh); + + int ctContacts0 = 0; + + for(unsigned int i=0;iside1 = Triint; + + /* + NOTE by Oleh_Derevenko: + The function continues checking triangles after maximal number + of contacts is reached because it selects maximal penetration depths. + See also comments in GenerateContact() + */ + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) + break; + } + + gim_trimesh_unlocks_work_data(ptrimesh); + GIM_DYNARRAY_DESTROY(collision_result); + + return ctContacts; +} +#endif + + +// GenerateContact - Written by Jeff Smith (jeff@burri.to) +// Generate a "unique" contact. A unique contact has a unique +// position or normal. If the potential contact has the same +// position and normal as an existing contact, but a larger +// penetration depth, this new depth is used instead +// +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxGeom* in_g1, dxGeom* in_g2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount) +{ + /* + NOTE by Oleh_Derevenko: + This function is called after maximal number of contacts has already been + collected because it has a side effect of replacing penetration depth of + existing contact with larger penetration depth of another matching normal contact. + If this logic is not necessary any more, you can bail out on reach of contact + number maximum immediately in dCollideBTL(). You will also need to correct + conditional statements after invocations of GenerateContact() in _cldClipping(). + */ + do + { + dContactGeom* Contact; + dVector3 diff; + + if (!(in_Flags & CONTACTS_UNIMPORTANT)) + { + bool duplicate = false; + for (int i=0; ipos[j]; + if (dDOT(diff, diff) < dEpsilon) + { + // same normal? + if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon)) + { + if (in_Depth > Contact->depth) + Contact->depth = in_Depth; + duplicate = true; + /* + NOTE by Oleh_Derevenko: + There may be a case when two normals are close to each other but not duplicate + while third normal is detected to be duplicate for both of them. + This is the only reason I can think of, there is no "break" statement. + Perhaps author considered it to be logical that the third normal would + replace the depth in both of initial contacts. + However, I consider it a questionable practice which should not + be applied without deep understanding of underlaying physics. + Even more, is this situation with close normal triplet acceptable at all? + Should not be two initial contacts reduced to one (replaced with the latter)? + If you know the answers for these questions, you may want to change this code. + See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp + */ + } + } + } + if (duplicate || OutTriCount == (in_Flags & NUMC_MASK)) + { + break; + } + } + else + { + dIASSERT(OutTriCount < (in_Flags & NUMC_MASK)); + } + + // Add a new contact + Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); + + Contact->pos[0] = in_ContactPos[0]; + Contact->pos[1] = in_ContactPos[1]; + Contact->pos[2] = in_ContactPos[2]; + Contact->pos[3] = 0.0; + + Contact->normal[0] = in_Normal[0]; + Contact->normal[1] = in_Normal[1]; + Contact->normal[2] = in_Normal[2]; + Contact->normal[3] = 0.0; + + Contact->depth = in_Depth; + + Contact->g1 = in_g1; + Contact->g2 = in_g2; + + OutTriCount++; + } + while (false); +} + +#endif // dTRIMESH_ENABLED diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp new file mode 100644 index 0000000..da235e0 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp @@ -0,0 +1,1181 @@ +/************************************************************************* +* * +* 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. * +* * +*************************************************************************/ + +/* + * Triangle-Capsule(Capsule) collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +// NOTES from Nguyen Binh +// 14 Apr : Seem to be robust +// There is a problem when you use original Step and set contact friction +// surface.mu = dInfinity; +// More description : +// When I dropped Capsule over the bunny ears, it seems to stuck +// there for a while. I think the cause is when you set surface.mu = dInfinity; +// the friction force is too high so it just hang the capsule there. +// So the good cure for this is to set mu = around 1.5 (in my case) +// For StepFast1, this become as solid as rock : StepFast1 just approximate +// friction force. + +// NOTES from Croteam's Alen +//As a side note... there are some extra contacts that can be generated +//on the edge between two triangles, and if the capsule penetrates deeply into +//the triangle (usually happens with large mass or low FPS), some such +//contacts can in some cases push the capsule away from the edge instead of +//away from the two triangles. This shows up as capsule slowing down a bit +//when hitting an edge while sliding along a flat tesselated grid of +//triangles. This is only if capsule is standing upwards. + +//Same thing can appear whenever a smooth object (e.g sphere) hits such an +//edge, and it needs to be solved as a special case probably. This is a +//problem we are looking forward to address soon. + +#include +#include +#include +#include +#include "collision_util.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_ENABLED + +// OPCODE version +#if dTRIMESH_OPCODE +// largest number, double or float +#if defined(dSINGLE) +#define MAX_REAL FLT_MAX +#define MIN_REAL (-FLT_MAX) +#else +#define MAX_REAL DBL_MAX +#define MIN_REAL (-DBL_MAX) +#endif + +// To optimize before send contacts to dynamic part +#define OPTIMIZE_CONTACTS + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) \ + (r)[0]=(a)[0] - (b)[0]; \ + (r)[1]=(a)[1] - (b)[1]; \ + (r)[2]=(a)[2] - (b)[2]; + + +// dVector3 +// a=b +#define SET(a,b) \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; + + +// dMatrix3 +// a=b +#define SETM(a,b) \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; \ + (a)[3]=(b)[3]; \ + (a)[4]=(b)[4]; \ + (a)[5]=(b)[5]; \ + (a)[6]=(b)[6]; \ + (a)[7]=(b)[7]; \ + (a)[8]=(b)[8]; \ + (a)[9]=(b)[9]; \ + (a)[10]=(b)[10]; \ + (a)[11]=(b)[11]; + + +// dVector3 +// r=a+b +#define ADD(a,b,r) \ + (r)[0]=(a)[0] + (b)[0]; \ + (r)[1]=(a)[1] + (b)[1]; \ + (r)[2]=(a)[2] + (b)[2]; + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) \ + (v)[0]=(m)[(a)+0]; \ + (v)[1]=(m)[(a)+4]; \ + (v)[2]=(m)[(a)+8]; + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) \ + ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ); \ + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) \ + plane[0]=normal[0];\ + plane[1]=normal[1];\ + plane[2]=normal[2];\ + plane[3]=d; + + +// dVector3 +// length of vector a +#define LENGTHOF(a) \ + dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);\ + +inline dReal _length2OfVector3(dVector3 v) +{ + return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] ); +} + + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +static sLocalContactData *gLocalContacts; +static unsigned int ctContacts = 0; + +// capsule data +// real time data +static dMatrix3 mCapsuleRotation; +static dVector3 vCapsulePosition; +static dVector3 vCapsuleAxis; +// static data +static dReal vCapsuleRadius; +static dReal fCapsuleSize; + +// mesh data +static dMatrix4 mHullDstPl; +static dMatrix3 mTriMeshRot; +static dVector3 mTriMeshPos; +static dVector3 vE0, vE1, vE2; + +// Two geom +dxGeom* gCylinder; +dxGeom* gTriMesh; + +// global collider data +static dVector3 vNormal; +static dReal fBestDepth; +static dReal fBestCenter; +static dReal fBestrt; +static int iBestAxis; +static dVector3 vN = {0,0,0,0}; + +static dVector3 vV0; +static dVector3 vV1; +static dVector3 vV2; + +// ODE contact's specific +static unsigned int iFlags; +static dContactGeom *ContactGeoms; +static int iStride; + +// Capsule lie on axis number 3 = (Z axis) +static const int nCAPSULE_AXIS = 2; + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + SUBTRACT(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + SUBTRACT(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +inline void _OptimizeLocalContacts() +{ + int nContacts = ctContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j])) + { + // If they are seem to be the samed then filtered + // out the least penetrate one + if (_IsBetter(gLocalContacts[j],gLocalContacts[i])) + { + gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} + +inline int _ProcessLocalContacts() +{ + if (ctContacts == 0) + { + return 0; + } + +#ifdef OPTIMIZE_CONTACTS + if (ctContacts > 1 && !(iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + unsigned int iContact = 0; + dContactGeom* Contact = 0; + + unsigned int nFinalContact = 0; + + for (iContact = 0; iContact < ctContacts; iContact ++) + { + // Ensure that we haven't created too many contacts + if( nFinalContact >= (iFlags & NUMC_MASK)) + { + break; + } + + if (1 == gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride); + Contact->depth = gLocalContacts[iContact].fDepth; + SET(Contact->normal,gLocalContacts[iContact].vNormal); + SET(Contact->pos,gLocalContacts[iContact].vPos); + Contact->g1 = gTriMesh; + Contact->g2 = gCylinder; + Contact->side2 = gLocalContacts[iContact].triIndex; + + nFinalContact++; + } + } + // debug + //if (nFinalContact != ctContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",ctContacts,ctContacts-nFinalContact); + //} + + return nFinalContact; +} + +BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 ); + dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return FALSE; + // if both points in front of the plane + } else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return TRUE; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + SET(vEpnt0,vIntersectionPoint); + } else + { + SET(vEpnt1,vIntersectionPoint); + } + return TRUE; + } + return TRUE; +} + +static BOOL _cldTestAxis(const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3 vAxis, + int iAxis, + BOOL bNoFlip = FALSE) +{ + + // calculate length of separating axis vector + dReal fL = LENGTHOF(vAxis); + // if not long enough + // TODO : dReal epsilon please + if ( fL < REAL(1e-5) ) + { + // do nothing + //iLastOutAxis = 0; + return TRUE; + } + + // otherwise normalize it + dNormalize3(vAxis); + + // project capsule on vAxis + dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))*(fCapsuleSize*REAL(0.5)-vCapsuleRadius) + vCapsuleRadius; + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dDOT( vV0 , vAxis ); + afv[1] = dDOT( vV1 , vAxis ); + afv[2] = dDOT( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = MIN_REAL; + + // for each vertex + for(int i=0; i<3; i++) + { + // find minimum + if (afv[i]fMax) + { + fMax = afv[i]; + } + } + + // find triangle's center of interval on axis + dReal fCenter = (fMin+fMax)*REAL(0.5); + // calculate triangles half interval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > ( frc + fTriangleRadius ) ) + { + // exit, we have no intersection + return FALSE; + } + + // calculate depth + dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius); + + // if greater then best found so far + if ( fDepth > fBestDepth ) + { + // remember depth + fBestDepth = fDepth; + fBestCenter = fCenter; + fBestrt = fTriangleRadius; + + vNormal[0] = vAxis[0]; + vNormal[1] = vAxis[1]; + vNormal[2] = vAxis[2]; + + iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if (fCenter<0 && !bNoFlip) + { + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + + fBestCenter = -fCenter; + } + } + + return TRUE; +} + +// helper for less key strokes +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + const dVector3& v4, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + SUBTRACT(v1,v2,t1); + dCROSS(t2,=,t1,v3); + dCROSS(r,=,t2,v4); +} + +static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + uint8 flags) +{ + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + dVector3 vCp1; + vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + // reset best axis + iBestAxis = 0; + // reset best depth + fBestDepth = -MAX_REAL; + // reset separating axis vector + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = 1e-6f; + + // Translate triangle to Cc cord. + SUBTRACT(v0 , vCapsulePosition, vV0); + SUBTRACT(v1 , vCapsulePosition, vV1); + SUBTRACT(v2 , vCapsulePosition, vV2); + + // We begin to test for 19 separating axis now + // I wonder does it help if we employ the method like ISA-GJK??? + // Or at least we should do experiment and find what axis will + // be most likely to be separating axis to check it first. + + // Original + // axis vN + //vAxis = -vN; + vAxis[0] = - vN[0]; + vAxis[1] = - vN[1]; + vAxis[2] = - vN[2]; + if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE)) + { + return FALSE; + } + + if (flags & dxTriMeshData::kEdge0) + { + // axis CxE0 - Edge 0 + dCROSS(vAxis,=,vCapsuleAxis,vE0); + //vAxis = dCROSS( vCapsuleAxis cross vE0 ); + if( _length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge1) + { + // axis CxE1 - Edge 1 + dCROSS(vAxis,=,vCapsuleAxis,vE1); + //vAxis = ( vCapsuleAxis cross vE1 ); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge2) + { + // axis CxE2 - Edge 2 + //vAxis = ( vCapsuleAxis cross vE2 ); + dCROSS(vAxis,=,vCapsuleAxis,vE2); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge0) + { + // first capsule point + // axis ((Cp0-V0) x E0) x E0 + _CalculateAxis(vCp0,v0,vE0,vE0,vAxis); + // vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge1) + { + // axis ((Cp0-V1) x E1) x E1 + _CalculateAxis(vCp0,v1,vE1,vE1,vAxis); + //vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge2) + { + // axis ((Cp0-V2) x E2) x E2 + _CalculateAxis(vCp0,v2,vE2,vE2,vAxis); + //vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge0) + { + // second capsule point + // axis ((Cp1-V0) x E0) x E0 + _CalculateAxis(vCp1,v0,vE0,vE0,vAxis); + //vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge1) + { + // axis ((Cp1-V1) x E1) x E1 + _CalculateAxis(vCp1,v1,vE1,vE1,vAxis); + //vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kEdge2) + { + // axis ((Cp1-V2) x E2) x E2 + _CalculateAxis(vCp1,v2,vE2,vE2,vAxis); + //vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert0) + { + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + _CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert1) + { + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + _CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert2) + { + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + _CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) { + return FALSE; + } + } + } + + // Test as separating axes direction vectors between each triangle + // edge and each capsule's cap center + + if (flags & dxTriMeshData::kVert0) + { + // first triangle vertex and first capsule point + //vAxis = v0 - vCp0; + SUBTRACT(v0,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert1) + { + // second triangle vertex and first capsule point + //vAxis = v1 - vCp0; + SUBTRACT(v1,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert2) + { + // third triangle vertex and first capsule point + //vAxis = v2 - vCp0; + SUBTRACT(v2,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert0) + { + // first triangle vertex and second capsule point + //vAxis = v0 - vCp1; + SUBTRACT(v0,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert1) + { + // second triangle vertex and second capsule point + //vAxis = v1 - vCp1; + SUBTRACT(v1,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) { + return FALSE; + } + } + } + + if (flags & dxTriMeshData::kVert2) + { + // third triangle vertex and second capsule point + //vAxis = v2 - vCp1; + SUBTRACT(v2,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) { + return FALSE; + } + } + } + + return TRUE; +} + +// test one mesh triangle on intersection with capsule +static void _cldTestOneTriangleVSCapsule( const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + uint8 flags) +{ + + // calculate edges + SUBTRACT(v1,v0,vE0); + SUBTRACT(v2,v1,vE1); + SUBTRACT(v0,v2,vE2); + + dVector3 _minus_vE0; + SUBTRACT(v0,v1,_minus_vE0); + + // calculate poly normal + dCROSS(vN,=,vE1,_minus_vE0); + dNormalize3(vN); + + // create plane from triangle + dReal plDistance = -dDOT(v0,vN); + dVector4 plTrianglePlane; + CONSTRUCTPLANE(plTrianglePlane,vN,plDistance); + + // calculate capsule distance to plane + dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition); + + // Capsule must be over positive side of triangle + if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + SET (vPnt0,v0); + dVector3 vPnt1; + SET (vPnt1,v1); + dVector3 vPnt2; + SET (vPnt2,v2); + + if (fDistanceCapsuleCenterToPlane < 0 ) + { + SET (vPnt0,v0); + SET (vPnt1,v2); + SET (vPnt2,v1); + } + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(FALSE); + // do nothing + return; + } + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius; + vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius; + vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint0[1] = vCposTrans[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint0[2] = vCposTrans[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + // transform capsule edge points into triangle space + vCEdgePoint0[0] -= vPnt0[0]; + vCEdgePoint0[1] -= vPnt0[1]; + vCEdgePoint0[2] -= vPnt0[2]; + + vCEdgePoint1[0] -= vPnt0[0]; + vCEdgePoint1[1] -= vPnt0[1]; + vCEdgePoint1[2] -= vPnt0[2]; + + dVector4 plPlane; + dVector3 _minus_vN; + _minus_vN[0] = -vN[0]; + _minus_vN[1] = -vN[1]; + _minus_vN[2] = -vN[2]; + // triangle plane + CONSTRUCTPLANE(plPlane,_minus_vN,0); + //plPlane = Plane4f( -vN, 0); + + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + // plane with edge 0 + dVector3 vTemp; + dCROSS(vTemp,=,vN,vE0); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCROSS(vTemp,=,vN,vE1); + CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-REAL(1e-5))); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCROSS(vTemp,=,vN,vE2); + CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { + return; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += vPnt0[0]; + vCEdgePoint0[1] += vPnt0[1]; + vCEdgePoint0[2] += vPnt0[2]; + + vCEdgePoint1[0] += vPnt0[0]; + vCEdgePoint1[1] += vPnt0[1]; + vCEdgePoint1[2] += vPnt0[2]; + + // calculate depths for both contact points + SUBTRACT(vCEdgePoint0,vCapsulePosition,vTemp); + dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt); + SUBTRACT(vCEdgePoint1,vCapsulePosition,vTemp); + dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt); + + // clamp depths to zero + if(fDepth0 < 0) + { + fDepth0 = 0.0f; + } + + if(fDepth1 < 0 ) + { + fDepth1 = 0.0f; + } + + // Cached contacts's data + // contact 0 + dIASSERT(ctContacts < (iFlags & NUMC_MASK)); // Do not call function if there is no room to store result + gLocalContacts[ctContacts].fDepth = fDepth0; + SET(gLocalContacts[ctContacts].vNormal,vNormal); + SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0); + gLocalContacts[ctContacts].nFlags = 1; + ctContacts++; + + if (ctContacts < (iFlags & NUMC_MASK)) { + // contact 1 + gLocalContacts[ctContacts].fDepth = fDepth1; + SET(gLocalContacts[ctContacts].vNormal,vNormal); + SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1); + gLocalContacts[ctContacts].nFlags = 1; + ctContacts++; + } + +} + +// capsule - trimesh by CroTeam +// Ported by Nguyem Binh +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)o1; + gCylinder = o2; + gTriMesh = o1; + + const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder); + memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3)); + + const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder); + memcpy(vCapsulePosition,pDst,sizeof(dVector3)); + + vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + + // Get size of Capsule + dGeomCapsuleGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize); + fCapsuleSize += 2*vCapsuleRadius; + + const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh); + memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3)); + + const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh); + memcpy(mTriMeshPos,pTriPos,sizeof(dVector3)); + + // global info for contact creation + iStride =skip; + iFlags =flags; + ContactGeoms =contact; + + // reset contact counter + ctContacts = 0; + + // reset best depth + fBestDepth = - MAX_REAL; + fBestCenter = 0; + fBestrt = 0; + + + + + // reset collision normal + vNormal[0] = REAL(0.0); + vNormal[1] = REAL(0.0); + vNormal[2] = REAL(0.0); + + // Will it better to use LSS here? -> confirm Pierre. + OBBCollider& Collider = TriMesh->_OBBCollider; + + // It is a potential issue to explicitly cast to float + // if custom width floating point type is introduced in OPCODE. + // It is necessary to make a typedef and cast to it + // (e.g. typedef float opc_float;) + // However I'm not sure in what header it should be added. + + Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]); + Point cExtents(/*(float)*/ vCapsuleRadius, /*(float)*/ vCapsuleRadius,/*(float)*/ fCapsuleSize/2); + + Matrix3x3 obbRot; + + obbRot[0][0] = /*(float)*/ mCapsuleRotation[0]; + obbRot[1][0] = /*(float)*/ mCapsuleRotation[1]; + obbRot[2][0] = /*(float)*/ mCapsuleRotation[2]; + + obbRot[0][1] = /*(float)*/ mCapsuleRotation[4]; + obbRot[1][1] = /*(float)*/ mCapsuleRotation[5]; + obbRot[2][1] = /*(float)*/ mCapsuleRotation[6]; + + obbRot[0][2] = /*(float)*/ mCapsuleRotation[8]; + obbRot[1][2] = /*(float)*/ mCapsuleRotation[9]; + obbRot[2][2] = /*(float)*/ mCapsuleRotation[10]; + + OBB obbCapsule(cCenter,cExtents,obbRot); + + Matrix4x4 CapsuleMatrix; + MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix); + + Matrix4x4 MeshMatrix; + MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix); + + // TC results + if (TriMesh->doBoxTC) { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){ + if (TriMesh->BoxTCCache[i].Geom == gCylinder){ + BoxTC = &TriMesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1]; + BoxTC->Geom = gCylinder; + BoxTC->FatCoeff = 1.0f; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix); + } + + if (! Collider.GetContactStatus()) { + // no collision occurred + return 0; + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0) + { + if (TriMesh->ArrayCallback != null) + { + TriMesh->ArrayCallback(TriMesh, gCylinder, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(iFlags & NUMC_MASK)); + + unsigned int ctContacts0 = ctContacts; + + uint8* UseFlags = TriMesh->Data->UseFlags; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!Callback(TriMesh, gCylinder, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv); + + uint8 flags = UseFlags ? UseFlags[Triint] : dxTriMeshData::kUseAll; + + // test this triangle + _cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags); + + // fill-in tri index for generated contacts + for (; ctContacts0=(iFlags & NUMC_MASK)) + { + break; + } + + } + } + + return _ProcessLocalContacts(); +} +#endif + +// GIMPACT version +#if dTRIMESH_GIMPACT +#define nCAPSULE_AXIS 2 +// capsule - trimesh By francisco leon +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)o1; + dxGeom* gCylinder = o2; + + //Get capsule params + dMatrix3 mCapsuleRotation; + dVector3 vCapsulePosition; + dVector3 vCapsuleAxis; + dReal vCapsuleRadius; + dReal fCapsuleSize; + dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder); + memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3)); + dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder); + memcpy(vCapsulePosition,pDst,sizeof(dVector3)); + //Axis + vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + // Get size of CCylinder + dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize); + fCapsuleSize*=0.5f; + //Set Capsule params + GIM_CAPSULE_DATA capsule; + + capsule.m_radius = vCapsuleRadius; + VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis); + VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1); + VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis); + VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2); + + +//Create contact list + GDYNAMIC_ARRAY trimeshcontacts; + GIM_CREATE_CONTACT_LIST(trimeshcontacts); + + //Collide trimeshe vs capsule + gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts); + + + if(trimeshcontacts.m_size == 0) + { + GIM_DYNARRAY_DESTROY(trimeshcontacts); + return 0; + } + + GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts); + + unsigned contactcount = trimeshcontacts.m_size; + unsigned contactmax = (unsigned)(flags & NUMC_MASK); + if (contactcount > contactmax) + { + contactcount = contactmax; + } + + dContactGeom* pcontact; + unsigned i; + + for (i=0;ipos[0] = ptrimeshcontacts->m_point[0]; + pcontact->pos[1] = ptrimeshcontacts->m_point[1]; + pcontact->pos[2] = ptrimeshcontacts->m_point[2]; + pcontact->pos[3] = 1.0f; + + pcontact->normal[0] = ptrimeshcontacts->m_normal[0]; + pcontact->normal[1] = ptrimeshcontacts->m_normal[1]; + pcontact->normal[2] = ptrimeshcontacts->m_normal[2]; + pcontact->normal[3] = 0; + + pcontact->depth = ptrimeshcontacts->m_depth; + pcontact->g1 = TriMesh; + pcontact->g2 = gCylinder; + + ptrimeshcontacts++; + } + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} +#endif + +#endif // dTRIMESH_ENABLED diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp new file mode 100644 index 0000000..717c763 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp @@ -0,0 +1,1255 @@ +// This file contains some code based on the code from Magic Software. +// That code is available under a Free Source License Agreement +// that can be found at http://www.magic-software.com/License/free.pdf + +#include +#include +#include +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between a point and a triangle. + + @param pfSParam Barycentric coordinate of triangle at point closest to p (u) + @param pfTParam Barycentric coordinate of triangle at point closest to p (v) + @return Shortest distance squared. + + The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* pfSParam, dReal* pfTParam ) +{ + dVector3 kDiff; + Vector3Subtract( triOrigin, p, kDiff ); + dReal fA00 = dDOT( triEdge0, triEdge0 ); + dReal fA01 = dDOT( triEdge0, triEdge1 ); + dReal fA11 = dDOT( triEdge1, triEdge1 ); + dReal fB0 = dDOT( kDiff, triEdge0 ); + dReal fB1 = dDOT( kDiff, triEdge1 ); + dReal fC = dDOT( kDiff, kDiff ); + dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); + dReal fS = fA01*fB1-fA11*fB0; + dReal fT = fA01*fB0-fA00*fB1; + dReal fSqrDist; + + if ( fS + fT <= fDet ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4 + { + if ( fB0 < REAL(0.0) ) + { + fT = REAL(0.0); + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else // region 3 + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else if ( fT < REAL(0.0) ) // region 5 + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else // region 0 + { + // minimum at interior point + if ( fDet == REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(0.0); + fSqrDist = dInfinity; + } + else + { + dReal fInvDet = REAL(1.0)/fDet; + fS *= fInvDet; + fT *= fInvDet; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + } + else + { + dReal fTmp0, fTmp1, fNumer, fDenom; + + if ( fS < REAL(0.0) ) // region 2 + { + fTmp0 = fA01 + fB0; + fTmp1 = fA11 + fB1; + if ( fTmp1 > fTmp0 ) + { + fNumer = fTmp1 - fTmp0; + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = fNumer/fDenom; + fT = REAL(1.0) - fS; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + else + { + fS = REAL(0.0); + if ( fTmp1 <= REAL(0.0) ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else if ( fT < REAL(0.0) ) // region 6 + { + fTmp0 = fA01 + fB1; + fTmp1 = fA00 + fB0; + if ( fTmp1 > fTmp0 ) + { + fNumer = fTmp1 - fTmp0; + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fT = REAL(1.0); + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = fNumer/fDenom; + fS = REAL(1.0) - fT; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + else + { + fT = REAL(0.0); + if ( fTmp1 <= REAL(0.0) ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + } + else // region 1 + { + fNumer = fA11 + fB1 - fA01 - fB0; + if ( fNumer <= REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = fNumer/fDenom; + fT = REAL(1.0) - fS; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + } + } + + if ( pfSParam ) + *pfSParam = (float)fS; + + if ( pfTParam ) + *pfTParam = (float)fT; + + return dReal(fabs(fSqrDist)); +} + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between two line segments. + @param pfSegP0 t value for seg1 where the shortest distance between + the segments exists. + @param pfSegP0 t value for seg2 where the shortest distance between + the segments exists. + @return Shortest distance squared. + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, + const dVector3 seg2Origin, const dVector3 seg2Direction, + dReal* pfSegP0, dReal* pfSegP1 ) +{ + const dReal gs_fTolerance = 1e-05f; + dVector3 kDiff, kNegDiff, seg1NegDirection; + Vector3Subtract( seg1Origin, seg2Origin, kDiff ); + Vector3Negate( kDiff, kNegDiff ); + dReal fA00 = dDOT( seg1Direction, seg1Direction ); + Vector3Negate( seg1Direction, seg1NegDirection ); + dReal fA01 = dDOT( seg1NegDirection, seg2Direction ); + dReal fA11 = dDOT( seg2Direction, seg2Direction ); + dReal fB0 = dDOT( kDiff, seg1Direction ); + dReal fC = dDOT( kDiff, kDiff ); + dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); + dReal fB1, fS, fT, fSqrDist, fTmp; + + if ( fDet >= gs_fTolerance ) + { + // line segments are not parallel + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = fA01*fB1-fA11*fB0; + fT = fA01*fB0-fA00*fB1; + + if ( fS >= REAL(0.0) ) + { + if ( fS <= fDet ) + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 0 (interior) + { + // minimum at two interior points of 3D lines + dReal fInvDet = REAL(1.0)/fDet; + fS *= fInvDet; + fT *= fInvDet; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + else // region 3 (side) + { + fT = REAL(1.0); + fTmp = fA01+fB0; + if ( fTmp >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else if ( -fTmp >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + } + else // region 7 (side) + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + } + else + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 1 (side) + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + else // region 2 (corner) + { + fTmp = fA01+fB0; + if ( -fTmp <= fA00 ) + { + fT = REAL(1.0); + if ( fTmp >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + else + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + } + } + else // region 8 (corner) + { + if ( -fB0 < fA00 ) + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + } + } + } + else + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 5 (side) + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + else // region 4 (corner) + { + fTmp = fA01+fB0; + if ( fTmp < REAL(0.0) ) + { + fT = REAL(1.0); + if ( -fTmp >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + } + else // region 6 (corner) + { + if ( fB0 < REAL(0.0) ) + { + fT = REAL(0.0); + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + } + } + else + { + // line segments are parallel + if ( fA01 > REAL(0.0) ) + { + // direction vectors form an obtuse angle + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 <= fA00 ) + { + fS = -fB0/fA00; + fT = REAL(0.0); + fSqrDist = fB0*fS+fC; + } + else + { + //fB1 = -kDiff % seg2.m; + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = REAL(1.0); + fTmp = fA00+fB0; + if ( -fTmp >= fA01 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1); + } + else + { + fT = -fTmp/fA01; + fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1)); + } + } + } + else + { + // direction vectors form an acute angle + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( fB0 <= REAL(0.0) ) + { + fS = -fB0/fA00; + fT = REAL(0.0); + fSqrDist = fB0*fS+fC; + } + else + { + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = REAL(0.0); + if ( fB0 >= -fA01 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB0/fA01; + fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT); + } + } + } + } + + if ( pfSegP0 ) + *pfSegP0 = fS; + + if ( pfSegP1 ) + *pfSegP1 = fT; + + return dReal(fabs(fSqrDist)); +} + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between a line segment and + a triangle. + + @param pfSegP t value for the line segment where the shortest distance between + the segment and the triangle occurs. + So the point along the segment that is the shortest distance + away from the triangle can be obtained by (seg.end - seg.start) * t. + @param pfTriP0 Barycentric coordinate of triangle at point closest to seg (u) + @param pfTriP1 Barycentric coordinate of triangle at point closest to seg (v) + @return Shortest distance squared. + + The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, + const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 ) +{ + const dReal gs_fTolerance = 1e-06f; + dVector3 segDirection, segNegDirection, kDiff, kNegDiff; + Vector3Subtract( segEnd, segOrigin, segDirection ); + Vector3Negate( segDirection, segNegDirection ); + Vector3Subtract( triOrigin, segOrigin, kDiff ); + Vector3Negate( kDiff, kNegDiff ); + dReal fA00 = dDOT( segDirection, segDirection ); + dReal fA01 = dDOT( segNegDirection, triEdge0 ); + dReal fA02 = dDOT( segNegDirection, triEdge1 ); + dReal fA11 = dDOT( triEdge0, triEdge0 ); + dReal fA12 = dDOT( triEdge0, triEdge1 ); + dReal fA22 = dDOT( triEdge1, triEdge1 ); + dReal fB0 = dDOT( kNegDiff, segDirection ); + dReal fB1 = dDOT( kDiff, triEdge0 ); + dReal fB2 = dDOT( kDiff, triEdge1 ); + + dVector3 kTriSegOrigin, kTriSegDirection, kPt; + dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0; + + // Set up for a relative error test on the angle between ray direction + // and triangle normal to determine parallel/nonparallel status. + dVector3 kN; + dCROSS( kN, =, triEdge0, triEdge1 ); + dReal fNSqrLen = dDOT( kN, kN ); + dReal fDot = dDOT( segDirection, kN ); + bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen); + + if ( bNotParallel ) + { + dReal fCof00 = fA11*fA22-fA12*fA12; + dReal fCof01 = fA02*fA12-fA01*fA22; + dReal fCof02 = fA01*fA12-fA02*fA11; + dReal fCof11 = fA00*fA22-fA02*fA02; + dReal fCof12 = fA02*fA01-fA00*fA12; + dReal fCof22 = fA00*fA11-fA01*fA01; + dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02); + dReal fRhs0 = -fB0*fInvDet; + dReal fRhs1 = -fB1*fInvDet; + dReal fRhs2 = -fB2*fInvDet; + + fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2; + fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2; + fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2; + + if ( fR < REAL(0.0) ) + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4m + { + // min on face s=0 or t=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3m + { + // min on face s=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR,&fT ); + fS = REAL(0.0); + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + else if ( fT < REAL(0.0) ) // region 5m + { + // min on face t=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 0m + { + // min on face r=0 + fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS, &fT ); + fR = REAL(0.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2m + { + // min on face s=0 or s+t=1 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6m + { + // min on face t=0 or s+t=1 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1m + { + // min on face s+t=1 or r=0 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + } + else if ( fR <= REAL(1.0) ) + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4 + { + // min on face s=0 or t=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3 + { + // min on face s=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + } + } + else if ( fT < REAL(0.0) ) // region 5 + { + // min on face t=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + } + else // region 0 + { + // global minimum is interior, done + fSqrDist = REAL(0.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2 + { + // min on face s=0 or s+t=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6 + { + // min on face t=0 or s+t=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1 + { + // min on face s+t=1 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + } + } + } + else // fR > 1 + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4p + { + // min on face s=0 or t=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3p + { + // min on face s=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + else if ( fT < REAL(0.0) ) // region 5p + { + // min on face t=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 0p + { + // min face on r=1 + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS, &fT ); + fR = REAL(1.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2p + { + // min on face s=0 or s+t=1 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6p + { + // min on face t=0 or s+t=1 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1p + { + // min on face s+t=1 or r=1 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + } + } + else + { + // segment and triangle are parallel + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, &fR, &fS ); + fT = REAL(0.0); + + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + + if ( pfSegP ) + *pfSegP = fR; + + if ( pfTriP0 ) + *pfTriP0 = fS; + + if ( pfTriP1 ) + *pfTriP1 = fT; + + return fSqrDist; +} diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp new file mode 100644 index 0000000..229966b --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp @@ -0,0 +1,456 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include +#include +#include + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_GIMPACT + +void dxTriMeshData::Preprocess(){ // stub +} + +dTriMeshDataID dGeomTriMeshDataCreate(){ + return new dxTriMeshData(); +} + +void dGeomTriMeshDataDestroy(dTriMeshDataID g){ + delete g; +} + +void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) { //stub +} + +dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) { + return NULL; // stub +} + +void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) { //stub +} + +void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) { + dUASSERT(g, "argument not trimesh data"); + return NULL; // stub +} + +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "argument not trimesh data"); + dIASSERT(Vertices); + dIASSERT(Indices); + + g->Build(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + true); +} + + +void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, (void*)NULL); +} + + +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "argument not trimesh data"); + + g->Build(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + false); +} + + +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) { + dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, NULL); +} + + +void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount, + const int* Normals){ +#ifdef dSINGLE + dGeomTriMeshDataBuildSingle1(g, + Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(unsigned int), + Normals); +#else + dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(unsigned int), + Normals); +#endif +} + + +void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount) { + dGeomTriMeshDataBuildSimple1(g, + Vertices, VertexCount, Indices, IndexCount, + (const int*)NULL); +} + +void dGeomTriMeshDataPreprocess(dTriMeshDataID g) +{ + dUASSERT(g, "argument not trimesh data"); + g->Preprocess(); +} + +void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen) +{ + dUASSERT(g, "argument not trimesh data"); + *buf = NULL; + *bufLen = 0; +} + +void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf) +{ + dUASSERT(g, "argument not trimesh data"); +// g->UseFlags = buf; +} + + +// Trimesh + +dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1){ + type = dTriMeshClass; + + dGeomTriMeshSetData(this,Data); + + /* TC has speed/space 'issues' that don't make it a clear + win by default on spheres/boxes. */ + this->doSphereTC = true; + this->doBoxTC = true; + this->doCapsuleTC = true; + +} + +dxTriMesh::~dxTriMesh(){ + + //Terminate Trimesh + gim_trimesh_destroy(&m_collision_trimesh); +} + + +void dxTriMesh::ClearTCCache(){ + +} + + +int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){ + return 1; +} + + +void dxTriMesh::computeAABB() +{ + //update trimesh transform + mat4f transform; + IDENTIFY_MATRIX_4X4(transform); + MakeMatrix(this, transform); + gim_trimesh_set_tranform(&m_collision_trimesh,transform); + + //Update trimesh boxes + gim_trimesh_update(&m_collision_trimesh); + + memcpy(aabb,&m_collision_trimesh.m_aabbset.m_global_bound,6*sizeof(GREAL)); +} + + +void dxTriMeshData::UpdateData() +{ +// BVTree.Refit(); +} + + +dGeomID dCreateTriMesh(dSpaceID space, + dTriMeshDataID Data, + dTriCallback* Callback, + dTriArrayCallback* ArrayCallback, + dTriRayCallback* RayCallback) +{ + dxTriMesh* Geom = new dxTriMesh(space, Data); + Geom->Callback = Callback; + Geom->ArrayCallback = ArrayCallback; + Geom->RayCallback = RayCallback; + + return Geom; +} + +void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->Callback = Callback; +} + +dTriCallback* dGeomTriMeshGetCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->Callback; +} + +void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->ArrayCallback = ArrayCallback; +} + +dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->ArrayCallback; +} + +void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->RayCallback = Callback; +} + +dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->RayCallback; +} + +void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + dxTriMesh* mesh = (dxTriMesh*) g; + mesh->Data = Data; + // I changed my data -- I know nothing about my own AABB anymore. + ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD); + + // GIMPACT only supports stride 12, so we need to catch the error early. + dUASSERT + ( + Data->m_VertexStride == 3*sizeof(dReal) && Data->m_TriStride == 3*sizeof(int), + "Gimpact trimesh only supports a stride of 3 dReal/int\n" + "This means that you cannot use dGeomTriMeshDataBuildSimple() with Gimpact.\n" + "Change the stride, or use Opcode trimeshes instead.\n" + ); + + //Create trimesh + if ( Data->m_Vertices ) + gim_trimesh_create_from_data + ( + &mesh->m_collision_trimesh, // gimpact mesh + ( vec3f *)(&Data->m_Vertices[0]), // vertices + Data->m_VertexCount, // nr of verts + 0, // copy verts? + ( GUINT *)(&Data->m_Indices[0]), // indices + Data->m_TriangleCount*3, // nr of indices + 0, // copy indices? + 1 // transformed reply + ); +} + +dTriMeshDataID dGeomTriMeshGetData(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->Data; +} + + + +void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + switch (geomClass) + { + case dSphereClass: + ((dxTriMesh*)g)->doSphereTC = (1 == enable); + break; + case dBoxClass: + ((dxTriMesh*)g)->doBoxTC = (1 == enable); + break; + case dCapsuleClass: +// case dCCylinderClass: + ((dxTriMesh*)g)->doCapsuleTC = (1 == enable); + break; + } +} + +int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + switch (geomClass) + { + case dSphereClass: + if (((dxTriMesh*)g)->doSphereTC) + return 1; + break; + case dBoxClass: + if (((dxTriMesh*)g)->doBoxTC) + return 1; + break; + case dCapsuleClass: + if (((dxTriMesh*)g)->doCapsuleTC) + return 1; + break; + } + return 0; +} + +void dGeomTriMeshClearTCCache(dGeomID g){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + Geom->ClearTCCache(); +} + +/* + * returns the TriMeshDataID + */ +dTriMeshDataID +dGeomTriMeshGetTriMeshDataID(dGeomID g) +{ + dxTriMesh* Geom = (dxTriMesh*) g; + return Geom->Data; +} + +// Getting data +void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + gim_trimesh_locks_work_data(&Geom->m_collision_trimesh); + gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, (*v0),(*v1),(*v2)); + gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh); + +} + +void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + dVector3 dv[3]; + gim_trimesh_locks_work_data(&Geom->m_collision_trimesh); + gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, dv[0],dv[1],dv[2]); + GetPointFromBarycentric(dv, u, v, Out); + gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh); +} + +int dGeomTriMeshGetTriangleCount (dGeomID g) +{ + dxTriMesh* Geom = (dxTriMesh*)g; + return gim_trimesh_get_triangle_count(&Geom->m_collision_trimesh); +} + +void dGeomTriMeshDataUpdate(dTriMeshDataID g) { + dUASSERT(g, "argument not trimesh data"); + g->UpdateData(); +} + + +// +// GIMPACT TRIMESH-TRIMESH COLLIDER +// + +int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (g2->type == dTriMeshClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh1 = (dxTriMesh*) g1; + dxTriMesh* TriMesh2 = (dxTriMesh*) g2; + //Create contact list + GDYNAMIC_ARRAY trimeshcontacts; + GIM_CREATE_CONTACT_LIST(trimeshcontacts); + + //Collide trimeshes + gim_trimesh_trimesh_collision(&TriMesh1->m_collision_trimesh,&TriMesh2->m_collision_trimesh,&trimeshcontacts); + + if(trimeshcontacts.m_size == 0) + { + GIM_DYNARRAY_DESTROY(trimeshcontacts); + return 0; + } + + GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts); + + + unsigned contactcount = trimeshcontacts.m_size; + unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK); + if (contactcount > maxcontacts) + { + contactcount = maxcontacts; + } + + dContactGeom* pcontact; + unsigned i; + + for (i=0;ipos[0] = ptrimeshcontacts->m_point[0]; + pcontact->pos[1] = ptrimeshcontacts->m_point[1]; + pcontact->pos[2] = ptrimeshcontacts->m_point[2]; + pcontact->pos[3] = 1.0f; + + pcontact->normal[0] = ptrimeshcontacts->m_normal[0]; + pcontact->normal[1] = ptrimeshcontacts->m_normal[1]; + pcontact->normal[2] = ptrimeshcontacts->m_normal[2]; + pcontact->normal[3] = 0; + + pcontact->depth = ptrimeshcontacts->m_depth; + pcontact->g1 = g1; + pcontact->g2 = g2; + + ptrimeshcontacts++; + } + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} + +#endif // dTRIMESH_GIMPACT +#endif // dTRIMESH_ENABLED diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_internal.h b/libraries/ode-0.9/ode/src/collision_trimesh_internal.h new file mode 100644 index 0000000..bf474a2 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_internal.h @@ -0,0 +1,495 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. +// Modified for FreeSOLID Compatibility by Rodrigo Hernandez + +#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_ +#define _ODE_COLLISION_TRIMESH_INTERNAL_H_ + +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags, + dContactGeom *contact, int skip ) +{ + // Swapped case, for code that needs it (heightfield initially) + // The other ray-geom colliders take geoms in a swapped order to the + // dCollideRTL function which is annoying when using function pointers. + return dCollideRTL( trimesh, ray, flags, contact, skip ); +} + +//**************************************************************************** +// dxTriMesh class + +#ifdef TRIMESH_INTERNAL + +#include "collision_kernel.h" +#include + +#if dTRIMESH_OPCODE +#define BAN_OPCODE_AUTOLINK +#include "Opcode.h" +using namespace Opcode; +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +#include +#endif + +struct dxTriMeshData : public dBase +{ + /* Array of flags for which edges and verts should be used on each triangle */ + enum UseFlags + { + kEdge0 = 0x1, + kEdge1 = 0x2, + kEdge2 = 0x4, + kVert0 = 0x8, + kVert1 = 0x10, + kVert2 = 0x20, + + kUseAll = 0xFF, + }; + + /* Setup the UseFlags array */ + void Preprocess(); + /* For when app changes the vertices */ + void UpdateData(); + +#if dTRIMESH_OPCODE + Model BVTree; + MeshInterface Mesh; + + dxTriMeshData(); + ~dxTriMeshData(); + + void Build(const void* Vertices, int VertexStide, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals, + bool Single); + + /* aabb in model space */ + dVector3 AABBCenter; + dVector3 AABBExtents; + + // data for use in collision resolution + const void* Normals; + uint8* UseFlags; +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT + const char* m_Vertices; + int m_VertexStride; + int m_VertexCount; + const char* m_Indices; + int m_TriangleCount; + int m_TriStride; + bool m_single; + + dxTriMeshData() + { + m_Vertices=NULL; + m_VertexStride = 12; + m_VertexCount = 0; + m_Indices = 0; + m_TriangleCount = 0; + m_TriStride = 12; + m_single = true; + } + + void Build(const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals, + bool Single) + { + dIASSERT(Vertices); + dIASSERT(Indices); + dIASSERT(VertexStride); + dIASSERT(TriStride); + dIASSERT(IndexCount); + m_Vertices=(const char *)Vertices; + m_VertexStride = VertexStride; + m_VertexCount = VertexCount; + m_Indices = (const char *)Indices; + m_TriangleCount = IndexCount/3; + m_TriStride = TriStride; + m_single = Single; + } + + inline void GetVertex(unsigned int i, dVector3 Out) + { + if(m_single) + { + const float * fverts = (const float * )(m_Vertices + m_VertexStride*i); + Out[0] = fverts[0]; + Out[1] = fverts[1]; + Out[2] = fverts[2]; + Out[3] = 1.0f; + } + else + { + const double * dverts = (const double * )(m_Vertices + m_VertexStride*i); + Out[0] = (float)dverts[0]; + Out[1] = (float)dverts[1]; + Out[2] = (float)dverts[2]; + Out[3] = 1.0f; + + } + } + + inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3]) + { + const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle); + triindices[0] = ind[0]; + triindices[1] = ind[1]; + triindices[2] = ind[2]; + } +#endif // dTRIMESH_GIMPACT +}; + + +struct dxTriMesh : public dxGeom{ + // Callbacks + dTriCallback* Callback; + dTriArrayCallback* ArrayCallback; + dTriRayCallback* RayCallback; + + // Data types + dxTriMeshData* Data; + + bool doSphereTC; + bool doBoxTC; + bool doCapsuleTC; + + // Functions + dxTriMesh(dSpaceID Space, dTriMeshDataID Data); + ~dxTriMesh(); + + void ClearTCCache(); + + int AABBTest(dxGeom* g, dReal aabb[6]); + void computeAABB(); + +#if dTRIMESH_OPCODE + // Instance data for last transform. + dMatrix4 last_trans; + + // Colliders + static PlanesCollider _PlanesCollider; + static SphereCollider _SphereCollider; + static OBBCollider _OBBCollider; + static RayCollider _RayCollider; + static AABBTreeCollider _AABBTreeCollider; + static LSSCollider _LSSCollider; + + // Some constants + static CollisionFaces Faces; + // Temporal coherence + struct SphereTC : public SphereCache{ + dxGeom* Geom; + }; + dArray SphereTCCache; + static SphereCache defaultSphereCache; + + struct BoxTC : public OBBCache{ + dxGeom* Geom; + }; + dArray BoxTCCache; + static OBBCache defaultBoxCache; + + struct CapsuleTC : public LSSCache{ + dxGeom* Geom; + }; + dArray CapsuleTCCache; + static LSSCache defaultCapsuleCache; +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT + GIM_TRIMESH m_collision_trimesh; +#endif // dTRIMESH_GIMPACT +}; + +#if 0 +#include "collision_kernel.h" +// Fetches a contact +inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ + dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK)); + return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); +} +#endif + +#if dTRIMESH_OPCODE +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){ + VertexPointers VP; + TriMesh->Data->Mesh.GetTriangle(VP, Index); + for (int i = 0; i < 3; i++){ + Out[i][0] = VP.Vertex[i]->x; + Out[i][1] = VP.Vertex[i]->y; + Out[i][2] = VP.Vertex[i]->z; + Out[i][3] = 0; + } +} + +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){ + VertexPointers VP; + TriMesh->Data->Mesh.GetTriangle(VP, Index); + for (int i = 0; i < 3; i++){ + dVector3 v; + v[0] = VP.Vertex[i]->x; + v[1] = VP.Vertex[i]->y; + v[2] = VP.Vertex[i]->z; + v[3] = 0; + + dMULTIPLY0_331(Out[i], Rotation, v); + Out[i][0] += Position[0]; + Out[i][1] += Position[1]; + Out[i][2] += Position[2]; + Out[i][3] = 0; + } +} + +inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){ + Out.m[0][0] = (float) Rotation[0]; + Out.m[1][0] = (float) Rotation[1]; + Out.m[2][0] = (float) Rotation[2]; + + Out.m[0][1] = (float) Rotation[4]; + Out.m[1][1] = (float) Rotation[5]; + Out.m[2][1] = (float) Rotation[6]; + + Out.m[0][2] = (float) Rotation[8]; + Out.m[1][2] = (float) Rotation[9]; + Out.m[2][2] = (float) Rotation[10]; + + Out.m[3][0] = (float) Position[0]; + Out.m[3][1] = (float) Position[1]; + Out.m[3][2] = (float) Position[2]; + + Out.m[0][3] = 0.0f; + Out.m[1][3] = 0.0f; + Out.m[2][3] = 0.0f; + Out.m[3][3] = 1.0f; + + return Out; +} + +inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + return MakeMatrix(Position, Rotation, Out); +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){ + // why is this not implemented? + dAASSERT(false); +} + +inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m) +{ + m[0][0] = (float) Rotation[0]; + m[0][1] = (float) Rotation[1]; + m[0][2] = (float) Rotation[2]; + + m[1][0] = (float) Rotation[4]; + m[1][1] = (float) Rotation[5]; + m[1][2] = (float) Rotation[6]; + + m[2][0] = (float) Rotation[8]; + m[2][1] = (float) Rotation[9]; + m[2][2] = (float) Rotation[10]; + + m[0][3] = (float) Position[0]; + m[1][3] = (float) Position[1]; + m[2][3] = (float) Position[2]; + +} + +inline void MakeMatrix(dxGeom* g, mat4f Out){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + MakeMatrix(Position, Rotation, Out); +} +#endif // dTRIMESH_GIMPACT + +// Outputs a matrix to 3 vectors +inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){ + Right[0] = Matrix[0 * 4 + 0]; + Right[1] = Matrix[1 * 4 + 0]; + Right[2] = Matrix[2 * 4 + 0]; + Right[3] = REAL(0.0); + Up[0] = Matrix[0 * 4 + 1]; + Up[1] = Matrix[1 * 4 + 1]; + Up[2] = Matrix[2 * 4 + 1]; + Up[3] = REAL(0.0); + Direction[0] = Matrix[0 * 4 + 2]; + Direction[1] = Matrix[1 * 4 + 2]; + Direction[2] = Matrix[2 * 4 + 2]; + Direction[3] = REAL(0.0); +} + +// Outputs a matrix to 3 vectors +inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){ + Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]); +} + +// Finds barycentric +inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){ + dReal w = REAL(1.0) - u - v; + + Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v); + Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v); + Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v); + Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v); +} + +// Performs a callback +inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){ + if (TriMesh->Callback != NULL){ + return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0); + } + else return true; +} + +// Some utilities +template const T& dcMAX(const T& x, const T& y){ + return x > y ? x : y; +} + +template const T& dcMIN(const T& x, const T& y){ + return x < y ? x : y; +} + +dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, + const dVector3 triEdge1, const dVector3 triEdge2, + dReal* pfSParam = 0, dReal* pfTParam = 0 ); + +dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, + const dVector3 seg2Origin, const dVector3 seg2Direction, + dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 ); + +dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, + const dVector3 triOrigin, + const dVector3 triEdge1, const dVector3 triEdge2, + dReal* t = 0, dReal* u = 0, dReal* v = 0 ); + +inline +void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result ) +{ + result[0] = left[0] - right[0]; + result[1] = left[1] - right[1]; + result[2] = left[2] - right[2]; + result[3] = REAL(0.0); +} + +inline +void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result ) +{ + result[0] = left[0] + right[0]; + result[1] = left[1] + right[1]; + result[2] = left[2] + right[2]; + result[3] = REAL(0.0); +} + +inline +void Vector3Negate( const dVector3 in, dVector3 out ) +{ + out[0] = -in[0]; + out[1] = -in[1]; + out[2] = -in[2]; + out[3] = REAL(0.0); +} + +inline +void Vector3Copy( const dVector3 in, dVector3 out ) +{ + out[0] = in[0]; + out[1] = in[1]; + out[2] = in[2]; + out[3] = REAL(0.0); +} + +inline +void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out ) +{ + out[0] = in[0] * scalar; + out[1] = in[1] * scalar; + out[2] = in[2] * scalar; + out[3] = REAL(0.0); +} + +inline +void TransformVector3( const dVector3 in, + const dMatrix3 orientation, const dVector3 position, + dVector3 out ) +{ + dMULTIPLY0_331( out, orientation, in ); + out[0] += position[0]; + out[1] += position[1]; + out[2] += position[2]; +} + +//------------------------------------------------------------------------------ +/** + @brief Check for intersection between triangle and capsule. + + @param dist [out] Shortest distance squared between the triangle and + the capsule segment (central axis). + @param t [out] t value of point on segment that's the shortest distance + away from the triangle, the coordinates of this point + can be found by (cap.seg.end - cap.seg.start) * t, + or cap.seg.ipol(t). + @param u [out] Barycentric coord on triangle. + @param v [out] Barycentric coord on triangle. + @return True if intersection exists. + + The third Barycentric coord is implicit, ie. w = 1.0 - u - v + The Barycentric coords give the location of the point on the triangle + closest to the capsule (where the distance between the two shapes + is the shortest). +*/ +inline +bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, + const dReal radius, const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* dist, dReal* t, dReal* u, dReal* v ) +{ + dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, + t, u, v ); + + if ( dist ) + *dist = sqrDist; + + return ( sqrDist <= (radius * radius) ); +} + +#endif //TRIMESH_INTERNAL + +#endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_ diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp new file mode 100644 index 0000000..07f3f8a --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp @@ -0,0 +1,833 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. + +#include +#include +#include +#include +#include "collision_util.h" +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_ENABLED +#if dTRIMESH_OPCODE + +// Trimesh data +dxTriMeshData::dxTriMeshData() : UseFlags( NULL ) +{ +#if !dTRIMESH_ENABLED + dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work"); +#endif +} + +dxTriMeshData::~dxTriMeshData() +{ + if ( UseFlags ) + delete [] UseFlags; +} + +void +dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* in_Normals, + bool Single) +{ +#if dTRIMESH_ENABLED + + Mesh.SetNbTriangles(IndexCount / 3); + Mesh.SetNbVertices(VertexCount); + Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices); + Mesh.SetStrides(TriStride, VertexStide); + Mesh.Single = Single; + + // Build tree + BuildSettings Settings; + // recommended in Opcode User Manual + //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER; + // used in ODE, why? + //Settings.mRules = SPLIT_BEST_AXIS; + + // best compromise? + Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER; + + + OPCODECREATE TreeBuilder; + TreeBuilder.mIMesh = &Mesh; + + TreeBuilder.mSettings = Settings; + TreeBuilder.mNoLeaf = true; + TreeBuilder.mQuantized = false; + + TreeBuilder.mKeepOriginal = false; + TreeBuilder.mCanRemap = false; + + + + BVTree.Build(TreeBuilder); + + // compute model space AABB + dVector3 AABBMax, AABBMin; + AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity; + AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity; + if( Single ) { + const char* verts = (const char*)Vertices; + for( int i = 0; i < VertexCount; ++i ) { + const float* v = (const float*)verts; + if( v[0] > AABBMax[0] ) AABBMax[0] = v[0]; + if( v[1] > AABBMax[1] ) AABBMax[1] = v[1]; + if( v[2] > AABBMax[2] ) AABBMax[2] = v[2]; + if( v[0] < AABBMin[0] ) AABBMin[0] = v[0]; + if( v[1] < AABBMin[1] ) AABBMin[1] = v[1]; + if( v[2] < AABBMin[2] ) AABBMin[2] = v[2]; + verts += VertexStide; + } + } else { + const char* verts = (const char*)Vertices; + for( int i = 0; i < VertexCount; ++i ) { + const double* v = (const double*)verts; + if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0]; + if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1]; + if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2]; + if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0]; + if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1]; + if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2]; + verts += VertexStide; + } + } + AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5); + AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5); + AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5); + AABBExtents[0] = AABBMax[0] - AABBCenter[0]; + AABBExtents[1] = AABBMax[1] - AABBCenter[1]; + AABBExtents[2] = AABBMax[2] - AABBCenter[2]; + + // user data (not used by OPCODE) + Normals = (dReal *) in_Normals; + + UseFlags = 0; + +#endif // dTRIMESH_ENABLED +} + +struct EdgeRecord +{ + int VertIdx1; // Index into vertex array for this edges vertices + int VertIdx2; + int TriIdx; // Index into triangle array for triangle this edge belongs to + + uint8 EdgeFlags; + uint8 Vert1Flags; + uint8 Vert2Flags; + bool Concave; +}; + +// Edge comparison function for qsort +static int EdgeCompare(const void* edge1, const void* edge2) +{ + EdgeRecord* e1 = (EdgeRecord*)edge1; + EdgeRecord* e2 = (EdgeRecord*)edge2; + + if (e1->VertIdx1 == e2->VertIdx1) + return e1->VertIdx2 - e2->VertIdx2; + else + return e1->VertIdx1 - e2->VertIdx1; +} + +void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const unsigned int* vertIdxs) +{ + if (edgeIdx == 0) + { + edge->EdgeFlags = dxTriMeshData::kEdge0; + edge->Vert1Flags = dxTriMeshData::kVert0; + edge->Vert2Flags = dxTriMeshData::kVert1; + edge->VertIdx1 = vertIdxs[0]; + edge->VertIdx2 = vertIdxs[1]; + } + else if (edgeIdx == 1) + { + edge->EdgeFlags = dxTriMeshData::kEdge1; + edge->Vert1Flags = dxTriMeshData::kVert1; + edge->Vert2Flags = dxTriMeshData::kVert2; + edge->VertIdx1 = vertIdxs[1]; + edge->VertIdx2 = vertIdxs[2]; + } + else if (edgeIdx == 2) + { + edge->EdgeFlags = dxTriMeshData::kEdge2; + edge->Vert1Flags = dxTriMeshData::kVert2; + edge->Vert2Flags = dxTriMeshData::kVert0; + edge->VertIdx1 = vertIdxs[2]; + edge->VertIdx2 = vertIdxs[0]; + } + + // Make sure vert index 1 is less than index 2 (for easier sorting) + if (edge->VertIdx1 > edge->VertIdx2) + { + unsigned int tempIdx = edge->VertIdx1; + edge->VertIdx1 = edge->VertIdx2; + edge->VertIdx2 = tempIdx; + + uint8 tempFlags = edge->Vert1Flags; + edge->Vert1Flags = edge->Vert2Flags; + edge->Vert2Flags = tempFlags; + } + + edge->TriIdx = triIdx; + edge->Concave = false; +} + +#if dTRIMESH_ENABLED + +// Get the vertex opposite this edge in the triangle +inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[]) +{ + if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) || + (edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0)) + { + return *vertices[2]; + } + else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) || + (edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1)) + { + return *vertices[0]; + } + else + return *vertices[1]; +} + +#endif // dTRIMESH_ENABLED + +void dxTriMeshData::Preprocess() +{ + +#if dTRIMESH_ENABLED + + // If this mesh has already been preprocessed, exit + if (UseFlags) + return; + + udword numTris = Mesh.GetNbTriangles(); + udword numEdges = numTris * 3; + + UseFlags = new uint8[numTris]; + memset(UseFlags, 0, sizeof(uint8) * numTris); + + EdgeRecord* records = new EdgeRecord[numEdges]; + + // Make a list of every edge in the mesh + const IndexedTriangle* tris = Mesh.GetTris(); + for (unsigned int i = 0; i < numTris; i++) + { + SetupEdge(&records[i*3], 0, i, tris->mVRef); + SetupEdge(&records[i*3+1], 1, i, tris->mVRef); + SetupEdge(&records[i*3+2], 2, i, tris->mVRef); + + tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride()); + } + + // Sort the edges, so the ones sharing the same verts are beside each other + qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare); + + // Go through the sorted list of edges and flag all the edges and vertices that we need to use + for (unsigned int i = 0; i < numEdges; i++) + { + EdgeRecord* rec1 = &records[i]; + EdgeRecord* rec2 = 0; + if (i < numEdges - 1) + rec2 = &records[i+1]; + + if (rec2 && + rec1->VertIdx1 == rec2->VertIdx1 && + rec1->VertIdx2 == rec2->VertIdx2) + { + VertexPointers vp; + Mesh.GetTriangle(vp, rec1->TriIdx); + + // Get the normal of the first triangle + Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]); + triNorm.Normalize(); + + // Get the vert opposite this edge in the first triangle + Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex); + + // Get the vert opposite this edge in the second triangle + Mesh.GetTriangle(vp, rec2->TriIdx); + Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex); + + float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize()); + + // We let the dot threshold for concavity get slightly negative to allow for rounding errors + static const float kConcaveThresh = -0.000001f; + + // This is a concave edge, leave it for the next pass + if (dot >= kConcaveThresh) + rec1->Concave = true; + // If this is a convex edge, mark its vertices and edge as used + else + UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags; + + // Skip the second edge + i++; + } + // This is a boundary edge + else + { + UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags; + } + } + + // Go through the list once more, and take any edge we marked as concave and + // clear it's vertices flags in any triangles they're used in + for (unsigned int i = 0; i < numEdges; i++) + { + EdgeRecord& er = records[i]; + + if (er.Concave) + { + for (unsigned int j = 0; j < numEdges; j++) + { + EdgeRecord& curER = records[j]; + + if (curER.VertIdx1 == er.VertIdx1 || + curER.VertIdx1 == er.VertIdx2) + UseFlags[curER.TriIdx] &= ~curER.Vert1Flags; + + if (curER.VertIdx2 == er.VertIdx1 || + curER.VertIdx2 == er.VertIdx2) + UseFlags[curER.TriIdx] &= ~curER.Vert2Flags; + } + } + } + + delete [] records; + +#endif // dTRIMESH_ENABLED + +} + +dTriMeshDataID dGeomTriMeshDataCreate(){ + return new dxTriMeshData(); +} + +void dGeomTriMeshDataDestroy(dTriMeshDataID g){ + delete g; +} + + + + +void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) +{ + dAASSERT(g) + dUASSERT(g->type == dTriMeshClass, "geom not trimesh"); + + for (int i=0; i<16; i++) + (((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ]; + + return; +} + + +dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) +{ + dAASSERT(g) + dUASSERT(g->type == dTriMeshClass, "geom not trimesh"); + + return (dReal*)(((dxTriMesh*)g)->last_trans); +} + + + + +void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) +{ + dUASSERT(g, "argument not trimesh data"); + + switch (data_id) + { + case TRIMESH_FACE_NORMALS: + g->Normals = (dReal *) in_data; + break; + + default: + dUASSERT(data_id, "invalid data type"); + break; + } + + return; +} + + + +void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) +{ + dUASSERT(g, "argument not trimesh data"); + + switch (data_id) + { + case TRIMESH_FACE_NORMALS: + return (void *) g->Normals; + break; + + default: + dUASSERT(data_id, "invalid data type"); + break; + } + + return NULL; +} + + +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "argument not trimesh data"); + + g->Build(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + true); +} + + +void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, (void*)NULL); +} + + +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "argument not trimesh data"); + + g->Build(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + false); +} + + +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) { + dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, NULL); +} + + +void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount, + const int* Normals){ +#ifdef dSINGLE + dGeomTriMeshDataBuildSingle1(g, + Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(unsigned int), + Normals); +#else + dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(unsigned int), + Normals); +#endif +} + + +void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount) { + dGeomTriMeshDataBuildSimple1(g, + Vertices, VertexCount, Indices, IndexCount, + (const int*)NULL); +} + +void dGeomTriMeshDataPreprocess(dTriMeshDataID g) +{ + dUASSERT(g, "argument not trimesh data"); + g->Preprocess(); +} + +void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen) +{ + dUASSERT(g, "argument not trimesh data"); +#if dTRIMESH_ENABLED + *buf = g->UseFlags; + *bufLen = g->Mesh.GetNbTriangles(); +#endif // dTRIMESH_ENABLED +} + +void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf) +{ + dUASSERT(g, "argument not trimesh data"); + g->UseFlags = buf; +} + + +#if dTRIMESH_ENABLED + +// Trimesh Class Statics +PlanesCollider dxTriMesh::_PlanesCollider; +SphereCollider dxTriMesh::_SphereCollider; +OBBCollider dxTriMesh::_OBBCollider; +RayCollider dxTriMesh::_RayCollider; +AABBTreeCollider dxTriMesh::_AABBTreeCollider; +LSSCollider dxTriMesh::_LSSCollider; + +SphereCache dxTriMesh::defaultSphereCache; +OBBCache dxTriMesh::defaultBoxCache; +LSSCache dxTriMesh::defaultCapsuleCache; + +CollisionFaces dxTriMesh::Faces; + +#endif // dTRIMESH_ENABLED + + +dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1) +{ + type = dTriMeshClass; + + this->Data = Data; + +#if dTRIMESH_ENABLED + + _RayCollider.SetDestination(&Faces); + + _PlanesCollider.SetTemporalCoherence(true); + + _SphereCollider.SetTemporalCoherence(true); + _SphereCollider.SetPrimitiveTests(false); + + _OBBCollider.SetTemporalCoherence(true); + + // no first-contact test (i.e. return full contact info) + _AABBTreeCollider.SetFirstContact( false ); + // temporal coherence only works with "first conact" tests + _AABBTreeCollider.SetTemporalCoherence(false); + // Perform full BV-BV tests (true) or SAT-lite tests (false) + _AABBTreeCollider.SetFullBoxBoxTest( true ); + // Perform full Primitive-BV tests (true) or SAT-lite tests (false) + _AABBTreeCollider.SetFullPrimBoxTest( true ); + _LSSCollider.SetTemporalCoherence(false); + +#endif // dTRIMESH_ENABLED + + /* TC has speed/space 'issues' that don't make it a clear + win by default on spheres/boxes. */ + this->doSphereTC = false; + this->doBoxTC = false; + this->doCapsuleTC = false; + +#if dTRIMESH_ENABLED + + const char* msg; + if ((msg =_AABBTreeCollider.ValidateSettings())) + dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__); + _LSSCollider.SetPrimitiveTests(false); + _LSSCollider.SetFirstContact(false); + +#endif // dTRIMESH_ENABLED + + for (int i=0; i<16; i++) + last_trans[i] = REAL( 0.0 ); +} + +dxTriMesh::~dxTriMesh(){ + // +} + +// Cleanup for allocations when shutting down ODE +void opcode_collider_cleanup() +{ +#if dTRIMESH_ENABLED + + // Clear TC caches + dxTriMesh::Faces.Empty(); + dxTriMesh::defaultSphereCache.TouchedPrimitives.Empty(); + dxTriMesh::defaultBoxCache.TouchedPrimitives.Empty(); + dxTriMesh::defaultCapsuleCache.TouchedPrimitives.Empty(); + +#endif // dTRIMESH_ENABLED +} + + + +void dxTriMesh::ClearTCCache() +{ +#if dTRIMESH_ENABLED + /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches - + but the destructor isn't called when doing this, so we would leak. + So, call the previous caches' containers' destructors by hand first. */ + int i, n; + n = SphereTCCache.size(); + for( i = 0; i < n; ++i ) { + SphereTCCache[i].~SphereTC(); + } + SphereTCCache.setSize(0); + n = BoxTCCache.size(); + for( i = 0; i < n; ++i ) { + BoxTCCache[i].~BoxTC(); + } + BoxTCCache.setSize(0); + n = CapsuleTCCache.size(); + for( i = 0; i < n; ++i ) { + CapsuleTCCache[i].~CapsuleTC(); + } + CapsuleTCCache.setSize(0); +#endif // dTRIMESH_ENABLED +} + + +int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){ + return 1; +} + + +void dxTriMesh::computeAABB() { + const dxTriMeshData* d = Data; + dVector3 c; + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dMULTIPLY0_331( c, R, d->AABBCenter ); + + dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) + + dFabs(R[1] * Data->AABBExtents[1]) + + dFabs(R[2] * Data->AABBExtents[2]); + dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) + + dFabs(R[5] * Data->AABBExtents[1]) + + dFabs(R[6] * Data->AABBExtents[2]); + dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) + + dFabs(R[9] * Data->AABBExtents[1]) + + dFabs(R[10] * Data->AABBExtents[2]); + + aabb[0] = c[0] + pos[0] - xrange; + aabb[1] = c[0] + pos[0] + xrange; + aabb[2] = c[1] + pos[1] - yrange; + aabb[3] = c[1] + pos[1] + yrange; + aabb[4] = c[2] + pos[2] - zrange; + aabb[5] = c[2] + pos[2] + zrange; +} + + +void dxTriMeshData::UpdateData() +{ +#if dTRIMESH_ENABLED + BVTree.Refit(); +#endif // dTRIMESH_ENABLED +} + + +dGeomID dCreateTriMesh(dSpaceID space, + dTriMeshDataID Data, + dTriCallback* Callback, + dTriArrayCallback* ArrayCallback, + dTriRayCallback* RayCallback) +{ + dxTriMesh* Geom = new dxTriMesh(space, Data); + Geom->Callback = Callback; + Geom->ArrayCallback = ArrayCallback; + Geom->RayCallback = RayCallback; + + return Geom; +} + +void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->Callback = Callback; +} + +dTriCallback* dGeomTriMeshGetCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->Callback; +} + +void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->ArrayCallback = ArrayCallback; +} + +dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->ArrayCallback; +} + +void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->RayCallback = Callback; +} + +dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->RayCallback; +} + +void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->Data = Data; + // I changed my data -- I know nothing about my own AABB anymore. + ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD); +} + +dTriMeshDataID dGeomTriMeshGetData(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->Data; +} + + + +void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + switch (geomClass) + { + case dSphereClass: + ((dxTriMesh*)g)->doSphereTC = (1 == enable); + break; + case dBoxClass: + ((dxTriMesh*)g)->doBoxTC = (1 == enable); + break; + case dCapsuleClass: + ((dxTriMesh*)g)->doCapsuleTC = (1 == enable); + break; + } +} + +int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + switch (geomClass) + { + case dSphereClass: + if (((dxTriMesh*)g)->doSphereTC) + return 1; + break; + case dBoxClass: + if (((dxTriMesh*)g)->doBoxTC) + return 1; + break; + case dCapsuleClass: + if (((dxTriMesh*)g)->doCapsuleTC) + return 1; + break; + } + return 0; +} + +void dGeomTriMeshClearTCCache(dGeomID g){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + Geom->ClearTCCache(); +} + +/* + * returns the TriMeshDataID + */ +dTriMeshDataID +dGeomTriMeshGetTriMeshDataID(dGeomID g) +{ + dxTriMesh* Geom = (dxTriMesh*) g; + return Geom->Data; +} + +// Getting data +void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + + dVector3 v[3]; + FetchTriangle(Geom, Index, Position, Rotation, v); + + if (v0){ + (*v0)[0] = v[0][0]; + (*v0)[1] = v[0][1]; + (*v0)[2] = v[0][2]; + (*v0)[3] = v[0][3]; + } + if (v1){ + (*v1)[0] = v[1][0]; + (*v1)[1] = v[1][1]; + (*v1)[2] = v[1][2]; + (*v1)[3] = v[1][3]; + } + if (v2){ + (*v2)[0] = v[2][0]; + (*v2)[1] = v[2][1]; + (*v2)[2] = v[2][2]; + (*v2)[3] = v[2][3]; + } +} + +void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + + dVector3 dv[3]; + FetchTriangle(Geom, Index, Position, Rotation, dv); + + GetPointFromBarycentric(dv, u, v, Out); +} + +int dGeomTriMeshGetTriangleCount (dGeomID g) +{ +#if dTRIMESH_ENABLED + dxTriMesh* Geom = (dxTriMesh*)g; + return Geom->Data->Mesh.GetNbTriangles(); +#else + return 0; +#endif // dTRIMESH_ENABLED +} + +void dGeomTriMeshDataUpdate(dTriMeshDataID g) { + dUASSERT(g, "argument not trimesh data"); + g->UpdateData(); +} + +#endif // dTRIMESH_OPCODE +#endif // dTRIMESH_ENABLED diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp new file mode 100644 index 0000000..54034aa --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp @@ -0,0 +1,213 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh - Plane collider by David Walters, July 2006 + +#include +#include +#include +#include + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#include "collision_std.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE +int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dTriMeshClass ); + dIASSERT( o2->type == dPlaneClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Alias pointers to the plane and trimesh + dxTriMesh* trimesh = (dxTriMesh*)( o1 ); + dxPlane* plane = (dxPlane*)( o2 ); + + int contact_count = 0; + + // Cache the maximum contact count. + const int contact_max = ( flags & NUMC_MASK ); + + // Cache trimesh position and rotation. + const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh ); + const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh ); + + // + // For all triangles. + // + + // Cache the triangle count. + const int tri_count = trimesh->Data->Mesh.GetNbTriangles(); + + VertexPointers VP; + dReal alpha; + dVector3 vertex; + +#if !defined(dSINGLE) || 1 + dVector3 int_vertex; // Intermediate vertex for double precision mode. +#endif // dSINGLE + + // For each triangle + for ( int t = 0; t < tri_count; ++t ) + { + // Get triangle, which should also use callback. + trimesh->Data->Mesh.GetTriangle( VP, t ); + + // For each vertex. + for ( int v = 0; v < 3; ++v ) + { + // + // Get Vertex + // + +#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation + + dMULTIPLY0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) ); + +#else // dDOUBLE || 1 + + // OPCODE data is in single precision format. + int_vertex[ 0 ] = VP.Vertex[ v ]->x; + int_vertex[ 1 ] = VP.Vertex[ v ]->y; + int_vertex[ 2 ] = VP.Vertex[ v ]->z; + + dMULTIPLY0_331( vertex, trimesh_R, int_vertex ); + +#endif // dSINGLE/dDOUBLE + + vertex[ 0 ] += trimesh_pos[ 0 ]; + vertex[ 1 ] += trimesh_pos[ 1 ]; + vertex[ 2 ] += trimesh_pos[ 2 ]; + + + // + // Collision? + // + + // If alpha < 0 then point is if front of plane. i.e. no contact + // If alpha = 0 then the point is on the plane + alpha = plane->p[ 3 ] - dDOT( plane->p, vertex ); + + // If alpha > 0 the point is behind the plane. CONTACT! + if ( alpha > 0 ) + { + // Alias the contact + dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip ); + + contact->pos[ 0 ] = vertex[ 0 ]; + contact->pos[ 1 ] = vertex[ 1 ]; + contact->pos[ 2 ] = vertex[ 2 ]; + + contact->normal[ 0 ] = plane->p[ 0 ]; + contact->normal[ 1 ] = plane->p[ 1 ]; + contact->normal[ 2 ] = plane->p[ 2 ]; + + contact->depth = alpha; + contact->g1 = plane; + contact->g2 = trimesh; + + ++contact_count; + + // All contact slots are full? + if ( contact_count >= contact_max ) + return contact_count; // <=== STOP HERE + } + } + } + + // Return contact count. + return contact_count; +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dTriMeshClass ); + dIASSERT( o2->type == dPlaneClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // Alias pointers to the plane and trimesh + dxTriMesh* trimesh = (dxTriMesh*)( o1 ); + vec4f plane; + dGeomPlaneGetParams(o2, plane); + + //Find collision + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result); + + gim_trimesh_plane_collision(&trimesh->m_collision_trimesh,plane,&collision_result); + + if(collision_result.m_size == 0 ) + { + GIM_DYNARRAY_DESTROY(collision_result); + return 0; + } + + + unsigned int contactcount = collision_result.m_size; + unsigned int contactmax = (unsigned int)(flags & NUMC_MASK); + if (contactcount > contactmax) + { + contactcount = contactmax; + } + + dContactGeom* pcontact; + vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result); + + for(unsigned int i = 0; i < contactcount; i++ ) + { + pcontact = SAFECONTACT(flags, contacts, i, skip); + + pcontact->pos[0] = (*planecontact_results)[0]; + pcontact->pos[1] = (*planecontact_results)[1]; + pcontact->pos[2] = (*planecontact_results)[2]; + pcontact->pos[3] = REAL(1.0); + + pcontact->normal[0] = plane[0]; + pcontact->normal[1] = plane[1]; + pcontact->normal[2] = plane[2]; + pcontact->normal[3] = 0; + + pcontact->depth = (*planecontact_results)[3]; + pcontact->g1 = o1; + pcontact->g2 = o2; + + planecontact_results++; + } + + GIM_DYNARRAY_DESTROY(collision_result); + + return (int)contactcount; +} +#endif // dTRIMESH_GIMPACT + + +#endif // dTRIMESH_ENABLED + diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp new file mode 100644 index 0000000..c0b97ca --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp @@ -0,0 +1,198 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. + +#include +#include +#include +#include + +#if dTRIMESH_ENABLED + +#include "collision_util.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (RayGeom->type == dRayClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + RayCollider& Collider = TriMesh->_RayCollider; + + dReal Length = dGeomRayGetLength(RayGeom); + + int FirstContact, BackfaceCull; + dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + + Collider.SetFirstContact(FirstContact != 0); + Collider.SetClosestHit(ClosestHit != 0); + Collider.SetCulling(BackfaceCull != 0); + Collider.SetMaxDist(Length); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + /* Make Ray */ + Ray WorldRay; + WorldRay.mOrig.x = Origin[0]; + WorldRay.mOrig.y = Origin[1]; + WorldRay.mOrig.z = Origin[2]; + WorldRay.mDir.x = Direction[0]; + WorldRay.mDir.y = Direction[1]; + WorldRay.mDir.z = Direction[2]; + + /* Intersect */ + Matrix4x4 amatrix; + int TriCount = 0; + if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) { + TriCount = TriMesh->Faces.GetNbFaces(); + } + + if (TriCount == 0) { + return 0; + } + + const CollisionFace* Faces = TriMesh->Faces.GetFaces(); + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++) { + if (TriMesh->RayCallback == null || + TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, + Faces[i].mU, Faces[i].mV)) { + const int& TriIndex = Faces[i].mFaceID; + if (!Callback(TriMesh, RayGeom, TriIndex)) { + continue; + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + dVector3 dv[3]; + FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); + + // No sense to save on single type conversion in algorithm of this size. + // If there would be a custom typedef for distance type it could be used + // instead of dReal. However using float directly is the loss of abstraction + // and possible loss of precision in future. + /*float*/ dReal T = Faces[i].mDistance; + Contact->pos[0] = Origin[0] + (Direction[0] * T); + Contact->pos[1] = Origin[1] + (Direction[1] * T); + Contact->pos[2] = Origin[2] + (Direction[2] * T); + Contact->pos[3] = REAL(0.0); + + dVector3 vu; + vu[0] = dv[1][0] - dv[0][0]; + vu[1] = dv[1][1] - dv[0][1]; + vu[2] = dv[1][2] - dv[0][2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = dv[2][0] - dv[0][0]; + vv[1] = dv[2][1] - dv[0][1]; + vv[2] = dv[2][2] - dv[0][2]; + vv[3] = REAL(0.0); + + dCROSS(Contact->normal, =, vv, vu); // Reversed + + dNormalize3(Contact->normal); + + Contact->depth = T; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + + OutTriCount++; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + if (OutTriCount >= (Flags & NUMC_MASK)) { + break; + } + } + } + return OutTriCount; +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (RayGeom->type == dRayClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + dReal Length = dGeomRayGetLength(RayGeom); + int FirstContact, BackfaceCull; + dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + char intersect=0; + GIM_TRIANGLE_RAY_CONTACT_DATA contact_data; + + if(ClosestHit) + { + intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + else + { + intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + + if(intersect == 0) + { + return 0; + } + + int OutTriCount = 0; + + if(!TriMesh->RayCallback || + TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v)) + { + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride); + VEC_COPY(Contact->pos,contact_data.m_point); + VEC_COPY(Contact->normal,contact_data.m_normal); + Contact->depth = contact_data.tparam; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + + OutTriCount = 1; + } + + return OutTriCount; +} +#endif // dTRIMESH_GIMPACT + +#endif // dTRIMESH_ENABLED + diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp new file mode 100644 index 0000000..7ee9b4b --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp @@ -0,0 +1,573 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. + +#include +#include +#include +#include +#include "collision_util.h" + +#if dTRIMESH_ENABLED + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE +#define MERGECONTACTS + +// Ripped from Opcode 1.1. +static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){ + + // now onto the bulk of the collision... + + dVector3 Diff; + Diff[0] = Origin[0] - Center[0]; + Diff[1] = Origin[1] - Center[1]; + Diff[2] = Origin[2] - Center[2]; + Diff[3] = Origin[3] - Center[3]; + + dReal A00 = dDOT(Edge0, Edge0); + dReal A01 = dDOT(Edge0, Edge1); + dReal A11 = dDOT(Edge1, Edge1); + + dReal B0 = dDOT(Diff, Edge0); + dReal B1 = dDOT(Diff, Edge1); + + dReal C = dDOT(Diff, Diff); + + dReal Det = dFabs(A00 * A11 - A01 * A01); + u = A01 * B1 - A11 * B0; + v = A01 * B0 - A00 * B1; + + dReal DistSq; + + if (u + v <= Det){ + if(u < REAL(0.0)){ + if(v < REAL(0.0)){ // region 4 + if(B0 < REAL(0.0)){ + v = REAL(0.0); + if (-B0 >= A00){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + else{ + u = REAL(0.0); + if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else if(-B1 >= A11){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else{ // region 3 + u = REAL(0.0); + if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else if(-B1 >= A11){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else if(v < REAL(0.0)){ // region 5 + v = REAL(0.0); + if (B0 >= REAL(0.0)){ + u = REAL(0.0); + DistSq = C; + } + else if (-B0 >= A00){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + else{ // region 0 + // minimum at interior point + if (Det == REAL(0.0)){ + u = REAL(0.0); + v = REAL(0.0); + DistSq = FLT_MAX; + } + else{ + dReal InvDet = REAL(1.0) / Det; + u *= InvDet; + v *= InvDet; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + } + else{ + dReal Tmp0, Tmp1, Numer, Denom; + + if(u < REAL(0.0)){ // region 2 + Tmp0 = A01 + B0; + Tmp1 = A11 + B1; + if (Tmp1 > Tmp0){ + Numer = Tmp1 - Tmp0; + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + u = REAL(1.0); + v = REAL(0.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = Numer / Denom; + v = REAL(1.0) - u; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + else{ + u = REAL(0.0); + if(Tmp1 <= REAL(0.0)){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else if(v < REAL(0.0)){ // region 6 + Tmp0 = A01 + B1; + Tmp1 = A00 + B0; + if (Tmp1 > Tmp0){ + Numer = Tmp1 - Tmp0; + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + v = REAL(1.0); + u = REAL(0.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = Numer / Denom; + u = REAL(1.0) - v; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + else{ + v = REAL(0.0); + if (Tmp1 <= REAL(0.0)){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else if(B0 >= REAL(0.0)){ + u = REAL(0.0); + DistSq = C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + } + else{ // region 1 + Numer = A11 + B1 - A01 - B0; + if (Numer <= REAL(0.0)){ + u = REAL(0.0); + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + u = REAL(1.0); + v = REAL(0.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = Numer / Denom; + v = REAL(1.0) - u; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + } + } + + Dist = dSqrt(dFabs(DistSq)); + + if (Dist <= Radius){ + Dist = Radius - Dist; + return true; + } + else return false; +} + +int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (SphereGeom->type == dSphereClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + // Init + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + SphereCollider& Collider = TriMesh->_SphereCollider; + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); + dReal Radius = dGeomSphereGetRadius(SphereGeom); + + // Sphere + Sphere Sphere; + Sphere.mCenter.x = Position[0]; + Sphere.mCenter.y = Position[1]; + Sphere.mCenter.z = Position[2]; + Sphere.mRadius = Radius; + + Matrix4x4 amatrix; + + // TC results + if (TriMesh->doSphereTC) { + dxTriMesh::SphereTC* sphereTC = 0; + for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){ + if (TriMesh->SphereTCCache[i].Geom == SphereGeom){ + sphereTC = &TriMesh->SphereTCCache[i]; + break; + } + } + + if (!sphereTC){ + TriMesh->SphereTCCache.push(dxTriMesh::SphereTC()); + + sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1]; + sphereTC->Geom = SphereGeom; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, + &MakeMatrix(TLPosition, TLRotation, amatrix)); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, + &MakeMatrix(TLPosition, TLRotation, amatrix)); + } + + if (! Collider.GetContactStatus()) { + // no collision occurred + return 0; + } + + // get results + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0){ + if (TriMesh->ArrayCallback != null){ + TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount); + } + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++){ + if (OutTriCount == (Flags & NUMC_MASK)){ + break; + } + + const int TriIndex = Triangles[i]; + + dVector3 dv[3]; + if (!Callback(TriMesh, SphereGeom, TriIndex)) + continue; + FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); + + dVector3& v0 = dv[0]; + dVector3& v1 = dv[1]; + dVector3& v2 = dv[2]; + + dVector3 vu; + vu[0] = v1[0] - v0[0]; + vu[1] = v1[1] - v0[1]; + vu[2] = v1[2] - v0[2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = v2[0] - v0[0]; + vv[1] = v2[1] - v0[1]; + vv[2] = v2[2] - v0[2]; + vv[3] = REAL(0.0); + + // Get plane coefficients + dVector4 Plane; + dCROSS(Plane, =, vu, vv); + + dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later + Plane[0] /= Area; + Plane[1] /= Area; + Plane[2] /= Area; + + Plane[3] = dDOT(Plane, v0); + + /* If the center of the sphere is within the positive halfspace of the + * triangle's plane, allow a contact to be generated. + * If the center of the sphere made it into the positive halfspace of a + * back-facing triangle, then the physics update and/or velocity needs + * to be adjusted (penetration has occured anyway). + */ + + dReal side = dDOT(Plane,Position) - Plane[3]; + + if(side < REAL(0.0)) { + continue; + } + + dReal Depth; + dReal u, v; + if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){ + continue; // Sphere doesn't hit triangle + } + + if (Depth < REAL(0.0)){ + Depth = REAL(0.0); + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + dReal w = REAL(1.0) - u - v; + Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v); + Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v); + Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v); + Contact->pos[3] = REAL(0.0); + + // Using normal as plane (reversed) + Contact->normal[0] = -Plane[0]; + Contact->normal[1] = -Plane[1]; + Contact->normal[2] = -Plane[2]; + Contact->normal[3] = REAL(0.0); + + // Depth returned from GetContactData is depth along + // contact point - sphere center direction + // we'll project it to contact normal + dVector3 dir; + dir[0] = Position[0]-Contact->pos[0]; + dir[1] = Position[1]-Contact->pos[1]; + dir[2] = Position[2]-Contact->pos[2]; + dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir)); + Contact->depth = Depth * dirProj; + //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance + Contact->side1 = TriIndex; + + //Contact->g1 = TriMesh; + //Contact->g2 = SphereGeom; + + OutTriCount++; + } +#ifdef MERGECONTACTS // Merge all contacts into 1 + if (OutTriCount != 0){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride); + + if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){ + Contact->normal[0] *= Contact->depth; + Contact->normal[1] *= Contact->depth; + Contact->normal[2] *= Contact->depth; + Contact->normal[3] *= Contact->depth; + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride); + + Contact->pos[0] += TempContact->pos[0]; + Contact->pos[1] += TempContact->pos[1]; + Contact->pos[2] += TempContact->pos[2]; + Contact->pos[3] += TempContact->pos[3]; + + Contact->normal[0] += TempContact->normal[0] * TempContact->depth; + Contact->normal[1] += TempContact->normal[1] * TempContact->depth; + Contact->normal[2] += TempContact->normal[2] * TempContact->depth; + Contact->normal[3] += TempContact->normal[3] * TempContact->depth; + } + + Contact->pos[0] /= OutTriCount; + Contact->pos[1] /= OutTriCount; + Contact->pos[2] /= OutTriCount; + Contact->pos[3] /= OutTriCount; + + // Remember to divide in square space. + Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount); + + dNormalize3(Contact->normal); + } + + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + + // TODO: + // Side1 now contains index of triangle that gave first hit + // Probably we should find index of triangle with deepest penetration + + return 1; + } + else return 0; +#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts + if (OutTriCount != 0){ + if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){ + dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal; + Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + Normal[0] += Contact->normal[0] * Contact->depth; + Normal[1] += Contact->normal[1] * Contact->depth; + Normal[2] += Contact->normal[2] * Contact->depth; + Normal[3] += Contact->normal[3] * Contact->depth; + } + dNormalize3(Normal); + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + Contact->normal[0] = Normal[0]; + Contact->normal[1] = Normal[1]; + Contact->normal[2] = Normal[2]; + Contact->normal[3] = Normal[3]; + + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + } + } + else{ + SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh; + SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom; + } + + return OutTriCount; + } + else return 0; +#else //MERGECONTACTNORMALS // Just gather penetration depths and return + for (int i = 0; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + //Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal)); + + /*Contact->normal[0] /= Contact->depth; + Contact->normal[1] /= Contact->depth; + Contact->normal[2] /= Contact->depth; + Contact->normal[3] /= Contact->depth;*/ + + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + } + + return OutTriCount; +#endif // MERGECONTACTS + } + else return 0; +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (SphereGeom->type == dSphereClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom); + dReal Radius = dGeomSphereGetRadius(SphereGeom); + //Create contact list + GDYNAMIC_ARRAY trimeshcontacts; + GIM_CREATE_CONTACT_LIST(trimeshcontacts); + + //Collide trimeshes + gim_trimesh_sphere_collision(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts); + + if(trimeshcontacts.m_size == 0) + { + GIM_DYNARRAY_DESTROY(trimeshcontacts); + return 0; + } + + GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts); + + unsigned contactcount = trimeshcontacts.m_size; + unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK); + if (contactcount > maxcontacts) + { + contactcount = maxcontacts; + } + + dContactGeom* pcontact; + unsigned i; + + for (i=0;ipos[0] = ptrimeshcontacts->m_point[0]; + pcontact->pos[1] = ptrimeshcontacts->m_point[1]; + pcontact->pos[2] = ptrimeshcontacts->m_point[2]; + pcontact->pos[3] = REAL(1.0); + + pcontact->normal[0] = ptrimeshcontacts->m_normal[0]; + pcontact->normal[1] = ptrimeshcontacts->m_normal[1]; + pcontact->normal[2] = ptrimeshcontacts->m_normal[2]; + pcontact->normal[3] = 0; + + pcontact->depth = ptrimeshcontacts->m_depth; + pcontact->g1 = g1; + pcontact->g2 = SphereGeom; + + ptrimeshcontacts++; + } + + GIM_DYNARRAY_DESTROY(trimeshcontacts); + + return (int)contactcount; +} +#endif // dTRIMESH_GIMPACT + +#endif // dTRIMESH_ENABLED diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp new file mode 100644 index 0000000..c9f209e --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp @@ -0,0 +1,2033 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// OPCODE TriMesh/TriMesh collision code by Jeff Smith (c) 2004 + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#include +#include +#include +#include + +// Classic Implementation +#if !dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE + +#define SMALL_ELT REAL(2.5e-4) +#define EXPANDED_ELT_THRESH REAL(1.0e-3) +#define DISTANCE_EPSILON REAL(1.0e-8) +#define VELOCITY_EPSILON REAL(1.0e-5) +#define TINY_PENETRATION REAL(5.0e-6) + +struct LineContactSet +{ + enum + { + MAX_POINTS = 8 + }; + + dVector3 Points[MAX_POINTS]; + int Count; +}; + + +static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); +static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*, + const dVector3, const dVector3, dReal, int&); +static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, + dReal isectpt1[3],dReal isectpt2[3]); +inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); +static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); +static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); +static bool FindTriSolidIntrsection(const dVector3 Tri[3], + const dVector4 Planes[6], int numSides, + LineContactSet& ClippedPolygon ); +static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); +static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, + dVector3 in_n, dVector3* in_col_v, dReal &out_depth); +static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point); +static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v); + + + + +/* some math macros */ +#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \ + dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \ + dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; } + +#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) + +#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; } + +#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; } + +#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; } + +#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; } + +#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; } + +#define COMBO(combo,p,t,q) { combo[0]=p[0]+t*q[0]; combo[1]=p[1]+t*q[1]; combo[2]=p[2]+t*q[2]; } + +#define LENGTH(x) ((dReal) dSqrt(dDOT(x, x))) + +#define DEPTH(d, p, q, n) d = (p[0] - q[0])*n[0] + (p[1] - q[1])*n[1] + (p[2] - q[2])*n[2]; + +inline const dReal dMin(const dReal x, const dReal y) +{ + return x < y ? x : y; +} + + +inline void +SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, + dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, + dVector3 n, dVector3 n1, dVector3 n2) +{ + if (pen_v == v1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } +} + + + + +int +dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (g2->type == dTriMeshClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh1 = (dxTriMesh*) g1; + dxTriMesh* TriMesh2 = (dxTriMesh*) g2; + + dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals; + dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals; + + const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); + // TLRotation1 = column-major order + const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); + + const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); + // TLRotation2 = column-major order + const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); + + AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider; + + static BVTCache ColCache; + ColCache.Model0 = &TriMesh1->Data->BVTree; + ColCache.Model1 = &TriMesh2->Data->BVTree; + + // Collision query + Matrix4x4 amatrix, bmatrix; + BOOL IsOk = Collider.Collide(ColCache, + &MakeMatrix(TLPosition1, TLRotation1, amatrix), + &MakeMatrix(TLPosition2, TLRotation2, bmatrix) ); + + + // Make "double" versions of these matrices, if appropriate + dMatrix4 A, B; + dMakeMatrix4(TLPosition1, TLRotation1, A); + dMakeMatrix4(TLPosition2, TLRotation2, B); + + + if (IsOk) { + // Get collision status => if true, objects overlap + if ( Collider.GetContactStatus() ) { + // Number of colliding pairs and list of pairs + int TriCount = Collider.GetNbPairs(); + const Pair* CollidingPairs = Collider.GetPairs(); + + if (TriCount > 0) { + // step through the pairs, adding contacts + int id1, id2; + int OutTriCount = 0; + dVector3 v1[3], v2[3], CoplanarPt; + dVector3 e1, e2, e3, n1, n2, n, ContactNormal; + dReal depth; + dVector3 orig_pos, old_pos1, old_pos2, elt1, elt2, elt_sum; + dVector3 elt_f1[3], elt_f2[3]; + dReal contact_elt_length = SMALL_ELT; + LineContactSet firstClippedTri, secondClippedTri; + dVector3 *firstClippedElt = new dVector3[LineContactSet::MAX_POINTS]; + dVector3 *secondClippedElt = new dVector3[LineContactSet::MAX_POINTS]; + + + // only do these expensive inversions once + dMatrix4 InvMatrix1, InvMatrix2; + dInvertMatrix4(A, InvMatrix1); + dInvertMatrix4(B, InvMatrix2); + + + for (int i = 0; i < TriCount; i++) { + + id1 = CollidingPairs[i].id0; + id2 = CollidingPairs[i].id1; + + // grab the colliding triangles + FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1); + FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2); + // Since we'll be doing matrix transfomrations, we need to + // make sure that all vertices have four elements + for (int j=0; j<3; j++) { + v1[j][3] = 1.0; + v2[j][3] = 1.0; + } + + + int IsCoplanar = 0; + dReal IsectPt1[3], IsectPt2[3]; + + // Sometimes OPCODE makes mistakes, so we look at the return + // value for TriTriIntersectWithIsectLine. A retcode of "0" + // means no intersection took place + if ( TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], + &IsCoplanar, + IsectPt1, IsectPt2) ) { + + // Compute the normals of the colliding faces + // + if (TriNormals1 == NULL) { + SUB( e1, v1[1], v1[0] ); + SUB( e2, v1[2], v1[0] ); + CROSS( n1, e1, e2 ); + dNormalize3(n1); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e1[0] = TriNormals1[id1*3]; + e1[1] = TriNormals1[id1*3 + 1]; + e1[2] = TriNormals1[id1*3 + 2]; + e1[3] = 0.0; + + //dMultiply1(n1, TLRotation1, e1, 3, 3, 1); + dMultiply0(n1, TLRotation1, e1, 3, 3, 1); + n1[3] = 1.0; + } + + if (TriNormals2 == NULL) { + SUB( e1, v2[1], v2[0] ); + SUB( e2, v2[2], v2[0] ); + CROSS( n2, e1, e2); + dNormalize3(n2); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e2[0] = TriNormals2[id2*3]; + e2[1] = TriNormals2[id2*3 + 1]; + e2[2] = TriNormals2[id2*3 + 2]; + e2[3] = 0.0; + + //dMultiply1(n2, TLRotation2, e2, 3, 3, 1); + dMultiply0(n2, TLRotation2, e2, 3, 3, 1); + n2[3] = 1.0; + } + + + if (IsCoplanar) { + // We can reach this case if the faces are coplanar, OR + // if they don't actually intersect. (OPCODE can make + // mistakes) + if (dFabs(dDOT(n1, n2)) > REAL(0.999)) { + // If the faces are coplanar, we declare that the point of + // contact is at the average location of the vertices of + // both faces + dVector3 ContactPt; + for (int j=0; j<3; j++) { + ContactPt[j] = 0.0; + for (int k=0; k<3; k++) + ContactPt[j] += v1[k][j] + v2[k][j]; + ContactPt[j] /= 6.0; + } + ContactPt[3] = 1.0; + + // and the contact normal is the normal of face 2 + // (could be face 1, because they are the same) + SET(n, n2); + + // and the penetration depth is the co-normal + // distance between any two vertices A and B, + // i.e. d = DOT(n, (A-B)) + DEPTH(depth, v1[1], v2[1], n); + if (depth < 0) + depth *= -1.0; + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, n, depth, OutTriCount); + } + } + else { + // Otherwise (in non-co-planar cases), we create a coplanar + // point -- the middle of the line of intersection -- that + // will be used for various computations down the road + for (int j=0; j<3; j++) + CoplanarPt[j] = ( (IsectPt1[j] + IsectPt2[j]) / REAL(2.0) ); + CoplanarPt[3] = 1.0; + + // Find the ELT of the coplanar point + // + dMultiply1(orig_pos, InvMatrix1, CoplanarPt, 4, 4, 1); + dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1); + SUB(elt1, CoplanarPt, old_pos1); + + dMultiply1(orig_pos, InvMatrix2, CoplanarPt, 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); + SUB(elt2, CoplanarPt, old_pos2); + + SUB(elt_sum, elt1, elt2); // net motion of the coplanar point + + + // Calculate how much the vertices of each face moved in the + // direction of the opposite face's normal + // + dReal total_dp1, total_dp2; + total_dp1 = 0.0; + total_dp2 = 0.0; + + for (int ii=0; ii<3; ii++) { + // find the estimated linear translation (ELT) of the vertices + // on face 1, wrt to the center of face 2. + + // un-transform this vertex by the current transform + dMultiply1(orig_pos, InvMatrix1, v1[ii], 4, 4, 1 ); + + // re-transform this vertex by last_trans (to get its old + // position) + dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1); + + // Then subtract this position from our current one to find + // the elapsed linear translation (ELT) + for (int k=0; k<3; k++) { + elt_f1[ii][k] = (v1[ii][k] - old_pos1[k]) - elt2[k]; + } + + // Take the dot product of the ELT for each vertex (wrt the + // center of face2) + total_dp1 += dFabs( dDOT(elt_f1[ii], n2) ); + } + + for (int ii=0; ii<3; ii++) { + // find the estimated linear translation (ELT) of the vertices + // on face 2, wrt to the center of face 1. + dMultiply1(orig_pos, InvMatrix2, v2[ii], 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + elt_f2[ii][k] = (v2[ii][k] - old_pos2[k]) - elt1[k]; + } + + // Take the dot product of the ELT for each vertex (wrt the + // center of face2) and add them + total_dp2 += dFabs( dDOT(elt_f2[ii], n1) ); + } + + + //////// + // Estimate the penetration depth. + // + dReal dp; + BOOL badPen = true; + dVector3 *pen_v; // the "penetrating vertices" + dVector3 *pen_elt; // the elt_f of the penetrating face + dVector3 *col_v; // the "collision vertices" (the penetrated face) + + SMULT(n2, n2, -1.0); // SF PATCH #1335183 + depth = 0.0; + if ((total_dp1 > DISTANCE_EPSILON) || (total_dp2 > DISTANCE_EPSILON)) { + //////// + // Find the collision normal, by finding the face + // that is pointed "most" in the direction of travel + // of the two triangles + // + if (total_dp2 > total_dp1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } + } + else { + // the total_dp is very small, so let's fall back + // to a different test + if (LENGTH(elt2) > LENGTH(elt1)) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } + } + + + for (int j=0; j<3; j++) + if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + + if (badPen) { + // try the other normal + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + for (int j=0; j<3; j++) + if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + + + //////////////////////////////////////// + // + // If we haven't found a good penetration, then we're probably straddling + // the edge of one of the objects, or the penetraing face is big + // enough that all of its vertices are outside the bounds of the + // penetrated face. + // In these cases, we do a more expensive test. We clip the penetrating + // triangle with a solid defined by the penetrated triangle, and repeat + // the tests above on this new polygon + if (badPen) { + + // Switch pen_v and n back again + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + + // Find the three sides (no top or bottom) of the solid defined by + // the edges of the penetrated triangle. + + // The dVector4 "plane" structures contain the following information: + // [0]-[2]: The normal of the face, pointing INWARDS (i.e. + // the inverse normal + // [3]: The distance between the face and the center of the + // solid, along the normal + dVector4 SolidPlanes[3]; + dVector3 tmp1; + dVector3 sn; + + for (int j=0; j<3; j++) { + e1[j] = col_v[1][j] - col_v[0][j]; + e2[j] = col_v[0][j] - col_v[2][j]; + e3[j] = col_v[2][j] - col_v[1][j]; + } + + // side 1 + CROSS(sn, e1, n); + dNormalize3(sn); + SMULT( SolidPlanes[0], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]); + + + // side 2 + CROSS(sn, e2, n); + dNormalize3(sn); + SMULT( SolidPlanes[1], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[2]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]); + + + // side 3 + CROSS(sn, e3, n); + dNormalize3(sn); + SMULT( SolidPlanes[2], sn, -1.0 ); + + ADD(tmp1, col_v[2], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]); + + + FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri); + + for (int j=0; jlast_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; + } + } + else { + dMultiply1(orig_pos, InvMatrix2, firstClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; + } + } + + if (dp >= 0.0) { + contact_elt_length = dFabs(dDOT(firstClippedElt[j], n)); + + depth = dp; + if (depth == 0.0) + depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + firstClippedTri.Points[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + } + } + + if (badPen) { + // Switch pen_v and n (again!) + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + + // Find the three sides (no top or bottom) of the solid created by + // the penetrated triangle. + // The dVector4 "plane" structures contain the following information: + // [0]-[2]: The normal of the face, pointing INWARDS (i.e. + // the inverse normal + // [3]: The distance between the face and the center of the + // solid, along the normal + dVector4 SolidPlanes[3]; + dVector3 tmp1; + + dVector3 sn; + for (int j=0; j<3; j++) { + e1[j] = col_v[1][j] - col_v[0][j]; + e2[j] = col_v[0][j] - col_v[2][j]; + e3[j] = col_v[2][j] - col_v[1][j]; + } + + // side 1 + CROSS(sn, e1, n); + dNormalize3(sn); + SMULT( SolidPlanes[0], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]); + + + // side 2 + CROSS(sn, e2, n); + dNormalize3(sn); + SMULT( SolidPlanes[1], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[2]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]); + + + // side 3 + CROSS(sn, e3, n); + dNormalize3(sn); + SMULT( SolidPlanes[2], sn, -1.0 ); + + ADD(tmp1, col_v[2], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]); + + FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri); + + for (int j=0; jlast_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; + } + } + else { + dMultiply1(orig_pos, InvMatrix2, secondClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; + } + } + + + if (dp >= 0.0) { + contact_elt_length = dFabs(dDOT(secondClippedElt[j],n)); + + depth = dp; + if (depth == 0.0) + depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + secondClippedTri.Points[j], n, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + + } + } + + + + ///////////////// + // All conventional tests have failed at this point, so now we deal with + // cases on a more "heuristic" basis + // + + if (badPen) { + // Switch pen_v and n (for the fourth time, so they're + // what my original guess said they were) + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + if (dFabs(dDOT(n1, n2)) < REAL(0.01)) { + // If we reach this point, we have (close to) perpindicular + // faces, either resting on each other or sliding in a + // direction orthogonal to both surface normals. + if (LENGTH(elt_sum) < DISTANCE_EPSILON) { + depth = dFabs(dDOT(n, elt_sum)); + + if (depth > REAL(1e-12)) { + dNormalize3(n); + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + CoplanarPt, n, depth, OutTriCount); + badPen = false; + } + else { + // If the two faces are (nearly) perfectly at rest with + // respect to each other, then we ignore the contact, + // allowing the objects to slip a little in the hopes + // that next frame, they'll give us something to work + // with. + badPen = false; + } + } + else { + // The faces are perpindicular, but moving significantly + // This can be sliding, or an unusual edge-straddling + // penetration. + dVector3 cn; + + CROSS(cn, n1, n2); + dNormalize3(cn); + SET(n, cn); + + // The shallowest ineterpenetration of the faces + // is the depth + dVector3 ContactPt; + dVector3 dvTmp; + dReal rTmp; + depth = dInfinity; + for (int j=0; j<3; j++) { + for (int k=0; k<3; k++) { + SUB(dvTmp, col_v[k], pen_v[j]); + + rTmp = dDOT(dvTmp, n); + if ( dFabs(rTmp) < dFabs(depth) ) { + depth = rTmp; + SET( ContactPt, pen_v[j] ); + contact_elt_length = dFabs(dDOT(pen_elt[j], n)); + } + } + } + if (depth < 0.0) { + SMULT(n, n, -1.0); + depth *= -1.0; + } + + if ((depth > 0.0) && (depth <= contact_elt_length)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, n, depth, OutTriCount); + badPen = false; + } + + } + } + } + + + if (badPen) { + // Use as the normal the direction of travel, rather than any particular + // face normal + // + dVector3 esn; + + if (pen_v == v1) { + SMULT(esn, elt_sum, -1.0); + } + else { + SET(esn, elt_sum); + } + dNormalize3(esn); + + + // The shallowest ineterpenetration of the faces + // is the depth + dVector3 ContactPt; + depth = dInfinity; + for (int j=0; j<3; j++) { + for (int k=0; k<3; k++) { + DEPTH(dp, col_v[k], pen_v[j], esn); + if ( (ExamineContactPoint(col_v, esn, pen_v[j])) && + ( dFabs(dp) < dFabs(depth)) ) { + depth = dp; + SET( ContactPt, pen_v[j] ); + contact_elt_length = dFabs(dDOT(pen_elt[j], esn)); + } + } + } + + if ((depth > 0.0) && (depth <= contact_elt_length)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, esn, depth, OutTriCount); + badPen = false; + } + } + + + if (badPen) { + // If the direction of motion is perpindicular to both normals + if ( (dFabs(dDOT(n1, elt_sum)) < REAL(0.01)) && (dFabs(dDOT(n2, elt_sum)) < REAL(0.01)) ) { + dVector3 esn; + if (pen_v == v1) { + SMULT(esn, elt_sum, -1.0); + } + else { + SET(esn, elt_sum); + } + + dNormalize3(esn); + + + // Look at the clipped points again, checking them against this + // new normal + for (int j=0; j= 0.0) { + contact_elt_length = dFabs(dDOT(firstClippedElt[j], esn)); + + depth = dp; + //if (depth == 0.0) + //depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + firstClippedTri.Points[j], esn, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + } + + if (badPen) { + // If this test failed, try it with the second set of clipped faces + for (int j=0; j= 0.0) { + contact_elt_length = dFabs(dDOT(secondClippedElt[j], esn)); + + depth = dp; + //if (depth == 0.0) + //depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + secondClippedTri.Points[j], esn, depth, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + } + } + } + } + + + + if (badPen) { + // if we have very little motion, we're dealing with resting contact + // and shouldn't reference the ELTs at all + // + if (LENGTH(elt_sum) < VELOCITY_EPSILON) { + + // instead of a "contact_elt_length" threshhold, we'll use an + // arbitrary, small one + for (int j=0; j<3; j++) { + DEPTH(dp, CoplanarPt, pen_v[j], n); + + if (dp == 0.0) + dp = TINY_PENETRATION; + + if ( (dp > 0.0) && (dp <= SMALL_ELT)) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, dp, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + + + if (badPen) { + // try the other normal + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + for (int j=0; j<3; j++) { + DEPTH(dp, CoplanarPt, pen_v[j], n); + + if (dp == 0.0) + dp = TINY_PENETRATION; + + if ( (dp > 0.0) && (dp <= SMALL_ELT)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, dp, OutTriCount); + badPen = false; + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + } + } + + + + } + } + + if (badPen) { + // find the nearest existing contact, and replicate it's + // normal and depth + // + dContactGeom* Contact; + dVector3 pos_diff; + dReal min_dist, dist; + + min_dist = dInfinity; + depth = 0.0; + for (int j=0; jpos, CoplanarPt); + + dist = dDOT(pos_diff, pos_diff); + if (dist < min_dist) { + min_dist = dist; + depth = Contact->depth; + SMULT(ContactNormal, Contact->normal, -1.0); + } + } + + if (depth > 0.0) { + // Add a tiny contact at the coplanar point + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + CoplanarPt, ContactNormal, depth, OutTriCount); + badPen = false; + } + } + + + if (badPen) { + // Add a tiny contact at the coplanar point + if (-dDOT(elt_sum, n1) > -dDOT(elt_sum, n2)) { + SET(ContactNormal, n1); + } + else { + SET(ContactNormal, n2); + } + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + CoplanarPt, ContactNormal, TINY_PENETRATION, OutTriCount); + badPen = false; + } + + + } // not coplanar (main loop) + } // TriTriIntersectWithIsectLine + + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { + break; + } + } + + // Free memory + delete[] firstClippedElt; + delete[] secondClippedElt; + + // Return the number of contacts + return OutTriCount; + } + } + } + + + // There was some kind of failure during the Collide call or + // there are no faces overlapping + return 0; +} + + + +static void +GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) +{ + dVector3 Out[3]; + + FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); + + for (int i = 0; i < 3; i++) + triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); +} + + +// +// +// +#define B11 B[0] +#define B12 B[1] +#define B13 B[2] +#define B14 B[3] +#define B21 B[4] +#define B22 B[5] +#define B23 B[6] +#define B24 B[7] +#define B31 B[8] +#define B32 B[9] +#define B33 B[10] +#define B34 B[11] +#define B41 B[12] +#define B42 B[13] +#define B43 B[14] +#define B44 B[15] + +#define Binv11 Binv[0] +#define Binv12 Binv[1] +#define Binv13 Binv[2] +#define Binv14 Binv[3] +#define Binv21 Binv[4] +#define Binv22 Binv[5] +#define Binv23 Binv[6] +#define Binv24 Binv[7] +#define Binv31 Binv[8] +#define Binv32 Binv[9] +#define Binv33 Binv[10] +#define Binv34 Binv[11] +#define Binv41 Binv[12] +#define Binv42 Binv[13] +#define Binv43 Binv[14] +#define Binv44 Binv[15] + +inline void +dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) +{ + B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; + B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; + B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; + + B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; +} + + +static void +dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) +{ + dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) + -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) + +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) + +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) + -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) + +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); + + dAASSERT (det != 0.0); + + det = 1.0 / det; + + Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); + Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); + Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); + Binv14 = 0.0f; + Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); + Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); + Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); + Binv24 = 0.0f; + Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); + Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); + Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); + Binv34 = 0.0f; + Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); + Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); + Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); + Binv44 = 1.0f; +} + + + +///////////////////////////////////////////////// +// +// Triangle/Triangle intersection utilities +// +// From the article "A Fast Triangle-Triangle Intersection Test", +// Journal of Graphics Tools, 2(2), 1997 +// +// Some of this functionality is duplicated in OPCODE (see +// OPC_TriTriOverlap.h) but we have replicated it here so we don't +// have to mess with the internals of OPCODE, as well as so we can +// further optimize some of the functions. +// +// This version computes the line of intersection as well (if they +// are not coplanar): +// int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], +// dReal U0[3],dReal U1[3],dReal U2[3], +// int *coplanar, +// dReal isectpt1[3],dReal isectpt2[3]); +// +// parameters: vertices of triangle 1: V0,V1,V2 +// vertices of triangle 2: U0,U1,U2 +// +// result : returns 1 if the triangles intersect, otherwise 0 +// "coplanar" returns whether the tris are coplanar +// isectpt1, isectpt2 are the endpoints of the line of +// intersection +// + + + +/* if USE_EPSILON_TEST is true then we do a check: + if |dv|b) \ + { \ + dReal c; \ + c=a; \ + a=b; \ + b=c; \ + } + +#define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \ + isect0=VV0+(VV1-VV0)*D0/(D0-D1); \ + isect1=VV0+(VV2-VV0)*D0/(D0-D2); + + +#define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \ + if(D0D1>0.0f) \ + { \ + /* here we know that D0D2<=0.0 */ \ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ + ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ + } \ + else if(D0D2>0.0f) \ + { \ + /* here we know that d0d1<=0.0 */ \ + ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \ + } \ + else if(D1!=0.0f) \ + { \ + ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ + } \ + else if(D2!=0.0f) \ + { \ + ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } + + + +/* this edge to edge test is based on Franlin Antonio's gem: + "Faster Line Segment Intersection", in Graphics Gems III, + pp. 199-202 */ +#define EDGE_EDGE_TEST(V0,U0,U1) \ + Bx=U0[i0]-U1[i0]; \ + By=U0[i1]-U1[i1]; \ + Cx=V0[i0]-U0[i0]; \ + Cy=V0[i1]-U0[i1]; \ + f=Ay*Bx-Ax*By; \ + d=By*Cx-Bx*Cy; \ + if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \ + { \ + e=Ax*Cy-Ay*Cx; \ + if(f>0) \ + { \ + if(e>=0 && e<=f) return 1; \ + } \ + else \ + { \ + if(e<=0 && e>=f) return 1; \ + } \ + } + +#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \ +{ \ + dReal Ax,Ay,Bx,By,Cx,Cy,e,d,f; \ + Ax=V1[i0]-V0[i0]; \ + Ay=V1[i1]-V0[i1]; \ + /* test edge U0,U1 against V0,V1 */ \ + EDGE_EDGE_TEST(V0,U0,U1); \ + /* test edge U1,U2 against V0,V1 */ \ + EDGE_EDGE_TEST(V0,U1,U2); \ + /* test edge U2,U1 against V0,V1 */ \ + EDGE_EDGE_TEST(V0,U2,U0); \ +} + +#define POINT_IN_TRI(V0,U0,U1,U2) \ +{ \ + dReal a,b,c,d0,d1,d2; \ + /* is T1 completly inside T2? */ \ + /* check if V0 is inside tri(U0,U1,U2) */ \ + a=U1[i1]-U0[i1]; \ + b=-(U1[i0]-U0[i0]); \ + c=-a*U0[i0]-b*U0[i1]; \ + d0=a*V0[i0]+b*V0[i1]+c; \ + \ + a=U2[i1]-U1[i1]; \ + b=-(U2[i0]-U1[i0]); \ + c=-a*U1[i0]-b*U1[i1]; \ + d1=a*V0[i0]+b*V0[i1]+c; \ + \ + a=U0[i1]-U2[i1]; \ + b=-(U0[i0]-U2[i0]); \ + c=-a*U2[i0]-b*U2[i1]; \ + d2=a*V0[i0]+b*V0[i1]+c; \ + if(d0*d1>0.0) \ + { \ + if(d0*d2>0.0) return 1; \ + } \ +} + +int coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3]) +{ + dReal A[3]; + short i0,i1; + /* first project onto an axis-aligned plane, that maximizes the area */ + /* of the triangles, compute indices: i0,i1. */ + A[0]= dFabs(N[0]); + A[1]= dFabs(N[1]); + A[2]= dFabs(N[2]); + if(A[0]>A[1]) + { + if(A[0]>A[2]) + { + i0=1; /* A[0] is greatest */ + i1=2; + } + else + { + i0=0; /* A[2] is greatest */ + i1=1; + } + } + else /* A[0]<=A[1] */ + { + if(A[2]>A[1]) + { + i0=0; /* A[2] is greatest */ + i1=1; + } + else + { + i0=0; /* A[1] is greatest */ + i1=2; + } + } + + /* test all edges of triangle 1 against the edges of triangle 2 */ + EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2); + EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2); + EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2); + + /* finally, test if tri1 is totally contained in tri2 or vice versa */ + POINT_IN_TRI(V0,U0,U1,U2); + POINT_IN_TRI(U0,V0,V1,V2); + + return 0; +} + + + +#define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \ +{ \ + if(D0D1>0.0f) \ + { \ + /* here we know that D0D2<=0.0 */ \ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ + A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \ + } \ + else if(D0D2>0.0f)\ + { \ + /* here we know that d0d1<=0.0 */ \ + A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \ + } \ + else if(D1!=0.0f) \ + { \ + A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \ + } \ + else if(D2!=0.0f) \ + { \ + A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } \ +} + + + + +/* sort so that a<=b */ +#define SORT2(a,b,smallest) \ + if(a>b) \ + { \ + dReal c; \ + c=a; \ + a=b; \ + b=c; \ + smallest=1; \ + } \ + else smallest=0; + + +inline void isect2(dReal VTX0[3],dReal VTX1[3],dReal VTX2[3],dReal VV0,dReal VV1,dReal VV2, + dReal D0,dReal D1,dReal D2,dReal *isect0,dReal *isect1,dReal isectpoint0[3],dReal isectpoint1[3]) +{ + dReal tmp=D0/(D0-D1); + dReal diff[3]; + *isect0=VV0+(VV1-VV0)*tmp; + SUB(diff,VTX1,VTX0); + MULT(diff,diff,tmp); + ADD(isectpoint0,diff,VTX0); + tmp=D0/(D0-D2); + *isect1=VV0+(VV2-VV0)*tmp; + SUB(diff,VTX2,VTX0); + MULT(diff,diff,tmp); + ADD(isectpoint1,VTX0,diff); +} + + +#if 0 +#define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \ + tmp=D0/(D0-D1); \ + isect0=VV0+(VV1-VV0)*tmp; \ + SUB(diff,VTX1,VTX0); \ + MULT(diff,diff,tmp); \ + ADD(isectpoint0,diff,VTX0); \ + tmp=D0/(D0-D2); +/* isect1=VV0+(VV2-VV0)*tmp; \ */ +/* SUB(diff,VTX2,VTX0); \ */ +/* MULT(diff,diff,tmp); \ */ +/* ADD(isectpoint1,VTX0,diff); */ +#endif + +inline int compute_intervals_isectline(dReal VERT0[3],dReal VERT1[3],dReal VERT2[3], + dReal VV0,dReal VV1,dReal VV2,dReal D0,dReal D1,dReal D2, + dReal D0D1,dReal D0D2,dReal *isect0,dReal *isect1, + dReal isectpoint0[3],dReal isectpoint1[3]) +{ + if(D0D1>0.0f) + { + /* here we know that D0D2<=0.0 */ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D0D2>0.0f) + { + /* here we know that d0d1<=0.0 */ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D1*D2>0.0f || D0!=0.0f) + { + /* here we know that d0d1<=0.0 or that D0!=0.0 */ + isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D1!=0.0f) + { + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D2!=0.0f) + { + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); + } + else + { + /* triangles are coplanar */ + return 1; + } + return 0; +} + +#define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \ + if(D0D1>0.0f) \ + { \ + /* here we know that D0D2<=0.0 */ \ + /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ + } +#if 0 + else if(D0D2>0.0f) \ + { \ + /* here we know that d0d1<=0.0 */ \ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D1!=0.0f) \ + { \ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D2!=0.0f) \ + { \ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + coplanar=1; \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } +#endif + + + +static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, + dReal isectpt1[3],dReal isectpt2[3]) +{ + dReal E1[3],E2[3]; + dReal N1[3],N2[3],d1,d2; + dReal du0,du1,du2,dv0,dv1,dv2; + dReal D[3]; + dReal isect1[2], isect2[2]; + dReal isectpointA1[3],isectpointA2[3]; + dReal isectpointB1[3],isectpointB2[3]; + dReal du0du1,du0du2,dv0dv1,dv0dv2; + short index; + dReal vp0,vp1,vp2; + dReal up0,up1,up2; + dReal b,c,max; + int smallest1,smallest2; + + /* compute plane equation of triangle(V0,V1,V2) */ + SUB(E1,V1,V0); + SUB(E2,V2,V0); + CROSS(N1,E1,E2); + d1=-DOT(N1,V0); + /* plane equation 1: N1.X+d1=0 */ + + /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/ + du0=DOT(N1,U0)+d1; + du1=DOT(N1,U1)+d1; + du2=DOT(N1,U2)+d1; + + /* coplanarity robustness check */ +#if USE_EPSILON_TEST==TRUE + if(dFabs(du0)0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */ + return 0; /* no intersection occurs */ + + /* compute plane of triangle (U0,U1,U2) */ + SUB(E1,U1,U0); + SUB(E2,U2,U0); + CROSS(N2,E1,E2); + d2=-DOT(N2,U0); + /* plane equation 2: N2.X+d2=0 */ + + /* put V0,V1,V2 into plane equation 2 */ + dv0=DOT(N2,V0)+d2; + dv1=DOT(N2,V1)+d2; + dv2=DOT(N2,V2)+d2; + +#if USE_EPSILON_TEST==TRUE + if(dFabs(dv0)0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */ + return 0; /* no intersection occurs */ + + /* compute direction of intersection line */ + CROSS(D,N1,N2); + + /* compute and index to the largest component of D */ + max= dFabs(D[0]); + index=0; + b= dFabs(D[1]); + c= dFabs(D[2]); + if(b>max) max=b,index=1; + if(c>max) max=c,index=2; + + /* this is the simplified projection onto L*/ + vp0=V0[index]; + vp1=V1[index]; + vp2=V2[index]; + + up0=U0[index]; + up1=U1[index]; + up2=U2[index]; + + /* compute interval for triangle 1 */ + *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2, + dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2); + if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); + + + /* compute interval for triangle 2 */ + compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2, + du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2); + + SORT2(isect1[0],isect1[1],smallest1); + SORT2(isect2[0],isect2[1],smallest2); + + if(isect1[1]isect1[1]) + { + if(smallest1==0) { SET(isectpt2,isectpointA2); } + else { SET(isectpt2,isectpointA1); } + } + else + { + if(smallest2==0) { SET(isectpt2,isectpointB2); } + else { SET(isectpt2,isectpointB1); } + } + } + return 1; +} + + + + + +// Find the intersectiojn point between a coplanar line segement, +// defined by X1 and X2, and a ray defined by X3 and direction N. +// +// This forumla for this calculation is: +// (c x b) . (a x b) +// Q = x1 + a ------------------- +// | a x b | ^2 +// +// where a = x2 - x1 +// b = x4 - x3 +// c = x3 - x1 +// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt +// and x4 is (CoplanarPt - n) +static int +IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, + dVector3 out_pt) +{ + dVector3 a, b, c, x4; + + ADD(x4, x3, n); // x4 = x3 + n + + SUB(a, x2, x1); // a = x2 - x1 + SUB(b, x4, x3); + SUB(c, x3, x1); + + dVector3 tmp1, tmp2; + CROSS(tmp1, c, b); + CROSS(tmp2, a, b); + + dReal num, denom; + num = dDOT(tmp1, tmp2); + denom = LENGTH( tmp2 ); + + dReal s; + s = num /(denom*denom); + + for (int i=0; i<3; i++) + out_pt[i] = x1[i] + a[i]*s; + + // Test if this intersection is "behind" x3, w.r.t. n + SUB(a, x3, out_pt); + if (dDOT(a, n) > 0.0) + return 0; + + // Test if this intersection point is outside the edge limits, + // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside + // else outside + SUB(a, out_pt, x1); + SUB(b, out_pt, x2); + if (dDOT(a,b) < 0.0) + return 1; + else + return 0; +} + + +// FindTriSolidIntersection - Clips the input trinagle TRI with the +// sides of a convex bounding solid, described by PLANES, returning +// the (convex) clipped polygon in CLIPPEDPOLYGON +// +static bool +FindTriSolidIntrsection(const dVector3 Tri[3], + const dVector4 Planes[6], int numSides, + LineContactSet& ClippedPolygon ) +{ + // Set up the LineContactSet structure + for (int k=0; k<3; k++) { + SET(ClippedPolygon.Points[k], Tri[k]); + } + ClippedPolygon.Count = 3; + + // Clip wrt the sides + for ( int i = 0; i < numSides; i++ ) + ClipConvexPolygonAgainstPlane( Planes[i], Planes[i][3], ClippedPolygon ); + + return (ClippedPolygon.Count > 0); +} + + + + +// ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by +// CONTACTS, with a plane (described by N and C). Note: the input +// vertices are assumed to be in counterclockwise order. +// +// This code is taken from The Nebula Device: +// http://nebuladevice.sourceforge.net/cgi-bin/twiki/view/Nebula/WebHome +// and is licensed under the following license: +// http://nebuladevice.sourceforge.net/doc/source/license.txt +// +static void +ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, + LineContactSet& Contacts ) +{ + // test on which side of line are the vertices + int Positive = 0, Negative = 0, PIndex = -1; + int Quantity = Contacts.Count; + + dReal Test[8]; + for ( int i = 0; i < Contacts.Count; i++ ) { + // An epsilon is used here because it is possible for the dot product + // and C to be exactly equal to each other (in theory), but differ + // slightly because of floating point problems. Thus, add a little + // to the test number to push actually equal numbers over the edge + // towards the positive. This should probably be somehow a relative + // tolerance, and I don't think multiplying by the constant is the best + // way to do this. + Test[i] = dDOT(N, Contacts.Points[i]) - C + dFabs(C)*REAL(1e-08); + + if (Test[i] >= REAL(0.0)) { + Positive++; + if (PIndex < 0) { + PIndex = i; + } + } + else Negative++; + } + + if (Positive > 0) { + if (Negative > 0) { + // plane transversely intersects polygon + dVector3 CV[8]; + int CQuantity = 0, Cur, Prv; + dReal T; + + if (PIndex > 0) { + // first clip vertex on line + Cur = PIndex; + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // vertices on positive side of line + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + + // last clip vertex on line + if (Cur < Quantity) { + Prv = Cur - 1; + } + else { + Cur = 0; + Prv = Quantity - 1; + } + + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + } + else { + // iPIndex is 0 + // vertices on positive side of line + Cur = 0; + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + + // last clip vertex on line + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // skip vertices on negative side + while (Cur < Quantity && Test[Cur] < REAL(0.0)) { + Cur++; + } + + // first clip vertex on line + if (Cur < Quantity) { + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // vertices on positive side of line + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + } + else { + // iCur = 0 + Prv = Quantity - 1; + T = Test[0] / (Test[0] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[0][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[0][0]); + CV[CQuantity][1] = Contacts.Points[0][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[0][1]); + CV[CQuantity][2] = Contacts.Points[0][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[0][2]); + CV[CQuantity][3] = Contacts.Points[0][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[0][3]); + CQuantity++; + } + } + Quantity = CQuantity; + memcpy( Contacts.Points, CV, CQuantity * sizeof(dVector3) ); + } + // else polygon fully on positive side of plane, nothing to do + Contacts.Count = Quantity; + } + else { + Contacts.Count = 0; // This should not happen, but for safety + } + +} + + + +// Determine if a potential collision point is +// +// +static int +ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point) +{ + // Cast a ray from in_point, along the collison normal. Does it intersect the + // collision face. + dReal t, u, v; + + if (!RayTriangleIntersect(in_point, in_n, v_col[0], v_col[1], v_col[2], + &t, &u, &v)) + return 0; + else + return 1; +} + + + +// RayTriangleIntersect - If an intersection is found, t contains the +// distance along the ray (dir) and u/v contain u/v coordinates into +// the triangle. Returns 0 if no hit is found +// From "Real-Time Rendering," page 305 +// +static int +RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v) + +{ + dReal edge1[3], edge2[3], tvec[3], pvec[3], qvec[3]; + dReal det,inv_det; + + // find vectors for two edges sharing vert0 + SUB(edge1, vert1, vert0); + SUB(edge2, vert2, vert0); + + // begin calculating determinant - also used to calculate U parameter + CROSS(pvec, dir, edge2); + + // if determinant is near zero, ray lies in plane of triangle + det = DOT(edge1, pvec); + + if ((det > REAL(-0.001)) && (det < REAL(0.001))) + return 0; + inv_det = 1.0 / det; + + // calculate distance from vert0 to ray origin + SUB(tvec, orig, vert0); + + // calculate U parameter and test bounds + *u = DOT(tvec, pvec) * inv_det; + if ((*u < 0.0) || (*u > 1.0)) + return 0; + + // prepare to test V parameter + CROSS(qvec, tvec, edge1); + + // calculate V parameter and test bounds + *v = DOT(dir, qvec) * inv_det; + if ((*v < 0.0) || ((*u + *v) > 1.0)) + return 0; + + // calculate t, ray intersects triangle + *t = DOT(edge2, qvec) * inv_det; + + return 1; +} + + + +static bool +SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, + dVector3 in_n, dVector3* in_col_v, dReal &out_depth) +{ + dReal dp = 0.0; + dReal contact_elt_length; + + DEPTH(dp, in_CoplanarPt, in_v, in_n); + + if (dp >= 0.0) { + // if the penetration depth (calculated above) is more than + // the contact point's ELT, then we've chosen the wrong face + // and should switch faces + contact_elt_length = dFabs(dDOT(in_elt, in_n)); + + if (dp == 0.0) + dp = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (dp < EXPANDED_ELT_THRESH)) + dp = contact_elt_length; + + if ( (dp > 0.0) && (dp <= contact_elt_length)) { + // Add a contact + + if ( ExamineContactPoint(in_col_v, in_n, in_v) ) { + out_depth = dp; + return true; + } + } + } + + return false; +} + + + + +// Generate a "unique" contact. A unique contact has a unique +// position or normal. If the potential contact has the same +// position and normal as an existing contact, but a larger +// penetration depth, this new depth is used instead +// +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxTriMesh* in_TriMesh1, dxTriMesh* in_TriMesh2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount) +{ + /* + NOTE by Oleh_Derevenko: + This function is called after maximal number of contacts has already been + collected because it has a side effect of replacing penetration depth of + existing contact with larger penetration depth of another matching normal contact. + If this logic is not necessary any more, you can bail out on reach of contact + number maximum immediately in dCollideTTL(). You will also need to correct + conditional statements after invocations of GenerateContact() in dCollideTTL(). + */ + dIASSERT(in_Depth >= 0.0); + //if (in_Depth < 0.0) -- the function is always called with depth >= 0 + // return; + + do + { + dContactGeom* Contact; + dVector3 diff; + + if (!(in_Flags & CONTACTS_UNIMPORTANT)) + { + bool duplicate = false; + + for (int i=0; ipos); + if (dDOT(diff, diff) < dEpsilon) + { + // same normal? + if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon)) + { + if (in_Depth > Contact->depth) { + Contact->depth = in_Depth; + SMULT( Contact->normal, in_Normal, -1.0); + Contact->normal[3] = 0.0; + } + duplicate = true; + /* + NOTE by Oleh_Derevenko: + There may be a case when two normals are close to each other but no duplicate + while third normal is detected to be duplicate for both of them. + This is the only reason I can think of, there is no "break" statement. + Perhaps author considered it to be logical that the third normal would + replace the depth in both of initial contacts. + However, I consider it a questionable practice which should not + be applied without deep understanding of underlaying physics. + Even more, is this situation with close normal triplet acceptable at all? + Should not be two initial contacts reduced to one (replaced with the latter)? + If you know the answers for these questions, you may want to change this code. + See the same statement in GenerateContact() of collision_trimesh_box.cpp + */ + } + } + } + + if (duplicate || OutTriCount == (in_Flags & NUMC_MASK)) + { + break; + } + } + else + { + dIASSERT(OutTriCount < (in_Flags & NUMC_MASK)); + } + + // Add a new contact + Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); + + SET( Contact->pos, in_ContactPos ); + Contact->pos[3] = 0.0; + + SMULT( Contact->normal, in_Normal, -1.0); + Contact->normal[3] = 0.0; + + Contact->depth = in_Depth; + + Contact->g1 = in_TriMesh1; + Contact->g2 = in_TriMesh2; + + OutTriCount++; + } + while (false); +} + +#endif // dTRIMESH_OPCODE +#endif // !dTRIMESH_USE_NEW_TRIMESH_TRIMESH_COLLIDER +#endif // dTRIMESH_ENABLED diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp new file mode 100644 index 0000000..c4af41c --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp @@ -0,0 +1,1304 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// OPCODE TriMesh/TriMesh collision code +// Written at 2006-10-28 by Francisco León (http://gimpact.sourceforge.net) + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#include +#include +#include +#include + +// New Implementation +#if dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE + +#define SMALL_ELT REAL(2.5e-4) +#define EXPANDED_ELT_THRESH REAL(1.0e-3) +#define DISTANCE_EPSILON REAL(1.0e-8) +#define VELOCITY_EPSILON REAL(1.0e-5) +#define TINY_PENETRATION REAL(5.0e-6) + +struct LineContactSet +{ + enum + { + MAX_POINTS = 8 + }; + + dVector3 Points[MAX_POINTS]; + int Count; +}; + + +static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); +inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); +static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); +static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); +static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); +static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v); + + +///returns the penetration depth +static dReal MostDeepPoints( + LineContactSet & points, + const dVector3 plane_normal, + dReal plane_dist, + LineContactSet & deep_points); +///returns the penetration depth +static dReal FindMostDeepPointsInTetra( + LineContactSet contact_points, + const dVector3 sourcetri[3],///triangle which contains contact_points + const dVector3 tetra[4], + const dVector4 tetraplanes[4], + dVector3 separating_normal, + LineContactSet deep_points); + +static bool ClipTriByTetra(const dVector3 tri[3], + const dVector3 tetra[4], + LineContactSet& Contacts); +static bool TriTriContacts(const dVector3 tr1[3], + const dVector3 tr2[3], + dxGeom* g1, dxGeom* g2, int Flags, + dContactGeom* Contacts, int Stride, + int &contactcount); + + +/* some math macros */ +#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \ + dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \ + dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; } + +#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) + +#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; } + +#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; } + +#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; } + +#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; } + +#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; } + +#define COMBO(combo,p,t,q) { combo[0]=p[0]+t*q[0]; combo[1]=p[1]+t*q[1]; combo[2]=p[2]+t*q[2]; } + +#define LENGTH(x) ((dReal) 1.0f/InvSqrt(dDOT(x, x))) + +#define DEPTH(d, p, q, n) d = (p[0] - q[0])*n[0] + (p[1] - q[1])*n[1] + (p[2] - q[2])*n[2]; + +inline const dReal dMin(const dReal x, const dReal y) +{ + return x < y ? x : y; +} + + +inline void +SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, + dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, + dVector3 n, dVector3 n1, dVector3 n2) +{ + if (pen_v == v1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } +} + +///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE/////////////////////////////// +////* Written by Francisco León (http://gimpact.sourceforge.net) */// +#define CONTACT_DIFF_EPSILON REAL(0.00001) +#if defined(dDOUBLE) +#define CONTACT_NORMAL_ZERO REAL(0.0000001) +#else // if defined(dSINGLE) +#define CONTACT_NORMAL_ZERO REAL(0.00001) +#endif +#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0) +#define dSQRT3 REAL(1.7320508075688773) + +struct CONTACT_KEY +{ + dContactGeom * m_contact; + unsigned int m_key; +}; + +#define MAXCONTACT_X_NODE 4 +struct CONTACT_KEY_HASH_NODE +{ + CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE]; + int m_keycount; +}; + +#define CONTACTS_HASHSIZE 256 +CONTACT_KEY_HASH_NODE g_hashcontactset[CONTACTS_HASHSIZE]; + + + +void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact) +{ + key.m_contact = contact; + + unsigned int hash=0; + + int i = 0; + + while (true) + { + dReal coord = contact->pos[i]; + coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT); + + unsigned int hash_input = ((unsigned int *)&coord)[0]; + if (sizeof(dReal) / sizeof(unsigned int) != 1) + { + dIASSERT(sizeof(dReal) / sizeof(unsigned int) == 2); + hash_input ^= ((unsigned int *)&coord)[1]; + } + + hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 ); + hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 ); + hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 ); + hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 ); + + if (++i == 3) + { + break; + } + + hash = (hash << 11) | (hash >> 21); + } + + key.m_key = hash; +} + + +static inline unsigned int MakeContactIndex(unsigned int key) +{ + dIASSERT(CONTACTS_HASHSIZE == 256); + + unsigned int index = key ^ (key >> 16); + index = (index ^ (index >> 8)) & 0xFF; + return index; +} + +dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node) +{ + for(int i=0;im_keycount;i++) + { + if(node->m_keyarray[i].m_key == contactkey->m_key) + { + dContactGeom *contactfound = node->m_keyarray[i].m_contact; + if (dDISTANCE(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/) + { + return contactfound; + } + } + } + + if (node->m_keycount < MAXCONTACT_X_NODE) + { + node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact; + node->m_keyarray[node->m_keycount].m_key = contactkey->m_key; + node->m_keycount++; + } + else + { + dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled"); + } + + return contactkey->m_contact; +} + +void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node) +{ + dIASSERT(node->m_keycount > 0); + + if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact) + { + node->m_keycount -= 1; + } + else + { + dIASSERT(node->m_keycount == MAXCONTACT_X_NODE); + } +} + +void RemoveArbitraryContactFromNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node) +{ + dIASSERT(node->m_keycount > 0); + + int keyindex, lastkeyindex = node->m_keycount - 1; + + // Do not check the last contact + for (keyindex = 0; keyindex < lastkeyindex; keyindex++) + { + if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact) + { + node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex]; + break; + } + } + + dIASSERT(keyindex < lastkeyindex || + node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match + + node->m_keycount = lastkeyindex; +} + +void UpdateArbitraryContactInNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node, + dContactGeom *pwithcontact) +{ + dIASSERT(node->m_keycount > 0); + + int keyindex, lastkeyindex = node->m_keycount - 1; + + // Do not check the last contact + for (keyindex = 0; keyindex < lastkeyindex; keyindex++) + { + if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact) + { + break; + } + } + + dIASSERT(keyindex < lastkeyindex || + node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match + + node->m_keyarray[keyindex].m_contact = pwithcontact; +} + +void ClearContactSet() +{ + memset(g_hashcontactset,0,sizeof(CONTACT_KEY_HASH_NODE)*CONTACTS_HASHSIZE); +} + +//return true if found +dContactGeom *InsertContactInSet(const CONTACT_KEY &newkey) +{ + unsigned int index = MakeContactIndex(newkey.m_key); + + return AddContactToNode(&newkey, &g_hashcontactset[index]); +} + +void RemoveNewContactFromSet(const CONTACT_KEY &contactkey) +{ + unsigned int index = MakeContactIndex(contactkey.m_key); + + RemoveNewContactFromNode(&contactkey, &g_hashcontactset[index]); +} + +void RemoveArbitraryContactFromSet(const CONTACT_KEY &contactkey) +{ + unsigned int index = MakeContactIndex(contactkey.m_key); + + RemoveArbitraryContactFromNode(&contactkey, &g_hashcontactset[index]); +} + +void UpdateArbitraryContactInSet(const CONTACT_KEY &contactkey, + dContactGeom *pwithcontact) +{ + unsigned int index = MakeContactIndex(contactkey.m_key); + + UpdateArbitraryContactInNode(&contactkey, &g_hashcontactset[index], pwithcontact); +} + +bool AllocNewContact( + const dVector3 newpoint, dContactGeom *& out_pcontact, + int Flags, dContactGeom* Contacts, + int Stride, int &contactcount) +{ + bool allocated_new = false; + + dContactGeom dLocalContact; + + dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ? + SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact; + + pcontact->pos[0] = newpoint[0]; + pcontact->pos[1] = newpoint[1]; + pcontact->pos[2] = newpoint[2]; + pcontact->pos[3] = 1.0f; + + CONTACT_KEY newkey; + UpdateContactKey(newkey, pcontact); + + dContactGeom *pcontactfound = InsertContactInSet(newkey); + if (pcontactfound == pcontact) + { + if (pcontactfound != &dLocalContact) + { + contactcount++; + } + else + { + RemoveNewContactFromSet(newkey); + pcontactfound = NULL; + } + + allocated_new = true; + } + + out_pcontact = pcontactfound; + return allocated_new; +} + +void FreeExistingContact(dContactGeom *pcontact, + int Flags, dContactGeom *Contacts, + int Stride, int &contactcount) +{ + CONTACT_KEY contactKey; + UpdateContactKey(contactKey, pcontact); + + RemoveArbitraryContactFromSet(contactKey); + + int lastContactIndex = contactcount - 1; + dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride); + + if (pcontact != plastContact) + { + *pcontact = *plastContact; + + CONTACT_KEY lastContactKey; + UpdateContactKey(lastContactKey, plastContact); + + UpdateArbitraryContactInSet(lastContactKey, pcontact); + } + + contactcount = lastContactIndex; +} + + +dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2, + const dVector3 point, + dVector3 normal, + dReal depth, + int Flags, + dContactGeom* Contacts, int Stride, + int &contactcount) +{ + dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code + + dContactGeom * pcontact; + + if (!AllocNewContact(point, pcontact, Flags, Contacts, Stride, contactcount)) + { + const dReal depthDifference = depth - pcontact->depth; + + if (depthDifference > CONTACT_DIFF_EPSILON) + { + pcontact->normal[0] = normal[0]; + pcontact->normal[1] = normal[1]; + pcontact->normal[2] = normal[2]; + pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging + pcontact->depth = depth; + + pcontact->g1 = g1; + pcontact->g2 = g2; + } + else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average + { + if(pcontact->g1 == g2) + { + MULT(normal,normal, REAL(-1.0)); + } + + const dReal oldLen = pcontact->normal[3]; + COMBO(pcontact->normal, normal, oldLen, pcontact->normal); + + const dReal len = LENGTH(pcontact->normal); + if (len > CONTACT_NORMAL_ZERO) + { + MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len); + pcontact->normal[3] = len; + } + else + { + FreeExistingContact(pcontact, Flags, Contacts, Stride, contactcount); + } + } + } + // Contact can be not available if buffer is full + else if (pcontact) + { + pcontact->normal[0] = normal[0]; + pcontact->normal[1] = normal[1]; + pcontact->normal[2] = normal[2]; + pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging + pcontact->depth = depth; + pcontact->g1 = g1; + pcontact->g2 = g2; + } + + return pcontact; +} + +//////////////////////////////////////////////////////////////////////////////////////////// + + + + + +int +dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (g2->type == dTriMeshClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh1 = (dxTriMesh*) g1; + dxTriMesh* TriMesh2 = (dxTriMesh*) g2; + + dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals; + dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals; + + const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); + // TLRotation1 = column-major order + const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); + + const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); + // TLRotation2 = column-major order + const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); + + AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider; + + + static BVTCache ColCache; + ColCache.Model0 = &TriMesh1->Data->BVTree; + ColCache.Model1 = &TriMesh2->Data->BVTree; + + ////Prepare contact list + ClearContactSet(); + + // Collision query + Matrix4x4 amatrix, bmatrix; + BOOL IsOk = Collider.Collide(ColCache, + &MakeMatrix(TLPosition1, TLRotation1, amatrix), + &MakeMatrix(TLPosition2, TLRotation2, bmatrix) ); + + + // Make "double" versions of these matrices, if appropriate + dMatrix4 A, B; + dMakeMatrix4(TLPosition1, TLRotation1, A); + dMakeMatrix4(TLPosition2, TLRotation2, B); + + + + + if (IsOk) { + // Get collision status => if true, objects overlap + if ( Collider.GetContactStatus() ) { + // Number of colliding pairs and list of pairs + int TriCount = Collider.GetNbPairs(); + const Pair* CollidingPairs = Collider.GetPairs(); + + if (TriCount > 0) { + // step through the pairs, adding contacts + int id1, id2; + int OutTriCount = 0; + dVector3 v1[3], v2[3]; + + // only do these expensive inversions once + /*dMatrix4 InvMatrix1, InvMatrix2; + dInvertMatrix4(A, InvMatrix1); + dInvertMatrix4(B, InvMatrix2);*/ + + + for (int i = 0; i < TriCount; i++) + { + id1 = CollidingPairs[i].id0; + id2 = CollidingPairs[i].id1; + + // grab the colliding triangles + FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1); + FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2); + // Since we'll be doing matrix transformations, we need to + // make sure that all vertices have four elements + for (int j=0; j<3; j++) { + v1[j][3] = 1.0; + v2[j][3] = 1.0; + } + + TriTriContacts(v1,v2, + g1, g2, Flags, + Contacts,Stride,OutTriCount); + + // Continue loop even after contacts are full + // as existing contacts' normals/depths might be updated + // Break only if contacts are not important + if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) + { + break; + } + } + + // Return the number of contacts + return OutTriCount; + + } + } + } + + + // There was some kind of failure during the Collide call or + // there are no faces overlapping + return 0; +} + + + +static void +GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) +{ + dVector3 Out[3]; + + FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); + + for (int i = 0; i < 3; i++) + triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); +} + + +// +// +// +#define B11 B[0] +#define B12 B[1] +#define B13 B[2] +#define B14 B[3] +#define B21 B[4] +#define B22 B[5] +#define B23 B[6] +#define B24 B[7] +#define B31 B[8] +#define B32 B[9] +#define B33 B[10] +#define B34 B[11] +#define B41 B[12] +#define B42 B[13] +#define B43 B[14] +#define B44 B[15] + +#define Binv11 Binv[0] +#define Binv12 Binv[1] +#define Binv13 Binv[2] +#define Binv14 Binv[3] +#define Binv21 Binv[4] +#define Binv22 Binv[5] +#define Binv23 Binv[6] +#define Binv24 Binv[7] +#define Binv31 Binv[8] +#define Binv32 Binv[9] +#define Binv33 Binv[10] +#define Binv34 Binv[11] +#define Binv41 Binv[12] +#define Binv42 Binv[13] +#define Binv43 Binv[14] +#define Binv44 Binv[15] + +inline void +dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) +{ + B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; + B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; + B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; + + B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; +} + + +static void +dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) +{ + dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) + -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) + +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) + +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) + -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) + +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); + + dAASSERT (det != 0.0); + + det = 1.0 / det; + + Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); + Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); + Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); + Binv14 = 0.0f; + Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); + Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); + Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); + Binv24 = 0.0f; + Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); + Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); + Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); + Binv34 = 0.0f; + Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); + Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); + Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); + Binv44 = 1.0f; +} + + + +// Find the intersectiojn point between a coplanar line segement, +// defined by X1 and X2, and a ray defined by X3 and direction N. +// +// This forumla for this calculation is: +// (c x b) . (a x b) +// Q = x1 + a ------------------- +// | a x b | ^2 +// +// where a = x2 - x1 +// b = x4 - x3 +// c = x3 - x1 +// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt +// and x4 is (CoplanarPt - n) +static int +IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, + dVector3 out_pt) +{ + dVector3 a, b, c, x4; + + ADD(x4, x3, n); // x4 = x3 + n + + SUB(a, x2, x1); // a = x2 - x1 + SUB(b, x4, x3); + SUB(c, x3, x1); + + dVector3 tmp1, tmp2; + CROSS(tmp1, c, b); + CROSS(tmp2, a, b); + + dReal num, denom; + num = dDOT(tmp1, tmp2); + denom = LENGTH( tmp2 ); + + dReal s; + s = num /(denom*denom); + + for (int i=0; i<3; i++) + out_pt[i] = x1[i] + a[i]*s; + + // Test if this intersection is "behind" x3, w.r.t. n + SUB(a, x3, out_pt); + if (dDOT(a, n) > 0.0) + return 0; + + // Test if this intersection point is outside the edge limits, + // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside + // else outside + SUB(a, out_pt, x1); + SUB(b, out_pt, x2); + if (dDOT(a,b) < 0.0) + return 1; + else + return 0; +} + + + +void PlaneClipSegment( const dVector3 s1, const dVector3 s2, + const dVector3 N, dReal C, dVector3 clipped) +{ + dReal dis1,dis2; + dis1 = DOT(s1,N)-C; + SUB(clipped,s2,s1); + dis2 = DOT(clipped,N); + MULT(clipped,clipped,-dis1/dis2); + ADD(clipped,clipped,s1); + clipped[3] = 1.0f; +} + +/* ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by + CONTACTS, with a plane (described by N and C distance from origin). + Note: the input vertices are assumed to be in invcounterclockwise order. + changed by Francisco Leon (http://gimpact.sourceforge.net) */ +static void +ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, + LineContactSet& Contacts ) +{ + int i, vi, prevclassif=32000, classif; + /* + classif 0 : back, 1 : front + */ + + dReal d; + dVector3 clipped[8]; + int clippedcount =0; + + if(Contacts.Count==0) + { + return; + } + for(i=0;i<=Contacts.Count;i++) + { + vi = i%Contacts.Count; + + d = DOT(N,Contacts.Points[vi]) - C; + ////classify point + if(d>REAL(1.0e-8)) classif = 1; + else classif = 0; + + if(classif == 0)//back + { + if(i>0) + { + if(prevclassif==1)///in front + { + + ///add clipped point + if(clippedcount<8) + { + PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi], + N,C,clipped[clippedcount]); + clippedcount++; + } + } + } + ///add point + if(clippedcount<8&&i0) + { + if(prevclassif==0) + { + ///add point + if(clippedcount<8) + { + PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi], + N,C,clipped[clippedcount]); + clippedcount++; + } + } + } + } + + prevclassif = classif; + } + + if(clippedcount==0) + { + Contacts.Count = 0; + return; + } + Contacts.Count = clippedcount; + memcpy( Contacts.Points, clipped, clippedcount * sizeof(dVector3) ); + return; +} + + +bool BuildPlane(const dVector3 s0, const dVector3 s1,const dVector3 s2, + dVector3 Normal, dReal & Dist) +{ + dVector3 e0,e1; + SUB(e0,s1,s0); + SUB(e1,s2,s0); + + CROSS(Normal,e0,e1); + + if (!dSafeNormalize3(Normal)) + { + return false; + } + + Dist = DOT(Normal,s0); + return true; + +} + +bool BuildEdgesDir(const dVector3 s0, const dVector3 s1, + const dVector3 t0, const dVector3 t1, + dVector3 crossdir) +{ + dVector3 e0,e1; + + SUB(e0,s1,s0); + SUB(e1,t1,t0); + CROSS(crossdir,e0,e1); + + if (!dSafeNormalize3(crossdir)) + { + return false; + } + return true; +} + + + +bool BuildEdgePlane( + const dVector3 s0, const dVector3 s1, + const dVector3 normal, + dVector3 plane_normal, + dReal & plane_dist) +{ + dVector3 e0; + + SUB(e0,s1,s0); + CROSS(plane_normal,e0,normal); + if (!dSafeNormalize3(plane_normal)) + { + return false; + } + plane_dist = DOT(plane_normal,s0); + return true; +} + + + + +/* +Positive penetration +Negative number: they are separated +*/ +dReal IntervalPenetration(dReal &vmin1,dReal &vmax1, + dReal &vmin2,dReal &vmax2) +{ + if(vmax1<=vmin2) + { + return -(vmin2-vmax1); + } + else + { + if(vmax2<=vmin1) + { + return -(vmin1-vmax2); + } + else + { + if(vmax1<=vmax2) + { + return vmax1-vmin2; + } + + return vmax2-vmin1; + } + + } + return 0; +} + +void FindInterval( + const dVector3 * vertices, int verticecount, + dVector3 dir,dReal &vmin,dReal &vmax) +{ + + dReal dist; + int i; + vmin = DOT(vertices[0],dir); + vmax = vmin; + for(i=1;idist) vmin=dist; + else if(vmaxmaxdeep) + { + maxdeep = dist; + deep_points.Count=1; + max_candidates[deep_points.Count-1] = i; + } + else if(dist+REAL(0.000001)>=maxdeep) + { + deep_points.Count++; + max_candidates[deep_points.Count-1] = i; + } + } + + for(i=0;i0.0) + { + MULT(separating_normal,separating_normal,-1.0f); + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + else + { + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + + } + else + { + vmin1 = DOT(separating_normal,tri2plane); + if(vmin1<0.0) + { + MULT(separating_normal,separating_normal,-1.0f); + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + else + { + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + + } + + + + + } + else + { + MULT(separating_normal,crossdir,1.0f/vmin1); + + vmin1 = DOT(separating_normal,tri1plane); + if(vmin1>0.0) + { + MULT(separating_normal,separating_normal,-1.0f); + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + else + { + deep_points.Count = 1; + SET(deep_points.Points[0],pt2); + } + + + } + + + }*/ + return maxdeep; +} + + + +///SUPPORT UP TO 8 CONTACTS +bool TriTriContacts(const dVector3 tr1[3], + const dVector3 tr2[3], + dxGeom* g1, dxGeom* g2, int Flags, + dContactGeom* Contacts, int Stride, + int &contactcount) +{ + + + dVector4 normal; + dReal depth; + ///Test Tri Vs Tri +// dContactGeom* pcontact; + int ccount = 0; + LineContactSet contactpoints; + contactpoints.Count = 0; + + + + ///find best direction + + depth = FindTriangleTriangleCollision( + tr1, + tr2, + normal, + contactpoints); + + + + if(depth<0.0f) return false; + + ccount = 0; + while (ccount +#include +#include +#include "collision_util.h" + +//**************************************************************************** + +int dCollideSpheres (dVector3 p1, dReal r1, + dVector3 p2, dReal r2, dContactGeom *c) +{ + // printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n", + // d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2); + + dReal d = dDISTANCE (p1,p2); + if (d > (r1 + r2)) return 0; + if (d <= 0) { + c->pos[0] = p1[0]; + c->pos[1] = p1[1]; + c->pos[2] = p1[2]; + c->normal[0] = 1; + c->normal[1] = 0; + c->normal[2] = 0; + c->depth = r1 + r2; + } + else { + dReal d1 = dRecip (d); + c->normal[0] = (p1[0]-p2[0])*d1; + c->normal[1] = (p1[1]-p2[1])*d1; + c->normal[2] = (p1[2]-p2[2])*d1; + dReal k = REAL(0.5) * (r2 - r1 - d); + c->pos[0] = p1[0] + c->normal[0]*k; + c->pos[1] = p1[1] + c->normal[1]*k; + c->pos[2] = p1[2] + c->normal[2]*k; + c->depth = r1 + r2 - d; + } + return 1; +} + + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta) +{ + dVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + dReal uaub = dDOT(ua,ub); + dReal q1 = dDOT(ua,p); + dReal q2 = -dDOT(ub,p); + dReal d = 1-uaub*uaub; + if (d <= REAL(0.0001)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = dRecip(d); + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + +// given two line segments A and B with endpoints a1-a2 and b1-b2, return the +// points on A and B that are closest to each other (in cp1 and cp2). +// in the case of parallel lines where there are multiple solutions, a +// solution involving the endpoint of at least one line will be returned. +// this will work correctly for zero length lines, e.g. if a1==a2 and/or +// b1==b2. +// +// the algorithm works by applying the voronoi clipping rule to the features +// of the line segments. the three features of each line segment are the two +// endpoints and the line between them. the voronoi clipping rule states that, +// for feature X on line A and feature Y on line B, the closest points PA and +// PB between X and Y are globally the closest points if PA is in V(Y) and +// PB is in V(X), where V(X) is the voronoi region of X. + +void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2, + const dVector3 b1, const dVector3 b2, + dVector3 cp1, dVector3 cp2) +{ + dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n; + dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det; + +#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2]; +#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; + + // check vertex-vertex features + + SET3 (a1a2,a2,-,a1); + SET3 (b1b2,b2,-,b1); + SET3 (a1b1,b1,-,a1); + da1 = dDOT(a1a2,a1b1); + db1 = dDOT(b1b2,a1b1); + if (da1 <= 0 && db1 >= 0) { + SET2 (cp1,a1); + SET2 (cp2,b1); + return; + } + + SET3 (a1b2,b2,-,a1); + da2 = dDOT(a1a2,a1b2); + db2 = dDOT(b1b2,a1b2); + if (da2 <= 0 && db2 <= 0) { + SET2 (cp1,a1); + SET2 (cp2,b2); + return; + } + + SET3 (a2b1,b1,-,a2); + da3 = dDOT(a1a2,a2b1); + db3 = dDOT(b1b2,a2b1); + if (da3 >= 0 && db3 >= 0) { + SET2 (cp1,a2); + SET2 (cp2,b1); + return; + } + + SET3 (a2b2,b2,-,a2); + da4 = dDOT(a1a2,a2b2); + db4 = dDOT(b1b2,a2b2); + if (da4 >= 0 && db4 <= 0) { + SET2 (cp1,a2); + SET2 (cp2,b2); + return; + } + + // check edge-vertex features. + // if one or both of the lines has zero length, we will never get to here, + // so we do not have to worry about the following divisions by zero. + + la = dDOT(a1a2,a1a2); + if (da1 >= 0 && da3 <= 0) { + k = da1 / la; + SET3 (n,a1b1,-,k*a1a2); + if (dDOT(b1b2,n) >= 0) { + SET3 (cp1,a1,+,k*a1a2); + SET2 (cp2,b1); + return; + } + } + + if (da2 >= 0 && da4 <= 0) { + k = da2 / la; + SET3 (n,a1b2,-,k*a1a2); + if (dDOT(b1b2,n) <= 0) { + SET3 (cp1,a1,+,k*a1a2); + SET2 (cp2,b2); + return; + } + } + + lb = dDOT(b1b2,b1b2); + if (db1 <= 0 && db2 >= 0) { + k = -db1 / lb; + SET3 (n,-a1b1,-,k*b1b2); + if (dDOT(a1a2,n) >= 0) { + SET2 (cp1,a1); + SET3 (cp2,b1,+,k*b1b2); + return; + } + } + + if (db3 <= 0 && db4 >= 0) { + k = -db3 / lb; + SET3 (n,-a2b1,-,k*b1b2); + if (dDOT(a1a2,n) <= 0) { + SET2 (cp1,a2); + SET3 (cp2,b1,+,k*b1b2); + return; + } + } + + // it must be edge-edge + + k = dDOT(a1a2,b1b2); + det = la*lb - k*k; + if (det <= 0) { + // this should never happen, but just in case... + SET2(cp1,a1); + SET2(cp2,b1); + return; + } + det = dRecip (det); + dReal alpha = (lb*da1 - k*db1) * det; + dReal beta = ( k*da1 - la*db1) * det; + SET3 (cp1,a1,+,alpha*a1a2); + SET3 (cp2,b1,+,beta*b1b2); + +# undef SET2 +# undef SET3 +} + + +// a simple root finding algorithm is used to find the value of 't' that +// satisfies: +// d|D(t)|^2/dt = 0 +// where: +// |D(t)| = |p(t)-b(t)| +// where p(t) is a point on the line parameterized by t: +// p(t) = p1 + t*(p2-p1) +// and b(t) is that same point clipped to the boundary of the box. in box- +// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components +// each of which looks like this: +// +// t_lo / +// ______/ -->t +// / t_hi +// / +// +// t_lo and t_hi are the t values where the line passes through the planes +// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt +// in a piecewise fashion from t=0 to t=1, stopping at the point where +// d|D(t)|^2/dt crosses from negative to positive. + +void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, + const dVector3 c, const dMatrix3 R, + const dVector3 side, + dVector3 lret, dVector3 bret) +{ + int i; + + // compute the start and delta of the line p1-p2 relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = p1[0] - c[0]; + tmp[1] = p1[1] - c[1]; + tmp[2] = p1[2] - c[2]; + dMULTIPLY1_331 (s,R,tmp); + tmp[0] = p2[0] - p1[0]; + tmp[1] = p2[1] - p1[1]; + tmp[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (v,R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = -1; + } + else sign[i] = 1; + } + + // compute v^2 + dVector3 v2; + v2[0] = v[0]*v[0]; + v2[1] = v[1]*v[1]; + v2[2] = v[2]*v[2]; + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * side[0]; + h[1] = REAL(0.5) * side[1]; + h[2] = REAL(0.5) * side[2]; + + // region is -1,0,+1 depending on which side of the box planes each + // coordinate is on. tanchor is the next t value at which there is a + // transition, or the last one if there are no more. + int region[3]; + dReal tanchor[3]; + + // Denormals are a problem, because we divide by v[i], and then + // multiply that by 0. Alas, infinity times 0 is infinity (!) + // We also use v2[i], which is v[i] squared. Here's how the epsilons + // are chosen: + // float epsilon = 1.175494e-038 (smallest non-denormal number) + // double epsilon = 2.225074e-308 (smallest non-denormal number) + // For single precision, choose an epsilon such that v[i] squared is + // not a denormal; this is for performance. + // For double precision, choose an epsilon such that v[i] is not a + // denormal; this is for correctness. (Jon Watte on mailinglist) + +#if defined( dSINGLE ) + const dReal tanchor_eps = REAL(1e-19); +#else + const dReal tanchor_eps = REAL(1e-307); +#endif + + // find the region and tanchor values for p1 + for (i=0; i<3; i++) { + if (v[i] > tanchor_eps) { + if (s[i] < -h[i]) { + region[i] = -1; + tanchor[i] = (-h[i]-s[i])/v[i]; + } + else { + region[i] = (s[i] > h[i]); + tanchor[i] = (h[i]-s[i])/v[i]; + } + } + else { + region[i] = 0; + tanchor[i] = 2; // this will never be a valid tanchor + } + } + + // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point + dReal t=0; + dReal dd2dt = 0; + for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i]; + if (dd2dt >= 0) goto got_answer; + + do { + // find the point on the line that is at the next clip plane boundary + dReal next_t = 1; + for (i=0; i<3; i++) { + if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t) + next_t = tanchor[i]; + } + + // compute d|d|^2/dt for the next t + dReal next_dd2dt = 0; + for (i=0; i<3; i++) { + next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]); + } + + // if the sign of d|d|^2/dt has changed, solution = the crossover point + if (next_dd2dt >= 0) { + dReal m = (next_dd2dt-dd2dt)/(next_t - t); + t -= dd2dt/m; + goto got_answer; + } + + // advance to the next anchor point / region + for (i=0; i<3; i++) { + if (tanchor[i] == next_t) { + tanchor[i] = (h[i]-s[i])/v[i]; + region[i]++; + } + } + t = next_t; + dd2dt = next_dd2dt; + } + while (t < 1); + t = 1; + + got_answer: + + // compute closest point on the line + for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1 + + // compute closest point on the box + for (i=0; i<3; i++) { + tmp[i] = sign[i] * (s[i] + t*v[i]); + if (tmp[i] < -h[i]) tmp[i] = -h[i]; + else if (tmp[i] > h[i]) tmp[i] = h[i]; + } + dMULTIPLY0_331 (s,R,tmp); + for (i=0; i<3; i++) bret[i] = s[i] + c[i]; +} + + +// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect +// or 0 if not. + +int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + // two boxes are disjoint if (and only if) there is a separating axis + // perpendicular to a face from one box or perpendicular to an edge from + // either box. the following tests are derived from: + // "OBB Tree: A Hierarchical Structure for Rapid Interference Detection", + // S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996. + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2. + // Qij is abs(Rij) + dVector3 p,pp; + dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5); + B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5); + + // for the following tests, excluding computation of Rij, in the worst case, + // 15 compares, 60 adds, 81 multiplies, and 24 absolutes. + // notation: R1=[u1 u2 u3], R2=[v1 v2 v3] + + // separating axis = u1,u2,u3 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); + if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0; + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); + if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0; + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); + if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0; + + // separating axis = v1,v2,v3 + if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0; + if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0; + if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0; + + // separating axis = u1 x (v1,v2,v3) + if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0; + if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0; + if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0; + + // separating axis = u2 x (v1,v2,v3) + if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0; + if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0; + if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0; + + // separating axis = u3 x (v1,v2,v3) + if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0; + if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0; + if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0; + + return 1; +} + +//**************************************************************************** +// other utility functions + +void dInfiniteAABB (dxGeom *geom, dReal aabb[6]) +{ + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + + +//**************************************************************************** +// Helpers for Croteam's collider - by Nguyen Binh + +int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = dPointPlaneDistance( vEpnt0 ,plPlane ); + dReal fDistance1 = dPointPlaneDistance( vEpnt1 ,plPlane ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return 0; + // if both points in front of the plane + } + else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return 1; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + dVector3Copy(vIntersectionPoint,vEpnt0); + } else + { + dVector3Copy(vIntersectionPoint,vEpnt1); + } + return 1; + } + return 1; +} + +// clip polygon with plane and generate new polygon points +void dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1= 0 ) { + // emit point + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - + (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - + (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - + (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + +} + +void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ,dReal fRadius) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1= 0 ) + { + // emit point + if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius) + { + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - + (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - + (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - + (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius) + { + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + } +} + diff --git a/libraries/ode-0.9/ode/src/collision_util.h b/libraries/ode-0.9/ode/src/collision_util.h new file mode 100644 index 0000000..4c479e3 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_util.h @@ -0,0 +1,340 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +some useful collision utility stuff. + +*/ + +#ifndef _ODE_COLLISION_UTIL_H_ +#define _ODE_COLLISION_UTIL_H_ + +#include +#include +#include +#include + + +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + +#if 1 +#include "collision_kernel.h" +// Fetches a contact +inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ + dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK)); + return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); +} +#endif + + +// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and +// return 1, else return 0. + +int dCollideSpheres (dVector3 p1, dReal r1, + dVector3 p2, dReal r2, dContactGeom *c); + + +// given two lines +// qa = pa + alpha* ua +// qb = pb + beta * ub +// where pa,pb are two points, ua,ub are two unit length vectors, and alpha, +// beta go from [-inf,inf], return alpha and beta such that qa and qb are +// as close as possible + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta); + + +// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length +// vector 'side'), compute the points of closest approach between the box +// and the line. return these points in 'lret' (the point on the line) and +// 'bret' (the point on the box). if the line actually penetrates the box +// then the solution is not unique, but only one solution will be returned. +// in this case the solution points will coincide. + +void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, + const dVector3 c, const dMatrix3 R, + const dVector3 side, + dVector3 lret, dVector3 bret); + +// 20 Apr 2004 +// Start code by Nguyen Binh +int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); +// clip polygon with plane and generate new polygon points +void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ); + +void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius); + +// Some vector math +inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c) +{ + c[0] = a[0] - b[0]; + c[1] = a[1] - b[1]; + c[2] = a[2] - b[2]; +} + +// Some vector math +inline void dVector3Scale(dVector3& a,dReal nScale) +{ + a[0] *= nScale ; + a[1] *= nScale ; + a[2] *= nScale ; +} + +inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c) +{ + c[0] = a[0] + b[0]; + c[1] = a[1] + b[1]; + c[2] = a[2] + b[2]; +} + +inline void dVector3Copy(const dVector3& a,dVector3& c) +{ + c[0] = a[0]; + c[1] = a[1]; + c[2] = a[2]; +} + +inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c) +{ + dCROSS(c,=,a,b); +} + +inline dReal dVector3Length(const dVector3& a) +{ + return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); +} + +inline dReal dVector3Dot(const dVector3& a,const dVector3& b) +{ + return dDOT(a,b); +} + +inline void dVector3Inv(dVector3& a) +{ + a[0] = -a[0]; + a[1] = -a[1]; + a[2] = -a[2]; +} + +inline dReal dVector3Length2(const dVector3& a) +{ + return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); +} + +inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v) +{ + v[0] = m[col + 0]; + v[1] = m[col + 4]; + v[2] = m[col + 8]; +} + +inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) +{ + r[0] = v[1] * m[2*4 + col] - v[2] * m[1*4 + col]; + r[1] = v[2] * m[0*4 + col] - v[0] * m[2*4 + col]; + r[2] = v[0] * m[1*4 + col] - v[1] * m[0*4 + col]; +} + +inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) +{ + r[0] = v[2] * m[1*4 + col] - v[1] * m[2*4 + col]; + r[1] = v[0] * m[2*4 + col] - v[2] * m[0*4 + col]; + r[2] = v[1] * m[0*4 + col] - v[0] * m[1*4 + col]; +} + +inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r) +{ + dMULTIPLY0_331(r,m,v); +} + +inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane) +{ + return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]); +} + +inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane) +{ + plane[0] = normal[0]; + plane[1] = normal[1]; + plane[2] = normal[2]; + plane[3] = distance; +} + +inline void dMatrix3Copy(const dReal* source,dMatrix3& dest) +{ + dest[0] = source[0]; + dest[1] = source[1]; + dest[2] = source[2]; + + dest[4] = source[4]; + dest[5] = source[5]; + dest[6] = source[6]; + + dest[8] = source[8]; + dest[9] = source[9]; + dest[10]= source[10]; +} + +inline dReal dMatrix3Det( const dMatrix3& mat ) +{ + dReal det; + + det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) + - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) + + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); + + return( det ); +} + + +inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst ) +{ + dReal det = dMatrix3Det( ma ); + + if ( dFabs( det ) < REAL(0.0005) ) + { + dRSetIdentity( dst ); + return; + } + + dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det; + dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det; + dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det; + + dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det; + dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det; + dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det; + + dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det; + dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det; + dst[10] = ma[0]*ma[5] - ma[1]*ma[4] / det; +} + +inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) +{ + + // Nguyen Binh : this code seem to be the fastest. + dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3]; + dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1]; + dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2]; + dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3]; + + dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1; + dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2; + dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0; + + /* + // nVidia SDK implementation + dVector3 uv, uuv; + dVector3 qvec; + qvec[0] = quat[1]; + qvec[1] = quat[2]; + qvec[2] = quat[3]; + + dVector3Cross(qvec,source,uv); + dVector3Cross(qvec,uv,uuv); + + dVector3Scale(uv,REAL(2.0)*quat[0]); + dVector3Scale(uuv,REAL(2.0)); + + dest[0] = source[0] + uv[0] + uuv[0]; + dest[1] = source[1] + uv[1] + uuv[1]; + dest[2] = source[2] + uv[2] + uuv[2]; + */ +} + +inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) +{ + + dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; + + if (norm > REAL(0.0)) + { + dQuaternion invQuat; + invQuat[0] = quat[0] / norm; + invQuat[1] = -quat[1] / norm; + invQuat[2] = -quat[2] / norm; + invQuat[3] = -quat[3] / norm; + + dQuatTransform(invQuat,source,dest); + + } + else + { + // Singular -> return identity + dVector3Copy(source,dest); + } +} + +inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ) +{ + rY = asin(mRot[0 * 4 + 2]); + if (rY < M_PI /2) + { + if (rY > -M_PI /2) + { + rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]); + rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]); + } + else + { + // not unique + rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]); + rZ = REAL(0.0); + } + } + else + { + // not unique + rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]); + rZ = REAL(0.0); + } +} + +inline void dQuatInv(const dQuaternion& source, dQuaternion& dest) +{ + dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3]; + + if (norm > 0.0f) + { + dest[0] = source[0] / norm; + dest[1] = -source[1] / norm; + dest[2] = -source[2] / norm; + dest[3] = -source[3] / norm; + } + else + { + // Singular -> return identity + dest[0] = REAL(1.0); + dest[1] = REAL(0.0); + dest[2] = REAL(0.0); + dest[3] = REAL(0.0); + } +} + + +#endif diff --git a/libraries/ode-0.9/ode/src/convex.cpp b/libraries/ode-0.9/ode/src/convex.cpp new file mode 100644 index 0000000..db93beb --- /dev/null +++ b/libraries/ode-0.9/ode/src/convex.cpp @@ -0,0 +1,1294 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* +Code for Convex Collision Detection +By Rodrigo Hernandez +*/ +//#include +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +#if _MSC_VER <= 1200 +#define dMIN(A,B) ((A)>(B) ? B : A) +#define dMAX(A,B) ((A)>(B) ? A : B) +#else +#define dMIN(A,B) std::min(A,B) +#define dMAX(A,B) std::max(A,B) +#endif + +//**************************************************************************** +// Convex public API +dxConvex::dxConvex (dSpaceID space, + dReal *_planes, + unsigned int _planecount, + dReal *_points, + unsigned int _pointcount, + unsigned int *_polygons) : + dxGeom (space,1) +{ + dAASSERT (_planes != NULL); + dAASSERT (_points != NULL); + dAASSERT (_polygons != NULL); + //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes); + type = dConvexClass; + planes = _planes; + planecount = _planecount; + // we need points as well + points = _points; + pointcount = _pointcount; + polygons=_polygons; + FillEdges(); +} + + +void dxConvex::computeAABB() +{ + // this can, and should be optimized + dVector3 point; + dMULTIPLY0_331 (point,final_posr->R,points); + aabb[0] = point[0]+final_posr->pos[0]; + aabb[1] = point[0]+final_posr->pos[0]; + aabb[2] = point[1]+final_posr->pos[1]; + aabb[3] = point[1]+final_posr->pos[1]; + aabb[4] = point[2]+final_posr->pos[2]; + aabb[5] = point[2]+final_posr->pos[2]; + for(unsigned int i=3;i<(pointcount*3);i+=3) + { + dMULTIPLY0_331 (point,final_posr->R,&points[i]); + aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]); + aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]); + aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]); + aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]); + aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]); + aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]); + } +} + +/*! \brief Populates the edges set, should be called only once whenever + the polygon array gets updated */ +void dxConvex::FillEdges() +{ + unsigned int *points_in_poly=polygons; + unsigned int *index=polygons+1; + for(unsigned int i=0;i::iterator it=edges.begin(); + it!=edges.end(); + ++it) + { + fprintf(stdout,"Edge: %d-%d\n",it->first,it->second); + } + */ +} + +dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount, + dReal *_points, + unsigned int _pointcount, + unsigned int *_polygons) +{ + //fprintf(stdout,"dxConvex dCreateConvex\n"); + return new dxConvex(space,_planes, _planecount, + _points, + _pointcount, + _polygons); +} + +void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount, + dReal *_points, + unsigned int _pointcount, + unsigned int *_polygons) +{ + //fprintf(stdout,"dxConvex dGeomSetConvex\n"); + dUASSERT (g && g->type == dConvexClass,"argument not a convex shape"); + dxConvex *s = (dxConvex*) g; + s->planes = _planes; + s->planecount = _planecount; + s->points = _points; + s->pointcount = _pointcount; + s->polygons=_polygons; +} + +//**************************************************************************** +// Helper Inlines +// + +/*! \brief Returns Whether or not the segment ab intersects plane p + \param a origin of the segment + \param b segment destination + \param p plane to test for intersection + \param t returns the time "t" in the segment ray that gives us the intersecting + point + \param q returns the intersection point + \return true if there is an intersection, otherwise false. +*/ +bool IntersectSegmentPlane(dVector3 a, + dVector3 b, + dVector4 p, + dReal &t, + dVector3 q) +{ + // Compute the t value for the directed line ab intersecting the plane + dVector3 ab; + ab[0]= b[0] - a[0]; + ab[1]= b[1] - a[1]; + ab[2]= b[2] - a[2]; + + t = (p[3] - dDOT(p,a)) / dDOT(p,ab); + + // If t in [0..1] compute and return intersection point + if (t >= 0.0 && t <= 1.0) + { + q[0] = a[0] + t * ab[0]; + q[1] = a[1] + t * ab[1]; + q[2] = a[2] + t * ab[2]; + return true; + } + // Else no intersection + return false; +} + +/*! \brief Returns the Closest Point in Ray 1 to Ray 2 + \param Origin1 The origin of Ray 1 + \param Direction1 The direction of Ray 1 + \param Origin1 The origin of Ray 2 + \param Direction1 The direction of Ray 3 + \param t the time "t" in Ray 1 that gives us the closest point + (closest_point=Origin1+(Direction*t). + \return true if there is a closest point, false if the rays are paralell. +*/ +inline bool ClosestPointInRay(const dVector3 Origin1, + const dVector3 Direction1, + const dVector3 Origin2, + const dVector3 Direction2, + dReal& t) +{ + dVector3 w = {Origin1[0]-Origin2[0], + Origin1[1]-Origin2[1], + Origin1[2]-Origin2[2]}; + dReal a = dDOT(Direction1 , Direction1); + dReal b = dDOT(Direction1 , Direction2); + dReal c = dDOT(Direction2 , Direction2); + dReal d = dDOT(Direction1 , w); + dReal e = dDOT(Direction2 , w); + dReal denominator = (a*c)-(b*b); + if(denominator==0.0f) + { + return false; + } + t = ((a*e)-(b*d))/denominator; + return true; +} + +/*! \brief Returns the Ray on which 2 planes intersect if they do. + \param p1 Plane 1 + \param p2 Plane 2 + \param p Contains the origin of the ray upon returning if planes intersect + \param d Contains the direction of the ray upon returning if planes intersect + \return true if the planes intersect, false if paralell. +*/ +inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d) +{ + // Compute direction of intersection line + //Cross(p1, p2,d); + dCROSS(d,=,p1,p2); + + // If d is (near) zero, the planes are parallel (and separated) + // or coincident, so they're not considered intersecting + dReal denom = dDOT(d, d); + if (denom < dEpsilon) return false; + + dVector3 n; + n[0]=p1[3]*p2[0] - p2[3]*p1[0]; + n[1]=p1[3]*p2[1] - p2[3]*p1[1]; + n[2]=p1[3]*p2[2] - p2[3]*p1[2]; + // Compute point on intersection line + //Cross(n, d,p); + dCROSS(p,=,n,d); + p[0]/=denom; + p[1]/=denom; + p[2]/=denom; + return true; +} + +/*! \brief Finds out if a point lies inside a 2D polygon + \param p Point to test + \param polygon a pointer to the start of the convex polygon index buffer + \param out the closest point in the polygon if the point is not inside + \return true if the point lies inside of the polygon, false if not. +*/ +inline bool IsPointInPolygon(dVector3 p, + unsigned int *polygon, + dxConvex *convex, + dVector3 out) +{ + // p is the point we want to check, + // polygon is a pointer to the polygon we + // are checking against, remember it goes + // number of vertices then that many indexes + // out returns the closest point on the border of the + // polygon if the point is not inside it. + size_t pointcount=polygon[0]; + dVector3 a; + dVector3 b; + dVector3 c; + dVector3 ab; + dVector3 ac; + dVector3 ap; + dVector3 bp; + dReal d1; + dReal d2; + dReal d3; + dReal d4; + dReal vc; + polygon++; // skip past pointcount + for(size_t i=0;ifinal_posr->R,&convex->points[(polygon[i]*3)]); + a[0]=convex->final_posr->pos[0]+a[0]; + a[1]=convex->final_posr->pos[1]+a[1]; + a[2]=convex->final_posr->pos[2]+a[2]; + + dMULTIPLY0_331 (b,convex->final_posr->R, + &convex->points[(polygon[(i+1)%pointcount]*3)]); + b[0]=convex->final_posr->pos[0]+b[0]; + b[1]=convex->final_posr->pos[1]+b[1]; + b[2]=convex->final_posr->pos[2]+b[2]; + + dMULTIPLY0_331 (c,convex->final_posr->R, + &convex->points[(polygon[(i+2)%pointcount]*3)]); + c[0]=convex->final_posr->pos[0]+c[0]; + c[1]=convex->final_posr->pos[1]+c[1]; + c[2]=convex->final_posr->pos[2]+c[2]; + + ab[0] = b[0] - a[0]; + ab[1] = b[1] - a[1]; + ab[2] = b[2] - a[2]; + ac[0] = c[0] - a[0]; + ac[1] = c[1] - a[1]; + ac[2] = c[2] - a[2]; + ap[0] = p[0] - a[0]; + ap[1] = p[1] - a[1]; + ap[2] = p[2] - a[2]; + d1 = dDOT(ab,ap); + d2 = dDOT(ac,ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + out[0]=a[0]; + out[1]=a[1]; + out[2]=a[2]; + return false; + } + bp[0] = p[0] - b[0]; + bp[1] = p[1] - b[1]; + bp[2] = p[2] - b[2]; + d3 = dDOT(ab,bp); + d4 = dDOT(ac,bp); + if (d3 >= 0.0f && d4 <= d3) + { + out[0]=b[0]; + out[1]=b[1]; + out[2]=b[2]; + return false; + } + vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + { + dReal v = d1 / (d1 - d3); + out[0] = a[0] + (ab[0]*v); + out[1] = a[1] + (ab[1]*v); + out[2] = a[2] + (ab[2]*v); + return false; + } + } + return true; +} + +int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxConvex *Convex = (dxConvex*) o1; + dxPlane *Plane = (dxPlane*) o2; + unsigned int contacts=0; + unsigned int maxc = flags & NUMC_MASK; + dVector3 v2; + +#define LTEQ_ZERO 0x10000000 +#define GTEQ_ZERO 0x20000000 +#define BOTH_SIGNS (LTEQ_ZERO | GTEQ_ZERO) + dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later + + unsigned int totalsign = 0; + for(unsigned int i=0;ipointcount;++i) + { + dMULTIPLY0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]); + dVector3Add(Convex->final_posr->pos, v2, v2); + + unsigned int distance2sign = GTEQ_ZERO; + dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D + if((distance2 <= REAL(0.0))) + { + distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS; + + if (contacts != maxc) + { + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dVector3Copy(Plane->p, target->normal); + dVector3Copy(v2, target->pos); + target->depth = -distance2; + target->g1 = Convex; + target->g2 = Plane; + contacts++; + } + } + + // Take new sign into account + totalsign |= distance2sign; + // Check if contacts are full and both signs have been already found + if ((contacts ^ maxc | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less + { + break; // Nothing can be changed any more + } + } + if (totalsign == BOTH_SIGNS) return contacts; + return 0; +#undef BOTH_SIGNS +#undef GTEQ_ZERO +#undef LTEQ_ZERO +} + +int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dConvexClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxSphere *Sphere = (dxSphere*) o1; + dxConvex *Convex = (dxConvex*) o2; + dReal dist,closestdist=dInfinity; + dVector4 plane; + // dVector3 contactpoint; + dVector3 offsetpos,out,temp; + unsigned int *pPoly=Convex->polygons; + int closestplane; + bool sphereinside=true; + /* + Do a good old sphere vs plane check first, + if a collision is found then check if the contact point + is within the polygon + */ + // offset the sphere final_posr->position into the convex space + offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0]; + offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1]; + offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2]; + //fprintf(stdout,"Begin Check\n"); + for(unsigned int i=0;iplanecount;++i) + { + // apply rotation to the plane + dMULTIPLY0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]); + plane[3]=(&Convex->planes[(i*4)])[3]; + // Get the distance from the sphere origin to the plane + dist = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D + if(dist>0) + { + // if we get here, we know the center of the sphere is + // outside of the convex hull. + if(distradius) + { + // if we get here we know the sphere surface penetrates + // the plane + if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,Convex,out)) + { + // finally if we get here we know that the + // sphere is directly touching the inside of the polyhedron + //fprintf(stdout,"Contact! distance=%f\n",dist); + contact->normal[0] = plane[0]; + contact->normal[1] = plane[1]; + contact->normal[2] = plane[2]; + contact->pos[0] = Sphere->final_posr->pos[0]+ + (-contact->normal[0]*Sphere->radius); + contact->pos[1] = Sphere->final_posr->pos[1]+ + (-contact->normal[1]*Sphere->radius); + contact->pos[2] = Sphere->final_posr->pos[2]+ + (-contact->normal[2]*Sphere->radius); + contact->depth = Sphere->radius-dist; + contact->g1 = Sphere; + contact->g2 = Convex; + return 1; + } + else + { + // the sphere may not be directly touching + // the polyhedron, but it may be touching + // a point or an edge, if the distance between + // the closest point on the poly (out) and the + // center of the sphere is less than the sphere + // radius we have a hit. + temp[0] = (Sphere->final_posr->pos[0]-out[0]); + temp[1] = (Sphere->final_posr->pos[1]-out[1]); + temp[2] = (Sphere->final_posr->pos[2]-out[2]); + dist=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]); + // avoid the sqrt unless really necesary + if(dist<(Sphere->radius*Sphere->radius)) + { + // We got an indirect hit + dist=dSqrt(dist); + contact->normal[0] = temp[0]/dist; + contact->normal[1] = temp[1]/dist; + contact->normal[2] = temp[2]/dist; + contact->pos[0] = Sphere->final_posr->pos[0]+ + (-contact->normal[0]*Sphere->radius); + contact->pos[1] = Sphere->final_posr->pos[1]+ + (-contact->normal[1]*Sphere->radius); + contact->pos[2] = Sphere->final_posr->pos[2]+ + (-contact->normal[2]*Sphere->radius); + contact->depth = Sphere->radius-dist; + contact->g1 = Sphere; + contact->g2 = Convex; + return 1; + } + } + } + sphereinside=false; + } + if(sphereinside) + { + if(closestdist>dFabs(dist)) + { + closestdist=dFabs(dist); + closestplane=i; + } + } + pPoly+=pPoly[0]+1; + } + if(sphereinside) + { + // if the center of the sphere is inside + // the Convex, we need to pop it out + dMULTIPLY0_331(contact->normal, + Convex->final_posr->R, + &Convex->planes[(closestplane*4)]); + contact->pos[0] = Sphere->final_posr->pos[0]; + contact->pos[1] = Sphere->final_posr->pos[1]; + contact->pos[2] = Sphere->final_posr->pos[2]; + contact->depth = closestdist+Sphere->radius; + contact->g1 = Sphere; + contact->g2 = Convex; + return 1; + } + return 0; +} + +int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxConvex *Convex = (dxConvex*) o1; + dxBox *Box = (dxBox*) o2; + + return 0; +} + +int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxConvex *Convex = (dxConvex*) o1; + dxCapsule *Capsule = (dxCapsule*) o2; + + return 0; +} + +/*! + \brief Retrieves the proper convex and plane index between 2 convex objects. + + Seidel's Algorithm does not discriminate between 2 different Convex Hulls, + it only cares about planes, so we feed it the planes of Convex 1 followed + by the planes of Convex 2 as a single collection of planes, + given an index into the single collection, + this function determines the correct convex object and index to retrieve + the current plane. + + \param c1 Convex 1 + \param c2 Convex 2 + \param i Plane Index to retrieve + \param index contains the translated index uppon return + \return a pointer to the convex object containing plane index "i" +*/ +inline dxConvex* GetPlaneIndex(dxConvex& c1, + dxConvex& c2, + unsigned int i,unsigned int& index) +{ + if(i>=c1.planecount) + { + index = i-c1.planecount; + return &c2; + } + index=i; + return &c1; +} + +inline void dumpplanes(dxConvex& cvx) +{ + // This is just a dummy debug function + dVector4 plane; + fprintf(stdout,"DUMP PLANES\n"); + for (unsigned int i=0;iR,&cvx.planes[(i*4)]); + // Translate + plane[3]= + (cvx.planes[(i*4)+3])+ + ((plane[0] * cvx.final_posr->pos[0]) + + (plane[1] * cvx.final_posr->pos[1]) + + (plane[2] * cvx.final_posr->pos[2])); + fprintf(stdout,"%f %f %f %f\n",plane[0],plane[1],plane[2],plane[3]); + } +} + +// this variable is for debuggin purpuses only, should go once everything works +static bool hit=false; + +/* + \brief Tests whether 2 convex objects collide + + Seidel's algorithm is a method to solve linear programs, + it finds the optimum vertex "v" of a set of functions, + in our case, the set of functions are the plane functions + for the 2 convex objects being tested. + We don't really care about the value optimum vertex though, + but the 2 convex objects only collide if this vertex exists, + otherwise, the set of functions is said to be "empty" or "void". + + Seidel's Original algorithm is recursive and not bound to any number + of dimensions, the one I present here is Iterative rather than recursive + and bound to 3 dimensions, which is what we care about. + + If you're interested (and you should be!) on the algorithm, the paper + by Raimund Seidel himself should explain a lot better than I did, you can + find it here: http://www.cs.berkeley.edu/~jrs/meshpapers/SeidelLP.pdf + + If posible, read the section about Linear Programming in + Christer Ericson's RealTime Collision Detection book. + + \note currently there seem to be some issues with this function since + it doesn't detect collisions except for the most simple tests :(. +*/ +bool SeidelLP(dxConvex& cvx1,dxConvex& cvx2) +{ + dVector3 c={1,0,0}; // The Objective vector can be anything + dVector3 solution; // We dont really need the solution so its local + dxConvex *cvx; + unsigned int index; + unsigned int planecount=cvx1.planecount+cvx2.planecount; + dReal sum,min,max,med; + dVector3 c1; // ,c2; + dVector4 aoveral,aoveram; // these will contain cached computations + unsigned int l,m,n; // l and m are the axes to the zerod dimensions, n is the axe for the last dimension + unsigned int i,j,k; + dVector4 eq1,eq2,eq3; // cached equations for 3d,2d and 1d respectivelly + // Get the support mapping for a HUGE bounding box in direction c + solution[0]= (c[0]>0) ? dInfinity : -dInfinity; + solution[1]= (c[1]>0) ? dInfinity : -dInfinity; + solution[2]= (c[2]>0) ? dInfinity : -dInfinity; + for( i=0;ifinal_posr->R,&cvx->planes[(index*4)]); + + // Translate + eq1[3]=(cvx->planes[(index*4)+3])+ + ((eq1[0] * cvx->final_posr->pos[0]) + + (eq1[1] * cvx->final_posr->pos[1]) + + (eq1[2] * cvx->final_posr->pos[2])); + // if(!hit) + // { + // fprintf(stdout,"Plane I %d: %f %f %f %f\n",i, + // cvx->planes[(index*4)+0], + // cvx->planes[(index*4)+1], + // cvx->planes[(index*4)+2], + // cvx->planes[(index*4)+3]); + // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",i, + // eq1[0], + // eq1[1],eq1[2],eq1[3]); + // fprintf(stdout,"POS %f,%f,%f\n", + // cvx->final_posr->pos[0], + // cvx->final_posr->pos[1], + // cvx->final_posr->pos[2]); + // } + // find if the solution is behind the current plane + sum= + ((eq1[0]*solution[0])+ + (eq1[1]*solution[1])+ + (eq1[2]*solution[2]))-eq1[3]; + // if not we need to discard the current solution + if(sum>0) + { + // go down a dimension by eliminating a variable + // first find l + l=0; + for( j=0;j<3;++j) + { + if(fabs(eq1[j])>fabs(eq1[l])) + { + l=j; + } + } + if(eq1[l]==0) + { + if(!hit) + { + fprintf(stdout,"Plane %d: %f %f %f %f is invalid\n",i, + eq1[0],eq1[1],eq1[2],eq1[3]); + } + return false; + } + // then compute a/a[l] c1 and solution + aoveral[0]=eq1[0]/eq1[l]; + aoveral[1]=eq1[1]/eq1[l]; + aoveral[2]=eq1[2]/eq1[l]; + aoveral[3]=eq1[3]/eq1[l]; + c1[0]=c[0]-((c[l]/eq1[l])*eq1[0]); + c1[1]=c[1]-((c[l]/eq1[l])*eq1[1]); + c1[2]=c[2]-((c[l]/eq1[l])*eq1[2]); + solution[0]=solution[0]-((solution[l]/eq1[l])*eq1[0]); + solution[1]=solution[1]-((solution[l]/eq1[l])*eq1[1]); + solution[2]=solution[2]-((solution[l]/eq1[l])*eq1[2]); + // iterate a to get the new equations with the help of a/a[l] + for( j=0;jfinal_posr->R,&cvx->planes[(index*4)]); + // Translate + eq2[3]=(cvx->planes[(index*4)+3])+ + ((eq2[0] * cvx->final_posr->pos[0]) + + (eq2[1] * cvx->final_posr->pos[1]) + + (eq2[2] * cvx->final_posr->pos[2])); + + // if(!hit) + // { + // fprintf(stdout,"Plane J %d: %f %f %f %f\n",j, + // cvx->planes[(index*4)+0], + // cvx->planes[(index*4)+1], + // cvx->planes[(index*4)+2], + // cvx->planes[(index*4)+3]); + // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",j, + // eq2[0], + // eq2[1], + // eq2[2], + // eq2[3]); + // fprintf(stdout,"POS %f,%f,%f\n", + // cvx->final_posr->pos[0], + // cvx->final_posr->pos[1], + // cvx->final_posr->pos[2]); + // } + + // Take The equation down a dimension + eq2[0]-=(cvx->planes[(index*4)+l]*aoveral[0]); + eq2[1]-=(cvx->planes[(index*4)+l]*aoveral[1]); + eq2[2]-=(cvx->planes[(index*4)+l]*aoveral[2]); + eq2[3]-=(cvx->planes[(index*4)+l]*aoveral[3]); + sum= + ((eq2[0]*solution[0])+ + (eq2[1]*solution[1])+ + (eq2[2]*solution[2]))-eq2[3]; + if(sum>0) + { + m=0; + for( k=0;k<3;++k) + { + if(fabs(eq2[k])>fabs(eq2[m])) + { + m=k; + } + } + if(eq2[m]==0) + { + /* + if(!hit) fprintf(stdout, + "Plane %d: %f %f %f %f is invalid\n",j, + eq2[0],eq2[1],eq2[2],eq2[3]); + */ + return false; + } + // then compute a/a[m] c1 and solution + aoveram[0]=eq2[0]/eq2[m]; + aoveram[1]=eq2[1]/eq2[m]; + aoveram[2]=eq2[2]/eq2[m]; + aoveram[3]=eq2[3]/eq2[m]; + c1[0]=c[0]-((c[m]/eq2[m])*eq2[0]); + c1[1]=c[1]-((c[m]/eq2[m])*eq2[1]); + c1[2]=c[2]-((c[m]/eq2[m])*eq2[2]); + solution[0]=solution[0]-((solution[m]/eq2[m])*eq2[0]); + solution[1]=solution[1]-((solution[m]/eq2[m])*eq2[1]); + solution[2]=solution[2]-((solution[m]/eq2[m])*eq2[2]); + // figure out the value for n by elimination + n = (l==0) ? ((m==1)? 2:1):((m==0)?((l==1)?2:1):0); + // iterate a to get the new equations with the help of a/a[l] + min=-dInfinity; + max=med=dInfinity; + for(k=0;kfinal_posr->R,&cvx->planes[(index*4)]); + // Translate + eq3[3]=(cvx->planes[(index*4)+3])+ + ((eq3[0] * cvx->final_posr->pos[0]) + + (eq3[1] * cvx->final_posr->pos[1]) + + (eq3[2] * cvx->final_posr->pos[2])); + // if(!hit) + // { + // fprintf(stdout,"Plane K %d: %f %f %f %f\n",k, + // cvx->planes[(index*4)+0], + // cvx->planes[(index*4)+1], + // cvx->planes[(index*4)+2], + // cvx->planes[(index*4)+3]); + // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",k, + // eq3[0], + // eq3[1], + // eq3[2], + // eq3[3]); + // fprintf(stdout,"POS %f,%f,%f\n", + // cvx->final_posr->pos[0], + // cvx->final_posr->pos[1], + // cvx->final_posr->pos[2]); + // } + + // Take the equation Down to 1D + eq3[0]-=(cvx->planes[(index*4)+m]*aoveram[0]); + eq3[1]-=(cvx->planes[(index*4)+m]*aoveram[1]); + eq3[2]-=(cvx->planes[(index*4)+m]*aoveram[2]); + eq3[3]-=(cvx->planes[(index*4)+m]*aoveram[3]); + if(eq3[n]>0) + { + max=dMIN(max,eq3[3]/eq3[n]); + } + else if(eq3[n]<0) + { + min=dMAX(min,eq3[3]/eq3[n]); + } + else + { + med=dMIN(med,eq3[3]); + } + } + } + if ((max=0) ? max:min; + // lift to 2D + solution[m] = (eq2[3]-(eq2[n]*solution[n]))/eq2[m]; + // lift to 3D + solution[l] = (eq1[3]-(eq1[m]*solution[m]+ + eq1[n]*solution[n]))/eq1[l]; + } + } + } + } + } + return true; +} + +/*! \brief A Support mapping function for convex shapes + \param dir direction to find the Support Point for + \param cvx convex object to find the support point for + \param out the support mapping in dir. + */ +inline void Support(dVector3 dir,dxConvex& cvx,dVector3 out) +{ + unsigned int index = 0; + dVector3 point; + dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points); + point[0]+=cvx.final_posr->pos[0]; + point[1]+=cvx.final_posr->pos[1]; + point[2]+=cvx.final_posr->pos[2]; + + dReal max = dDOT(point,dir); + dReal tmp; + for (unsigned int i = 1; i < cvx.pointcount; ++i) + { + dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+(i*3)); + point[0]+=cvx.final_posr->pos[0]; + point[1]+=cvx.final_posr->pos[1]; + point[2]+=cvx.final_posr->pos[2]; + tmp = dDOT(point, dir); + if (tmp > max) + { + out[0]=point[0]; + out[1]=point[1]; + out[2]=point[2]; + max = tmp; + } + } +} + +inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max) +{ + dVector3 point; + dReal value; + //fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]); + dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points); + //fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]); + point[0]+=cvx.final_posr->pos[0]; + point[1]+=cvx.final_posr->pos[1]; + point[2]+=cvx.final_posr->pos[2]; + max = min = dDOT(axis,cvx.points); + for (unsigned int i = 1; i < cvx.pointcount; ++i) + { + dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+(i*3)); + point[0]+=cvx.final_posr->pos[0]; + point[1]+=cvx.final_posr->pos[1]; + point[2]+=cvx.final_posr->pos[2]; + value=dDOT(axis,point); + if(valuemax) + { + max=value; + } + } + //fprintf(stdout,"Compute Interval Min Max %f,%f\n",min,max); +} + +/*! \brief Does an axis separation test between the 2 convex shapes +using faces and edges */ +int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags, + dContactGeom *contact, int skip) +{ + dVector4 plane,savedplane; + dReal min1,max1,min2,max2,min_depth=-dInfinity; + dVector3 e1,e2,t; + int maxc = flags & NUMC_MASK; // this is causing a segfault + dIASSERT(maxc != 0); + dxConvex *g1,*g2; + unsigned int *pPoly; + dVector3 v; + // Test faces of cvx1 for separation + pPoly=cvx1.polygons; + for(unsigned int i=0;iR,cvx1.planes+(i*4)); + dNormalize3(plane); + // Translate + plane[3]= + (cvx1.planes[(i*4)+3])+ + ((plane[0] * cvx1.final_posr->pos[0]) + + (plane[1] * cvx1.final_posr->pos[1]) + + (plane[2] * cvx1.final_posr->pos[2])); + ComputeInterval(cvx1,plane,min1,max1); + ComputeInterval(cvx2,plane,min2,max2); + //fprintf(stdout,"width %f\n",max1-min1); + if(max2min2)&&(max1R, + cvx2.planes+(i*4)); + dNormalize3(plane); + // Translate + plane[3]= + (cvx2.planes[(i*4)+3])+ + ((plane[0] * cvx2.final_posr->pos[0]) + + (plane[1] * cvx2.final_posr->pos[1]) + + (plane[2] * cvx2.final_posr->pos[2])); + ComputeInterval(cvx2,plane,min1,max1); + ComputeInterval(cvx1,plane,min2,max2); + //fprintf(stdout,"width %f\n",max1-min1); + if(max2min2)&&(max1::iterator i = cvx1.edges.begin(); + i!= cvx1.edges.end(); + ++i) + { + // we only need to apply rotation here + dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+(i->first*3)); + dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+(i->second*3)); + e1[0]-=t[0]; + e1[1]-=t[1]; + e1[2]-=t[2]; + for(std::set::iterator j = cvx2.edges.begin(); + j!= cvx2.edges.end(); + ++j) + { + // we only need to apply rotation here + dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+(j->first*3)); + dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+(j->second*3)); + e2[0]-=t[0]; + e2[1]-=t[1]; + e2[2]-=t[2]; + dCROSS(plane,=,e1,e2); + plane[3]=0; + ComputeInterval(cvx1,plane,min1,max1); + ComputeInterval(cvx2,plane,min2,max2); + if(max2pointcount;++i) + { + dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]); + dVector3Add(g1->final_posr->pos, v, v); + + dReal distance = dVector3Dot(savedplane, v) - savedplane[3]; // Ax + By + Cz - D + if(distance<0) + { + dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip); + dVector3Copy(savedplane, target->normal); + dVector3Copy(v, target->pos); + target->depth = -distance; + target->g1 = g1; + target->g2 = g2; +/* -- uncomment if you are debugging + if(cvxhit<2) + fprintf(stdout,"Contact: %f,%f,%f depth %f\n", + (double)target->pos[0], + (double)target->pos[1], + (double)target->pos[2], + (double)target->depth); +*/ + contacts++; + if (contacts==maxc) break; + } + } +/* -- uncomment if you are debugging + cvxhit++; +*/ + + return contacts; +} + +int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConvexClass); + dIASSERT (o2->type == dConvexClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + +// if(!hit) fprintf(stdout,"dCollideConvexConvex\n"); + dxConvex *Convex1 = (dxConvex*) o1; + dxConvex *Convex2 = (dxConvex*) o2; + int contacts; + if(contacts=TestConvexIntersection(*Convex1,*Convex2,flags, + contact,skip)) + { + //fprintf(stdout,"We have a Hit!\n"); + } + return contacts; +} + +#if 0 + +int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT( o1->type == dRayClass ); + dIASSERT( o2->type == dConvexClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay* ray = (dxRay*) o1; + dxConvex* convex = (dxConvex*) o2; + dVector3 origin,destination,contactpoint,out; + dReal depth; + dVector4 plane; + unsigned int *pPoly=convex->polygons; + // Calculate ray origin and destination + destination[0]=0; + destination[1]=0; + destination[2]= ray->length; + // -- Rotate -- + dMULTIPLY0_331(destination,ray->final_posr->R,destination); + origin[0]=ray->final_posr->pos[0]; + origin[1]=ray->final_posr->pos[1]; + origin[2]=ray->final_posr->pos[2]; + destination[0]+=origin[0]; + destination[1]+=origin[1]; + destination[2]+=origin[2]; + for(int i=0;iplanecount;++i) + { + // Rotate + dMULTIPLY0_331(plane,convex->final_posr->R,convex->planes+(i*4)); + // Translate + plane[3]= + (convex->planes[(i*4)+3])+ + ((plane[0] * convex->final_posr->pos[0]) + + (plane[1] * convex->final_posr->pos[1]) + + (plane[2] * convex->final_posr->pos[2])); + if(IntersectSegmentPlane(origin, + destination, + plane, + depth, + contactpoint)) + { + if(IsPointInPolygon(contactpoint,pPoly,convex,out)) + { + contact->pos[0]=contactpoint[0]; + contact->pos[1]=contactpoint[1]; + contact->pos[2]=contactpoint[2]; + contact->normal[0]=plane[0]; + contact->normal[1]=plane[1]; + contact->normal[2]=plane[2]; + contact->depth=depth; + contact->g1 = ray; + contact->g2 = convex; + return 1; + } + } + pPoly+=pPoly[0]+1; + } + return 0; +} + +#else + +// Ray - Convex collider by David Walters, June 2006 +int dCollideRayConvex( dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip ) +{ + dIASSERT( skip >= (int)sizeof(dContactGeom) ); + dIASSERT( o1->type == dRayClass ); + dIASSERT( o2->type == dConvexClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay* ray = (dxRay*) o1; + dxConvex* convex = (dxConvex*) o2; + + contact->g1 = ray; + contact->g2 = convex; + + dReal alpha, beta, nsign; + int flag; + + // + // Compute some useful info + // + + flag = 0; // Assume start point is behind all planes. + + for ( unsigned int i = 0; i < convex->planecount; ++i ) + { + // Alias this plane. + dReal* plane = convex->planes + ( i * 4 ); + + // If alpha >= 0 then start point is outside of plane. + alpha = dDOT( plane, ray->final_posr->pos ) - plane[3]; + + // If any alpha is positive, then + // the ray start is _outside_ of the hull + if ( alpha >= 0 ) + { + flag = 1; + break; + } + } + + // If the ray starts inside the convex hull, then everything is flipped. + nsign = ( flag ) ? REAL( 1.0 ) : REAL( -1.0 ); + + + // + // Find closest contact point + // + + // Assume no contacts. + contact->depth = dInfinity; + + for ( unsigned int i = 0; i < convex->planecount; ++i ) + { + // Alias this plane. + dReal* plane = convex->planes + ( i * 4 ); + + // If alpha >= 0 then point is outside of plane. + alpha = nsign * ( dDOT( plane, ray->final_posr->pos ) - plane[3] ); + + // Compute [ plane-normal DOT ray-normal ], (/flip) + beta = dDOT13( plane, ray->final_posr->R+2 ) * nsign; + + // Ray is pointing at the plane? ( beta < 0 ) + // Ray start to plane is within maximum ray length? + // Ray start to plane is closer than the current best distance? + if ( beta < -dEpsilon && + alpha >= 0 && alpha <= ray->length && + alpha < contact->depth ) + { + // Compute contact point on convex hull surface. + contact->pos[0] = ray->final_posr->pos[0] + alpha * ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha * ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha * ray->final_posr->R[2*4+2]; + + flag = 0; + + // For all _other_ planes. + for ( unsigned int j = 0; j < convex->planecount; ++j ) + { + if ( i == j ) + continue; // Skip self. + + // Alias this plane. + dReal* planej = convex->planes + ( j * 4 ); + + // If beta >= 0 then start is outside of plane. + beta = dDOT( planej, contact->pos ) - plane[3]; + + // If any beta is positive, then the contact point + // is not on the surface of the convex hull - it's just + // intersecting some part of its infinite extent. + if ( beta > dEpsilon ) + { + flag = 1; + break; + } + } + + // Contact point isn't outside hull's surface? then it's a good contact! + if ( flag == 0 ) + { + // Store the contact normal, possibly flipped. + contact->normal[0] = nsign * plane[0]; + contact->normal[1] = nsign * plane[1]; + contact->normal[2] = nsign * plane[2]; + + // Store depth + contact->depth = alpha; + + if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length ) + { + // Break on any contact if contacts are not important + break; + } + } + } + } + + // Contact? + return ( contact->depth <= ray->length ); +} + +#endif + +//<-- Convex Collision diff --git a/libraries/ode-0.9/ode/src/cylinder.cpp b/libraries/ode-0.9/ode/src/cylinder.cpp new file mode 100644 index 0000000..39a6cf3 --- /dev/null +++ b/libraries/ode-0.9/ode/src/cylinder.cpp @@ -0,0 +1,100 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +// flat cylinder public API + +dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) : +dxGeom (space,1) +{ + dAASSERT (_radius > 0 && _length > 0); + type = dCylinderClass; + radius = _radius; + lz = _length; +} + + +void dxCylinder::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = dFabs (R[0] * radius) + dFabs (R[1] * radius) + REAL(0.5)* dFabs (R[2] * + lz); + dReal yrange = dFabs (R[4] * radius) + dFabs (R[5] * radius) + REAL(0.5)* dFabs (R[6] * + lz); + dReal zrange = dFabs (R[8] * radius) + dFabs (R[9] * radius) + REAL(0.5)* dFabs (R[10] * + lz); + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length) +{ + return new dxCylinder (space,radius,length); +} + +void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length) +{ + dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); + dAASSERT (radius > 0 && length > 0); + dxCylinder *c = (dxCylinder*) cylinder; + c->radius = radius; + c->lz = length; + dGeomMoved (cylinder); +} + +void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length) +{ + dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); + dxCylinder *c = (dxCylinder*) cylinder; + *radius = c->radius; + *length = c->lz; +} + + diff --git a/libraries/ode-0.9/ode/src/error.cpp b/libraries/ode-0.9/ode/src/error.cpp new file mode 100644 index 0000000..9b33db5 --- /dev/null +++ b/libraries/ode-0.9/ode/src/error.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 + + +static dMessageFunction *error_function = 0; +static dMessageFunction *debug_function = 0; +static dMessageFunction *message_function = 0; + + +extern "C" void dSetErrorHandler (dMessageFunction *fn) +{ + error_function = fn; +} + + +extern "C" void dSetDebugHandler (dMessageFunction *fn) +{ + debug_function = fn; +} + + +extern "C" void dSetMessageHandler (dMessageFunction *fn) +{ + message_function = fn; +} + + +extern "C" dMessageFunction *dGetErrorHandler() +{ + return error_function; +} + + +extern "C" dMessageFunction *dGetDebugHandler() +{ + return debug_function; +} + + +extern "C" dMessageFunction *dGetMessageHandler() +{ + return message_function; +} + + +static void printMessage (int num, const char *msg1, const char *msg2, + va_list ap) +{ + fflush (stderr); + fflush (stdout); + if (num) fprintf (stderr,"\n%s %d: ",msg1,num); + else fprintf (stderr,"\n%s: ",msg1); + vfprintf (stderr,msg2,ap); + fprintf (stderr,"\n"); + fflush (stderr); +} + +//**************************************************************************** +// unix + +#ifndef WIN32 + +extern "C" void dError (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (error_function) error_function (num,msg,ap); + else printMessage (num,"ODE Error",msg,ap); + exit (1); +} + + +extern "C" void dDebug (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (debug_function) debug_function (num,msg,ap); + else printMessage (num,"ODE INTERNAL ERROR",msg,ap); + // *((char *)0) = 0; ... commit SEGVicide + abort(); +} + + +extern "C" void dMessage (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (message_function) message_function (num,msg,ap); + else printMessage (num,"ODE Message",msg,ap); +} + +#endif + +//**************************************************************************** +// windows + +#ifdef WIN32 + +// isn't cygwin annoying! +#ifdef CYGWIN +#define _snprintf snprintf +#define _vsnprintf vsnprintf +#endif + + +#include "windows.h" + + +extern "C" void dError (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (error_function) error_function (num,msg,ap); + else { + char s[1000],title[100]; + _snprintf (title,sizeof(title),"ODE Error %d",num); + _vsnprintf (s,sizeof(s),msg,ap); + s[sizeof(s)-1] = 0; + MessageBox(0,s,title,MB_OK | MB_ICONWARNING); + } + exit (1); +} + + +extern "C" void dDebug (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (debug_function) debug_function (num,msg,ap); + else { + char s[1000],title[100]; + _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num); + _vsnprintf (s,sizeof(s),msg,ap); + s[sizeof(s)-1] = 0; + MessageBox(0,s,title,MB_OK | MB_ICONSTOP); + } + abort(); +} + + +extern "C" void dMessage (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (message_function) message_function (num,msg,ap); + else printMessage (num,"ODE Message",msg,ap); +} + + +#endif diff --git a/libraries/ode-0.9/ode/src/export-dif.cpp b/libraries/ode-0.9/ode/src/export-dif.cpp new file mode 100644 index 0000000..577d3e1 --- /dev/null +++ b/libraries/ode-0.9/ode/src/export-dif.cpp @@ -0,0 +1,568 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + * Export a DIF (Dynamics Interchange Format) file. + */ + + +// @@@ TODO: +// * export all spaces, and geoms in spaces, not just ones attached to bodies +// (separate export function?) +// * say the space each geom is in, so reader can construct space heirarchy +// * limot --> separate out into limits and motors? +// * make sure ODE-specific parameters divided out + + +#include "ode/ode.h" +#include "objects.h" +#include "joint.h" +#include "collision_kernel.h" + +//*************************************************************************** +// utility + +struct PrintingContext { + FILE *file; // file to write to + int precision; // digits of precision to print + int indent; // number of levels of indent + + void printIndent(); + void printReal (dReal x); + void print (const char *name, int x); + void print (const char *name, dReal x); + void print (const char *name, const dReal *x, int n=3); + void print (const char *name, const char *x=0); + void printNonzero (const char *name, dReal x); + void printNonzero (const char *name, const dReal x[3]); +}; + + +void PrintingContext::printIndent() +{ + for (int i=0; i= 0) { + c.printIndent(); + fprintf (c.file,"limit%d = {\n",num); + } + else { + c.print ("limit = {"); + } + c.indent++; + c.print ("low_stop",limot.lostop); + c.print ("high_stop",limot.histop); + c.printNonzero ("bounce",limot.bounce); + c.print ("ODE = {"); + c.indent++; + c.printNonzero ("stop_erp",limot.stop_erp); + c.printNonzero ("stop_cfm",limot.stop_cfm); + c.indent--; + c.print ("},"); + c.indent--; + c.print ("},"); + + if (num >= 0) { + c.printIndent(); + fprintf (c.file,"motor%d = {\n",num); + } + else { + c.print ("motor = {"); + } + c.indent++; + c.printNonzero ("vel",limot.vel); + c.printNonzero ("fmax",limot.fmax); + c.print ("ODE = {"); + c.indent++; + c.printNonzero ("fudge_factor",limot.fudge_factor); + c.printNonzero ("normal_cfm",limot.normal_cfm); + c.indent--; + c.print ("},"); + c.indent--; + c.print ("},"); +} + + +static const char *getJointName (dxJoint *j) +{ + switch (j->vtable->typenum) { + case dJointTypeBall: return "ball"; + case dJointTypeHinge: return "hinge"; + case dJointTypeSlider: return "slider"; + case dJointTypeContact: return "contact"; + case dJointTypeUniversal: return "universal"; + case dJointTypeHinge2: return "ODE_hinge2"; + case dJointTypeFixed: return "fixed"; + case dJointTypeNull: return "null"; + case dJointTypeAMotor: return "ODE_angular_motor"; + case dJointTypeLMotor: return "ODE_linear_motor"; + case dJointTypePR: return "PR"; + } + return "unknown"; +} + + +static void printBall (PrintingContext &c, dxJoint *j) +{ + dxJointBall *b = (dxJointBall*) j; + c.print ("anchor1",b->anchor1); + c.print ("anchor2",b->anchor2); +} + + +static void printHinge (PrintingContext &c, dxJoint *j) +{ + dxJointHinge *h = (dxJointHinge*) j; + c.print ("anchor1",h->anchor1); + c.print ("anchor2",h->anchor2); + c.print ("axis1",h->axis1); + c.print ("axis2",h->axis2); + c.print ("qrel",h->qrel,4); + printLimot (c,h->limot,-1); +} + + +static void printSlider (PrintingContext &c, dxJoint *j) +{ + dxJointSlider *s = (dxJointSlider*) j; + c.print ("axis1",s->axis1); + c.print ("qrel",s->qrel,4); + c.print ("offset",s->offset); + printLimot (c,s->limot,-1); +} + + +static void printContact (PrintingContext &c, dxJoint *j) +{ + dxJointContact *ct = (dxJointContact*) j; + int mode = ct->contact.surface.mode; + c.print ("pos",ct->contact.geom.pos); + c.print ("normal",ct->contact.geom.normal); + c.print ("depth",ct->contact.geom.depth); + //@@@ may want to write the geoms g1 and g2 that are involved, for debugging. + // to do this we must have written out all geoms in all spaces, not just + // geoms that are attached to bodies. + c.print ("mu",ct->contact.surface.mu); + if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2); + if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce); + if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel); + if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp); + if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm); + if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1); + if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2); + if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1); + if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2); + int fa = 0; // friction approximation code + if (mode & dContactApprox1_1) fa |= 1; + if (mode & dContactApprox1_2) fa |= 2; + if (fa) c.print ("friction_approximation",fa); + if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1); +} + + +static void printUniversal (PrintingContext &c, dxJoint *j) +{ + dxJointUniversal *u = (dxJointUniversal*) j; + c.print ("anchor1",u->anchor1); + c.print ("anchor2",u->anchor2); + c.print ("axis1",u->axis1); + c.print ("axis2",u->axis2); + c.print ("qrel1",u->qrel1,4); + c.print ("qrel2",u->qrel2,4); + printLimot (c,u->limot1,1); + printLimot (c,u->limot2,2); +} + + +static void printHinge2 (PrintingContext &c, dxJoint *j) +{ + dxJointHinge2 *h = (dxJointHinge2*) j; + c.print ("anchor1",h->anchor1); + c.print ("anchor2",h->anchor2); + c.print ("axis1",h->axis1); + c.print ("axis2",h->axis2); + c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available + c.print ("v2",h->v2); + c.print ("susp_erp",h->susp_erp); + c.print ("susp_cfm",h->susp_cfm); + printLimot (c,h->limot1,1); + printLimot (c,h->limot2,2); +} + +static void printPR (PrintingContext &c, dxJoint *j) +{ + dxJointPR *pr = (dxJointPR*) j; + c.print ("anchor2",pr->anchor2); + c.print ("axisR1",pr->axisR1); + c.print ("axisR2",pr->axisR2); + c.print ("axisP1",pr->axisP1); + c.print ("qrel",pr->qrel,4); + c.print ("offset",pr->offset); + printLimot (c,pr->limotP,1); + printLimot (c,pr->limotR,2); +} + + +static void printFixed (PrintingContext &c, dxJoint *j) +{ + dxJointFixed *f = (dxJointFixed*) j; + c.print ("qrel",f->qrel); + c.print ("offset",f->offset); +} + +static void printLMotor (PrintingContext &c, dxJoint *j) +{ + dxJointLMotor *a = (dxJointLMotor*) j; + c.print("num", a->num); + c.printIndent(); + fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]); + c.print ("axis1",a->axis[0]); + c.print ("axis2",a->axis[1]); + c.print ("axis3",a->axis[2]); + for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1); +} + + +static void printAMotor (PrintingContext &c, dxJoint *j) +{ + dxJointAMotor *a = (dxJointAMotor*) j; + c.print ("num",a->num); + c.print ("mode",a->mode); + c.printIndent(); + fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]); + c.print ("axis1",a->axis[0]); + c.print ("axis2",a->axis[1]); + c.print ("axis3",a->axis[2]); + for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1); + c.print ("angle1",a->angle[0]); + c.print ("angle2",a->angle[1]); + c.print ("angle3",a->angle[2]); +} + +//*************************************************************************** +// geometry + +static void printGeom (PrintingContext &c, dxGeom *g); + +static void printSphere (PrintingContext &c, dxGeom *g) +{ + c.print ("type","sphere"); + c.print ("radius",dGeomSphereGetRadius (g)); +} + + +static void printBox (PrintingContext &c, dxGeom *g) +{ + dVector3 sides; + dGeomBoxGetLengths (g,sides); + c.print ("type","box"); + c.print ("sides",sides); +} + + + +static void printCapsule (PrintingContext &c, dxGeom *g) +{ + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + c.print ("type","capsule"); + c.print ("radius",radius); + c.print ("length",length); +} + + +static void printPlane (PrintingContext &c, dxGeom *g) +{ + dVector4 e; + dGeomPlaneGetParams (g,e); + c.print ("type","plane"); + c.print ("normal",e); + c.print ("d",e[3]); +} + + + +static void printRay (PrintingContext &c, dxGeom *g) +{ + dReal length = dGeomRayGetLength (g); + c.print ("type","ray"); + c.print ("length",length); +} + + + +static void printGeomTransform (PrintingContext &c, dxGeom *g) +{ + dxGeom *g2 = dGeomTransformGetGeom (g); + const dReal *pos = dGeomGetPosition (g2); + dQuaternion q; + dGeomGetQuaternion (g2,q); + c.print ("type","transform"); + c.print ("pos",pos); + c.print ("q",q,4); + c.print ("geometry = {"); + c.indent++; + printGeom (c,g2); + c.indent--; + c.print ("}"); +} + + + +static void printTriMesh (PrintingContext &c, dxGeom *g) +{ + c.print ("type","trimesh"); + //@@@ i don't think that the trimesh accessor functions are really + // sufficient to read out all the triangle data, and anyway we + // should have a method of not duplicating trimesh data that is + // shared. +} + + +static void printGeom (PrintingContext &c, dxGeom *g) +{ + unsigned long category = dGeomGetCategoryBits (g); + if (category != (unsigned long)(~0)) { + c.printIndent(); + fprintf (c.file,"category_bits = %lu\n",category); + } + unsigned long collide = dGeomGetCollideBits (g); + if (collide != (unsigned long)(~0)) { + c.printIndent(); + fprintf (c.file,"collide_bits = %lu\n",collide); + } + if (!dGeomIsEnabled (g)) { + c.print ("disabled",1); + } + switch (g->type) { + case dSphereClass: printSphere (c,g); break; + case dBoxClass: printBox (c,g); break; + case dCapsuleClass: printCapsule (c,g); break; + case dPlaneClass: printPlane (c,g); break; + case dRayClass: printRay (c,g); break; + case dGeomTransformClass: printGeomTransform (c,g); break; + case dTriMeshClass: printTriMesh (c,g); break; + } +} + +//*************************************************************************** +// world + +void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix) +{ + PrintingContext c; + c.file = file; +#if defined(dSINGLE) + c.precision = 7; +#else + c.precision = 15; +#endif + c.indent = 1; + + fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix); + c.print ("gravity",w->gravity); + c.print ("ODE = {"); + c.indent++; + c.print ("ERP",w->global_erp); + c.print ("CFM",w->global_cfm); + c.print ("auto_disable = {"); + c.indent++; + c.print ("linear_threshold",w->adis.linear_average_threshold); + c.print ("angular_threshold",w->adis.angular_average_threshold); + c.print ("average_samples",(int)w->adis.average_samples); + c.print ("idle_time",w->adis.idle_time); + c.print ("idle_steps",w->adis.idle_steps); + fprintf (file,"\t\t},\n\t},\n}\n"); + c.indent -= 3; + + // bodies + int num = 0; + fprintf (file,"%sbody = {}\n",prefix); + for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) { + b->tag = num; + fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix); + c.indent++; + c.print ("pos",b->posr.pos); + c.print ("q",b->q,4); + c.print ("lvel",b->lvel); + c.print ("avel",b->avel); + c.print ("mass",b->mass.mass); + fprintf (file,"\tI = {{"); + for (int i=0; i<3; i++) { + for (int j=0; j<3; j++) { + c.printReal (b->mass.I[i*4+j]); + if (j < 2) fputc (',',file); + } + if (i < 2) fprintf (file,"},{"); + } + fprintf (file,"}},\n"); + c.printNonzero ("com",b->mass.c); + c.print ("ODE = {"); + c.indent++; + if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1); + if (b->flags & dxBodyDisabled) c.print ("disabled",1); + if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1); + if (b->flags & dxBodyAutoDisable) { + c.print ("auto_disable = {"); + c.indent++; + c.print ("linear_threshold",b->adis.linear_average_threshold); + c.print ("angular_threshold",b->adis.angular_average_threshold); + c.print ("average_samples",(int)b->adis.average_samples); + c.print ("idle_time",b->adis.idle_time); + c.print ("idle_steps",b->adis.idle_steps); + c.print ("time_left",b->adis_timeleft); + c.print ("steps_left",b->adis_stepsleft); + c.indent--; + c.print ("},"); + } + c.printNonzero ("facc",b->facc); + c.printNonzero ("tacc",b->tacc); + if (b->flags & dxBodyFlagFiniteRotationAxis) { + c.print ("finite_rotation_axis",b->finite_rot_axis); + } + c.indent--; + c.print ("},"); + if (b->geom) { + c.print ("geometry = {"); + c.indent++; + for (dxGeom *g=b->geom; g; g=g->body_next) { + c.print ("{"); + c.indent++; + printGeom (c,g); + c.indent--; + c.print ("},"); + } + c.indent--; + c.print ("},"); + } + c.indent--; + c.print ("}"); + num++; + } + + // joints + num = 0; + fprintf (file,"%sjoint = {}\n",prefix); + for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) { + c.indent++; + const char *name = getJointName (j); + fprintf (file, + "%sjoint[%d] = dynamics.%s_joint {\n" + "\tworld = %sworld,\n" + "\tbody = {" + ,prefix,num,name,prefix); + + if ( j->node[0].body ) + fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag); + if ( j->node[1].body ) + fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag); + + switch (j->vtable->typenum) { + case dJointTypeBall: printBall (c,j); break; + case dJointTypeHinge: printHinge (c,j); break; + case dJointTypeSlider: printSlider (c,j); break; + case dJointTypeContact: printContact (c,j); break; + case dJointTypeUniversal: printUniversal (c,j); break; + case dJointTypeHinge2: printHinge2 (c,j); break; + case dJointTypeFixed: printFixed (c,j); break; + case dJointTypeAMotor: printAMotor (c,j); break; + case dJointTypeLMotor: printLMotor (c,j); break; + case dJointTypePR: printPR (c,j); break; + } + c.indent--; + c.print ("}"); + num++; + } +} diff --git a/libraries/ode-0.9/ode/src/fastdot.c b/libraries/ode-0.9/ode/src/fastdot.c new file mode 100644 index 0000000..148d2dd --- /dev/null +++ b/libraries/ode-0.9/ode/src/fastdot.c @@ -0,0 +1,30 @@ +/* generated code, do not edit. */ + +#include "ode/matrix.h" + + +dReal dDot (const dReal *a, const dReal *b, int n) +{ + dReal p0,q0,m0,p1,q1,m1,sum; + sum = 0; + n -= 2; + while (n >= 0) { + p0 = a[0]; q0 = b[0]; + m0 = p0 * q0; + p1 = a[1]; q1 = b[1]; + m1 = p1 * q1; + sum += m0; + sum += m1; + a += 2; + b += 2; + n -= 2; + } + n += 2; + while (n > 0) { + sum += (*a) * (*b); + a++; + b++; + n--; + } + return sum; +} diff --git a/libraries/ode-0.9/ode/src/fastldlt.c b/libraries/ode-0.9/ode/src/fastldlt.c new file mode 100644 index 0000000..df2ea6e --- /dev/null +++ b/libraries/ode-0.9/ode/src/fastldlt.c @@ -0,0 +1,381 @@ +/* generated code, do not edit. */ + +#include "ode/matrix.h" + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ + +static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,m11,Z21,m21,p1,q1,p2,*ex; + const dReal *ell; + int i,j; + /* compute all 2 x 1 blocks of X */ + for (i=0; i < n; i+=2) { + /* compute all 2 x 1 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-2; j >= 0; j -= 2) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + p2=ell[lskip1]; + m21 = p2 * q1; + Z11 += m11; + Z21 += m21; + /* compute outer product and add it to the Z matrix */ + p1=ell[1]; + q1=ex[1]; + m11 = p1 * q1; + p2=ell[1+lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z21 += m21; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + p2=ell[lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z21 += m21; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + /* end of outer loop */ + } +} + +/* solve L*X=B, with B containing 2 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*2 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ + +static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex; + const dReal *ell; + int i,j; + /* compute all 2 x 2 blocks of X */ + for (i=0; i < n; i+=2) { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z12=0; + Z21=0; + Z22=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-2; j >= 0; j -= 2) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + q2=ex[lskip1]; + m12 = p1 * q2; + p2=ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* compute outer product and add it to the Z matrix */ + p1=ell[1]; + q1=ex[1]; + m11 = p1 * q1; + q2=ex[1+lskip1]; + m12 = p1 * q2; + p2=ell[1+lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + q2=ex[lskip1]; + m12 = p1 * q2; + p2=ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + Z12 = ex[lskip1] - Z12; + ex[lskip1] = Z12; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + Z22 = ex[1+lskip1] - Z22 - p1*Z12; + ex[1+lskip1] = Z22; + /* end of outer loop */ + } +} + + +void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1) +{ + int i,j; + dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22; + if (n < 1) return; + + for (i=0; i<=n-2; i += 2) { + /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ + dSolveL1_2 (A,A+i*nskip1,i,nskip1); + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + Z21 = 0; + Z22 = 0; + ell = A+i*nskip1; + dee = d; + for (j=i-6; j >= 0; j -= 6) { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1*dd; + q2 = p2*dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[1]; + p2 = ell[1+nskip1]; + dd = dee[1]; + q1 = p1*dd; + q2 = p2*dd; + ell[1] = q1; + ell[1+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[2]; + p2 = ell[2+nskip1]; + dd = dee[2]; + q1 = p1*dd; + q2 = p2*dd; + ell[2] = q1; + ell[2+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[3]; + p2 = ell[3+nskip1]; + dd = dee[3]; + q1 = p1*dd; + q2 = p2*dd; + ell[3] = q1; + ell[3+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[4]; + p2 = ell[4+nskip1]; + dd = dee[4]; + q1 = p1*dd; + q2 = p2*dd; + ell[4] = q1; + ell[4+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[5]; + p2 = ell[5+nskip1]; + dd = dee[5]; + q1 = p1*dd; + q2 = p2*dd; + ell[5] = q1; + ell[5+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1*dd; + q2 = p2*dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell++; + dee++; + } + /* solve for diagonal 2 x 2 block at A(i,i) */ + Z11 = ell[0] - Z11; + Z21 = ell[nskip1] - Z21; + Z22 = ell[1+nskip1] - Z22; + dee = d + i; + /* factorize 2 x 2 block Z,dee */ + /* factorize row 1 */ + dee[0] = dRecip(Z11); + /* factorize row 2 */ + sum = 0; + q1 = Z21; + q2 = q1 * dee[0]; + Z21 = q2; + sum += q1*q2; + dee[1] = dRecip(Z22 - sum); + /* done factorizing 2 x 2 block */ + ell[nskip1] = Z21; + } + /* compute the (less than 2) rows at the bottom */ + switch (n-i) { + case 0: + break; + + case 1: + dSolveL1_1 (A,A+i*nskip1,i,nskip1); + /* scale the elements in a 1 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + ell = A+i*nskip1; + dee = d; + for (j=i-6; j >= 0; j -= 6) { + p1 = ell[0]; + dd = dee[0]; + q1 = p1*dd; + ell[0] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[1]; + dd = dee[1]; + q1 = p1*dd; + ell[1] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[2]; + dd = dee[2]; + q1 = p1*dd; + ell[2] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[3]; + dd = dee[3]; + q1 = p1*dd; + ell[3] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[4]; + dd = dee[4]; + q1 = p1*dd; + ell[4] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[5]; + dd = dee[5]; + q1 = p1*dd; + ell[5] = q1; + m11 = p1*q1; + Z11 += m11; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) { + p1 = ell[0]; + dd = dee[0]; + q1 = p1*dd; + ell[0] = q1; + m11 = p1*q1; + Z11 += m11; + ell++; + dee++; + } + /* solve for diagonal 1 x 1 block at A(i,i) */ + Z11 = ell[0] - Z11; + dee = d + i; + /* factorize 1 x 1 block Z,dee */ + /* factorize row 1 */ + dee[0] = dRecip(Z11); + /* done factorizing 1 x 1 block */ + break; + + default: *((char*)0)=0; /* this should never happen! */ + } +} diff --git a/libraries/ode-0.9/ode/src/fastlsolve.c b/libraries/ode-0.9/ode/src/fastlsolve.c new file mode 100644 index 0000000..0ae99d6 --- /dev/null +++ b/libraries/ode-0.9/ode/src/fastlsolve.c @@ -0,0 +1,298 @@ +/* generated code, do not edit. */ + +#include "ode/matrix.h" + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 4*4. + * if this is in the factorizer source file, n must be a multiple of 4. + */ + +void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex; + const dReal *ell; + int lskip2,lskip3,i,j; + /* compute lskip values */ + lskip2 = 2*lskip1; + lskip3 = 3*lskip1; + /* compute all 4 x 1 blocks of X */ + for (i=0; i <= n-4; i+=4) { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + Z31=0; + Z41=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-12; j >= 0; j -= 12) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[lskip1]; + p3=ell[lskip2]; + p4=ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[1]; + q1=ex[1]; + p2=ell[1+lskip1]; + p3=ell[1+lskip2]; + p4=ell[1+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[2]; + q1=ex[2]; + p2=ell[2+lskip1]; + p3=ell[2+lskip2]; + p4=ell[2+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[3]; + q1=ex[3]; + p2=ell[3+lskip1]; + p3=ell[3+lskip2]; + p4=ell[3+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[4]; + q1=ex[4]; + p2=ell[4+lskip1]; + p3=ell[4+lskip2]; + p4=ell[4+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[5]; + q1=ex[5]; + p2=ell[5+lskip1]; + p3=ell[5+lskip2]; + p4=ell[5+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[6]; + q1=ex[6]; + p2=ell[6+lskip1]; + p3=ell[6+lskip2]; + p4=ell[6+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[7]; + q1=ex[7]; + p2=ell[7+lskip1]; + p3=ell[7+lskip2]; + p4=ell[7+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[8]; + q1=ex[8]; + p2=ell[8+lskip1]; + p3=ell[8+lskip2]; + p4=ell[8+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[9]; + q1=ex[9]; + p2=ell[9+lskip1]; + p3=ell[9+lskip2]; + p4=ell[9+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[10]; + q1=ex[10]; + p2=ell[10+lskip1]; + p3=ell[10+lskip2]; + p4=ell[10+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[11]; + q1=ex[11]; + p2=ell[11+lskip1]; + p3=ell[11+lskip2]; + p4=ell[11+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[lskip1]; + p3=ell[lskip2]; + p4=ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + p1 = ell[lskip2]; + p2 = ell[1+lskip2]; + Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21; + ex[2] = Z31; + p1 = ell[lskip3]; + p2 = ell[1+lskip3]; + p3 = ell[2+lskip3]; + Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; + ex[3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-12; j >= 0; j -= 12) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[1]; + q1=ex[1]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[2]; + q1=ex[2]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[3]; + q1=ex[3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[4]; + q1=ex[4]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[5]; + q1=ex[5]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[6]; + q1=ex[6]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[7]; + q1=ex[7]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[8]; + q1=ex[8]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[9]; + q1=ex[9]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[10]; + q1=ex[10]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[11]; + q1=ex[11]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } +} diff --git a/libraries/ode-0.9/ode/src/fastltsolve.c b/libraries/ode-0.9/ode/src/fastltsolve.c new file mode 100644 index 0000000..eb950f6 --- /dev/null +++ b/libraries/ode-0.9/ode/src/fastltsolve.c @@ -0,0 +1,199 @@ +/* generated code, do not edit. */ + +#include "ode/matrix.h" + +/* solve L^T * x=b, with b containing 1 right hand side. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * b is an n*1 matrix that contains the right hand side. + * b is overwritten with x. + * this processes blocks of 4. + */ + +void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; + const dReal *ell; + int lskip2,lskip3,i,j; + /* special handling for L and B because we're solving L1 *transpose* */ + L = L + (n-1)*(lskip1+1); + B = B + n-1; + lskip1 = -lskip1; + /* compute lskip values */ + lskip2 = 2*lskip1; + lskip3 = 3*lskip1; + /* compute all 4 x 1 blocks of X */ + for (i=0; i <= n-4; i+=4) { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + Z31=0; + Z41=0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-4; j >= 0; j -= 4) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-1]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-2]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-3]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[-1]; + Z21 = ex[-1] - Z21 - p1*Z11; + ex[-1] = Z21; + p1 = ell[-2]; + p2 = ell[-2+lskip1]; + Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21; + ex[-2] = Z31; + p1 = ell[-3]; + p2 = ell[-3+lskip1]; + p3 = ell[-3+lskip2]; + Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; + ex[-3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11=0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-4; j >= 0; j -= 4) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-1]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-2]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } +} diff --git a/libraries/ode-0.9/ode/src/heightfield.cpp b/libraries/ode-0.9/ode/src/heightfield.cpp new file mode 100644 index 0000000..36755fc --- /dev/null +++ b/libraries/ode-0.9/ode/src/heightfield.cpp @@ -0,0 +1,1812 @@ +// dHeightfield Collider +// Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com +// Martijn Buijs 2006 http://home.planet.nl/~buijs512/ +// Based on Terrain & Cone contrib by: +// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com +// Some code inspired by Magic Software + + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" +#include "heightfield.h" + + + +#if dTRIMESH_ENABLED +#include "collision_trimesh_internal.h" +#endif // dTRIMESH_ENABLED + +#define TERRAINTOL REAL(0.0) + +#define dMIN(A,B) ((A)>(B) ? B : A) +#define dMAX(A,B) ((A)>(B) ? A : B) + + +// Three-way MIN and MAX +#define dMIN3(A,B,C) ( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) ) +#define dMAX3(A,B,C) ( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) ) + +#define dOPESIGN(a, op1, op2,b) \ + (a)[0] op1 op2 ((b)[0]); \ + (a)[1] op1 op2 ((b)[1]); \ + (a)[2] op1 op2 ((b)[2]); + +#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \ + \ + dVector3Copy (MyPoint, myRay.final_posr->pos); \ + myRay.final_posr->R[2] = MyVector[0]; \ + myRay.final_posr->R[6] = MyVector[1]; \ + myRay.final_posr->R[10] = MyVector[2]; \ + dGeomMoved (&myRay); \ + } + +#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \ + \ + MyPlane->p[0] = MyPlaneDef[0]; \ + MyPlane->p[1] = MyPlaneDef[1]; \ + MyPlane->p[2] = MyPlaneDef[2]; \ + MyPlane->p[3] = MyPlaneDef[3]; \ + dGeomMoved (MyPlane); \ + } +//////// Local Build Option //////////////////////////////////////////////////// + +// Uncomment this #define to use the (0,0) corner of the geom as the origin, +// rather than the center. This was the way the original heightfield worked, +// but as it does not match the way all other geometries work, so for constancy it +// was changed to work like this. + +// #define DHEIGHTFIELD_CORNER_ORIGIN + + +// Uncomment this #define to add heightfield triangles edge colliding +// Code is not guaranteed and I didn't find the need to add that as +// colliding planes triangles and edge triangles seems enough. +// #define _HEIGHTFIELDEDGECOLLIDING + + +//////// dxHeightfieldData ///////////////////////////////////////////////////////////// + +// dxHeightfieldData constructor +dxHeightfieldData::dxHeightfieldData() +{ + // +} + + +// build Heightfield data +void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples, + dReal fWidth, dReal fDepth, + dReal fScale, dReal fOffset, dReal fThickness, + int bWrapMode ) +{ + dIASSERT( fWidth > REAL( 0.0 ) ); + dIASSERT( fDepth > REAL( 0.0 ) ); + dIASSERT( nWidthSamples > 0 ); + dIASSERT( nDepthSamples > 0 ); + + // x,z bounds + m_fWidth = fWidth; + m_fDepth = fDepth; + + // cache half x,z bounds + m_fHalfWidth = fWidth / REAL( 2.0 ); + m_fHalfDepth = fDepth / REAL( 2.0 ); + + // scale and offset + m_fScale = fScale; + m_fOffset = fOffset; + + // infinite min height bounds + m_fThickness = fThickness; + + // number of vertices per side + m_nWidthSamples = nWidthSamples; + m_nDepthSamples = nDepthSamples; + + m_fSampleWidth = m_fWidth / ( m_nWidthSamples - 1 ); + m_fSampleDepth = m_fDepth / ( m_nDepthSamples - 1 ); + + m_fInvSampleWidth = 1 / m_fSampleWidth; + m_fInvSampleDepth = 1 / m_fSampleDepth; + + // finite or repeated terrain? + m_bWrapMode = bWrapMode; +} + + +// recomputes heights bounds +void dxHeightfieldData::ComputeHeightBounds() +{ + static int i; + static dReal h; + static unsigned char *data_byte; + static short *data_short; + static float *data_float; + static double *data_double; + + switch ( m_nGetHeightMode ) + { + + // callback + case 0: + // change nothing, keep using default or user specified bounds + return; + + // byte + case 1: + data_byte = (unsigned char*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + // short + case 2: + data_short = (short*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + // float + case 3: + data_float = (float*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + // double + case 4: + data_double = (double*)m_pHeightData; + m_fMinHeight = dInfinity; + m_fMaxHeight = -dInfinity; + + for (i=0; i( data_double[i] ); + if (h < m_fMinHeight) m_fMinHeight = h; + if (h > m_fMaxHeight) m_fMaxHeight = h; + } + + break; + + } + + // scale and offset + m_fMinHeight *= m_fScale; + m_fMaxHeight *= m_fScale; + m_fMinHeight += m_fOffset; + m_fMaxHeight += m_fOffset; + + // add thickness + m_fMinHeight -= m_fThickness; +} + + +// returns whether point is over terrain Cell triangle? +bool dxHeightfieldData::IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const +{ + { + const dReal MaxX = CellOrigin[0] + m_fSampleWidth; + const dReal TolX = m_fSampleWidth * TERRAINTOL; + if ((pos[0]MaxX+TolX)) + return false; + } + + { + const dReal MaxZ = CellOrigin[2] + m_fSampleDepth; + const dReal TolZ = m_fSampleDepth * TERRAINTOL; + if ((pos[2]MaxZ+TolZ)) + return false; + } + + // add X percentage position on cell and Z percentage position on cell + const dReal pctTotal = (pos[0] - CellOrigin[0]) * m_fInvSampleWidth + + (pos[2] - CellOrigin[2]) * m_fInvSampleDepth; + + if (isABC) + { + if (pctTotal >= REAL(1.0) + TERRAINTOL) + return false; + else + return true; + } + else if (pctTotal <= REAL(1.0) - TERRAINTOL) + { + return false; + } + return true; +} +// returns whether point is over terrain Cell triangle? +bool dxHeightfieldData::IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const +{ + dReal MaxX, MinX; + dReal MaxZ, MinZ; + if (isABC) + { + // point A + MinX = CellOrigin[0]; + MaxX = CellOrigin[0] + m_fSampleWidth; + + MinZ = CellOrigin[2]; + MaxZ = CellOrigin[2] + m_fSampleDepth; + } + else + { + // point D + MinX = CellOrigin[0] - m_fSampleWidth; + MaxX = CellOrigin[0]; + + MinZ = CellOrigin[2] - m_fSampleDepth; + MaxZ = CellOrigin[2]; + } + + // check if inside CELL + { + const dReal TolX = m_fSampleWidth * TERRAINTOL; + if ((pos[0]MaxX+TolX)) + return false; + } + + { + const dReal TolZ = m_fSampleDepth * TERRAINTOL; + if ((pos[2]MaxZ+TolZ)) + return false; + } + + // Sum up X percentage position on cell and Z percentage position on cell + const dReal pctTotal = (pos[0] - MinX) * m_fInvSampleWidth + + (pos[2] - MinZ) * m_fInvSampleDepth; + + // check if inside respective Triangle of Cell + if (isABC) + { + if (pctTotal >= REAL(1.0) + TERRAINTOL) + return false; + else + return true; + } + else if (pctTotal <= REAL(1.0) - TERRAINTOL) + { + return false; + } + return true; +} + + +// returns height at given sample coordinates +dReal dxHeightfieldData::GetHeight( int x, int z ) +{ + static dReal h; + static unsigned char *data_byte; + static short *data_short; + static float *data_float; + static double *data_double; + + if ( m_bWrapMode == 0 ) + { + // Finite + if ( x < 0 ) x = 0; + if ( z < 0 ) z = 0; + if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1; + if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1; + } + else + { + // Infinite + x %= m_nWidthSamples - 1; + z %= m_nDepthSamples - 1; + if ( x < 0 ) x += m_nWidthSamples - 1; + if ( z < 0 ) z += m_nDepthSamples - 1; + } + + switch ( m_nGetHeightMode ) + { + + // callback (dReal) + case 0: + h = (*m_pGetHeightCallback)(m_pUserData, x, z); + break; + + // byte + case 1: + data_byte = (unsigned char*)m_pHeightData; + h = data_byte[x+(z * m_nWidthSamples)]; + break; + + // short + case 2: + data_short = (short*)m_pHeightData; + h = data_short[x+(z * m_nWidthSamples)]; + break; + + // float + case 3: + data_float = (float*)m_pHeightData; + h = data_float[x+(z * m_nWidthSamples)]; + break; + + // double + case 4: + data_double = (double*)m_pHeightData; + h = static_cast< dReal >( data_double[x+(z * m_nWidthSamples)] ); + break; + } + + return (h * m_fScale) + m_fOffset; +} + + +// returns height at given coordinates +dReal dxHeightfieldData::GetHeight( dReal x, dReal z ) +{ + dReal dnX = dFloor( x * m_fInvSampleWidth ); + dReal dnZ = dFloor( z * m_fInvSampleDepth ); + + dReal dx = ( x - ( dnX * m_fSampleWidth ) ) * m_fInvSampleWidth; + dReal dz = ( z - ( dnZ * m_fSampleDepth ) ) * m_fInvSampleDepth; + + int nX = int( dnX ); + int nZ = int( dnZ ); + + //dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) ); + //dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) ); + + dReal y, y0; + + if ( dx + dz < REAL( 1.0 ) ) + { + y0 = GetHeight( nX, nZ ); + + y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * dx + + ( GetHeight( nX, nZ + 1 ) - y0 ) * dz; + } + else + { + y0 = GetHeight( nX + 1, nZ + 1 ); + + y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * ( REAL(1.0) - dz ) + + ( GetHeight( nX, nZ + 1 ) - y0 ) * ( REAL(1.0) - dx ); + } + + return y; +} + + +// dxHeightfieldData destructor +dxHeightfieldData::~dxHeightfieldData() +{ + static unsigned char *data_byte; + static short *data_short; + static float *data_float; + static double *data_double; + + dIASSERT( m_pHeightData ); + + if ( m_bCopyHeightData ) + { + switch ( m_nGetHeightMode ) + { + + // callback + case 0: + // do nothing + break; + + // byte + case 1: + data_byte = (unsigned char*)m_pHeightData; + delete [] data_byte; + break; + + // short + case 2: + data_short = (short*)m_pHeightData; + delete [] data_short; + break; + + // float + case 3: + data_float = (float*)m_pHeightData; + delete [] data_float; + break; + + // double + case 4: + data_double = (double*)m_pHeightData; + delete [] data_double; + break; + + } + } +} + + +//////// dxHeightfield ///////////////////////////////////////////////////////////////// + + +// dxHeightfield constructor +dxHeightfield::dxHeightfield( dSpaceID space, + dHeightfieldDataID data, + int bPlaceable ) : + dxGeom( space, bPlaceable ), + tempPlaneBuffer(0), + tempPlaneInstances(0), + tempPlaneBufferSize(0), + tempTriangleBuffer(0), + tempTriangleBufferSize(0), + tempHeightBuffer(0), + tempHeightInstances(0), + tempHeightBufferSizeX(0), + tempHeightBufferSizeZ(0) +{ + type = dHeightfieldClass; + this->m_p_data = data; +} + + +// compute axis aligned bounding box +void dxHeightfield::computeAABB() +{ + const dxHeightfieldData *d = m_p_data; + + if ( d->m_bWrapMode == 0 ) + { + // Finite + if ( gflags & GEOM_PLACEABLE ) + { + dReal dx[6], dy[6], dz[6]; + + // Y-axis + dy[0] = ( final_posr->R[ 1] * d->m_fMinHeight ); + dy[1] = ( final_posr->R[ 5] * d->m_fMinHeight ); + dy[2] = ( final_posr->R[ 9] * d->m_fMinHeight ); + dy[3] = ( final_posr->R[ 1] * d->m_fMaxHeight ); + dy[4] = ( final_posr->R[ 5] * d->m_fMaxHeight ); + dy[5] = ( final_posr->R[ 9] * d->m_fMaxHeight ); + +#ifdef DHEIGHTFIELD_CORNER_ORIGIN + + // X-axis + dx[0] = 0; dx[3] = ( final_posr->R[ 0] * d->m_fWidth ); + dx[1] = 0; dx[4] = ( final_posr->R[ 4] * d->m_fWidth ); + dx[2] = 0; dx[5] = ( final_posr->R[ 8] * d->m_fWidth ); + + // Z-axis + dz[0] = 0; dz[3] = ( final_posr->R[ 2] * d->m_fDepth ); + dz[1] = 0; dz[4] = ( final_posr->R[ 6] * d->m_fDepth ); + dz[2] = 0; dz[5] = ( final_posr->R[10] * d->m_fDepth ); + +#else // DHEIGHTFIELD_CORNER_ORIGIN + + // X-axis + dx[0] = ( final_posr->R[ 0] * -d->m_fHalfWidth ); + dx[1] = ( final_posr->R[ 4] * -d->m_fHalfWidth ); + dx[2] = ( final_posr->R[ 8] * -d->m_fHalfWidth ); + dx[3] = ( final_posr->R[ 0] * d->m_fHalfWidth ); + dx[4] = ( final_posr->R[ 4] * d->m_fHalfWidth ); + dx[5] = ( final_posr->R[ 8] * d->m_fHalfWidth ); + + // Z-axis + dz[0] = ( final_posr->R[ 2] * -d->m_fHalfDepth ); + dz[1] = ( final_posr->R[ 6] * -d->m_fHalfDepth ); + dz[2] = ( final_posr->R[10] * -d->m_fHalfDepth ); + dz[3] = ( final_posr->R[ 2] * d->m_fHalfDepth ); + dz[4] = ( final_posr->R[ 6] * d->m_fHalfDepth ); + dz[5] = ( final_posr->R[10] * d->m_fHalfDepth ); + +#endif // DHEIGHTFIELD_CORNER_ORIGIN + + // X extents + aabb[0] = final_posr->pos[0] + + dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) ); + aabb[1] = final_posr->pos[0] + + dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) ); + + // Y extents + aabb[2] = final_posr->pos[1] + + dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) ); + aabb[3] = final_posr->pos[1] + + dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) ); + + // Z extents + aabb[4] = final_posr->pos[2] + + dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) ); + aabb[5] = final_posr->pos[2] + + dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) ); + } + else + { + +#ifdef DHEIGHTFIELD_CORNER_ORIGIN + + aabb[0] = 0; aabb[1] = d->m_fWidth; + aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; + aabb[4] = 0; aabb[5] = d->m_fDepth; + +#else // DHEIGHTFIELD_CORNER_ORIGIN + + aabb[0] = -d->m_fHalfWidth; aabb[1] = +d->m_fHalfWidth; + aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; + aabb[4] = -d->m_fHalfDepth; aabb[5] = +d->m_fHalfDepth; + +#endif // DHEIGHTFIELD_CORNER_ORIGIN + + } + } + else + { + // Infinite + if ( gflags & GEOM_PLACEABLE ) + { + aabb[0] = -dInfinity; aabb[1] = +dInfinity; + aabb[2] = -dInfinity; aabb[3] = +dInfinity; + aabb[4] = -dInfinity; aabb[5] = +dInfinity; + } + else + { + aabb[0] = -dInfinity; aabb[1] = +dInfinity; + aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; + aabb[4] = -dInfinity; aabb[5] = +dInfinity; + } + } + +} + + +// dxHeightfield destructor +dxHeightfield::~dxHeightfield() +{ + resetTriangleBuffer(); + resetPlaneBuffer(); + resetHeightBuffer(); +} + +void dxHeightfield::allocateTriangleBuffer(size_t numTri) +{ + size_t alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT); + tempTriangleBufferSize = alignedNumTri; + tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri]; +} + +void dxHeightfield::resetTriangleBuffer() +{ + delete[] tempTriangleBuffer; +} + +void dxHeightfield::allocatePlaneBuffer(size_t numTri) +{ + size_t alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT); + tempPlaneBufferSize = alignedNumTri; + tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri]; + tempPlaneInstances = new HeightFieldPlane[alignedNumTri]; + + HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances; + for (size_t indexTri = 0; indexTri != alignedNumTri; indexTri++) + { + tempPlaneBuffer[indexTri] = ptrPlaneMatrix; + ptrPlaneMatrix += 1; + } +} + +void dxHeightfield::resetPlaneBuffer() +{ + delete[] tempPlaneInstances; + delete[] tempPlaneBuffer; +} + +void dxHeightfield::allocateHeightBuffer(size_t numX, size_t numZ) +{ + size_t alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X); + size_t alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z); + tempHeightBufferSizeX = alignedNumX; + tempHeightBufferSizeZ = alignedNumZ; + tempHeightBuffer = new HeightFieldVertex *[alignedNumX]; + size_t numCells = alignedNumX * alignedNumZ; + tempHeightInstances = new HeightFieldVertex [numCells]; + + HeightFieldVertex *ptrHeightMatrix = tempHeightInstances; + for (size_t indexX = 0; indexX != alignedNumX; indexX++) + { + tempHeightBuffer[indexX] = ptrHeightMatrix; + ptrHeightMatrix += alignedNumZ; + } +} + +void dxHeightfield::resetHeightBuffer() +{ + delete[] tempHeightInstances; + delete[] tempHeightBuffer; +} +//////// Heightfield data interface //////////////////////////////////////////////////// + + +dHeightfieldDataID dGeomHeightfieldDataCreate() +{ + return new dxHeightfieldData(); +} + + +void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d, + void* pUserData, dHeightfieldGetHeight* pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "argument not Heightfield data" ); + dIASSERT( pCallback ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // callback + d->m_nGetHeightMode = 0; + d->m_pUserData = pUserData; + d->m_pGetHeightCallback = pCallback; + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + + // default bounds + d->m_fMinHeight = -dInfinity; + d->m_fMaxHeight = dInfinity; +} + + +void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d, + const unsigned char *pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 1; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new unsigned char[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( unsigned char ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d, + const short* pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 2; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new short[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( short ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d, + const float *pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 3; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new float[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( float ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + +void dGeomHeightfieldDataBuildDouble( dHeightfieldDataID d, + const double *pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. + dIASSERT( depthSamples >= 2 ); + + // set info + d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); + d->m_nGetHeightMode = 4; + d->m_bCopyHeightData = bCopyHeightData; + + if ( d->m_bCopyHeightData == 0 ) + { + // Data is referenced only. + d->m_pHeightData = pHeightData; + } + else + { + // We own the height data, allocate storage + d->m_pHeightData = new double[ d->m_nWidthSamples * d->m_nDepthSamples ]; + dIASSERT( d->m_pHeightData ); + + // Copy data. + memcpy( (void*)d->m_pHeightData, pHeightData, + sizeof( double ) * d->m_nWidthSamples * d->m_nDepthSamples ); + } + + // Find height bounds + d->ComputeHeightBounds(); +} + + + + +void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight ) +{ + dUASSERT(d, "Argument not Heightfield data"); + d->m_fMinHeight = ( minHeight * d->m_fScale ) + d->m_fOffset - d->m_fThickness; + d->m_fMaxHeight = ( maxHeight * d->m_fScale ) + d->m_fOffset; +} + + +void dGeomHeightfieldDataDestroy( dHeightfieldDataID d ) +{ + dUASSERT(d, "argument not Heightfield data"); + delete d; +} + + +//////// Heightfield geom interface //////////////////////////////////////////////////// + + +dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable ) +{ + return new dxHeightfield( space, data, bPlaceable ); +} + + +void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d ) +{ + dxHeightfield* geom = (dxHeightfield*) g; + geom->data = d; +} + + +dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g ) +{ + dxHeightfield* geom = (dxHeightfield*) g; + return geom->m_p_data; +} + +//////// dxHeightfield ///////////////////////////////////////////////////////////////// + + +// Typedef for generic 'get point depth' function +typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z ); + + +#define DMESS(A) \ + dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ + x,z,A, \ + pContact->depth, \ + dGeomSphereGetRadius(o2), \ + pContact->pos[0], \ + pContact->pos[1], \ + pContact->pos[2], \ + pContact->normal[0], \ + pContact->normal[1], \ + pContact->normal[2]); + +static inline bool DescendingTriangleSort(const HeightFieldTriangle * const A, const HeightFieldTriangle * const B) +{ + return ((A->maxAAAB - B->maxAAAB) > dEpsilon); +} +static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B) +{ + return ((A->maxAAAB - B->maxAAAB) > dEpsilon); +} + +void dxHeightfield::sortPlanes(const size_t numPlanes) +{ + bool has_swapped = true; + do + { + has_swapped = false;//reset flag + for (size_t i = 0; i < numPlanes - 1; i++) + { + //if they are in the wrong order + if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1])) + { + //exchange them + HeightFieldPlane * tempPlane = tempPlaneBuffer[i]; + tempPlaneBuffer[i] = tempPlaneBuffer[i + 1]; + tempPlaneBuffer[i + 1] = tempPlane; + + //we have swapped at least once, list may not be sorted yet + has_swapped = true; + } + } + } //if no swaps were made during this pass, the list has been sorted + while (has_swapped); +} + +static inline dReal DistancePointToLine(const dVector3 &_point, + const dVector3 &_pt0, + const dVector3 &_Edge, + const dReal _Edgelength) +{ + dVector3 v; + dVector3Subtract(_point, _pt0, v); + dVector3 s; + dVector3Copy (_Edge, s); + const dReal dot = dVector3Dot(v, _Edge) / _Edgelength; + dVector3Scale(s, dot); + dVector3Subtract(v, s, v); + return dVector3Length(v); +} + + + + +int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ, + dxGeom* o2, const int numMaxContactsPossible, + int flags, dContactGeom* contact, + int skip ) +{ + dContactGeom *pContact = 0; + int x, z; + // check if not above or inside terrain first + // while filling a heightmap partial temporary buffer + const unsigned int numX = (maxX - minX) + 1; + const unsigned int numZ = (maxZ - minZ) + 1; + const dReal minO2Height = o2->aabb[2]; + const dReal maxO2Height = o2->aabb[3]; + unsigned int x_local, z_local; + dReal maxY = - dInfinity; + dReal minY = dInfinity; + // localize and const for faster access + const dReal cfSampleWidth = m_p_data->m_fSampleWidth; + const dReal cfSampleDepth = m_p_data->m_fSampleDepth; + { + if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ) + { + resetHeightBuffer(); + allocateHeightBuffer(numX, numZ); + } + + dReal Xpos, Ypos; + Xpos = minX * cfSampleWidth; + + + for ( x = minX, x_local = 0; x_local < numX; x++, x_local++) + { + const dReal c_Xpos = Xpos; + HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; + Ypos = minZ * cfSampleDepth; + for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++) + { + const dReal h = m_p_data->GetHeight(x, z); + HeightFieldRow[z_local].vertex[0] = c_Xpos; + HeightFieldRow[z_local].vertex[1] = h; + HeightFieldRow[z_local].vertex[2] = Ypos; + + + maxY = dMAX(maxY, h); + minY = dMIN(minY, h); + + + Ypos += cfSampleDepth; + } + Xpos += cfSampleWidth; + } + if (minO2Height - maxY > -dEpsilon ) + { + //totally above heightfield + return 0; + } + if (minY - maxO2Height > -dEpsilon ) + { + // totally under heightfield + pContact = CONTACT(contact, 0); + + pContact->pos[0] = o2->final_posr->pos[0]; + pContact->pos[1] = minY; + pContact->pos[2] = o2->final_posr->pos[2]; + + pContact->normal[0] = 0; + pContact->normal[1] = - 1; + pContact->normal[2] = 0; + + pContact->depth = minY - maxO2Height; + + return 1; + } + } + // get All Planes that could collide against. + dColliderFn *geomRayNCollider; + dColliderFn *geomNPlaneCollider; + dGetDepthFn *geomNDepthGetter; + + // int max_collisionContact = numMaxContactsPossible; -- not used + switch (o2->type) + { + case dRayClass: + geomRayNCollider = NULL; + geomNPlaneCollider = dCollideRayPlane; + geomNDepthGetter = NULL; + //max_collisionContact = 1; + break; + + case dSphereClass: + geomRayNCollider = dCollideRaySphere; + geomNPlaneCollider = dCollideSpherePlane; + geomNDepthGetter = dGeomSpherePointDepth; + //max_collisionContact = 3; + break; + + case dBoxClass: + geomRayNCollider = dCollideRayBox; + geomNPlaneCollider = dCollideBoxPlane; + geomNDepthGetter = dGeomBoxPointDepth; + //max_collisionContact = 8; + break; + + case dCapsuleClass: + geomRayNCollider = dCollideRayCapsule; + geomNPlaneCollider = dCollideCapsulePlane; + geomNDepthGetter = dGeomCapsulePointDepth; + // max_collisionContact = 3; + break; + + case dCylinderClass: + geomRayNCollider = dCollideRayCylinder; + geomNPlaneCollider = dCollideCylinderPlane; + geomNDepthGetter = NULL;// TODO: dGeomCCylinderPointDepth + //max_collisionContact = 3; + break; + + case dConvexClass: + geomRayNCollider = dCollideRayConvex; + geomNPlaneCollider = dCollideConvexPlane; + geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth; + //max_collisionContact = 3; + break; + +#if dTRIMESH_ENABLED + + case dTriMeshClass: + geomRayNCollider = dCollideRayTrimesh; + geomNPlaneCollider = dCollideTrimeshPlane; + geomNDepthGetter = NULL;// TODO: dGeomTrimeshPointDepth; + //max_collisionContact = 3; + break; + +#endif // dTRIMESH_ENABLED + + default: + dIASSERT(0); // Shouldn't ever get here. + break; + + } + + dxPlane myplane(0,0,0,0,0); + dxPlane* sliding_plane = &myplane; + dReal triplane[4]; + int i; + + // check some trivial case. + // Vector Up plane + if (maxY - minY < dEpsilon) + { + // it's a single plane. + triplane[0] = 0; + triplane[1] = 1; + triplane[2] = 0; + triplane[3] = minY; + dGeomPlaneSetNoNormalize (sliding_plane, triplane); + // find collision and compute contact points + const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip); + dIASSERT(numTerrainContacts <= numMaxContactsPossible); + for (i = 0; i < numTerrainContacts; i++) + { + pContact = CONTACT(contact, i*skip); + dOPESIGN(pContact->normal, =, -, triplane); + } + return numTerrainContacts; + } + // unique plane + { + // check for very simple plane heightfield + dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity; + dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity; + + + dReal lastXHeight = tempHeightBuffer[0][0].vertex[1]; + for ( x_local = 1; x_local < numX; x_local++) + { + HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; + + const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight; + + maxXHeightDelta = dMAX (maxXHeightDelta, deltaX); + minXHeightDelta = dMIN (minXHeightDelta, deltaX); + + dReal lastZHeight = HeightFieldRow[0].vertex[1]; + for ( z_local = 1; z_local < numZ; z_local++) + { + const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight); + + maxZHeightDelta = dMAX (maxZHeightDelta, deltaZ); + minZHeightDelta = dMIN (minZHeightDelta, deltaZ); + + } + } + + if (maxZHeightDelta - minZHeightDelta < dEpsilon && + maxXHeightDelta - minXHeightDelta < dEpsilon ) + { + // it's a single plane. + const dVector3 &A = tempHeightBuffer[0][0].vertex; + const dVector3 &B = tempHeightBuffer[1][0].vertex; + const dVector3 &C = tempHeightBuffer[0][1].vertex; + + // define 2 edges and a point that will define collision plane + { + dVector3 Edge1, Edge2; + dVector3Subtract(C, A, Edge1); + dVector3Subtract(B, A, Edge2); + dVector3Cross(Edge1, Edge2, triplane); + } + + // Define Plane + // Normalize plane normal + const dReal dinvlength = REAL(1.0) / dVector3Length(triplane); + triplane[0] *= dinvlength; + triplane[1] *= dinvlength; + triplane[2] *= dinvlength; + // get distance to origin from plane + triplane[3] = dVector3Dot(triplane, A); + + dGeomPlaneSetNoNormalize (sliding_plane, triplane); + // find collision and compute contact points + const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip); + dIASSERT(numTerrainContacts <= numMaxContactsPossible); + for (i = 0; i < numTerrainContacts; i++) + { + pContact = CONTACT(contact, i*skip); + dOPESIGN(pContact->normal, =, -, triplane); + } + return numTerrainContacts; + } + } + + + int numTerrainContacts = 0; + dContactGeom *PlaneContact = m_p_data->m_contacts; + + const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2; + if (tempTriangleBufferSize < numTriMax) + { + resetTriangleBuffer(); + allocateTriangleBuffer(numTriMax); + } + + // Sorting triangle/plane resulting from heightfield zone + // Perhaps that would be necessary in case of too much limited + // maximum contact point... + // or in complex mesh case (trimesh and convex) + // need some test or insights on this before enabling this. + const bool isContactNumPointsLimited = + true; + // (numMaxContacts < 8) + // || o2->type == dConvexClass + // || o2->type == dTriMeshClass + // || (numMaxContacts < (int)numTriMax) + + + + // if small heightfield triangle related to O2 colliding + // or no Triangle colliding at all. + bool needFurtherPasses = (o2->type == dTriMeshClass); + //compute Ratio between Triangle size and O2 aabb size + // no FurtherPasses are needed in ray class + if (o2->type != dRayClass && needFurtherPasses == false) + { + const dReal xratio = (o2->aabb[1] - o2->aabb[0]) * m_p_data->m_fInvSampleWidth; + if (xratio > REAL(1.5)) + needFurtherPasses = true; + else + { + const dReal zratio = (o2->aabb[5] - o2->aabb[4]) * m_p_data->m_fInvSampleDepth; + if (zratio > REAL(1.5)) + needFurtherPasses = true; + } + + } + + unsigned int numTri = 0; + HeightFieldVertex *A, *B, *C, *D; + /* (y is up) + A--------B-...x + | /| + | / | + | / | + | / | + | / | + | / | + | / | + |/ | + C--------D + . + . + . + z + */ + // keep only triangle that does intersect geom + for ( x = minX, x_local = 0; x < maxX; x++, x_local++) + { + HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; + HeightFieldVertex *HeightFieldNextRow = tempHeightBuffer[x_local + 1]; + + // First A + C = &HeightFieldRow [0]; + // First B + D = &HeightFieldNextRow[0]; + for ( z = minZ, z_local = 0; z < maxZ; z++, z_local++) + { + A = C; + B = D; + + C = &HeightFieldRow [z_local + 1]; + D = &HeightFieldNextRow[z_local + 1]; + + const dReal AHeight = A->vertex[1]; + const dReal BHeight = B->vertex[1]; + const dReal CHeight = C->vertex[1]; + const dReal DHeight = D->vertex[1]; + + const bool isACollide = 0 < AHeight - minO2Height; + const bool isBCollide = 0 < BHeight - minO2Height; + const bool isCCollide = 0 < CHeight - minO2Height; + const bool isDCollide = 0 < DHeight - minO2Height; + + A->state = !(isACollide); + B->state = !(isBCollide); + C->state = !(isCCollide); + D->state = !(isCCollide); + + if (isACollide || isBCollide || isCCollide) + { + HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++]; + + CurrTriUp->state = false; + + // changing point order here implies to change it in isOnHeightField + CurrTriUp->vertices[0] = A; + CurrTriUp->vertices[1] = B; + CurrTriUp->vertices[2] = C; + + if (isContactNumPointsLimited) + CurrTriUp->setMinMax(); + CurrTriUp->isUp = true; + } + + if (isBCollide || isCCollide || isDCollide) + { + HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++]; + + CurrTriDown->state = false; + // changing point order here implies to change it in isOnHeightField + + CurrTriDown->vertices[0] = D; + CurrTriDown->vertices[1] = B; + CurrTriDown->vertices[2] = C; + + + if (isContactNumPointsLimited) + CurrTriDown->setMinMax(); + CurrTriDown->isUp = false; + } + + + if (needFurtherPasses && + (isBCollide || isCCollide) + && + (AHeight - CHeight > 0 && + AHeight - BHeight > 0 && + DHeight - CHeight > 0 && + DHeight - BHeight > 0)) + { + // That means Edge BC is concave, therefore + // BC Edge and B and C vertices cannot collide + + B->state = true; + C->state = true; + } + // should find a way to check other edges (AB, BD, CD) too for concavity + } + } + + // at least on triangle should intersect geom + dIASSERT (numTri != 0); + // pass1: VS triangle as Planes + // Group Triangle by same plane definition + // as Terrain often has many triangles using same plane definition + // then collide against that list of triangles. + { + + dVector3 Edge1, Edge2; + //compute all triangles normals. + for (unsigned int k = 0; k < numTri; k++) + { + HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; + + // define 2 edges and a point that will define collision plane + dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1); + dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2); + + // find a perpendicular vector to the triangle + if (itTriangle->isUp) + dVector3Cross(Edge1, Edge2, triplane); + else + dVector3Cross(Edge2, Edge1, triplane); + + // Define Plane + // Normalize plane normal + const dReal dinvlength = REAL(1.0) / dVector3Length(triplane); + triplane[0] *= dinvlength; + triplane[1] *= dinvlength; + triplane[2] *= dinvlength; + // get distance to origin from plane + triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex); + + // saves normal for collision check (planes, triangles, vertices and edges.) + dVector3Copy(triplane, itTriangle->planeDef); + // saves distance for collision check (planes, triangles, vertices and edges.) + itTriangle->planeDef[3] = triplane[3]; + } + + // group by Triangles by Planes sharing shame plane definition + if (tempPlaneBufferSize < numTri) + { + resetPlaneBuffer(); + allocatePlaneBuffer(numTri); + } + unsigned int numPlanes = 0; + for (unsigned int k = 0; k < numTri; k++) + { + HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k]; + + if (tri_base->state == true) + continue;// already tested or added to plane list. + + HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes]; + currPlane->resetTriangleListSize(numTri - k); + currPlane->addTriangle(tri_base); + // saves normal for collision check (planes, triangles, vertices and edges.) + dVector3Copy(tri_base->planeDef, currPlane->planeDef); + // saves distance for collision check (planes, triangles, vertices and edges.) + currPlane->planeDef[3]= tri_base->planeDef[3]; + + const dReal normx = tri_base->planeDef[0]; + const dReal normy = tri_base->planeDef[1]; + const dReal normz = tri_base->planeDef[2]; + const dReal dist = tri_base->planeDef[3]; + + for (unsigned int m = k + 1; m < numTri; m++) + { + + HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m]; + if (tri_test->state == true) + continue;// already tested or added to plane list. + + // normals and distance are the same. + if ( + dFabs(normy - tri_test->planeDef[1]) < dEpsilon && + dFabs(dist - tri_test->planeDef[3]) < dEpsilon && + dFabs(normx - tri_test->planeDef[0]) < dEpsilon && + dFabs(normz - tri_test->planeDef[2]) < dEpsilon + ) + { + currPlane->addTriangle (tri_test); + tri_test->state = true; + } + } + + tri_base->state = true; + if (isContactNumPointsLimited) + currPlane->setMinMax(); + + numPlanes++; + } + + // sort planes + if (isContactNumPointsLimited) + sortPlanes(numPlanes); + +#if !defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) + /* + Note by Oleh_Derevenko: + It seems to be incorrect to limit contact count by some particular value + since some of them (and even all of them) may be culled in following condition. + However I do not see an easy way to fix this. + If not that culling the flags modification should be changed here and + additionally repeated after some contacts have been generated (in "if (didCollide)"). + The maximum of contacts in flags would then be set to minimum of contacts + remaining and HEIGHTFIELDMAXCONTACTPERCELL. + */ + int planeTestFlags = (flags & ~NUMC_MASK) | HEIGHTFIELDMAXCONTACTPERCELL; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); +#else // if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) + int numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + int planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); +#endif + + for (unsigned int k = 0; k < numPlanes; k++) + { + HeightFieldPlane * const itPlane = tempPlaneBuffer[k]; + + //set Geom + dGeomPlaneSetNoNormalize (sliding_plane, itPlane->planeDef); + //dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]); + // find collision and compute contact points + bool didCollide = false; + const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, planeTestFlags, PlaneContact, sizeof(dContactGeom)); + const size_t planeTriListSize = itPlane->trianglelistCurrentSize; + for (i = 0; i < numPlaneContacts; i++) + { + // Check if contact point found in plane is inside Triangle. + const dVector3 &pCPos = PlaneContact[i].pos; + for (size_t b = 0; planeTriListSize > b; b++) + { + if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0]->vertex, + pCPos, + itPlane->trianglelist[b]->isUp)) + { + pContact = CONTACT(contact, numTerrainContacts*skip); + dVector3Copy(pCPos, pContact->pos); + dOPESIGN(pContact->normal, =, -, itPlane->planeDef); + pContact->depth = PlaneContact[i].depth; + numTerrainContacts++; + if ( numTerrainContacts == numMaxContactsPossible ) + return numTerrainContacts; + + didCollide = true; + break; + } + } + } + if (didCollide) + { +#if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) + /* Note by Oleh_Derevenko: + This code is not used - see another note above + */ + numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); +#endif + for (size_t b = 0; planeTriListSize > b; b++) + { + // flag Triangles Vertices as collided + // to prevent any collision test of those + for (i = 0; i < 3; i++) + itPlane->trianglelist[b]->vertices[i]->state = true; + } + } + else + { + // flag triangle as not collided so that Vertices or Edge + // of that triangles will be checked. + for (size_t b = 0; planeTriListSize > b; b++) + { + itPlane->trianglelist[b]->state = false; + } + } + } + } + + + + // pass2: VS triangle vertices + if (needFurtherPasses) + { + dxRay tempRay(0, 1); + dReal depth; + bool vertexCollided; + + // Only one contact is necessary for ray test + int rayTestFlags = (flags & ~NUMC_MASK) | 1; + dIASSERT((1 & ~NUMC_MASK) == 0); + // + // Find Contact Penetration Depth of each vertices + // + for (unsigned int k = 0; k < numTri; k++) + { + const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; + if (itTriangle->state == true) + continue;// plane triangle did already collide. + + for (size_t i = 0; i < 3; i++) + { + HeightFieldVertex *vertex = itTriangle->vertices[i]; + if (vertex->state == true) + continue;// vertice did already collide. + + vertexCollided = false; + const dVector3 &triVertex = vertex->vertex; + if ( geomNDepthGetter ) + { + depth = geomNDepthGetter( o2, + triVertex[0], triVertex[1], triVertex[2] ); + if (depth + dEpsilon < 0) + vertexCollided = true; + } + else + { + // We don't have a GetDepth function, so do a ray cast instead. + // NOTE: This isn't ideal, and a GetDepth function should be + // written for all geom classes. + tempRay.length = (minO2Height - triVertex[1]) * REAL(1000.0); + + //dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2], + // - itTriangle->Normal[0], - itTriangle->Normal[1], - itTriangle->Normal[2] ); + dGeomRaySetNoNormalize(tempRay, triVertex, itTriangle->planeDef); + + if ( geomRayNCollider( &tempRay, o2, rayTestFlags, PlaneContact, sizeof( dContactGeom ) ) ) + { + depth = PlaneContact[0].depth; + vertexCollided = true; + } + } + if (vertexCollided) + { + pContact = CONTACT(contact, numTerrainContacts*skip); + //create contact using vertices + dVector3Copy (triVertex, pContact->pos); + //create contact using Plane Normal + dOPESIGN(pContact->normal, =, -, itTriangle->planeDef); + + pContact->depth = depth; + + numTerrainContacts++; + if ( numTerrainContacts == numMaxContactsPossible ) + return numTerrainContacts; + + vertex->state = true; + } + } + } + } + +#ifdef _HEIGHTFIELDEDGECOLLIDING + // pass3: VS triangle Edges + if (needFurtherPasses) + { + dVector3 Edge; + dxRay edgeRay(0, 1); + + int numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + int triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); + + for (unsigned int k = 0; k < numTri; k++) + { + const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; + + if (itTriangle->state == true) + continue;// plane did already collide. + + for (size_t m = 0; m < 3; m++) + { + const size_t next = (m + 1) % 3; + HeightFieldVertex *vertex0 = itTriangle->vertices[m]; + HeightFieldVertex *vertex1 = itTriangle->vertices[next]; + + // not concave or under the AABB + // nor triangle already collided against vertices + if (vertex0->state == true && vertex1->state == true) + continue;// plane did already collide. + + dVector3Subtract(vertex1->vertex, vertex0->vertex, Edge); + edgeRay.length = dVector3Length (Edge); + dGeomRaySetNoNormalize(edgeRay, vertex1->vertex, Edge); + int prevTerrainContacts = numTerrainContacts; + pContact = CONTACT(contact, prevTerrainContacts*skip); + const int numCollision = geomRayNCollider(&edgeRay,o2,triTestFlags,pContact,skip); + dIASSERT(numCollision <= numMaxContactsPerTri); + + if (numCollision) + { + numTerrainContacts += numCollision; + + do + { + pContact = CONTACT(contact, prevTerrainContacts*skip); + + //create contact using Plane Normal + dOPESIGN(pContact->normal, =, -, itTriangle->planeDef); + + pContact->depth = DistancePointToLine(pContact->pos, vertex1->vertex, Edge, edgeRay.length); + } + while (++prevTerrainContacts != numTerrainContacts); + + if ( numTerrainContacts == numMaxContactsPossible ) + return numTerrainContacts; + + numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); + triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri; + dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); + } + } + + itTriangle->vertices[0]->state = true; + itTriangle->vertices[1]->state = true; + itTriangle->vertices[2]->state = true; + } + } +#endif // _HEIGHTFIELDEDGECOLLIDING + return numTerrainContacts; +} + +int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip ) +{ + dIASSERT( skip >= (int)sizeof(dContactGeom) ); + dIASSERT( o1->type == dHeightfieldClass ); + dIASSERT((flags & NUMC_MASK) >= 1); + + int i; + + // if ((flags & NUMC_MASK) == 0) -- An assertion check is made on entry + // { flags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); } + + int numMaxTerrainContacts = (flags & NUMC_MASK); + + dxHeightfield *terrain = (dxHeightfield*) o1; + + dVector3 posbak; + dMatrix3 Rbak; + dReal aabbbak[6]; + int gflagsbak; + dVector3 pos0,pos1; + dMatrix3 R1; + + int numTerrainContacts = 0; + + //@@ Should find a way to set reComputeAABB to false in default case + // aka DHEIGHTFIELD_CORNER_ORIGIN not defined and terrain not PLACEABLE + // so that we can free some memory and speed up things a bit + // while saving some precision loss +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + const bool reComputeAABB = true; +#else + const bool reComputeAABB = ( terrain->gflags & GEOM_PLACEABLE ) ? true : false; +#endif //DHEIGHTFIELD_CORNER_ORIGIN + + // + // Transform O2 into Heightfield Space + // + if (reComputeAABB) + { + // Backup original o2 position, rotation and AABB. + dVector3Copy( o2->final_posr->pos, posbak ); + dMatrix3Copy( o2->final_posr->R, Rbak ); + memcpy( aabbbak, o2->aabb, sizeof( dReal ) * 6 ); + gflagsbak = o2->gflags; + } + + if ( terrain->gflags & GEOM_PLACEABLE ) + { + // Transform o2 into heightfield space. + dOP( pos0, -, o2->final_posr->pos, terrain->final_posr->pos ); + dMULTIPLY1_331( pos1, terrain->final_posr->R, pos0 ); + dMULTIPLY1_333( R1, terrain->final_posr->R, o2->final_posr->R ); + + // Update o2 with transformed position and rotation. + dVector3Copy( pos1, o2->final_posr->pos ); + dMatrix3Copy( R1, o2->final_posr->R ); + } + +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth; + o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth; +#endif // DHEIGHTFIELD_CORNER_ORIGIN + + // Rebuild AABB for O2 + if (reComputeAABB) + o2->computeAABB(); + + // + // Collide + // + + //check if inside boundaries + // using O2 aabb + // aabb[6] is (minx, maxx, miny, maxy, minz, maxz) + const bool wrapped = terrain->m_p_data->m_bWrapMode != 0; + + int nMinX; + int nMaxX; + int nMinZ; + int nMaxZ; + + if ( !wrapped ) + { + if ( o2->aabb[0] > terrain->m_p_data->m_fWidth //MinX + && o2->aabb[4] > terrain->m_p_data->m_fDepth)//MinZ + goto dCollideHeightfieldExit; + + if ( o2->aabb[1] < 0 //MaxX + && o2->aabb[5] < 0) //MaxZ + goto dCollideHeightfieldExit; + + } + + nMinX = int(dFloor(o2->aabb[0] * terrain->m_p_data->m_fInvSampleWidth)); + nMaxX = int(dFloor(o2->aabb[1] * terrain->m_p_data->m_fInvSampleWidth)) + 1; + nMinZ = int(dFloor(o2->aabb[4] * terrain->m_p_data->m_fInvSampleDepth)); + nMaxZ = int(dFloor(o2->aabb[5] * terrain->m_p_data->m_fInvSampleDepth)) + 1; + + if ( !wrapped ) + { + nMinX = dMAX( nMinX, 0 ); + nMaxX = dMIN( nMaxX, terrain->m_p_data->m_nWidthSamples - 1 ); + nMinZ = dMAX( nMinZ, 0 ); + nMaxZ = dMIN( nMaxZ, terrain->m_p_data->m_nDepthSamples - 1 ); + + dIASSERT ((nMinX < nMaxX) || (nMinZ < nMaxZ)) + } + + + + numTerrainContacts += terrain->dCollideHeightfieldZone( + nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + + dIASSERT( numTerrainContacts <= numMaxTerrainContacts ); + + dContactGeom *pContact; + for ( i = 0; i < numTerrainContacts; ++i ) + { + pContact = CONTACT(contact,i*skip); + pContact->g1 = o1; + pContact->g2 = o2; + } + + + //------------------------------------------------------------------------------ + +dCollideHeightfieldExit: + + if (reComputeAABB) + { + // Restore o2 position, rotation and AABB + dVector3Copy( posbak, o2->final_posr->pos ); + dMatrix3Copy( Rbak, o2->final_posr->R ); + memcpy( o2->aabb, aabbbak, sizeof(dReal)*6 ); + o2->gflags = gflagsbak; + + // + // Transform Contacts to World Space + // + if ( terrain->gflags & GEOM_PLACEABLE ) + { + for ( i = 0; i < numTerrainContacts; ++i ) + { + pContact = CONTACT(contact,i*skip); + dOPE( pos0, =, pContact->pos ); + +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth; + pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth; +#endif // !DHEIGHTFIELD_CORNER_ORIGIN + + dMULTIPLY0_331( pContact->pos, terrain->final_posr->R, pos0 ); + + dOP( pContact->pos, +, pContact->pos, terrain->final_posr->pos ); + dOPE( pos0, =, pContact->normal ); + + dMULTIPLY0_331( pContact->normal, terrain->final_posr->R, pos0 ); + } + } +#ifndef DHEIGHTFIELD_CORNER_ORIGIN + else + { + for ( i = 0; i < numTerrainContacts; ++i ) + { + pContact = CONTACT(contact,i*skip); + pContact->pos[ 0 ] -= terrain->m_p_data->m_fHalfWidth; + pContact->pos[ 2 ] -= terrain->m_p_data->m_fHalfDepth; + } + } +#endif // !DHEIGHTFIELD_CORNER_ORIGIN + } + // Return contact count. + return numTerrainContacts; +} + + diff --git a/libraries/ode-0.9/ode/src/heightfield.h b/libraries/ode-0.9/ode/src/heightfield.h new file mode 100644 index 0000000..f4c5952 --- /dev/null +++ b/libraries/ode-0.9/ode/src/heightfield.h @@ -0,0 +1,225 @@ +// dHeightfield Collider +// Martijn Buijs 2006 http://home.planet.nl/~buijs512/ +// Based on Terrain & Cone contrib by: +// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com + +#ifndef _DHEIGHTFIELD_H_ +#define _DHEIGHTFIELD_H_ +//------------------------------------------------------------------------------ + +#include +#include "collision_kernel.h" + + +#define HEIGHTFIELDMAXCONTACTPERCELL 10 + +// +// dxHeightfieldData +// +// Heightfield Data structure +// +struct dxHeightfieldData +{ + dReal m_fWidth; // World space heightfield dimension on X axis + dReal m_fDepth; // World space heightfield dimension on Z axis + dReal m_fSampleWidth; // Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1)) + dReal m_fSampleDepth; // Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1)) + dReal m_fInvSampleWidth; // Cache of inverse Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1)) + dReal m_fInvSampleDepth; // Cache of inverse Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1)) + + dReal m_fHalfWidth; // Cache of half of m_fWidth + dReal m_fHalfDepth; // Cache of half of m_fDepth + + dReal m_fMinHeight; // Min sample height value (scaled and offset) + dReal m_fMaxHeight; // Max sample height value (scaled and offset) + dReal m_fThickness; // Surface thickness (added to bottom AABB) + dReal m_fScale; // Sample value multiplier + dReal m_fOffset; // Vertical sample offset + + int m_nWidthSamples; // Vertex count on X axis edge (number of samples) + int m_nDepthSamples; // Vertex count on Z axis edge (number of samples) + int m_bCopyHeightData; // Do we own the sample data? + int m_bWrapMode; // Heightfield wrapping mode (0=finite, 1=infinite) + int m_nGetHeightMode; // GetHeight mode ( 0=callback, 1=byte, 2=short, 3=float ) + + const void* m_pHeightData; // Sample data array + void* m_pUserData; // Callback user data + + dContactGeom m_contacts[HEIGHTFIELDMAXCONTACTPERCELL]; + + dHeightfieldGetHeight* m_pGetHeightCallback; // Callback pointer. + + dxHeightfieldData(); + ~dxHeightfieldData(); + + void SetData( int nWidthSamples, int nDepthSamples, + dReal fWidth, dReal fDepth, + dReal fScale, dReal fOffset, + dReal fThickness, int bWrapMode ); + + void ComputeHeightBounds(); + + bool IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const; + bool IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const; + + dReal GetHeight(int x, int z); + dReal GetHeight(dReal x, dReal z); + +}; + +class HeightFieldVertex; +class HeightFieldEdge; +class HeightFieldTriangle; + +class HeightFieldVertex +{ +public: + HeightFieldVertex(){}; + + dVector3 vertex; + bool state; +}; + +class HeightFieldEdge +{ +public: + HeightFieldEdge(){}; + + HeightFieldVertex *vertices[2]; +}; +// +// HeightFieldTriangle +// +// HeightFieldTriangle composing heightfield mesh +// +class HeightFieldTriangle +{ +public: + HeightFieldTriangle(){}; + + inline void setMinMax() + { + maxAAAB = vertices[0]->vertex[1] > vertices[1]->vertex[1] ? vertices[0]->vertex[1] : vertices[1]->vertex[1]; + maxAAAB = vertices[2]->vertex[1] > maxAAAB ? vertices[2]->vertex[1] : maxAAAB; + }; + + HeightFieldVertex *vertices[3]; + dReal planeDef[4]; + dReal maxAAAB; + + bool isUp; + bool state; +}; +// +// HeightFieldTriangle +// +// HeightFieldPlane composing heightfield mesh +// +class HeightFieldPlane +{ +public: + HeightFieldPlane(): + trianglelist(0), + trianglelistReservedSize(0), + trianglelistCurrentSize(0) + { + + }; + ~HeightFieldPlane() + { + delete [] trianglelist; + }; + + inline void setMinMax() + { + const size_t asize = trianglelistCurrentSize; + if (asize > 0) + { + maxAAAB = trianglelist[0]->maxAAAB; + for (size_t k = 1; asize > k; k++) + { + if (trianglelist[k]->maxAAAB > maxAAAB) + maxAAAB = trianglelist[k]->maxAAAB; + } + } + }; + + void resetTriangleListSize(const size_t newSize) + { + if (trianglelistReservedSize < newSize) + { + delete [] trianglelist; + trianglelistReservedSize = newSize; + trianglelist = new HeightFieldTriangle *[newSize]; + } + trianglelistCurrentSize = 0; + } + + void addTriangle(HeightFieldTriangle *tri) + { + dIASSERT(trianglelistCurrentSize < trianglelistReservedSize); + + trianglelist[trianglelistCurrentSize++] = tri; + } + + HeightFieldTriangle **trianglelist; + size_t trianglelistReservedSize; + size_t trianglelistCurrentSize; + + dReal maxAAAB; + dReal planeDef[4]; +}; +// +// dxHeightfield +// +// Heightfield geom structure +// +struct dxHeightfield : public dxGeom +{ + dxHeightfieldData* m_p_data; + + dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable ); + ~dxHeightfield(); + + void computeAABB(); + + int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ, + dxGeom *o2, const int numMaxContacts, + int flags, dContactGeom *contact, int skip ); + + enum + { + TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 4, + TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X = 4, + TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z = 4, + TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 1, // Triangles are easy to reallocate and hard to predict + }; + + static inline size_t AlignBufferSize(size_t value, size_t alignment) { dIASSERT((alignment & (alignment - 1)) == 0); return (value + (alignment - 1)) & ~(alignment - 1); } + + void allocateTriangleBuffer(size_t numTri); + void resetTriangleBuffer(); + void allocatePlaneBuffer(size_t numTri); + void resetPlaneBuffer(); + void allocateHeightBuffer(size_t numX, size_t numZ); + void resetHeightBuffer(); + + void sortPlanes(const size_t numPlanes); + + HeightFieldPlane **tempPlaneBuffer; + HeightFieldPlane *tempPlaneInstances; + size_t tempPlaneBufferSize; + + HeightFieldTriangle *tempTriangleBuffer; + size_t tempTriangleBufferSize; + + HeightFieldVertex **tempHeightBuffer; + HeightFieldVertex *tempHeightInstances; + size_t tempHeightBufferSizeX; + size_t tempHeightBufferSizeZ; + +}; + + +//------------------------------------------------------------------------------ +#endif //_DHEIGHTFIELD_H_ diff --git a/libraries/ode-0.9/ode/src/joint.cpp b/libraries/ode-0.9/ode/src/joint.cpp new file mode 100644 index 0000000..d83294b --- /dev/null +++ b/libraries/ode-0.9/ode/src/joint.cpp @@ -0,0 +1,4065 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +design note: the general principle for giving a joint the option of connecting +to the static environment (i.e. the absolute frame) is to check the second +body (joint->node[1].body), and if it is zero then behave as if its body +transform is the identity. + +*/ + +#include +#include +#include +#include +#include "joint.h" + +//**************************************************************************** +// externs + +// extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +// extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); + +//**************************************************************************** +// utility + +// set three "ball-and-socket" rows in the constraint equation, and the +// corresponding right hand side. + +static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); + dCROSSMAT (info->J1a,a1,s,-,+); + if (joint->node[1].body) { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + // set right hand side + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) { + info->c[j] = k * (a2[j] + joint->node[1].body->posr.pos[j] - + a1[j] - joint->node[0].body->posr.pos[j]); + } + } + else { + for (int j=0; j<3; j++) { + info->c[j] = k * (anchor2[j] - a1[j] - + joint->node[0].body->posr.pos[j]); + } + } +} + + +// this is like setBall(), except that `axis' is a unit length vector +// (in global coordinates) that should be used for the first jacobian +// position row (the other two row vectors will be derived from this). +// `erp1' is the erp value to use along the axis. + +static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2, + dVector3 axis, dReal erp1) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int i,s = info->rowskip; + + // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], + // [0 1 0] and [0 0 1], which makes everything much easier. + dVector3 q1,q2; + dPlaneSpace (axis,q1,q2); + + // set jacobian + for (i=0; i<3; i++) info->J1l[i] = axis[i]; + for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; + for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; + dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); + dCROSS (info->J1a,=,a1,axis); + dCROSS (info->J1a+s,=,a1,q1); + dCROSS (info->J1a+2*s,=,a1,q2); + if (joint->node[1].body) { + for (i=0; i<3; i++) info->J2l[i] = -axis[i]; + for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; + for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; + dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); + dCROSS (info->J2a,= -,a2,axis); + dCROSS (info->J2a+s,= -,a2,q1); + dCROSS (info->J2a+2*s,= -,a2,q2); + } + + // set right hand side - measure error along (axis,q1,q2) + dReal k1 = info->fps * erp1; + dReal k = info->fps * info->erp; + + for (i=0; i<3; i++) a1[i] += joint->node[0].body->posr.pos[i]; + if (joint->node[1].body) { + for (i=0; i<3; i++) a2[i] += joint->node[1].body->posr.pos[i]; + info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); + } + else { + info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); + } +} + + +// set three orientation rows in the constraint equation, and the +// corresponding right hand side. + +static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) +{ + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->J1a[start_index] = 1; + info->J1a[start_index + s + 1] = 1; + info->J1a[start_index + s*2+2] = 1; + if (joint->node[1].body) { + info->J2a[start_index] = -1; + info->J2a[start_index + s+1] = -1; + info->J2a[start_index + s*2+2] = -1; + } + + // compute the right hand side. the first three elements will result in + // relative angular velocity of the two bodies - this is set to bring them + // back into alignment. the correcting angular velocity is + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * u + // = (erp*fps) * theta * u + // where rotation along unit length axis u by theta brings body 2's frame + // to qrel with respect to body 1's frame. using a small angle approximation + // for sin(), this gives + // angular_velocity = (erp*fps) * 2 * v + // where the quaternion of the relative rotation between the two bodies is + // q = [cos(theta/2) sin(theta/2)*u] = [s v] + + // get qerr = relative rotation (rotation error) between two bodies + dQuaternion qerr,e; + if (joint->node[1].body) { + dQuaternion qq; + dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); + dQMultiply2 (qerr,qq,qrel); + } + else { + dQMultiply3 (qerr,joint->node[0].body->q,qrel); + } + if (qerr[0] < 0) { + qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small + qerr[2] = -qerr[2]; + qerr[3] = -qerr[3]; + } + dMULTIPLY0_331 (e,joint->node[0].body->posr.R,qerr+1); // @@@ bad SIMD padding! + dReal k = info->fps * info->erp; + info->c[start_row] = 2*k * e[0]; + info->c[start_row+1] = 2*k * e[1]; + info->c[start_row+2] = 2*k * e[2]; +} + + +// compute anchor points relative to bodies + +static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 anchor1, dVector3 anchor2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x - j->node[0].body->posr.pos[0]; + q[1] = y - j->node[0].body->posr.pos[1]; + q[2] = z - j->node[0].body->posr.pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor1,j->node[0].body->posr.R,q); + if (j->node[1].body) { + q[0] = x - j->node[1].body->posr.pos[0]; + q[1] = y - j->node[1].body->posr.pos[1]; + q[2] = z - j->node[1].body->posr.pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor2,j->node[1].body->posr.R,q); + } + else { + anchor2[0] = x; + anchor2[1] = y; + anchor2[2] = z; + } + } + anchor1[3] = 0; + anchor2[3] = 0; +} + + +// compute axes relative to bodies. either axis1 or axis2 can be 0. + +static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 axis1, dVector3 axis2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + if (axis1) { + dMULTIPLY1_331 (axis1,j->node[0].body->posr.R,q); + axis1[3] = 0; + } + if (axis2) { + if (j->node[1].body) { + dMULTIPLY1_331 (axis2,j->node[1].body->posr.R,q); + } + else { + axis2[0] = x; + axis2[1] = y; + axis2[2] = z; + } + axis2[3] = 0; + } + } +} + + +static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->posr.R,anchor1); + result[0] += j->node[0].body->posr.pos[0]; + result[1] += j->node[0].body->posr.pos[1]; + result[2] += j->node[0].body->posr.pos[2]; + } +} + + +static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->posr.R,anchor2); + result[0] += j->node[1].body->posr.pos[0]; + result[1] += j->node[1].body->posr.pos[1]; + result[2] += j->node[1].body->posr.pos[2]; + } + else { + result[0] = anchor2[0]; + result[1] = anchor2[1]; + result[2] = anchor2[2]; + } +} + + +static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->posr.R,axis1); + } +} + + +static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->posr.R,axis2); + } + else { + result[0] = axis2[0]; + result[1] = axis2[1]; + result[2] = axis2[2]; + } +} + + +static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) +{ + // the angle between the two bodies is extracted from the quaternion that + // represents the relative rotation between them. recall that a quaternion + // q is: + // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] + // where s is a scalar and v is a 3-vector. u is a unit length axis and + // theta is a rotation along that axis. we can get theta/2 by: + // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) + // but we can't get sin(theta/2) directly, only its absolute value, i.e.: + // |v| = |sin(theta/2)| * |u| + // = |sin(theta/2)| + // using this value will have a strange effect. recall that there are two + // quaternion representations of a given rotation, q and -q. typically as + // a body rotates along the axis it will go through a complete cycle using + // one representation and then the next cycle will use the other + // representation. this corresponds to u pointing in the direction of the + // hinge axis and then in the opposite direction. the result is that theta + // will appear to go "backwards" every other cycle. here is a fix: if u + // points "away" from the direction of the hinge (motor) axis (i.e. more + // than 90 degrees) then use -q instead of q. this represents the same + // rotation, but results in the cos(theta/2) value being sign inverted. + + // extract the angle from the quaternion. cost2 = cos(theta/2), + // sint2 = |sin(theta/2)| + dReal cost2 = qrel[0]; + dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); + dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions + (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis + (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction + + // the angle we get will be between 0..2*pi, but we want to return angles + // between -pi..pi + if (theta > M_PI) theta -= 2*M_PI; + + // the angle we've just extracted has the wrong sign + theta = -theta; + + return theta; +} + + +// given two bodies (body1,body2), the hinge axis that they are connected by +// w.r.t. body1 (axis), and the initial relative orientation between them +// (q_initial), return the relative rotation angle. the initial relative +// orientation corresponds to an angle of zero. if body2 is 0 then measure the +// angle between body1 and the static frame. +// +// this will not return the correct angle if the bodies rotate along any axis +// other than the given hinge axis. + +static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, + dQuaternion q_initial) +{ + // get qrel = relative rotation between the two bodies + dQuaternion qrel; + if (body2) { + dQuaternion qq; + dQMultiply1 (qq,body1->q,body2->q); + dQMultiply2 (qrel,qq,q_initial); + } + else { + // pretend body2->q is the identity + dQMultiply3 (qrel,body1->q,q_initial); + } + + return getHingeAngleFromRelativeQuat (qrel,axis); +} + +//**************************************************************************** +// dxJointLimitMotor + +void dxJointLimitMotor::init (dxWorld *world) +{ + vel = 0; + fmax = 0; + lostop = -dInfinity; + histop = dInfinity; + fudge_factor = 1; + normal_cfm = world->global_cfm; + stop_erp = world->global_erp; + stop_cfm = world->global_cfm; + bounce = 0; + limit = 0; + limit_err = 0; +} + + +void dxJointLimitMotor::set (int num, dReal value) +{ + switch (num) { + case dParamLoStop: + lostop = value; + break; + case dParamHiStop: + histop = value; + break; + case dParamVel: + vel = value; + break; + case dParamFMax: + if (value >= 0) fmax = value; + break; + case dParamFudgeFactor: + if (value >= 0 && value <= 1) fudge_factor = value; + break; + case dParamBounce: + bounce = value; + break; + case dParamCFM: + normal_cfm = value; + break; + case dParamStopERP: + stop_erp = value; + break; + case dParamStopCFM: + stop_cfm = value; + break; + } +} + + +dReal dxJointLimitMotor::get (int num) +{ + switch (num) { + case dParamLoStop: return lostop; + case dParamHiStop: return histop; + case dParamVel: return vel; + case dParamFMax: return fmax; + case dParamFudgeFactor: return fudge_factor; + case dParamBounce: return bounce; + case dParamCFM: return normal_cfm; + case dParamStopERP: return stop_erp; + case dParamStopCFM: return stop_cfm; + default: return 0; + } +} + + +int dxJointLimitMotor::testRotationalLimit (dReal angle) +{ + if (angle <= lostop) { + limit = 1; + limit_err = angle - lostop; + return 1; + } + else if (angle >= histop) { + limit = 2; + limit_err = angle - histop; + return 1; + } + else { + limit = 0; + return 0; + } +} + + +int dxJointLimitMotor::addLimot (dxJoint *joint, + dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational) +{ + int srow = row * info->rowskip; + + // if the joint is powered, or has joint limits, add in the extra row + int powered = fmax > 0; + if (powered || limit) { + dReal *J1 = rotational ? info->J1a : info->J1l; + dReal *J2 = rotational ? info->J2a : info->J2l; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (joint->node[1].body) { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && joint->node[1].body) { + dVector3 c; + c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); + dCROSS (ltd,=,c,ax1); + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[srow+2] = ltd[2]; + } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (lostop == histop)) powered = 0; + + if (powered) { + info->cfm[row] = normal_cfm; + if (! limit) { + info->c[row] = vel; + info->lo[row] = -fmax; + info->hi[row] = fmax; + } + else { + // the joint is at a limit, AND is being powered. if the joint is + // being powered into the limit then we apply the maximum motor force + // in that direction, because the motor is working against the + // immovable limit. if the joint is being powered away from the limit + // then we have problems because actually we need *two* lcp + // constraints to handle this case. so we fake it and apply some + // fraction of the maximum force. the fraction to use can be set as + // a fudge factor. + + dReal fm = fmax; + if ((vel > 0) || (vel==0 && limit==2)) fm = -fm; + + // if we're powering away from the limit, apply the fudge factor + if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; + + if (rotational) { + dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], + -fm*ax1[2]); + if (joint->node[1].body) + dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + } + else { + dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); + if (joint->node[1].body) { + dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + + // linear limot torque decoupling step: refer to above discussion + dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + } + } + } + } + + if (limit) { + dReal k = info->fps * stop_erp; + info->c[row] = -k * limit_err; + info->cfm[row] = stop_cfm; + + if (lostop == histop) { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else { + if (limit == 1) { + // low limit + info->lo[row] = 0; + info->hi[row] = dInfinity; + } + else { + // high limit + info->lo[row] = -dInfinity; + info->hi[row] = 0; + } + + // deal with bounce + if (bounce > 0) { + // calculate joint velocity + dReal vel; + if (rotational) { + vel = dDOT(joint->node[0].body->avel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->avel,ax1); + } + else { + vel = dDOT(joint->node[0].body->lvel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->lvel,ax1); + } + + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) { + // low limit + if (vel < 0) { + dReal newc = -bounce * vel; + if (newc > info->c[row]) info->c[row] = newc; + } + } + else { + // high limit - all those computations are reversed + if (vel > 0) { + dReal newc = -bounce * vel; + if (newc < info->c[row]) info->c[row] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + +//**************************************************************************** +// ball and socket + +static void ballInit (dxJointBall *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + j->erp = j->world->global_erp; + j->cfm = j->world->global_cfm; +} + + +static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) +{ + info->m = 3; + info->nub = 3; +} + + +static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) +{ + info->erp = joint->erp; + info->cfm[0] = joint->cfm; + info->cfm[1] = joint->cfm; + info->cfm[2] = joint->cfm; + setBall (joint,info,joint->anchor1,joint->anchor2); +} + + +void dJointSetBallAnchor (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointBall* joint = (dxJointBall*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); +} + + +void dJointSetBallAnchor2 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointBall* joint = (dxJointBall*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + joint->anchor2[0] = x; + joint->anchor2[1] = y; + joint->anchor2[2] = z; + joint->anchor2[3] = 0; + +} + +void dJointGetBallAnchor (dJointID j, dVector3 result) +{ + dxJointBall* joint = (dxJointBall*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +void dJointGetBallAnchor2 (dJointID j, dVector3 result) +{ + dxJointBall* joint = (dxJointBall*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +void dxJointBall::set (int num, dReal value) +{ + switch (num) { + case dParamCFM: + cfm = value; + break; + case dParamERP: + erp = value; + break; + } +} + + +dReal dxJointBall::get (int num) +{ + switch (num) { + case dParamCFM: + return cfm; + case dParamERP: + return erp; + default: + return 0; + } +} + + +void dJointSetBallParam (dJointID j, int parameter, dReal value) +{ + dxJointBall* joint = (dxJointBall*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball joint"); + joint->set (parameter,value); +} + + +dReal dJointGetBallParam (dJointID j, int parameter) +{ + dxJointBall* joint = (dxJointBall*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball joint"); + return joint->get (parameter); +} + + +dxJoint::Vtable __dball_vtable = { + sizeof(dxJointBall), + (dxJoint::init_fn*) ballInit, + (dxJoint::getInfo1_fn*) ballGetInfo1, + (dxJoint::getInfo2_fn*) ballGetInfo2, + dJointTypeBall}; + +//**************************************************************************** +// hinge + +static void hingeInit (dxJointHinge *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[0] = 1; + dSetZero (j->qrel,4); + j->limot.init (j->world); +} + + +static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered hinge needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && + j->limot.lostop <= j->limot.histop) { + dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, + j->qrel); + if (j->limot.testRotationalLimit (angle)) info->m = 6; + } +} + + +static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) +{ + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the two hinge rows. the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body + dVector3 p,q; // plane space vectors for ax1 + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); + dPlaneSpace (ax1,p,q); + + int s3=3*info->rowskip; + int s4=4*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + info->J1a[s4+0] = q[0]; + info->J1a[s4+1] = q[1]; + info->J1a[s4+2] = q[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + info->J2a[s4+0] = -q[0]; + info->J2a[s4+1] = -q[1]; + info->J2a[s4+2] = -q[2]; + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + + dVector3 ax2,b; + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } + dCROSS (b,=,ax1,ax2); + dReal k = info->fps * info->erp; + info->c[3] = k * dDOT(b,p); + info->c[4] = k * dDOT(b,q); + + // if the hinge is powered, or has joint limits, add in the stuff + joint->limot.addLimot (joint,info,5,ax1,1); +} + + +// compute initial relative rotation body1 -> body2, or env -> body1 + +static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) +{ + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + } + else { + // set joint->qrel to the transpose of the first body q + joint->qrel[0] = joint->node[0].body->q[0]; + for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + } + } +} + + +void dJointSetHingeAnchor (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + hingeComputeInitialRelativeRotation (joint); +} + + +void dJointSetHingeAnchorDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + + if (joint->node[0].body) { + dReal q[4]; + q[0] = x - joint->node[0].body->posr.pos[0]; + q[1] = y - joint->node[0].body->posr.pos[1]; + q[2] = z - joint->node[0].body->posr.pos[2]; + q[3] = 0; + dMULTIPLY1_331 (joint->anchor1,joint->node[0].body->posr.R,q); + + if (joint->node[1].body) { + q[0] = x - joint->node[1].body->posr.pos[0]; + q[1] = y - joint->node[1].body->posr.pos[1]; + q[2] = z - joint->node[1].body->posr.pos[2]; + q[3] = 0; + dMULTIPLY1_331 (joint->anchor2,joint->node[1].body->posr.R,q); + } + else { + // Move the relative displacement between the passive body and the + // anchor in the same direction as the passive body has just moved + joint->anchor2[0] = x + dx; + joint->anchor2[1] = y + dy; + joint->anchor2[2] = z + dz; + } + } + joint->anchor1[3] = 0; + joint->anchor2[3] = 0; + + hingeComputeInitialRelativeRotation (joint); +} + + + +void dJointSetHingeAxis (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAxes (joint,x,y,z,joint->axis1,joint->axis2); + hingeComputeInitialRelativeRotation (joint); +} + + +void dJointGetHingeAnchor (dJointID j, dVector3 result) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +void dJointGetHingeAnchor2 (dJointID j, dVector3 result) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +void dJointGetHingeAxis (dJointID j, dVector3 result) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + getAxis (joint,result,joint->axis1); +} + + +void dJointSetHingeParam (dJointID j, int parameter, dReal value) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + joint->limot.set (parameter,value); +} + + +dReal dJointGetHingeParam (dJointID j, int parameter) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + return joint->limot.get (parameter); +} + + +dReal dJointGetHingeAngle (dJointID j) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->node[0].body) { + dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, + joint->qrel); + if (joint->flags & dJOINT_REVERSE) + return -ang; + else + return ang; + } + else return 0; +} + + +dReal dJointGetHingeAngleRate (dJointID j) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + if (joint->flags & dJOINT_REVERSE) rate = - rate; + return rate; + } + else return 0; +} + + +void dJointAddHingeTorque (dJointID j, dReal torque) +{ + dxJointHinge* joint = (dxJointHinge*)j; + dVector3 axis; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + + if (joint->flags & dJOINT_REVERSE) + torque = -torque; + + getAxis (joint,axis,joint->axis1); + axis[0] *= torque; + axis[1] *= torque; + axis[2] *= torque; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dhinge_vtable = { + sizeof(dxJointHinge), + (dxJoint::init_fn*) hingeInit, + (dxJoint::getInfo1_fn*) hingeGetInfo1, + (dxJoint::getInfo2_fn*) hingeGetInfo2, + dJointTypeHinge}; + +//**************************************************************************** +// slider + +static void sliderInit (dxJointSlider *j) +{ + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->qrel,4); + dSetZero (j->offset,4); + j->limot.init (j->world); +} + + +dReal dJointGetSliderPosition (dJointID j) +{ + dxJointSlider* joint = (dxJointSlider*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1,q; + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); + + if (joint->node[1].body) { + // get body2 + offset point in global coordinates + dMULTIPLY0_331 (q,joint->node[1].body->posr.R,joint->offset); + for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - q[i] - + joint->node[1].body->posr.pos[i]; + } + else { + for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - + joint->offset[i]; + + } + return dDOT(ax1,q); +} + + +dReal dJointGetSliderPositionRate (dJointID j) +{ + dxJointSlider* joint = (dxJointSlider*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1; + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); + + if (joint->node[1].body) { + return dDOT(ax1,joint->node[0].body->lvel) - + dDOT(ax1,joint->node[1].body->lvel); + } + else { + return dDOT(ax1,joint->node[0].body->lvel); + } +} + + +static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered slider needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + j->limot.limit = 0; + if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && + j->limot.lostop <= j->limot.histop) { + // measure joint position + dReal pos = dJointGetSliderPosition (j); + if (pos <= j->limot.lostop) { + j->limot.limit = 1; + j->limot.limit_err = pos - j->limot.lostop; + info->m = 6; + } + else if (pos >= j->limot.histop) { + j->limot.limit = 2; + j->limot.limit_err = pos - j->limot.histop; + info->m = 6; + } + } +} + + +static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s3=3*s,s4=4*s; + + // pull out pos and R for both bodies. also get the `connection' + // vector pos2-pos1. + + dReal *pos1,*pos2,*R1,*R2; + dVector3 c; + pos1 = joint->node[0].body->posr.pos; + R1 = joint->node[0].body->posr.R; + if (joint->node[1].body) { + pos2 = joint->node[1].body->posr.pos; + R2 = joint->node[1].body->posr.R; + for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; + } + else { + pos2 = 0; + R2 = 0; + } + + // 3 rows to make body rotations equal + setFixedOrientation(joint, info, joint->qrel, 0); + + // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + + dVector3 ax1; // joint axis in global coordinates (unit length) + dVector3 p,q; // plane space of ax1 + dMULTIPLY0_331 (ax1,R1,joint->axis1); + dPlaneSpace (ax1,p,q); + if (joint->node[1].body) { + dVector3 tmp; + dCROSS (tmp, = REAL(0.5) * ,c,p); + for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + dCROSS (tmp, = REAL(0.5) * ,c,q); + for (i=0; i<3; i++) info->J1a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; + for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; + } + for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; + for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; + + // compute last two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + dVector3 ofs; // offset point in global coordinates + dMULTIPLY0_331 (ofs,R2,joint->offset); + for (i=0; i<3; i++) c[i] += ofs[i]; + info->c[3] = k * dDOT(p,c); + info->c[4] = k * dDOT(q,c); + } + else { + dVector3 ofs; // offset point in global coordinates + for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; + info->c[3] = k * dDOT(p,ofs); + info->c[4] = k * dDOT(q,ofs); + } + + // if the slider is powered, or has joint limits, add in the extra row + joint->limot.addLimot (joint,info,5,ax1,0); +} + + +void dJointSetSliderAxis (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointSlider* joint = (dxJointSlider*)j; + int i; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + setAxes (joint,x,y,z,joint->axis1,0); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dVector3 c; + for (i=0; i<3; i++) + c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; + } +} + + +void dJointSetSliderAxisDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz) +{ + dxJointSlider* joint = (dxJointSlider*)j; + int i; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + setAxes (joint,x,y,z,joint->axis1,0); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dVector3 c; + for (i=0; i<3; i++) + c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + + for (i=1; i<4; i++) + joint->qrel[i] = -joint->node[0].body->q[i]; + + joint->offset[0] = joint->node[0].body->posr.pos[0] + dx; + joint->offset[1] = joint->node[0].body->posr.pos[1] + dy; + joint->offset[2] = joint->node[0].body->posr.pos[2] + dz; + } +} + + + +void dJointGetSliderAxis (dJointID j, dVector3 result) +{ + dxJointSlider* joint = (dxJointSlider*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + getAxis (joint,result,joint->axis1); +} + + +void dJointSetSliderParam (dJointID j, int parameter, dReal value) +{ + dxJointSlider* joint = (dxJointSlider*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + joint->limot.set (parameter,value); +} + + +dReal dJointGetSliderParam (dJointID j, int parameter) +{ + dxJointSlider* joint = (dxJointSlider*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + return joint->limot.get (parameter); +} + + +void dJointAddSliderForce (dJointID j, dReal force) +{ + dxJointSlider* joint = (dxJointSlider*)j; + dVector3 axis; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + if (joint->flags & dJOINT_REVERSE) + force -= force; + + getAxis (joint,axis,joint->axis1); + axis[0] *= force; + axis[1] *= force; + axis[2] *= force; + + if (joint->node[0].body != 0) + dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); + if (joint->node[1].body != 0) + dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); + + if (joint->node[0].body != 0 && joint->node[1].body != 0) { + // linear torque decoupling: + // we have to compensate the torque, that this slider force may generate + // if body centers are not aligned along the slider axis + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + + dVector3 c; + c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); + dCROSS (ltd,=,c,axis); + + dBodyAddTorque (joint->node[0].body,ltd[0],ltd[1], ltd[2]); + dBodyAddTorque (joint->node[1].body,ltd[0],ltd[1], ltd[2]); + } +} + + +dxJoint::Vtable __dslider_vtable = { + sizeof(dxJointSlider), + (dxJoint::init_fn*) sliderInit, + (dxJoint::getInfo1_fn*) sliderGetInfo1, + (dxJoint::getInfo2_fn*) sliderGetInfo2, + dJointTypeSlider}; + +//**************************************************************************** +// contact + +static void contactInit (dxJointContact *j) +{ + // default frictionless contact. hmmm, this info gets overwritten straight + // away anyway, so why bother? +#if 0 /* so don't bother ;) */ + j->contact.surface.mode = 0; + j->contact.surface.mu = 0; + dSetZero (j->contact.geom.pos,4); + dSetZero (j->contact.geom.normal,4); + j->contact.geom.depth = 0; +#endif +} + + +static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) +{ + // make sure mu's >= 0, then calculate number of constraint rows and number + // of unbounded rows. + int m = 1, nub=0; + if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; + if (j->contact.surface.mode & dContactMu2) { + if (j->contact.surface.mu > 0) m++; + if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; + if (j->contact.surface.mu2 > 0) m++; + if (j->contact.surface.mu == dInfinity) nub ++; + if (j->contact.surface.mu2 == dInfinity) nub ++; + } + else { + if (j->contact.surface.mu > 0) m += 2; + if (j->contact.surface.mu == dInfinity) nub += 2; + } + + j->the_m = m; + info->m = m; + info->nub = nub; +} + + +static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) +{ + int s = info->rowskip; + int s2 = 2*s; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + if (j->flags & dJOINT_REVERSE) { + normal[0] = - j->contact.geom.normal[0]; + normal[1] = - j->contact.geom.normal[1]; + normal[2] = - j->contact.geom.normal[2]; + } + else { + normal[0] = j->contact.geom.normal[0]; + normal[1] = j->contact.geom.normal[1]; + normal[2] = j->contact.geom.normal[2]; + } + normal[3] = 0; // @@@ hmmm + + // c1,c2 = contact points with respect to body PORs + dVector3 c1,c2; + c1[0] = j->contact.geom.pos[0] - j->node[0].body->posr.pos[0]; + c1[1] = j->contact.geom.pos[1] - j->node[0].body->posr.pos[1]; + c1[2] = j->contact.geom.pos[2] - j->node[0].body->posr.pos[2]; + + // set jacobian for normal + info->J1l[0] = normal[0]; + info->J1l[1] = normal[1]; + info->J1l[2] = normal[2]; + dCROSS (info->J1a,=,c1,normal); + if (j->node[1].body) { + c2[0] = j->contact.geom.pos[0] - j->node[1].body->posr.pos[0]; + c2[1] = j->contact.geom.pos[1] - j->node[1].body->posr.pos[1]; + c2[2] = j->contact.geom.pos[2] - j->node[1].body->posr.pos[2]; + info->J2l[0] = -normal[0]; + info->J2l[1] = -normal[1]; + info->J2l[2] = -normal[2]; + dCROSS (info->J2a,= -,c2,normal); + } + + // set right hand side and cfm value for normal + dReal erp = info->erp; + if (j->contact.surface.mode & dContactSoftERP) + erp = j->contact.surface.soft_erp; + dReal k = info->fps * erp; + dReal depth = j->contact.geom.depth - j->world->contactp.min_depth; + if (depth < 0) depth = 0; + + const dReal maxvel = j->world->contactp.max_vel; + info->c[0] = k*depth; + if (info->c[0] > maxvel) + info->c[0] = maxvel; + + if (j->contact.surface.mode & dContactSoftCFM) + info->cfm[0] = j->contact.surface.soft_cfm; + + // deal with bounce + if (j->contact.surface.mode & dContactBounce) { + // calculate outgoing velocity (-ve for incoming contact) + dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + + dDOT(info->J1a,j->node[0].body->avel); + if (j->node[1].body) { + outgoing += dDOT(info->J2l,j->node[1].body->lvel) + + dDOT(info->J2a,j->node[1].body->avel); + } + // only apply bounce if the outgoing velocity is greater than the + // threshold, and if the resulting c[0] exceeds what we already have. + if (j->contact.surface.bounce_vel >= 0 && + (-outgoing) > j->contact.surface.bounce_vel) { + dReal newc = - j->contact.surface.bounce * outgoing; + if (newc > info->c[0]) info->c[0] = newc; + } + } + + // set LCP limits for normal + info->lo[0] = 0; + info->hi[0] = dInfinity; + + // now do jacobian for tangential forces + dVector3 t1,t2; // two vectors tangential to normal + + // first friction direction + if (j->the_m >= 2) { + if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? + t1[0] = j->contact.fdir1[0]; + t1[1] = j->contact.fdir1[1]; + t1[2] = j->contact.fdir1[2]; + dCROSS (t2,=,normal,t1); + } + else { + dPlaneSpace (normal,t1,t2); + } + info->J1l[s+0] = t1[0]; + info->J1l[s+1] = t1[1]; + info->J1l[s+2] = t1[2]; + dCROSS (info->J1a+s,=,c1,t1); + if (j->node[1].body) { + info->J2l[s+0] = -t1[0]; + info->J2l[s+1] = -t1[1]; + info->J2l[s+2] = -t1[2]; + dCROSS (info->J2a+s,= -,c2,t1); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion1) { + info->c[1] = j->contact.surface.motion1; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + info->lo[1] = -j->contact.surface.mu; + info->hi[1] = j->contact.surface.mu; + if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip1) + info->cfm[1] = j->contact.surface.slip1; + } + + // second friction direction + if (j->the_m >= 3) { + info->J1l[s2+0] = t2[0]; + info->J1l[s2+1] = t2[1]; + info->J1l[s2+2] = t2[2]; + dCROSS (info->J1a+s2,=,c1,t2); + if (j->node[1].body) { + info->J2l[s2+0] = -t2[0]; + info->J2l[s2+1] = -t2[1]; + info->J2l[s2+2] = -t2[2]; + dCROSS (info->J2a+s2,= -,c2,t2); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion2) { + info->c[2] = j->contact.surface.motion2; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + if (j->contact.surface.mode & dContactMu2) { + info->lo[2] = -j->contact.surface.mu2; + info->hi[2] = j->contact.surface.mu2; + } + else { + info->lo[2] = -j->contact.surface.mu; + info->hi[2] = j->contact.surface.mu; + } + if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip2) + info->cfm[2] = j->contact.surface.slip2; + } +} + + +dxJoint::Vtable __dcontact_vtable = { + sizeof(dxJointContact), + (dxJoint::init_fn*) contactInit, + (dxJoint::getInfo1_fn*) contactGetInfo1, + (dxJoint::getInfo2_fn*) contactGetInfo2, + dJointTypeContact}; + +//**************************************************************************** +// hinge 2. note that this joint must be attached to two bodies for it to work + +static dReal measureHinge2Angle (dxJointHinge2 *joint) +{ + dVector3 a1,a2; + dMULTIPLY0_331 (a1,joint->node[1].body->posr.R,joint->axis2); + dMULTIPLY1_331 (a2,joint->node[0].body->posr.R,a1); + dReal x = dDOT(joint->v1,a2); + dReal y = dDOT(joint->v2,a2); + return -dAtan2 (y,x); +} + + +static void hinge2Init (dxJointHinge2 *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + j->c0 = 0; + j->s0 = 0; + + dSetZero (j->v1,4); + j->v1[0] = 1; + dSetZero (j->v2,4); + j->v2[1] = 1; + + j->limot1.init (j->world); + j->limot2.init (j->world); + + j->susp_erp = j->world->global_erp; + j->susp_cfm = j->world->global_cfm; + + j->flags |= dJOINT_TWOBODIES; +} + + +static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) +{ + info->m = 4; + info->nub = 4; + + // see if we're powered or at a joint limit for axis 1 + int atlimit=0; + if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop) { + dReal angle = measureHinge2Angle (j); + if (j->limot1.testRotationalLimit (angle)) atlimit = 1; + } + if (atlimit || j->limot1.fmax > 0) info->m++; + + // see if we're powering axis 2 (we currently never limit this axis) + j->limot2.limit = 0; + if (j->limot2.fmax > 0) info->m++; +} + + +// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are +// relative to body 1 and 2 initially) and then computes the constrained +// rotational axis as the cross product of ax1 and ax2. +// the sin and cos of the angle between axis 1 and 2 is computed, this comes +// from dot and cross product rules. + +#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ + dVector3 ax1,ax2; \ + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); \ + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); \ + dCROSS (axis,=,ax1,ax2); \ + sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ + cos_angle = dDOT (ax1,ax2); + + +static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) +{ + // get information we need to set the hinge row + dReal s,c; + dVector3 q; + HINGE2_GET_AXIS_INFO (q,s,c); + dNormalize3 (q); // @@@ quicker: divide q by s ? + + // set the three ball-and-socket rows (aligned to the suspension axis ax1) + setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); + + // set the hinge row + int s3=3*info->rowskip; + info->J1a[s3+0] = q[0]; + info->J1a[s3+1] = q[1]; + info->J1a[s3+2] = q[2]; + if (joint->node[1].body) { + info->J2a[s3+0] = -q[0]; + info->J2a[s3+1] = -q[1]; + info->J2a[s3+2] = -q[2]; + } + + // compute the right hand side for the constrained rotational DOF. + // axis 1 and axis 2 are separated by an angle `theta'. the desired + // separation angle is theta0. sin(theta0) and cos(theta0) are recorded + // in the joint structure. the correcting angular velocity is: + // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize + // = (erp*fps) * (theta0-theta) + // (theta0-theta) can be computed using the following small-angle-difference + // approximation: + // theta0-theta ~= tan(theta0-theta) + // = sin(theta0-theta)/cos(theta0-theta) + // = (c*s0 - s*c0) / (c*c0 + s*s0) + // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 + // where c = cos(theta), s = sin(theta) + // c0 = cos(theta0), s0 = sin(theta0) + + dReal k = info->fps * info->erp; + info->c[3] = k * (joint->c0 * s - joint->s0 * c); + + // if the axis1 hinge is powered, or has joint limits, add in more stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the axis2 hinge is powered, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); + + // set parameter for the suspension + info->cfm[0] = joint->susp_cfm; +} + + +// compute vectors v1 and v2 (embedded in body1), used to measure angle +// between body 1 and body 2 + +static void makeHinge2V1andV2 (dxJointHinge2 *joint) +{ + if (joint->node[0].body) { + // get axis 1 and 2 in global coords + dVector3 ax1,ax2,v; + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); + + // don't do anything if the axis1 or axis2 vectors are zero or the same + if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || + (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || + (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; + + // modify axis 2 so it's perpendicular to axis 1 + dReal k = dDOT(ax1,ax2); + for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; + dNormalize3 (ax2); + + // make v1 = modified axis2, v2 = axis1 x (modified axis2) + dCROSS (v,=,ax1,ax2); + dMULTIPLY1_331 (joint->v1,joint->node[0].body->posr.R,ax2); + dMULTIPLY1_331 (joint->v2,joint->node[0].body->posr.R,v); + } +} + + +void dJointSetHinge2Anchor (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + makeHinge2V1andV2 (joint); +} + + +void dJointSetHinge2Axis1 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis1,joint->node[0].body->posr.R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +void dJointSetHinge2Axis2 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis2,joint->node[1].body->posr.R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +void dJointSetHinge2Param (dJointID j, int parameter, dReal value) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + if (parameter == dParamSuspensionERP) joint->susp_erp = value; + else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; + else joint->limot1.set (parameter,value); + } +} + + +void dJointGetHinge2Anchor (dJointID j, dVector3 result) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +void dJointGetHinge2Anchor2 (dJointID j, dVector3 result) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +void dJointGetHinge2Axis1 (dJointID j, dVector3 result) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis1); + } +} + + +void dJointGetHinge2Axis2 (dJointID j, dVector3 result) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis2); + } +} + + +dReal dJointGetHinge2Param (dJointID j, int parameter) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + if (parameter == dParamSuspensionERP) return joint->susp_erp; + else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; + else return joint->limot1.get (parameter); + } +} + + +dReal dJointGetHinge2Angle1 (dJointID j) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) return measureHinge2Angle (joint); + else return 0; +} + + +dReal dJointGetHinge2Angle1Rate (dJointID j) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +dReal dJointGetHinge2Angle2Rate (dJointID j) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body && joint->node[1].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[1].body->posr.R,joint->axis2); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +void dJointAddHinge2Torques (dJointID j, dReal torque1, dReal torque2) +{ + dxJointHinge2* joint = (dxJointHinge2*)j; + dVector3 axis1, axis2; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + + if (joint->node[0].body && joint->node[1].body) { + dMULTIPLY0_331 (axis1,joint->node[0].body->posr.R,joint->axis1); + dMULTIPLY0_331 (axis2,joint->node[1].body->posr.R,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); + } +} + + +dxJoint::Vtable __dhinge2_vtable = { + sizeof(dxJointHinge2), + (dxJoint::init_fn*) hinge2Init, + (dxJoint::getInfo1_fn*) hinge2GetInfo1, + (dxJoint::getInfo2_fn*) hinge2GetInfo2, + dJointTypeHinge2}; + +//**************************************************************************** +// universal + +// I just realized that the universal joint is equivalent to a hinge 2 joint with +// perfectly stiff suspension. By comparing the hinge 2 implementation to +// the universal implementation, you may be able to improve this +// implementation (or, less likely, the hinge2 implementation). + +static void universalInit (dxJointUniversal *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + dSetZero(j->qrel1,4); + dSetZero(j->qrel2,4); + j->limot1.init (j->world); + j->limot2.init (j->world); +} + + +static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) +{ + // This says "ax1 = joint->node[0].body->posr.R * joint->axis1" + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); + + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } +} + +static void getUniversalAngles(dxJointUniversal *joint, dReal *angle1, dReal *angle2) +{ + if (joint->node[0].body) + { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes (R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + + dRtoQ (R, qcross); + + + // This code is essentialy the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1 (qq, joint->node[0].body->q, qcross); + dQMultiply2 (qrel, qq, joint->qrel1); + + *angle1 = getHingeAngleFromRelativeQuat(qrel, joint->axis1); + + // This is equivalent to + // dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + // You see that the R is constructed from the same 2 axis as for angle1 + // but the first and second axis are swapped. + // So we can take the first R and rapply a rotation to it. + // The rotation is around the axis between the 2 axes (ax1 and ax2). + // We do a rotation of 180deg. + + dQuaternion qcross2; + // Find the vector between ax1 and ax2 (i.e. in the middle) + // We need to turn around this vector by 180deg + + // The 2 axes should be normalize so to find the vector between the 2. + // Add and devide by 2 then normalize or simply normalize + // ax2 + // ^ + // | + // | + /// *------------> ax1 + // We want the vector a 45deg + // + // N.B. We don't need to normalize the ax1 and ax2 since there are + // normalized when we set them. + + // We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z] + qrel[0] = 0; // equivalent to cos(Pi/2) + qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1 + qrel[2] = ax1[1] + ax2[1]; + qrel[3] = ax1[2] + ax2[2]; + + dReal l = dRecip(sqrt(qrel[1]*qrel[1] + qrel[2]*qrel[2] + qrel[3]*qrel[3])); + qrel[1] *= l; + qrel[2] *= l; + qrel[3] *= l; + + dQMultiply0 (qcross2, qrel, qcross); + + if (joint->node[1].body) { + dQMultiply1 (qq, joint->node[1].body->q, qcross2); + dQMultiply2 (qrel, qq, joint->qrel2); + } + else { + // pretend joint->node[1].body->q is the identity + dQMultiply2 (qrel, qcross2, joint->qrel2); + } + + *angle2 = - getHingeAngleFromRelativeQuat(qrel, joint->axis2); + + } + else + { + *angle1 = 0; + *angle2 = 0; + } +} + +static dReal getUniversalAngle1(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ (R,qcross); + + // This code is essential the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1 (qq,joint->node[0].body->q,qcross); + dQMultiply2 (qrel,qq,joint->qrel1); + + return getHingeAngleFromRelativeQuat(qrel, joint->axis1); + } + return 0; +} + + +static dReal getUniversalAngle2(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + + if (joint->node[1].body) { + dQMultiply1 (qq, joint->node[1].body->q, qcross); + dQMultiply2 (qrel,qq,joint->qrel2); + } + else { + // pretend joint->node[1].body->q is the identity + dQMultiply2 (qrel,qcross, joint->qrel2); + } + + return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); + } + return 0; +} + + +static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) +{ + info->nub = 4; + info->m = 4; + + // see if we're powered or at a joint limit. + bool constraint1 = j->limot1.fmax > 0; + bool constraint2 = j->limot2.fmax > 0; + + bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop; + bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && + j->limot2.lostop <= j->limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + if (limiting1 || limiting2) { + dReal angle1, angle2; + getUniversalAngles (j, &angle1, &angle2); + if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; + if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; + } + if (constraint1) + info->m++; + if (constraint2) + info->m++; +} + + +static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) +{ + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the universal joint row. the angular velocity about an axis + // perpendicular to both joint axes should be equal. thus the constraint + // equation is + // p*w1 - p*w2 = 0 + // where p is a vector normal to both joint axes, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dVector3 ax2_temp; + // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate + // about this. + dVector3 p; + dReal k; + + getUniversalAxes(joint, ax1, ax2); + k = dDOT(ax1, ax2); + ax2_temp[0] = ax2[0] - k*ax1[0]; + ax2_temp[1] = ax2[1] - k*ax1[1]; + ax2_temp[2] = ax2[2] - k*ax1[2]; + dCROSS(p, =, ax1, ax2_temp); + dNormalize3(p); + + int s3=3*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p to bring the axes back to perpendicular. + // If ax1, ax2 are unit length joint axes as computed from body1 and + // body2, we need to rotate both bodies along the axis p. If theta + // is the angle between ax1 and ax2, we need an angular velocity + // along p to cover the angle erp * (theta - Pi/2) in one step: + // + // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize + // = (erp*fps) * (theta - Pi/2) + // + // if theta is close to Pi/2, + // theta - Pi/2 ~= cos(theta), so + // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) + + info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); + + // if the first angle is powered, or has joint limits, add in the stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the second angle is powered, or has joint limits, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); +} + + +static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) +{ + if (joint->node[0].body) { + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross; + + getUniversalAxes(joint, ax1, ax2); + + // Axis 1. + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ(R, qcross); + dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); + + // Axis 2. + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + if (joint->node[1].body) { + dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); + } + else { + // set joint->qrel to qcross + for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; + } + } +} + + +void dJointSetUniversalAnchor (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + universalComputeInitialRelativeRotations(joint); +} + + +void dJointSetUniversalAxis1 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,NULL,joint->axis2); + else + setAxes (joint,x,y,z,joint->axis1,NULL); + universalComputeInitialRelativeRotations(joint); +} + + +void dJointSetUniversalAxis2 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,joint->axis1,NULL); + else + setAxes (joint,x,y,z,NULL,joint->axis2); + universalComputeInitialRelativeRotations(joint); +} + + +void dJointGetUniversalAnchor (dJointID j, dVector3 result) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +void dJointGetUniversalAnchor2 (dJointID j, dVector3 result) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +void dJointGetUniversalAxis1 (dJointID j, dVector3 result) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,result,joint->axis2); + else + getAxis (joint,result,joint->axis1); +} + + +void dJointGetUniversalAxis2 (dJointID j, dVector3 result) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,result,joint->axis1); + else + getAxis2 (joint,result,joint->axis2); +} + + +void dJointSetUniversalParam (dJointID j, int parameter, dReal value) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + joint->limot1.set (parameter,value); + } +} + + +dReal dJointGetUniversalParam (dJointID j, int parameter) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + return joint->limot1.get (parameter); + } +} + +void dJointGetUniversalAngles (dJointID j, dReal *angle1, dReal *angle2) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngles (joint, angle2, angle1); + else + return getUniversalAngles (joint, angle1, angle2); +} + + +dReal dJointGetUniversalAngle1 (dJointID j) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle2 (joint); + else + return getUniversalAngle1 (joint); +} + + +dReal dJointGetUniversalAngle2 (dJointID j) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle1 (joint); + else + return getUniversalAngle2 (joint); +} + + +dReal dJointGetUniversalAngle1Rate (dJointID j) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,axis,joint->axis2); + else + getAxis (joint,axis,joint->axis1); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +dReal dJointGetUniversalAngle2Rate (dJointID j) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,axis,joint->axis1); + else + getAxis2 (joint,axis,joint->axis2); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +void dJointAddUniversalTorques (dJointID j, dReal torque1, dReal torque2) +{ + dxJointUniversal* joint = (dxJointUniversal*)j; + dVector3 axis1, axis2; + dAASSERT(joint); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->flags & dJOINT_REVERSE) { + dReal temp = torque1; + torque1 = - torque2; + torque2 = - temp; + } + + getAxis (joint,axis1,joint->axis1); + getAxis2 (joint,axis2,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); +} + + + + + +dxJoint::Vtable __duniversal_vtable = { + sizeof(dxJointUniversal), + (dxJoint::init_fn*) universalInit, + (dxJoint::getInfo1_fn*) universalGetInfo1, + (dxJoint::getInfo2_fn*) universalGetInfo2, + dJointTypeUniversal}; + + + +//**************************************************************************** +// Prismatic and Rotoide + +static void PRInit (dxJointPR *j) +{ + // Default Position + // Z^ + // | Body 1 P R Body2 + // |+---------+ _ _ +-----------+ + // || |----|----(_)--------+ | + // |+---------+ - +-----------+ + // | + // X.-----------------------------------------> Y + // N.B. X is comming out of the page + dSetZero (j->anchor2,4); + + dSetZero (j->axisR1,4); + j->axisR1[0] = 1; + dSetZero (j->axisR2,4); + j->axisR2[0] = 1; + + dSetZero (j->axisP1,4); + j->axisP1[1] = 1; + dSetZero (j->qrel,4); + dSetZero (j->offset,4); + + j->limotR.init (j->world); + j->limotP.init (j->world); +} + + +dReal dJointGetPRPosition (dJointID j) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + + dVector3 q; + // get the offset in global coordinates + dMULTIPLY0_331 (q,joint->node[0].body->posr.R,joint->offset); + + if (joint->node[1].body) { + dVector3 anchor2; + + // get the anchor2 in global coordinates + dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2); + + q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) - + (joint->node[1].body->posr.pos[0] + anchor2[0]) ); + q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) - + (joint->node[1].body->posr.pos[1] + anchor2[1]) ); + q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) - + (joint->node[1].body->posr.pos[2] + anchor2[2]) ); + + } + else { + //N.B. When there is no body 2 the joint->anchor2 is already in + // global coordinates + + q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) - + (joint->anchor2[0]) ); + q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) - + (joint->anchor2[1]) ); + q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) - + (joint->anchor2[2]) ); + + } + + dVector3 axP; + // get prismatic axis in global coordinates + dMULTIPLY0_331 (axP,joint->node[0].body->posr.R,joint->axisP1); + + return dDOT(axP, q); +} + + +dReal dJointGetPRPositionRate (dJointID j) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + + if (joint->node[0].body) { + // We want to find the rate of change of the prismatic part of the joint + // We can find it by looking at the speed difference between body1 and the + // anchor point. + + // r will be used to find the distance between body1 and the anchor point + dVector3 r; + if (joint->node[1].body) { + // Find joint->anchor2 in global coordinates + dVector3 anchor2; + dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2); + + r[0] = joint->node[0].body->posr.pos[0] - anchor2[0]; + r[1] = joint->node[0].body->posr.pos[1] - anchor2[1]; + r[2] = joint->node[0].body->posr.pos[2] - anchor2[2]; + } + else { + //N.B. When there is no body 2 the joint->anchor2 is already in + // global coordinates + r[0] = joint->node[0].body->posr.pos[0] - joint->anchor2[0]; + r[1] = joint->node[0].body->posr.pos[1] - joint->anchor2[1]; + r[2] = joint->node[0].body->posr.pos[2] - joint->anchor2[2]; + } + + // The body1 can have velocity coming from the rotation of + // the rotoide axis. We need to remove this. + + // Take only the angular rotation coming from the rotation + // of the rotoide articulation + // N.B. Body1 and Body2 should have the same rotation along axis + // other than the rotoide axis. + dVector3 angular; + dMULTIPLY0_331 (angular,joint->node[0].body->posr.R,joint->axisR1); + dReal omega = dDOT(angular, joint->node[0].body->avel); + angular[0] *= omega; + angular[1] *= omega; + angular[2] *= omega; + + // Find the contribution of the angular rotation to the linear speed + // N.B. We do vel = r X w instead of vel = w x r to have vel negative + // since we want to remove it from the linear velocity of the body + dVector3 lvel1; + dCROSS(lvel1, =, r, angular); + + lvel1[0] += joint->node[0].body->lvel[0]; + lvel1[1] += joint->node[0].body->lvel[1]; + lvel1[2] += joint->node[0].body->lvel[2]; + + // Since we want rate of change along the prismatic axis + // get axisP1 in global coordinates and get the component + // along this axis only + dVector3 axP1; + dMULTIPLY0_331 (axP1,joint->node[0].body->posr.R,joint->axisP1); + return dDOT(axP1, lvel1); + } + + return 0.0; +} + + + +static void PRGetInfo1 (dxJointPR *j, dxJoint::Info1 *info) +{ + info->m = 4; + info->nub = 4; + + bool added = false; + + added = false; + // see if the prismatic articulation is powered + if (j->limotP.fmax > 0) + { + added = true; + (info->m)++; // powered needs an extra constraint row + } + + // see if we're at a joint limit. + j->limotP.limit = 0; + if ((j->limotP.lostop > -dInfinity || j->limotP.histop < dInfinity) && + j->limotP.lostop <= j->limotP.histop) { + // measure joint position + dReal pos = dJointGetPRPosition (j); + if (pos <= j->limotP.lostop) { + j->limotP.limit = 1; + j->limotP.limit_err = pos - j->limotP.lostop; + if (!added) + (info->m)++; + } + + if (pos >= j->limotP.histop) { + j->limotP.limit = 2; + j->limotP.limit_err = pos - j->limotP.histop; + if (!added) + (info->m)++; + } + } + +} + + + +static void PRGetInfo2 (dxJointPR *joint, dxJoint::Info2 *info) +{ + int s = info->rowskip; + int s2= 2*s; + int s3= 3*s; + int s4= 4*s; + + dReal k = info->fps * info->erp; + + + dVector3 q; // plane space of axP and after that axR + + // pull out pos and R for both bodies. also get the `connection' + // vector pos2-pos1. + + dReal *pos1,*pos2,*R1,*R2; + pos1 = joint->node[0].body->posr.pos; + R1 = joint->node[0].body->posr.R; + if (joint->node[1].body) { + pos2 = joint->node[1].body->posr.pos; + R2 = joint->node[1].body->posr.R; + } + else { + // pos2 = 0; // N.B. We can do that to be safe but it is no necessary + // R2 = 0; // N.B. We can do that to be safe but it is no necessary + } + + + dVector3 axP; // Axis of the prismatic joint in global frame + dMULTIPLY0_331 (axP, R1, joint->axisP1); + + // distance between the body1 and the anchor2 in global frame + // Calculated in the same way as the offset + dVector3 dist; + + if (joint->node[1].body) + { + dMULTIPLY0_331 (dist, R2, joint->anchor2); + dist[0] += pos2[0] - pos1[0]; + dist[1] += pos2[1] - pos1[1]; + dist[2] += pos2[2] - pos1[2]; + } + else { + dist[0] = joint->anchor2[0] - pos1[0]; + dist[1] = joint->anchor2[1] - pos1[1]; + dist[2] = joint->anchor2[2] - pos1[2]; + } + + + // ====================================================================== + // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered + + // Set the two rotoide rows. The rotoide axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the rotoide axis should be equal. Thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the rotoide axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + dVector3 ax1; + dMULTIPLY0_331 (ax1, joint->node[0].body->posr.R, joint->axisR1); + dCROSS(q , =, ax1, axP); + + info->J1a[0] = axP[0]; + info->J1a[1] = axP[1]; + info->J1a[2] = axP[2]; + info->J1a[s+0] = q[0]; + info->J1a[s+1] = q[1]; + info->J1a[s+2] = q[2]; + + if (joint->node[1].body) { + info->J2a[0] = -axP[0]; + info->J2a[1] = -axP[1]; + info->J2a[2] = -axP[2]; + info->J2a[s+0] = -q[0]; + info->J2a[s+1] = -q[1]; + info->J2a[s+2] = -q[2]; + } + + + // Compute the right hand side of the constraint equation set. Relative + // body velocities along p and q to bring the rotoide back into alignment. + // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame. + // We need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + + dVector3 ax2; + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2, R2, joint->axisR2); + } + else { + ax2[0] = joint->axisR2[0]; + ax2[1] = joint->axisR2[1]; + ax2[2] = joint->axisR2[2]; + } + + dVector3 b; + dCROSS (b,=,ax1, ax2); + info->c[0] = k * dDOT(b, axP); + info->c[1] = k * dDOT(b, q); + + + + // ========================== + // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered + // or 5 if rotoide and prismatic powered + + // two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the prismatic axis is disregarded. for symmetry we + // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + + // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist' + // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p + // v_p is speed of prismatic joint (i.e. elongation rate) + // Since the constraints are perpendicular to v_p we have: + // p dot v_p = 0 and q dot v_p = 0 + // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 ) + // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 ) + // == + // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist + // since a . (b x c) = - b . (a x c) = - (a x c) . b + // and a x b = - b x a + // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0 + // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0 + // Coeff for 1er line of: J1l => ax1, J2l => -ax1 + // Coeff for 2er line of: J1l => q, J2l => -q + // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1 + // Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q + + + dCROSS ((info->J1a)+s2, = , dist, ax1); + + dCROSS ((info->J1a)+s3, = , dist, q); + + + info->J1l[s2+0] = ax1[0]; + info->J1l[s2+1] = ax1[1]; + info->J1l[s2+2] = ax1[2]; + + info->J1l[s3+0] = q[0]; + info->J1l[s3+1] = q[1]; + info->J1l[s3+2] = q[2]; + + if (joint->node[1].body) { + dVector3 anchor2; + + // Calculate anchor2 in world coordinate + dMULTIPLY0_331 (anchor2, R2, joint->anchor2); + + // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value + dCROSS ((info->J2a)+s2, = , ax2, anchor2); // since ax1 == ax2 + + // The cross product is in reverse order since we want the negative value + dCROSS ((info->J2a)+s3, = , q, anchor2); + + info->J2l[s2+0] = -ax1[0]; + info->J2l[s2+1] = -ax1[1]; + info->J2l[s2+2] = -ax1[2]; + + info->J2l[s3+0] = -q[0]; + info->J2l[s3+1] = -q[1]; + info->J2l[s3+2] = -q[2]; + } + + + // We want to make correction for motion not in the line of the axisP + // We calculate the displacement w.r.t. the anchor pt. + // + // compute the elements 2 and 3 of right hand side. + // we want to align the offset point (in body 2's frame) with the center of body 1. + // The position should be the same when we are not along the prismatic axis + dVector3 err; + dMULTIPLY0_331 (err, R1, joint->offset); + err[0] += dist[0]; + err[1] += dist[1]; + err[2] += dist[2]; + info->c[2] = k * dDOT(ax1, err); + info->c[3] = k * dDOT(q, err); + + // Here we can't use addLimot because of some assumption in the function + int powered = joint->limotP.fmax > 0; + if (powered || joint->limotP.limit) { + info->J1l[s4+0] = axP[0]; + info->J1l[s4+1] = axP[1]; + info->J1l[s4+2] = axP[2]; + if (joint->node[1].body) { + info->J2l[s4+0] = -axP[0]; + info->J2l[s4+1] = -axP[1]; + info->J2l[s4+2] = -axP[2]; + } + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (joint->node[1].body) { + dVector3 c; + c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); + dReal val = dDOT(q, c); + c[0] -= val * c[0]; + c[1] -= val * c[1]; + c[2] -= val * c[2]; + + dCROSS (ltd,=,c,axP); + info->J1a[s4+0] = ltd[0]; + info->J1a[s4+1] = ltd[1]; + info->J1a[s4+2] = ltd[2]; + info->J2a[s4+0] = ltd[0]; + info->J2a[s4+1] = ltd[1]; + info->J2a[s4+2] = ltd[2]; + } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (joint->limotP.limit && (joint->limotP.lostop == joint->limotP.histop)) + powered = 0; + + int row = 4; + if (powered) { + info->cfm[row] = joint->limotP.normal_cfm; + if (!joint->limotP.limit) { + info->c[row] = joint->limotP.vel; + info->lo[row] = -joint->limotP.fmax; + info->hi[row] = joint->limotP.fmax; + } + else { + // the joint is at a limit, AND is being powered. if the joint is + // being powered into the limit then we apply the maximum motor force + // in that direction, because the motor is working against the + // immovable limit. if the joint is being powered away from the limit + // then we have problems because actually we need *two* lcp + // constraints to handle this case. so we fake it and apply some + // fraction of the maximum force. the fraction to use can be set as + // a fudge factor. + + dReal fm = joint->limotP.fmax; + dReal vel = joint->limotP.vel; + int limit = joint->limotP.limit; + if ((vel > 0) || (vel==0 && limit==2)) fm = -fm; + + // if we're powering away from the limit, apply the fudge factor + if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) + fm *= joint->limotP.fudge_factor; + + + dBodyAddForce (joint->node[0].body,-fm*axP[0],-fm*axP[1],-fm*axP[2]); + + if (joint->node[1].body) { + dBodyAddForce (joint->node[1].body,fm*axP[0],fm*axP[1],fm*axP[2]); + + // linear limot torque decoupling step: refer to above discussion + dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + } + } + } + + if (joint->limotP.limit) { + dReal k = info->fps * joint->limotP.stop_erp; + info->c[row] = -k * joint->limotP.limit_err; + info->cfm[row] = joint->limotP.stop_cfm; + + if (joint->limotP.lostop == joint->limotP.histop) { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else { + if (joint->limotP.limit == 1) { + // low limit + info->lo[row] = 0; + info->hi[row] = dInfinity; + } + else { + // high limit + info->lo[row] = -dInfinity; + info->hi[row] = 0; + } + + // deal with bounce + if (joint->limotP.bounce > 0) { + // calculate joint velocity + dReal vel; + vel = dDOT(joint->node[0].body->lvel, axP); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->lvel, axP); + + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (joint->limotP.limit == 1) { + // low limit + if (vel < 0) { + dReal newc = -joint->limotP.bounce * vel; + if (newc > info->c[row]) info->c[row] = newc; + } + } + else { + // high limit - all those computations are reversed + if (vel > 0) { + dReal newc = -joint->limotP.bounce * vel; + if (newc < info->c[row]) info->c[row] = newc; + } + } + } + } + } + } +} + + +// compute initial relative rotation body1 -> body2, or env -> body1 +static void PRComputeInitialRelativeRotation (dxJointPR *joint) +{ + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + } + else { + // set joint->qrel to the transpose of the first body q + joint->qrel[0] = joint->node[0].body->q[0]; + for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + // WARNING do we need the - in -joint->node[0].body->q[i]; or not + } + } +} + +void dJointSetPRAnchor (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + + dVector3 dummy; + setAnchors (joint,x,y,z,dummy,joint->anchor2); +} + + +void dJointSetPRAxis1 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + + setAxes (joint,x,y,z,joint->axisP1, 0); + + PRComputeInitialRelativeRotation (joint); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute distance between anchor of body1 w.r.t center of body 2 + dVector3 c; + if (joint->node[1].body) { + dVector3 anchor2; + dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R, joint->anchor2); + + c[0] = ( joint->node[1].body->posr.pos[0] + anchor2[0] - + joint->node[0].body->posr.pos[0] ); + c[1] = ( joint->node[1].body->posr.pos[1] + anchor2[1] - + joint->node[0].body->posr.pos[1] ); + c[2] = ( joint->node[1].body->posr.pos[2] + anchor2[2] - + joint->node[0].body->posr.pos[2] ); + } + else if (joint->node[0].body) { + c[0] = joint->anchor2[0] - joint->node[0].body->posr.pos[0]; + c[1] = joint->anchor2[1] - joint->node[0].body->posr.pos[1]; + c[2] = joint->anchor2[2] - joint->node[0].body->posr.pos[2]; + } + else + { + joint->offset[0] = joint->anchor2[0]; + joint->offset[1] = joint->anchor2[1]; + joint->offset[2] = joint->anchor2[2]; + + return; + } + + + dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,c); +} + + +void dJointSetPRAxis2 (dJointID j, dReal x, dReal y, dReal z) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + setAxes (joint,x,y,z,joint->axisR1,joint->axisR2); + PRComputeInitialRelativeRotation (joint); +} + + +void dJointSetPRParam (dJointID j, int parameter, dReal value) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + if ((parameter & 0xff00) == 0x100) { + joint->limotR.set (parameter,value); + } + else { + joint->limotP.set (parameter & 0xff,value); + } +} + +void dJointGetPRAnchor (dJointID j, dVector3 result) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + + if (joint->node[1].body) + getAnchor2 (joint,result,joint->anchor2); + else + { + result[0] = joint->anchor2[0]; + result[1] = joint->anchor2[1]; + result[2] = joint->anchor2[2]; + } + +} + +void dJointGetPRAxis1 (dJointID j, dVector3 result) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + getAxis(joint, result, joint->axisP1); +} + +void dJointGetPRAxis2 (dJointID j, dVector3 result) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + getAxis(joint, result, joint->axisR1); +} + +dReal dJointGetPRParam (dJointID j, int parameter) +{ + dxJointPR* joint = (dxJointPR*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not Prismatic and Rotoide"); + if ((parameter & 0xff00) == 0x100) { + return joint->limotR.get (parameter & 0xff); + } + else { + return joint->limotP.get (parameter); + } +} + +void dJointAddPRTorque (dJointID j, dReal torque) +{ + dxJointPR* joint = (dxJointPR*)j; + dVector3 axis; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide"); + + if (joint->flags & dJOINT_REVERSE) + torque = -torque; + + getAxis (joint,axis,joint->axisR1); + axis[0] *= torque; + axis[1] *= torque; + axis[2] *= torque; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dPR_vtable = { + sizeof(dxJointPR), + (dxJoint::init_fn*) PRInit, + (dxJoint::getInfo1_fn*) PRGetInfo1, + (dxJoint::getInfo2_fn*) PRGetInfo2, + dJointTypePR +}; + + +//**************************************************************************** +// angular motor + +static void amotorInit (dxJointAMotor *j) +{ + int i; + j->num = 0; + j->mode = dAMotorUser; + for (i=0; i<3; i++) { + j->rel[i] = 0; + dSetZero (j->axis[i],4); + j->limot[i].init (j->world); + j->angle[i] = 0; + } + dSetZero (j->reference1,4); + dSetZero (j->reference2,4); +} + + +// compute the 3 axes in global coordinates + +static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) +{ + if (joint->mode == dAMotorEuler) { + // special handling for euler mode + dMULTIPLY0_331 (ax[0],joint->node[0].body->posr.R,joint->axis[0]); + if (joint->node[1].body) { + dMULTIPLY0_331 (ax[2],joint->node[1].body->posr.R,joint->axis[2]); + } + else { + ax[2][0] = joint->axis[2][0]; + ax[2][1] = joint->axis[2][1]; + ax[2][2] = joint->axis[2][2]; + } + dCROSS (ax[1],=,ax[2],ax[0]); + dNormalize3 (ax[1]); + } + else { + for (int i=0; i < joint->num; i++) { + if (joint->rel[i] == 1) { + // relative to b1 + dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); + } + else if (joint->rel[i] == 2) { + // relative to b2 + if (joint->node[1].body) { // jds: don't assert, just ignore + dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); + } + } + else { + // global - just copy it + ax[i][0] = joint->axis[i][0]; + ax[i][1] = joint->axis[i][1]; + ax[i][2] = joint->axis[i][2]; + } + } + } +} + + +static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) +{ + // assumptions: + // global axes already calculated --> ax + // axis[0] is relative to body 1 --> global ax[0] + // axis[2] is relative to body 2 --> global ax[2] + // ax[1] = ax[2] x ax[0] + // original ax[0] and ax[2] are perpendicular + // reference1 is perpendicular to ax[0] (in body 1 frame) + // reference2 is perpendicular to ax[2] (in body 2 frame) + // all ax[] and reference vectors are unit length + + // calculate references in global frame + dVector3 ref1,ref2; + dMULTIPLY0_331 (ref1,joint->node[0].body->posr.R,joint->reference1); + if (joint->node[1].body) { + dMULTIPLY0_331 (ref2,joint->node[1].body->posr.R,joint->reference2); + } + else { + ref2[0] = joint->reference2[0]; + ref2[1] = joint->reference2[1]; + ref2[2] = joint->reference2[2]; + } + + // get q perpendicular to both ax[0] and ref1, get first euler angle + dVector3 q; + dCROSS (q,=,ax[0],ref1); + joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); + + // get q perpendicular to both ax[0] and ax[1], get second euler angle + dCROSS (q,=,ax[0],ax[1]); + joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); + + // get q perpendicular to both ax[1] and ax[2], get third euler angle + dCROSS (q,=,ax[1],ax[2]); + joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); +} + + +// set the reference vectors as follows: +// * reference1 = current axis[2] relative to body 1 +// * reference2 = current axis[0] relative to body 2 +// this assumes that: +// * axis[0] is relative to body 1 +// * axis[2] is relative to body 2 + +static void amotorSetEulerReferenceVectors (dxJointAMotor *j) +{ + if (j->node[0].body && j->node[1].body) { + dVector3 r; // axis[2] and axis[0] in global coordinates + dMULTIPLY0_331 (r,j->node[1].body->posr.R,j->axis[2]); + dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); + dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); + dMULTIPLY1_331 (j->reference2,j->node[1].body->posr.R,r); + } + + else { // jds + // else if (j->node[0].body) { + // dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]); + // dMULTIPLY0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]); + + // We want to handle angular motors attached to passive geoms + dVector3 r; // axis[2] and axis[0] in global coordinates + r[0] = j->axis[2][0]; r[1] = j->axis[2][1]; r[2] = j->axis[2][2]; r[3] = j->axis[2][3]; + dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); + dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); + j->reference2[0] += r[0]; j->reference2[1] += r[1]; + j->reference2[2] += r[2]; j->reference2[3] += r[3]; + } +} + + +static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; + + // compute the axes and angles, if in euler mode + if (j->mode == dAMotorEuler) { + dVector3 ax[3]; + amotorComputeGlobalAxes (j,ax); + amotorComputeEulerAngles (j,ax); + } + + // see if we're powered or at a joint limit for each axis + for (int i=0; i < j->num; i++) { + if (j->limot[i].testRotationalLimit (j->angle[i]) || + j->limot[i].fmax > 0) { + info->m++; + } + } +} + + +static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) +{ + int i; + + // compute the axes (if not global) + dVector3 ax[3]; + amotorComputeGlobalAxes (joint,ax); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + dVector3 *axptr[3]; + axptr[0] = &ax[0]; + axptr[1] = &ax[1]; + axptr[2] = &ax[2]; + + dVector3 ax0_cross_ax1; + dVector3 ax1_cross_ax2; + if (joint->mode == dAMotorEuler) { + dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); + axptr[2] = &ax0_cross_ax1; + dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); + axptr[0] = &ax1_cross_ax2; + } + + int row=0; + for (i=0; i < joint->num; i++) { + row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); + } +} + + +void dJointSetAMotorNumAxes (dJointID j, int num) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint && num >= 0 && num <= 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorEuler) { + joint->num = 3; + } + else { + if (num < 0) num = 0; + if (num > 3) num = 3; + joint->num = num; + } +} + + +void dJointSetAMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + dUASSERT(!(!joint->node[1].body && (joint->flags & dJOINT_REVERSE) && rel == 1),"no first body, can't set axis rel=1"); + dUASSERT(!(!joint->node[1].body && !(joint->flags & dJOINT_REVERSE) && rel == 2),"no second body, can't set axis rel=2"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + + // adjust rel to match the internal body order + if (!joint->node[1].body && rel==2) rel = 1; + + joint->rel[anum] = rel; + + // x,y,z is always in global coordinates regardless of rel, so we may have + // to convert it to be relative to a body + dVector3 r; + r[0] = x; + r[1] = y; + r[2] = z; + r[3] = 0; + if (rel > 0) { + if (rel==1) { + dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); + } + else { + // don't assert; handle the case of attachment to a bodiless geom + if (joint->node[1].body) { // jds + dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); + } + else { + joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3]; + } + } + } + else { + joint->axis[anum][0] = r[0]; + joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; + } + dNormalize3 (joint->axis[anum]); + if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); +} + + +void dJointSetAMotorAngle (dJointID j, int anum, dReal angle) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorUser) { + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + joint->angle[anum] = angle; + } +} + + +void dJointSetAMotorParam (dJointID j, int parameter, dReal value) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + joint->limot[anum].set (parameter, value); +} + + +void dJointSetAMotorMode (dJointID j, int mode) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + joint->mode = mode; + if (joint->mode == dAMotorEuler) { + joint->num = 3; + amotorSetEulerReferenceVectors (joint); + } +} + + +int dJointGetAMotorNumAxes (dJointID j) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->num; +} + + +void dJointGetAMotorAxis (dJointID j, int anum, dVector3 result) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + if (joint->rel[anum] > 0) { + if (joint->rel[anum]==1) { + dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis[anum]); + } + else { + if (joint->node[1].body) { // jds + dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis[anum]); + } + else { + result[0] = joint->axis[anum][0]; result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; result[3] = joint->axis[anum][3]; + } + } + } + else { + result[0] = joint->axis[anum][0]; + result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; + } +} + + +int dJointGetAMotorAxisRel (dJointID j, int anum) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + return joint->rel[anum]; +} + + +dReal dJointGetAMotorAngle (dJointID j, int anum) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + return joint->angle[anum]; +} + + +dReal dJointGetAMotorAngleRate (dJointID j, int anum) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + // @@@ + dDebug (0,"not yet implemented"); + return 0; +} + + +dReal dJointGetAMotorParam (dJointID j, int parameter) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + return joint->limot[anum].get (parameter); +} + + +int dJointGetAMotorMode (dJointID j) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->mode; +} + + +void dJointAddAMotorTorques (dJointID j, dReal torque1, dReal torque2, dReal torque3) +{ + dxJointAMotor* joint = (dxJointAMotor*)j; + dVector3 axes[3]; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + + if (joint->num == 0) + return; + dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); + + amotorComputeGlobalAxes (joint,axes); + axes[0][0] *= torque1; + axes[0][1] *= torque1; + axes[0][2] *= torque1; + if (joint->num >= 2) { + axes[0][0] += axes[1][0] * torque2; + axes[0][1] += axes[1][1] * torque2; + axes[0][2] += axes[1][2] * torque2; + if (joint->num >= 3) { + axes[0][0] += axes[2][0] * torque3; + axes[0][1] += axes[2][1] * torque3; + axes[0][2] += axes[2][2] * torque3; + } + } + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); +} + + +dxJoint::Vtable __damotor_vtable = { + sizeof(dxJointAMotor), + (dxJoint::init_fn*) amotorInit, + (dxJoint::getInfo1_fn*) amotorGetInfo1, + (dxJoint::getInfo2_fn*) amotorGetInfo2, + dJointTypeAMotor}; + + + +//**************************************************************************** +// lmotor joint +static void lmotorInit (dxJointLMotor *j) +{ + int i; + j->num = 0; + for (i=0;i<3;i++) { + dSetZero(j->axis[i],4); + j->limot[i].init(j->world); + } +} + +static void lmotorComputeGlobalAxes (dxJointLMotor *joint, dVector3 ax[3]) +{ + for (int i=0; i< joint->num; i++) { + if (joint->rel[i] == 1) { + dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); + } + else if (joint->rel[i] == 2) { + if (joint->node[1].body) { // jds: don't assert, just ignore + dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); + } + } else { + ax[i][0] = joint->axis[i][0]; + ax[i][1] = joint->axis[i][1]; + ax[i][2] = joint->axis[i][2]; + } + } +} + +static void lmotorGetInfo1 (dxJointLMotor *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; + for (int i=0; i < j->num; i++) { + if (j->limot[i].fmax > 0) { + info->m++; + } + } +} + +static void lmotorGetInfo2 (dxJointLMotor *joint, dxJoint::Info2 *info) +{ + int row=0; + dVector3 ax[3]; + lmotorComputeGlobalAxes(joint, ax); + + for (int i=0;inum;i++) { + row += joint->limot[i].addLimot(joint,info,row,ax[i], 0); + } +} + +void dJointSetLMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) +{ + dxJointLMotor* joint = (dxJointLMotor*)j; +//for now we are ignoring rel! + dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); + dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + + if (!joint->node[1].body && rel==2) rel = 1; //ref 1 + + joint->rel[anum] = rel; + + dVector3 r; + r[0] = x; + r[1] = y; + r[2] = z; + r[3] = 0; + if (rel > 0) { + if (rel==1) { + dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); + } else { + //second body has to exists thanks to ref 1 line + dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); + } + } else { + joint->axis[anum][0] = r[0]; + joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; + } + + dNormalize3 (joint->axis[anum]); +} + +void dJointSetLMotorNumAxes (dJointID j, int num) +{ + dxJointLMotor* joint = (dxJointLMotor*)j; + dAASSERT(joint && num >= 0 && num <= 3); + dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); + if (num < 0) num = 0; + if (num > 3) num = 3; + joint->num = num; +} + +void dJointSetLMotorParam (dJointID j, int parameter, dReal value) +{ + dxJointLMotor* joint = (dxJointLMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + joint->limot[anum].set (parameter, value); +} + +int dJointGetLMotorNumAxes (dJointID j) +{ + dxJointLMotor* joint = (dxJointLMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); + return joint->num; +} + + +void dJointGetLMotorAxis (dJointID j, int anum, dVector3 result) +{ + dxJointLMotor* joint = (dxJointLMotor*)j; + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + result[0] = joint->axis[anum][0]; + result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; +} + +dReal dJointGetLMotorParam (dJointID j, int parameter) +{ + dxJointLMotor* joint = (dxJointLMotor*)j; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + return joint->limot[anum].get (parameter); +} + +dxJoint::Vtable __dlmotor_vtable = { + sizeof(dxJointLMotor), + (dxJoint::init_fn*) lmotorInit, + (dxJoint::getInfo1_fn*) lmotorGetInfo1, + (dxJoint::getInfo2_fn*) lmotorGetInfo2, + dJointTypeLMotor +}; + + +//**************************************************************************** +// fixed joint + +static void fixedInit (dxJointFixed *j) +{ + dSetZero (j->offset,4); + dSetZero (j->qrel,4); + j->erp = j->world->global_erp; + j->cfm = j->world->global_cfm; +} + + +static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) +{ + info->m = 6; + info->nub = 6; +} + + +static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) +{ + int s = info->rowskip; + + // Three rows for orientation + setFixedOrientation(joint, info, joint->qrel, 3); + + // Three rows for position. + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + info->erp = joint->erp; + info->cfm[0] = joint->cfm; + info->cfm[1] = joint->cfm; + info->cfm[2] = joint->cfm; + + dVector3 ofs; + dMULTIPLY0_331 (ofs,joint->node[0].body->posr.R,joint->offset); + if (joint->node[1].body) { + dCROSSMAT (info->J1a,ofs,s,+,-); + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + } + + // set right hand side for the first three rows (linear) + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->node[1].body->posr.pos[j] - + joint->node[0].body->posr.pos[j] + ofs[j]); + } + else { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->offset[j] - joint->node[0].body->posr.pos[j]); + } +} + + +void dJointSetFixed (dJointID j) +{ + dxJointFixed* joint = (dxJointFixed*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); + int i; + + // This code is taken from sJointSetSliderAxis(), we should really put the + // common code in its own function. + // compute the offset between the bodies + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dReal ofs[4]; + for (i=0; i<4; i++) ofs[i] = joint->node[0].body->posr.pos[i]; + for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->posr.pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,ofs); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; + } + } +} + +void dxJointFixed::set (int num, dReal value) +{ + switch (num) { + case dParamCFM: + cfm = value; + break; + case dParamERP: + erp = value; + break; + } +} + + +dReal dxJointFixed::get (int num) +{ + switch (num) { + case dParamCFM: + return cfm; + case dParamERP: + return erp; + default: + return 0; + } +} + + +void dJointSetFixedParam (dJointID j, int parameter, dReal value) +{ + dxJointFixed* joint = (dxJointFixed*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed joint"); + joint->set (parameter,value); +} + + +dReal dJointGetFixedParam (dJointID j, int parameter) +{ + dxJointFixed* joint = (dxJointFixed*)j; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed joint"); + return joint->get (parameter); +} + + +dxJoint::Vtable __dfixed_vtable = { + sizeof(dxJointFixed), + (dxJoint::init_fn*) fixedInit, + (dxJoint::getInfo1_fn*) fixedGetInfo1, + (dxJoint::getInfo2_fn*) fixedGetInfo2, + dJointTypeFixed}; + +//**************************************************************************** +// null joint + +static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; +} + + +static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) +{ + dDebug (0,"this should never get called"); +} + + +dxJoint::Vtable __dnull_vtable = { + sizeof(dxJointNull), + (dxJoint::init_fn*) 0, + (dxJoint::getInfo1_fn*) nullGetInfo1, + (dxJoint::getInfo2_fn*) nullGetInfo2, + dJointTypeNull}; + + + + +/* + This code is part of the Plane2D ODE joint + by psero@gmx.de + Wed Apr 23 18:53:43 CEST 2003 + + Add this code to the file: ode/src/joint.cpp +*/ + + +# define VoXYZ(v1, o1, x, y, z) \ + ( \ + (v1)[0] o1 (x), \ + (v1)[1] o1 (y), \ + (v1)[2] o1 (z) \ + ) + +static dReal Midentity[3][3] = + { + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1, } + }; + + + +static void plane2dInit (dxJointPlane2D *j) +/*********************************************/ +{ + /* MINFO ("plane2dInit ()"); */ + j->motor_x.init (j->world); + j->motor_y.init (j->world); + j->motor_angle.init (j->world); +} + + + +static void plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info) +/***********************************************************************/ +{ + /* MINFO ("plane2dGetInfo1 ()"); */ + + info->nub = 3; + info->m = 3; + + if (j->motor_x.fmax > 0) + j->row_motor_x = info->m ++; + if (j->motor_y.fmax > 0) + j->row_motor_y = info->m ++; + if (j->motor_angle.fmax > 0) + j->row_motor_angle = info->m ++; +} + + + +static void plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info) +/***************************************************************************/ +{ + int r0 = 0, + r1 = info->rowskip, + r2 = 2 * r1; + dReal eps = info->fps * info->erp; + + /* MINFO ("plane2dGetInfo2 ()"); */ + +/* + v = v1, w = omega1 + (v2, omega2 not important (== static environment)) + + constraint equations: + xz = 0 + wx = 0 + wy = 0 + + <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 ) + ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 ) + ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 ) + J1/J1l Omega1/J1a +*/ + + // fill in linear and angular coeff. for left hand side: + + VoXYZ (&info->J1l[r0], =, 0, 0, 1); + VoXYZ (&info->J1l[r1], =, 0, 0, 0); + VoXYZ (&info->J1l[r2], =, 0, 0, 0); + + VoXYZ (&info->J1a[r0], =, 0, 0, 0); + VoXYZ (&info->J1a[r1], =, 1, 0, 0); + VoXYZ (&info->J1a[r2], =, 0, 1, 0); + + // error correction (against drift): + + // a) linear vz, so that z (== pos[2]) == 0 + info->c[0] = eps * -joint->node[0].body->posr.pos[2]; + +# if 0 + // b) angular correction? -> left to application !!! + dReal *body_z_axis = &joint->node[0].body->R[8]; + info->c[1] = eps * +atan2 (body_z_axis[1], body_z_axis[2]); // wx error + info->c[2] = eps * -atan2 (body_z_axis[0], body_z_axis[2]); // wy error +# endif + + // if the slider is powered, or has joint limits, add in the extra row: + + if (joint->row_motor_x > 0) + joint->motor_x.addLimot ( + joint, info, joint->row_motor_x, Midentity[0], 0); + + if (joint->row_motor_y > 0) + joint->motor_y.addLimot ( + joint, info, joint->row_motor_y, Midentity[1], 0); + + if (joint->row_motor_angle > 0) + joint->motor_angle.addLimot ( + joint, info, joint->row_motor_angle, Midentity[2], 1); +} + + + +dxJoint::Vtable __dplane2d_vtable = +{ + sizeof (dxJointPlane2D), + (dxJoint::init_fn*) plane2dInit, + (dxJoint::getInfo1_fn*) plane2dGetInfo1, + (dxJoint::getInfo2_fn*) plane2dGetInfo2, + dJointTypePlane2D +}; + + +void dJointSetPlane2DXParam (dxJoint *joint, + int parameter, dReal value) +{ + dUASSERT (joint, "bad joint argument"); + dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); + dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); + joint2d->motor_x.set (parameter, value); +} + + +void dJointSetPlane2DYParam (dxJoint *joint, + int parameter, dReal value) +{ + dUASSERT (joint, "bad joint argument"); + dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); + dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); + joint2d->motor_y.set (parameter, value); +} + + + +void dJointSetPlane2DAngleParam (dxJoint *joint, + int parameter, dReal value) +{ + dUASSERT (joint, "bad joint argument"); + dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); + dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); + joint2d->motor_angle.set (parameter, value); +} + + + diff --git a/libraries/ode-0.9/ode/src/joint.h b/libraries/ode-0.9/ode/src/joint.h new file mode 100644 index 0000000..27c013e --- /dev/null +++ b/libraries/ode-0.9/ode/src/joint.h @@ -0,0 +1,346 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_H_ +#define _ODE_JOINT_H_ + + +#include "objects.h" +#include +#include "obstack.h" + + +// joint flags +enum { + // if this flag is set, the joint was allocated in a joint group + dJOINT_INGROUP = 1, + + // if this flag is set, the joint was attached with arguments (0,body). + // our convention is to treat all attaches as (body,0), i.e. so node[0].body + // is always nonzero, so this flag records the fact that the arguments were + // swapped. + dJOINT_REVERSE = 2, + + // if this flag is set, the joint can not have just one body attached to it, + // it must have either zero or two bodies attached. + dJOINT_TWOBODIES = 4 +}; + + +// there are two of these nodes in the joint, one for each connection to a +// body. these are node of a linked list kept by each body of it's connecting +// joints. but note that the body pointer in each node points to the body that +// makes use of the *other* node, not this node. this trick makes it a bit +// easier to traverse the body/joint graph. + +struct dxJointNode { + dxJoint *joint; // pointer to enclosing dxJoint object + dxBody *body; // *other* body this joint is connected to + dxJointNode *next; // next node in body's list of connected joints +}; + + +struct dxJoint : public dObject { + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + struct Info1 { + int m,nub; + }; + + // info returned by getInfo2 function + + struct Info2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + dReal fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + dReal *J1l,*J1a,*J2l,*J2a; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + dReal *c,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + dReal *lo,*hi; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + // virtual function table: size of the joint structure, function pointers. + // we do it this way instead of using C++ virtual functions because + // sometimes we need to allocate joints ourself within a memory pool. + + typedef void init_fn (dxJoint *joint); + typedef void getInfo1_fn (dxJoint *joint, Info1 *info); + typedef void getInfo2_fn (dxJoint *joint, Info2 *info); + struct Vtable { + int size; + init_fn *init; + getInfo1_fn *getInfo1; + getInfo2_fn *getInfo2; + int typenum; // a dJointTypeXXX type number + }; + + Vtable *vtable; // virtual function table + int flags; // dJOINT_xxx flags + dxJointNode node[2]; // connections to bodies. node[1].body can be 0 + dJointFeedback *feedback; // optional feedback structure + dReal lambda[6]; // lambda generated by last step +}; + + +// joint group. NOTE: any joints in the group that have their world destroyed +// will have their world pointer set to 0. + +struct dxJointGroup : public dBase { + int num; // number of joints on the stack + dObStack stack; // a stack of (possibly differently sized) dxJoint +}; // objects. + + +// common limit and motor information for a single joint axis of movement +struct dxJointLimitMotor { + dReal vel,fmax; // powered joint: velocity, max force + dReal lostop,histop; // joint limits, relative to initial position + dReal fudge_factor; // when powering away from joint limits + dReal normal_cfm; // cfm to use when not at a stop + dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit + dReal bounce; // restitution factor + // variables used between getInfo1() and getInfo2() + int limit; // 0=free, 1=at lo limit, 2=at hi limit + dReal limit_err; // if at limit, amount over limit + + void init (dxWorld *); + void set (int num, dReal value); + dReal get (int num); + int testRotationalLimit (dReal angle); + int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational); +}; + + +// ball and socket + +struct dxJointBall : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dReal erp; // error reduction + dReal cfm; // constraint force mix in + void set (int num, dReal value); + dReal get (int num); +}; +extern struct dxJoint::Vtable __dball_vtable; + + +// hinge + +struct dxJointHinge : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dhinge_vtable; + + +// universal + +struct dxJointUniversal : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece + dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 + dxJointLimitMotor limot1; // limit and motor information for axis1 + dxJointLimitMotor limot2; // limit and motor information for axis2 +}; +extern struct dxJoint::Vtable __duniversal_vtable; + + +/** + * The axisP must be perpendicular to axis2 + * 
+ *                                        +-------------+
+ *                                        |      x      |
+ *                                        +------------\+
+ * Prismatic articulation                   ..     ..
+ *                       |                ..     ..
+ *                      \/              ..      ..
+ * +--------------+    --|        __..      ..  anchor2
+ * |      x       | .....|.......(__)     ..
+ * +--------------+    --|         ^     <
+ *        |----------------------->|
+ *            Offset               |--- Rotoide articulation
+ *
+ */ +struct dxJointPR : public dxJoint { + + dVector3 anchor2; ///< @brief Position of the rotoide articulation + ///< w.r.t second body. + ///< @note Position of body 2 in world frame + + ///< anchor2 in world frame give the position + ///< of the rotoide articulation + dVector3 axisR1; ///< axis of the rotoide articulation w.r.t first body. + ///< @note This is considered as axis1 from the parameter + ///< view. + dVector3 axisR2; ///< axis of the rotoide articulation w.r.t second body. + ///< @note This is considered also as axis1 from the + ///< parameter view + dVector3 axisP1; ///< axis for the prismatic articulation w.r.t first body. + ///< @note This is considered as axis2 in from the parameter + ///< view + dQuaternion qrel; ///< initial relative rotation body1 -> body2. + dVector3 offset; ///< @brief vector between the body1 and the rotoide + ///< articulation. + ///< + ///< Going from the first to the second in the frame + ///< of body1. + ///< That should be aligned with body1 center along axisP + ///< This is calculated whe the axis are set. + dxJointLimitMotor limotR; ///< limit and motor information for the rotoide articulation. + dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation. +}; +extern struct dxJoint::Vtable __dPR_vtable; + + + +// slider. if body2 is 0 then qrel is the absolute rotation of body1 and +// offset is the position of body1 center along axis1. + +struct dxJointSlider : public dxJoint { + dVector3 axis1; // axis w.r.t first body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // point relative to body2 that should be + // aligned with body1 center along axis1 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dslider_vtable; + + +// contact + +struct dxJointContact : public dxJoint { + int the_m; // number of rows computed by getInfo1 + dContact contact; +}; +extern struct dxJoint::Vtable __dcontact_vtable; + + +// hinge 2 + +struct dxJointHinge2 : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis 1 w.r.t first body + dVector3 axis2; // axis 2 w.r.t second body + dReal c0,s0; // cos,sin of desired angle between axis 1,2 + dVector3 v1,v2; // angle ref vectors embedded in first body + dxJointLimitMotor limot1; // limit+motor info for axis 1 + dxJointLimitMotor limot2; // limit+motor info for axis 2 + dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) +}; +extern struct dxJoint::Vtable __dhinge2_vtable; + + +// angular motor + +struct dxJointAMotor : public dxJoint { + int num; // number of axes (0..3) + int mode; // a dAMotorXXX constant + int rel[3]; // what the axes are relative to (global,b1,b2) + dVector3 axis[3]; // three axes + dxJointLimitMotor limot[3]; // limit+motor info for axes + dReal angle[3]; // user-supplied angles for axes + // these vectors are used for calculating euler angles + dVector3 reference1; // original axis[2], relative to body 1 + dVector3 reference2; // original axis[0], relative to body 2 +}; +extern struct dxJoint::Vtable __damotor_vtable; + + +struct dxJointLMotor : public dxJoint { + int num; + int rel[3]; + dVector3 axis[3]; + dxJointLimitMotor limot[3]; +}; + +extern struct dxJoint::Vtable __dlmotor_vtable; + + +// 2d joint, constrains to z == 0 + +struct dxJointPlane2D : public dxJoint +{ + int row_motor_x; + int row_motor_y; + int row_motor_angle; + dxJointLimitMotor motor_x; + dxJointLimitMotor motor_y; + dxJointLimitMotor motor_angle; +}; + +extern struct dxJoint::Vtable __dplane2d_vtable; + + +// fixed + +struct dxJointFixed : public dxJoint { + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // relative offset between the bodies + dReal erp; // error reduction parameter + dReal cfm; // constraint force mix-in + void set (int num, dReal value); + dReal get (int num); +}; +extern struct dxJoint::Vtable __dfixed_vtable; + + +// null joint, for testing only + +struct dxJointNull : public dxJoint { +}; +extern struct dxJoint::Vtable __dnull_vtable; + + +#endif diff --git a/libraries/ode-0.9/ode/src/lcp.cpp b/libraries/ode-0.9/ode/src/lcp.cpp new file mode 100644 index 0000000..d03d3e8 --- /dev/null +++ b/libraries/ode-0.9/ode/src/lcp.cpp @@ -0,0 +1,2007 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + + +THE ALGORITHM +------------- + +solve A*x = b+w, with x and w subject to certain LCP conditions. +each x(i),w(i) must lie on one of the three line segments in the following +diagram. each line segment corresponds to one index set : + + w(i) + /|\ | : + | | : + | |i in N : + w>0 | |state[i]=0 : + | | : + | | : i in C + w=0 + +-----------------------+ + | : | + | : | + w<0 | : |i in N + | : |state[i]=1 + | : | + | : | + +-------|-----------|-----------|----------> x(i) + lo 0 hi + +the Dantzig algorithm proceeds as follows: + for i=1:n + * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or + negative towards the line. as this is done, the other (x(j),w(j)) + for j= 0. this makes the algorithm a bit +simpler, because the starting point for x(i),w(i) is always on the dotted +line x=0 and x will only ever increase in one direction, so it can only hit +two out of the three line segments. + + +NOTES +----- + +this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m". +the implementation is split into an LCP problem object (dLCP) and an LCP +driver function. most optimization occurs in the dLCP object. + +a naive implementation of the algorithm requires either a lot of data motion +or a lot of permutation-array lookup, because we are constantly re-ordering +rows and columns. to avoid this and make a more optimized algorithm, a +non-trivial data structure is used to represent the matrix A (this is +implemented in the fast version of the dLCP object). + +during execution of this algorithm, some indexes in A are clamped (set C), +some are non-clamped (set N), and some are "don't care" (where x=0). +A,x,b,w (and other problem vectors) are permuted such that the clamped +indexes are first, the unclamped indexes are next, and the don't-care +indexes are last. this permutation is recorded in the array `p'. +initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped, +the corresponding elements of p are swapped. + +because the C and N elements are grouped together in the rows of A, we can do +lots of work with a fast dot product function. if A,x,etc were not permuted +and we only had a permutation array, then those dot products would be much +slower as we would have a permutation array lookup in some inner loops. + +A is accessed through an array of row pointers, so that element (i,j) of the +permuted matrix is A[i][j]. this makes row swapping fast. for column swapping +we still have to actually move the data. + +during execution of this algorithm we maintain an L*D*L' factorization of +the clamped submatrix of A (call it `AC') which is the top left nC*nC +submatrix of A. there are two ways we could arrange the rows/columns in AC. + +(1) AC is always permuted such that L*D*L' = AC. this causes a problem + when a row/column is removed from C, because then all the rows/columns of A + between the deleted index and the end of C need to be rotated downward. + this results in a lot of data motion and slows things down. +(2) L*D*L' is actually a factorization of a *permutation* of AC (which is + itself a permutation of the underlying A). this is what we do - the + permutation is recorded in the vector C. call this permutation A[C,C]. + when a row/column is removed from C, all we have to do is swap two + rows/columns and manipulate C. + +*/ + +#include +#include "lcp.h" +#include +#include +#include "mat.h" // for testing +#include // for testing + +//*************************************************************************** +// code generation parameters + +// LCP debugging (mosty for fast dLCP) - this slows things down a lot +//#define DEBUG_LCP + +//#define dLCP_SLOW // use slow dLCP object +#define dLCP_FAST // use fast dLCP object + +// option 1 : matrix row pointers (less data copying) +#define ROWPTRS +#define ATYPE dReal ** +#define AROW(i) (A[i]) + +// option 2 : no matrix row pointers (slightly faster inner loops) +//#define NOROWPTRS +//#define ATYPE dReal * +//#define AROW(i) (A+(i)*nskip) + +// use protected, non-stack memory allocation system + +#ifdef dUSE_MALLOC_FOR_ALLOCA +extern unsigned int dMemoryFlag; + +#define ALLOCA(t,v,s) t* v = (t*) malloc(s) +#define UNALLOCA(t) free(t) + +#else + +#define ALLOCA(t,v,s) t* v =(t*)dALLOCA16(s) +#define UNALLOCA(t) /* nothing */ + +#endif + +#define NUB_OPTIMIZATIONS + +//*************************************************************************** + +// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of +// A is nskip. this only references and swaps the lower triangle. +// if `do_fast_row_swaps' is nonzero and row pointers are being used, then +// rows will be swapped by exchanging row pointers. otherwise the data will +// be copied. + +static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip, + int do_fast_row_swaps) +{ + int i; + dAASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && + nskip >= n && i1 < i2); + +# ifdef ROWPTRS + for (i=i1+1; i 0) { + memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal)); + memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal)); + memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal)); + } + for (i=i1+1; i0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && + i1 <= i2); + if (i1==i2) return; + swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) + return; +#endif + tmp = x[i1]; + x[i1] = x[i2]; + x[i2] = tmp; + tmp = b[i1]; + b[i1] = b[i2]; + b[i2] = tmp; + tmp = w[i1]; + w[i1] = w[i2]; + w[i2] = tmp; + tmp = lo[i1]; + lo[i1] = lo[i2]; + lo[i2] = tmp; + tmp = hi[i1]; + hi[i1] = hi[i2]; + hi[i2] = tmp; + tmpi = p[i1]; + p[i1] = p[i2]; + p[i2] = tmpi; + tmpi = state[i1]; + state[i1] = state[i2]; + state[i2] = tmpi; + if (findex) { + tmpi = findex[i1]; + findex[i1] = findex[i2]; + findex[i2] = tmpi; + } +} + + +// for debugging - check that L,d is the factorization of A[C,C]. +// A[C,C] has size nC*nC and leading dimension nskip. +// L has size nC*nC and leading dimension nskip. +// d has size nC. + +#ifdef DEBUG_LCP + +static void checkFactorization (ATYPE A, dReal *_L, dReal *_d, + int nC, int *C, int nskip) +{ + int i,j; + if (nC==0) return; + + // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C] + dMatrix A1 (nC,nC); + for (i=0; i 1e-8) + dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff); +} + +#endif + + +// for debugging + +#ifdef DEBUG_LCP + +static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C) +{ + int j,k; + dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n); + for (k=0; k= 0 && p[k] < i); + for (k=i; k C,N; // index sets + int last_i_for_solve1; // last i value given to solve1 + + dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, + dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, + dReal *_Dell, dReal *_ell, dReal *_tmp, + int *_state, int *_findex, int *_p, int *_C, dReal **Arows); + // the constructor is given an initial problem description (A,x,b,w) and + // space for other working data (which the caller may allocate on the stack). + // some of this data is specific to the fast dLCP implementation. + // the matrices A and L have size n*n, vectors have size n*1. + // A represents a symmetric matrix but only the lower triangle is valid. + // `nub' is the number of unbounded indexes at the start. all the indexes + // 0..nub-1 will be put into C. + + ~dLCP(); + + int getNub() { return nub; } + // return the value of `nub'. the constructor may want to change it, + // so the caller should find out its new value. + + // transfer functions: transfer index i to the given set (C or N). indexes + // less than `nub' can never be given. A,x,b,w,etc may be permuted by these + // functions, the caller must be robust to this. + + void transfer_i_to_C (int i); + // this assumes C and N span 1:i-1. this also assumes that solve1() has + // been recently called for the same i without any other transfer + // functions in between (thereby allowing some data reuse for the fast + // implementation). + void transfer_i_to_N (int i); + // this assumes C and N span 1:i-1. + void transfer_i_from_N_to_C (int i); + void transfer_i_from_C_to_N (int i); + + int numC(); + int numN(); + // return the number of indexes in set C/N + + int indexC (int i); + int indexN (int i); + // return index i in set C/N. + + // accessor and arithmetic functions. Aij translates as A(i,j), etc. + // make sure that only the lower triangle of A is ever referenced. + + dReal Aii (int i); + dReal AiC_times_qC (int i, dReal *q); + dReal AiN_times_qN (int i, dReal *q); // for all Nj + void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj + void pN_plusequals_ANi (dReal *p, int i, int sign=1); + // for all Nj. sign = +1,-1. assumes i > maximum index in N. + void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q); + void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj + void solve1 (dReal *a, int i, int dir=1, int only_transfer=0); + // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1. + // the fast version of this function computes some data that is needed by + // transfer_i_to_C(). if only_transfer is nonzero then this function + // *only* computes that data, it does not set a(C). + + void unpermute(); + // call this at the end of the LCP function. if the x/w values have been + // permuted then this will unscramble them. +}; + + +dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, + dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, + dReal *_Dell, dReal *_ell, dReal *_tmp, + int *_state, int *_findex, int *_p, int *_C, dReal **Arows) +{ + dUASSERT (_findex==0,"slow dLCP object does not support findex array"); + + n = _n; + nub = _nub; + Adata = _Adata; + A = 0; + x = _x; + b = _b; + w = _w; + lo = _lo; + hi = _hi; + nskip = dPAD(n); + dSetZero (x,n); + last_i_for_solve1 = -1; + + int i,j; + C.setSize (n); + N.setSize (n); + for (i=0; i0, put all indexes 0..nub-1 into C and solve for x + if (nub > 0) { + for (i=0; i= i) dDebug (0,"N assumption violated"); + if (sign > 0) { + for (k=0; k 0) { + for (ii=0; ii nub + if (nub < n) { + for (k=0; k<100; k++) { + int i1,i2; + do { + i1 = dRandInt(n-nub)+nub; + i2 = dRandInt(n-nub)+nub; + } + while (i1 > i2); + //printf ("--> %d %d\n",i1,i2); + swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0); + } + } + */ + + // permute the problem so that *all* the unbounded variables are at the + // start, i.e. look for unbounded variables not included in `nub'. we can + // potentially push up `nub' this way and get a bigger initial factorization. + // note that when we swap rows/cols here we must not just swap row pointers, + // as the initial factorization relies on the data being all in one chunk. + // variables that have findex >= 0 are *not* considered to be unbounded even + // if lo=-inf and hi=inf - this is because these limits may change during the + // solution process. + + for (k=nub; k= 0) continue; + if (lo[k]==-dInfinity && hi[k]==dInfinity) { + swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0); + nub++; + } + } + + // if there are unbounded variables at the start, factorize A up to that + // point and solve for x. this puts all indexes 0..nub-1 into C. + if (nub > 0) { + for (k=0; k nub such that all findex variables are at the end + if (findex) { + int num_at_end = 0; + for (k=n-1; k >= nub; k--) { + if (findex[k] >= 0) { + swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1); + num_at_end++; + } + } + } + + // print info about indexes + /* + for (k=0; k 0) { + // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) + for (j=0; j 0) { + dReal *aptr = AROW(i); +# ifdef NUB_OPTIMIZATIONS + // if nub>0, initial part of aptr unpermuted + for (j=0; j 0) { + for (int i=0; i 0) { + dReal *aptr = AROW(i); +# ifdef NUB_OPTIMIZATIONS + // if nub>0, initial part of aptr[] is guaranteed unpermuted + for (j=0; j 0) { + for (j=0; j0 && A && x && b && w && nub == 0); + + int i,k; + int nskip = dPAD(n); + ALLOCA (dReal,L,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (L == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,d,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (d == NULL) { + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_x,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_x == NULL) { + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_w,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_w == NULL) { + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,Dell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Dell == NULL) { + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,ell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ell == NULL) { + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,tmp,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (tmp == NULL) { + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal*,Arows,n*sizeof(dReal*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Arows == NULL) { + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,p,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (p == NULL) { + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,C,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (C == NULL) { + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,dummy,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dummy == NULL) { + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + + dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows); + nub = lcp.getNub(); + + for (i=0; i= 0) { + lcp.transfer_i_to_N (i); + } + else { + for (;;) { + // compute: delta_x(C) = -A(C,C)\A(C,i) + dSetZero (delta_x,n); + lcp.solve1 (delta_x,i); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + UNALLOCA(dummy); + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + return; + } +#endif + delta_x[i] = 1; + + // compute: delta_w = A*delta_x + dSetZero (delta_w,n); + lcp.pN_equals_ANC_times_qC (delta_w,delta_x); + lcp.pN_plusequals_ANi (delta_w,i); + delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i); + + // find index to switch + int si = i; // si = switch index + int si_in_N = 0; // set to 1 if si in N + dReal s = -w[i]/delta_w[i]; + + if (s <= 0) { + dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); + if (i < (n-1)) { + dSetZero (x+i,n-i); + dSetZero (w+i,n-i); + } + goto done; + } + + for (k=0; k < lcp.numN(); k++) { + if (delta_w[lcp.indexN(k)] < 0) { + dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)]; + if (s2 < s) { + s = s2; + si = lcp.indexN(k); + si_in_N = 1; + } + } + } + for (k=0; k < lcp.numC(); k++) { + if (delta_x[lcp.indexC(k)] < 0) { + dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)]; + if (s2 < s) { + s = s2; + si = lcp.indexC(k); + si_in_N = 0; + } + } + } + + // apply x = x + s * delta_x + lcp.pC_plusequals_s_times_qC (x,s,delta_x); + x[i] += s; + lcp.pN_plusequals_s_times_qN (w,s,delta_w); + w[i] += s * delta_w[i]; + + // switch indexes between sets if necessary + if (si==i) { + w[i] = 0; + lcp.transfer_i_to_C (i); + break; + } + if (si_in_N) { + w[si] = 0; + lcp.transfer_i_from_N_to_C (si); + } + else { + x[si] = 0; + lcp.transfer_i_from_C_to_N (si); + } + } + } + } + + done: + lcp.unpermute(); + + UNALLOCA (L); + UNALLOCA (d); + UNALLOCA (delta_x); + UNALLOCA (delta_w); + UNALLOCA (Dell); + UNALLOCA (ell); + UNALLOCA (tmp); + UNALLOCA (Arows); + UNALLOCA (p); + UNALLOCA (C); + UNALLOCA (dummy); +} + +//*************************************************************************** +// an optimized Dantzig LCP driver routine for the lo-hi LCP problem. + +void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, + dReal *w, int nub, dReal *lo, dReal *hi, int *findex) +{ + dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n); + + int i,k,hit_first_friction_index = 0; + int nskip = dPAD(n); + + // if all the variables are unbounded then we can just factor, solve, + // and return + if (nub >= n) { + dFactorLDLT (A,w,n,nskip); // use w for d + dSolveLDLT (A,w,b,n,nskip); + memcpy (x,b,n*sizeof(dReal)); + dSetZero (w,n); + + return; + } +# ifndef dNODEBUG + // check restrictions on lo and hi + for (k=0; k= 0); +# endif + ALLOCA (dReal,L,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (L == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,d,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (d == NULL) { + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_x,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_x == NULL) { + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_w,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_w == NULL) { + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,Dell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Dell == NULL) { + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,ell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ell == NULL) { + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal*,Arows,n*sizeof(dReal*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Arows == NULL) { + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,p,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (p == NULL) { + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,C,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (C == NULL) { + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + int dir; + dReal dirf; + + // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) + ALLOCA (int,state,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (state == NULL) { + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + // create LCP object. note that tmp is set to delta_w to save space, this + // optimization relies on knowledge of how tmp is used, so be careful! + dLCP *lcp=new dLCP(n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows); + nub = lcp->getNub(); + + // loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the + // LCP conditions then i is added to the appropriate index set. otherwise + // x(i),w(i) is driven either +ve or -ve to force it to the valid region. + // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. + // while driving x(i) we maintain the LCP conditions on the other variables + // 0..i-1. we do this by watching out for other x(i),w(i) values going + // outside the valid region, and then switching them between index sets + // when that happens. + + for (i=nub; i= 0) { + // un-permute x into delta_w, which is not being used at the moment + for (k=0; kAiC_times_qC (i,x) + lcp->AiN_times_qN (i,x) - b[i]; + + // if lo=hi=0 (which can happen for tangential friction when normals are + // 0) then the index will be assigned to set N with some state. however, + // set C's line has zero size, so the index will always remain in set N. + // with the "normal" switching logic, if w changed sign then the index + // would have to switch to set C and then back to set N with an inverted + // state. this is pointless, and also computationally expensive. to + // prevent this from happening, we use the rule that indexes with lo=hi=0 + // will never be checked for set changes. this means that the state for + // these indexes may be incorrect, but that doesn't matter. + + // see if x(i),w(i) is in a valid region + if (lo[i]==0 && w[i] >= 0) { + lcp->transfer_i_to_N (i); + state[i] = 0; + } + else if (hi[i]==0 && w[i] <= 0) { + lcp->transfer_i_to_N (i); + state[i] = 1; + } + else if (w[i]==0) { + // this is a degenerate case. by the time we get to this test we know + // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, + // and similarly that hi > 0. this means that the line segment + // corresponding to set C is at least finite in extent, and we are on it. + // NOTE: we must call lcp->solve1() before lcp->transfer_i_to_C() + lcp->solve1 (delta_x,i,0,1); + +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + UNALLOCA(state); + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + return; + } +#endif + + lcp->transfer_i_to_C (i); + } + else { + // we must push x(i) and w(i) + for (;;) { + // find direction to push on x(i) + if (w[i] <= 0) { + dir = 1; + dirf = REAL(1.0); + } + else { + dir = -1; + dirf = REAL(-1.0); + } + + // compute: delta_x(C) = -dir*A(C,C)\A(C,i) + lcp->solve1 (delta_x,i,dir); + +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + UNALLOCA(state); + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + return; + } +#endif + + // note that delta_x[i] = dirf, but we wont bother to set it + + // compute: delta_w = A*delta_x ... note we only care about + // delta_w(N) and delta_w(i), the rest is ignored + lcp->pN_equals_ANC_times_qC (delta_w,delta_x); + lcp->pN_plusequals_ANi (delta_w,i,dir); + delta_w[i] = lcp->AiC_times_qC (i,delta_x) + lcp->Aii(i)*dirf; + + // find largest step we can take (size=s), either to drive x(i),w(i) + // to the valid LCP region or to drive an already-valid variable + // outside the valid region. + + int cmd = 1; // index switching command + int si = 0; // si = index to switch if cmd>3 + dReal s = -w[i]/delta_w[i]; + if (dir > 0) { + if (hi[i] < dInfinity) { + dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i) + if (s2 < s) { + s = s2; + cmd = 3; + } + } + } + else { + if (lo[i] > -dInfinity) { + dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i) + if (s2 < s) { + s = s2; + cmd = 2; + } + } + } + + for (k=0; k < lcp->numN(); k++) { + if ((state[lcp->indexN(k)]==0 && delta_w[lcp->indexN(k)] < 0) || + (state[lcp->indexN(k)]!=0 && delta_w[lcp->indexN(k)] > 0)) { + // don't bother checking if lo=hi=0 + if (lo[lcp->indexN(k)] == 0 && hi[lcp->indexN(k)] == 0) continue; + dReal s2 = -w[lcp->indexN(k)] / delta_w[lcp->indexN(k)]; + if (s2 < s) { + s = s2; + cmd = 4; + si = lcp->indexN(k); + } + } + } + + for (k=nub; k < lcp->numC(); k++) { + if (delta_x[lcp->indexC(k)] < 0 && lo[lcp->indexC(k)] > -dInfinity) { + dReal s2 = (lo[lcp->indexC(k)]-x[lcp->indexC(k)]) / + delta_x[lcp->indexC(k)]; + if (s2 < s) { + s = s2; + cmd = 5; + si = lcp->indexC(k); + } + } + if (delta_x[lcp->indexC(k)] > 0 && hi[lcp->indexC(k)] < dInfinity) { + dReal s2 = (hi[lcp->indexC(k)]-x[lcp->indexC(k)]) / + delta_x[lcp->indexC(k)]; + if (s2 < s) { + s = s2; + cmd = 6; + si = lcp->indexC(k); + } + } + } + + //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", + // "C->NL","C->NH"}; + //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); + + // if s <= 0 then we've got a problem. if we just keep going then + // we're going to get stuck in an infinite loop. instead, just cross + // our fingers and exit with the current solution. + if (s <= 0) { + dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); + if (i < (n-1)) { + dSetZero (x+i,n-i); + dSetZero (w+i,n-i); + } + goto done; + } + + // apply x = x + s * delta_x + lcp->pC_plusequals_s_times_qC (x,s,delta_x); + x[i] += s * dirf; + + // apply w = w + s * delta_w + lcp->pN_plusequals_s_times_qN (w,s,delta_w); + w[i] += s * delta_w[i]; + + // switch indexes between sets if necessary + switch (cmd) { + case 1: // done + w[i] = 0; + lcp->transfer_i_to_C (i); + break; + case 2: // done + x[i] = lo[i]; + state[i] = 0; + lcp->transfer_i_to_N (i); + break; + case 3: // done + x[i] = hi[i]; + state[i] = 1; + lcp->transfer_i_to_N (i); + break; + case 4: // keep going + w[si] = 0; + lcp->transfer_i_from_N_to_C (si); + break; + case 5: // keep going + x[si] = lo[si]; + state[si] = 0; + lcp->transfer_i_from_C_to_N (si); + break; + case 6: // keep going + x[si] = hi[si]; + state[si] = 1; + lcp->transfer_i_from_C_to_N (si); + break; + } + + if (cmd <= 3) break; + } + } + } + + done: + lcp->unpermute(); + delete lcp; + + UNALLOCA (L); + UNALLOCA (d); + UNALLOCA (delta_x); + UNALLOCA (delta_w); + UNALLOCA (Dell); + UNALLOCA (ell); + UNALLOCA (Arows); + UNALLOCA (p); + UNALLOCA (C); + UNALLOCA (state); +} + +//*************************************************************************** +// accuracy and timing test + +extern "C" ODE_API void dTestSolveLCP() +{ + int n = 100; + int i,nskip = dPAD(n); +#ifdef dDOUBLE + const dReal tol = REAL(1e-9); +#endif +#ifdef dSINGLE + const dReal tol = REAL(1e-4); +#endif + printf ("dTestSolveLCP()\n"); + + ALLOCA (dReal,A,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,x,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (x == NULL) { + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,b,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (b == NULL) { + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,w,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (w == NULL) { + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,lo,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo == NULL) { + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,hi,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi == NULL) { + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + ALLOCA (dReal,A2,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A2 == NULL) { + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,b2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (b2 == NULL) { + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,lo2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo2 == NULL) { + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,hi2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi2 == NULL) { + UNALLOCA (lo2); + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,tmp1,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (tmp1 == NULL) { + UNALLOCA (hi2); + UNALLOCA (lo2); + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,tmp2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (tmp2 == NULL) { + UNALLOCA (tmp1); + UNALLOCA (hi2); + UNALLOCA (lo2); + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + double total_time = 0; + for (int count=0; count < 1000; count++) { + + // form (A,b) = a random positive definite LCP problem + dMakeRandomMatrix (A2,n,n,1.0); + dMultiply2 (A,A2,A2,n,n,n); + dMakeRandomMatrix (x,n,1,1.0); + dMultiply0 (b,A,x,n,n,1); + for (i=0; i tol ? "FAILED" : "passed"); + if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff); + int n1=0,n2=0,n3=0; + for (i=0; i= 0) { + n1++; // ok + } + else if (x[i]==hi[i] && w[i] <= 0) { + n2++; // ok + } + else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) { + n3++; // ok + } + else { + dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i, + x[i],w[i],lo[i],hi[i]); + } + } + + // pacifier + printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3); + printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average); + } + + UNALLOCA (A); + UNALLOCA (x); + UNALLOCA (b); + UNALLOCA (w); + UNALLOCA (lo); + UNALLOCA (hi); + UNALLOCA (A2); + UNALLOCA (b2); + UNALLOCA (lo2); + UNALLOCA (hi2); + UNALLOCA (tmp1); + UNALLOCA (tmp2); +} diff --git a/libraries/ode-0.9/ode/src/lcp.h b/libraries/ode-0.9/ode/src/lcp.h new file mode 100644 index 0000000..484902c --- /dev/null +++ b/libraries/ode-0.9/ode/src/lcp.h @@ -0,0 +1,58 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i) +satisfies one of + (1) x = lo, w >= 0 + (2) x = hi, w <= 0 + (3) lo < x < hi, w = 0 +A is a matrix of dimension n*n, everything else is a vector of size n*1. +lo and hi can be +/- dInfinity as needed. the first `nub' variables are +unbounded, i.e. hi and lo are assumed to be +/- dInfinity. + +we restrict lo(i) <= 0 and hi(i) >= 0. + +the original data (A,b) may be modified by this function. + +if the `findex' (friction index) parameter is nonzero, it points to an array +of index values. in this case constraints that have findex[i] >= 0 are +special. all non-special constraints are solved for, then the lo and hi values +for the special constraints are set: + hi[i] = abs( hi[i] * x[findex[i]] ) + lo[i] = -hi[i] +and the solution continues. this mechanism allows a friction approximation +to be implemented. the first `nub' variables are assumed to have findex < 0. + +*/ + + +#ifndef _ODE_LCP_H_ +#define _ODE_LCP_H_ + + +void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w, + int nub, dReal *lo, dReal *hi, int *findex); + + +#endif diff --git a/libraries/ode-0.9/ode/src/mass.cpp b/libraries/ode-0.9/ode/src/mass.cpp new file mode 100644 index 0000000..7d0b1c2 --- /dev/null +++ b/libraries/ode-0.9/ode/src/mass.cpp @@ -0,0 +1,529 @@ +/************************************************************************* + * * + * 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 + +// Local dependencies +#include "collision_kernel.h" + +#define SQR(x) ((x)*(x)) //!< Returns x square +#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube + +#define _I(i,j) I[(i)*4+(j)] + + +// return 1 if ok, 0 if bad + +int dMassCheck (const dMass *m) +{ + int i; + + if (m->mass <= 0) { + dDEBUGMSG ("mass must be > 0"); + return 0; + } + if (!dIsPositiveDefinite (m->I,3)) { + dDEBUGMSG ("inertia must be positive definite"); + return 0; + } + + // verify that the center of mass position is consistent with the mass + // and inertia matrix. this is done by checking that the inertia around + // the center of mass is also positive definite. from the comment in + // dMassTranslate(), if the body is translated so that its center of mass + // is at the point of reference, then the new inertia is: + // I + mass*crossmat(c)^2 + // note that requiring this to be positive definite is exactly equivalent + // to requiring that the spatial inertia matrix + // [ mass*eye(3,3) M*crossmat(c)^T ] + // [ M*crossmat(c) I ] + // is positive definite, given that I is PD and mass>0. see the theorem + // about partitioned PD matrices for proof. + + dMatrix3 I2,chat; + dSetZero (chat,12); + dCROSSMAT (chat,m->c,4,+,-); + dMULTIPLY0_333 (I2,chat,chat); + for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i]; + for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i]; + for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i]; + if (!dIsPositiveDefinite (I2,3)) { + dDEBUGMSG ("center of mass inconsistent with mass parameters"); + return 0; + } + return 1; +} + + +void dMassSetZero (dMass *m) +{ + dAASSERT (m); + m->mass = REAL(0.0); + dSetZero (m->c,sizeof(m->c) / sizeof(dReal)); + dSetZero (m->I,sizeof(m->I) / sizeof(dReal)); +} + + +void dMassSetParameters (dMass *m, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal I11, dReal I22, dReal I33, + dReal I12, dReal I13, dReal I23) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = themass; + m->c[0] = cgx; + m->c[1] = cgy; + m->c[2] = cgz; + m->_I(0,0) = I11; + m->_I(1,1) = I22; + m->_I(2,2) = I33; + m->_I(0,1) = I12; + m->_I(0,2) = I13; + m->_I(1,2) = I23; + m->_I(1,0) = I12; + m->_I(2,0) = I13; + m->_I(2,1) = I23; + dMassCheck (m); +} + + +void dMassSetSphere (dMass *m, dReal density, dReal radius) +{ + dMassSetSphereTotal (m, (REAL(4.0)/REAL(3.0)) * M_PI * + radius*radius*radius * density, radius); +} + + +void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = total_mass; + dReal II = REAL(0.4) * total_mass * radius*radius; + m->_I(0,0) = II; + m->_I(1,1) = II; + m->_I(2,2) = II; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetCapsule (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dReal M1,M2,Ia,Ib; + dAASSERT (m); + dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); + dMassSetZero (m); + M1 = M_PI*radius*radius*length*density; // cylinder mass + M2 = (REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density; // total cap mass + m->mass = M1+M2; + Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) + + M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length); + Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius; + m->_I(0,0) = Ia; + m->_I(1,1) = Ia; + m->_I(2,2) = Ia; + m->_I(direction-1,direction-1) = Ib; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction, + dReal a, dReal b) +{ + dMassSetCapsule (m, 1.0, direction, a, b); + dMassAdjust (m, total_mass); +} + + +void dMassSetCylinder (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dMassSetCylinderTotal (m, M_PI*radius*radius*length*density, + direction, radius, length); +} + +void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction, + dReal radius, dReal length) +{ + dReal r2,I; + dAASSERT (m); + dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); + dMassSetZero (m); + r2 = radius*radius; + m->mass = total_mass; + I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length); + m->_I(0,0) = I; + m->_I(1,1) = I; + m->_I(2,2) = I; + m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetBox (dMass *m, dReal density, + dReal lx, dReal ly, dReal lz) +{ + dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz); +} + + +void dMassSetBoxTotal (dMass *m, dReal total_mass, + dReal lx, dReal ly, dReal lz) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = total_mass; + m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz); + m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz); + m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly); + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + + + + + +#if dTRIMESH_ENABLED + +/* + * dMassSetTrimesh, implementation by Gero Mueller. + * Based on Brian Mirtich, "Fast and Accurate Computation of + * Polyhedral Mass Properties," journal of graphics tools, volume 1, + * number 2, 1996. +*/ +void dMassSetTrimesh( dMass *m, dReal density, dGeomID g ) +{ + dAASSERT (m); + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dMassSetZero (m); + + unsigned int triangles = dGeomTriMeshGetTriangleCount( g ); + + dReal nx, ny, nz; + unsigned int i, A, B, C; + // face integrals + dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; + + // projection integrals + dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; + + dReal T0 = 0; + dReal T1[3] = {0., 0., 0.}; + dReal T2[3] = {0., 0., 0.}; + dReal TP[3] = {0., 0., 0.}; + + for( i = 0; i < triangles; i++ ) + { + dVector3 v0, v1, v2; + dGeomTriMeshGetTriangle( g, i, &v0, &v1, &v2); + + dVector3 n, a, b; + dOP( a, -, v1, v0 ); + dOP( b, -, v2, v0 ); + dCROSS( n, =, b, a ); + nx = fabs(n[0]); + ny = fabs(n[1]); + nz = fabs(n[2]); + + if( nx > ny && nx > nz ) + C = 0; + else + C = (ny > nz) ? 1 : 2; + + A = (C + 1) % 3; + B = (A + 1) % 3; + + // calculate face integrals + { + dReal w; + dReal k1, k2, k3, k4; + + //compProjectionIntegrals(f); + { + dReal a0, a1, da; + dReal b0, b1, db; + dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; + dReal a1_2, a1_3, b1_2, b1_3; + dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; + dReal Cab, Kab, Caab, Kaab, Cabb, Kabb; + + P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; + + for( int j = 0; j < 3; j++) + { + switch(j) + { + case 0: + a0 = v0[A]; + b0 = v0[B]; + a1 = v1[A]; + b1 = v1[B]; + break; + case 1: + a0 = v1[A]; + b0 = v1[B]; + a1 = v2[A]; + b1 = v2[B]; + break; + case 2: + a0 = v2[A]; + b0 = v2[B]; + a1 = v0[A]; + b1 = v0[B]; + break; + } + da = a1 - a0; + db = b1 - b0; + a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; + b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; + a1_2 = a1 * a1; a1_3 = a1_2 * a1; + b1_2 = b1 * b1; b1_3 = b1_2 * b1; + + C1 = a1 + a0; + Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; + Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; + Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; + Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; + Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; + Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; + + P1 += db*C1; + Pa += db*Ca; + Paa += db*Caa; + Paaa += db*Caaa; + Pb += da*Cb; + Pbb += da*Cbb; + Pbbb += da*Cbbb; + Pab += db*(b1*Cab + b0*Kab); + Paab += db*(b1*Caab + b0*Kaab); + Pabb += da*(a1*Cabb + a0*Kabb); + } + + P1 /= 2.0; + Pa /= 6.0; + Paa /= 12.0; + Paaa /= 20.0; + Pb /= -6.0; + Pbb /= -12.0; + Pbbb /= -20.0; + Pab /= 24.0; + Paab /= 60.0; + Pabb /= -60.0; + } + + w = - dDOT(n, v0); + + k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; + + Fa = k1 * Pa; + Fb = k1 * Pb; + Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); + + Faa = k1 * Paa; + Fbb = k1 * Pbb; + Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faaa = k1 * Paaa; + Fbbb = k1 * Pbbb; + Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab + + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb + + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) + + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faab = k1 * Paab; + Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); + Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb + + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); + } + + + T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); + + T1[A] += n[A] * Faa; + T1[B] += n[B] * Fbb; + T1[C] += n[C] * Fcc; + T2[A] += n[A] * Faaa; + T2[B] += n[B] * Fbbb; + T2[C] += n[C] * Fccc; + TP[A] += n[A] * Faab; + TP[B] += n[B] * Fbbc; + TP[C] += n[C] * Fcca; + } + + T1[0] /= 2; T1[1] /= 2; T1[2] /= 2; + T2[0] /= 3; T2[1] /= 3; T2[2] /= 3; + TP[0] /= 2; TP[1] /= 2; TP[2] /= 2; + + m->mass = density * T0; + m->_I(0,0) = density * (T2[1] + T2[2]); + m->_I(1,1) = density * (T2[2] + T2[0]); + m->_I(2,2) = density * (T2[0] + T2[1]); + m->_I(0,1) = - density * TP[0]; + m->_I(1,0) = - density * TP[0]; + m->_I(2,1) = - density * TP[1]; + m->_I(1,2) = - density * TP[1]; + m->_I(2,0) = - density * TP[2]; + m->_I(0,2) = - density * TP[2]; + + // Added to address SF bug 1729095 + dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 ); + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassSetTrimeshTotal( dMass *m, dReal total_mass, dGeomID g) +{ + dAASSERT( m ); + dUASSERT( g && g->type == dTriMeshClass, "argument not a trimesh" ); + dMassSetTrimesh( m, 1.0, g ); + dMassAdjust( m, total_mass ); +} + +#endif // dTRIMESH_ENABLED + + + + +void dMassAdjust (dMass *m, dReal newmass) +{ + dAASSERT (m); + dReal scale = newmass / m->mass; + m->mass = newmass; + for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassTranslate (dMass *m, dReal x, dReal y, dReal z) +{ + // if the body is translated by `a' relative to its point of reference, + // the new inertia about the point of reference is: + // + // I + mass*(crossmat(c)^2 - crossmat(c+a)^2) + // + // where c is the existing center of mass and I is the old inertia. + + int i,j; + dMatrix3 ahat,chat,t1,t2; + dReal a[3]; + + dAASSERT (m); + + // adjust inertia matrix + dSetZero (chat,12); + dCROSSMAT (chat,m->c,4,+,-); + a[0] = x + m->c[0]; + a[1] = y + m->c[1]; + a[2] = z + m->c[2]; + dSetZero (ahat,12); + dCROSSMAT (ahat,a,4,+,-); + dMULTIPLY0_333 (t1,ahat,ahat); + dMULTIPLY0_333 (t2,chat,chat); + for (i=0; i<3; i++) for (j=0; j<3; j++) + m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]); + + // ensure perfect symmetry + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); + + // adjust center of mass + m->c[0] += x; + m->c[1] += y; + m->c[2] += z; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassRotate (dMass *m, const dMatrix3 R) +{ + // if the body is rotated by `R' relative to its point of reference, + // the new inertia about the point of reference is: + // + // R * I * R' + // + // where I is the old inertia. + + dMatrix3 t1; + dReal t2[3]; + + dAASSERT (m); + + // rotate inertia matrix + dMULTIPLY2_333 (t1,m->I,R); + dMULTIPLY0_333 (m->I,R,t1); + + // ensure perfect symmetry + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); + + // rotate center of mass + dMULTIPLY0_331 (t2,R,m->c); + m->c[0] = t2[0]; + m->c[1] = t2[1]; + m->c[2] = t2[2]; + +# ifndef dNODEBUG + dMassCheck (m); +# endif +} + + +void dMassAdd (dMass *a, const dMass *b) +{ + int i; + dAASSERT (a && b); + dReal denom = dRecip (a->mass + b->mass); + for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom; + a->mass += b->mass; + for (i=0; i<12; i++) a->I[i] += b->I[i]; +} diff --git a/libraries/ode-0.9/ode/src/mat.cpp b/libraries/ode-0.9/ode/src/mat.cpp new file mode 100644 index 0000000..6e635dc --- /dev/null +++ b/libraries/ode-0.9/ode/src/mat.cpp @@ -0,0 +1,230 @@ +/************************************************************************* + * * + * 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 +#include +#include "mat.h" + + +dMatrix::dMatrix() +{ + n = 0; + m = 0; + data = 0; +} + + +dMatrix::dMatrix (int rows, int cols) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + dSetZero (data,n*m); +} + + +dMatrix::dMatrix (const dMatrix &a) +{ + n = a.n; + m = a.m; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); +} + + +dMatrix::dMatrix (int rows, int cols, + dReal *_data, int rowskip, int colskip) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + for (int i=0; i= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)"); + return data [i*m+j]; +} + + +void dMatrix::operator= (const dMatrix &a) +{ + if (data) dFree (data,n*m*sizeof(dReal)); + n = a.n; + m = a.m; + if (n > 0 && m > 0) { + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); + } + else data = 0; +} + + +void dMatrix::operator= (dReal a) +{ + for (int i=0; i= n || q[i] < 0 || q[i] >= m) + dDebug (0,"Matrix select, bad index arrays"); + r.data[i*nq+j] = data[p[i]*m+q[j]]; + } + } + return r; +} + + +dMatrix dMatrix::operator + (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes"); + dMatrix r (n,m); + for (int i=0; i max) max = diff; + } + } + return max; +} diff --git a/libraries/ode-0.9/ode/src/mat.h b/libraries/ode-0.9/ode/src/mat.h new file mode 100644 index 0000000..2814a01 --- /dev/null +++ b/libraries/ode-0.9/ode/src/mat.h @@ -0,0 +1,71 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// matrix class. this is mostly for convenience in the testing code, it is +// not optimized at all. correctness is much more importance here. + +#ifndef _ODE_MAT_H_ +#define _ODE_MAT_H_ + +#include + + +class dMatrix { + int n,m; // matrix dimension, n,m >= 0 + dReal *data; // if nonzero, n*m elements allocated on the heap + +public: + // constructors, destructors + dMatrix(); // make default 0x0 matrix + dMatrix (int rows, int cols); // construct zero matrix of given size + dMatrix (const dMatrix &); // construct copy of given matrix + // create copy of given data - element (i,j) is data[i*rowskip+j*colskip] + dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip); + ~dMatrix(); // destructor + + // data movement + dReal & operator () (int i, int j); // reference an element + void operator= (const dMatrix &); // matrix = matrix + void operator= (dReal); // matrix = scalar + dMatrix transpose(); // return transposed matrix + // return a permuted submatrix of this matrix, made up of the rows in p + // and the columns in q. p has np elements, q has nq elements. + dMatrix select (int np, int *p, int nq, int *q); + + // operators + dMatrix operator + (const dMatrix &); + dMatrix operator - (const dMatrix &); + dMatrix operator - (); + dMatrix operator * (const dMatrix &); + void operator += (const dMatrix &); + void operator -= (const dMatrix &); + + // utility + void clearUpperTriangle(); + void clearLowerTriangle(); + void makeRandom (dReal range); + void print (char *fmt = "%10.4f ", FILE *f=stdout); + dReal maxDifference (const dMatrix &); +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/matrix.cpp b/libraries/ode-0.9/ode/src/matrix.cpp new file mode 100644 index 0000000..16afe91 --- /dev/null +++ b/libraries/ode-0.9/ode/src/matrix.cpp @@ -0,0 +1,358 @@ +/************************************************************************* + * * + * 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 + +// misc defines +#define ALLOCA dALLOCA16 + + +void dSetZero (dReal *a, int n) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = 0; + n--; + } +} + + +void dSetValue (dReal *a, int n, dReal value) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = value; + n--; + } +} + + +void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) +{ + int i,j,k,qskip,rskip,rpad; + dAASSERT (A && B && C && p>0 && q>0 && r>0); + qskip = dPAD(q); + rskip = dPAD(r); + rpad = rskip - r; + dReal sum; + const dReal *b,*c,*bb; + bb = B; + for (i=p; i; i--) { + for (j=0 ; j0 && q>0 && r>0); + pskip = dPAD(p); + rskip = dPAD(r); + for (i=0; i0 && q>0 && r>0); + rpad = dPAD(r) - r; + qskip = dPAD(q); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + z = 0; + sum = 0; + for (k=q; k; k--,z++) sum += bb[z] * cc[z]; + *(A++) = sum; + cc += qskip; + } + A += rpad; + bb += qskip; + } +} + + +int dFactorCholesky (dReal *A, int n) +{ + int i,j,k,nskip; + dReal sum,*a,*b,*aa,*bb,*cc,*recip; + dAASSERT (n > 0 && A); + nskip = dPAD (n); + recip = (dReal*) ALLOCA (n * sizeof(dReal)); + aa = A; + for (i=0; i 0 && L && b); + nskip = dPAD (n); + y = (dReal*) ALLOCA (n*sizeof(dReal)); + for (i=0; i= 0; i--) { + sum = 0; + for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k]; + b[i] = (y[i]-sum)/L[i*nskip+i]; + } +} + + +int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) +{ + int i,j,nskip; + dReal *L,*x; + dAASSERT (n > 0 && A && Ainv); + nskip = dPAD (n); + L = (dReal*) ALLOCA (nskip*n*sizeof(dReal)); + memcpy (L,A,nskip*n*sizeof(dReal)); + x = (dReal*) ALLOCA (n*sizeof(dReal)); + if (dFactorCholesky (L,n)==0) return 0; + dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0 + for (i=0; i 0 && A); + int nskip = dPAD (n); + Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal)); + memcpy (Acopy,A,nskip*n * sizeof(dReal)); + return dFactorCholesky (Acopy,n); +} + + +/***** this has been replaced by a faster version +void dSolveL1T (const dReal *L, dReal *b, int n, int nskip) +{ + int i,j; + dAASSERT (L && b && n >= 0 && nskip >= n); + dReal sum; + for (i=n-2; i>=0; i--) { + sum = 0; + for (j=i+1; j= 0); + for (int i=0; i 0 && nskip >= n); + dSolveL1 (L,b,n,nskip); + dVectorScale (b,d,n); + dSolveL1T (L,b,n,nskip); +} + + +void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip) +{ + int j,p; + dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee; + dAASSERT (L && d && a && n > 0 && nskip >= n); + + if (n < 2) return; + W1 = (dReal*) ALLOCA (n*sizeof(dReal)); + W2 = (dReal*) ALLOCA (n*sizeof(dReal)); + + W1[0] = 0; + W2[0] = 0; + for (j=1; j j) ? _GETA(i,j) : _GETA(j,i)) + + +void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, + int n1, int n2, int r, int nskip) +{ + int i; + dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && + n1 >= n2 && nskip >= n1); + #ifndef dNODEBUG + for (i=0; i= 0 && p[i] < n1); + #endif + + if (r==n2-1) { + return; // deleting last row/col is easy + } + else if (r==0) { + dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal)); + for (i=0; i 0 && nskip >= n && r >= 0 && r < n); + if (r >= n-1) return; + if (r > 0) { + for (i=0; i +#include +#include + + +static dAllocFunction *allocfn = 0; +static dReallocFunction *reallocfn = 0; +static dFreeFunction *freefn = 0; + + + +void dSetAllocHandler (dAllocFunction *fn) +{ + allocfn = fn; +} + + +void dSetReallocHandler (dReallocFunction *fn) +{ + reallocfn = fn; +} + + +void dSetFreeHandler (dFreeFunction *fn) +{ + freefn = fn; +} + + +dAllocFunction *dGetAllocHandler() +{ + return allocfn; +} + + +dReallocFunction *dGetReallocHandler() +{ + return reallocfn; +} + + +dFreeFunction *dGetFreeHandler() +{ + return freefn; +} + + +void * dAlloc (size_t size) +{ + if (allocfn) return allocfn (size); else return malloc (size); +} + + +void * dRealloc (void *ptr, size_t oldsize, size_t newsize) +{ + if (reallocfn) return reallocfn (ptr,oldsize,newsize); + else return realloc (ptr,newsize); +} + + +void dFree (void *ptr, size_t size) +{ + if (!ptr) return; + if (freefn) freefn (ptr,size); else free (ptr); +} diff --git a/libraries/ode-0.9/ode/src/misc.cpp b/libraries/ode-0.9/ode/src/misc.cpp new file mode 100644 index 0000000..1a6f263 --- /dev/null +++ b/libraries/ode-0.9/ode/src/misc.cpp @@ -0,0 +1,169 @@ +/************************************************************************* + * * + * 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 + +//**************************************************************************** +// random numbers + +static unsigned long seed = 0; + +unsigned long dRand() +{ + seed = (1664525L*seed + 1013904223L) & 0xffffffff; + return seed; +} + + +unsigned long dRandGetSeed() +{ + return seed; +} + + +void dRandSetSeed (unsigned long s) +{ + seed = s; +} + + +int dTestRand() +{ + unsigned long oldseed = seed; + int ret = 1; + seed = 0; + if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 || + dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 || + dRand() != 0x6252e503) ret = 0; + seed = oldseed; + return ret; +} + + +// adam's all-int straightforward(?) dRandInt (0..n-1) +int dRandInt (int n) +{ + // seems good; xor-fold and modulus + const unsigned long un = n; + unsigned long r = dRand(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } + } + } + + return (int) (r % un); +} + + +dReal dRandReal() +{ + return ((dReal) dRand()) / ((dReal) 0xffffffff); +} + +//**************************************************************************** +// matrix utility stuff + +void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f) +{ + int i,j; + int skip = dPAD(m); + for (i=0; i max) max = diff; + } + } + return max; +} + + +dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n) +{ + int i,j; + int skip = dPAD(n); + dReal diff,max; + max = 0; + for (i=0; i max) max = diff; + } + } + return max; +} diff --git a/libraries/ode-0.9/ode/src/objects.h b/libraries/ode-0.9/ode/src/objects.h new file mode 100644 index 0000000..f286e2d --- /dev/null +++ b/libraries/ode-0.9/ode/src/objects.h @@ -0,0 +1,138 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// object, body, and world structs. + + +#ifndef _ODE_OBJECT_H_ +#define _ODE_OBJECT_H_ + +#include +#include +#include +#include "array.h" + + +// some body flags + +enum { + dxBodyFlagFiniteRotation = 1, // use finite rotations + dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis + dxBodyDisabled = 4, // body is disabled + dxBodyNoGravity = 8, // body is not influenced by gravity + dxBodyAutoDisable = 16 // enable auto-disable on body +}; + + +// base class that does correct object allocation / deallocation + +struct dBase { + void *operator new (size_t size) { return dAlloc (size); } + void operator delete (void *ptr, size_t size) { dFree (ptr,size); } + void *operator new[] (size_t size) { return dAlloc (size); } + void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); } +}; + + +// base class for bodies and joints + +struct dObject : public dBase { + dxWorld *world; // world this object is in + dObject *next; // next object of this type in list + dObject **tome; // pointer to previous object's next ptr + void *userdata; // user settable data + int tag; // used by dynamics algorithms +}; + + +// auto disable parameters +struct dxAutoDisable { + dReal idle_time; // time the body needs to be idle to auto-disable it + int idle_steps; // steps the body needs to be idle to auto-disable it + dReal linear_average_threshold; // linear (squared) average velocity threshold + dReal angular_average_threshold; // angular (squared) average velocity threshold + unsigned int average_samples; // size of the average_lvel and average_avel buffers +}; + + +// quick-step parameters +struct dxQuickStepParameters { + int num_iterations; // number of SOR iterations to perform + dReal w; // the SOR over-relaxation parameter +}; + + +// contact generation parameters +struct dxContactParameters { + dReal max_vel; // maximum correcting velocity + dReal min_depth; // thickness of 'surface layer' +}; + + + +// position vector and rotation matrix for geometry objects that are not +// connected to bodies. + +struct dxPosR { + dVector3 pos; + dMatrix3 R; +}; + +struct dxBody : public dObject { + dxJointNode *firstjoint; // list of attached joints + int flags; // some dxBodyFlagXXX flags + dGeomID geom; // first collision geom associated with body + dMass mass; // mass parameters about POR + dMatrix3 invI; // inverse of mass.I + dReal invMass; // 1 / mass.mass + dxPosR posr; // position and orientation of point of reference + dQuaternion q; // orientation quaternion + dVector3 lvel,avel; // linear and angular velocity of POR + dVector3 facc,tacc; // force and torque accumulators + dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none + + // auto-disable information + dxAutoDisable adis; // auto-disable parameters + dReal adis_timeleft; // time left to be idle + int adis_stepsleft; // steps left to be idle + dVector3* average_lvel_buffer; // buffer for the linear average velocity calculation + dVector3* average_avel_buffer; // buffer for the angular average velocity calculation + unsigned int average_counter; // counter/index to fill the average-buffers + int average_ready; // indicates ( with = 1 ), if the Body's buffers are ready for average-calculations +}; + + +struct dxWorld : public dBase { + dxBody *firstbody; // body linked list + dxJoint *firstjoint; // joint linked list + int nb,nj; // number of bodies and joints in lists + dVector3 gravity; // gravity vector (m/s/s) + dReal global_erp; // global error reduction parameter + dReal global_cfm; // global costraint force mixing parameter + dxAutoDisable adis; // auto-disable parameters + int adis_flag; // auto-disable flag for new bodies + dxQuickStepParameters qs; + dxContactParameters contactp; +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/obstack.cpp b/libraries/ode-0.9/ode/src/obstack.cpp new file mode 100644 index 0000000..0840396 --- /dev/null +++ b/libraries/ode-0.9/ode/src/obstack.cpp @@ -0,0 +1,130 @@ +/************************************************************************* + * * + * 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 "obstack.h" + +//**************************************************************************** +// macros and constants + +#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \ + ofs = (size_t) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) ); + +#define MAX_ALLOC_SIZE \ + ((size_t)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1)) + +//**************************************************************************** +// dObStack + +dObStack::dObStack() +{ + first = 0; + last = 0; + current_arena = 0; + current_ofs = 0; +} + + +dObStack::~dObStack() +{ + // free all arenas + Arena *a,*nexta; + a = first; + while (a) { + nexta = a->next; + dFree (a,dOBSTACK_ARENA_SIZE); + a = nexta; + } +} + + +void *dObStack::alloc (int num_bytes) +{ + if ((size_t)num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large"); + + // allocate or move to a new arena if necessary + if (!first) { + // allocate the first arena if necessary + first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); + first->next = 0; + first->used = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used); + } + else { + // we already have one or more arenas, see if a new arena must be used + if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) { + if (!last->next) { + last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); + last->next->next = 0; + } + last = last->next; + last->used = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used); + } + } + + // allocate an area in the arena + char *c = ((char*) last) + last->used; + last->used += num_bytes; + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used); + return c; +} + + +void dObStack::freeAll() +{ + last = first; + if (first) { + first->used = sizeof(Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used); + } +} + + +void *dObStack::rewind() +{ + current_arena = first; + current_ofs = sizeof (Arena); + if (current_arena) { + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs) + return ((char*) current_arena) + current_ofs; + } + else return 0; +} + + +void *dObStack::next (int num_bytes) +{ + // this functions like alloc, except that no new storage is ever allocated + if (!current_arena) return 0; + current_ofs += num_bytes; + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs); + if (current_ofs >= current_arena->used) { + current_arena = current_arena->next; + if (!current_arena) return 0; + current_ofs = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs); + } + return ((char*) current_arena) + current_ofs; +} diff --git a/libraries/ode-0.9/ode/src/obstack.h b/libraries/ode-0.9/ode/src/obstack.h new file mode 100644 index 0000000..b5f715b --- /dev/null +++ b/libraries/ode-0.9/ode/src/obstack.h @@ -0,0 +1,68 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_OBSTACK_H_ +#define _ODE_OBSTACK_H_ + +#include "objects.h" + +// each obstack Arena pointer points to a block of this many bytes +#define dOBSTACK_ARENA_SIZE 16384 + + +struct dObStack : public dBase { + struct Arena { + Arena *next; // next arena in linked list + size_t used; // total number of bytes used in this arena, counting + }; // this header + + Arena *first; // head of the arena linked list. 0 if no arenas yet + Arena *last; // arena where blocks are currently being allocated + + // used for iterator + Arena *current_arena; + size_t current_ofs; + + dObStack(); + ~dObStack(); + + void *alloc (int num_bytes); + // allocate a block in the last arena, allocating a new arena if necessary. + // it is a runtime error if num_bytes is larger than the arena size. + + void freeAll(); + // free all blocks in all arenas. this does not deallocate the arenas + // themselves, so future alloc()s will reuse them. + + void *rewind(); + // rewind the obstack iterator, and return the address of the first + // allocated block. return 0 if there are no allocated blocks. + + void *next (int num_bytes); + // return the address of the next allocated block. 'num_bytes' is the size + // of the previous block. this returns null if there are no more arenas. + // the sequence of 'num_bytes' parameters passed to next() during a + // traversal of the list must exactly match the parameters passed to alloc(). +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/ode.cpp b/libraries/ode-0.9/ode/src/ode.cpp new file mode 100644 index 0000000..46f559a --- /dev/null +++ b/libraries/ode-0.9/ode/src/ode.cpp @@ -0,0 +1,1732 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +// this source file is mostly concerned with the data structures, not the +// numerics. + +#include "objects.h" +#include +#include "joint.h" +#include +#include +#include "step.h" +#include "quickstep.h" +#include "util.h" +#include +#include + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// utility + +static inline void initObject (dObject *obj, dxWorld *w) +{ + obj->world = w; + obj->next = 0; + obj->tome = 0; + obj->userdata = 0; + obj->tag = 0; +} + + +// add an object `obj' to the list who's head pointer is pointed to by `first'. + +static inline void addObjectToList (dObject *obj, dObject **first) +{ + obj->next = *first; + obj->tome = first; + if (*first) (*first)->tome = &obj->next; + (*first) = obj; +} + + +// remove the object from the linked list + +static inline void removeObjectFromList (dObject *obj) +{ + if (obj->next) obj->next->tome = obj->tome; + *(obj->tome) = obj->next; + // safeguard + obj->next = 0; + obj->tome = 0; +} + + +// remove the joint from neighbour lists of all connected bodies + +static void removeJointReferencesFromAttachedBodies (dxJoint *j) +{ + for (int i=0; i<2; i++) { + dxBody *body = j->node[i].body; + if (body) { + dxJointNode *n = body->firstjoint; + dxJointNode *last = 0; + while (n) { + if (n->joint == j) { + if (last) last->next = n->next; + else body->firstjoint = n->next; + break; + } + last = n; + n = n->next; + } + } + } + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; +} + +//**************************************************************************** +// debugging + +// see if an object list loops on itself (if so, it's bad). + +static int listHasLoops (dObject *first) +{ + if (first==0 || first->next==0) return 0; + dObject *a=first,*b=first->next; + int skip=0; + while (b) { + if (a==b) return 1; + b = b->next; + if (skip) a = a->next; + skip ^= 1; + } + return 0; +} + + +// check the validity of the world data structures + +static void checkWorld (dxWorld *w) +{ + dxBody *b; + dxJoint *j; + + // check there are no loops + if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops"); + if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops"); + + // check lists are well formed (check `tome' pointers) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + if (b->next && b->next->tome != &b->next) + dDebug (0,"bad tome pointer in body list"); + } + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->next && j->next->tome != &j->next) + dDebug (0,"bad tome pointer in joint list"); + } + + // check counts + int n = 0; + for (b=w->firstbody; b; b=(dxBody*)b->next) n++; + if (w->nb != n) dDebug (0,"body count incorrect"); + n = 0; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++; + if (w->nj != n) dDebug (0,"joint count incorrect"); + + // set all tag values to a known value + static int count = 0; + count++; + for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count; + + // check all body/joint world pointers are ok + for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w) + dDebug (0,"bad world pointer in body list"); + for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w) + dDebug (0,"bad world pointer in joint list"); + + /* + // check for half-connected joints - actually now these are valid + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body || j->node[1].body) { + if (!(j->node[0].body && j->node[1].body)) + dDebug (0,"half connected joint found"); + } + } + */ + + // check that every joint node appears in the joint lists of both bodies it + // attaches + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + for (int i=0; i<2; i++) { + if (j->node[i].body) { + int ok = 0; + for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) { + if (n->joint == j) ok = 1; + } + if (ok==0) dDebug (0,"joint not in joint list of attached body"); + } + } + } + + // check all body joint lists (correct body ptrs) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (&n->joint->node[0] == n) { + if (n->joint->node[1].body != b) + dDebug (0,"bad body pointer in joint node of body list (1)"); + } + else { + if (n->joint->node[0].body != b) + dDebug (0,"bad body pointer in joint node of body list (2)"); + } + if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body"); + } + } + + // check all body pointers in joints, check they are distinct + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body && (j->node[0].body == j->node[1].body)) + dDebug (0,"non-distinct body pointers in joint"); + if ((j->node[0].body && j->node[0].body->tag != count) || + (j->node[1].body && j->node[1].body->tag != count)) + dDebug (0,"bad body pointer in joint"); + } +} + + +void dWorldCheck (dxWorld *w) +{ + checkWorld (w); +} + +//**************************************************************************** +// body +dxWorld* dBodyGetWorld (dxBody* b) +{ + dAASSERT (b); + return b->world; +} + +dxBody *dBodyCreate (dxWorld *w) +{ + dAASSERT (w); + dxBody *b = new dxBody; + initObject (b,w); + b->firstjoint = 0; + b->flags = 0; + b->geom = 0; + b->average_lvel_buffer = 0; + b->average_avel_buffer = 0; + dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0); + dSetZero (b->invI,4*3); + b->invI[0] = 1; + b->invI[5] = 1; + b->invI[10] = 1; + b->invMass = 1; + dSetZero (b->posr.pos,4); + dSetZero (b->q,4); + b->q[0] = 1; + dRSetIdentity (b->posr.R); + dSetZero (b->lvel,4); + dSetZero (b->avel,4); + dSetZero (b->facc,4); + dSetZero (b->tacc,4); + dSetZero (b->finite_rot_axis,4); + addObjectToList (b,(dObject **) &w->firstbody); + w->nb++; + + // set auto-disable parameters + b->average_avel_buffer = b->average_lvel_buffer = 0; // no buffer at beginnin + dBodySetAutoDisableDefaults (b); // must do this after adding to world + b->adis_stepsleft = b->adis.idle_steps; + b->adis_timeleft = b->adis.idle_time; + b->average_counter = 0; + b->average_ready = 0; // average buffer not filled on the beginning + dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples); + + return b; +} + + +void dBodyDestroy (dxBody *b) +{ + dAASSERT (b); + + // all geoms that link to this body must be notified that the body is about + // to disappear. note that the call to dGeomSetBody(geom,0) will result in + // dGeomGetBodyNext() returning 0 for the body, so we must get the next body + // before setting the body to 0. + dxGeom *next_geom = 0; + for (dxGeom *geom = b->geom; geom; geom = next_geom) { + next_geom = dGeomGetBodyNext (geom); + dGeomSetBody (geom,0); + } + + // detach all neighbouring joints, then delete this body. + dxJointNode *n = b->firstjoint; + while (n) { + // sneaky trick to speed up removal of joint references (black magic) + n->joint->node[(n == n->joint->node)].body = 0; + + dxJointNode *next = n->next; + n->next = 0; + removeJointReferencesFromAttachedBodies (n->joint); + n = next; + } + removeObjectFromList (b); + b->world->nb--; + + // delete the average buffers + if(b->average_lvel_buffer) + { + delete[] (b->average_lvel_buffer); + b->average_lvel_buffer = 0; + } + if(b->average_avel_buffer) + { + delete[] (b->average_avel_buffer); + b->average_avel_buffer = 0; + } + + delete b; +} + + +void dBodySetData (dBodyID b, void *data) +{ + dAASSERT (b); + b->userdata = data; +} + + +void *dBodyGetData (dBodyID b) +{ + dAASSERT (b); + return b->userdata; +} + + +void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->posr.pos[0] = x; + b->posr.pos[1] = y; + b->posr.pos[2] = z; + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetRotation (dBodyID b, const dMatrix3 R) +{ + dAASSERT (b && R); + dQuaternion q; + dRtoQ (R,q); + dNormalize4 (q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dQtoR (b->q,b->posr.R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetQuaternion (dBodyID b, const dQuaternion q) +{ + dAASSERT (b && q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dNormalize4 (b->q); + dQtoR (b->q,b->posr.R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->lvel[0] = x; + b->lvel[1] = y; + b->lvel[2] = z; +} + + +void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->avel[0] = x; + b->avel[1] = y; + b->avel[2] = z; +} + + +const dReal * dBodyGetPosition (dBodyID b) +{ + dAASSERT (b); + return b->posr.pos; +} + + +void dBodyCopyPosition (dBodyID b, dVector3 pos) +{ + dAASSERT (b); + dReal* src = b->posr.pos; + pos[0] = src[0]; + pos[1] = src[1]; + pos[2] = src[2]; +} + + +const dReal * dBodyGetRotation (dBodyID b) +{ + dAASSERT (b); + return b->posr.R; +} + + +void dBodyCopyRotation (dBodyID b, dMatrix3 R) +{ + dAASSERT (b); + const dReal* src = b->posr.R; + R[0] = src[0]; + R[1] = src[1]; + R[2] = src[2]; + R[3] = src[3]; + R[4] = src[4]; + R[5] = src[5]; + R[6] = src[6]; + R[7] = src[7]; + R[8] = src[8]; + R[9] = src[9]; + R[10] = src[10]; + R[11] = src[11]; +} + + +const dReal * dBodyGetQuaternion (dBodyID b) +{ + dAASSERT (b); + return b->q; +} + + +void dBodyCopyQuaternion (dBodyID b, dQuaternion quat) +{ + dAASSERT (b); + dReal* src = b->q; + quat[0] = src[0]; + quat[1] = src[1]; + quat[2] = src[2]; + quat[3] = src[3]; +} + + +const dReal * dBodyGetLinearVel (dBodyID b) +{ + dAASSERT (b); + return b->lvel; +} + + +const dReal * dBodyGetAngularVel (dBodyID b) +{ + dAASSERT (b); + return b->avel; +} + + +void dBodySetMass (dBodyID b, const dMass *mass) +{ + dAASSERT (b && mass ); + dIASSERT(dMassCheck(mass)); + + // The centre of mass must be at the origin. + // Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it. + dUASSERT( fabs( mass->c[0] ) <= dEpsilon && + fabs( mass->c[1] ) <= dEpsilon && + fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." ) + + memcpy (&b->mass,mass,sizeof(dMass)); + if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) { + dDEBUGMSG ("inertia must be positive definite!"); + dRSetIdentity (b->invI); + } + b->invMass = dRecip(b->mass.mass); +} + + +void dBodyGetMass (dBodyID b, dMass *mass) +{ + dAASSERT (b && mass); + memcpy (mass,&b->mass,sizeof(dMass)); +} + + +void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; +} + + +void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->tacc[0] += fx; + b->tacc[1] += fy; + b->tacc[2] += fz; +} + + +void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->posr.R,t1); + b->facc[0] += t2[0]; + b->facc[1] += t2[1]; + b->facc[2] += t2[2]; +} + + +void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->posr.R,t1); + b->tacc[0] += t2[0]; + b->tacc[1] += t2[1]; + b->tacc[2] += t2[2]; +} + + +void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; + dVector3 f,q; + f[0] = fx; + f[1] = fy; + f[2] = fz; + q[0] = px - b->posr.pos[0]; + q[1] = py - b->posr.pos[1]; + q[2] = pz - b->posr.pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 prel,f,p; + f[0] = fx; + f[1] = fy; + f[2] = fz; + f[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->posr.R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,f; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + dMULTIPLY0_331 (f,b->posr.R,frel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dVector3 q; + q[0] = px - b->posr.pos[0]; + q[1] = py - b->posr.pos[1]; + q[2] = pz - b->posr.pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,prel,f,p; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (f,b->posr.R,frel); + dMULTIPLY0_331 (p,b->posr.R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +const dReal * dBodyGetForce (dBodyID b) +{ + dAASSERT (b); + return b->facc; +} + + +const dReal * dBodyGetTorque (dBodyID b) +{ + dAASSERT (b); + return b->tacc; +} + + +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->facc[0] = x; + b->facc[1] = y; + b->facc[2] = z; +} + + +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->tacc[0] = x; + b->tacc[1] = y; + b->tacc[2] = z; +} + + +void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->posr.R,prel); + result[0] = p[0] + b->posr.pos[0]; + result[1] = p[1] + b->posr.pos[1]; + result[2] = p[2] + b->posr.pos[2]; +} + + +void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->posr.R,prel); + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px - b->posr.pos[0]; + p[1] = py - b->posr.pos[1]; + p[2] = pz - b->posr.pos[2]; + p[3] = 0; + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel; + prel[0] = px - b->posr.pos[0]; + prel[1] = py - b->posr.pos[1]; + prel[2] = pz - b->posr.pos[2]; + prel[3] = 0; + dMULTIPLY1_331 (result,b->posr.R,prel); +} + + +void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY0_331 (result,b->posr.R,p); +} + + +void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY1_331 (result,b->posr.R,p); +} + + +void dBodySetFiniteRotationMode (dBodyID b, int mode) +{ + dAASSERT (b); + b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis); + if (mode) { + b->flags |= dxBodyFlagFiniteRotation; + if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 || + b->finite_rot_axis[2] != 0) { + b->flags |= dxBodyFlagFiniteRotationAxis; + } + } +} + + +void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->finite_rot_axis[0] = x; + b->finite_rot_axis[1] = y; + b->finite_rot_axis[2] = z; + if (x != 0 || y != 0 || z != 0) { + dNormalize3 (b->finite_rot_axis); + b->flags |= dxBodyFlagFiniteRotationAxis; + } + else { + b->flags &= ~dxBodyFlagFiniteRotationAxis; + } +} + + +int dBodyGetFiniteRotationMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyFlagFiniteRotation) != 0); +} + + +void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result) +{ + dAASSERT (b); + result[0] = b->finite_rot_axis[0]; + result[1] = b->finite_rot_axis[1]; + result[2] = b->finite_rot_axis[2]; +} + + +int dBodyGetNumJoints (dBodyID b) +{ + dAASSERT (b); + int count=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, count++); + return count; +} + + +dJointID dBodyGetJoint (dBodyID b, int index) +{ + dAASSERT (b); + int i=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) { + if (i == index) return n->joint; + } + return 0; +} + + +void dBodyEnable (dBodyID b) +{ + dAASSERT (b); + b->flags &= ~dxBodyDisabled; + b->adis_stepsleft = b->adis.idle_steps; + b->adis_timeleft = b->adis.idle_time; + // no code for average-processing needed here +} + + +void dBodyDisable (dBodyID b) +{ + dAASSERT (b); + b->flags |= dxBodyDisabled; +} + + +int dBodyIsEnabled (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyDisabled) == 0); +} + + +void dBodySetGravityMode (dBodyID b, int mode) +{ + dAASSERT (b); + if (mode) b->flags &= ~dxBodyNoGravity; + else b->flags |= dxBodyNoGravity; +} + + +int dBodyGetGravityMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyNoGravity) == 0); +} + + +// body auto-disable functions + +dReal dBodyGetAutoDisableLinearThreshold (dBodyID b) +{ + dAASSERT(b); + return dSqrt (b->adis.linear_average_threshold); +} + + +void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold) +{ + dAASSERT(b); + b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold; +} + + +dReal dBodyGetAutoDisableAngularThreshold (dBodyID b) +{ + dAASSERT(b); + return dSqrt (b->adis.angular_average_threshold); +} + + +void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold) +{ + dAASSERT(b); + b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold; +} + + +int dBodyGetAutoDisableAverageSamplesCount (dBodyID b) +{ + dAASSERT(b); + return b->adis.average_samples; +} + + +void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count) +{ + dAASSERT(b); + b->adis.average_samples = average_samples_count; + // update the average buffers + if(b->average_lvel_buffer) + { + delete[] b->average_lvel_buffer; + b->average_lvel_buffer = 0; + } + if(b->average_avel_buffer) + { + delete[] b->average_avel_buffer; + b->average_avel_buffer = 0; + } + if(b->adis.average_samples > 0) + { + b->average_lvel_buffer = new dVector3[b->adis.average_samples]; + b->average_avel_buffer = new dVector3[b->adis.average_samples]; + } + else + { + b->average_lvel_buffer = 0; + b->average_avel_buffer = 0; + } + // new buffer is empty + b->average_counter = 0; + b->average_ready = 0; +} + + +int dBodyGetAutoDisableSteps (dBodyID b) +{ + dAASSERT(b); + return b->adis.idle_steps; +} + + +void dBodySetAutoDisableSteps (dBodyID b, int steps) +{ + dAASSERT(b); + b->adis.idle_steps = steps; +} + + +dReal dBodyGetAutoDisableTime (dBodyID b) +{ + dAASSERT(b); + return b->adis.idle_time; +} + + +void dBodySetAutoDisableTime (dBodyID b, dReal time) +{ + dAASSERT(b); + b->adis.idle_time = time; +} + + +int dBodyGetAutoDisableFlag (dBodyID b) +{ + dAASSERT(b); + return ((b->flags & dxBodyAutoDisable) != 0); +} + + +void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable) +{ + dAASSERT(b); + if (!do_auto_disable) + { + b->flags &= ~dxBodyAutoDisable; + // (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag + b->flags &= ~dxBodyDisabled; + b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world); + b->adis.idle_time = dWorldGetAutoDisableTime(b->world); + // resetting the average calculations too + dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) ); + } + else + { + b->flags |= dxBodyAutoDisable; + } +} + + +void dBodySetAutoDisableDefaults (dBodyID b) +{ + dAASSERT(b); + dWorldID w = b->world; + dAASSERT(w); + b->adis = w->adis; + dBodySetAutoDisableFlag (b, w->adis_flag); +} + +//**************************************************************************** +// joints + +static void dJointInit (dxWorld *w, dxJoint *j) +{ + dIASSERT (w && j); + initObject (j,w); + j->vtable = 0; + j->flags = 0; + j->node[0].joint = j; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].joint = j; + j->node[1].body = 0; + j->node[1].next = 0; + dSetZero (j->lambda,6); + addObjectToList (j,(dObject **) &w->firstjoint); + w->nj++; +} + + +static dxJoint *createJoint (dWorldID w, dJointGroupID group, + dxJoint::Vtable *vtable) +{ + dIASSERT (w && vtable); + dxJoint *j; + if (group) { + j = (dxJoint*) group->stack.alloc (vtable->size); + group->num++; + } + else j = (dxJoint*) dAlloc (vtable->size); + dJointInit (w,j); + j->vtable = vtable; + if (group) j->flags |= dJOINT_INGROUP; + if (vtable->init) vtable->init (j); + j->feedback = 0; + return j; +} + + +dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dball_vtable); +} + + +dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge_vtable); +} + + +dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dslider_vtable); +} + + +dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group, + const dContact *c) +{ + dAASSERT (w && c); + dxJointContact *j = (dxJointContact *) + createJoint (w,group,&__dcontact_vtable); + j->contact = *c; + return j; +} + + +dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge2_vtable); +} + + +dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__duniversal_vtable); +} + +dxJoint * dJointCreatePR (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dPR_vtable); +} + +dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dfixed_vtable); +} + + +dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dnull_vtable); +} + + +dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__damotor_vtable); +} + +dxJoint * dJointCreateLMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dlmotor_vtable); +} + +dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dplane2d_vtable); +} + +void dJointDestroy (dxJoint *j) +{ + dAASSERT (j); + if (j->flags & dJOINT_INGROUP) return; + removeJointReferencesFromAttachedBodies (j); + removeObjectFromList (j); + j->world->nj--; + dFree (j,j->vtable->size); +} + + +dJointGroupID dJointGroupCreate (int max_size) +{ + // not any more ... dUASSERT (max_size > 0,"max size must be > 0"); + dxJointGroup *group = new dxJointGroup; + group->num = 0; + return group; +} + + +void dJointGroupDestroy (dJointGroupID group) +{ + dAASSERT (group); + dJointGroupEmpty (group); + delete group; +} + + +void dJointGroupEmpty (dJointGroupID group) +{ + // the joints in this group are detached starting from the most recently + // added (at the top of the stack). this helps ensure that the various + // linked lists are not traversed too much, as the joints will hopefully + // be at the start of those lists. + // if any group joints have their world pointer set to 0, their world was + // previously destroyed. no special handling is required for these joints. + + dAASSERT (group); + int i; + dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*)); + dxJoint *j = (dxJoint*) group->stack.rewind(); + for (i=0; i < group->num; i++) { + jlist[i] = j; + j = (dxJoint*) (group->stack.next (j->vtable->size)); + } + for (i=group->num-1; i >= 0; i--) { + if (jlist[i]->world) { + removeJointReferencesFromAttachedBodies (jlist[i]); + removeObjectFromList (jlist[i]); + jlist[i]->world->nj--; + } + } + group->num = 0; + group->stack.freeAll(); +} + + +void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2) +{ + // check arguments + dUASSERT (joint,"bad joint argument"); + dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2"); + dxWorld *world = joint->world; + dUASSERT ( (!body1 || body1->world == world) && + (!body2 || body2->world == world), + "joint and bodies must be in same world"); + + // check if the joint can not be attached to just one body + dUASSERT (!((joint->flags & dJOINT_TWOBODIES) && + ((body1 != 0) ^ (body2 != 0))), + "joint can not be attached to just one body"); + + // remove any existing body attachments + if (joint->node[0].body || joint->node[1].body) { + removeJointReferencesFromAttachedBodies (joint); + } + + // if a body is zero, make sure that it is body2, so 0 --> node[1].body + if (body1==0) { + body1 = body2; + body2 = 0; + joint->flags |= dJOINT_REVERSE; + } + else { + joint->flags &= (~dJOINT_REVERSE); + } + + // attach to new bodies + joint->node[0].body = body1; + joint->node[1].body = body2; + if (body1) { + joint->node[1].next = body1->firstjoint; + body1->firstjoint = &joint->node[1]; + } + else joint->node[1].next = 0; + if (body2) { + joint->node[0].next = body2->firstjoint; + body2->firstjoint = &joint->node[0]; + } + else { + joint->node[0].next = 0; + } +} + + +void dJointSetData (dxJoint *joint, void *data) +{ + dAASSERT (joint); + joint->userdata = data; +} + + +void *dJointGetData (dxJoint *joint) +{ + dAASSERT (joint); + return joint->userdata; +} + + +int dJointGetType (dxJoint *joint) +{ + dAASSERT (joint); + return joint->vtable->typenum; +} + + +dBodyID dJointGetBody (dxJoint *joint, int index) +{ + dAASSERT (joint); + if (index == 0 || index == 1) { + if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body; + else return joint->node[index].body; + } + else return 0; +} + + +void dJointSetFeedback (dxJoint *joint, dJointFeedback *f) +{ + dAASSERT (joint); + joint->feedback = f; +} + + +dJointFeedback *dJointGetFeedback (dxJoint *joint) +{ + dAASSERT (joint); + return joint->feedback; +} + + + +dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2) +{ + dAASSERT (in_b1 || in_b2); + + dBodyID b1, b2; + + if (in_b1 == 0) { + b1 = in_b2; + b2 = in_b1; + } + else { + b1 = in_b1; + b2 = in_b2; + } + + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return n->joint; + } + + return 0; +} + + + +int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list) +{ + dAASSERT (in_b1 || in_b2); + + + dBodyID b1, b2; + + if (in_b1 == 0) { + b1 = in_b2; + b2 = in_b1; + } + else { + b1 = in_b1; + b2 = in_b2; + } + + // look through b1's neighbour list for b2 + int numConnectingJoints = 0; + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) + out_list[numConnectingJoints++] = n->joint; + } + + return numConnectingJoints; +} + + +int dAreConnected (dBodyID b1, dBodyID b2) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return 1; + } + return 0; +} + + +int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1; + } + return 0; +} + +//**************************************************************************** +// world + +dxWorld * dWorldCreate() +{ + dxWorld *w = new dxWorld; + w->firstbody = 0; + w->firstjoint = 0; + w->nb = 0; + w->nj = 0; + dSetZero (w->gravity,4); + w->global_erp = REAL(0.2); +#if defined(dSINGLE) + w->global_cfm = 1e-5f; +#elif defined(dDOUBLE) + w->global_cfm = 1e-10; +#else + #error dSINGLE or dDOUBLE must be defined +#endif + + w->adis.idle_steps = 10; + w->adis.idle_time = 0; + w->adis_flag = 0; + w->adis.average_samples = 1; // Default is 1 sample => Instantaneous velocity + w->adis.angular_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared) + w->adis.linear_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared) + + w->qs.num_iterations = 20; + w->qs.w = REAL(1.3); + + w->contactp.max_vel = dInfinity; + w->contactp.min_depth = 0; + + return w; +} + + +void dWorldDestroy (dxWorld *w) +{ + // delete all bodies and joints + dAASSERT (w); + dxBody *nextb, *b = w->firstbody; + while (b) { + nextb = (dxBody*) b->next; + if(b->average_lvel_buffer) + { + delete[] (b->average_lvel_buffer); + b->average_lvel_buffer = 0; + } + if(b->average_avel_buffer) + { + delete[] (b->average_avel_buffer); + b->average_avel_buffer = 0; + } + dBodyDestroy(b); // calling here dBodyDestroy for correct destroying! (i.e. the average buffers) + b = nextb; + } + dxJoint *nextj, *j = w->firstjoint; + while (j) { + nextj = (dxJoint*)j->next; + if (j->flags & dJOINT_INGROUP) { + // the joint is part of a group, so "deactivate" it instead + j->world = 0; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; + dMessage (0,"warning: destroying world containing grouped joints"); + } + else { + dFree (j,j->vtable->size); + } + j = nextj; + } + delete w; +} + + +void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z) +{ + dAASSERT (w); + w->gravity[0] = x; + w->gravity[1] = y; + w->gravity[2] = z; +} + + +void dWorldGetGravity (dWorldID w, dVector3 g) +{ + dAASSERT (w); + g[0] = w->gravity[0]; + g[1] = w->gravity[1]; + g[2] = w->gravity[2]; +} + + +void dWorldSetERP (dWorldID w, dReal erp) +{ + dAASSERT (w); + w->global_erp = erp; +} + + +dReal dWorldGetERP (dWorldID w) +{ + dAASSERT (w); + return w->global_erp; +} + + +void dWorldSetCFM (dWorldID w, dReal cfm) +{ + dAASSERT (w); + w->global_cfm = cfm; +} + + +dReal dWorldGetCFM (dWorldID w) +{ + dAASSERT (w); + return w->global_cfm; +} + + +void dWorldStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + dxProcessIslands (w,stepsize,&dInternalStepIsland); +} + + +void dWorldQuickStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + dxProcessIslands (w,stepsize,&dxQuickStepper); +} + + +void dWorldImpulseToForce (dWorldID w, dReal stepsize, + dReal ix, dReal iy, dReal iz, + dVector3 force) +{ + dAASSERT (w); + stepsize = dRecip(stepsize); + force[0] = stepsize * ix; + force[1] = stepsize * iy; + force[2] = stepsize * iz; + // @@@ force[3] = 0; +} + + +// world auto-disable functions + +dReal dWorldGetAutoDisableLinearThreshold (dWorldID w) +{ + dAASSERT(w); + return dSqrt (w->adis.linear_average_threshold); +} + + +void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_average_threshold) +{ + dAASSERT(w); + w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold; +} + + +dReal dWorldGetAutoDisableAngularThreshold (dWorldID w) +{ + dAASSERT(w); + return dSqrt (w->adis.angular_average_threshold); +} + + +void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_average_threshold) +{ + dAASSERT(w); + w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold; +} + + +int dWorldGetAutoDisableAverageSamplesCount (dWorldID w) +{ + dAASSERT(w); + return w->adis.average_samples; +} + + +void dWorldSetAutoDisableAverageSamplesCount (dWorldID w, unsigned int average_samples_count) +{ + dAASSERT(w); + w->adis.average_samples = average_samples_count; +} + + +int dWorldGetAutoDisableSteps (dWorldID w) +{ + dAASSERT(w); + return w->adis.idle_steps; +} + + +void dWorldSetAutoDisableSteps (dWorldID w, int steps) +{ + dAASSERT(w); + w->adis.idle_steps = steps; +} + + +dReal dWorldGetAutoDisableTime (dWorldID w) +{ + dAASSERT(w); + return w->adis.idle_time; +} + + +void dWorldSetAutoDisableTime (dWorldID w, dReal time) +{ + dAASSERT(w); + w->adis.idle_time = time; +} + + +int dWorldGetAutoDisableFlag (dWorldID w) +{ + dAASSERT(w); + return w->adis_flag; +} + + +void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable) +{ + dAASSERT(w); + w->adis_flag = (do_auto_disable != 0); +} + + +void dWorldSetQuickStepNumIterations (dWorldID w, int num) +{ + dAASSERT(w); + w->qs.num_iterations = num; +} + + +int dWorldGetQuickStepNumIterations (dWorldID w) +{ + dAASSERT(w); + return w->qs.num_iterations; +} + + +void dWorldSetQuickStepW (dWorldID w, dReal param) +{ + dAASSERT(w); + w->qs.w = param; +} + + +dReal dWorldGetQuickStepW (dWorldID w) +{ + dAASSERT(w); + return w->qs.w; +} + + +void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel) +{ + dAASSERT(w); + w->contactp.max_vel = vel; +} + + +dReal dWorldGetContactMaxCorrectingVel (dWorldID w) +{ + dAASSERT(w); + return w->contactp.max_vel; +} + + +void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth) +{ + dAASSERT(w); + w->contactp.min_depth = depth; +} + + +dReal dWorldGetContactSurfaceLayer (dWorldID w) +{ + dAASSERT(w); + return w->contactp.min_depth; +} + +//**************************************************************************** +// testing + +#define NUM 100 + +#define DO(x) + + +extern "C" void dTestDataStructures() +{ + int i; + DO(printf ("testDynamicsStuff()\n")); + + dBodyID body [NUM]; + int nb = 0; + dJointID joint [NUM]; + int nj = 0; + + for (i=0; i 0.5) { + DO(printf ("creating body\n")); + body[nb] = dBodyCreate (w); + DO(printf ("\t--> %p\n",body[nb])); + nb++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj < NUM && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + DO(printf ("creating joint, attaching to %p,%p\n",b1,b2)); + joint[nj] = dJointCreateBall (w,0); + DO(printf ("\t-->%p\n",joint[nj])); + checkWorld (w); + dJointAttach (joint[nj],b1,b2); + nj++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nj > 0 && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + int k = dRand() % nj; + DO(printf ("reattaching joint %p\n",joint[k])); + dJointAttach (joint[k],b1,b2); + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nb > 0 && dRandReal() > 0.5) { + int k = dRand() % nb; + DO(printf ("destroying body %p\n",body[k])); + dBodyDestroy (body[k]); + checkWorld (w); + for (; k < (NUM-1); k++) body[k] = body[k+1]; + nb--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj > 0 && dRandReal() > 0.5) { + int k = dRand() % nj; + DO(printf ("destroying joint %p\n",joint[k])); + dJointDestroy (joint[k]); + checkWorld (w); + for (; k < (NUM-1); k++) joint[k] = joint[k+1]; + nj--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + + /* + printf ("creating world\n"); + dWorldID w = dWorldCreate(); + checkWorld (w); + printf ("creating body\n"); + dBodyID b1 = dBodyCreate (w); + checkWorld (w); + printf ("creating body\n"); + dBodyID b2 = dBodyCreate (w); + checkWorld (w); + printf ("creating joint\n"); + dJointID j = dJointCreateBall (w); + checkWorld (w); + printf ("attaching joint\n"); + dJointAttach (j,b1,b2); + checkWorld (w); + printf ("destroying joint\n"); + dJointDestroy (j); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b1); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b2); + checkWorld (w); + printf ("destroying world\n"); + dWorldDestroy (w); + */ +} diff --git a/libraries/ode-0.9/ode/src/odemath.cpp b/libraries/ode-0.9/ode/src/odemath.cpp new file mode 100644 index 0000000..11bc84e --- /dev/null +++ b/libraries/ode-0.9/ode/src/odemath.cpp @@ -0,0 +1,178 @@ +/************************************************************************* + * * + * 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 + +// get some math functions under windows +#ifdef WIN32 +#include +#ifndef CYGWIN // added by andy for cygwin +#undef copysign +#define copysign(a,b) ((dReal)_copysign(a,b)) +#endif // added by andy for cygwin +#endif + +#undef dNormalize3 +#undef dNormalize4 + + +// this may be called for vectors `a' with extremely small magnitude, for +// example the result of a cross product on two nearly perpendicular vectors. +// we must be robust to these small vectors. to prevent numerical error, +// first find the component a[i] with the largest magnitude and then scale +// all the components by 1/a[i]. then we can compute the length of `a' and +// scale the components by 1/l. this has been verified to work with vectors +// containing the smallest representable numbers. + +int dSafeNormalize3 (dVector3 a) +{ + dReal a0,a1,a2,aa0,aa1,aa2,l; + dAASSERT (a); + a0 = a[0]; + a1 = a[1]; + a2 = a[2]; + aa0 = dFabs(a0); + aa1 = dFabs(a1); + aa2 = dFabs(a2); + if (aa1 > aa0) { + if (aa2 > aa1) { + goto aa2_largest; + } + else { // aa1 is largest + a0 /= aa1; + a2 /= aa1; + l = dRecipSqrt (a0*a0 + a2*a2 + 1); + a[0] = a0*l; + a[1] = dCopySign(l,a1); + a[2] = a2*l; + } + } + else { + if (aa2 > aa0) { + aa2_largest: // aa2 is largest + a0 /= aa2; + a1 /= aa2; + l = dRecipSqrt (a0*a0 + a1*a1 + 1); + a[0] = a0*l; + a[1] = a1*l; + a[2] = dCopySign(l,a2); + } + else { // aa0 is largest + if (aa0 <= 0) { + a[0] = 1; // if all a's are zero, this is where we'll end up. + a[1] = 0; // return a default unit length vector. + a[2] = 0; + return 0; + } + a1 /= aa0; + a2 /= aa0; + l = dRecipSqrt (a1*a1 + a2*a2 + 1); + a[0] = dCopySign(l,a0); + a[1] = a1*l; + a[2] = a2*l; + } + } + return 1; +} + +/* OLD VERSION */ +/* +void dNormalize3 (dVector3 a) +{ + dIASSERT (a); + dReal l = dDOT(a,a); + if (l > 0) { + l = dRecipSqrt(l); + a[0] *= l; + a[1] *= l; + a[2] *= l; + } + else { + a[0] = 1; + a[1] = 0; + a[2] = 0; + } +} +*/ + +void dNormalize3(dVector3 a) +{ + _dNormalize3(a); +} + + +int dSafeNormalize4 (dVector4 a) +{ + dAASSERT (a); + dReal l = dDOT(a,a)+a[3]*a[3]; + if (l > 0) { + l = dRecipSqrt(l); + a[0] *= l; + a[1] *= l; + a[2] *= l; + a[3] *= l; + return 1; + } + else { + a[0] = 1; + a[1] = 0; + a[2] = 0; + a[3] = 0; + return 0; + } +} + +void dNormalize4(dVector4 a) +{ + _dNormalize4(a); +} + + +void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q) +{ + dAASSERT (n && p && q); + if (dFabs(n[2]) > M_SQRT1_2) { + // choose p in y-z plane + dReal a = n[1]*n[1] + n[2]*n[2]; + dReal k = dRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + dReal a = n[0]*n[0] + n[1]*n[1]; + dReal k = dRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} diff --git a/libraries/ode-0.9/ode/src/plane.cpp b/libraries/ode-0.9/ode/src/plane.cpp new file mode 100644 index 0000000..e3afb49 --- /dev/null +++ b/libraries/ode-0.9/ode/src/plane.cpp @@ -0,0 +1,145 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// plane public API + +static void make_sure_plane_normal_has_unit_length (dxPlane *g) +{ + dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2]; + if (l > 0) { + l = dRecipSqrt(l); + g->p[0] *= l; + g->p[1] *= l; + g->p[2] *= l; + g->p[3] *= l; + } + else { + g->p[0] = 1; + g->p[1] = 0; + g->p[2] = 0; + g->p[3] = 0; + } +} + + +dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) : + dxGeom (space,0) +{ + type = dPlaneClass; + p[0] = a; + p[1] = b; + p[2] = c; + p[3] = d; + make_sure_plane_normal_has_unit_length (this); +} + + +void dxPlane::computeAABB() +{ + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + + // Planes that have normal vectors aligned along an axis can use a + // less comprehensive (half space) bounding box. + + if ( p[1] == 0.0f && p[2] == 0.0f ) { + // normal aligned with x-axis + aabb[0] = (p[0] > 0) ? -dInfinity : -p[3]; + aabb[1] = (p[0] > 0) ? p[3] : dInfinity; + } else + if ( p[0] == 0.0f && p[2] == 0.0f ) { + // normal aligned with y-axis + aabb[2] = (p[1] > 0) ? -dInfinity : -p[3]; + aabb[3] = (p[1] > 0) ? p[3] : dInfinity; + } else + if ( p[0] == 0.0f && p[1] == 0.0f ) { + // normal aligned with z-axis + aabb[4] = (p[2] > 0) ? -dInfinity : -p[3]; + aabb[5] = (p[2] > 0) ? p[3] : dInfinity; + } +} + + +dGeomID dCreatePlane (dSpaceID space, + dReal a, dReal b, dReal c, dReal d) +{ + return new dxPlane (space,a,b,c,d); +} + + +void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + p->p[0] = a; + p->p[1] = b; + p->p[2] = c; + p->p[3] = d; + make_sure_plane_normal_has_unit_length (p); + dGeomMoved (g); +} + + +void dGeomPlaneGetParams (dGeomID g, dVector4 result) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + result[0] = p->p[0]; + result[1] = p->p[1]; + result[2] = p->p[2]; + result[3] = p->p[3]; +} + + +dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z; +} diff --git a/libraries/ode-0.9/ode/src/quickstep.cpp b/libraries/ode-0.9/ode/src/quickstep.cpp new file mode 100644 index 0000000..07aa9f7 --- /dev/null +++ b/libraries/ode-0.9/ode/src/quickstep.cpp @@ -0,0 +1,880 @@ +/************************************************************************* + * * + * 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 "objects.h" +#include "joint.h" +#include +#include +#include +#include +#include +#include +#include +#include "lcp.h" +#include "util.h" + +#define ALLOCA dALLOCA16 + +typedef const dReal *dRealPtr; +typedef dReal *dRealMutablePtr; +#define dRealArray(name,n) dReal name[n]; +#define dRealAllocaArray(name,n) dReal *name = (dReal*) ALLOCA ((n)*sizeof(dReal)); + +//*************************************************************************** +// configuration + +// for the SOR and CG methods: +// uncomment the following line to use warm starting. this definitely +// help for motor-driven joints. unfortunately it appears to hurt +// with high-friction contacts using the SOR method. use with care + +//#define WARM_STARTING 1 + + +// for the SOR method: +// uncomment the following line to determine a new constraint-solving +// order for each iteration. however, the qsort per iteration is expensive, +// and the optimal order is somewhat problem dependent. +// @@@ try the leaf->root ordering. + +//#define REORDER_CONSTRAINTS 1 + + +// for the SOR method: +// uncomment the following line to randomly reorder constraint rows +// during the solution. depending on the situation, this can help a lot +// or hardly at all, but it doesn't seem to hurt. + +#define RANDOMLY_REORDER_CONSTRAINTS 1 + +//**************************************************************************** +// special matrix multipliers + +// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q) +static void Multiply1_12q1 (dReal *A, dReal *B, dReal *C, int q) +{ + int k; + dReal sum; + dIASSERT (q>0 && A && B && C); + sum = 0; + for (k=0; kinvMass; + for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; + dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); + if (b2 >= 0) { + k = body[b2]->invMass; + for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; + dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); + } + J_ptr += 12; + iMJ_ptr += 12; + } +} + + +// compute out = inv(M)*J'*in. + +static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dSetZero (out,6*nb); + dRealPtr iMJ_ptr = iMJ; + for (i=0; i= 0) { + out_ptr = out + b2*6; + for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; + } + iMJ_ptr += 6; + } +} + + +// compute out = J*in. + +static void multiply_J (int m, dRealMutablePtr J, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dRealPtr J_ptr = J; + for (i=0; i= 0) { + in_ptr = in + b2*6; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + } + J_ptr += 6; + out[i] = sum; + } +} + + +// compute out = (J*inv(M)*J' + cfm)*in. +// use z as an nb*6 temporary. + +static void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, + dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out) +{ + multiply_invM_JT (m,nb,iMJ,jb,in,z); + multiply_J (m,J,jb,z,out); + + // add cfm + for (int i=0; inum_iterations; + + // precompute iMJ = inv(M)*J' + dRealAllocaArray (iMJ,m*12); + compute_invM_JT (m,J,iMJ,jb,body,invI); + + dReal last_rho = 0; + dRealAllocaArray (r,m); + dRealAllocaArray (z,m); + dRealAllocaArray (p,m); + dRealAllocaArray (q,m); + + // precompute 1 / diagonals of A + dRealAllocaArray (Ad,m); + dRealPtr iMJ_ptr = iMJ; + dRealPtr J_ptr = J; + for (i=0; i= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = REAL(1.0) / (sum + cfm[i]); + } + +#ifdef WARM_STARTING + // compute residual r = b - A*lambda + multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r); + for (i=0; ifindex < 0 && i2->findex >= 0) return -1; + if (i1->findex >= 0 && i2->findex < 0) return 1; + if (i1->error < i2->error) return -1; + if (i1->error > i2->error) return 1; + return 0; +} + +#endif + + +static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body, + dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b, + dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, + dxQuickStepParameters *qs) +{ + const int num_iterations = qs->num_iterations; + const dReal sor_w = qs->w; // SOR over-relaxation parameter + + int i,j; + +#ifdef WARM_STARTING + // for warm starting, this seems to be necessary to prevent + // jerkiness in motor-driven joints. i have no idea why this works. + for (i=0; i= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = sor_w / (sum + cfm[i]); + } + + // scale J and b by Ad + J_ptr = J; + for (i=0; i v2) ? v1 : v2; + if (max > 0) { + //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; + order[i].error = dFabs(lambda[i]-last_lambda[i]); + } + else { + order[i].error = dInfinity; + } + order[i].findex = findex[i]; + order[i].index = i; + } + } + qsort (order,m,sizeof(IndexError),&compare_index_error); + + //@@@ potential optimization: swap lambda and last_lambda pointers rather + // than copying the data. we must make sure lambda is properly + // returned to the caller + memcpy (last_lambda,lambda,m*sizeof(dReal)); +#endif +#ifdef RANDOMLY_REORDER_CONSTRAINTS + if ((iteration & 7) == 0) { + for (i=1; i= 0) { + hi[index] = dFabs (hicopy[index] * lambda[findex[index]]); + lo[index] = -hi[index]; + } + + int b1 = jb[index*2]; + int b2 = jb[index*2+1]; + dReal delta = b[index] - lambda[index]*Ad[index]; + dRealMutablePtr fc_ptr = fc + 6*b1; + + // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case + delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + + fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = fc + 6*b2; + delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + + fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + + fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; + } + + // compute lambda and clamp it to [lo,hi]. + // @@@ potential optimization: does SSE have clamping instructions + // to save test+jump penalties here? + dReal new_lambda = lambda[index] + delta; + if (new_lambda < lo[index]) { + delta = lo[index]-lambda[index]; + lambda[index] = lo[index]; + } + else if (new_lambda > hi[index]) { + delta = hi[index]-lambda[index]; + lambda[index] = hi[index]; + } + else { + lambda[index] = new_lambda; + } + + //@@@ a trick that may or may not help + //dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations)); + //delta *= ramp; + + // update fc. + // @@@ potential optimization: SIMD for this and the b2 >= 0 case + fc_ptr = fc + 6*b1; + fc_ptr[0] += delta * iMJ_ptr[0]; + fc_ptr[1] += delta * iMJ_ptr[1]; + fc_ptr[2] += delta * iMJ_ptr[2]; + fc_ptr[3] += delta * iMJ_ptr[3]; + fc_ptr[4] += delta * iMJ_ptr[4]; + fc_ptr[5] += delta * iMJ_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = fc + 6*b2; + fc_ptr[0] += delta * iMJ_ptr[6]; + fc_ptr[1] += delta * iMJ_ptr[7]; + fc_ptr[2] += delta * iMJ_ptr[8]; + fc_ptr[3] += delta * iMJ_ptr[9]; + fc_ptr[4] += delta * iMJ_ptr[10]; + fc_ptr[5] += delta * iMJ_ptr[11]; + } + } + } +} + + +void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j; + IFTIMING(dTimerStart("preprocessing");) + + dReal stepsize1 = dRecip(stepsize); + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints + dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. + //dRealAllocaArray (I,3*4*nb); // need to remember all I's for feedback purposes only + dRealAllocaArray (invI,3*4*nb); + for (i=0; iinvI,body[i]->posr.R); + dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); +#ifdef dGYROSCOPIC + dMatrix3 I; + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R); + //dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp); + dMULTIPLY0_333 (I,body[i]->posr.R,tmp); + // compute rotational force + //dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dMULTIPLY0_331 (tmp,I,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); +#endif + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get joint information (m = total constraint dimension, nub = number of unbounded variables). + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + //@@@ do we really need to save all the info1's + dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // create the row offset array + int m = 0; + int *ofs = (int*) ALLOCA (nj*sizeof(int)); + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dRealAllocaArray (c,m); + dRealAllocaArray (cfm,m); + dRealAllocaArray (lo,m); + dRealAllocaArray (hi,m); + int *findex = (int*) ALLOCA (m*sizeof(int)); + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + int mfb = 0; // number of rows of Jacobian we will have to save for joint feedback + for (i=0; ivtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + if (joint[i]->feedback) + mfb += info[i].m; + } + + // we need a copy of Jacobian for joint feedbacks + // because it gets destroyed by SOR solver + // instead of saving all Jacobian, we can save just rows + // for joints, that requested feedback (which is normaly much less) + dRealAllocaArray (Jcopy,mfb*12); + if (mfb > 0) { + mfb = 0; + for (i=0; ifeedback) { + memcpy(Jcopy+mfb*12, J+ofs[i]*12, info[i].m*12*sizeof(dReal)); + mfb += info[i].m; + } + } + + + // create an array of body numbers for each joint row + int *jb_ptr = jb; + for (i=0; inode[0].body) ? (joint[i]->node[0].body->tag) : -1; + int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->tag) : -1; + for (j=0; jinvMass; + for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc); + for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1; + } + + // put J*tmp1 into rhs + dRealAllocaArray (rhs,m); + multiply_J (m,J,jb,tmp1,rhs); + + // complete rhs + for (i=0; ilambda,info[i].m * sizeof(dReal)); + } +#endif + + // solve the LCP problem and get lambda and invM*constraint_force + IFTIMING (dTimerNow ("solving LCP problem");) + dRealAllocaArray (cforce,nb*6); + SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,&world->qs); + +#ifdef WARM_STARTING + // save lambda for the next iteration + //@@@ note that this doesn't work for contact joints yet, as they are + // recreated every iteration + for (i=0; ilambda,lambda+ofs[i],info[i].m * sizeof(dReal)); + } +#endif + + // note that the SOR method overwrites rhs and J at this point, so + // they should not be used again. + + // add stepsize * cforce to the body velocity + for (i=0; ilvel[j] += stepsize * cforce[i*6+j]; + for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j]; + } + + // if joint feedback is requested, compute the constraint force. + // BUT: cforce is inv(M)*J'*lambda, whereas we want just J'*lambda, + // so we must compute M*cforce. + // @@@ if any joint has a feedback request we compute the entire + // adjusted cforce, which is not the most efficient way to do it. + /*for (j=0; jfeedback) { + // compute adjusted cforce + for (i=0; imass.mass; + cforce [i*6+0] *= k; + cforce [i*6+1] *= k; + cforce [i*6+2] *= k; + dVector3 tmp; + dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3); + cforce [i*6+3] = tmp[0]; + cforce [i*6+4] = tmp[1]; + cforce [i*6+5] = tmp[2]; + } + // compute feedback for this and all remaining joints + for (; jfeedback; + if (fb) { + int b1 = joint[j]->node[0].body->tag; + memcpy (fb->f1,cforce+b1*6,3*sizeof(dReal)); + memcpy (fb->t1,cforce+b1*6+3,3*sizeof(dReal)); + if (joint[j]->node[1].body) { + int b2 = joint[j]->node[1].body->tag; + memcpy (fb->f2,cforce+b2*6,3*sizeof(dReal)); + memcpy (fb->t2,cforce+b2*6+3,3*sizeof(dReal)); + } + } + } + } + }*/ + + if (mfb > 0) { + // straightforward computation of joint constraint forces: + // multiply related lambdas with respective J' block for joints + // where feedback was requested + mfb = 0; + for (i=0; ifeedback) { + dJointFeedback *fb = joint[i]->feedback; + dReal data[6]; + Multiply1_12q1 (data, Jcopy+mfb*12, lambda+ofs[i], info[i].m); + fb->f1[0] = data[0]; + fb->f1[1] = data[1]; + fb->f1[2] = data[2]; + fb->t1[0] = data[3]; + fb->t1[1] = data[4]; + fb->t1[2] = data[5]; + if (joint[i]->node[1].body) + { + Multiply1_12q1 (data, Jcopy+mfb*12+6, lambda+ofs[i], info[i].m); + fb->f2[0] = data[0]; + fb->f2[1] = data[1]; + fb->f2[2] = data[2]; + fb->t2[0] = data[3]; + fb->t2[1] = data[4]; + fb->t2[2] = data[5]; + } + mfb += info[i].m; + } + } + } + } + + // compute the velocity update: + // add stepsize * invM * fe to the body velocity + + IFTIMING (dTimerNow ("compute velocity update");) + for (i=0; iinvMass; + for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j]; + for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize; + dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc); + } + +#if 0 + // check that the updated velocity obeys the constraint (this check needs unmodified J) + dRealAllocaArray (vel,nb*6); + for (i=0; ilvel[j]; + for (j=0; j<3; j++) vel[i*6+3+j] = body[i]->avel[j]; + } + dRealAllocaArray (tmp,m); + multiply_J (m,J,jb,vel,tmp); + dReal error = 0; + for (i=0; ifacc,3); + dSetZero (body[i]->tacc,3); + } + + IFTIMING (dTimerEnd();) + IFTIMING (if (m > 0) dTimerReport (stdout,1);) +} diff --git a/libraries/ode-0.9/ode/src/quickstep.h b/libraries/ode-0.9/ode/src/quickstep.h new file mode 100644 index 0000000..43863e7 --- /dev/null +++ b/libraries/ode-0.9/ode/src/quickstep.h @@ -0,0 +1,33 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_QUICK_STEP_H_ +#define _ODE_QUICK_STEP_H_ + +#include + + +void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize); + + +#endif diff --git a/libraries/ode-0.9/ode/src/ray.cpp b/libraries/ode-0.9/ode/src/ray.cpp new file mode 100644 index 0000000..adf61ac --- /dev/null +++ b/libraries/ode-0.9/ode/src/ray.cpp @@ -0,0 +1,686 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// ray public API + +dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1) +{ + type = dRayClass; + length = _length; +} + + +void dxRay::computeAABB() +{ + dVector3 e; + e[0] = final_posr->pos[0] + final_posr->R[0*4+2]*length; + e[1] = final_posr->pos[1] + final_posr->R[1*4+2]*length; + e[2] = final_posr->pos[2] + final_posr->R[2*4+2]*length; + + if (final_posr->pos[0] < e[0]){ + aabb[0] = final_posr->pos[0]; + aabb[1] = e[0]; + } + else{ + aabb[0] = e[0]; + aabb[1] = final_posr->pos[0]; + } + + if (final_posr->pos[1] < e[1]){ + aabb[2] = final_posr->pos[1]; + aabb[3] = e[1]; + } + else{ + aabb[2] = e[1]; + aabb[3] = final_posr->pos[1]; + } + + if (final_posr->pos[2] < e[2]){ + aabb[4] = final_posr->pos[2]; + aabb[5] = e[2]; + } + else{ + aabb[4] = e[2]; + aabb[5] = final_posr->pos[2]; + } +} + + +dGeomID dCreateRay (dSpaceID space, dReal length) +{ + return new dxRay (space,length); +} + + +void dGeomRaySetLength (dGeomID g, dReal length) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + r->length = length; + dGeomMoved (g); +} + + +dReal dGeomRayGetLength (dGeomID g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + return r->length; +} + + +void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, + dReal dx, dReal dy, dReal dz) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + g->recomputePosr(); + dReal* rot = g->final_posr->R; + dReal* pos = g->final_posr->pos; + dVector3 n; + pos[0] = px; + pos[1] = py; + pos[2] = pz; + + n[0] = dx; + n[1] = dy; + n[2] = dz; + dNormalize3(n); + rot[0*4+2] = n[0]; + rot[1*4+2] = n[1]; + rot[2*4+2] = n[2]; + dGeomMoved (g); +} + + +void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + g->recomputePosr(); + start[0] = g->final_posr->pos[0]; + start[1] = g->final_posr->pos[1]; + start[2] = g->final_posr->pos[2]; + dir[0] = g->final_posr->R[0*4+2]; + dir[1] = g->final_posr->R[1*4+2]; + dir[2] = g->final_posr->R[2*4+2]; +} + + +void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + if (FirstContact){ + g->gflags |= RAY_FIRSTCONTACT; + } + else g->gflags &= ~RAY_FIRSTCONTACT; + + if (BackfaceCull){ + g->gflags |= RAY_BACKFACECULL; + } + else g->gflags &= ~RAY_BACKFACECULL; +} + + +void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0); + (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0); +} + + +void dGeomRaySetClosestHit (dxGeom *g, int closestHit) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + if (closestHit){ + g->gflags |= RAY_CLOSEST_HIT; + } + else g->gflags &= ~RAY_CLOSEST_HIT; +} + + +int dGeomRayGetClosestHit (dxGeom *g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + return ((g->gflags & RAY_CLOSEST_HIT) != 0); +} + + + +// if mode==1 then use the sphere exit contact, not the entry contact + +static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, + dContactGeom *contact, int mode) +{ + dVector3 q; + q[0] = ray->final_posr->pos[0] - sphere_pos[0]; + q[1] = ray->final_posr->pos[1] - sphere_pos[1]; + q[2] = ray->final_posr->pos[2] - sphere_pos[2]; + dReal B = dDOT14(q,ray->final_posr->R+2); + dReal C = dDOT(q,q) - radius*radius; + // note: if C <= 0 then the start of the ray is inside the sphere + dReal k = B*B - C; + if (k < 0) return 0; + k = dSqrt(k); + dReal alpha; + if (mode && C >= 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + else { + alpha = -B - k; + if (alpha < 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + } + if (alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0); + contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]); + contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]); + contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; +} + + +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxSphere *sphere = (dxSphere*) o2; + contact->g1 = ray; + contact->g2 = sphere; + return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0); +} + + +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = ray; + contact->g2 = box; + + int i; + + // compute the start and delta of the ray relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0]; + tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1]; + tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2]; + dMULTIPLY1_331 (s,box->final_posr->R,tmp); + tmp[0] = ray->final_posr->R[0*4+2]; + tmp[1] = ray->final_posr->R[1*4+2]; + tmp[2] = ray->final_posr->R[2*4+2]; + dMULTIPLY1_331 (v,box->final_posr->R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = 1; + } + else sign[i] = -1; + } + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * box->side[0]; + h[1] = REAL(0.5) * box->side[1]; + h[2] = REAL(0.5) * box->side[2]; + + // do a few early exit tests + if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || + (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || + (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || + (v[0] == 0 && v[1] == 0 && v[2] == 0)) { + return 0; + } + + // compute the t=[lo..hi] range for where s+v*t intersects the box + dReal lo = -dInfinity; + dReal hi = dInfinity; + int nlo = 0, nhi = 0; + for (i=0; i<3; i++) { + if (v[i] != 0) { + dReal k = (-h[i] - s[i])/v[i]; + if (k > lo) { + lo = k; + nlo = i; + } + k = (h[i] - s[i])/v[i]; + if (k < hi) { + hi = k; + nhi = i; + } + } + } + + // check if the ray intersects + if (lo > hi) return 0; + dReal alpha; + int n; + if (lo >= 0) { + alpha = lo; + n = nlo; + } + else { + alpha = hi; + n = nhi; + } + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + contact->normal[0] = box->final_posr->R[0*4+n] * sign[n]; + contact->normal[1] = box->final_posr->R[1*4+n] * sign[n]; + contact->normal[2] = box->final_posr->R[2*4+n] * sign[n]; + contact->depth = alpha; + return 1; +} + + +int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dCapsuleClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxCapsule *ccyl = (dxCapsule*) o2; + + contact->g1 = ray; + contact->g2 = ccyl; + dReal lz2 = ccyl->lz * REAL(0.5); + + // compute some useful info + dVector3 cs,q,r; + dReal C,k; + cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0]; + cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1]; + cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2]; + k = dDOT41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis + q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0]; + q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1]; + q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2]; + C = dDOT(q,q) - ccyl->radius*ccyl->radius; + // if C < 0 then ray start position within infinite extension of cylinder + + // see if ray start position is inside the capped cylinder + int inside_ccyl = 0; + if (C < 0) { + if (k < -lz2) k = -lz2; + else if (k > lz2) k = lz2; + r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]; + r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]; + r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]; + if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) + + (ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) + + (ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) { + inside_ccyl = 1; + } + } + + // compute ray collision with infinite cylinder, except for the case where + // the ray is outside the capped cylinder but within the infinite cylinder + // (it that case the ray can only hit endcaps) + if (!inside_ccyl && C < 0) { + // set k to cap position to check + if (k < 0) k = -lz2; else k = lz2; + } + else { + dReal uv = dDOT44(ccyl->final_posr->R+2,ray->final_posr->R+2); + r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2]; + r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2]; + r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2]; + dReal A = dDOT(r,r); + dReal B = 2*dDOT(q,r); + k = B*B-4*A*C; + if (k < 0) { + // the ray does not intersect the infinite cylinder, but if the ray is + // inside and parallel to the cylinder axis it may intersect the end + // caps. set k to cap position to check. + if (!inside_ccyl) return 0; + if (uv < 0) k = -lz2; else k = lz2; + } + else { + k = dSqrt(k); + A = dRecip (2*A); + dReal alpha = (-B-k)*A; + if (alpha < 0) { + alpha = (-B+k)*A; + if (alpha < 0) return 0; + } + if (alpha > ray->length) return 0; + + // the ray intersects the infinite cylinder. check to see if the + // intersection point is between the caps + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + q[0] = contact->pos[0] - ccyl->final_posr->pos[0]; + q[1] = contact->pos[1] - ccyl->final_posr->pos[1]; + q[2] = contact->pos[2] - ccyl->final_posr->pos[2]; + k = dDOT14(q,ccyl->final_posr->R+2); + dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0); + if (k >= -lz2 && k <= lz2) { + contact->normal[0] = nsign * (contact->pos[0] - + (ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2])); + contact->normal[1] = nsign * (contact->pos[1] - + (ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2])); + contact->normal[2] = nsign * (contact->pos[2] - + (ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2])); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; + } + + // the infinite cylinder intersection point is not between the caps. + // set k to cap position to check. + if (k < 0) k = -lz2; else k = lz2; + } + } + + // check for ray intersection with the caps. k must indicate the cap + // position to check + q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]; + q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]; + q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]; + return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl); +} + + +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxRay *ray = (dxRay*) o1; + dxPlane *plane = (dxPlane*) o2; + + dReal alpha = plane->p[3] - dDOT (plane->p,ray->final_posr->pos); + // note: if alpha > 0 the starting point is below the plane + dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0); + dReal k = dDOT14(plane->p,ray->final_posr->R+2); + if (k==0) return 0; // ray parallel to plane + alpha /= k; + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; + contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; + contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; + contact->normal[0] = nsign*plane->p[0]; + contact->normal[1] = nsign*plane->p[1]; + contact->normal[2] = nsign*plane->p[2]; + contact->depth = alpha; + contact->g1 = ray; + contact->g2 = plane; + return 1; +} + +// Ray - Cylinder collider by David Walters (June 2006) +int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip ) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dRayClass ); + dIASSERT( o2->type == dCylinderClass ); + dIASSERT( (flags & NUMC_MASK) >= 1 ); + + dxRay* ray = (dxRay*)( o1 ); + dxCylinder* cyl = (dxCylinder*)( o2 ); + + // Fill in contact information. + contact->g1 = ray; + contact->g2 = cyl; + + const dReal half_length = cyl->lz * REAL( 0.5 ); + + // + // Compute some useful info + // + + dVector3 q, r; + dReal d, C, k; + + // Vector 'r', line segment from C to R (ray start) ( r = R - C ) + r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0]; + r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1]; + r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2]; + + // Distance that ray start is along cyl axis ( Z-axis direction ) + d = dDOT41( cyl->final_posr->R + 2, r ); + + // + // Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz). + // + // Point on axis ( in world space ): cp = ( d * Cz ) + C + // + // Line 'q' from R to cp: q = cp - R + // q = ( d * Cz ) + C - R + // q = ( d * Cz ) - ( R - C ) + + q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ]; + q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ]; + q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ]; + + + // Compute square length of 'q'. Subtract from radius squared to + // get square distance 'C' between the line q and the radius. + + // if C < 0 then ray start position is within infinite extension of cylinder + + C = dDOT( q, q ) - ( cyl->radius * cyl->radius ); + + // Compute the projection of ray direction normal onto cylinder direction normal. + dReal uv = dDOT44( cyl->final_posr->R+2, ray->final_posr->R+2 ); + + + + // + // Find ray collision with infinite cylinder + // + + // Compute vector from end of ray direction normal to projection on cylinder direction normal. + r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2]; + r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2]; + r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2]; + + + // Quadratic Formula Magic + // Compute discriminant 'k': + + // k < 0 : No intersection + // k = 0 : Tangent + // k > 0 : Intersection + + dReal A = dDOT( r, r ); + dReal B = 2 * dDOT( q, r ); + + k = B*B - 4*A*C; + + + + + // + // Collision with Flat Caps ? + // + + // No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases ) + if ( k < dEpsilon && C <= 0 ) + { + // The ray does not intersect the edge of the infinite cylinder, + // but the ray start is inside and so must run parallel to the axis. + // It may yet intersect an end cap. The following cases are valid: + + // -ve-cap , -half centre +half , +ve-cap + // <<================|-------------------|------------->>>---|================>> + // | | + // | d-------------------> 1. + // 2. d------------------> | + // 3. <------------------d | + // | <-------------------d 4. + // | | + // <<================|-------------------|------------->>>---|===============>> + + // Negative if the ray and cylinder axes point in opposite directions. + const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 ); + + // Negative if the ray start is inside the cylinder + const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 ); + + // Ray and Cylinder axes run in the same direction ( cases 1, 2 ) + // Ray and Cylinder axes run in opposite directions ( cases 3, 4 ) + if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) || + ( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) ) + { + return 0; // No intersection with caps or curved surface. + } + + // Compute depth (distance from ray to cylinder) + contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) ); + + // Compute contact point. + contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] ); + contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] ); + contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] ); + + // Compute reflected contact normal. + contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] ); + contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] ); + contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] ); + + // Contact! + return 1; + } + + + + // + // Collision with Curved Edge ? + // + + if ( k > 0 ) + { + // Finish off quadratic formula to get intersection co-efficient + k = dSqrt( k ); + A = dRecip( 2 * A ); + + // Compute distance along line to contact point. + dReal alpha = ( -B - k ) * A; + if ( alpha < 0 ) + { + // Flip in the other direction. + alpha = ( -B + k ) * A; + } + + // Intersection point is within ray length? + if ( alpha >= 0 && alpha <= ray->length ) + { + // The ray intersects the infinite cylinder! + + // Compute contact point. + contact->pos[0] = ray->final_posr->pos[0] + ( alpha * ray->final_posr->R[0*4+2] ); + contact->pos[1] = ray->final_posr->pos[1] + ( alpha * ray->final_posr->R[1*4+2] ); + contact->pos[2] = ray->final_posr->pos[2] + ( alpha * ray->final_posr->R[2*4+2] ); + + // q is the vector from the cylinder centre to the contact point. + q[0] = contact->pos[0] - cyl->final_posr->pos[0]; + q[1] = contact->pos[1] - cyl->final_posr->pos[1]; + q[2] = contact->pos[2] - cyl->final_posr->pos[2]; + + // Compute the distance along the cylinder axis of this contact point. + d = dDOT14( q, cyl->final_posr->R+2 ); + + // Check to see if the intersection point is between the flat end caps + if ( d >= -half_length && d <= +half_length ) + { + // Flip the normal if the start point is inside the cylinder. + const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 ); + + // Compute contact normal. + contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2])); + contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2])); + contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2])); + dNormalize3( contact->normal ); + + // Store depth. + contact->depth = alpha; + + // Contact! + return 1; + } + } + } + + // No contact with anything. + return 0; +} + diff --git a/libraries/ode-0.9/ode/src/rotation.cpp b/libraries/ode-0.9/ode/src/rotation.cpp new file mode 100644 index 0000000..adb933b --- /dev/null +++ b/libraries/ode-0.9/ode/src/rotation.cpp @@ -0,0 +1,316 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the +"rotation axis" and s is the "rotation angle". + +*/ + +#include +#include + + +#define _R(i,j) R[(i)*4+(j)] + +#define SET_3x3_IDENTITY \ + _R(0,0) = REAL(1.0); \ + _R(0,1) = REAL(0.0); \ + _R(0,2) = REAL(0.0); \ + _R(0,3) = REAL(0.0); \ + _R(1,0) = REAL(0.0); \ + _R(1,1) = REAL(1.0); \ + _R(1,2) = REAL(0.0); \ + _R(1,3) = REAL(0.0); \ + _R(2,0) = REAL(0.0); \ + _R(2,1) = REAL(0.0); \ + _R(2,2) = REAL(1.0); \ + _R(2,3) = REAL(0.0); + + +void dRSetIdentity (dMatrix3 R) +{ + dAASSERT (R); + SET_3x3_IDENTITY; +} + + +void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal angle) +{ + dAASSERT (R); + dQuaternion q; + dQFromAxisAndAngle (q,ax,ay,az,angle); + dQtoR (q,R); +} + + +void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi) +{ + dReal sphi,cphi,stheta,ctheta,spsi,cpsi; + dAASSERT (R); + sphi = dSin(phi); + cphi = dCos(phi); + stheta = dSin(theta); + ctheta = dCos(theta); + spsi = dSin(psi); + cpsi = dCos(psi); + _R(0,0) = cpsi*ctheta; + _R(0,1) = spsi*ctheta; + _R(0,2) =-stheta; + _R(0,3) = REAL(0.0); + _R(1,0) = cpsi*stheta*sphi - spsi*cphi; + _R(1,1) = spsi*stheta*sphi + cpsi*cphi; + _R(1,2) = ctheta*sphi; + _R(1,3) = REAL(0.0); + _R(2,0) = cpsi*stheta*cphi + spsi*sphi; + _R(2,1) = spsi*stheta*cphi - cpsi*sphi; + _R(2,2) = ctheta*cphi; + _R(2,3) = REAL(0.0); +} + + +void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal bx, dReal by, dReal bz) +{ + dReal l,k; + dAASSERT (R); + l = dSqrt (ax*ax + ay*ay + az*az); + if (l <= REAL(0.0)) { + dDEBUGMSG ("zero length vector"); + return; + } + l = dRecip(l); + ax *= l; + ay *= l; + az *= l; + k = ax*bx + ay*by + az*bz; + bx -= k*ax; + by -= k*ay; + bz -= k*az; + l = dSqrt (bx*bx + by*by + bz*bz); + if (l <= REAL(0.0)) { + dDEBUGMSG ("zero length vector"); + return; + } + l = dRecip(l); + bx *= l; + by *= l; + bz *= l; + _R(0,0) = ax; + _R(1,0) = ay; + _R(2,0) = az; + _R(0,1) = bx; + _R(1,1) = by; + _R(2,1) = bz; + _R(0,2) = - by*az + ay*bz; + _R(1,2) = - bz*ax + az*bx; + _R(2,2) = - bx*ay + ax*by; + _R(0,3) = REAL(0.0); + _R(1,3) = REAL(0.0); + _R(2,3) = REAL(0.0); +} + + +void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az) +{ + dVector3 n,p,q; + n[0] = ax; + n[1] = ay; + n[2] = az; + dNormalize3 (n); + dPlaneSpace (n,p,q); + _R(0,0) = p[0]; + _R(1,0) = p[1]; + _R(2,0) = p[2]; + _R(0,1) = q[0]; + _R(1,1) = q[1]; + _R(2,1) = q[2]; + _R(0,2) = n[0]; + _R(1,2) = n[1]; + _R(2,2) = n[2]; + _R(0,3) = REAL(0.0); + _R(1,3) = REAL(0.0); + _R(2,3) = REAL(0.0); +} + + +void dQSetIdentity (dQuaternion q) +{ + dAASSERT (q); + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; +} + + +void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, + dReal angle) +{ + dAASSERT (q); + dReal l = ax*ax + ay*ay + az*az; + if (l > REAL(0.0)) { + angle *= REAL(0.5); + q[0] = dCos (angle); + l = dSin(angle) * dRecipSqrt(l); + q[1] = ax*l; + q[2] = ay*l; + q[3] = az*l; + } + else { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } +} + + +void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; + qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; + qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; + qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; +} + + +void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; + qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; + qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; + qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; +} + + +void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; + qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; + qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; + qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; +} + + +void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; + qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; + qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; + qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; +} + + +// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction +// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained +// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon +// University, 1997. + +void dRfromQ (dMatrix3 R, const dQuaternion q) +{ + dAASSERT (q && R); + // q = (s,vx,vy,vz) + dReal qq1 = 2*q[1]*q[1]; + dReal qq2 = 2*q[2]*q[2]; + dReal qq3 = 2*q[3]*q[3]; + _R(0,0) = 1 - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = REAL(0.0); + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1 - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = REAL(0.0); + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1 - qq1 - qq2; + _R(2,3) = REAL(0.0); +} + + +void dQfromR (dQuaternion q, const dMatrix3 R) +{ + dAASSERT (q && R); + dReal tr,s; + tr = _R(0,0) + _R(1,1) + _R(2,2); + if (tr >= 0) { + s = dSqrt (tr + 1); + q[0] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[1] = (_R(2,1) - _R(1,2)) * s; + q[2] = (_R(0,2) - _R(2,0)) * s; + q[3] = (_R(1,0) - _R(0,1)) * s; + } + else { + // find the largest diagonal element and jump to the appropriate case + if (_R(1,1) > _R(0,0)) { + if (_R(2,2) > _R(1,1)) goto case_2; + goto case_1; + } + if (_R(2,2) > _R(0,0)) goto case_2; + goto case_0; + + case_0: + s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1); + q[1] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[2] = (_R(0,1) + _R(1,0)) * s; + q[3] = (_R(2,0) + _R(0,2)) * s; + q[0] = (_R(2,1) - _R(1,2)) * s; + return; + + case_1: + s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1); + q[2] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[3] = (_R(1,2) + _R(2,1)) * s; + q[1] = (_R(0,1) + _R(1,0)) * s; + q[0] = (_R(0,2) - _R(2,0)) * s; + return; + + case_2: + s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1); + q[3] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[1] = (_R(2,0) + _R(0,2)) * s; + q[2] = (_R(1,2) + _R(2,1)) * s; + q[0] = (_R(1,0) - _R(0,1)) * s; + return; + } +} + + +void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q) +{ + dAASSERT (w && q && dq); + dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]); + dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]); + dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]); + dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]); +} diff --git a/libraries/ode-0.9/ode/src/scrapbook.cpp b/libraries/ode-0.9/ode/src/scrapbook.cpp new file mode 100644 index 0000000..2621814 --- /dev/null +++ b/libraries/ode-0.9/ode/src/scrapbook.cpp @@ -0,0 +1,485 @@ + +/* + +this is code that was once useful but has now been obseleted. + +this file should not be compiled as part of ODE! + +*/ + +//*************************************************************************** +// intersect a line segment with a plane + +extern "C" int dClipLineToBox (const dVector3 p1, const dVector3 p2, + const dVector3 p, const dMatrix3 R, + const dVector3 side) +{ + // compute the start and end of the line (p1 and p2) relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,e; + tmp[0] = p1[0] - p[0]; + tmp[1] = p1[1] - p[1]; + tmp[2] = p1[2] - p[2]; + dMULTIPLY1_331 (s,R,tmp); + tmp[0] = p2[0] - p[0]; + tmp[1] = p2[1] - p[1]; + tmp[2] = p2[2] - p[2]; + dMULTIPLY1_331 (e,R,tmp); + + // compute the vector 'v' from the start point to the end point + dVector3 v; + v[0] = e[0] - s[0]; + v[1] = e[1] - s[1]; + v[2] = e[2] - s[2]; + + // a point on the line is defined by the parameter 't'. t=0 corresponds + // to the start of the line, t=1 corresponds to the end of the line. + // we will clip the line to the box by finding the range of t where a + // point on the line is inside the box. the currently known bounds for + // t and tlo..thi. + dReal tlo=0,thi=1; + + // clip in the X/Y/Z direction + for (int i=0; i<3; i++) { + // first adjust s,e for the current t range. this is redundant for the + // first iteration, but never mind. + e[i] = s[i] + thi*v[i]; + s[i] = s[i] + tlo*v[i]; + // compute where t intersects the positive and negative sides. + dReal tp = ( side[i] - s[i])/v[i]; // @@@ handle case where denom=0 + dReal tm = (-side[i] - s[i])/v[i]; + // handle 9 intersection cases + if (s[i] <= -side[i]) { + tlo = tm; + if (e[i] <= -side[i]) return 0; + else if (e[i] >= side[i]) thi = tp; + } + else if (s[i] <= side[i]) { + if (e[i] <= -side[i]) thi = tm; + else if (e[i] >= side[i]) thi = tp; + } + else { + tlo = tp; + if (e[i] <= -side[i]) thi = tm; + else if (e[i] >= side[i]) return 0; + } + } + + //... @@@ AT HERE @@@ + + return 1; +} + + +//*************************************************************************** +// a nice try at C-B collision. unfortunately it doesn't work. the logic +// for testing for line-box intersection is correct, but unfortunately the +// closest-point distance estimates are often too large. as a result contact +// points are placed incorrectly. + + +int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + int i; + + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->_class->num == dCCylinderClass); + dIASSERT (o2->_class->num == dBoxClass); + contact->g1 = const_cast (o1); + contact->g2 = const_cast (o2); + dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1); + dxBox *box = (dxBox*) CLASSDATA(o2); + + // get p1,p2 = cylinder axis endpoints, get radius + dVector3 p1,p2; + dReal clen = cyl->lz * REAL(0.5); + p1[0] = o1->pos[0] + clen * o1->R[2]; + p1[1] = o1->pos[1] + clen * o1->R[6]; + p1[2] = o1->pos[2] + clen * o1->R[10]; + p2[0] = o1->pos[0] - clen * o1->R[2]; + p2[1] = o1->pos[1] - clen * o1->R[6]; + p2[2] = o1->pos[2] - clen * o1->R[10]; + dReal radius = cyl->radius; + + // copy out box center, rotation matrix, and side array + dReal *c = o2->pos; + dReal *R = o2->R; + dReal *side = box->side; + + // compute the start and end of the line (p1 and p2) relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp3,s,e; + tmp3[0] = p1[0] - c[0]; + tmp3[1] = p1[1] - c[1]; + tmp3[2] = p1[2] - c[2]; + dMULTIPLY1_331 (s,R,tmp3); + tmp3[0] = p2[0] - c[0]; + tmp3[1] = p2[1] - c[1]; + tmp3[2] = p2[2] - c[2]; + dMULTIPLY1_331 (e,R,tmp3); + + // compute the vector 'v' from the start point to the end point + dVector3 v; + v[0] = e[0] - s[0]; + v[1] = e[1] - s[1]; + v[2] = e[2] - s[2]; + + // compute the half-sides of the box + dReal S0 = side[0] * REAL(0.5); + dReal S1 = side[1] * REAL(0.5); + dReal S2 = side[2] * REAL(0.5); + + // compute the size of the bounding box around the line segment + dReal B0 = dFabs (v[0]); + dReal B1 = dFabs (v[1]); + dReal B2 = dFabs (v[2]); + + // for all 6 separation axes, measure the penetration depth. if any depth is + // less than 0 then the objects don't penetrate at all so we can just + // return 0. find the axis with the smallest depth, and record its normal. + + // note: normalR is set to point to a column of R if that is the smallest + // depth normal so far. otherwise normalR is 0 and normalC is set to a + // vector relative to the box. invert_normal is 1 if the sign of the normal + // should be flipped. + + dReal depth,trial_depth,tmp,length; + const dReal *normalR=0; + dVector3 normalC; + int invert_normal = 0; + int code = 0; // 0=no contact, 1-3=face contact, 4-6=edge contact + + depth = dInfinity; + + // look at face-normal axes + +#undef TEST +#define TEST(center,depth_expr,norm,contact_code) \ + tmp = (center); \ + trial_depth = radius + REAL(0.5) * ((depth_expr) - dFabs(tmp)); \ + if (trial_depth < 0) return 0; \ + if (trial_depth < depth) { \ + depth = trial_depth; \ + normalR = (norm); \ + invert_normal = (tmp < 0); \ + code = contact_code; \ + } + + TEST (s[0]+e[0], side[0] + B0, R+0, 1); + TEST (s[1]+e[1], side[1] + B1, R+1, 2); + TEST (s[2]+e[2], side[2] + B2, R+2, 3); + + // look at v x box-edge axes + +#undef TEST +#define TEST(box_radius,line_offset,nx,ny,nz,contact_code) \ + tmp = (line_offset); \ + trial_depth = (box_radius) - dFabs(tmp); \ + length = dSqrt ((nx)*(nx) + (ny)*(ny) + (nz)*(nz)); \ + if (length > 0) { \ + length = dRecip(length); \ + trial_depth = trial_depth * length + radius; \ + if (trial_depth < 0) return 0; \ + if (trial_depth < depth) { \ + depth = trial_depth; \ + normalR = 0; \ + normalC[0] = (nx)*length; \ + normalC[1] = (ny)*length; \ + normalC[2] = (nz)*length; \ + invert_normal = (tmp < 0); \ + code = contact_code; \ + } \ + } + + TEST (B2*S1+B1*S2,v[1]*s[2]-v[2]*s[1], 0,-v[2],v[1], 4); + TEST (B2*S0+B0*S2,v[2]*s[0]-v[0]*s[2], v[2],0,-v[0], 5); + TEST (B1*S0+B0*S1,v[0]*s[1]-v[1]*s[0], -v[1],v[0],0, 6); + +#undef TEST + + // if we get to this point, the box and ccylinder interpenetrate. + // compute the normal in global coordinates. + dReal *normal = contact[0].normal; + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + + // set the depth + contact[0].depth = depth; + + if (code == 0) { + return 0; // should never get here + } + else if (code >= 4) { + // handle edge contacts + // find an endpoint q1 on the intersecting edge of the box + dVector3 q1; + dReal sign[3]; + for (i=0; i<3; i++) q1[i] = c[i]; + sign[0] = (dDOT14(normal,R+0) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) q1[i] += sign[0] * S0 * R[i*4]; + sign[1] = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) q1[i] += sign[1] * S1 * R[i*4+1]; + sign[2] = (dDOT14(normal,R+2) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) q1[i] += sign[2] * S2 * R[i*4+2]; + + // find the other endpoint q2 of the intersecting edge + dVector3 q2; + for (i=0; i<3; i++) + q2[i] = q1[i] - R[code-4 + i*4] * (sign[code-4] * side[code-4]); + + // determine the closest point between the box edge and the line segment + dVector3 cp1,cp2; + dClosestLineSegmentPoints (q1,q2, p1,p2, cp1,cp2); + for (i=0; i<3; i++) contact[0].pos[i] = cp1[i] - REAL(0.5)*normal[i]*depth; + return 1; + } + else { + // handle face contacts. + // @@@ temporary: make deepest vertex on the line the contact point. + // @@@ this kind of works, but we sometimes need two contact points for + // @@@ stability. + + // compute 'v' in global coordinates + dVector3 gv; + for (i=0; i<3; i++) gv[i] = p2[i] - p1[i]; + + if (dDOT (normal,gv) > 0) { + for (i=0; i<3; i++) + contact[0].pos[i] = p1[i] + (depth*REAL(0.5)-radius)*normal[i]; + } + else { + for (i=0; i<3; i++) + contact[0].pos[i] = p2[i] + (depth*REAL(0.5)-radius)*normal[i]; + } + return 1; + } +} + +//*************************************************************************** +// this function works, it's just not being used for anything at the moment: + +// given a box (R,side), `R' is the rotation matrix for the box, and `side' +// is a vector of x/y/z side lengths, return the size of the interval of the +// box projected along the given axis. if the axis has unit length then the +// return value will be the actual diameter, otherwise the result will be +// scaled by the axis length. + +static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side, + const dVector3 axis) +{ + dVector3 q; + dMULTIPLY1_331 (q,R,axis); // transform axis to body-relative + return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + dFabs(q[2])*side[2]; +} + +//*************************************************************************** +// the old capped cylinder to capped cylinder collision code. this fails to +// detect cap-to-cap contact points when the cylinder axis are aligned, but +// other that that it is pretty robust. + +// this returns at most one contact point when the two cylinder's axes are not +// aligned, and at most two (for stability) when they are aligned. +// the algorithm minimizes the distance between two "sample spheres" that are +// positioned along the cylinder axes according to: +// sphere1 = pos1 + alpha1 * axis1 +// sphere2 = pos2 + alpha2 * axis2 +// alpha1 and alpha2 are limited to +/- half the length of the cylinders. +// the algorithm works by finding a solution that has both alphas free, or +// a solution that has one or both alphas fixed to the ends of the cylinder. + +int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + int i; + const dReal tolerance = REAL(1e-5); + + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dCCylinderClass); + dxCCylinder *cyl1 = (dxCCylinder*) o1; + dxCCylinder *cyl2 = (dxCCylinder*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // copy out some variables, for convenience + dReal lz1 = cyl1->lz * REAL(0.5); + dReal lz2 = cyl2->lz * REAL(0.5); + dReal *pos1 = o1->pos; + dReal *pos2 = o2->pos; + dReal axis1[3],axis2[3]; + axis1[0] = o1->R[2]; + axis1[1] = o1->R[6]; + axis1[2] = o1->R[10]; + axis2[0] = o2->R[2]; + axis2[1] = o2->R[6]; + axis2[2] = o2->R[10]; + + dReal alpha1,alpha2,sphere1[3],sphere2[3]; + int fix1 = 0; // 0 if alpha1 is free, +/-1 to fix at +/- lz1 + int fix2 = 0; // 0 if alpha2 is free, +/-1 to fix at +/- lz2 + + for (int count=0; count<9; count++) { + // find a trial solution by fixing or not fixing the alphas + if (fix1) { + if (fix2) { + // alpha1 and alpha2 are fixed, so the solution is easy + if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1; + if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2; + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + } + else { + // fix alpha1 but let alpha2 be free + if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1; + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + alpha2 = (axis2[0]*(sphere1[0]-pos2[0]) + + axis2[1]*(sphere1[1]-pos2[1]) + + axis2[2]*(sphere1[2]-pos2[2])); + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + } + } + else { + if (fix2) { + // fix alpha2 but let alpha1 be free + if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + alpha1 = (axis1[0]*(sphere2[0]-pos1[0]) + + axis1[1]*(sphere2[1]-pos1[1]) + + axis1[2]*(sphere2[2]-pos1[2])); + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + } + else { + // let alpha1 and alpha2 be free + // compute determinant of d(d^2)\d(alpha) jacobian + dReal a1a2 = dDOT (axis1,axis2); + dReal det = REAL(1.0)-a1a2*a1a2; + if (det < tolerance) { + // the cylinder axes (almost) parallel, so we will generate up to two + // contacts. the solution matrix is rank deficient so alpha1 and + // alpha2 are related by: + // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) + // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) + // first compute where the two cylinders overlap in alpha1 space: + if (a1a2 < 0) { + axis2[0] = -axis2[0]; + axis2[1] = -axis2[1]; + axis2[2] = -axis2[2]; + } + dReal q[3]; + for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; + dReal k = dDOT (axis1,q); + dReal a1lo = -lz1; + dReal a1hi = lz1; + dReal a2lo = -lz2 - k; + dReal a2hi = lz2 - k; + dReal lo = (a1lo > a2lo) ? a1lo : a2lo; + dReal hi = (a1hi < a2hi) ? a1hi : a2hi; + if (lo <= hi) { + int num_contacts = flags & NUMC_MASK; + if (num_contacts >= 2 && lo < hi) { + // generate up to two contacts. if one of those contacts is + // not made, fall back on the one-contact strategy. + for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; + int n1 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + if (n1) { + for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; + dContactGeom *c2 = CONTACT(contact,skip); + int n2 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius, c2); + if (n2) { + c2->g1 = o1; + c2->g2 = o2; + return 2; + } + } + } + + // just one contact to generate, so put it in the middle of + // the range + alpha1 = (lo + hi) * REAL(0.5); + alpha2 = alpha1 + k; + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + return dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + } + else return 0; + } + det = REAL(1.0)/det; + dReal delta[3]; + for (i=0; i<3; i++) delta[i] = pos1[i] - pos2[i]; + dReal q1 = dDOT (delta,axis1); + dReal q2 = dDOT (delta,axis2); + alpha1 = det*(a1a2*q2-q1); + alpha2 = det*(q2-a1a2*q1); + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + } + } + + // if the alphas are outside their allowed ranges then fix them and + // try again + if (fix1==0) { + if (alpha1 < -lz1) { + fix1 = -1; + continue; + } + if (alpha1 > lz1) { + fix1 = 1; + continue; + } + } + if (fix2==0) { + if (alpha2 < -lz2) { + fix2 = -1; + continue; + } + if (alpha2 > lz2) { + fix2 = 1; + continue; + } + } + + // unfix the alpha variables if the local distance gradient indicates + // that we are not yet at the minimum + dReal tmp[3]; + for (i=0; i<3; i++) tmp[i] = sphere1[i] - sphere2[i]; + if (fix1) { + dReal gradient = dDOT (tmp,axis1); + if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) { + fix1 = 0; + continue; + } + } + if (fix2) { + dReal gradient = -dDOT (tmp,axis2); + if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) { + fix2 = 0; + continue; + } + } + return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); + } + // if we go through the loop too much, then give up. we should NEVER get to + // this point (i hope). + dMessage (0,"dCollideCC(): too many iterations"); + return 0; +} diff --git a/libraries/ode-0.9/ode/src/sphere.cpp b/libraries/ode-0.9/ode/src/sphere.cpp new file mode 100644 index 0000000..fa25aac --- /dev/null +++ b/libraries/ode-0.9/ode/src/sphere.cpp @@ -0,0 +1,241 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + + +//**************************************************************************** +// sphere public API + +dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1) +{ + dAASSERT (_radius > 0); + type = dSphereClass; + radius = _radius; +} + + +void dxSphere::computeAABB() +{ + aabb[0] = final_posr->pos[0] - radius; + aabb[1] = final_posr->pos[0] + radius; + aabb[2] = final_posr->pos[1] - radius; + aabb[3] = final_posr->pos[1] + radius; + aabb[4] = final_posr->pos[2] - radius; + aabb[5] = final_posr->pos[2] + radius; +} + + +dGeomID dCreateSphere (dSpaceID space, dReal radius) +{ + return new dxSphere (space,radius); +} + + +void dGeomSphereSetRadius (dGeomID g, dReal radius) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dAASSERT (radius > 0); + dxSphere *s = (dxSphere*) g; + s->radius = radius; + dGeomMoved (g); +} + + +dReal dGeomSphereGetRadius (dGeomID g) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dxSphere *s = (dxSphere*) g; + return s->radius; +} + + +dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + g->recomputePosr(); + + dxSphere *s = (dxSphere*) g; + dReal * pos = s->final_posr->pos; + return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) + + (y-pos[1])*(y-pos[1]) + + (z-pos[2])*(z-pos[2])); +} + +//**************************************************************************** +// pairwise collision functions for standard geom types + +int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dSphereClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxSphere *sphere1 = (dxSphere*) o1; + dxSphere *sphere2 = (dxSphere*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + return dCollideSpheres (o1->final_posr->pos,sphere1->radius, + o2->final_posr->pos,sphere2->radius,contact); +} + + +int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dBoxClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + // this is easy. get the sphere center `p' relative to the box, and then clip + // that to the boundary of the box (call that point `q'). if q is on the + // boundary of the box and |p-q| is <= sphere radius, they touch. + // if q is inside the box, the sphere is inside the box, so set a contact + // normal to push the sphere to the closest box face. + + dVector3 l,t,p,q,r; + dReal depth; + int onborder = 0; + + dxSphere *sphere = (dxSphere*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0]; + p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1]; + p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2]; + + l[0] = box->side[0]*REAL(0.5); + t[0] = dDOT14(p,o2->final_posr->R); + if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; } + if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; } + + l[1] = box->side[1]*REAL(0.5); + t[1] = dDOT14(p,o2->final_posr->R+1); + if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; } + if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; } + + t[2] = dDOT14(p,o2->final_posr->R+2); + l[2] = box->side[2]*REAL(0.5); + if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; } + if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; } + + if (!onborder) { + // sphere center inside box. find closest face to `t' + dReal min_distance = l[0] - dFabs(t[0]); + int mini = 0; + for (int i=1; i<3; i++) { + dReal face_distance = l[i] - dFabs(t[i]); + if (face_distance < min_distance) { + min_distance = face_distance; + mini = i; + } + } + // contact position = sphere center + contact->pos[0] = o1->final_posr->pos[0]; + contact->pos[1] = o1->final_posr->pos[1]; + contact->pos[2] = o1->final_posr->pos[2]; + // contact normal points to closest face + dVector3 tmp; + tmp[0] = 0; + tmp[1] = 0; + tmp[2] = 0; + tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0); + dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp); + // contact depth = distance to wall along normal plus radius + contact->depth = min_distance + sphere->radius; + return 1; + } + + t[3] = 0; //@@@ hmmm + dMULTIPLY0_331 (q,o2->final_posr->R,t); + r[0] = p[0] - q[0]; + r[1] = p[1] - q[1]; + r[2] = p[2] - q[2]; + depth = sphere->radius - dSqrt(dDOT(r,r)); + if (depth < 0) return 0; + contact->pos[0] = q[0] + o2->final_posr->pos[0]; + contact->pos[1] = q[1] + o2->final_posr->pos[1]; + contact->pos[2] = q[2] + o2->final_posr->pos[2]; + contact->normal[0] = r[0]; + contact->normal[1] = r[1]; + contact->normal[2] = r[2]; + dNormalize3 (contact->normal); + contact->depth = depth; + return 1; +} + + +int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dPlaneClass); + dIASSERT ((flags & NUMC_MASK) >= 1); + + dxSphere *sphere = (dxSphere*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + dReal k = dDOT (o1->final_posr->pos,plane->p); + dReal depth = plane->p[3] - k + sphere->radius; + if (depth >= 0) { + contact->normal[0] = plane->p[0]; + contact->normal[1] = plane->p[1]; + contact->normal[2] = plane->p[2]; + contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius; + contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius; + contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius; + contact->depth = depth; + return 1; + } + else return 0; +} diff --git a/libraries/ode-0.9/ode/src/stack.cpp b/libraries/ode-0.9/ode/src/stack.cpp new file mode 100644 index 0000000..e062f92 --- /dev/null +++ b/libraries/ode-0.9/ode/src/stack.cpp @@ -0,0 +1,114 @@ +/************************************************************************* + * * + * 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 should not be compiled any more @@@ + +#include +#include +#include "stack.h" +#include "ode/error.h" +#include "ode/config.h" + +//**************************************************************************** +// unix version that uses mmap(). some systems have anonymous mmaps and some +// need to mmap /dev/zero. + +#ifndef WIN32 + +#include +#include +#include +#include +#include + + +void dStack::init (int max_size) +{ + if (sizeof(long int) != sizeof(char*)) dDebug (0,"internal"); + if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0"); + +#ifndef MMAP_ANONYMOUS + static int dev_zero_fd = -1; // cached file descriptor for /dev/zero + if (dev_zero_fd < 0) dev_zero_fd = open ("/dev/zero", O_RDWR); + if (dev_zero_fd < 0) dError (0,"can't open /dev/zero (%s)",strerror(errno)); + base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, MAP_PRIVATE, + dev_zero_fd,0); +#else + base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, + MAP_PRIVATE | MAP_ANON,0,0); +#endif + + if (int(base) == -1) dError (0,"Stack::init(), mmap() failed, " + "max_size=%d (%s)",max_size,strerror(errno)); + size = max_size; + pointer = base; + frame = 0; +} + + +void dStack::destroy() +{ + munmap (base,size); + base = 0; + size = 0; + pointer = 0; + frame = 0; +} + +#endif + +//**************************************************************************** + +#ifdef WIN32 + +#include "windows.h" + + +void dStack::init (int max_size) +{ + if (sizeof(LPVOID) != sizeof(char*)) dDebug (0,"internal"); + if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0"); + base = (char*) VirtualAlloc (NULL,max_size,MEM_RESERVE,PAGE_READWRITE); + if (base == 0) dError (0,"Stack::init(), VirtualAlloc() failed, " + "max_size=%d",max_size); + size = max_size; + pointer = base; + frame = 0; + committed = 0; + + // get page size + SYSTEM_INFO info; + GetSystemInfo (&info); + pagesize = info.dwPageSize; +} + + +void dStack::destroy() +{ + VirtualFree (base,0,MEM_RELEASE); + base = 0; + size = 0; + pointer = 0; + frame = 0; +} + +#endif diff --git a/libraries/ode-0.9/ode/src/stack.h b/libraries/ode-0.9/ode/src/stack.h new file mode 100644 index 0000000..5afff41 --- /dev/null +++ b/libraries/ode-0.9/ode/src/stack.h @@ -0,0 +1,138 @@ +/************************************************************************* + * * + * 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 comes from the `reuse' library. copy any changes back to the source. + +these stack allocation functions are a replacement for alloca(), except that +they allocate memory from a separate pool. + +advantages over alloca(): + - consecutive allocations are guaranteed to be contiguous with increasing + address. + - functions can allocate stack memory that is returned to the caller, + in other words pushing and popping stack frames is optional. + +disadvantages compared to alloca(): + - less portable + - slightly slower, although still orders of magnitude faster than malloc(). + - longjmp() and exceptions do not deallocate stack memory (but who cares?). + +just like alloca(): + - using too much stack memory does not fail gracefully, it fails with a + segfault. + +*/ + + +#ifndef _ODE_STACK_H_ +#define _ODE_STACK_H_ + + +#ifdef WIN32 +#include "windows.h" +#endif + + +struct dStack { + char *base; // bottom of the stack + int size; // maximum size of the stack + char *pointer; // current top of the stack + char *frame; // linked list of stack frame ptrs +# ifdef WIN32 // stuff for windows: + int pagesize; // - page size - this is ASSUMED to be a power of 2 + int committed; // - bytes committed in allocated region +#endif + + // initialize the stack. `max_size' is the maximum size that the stack can + // reach. on unix and windows a `virtual' memory block of this size is + // mapped into the address space but does not actually consume physical + // memory until it is referenced - so it is safe to set this to a high value. + + void init (int max_size); + + + // destroy the stack. this unmaps any virtual memory that was allocated. + + void destroy(); + + + // allocate `size' bytes from the stack and return a pointer to the allocated + // memory. `size' must be >= 0. the returned pointer will be aligned to the + // size of a long int. + + char * alloc (int size) + { + char *ret = pointer; + pointer += ((size-1) | (sizeof(long int)-1) )+1; +# ifdef WIN32 + // for windows we need to commit pages as they are required + if ((pointer-base) > committed) { + committed = ((pointer-base-1) | (pagesize-1))+1; // round up to pgsize + VirtualAlloc (base,committed,MEM_COMMIT,PAGE_READWRITE); + } +# endif + return ret; + } + + + // return the address that will be returned by the next call to alloc() + + char *nextAlloc() + { + return pointer; + } + + + // push and pop the current size of the stack. pushFrame() saves the current + // frame pointer on the stack, and popFrame() retrieves it. a typical + // stack-using function will bracket alloc() calls with pushFrame() and + // popFrame(). both functions return the current stack pointer - this should + // be the same value for the two bracketing calls. calling popFrame() too + // many times will result in a segfault. + + char * pushFrame() + { + char *newframe = pointer; + char **addr = (char**) alloc (sizeof(char*)); + *addr = frame; + frame = newframe; + return newframe; + + /* OLD CODE + *((char**)pointer) = frame; + frame = pointer; + char *ret = pointer; + pointer += sizeof(char*); + return ret; + */ + } + + char * popFrame() + { + pointer = frame; + frame = *((char**)pointer); + return pointer; + } +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/step.cpp b/libraries/ode-0.9/ode/src/step.cpp new file mode 100644 index 0000000..19d0473 --- /dev/null +++ b/libraries/ode-0.9/ode/src/step.cpp @@ -0,0 +1,1795 @@ +/************************************************************************* + * * + * 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 "objects.h" +#include "joint.h" +#include +#include +#include +#include +#include +#include +#include "lcp.h" +#include "util.h" + +//**************************************************************************** +// misc defines + +#define FAST_FACTOR +//#define TIMING + +// memory allocation system +#ifdef dUSE_MALLOC_FOR_ALLOCA +unsigned int dMemoryFlag; +#define REPORT_OUT_OF_MEMORY fprintf(stderr, "Insufficient memory to complete rigid body simulation. Results will not be accurate.\n") + +#define ALLOCA(t,v,s) t* v=(t*)malloc(s) +#define UNALLOCA(t) free(t) + +#else +#define ALLOCA(t,v,s) t* v=(t*)dALLOCA16(s) +#define UNALLOCA(t) /* nothing */ +#endif + + +//**************************************************************************** +// debugging - comparison of various vectors and matrices produced by the +// slow and fast versions of the stepper. + +//#define COMPARE_METHODS + +#ifdef COMPARE_METHODS +#include "testing.h" +dMatrixComparison comparator; +#endif + +// undef to use the fast decomposition +#define DIRECT_CHOLESKY +#undef REPORT_ERROR + +//**************************************************************************** +// special matrix multipliers + +// this assumes the 4th and 8th rows of B and C are zero. + +static void Multiply2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) = sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B and C are zero. + +static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) += sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q) +{ + int k; + dReal sum; + dIASSERT (q>0 && A && B && C); + sum = 0; + for (k=0; k0 && A && B && C); + sum = 0; + for (k=0; ktag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (joint == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. + // @@@ check computation of rotational force. + ALLOCA(dReal,I,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (I == NULL) { + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,invI,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (invI == NULL) { + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->posr.R); + dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R); + dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + int m = 0; + ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (info == NULL) { + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + ALLOCA(int,ofs,nj*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ofs == NULL) { + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; iinvMass; + MM[nskip+1] = body[i]->invMass; + MM[2*nskip+2] = body[i]->invMass; + MM += 3*nskip+3; + for (j=0; j<3; j++) for (k=0; k<3; k++) { + MM[j*nskip+k] = invI[i*12+j*4+k]; + } + } + + // assemble some body vectors: fe = external forces, v = velocities + ALLOCA(dReal,fe,n6*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (fe == NULL) { + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + ALLOCA(dReal,v,n6*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (v == NULL) { + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + //dSetZero (fe,n6); + //dSetZero (v,n6); + for (i=0; ifacc[j]; + for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j]; + for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j]; + for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j]; + } + + // this will be set to the velocity update + ALLOCA(dReal,vnew,n6*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (vnew == NULL) { + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (vnew,n6); + + // if there are constraints, compute cforce + if (m > 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + ALLOCA(dReal,c,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (c == NULL) { + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,cfm,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (cfm == NULL) { + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,lo,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo == NULL) { + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,hi,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi == NULL) { + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(int,findex,m*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (findex == NULL) { + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; inode[0].body->tag; + Jinfo.J1a = Jinfo.J1l + 3; + if (joint[i]->node[1].body) { + Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag; + Jinfo.J2a = Jinfo.J2l + 3; + } + else { + Jinfo.J2l = 0; + Jinfo.J2a = 0; + } + Jinfo.c = c + ofs[i]; + Jinfo.cfm = cfm + ofs[i]; + Jinfo.lo = lo + ofs[i]; + Jinfo.hi = hi + ofs[i]; + Jinfo.findex = findex + ofs[i]; + joint[i]->vtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J' +# ifdef TIMING + dTimerNow ("compute A"); +# endif + ALLOCA(dReal,JinvM,m*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (JinvM == NULL) { + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + //dSetZero (JinvM,m*nskip); + dMultiply0 (JinvM,J,invM,m,n6,n6); + int mskip = dPAD(m); + ALLOCA(dReal,A,m*mskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A == NULL) { + UNALLOCA(JinvM); + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + //dSetZero (A,m*mskip); + dMultiply2 (A,JinvM,J,m,n6,m); + + // add cfm to the diagonal of A + for (i=0; ilvel[j] = vnew[i*6+j]; + for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j]; + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +#ifdef TIMING + dTimerNow ("update position"); +#endif + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +#ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +#endif + + UNALLOCA(joint); + UNALLOCA(I); + UNALLOCA(invI); + UNALLOCA(info); + UNALLOCA(ofs); + UNALLOCA(invM); + UNALLOCA(fe); + UNALLOCA(v); + UNALLOCA(vnew); +} + +//**************************************************************************** +// an optimized version of dInternalStepIsland1() + +void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j,k; +#ifdef TIMING + dTimerStart("preprocessing"); +#endif + + dReal stepsize1 = dRecip(stepsize); + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (joint == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. +#ifdef dGYROSCOPIC + ALLOCA(dReal,I,3*nb*4*sizeof(dReal)); +# ifdef dUSE_MALLOC_FOR_ALLOCA + if (I == NULL) { + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +# endif +#else + dReal *I = NULL; +#endif // for dGYROSCOPIC + + ALLOCA(dReal,invI,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (invI == NULL) { + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; iinvI,body[i]->posr.R); + dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); +#ifdef dGYROSCOPIC + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R); + dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp); + + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); +#endif + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (info == NULL) { + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(int,ofs,nj*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ofs == NULL) { + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + joint[i]->tag = i; + i++; + } + else { + joint[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + ALLOCA(dReal,c,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (c == NULL) { + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,cfm,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (cfm == NULL) { + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,lo,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo == NULL) { + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,hi,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi == NULL) { + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(int,findex,m*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (findex == NULL) { + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; ivtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + ALLOCA(dReal,JinvM,2*m*8*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (JinvM == NULL) { + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (JinvM,2*m*8); + for (i=0; inode[0].body->tag; + dReal body_invMass = body[b]->invMass; + dReal *body_invI = invI + b*12; + dReal *Jsrc = J + 2*8*ofs[i]; + dReal *Jdst = JinvM + 2*8*ofs[i]; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + if (joint[i]->node[1].body) { + b = joint[i]->node[1].body->tag; + body_invMass = body[b]->invMass; + body_invI = invI + b*12; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + } + } + + // now compute A = JinvM * J'. A's rows and columns are grouped by joint, + // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero + // if joints i and j have at least one body in common. this fact suggests + // the algorithm used to fill A: + // + // for b = all bodies + // n = number of joints attached to body b + // for i = 1..n + // for j = i+1..n + // ii = actual joint number for i + // jj = actual joint number for j + // // (ii,jj) will be set to all pairs of joints around body b + // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)' + // + // this algorithm catches all pairs of joints that have at least one body + // in common. it does not compute the diagonal blocks of A however - + // another similar algorithm does that. + + int mskip = dPAD(m); + ALLOCA(dReal,A,m*mskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A == NULL) { + UNALLOCA(JinvM); + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (A,m*mskip); + for (i=0; ifirstjoint; n1; n1=n1->next) { + for (dxJointNode *n2=n1->next; n2; n2=n2->next) { + // get joint numbers and ensure ofs[j1] >= ofs[j2] + int j1 = n1->joint->tag; + int j2 = n2->joint->tag; + if (ofs[j1] < ofs[j2]) { + int tmp = j1; + j1 = j2; + j2 = tmp; + } + + // if either joint was tagged as -1 then it is an inactive (m=0) + // joint that should not be considered + if (j1==-1 || j2==-1) continue; + + // determine if body i is the 1st or 2nd body of joints j1 and j2 + int jb1 = (joint[j1]->node[1].body == body[i]); + int jb2 = (joint[j2]->node[1].body == body[i]); + // jb1/jb2 must be 0 for joints with only one body + dIASSERT(joint[j1]->node[1].body || jb1==0); + dIASSERT(joint[j2]->node[1].body || jb2==0); + + // set block of A + MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2], + JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m, + J + 2*8*ofs[j2] + jb2*8*info[j2].m, + info[j1].m,info[j2].m, mskip); + } + } + } + // compute diagonal blocks of A + for (i=0; inode[1].body) { + MultiplyAdd2_p8r (A + ofs[i]*(mskip+1), + JinvM + 2*8*ofs[i] + 8*info[i].m, + J + 2*8*ofs[i] + 8*info[i].m, + info[i].m,info[i].m, mskip); + } + } + + // add cfm to the diagonal of A + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass + + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc); + for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + ALLOCA(dReal,rhs,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (rhs == NULL) { + UNALLOCA(tmp1); + UNALLOCA(A); + UNALLOCA(JinvM); + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + //dSetZero (rhs,m); + for (i=0; inode[0].body->tag, info[i].m); + if (joint[i]->node[1].body) { + MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m, + tmp1 + 8*joint[i]->node[1].body->tag, info[i].m); + } + } + // complete rhs + for (i=0; inode[0].body; + dxBody* b2 = joint[i]->node[1].body; + dJointFeedback *fb = joint[i]->feedback; + + if (fb) { + // the user has requested feedback on the amount of force that this + // joint is applying to the bodies. we use a slightly slower + // computation that splits out the force components and puts them + // in the feedback structure. + dReal data1[8],data2[8]; + Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); + dReal *cf1 = cforce + 8*b1->tag; + cf1[0] += (fb->f1[0] = data1[0]); + cf1[1] += (fb->f1[1] = data1[1]); + cf1[2] += (fb->f1[2] = data1[2]); + cf1[4] += (fb->t1[0] = data1[4]); + cf1[5] += (fb->t1[1] = data1[5]); + cf1[6] += (fb->t1[2] = data1[6]); + if (b2){ + Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + dReal *cf2 = cforce + 8*b2->tag; + cf2[0] += (fb->f2[0] = data2[0]); + cf2[1] += (fb->f2[1] = data2[1]); + cf2[2] += (fb->f2[2] = data2[2]); + cf2[4] += (fb->t2[0] = data2[4]); + cf2[5] += (fb->t2[1] = data2[5]); + cf2[6] += (fb->t2[2] = data2[6]); + } + } + else { + // no feedback is required, let's compute cforce the faster way + MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m); + if (b2) { + MultiplyAdd1_8q1 (cforce + 8*b2->tag, + JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + } + } + } + UNALLOCA(c); + UNALLOCA(cfm); + UNALLOCA(lo); + UNALLOCA(hi); + UNALLOCA(findex); + UNALLOCA(J); + UNALLOCA(JinvM); + UNALLOCA(A); + UNALLOCA(tmp1); + UNALLOCA(rhs); + UNALLOCA(lambda); + UNALLOCA(residual); + } + + // compute the velocity update +#ifdef TIMING + dTimerNow ("compute velocity update"); +#endif + + // add fe to cforce + for (i=0; ifacc[j]; + for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j]; + } + // multiply cforce by stepsize + for (i=0; i < nb*8; i++) cforce[i] *= stepsize; + // add invM * cforce to the body velocity + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j]; + dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4); + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +# ifdef TIMING + dTimerNow ("update position"); +# endif + for (i=0; ilvel[j]; + for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j]; + } + comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew"); + UNALLOCA(tmp); +#endif + +#ifdef TIMING + dTimerNow ("tidy up"); +#endif + + // zero all force accumulators + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +#ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +#endif + + UNALLOCA(joint); + UNALLOCA(I); + UNALLOCA(invI); + UNALLOCA(info); + UNALLOCA(ofs); + UNALLOCA(cforce); +} + +//**************************************************************************** + +void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *joint, int nj, dReal stepsize) +{ + +#ifdef dUSE_MALLOC_FOR_ALLOCA + dMemoryFlag = d_MEMORY_OK; +#endif + +#ifndef COMPARE_METHODS + dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); + +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + REPORT_OUT_OF_MEMORY; + return; + } +#endif + +#endif + +#ifdef COMPARE_METHODS + int i; + + // save body state + ALLOCA(dxBody,state,nb*sizeof(dxBody)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (state == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + REPORT_OUT_OF_MEMORY; + return; + } +#endif + for (i=0; i + + +void dInternalStepIsland (dxWorld *world, + dxBody * const *body, int nb, + dxJoint * const *joint, int nj, + dReal stepsize); + + + +#endif diff --git a/libraries/ode-0.9/ode/src/stepfast.cpp b/libraries/ode-0.9/ode/src/stepfast.cpp new file mode 100755 index 0000000..35c45db --- /dev/null +++ b/libraries/ode-0.9/ode/src/stepfast.cpp @@ -0,0 +1,1139 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * Fast iterative solver, David Whittaker. Email: david@csworkbench.com * + * * + * 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 is the StepFast code by David Whittaker. This code is faster, but +// sometimes less stable than, the original "big matrix" code. +// Refer to the user's manual for more information. +// Note that this source file duplicates a lot of stuff from step.cpp, +// eventually we should move the common code to a third file. + +#include "objects.h" +#include "joint.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "lcp.h" +#include "step.h" +#include "util.h" + + +// misc defines + +#define ALLOCA dALLOCA16 + +#define RANDOM_JOINT_ORDER +//#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate) +#define SLOW_LCP //use the old LCP solver +//#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work +//#define TIMING + + +static int autoEnableDepth = 2; + +void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth) +{ + if (autodepth > 0) + autoEnableDepth = autodepth; + else + autoEnableDepth = 0; +} + +int dWorldGetAutoEnableDepthSF1 (dxWorld *world) +{ + return autoEnableDepth; +} + +//little bit of math.... the _sym_ functions assume the return matrix will be symmetric +static void +Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) = *ad = sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + +static void +MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) += sum; + *ad += sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q) +{ + int k; + dReal sum; + dIASSERT (q > 0 && A && B && C); + sum = 0; + for (k = 0; k < q; k++) + sum += B[k * 8] * C[k]; + A[0] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[1 + k * 8] * C[k]; + A[1] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[2 + k * 8] * C[k]; + A[2] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[4 + k * 8] * C[k]; + A[4] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[5 + k * 8] * C[k]; + A[5] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[6 + k * 8] * C[k]; + A[6] = sum; +} + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal +sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs (x) < 1.0e-4) + return REAL (1.0) - x * x * REAL (0.166666666666666666667); + else + return dSin (x) / x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +static inline void +moveAndRotateBody (dxBody * b, dReal h) +{ + int j; + + // handle linear velocity + for (j = 0; j < 3; j++) + b->posr.pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) + { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv, irv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis, b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL (0.5); + dReal theta = k * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else + { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); + h *= REAL (0.5); + dReal theta = wlen * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2, q, b->q); + for (j = 0; j < 4; j++) + b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + dReal dq[4]; + dWtoDQ (irv, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + } + else + { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q, b->posr.R); + + // notify all attached geoms that this body has moved + for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +//This is an implementation of the iterated/relaxation algorithm. +//Here is a quick overview of the algorithm per Sergi Valverde's posts to the +//mailing list: +// +// for i=0..N-1 do +// for c = 0..C-1 do +// Solve constraint c-th +// Apply forces to constraint bodies +// next +// next +// Integrate bodies + +void +dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) +{ + int i, j, k; +# ifdef TIMING + dTimerNow ("constraint preprocessing"); +# endif + + dReal stepsize1 = dRecip (stepsize); + + int m = info.m; + // nothing to do if no constraints. + if (m <= 0) + return; + + int nub = 0; + if (info.nub == info.m) + nub = m; + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal JinvM[2 * 6 * 8]; + //dSetZero (JinvM, 2 * m * 8); + + dReal *Jsrc = Jinfo.J1l; + dReal *Jdst = JinvM; + if (body[0]) + { + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[0]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); + Jsrc += 8; + Jdst += 8; + } + } + if (body[1]) + { + Jsrc = Jinfo.J2l; + Jdst = JinvM + 8 * m; + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[1]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); + Jsrc += 8; + Jdst += 8; + } + } + + + // now compute A = JinvM * J'. + int mskip = dPAD (m); + dReal A[6 * 8]; + //dSetZero (A, 6 * 8); + + if (body[0]) { + Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); + if (body[1]) + MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, + m, mskip); + } else { + if (body[1]) + Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, + m, mskip); + } + + // add cfm to the diagonal of A + for (i = 0; i < m; i++) + A[i * mskip + i] += Jinfo.cfm[i] * stepsize1; + + // compute the right hand side `rhs' +# ifdef TIMING + dTimerNow ("compute rhs"); +# endif + dReal tmp1[16]; + //dSetZero (tmp1, 16); + // put v/h + invM*fe into tmp1 + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); + for (j = 0; j < 3; j++) + tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + dReal rhs[6]; + //dSetZero (rhs, 6); + + if (body[0]) { + Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); + if (body[1]) + MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); + } else { + if (body[1]) + Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); + } + + // complete rhs + for (i = 0; i < m; i++) + rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i]; + +#ifdef SLOW_LCP + // solve the LCP problem and get lambda. + // this will destroy A but that's okay +# ifdef TIMING + dTimerNow ("solving LCP problem"); +# endif + dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal lo[6], hi[6]; + memcpy (lo, Jinfo.lo, m * sizeof (dReal)); + memcpy (hi, Jinfo.hi, m * sizeof (dReal)); + dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); +#endif + + // LCP Solver replacement: + // This algorithm goes like this: + // Do a straightforward LDLT factorization of the matrix A, solving for + // A*x = rhs + // For each x[i] that is outside of the bounds of lo[i] and hi[i], + // clamp x[i] into that range. + // Substitute into A the now known x's + // subtract the residual away from the rhs. + // Remove row and column i from L, updating the factorization + // place the known x's at the end of the array, keeping up with location in p + // Repeat until all constraints have been clamped or all are within bounds + // + // This is probably only faster in the single joint case where only one repeat is + // the norm. + +#ifdef FAST_FACTOR + // factorize A (L*D*L'=A) +# ifdef TIMING + dTimerNow ("factorize A"); +# endif + dReal d[6]; + dReal L[6 * 8]; + memcpy (L, A, m * mskip * sizeof (dReal)); + dFactorLDLT (L, d, m, mskip); + + // compute lambda +# ifdef TIMING + dTimerNow ("compute lambda"); +# endif + + int left = m; //constraints left to solve. + int remove[6]; + dReal lambda[6]; + dReal x[6]; + int p[6]; + for (i = 0; i < 6; i++) + p[i] = i; + while (true) + { + memcpy (x, rhs, left * sizeof (dReal)); + dSolveLDLT (L, d, x, left, mskip); + + int fixed = 0; + for (i = 0; i < left; i++) + { + j = p[i]; + remove[i] = false; + // This isn't the exact same use of findex as dSolveLCP.... since x[findex] + // may change after I've already clamped x[i], but it should be close + if (Jinfo.findex[j] > -1) + { + dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]); + if (x[i] > f) + x[i] = f; + else if (x[i] < -f) + x[i] = -f; + else + continue; + } + else + { + if (x[i] > Jinfo.hi[j]) + x[i] = Jinfo.hi[j]; + else if (x[i] < Jinfo.lo[j]) + x[i] = Jinfo.lo[j]; + else + continue; + } + remove[i] = true; + fixed++; + } + if (fixed == 0 || fixed == left) //no change or all constraints solved + break; + + for (i = 0; i < left; i++) //sub in to right hand side. + if (remove[i]) + for (j = 0; j < left; j++) + if (!remove[j]) + rhs[j] -= A[j * mskip + i] * x[i]; + + for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables + { + if (remove[r]) + { + //dRemoveLDLT adapted for use without row pointers. + if (r == left - 1) + { + left--; + continue; // deleting last row/col is easy + } + else if (r == 0) + { + dReal a[6]; + for (i = 0; i < left; i++) + a[i] = -A[i * mskip]; + a[0] += REAL (1.0); + dLDLTAddTL (L, d, a, left, mskip); + } + else + { + dReal t[6]; + dReal a[6]; + for (i = 0; i < r; i++) + t[i] = L[r * mskip + i] / d[i]; + for (i = 0; i < left - r; i++) + a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r]; + a[0] += REAL (1.0); + dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip); + } + + dRemoveRowCol (L, left, mskip, r); + //end dRemoveLDLT + + left--; + if (r < (left - 1)) + { + dReal tx = x[r]; + memmove (d + r, d + r + 1, (left - r) * sizeof (dReal)); + memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal)); + //x will get written over by rhs anyway, no need to move it around + //just store the fixed value we just discovered in it. + x[left] = tx; + for (i = 0; i < m; i++) + if (p[i] > r && p[i] <= left) + p[i]--; + p[r] = left; + } + } + } + } + + for (i = 0; i < m; i++) + lambda[i] = x[p[i]]; +# endif + // compute the constraint force `cforce' +# ifdef TIMING + dTimerNow ("compute constraint force"); +#endif + + // compute cforce = J'*lambda + dJointFeedback *fb = joint->feedback; + dReal cforce[16]; + //dSetZero (cforce, 16); + + if (fb) + { + // the user has requested feedback on the amount of force that this + // joint is applying to the bodies. we use a slightly slower + // computation that splits out the force components and puts them + // in the feedback structure. + dReal data1[8], data2[8]; + if (body[0]) + { + Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); + dReal *cf1 = cforce; + cf1[0] = (fb->f1[0] = data1[0]); + cf1[1] = (fb->f1[1] = data1[1]); + cf1[2] = (fb->f1[2] = data1[2]); + cf1[4] = (fb->t1[0] = data1[4]); + cf1[5] = (fb->t1[1] = data1[5]); + cf1[6] = (fb->t1[2] = data1[6]); + } + if (body[1]) + { + Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); + dReal *cf2 = cforce + 8; + cf2[0] = (fb->f2[0] = data2[0]); + cf2[1] = (fb->f2[1] = data2[1]); + cf2[2] = (fb->f2[2] = data2[2]); + cf2[4] = (fb->t2[0] = data2[4]); + cf2[5] = (fb->t2[1] = data2[5]); + cf2[6] = (fb->t2[2] = data2[6]); + } + } + else + { + // no feedback is required, let's compute cforce the faster way + if (body[0]) + Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); + if (body[1]) + Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); + } + + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + { + body[i]->facc[j] += cforce[i * 8 + j]; + body[i]->tacc[j] += cforce[i * 8 + 4 + j]; + } + } +} + +void +dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) +{ +# ifdef TIMING + dTimerNow ("preprocessing"); +# endif + dxBody *bodyPair[2], *body; + dReal *GIPair[2], *GinvIPair[2]; + dxJoint *joint; + int iter, b, j, i; + dReal ministep = stepsize / maxiterations; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); + memcpy (joints, _joints, nj * sizeof (dxJoint *)); + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); + int *ofs = (int *) ALLOCA (nj * sizeof (int)); + for (i = 0, j = 0; j < nj; j++) + { // i=dest, j=src + joints[j]->vtable->getInfo1 (joints[j], info + i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) + { + joints[i] = joints[j]; + joints[i]->tag = i; + i++; + } + else + { + joints[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i = 0; i < nj; i++) + { + ofs[i] = m; + m += info[i].m; + } + dReal *c = NULL; + dReal *cfm = NULL; + dReal *lo = NULL; + dReal *hi = NULL; + int *findex = NULL; + + dReal *J = NULL; + dxJoint::Info2 * Jinfo = NULL; + + if (m) + { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + c = (dReal *) ALLOCA (m * sizeof (dReal)); + cfm = (dReal *) ALLOCA (m * sizeof (dReal)); + lo = (dReal *) ALLOCA (m * sizeof (dReal)); + hi = (dReal *) ALLOCA (m * sizeof (dReal)); + findex = (int *) ALLOCA (m * sizeof (int)); + dSetZero (c, m); + dSetValue (cfm, m, world->global_cfm); + dSetValue (lo, m, -dInfinity); + dSetValue (hi, m, dInfinity); + for (i = 0; i < m; i++) + findex[i] = -1; + + // get jacobian data from constraints. a (2*m)x8 matrix will be created + // to store the two jacobian blocks from each constraint. it has this + // format: + // + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) + // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) + // etc... + // + // (lll) = linear jacobian data + // (aaa) = angular jacobian data + // +# ifdef TIMING + dTimerNow ("create J"); +# endif + J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); + dSetZero (J, 2 * m * 8); + Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); + for (i = 0; i < nj; i++) + { + Jinfo[i].rowskip = 8; + Jinfo[i].fps = dRecip (stepsize); + Jinfo[i].erp = world->global_erp; + Jinfo[i].J1l = J + 2 * 8 * ofs[i]; + Jinfo[i].J1a = Jinfo[i].J1l + 4; + Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; + Jinfo[i].J2a = Jinfo[i].J2l + 4; + Jinfo[i].c = c + ofs[i]; + Jinfo[i].cfm = cfm + ofs[i]; + Jinfo[i].lo = lo + ofs[i]; + Jinfo[i].hi = hi + ofs[i]; + Jinfo[i].findex = findex + ofs[i]; + //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); + } + + } + + dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + for (b = 0; b < nb; b++) + { + for (i = 0; i < 4; i++) + { + saveFacc[b * 4 + i] = bodies[b]->facc[i]; + saveTacc[b * 4 + i] = bodies[b]->tacc[i]; + } + bodies[b]->tag = b; + } + + for (iter = 0; iter < maxiterations; iter++) + { +# ifdef TIMING + dTimerNow ("applying inertia and gravity"); +# endif + dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R); + dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->invI, body->posr.R); + dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp); + + for (i = 0; i < 4; i++) + body->tacc[i] = saveTacc[b * 4 + i]; +#ifdef dGYROSCOPIC + // compute rotational force + dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel); + dCROSS (body->tacc, -=, body->avel, tmp); +#endif + + // add the gravity force to all bodies + if ((body->flags & dxBodyNoGravity) == 0) + { + body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0]; + body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1]; + body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2]; + body->facc[3] = 0; + } else { + body->facc[0] = saveFacc[b * 4 + 0]; + body->facc[1] = saveFacc[b * 4 + 1]; + body->facc[2] = saveFacc[b * 4 + 2]; + body->facc[3] = 0; + } + + } + +#ifdef RANDOM_JOINT_ORDER +#ifdef TIMING + dTimerNow ("randomizing joint order"); +#endif + //randomize the order of the joints by looping through the array + //and swapping the current joint pointer with a random one before it. + for (j = 0; j < nj; j++) + { + joint = joints[j]; + dxJoint::Info1 i1 = info[j]; + dxJoint::Info2 i2 = Jinfo[j]; + const int r = dRandInt(j+1); + dIASSERT (r < nj); + joints[j] = joints[r]; + info[j] = info[r]; + Jinfo[j] = Jinfo[r]; + joints[r] = joint; + info[r] = i1; + Jinfo[r] = i2; + } +#endif + + //now iterate through the random ordered joint array we created. + for (j = 0; j < nj; j++) + { +#ifdef TIMING + dTimerNow ("setting up joint"); +#endif + joint = joints[j]; + bodyPair[0] = joint->node[0].body; + bodyPair[1] = joint->node[1].body; + + if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) + bodyPair[0] = 0; + if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) + bodyPair[1] = 0; + + //if this joint is not connected to any enabled bodies, skip it. + if (!bodyPair[0] && !bodyPair[1]) + continue; + + if (bodyPair[0]) + { + GIPair[0] = globalI + bodyPair[0]->tag * 12; + GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; + } + if (bodyPair[1]) + { + GIPair[1] = globalI + bodyPair[1]->tag * 12; + GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; + } + + joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); + + //dInternalStepIslandFast is an exact copy of the old routine with one + //modification: the calculated forces are added back to the facc and tacc + //vectors instead of applying them to the bodies and moving them. + if (info[j].m > 0) + { + dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); + } + } + // } +# ifdef TIMING + dTimerNow ("moving bodies"); +# endif + //Now we can simulate all the free floating bodies, and move them. + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + for (i = 0; i < 4; i++) + { + body->facc[i] *= ministep; + body->tacc[i] *= ministep; + } + + //apply torque + dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); + + //apply force + for (i = 0; i < 3; i++) + body->lvel[i] += body->invMass * body->facc[i]; + + //move It! + moveAndRotateBody (body, ministep); + } + } + for (b = 0; b < nb; b++) + for (j = 0; j < 4; j++) + bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; +} + + +#ifdef NO_ISLANDS + +// Since the iterative algorithm doesn't care about islands of bodies, this is a +// faster algorithm that just sends it all the joints and bodies in one array. +// It's downfall is it's inability to handle disabled bodies as well as the old one. +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ + // nothing to do if no bodies + if (world->nb <= 0) + return; + + dInternalHandleAutoDisabling (world,stepsize); + +# ifdef TIMING + dTimerStart ("creating joint and body arrays"); +# endif + dxBody **bodies, *body; + dxJoint **joints, *joint; + joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + + int nj = 0; + for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) + joints[nj++] = joint; + + int nb = 0; + for (body = world->firstbody; body; body = (dxBody *) body->next) + bodies[nb++] = body; + + dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations); +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#else + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ +#ifdef TIMING + dTimerStart ("Island Setup"); +#endif + dxBody *b, *bb, **body; + dxJoint *j, **joint; + + // nothing to do if no bodies + if (world->nb <= 0) + return; + + dInternalHandleAutoDisabling (world,stepsize); + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + int tbcount = 0; + int tjcount = 0; + + // set all body/joint tags to 0 + for (b = world->firstbody; b; b = (dxBody *) b->next) + b->tag = 0; + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *)); + int *autostack = (int *) ALLOCA (stackalloc * sizeof (int)); + + for (bb = world->firstbody; bb; bb = (dxBody *) bb->next) + { +#ifdef TIMING + dTimerNow ("Island Processing"); +#endif + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) + continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + int autoDepth = autoEnableDepth; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) + { + b = stack[--stacksize]; // pop body off stack + autoDepth = autostack[stacksize]; + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode * n = b->firstjoint; n; n = n->next) + { + if (!n->joint->tag) + { + int thisDepth = autoEnableDepth; + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) + { + if (n->body->flags & dxBodyDisabled) + thisDepth = autoDepth - 1; + if (thisDepth < 0) + continue; + n->body->flags &= ~dxBodyDisabled; + n->body->tag = 1; + autostack[stacksize] = thisDepth; + stack[stacksize++] = n->body; + } + } + } + dIASSERT (stacksize <= world->nb); + dIASSERT (stacksize <= world->nj); + } + + // now do something with body and joint lists + dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i = 0; i < bcount; i++) + { + body[i]->tag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i = 0; i < jcount; i++) + joint[i]->tag = 1; + + tbcount += bcount; + tjcount += jcount; + } + +#ifdef TIMING + dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount); +#endif + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b = world->firstbody; b; b = (dxBody *) b->next) + { + if (b->flags & dxBodyDisabled) + { + if (b->tag) + dDebug (0, "disabled body tagged"); + } + else + { + if (!b->tag) + dDebug (0, "enabled body not tagged"); + } + } + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0)) + { + if (!j->tag) + dDebug (0, "attached enabled joint not tagged"); + } + else + { + if (j->tag) + dDebug (0, "unattached or disabled joint tagged"); + } + } +# endif + +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#endif + + +void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations) +{ + dUASSERT (w, "bad world argument"); + dUASSERT (stepsize > 0, "stepsize must be > 0"); + processIslandsFast (w, stepsize, maxiterations); +} diff --git a/libraries/ode-0.9/ode/src/testing.cpp b/libraries/ode-0.9/ode/src/testing.cpp new file mode 100644 index 0000000..a22f865 --- /dev/null +++ b/libraries/ode-0.9/ode/src/testing.cpp @@ -0,0 +1,243 @@ +/************************************************************************* + * * + * 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 "testing.h" + +#ifdef dDOUBLE +static const dReal tol = 1.0e-9; +#else +static const dReal tol = 1.0e-5f; +#endif + + +// matrix header on the stack + +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" ODE_API 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); +} diff --git a/libraries/ode-0.9/ode/src/testing.h b/libraries/ode-0.9/ode/src/testing.h new file mode 100644 index 0000000..4d19ff3 --- /dev/null +++ b/libraries/ode-0.9/ode/src/testing.h @@ -0,0 +1,65 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* stuff used for testing */ + +#ifndef _ODE_TESTING_H_ +#define _ODE_TESTING_H_ + +#include +#include "array.h" + + +// compare a sequence of named matrices/vectors, i.e. to make sure that two +// different pieces of code are giving the same results. + +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 +}; + + +#endif diff --git a/libraries/ode-0.9/ode/src/timer.cpp b/libraries/ode-0.9/ode/src/timer.cpp new file mode 100644 index 0000000..f754bf1 --- /dev/null +++ b/libraries/ode-0.9/ode/src/timer.cpp @@ -0,0 +1,421 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +TODO +---- + +* gettimeofday() and the pentium time stamp counter return the real time, + not the process time. fix this somehow! + +*/ + +#include +#include + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// implementation for windows based on the multimedia performance counter. + +#ifdef WIN32 + +#include "windows.h" + +static inline void getClockCount (unsigned long cc[2]) +{ + LARGE_INTEGER a; + QueryPerformanceCounter (&a); + cc[0] = a.LowPart; + cc[1] = a.HighPart; +} + + +static inline void serialize() +{ +} + + +static inline double loadClockCount (unsigned long cc[2]) +{ + LARGE_INTEGER a; + a.LowPart = cc[0]; + a.HighPart = cc[1]; + return double(a.QuadPart); +} + + +double dTimerResolution() +{ + return 1.0/dTimerTicksPerSecond(); +} + + +double dTimerTicksPerSecond() +{ + static int query=0; + static double hz=0.0; + if (!query) { + LARGE_INTEGER a; + QueryPerformanceFrequency (&a); + hz = double(a.QuadPart); + query = 1; + } + return hz; +} + +#endif + +//**************************************************************************** +// implementation based on the pentium time stamp counter. the timer functions +// can be serializing or non-serializing. serializing will ensure that all +// instructions have executed and data has been written back before the cpu +// time stamp counter is read. the CPUID instruction is used to serialize. + +#if defined(PENTIUM) && !defined(WIN32) + +// we need to know the clock rate so that the timing function can report +// accurate times. this number only needs to be set accurately if we're +// doing performance tests and care about real-world time numbers - otherwise, +// just ignore this. i have not worked out how to determine this number +// automatically yet. + +#define PENTIUM_HZ (500e6) + +static inline void getClockCount (unsigned long cc[2]) +{ +#ifndef X86_64_SYSTEM + asm volatile ( + "rdtsc\n" + "movl %%eax,(%%esi)\n" + "movl %%edx,4(%%esi)\n" + : : "S" (cc) : "%eax","%edx","cc","memory"); +#else + asm volatile ( + "rdtsc\n" + "movl %%eax,(%%rsi)\n" + "movl %%edx,4(%%rsi)\n" + : : "S" (cc) : "%eax","%edx","cc","memory"); +#endif +} + + +static inline void serialize() +{ +#ifndef X86_64_SYSTEM + asm volatile ( + "mov $0,%%eax\n" + "push %%ebx\n" + "cpuid\n" + "pop %%ebx\n" + : : : "%eax","%ecx","%edx","cc","memory"); +#else + asm volatile ( + "mov $0,%%rax\n" + "push %%rbx\n" + "cpuid\n" + "pop %%rbx\n" + : : : "%rax","%rcx","%rdx","cc","memory"); +#endif +} + + +static inline double loadClockCount (unsigned long a[2]) +{ + double ret; +#ifndef X86_64_SYSTEM + asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : + "cc","memory"); +#else + asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : + "cc","memory"); +#endif + return ret; +} + + +double dTimerResolution() +{ + return 1.0/PENTIUM_HZ; +} + + +double dTimerTicksPerSecond() +{ + return PENTIUM_HZ; +} + +#endif + +//**************************************************************************** +// otherwise, do the implementation based on gettimeofday(). + +#if !defined(PENTIUM) && !defined(WIN32) + +#ifndef macintosh + +#include +#include + + +static inline void getClockCount (unsigned long cc[2]) +{ + struct timeval tv; + gettimeofday (&tv,0); + cc[0] = tv.tv_usec; + cc[1] = tv.tv_sec; +} + +#else // macintosh + +#include +#include + +static inline void getClockCount (unsigned long cc[2]) +{ + UnsignedWide ms; + Microseconds (&ms); + cc[1] = ms.lo / 1000000; + cc[0] = ms.lo - ( cc[1] * 1000000 ); +} + +#endif + + +static inline void serialize() +{ +} + + +static inline double loadClockCount (unsigned long a[2]) +{ + return a[1]*1.0e6 + a[0]; +} + + +double dTimerResolution() +{ + unsigned long cc1[2],cc2[2]; + getClockCount (cc1); + do { + getClockCount (cc2); + } + while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); + do { + getClockCount (cc1); + } + while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); + double t1 = loadClockCount (cc1); + double t2 = loadClockCount (cc2); + return (t1-t2) / dTimerTicksPerSecond(); +} + + +double dTimerTicksPerSecond() +{ + return 1000000; +} + +#endif + +//**************************************************************************** +// stop watches + +void dStopwatchReset (dStopwatch *s) +{ + s->time = 0; + s->cc[0] = 0; + s->cc[1] = 0; +} + + +void dStopwatchStart (dStopwatch *s) +{ + serialize(); + getClockCount (s->cc); +} + + +void dStopwatchStop (dStopwatch *s) +{ + unsigned long cc[2]; + serialize(); + getClockCount (cc); + double t1 = loadClockCount (s->cc); + double t2 = loadClockCount (cc); + s->time += t2-t1; +} + + +double dStopwatchTime (dStopwatch *s) +{ + return s->time / dTimerTicksPerSecond(); +} + +//**************************************************************************** +// code timers + +// maximum number of events to record +#define MAXNUM 100 + +static int num = 0; // number of entries used in event array +static struct { + unsigned long cc[2]; // clock counts + double total_t; // total clocks used in this slot. + double total_p; // total percentage points used in this slot. + int count; // number of times this slot has been updated. + char *description; // pointer to static string +} event[MAXNUM]; + + +// make sure all slot totals and counts reset to 0 at start + +static void initSlots() +{ + static int initialized=0; + if (!initialized) { + for (int i=0; i (description); + num = 1; + serialize(); + getClockCount (event[0].cc); +} + + +void dTimerNow (const char *description) +{ + if (num < MAXNUM) { + // do not serialize + getClockCount (event[num].cc); + event[num].description = const_cast (description); + num++; + } +} + + +void dTimerEnd() +{ + if (num < MAXNUM) { + serialize(); + getClockCount (event[num].cc); + event[num].description = "TOTAL"; + num++; + } +} + +//**************************************************************************** +// print report + +static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt) +{ + if (a >= 0.999999) { + fprintf (f,fmt,a); + return; + } + a *= 1000.0; + if (a >= 0.999999) { + fprintf (f,fmt,a); + fprintf (f,"m"); + return; + } + a *= 1000.0; + if (a >= 0.999999) { + fprintf (f,fmt,a); + fprintf (f,"u"); + return; + } + a *= 1000.0; + fprintf (f,fmt,a); + fprintf (f,"n"); +} + + +void dTimerReport (FILE *fout, int average) +{ + int i; + size_t maxl; + double ccunit = 1.0/dTimerTicksPerSecond(); + fprintf (fout,"\nTimer Report ("); + fprintDoubleWithPrefix (fout,ccunit,"%.2f "); + fprintf (fout,"s resolution)\n------------\n"); + if (num < 1) return; + + // get maximum description length + maxl = 0; + for (i=0; i maxl) maxl = l; + } + + // calculate total time + double t1 = loadClockCount (event[0].cc); + double t2 = loadClockCount (event[num-1].cc); + double total = t2 - t1; + if (total <= 0) total = 1; + + // compute time difference for all slots except the last one. update totals + double *times = (double*) ALLOCA (num * sizeof(double)); + for (i=0; i < (num-1); i++) { + double t1 = loadClockCount (event[i].cc); + double t2 = loadClockCount (event[i+1].cc); + times[i] = t2 - t1; + event[i].count++; + event[i].total_t += times[i]; + event[i].total_p += times[i]/total * 100.0; + } + + // print report (with optional averages) + for (i=0; ifirstbody; bb; bb=(dxBody*)bb->next ) + { + // don't freeze objects mid-air (patch 1586738) + if ( bb->firstjoint == NULL ) continue; + + // nothing to do unless this body is currently enabled and has + // the auto-disable flag set + if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue; + + // if sampling / threshold testing is disabled, we can never sleep. + if ( bb->adis.average_samples == 0 ) continue; + + // + // see if the body is idle + // + +#ifndef dNODEBUG + // sanity check + if ( bb->average_counter >= bb->adis.average_samples ) + { + dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" ); + + // something is going wrong, reset the average-calculations + bb->average_ready = 0; // not ready for average calculation + bb->average_counter = 0; // reset the buffer index + } +#endif // dNODEBUG + + // sample the linear and angular velocity + bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0]; + bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1]; + bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2]; + bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0]; + bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1]; + bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2]; + bb->average_counter++; + + // buffer ready test + if ( bb->average_counter >= bb->adis.average_samples ) + { + bb->average_counter = 0; // fill the buffer from the beginning + bb->average_ready = 1; // this body is ready now for average calculation + } + + int idle = 0; // Assume it's in motion unless we have samples to disprove it. + + // enough samples? + if ( bb->average_ready ) + { + idle = 1; // Initial assumption: IDLE + + // the sample buffers are filled and ready for calculation + dVector3 average_lvel, average_avel; + + // Store first velocity samples + average_lvel[0] = bb->average_lvel_buffer[0][0]; + average_avel[0] = bb->average_avel_buffer[0][0]; + average_lvel[1] = bb->average_lvel_buffer[0][1]; + average_avel[1] = bb->average_avel_buffer[0][1]; + average_lvel[2] = bb->average_lvel_buffer[0][2]; + average_avel[2] = bb->average_avel_buffer[0][2]; + + // If we're not in "instantaneous mode" + if ( bb->adis.average_samples > 1 ) + { + // add remaining velocities together + for ( unsigned int i = 1; i < bb->adis.average_samples; ++i ) + { + average_lvel[0] += bb->average_lvel_buffer[i][0]; + average_avel[0] += bb->average_avel_buffer[i][0]; + average_lvel[1] += bb->average_lvel_buffer[i][1]; + average_avel[1] += bb->average_avel_buffer[i][1]; + average_lvel[2] += bb->average_lvel_buffer[i][2]; + average_avel[2] += bb->average_avel_buffer[i][2]; + } + + // make average + dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples ); + + average_lvel[0] *= r1; + average_avel[0] *= r1; + average_lvel[1] *= r1; + average_avel[1] *= r1; + average_lvel[2] *= r1; + average_avel[2] *= r1; + } + + // threshold test + dReal av_lspeed, av_aspeed; + av_lspeed = dDOT( average_lvel, average_lvel ); + if ( av_lspeed > bb->adis.linear_average_threshold ) + { + idle = 0; // average linear velocity is too high for idle + } + else + { + av_aspeed = dDOT( average_avel, average_avel ); + if ( av_aspeed > bb->adis.angular_average_threshold ) + { + idle = 0; // average angular velocity is too high for idle + } + } + } + + // if it's idle, accumulate steps and time. + // these counters won't overflow because this code doesn't run for disabled bodies. + if (idle) { + bb->adis_stepsleft--; + bb->adis_timeleft -= stepsize; + } + else { + // Reset countdowns + bb->adis_stepsleft = bb->adis.idle_steps; + bb->adis_timeleft = bb->adis.idle_time; + } + + // disable the body if it's idle for a long enough time + if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 ) + { + bb->flags |= dxBodyDisabled; // set the disable flag + + // disabling bodies should also include resetting the velocity + // should prevent jittering in big "islands" + bb->lvel[0] = 0; + bb->lvel[1] = 0; + bb->lvel[2] = 0; + bb->avel[0] = 0; + bb->avel[1] = 0; + bb->avel[2] = 0; + } + } +} + + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667); + else return dSin(x)/x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +void dxStepBody (dxBody *b, dReal h) +{ + int j; + + // handle linear velocity + for (j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis,b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL(0.5); + dReal theta = k * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + + b->avel[2]*b->avel[2]); + h *= REAL(0.5); + dReal theta = wlen * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2,q,b->q); + for (j=0; j<4; j++) b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) { + dReal dq[4]; + dWtoDQ (irv,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + } + else { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q,b->posr.R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) +{ + dxBody *b,*bb,**body; + dxJoint *j,**joint; + + // nothing to do if no bodies + if (world->nb <= 0) return; + + // handle auto-disabling of bodies + dInternalHandleAutoDisabling (world,stepsize); + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*)); + joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + + // set all body/joint tags to 0 + for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; + for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); + + for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) { + b = stack[--stacksize]; // pop body off stack + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (!n->joint->tag) { + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) { + n->body->tag = 1; + stack[stacksize++] = n->body; + } + } + } + dIASSERT(stacksize <= world->nb); + dIASSERT(stacksize <= world->nj); + } + + // now do something with body and joint lists + stepper (world,body,bcount,joint,jcount,stepsize); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i=0; itag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i=0; itag = 1; + } + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b=world->firstbody; b; b=(dxBody*)b->next) { + if (b->flags & dxBodyDisabled) { + if (b->tag) dDebug (0,"disabled body tagged"); + } + else { + if (!b->tag) dDebug (0,"enabled body not tagged"); + } + } + for (j=world->firstjoint; j; j=(dxJoint*)j->next) { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || + (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { + if (!j->tag) dDebug (0,"attached enabled joint not tagged"); + } + else { + if (j->tag) dDebug (0,"unattached or disabled joint tagged"); + } + } +# endif +} diff --git a/libraries/ode-0.9/ode/src/util.h b/libraries/ode-0.9/ode/src/util.h new file mode 100644 index 0000000..a8e6390 --- /dev/null +++ b/libraries/ode-0.9/ode/src/util.h @@ -0,0 +1,38 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_UTIL_H_ +#define _ODE_UTIL_H_ + +#include "objects.h" + + +void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize); +void dxStepBody (dxBody *b, dReal h); + +typedef void (*dstepper_fn_t) (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize); + +void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper); + + +#endif -- cgit v1.1