From 2f8d7092bc2c9609fa98d6888106b96f38b22828 Mon Sep 17 00:00:00 2001 From: dan miller Date: Sun, 21 Oct 2007 08:36:32 +0000 Subject: libraries moved to opensim-libs, a new repository --- 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 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/Makefile.am delete mode 100644 libraries/ode-0.9/ode/src/Makefile.in delete mode 100644 libraries/ode-0.9/ode/src/array.cpp delete mode 100644 libraries/ode-0.9/ode/src/array.h delete mode 100644 libraries/ode-0.9/ode/src/box.cpp delete mode 100644 libraries/ode-0.9/ode/src/capsule.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_box.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_kernel.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_kernel.h delete mode 100644 libraries/ode-0.9/ode/src/collision_quadtreespace.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_space.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_space_internal.h delete mode 100644 libraries/ode-0.9/ode/src/collision_std.h delete mode 100644 libraries/ode-0.9/ode/src/collision_transform.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_transform.h delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_box.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_internal.h delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_util.cpp delete mode 100644 libraries/ode-0.9/ode/src/collision_util.h delete mode 100644 libraries/ode-0.9/ode/src/convex.cpp delete mode 100644 libraries/ode-0.9/ode/src/cylinder.cpp delete mode 100644 libraries/ode-0.9/ode/src/error.cpp delete mode 100644 libraries/ode-0.9/ode/src/export-dif.cpp delete mode 100644 libraries/ode-0.9/ode/src/fastdot.c delete mode 100644 libraries/ode-0.9/ode/src/fastldlt.c delete mode 100644 libraries/ode-0.9/ode/src/fastlsolve.c delete mode 100644 libraries/ode-0.9/ode/src/fastltsolve.c delete mode 100644 libraries/ode-0.9/ode/src/heightfield.cpp delete mode 100644 libraries/ode-0.9/ode/src/heightfield.h delete mode 100644 libraries/ode-0.9/ode/src/joint.cpp delete mode 100644 libraries/ode-0.9/ode/src/joint.h delete mode 100644 libraries/ode-0.9/ode/src/lcp.cpp delete mode 100644 libraries/ode-0.9/ode/src/lcp.h delete mode 100644 libraries/ode-0.9/ode/src/mass.cpp delete mode 100644 libraries/ode-0.9/ode/src/mat.cpp delete mode 100644 libraries/ode-0.9/ode/src/mat.h delete mode 100644 libraries/ode-0.9/ode/src/matrix.cpp delete mode 100644 libraries/ode-0.9/ode/src/memory.cpp delete mode 100644 libraries/ode-0.9/ode/src/misc.cpp delete mode 100644 libraries/ode-0.9/ode/src/objects.h delete mode 100644 libraries/ode-0.9/ode/src/obstack.cpp delete mode 100644 libraries/ode-0.9/ode/src/obstack.h delete mode 100644 libraries/ode-0.9/ode/src/ode.cpp delete mode 100644 libraries/ode-0.9/ode/src/odemath.cpp delete mode 100644 libraries/ode-0.9/ode/src/plane.cpp delete mode 100644 libraries/ode-0.9/ode/src/quickstep.cpp delete mode 100644 libraries/ode-0.9/ode/src/quickstep.h delete mode 100644 libraries/ode-0.9/ode/src/ray.cpp delete mode 100644 libraries/ode-0.9/ode/src/rotation.cpp delete mode 100644 libraries/ode-0.9/ode/src/scrapbook.cpp delete mode 100644 libraries/ode-0.9/ode/src/sphere.cpp delete mode 100644 libraries/ode-0.9/ode/src/stack.cpp delete mode 100644 libraries/ode-0.9/ode/src/stack.h delete mode 100644 libraries/ode-0.9/ode/src/step.cpp delete mode 100644 libraries/ode-0.9/ode/src/step.h delete mode 100755 libraries/ode-0.9/ode/src/stepfast.cpp delete mode 100644 libraries/ode-0.9/ode/src/testing.cpp delete mode 100644 libraries/ode-0.9/ode/src/testing.h delete mode 100644 libraries/ode-0.9/ode/src/timer.cpp delete mode 100644 libraries/ode-0.9/ode/src/util.cpp delete 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 deleted file mode 100644 index 1b0cf19..0000000 --- a/libraries/ode-0.9/ode/src/Makefile.am +++ /dev/null @@ -1,207 +0,0 @@ -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 deleted file mode 100644 index 548f2e2..0000000 --- a/libraries/ode-0.9/ode/src/Makefile.in +++ /dev/null @@ -1,2290 +0,0 @@ -# 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 deleted file mode 100644 index cbb1a6e..0000000 --- a/libraries/ode-0.9/ode/src/array.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 307206c..0000000 --- a/libraries/ode-0.9/ode/src/array.h +++ /dev/null @@ -1,135 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index f328651..0000000 --- a/libraries/ode-0.9/ode/src/box.cpp +++ /dev/null @@ -1,847 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 5680baa..0000000 --- a/libraries/ode-0.9/ode/src/capsule.cpp +++ /dev/null @@ -1,369 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index eb14c72..0000000 --- a/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp +++ /dev/null @@ -1,1007 +0,0 @@ -/************************************************************************* -* * -* 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 deleted file mode 100644 index f85701d..0000000 --- a/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/************************************************************************* -* * -* 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 deleted file mode 100644 index 964c531..0000000 --- a/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp +++ /dev/null @@ -1,264 +0,0 @@ -/************************************************************************* -* * -* 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 deleted file mode 100644 index 342be04..0000000 --- a/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp +++ /dev/null @@ -1,1145 +0,0 @@ -/************************************************************************* -* * -* 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 deleted file mode 100644 index b885603..0000000 --- a/libraries/ode-0.9/ode/src/collision_kernel.cpp +++ /dev/null @@ -1,1103 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index d5a2bc4..0000000 --- a/libraries/ode-0.9/ode/src/collision_kernel.h +++ /dev/null @@ -1,214 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 86a1833..0000000 --- a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp +++ /dev/null @@ -1,584 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 1a8fc81..0000000 --- a/libraries/ode-0.9/ode/src/collision_space.cpp +++ /dev/null @@ -1,790 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 004276a..0000000 --- a/libraries/ode-0.9/ode/src/collision_space_internal.h +++ /dev/null @@ -1,84 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index d203ad0..0000000 --- a/libraries/ode-0.9/ode/src/collision_std.h +++ /dev/null @@ -1,172 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 85fd623..0000000 --- a/libraries/ode-0.9/ode/src/collision_transform.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 406a687..0000000 --- a/libraries/ode-0.9/ode/src/collision_transform.h +++ /dev/null @@ -1,40 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 0194d02..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp +++ /dev/null @@ -1,1497 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index da235e0..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp +++ /dev/null @@ -1,1181 +0,0 @@ -/************************************************************************* -* * -* 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 deleted file mode 100644 index 717c763..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp +++ /dev/null @@ -1,1255 +0,0 @@ -// 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 deleted file mode 100644 index 229966b..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp +++ /dev/null @@ -1,456 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index bf474a2..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_internal.h +++ /dev/null @@ -1,495 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 07f3f8a..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp +++ /dev/null @@ -1,833 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 54034aa..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index c0b97ca..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 7ee9b4b..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp +++ /dev/null @@ -1,573 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index c9f209e..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp +++ /dev/null @@ -1,2033 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index c4af41c..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp +++ /dev/null @@ -1,1304 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 4c479e3..0000000 --- a/libraries/ode-0.9/ode/src/collision_util.h +++ /dev/null @@ -1,340 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index db93beb..0000000 --- a/libraries/ode-0.9/ode/src/convex.cpp +++ /dev/null @@ -1,1294 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 39a6cf3..0000000 --- a/libraries/ode-0.9/ode/src/cylinder.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 9b33db5..0000000 --- a/libraries/ode-0.9/ode/src/error.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 577d3e1..0000000 --- a/libraries/ode-0.9/ode/src/export-dif.cpp +++ /dev/null @@ -1,568 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 148d2dd..0000000 --- a/libraries/ode-0.9/ode/src/fastdot.c +++ /dev/null @@ -1,30 +0,0 @@ -/* 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 deleted file mode 100644 index df2ea6e..0000000 --- a/libraries/ode-0.9/ode/src/fastldlt.c +++ /dev/null @@ -1,381 +0,0 @@ -/* 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 deleted file mode 100644 index 0ae99d6..0000000 --- a/libraries/ode-0.9/ode/src/fastlsolve.c +++ /dev/null @@ -1,298 +0,0 @@ -/* 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 deleted file mode 100644 index eb950f6..0000000 --- a/libraries/ode-0.9/ode/src/fastltsolve.c +++ /dev/null @@ -1,199 +0,0 @@ -/* 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 deleted file mode 100644 index 36755fc..0000000 --- a/libraries/ode-0.9/ode/src/heightfield.cpp +++ /dev/null @@ -1,1812 +0,0 @@ -// 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 deleted file mode 100644 index f4c5952..0000000 --- a/libraries/ode-0.9/ode/src/heightfield.h +++ /dev/null @@ -1,225 +0,0 @@ -// 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 deleted file mode 100644 index d83294b..0000000 --- a/libraries/ode-0.9/ode/src/joint.cpp +++ /dev/null @@ -1,4065 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 27c013e..0000000 --- a/libraries/ode-0.9/ode/src/joint.h +++ /dev/null @@ -1,346 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index d03d3e8..0000000 --- a/libraries/ode-0.9/ode/src/lcp.cpp +++ /dev/null @@ -1,2007 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 484902c..0000000 --- a/libraries/ode-0.9/ode/src/lcp.h +++ /dev/null @@ -1,58 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 7d0b1c2..0000000 --- a/libraries/ode-0.9/ode/src/mass.cpp +++ /dev/null @@ -1,529 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 6e635dc..0000000 --- a/libraries/ode-0.9/ode/src/mat.cpp +++ /dev/null @@ -1,230 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 2814a01..0000000 --- a/libraries/ode-0.9/ode/src/mat.h +++ /dev/null @@ -1,71 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 16afe91..0000000 --- a/libraries/ode-0.9/ode/src/matrix.cpp +++ /dev/null @@ -1,358 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 1a6f263..0000000 --- a/libraries/ode-0.9/ode/src/misc.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index f286e2d..0000000 --- a/libraries/ode-0.9/ode/src/objects.h +++ /dev/null @@ -1,138 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 0840396..0000000 --- a/libraries/ode-0.9/ode/src/obstack.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index b5f715b..0000000 --- a/libraries/ode-0.9/ode/src/obstack.h +++ /dev/null @@ -1,68 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 46f559a..0000000 --- a/libraries/ode-0.9/ode/src/ode.cpp +++ /dev/null @@ -1,1732 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 11bc84e..0000000 --- a/libraries/ode-0.9/ode/src/odemath.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index e3afb49..0000000 --- a/libraries/ode-0.9/ode/src/plane.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 07aa9f7..0000000 --- a/libraries/ode-0.9/ode/src/quickstep.cpp +++ /dev/null @@ -1,880 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 43863e7..0000000 --- a/libraries/ode-0.9/ode/src/quickstep.h +++ /dev/null @@ -1,33 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index adf61ac..0000000 --- a/libraries/ode-0.9/ode/src/ray.cpp +++ /dev/null @@ -1,686 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index adb933b..0000000 --- a/libraries/ode-0.9/ode/src/rotation.cpp +++ /dev/null @@ -1,316 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 2621814..0000000 --- a/libraries/ode-0.9/ode/src/scrapbook.cpp +++ /dev/null @@ -1,485 +0,0 @@ - -/* - -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 deleted file mode 100644 index fa25aac..0000000 --- a/libraries/ode-0.9/ode/src/sphere.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index e062f92..0000000 --- a/libraries/ode-0.9/ode/src/stack.cpp +++ /dev/null @@ -1,114 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 5afff41..0000000 --- a/libraries/ode-0.9/ode/src/stack.h +++ /dev/null @@ -1,138 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 19d0473..0000000 --- a/libraries/ode-0.9/ode/src/step.cpp +++ /dev/null @@ -1,1795 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100755 index 35c45db..0000000 --- a/libraries/ode-0.9/ode/src/stepfast.cpp +++ /dev/null @@ -1,1139 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index a22f865..0000000 --- a/libraries/ode-0.9/ode/src/testing.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 4d19ff3..0000000 --- a/libraries/ode-0.9/ode/src/testing.h +++ /dev/null @@ -1,65 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index f754bf1..0000000 --- a/libraries/ode-0.9/ode/src/timer.cpp +++ /dev/null @@ -1,421 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index a8e6390..0000000 --- a/libraries/ode-0.9/ode/src/util.h +++ /dev/null @@ -1,38 +0,0 @@ -/************************************************************************* - * * - * 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