aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ode-0.9/ode/src/Makefile.am207
-rw-r--r--libraries/ode-0.9/ode/src/Makefile.in2290
-rw-r--r--libraries/ode-0.9/ode/src/array.cpp80
-rw-r--r--libraries/ode-0.9/ode/src/array.h135
-rw-r--r--libraries/ode-0.9/ode/src/box.cpp847
-rw-r--r--libraries/ode-0.9/ode/src/capsule.cpp369
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_box.cpp1007
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp254
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp264
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp1145
-rw-r--r--libraries/ode-0.9/ode/src/collision_kernel.cpp1103
-rw-r--r--libraries/ode-0.9/ode/src/collision_kernel.h214
-rw-r--r--libraries/ode-0.9/ode/src/collision_quadtreespace.cpp584
-rw-r--r--libraries/ode-0.9/ode/src/collision_space.cpp790
-rw-r--r--libraries/ode-0.9/ode/src/collision_space_internal.h84
-rw-r--r--libraries/ode-0.9/ode/src/collision_std.h172
-rw-r--r--libraries/ode-0.9/ode/src/collision_transform.cpp232
-rw-r--r--libraries/ode-0.9/ode/src/collision_transform.h40
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_box.cpp1497
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp1181
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp1255
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp456
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_internal.h495
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp833
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp213
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp198
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp573
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp2033
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp1304
-rw-r--r--libraries/ode-0.9/ode/src/collision_util.cpp612
-rw-r--r--libraries/ode-0.9/ode/src/collision_util.h340
-rw-r--r--libraries/ode-0.9/ode/src/convex.cpp1294
-rw-r--r--libraries/ode-0.9/ode/src/cylinder.cpp100
-rw-r--r--libraries/ode-0.9/ode/src/error.cpp172
-rw-r--r--libraries/ode-0.9/ode/src/export-dif.cpp568
-rw-r--r--libraries/ode-0.9/ode/src/fastdot.c30
-rw-r--r--libraries/ode-0.9/ode/src/fastldlt.c381
-rw-r--r--libraries/ode-0.9/ode/src/fastlsolve.c298
-rw-r--r--libraries/ode-0.9/ode/src/fastltsolve.c199
-rw-r--r--libraries/ode-0.9/ode/src/heightfield.cpp1812
-rw-r--r--libraries/ode-0.9/ode/src/heightfield.h225
-rw-r--r--libraries/ode-0.9/ode/src/joint.cpp4065
-rw-r--r--libraries/ode-0.9/ode/src/joint.h346
-rw-r--r--libraries/ode-0.9/ode/src/lcp.cpp2007
-rw-r--r--libraries/ode-0.9/ode/src/lcp.h58
-rw-r--r--libraries/ode-0.9/ode/src/mass.cpp529
-rw-r--r--libraries/ode-0.9/ode/src/mat.cpp230
-rw-r--r--libraries/ode-0.9/ode/src/mat.h71
-rw-r--r--libraries/ode-0.9/ode/src/matrix.cpp358
-rw-r--r--libraries/ode-0.9/ode/src/memory.cpp87
-rw-r--r--libraries/ode-0.9/ode/src/misc.cpp169
-rw-r--r--libraries/ode-0.9/ode/src/objects.h138
-rw-r--r--libraries/ode-0.9/ode/src/obstack.cpp130
-rw-r--r--libraries/ode-0.9/ode/src/obstack.h68
-rw-r--r--libraries/ode-0.9/ode/src/ode.cpp1732
-rw-r--r--libraries/ode-0.9/ode/src/odemath.cpp178
-rw-r--r--libraries/ode-0.9/ode/src/plane.cpp145
-rw-r--r--libraries/ode-0.9/ode/src/quickstep.cpp880
-rw-r--r--libraries/ode-0.9/ode/src/quickstep.h33
-rw-r--r--libraries/ode-0.9/ode/src/ray.cpp686
-rw-r--r--libraries/ode-0.9/ode/src/rotation.cpp316
-rw-r--r--libraries/ode-0.9/ode/src/scrapbook.cpp485
-rw-r--r--libraries/ode-0.9/ode/src/sphere.cpp241
-rw-r--r--libraries/ode-0.9/ode/src/stack.cpp114
-rw-r--r--libraries/ode-0.9/ode/src/stack.h138
-rw-r--r--libraries/ode-0.9/ode/src/step.cpp1795
-rw-r--r--libraries/ode-0.9/ode/src/step.h36
-rwxr-xr-xlibraries/ode-0.9/ode/src/stepfast.cpp1139
-rw-r--r--libraries/ode-0.9/ode/src/testing.cpp243
-rw-r--r--libraries/ode-0.9/ode/src/testing.h65
-rw-r--r--libraries/ode-0.9/ode/src/timer.cpp421
-rw-r--r--libraries/ode-0.9/ode/src/util.cpp374
-rw-r--r--libraries/ode-0.9/ode/src/util.h38
73 files changed, 43201 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/Makefile.am b/libraries/ode-0.9/ode/src/Makefile.am
new file mode 100644
index 0000000..1b0cf19
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/Makefile.am
@@ -0,0 +1,207 @@
1AM_CXXFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
2AM_CPPFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
3AM_CFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
4lib_LIBRARIES = libode.a
5libode_a_CPPFLAGS = -O2
6
7libode_a_CPPFLAGS += -fPIC
8
9# Fake an executable in order to get a shared library
10# Note the elegant and cunning way to trick Autotools to install a program
11# in a lib directory. --Rodrigo
12traplibdir=$(libdir)
13EXEEXT=@so_ext@
14traplib_PROGRAMS=libode
15libode_SOURCES=
16libode_DEPENDENCIES = libfast.a libode.a
17libode_LDFLAGS= @SHARED_LDFLAGS@
18if USE_SONAME
19libode_LDFLAGS+=-Wl,-soname,@ODE_SONAME@
20endif
21libode_LDADD=$(libode_a_OBJECTS) $(libfast_a_OBJECTS)
22
23if OPCODE
24libode_DEPENDENCIES+= libOPCODE.a
25libode_LDADD+=$(libOPCODE_a_OBJECTS)
26endif
27
28
29if GIMPACT
30libode_DEPENDENCIES+= libGIMPACT.a
31libode_LDADD+=$(libGIMPACT_a_OBJECTS)
32endif
33
34
35# convenience library to simulate per object cflags
36noinst_LIBRARIES= libfast.a
37libfast_a_CFLAGS= -O1
38libfast_a_SOURCES= fastldlt.c fastltsolve.c fastdot.c fastlsolve.c
39
40libfast_a_CFLAGS += -fPIC
41
42libode_a_DEPENDENCIES = libfast.a
43libode_a_LIBADD= $(libfast_a_OBJECTS)
44
45libode_a_SOURCES = objects.h \
46 obstack.cpp \
47 collision_util.cpp \
48 obstack.h \
49 array.cpp \
50 collision_util.h \
51 ode.cpp \
52 array.h \
53 error.cpp \
54 odemath.cpp \
55 collision_kernel.cpp \
56 export-dif.cpp \
57 quickstep.cpp \
58 collision_kernel.h \
59 quickstep.h \
60 collision_quadtreespace.cpp \
61 rotation.cpp \
62 collision_space.cpp \
63 collision_space_internal.h \
64 collision_cylinder_box.cpp \
65 collision_cylinder_sphere.cpp \
66 collision_cylinder_plane.cpp \
67 sphere.cpp \
68 box.cpp \
69 capsule.cpp \
70 plane.cpp \
71 ray.cpp \
72 cylinder.cpp \
73 convex.cpp \
74 joint.cpp \
75 stack.h \
76 collision_std.h \
77 joint.h \
78 step.cpp \
79 collision_transform.cpp \
80 lcp.cpp \
81 step.h \
82 collision_transform.h \
83 lcp.h \
84 stepfast.cpp \
85 mass.cpp \
86 testing.cpp \
87 mat.cpp \
88 testing.h \
89 mat.h \
90 timer.cpp \
91 matrix.cpp \
92 util.cpp \
93 memory.cpp \
94 util.h \
95 misc.cpp \
96 heightfield.cpp \
97 heightfield.h
98
99
100
101###################################
102# G I M P A C T S T U F F
103###################################
104
105
106if GIMPACT
107noinst_LIBRARIES+= libGIMPACT.a
108libGIMPACT_a_CPPFLAGS= -O2 -fno-strict-aliasing -fPIC
109
110libode_a_SOURCES+= collision_trimesh_gimpact.cpp
111
112libGIMPACT_a_SOURCES = \
113 @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp \
114 @TOPDIR@/GIMPACT/src/gim_contact.cpp \
115 @TOPDIR@/GIMPACT/src/gim_math.cpp \
116 @TOPDIR@/GIMPACT/src/gim_memory.cpp \
117 @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp \
118 @TOPDIR@/GIMPACT/src/gim_trimesh.cpp \
119 @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp \
120 @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp \
121 @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp \
122 @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp \
123 @TOPDIR@/GIMPACT/src/gimpact.cpp
124
125libode_a_DEPENDENCIES+=libGIMPACT.a
126libode_a_LIBADD+= $(libGIMPACT_a_OBJECTS)
127AM_CXXFLAGS += -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT
128AM_CFLAGS += -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT
129
130libode_a_SOURCES+= collision_trimesh_trimesh.cpp \
131 collision_trimesh_sphere.cpp \
132 collision_trimesh_ray.cpp \
133 collision_trimesh_opcode.cpp \
134 collision_trimesh_box.cpp \
135 collision_trimesh_ccylinder.cpp \
136 collision_trimesh_distance.cpp \
137 collision_trimesh_internal.h \
138 collision_cylinder_trimesh.cpp \
139 collision_trimesh_plane.cpp
140endif
141
142
143
144#################################
145# O P C O D E S T U F F
146#################################
147
148
149if OPCODE
150noinst_LIBRARIES+= libOPCODE.a
151libOPCODE_a_CPPFLAGS= -O2 -fno-strict-aliasing -fPIC
152
153
154libOPCODE_a_SOURCES= @TOPDIR@/OPCODE/OPC_AABBCollider.cpp \
155 @TOPDIR@/OPCODE/OPC_AABBTree.cpp \
156 @TOPDIR@/OPCODE/OPC_BaseModel.cpp \
157 @TOPDIR@/OPCODE/OPC_BoxPruning.cpp \
158 @TOPDIR@/OPCODE/OPC_Collider.cpp \
159 @TOPDIR@/OPCODE/OPC_Common.cpp \
160 @TOPDIR@/OPCODE/OPC_HybridModel.cpp \
161 @TOPDIR@/OPCODE/OPC_LSSCollider.cpp \
162 @TOPDIR@/OPCODE/OPC_MeshInterface.cpp \
163 @TOPDIR@/OPCODE/OPC_Model.cpp \
164 @TOPDIR@/OPCODE/OPC_OBBCollider.cpp \
165 @TOPDIR@/OPCODE/Opcode.cpp \
166 @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp \
167 @TOPDIR@/OPCODE/OPC_Picking.cpp \
168 @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp \
169 @TOPDIR@/OPCODE/OPC_RayCollider.cpp \
170 @TOPDIR@/OPCODE/OPC_SphereCollider.cpp \
171 @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp \
172 @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp \
173 @TOPDIR@/OPCODE/OPC_TreeCollider.cpp \
174 @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp \
175 @TOPDIR@/OPCODE/Ice/IceAABB.cpp \
176 @TOPDIR@/OPCODE/Ice/IceContainer.cpp \
177 @TOPDIR@/OPCODE/Ice/IceHPoint.cpp \
178 @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp \
179 @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp \
180 @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp \
181 @TOPDIR@/OPCODE/Ice/IceOBB.cpp \
182 @TOPDIR@/OPCODE/Ice/IcePlane.cpp \
183 @TOPDIR@/OPCODE/Ice/IcePoint.cpp \
184 @TOPDIR@/OPCODE/Ice/IceRandom.cpp \
185 @TOPDIR@/OPCODE/Ice/IceRay.cpp \
186 @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp \
187 @TOPDIR@/OPCODE/Ice/IceSegment.cpp \
188 @TOPDIR@/OPCODE/Ice/IceTriangle.cpp \
189 @TOPDIR@/OPCODE/Ice/IceUtils.cpp
190libode_a_DEPENDENCIES+=libOPCODE.a
191
192libode_a_LIBADD+= $(libOPCODE_a_OBJECTS)
193AM_CXXFLAGS += -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE
194AM_CFLAGS += -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE
195libode_a_SOURCES+= collision_trimesh_trimesh.cpp \
196 collision_trimesh_sphere.cpp \
197 collision_trimesh_ray.cpp \
198 collision_trimesh_opcode.cpp \
199 collision_trimesh_box.cpp \
200 collision_trimesh_ccylinder.cpp \
201 collision_trimesh_distance.cpp \
202 collision_trimesh_internal.h \
203 collision_cylinder_trimesh.cpp \
204 collision_trimesh_plane.cpp
205endif
206
207
diff --git a/libraries/ode-0.9/ode/src/Makefile.in b/libraries/ode-0.9/ode/src/Makefile.in
new file mode 100644
index 0000000..548f2e2
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/Makefile.in
@@ -0,0 +1,2290 @@
1# Makefile.in generated by automake 1.10 from Makefile.am.
2# @configure_input@
3
4# Copyright (C) 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002,
5# 2003, 2004, 2005, 2006 Free Software Foundation, Inc.
6# This Makefile.in is free software; the Free Software Foundation
7# gives unlimited permission to copy and/or distribute it,
8# with or without modifications, as long as this notice is preserved.
9
10# This program is distributed in the hope that it will be useful,
11# but WITHOUT ANY WARRANTY, to the extent permitted by law; without
12# even the implied warranty of MERCHANTABILITY or FITNESS FOR A
13# PARTICULAR PURPOSE.
14
15@SET_MAKE@
16
17
18VPATH = @srcdir@
19pkgdatadir = $(datadir)/@PACKAGE@
20pkglibdir = $(libdir)/@PACKAGE@
21pkgincludedir = $(includedir)/@PACKAGE@
22am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd
23install_sh_DATA = $(install_sh) -c -m 644
24install_sh_PROGRAM = $(install_sh) -c
25install_sh_SCRIPT = $(install_sh) -c
26INSTALL_HEADER = $(INSTALL_DATA)
27transform = $(program_transform_name)
28NORMAL_INSTALL = :
29PRE_INSTALL = :
30POST_INSTALL = :
31NORMAL_UNINSTALL = :
32PRE_UNINSTALL = :
33POST_UNINSTALL = :
34build_triplet = @build@
35host_triplet = @host@
36target_triplet = @target@
37traplib_PROGRAMS = libode$(EXEEXT)
38@USE_SONAME_TRUE@am__append_1 = -Wl,-soname,@ODE_SONAME@
39@OPCODE_TRUE@am__append_2 = libOPCODE.a
40@OPCODE_TRUE@am__append_3 = $(libOPCODE_a_OBJECTS)
41@GIMPACT_TRUE@am__append_4 = libGIMPACT.a
42@GIMPACT_TRUE@am__append_5 = $(libGIMPACT_a_OBJECTS)
43
44###################################
45# G I M P A C T S T U F F
46###################################
47@GIMPACT_TRUE@am__append_6 = libGIMPACT.a
48@GIMPACT_TRUE@am__append_7 = collision_trimesh_gimpact.cpp \
49@GIMPACT_TRUE@ collision_trimesh_trimesh.cpp \
50@GIMPACT_TRUE@ collision_trimesh_sphere.cpp \
51@GIMPACT_TRUE@ collision_trimesh_ray.cpp \
52@GIMPACT_TRUE@ collision_trimesh_opcode.cpp \
53@GIMPACT_TRUE@ collision_trimesh_box.cpp \
54@GIMPACT_TRUE@ collision_trimesh_ccylinder.cpp \
55@GIMPACT_TRUE@ collision_trimesh_distance.cpp \
56@GIMPACT_TRUE@ collision_trimesh_internal.h \
57@GIMPACT_TRUE@ collision_cylinder_trimesh.cpp \
58@GIMPACT_TRUE@ collision_trimesh_plane.cpp
59@GIMPACT_TRUE@am__append_8 = libGIMPACT.a
60@GIMPACT_TRUE@am__append_9 = $(libGIMPACT_a_OBJECTS)
61@GIMPACT_TRUE@am__append_10 = -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT
62@GIMPACT_TRUE@am__append_11 = -I@TOPDIR@/GIMPACT/include -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT
63
64#################################
65# O P C O D E S T U F F
66#################################
67@OPCODE_TRUE@am__append_12 = libOPCODE.a
68@OPCODE_TRUE@am__append_13 = libOPCODE.a
69@OPCODE_TRUE@am__append_14 = $(libOPCODE_a_OBJECTS)
70@OPCODE_TRUE@am__append_15 = -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE
71@OPCODE_TRUE@am__append_16 = -I@TOPDIR@/OPCODE -I@TOPDIR@/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE
72@OPCODE_TRUE@am__append_17 = collision_trimesh_trimesh.cpp \
73@OPCODE_TRUE@ collision_trimesh_sphere.cpp \
74@OPCODE_TRUE@ collision_trimesh_ray.cpp \
75@OPCODE_TRUE@ collision_trimesh_opcode.cpp \
76@OPCODE_TRUE@ collision_trimesh_box.cpp \
77@OPCODE_TRUE@ collision_trimesh_ccylinder.cpp \
78@OPCODE_TRUE@ collision_trimesh_distance.cpp \
79@OPCODE_TRUE@ collision_trimesh_internal.h \
80@OPCODE_TRUE@ collision_cylinder_trimesh.cpp \
81@OPCODE_TRUE@ collision_trimesh_plane.cpp
82
83subdir = ode/src
84DIST_COMMON = $(srcdir)/Makefile.am $(srcdir)/Makefile.in
85ACLOCAL_M4 = $(top_srcdir)/aclocal.m4
86am__aclocal_m4_deps = $(top_srcdir)/configure.in
87am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \
88 $(ACLOCAL_M4)
89mkinstalldirs = $(install_sh) -d
90CONFIG_HEADER = $(top_builddir)/include/ode/config.h
91CONFIG_CLEAN_FILES =
92am__vpath_adj_setup = srcdirstrip=`echo "$(srcdir)" | sed 's|.|.|g'`;
93am__vpath_adj = case $$p in \
94 $(srcdir)/*) f=`echo "$$p" | sed "s|^$$srcdirstrip/||"`;; \
95 *) f=$$p;; \
96 esac;
97am__strip_dir = `echo $$p | sed -e 's|^.*/||'`;
98am__installdirs = "$(DESTDIR)$(libdir)" "$(DESTDIR)$(traplibdir)"
99libLIBRARIES_INSTALL = $(INSTALL_DATA)
100LIBRARIES = $(lib_LIBRARIES) $(noinst_LIBRARIES)
101AR = ar
102ARFLAGS = cru
103libGIMPACT_a_AR = $(AR) $(ARFLAGS)
104libGIMPACT_a_LIBADD =
105am__libGIMPACT_a_SOURCES_DIST = \
106 @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp \
107 @TOPDIR@/GIMPACT/src/gim_contact.cpp \
108 @TOPDIR@/GIMPACT/src/gim_math.cpp \
109 @TOPDIR@/GIMPACT/src/gim_memory.cpp \
110 @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp \
111 @TOPDIR@/GIMPACT/src/gim_trimesh.cpp \
112 @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp \
113 @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp \
114 @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp \
115 @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp \
116 @TOPDIR@/GIMPACT/src/gimpact.cpp
117@GIMPACT_TRUE@am_libGIMPACT_a_OBJECTS = \
118@GIMPACT_TRUE@ libGIMPACT_a-gim_boxpruning.$(OBJEXT) \
119@GIMPACT_TRUE@ libGIMPACT_a-gim_contact.$(OBJEXT) \
120@GIMPACT_TRUE@ libGIMPACT_a-gim_math.$(OBJEXT) \
121@GIMPACT_TRUE@ libGIMPACT_a-gim_memory.$(OBJEXT) \
122@GIMPACT_TRUE@ libGIMPACT_a-gim_tri_tri_overlap.$(OBJEXT) \
123@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh.$(OBJEXT) \
124@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_capsule_collision.$(OBJEXT) \
125@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_ray_collision.$(OBJEXT) \
126@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_sphere_collision.$(OBJEXT) \
127@GIMPACT_TRUE@ libGIMPACT_a-gim_trimesh_trimesh_collision.$(OBJEXT) \
128@GIMPACT_TRUE@ libGIMPACT_a-gimpact.$(OBJEXT)
129libGIMPACT_a_OBJECTS = $(am_libGIMPACT_a_OBJECTS)
130libOPCODE_a_AR = $(AR) $(ARFLAGS)
131libOPCODE_a_LIBADD =
132am__libOPCODE_a_SOURCES_DIST = @TOPDIR@/OPCODE/OPC_AABBCollider.cpp \
133 @TOPDIR@/OPCODE/OPC_AABBTree.cpp \
134 @TOPDIR@/OPCODE/OPC_BaseModel.cpp \
135 @TOPDIR@/OPCODE/OPC_BoxPruning.cpp \
136 @TOPDIR@/OPCODE/OPC_Collider.cpp \
137 @TOPDIR@/OPCODE/OPC_Common.cpp \
138 @TOPDIR@/OPCODE/OPC_HybridModel.cpp \
139 @TOPDIR@/OPCODE/OPC_LSSCollider.cpp \
140 @TOPDIR@/OPCODE/OPC_MeshInterface.cpp \
141 @TOPDIR@/OPCODE/OPC_Model.cpp \
142 @TOPDIR@/OPCODE/OPC_OBBCollider.cpp @TOPDIR@/OPCODE/Opcode.cpp \
143 @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp \
144 @TOPDIR@/OPCODE/OPC_Picking.cpp \
145 @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp \
146 @TOPDIR@/OPCODE/OPC_RayCollider.cpp \
147 @TOPDIR@/OPCODE/OPC_SphereCollider.cpp \
148 @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp \
149 @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp \
150 @TOPDIR@/OPCODE/OPC_TreeCollider.cpp \
151 @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp \
152 @TOPDIR@/OPCODE/Ice/IceAABB.cpp \
153 @TOPDIR@/OPCODE/Ice/IceContainer.cpp \
154 @TOPDIR@/OPCODE/Ice/IceHPoint.cpp \
155 @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp \
156 @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp \
157 @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp \
158 @TOPDIR@/OPCODE/Ice/IceOBB.cpp \
159 @TOPDIR@/OPCODE/Ice/IcePlane.cpp \
160 @TOPDIR@/OPCODE/Ice/IcePoint.cpp \
161 @TOPDIR@/OPCODE/Ice/IceRandom.cpp \
162 @TOPDIR@/OPCODE/Ice/IceRay.cpp \
163 @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp \
164 @TOPDIR@/OPCODE/Ice/IceSegment.cpp \
165 @TOPDIR@/OPCODE/Ice/IceTriangle.cpp \
166 @TOPDIR@/OPCODE/Ice/IceUtils.cpp
167@OPCODE_TRUE@am_libOPCODE_a_OBJECTS = \
168@OPCODE_TRUE@ libOPCODE_a-OPC_AABBCollider.$(OBJEXT) \
169@OPCODE_TRUE@ libOPCODE_a-OPC_AABBTree.$(OBJEXT) \
170@OPCODE_TRUE@ libOPCODE_a-OPC_BaseModel.$(OBJEXT) \
171@OPCODE_TRUE@ libOPCODE_a-OPC_BoxPruning.$(OBJEXT) \
172@OPCODE_TRUE@ libOPCODE_a-OPC_Collider.$(OBJEXT) \
173@OPCODE_TRUE@ libOPCODE_a-OPC_Common.$(OBJEXT) \
174@OPCODE_TRUE@ libOPCODE_a-OPC_HybridModel.$(OBJEXT) \
175@OPCODE_TRUE@ libOPCODE_a-OPC_LSSCollider.$(OBJEXT) \
176@OPCODE_TRUE@ libOPCODE_a-OPC_MeshInterface.$(OBJEXT) \
177@OPCODE_TRUE@ libOPCODE_a-OPC_Model.$(OBJEXT) \
178@OPCODE_TRUE@ libOPCODE_a-OPC_OBBCollider.$(OBJEXT) \
179@OPCODE_TRUE@ libOPCODE_a-Opcode.$(OBJEXT) \
180@OPCODE_TRUE@ libOPCODE_a-OPC_OptimizedTree.$(OBJEXT) \
181@OPCODE_TRUE@ libOPCODE_a-OPC_Picking.$(OBJEXT) \
182@OPCODE_TRUE@ libOPCODE_a-OPC_PlanesCollider.$(OBJEXT) \
183@OPCODE_TRUE@ libOPCODE_a-OPC_RayCollider.$(OBJEXT) \
184@OPCODE_TRUE@ libOPCODE_a-OPC_SphereCollider.$(OBJEXT) \
185@OPCODE_TRUE@ libOPCODE_a-OPC_SweepAndPrune.$(OBJEXT) \
186@OPCODE_TRUE@ libOPCODE_a-OPC_TreeBuilders.$(OBJEXT) \
187@OPCODE_TRUE@ libOPCODE_a-OPC_TreeCollider.$(OBJEXT) \
188@OPCODE_TRUE@ libOPCODE_a-OPC_VolumeCollider.$(OBJEXT) \
189@OPCODE_TRUE@ libOPCODE_a-IceAABB.$(OBJEXT) \
190@OPCODE_TRUE@ libOPCODE_a-IceContainer.$(OBJEXT) \
191@OPCODE_TRUE@ libOPCODE_a-IceHPoint.$(OBJEXT) \
192@OPCODE_TRUE@ libOPCODE_a-IceIndexedTriangle.$(OBJEXT) \
193@OPCODE_TRUE@ libOPCODE_a-IceMatrix3x3.$(OBJEXT) \
194@OPCODE_TRUE@ libOPCODE_a-IceMatrix4x4.$(OBJEXT) \
195@OPCODE_TRUE@ libOPCODE_a-IceOBB.$(OBJEXT) \
196@OPCODE_TRUE@ libOPCODE_a-IcePlane.$(OBJEXT) \
197@OPCODE_TRUE@ libOPCODE_a-IcePoint.$(OBJEXT) \
198@OPCODE_TRUE@ libOPCODE_a-IceRandom.$(OBJEXT) \
199@OPCODE_TRUE@ libOPCODE_a-IceRay.$(OBJEXT) \
200@OPCODE_TRUE@ libOPCODE_a-IceRevisitedRadix.$(OBJEXT) \
201@OPCODE_TRUE@ libOPCODE_a-IceSegment.$(OBJEXT) \
202@OPCODE_TRUE@ libOPCODE_a-IceTriangle.$(OBJEXT) \
203@OPCODE_TRUE@ libOPCODE_a-IceUtils.$(OBJEXT)
204libOPCODE_a_OBJECTS = $(am_libOPCODE_a_OBJECTS)
205libfast_a_AR = $(AR) $(ARFLAGS)
206libfast_a_LIBADD =
207am_libfast_a_OBJECTS = libfast_a-fastldlt.$(OBJEXT) \
208 libfast_a-fastltsolve.$(OBJEXT) libfast_a-fastdot.$(OBJEXT) \
209 libfast_a-fastlsolve.$(OBJEXT)
210libfast_a_OBJECTS = $(am_libfast_a_OBJECTS)
211libode_a_AR = $(AR) $(ARFLAGS)
212am__libode_a_SOURCES_DIST = objects.h obstack.cpp collision_util.cpp \
213 obstack.h array.cpp collision_util.h ode.cpp array.h error.cpp \
214 odemath.cpp collision_kernel.cpp export-dif.cpp quickstep.cpp \
215 collision_kernel.h quickstep.h collision_quadtreespace.cpp \
216 rotation.cpp collision_space.cpp collision_space_internal.h \
217 collision_cylinder_box.cpp collision_cylinder_sphere.cpp \
218 collision_cylinder_plane.cpp sphere.cpp box.cpp capsule.cpp \
219 plane.cpp ray.cpp cylinder.cpp convex.cpp joint.cpp stack.h \
220 collision_std.h joint.h step.cpp collision_transform.cpp \
221 lcp.cpp step.h collision_transform.h lcp.h stepfast.cpp \
222 mass.cpp testing.cpp mat.cpp testing.h mat.h timer.cpp \
223 matrix.cpp util.cpp memory.cpp util.h misc.cpp heightfield.cpp \
224 heightfield.h collision_trimesh_gimpact.cpp \
225 collision_trimesh_trimesh.cpp collision_trimesh_sphere.cpp \
226 collision_trimesh_ray.cpp collision_trimesh_opcode.cpp \
227 collision_trimesh_box.cpp collision_trimesh_ccylinder.cpp \
228 collision_trimesh_distance.cpp collision_trimesh_internal.h \
229 collision_cylinder_trimesh.cpp collision_trimesh_plane.cpp
230@GIMPACT_TRUE@am__objects_1 = \
231@GIMPACT_TRUE@ libode_a-collision_trimesh_gimpact.$(OBJEXT) \
232@GIMPACT_TRUE@ libode_a-collision_trimesh_trimesh.$(OBJEXT) \
233@GIMPACT_TRUE@ libode_a-collision_trimesh_sphere.$(OBJEXT) \
234@GIMPACT_TRUE@ libode_a-collision_trimesh_ray.$(OBJEXT) \
235@GIMPACT_TRUE@ libode_a-collision_trimesh_opcode.$(OBJEXT) \
236@GIMPACT_TRUE@ libode_a-collision_trimesh_box.$(OBJEXT) \
237@GIMPACT_TRUE@ libode_a-collision_trimesh_ccylinder.$(OBJEXT) \
238@GIMPACT_TRUE@ libode_a-collision_trimesh_distance.$(OBJEXT) \
239@GIMPACT_TRUE@ libode_a-collision_cylinder_trimesh.$(OBJEXT) \
240@GIMPACT_TRUE@ libode_a-collision_trimesh_plane.$(OBJEXT)
241@OPCODE_TRUE@am__objects_2 = \
242@OPCODE_TRUE@ libode_a-collision_trimesh_trimesh.$(OBJEXT) \
243@OPCODE_TRUE@ libode_a-collision_trimesh_sphere.$(OBJEXT) \
244@OPCODE_TRUE@ libode_a-collision_trimesh_ray.$(OBJEXT) \
245@OPCODE_TRUE@ libode_a-collision_trimesh_opcode.$(OBJEXT) \
246@OPCODE_TRUE@ libode_a-collision_trimesh_box.$(OBJEXT) \
247@OPCODE_TRUE@ libode_a-collision_trimesh_ccylinder.$(OBJEXT) \
248@OPCODE_TRUE@ libode_a-collision_trimesh_distance.$(OBJEXT) \
249@OPCODE_TRUE@ libode_a-collision_cylinder_trimesh.$(OBJEXT) \
250@OPCODE_TRUE@ libode_a-collision_trimesh_plane.$(OBJEXT)
251am_libode_a_OBJECTS = libode_a-obstack.$(OBJEXT) \
252 libode_a-collision_util.$(OBJEXT) libode_a-array.$(OBJEXT) \
253 libode_a-ode.$(OBJEXT) libode_a-error.$(OBJEXT) \
254 libode_a-odemath.$(OBJEXT) libode_a-collision_kernel.$(OBJEXT) \
255 libode_a-export-dif.$(OBJEXT) libode_a-quickstep.$(OBJEXT) \
256 libode_a-collision_quadtreespace.$(OBJEXT) \
257 libode_a-rotation.$(OBJEXT) libode_a-collision_space.$(OBJEXT) \
258 libode_a-collision_cylinder_box.$(OBJEXT) \
259 libode_a-collision_cylinder_sphere.$(OBJEXT) \
260 libode_a-collision_cylinder_plane.$(OBJEXT) \
261 libode_a-sphere.$(OBJEXT) libode_a-box.$(OBJEXT) \
262 libode_a-capsule.$(OBJEXT) libode_a-plane.$(OBJEXT) \
263 libode_a-ray.$(OBJEXT) libode_a-cylinder.$(OBJEXT) \
264 libode_a-convex.$(OBJEXT) libode_a-joint.$(OBJEXT) \
265 libode_a-step.$(OBJEXT) libode_a-collision_transform.$(OBJEXT) \
266 libode_a-lcp.$(OBJEXT) libode_a-stepfast.$(OBJEXT) \
267 libode_a-mass.$(OBJEXT) libode_a-testing.$(OBJEXT) \
268 libode_a-mat.$(OBJEXT) libode_a-timer.$(OBJEXT) \
269 libode_a-matrix.$(OBJEXT) libode_a-util.$(OBJEXT) \
270 libode_a-memory.$(OBJEXT) libode_a-misc.$(OBJEXT) \
271 libode_a-heightfield.$(OBJEXT) $(am__objects_1) \
272 $(am__objects_2)
273libode_a_OBJECTS = $(am_libode_a_OBJECTS)
274traplibPROGRAMS_INSTALL = $(INSTALL_PROGRAM)
275PROGRAMS = $(traplib_PROGRAMS)
276am_libode_OBJECTS =
277libode_OBJECTS = $(am_libode_OBJECTS)
278libode_LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(libode_LDFLAGS) \
279 $(LDFLAGS) -o $@
280DEFAULT_INCLUDES = -I. -I$(top_builddir)/include/ode@am__isrc@
281depcomp = $(SHELL) $(top_srcdir)/depcomp
282am__depfiles_maybe = depfiles
283COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \
284 $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS)
285CCLD = $(CC)
286LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@
287CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \
288 $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS)
289CXXLD = $(CXX)
290CXXLINK = $(CXXLD) $(AM_CXXFLAGS) $(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) \
291 -o $@
292SOURCES = $(libGIMPACT_a_SOURCES) $(libOPCODE_a_SOURCES) \
293 $(libfast_a_SOURCES) $(libode_a_SOURCES) $(libode_SOURCES)
294DIST_SOURCES = $(am__libGIMPACT_a_SOURCES_DIST) \
295 $(am__libOPCODE_a_SOURCES_DIST) $(libfast_a_SOURCES) \
296 $(am__libode_a_SOURCES_DIST) $(libode_SOURCES)
297ETAGS = etags
298CTAGS = ctags
299DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST)
300ACLOCAL = @ACLOCAL@
301ALLOCA = @ALLOCA@
302AMTAR = @AMTAR@
303ARCHFLAGS = @ARCHFLAGS@
304AUTOCONF = @AUTOCONF@
305AUTOHEADER = @AUTOHEADER@
306AUTOMAKE = @AUTOMAKE@
307AWK = @AWK@
308CC = @CC@
309CCDEPMODE = @CCDEPMODE@
310CFLAGS = @CFLAGS@
311CPP = @CPP@
312CPPFLAGS = @CPPFLAGS@
313CXX = @CXX@
314CXXDEPMODE = @CXXDEPMODE@
315CXXFLAGS = @CXXFLAGS@
316CYGPATH_W = @CYGPATH_W@
317DEFS = @DEFS@
318DEPDIR = @DEPDIR@
319DRAWSTUFF = @DRAWSTUFF@
320ECHO_C = @ECHO_C@
321ECHO_N = @ECHO_N@
322ECHO_T = @ECHO_T@
323EGREP = @EGREP@
324EXEEXT = @so_ext@
325GL_LIBS = @GL_LIBS@
326GREP = @GREP@
327INSTALL = @INSTALL@
328INSTALL_DATA = @INSTALL_DATA@
329INSTALL_PROGRAM = @INSTALL_PROGRAM@
330INSTALL_SCRIPT = @INSTALL_SCRIPT@
331INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@
332LDFLAGS = @LDFLAGS@
333LIBOBJS = @LIBOBJS@
334LIBS = @LIBS@
335LTLIBOBJS = @LTLIBOBJS@
336MAKEINFO = @MAKEINFO@
337MKDIR_P = @MKDIR_P@
338OBJEXT = @OBJEXT@
339ODE_AGE = @ODE_AGE@
340ODE_CURRENT = @ODE_CURRENT@
341ODE_RELEASE = @ODE_RELEASE@
342ODE_REVISION = @ODE_REVISION@
343ODE_SONAME = @ODE_SONAME@
344PACKAGE = @PACKAGE@
345PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@
346PACKAGE_NAME = @PACKAGE_NAME@
347PACKAGE_STRING = @PACKAGE_STRING@
348PACKAGE_TARNAME = @PACKAGE_TARNAME@
349PACKAGE_VERSION = @PACKAGE_VERSION@
350PATH_SEPARATOR = @PATH_SEPARATOR@
351RANLIB = @RANLIB@
352SET_MAKE = @SET_MAKE@
353SHARED_LDFLAGS = @SHARED_LDFLAGS@
354SHELL = @SHELL@
355STRIP = @STRIP@
356TOPDIR = @TOPDIR@
357VERSION = @VERSION@
358WINDRES = @WINDRES@
359XMKMF = @XMKMF@
360X_CFLAGS = @X_CFLAGS@
361X_EXTRA_LIBS = @X_EXTRA_LIBS@
362X_LIBS = @X_LIBS@
363X_PRE_LIBS = @X_PRE_LIBS@
364abs_builddir = @abs_builddir@
365abs_srcdir = @abs_srcdir@
366abs_top_builddir = @abs_top_builddir@
367abs_top_srcdir = @abs_top_srcdir@
368ac_ct_CC = @ac_ct_CC@
369ac_ct_CXX = @ac_ct_CXX@
370ac_ct_WINDRES = @ac_ct_WINDRES@
371am__include = @am__include@
372am__leading_dot = @am__leading_dot@
373am__quote = @am__quote@
374am__tar = @am__tar@
375am__untar = @am__untar@
376bindir = @bindir@
377build = @build@
378build_alias = @build_alias@
379build_cpu = @build_cpu@
380build_os = @build_os@
381build_vendor = @build_vendor@
382builddir = @builddir@
383datadir = @datadir@
384datarootdir = @datarootdir@
385docdir = @docdir@
386dvidir = @dvidir@
387exec_prefix = @exec_prefix@
388host = @host@
389host_alias = @host_alias@
390host_cpu = @host_cpu@
391host_os = @host_os@
392host_vendor = @host_vendor@
393htmldir = @htmldir@
394includedir = @includedir@
395infodir = @infodir@
396install_sh = @install_sh@
397libdir = @libdir@
398libexecdir = @libexecdir@
399localedir = @localedir@
400localstatedir = @localstatedir@
401mandir = @mandir@
402mkdir_p = @mkdir_p@
403oldincludedir = @oldincludedir@
404pdfdir = @pdfdir@
405prefix = @prefix@
406program_transform_name = @program_transform_name@
407psdir = @psdir@
408sbindir = @sbindir@
409sharedstatedir = @sharedstatedir@
410so_ext = @so_ext@
411srcdir = @srcdir@
412sysconfdir = @sysconfdir@
413target = @target@
414target_alias = @target_alias@
415target_cpu = @target_cpu@
416target_os = @target_os@
417target_vendor = @target_vendor@
418top_builddir = @top_builddir@
419top_srcdir = @top_srcdir@
420AM_CXXFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include \
421 -I$(top_builddir)/include $(am__append_10) $(am__append_15)
422AM_CPPFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include -I$(top_builddir)/include
423AM_CFLAGS = @ARCHFLAGS@ -I$(top_srcdir)/include \
424 -I$(top_builddir)/include $(am__append_11) $(am__append_16)
425lib_LIBRARIES = libode.a
426libode_a_CPPFLAGS = -O2 -fPIC
427
428# Fake an executable in order to get a shared library
429# Note the elegant and cunning way to trick Autotools to install a program
430# in a lib directory. --Rodrigo
431traplibdir = $(libdir)
432libode_SOURCES =
433libode_DEPENDENCIES = libfast.a libode.a $(am__append_2) \
434 $(am__append_4)
435libode_LDFLAGS = @SHARED_LDFLAGS@ $(am__append_1)
436libode_LDADD = $(libode_a_OBJECTS) $(libfast_a_OBJECTS) \
437 $(am__append_3) $(am__append_5)
438
439# convenience library to simulate per object cflags
440noinst_LIBRARIES = libfast.a $(am__append_6) $(am__append_12)
441libfast_a_CFLAGS = -O1 -fPIC
442libfast_a_SOURCES = fastldlt.c fastltsolve.c fastdot.c fastlsolve.c
443libode_a_DEPENDENCIES = libfast.a $(am__append_8) $(am__append_13)
444libode_a_LIBADD = $(libfast_a_OBJECTS) $(am__append_9) \
445 $(am__append_14)
446libode_a_SOURCES = objects.h obstack.cpp collision_util.cpp obstack.h \
447 array.cpp collision_util.h ode.cpp array.h error.cpp \
448 odemath.cpp collision_kernel.cpp export-dif.cpp quickstep.cpp \
449 collision_kernel.h quickstep.h collision_quadtreespace.cpp \
450 rotation.cpp collision_space.cpp collision_space_internal.h \
451 collision_cylinder_box.cpp collision_cylinder_sphere.cpp \
452 collision_cylinder_plane.cpp sphere.cpp box.cpp capsule.cpp \
453 plane.cpp ray.cpp cylinder.cpp convex.cpp joint.cpp stack.h \
454 collision_std.h joint.h step.cpp collision_transform.cpp \
455 lcp.cpp step.h collision_transform.h lcp.h stepfast.cpp \
456 mass.cpp testing.cpp mat.cpp testing.h mat.h timer.cpp \
457 matrix.cpp util.cpp memory.cpp util.h misc.cpp heightfield.cpp \
458 heightfield.h $(am__append_7) $(am__append_17)
459@GIMPACT_TRUE@libGIMPACT_a_CPPFLAGS = -O2 -fno-strict-aliasing -fPIC
460@GIMPACT_TRUE@libGIMPACT_a_SOURCES = \
461@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp \
462@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_contact.cpp \
463@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_math.cpp \
464@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_memory.cpp \
465@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp \
466@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh.cpp \
467@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp \
468@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp \
469@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp \
470@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp \
471@GIMPACT_TRUE@ @TOPDIR@/GIMPACT/src/gimpact.cpp
472
473@OPCODE_TRUE@libOPCODE_a_CPPFLAGS = -O2 -fno-strict-aliasing -fPIC
474@OPCODE_TRUE@libOPCODE_a_SOURCES = @TOPDIR@/OPCODE/OPC_AABBCollider.cpp \
475@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_AABBTree.cpp \
476@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_BaseModel.cpp \
477@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_BoxPruning.cpp \
478@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Collider.cpp \
479@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Common.cpp \
480@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_HybridModel.cpp \
481@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_LSSCollider.cpp \
482@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_MeshInterface.cpp \
483@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Model.cpp \
484@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_OBBCollider.cpp \
485@OPCODE_TRUE@ @TOPDIR@/OPCODE/Opcode.cpp \
486@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp \
487@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_Picking.cpp \
488@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp \
489@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_RayCollider.cpp \
490@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_SphereCollider.cpp \
491@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp \
492@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp \
493@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_TreeCollider.cpp \
494@OPCODE_TRUE@ @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp \
495@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceAABB.cpp \
496@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceContainer.cpp \
497@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceHPoint.cpp \
498@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp \
499@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp \
500@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp \
501@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceOBB.cpp \
502@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IcePlane.cpp \
503@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IcePoint.cpp \
504@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceRandom.cpp \
505@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceRay.cpp \
506@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp \
507@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceSegment.cpp \
508@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceTriangle.cpp \
509@OPCODE_TRUE@ @TOPDIR@/OPCODE/Ice/IceUtils.cpp
510
511all: all-am
512
513.SUFFIXES:
514.SUFFIXES: .c .cpp .o .obj
515$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps)
516 @for dep in $?; do \
517 case '$(am__configure_deps)' in \
518 *$$dep*) \
519 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh \
520 && exit 0; \
521 exit 1;; \
522 esac; \
523 done; \
524 echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/src/Makefile'; \
525 cd $(top_srcdir) && \
526 $(AUTOMAKE) --foreign ode/src/Makefile
527.PRECIOUS: Makefile
528Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status
529 @case '$?' in \
530 *config.status*) \
531 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \
532 *) \
533 echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \
534 cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \
535 esac;
536
537$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES)
538 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
539
540$(top_srcdir)/configure: $(am__configure_deps)
541 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
542$(ACLOCAL_M4): $(am__aclocal_m4_deps)
543 cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
544install-libLIBRARIES: $(lib_LIBRARIES)
545 @$(NORMAL_INSTALL)
546 test -z "$(libdir)" || $(MKDIR_P) "$(DESTDIR)$(libdir)"
547 @list='$(lib_LIBRARIES)'; for p in $$list; do \
548 if test -f $$p; then \
549 f=$(am__strip_dir) \
550 echo " $(libLIBRARIES_INSTALL) '$$p' '$(DESTDIR)$(libdir)/$$f'"; \
551 $(libLIBRARIES_INSTALL) "$$p" "$(DESTDIR)$(libdir)/$$f"; \
552 else :; fi; \
553 done
554 @$(POST_INSTALL)
555 @list='$(lib_LIBRARIES)'; for p in $$list; do \
556 if test -f $$p; then \
557 p=$(am__strip_dir) \
558 echo " $(RANLIB) '$(DESTDIR)$(libdir)/$$p'"; \
559 $(RANLIB) "$(DESTDIR)$(libdir)/$$p"; \
560 else :; fi; \
561 done
562
563uninstall-libLIBRARIES:
564 @$(NORMAL_UNINSTALL)
565 @list='$(lib_LIBRARIES)'; for p in $$list; do \
566 p=$(am__strip_dir) \
567 echo " rm -f '$(DESTDIR)$(libdir)/$$p'"; \
568 rm -f "$(DESTDIR)$(libdir)/$$p"; \
569 done
570
571clean-libLIBRARIES:
572 -test -z "$(lib_LIBRARIES)" || rm -f $(lib_LIBRARIES)
573
574clean-noinstLIBRARIES:
575 -test -z "$(noinst_LIBRARIES)" || rm -f $(noinst_LIBRARIES)
576libGIMPACT.a: $(libGIMPACT_a_OBJECTS) $(libGIMPACT_a_DEPENDENCIES)
577 -rm -f libGIMPACT.a
578 $(libGIMPACT_a_AR) libGIMPACT.a $(libGIMPACT_a_OBJECTS) $(libGIMPACT_a_LIBADD)
579 $(RANLIB) libGIMPACT.a
580libOPCODE.a: $(libOPCODE_a_OBJECTS) $(libOPCODE_a_DEPENDENCIES)
581 -rm -f libOPCODE.a
582 $(libOPCODE_a_AR) libOPCODE.a $(libOPCODE_a_OBJECTS) $(libOPCODE_a_LIBADD)
583 $(RANLIB) libOPCODE.a
584libfast.a: $(libfast_a_OBJECTS) $(libfast_a_DEPENDENCIES)
585 -rm -f libfast.a
586 $(libfast_a_AR) libfast.a $(libfast_a_OBJECTS) $(libfast_a_LIBADD)
587 $(RANLIB) libfast.a
588libode.a: $(libode_a_OBJECTS) $(libode_a_DEPENDENCIES)
589 -rm -f libode.a
590 $(libode_a_AR) libode.a $(libode_a_OBJECTS) $(libode_a_LIBADD)
591 $(RANLIB) libode.a
592install-traplibPROGRAMS: $(traplib_PROGRAMS)
593 @$(NORMAL_INSTALL)
594 test -z "$(traplibdir)" || $(MKDIR_P) "$(DESTDIR)$(traplibdir)"
595 @list='$(traplib_PROGRAMS)'; for p in $$list; do \
596 p1=`echo $$p|sed 's/$(EXEEXT)$$//'`; \
597 if test -f $$p \
598 ; then \
599 f=`echo "$$p1" | sed 's,^.*/,,;$(transform);s/$$/$(EXEEXT)/'`; \
600 echo " $(INSTALL_PROGRAM_ENV) $(traplibPROGRAMS_INSTALL) '$$p' '$(DESTDIR)$(traplibdir)/$$f'"; \
601 $(INSTALL_PROGRAM_ENV) $(traplibPROGRAMS_INSTALL) "$$p" "$(DESTDIR)$(traplibdir)/$$f" || exit 1; \
602 else :; fi; \
603 done
604
605uninstall-traplibPROGRAMS:
606 @$(NORMAL_UNINSTALL)
607 @list='$(traplib_PROGRAMS)'; for p in $$list; do \
608 f=`echo "$$p" | sed 's,^.*/,,;s/$(EXEEXT)$$//;$(transform);s/$$/$(EXEEXT)/'`; \
609 echo " rm -f '$(DESTDIR)$(traplibdir)/$$f'"; \
610 rm -f "$(DESTDIR)$(traplibdir)/$$f"; \
611 done
612
613clean-traplibPROGRAMS:
614 -test -z "$(traplib_PROGRAMS)" || rm -f $(traplib_PROGRAMS)
615libode$(EXEEXT): $(libode_OBJECTS) $(libode_DEPENDENCIES)
616 @rm -f libode$(EXEEXT)
617 $(libode_LINK) $(libode_OBJECTS) $(libode_LDADD) $(LIBS)
618
619mostlyclean-compile:
620 -rm -f *.$(OBJEXT)
621
622distclean-compile:
623 -rm -f *.tab.c
624
625@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_boxpruning.Po@am__quote@
626@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_contact.Po@am__quote@
627@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_math.Po@am__quote@
628@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_memory.Po@am__quote@
629@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Po@am__quote@
630@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh.Po@am__quote@
631@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Po@am__quote@
632@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Po@am__quote@
633@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Po@am__quote@
634@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Po@am__quote@
635@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libGIMPACT_a-gimpact.Po@am__quote@
636@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceAABB.Po@am__quote@
637@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceContainer.Po@am__quote@
638@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceHPoint.Po@am__quote@
639@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Po@am__quote@
640@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceMatrix3x3.Po@am__quote@
641@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceMatrix4x4.Po@am__quote@
642@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceOBB.Po@am__quote@
643@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IcePlane.Po@am__quote@
644@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IcePoint.Po@am__quote@
645@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceRandom.Po@am__quote@
646@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceRay.Po@am__quote@
647@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Po@am__quote@
648@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceSegment.Po@am__quote@
649@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceTriangle.Po@am__quote@
650@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-IceUtils.Po@am__quote@
651@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Po@am__quote@
652@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_AABBTree.Po@am__quote@
653@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_BaseModel.Po@am__quote@
654@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Po@am__quote@
655@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Collider.Po@am__quote@
656@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Common.Po@am__quote@
657@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_HybridModel.Po@am__quote@
658@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Po@am__quote@
659@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Po@am__quote@
660@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Model.Po@am__quote@
661@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Po@am__quote@
662@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Po@am__quote@
663@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_Picking.Po@am__quote@
664@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Po@am__quote@
665@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_RayCollider.Po@am__quote@
666@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Po@am__quote@
667@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Po@am__quote@
668@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Po@am__quote@
669@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Po@am__quote@
670@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Po@am__quote@
671@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libOPCODE_a-Opcode.Po@am__quote@
672@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastdot.Po@am__quote@
673@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastldlt.Po@am__quote@
674@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastlsolve.Po@am__quote@
675@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libfast_a-fastltsolve.Po@am__quote@
676@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-array.Po@am__quote@
677@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-box.Po@am__quote@
678@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-capsule.Po@am__quote@
679@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_box.Po@am__quote@
680@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_plane.Po@am__quote@
681@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_sphere.Po@am__quote@
682@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_cylinder_trimesh.Po@am__quote@
683@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_kernel.Po@am__quote@
684@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_quadtreespace.Po@am__quote@
685@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_space.Po@am__quote@
686@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_transform.Po@am__quote@
687@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_box.Po@am__quote@
688@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_ccylinder.Po@am__quote@
689@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_distance.Po@am__quote@
690@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_gimpact.Po@am__quote@
691@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_opcode.Po@am__quote@
692@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_plane.Po@am__quote@
693@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_ray.Po@am__quote@
694@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_sphere.Po@am__quote@
695@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_trimesh_trimesh.Po@am__quote@
696@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-collision_util.Po@am__quote@
697@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-convex.Po@am__quote@
698@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-cylinder.Po@am__quote@
699@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-error.Po@am__quote@
700@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-export-dif.Po@am__quote@
701@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-heightfield.Po@am__quote@
702@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-joint.Po@am__quote@
703@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-lcp.Po@am__quote@
704@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-mass.Po@am__quote@
705@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-mat.Po@am__quote@
706@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-matrix.Po@am__quote@
707@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-memory.Po@am__quote@
708@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-misc.Po@am__quote@
709@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-obstack.Po@am__quote@
710@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-ode.Po@am__quote@
711@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-odemath.Po@am__quote@
712@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-plane.Po@am__quote@
713@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-quickstep.Po@am__quote@
714@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-ray.Po@am__quote@
715@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-rotation.Po@am__quote@
716@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-sphere.Po@am__quote@
717@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-step.Po@am__quote@
718@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-stepfast.Po@am__quote@
719@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-testing.Po@am__quote@
720@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-timer.Po@am__quote@
721@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/libode_a-util.Po@am__quote@
722
723.c.o:
724@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
725@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
726@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
727@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
728@am__fastdepCC_FALSE@ $(COMPILE) -c $<
729
730.c.obj:
731@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'`
732@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
733@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
734@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
735@am__fastdepCC_FALSE@ $(COMPILE) -c `$(CYGPATH_W) '$<'`
736
737libfast_a-fastldlt.o: fastldlt.c
738@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
739@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastldlt.Tpo $(DEPDIR)/libfast_a-fastldlt.Po
740@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastldlt.c' object='libfast_a-fastldlt.o' libtool=no @AMDEPBACKSLASH@
741@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
742@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
743
744libfast_a-fastldlt.obj: fastldlt.c
745@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`
746@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastldlt.Tpo $(DEPDIR)/libfast_a-fastldlt.Po
747@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastldlt.c' object='libfast_a-fastldlt.obj' libtool=no @AMDEPBACKSLASH@
748@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
749@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`
750
751libfast_a-fastltsolve.o: fastltsolve.c
752@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
753@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastltsolve.Tpo $(DEPDIR)/libfast_a-fastltsolve.Po
754@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastltsolve.c' object='libfast_a-fastltsolve.o' libtool=no @AMDEPBACKSLASH@
755@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
756@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
757
758libfast_a-fastltsolve.obj: fastltsolve.c
759@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`
760@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastltsolve.Tpo $(DEPDIR)/libfast_a-fastltsolve.Po
761@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastltsolve.c' object='libfast_a-fastltsolve.obj' libtool=no @AMDEPBACKSLASH@
762@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
763@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`
764
765libfast_a-fastdot.o: fastdot.c
766@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
767@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastdot.Tpo $(DEPDIR)/libfast_a-fastdot.Po
768@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastdot.c' object='libfast_a-fastdot.o' libtool=no @AMDEPBACKSLASH@
769@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
770@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
771
772libfast_a-fastdot.obj: fastdot.c
773@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`
774@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastdot.Tpo $(DEPDIR)/libfast_a-fastdot.Po
775@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastdot.c' object='libfast_a-fastdot.obj' libtool=no @AMDEPBACKSLASH@
776@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
777@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`
778
779libfast_a-fastlsolve.o: fastlsolve.c
780@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
781@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastlsolve.Tpo $(DEPDIR)/libfast_a-fastlsolve.Po
782@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastlsolve.c' object='libfast_a-fastlsolve.o' libtool=no @AMDEPBACKSLASH@
783@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
784@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
785
786libfast_a-fastlsolve.obj: fastlsolve.c
787@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`
788@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/libfast_a-fastlsolve.Tpo $(DEPDIR)/libfast_a-fastlsolve.Po
789@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='fastlsolve.c' object='libfast_a-fastlsolve.obj' libtool=no @AMDEPBACKSLASH@
790@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@
791@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`
792
793.cpp.o:
794@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
795@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
796@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
797@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
798@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ $<
799
800.cpp.obj:
801@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'`
802@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
803@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
804@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
805@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'`
806
807libGIMPACT_a-gim_boxpruning.o: @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp
808@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
809@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Tpo $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Po
810@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp' object='libGIMPACT_a-gim_boxpruning.o' libtool=no @AMDEPBACKSLASH@
811@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
812@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
813
814libGIMPACT_a-gim_boxpruning.obj: @TOPDIR@/GIMPACT/src/gim_boxpruning.cpp
815@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`
816@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Tpo $(DEPDIR)/libGIMPACT_a-gim_boxpruning.Po
817@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_boxpruning.cpp' object='libGIMPACT_a-gim_boxpruning.obj' libtool=no @AMDEPBACKSLASH@
818@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
819@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`
820
821libGIMPACT_a-gim_contact.o: @TOPDIR@/GIMPACT/src/gim_contact.cpp
822@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
823@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_contact.Tpo $(DEPDIR)/libGIMPACT_a-gim_contact.Po
824@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_contact.cpp' object='libGIMPACT_a-gim_contact.o' libtool=no @AMDEPBACKSLASH@
825@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
826@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
827
828libGIMPACT_a-gim_contact.obj: @TOPDIR@/GIMPACT/src/gim_contact.cpp
829@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`
830@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_contact.Tpo $(DEPDIR)/libGIMPACT_a-gim_contact.Po
831@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_contact.cpp' object='libGIMPACT_a-gim_contact.obj' libtool=no @AMDEPBACKSLASH@
832@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
833@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`
834
835libGIMPACT_a-gim_math.o: @TOPDIR@/GIMPACT/src/gim_math.cpp
836@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
837@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_math.Tpo $(DEPDIR)/libGIMPACT_a-gim_math.Po
838@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_math.cpp' object='libGIMPACT_a-gim_math.o' libtool=no @AMDEPBACKSLASH@
839@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
840@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
841
842libGIMPACT_a-gim_math.obj: @TOPDIR@/GIMPACT/src/gim_math.cpp
843@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`
844@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_math.Tpo $(DEPDIR)/libGIMPACT_a-gim_math.Po
845@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_math.cpp' object='libGIMPACT_a-gim_math.obj' libtool=no @AMDEPBACKSLASH@
846@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
847@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`
848
849libGIMPACT_a-gim_memory.o: @TOPDIR@/GIMPACT/src/gim_memory.cpp
850@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
851@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_memory.Tpo $(DEPDIR)/libGIMPACT_a-gim_memory.Po
852@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_memory.cpp' object='libGIMPACT_a-gim_memory.o' libtool=no @AMDEPBACKSLASH@
853@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
854@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
855
856libGIMPACT_a-gim_memory.obj: @TOPDIR@/GIMPACT/src/gim_memory.cpp
857@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`
858@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_memory.Tpo $(DEPDIR)/libGIMPACT_a-gim_memory.Po
859@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_memory.cpp' object='libGIMPACT_a-gim_memory.obj' libtool=no @AMDEPBACKSLASH@
860@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
861@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`
862
863libGIMPACT_a-gim_tri_tri_overlap.o: @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp
864@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
865@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Tpo $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Po
866@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@
867@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
868@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
869
870libGIMPACT_a-gim_tri_tri_overlap.obj: @TOPDIR@/GIMPACT/src/gim_tri_tri_overlap.cpp
871@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`
872@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Tpo $(DEPDIR)/libGIMPACT_a-gim_tri_tri_overlap.Po
873@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@
874@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
875@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`
876
877libGIMPACT_a-gim_trimesh.o: @TOPDIR@/GIMPACT/src/gim_trimesh.cpp
878@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
879@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh.Po
880@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh.cpp' object='libGIMPACT_a-gim_trimesh.o' libtool=no @AMDEPBACKSLASH@
881@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
882@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
883
884libGIMPACT_a-gim_trimesh.obj: @TOPDIR@/GIMPACT/src/gim_trimesh.cpp
885@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`
886@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh.Po
887@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gim_trimesh.cpp' object='libGIMPACT_a-gim_trimesh.obj' libtool=no @AMDEPBACKSLASH@
888@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
889@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`
890
891libGIMPACT_a-gim_trimesh_capsule_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp
892@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
893@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Po
894@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@
895@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
896@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
897
898libGIMPACT_a-gim_trimesh_capsule_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_capsule_collision.cpp
899@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`
900@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_capsule_collision.Po
901@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@
902@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
903@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`
904
905libGIMPACT_a-gim_trimesh_ray_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp
906@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
907@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Po
908@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@
909@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
910@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
911
912libGIMPACT_a-gim_trimesh_ray_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_ray_collision.cpp
913@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`
914@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_ray_collision.Po
915@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@
916@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
917@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`
918
919libGIMPACT_a-gim_trimesh_sphere_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp
920@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
921@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Po
922@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@
923@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
924@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
925
926libGIMPACT_a-gim_trimesh_sphere_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_sphere_collision.cpp
927@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`
928@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_sphere_collision.Po
929@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@
930@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
931@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`
932
933libGIMPACT_a-gim_trimesh_trimesh_collision.o: @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp
934@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
935@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Po
936@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@
937@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
938@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
939
940libGIMPACT_a-gim_trimesh_trimesh_collision.obj: @TOPDIR@/GIMPACT/src/gim_trimesh_trimesh_collision.cpp
941@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`
942@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Tpo $(DEPDIR)/libGIMPACT_a-gim_trimesh_trimesh_collision.Po
943@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@
944@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
945@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`
946
947libGIMPACT_a-gimpact.o: @TOPDIR@/GIMPACT/src/gimpact.cpp
948@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
949@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gimpact.Tpo $(DEPDIR)/libGIMPACT_a-gimpact.Po
950@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gimpact.cpp' object='libGIMPACT_a-gimpact.o' libtool=no @AMDEPBACKSLASH@
951@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
952@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
953
954libGIMPACT_a-gimpact.obj: @TOPDIR@/GIMPACT/src/gimpact.cpp
955@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`
956@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libGIMPACT_a-gimpact.Tpo $(DEPDIR)/libGIMPACT_a-gimpact.Po
957@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/GIMPACT/src/gimpact.cpp' object='libGIMPACT_a-gimpact.obj' libtool=no @AMDEPBACKSLASH@
958@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
959@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`
960
961libOPCODE_a-OPC_AABBCollider.o: @TOPDIR@/OPCODE/OPC_AABBCollider.cpp
962@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
963@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Po
964@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBCollider.cpp' object='libOPCODE_a-OPC_AABBCollider.o' libtool=no @AMDEPBACKSLASH@
965@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
966@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
967
968libOPCODE_a-OPC_AABBCollider.obj: @TOPDIR@/OPCODE/OPC_AABBCollider.cpp
969@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`
970@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBCollider.Po
971@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBCollider.cpp' object='libOPCODE_a-OPC_AABBCollider.obj' libtool=no @AMDEPBACKSLASH@
972@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
973@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`
974
975libOPCODE_a-OPC_AABBTree.o: @TOPDIR@/OPCODE/OPC_AABBTree.cpp
976@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
977@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Po
978@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBTree.cpp' object='libOPCODE_a-OPC_AABBTree.o' libtool=no @AMDEPBACKSLASH@
979@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
980@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
981
982libOPCODE_a-OPC_AABBTree.obj: @TOPDIR@/OPCODE/OPC_AABBTree.cpp
983@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`
984@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_AABBTree.Po
985@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_AABBTree.cpp' object='libOPCODE_a-OPC_AABBTree.obj' libtool=no @AMDEPBACKSLASH@
986@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
987@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`
988
989libOPCODE_a-OPC_BaseModel.o: @TOPDIR@/OPCODE/OPC_BaseModel.cpp
990@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
991@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Po
992@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BaseModel.cpp' object='libOPCODE_a-OPC_BaseModel.o' libtool=no @AMDEPBACKSLASH@
993@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
994@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
995
996libOPCODE_a-OPC_BaseModel.obj: @TOPDIR@/OPCODE/OPC_BaseModel.cpp
997@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`
998@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_BaseModel.Po
999@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BaseModel.cpp' object='libOPCODE_a-OPC_BaseModel.obj' libtool=no @AMDEPBACKSLASH@
1000@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1001@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`
1002
1003libOPCODE_a-OPC_BoxPruning.o: @TOPDIR@/OPCODE/OPC_BoxPruning.cpp
1004@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
1005@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Tpo $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Po
1006@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BoxPruning.cpp' object='libOPCODE_a-OPC_BoxPruning.o' libtool=no @AMDEPBACKSLASH@
1007@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1008@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
1009
1010libOPCODE_a-OPC_BoxPruning.obj: @TOPDIR@/OPCODE/OPC_BoxPruning.cpp
1011@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`
1012@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Tpo $(DEPDIR)/libOPCODE_a-OPC_BoxPruning.Po
1013@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_BoxPruning.cpp' object='libOPCODE_a-OPC_BoxPruning.obj' libtool=no @AMDEPBACKSLASH@
1014@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1015@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`
1016
1017libOPCODE_a-OPC_Collider.o: @TOPDIR@/OPCODE/OPC_Collider.cpp
1018@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
1019@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Collider.Tpo $(DEPDIR)/libOPCODE_a-OPC_Collider.Po
1020@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Collider.cpp' object='libOPCODE_a-OPC_Collider.o' libtool=no @AMDEPBACKSLASH@
1021@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1022@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
1023
1024libOPCODE_a-OPC_Collider.obj: @TOPDIR@/OPCODE/OPC_Collider.cpp
1025@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`
1026@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Collider.Tpo $(DEPDIR)/libOPCODE_a-OPC_Collider.Po
1027@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Collider.cpp' object='libOPCODE_a-OPC_Collider.obj' libtool=no @AMDEPBACKSLASH@
1028@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1029@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`
1030
1031libOPCODE_a-OPC_Common.o: @TOPDIR@/OPCODE/OPC_Common.cpp
1032@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
1033@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Common.Tpo $(DEPDIR)/libOPCODE_a-OPC_Common.Po
1034@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Common.cpp' object='libOPCODE_a-OPC_Common.o' libtool=no @AMDEPBACKSLASH@
1035@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1036@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
1037
1038libOPCODE_a-OPC_Common.obj: @TOPDIR@/OPCODE/OPC_Common.cpp
1039@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`
1040@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Common.Tpo $(DEPDIR)/libOPCODE_a-OPC_Common.Po
1041@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Common.cpp' object='libOPCODE_a-OPC_Common.obj' libtool=no @AMDEPBACKSLASH@
1042@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1043@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`
1044
1045libOPCODE_a-OPC_HybridModel.o: @TOPDIR@/OPCODE/OPC_HybridModel.cpp
1046@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
1047@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Po
1048@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_HybridModel.cpp' object='libOPCODE_a-OPC_HybridModel.o' libtool=no @AMDEPBACKSLASH@
1049@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1050@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
1051
1052libOPCODE_a-OPC_HybridModel.obj: @TOPDIR@/OPCODE/OPC_HybridModel.cpp
1053@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`
1054@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Tpo $(DEPDIR)/libOPCODE_a-OPC_HybridModel.Po
1055@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_HybridModel.cpp' object='libOPCODE_a-OPC_HybridModel.obj' libtool=no @AMDEPBACKSLASH@
1056@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1057@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`
1058
1059libOPCODE_a-OPC_LSSCollider.o: @TOPDIR@/OPCODE/OPC_LSSCollider.cpp
1060@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
1061@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Po
1062@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_LSSCollider.cpp' object='libOPCODE_a-OPC_LSSCollider.o' libtool=no @AMDEPBACKSLASH@
1063@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1064@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
1065
1066libOPCODE_a-OPC_LSSCollider.obj: @TOPDIR@/OPCODE/OPC_LSSCollider.cpp
1067@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`
1068@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_LSSCollider.Po
1069@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_LSSCollider.cpp' object='libOPCODE_a-OPC_LSSCollider.obj' libtool=no @AMDEPBACKSLASH@
1070@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1071@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`
1072
1073libOPCODE_a-OPC_MeshInterface.o: @TOPDIR@/OPCODE/OPC_MeshInterface.cpp
1074@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
1075@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Tpo $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Po
1076@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_MeshInterface.cpp' object='libOPCODE_a-OPC_MeshInterface.o' libtool=no @AMDEPBACKSLASH@
1077@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1078@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
1079
1080libOPCODE_a-OPC_MeshInterface.obj: @TOPDIR@/OPCODE/OPC_MeshInterface.cpp
1081@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`
1082@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Tpo $(DEPDIR)/libOPCODE_a-OPC_MeshInterface.Po
1083@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_MeshInterface.cpp' object='libOPCODE_a-OPC_MeshInterface.obj' libtool=no @AMDEPBACKSLASH@
1084@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1085@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`
1086
1087libOPCODE_a-OPC_Model.o: @TOPDIR@/OPCODE/OPC_Model.cpp
1088@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
1089@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Model.Tpo $(DEPDIR)/libOPCODE_a-OPC_Model.Po
1090@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Model.cpp' object='libOPCODE_a-OPC_Model.o' libtool=no @AMDEPBACKSLASH@
1091@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1092@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
1093
1094libOPCODE_a-OPC_Model.obj: @TOPDIR@/OPCODE/OPC_Model.cpp
1095@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`
1096@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Model.Tpo $(DEPDIR)/libOPCODE_a-OPC_Model.Po
1097@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Model.cpp' object='libOPCODE_a-OPC_Model.obj' libtool=no @AMDEPBACKSLASH@
1098@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1099@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`
1100
1101libOPCODE_a-OPC_OBBCollider.o: @TOPDIR@/OPCODE/OPC_OBBCollider.cpp
1102@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
1103@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Po
1104@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OBBCollider.cpp' object='libOPCODE_a-OPC_OBBCollider.o' libtool=no @AMDEPBACKSLASH@
1105@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1106@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
1107
1108libOPCODE_a-OPC_OBBCollider.obj: @TOPDIR@/OPCODE/OPC_OBBCollider.cpp
1109@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`
1110@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_OBBCollider.Po
1111@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OBBCollider.cpp' object='libOPCODE_a-OPC_OBBCollider.obj' libtool=no @AMDEPBACKSLASH@
1112@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1113@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`
1114
1115libOPCODE_a-Opcode.o: @TOPDIR@/OPCODE/Opcode.cpp
1116@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
1117@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-Opcode.Tpo $(DEPDIR)/libOPCODE_a-Opcode.Po
1118@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Opcode.cpp' object='libOPCODE_a-Opcode.o' libtool=no @AMDEPBACKSLASH@
1119@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1120@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
1121
1122libOPCODE_a-Opcode.obj: @TOPDIR@/OPCODE/Opcode.cpp
1123@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`
1124@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-Opcode.Tpo $(DEPDIR)/libOPCODE_a-Opcode.Po
1125@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Opcode.cpp' object='libOPCODE_a-Opcode.obj' libtool=no @AMDEPBACKSLASH@
1126@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1127@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`
1128
1129libOPCODE_a-OPC_OptimizedTree.o: @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp
1130@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
1131@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Po
1132@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp' object='libOPCODE_a-OPC_OptimizedTree.o' libtool=no @AMDEPBACKSLASH@
1133@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1134@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
1135
1136libOPCODE_a-OPC_OptimizedTree.obj: @TOPDIR@/OPCODE/OPC_OptimizedTree.cpp
1137@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`
1138@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Tpo $(DEPDIR)/libOPCODE_a-OPC_OptimizedTree.Po
1139@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_OptimizedTree.cpp' object='libOPCODE_a-OPC_OptimizedTree.obj' libtool=no @AMDEPBACKSLASH@
1140@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1141@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`
1142
1143libOPCODE_a-OPC_Picking.o: @TOPDIR@/OPCODE/OPC_Picking.cpp
1144@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
1145@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Picking.Tpo $(DEPDIR)/libOPCODE_a-OPC_Picking.Po
1146@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Picking.cpp' object='libOPCODE_a-OPC_Picking.o' libtool=no @AMDEPBACKSLASH@
1147@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1148@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
1149
1150libOPCODE_a-OPC_Picking.obj: @TOPDIR@/OPCODE/OPC_Picking.cpp
1151@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`
1152@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_Picking.Tpo $(DEPDIR)/libOPCODE_a-OPC_Picking.Po
1153@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_Picking.cpp' object='libOPCODE_a-OPC_Picking.obj' libtool=no @AMDEPBACKSLASH@
1154@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1155@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`
1156
1157libOPCODE_a-OPC_PlanesCollider.o: @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp
1158@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
1159@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Po
1160@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp' object='libOPCODE_a-OPC_PlanesCollider.o' libtool=no @AMDEPBACKSLASH@
1161@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1162@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
1163
1164libOPCODE_a-OPC_PlanesCollider.obj: @TOPDIR@/OPCODE/OPC_PlanesCollider.cpp
1165@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`
1166@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_PlanesCollider.Po
1167@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_PlanesCollider.cpp' object='libOPCODE_a-OPC_PlanesCollider.obj' libtool=no @AMDEPBACKSLASH@
1168@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1169@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`
1170
1171libOPCODE_a-OPC_RayCollider.o: @TOPDIR@/OPCODE/OPC_RayCollider.cpp
1172@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
1173@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Po
1174@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_RayCollider.cpp' object='libOPCODE_a-OPC_RayCollider.o' libtool=no @AMDEPBACKSLASH@
1175@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1176@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
1177
1178libOPCODE_a-OPC_RayCollider.obj: @TOPDIR@/OPCODE/OPC_RayCollider.cpp
1179@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`
1180@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_RayCollider.Po
1181@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_RayCollider.cpp' object='libOPCODE_a-OPC_RayCollider.obj' libtool=no @AMDEPBACKSLASH@
1182@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1183@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`
1184
1185libOPCODE_a-OPC_SphereCollider.o: @TOPDIR@/OPCODE/OPC_SphereCollider.cpp
1186@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
1187@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Po
1188@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SphereCollider.cpp' object='libOPCODE_a-OPC_SphereCollider.o' libtool=no @AMDEPBACKSLASH@
1189@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1190@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
1191
1192libOPCODE_a-OPC_SphereCollider.obj: @TOPDIR@/OPCODE/OPC_SphereCollider.cpp
1193@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`
1194@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_SphereCollider.Po
1195@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SphereCollider.cpp' object='libOPCODE_a-OPC_SphereCollider.obj' libtool=no @AMDEPBACKSLASH@
1196@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1197@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`
1198
1199libOPCODE_a-OPC_SweepAndPrune.o: @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp
1200@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
1201@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Tpo $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Po
1202@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp' object='libOPCODE_a-OPC_SweepAndPrune.o' libtool=no @AMDEPBACKSLASH@
1203@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1204@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
1205
1206libOPCODE_a-OPC_SweepAndPrune.obj: @TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp
1207@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`
1208@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Tpo $(DEPDIR)/libOPCODE_a-OPC_SweepAndPrune.Po
1209@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_SweepAndPrune.cpp' object='libOPCODE_a-OPC_SweepAndPrune.obj' libtool=no @AMDEPBACKSLASH@
1210@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1211@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`
1212
1213libOPCODE_a-OPC_TreeBuilders.o: @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp
1214@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
1215@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Po
1216@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp' object='libOPCODE_a-OPC_TreeBuilders.o' libtool=no @AMDEPBACKSLASH@
1217@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1218@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
1219
1220libOPCODE_a-OPC_TreeBuilders.obj: @TOPDIR@/OPCODE/OPC_TreeBuilders.cpp
1221@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`
1222@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeBuilders.Po
1223@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeBuilders.cpp' object='libOPCODE_a-OPC_TreeBuilders.obj' libtool=no @AMDEPBACKSLASH@
1224@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1225@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`
1226
1227libOPCODE_a-OPC_TreeCollider.o: @TOPDIR@/OPCODE/OPC_TreeCollider.cpp
1228@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
1229@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Po
1230@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeCollider.cpp' object='libOPCODE_a-OPC_TreeCollider.o' libtool=no @AMDEPBACKSLASH@
1231@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1232@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
1233
1234libOPCODE_a-OPC_TreeCollider.obj: @TOPDIR@/OPCODE/OPC_TreeCollider.cpp
1235@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`
1236@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_TreeCollider.Po
1237@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_TreeCollider.cpp' object='libOPCODE_a-OPC_TreeCollider.obj' libtool=no @AMDEPBACKSLASH@
1238@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1239@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`
1240
1241libOPCODE_a-OPC_VolumeCollider.o: @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp
1242@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
1243@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Po
1244@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp' object='libOPCODE_a-OPC_VolumeCollider.o' libtool=no @AMDEPBACKSLASH@
1245@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1246@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
1247
1248libOPCODE_a-OPC_VolumeCollider.obj: @TOPDIR@/OPCODE/OPC_VolumeCollider.cpp
1249@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`
1250@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Tpo $(DEPDIR)/libOPCODE_a-OPC_VolumeCollider.Po
1251@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/OPC_VolumeCollider.cpp' object='libOPCODE_a-OPC_VolumeCollider.obj' libtool=no @AMDEPBACKSLASH@
1252@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1253@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`
1254
1255libOPCODE_a-IceAABB.o: @TOPDIR@/OPCODE/Ice/IceAABB.cpp
1256@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
1257@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceAABB.Tpo $(DEPDIR)/libOPCODE_a-IceAABB.Po
1258@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceAABB.cpp' object='libOPCODE_a-IceAABB.o' libtool=no @AMDEPBACKSLASH@
1259@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1260@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
1261
1262libOPCODE_a-IceAABB.obj: @TOPDIR@/OPCODE/Ice/IceAABB.cpp
1263@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`
1264@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceAABB.Tpo $(DEPDIR)/libOPCODE_a-IceAABB.Po
1265@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceAABB.cpp' object='libOPCODE_a-IceAABB.obj' libtool=no @AMDEPBACKSLASH@
1266@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1267@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`
1268
1269libOPCODE_a-IceContainer.o: @TOPDIR@/OPCODE/Ice/IceContainer.cpp
1270@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
1271@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceContainer.Tpo $(DEPDIR)/libOPCODE_a-IceContainer.Po
1272@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceContainer.cpp' object='libOPCODE_a-IceContainer.o' libtool=no @AMDEPBACKSLASH@
1273@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1274@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
1275
1276libOPCODE_a-IceContainer.obj: @TOPDIR@/OPCODE/Ice/IceContainer.cpp
1277@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`
1278@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceContainer.Tpo $(DEPDIR)/libOPCODE_a-IceContainer.Po
1279@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceContainer.cpp' object='libOPCODE_a-IceContainer.obj' libtool=no @AMDEPBACKSLASH@
1280@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1281@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`
1282
1283libOPCODE_a-IceHPoint.o: @TOPDIR@/OPCODE/Ice/IceHPoint.cpp
1284@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
1285@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceHPoint.Tpo $(DEPDIR)/libOPCODE_a-IceHPoint.Po
1286@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceHPoint.cpp' object='libOPCODE_a-IceHPoint.o' libtool=no @AMDEPBACKSLASH@
1287@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1288@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
1289
1290libOPCODE_a-IceHPoint.obj: @TOPDIR@/OPCODE/Ice/IceHPoint.cpp
1291@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`
1292@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceHPoint.Tpo $(DEPDIR)/libOPCODE_a-IceHPoint.Po
1293@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceHPoint.cpp' object='libOPCODE_a-IceHPoint.obj' libtool=no @AMDEPBACKSLASH@
1294@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1295@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`
1296
1297libOPCODE_a-IceIndexedTriangle.o: @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp
1298@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
1299@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Po
1300@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp' object='libOPCODE_a-IceIndexedTriangle.o' libtool=no @AMDEPBACKSLASH@
1301@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1302@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
1303
1304libOPCODE_a-IceIndexedTriangle.obj: @TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp
1305@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`
1306@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceIndexedTriangle.Po
1307@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceIndexedTriangle.cpp' object='libOPCODE_a-IceIndexedTriangle.obj' libtool=no @AMDEPBACKSLASH@
1308@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1309@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`
1310
1311libOPCODE_a-IceMatrix3x3.o: @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp
1312@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
1313@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Po
1314@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp' object='libOPCODE_a-IceMatrix3x3.o' libtool=no @AMDEPBACKSLASH@
1315@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1316@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
1317
1318libOPCODE_a-IceMatrix3x3.obj: @TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp
1319@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`
1320@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix3x3.Po
1321@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix3x3.cpp' object='libOPCODE_a-IceMatrix3x3.obj' libtool=no @AMDEPBACKSLASH@
1322@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1323@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`
1324
1325libOPCODE_a-IceMatrix4x4.o: @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp
1326@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
1327@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Po
1328@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp' object='libOPCODE_a-IceMatrix4x4.o' libtool=no @AMDEPBACKSLASH@
1329@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1330@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
1331
1332libOPCODE_a-IceMatrix4x4.obj: @TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp
1333@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`
1334@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Tpo $(DEPDIR)/libOPCODE_a-IceMatrix4x4.Po
1335@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceMatrix4x4.cpp' object='libOPCODE_a-IceMatrix4x4.obj' libtool=no @AMDEPBACKSLASH@
1336@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1337@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`
1338
1339libOPCODE_a-IceOBB.o: @TOPDIR@/OPCODE/Ice/IceOBB.cpp
1340@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
1341@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceOBB.Tpo $(DEPDIR)/libOPCODE_a-IceOBB.Po
1342@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceOBB.cpp' object='libOPCODE_a-IceOBB.o' libtool=no @AMDEPBACKSLASH@
1343@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1344@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
1345
1346libOPCODE_a-IceOBB.obj: @TOPDIR@/OPCODE/Ice/IceOBB.cpp
1347@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`
1348@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceOBB.Tpo $(DEPDIR)/libOPCODE_a-IceOBB.Po
1349@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceOBB.cpp' object='libOPCODE_a-IceOBB.obj' libtool=no @AMDEPBACKSLASH@
1350@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1351@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`
1352
1353libOPCODE_a-IcePlane.o: @TOPDIR@/OPCODE/Ice/IcePlane.cpp
1354@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
1355@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePlane.Tpo $(DEPDIR)/libOPCODE_a-IcePlane.Po
1356@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePlane.cpp' object='libOPCODE_a-IcePlane.o' libtool=no @AMDEPBACKSLASH@
1357@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1358@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
1359
1360libOPCODE_a-IcePlane.obj: @TOPDIR@/OPCODE/Ice/IcePlane.cpp
1361@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`
1362@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePlane.Tpo $(DEPDIR)/libOPCODE_a-IcePlane.Po
1363@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePlane.cpp' object='libOPCODE_a-IcePlane.obj' libtool=no @AMDEPBACKSLASH@
1364@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1365@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`
1366
1367libOPCODE_a-IcePoint.o: @TOPDIR@/OPCODE/Ice/IcePoint.cpp
1368@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
1369@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePoint.Tpo $(DEPDIR)/libOPCODE_a-IcePoint.Po
1370@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePoint.cpp' object='libOPCODE_a-IcePoint.o' libtool=no @AMDEPBACKSLASH@
1371@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1372@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
1373
1374libOPCODE_a-IcePoint.obj: @TOPDIR@/OPCODE/Ice/IcePoint.cpp
1375@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`
1376@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IcePoint.Tpo $(DEPDIR)/libOPCODE_a-IcePoint.Po
1377@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IcePoint.cpp' object='libOPCODE_a-IcePoint.obj' libtool=no @AMDEPBACKSLASH@
1378@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1379@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`
1380
1381libOPCODE_a-IceRandom.o: @TOPDIR@/OPCODE/Ice/IceRandom.cpp
1382@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
1383@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRandom.Tpo $(DEPDIR)/libOPCODE_a-IceRandom.Po
1384@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRandom.cpp' object='libOPCODE_a-IceRandom.o' libtool=no @AMDEPBACKSLASH@
1385@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1386@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
1387
1388libOPCODE_a-IceRandom.obj: @TOPDIR@/OPCODE/Ice/IceRandom.cpp
1389@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`
1390@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRandom.Tpo $(DEPDIR)/libOPCODE_a-IceRandom.Po
1391@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRandom.cpp' object='libOPCODE_a-IceRandom.obj' libtool=no @AMDEPBACKSLASH@
1392@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1393@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`
1394
1395libOPCODE_a-IceRay.o: @TOPDIR@/OPCODE/Ice/IceRay.cpp
1396@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
1397@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRay.Tpo $(DEPDIR)/libOPCODE_a-IceRay.Po
1398@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRay.cpp' object='libOPCODE_a-IceRay.o' libtool=no @AMDEPBACKSLASH@
1399@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1400@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
1401
1402libOPCODE_a-IceRay.obj: @TOPDIR@/OPCODE/Ice/IceRay.cpp
1403@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`
1404@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRay.Tpo $(DEPDIR)/libOPCODE_a-IceRay.Po
1405@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRay.cpp' object='libOPCODE_a-IceRay.obj' libtool=no @AMDEPBACKSLASH@
1406@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1407@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`
1408
1409libOPCODE_a-IceRevisitedRadix.o: @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp
1410@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
1411@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Tpo $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Po
1412@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp' object='libOPCODE_a-IceRevisitedRadix.o' libtool=no @AMDEPBACKSLASH@
1413@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1414@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
1415
1416libOPCODE_a-IceRevisitedRadix.obj: @TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp
1417@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`
1418@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Tpo $(DEPDIR)/libOPCODE_a-IceRevisitedRadix.Po
1419@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceRevisitedRadix.cpp' object='libOPCODE_a-IceRevisitedRadix.obj' libtool=no @AMDEPBACKSLASH@
1420@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1421@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`
1422
1423libOPCODE_a-IceSegment.o: @TOPDIR@/OPCODE/Ice/IceSegment.cpp
1424@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
1425@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceSegment.Tpo $(DEPDIR)/libOPCODE_a-IceSegment.Po
1426@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceSegment.cpp' object='libOPCODE_a-IceSegment.o' libtool=no @AMDEPBACKSLASH@
1427@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1428@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
1429
1430libOPCODE_a-IceSegment.obj: @TOPDIR@/OPCODE/Ice/IceSegment.cpp
1431@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`
1432@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceSegment.Tpo $(DEPDIR)/libOPCODE_a-IceSegment.Po
1433@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceSegment.cpp' object='libOPCODE_a-IceSegment.obj' libtool=no @AMDEPBACKSLASH@
1434@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1435@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`
1436
1437libOPCODE_a-IceTriangle.o: @TOPDIR@/OPCODE/Ice/IceTriangle.cpp
1438@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
1439@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceTriangle.Po
1440@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceTriangle.cpp' object='libOPCODE_a-IceTriangle.o' libtool=no @AMDEPBACKSLASH@
1441@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1442@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
1443
1444libOPCODE_a-IceTriangle.obj: @TOPDIR@/OPCODE/Ice/IceTriangle.cpp
1445@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`
1446@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceTriangle.Tpo $(DEPDIR)/libOPCODE_a-IceTriangle.Po
1447@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceTriangle.cpp' object='libOPCODE_a-IceTriangle.obj' libtool=no @AMDEPBACKSLASH@
1448@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1449@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`
1450
1451libOPCODE_a-IceUtils.o: @TOPDIR@/OPCODE/Ice/IceUtils.cpp
1452@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
1453@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceUtils.Tpo $(DEPDIR)/libOPCODE_a-IceUtils.Po
1454@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceUtils.cpp' object='libOPCODE_a-IceUtils.o' libtool=no @AMDEPBACKSLASH@
1455@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1456@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
1457
1458libOPCODE_a-IceUtils.obj: @TOPDIR@/OPCODE/Ice/IceUtils.cpp
1459@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`
1460@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libOPCODE_a-IceUtils.Tpo $(DEPDIR)/libOPCODE_a-IceUtils.Po
1461@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='@TOPDIR@/OPCODE/Ice/IceUtils.cpp' object='libOPCODE_a-IceUtils.obj' libtool=no @AMDEPBACKSLASH@
1462@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1463@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`
1464
1465libode_a-obstack.o: obstack.cpp
1466@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
1467@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-obstack.Tpo $(DEPDIR)/libode_a-obstack.Po
1468@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='obstack.cpp' object='libode_a-obstack.o' libtool=no @AMDEPBACKSLASH@
1469@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1470@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
1471
1472libode_a-obstack.obj: obstack.cpp
1473@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`
1474@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-obstack.Tpo $(DEPDIR)/libode_a-obstack.Po
1475@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='obstack.cpp' object='libode_a-obstack.obj' libtool=no @AMDEPBACKSLASH@
1476@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1477@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`
1478
1479libode_a-collision_util.o: collision_util.cpp
1480@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
1481@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_util.Tpo $(DEPDIR)/libode_a-collision_util.Po
1482@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_util.cpp' object='libode_a-collision_util.o' libtool=no @AMDEPBACKSLASH@
1483@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1484@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
1485
1486libode_a-collision_util.obj: collision_util.cpp
1487@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`
1488@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_util.Tpo $(DEPDIR)/libode_a-collision_util.Po
1489@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_util.cpp' object='libode_a-collision_util.obj' libtool=no @AMDEPBACKSLASH@
1490@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1491@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`
1492
1493libode_a-array.o: array.cpp
1494@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
1495@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-array.Tpo $(DEPDIR)/libode_a-array.Po
1496@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='array.cpp' object='libode_a-array.o' libtool=no @AMDEPBACKSLASH@
1497@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1498@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
1499
1500libode_a-array.obj: array.cpp
1501@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`
1502@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-array.Tpo $(DEPDIR)/libode_a-array.Po
1503@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='array.cpp' object='libode_a-array.obj' libtool=no @AMDEPBACKSLASH@
1504@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1505@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`
1506
1507libode_a-ode.o: ode.cpp
1508@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
1509@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ode.Tpo $(DEPDIR)/libode_a-ode.Po
1510@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ode.cpp' object='libode_a-ode.o' libtool=no @AMDEPBACKSLASH@
1511@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1512@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
1513
1514libode_a-ode.obj: ode.cpp
1515@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`
1516@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ode.Tpo $(DEPDIR)/libode_a-ode.Po
1517@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ode.cpp' object='libode_a-ode.obj' libtool=no @AMDEPBACKSLASH@
1518@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1519@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`
1520
1521libode_a-error.o: error.cpp
1522@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
1523@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-error.Tpo $(DEPDIR)/libode_a-error.Po
1524@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='error.cpp' object='libode_a-error.o' libtool=no @AMDEPBACKSLASH@
1525@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1526@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
1527
1528libode_a-error.obj: error.cpp
1529@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`
1530@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-error.Tpo $(DEPDIR)/libode_a-error.Po
1531@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='error.cpp' object='libode_a-error.obj' libtool=no @AMDEPBACKSLASH@
1532@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1533@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`
1534
1535libode_a-odemath.o: odemath.cpp
1536@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
1537@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-odemath.Tpo $(DEPDIR)/libode_a-odemath.Po
1538@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='odemath.cpp' object='libode_a-odemath.o' libtool=no @AMDEPBACKSLASH@
1539@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1540@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
1541
1542libode_a-odemath.obj: odemath.cpp
1543@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`
1544@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-odemath.Tpo $(DEPDIR)/libode_a-odemath.Po
1545@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='odemath.cpp' object='libode_a-odemath.obj' libtool=no @AMDEPBACKSLASH@
1546@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1547@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`
1548
1549libode_a-collision_kernel.o: collision_kernel.cpp
1550@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
1551@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_kernel.Tpo $(DEPDIR)/libode_a-collision_kernel.Po
1552@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_kernel.cpp' object='libode_a-collision_kernel.o' libtool=no @AMDEPBACKSLASH@
1553@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1554@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
1555
1556libode_a-collision_kernel.obj: collision_kernel.cpp
1557@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`
1558@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_kernel.Tpo $(DEPDIR)/libode_a-collision_kernel.Po
1559@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_kernel.cpp' object='libode_a-collision_kernel.obj' libtool=no @AMDEPBACKSLASH@
1560@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1561@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`
1562
1563libode_a-export-dif.o: export-dif.cpp
1564@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
1565@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-export-dif.Tpo $(DEPDIR)/libode_a-export-dif.Po
1566@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='export-dif.cpp' object='libode_a-export-dif.o' libtool=no @AMDEPBACKSLASH@
1567@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1568@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
1569
1570libode_a-export-dif.obj: export-dif.cpp
1571@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`
1572@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-export-dif.Tpo $(DEPDIR)/libode_a-export-dif.Po
1573@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='export-dif.cpp' object='libode_a-export-dif.obj' libtool=no @AMDEPBACKSLASH@
1574@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1575@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`
1576
1577libode_a-quickstep.o: quickstep.cpp
1578@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
1579@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-quickstep.Tpo $(DEPDIR)/libode_a-quickstep.Po
1580@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='quickstep.cpp' object='libode_a-quickstep.o' libtool=no @AMDEPBACKSLASH@
1581@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1582@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
1583
1584libode_a-quickstep.obj: quickstep.cpp
1585@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`
1586@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-quickstep.Tpo $(DEPDIR)/libode_a-quickstep.Po
1587@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='quickstep.cpp' object='libode_a-quickstep.obj' libtool=no @AMDEPBACKSLASH@
1588@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1589@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`
1590
1591libode_a-collision_quadtreespace.o: collision_quadtreespace.cpp
1592@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
1593@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_quadtreespace.Tpo $(DEPDIR)/libode_a-collision_quadtreespace.Po
1594@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_quadtreespace.cpp' object='libode_a-collision_quadtreespace.o' libtool=no @AMDEPBACKSLASH@
1595@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1596@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
1597
1598libode_a-collision_quadtreespace.obj: collision_quadtreespace.cpp
1599@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`
1600@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_quadtreespace.Tpo $(DEPDIR)/libode_a-collision_quadtreespace.Po
1601@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_quadtreespace.cpp' object='libode_a-collision_quadtreespace.obj' libtool=no @AMDEPBACKSLASH@
1602@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1603@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`
1604
1605libode_a-rotation.o: rotation.cpp
1606@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
1607@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-rotation.Tpo $(DEPDIR)/libode_a-rotation.Po
1608@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='rotation.cpp' object='libode_a-rotation.o' libtool=no @AMDEPBACKSLASH@
1609@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1610@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
1611
1612libode_a-rotation.obj: rotation.cpp
1613@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`
1614@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-rotation.Tpo $(DEPDIR)/libode_a-rotation.Po
1615@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='rotation.cpp' object='libode_a-rotation.obj' libtool=no @AMDEPBACKSLASH@
1616@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1617@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`
1618
1619libode_a-collision_space.o: collision_space.cpp
1620@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
1621@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_space.Tpo $(DEPDIR)/libode_a-collision_space.Po
1622@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_space.cpp' object='libode_a-collision_space.o' libtool=no @AMDEPBACKSLASH@
1623@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1624@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
1625
1626libode_a-collision_space.obj: collision_space.cpp
1627@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`
1628@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_space.Tpo $(DEPDIR)/libode_a-collision_space.Po
1629@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_space.cpp' object='libode_a-collision_space.obj' libtool=no @AMDEPBACKSLASH@
1630@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1631@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`
1632
1633libode_a-collision_cylinder_box.o: collision_cylinder_box.cpp
1634@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
1635@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_box.Tpo $(DEPDIR)/libode_a-collision_cylinder_box.Po
1636@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_box.cpp' object='libode_a-collision_cylinder_box.o' libtool=no @AMDEPBACKSLASH@
1637@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1638@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
1639
1640libode_a-collision_cylinder_box.obj: collision_cylinder_box.cpp
1641@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`
1642@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_box.Tpo $(DEPDIR)/libode_a-collision_cylinder_box.Po
1643@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_box.cpp' object='libode_a-collision_cylinder_box.obj' libtool=no @AMDEPBACKSLASH@
1644@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1645@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`
1646
1647libode_a-collision_cylinder_sphere.o: collision_cylinder_sphere.cpp
1648@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
1649@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_sphere.Tpo $(DEPDIR)/libode_a-collision_cylinder_sphere.Po
1650@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_sphere.cpp' object='libode_a-collision_cylinder_sphere.o' libtool=no @AMDEPBACKSLASH@
1651@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1652@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
1653
1654libode_a-collision_cylinder_sphere.obj: collision_cylinder_sphere.cpp
1655@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`
1656@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_sphere.Tpo $(DEPDIR)/libode_a-collision_cylinder_sphere.Po
1657@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_sphere.cpp' object='libode_a-collision_cylinder_sphere.obj' libtool=no @AMDEPBACKSLASH@
1658@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1659@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`
1660
1661libode_a-collision_cylinder_plane.o: collision_cylinder_plane.cpp
1662@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
1663@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_plane.Tpo $(DEPDIR)/libode_a-collision_cylinder_plane.Po
1664@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_plane.cpp' object='libode_a-collision_cylinder_plane.o' libtool=no @AMDEPBACKSLASH@
1665@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1666@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
1667
1668libode_a-collision_cylinder_plane.obj: collision_cylinder_plane.cpp
1669@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`
1670@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_plane.Tpo $(DEPDIR)/libode_a-collision_cylinder_plane.Po
1671@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_plane.cpp' object='libode_a-collision_cylinder_plane.obj' libtool=no @AMDEPBACKSLASH@
1672@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1673@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`
1674
1675libode_a-sphere.o: sphere.cpp
1676@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
1677@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-sphere.Tpo $(DEPDIR)/libode_a-sphere.Po
1678@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='sphere.cpp' object='libode_a-sphere.o' libtool=no @AMDEPBACKSLASH@
1679@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1680@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
1681
1682libode_a-sphere.obj: sphere.cpp
1683@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`
1684@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-sphere.Tpo $(DEPDIR)/libode_a-sphere.Po
1685@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='sphere.cpp' object='libode_a-sphere.obj' libtool=no @AMDEPBACKSLASH@
1686@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1687@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`
1688
1689libode_a-box.o: box.cpp
1690@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
1691@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-box.Tpo $(DEPDIR)/libode_a-box.Po
1692@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='box.cpp' object='libode_a-box.o' libtool=no @AMDEPBACKSLASH@
1693@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1694@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
1695
1696libode_a-box.obj: box.cpp
1697@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`
1698@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-box.Tpo $(DEPDIR)/libode_a-box.Po
1699@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='box.cpp' object='libode_a-box.obj' libtool=no @AMDEPBACKSLASH@
1700@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1701@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`
1702
1703libode_a-capsule.o: capsule.cpp
1704@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
1705@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-capsule.Tpo $(DEPDIR)/libode_a-capsule.Po
1706@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='capsule.cpp' object='libode_a-capsule.o' libtool=no @AMDEPBACKSLASH@
1707@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1708@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
1709
1710libode_a-capsule.obj: capsule.cpp
1711@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`
1712@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-capsule.Tpo $(DEPDIR)/libode_a-capsule.Po
1713@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='capsule.cpp' object='libode_a-capsule.obj' libtool=no @AMDEPBACKSLASH@
1714@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1715@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`
1716
1717libode_a-plane.o: plane.cpp
1718@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
1719@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-plane.Tpo $(DEPDIR)/libode_a-plane.Po
1720@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='plane.cpp' object='libode_a-plane.o' libtool=no @AMDEPBACKSLASH@
1721@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1722@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
1723
1724libode_a-plane.obj: plane.cpp
1725@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`
1726@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-plane.Tpo $(DEPDIR)/libode_a-plane.Po
1727@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='plane.cpp' object='libode_a-plane.obj' libtool=no @AMDEPBACKSLASH@
1728@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1729@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`
1730
1731libode_a-ray.o: ray.cpp
1732@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
1733@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ray.Tpo $(DEPDIR)/libode_a-ray.Po
1734@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ray.cpp' object='libode_a-ray.o' libtool=no @AMDEPBACKSLASH@
1735@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1736@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
1737
1738libode_a-ray.obj: ray.cpp
1739@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`
1740@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-ray.Tpo $(DEPDIR)/libode_a-ray.Po
1741@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='ray.cpp' object='libode_a-ray.obj' libtool=no @AMDEPBACKSLASH@
1742@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1743@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`
1744
1745libode_a-cylinder.o: cylinder.cpp
1746@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
1747@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-cylinder.Tpo $(DEPDIR)/libode_a-cylinder.Po
1748@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='cylinder.cpp' object='libode_a-cylinder.o' libtool=no @AMDEPBACKSLASH@
1749@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1750@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
1751
1752libode_a-cylinder.obj: cylinder.cpp
1753@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`
1754@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-cylinder.Tpo $(DEPDIR)/libode_a-cylinder.Po
1755@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='cylinder.cpp' object='libode_a-cylinder.obj' libtool=no @AMDEPBACKSLASH@
1756@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1757@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`
1758
1759libode_a-convex.o: convex.cpp
1760@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
1761@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-convex.Tpo $(DEPDIR)/libode_a-convex.Po
1762@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='convex.cpp' object='libode_a-convex.o' libtool=no @AMDEPBACKSLASH@
1763@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1764@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
1765
1766libode_a-convex.obj: convex.cpp
1767@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`
1768@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-convex.Tpo $(DEPDIR)/libode_a-convex.Po
1769@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='convex.cpp' object='libode_a-convex.obj' libtool=no @AMDEPBACKSLASH@
1770@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1771@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`
1772
1773libode_a-joint.o: joint.cpp
1774@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
1775@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-joint.Tpo $(DEPDIR)/libode_a-joint.Po
1776@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='joint.cpp' object='libode_a-joint.o' libtool=no @AMDEPBACKSLASH@
1777@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1778@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
1779
1780libode_a-joint.obj: joint.cpp
1781@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`
1782@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-joint.Tpo $(DEPDIR)/libode_a-joint.Po
1783@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='joint.cpp' object='libode_a-joint.obj' libtool=no @AMDEPBACKSLASH@
1784@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1785@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`
1786
1787libode_a-step.o: step.cpp
1788@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
1789@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-step.Tpo $(DEPDIR)/libode_a-step.Po
1790@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='step.cpp' object='libode_a-step.o' libtool=no @AMDEPBACKSLASH@
1791@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1792@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
1793
1794libode_a-step.obj: step.cpp
1795@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`
1796@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-step.Tpo $(DEPDIR)/libode_a-step.Po
1797@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='step.cpp' object='libode_a-step.obj' libtool=no @AMDEPBACKSLASH@
1798@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1799@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`
1800
1801libode_a-collision_transform.o: collision_transform.cpp
1802@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
1803@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_transform.Tpo $(DEPDIR)/libode_a-collision_transform.Po
1804@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_transform.cpp' object='libode_a-collision_transform.o' libtool=no @AMDEPBACKSLASH@
1805@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1806@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
1807
1808libode_a-collision_transform.obj: collision_transform.cpp
1809@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`
1810@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_transform.Tpo $(DEPDIR)/libode_a-collision_transform.Po
1811@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_transform.cpp' object='libode_a-collision_transform.obj' libtool=no @AMDEPBACKSLASH@
1812@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1813@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`
1814
1815libode_a-lcp.o: lcp.cpp
1816@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
1817@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-lcp.Tpo $(DEPDIR)/libode_a-lcp.Po
1818@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='lcp.cpp' object='libode_a-lcp.o' libtool=no @AMDEPBACKSLASH@
1819@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1820@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
1821
1822libode_a-lcp.obj: lcp.cpp
1823@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`
1824@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-lcp.Tpo $(DEPDIR)/libode_a-lcp.Po
1825@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='lcp.cpp' object='libode_a-lcp.obj' libtool=no @AMDEPBACKSLASH@
1826@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1827@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`
1828
1829libode_a-stepfast.o: stepfast.cpp
1830@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
1831@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-stepfast.Tpo $(DEPDIR)/libode_a-stepfast.Po
1832@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='stepfast.cpp' object='libode_a-stepfast.o' libtool=no @AMDEPBACKSLASH@
1833@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1834@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
1835
1836libode_a-stepfast.obj: stepfast.cpp
1837@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`
1838@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-stepfast.Tpo $(DEPDIR)/libode_a-stepfast.Po
1839@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='stepfast.cpp' object='libode_a-stepfast.obj' libtool=no @AMDEPBACKSLASH@
1840@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1841@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`
1842
1843libode_a-mass.o: mass.cpp
1844@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
1845@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mass.Tpo $(DEPDIR)/libode_a-mass.Po
1846@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mass.cpp' object='libode_a-mass.o' libtool=no @AMDEPBACKSLASH@
1847@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1848@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
1849
1850libode_a-mass.obj: mass.cpp
1851@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`
1852@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mass.Tpo $(DEPDIR)/libode_a-mass.Po
1853@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mass.cpp' object='libode_a-mass.obj' libtool=no @AMDEPBACKSLASH@
1854@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1855@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`
1856
1857libode_a-testing.o: testing.cpp
1858@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
1859@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-testing.Tpo $(DEPDIR)/libode_a-testing.Po
1860@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='testing.cpp' object='libode_a-testing.o' libtool=no @AMDEPBACKSLASH@
1861@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1862@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
1863
1864libode_a-testing.obj: testing.cpp
1865@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`
1866@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-testing.Tpo $(DEPDIR)/libode_a-testing.Po
1867@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='testing.cpp' object='libode_a-testing.obj' libtool=no @AMDEPBACKSLASH@
1868@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1869@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`
1870
1871libode_a-mat.o: mat.cpp
1872@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
1873@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mat.Tpo $(DEPDIR)/libode_a-mat.Po
1874@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mat.cpp' object='libode_a-mat.o' libtool=no @AMDEPBACKSLASH@
1875@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1876@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
1877
1878libode_a-mat.obj: mat.cpp
1879@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`
1880@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-mat.Tpo $(DEPDIR)/libode_a-mat.Po
1881@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='mat.cpp' object='libode_a-mat.obj' libtool=no @AMDEPBACKSLASH@
1882@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1883@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`
1884
1885libode_a-timer.o: timer.cpp
1886@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
1887@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-timer.Tpo $(DEPDIR)/libode_a-timer.Po
1888@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='timer.cpp' object='libode_a-timer.o' libtool=no @AMDEPBACKSLASH@
1889@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1890@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
1891
1892libode_a-timer.obj: timer.cpp
1893@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`
1894@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-timer.Tpo $(DEPDIR)/libode_a-timer.Po
1895@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='timer.cpp' object='libode_a-timer.obj' libtool=no @AMDEPBACKSLASH@
1896@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1897@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`
1898
1899libode_a-matrix.o: matrix.cpp
1900@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
1901@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-matrix.Tpo $(DEPDIR)/libode_a-matrix.Po
1902@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='matrix.cpp' object='libode_a-matrix.o' libtool=no @AMDEPBACKSLASH@
1903@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1904@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
1905
1906libode_a-matrix.obj: matrix.cpp
1907@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`
1908@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-matrix.Tpo $(DEPDIR)/libode_a-matrix.Po
1909@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='matrix.cpp' object='libode_a-matrix.obj' libtool=no @AMDEPBACKSLASH@
1910@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1911@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`
1912
1913libode_a-util.o: util.cpp
1914@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
1915@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-util.Tpo $(DEPDIR)/libode_a-util.Po
1916@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='util.cpp' object='libode_a-util.o' libtool=no @AMDEPBACKSLASH@
1917@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1918@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
1919
1920libode_a-util.obj: util.cpp
1921@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`
1922@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-util.Tpo $(DEPDIR)/libode_a-util.Po
1923@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='util.cpp' object='libode_a-util.obj' libtool=no @AMDEPBACKSLASH@
1924@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1925@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`
1926
1927libode_a-memory.o: memory.cpp
1928@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
1929@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-memory.Tpo $(DEPDIR)/libode_a-memory.Po
1930@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='memory.cpp' object='libode_a-memory.o' libtool=no @AMDEPBACKSLASH@
1931@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1932@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
1933
1934libode_a-memory.obj: memory.cpp
1935@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`
1936@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-memory.Tpo $(DEPDIR)/libode_a-memory.Po
1937@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='memory.cpp' object='libode_a-memory.obj' libtool=no @AMDEPBACKSLASH@
1938@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1939@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`
1940
1941libode_a-misc.o: misc.cpp
1942@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
1943@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-misc.Tpo $(DEPDIR)/libode_a-misc.Po
1944@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='misc.cpp' object='libode_a-misc.o' libtool=no @AMDEPBACKSLASH@
1945@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1946@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
1947
1948libode_a-misc.obj: misc.cpp
1949@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`
1950@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-misc.Tpo $(DEPDIR)/libode_a-misc.Po
1951@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='misc.cpp' object='libode_a-misc.obj' libtool=no @AMDEPBACKSLASH@
1952@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1953@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`
1954
1955libode_a-heightfield.o: heightfield.cpp
1956@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
1957@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-heightfield.Tpo $(DEPDIR)/libode_a-heightfield.Po
1958@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='heightfield.cpp' object='libode_a-heightfield.o' libtool=no @AMDEPBACKSLASH@
1959@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1960@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
1961
1962libode_a-heightfield.obj: heightfield.cpp
1963@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`
1964@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-heightfield.Tpo $(DEPDIR)/libode_a-heightfield.Po
1965@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='heightfield.cpp' object='libode_a-heightfield.obj' libtool=no @AMDEPBACKSLASH@
1966@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1967@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`
1968
1969libode_a-collision_trimesh_gimpact.o: collision_trimesh_gimpact.cpp
1970@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
1971@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_gimpact.Tpo $(DEPDIR)/libode_a-collision_trimesh_gimpact.Po
1972@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_gimpact.cpp' object='libode_a-collision_trimesh_gimpact.o' libtool=no @AMDEPBACKSLASH@
1973@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1974@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
1975
1976libode_a-collision_trimesh_gimpact.obj: collision_trimesh_gimpact.cpp
1977@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`
1978@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_gimpact.Tpo $(DEPDIR)/libode_a-collision_trimesh_gimpact.Po
1979@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_gimpact.cpp' object='libode_a-collision_trimesh_gimpact.obj' libtool=no @AMDEPBACKSLASH@
1980@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1981@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`
1982
1983libode_a-collision_trimesh_trimesh.o: collision_trimesh_trimesh.cpp
1984@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
1985@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_trimesh.Tpo $(DEPDIR)/libode_a-collision_trimesh_trimesh.Po
1986@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_trimesh.cpp' object='libode_a-collision_trimesh_trimesh.o' libtool=no @AMDEPBACKSLASH@
1987@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1988@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
1989
1990libode_a-collision_trimesh_trimesh.obj: collision_trimesh_trimesh.cpp
1991@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`
1992@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_trimesh.Tpo $(DEPDIR)/libode_a-collision_trimesh_trimesh.Po
1993@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_trimesh.cpp' object='libode_a-collision_trimesh_trimesh.obj' libtool=no @AMDEPBACKSLASH@
1994@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
1995@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`
1996
1997libode_a-collision_trimesh_sphere.o: collision_trimesh_sphere.cpp
1998@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
1999@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_sphere.Tpo $(DEPDIR)/libode_a-collision_trimesh_sphere.Po
2000@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_sphere.cpp' object='libode_a-collision_trimesh_sphere.o' libtool=no @AMDEPBACKSLASH@
2001@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2002@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
2003
2004libode_a-collision_trimesh_sphere.obj: collision_trimesh_sphere.cpp
2005@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`
2006@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_sphere.Tpo $(DEPDIR)/libode_a-collision_trimesh_sphere.Po
2007@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_sphere.cpp' object='libode_a-collision_trimesh_sphere.obj' libtool=no @AMDEPBACKSLASH@
2008@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2009@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`
2010
2011libode_a-collision_trimesh_ray.o: collision_trimesh_ray.cpp
2012@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
2013@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ray.Tpo $(DEPDIR)/libode_a-collision_trimesh_ray.Po
2014@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ray.cpp' object='libode_a-collision_trimesh_ray.o' libtool=no @AMDEPBACKSLASH@
2015@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2016@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
2017
2018libode_a-collision_trimesh_ray.obj: collision_trimesh_ray.cpp
2019@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`
2020@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ray.Tpo $(DEPDIR)/libode_a-collision_trimesh_ray.Po
2021@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ray.cpp' object='libode_a-collision_trimesh_ray.obj' libtool=no @AMDEPBACKSLASH@
2022@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2023@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`
2024
2025libode_a-collision_trimesh_opcode.o: collision_trimesh_opcode.cpp
2026@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
2027@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_opcode.Tpo $(DEPDIR)/libode_a-collision_trimesh_opcode.Po
2028@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_opcode.cpp' object='libode_a-collision_trimesh_opcode.o' libtool=no @AMDEPBACKSLASH@
2029@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2030@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
2031
2032libode_a-collision_trimesh_opcode.obj: collision_trimesh_opcode.cpp
2033@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`
2034@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_opcode.Tpo $(DEPDIR)/libode_a-collision_trimesh_opcode.Po
2035@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_opcode.cpp' object='libode_a-collision_trimesh_opcode.obj' libtool=no @AMDEPBACKSLASH@
2036@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2037@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`
2038
2039libode_a-collision_trimesh_box.o: collision_trimesh_box.cpp
2040@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
2041@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_box.Tpo $(DEPDIR)/libode_a-collision_trimesh_box.Po
2042@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_box.cpp' object='libode_a-collision_trimesh_box.o' libtool=no @AMDEPBACKSLASH@
2043@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2044@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
2045
2046libode_a-collision_trimesh_box.obj: collision_trimesh_box.cpp
2047@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`
2048@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_box.Tpo $(DEPDIR)/libode_a-collision_trimesh_box.Po
2049@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_box.cpp' object='libode_a-collision_trimesh_box.obj' libtool=no @AMDEPBACKSLASH@
2050@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2051@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`
2052
2053libode_a-collision_trimesh_ccylinder.o: collision_trimesh_ccylinder.cpp
2054@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
2055@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Tpo $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Po
2056@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ccylinder.cpp' object='libode_a-collision_trimesh_ccylinder.o' libtool=no @AMDEPBACKSLASH@
2057@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2058@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
2059
2060libode_a-collision_trimesh_ccylinder.obj: collision_trimesh_ccylinder.cpp
2061@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`
2062@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Tpo $(DEPDIR)/libode_a-collision_trimesh_ccylinder.Po
2063@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_ccylinder.cpp' object='libode_a-collision_trimesh_ccylinder.obj' libtool=no @AMDEPBACKSLASH@
2064@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2065@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`
2066
2067libode_a-collision_trimesh_distance.o: collision_trimesh_distance.cpp
2068@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
2069@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_distance.Tpo $(DEPDIR)/libode_a-collision_trimesh_distance.Po
2070@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_distance.cpp' object='libode_a-collision_trimesh_distance.o' libtool=no @AMDEPBACKSLASH@
2071@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2072@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
2073
2074libode_a-collision_trimesh_distance.obj: collision_trimesh_distance.cpp
2075@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`
2076@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_distance.Tpo $(DEPDIR)/libode_a-collision_trimesh_distance.Po
2077@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_distance.cpp' object='libode_a-collision_trimesh_distance.obj' libtool=no @AMDEPBACKSLASH@
2078@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2079@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`
2080
2081libode_a-collision_cylinder_trimesh.o: collision_cylinder_trimesh.cpp
2082@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
2083@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_trimesh.Tpo $(DEPDIR)/libode_a-collision_cylinder_trimesh.Po
2084@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_trimesh.cpp' object='libode_a-collision_cylinder_trimesh.o' libtool=no @AMDEPBACKSLASH@
2085@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2086@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
2087
2088libode_a-collision_cylinder_trimesh.obj: collision_cylinder_trimesh.cpp
2089@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`
2090@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_cylinder_trimesh.Tpo $(DEPDIR)/libode_a-collision_cylinder_trimesh.Po
2091@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_cylinder_trimesh.cpp' object='libode_a-collision_cylinder_trimesh.obj' libtool=no @AMDEPBACKSLASH@
2092@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2093@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`
2094
2095libode_a-collision_trimesh_plane.o: collision_trimesh_plane.cpp
2096@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
2097@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_plane.Tpo $(DEPDIR)/libode_a-collision_trimesh_plane.Po
2098@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_plane.cpp' object='libode_a-collision_trimesh_plane.o' libtool=no @AMDEPBACKSLASH@
2099@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2100@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
2101
2102libode_a-collision_trimesh_plane.obj: collision_trimesh_plane.cpp
2103@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`
2104@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/libode_a-collision_trimesh_plane.Tpo $(DEPDIR)/libode_a-collision_trimesh_plane.Po
2105@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='collision_trimesh_plane.cpp' object='libode_a-collision_trimesh_plane.obj' libtool=no @AMDEPBACKSLASH@
2106@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
2107@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`
2108
2109ID: $(HEADERS) $(SOURCES) $(LISP) $(TAGS_FILES)
2110 list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \
2111 unique=`for i in $$list; do \
2112 if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
2113 done | \
2114 $(AWK) ' { files[$$0] = 1; } \
2115 END { for (i in files) print i; }'`; \
2116 mkid -fID $$unique
2117tags: TAGS
2118
2119TAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \
2120 $(TAGS_FILES) $(LISP)
2121 tags=; \
2122 here=`pwd`; \
2123 list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \
2124 unique=`for i in $$list; do \
2125 if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
2126 done | \
2127 $(AWK) ' { files[$$0] = 1; } \
2128 END { for (i in files) print i; }'`; \
2129 if test -z "$(ETAGS_ARGS)$$tags$$unique"; then :; else \
2130 test -n "$$unique" || unique=$$empty_fix; \
2131 $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \
2132 $$tags $$unique; \
2133 fi
2134ctags: CTAGS
2135CTAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \
2136 $(TAGS_FILES) $(LISP)
2137 tags=; \
2138 here=`pwd`; \
2139 list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \
2140 unique=`for i in $$list; do \
2141 if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
2142 done | \
2143 $(AWK) ' { files[$$0] = 1; } \
2144 END { for (i in files) print i; }'`; \
2145 test -z "$(CTAGS_ARGS)$$tags$$unique" \
2146 || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \
2147 $$tags $$unique
2148
2149GTAGS:
2150 here=`$(am__cd) $(top_builddir) && pwd` \
2151 && cd $(top_srcdir) \
2152 && gtags -i $(GTAGS_ARGS) $$here
2153
2154distclean-tags:
2155 -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags
2156
2157distdir: $(DISTFILES)
2158 @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \
2159 topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \
2160 list='$(DISTFILES)'; \
2161 dist_files=`for file in $$list; do echo $$file; done | \
2162 sed -e "s|^$$srcdirstrip/||;t" \
2163 -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \
2164 case $$dist_files in \
2165 */*) $(MKDIR_P) `echo "$$dist_files" | \
2166 sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \
2167 sort -u` ;; \
2168 esac; \
2169 for file in $$dist_files; do \
2170 if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \
2171 if test -d $$d/$$file; then \
2172 dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \
2173 if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \
2174 cp -pR $(srcdir)/$$file $(distdir)$$dir || exit 1; \
2175 fi; \
2176 cp -pR $$d/$$file $(distdir)$$dir || exit 1; \
2177 else \
2178 test -f $(distdir)/$$file \
2179 || cp -p $$d/$$file $(distdir)/$$file \
2180 || exit 1; \
2181 fi; \
2182 done
2183check-am: all-am
2184check: check-am
2185all-am: Makefile $(LIBRARIES) $(PROGRAMS)
2186installdirs:
2187 for dir in "$(DESTDIR)$(libdir)" "$(DESTDIR)$(traplibdir)"; do \
2188 test -z "$$dir" || $(MKDIR_P) "$$dir"; \
2189 done
2190install: install-am
2191install-exec: install-exec-am
2192install-data: install-data-am
2193uninstall: uninstall-am
2194
2195install-am: all-am
2196 @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am
2197
2198installcheck: installcheck-am
2199install-strip:
2200 $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \
2201 install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \
2202 `test -z '$(STRIP)' || \
2203 echo "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'"` install
2204mostlyclean-generic:
2205
2206clean-generic:
2207
2208distclean-generic:
2209 -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES)
2210
2211maintainer-clean-generic:
2212 @echo "This command is intended for maintainers to use"
2213 @echo "it deletes files that may require special tools to rebuild."
2214clean: clean-am
2215
2216clean-am: clean-generic clean-libLIBRARIES clean-noinstLIBRARIES \
2217 clean-traplibPROGRAMS mostlyclean-am
2218
2219distclean: distclean-am
2220 -rm -rf ./$(DEPDIR)
2221 -rm -f Makefile
2222distclean-am: clean-am distclean-compile distclean-generic \
2223 distclean-tags
2224
2225dvi: dvi-am
2226
2227dvi-am:
2228
2229html: html-am
2230
2231info: info-am
2232
2233info-am:
2234
2235install-data-am: install-traplibPROGRAMS
2236
2237install-dvi: install-dvi-am
2238
2239install-exec-am: install-libLIBRARIES
2240
2241install-html: install-html-am
2242
2243install-info: install-info-am
2244
2245install-man:
2246
2247install-pdf: install-pdf-am
2248
2249install-ps: install-ps-am
2250
2251installcheck-am:
2252
2253maintainer-clean: maintainer-clean-am
2254 -rm -rf ./$(DEPDIR)
2255 -rm -f Makefile
2256maintainer-clean-am: distclean-am maintainer-clean-generic
2257
2258mostlyclean: mostlyclean-am
2259
2260mostlyclean-am: mostlyclean-compile mostlyclean-generic
2261
2262pdf: pdf-am
2263
2264pdf-am:
2265
2266ps: ps-am
2267
2268ps-am:
2269
2270uninstall-am: uninstall-libLIBRARIES uninstall-traplibPROGRAMS
2271
2272.MAKE: install-am install-strip
2273
2274.PHONY: CTAGS GTAGS all all-am check check-am clean clean-generic \
2275 clean-libLIBRARIES clean-noinstLIBRARIES clean-traplibPROGRAMS \
2276 ctags distclean distclean-compile distclean-generic \
2277 distclean-tags distdir dvi dvi-am html html-am info info-am \
2278 install install-am install-data install-data-am install-dvi \
2279 install-dvi-am install-exec install-exec-am install-html \
2280 install-html-am install-info install-info-am \
2281 install-libLIBRARIES install-man install-pdf install-pdf-am \
2282 install-ps install-ps-am install-strip install-traplibPROGRAMS \
2283 installcheck installcheck-am installdirs maintainer-clean \
2284 maintainer-clean-generic mostlyclean mostlyclean-compile \
2285 mostlyclean-generic pdf pdf-am ps ps-am tags uninstall \
2286 uninstall-am uninstall-libLIBRARIES uninstall-traplibPROGRAMS
2287
2288# Tell versions [3.59,3.63) of GNU make to not export all variables.
2289# Otherwise a system limit (for SysV at least) may be exceeded.
2290.NOEXPORT:
diff --git a/libraries/ode-0.9/ode/src/array.cpp b/libraries/ode-0.9/ode/src/array.cpp
new file mode 100644
index 0000000..cbb1a6e
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/array.cpp
@@ -0,0 +1,80 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/memory.h>
25#include <ode/error.h>
26#include "array.h"
27
28
29static inline int roundUpToPowerOfTwo (int x)
30{
31 int i = 1;
32 while (i < x) i <<= 1;
33 return i;
34}
35
36
37void dArrayBase::_freeAll (int sizeofT)
38{
39 if (_data) {
40 if (_data == this+1) return; // if constructLocalArray() was called
41 dFree (_data,_anum * sizeofT);
42 }
43}
44
45
46void dArrayBase::_setSize (int newsize, int sizeofT)
47{
48 if (newsize < 0) return;
49 if (newsize > _anum) {
50 if (_data == this+1) {
51 // this is a no-no, because constructLocalArray() was called
52 dDebug (0,"setSize() out of space in LOCAL array");
53 }
54 int newanum = roundUpToPowerOfTwo (newsize);
55 if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT);
56 else _data = dAlloc (newanum*sizeofT);
57 _anum = newanum;
58 }
59 _size = newsize;
60}
61
62
63void * dArrayBase::operator new (size_t size)
64{
65 return dAlloc (size);
66}
67
68
69void dArrayBase::operator delete (void *ptr, size_t size)
70{
71 dFree (ptr,size);
72}
73
74
75void dArrayBase::constructLocalArray (int __anum)
76{
77 _size = 0;
78 _anum = __anum;
79 _data = this+1;
80}
diff --git a/libraries/ode-0.9/ode/src/array.h b/libraries/ode-0.9/ode/src/array.h
new file mode 100644
index 0000000..307206c
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/array.h
@@ -0,0 +1,135 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/* this comes from the `reuse' library. copy any changes back to the source.
24 *
25 * Variable sized array template. The array is always stored in a contiguous
26 * chunk. The array can be resized. A size increase will cause more memory
27 * to be allocated, and may result in relocation of the array memory.
28 * A size decrease has no effect on the memory allocation.
29 *
30 * Array elements with constructors or destructors are not supported!
31 * But if you must have such elements, here's what to know/do:
32 * - Bitwise copy is used when copying whole arrays.
33 * - When copying individual items (via push(), insert() etc) the `='
34 * (equals) operator is used. Thus you should define this operator to do
35 * a bitwise copy. You should probably also define the copy constructor.
36 */
37
38
39#ifndef _ODE_ARRAY_H_
40#define _ODE_ARRAY_H_
41
42#include <ode/config.h>
43
44
45// this base class has no constructors or destructor, for your convenience.
46
47class dArrayBase {
48protected:
49 int _size; // number of elements in `data'
50 int _anum; // allocated number of elements in `data'
51 void *_data; // array data
52
53 void _freeAll (int sizeofT);
54 void _setSize (int newsize, int sizeofT);
55 // set the array size to `newsize', allocating more memory if necessary.
56 // if newsize>_anum and is a power of two then this is guaranteed to
57 // set _size and _anum to newsize.
58
59public:
60 // not: dArrayBase () { _size=0; _anum=0; _data=0; }
61
62 int size() const { return _size; }
63 int allocatedSize() const { return _anum; }
64 void * operator new (size_t size);
65 void operator delete (void *ptr, size_t size);
66
67 void constructor() { _size=0; _anum=0; _data=0; }
68 // if this structure is allocated with malloc() instead of new, you can
69 // call this to set it up.
70
71 void constructLocalArray (int __anum);
72 // this helper function allows non-reallocating arrays to be constructed
73 // on the stack (or in the heap if necessary). this is something of a
74 // kludge and should be used with extreme care. this function acts like
75 // a constructor - it is called on uninitialized memory that will hold the
76 // Array structure and the data. __anum is the number of elements that
77 // are allocated. the memory MUST be allocated with size:
78 // sizeof(ArrayBase) + __anum*sizeof(T)
79 // arrays allocated this way will never try to reallocate or free the
80 // memory - that's your job.
81};
82
83
84template <class T> class dArray : public dArrayBase {
85public:
86 void equals (const dArray<T> &x) {
87 setSize (x.size());
88 memcpy (_data,x._data,x._size * sizeof(T));
89 }
90
91 dArray () { constructor(); }
92 dArray (const dArray<T> &x) { constructor(); equals (x); }
93 ~dArray () { _freeAll(sizeof(T)); }
94 void setSize (int newsize) { _setSize (newsize,sizeof(T)); }
95 T *data() const { return (T*) _data; }
96 T & operator[] (int i) const { return ((T*)_data)[i]; }
97 void operator = (const dArray<T> &x) { equals (x); }
98
99 void push (const T item) {
100 if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
101 memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T));
102 }
103
104 void swap (dArray<T> &x) {
105 int tmp1;
106 void *tmp2;
107 tmp1=_size; _size=x._size; x._size=tmp1;
108 tmp1=_anum; _anum=x._anum; x._anum=tmp1;
109 tmp2=_data; _data=x._data; x._data=tmp2;
110 }
111
112 // insert the item at the position `i'. if i<0 then add the item to the
113 // start, if i >= size then add the item to the end of the array.
114 void insert (int i, const T item) {
115 if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
116 if (i >= (_size-1)) i = _size-1; // add to end
117 else {
118 if (i < 0) i=0; // add to start
119 int n = _size-1-i;
120 if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T));
121 }
122 ((T*)_data)[i] = item;
123 }
124
125 void remove (int i) {
126 if (i >= 0 && i < _size) { // passing this test guarantees size>0
127 int n = _size-1-i;
128 if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T));
129 _size--;
130 }
131 }
132};
133
134
135#endif
diff --git a/libraries/ode-0.9/ode/src/box.cpp b/libraries/ode-0.9/ode/src/box.cpp
new file mode 100644
index 0000000..f328651
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/box.cpp
@@ -0,0 +1,847 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::g1 and dContactGeom::g2.
29
30*/
31
32#include <ode/common.h>
33#include <ode/collision.h>
34#include <ode/matrix.h>
35#include <ode/rotation.h>
36#include <ode/odemath.h>
37#include "collision_kernel.h"
38#include "collision_std.h"
39#include "collision_util.h"
40
41#ifdef _MSC_VER
42#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43#endif
44
45//****************************************************************************
46// box public API
47
48dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
49{
50 dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
51 type = dBoxClass;
52 side[0] = lx;
53 side[1] = ly;
54 side[2] = lz;
55}
56
57
58void dxBox::computeAABB()
59{
60 const dMatrix3& R = final_posr->R;
61 const dVector3& pos = final_posr->pos;
62
63 dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) +
64 dFabs (R[1] * side[1]) + dFabs (R[2] * side[2]));
65 dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) +
66 dFabs (R[5] * side[1]) + dFabs (R[6] * side[2]));
67 dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) +
68 dFabs (R[9] * side[1]) + dFabs (R[10] * side[2]));
69 aabb[0] = pos[0] - xrange;
70 aabb[1] = pos[0] + xrange;
71 aabb[2] = pos[1] - yrange;
72 aabb[3] = pos[1] + yrange;
73 aabb[4] = pos[2] - zrange;
74 aabb[5] = pos[2] + zrange;
75}
76
77
78dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
79{
80 return new dxBox (space,lx,ly,lz);
81}
82
83
84void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
85{
86 dUASSERT (g && g->type == dBoxClass,"argument not a box");
87 dAASSERT (lx > 0 && ly > 0 && lz > 0);
88 dxBox *b = (dxBox*) g;
89 b->side[0] = lx;
90 b->side[1] = ly;
91 b->side[2] = lz;
92 dGeomMoved (g);
93}
94
95
96void dGeomBoxGetLengths (dGeomID g, dVector3 result)
97{
98 dUASSERT (g && g->type == dBoxClass,"argument not a box");
99 dxBox *b = (dxBox*) g;
100 result[0] = b->side[0];
101 result[1] = b->side[1];
102 result[2] = b->side[2];
103}
104
105
106dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
107{
108 dUASSERT (g && g->type == dBoxClass,"argument not a box");
109 g->recomputePosr();
110 dxBox *b = (dxBox*) g;
111
112 // Set p = (x,y,z) relative to box center
113 //
114 // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)
115
116 dVector3 p,q;
117
118 p[0] = x - b->final_posr->pos[0];
119 p[1] = y - b->final_posr->pos[1];
120 p[2] = z - b->final_posr->pos[2];
121
122 // Rotate p into box's coordinate frame, so we can
123 // treat the OBB as an AABB
124
125 dMULTIPLY1_331 (q,b->final_posr->R,p);
126
127 // Record distance from point to each successive box side, and see
128 // if the point is inside all six sides
129
130 dReal dist[6];
131 int i;
132
133 bool inside = true;
134
135 for (i=0; i < 3; i++) {
136 dReal side = b->side[i] * REAL(0.5);
137
138 dist[i ] = side - q[i];
139 dist[i+3] = side + q[i];
140
141 if ((dist[i] < 0) || (dist[i+3] < 0)) {
142 inside = false;
143 }
144 }
145
146 // If point is inside the box, the depth is the smallest positive distance
147 // to any side
148
149 if (inside) {
150 dReal smallest_dist = (dReal) (unsigned) -1;
151
152 for (i=0; i < 6; i++) {
153 if (dist[i] < smallest_dist) smallest_dist = dist[i];
154 }
155
156 return smallest_dist;
157 }
158
159 // Otherwise, if point is outside the box, the depth is the largest
160 // distance to any side. This is an approximation to the 'proper'
161 // solution (the proper solution may be larger in some cases).
162
163 dReal largest_dist = 0;
164
165 for (i=0; i < 6; i++) {
166 if (dist[i] > largest_dist) largest_dist = dist[i];
167 }
168
169 return -largest_dist;
170}
171
172//****************************************************************************
173// box-box collision utility
174
175
176// find all the intersection points between the 2D rectangle with vertices
177// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
178// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
179//
180// the intersection points are returned as x,y pairs in the 'ret' array.
181// the number of intersection points is returned by the function (this will
182// be in the range 0 to 8).
183
184static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16])
185{
186 // q (and r) contain nq (and nr) coordinate points for the current (and
187 // chopped) polygons
188 int nq=4,nr;
189 dReal buffer[16];
190 dReal *q = p;
191 dReal *r = ret;
192 for (int dir=0; dir <= 1; dir++) {
193 // direction notation: xy[0] = x axis, xy[1] = y axis
194 for (int sign=-1; sign <= 1; sign += 2) {
195 // chop q along the line xy[dir] = sign*h[dir]
196 dReal *pq = q;
197 dReal *pr = r;
198 nr = 0;
199 for (int i=nq; i > 0; i--) {
200 // go through all points in q and all lines between adjacent points
201 if (sign*pq[dir] < h[dir]) {
202 // this point is inside the chopping line
203 pr[0] = pq[0];
204 pr[1] = pq[1];
205 pr += 2;
206 nr++;
207 if (nr & 8) {
208 q = r;
209 goto done;
210 }
211 }
212 dReal *nextq = (i > 1) ? pq+2 : q;
213 if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
214 // this line crosses the chopping line
215 pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
216 (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
217 pr[dir] = sign*h[dir];
218 pr += 2;
219 nr++;
220 if (nr & 8) {
221 q = r;
222 goto done;
223 }
224 }
225 pq += 2;
226 }
227 q = r;
228 r = (q==ret) ? buffer : ret;
229 nq = nr;
230 }
231 }
232 done:
233 if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal));
234 return nr;
235}
236
237
238// given n points in the plane (array p, of size 2*n), generate m points that
239// best represent the whole set. the definition of 'best' here is not
240// predetermined - the idea is to select points that give good box-box
241// collision detection behavior. the chosen point indexes are returned in the
242// array iret (of size m). 'i0' is always the first entry in the array.
243// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
244// in the range [0..n-1].
245
246void cullPoints (int n, dReal p[], int m, int i0, int iret[])
247{
248 // compute the centroid of the polygon in cx,cy
249 int i,j;
250 dReal a,cx,cy,q;
251 if (n==1) {
252 cx = p[0];
253 cy = p[1];
254 }
255 else if (n==2) {
256 cx = REAL(0.5)*(p[0] + p[2]);
257 cy = REAL(0.5)*(p[1] + p[3]);
258 }
259 else {
260 a = 0;
261 cx = 0;
262 cy = 0;
263 for (i=0; i<(n-1); i++) {
264 q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
265 a += q;
266 cx += q*(p[i*2]+p[i*2+2]);
267 cy += q*(p[i*2+1]+p[i*2+3]);
268 }
269 q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
270 a = dRecip(REAL(3.0)*(a+q));
271 cx = a*(cx + q*(p[n*2-2]+p[0]));
272 cy = a*(cy + q*(p[n*2-1]+p[1]));
273 }
274
275 // compute the angle of each point w.r.t. the centroid
276 dReal A[8];
277 for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
278
279 // search for points that have angles closest to A[i0] + i*(2*pi/m).
280 int avail[8];
281 for (i=0; i<n; i++) avail[i] = 1;
282 avail[i0] = 0;
283 iret[0] = i0;
284 iret++;
285 for (j=1; j<m; j++) {
286 a = dReal(j)*(2*M_PI/m) + A[i0];
287 if (a > M_PI) a -= 2*M_PI;
288 dReal maxdiff=1e9,diff;
289#ifndef dNODEBUG
290 *iret = i0; // iret is not allowed to keep this value
291#endif
292 for (i=0; i<n; i++) {
293 if (avail[i]) {
294 diff = dFabs (A[i]-a);
295 if (diff > M_PI) diff = 2*M_PI - diff;
296 if (diff < maxdiff) {
297 maxdiff = diff;
298 *iret = i;
299 }
300 }
301 }
302#ifndef dNODEBUG
303 dIASSERT (*iret != i0); // ensure iret got set
304#endif
305 avail[*iret] = 0;
306 iret++;
307 }
308}
309
310
311// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
312// generate contact points. this returns 0 if there is no contact otherwise
313// it returns the number of contacts generated.
314// `normal' returns the contact normal.
315// `depth' returns the maximum penetration depth along that normal.
316// `return_code' returns a number indicating the type of contact that was
317// detected:
318// 1,2,3 = box 2 intersects with a face of box 1
319// 4,5,6 = box 1 intersects with a face of box 2
320// 7..15 = edge-edge contact
321// `maxc' is the maximum number of contacts allowed to be generated, i.e.
322// the size of the `contact' array.
323// `contact' and `skip' are the contact array information provided to the
324// collision functions. this function only fills in the position and depth
325// fields.
326
327
328int dBoxBox (const dVector3 p1, const dMatrix3 R1,
329 const dVector3 side1, const dVector3 p2,
330 const dMatrix3 R2, const dVector3 side2,
331 dVector3 normal, dReal *depth, int *return_code,
332 int flags, dContactGeom *contact, int skip)
333{
334 const dReal fudge_factor = REAL(1.05);
335 dVector3 p,pp,normalC;
336 const dReal *normalR = 0;
337 dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
338 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
339 int i,j,invert_normal,code;
340
341 // get vector from centers of box 1 to box 2, relative to box 1
342 p[0] = p2[0] - p1[0];
343 p[1] = p2[1] - p1[1];
344 p[2] = p2[2] - p1[2];
345 dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
346
347 // get side lengths / 2
348 A[0] = side1[0]*REAL(0.5);
349 A[1] = side1[1]*REAL(0.5);
350 A[2] = side1[2]*REAL(0.5);
351 B[0] = side2[0]*REAL(0.5);
352 B[1] = side2[1]*REAL(0.5);
353 B[2] = side2[2]*REAL(0.5);
354
355 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
356 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
357 R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
358 R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
359
360 Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
361 Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
362 Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
363
364 // for all 15 possible separating axes:
365 // * see if the axis separates the boxes. if so, return 0.
366 // * find the depth of the penetration along the separating axis (s2)
367 // * if this is the largest depth so far, record it.
368 // the normal vector will be set to the separating axis with the smallest
369 // depth. note: normalR is set to point to a column of R1 or R2 if that is
370 // the smallest depth normal so far. otherwise normalR is 0 and normalC is
371 // set to a vector relative to body 1. invert_normal is 1 if the sign of
372 // the normal should be flipped.
373
374 do {
375#define TST(expr1,expr2,norm,cc) \
376 expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
377 s2 = dFabs(expr1_val) - (expr2); \
378 if (s2 > 0) return 0; \
379 if (s2 > s) { \
380 s = s2; \
381 normalR = norm; \
382 invert_normal = ((expr1_val) < 0); \
383 code = (cc); \
384 if (flags & CONTACTS_UNIMPORTANT) break; \
385 }
386
387 s = -dInfinity;
388 invert_normal = 0;
389 code = 0;
390
391 // separating axis = u1,u2,u3
392 TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
393 TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
394 TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
395
396 // separating axis = v1,v2,v3
397 TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
398 TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
399 TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
400
401 // note: cross product axes need to be scaled when s is computed.
402 // normal (n1,n2,n3) is relative to box 1.
403#undef TST
404#define TST(expr1,expr2,n1,n2,n3,cc) \
405 expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
406 s2 = dFabs(expr1_val) - (expr2); \
407 if (s2 > 0) return 0; \
408 l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
409 if (l > 0) { \
410 s2 /= l; \
411 if (s2*fudge_factor > s) { \
412 s = s2; \
413 normalR = 0; \
414 normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
415 invert_normal = ((expr1_val) < 0); \
416 code = (cc); \
417 if (flags & CONTACTS_UNIMPORTANT) break; \
418 } \
419 }
420
421 // separating axis = u1 x (v1,v2,v3)
422 TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
423 TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
424 TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
425
426 // separating axis = u2 x (v1,v2,v3)
427 TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
428 TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
429 TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
430
431 // separating axis = u3 x (v1,v2,v3)
432 TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
433 TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
434 TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
435#undef TST
436 } while (0);
437
438 if (!code) return 0;
439
440 // if we get to this point, the boxes interpenetrate. compute the normal
441 // in global coordinates.
442 if (normalR) {
443 normal[0] = normalR[0];
444 normal[1] = normalR[4];
445 normal[2] = normalR[8];
446 }
447 else {
448 dMULTIPLY0_331 (normal,R1,normalC);
449 }
450 if (invert_normal) {
451 normal[0] = -normal[0];
452 normal[1] = -normal[1];
453 normal[2] = -normal[2];
454 }
455 *depth = -s;
456
457 // compute contact point(s)
458
459 if (code > 6) {
460 // an edge from box 1 touches an edge from box 2.
461 // find a point pa on the intersecting edge of box 1
462 dVector3 pa;
463 dReal sign;
464 for (i=0; i<3; i++) pa[i] = p1[i];
465 for (j=0; j<3; j++) {
466 sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
467 for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
468 }
469
470 // find a point pb on the intersecting edge of box 2
471 dVector3 pb;
472 for (i=0; i<3; i++) pb[i] = p2[i];
473 for (j=0; j<3; j++) {
474 sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
475 for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
476 }
477
478 dReal alpha,beta;
479 dVector3 ua,ub;
480 for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
481 for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
482
483 dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
484 for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
485 for (i=0; i<3; i++) pb[i] += ub[i]*beta;
486
487 for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
488 contact[0].depth = *depth;
489 *return_code = code;
490 return 1;
491 }
492
493 // okay, we have a face-something intersection (because the separating
494 // axis is perpendicular to a face). define face 'a' to be the reference
495 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
496 // the incident face (the closest face of the other box).
497
498 const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
499 if (code <= 3) {
500 Ra = R1;
501 Rb = R2;
502 pa = p1;
503 pb = p2;
504 Sa = A;
505 Sb = B;
506 }
507 else {
508 Ra = R2;
509 Rb = R1;
510 pa = p2;
511 pb = p1;
512 Sa = B;
513 Sb = A;
514 }
515
516 // nr = normal vector of reference face dotted with axes of incident box.
517 // anr = absolute values of nr.
518 dVector3 normal2,nr,anr;
519 if (code <= 3) {
520 normal2[0] = normal[0];
521 normal2[1] = normal[1];
522 normal2[2] = normal[2];
523 }
524 else {
525 normal2[0] = -normal[0];
526 normal2[1] = -normal[1];
527 normal2[2] = -normal[2];
528 }
529 dMULTIPLY1_331 (nr,Rb,normal2);
530 anr[0] = dFabs (nr[0]);
531 anr[1] = dFabs (nr[1]);
532 anr[2] = dFabs (nr[2]);
533
534 // find the largest compontent of anr: this corresponds to the normal
535 // for the indident face. the other axis numbers of the indicent face
536 // are stored in a1,a2.
537 int lanr,a1,a2;
538 if (anr[1] > anr[0]) {
539 if (anr[1] > anr[2]) {
540 a1 = 0;
541 lanr = 1;
542 a2 = 2;
543 }
544 else {
545 a1 = 0;
546 a2 = 1;
547 lanr = 2;
548 }
549 }
550 else {
551 if (anr[0] > anr[2]) {
552 lanr = 0;
553 a1 = 1;
554 a2 = 2;
555 }
556 else {
557 a1 = 0;
558 a2 = 1;
559 lanr = 2;
560 }
561 }
562
563 // compute center point of incident face, in reference-face coordinates
564 dVector3 center;
565 if (nr[lanr] < 0) {
566 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
567 }
568 else {
569 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
570 }
571
572 // find the normal and non-normal axis numbers of the reference box
573 int codeN,code1,code2;
574 if (code <= 3) codeN = code-1; else codeN = code-4;
575 if (codeN==0) {
576 code1 = 1;
577 code2 = 2;
578 }
579 else if (codeN==1) {
580 code1 = 0;
581 code2 = 2;
582 }
583 else {
584 code1 = 0;
585 code2 = 1;
586 }
587
588 // find the four corners of the incident face, in reference-face coordinates
589 dReal quad[8]; // 2D coordinate of incident face (x,y pairs)
590 dReal c1,c2,m11,m12,m21,m22;
591 c1 = dDOT14 (center,Ra+code1);
592 c2 = dDOT14 (center,Ra+code2);
593 // optimize this? - we have already computed this data above, but it is not
594 // stored in an easy-to-index format. for now it's quicker just to recompute
595 // the four dot products.
596 m11 = dDOT44 (Ra+code1,Rb+a1);
597 m12 = dDOT44 (Ra+code1,Rb+a2);
598 m21 = dDOT44 (Ra+code2,Rb+a1);
599 m22 = dDOT44 (Ra+code2,Rb+a2);
600 {
601 dReal k1 = m11*Sb[a1];
602 dReal k2 = m21*Sb[a1];
603 dReal k3 = m12*Sb[a2];
604 dReal k4 = m22*Sb[a2];
605 quad[0] = c1 - k1 - k3;
606 quad[1] = c2 - k2 - k4;
607 quad[2] = c1 - k1 + k3;
608 quad[3] = c2 - k2 + k4;
609 quad[4] = c1 + k1 + k3;
610 quad[5] = c2 + k2 + k4;
611 quad[6] = c1 + k1 - k3;
612 quad[7] = c2 + k2 - k4;
613 }
614
615 // find the size of the reference face
616 dReal rect[2];
617 rect[0] = Sa[code1];
618 rect[1] = Sa[code2];
619
620 // intersect the incident and reference faces
621 dReal ret[16];
622 int n = intersectRectQuad (rect,quad,ret);
623 if (n < 1) return 0; // this should never happen
624
625 // convert the intersection points into reference-face coordinates,
626 // and compute the contact position and depth for each point. only keep
627 // those points that have a positive (penetrating) depth. delete points in
628 // the 'ret' array as necessary so that 'point' and 'ret' correspond.
629 dReal point[3*8]; // penetrating contact points
630 dReal dep[8]; // depths for those points
631 dReal det1 = dRecip(m11*m22 - m12*m21);
632 m11 *= det1;
633 m12 *= det1;
634 m21 *= det1;
635 m22 *= det1;
636 int cnum = 0; // number of penetrating contact points found
637 for (j=0; j < n; j++) {
638 dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
639 dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
640 for (i=0; i<3; i++) point[cnum*3+i] =
641 center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
642 dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
643 if (dep[cnum] >= 0) {
644 ret[cnum*2] = ret[j*2];
645 ret[cnum*2+1] = ret[j*2+1];
646 cnum++;
647 if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
648 break;
649 }
650 }
651 }
652 if (cnum < 1) {
653 return 0; // this should not happen, yet does at times (demo_plane2d single precision).
654 }
655
656 // we can't generate more contacts than we actually have
657 int maxc = flags & NUMC_MASK;
658 if (maxc > cnum) maxc = cnum;
659 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
660
661 if (cnum <= maxc) {
662 // we have less contacts than we need, so we use them all
663 for (j=0; j < cnum; j++) {
664 dContactGeom *con = CONTACT(contact,skip*j);
665 for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
666 con->depth = dep[j];
667 }
668 }
669 else {
670 dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
671 // we have more contacts than are wanted, some of them must be culled.
672 // find the deepest point, it is always the first contact.
673 int i1 = 0;
674 dReal maxdepth = dep[0];
675 for (i=1; i<cnum; i++) {
676 if (dep[i] > maxdepth) {
677 maxdepth = dep[i];
678 i1 = i;
679 }
680 }
681
682 int iret[8];
683 cullPoints (cnum,ret,maxc,i1,iret);
684
685 for (j=0; j < maxc; j++) {
686 dContactGeom *con = CONTACT(contact,skip*j);
687 for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
688 con->depth = dep[iret[j]];
689 }
690 cnum = maxc;
691 }
692
693 *return_code = code;
694 return cnum;
695}
696
697
698
699int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
700 dContactGeom *contact, int skip)
701{
702 dIASSERT (skip >= (int)sizeof(dContactGeom));
703 dIASSERT (o1->type == dBoxClass);
704 dIASSERT (o2->type == dBoxClass);
705 dIASSERT ((flags & NUMC_MASK) >= 1);
706
707 dVector3 normal;
708 dReal depth;
709 int code;
710 dxBox *b1 = (dxBox*) o1;
711 dxBox *b2 = (dxBox*) o2;
712 int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
713 normal,&depth,&code,flags,contact,skip);
714 for (int i=0; i<num; i++) {
715 CONTACT(contact,i*skip)->normal[0] = -normal[0];
716 CONTACT(contact,i*skip)->normal[1] = -normal[1];
717 CONTACT(contact,i*skip)->normal[2] = -normal[2];
718 CONTACT(contact,i*skip)->g1 = o1;
719 CONTACT(contact,i*skip)->g2 = o2;
720 }
721 return num;
722}
723
724
725int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
726 int flags, dContactGeom *contact, int skip)
727{
728 dIASSERT (skip >= (int)sizeof(dContactGeom));
729 dIASSERT (o1->type == dBoxClass);
730 dIASSERT (o2->type == dPlaneClass);
731 dIASSERT ((flags & NUMC_MASK) >= 1);
732
733 dxBox *box = (dxBox*) o1;
734 dxPlane *plane = (dxPlane*) o2;
735
736 contact->g1 = o1;
737 contact->g2 = o2;
738 int ret = 0;
739
740 //@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
741 const dReal *R = o1->final_posr->R; // rotation of box
742 const dReal *n = plane->p; // normal vector
743
744 // project sides lengths along normal vector, get absolute values
745 dReal Q1 = dDOT14(n,R+0);
746 dReal Q2 = dDOT14(n,R+1);
747 dReal Q3 = dDOT14(n,R+2);
748 dReal A1 = box->side[0] * Q1;
749 dReal A2 = box->side[1] * Q2;
750 dReal A3 = box->side[2] * Q3;
751 dReal B1 = dFabs(A1);
752 dReal B2 = dFabs(A2);
753 dReal B3 = dFabs(A3);
754
755 // early exit test
756 dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->final_posr->pos);
757 if (depth < 0) return 0;
758
759 // find number of contacts requested
760 int maxc = flags & NUMC_MASK;
761 // if (maxc < 1) maxc = 1; // an assertion is made on entry
762 if (maxc > 3) maxc = 3; // not more than 3 contacts per box allowed
763
764 // find deepest point
765 dVector3 p;
766 p[0] = o1->final_posr->pos[0];
767 p[1] = o1->final_posr->pos[1];
768 p[2] = o1->final_posr->pos[2];
769#define FOO(i,op) \
770 p[0] op REAL(0.5)*box->side[i] * R[0+i]; \
771 p[1] op REAL(0.5)*box->side[i] * R[4+i]; \
772 p[2] op REAL(0.5)*box->side[i] * R[8+i];
773#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) }
774 BAR(0,1);
775 BAR(1,2);
776 BAR(2,3);
777#undef FOO
778#undef BAR
779
780 // the deepest point is the first contact point
781 contact->pos[0] = p[0];
782 contact->pos[1] = p[1];
783 contact->pos[2] = p[2];
784 contact->normal[0] = n[0];
785 contact->normal[1] = n[1];
786 contact->normal[2] = n[2];
787 contact->depth = depth;
788 ret = 1; // ret is number of contact points found so far
789 if (maxc == 1) goto done;
790
791 // get the second and third contact points by starting from `p' and going
792 // along the two sides with the smallest projected length.
793
794#define FOO(i,j,op) \
795 CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \
796 CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \
797 CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j];
798#define BAR(ctact,side,sideinc) \
799 depth -= B ## sideinc; \
800 if (depth < 0) goto done; \
801 if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \
802 CONTACT(contact,ctact*skip)->depth = depth; \
803 ret++;
804
805 CONTACT(contact,skip)->normal[0] = n[0];
806 CONTACT(contact,skip)->normal[1] = n[1];
807 CONTACT(contact,skip)->normal[2] = n[2];
808 if (maxc == 3) {
809 CONTACT(contact,2*skip)->normal[0] = n[0];
810 CONTACT(contact,2*skip)->normal[1] = n[1];
811 CONTACT(contact,2*skip)->normal[2] = n[2];
812 }
813
814 if (B1 < B2) {
815 if (B3 < B1) goto use_side_3; else {
816 BAR(1,0,1); // use side 1
817 if (maxc == 2) goto done;
818 if (B2 < B3) goto contact2_2; else goto contact2_3;
819 }
820 }
821 else {
822 if (B3 < B2) {
823 use_side_3: // use side 3
824 BAR(1,2,3);
825 if (maxc == 2) goto done;
826 if (B1 < B2) goto contact2_1; else goto contact2_2;
827 }
828 else {
829 BAR(1,1,2); // use side 2
830 if (maxc == 2) goto done;
831 if (B1 < B3) goto contact2_1; else goto contact2_3;
832 }
833 }
834
835 contact2_1: BAR(2,0,1); goto done;
836 contact2_2: BAR(2,1,2); goto done;
837 contact2_3: BAR(2,2,3); goto done;
838#undef FOO
839#undef BAR
840
841 done:
842 for (int i=0; i<ret; i++) {
843 CONTACT(contact,i*skip)->g1 = o1;
844 CONTACT(contact,i*skip)->g2 = o2;
845 }
846 return ret;
847}
diff --git a/libraries/ode-0.9/ode/src/capsule.cpp b/libraries/ode-0.9/ode/src/capsule.cpp
new file mode 100644
index 0000000..5680baa
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/capsule.cpp
@@ -0,0 +1,369 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::g1 and dContactGeom::g2.
29
30*/
31
32#include <ode/common.h>
33#include <ode/collision.h>
34#include <ode/matrix.h>
35#include <ode/rotation.h>
36#include <ode/odemath.h>
37#include "collision_kernel.h"
38#include "collision_std.h"
39#include "collision_util.h"
40
41#ifdef _MSC_VER
42#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43#endif
44
45//****************************************************************************
46// capped cylinder public API
47
48dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) :
49 dxGeom (space,1)
50{
51 dAASSERT (_radius > 0 && _length > 0);
52 type = dCapsuleClass;
53 radius = _radius;
54 lz = _length;
55}
56
57
58void dxCapsule::computeAABB()
59{
60 const dMatrix3& R = final_posr->R;
61 const dVector3& pos = final_posr->pos;
62
63 dReal xrange = dFabs(R[2] * lz) * REAL(0.5) + radius;
64 dReal yrange = dFabs(R[6] * lz) * REAL(0.5) + radius;
65 dReal zrange = dFabs(R[10] * lz) * REAL(0.5) + radius;
66 aabb[0] = pos[0] - xrange;
67 aabb[1] = pos[0] + xrange;
68 aabb[2] = pos[1] - yrange;
69 aabb[3] = pos[1] + yrange;
70 aabb[4] = pos[2] - zrange;
71 aabb[5] = pos[2] + zrange;
72}
73
74
75dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length)
76{
77 return new dxCapsule (space,radius,length);
78}
79
80
81void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)
82{
83 dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
84 dAASSERT (radius > 0 && length > 0);
85 dxCapsule *c = (dxCapsule*) g;
86 c->radius = radius;
87 c->lz = length;
88 dGeomMoved (g);
89}
90
91
92void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length)
93{
94 dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
95 dxCapsule *c = (dxCapsule*) g;
96 *radius = c->radius;
97 *length = c->lz;
98}
99
100
101dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z)
102{
103 dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
104 g->recomputePosr();
105 dxCapsule *c = (dxCapsule*) g;
106
107 const dReal* R = g->final_posr->R;
108 const dReal* pos = g->final_posr->pos;
109
110 dVector3 a;
111 a[0] = x - pos[0];
112 a[1] = y - pos[1];
113 a[2] = z - pos[2];
114 dReal beta = dDOT14(a,R+2);
115 dReal lz2 = c->lz*REAL(0.5);
116 if (beta < -lz2) beta = -lz2;
117 else if (beta > lz2) beta = lz2;
118 a[0] = c->final_posr->pos[0] + beta*R[0*4+2];
119 a[1] = c->final_posr->pos[1] + beta*R[1*4+2];
120 a[2] = c->final_posr->pos[2] + beta*R[2*4+2];
121 return c->radius -
122 dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2]));
123}
124
125
126
127int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags,
128 dContactGeom *contact, int skip)
129{
130 dIASSERT (skip >= (int)sizeof(dContactGeom));
131 dIASSERT (o1->type == dCapsuleClass);
132 dIASSERT (o2->type == dSphereClass);
133 dIASSERT ((flags & NUMC_MASK) >= 1);
134
135 dxCapsule *ccyl = (dxCapsule*) o1;
136 dxSphere *sphere = (dxSphere*) o2;
137
138 contact->g1 = o1;
139 contact->g2 = o2;
140
141 // find the point on the cylinder axis that is closest to the sphere
142 dReal alpha =
143 o1->final_posr->R[2] * (o2->final_posr->pos[0] - o1->final_posr->pos[0]) +
144 o1->final_posr->R[6] * (o2->final_posr->pos[1] - o1->final_posr->pos[1]) +
145 o1->final_posr->R[10] * (o2->final_posr->pos[2] - o1->final_posr->pos[2]);
146 dReal lz2 = ccyl->lz * REAL(0.5);
147 if (alpha > lz2) alpha = lz2;
148 if (alpha < -lz2) alpha = -lz2;
149
150 // collide the spheres
151 dVector3 p;
152 p[0] = o1->final_posr->pos[0] + alpha * o1->final_posr->R[2];
153 p[1] = o1->final_posr->pos[1] + alpha * o1->final_posr->R[6];
154 p[2] = o1->final_posr->pos[2] + alpha * o1->final_posr->R[10];
155 return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact);
156}
157
158
159int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
160 dContactGeom *contact, int skip)
161{
162 dIASSERT (skip >= (int)sizeof(dContactGeom));
163 dIASSERT (o1->type == dCapsuleClass);
164 dIASSERT (o2->type == dBoxClass);
165 dIASSERT ((flags & NUMC_MASK) >= 1);
166
167 dxCapsule *cyl = (dxCapsule*) o1;
168 dxBox *box = (dxBox*) o2;
169
170 contact->g1 = o1;
171 contact->g2 = o2;
172
173 // get p1,p2 = cylinder axis endpoints, get radius
174 dVector3 p1,p2;
175 dReal clen = cyl->lz * REAL(0.5);
176 p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2];
177 p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6];
178 p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10];
179 p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2];
180 p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6];
181 p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10];
182 dReal radius = cyl->radius;
183
184 // copy out box center, rotation matrix, and side array
185 dReal *c = o2->final_posr->pos;
186 dReal *R = o2->final_posr->R;
187 const dReal *side = box->side;
188
189 // get the closest point between the cylinder axis and the box
190 dVector3 pl,pb;
191 dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
192
193 // generate contact point
194 return dCollideSpheres (pl,radius,pb,0,contact);
195}
196
197
198int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
199 int flags, dContactGeom *contact, int skip)
200{
201 dIASSERT (skip >= (int)sizeof(dContactGeom));
202 dIASSERT (o1->type == dCapsuleClass);
203 dIASSERT (o2->type == dCapsuleClass);
204 dIASSERT ((flags & NUMC_MASK) >= 1);
205
206 int i;
207 const dReal tolerance = REAL(1e-5);
208
209 dxCapsule *cyl1 = (dxCapsule*) o1;
210 dxCapsule *cyl2 = (dxCapsule*) o2;
211
212 contact->g1 = o1;
213 contact->g2 = o2;
214
215 // copy out some variables, for convenience
216 dReal lz1 = cyl1->lz * REAL(0.5);
217 dReal lz2 = cyl2->lz * REAL(0.5);
218 dReal *pos1 = o1->final_posr->pos;
219 dReal *pos2 = o2->final_posr->pos;
220 dReal axis1[3],axis2[3];
221 axis1[0] = o1->final_posr->R[2];
222 axis1[1] = o1->final_posr->R[6];
223 axis1[2] = o1->final_posr->R[10];
224 axis2[0] = o2->final_posr->R[2];
225 axis2[1] = o2->final_posr->R[6];
226 axis2[2] = o2->final_posr->R[10];
227
228 // if the cylinder axes are close to parallel, we'll try to detect up to
229 // two contact points along the body of the cylinder. if we can't find any
230 // points then we'll fall back to the closest-points algorithm. note that
231 // we are not treating this special case for reasons of degeneracy, but
232 // because we want two contact points in some situations. the closet-points
233 // algorithm is robust in all casts, but it can return only one contact.
234
235 dVector3 sphere1,sphere2;
236 dReal a1a2 = dDOT (axis1,axis2);
237 dReal det = REAL(1.0)-a1a2*a1a2;
238 if (det < tolerance) {
239 // the cylinder axes (almost) parallel, so we will generate up to two
240 // contacts. alpha1 and alpha2 (line position parameters) are related by:
241 // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2)
242 // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2)
243 // first compute where the two cylinders overlap in alpha1 space:
244 if (a1a2 < 0) {
245 axis2[0] = -axis2[0];
246 axis2[1] = -axis2[1];
247 axis2[2] = -axis2[2];
248 }
249 dReal q[3];
250 for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
251 dReal k = dDOT (axis1,q);
252 dReal a1lo = -lz1;
253 dReal a1hi = lz1;
254 dReal a2lo = -lz2 - k;
255 dReal a2hi = lz2 - k;
256 dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
257 dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
258 if (lo <= hi) {
259 int num_contacts = flags & NUMC_MASK;
260 if (num_contacts >= 2 && lo < hi) {
261 // generate up to two contacts. if one of those contacts is
262 // not made, fall back on the one-contact strategy.
263 for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
264 for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
265 int n1 = dCollideSpheres (sphere1,cyl1->radius,
266 sphere2,cyl2->radius,contact);
267 if (n1) {
268 for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
269 for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
270 dContactGeom *c2 = CONTACT(contact,skip);
271 int n2 = dCollideSpheres (sphere1,cyl1->radius,
272 sphere2,cyl2->radius, c2);
273 if (n2) {
274 c2->g1 = o1;
275 c2->g2 = o2;
276 return 2;
277 }
278 }
279 }
280
281 // just one contact to generate, so put it in the middle of
282 // the range
283 dReal alpha1 = (lo + hi) * REAL(0.5);
284 dReal alpha2 = alpha1 + k;
285 for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
286 for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
287 return dCollideSpheres (sphere1,cyl1->radius,
288 sphere2,cyl2->radius,contact);
289 }
290 }
291
292 // use the closest point algorithm
293 dVector3 a1,a2,b1,b2;
294 a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1;
295 a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1;
296 a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1;
297 a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1;
298 a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1;
299 a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1;
300 b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2;
301 b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2;
302 b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2;
303 b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2;
304 b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2;
305 b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2;
306
307 dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2);
308 return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
309}
310
311
312int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
313 dContactGeom *contact, int skip)
314{
315 dIASSERT (skip >= (int)sizeof(dContactGeom));
316 dIASSERT (o1->type == dCapsuleClass);
317 dIASSERT (o2->type == dPlaneClass);
318 dIASSERT ((flags & NUMC_MASK) >= 1);
319
320 dxCapsule *ccyl = (dxCapsule*) o1;
321 dxPlane *plane = (dxPlane*) o2;
322
323 // collide the deepest capping sphere with the plane
324 dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
325 dVector3 p;
326 p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
327 p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
328 p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
329
330 dReal k = dDOT (p,plane->p);
331 dReal depth = plane->p[3] - k + ccyl->radius;
332 if (depth < 0) return 0;
333 contact->normal[0] = plane->p[0];
334 contact->normal[1] = plane->p[1];
335 contact->normal[2] = plane->p[2];
336 contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
337 contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
338 contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
339 contact->depth = depth;
340
341 int ncontacts = 1;
342 if ((flags & NUMC_MASK) >= 2) {
343 // collide the other capping sphere with the plane
344 p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
345 p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
346 p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
347
348 k = dDOT (p,plane->p);
349 depth = plane->p[3] - k + ccyl->radius;
350 if (depth >= 0) {
351 dContactGeom *c2 = CONTACT(contact,skip);
352 c2->normal[0] = plane->p[0];
353 c2->normal[1] = plane->p[1];
354 c2->normal[2] = plane->p[2];
355 c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
356 c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
357 c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
358 c2->depth = depth;
359 ncontacts = 2;
360 }
361 }
362
363 for (int i=0; i < ncontacts; i++) {
364 CONTACT(contact,i*skip)->g1 = o1;
365 CONTACT(contact,i*skip)->g2 = o2;
366 }
367 return ncontacts;
368}
369
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp
new file mode 100644
index 0000000..eb14c72
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_box.cpp
@@ -0,0 +1,1007 @@
1/*************************************************************************
2* *
3* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4* All rights reserved. Email: russ@q12.org Web: www.q12.org *
5* *
6* This library is free software; you can redistribute it and/or *
7* modify it under the terms of EITHER: *
8* (1) The GNU Lesser General Public License as published by the Free *
9* Software Foundation; either version 2.1 of the License, or (at *
10* your option) any later version. The text of the GNU Lesser *
11* General Public License is included with this library in the *
12* file LICENSE.TXT. *
13* (2) The BSD-style license that is included with this library in *
14* the file LICENSE-BSD.TXT. *
15* *
16* This library is distributed in the hope that it will be useful, *
17* but WITHOUT ANY WARRANTY; without even the implied warranty of *
18* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20* *
21*************************************************************************/
22
23/*
24 * Cylinder-box collider by Alen Ladavac
25 * Ported to ODE by Nguyen Binh
26 */
27
28#include <ode/collision.h>
29#include <ode/matrix.h>
30#include <ode/rotation.h>
31#include <ode/odemath.h>
32#include "collision_util.h"
33
34static const int MAX_CYLBOX_CLIP_POINTS = 16;
35static const int nCYLINDER_AXIS = 2;
36// Number of segment of cylinder base circle.
37// Must be divisible by 4.
38static const int nCYLINDER_SEGMENT = 8;
39
40#define MAX_FLOAT dInfinity
41
42// Data that passed through the collider's functions
43typedef struct _sCylinderBoxData
44{
45 // cylinder parameters
46 dMatrix3 mCylinderRot;
47 dVector3 vCylinderPos;
48 dVector3 vCylinderAxis;
49 dReal fCylinderRadius;
50 dReal fCylinderSize;
51 dVector3 avCylinderNormals[nCYLINDER_SEGMENT];
52
53 // box parameters
54
55 dMatrix3 mBoxRot;
56 dVector3 vBoxPos;
57 dVector3 vBoxHalfSize;
58 // box vertices array : 8 vertices
59 dVector3 avBoxVertices[8];
60
61 // global collider data
62 dVector3 vDiff;
63 dVector3 vNormal;
64 dReal fBestDepth;
65 dReal fBestrb;
66 dReal fBestrc;
67 int iBestAxis;
68
69 // contact data
70 dVector3 vEp0, vEp1;
71 dReal fDepth0, fDepth1;
72
73 // ODE stuff
74 dGeomID gBox;
75 dGeomID gCylinder;
76 dContactGeom* gContact;
77 int iFlags;
78 int iSkip;
79 int nContacts;
80
81} sCylinderBoxData;
82
83
84// initialize collision data
85void _cldInitCylinderBox(sCylinderBoxData& cData)
86{
87 // get cylinder position, orientation
88 const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
89 dMatrix3Copy(pRotCyc,cData.mCylinderRot);
90
91 const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
92 dVector3Copy(*pPosCyc,cData.vCylinderPos);
93
94 dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
95
96 // get cylinder radius and size
97 dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
98
99 // get box position, orientation, size
100 const dReal* pRotBox = dGeomGetRotation(cData.gBox);
101 dMatrix3Copy(pRotBox,cData.mBoxRot);
102 const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
103 dVector3Copy(*pPosBox,cData.vBoxPos);
104
105 dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
106 cData.vBoxHalfSize[0] *= REAL(0.5);
107 cData.vBoxHalfSize[1] *= REAL(0.5);
108 cData.vBoxHalfSize[2] *= REAL(0.5);
109
110 // vertex 0
111 cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
112 cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
113 cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
114
115 // vertex 1
116 cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
117 cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
118 cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
119
120 // vertex 2
121 cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
122 cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
123 cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
124
125 // vertex 3
126 cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
127 cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
128 cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
129
130 // vertex 4
131 cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
132 cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
133 cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
134
135 // vertex 5
136 cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
137 cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
138 cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
139
140 // vertex 6
141 cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
142 cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
143 cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
144
145 // vertex 7
146 cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
147 cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
148 cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
149
150 // temp index
151 int i = 0;
152 dVector3 vTempBoxVertices[8];
153 // transform vertices in absolute space
154 for(i=0; i < 8; i++)
155 {
156 dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
157 dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
158 }
159
160 // find relative position
161 dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
162 cData.fBestDepth = MAX_FLOAT;
163 cData.vNormal[0] = REAL(0.0);
164 cData.vNormal[1] = REAL(0.0);
165 cData.vNormal[2] = REAL(0.0);
166
167 // calculate basic angle for nCYLINDER_SEGMENT-gon
168 dReal fAngle = M_PI/nCYLINDER_SEGMENT;
169
170 // calculate angle increment
171 dReal fAngleIncrement = fAngle * REAL(2.0);
172
173 // calculate nCYLINDER_SEGMENT-gon points
174 for(i = 0; i < nCYLINDER_SEGMENT; i++)
175 {
176 cData.avCylinderNormals[i][0] = -dCos(fAngle);
177 cData.avCylinderNormals[i][1] = -dSin(fAngle);
178 cData.avCylinderNormals[i][2] = 0;
179
180 fAngle += fAngleIncrement;
181 }
182
183 cData.fBestrb = 0;
184 cData.fBestrc = 0;
185 cData.iBestAxis = 0;
186 cData.nContacts = 0;
187
188}
189
190// test for given separating axis
191int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis )
192{
193 // check length of input normal
194 dReal fL = dVector3Length(vInputNormal);
195 // if not long enough
196 if ( fL < REAL(1e-5) )
197 {
198 // do nothing
199 return 1;
200 }
201
202 // otherwise make it unit for sure
203 dNormalize3(vInputNormal);
204
205 // project box and Cylinder on mAxis
206 dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal);
207
208 dReal frc;
209
210 if (fdot1 > REAL(1.0))
211 {
212 fdot1 = REAL(1.0);
213 frc = dFabs(cData.fCylinderSize*REAL(0.5));
214 }
215
216 // project box and capsule on iAxis
217 frc = dFabs( fdot1 * (cData.fCylinderSize*REAL(0.5))) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
218
219 dVector3 vTemp1;
220 dReal frb = REAL(0.0);
221
222 dMat3GetCol(cData.mBoxRot,0,vTemp1);
223 frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0];
224
225 dMat3GetCol(cData.mBoxRot,1,vTemp1);
226 frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1];
227
228 dMat3GetCol(cData.mBoxRot,2,vTemp1);
229 frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2];
230
231 // project their distance on separating axis
232 dReal fd = dVector3Dot(cData.vDiff,vInputNormal);
233
234 // if they do not overlap exit, we have no intersection
235 if ( dFabs(fd) > frc+frb )
236 {
237 return 0;
238 }
239
240 // get depth
241 dReal fDepth = - dFabs(fd) + (frc+frb);
242
243 // get maximum depth
244 if ( fDepth < cData.fBestDepth )
245 {
246 cData.fBestDepth = fDepth;
247 dVector3Copy(vInputNormal,cData.vNormal);
248 cData.iBestAxis = iAxis;
249 cData.fBestrb = frb;
250 cData.fBestrc = frc;
251
252 // flip normal if interval is wrong faced
253 if (fd > 0)
254 {
255 dVector3Inv(cData.vNormal);
256 }
257 }
258
259 return 1;
260}
261
262
263// check for separation between box edge and cylinder circle edge
264int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
265 const dVector3 &vCenterPoint,
266 const dVector3 &vVx0, const dVector3 &vVx1,
267 int iAxis )
268{
269 // calculate direction of edge
270 dVector3 vDirEdge;
271 dVector3Subtract(vVx1,vVx0,vDirEdge);
272 dNormalize3(vDirEdge);
273 // starting point of edge
274 dVector3 vEStart;
275 dVector3Copy(vVx0,vEStart);;
276
277 // calculate angle cosine between cylinder axis and edge
278 dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);
279
280 // if edge is perpendicular to cylinder axis
281 if(dFabs(fdot2) < REAL(1e-5))
282 {
283 // this can't be separating axis, because edge is parallel to circle plane
284 return 1;
285 }
286
287 // find point of intersection between edge line and circle plane
288 dVector3 vTemp1;
289 dVector3Subtract(vCenterPoint,vEStart,vTemp1);
290 dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
291 dVector3 vpnt;
292 vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2);
293 vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2);
294 vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2);
295
296 // find tangent vector on circle with same center (vCenterPoint) that
297 // touches point of intersection (vpnt)
298 dVector3 vTangent;
299 dVector3Subtract(vCenterPoint,vpnt,vTemp1);
300 dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
301
302 // find vector orthogonal both to tangent and edge direction
303 dVector3 vAxis;
304 dVector3Cross(vTangent,vDirEdge,vAxis);
305
306 // use that vector as separating axis
307 return _cldTestAxis( cData, vAxis, iAxis );
308}
309
310// Test separating axis for collision
311int _cldTestSeparatingAxes(sCylinderBoxData& cData)
312{
313 // reset best axis
314 cData.fBestDepth = MAX_FLOAT;
315 cData.iBestAxis = 0;
316 cData.fBestrb = 0;
317 cData.fBestrc = 0;
318 cData.nContacts = 0;
319
320 dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
321
322 // Epsilon value for checking axis vector length
323 const dReal fEpsilon = REAL(1e-6);
324
325 // axis A0
326 dMat3GetCol(cData.mBoxRot, 0 , vAxis);
327 if (!_cldTestAxis( cData, vAxis, 1 ))
328 {
329 return 0;
330 }
331
332 // axis A1
333 dMat3GetCol(cData.mBoxRot, 1 , vAxis);
334 if (!_cldTestAxis( cData, vAxis, 2 ))
335 {
336 return 0;
337 }
338
339 // axis A2
340 dMat3GetCol(cData.mBoxRot, 2 , vAxis);
341 if (!_cldTestAxis( cData, vAxis, 3 ))
342 {
343 return 0;
344 }
345
346 // axis C - Cylinder Axis
347 //vAxis = vCylinderAxis;
348 dVector3Copy(cData.vCylinderAxis , vAxis);
349 if (!_cldTestAxis( cData, vAxis, 4 ))
350 {
351 return 0;
352 }
353
354 // axis CxA0
355 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 ));
356 dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis);
357 if(dVector3Length2( vAxis ) > fEpsilon )
358 {
359 if (!_cldTestAxis( cData, vAxis, 5 ))
360 {
361 return 0;
362 }
363 }
364
365 // axis CxA1
366 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 ));
367 dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis);
368 if(dVector3Length2( vAxis ) > fEpsilon )
369 {
370 if (!_cldTestAxis( cData, vAxis, 6 ))
371 {
372 return 0;
373 }
374 }
375
376 // axis CxA2
377 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 ));
378 dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis);
379 if(dVector3Length2( vAxis ) > fEpsilon )
380 {
381 if (!_cldTestAxis( cData, vAxis, 7 ))
382 {
383 return 0;
384 }
385 }
386
387 int i = 0;
388 dVector3 vTemp1;
389 dVector3 vTemp2;
390 // here we check box's vertices axis
391 for(i=0; i< 8; i++)
392 {
393 //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos));
394 dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1);
395 dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2);
396 //vAxis = ( vCylinderAxis cross vAxis );
397 dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis);
398 if(dVector3Length2( vAxis ) > fEpsilon )
399 {
400 if (!_cldTestAxis( cData, vAxis, 8 + i ))
401 {
402 return 0;
403 }
404 }
405 }
406
407 // ************************************
408 // this is defined for first 12 axes
409 // normal of plane that contains top circle of cylinder
410 // center of top circle of cylinder
411 dVector3 vcc;
412 vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
413 vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
414 vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
415 // ************************************
416
417 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16))
418 {
419 return 0;
420 }
421
422 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17))
423 {
424 return 0;
425 }
426
427 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18))
428 {
429 return 0;
430 }
431
432 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19))
433 {
434 return 0;
435 }
436
437
438 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20))
439 {
440 return 0;
441 }
442
443 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21))
444 {
445 return 0;
446 }
447
448 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22))
449 {
450 return 0;
451 }
452
453 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23))
454 {
455 return 0;
456 }
457
458 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24))
459 {
460 return 0;
461 }
462
463 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25))
464 {
465 return 0;
466 }
467
468 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26))
469 {
470 return 0;
471 }
472
473 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27))
474 {
475 return 0;
476 }
477
478 // ************************************
479 // this is defined for second 12 axes
480 // normal of plane that contains bottom circle of cylinder
481 // center of bottom circle of cylinder
482 // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5));
483 vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
484 vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
485 vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
486 // ************************************
487
488 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28))
489 {
490 return 0;
491 }
492
493 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29))
494 {
495 return 0;
496 }
497
498 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30))
499 {
500 return 0;
501 }
502
503 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31))
504 {
505 return 0;
506 }
507
508 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32))
509 {
510 return 0;
511 }
512
513 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33))
514 {
515 return 0;
516 }
517
518 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34))
519 {
520 return 0;
521 }
522
523 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35))
524 {
525 return 0;
526 }
527
528 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36))
529 {
530 return 0;
531 }
532
533 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37))
534 {
535 return 0;
536 }
537
538 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38))
539 {
540 return 0;
541 }
542
543 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39))
544 {
545 return 0;
546 }
547
548 return 1;
549}
550
551int _cldClipCylinderToBox(sCylinderBoxData& cData)
552{
553 dIASSERT(cData.nContacts != (cData.iFlags & NUMC_MASK));
554
555 // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
556 dVector3 vN;
557 dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal);
558 vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1;
559 vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1;
560 vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1;
561
562 // normalize that vector
563 dNormalize3(vN);
564
565 // translate cylinder end points by the vector
566 dVector3 vCposTrans;
567 vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius;
568 vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius;
569 vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius;
570
571 cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
572 cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
573 cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
574
575 cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
576 cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
577 cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
578
579 // transform edge points in box space
580 cData.vEp0[0] -= cData.vBoxPos[0];
581 cData.vEp0[1] -= cData.vBoxPos[1];
582 cData.vEp0[2] -= cData.vBoxPos[2];
583
584 cData.vEp1[0] -= cData.vBoxPos[0];
585 cData.vEp1[1] -= cData.vBoxPos[1];
586 cData.vEp1[2] -= cData.vBoxPos[2];
587
588 dVector3 vTemp1;
589 // clip the edge to box
590 dVector4 plPlane;
591 // plane 0 +x
592 dMat3GetCol(cData.mBoxRot,0,vTemp1);
593 dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
594 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
595 {
596 return 0;
597 }
598
599 // plane 1 +y
600 dMat3GetCol(cData.mBoxRot,1,vTemp1);
601 dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
602 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
603 {
604 return 0;
605 }
606
607 // plane 2 +z
608 dMat3GetCol(cData.mBoxRot,2,vTemp1);
609 dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
610 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
611 {
612 return 0;
613 }
614
615 // plane 3 -x
616 dMat3GetCol(cData.mBoxRot,0,vTemp1);
617 dVector3Inv(vTemp1);
618 dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
619 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
620 {
621 return 0;
622 }
623
624 // plane 4 -y
625 dMat3GetCol(cData.mBoxRot,1,vTemp1);
626 dVector3Inv(vTemp1);
627 dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
628 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
629 {
630 return 0;
631 }
632
633 // plane 5 -z
634 dMat3GetCol(cData.mBoxRot,2,vTemp1);
635 dVector3Inv(vTemp1);
636 dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
637 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
638 {
639 return 0;
640 }
641
642 // calculate depths for both contact points
643 cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal);
644 cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal);
645
646 // clamp depths to 0
647 if(cData.fDepth0<0)
648 {
649 cData.fDepth0 = REAL(0.0);
650 }
651
652 if(cData.fDepth1<0)
653 {
654 cData.fDepth1 = REAL(0.0);
655 }
656
657 // back transform edge points from box to absolute space
658 cData.vEp0[0] += cData.vBoxPos[0];
659 cData.vEp0[1] += cData.vBoxPos[1];
660 cData.vEp0[2] += cData.vBoxPos[2];
661
662 cData.vEp1[0] += cData.vBoxPos[0];
663 cData.vEp1[1] += cData.vBoxPos[1];
664 cData.vEp1[2] += cData.vBoxPos[2];
665
666 dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
667 Contact0->depth = cData.fDepth0;
668 dVector3Copy(cData.vNormal,Contact0->normal);
669 dVector3Copy(cData.vEp0,Contact0->pos);
670 Contact0->g1 = cData.gCylinder;
671 Contact0->g2 = cData.gBox;
672 dVector3Inv(Contact0->normal);
673 cData.nContacts++;
674
675 if (cData.nContacts != (cData.iFlags & NUMC_MASK))
676 {
677 dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
678 Contact1->depth = cData.fDepth1;
679 dVector3Copy(cData.vNormal,Contact1->normal);
680 dVector3Copy(cData.vEp1,Contact1->pos);
681 Contact1->g1 = cData.gCylinder;
682 Contact1->g2 = cData.gBox;
683 dVector3Inv(Contact1->normal);
684 cData.nContacts++;
685 }
686
687 return 1;
688}
689
690
691void _cldClipBoxToCylinder(sCylinderBoxData& cData )
692{
693 dIASSERT(cData.nContacts != (cData.iFlags & NUMC_MASK));
694
695 dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
696 // check which circle from cylinder we take for clipping
697 if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) )
698 {
699 // get top circle
700 vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
701 vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
702 vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
703
704 vCylinderCircleNormal_Rel[0] = REAL(0.0);
705 vCylinderCircleNormal_Rel[1] = REAL(0.0);
706 vCylinderCircleNormal_Rel[2] = REAL(0.0);
707 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
708 }
709 else
710 {
711 // get bottom circle
712 vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
713 vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
714 vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
715
716 vCylinderCircleNormal_Rel[0] = REAL(0.0);
717 vCylinderCircleNormal_Rel[1] = REAL(0.0);
718 vCylinderCircleNormal_Rel[2] = REAL(0.0);
719 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
720 }
721
722 // vNr is normal in Box frame, pointing from Cylinder to Box
723 dVector3 vNr;
724 dMatrix3 mBoxInv;
725
726 // Find a way to use quaternion
727 dMatrix3Inv(cData.mBoxRot,mBoxInv);
728 dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr);
729
730 dVector3 vAbsNormal;
731
732 vAbsNormal[0] = dFabs( vNr[0] );
733 vAbsNormal[1] = dFabs( vNr[1] );
734 vAbsNormal[2] = dFabs( vNr[2] );
735
736 // find which face in box is closest to cylinder
737 int iB0, iB1, iB2;
738
739 // Different from Croteam's code
740 if (vAbsNormal[1] > vAbsNormal[0])
741 {
742 // 1 > 0
743 if (vAbsNormal[0]> vAbsNormal[2])
744 {
745 // 0 > 2 -> 1 > 0 >2
746 iB0 = 1; iB1 = 0; iB2 = 2;
747 }
748 else
749 {
750 // 2 > 0-> Must compare 1 and 2
751 if (vAbsNormal[1] > vAbsNormal[2])
752 {
753 // 1 > 2 -> 1 > 2 > 0
754 iB0 = 1; iB1 = 2; iB2 = 0;
755 }
756 else
757 {
758 // 2 > 1 -> 2 > 1 > 0;
759 iB0 = 2; iB1 = 1; iB2 = 0;
760 }
761 }
762 }
763 else
764 {
765 // 0 > 1
766 if (vAbsNormal[1] > vAbsNormal[2])
767 {
768 // 1 > 2 -> 0 > 1 > 2
769 iB0 = 0; iB1 = 1; iB2 = 2;
770 }
771 else
772 {
773 // 2 > 1 -> Must compare 0 and 2
774 if (vAbsNormal[0] > vAbsNormal[2])
775 {
776 // 0 > 2 -> 0 > 2 > 1;
777 iB0 = 0; iB1 = 2; iB2 = 1;
778 }
779 else
780 {
781 // 2 > 0 -> 2 > 0 > 1;
782 iB0 = 2; iB1 = 0; iB2 = 1;
783 }
784 }
785 }
786
787 dVector3 vCenter;
788 // find center of box polygon
789 dVector3 vTemp;
790 if (vNr[iB0] > 0)
791 {
792 dMat3GetCol(cData.mBoxRot,iB0,vTemp);
793 vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0];
794 vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1];
795 vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2];
796 }
797 else
798 {
799 dMat3GetCol(cData.mBoxRot,iB0,vTemp);
800 vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0];
801 vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1];
802 vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2];
803 }
804
805 // find the vertices of box polygon
806 dVector3 avPoints[4];
807 dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
808 dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];
809
810 int i=0;
811 for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++)
812 {
813 avTempArray1[i][0] = REAL(0.0);
814 avTempArray1[i][1] = REAL(0.0);
815 avTempArray1[i][2] = REAL(0.0);
816
817 avTempArray2[i][0] = REAL(0.0);
818 avTempArray2[i][1] = REAL(0.0);
819 avTempArray2[i][2] = REAL(0.0);
820 }
821
822 dVector3 vAxis1, vAxis2;
823
824 dMat3GetCol(cData.mBoxRot,iB1,vAxis1);
825 dMat3GetCol(cData.mBoxRot,iB2,vAxis2);
826
827 avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
828 avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
829 avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
830
831 avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
832 avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
833 avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
834
835 avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
836 avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
837 avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
838
839 avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
840 avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
841 avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
842
843 // transform box points to space of cylinder circle
844 dMatrix3 mCylinderInv;
845 dMatrix3Inv(cData.mCylinderRot,mCylinderInv);
846
847 for(i=0; i<4; i++)
848 {
849 dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
850 dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
851 }
852
853 int iTmpCounter1 = 0;
854 int iTmpCounter2 = 0;
855 dVector4 plPlane;
856
857 // plane of cylinder that contains circle for intersection
858 dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
859 dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);
860
861
862 // Body of base circle of Cylinder
863 int nCircleSegment = 0;
864 for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++)
865 {
866 dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);
867
868 if (0 == (nCircleSegment % 2))
869 {
870 dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
871 }
872 else
873 {
874 dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
875 }
876
877 dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS );
878 dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS );
879 }
880
881 // back transform clipped points to absolute space
882 dReal ftmpdot;
883 dReal fTempDepth;
884 dVector3 vPoint;
885
886 if (nCircleSegment % 2)
887 {
888 for( i=0; i<iTmpCounter2; i++)
889 {
890 dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]);
891 vPoint[0] += vCylinderCirclePos[0];
892 vPoint[1] += vCylinderCirclePos[1];
893 vPoint[2] += vCylinderCirclePos[2];
894
895 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
896 ftmpdot = dVector3Dot(vTemp, cData.vNormal);
897 fTempDepth = cData.fBestrc - ftmpdot;
898 // Depth must be positive
899 if (fTempDepth > REAL(0.0))
900 {
901 // generate contacts
902 dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
903 Contact0->depth = fTempDepth;
904 dVector3Copy(cData.vNormal,Contact0->normal);
905 dVector3Copy(vPoint,Contact0->pos);
906 Contact0->g1 = cData.gCylinder;
907 Contact0->g2 = cData.gBox;
908 dVector3Inv(Contact0->normal);
909 cData.nContacts++;
910
911 if (cData.nContacts == (cData.iFlags & NUMC_MASK))
912 {
913 break;
914 }
915 }
916 }
917 }
918 else
919 {
920 for( i=0; i<iTmpCounter1; i++)
921 {
922 dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
923 vPoint[0] += vCylinderCirclePos[0];
924 vPoint[1] += vCylinderCirclePos[1];
925 vPoint[2] += vCylinderCirclePos[2];
926
927 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
928 ftmpdot = dVector3Dot(vTemp, cData.vNormal);
929 fTempDepth = cData.fBestrc - ftmpdot;
930 // Depth must be positive
931 if (fTempDepth > REAL(0.0))
932 {
933 // generate contacts
934 dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
935 Contact0->depth = fTempDepth;
936 dVector3Copy(cData.vNormal,Contact0->normal);
937 dVector3Copy(vPoint,Contact0->pos);
938 Contact0->g1 = cData.gCylinder;
939 Contact0->g2 = cData.gBox;
940 dVector3Inv(Contact0->normal);
941 cData.nContacts++;
942
943 if (cData.nContacts == (cData.iFlags & NUMC_MASK))
944 {
945 break;
946 }
947 }
948 }
949 }
950}
951
952
953// Cylinder - Box by CroTeam
954// Ported by Nguyen Binh
955int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
956{
957 dIASSERT (skip >= (int)sizeof(dContactGeom));
958 dIASSERT (o1->type == dCylinderClass);
959 dIASSERT (o2->type == dBoxClass);
960 dIASSERT ((flags & NUMC_MASK) >= 1);
961
962 sCylinderBoxData cData;
963
964 // Assign ODE stuff
965 cData.gCylinder = o1;
966 cData.gBox = o2;
967 cData.iFlags = flags;
968 cData.iSkip = skip;
969 cData.gContact = contact;
970
971 // initialize collider
972 _cldInitCylinderBox( cData );
973
974 // do intersection test and find best separating axis
975 if(!_cldTestSeparatingAxes( cData ) )
976 {
977 // if not found do nothing
978 return 0;
979 }
980
981 // if best separation axis is not found
982 if ( cData.iBestAxis == 0 )
983 {
984 // this should not happen (we should already exit in that case)
985 dIASSERT(0);
986 // do nothing
987 return 0;
988 }
989
990 dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis);
991 // choose which clipping method are we going to apply
992 if (dFabs(fdot) < REAL(0.9) )
993 {
994 // clip cylinder over box
995 if(!_cldClipCylinderToBox(cData))
996 {
997 return 0;
998 }
999 }
1000 else
1001 {
1002 _cldClipBoxToCylinder(cData);
1003 }
1004
1005 return cData.nContacts;
1006}
1007
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp
new file mode 100644
index 0000000..f85701d
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_plane.cpp
@@ -0,0 +1,254 @@
1/*************************************************************************
2* *
3* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4* All rights reserved. Email: russ@q12.org Web: www.q12.org *
5* *
6* This library is free software; you can redistribute it and/or *
7* modify it under the terms of EITHER: *
8* (1) The GNU Lesser General Public License as published by the Free *
9* Software Foundation; either version 2.1 of the License, or (at *
10* your option) any later version. The text of the GNU Lesser *
11* General Public License is included with this library in the *
12* file LICENSE.TXT. *
13* (2) The BSD-style license that is included with this library in *
14* the file LICENSE-BSD.TXT. *
15* *
16* This library is distributed in the hope that it will be useful, *
17* but WITHOUT ANY WARRANTY; without even the implied warranty of *
18* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20* *
21*************************************************************************/
22
23
24/*
25 * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de )
26 *
27 * This testing basically comes down to testing the intersection
28 * of the cylinder caps (discs) with the plane.
29 *
30 */
31
32#include <ode/collision.h>
33#include <ode/matrix.h>
34#include <ode/rotation.h>
35#include <ode/odemath.h>
36#include <ode/objects.h>
37
38#include "collision_kernel.h" // for dxGeom
39#include "collision_util.h"
40
41
42int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
43{
44 dIASSERT (skip >= (int)sizeof(dContactGeom));
45 dIASSERT (Cylinder->type == dCylinderClass);
46 dIASSERT (Plane->type == dPlaneClass);
47 dIASSERT ((flags & NUMC_MASK) >= 1);
48
49 int GeomCount = 0; // count of used contactgeoms
50
51#ifdef dSINGLE
52 const dReal toleranz = REAL(0.0001);
53#endif
54#ifdef dDOUBLE
55 const dReal toleranz = REAL(0.0000001);
56#endif
57
58 // Get the properties of the cylinder (length+radius)
59 dReal radius, length;
60 dGeomCylinderGetParams(Cylinder, &radius, &length);
61 dVector3 &cylpos = Cylinder->final_posr->pos;
62 // and the plane
63 dVector4 planevec;
64 dGeomPlaneGetParams(Plane, planevec);
65 dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
66 dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
67
68 dVector3 G1Pos1, G1Pos2, vDir1;
69 vDir1[0] = Cylinder->final_posr->R[2];
70 vDir1[1] = Cylinder->final_posr->R[6];
71 vDir1[2] = Cylinder->final_posr->R[10];
72
73 dReal s;
74 s = length * REAL(0.5);
75 G1Pos2[0] = vDir1[0] * s + cylpos[0];
76 G1Pos2[1] = vDir1[1] * s + cylpos[1];
77 G1Pos2[2] = vDir1[2] * s + cylpos[2];
78
79 G1Pos1[0] = vDir1[0] * -s + cylpos[0];
80 G1Pos1[1] = vDir1[1] * -s + cylpos[1];
81 G1Pos1[2] = vDir1[2] * -s + cylpos[2];
82
83 dVector3 C;
84
85 // parallel-check
86 s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
87 if(s < 0)
88 s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
89 else
90 s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
91 if(s < toleranz && s > (-toleranz))
92 {
93 // discs are parallel to the plane
94
95 // 1.compute if, and where contacts are
96 dVector3 P;
97 s = planevec[3] - dVector3Dot(planevec, G1Pos1);
98 dReal t;
99 t = planevec[3] - dVector3Dot(planevec, G1Pos2);
100 if(s >= t) // s == t does never happen,
101 {
102 if(s >= 0)
103 {
104 // 1. Disc
105 dVector3Copy(G1Pos1, P);
106 }
107 else
108 return GeomCount; // no contacts
109 }
110 else
111 {
112 if(t >= 0)
113 {
114 // 2. Disc
115 dVector3Copy(G1Pos2, P);
116 }
117 else
118 return GeomCount; // no contacts
119 }
120
121 // 2. generate a coordinate-system on the disc
122 dVector3 V1, V2;
123 if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
124 {
125 // not x-axis
126 V1[0] = vDir1[0] + REAL(1.0); // random value
127 V1[1] = vDir1[1];
128 V1[2] = vDir1[2];
129 }
130 else
131 {
132 // maybe x-axis
133 V1[0] = vDir1[0];
134 V1[1] = vDir1[1] + REAL(1.0); // random value
135 V1[2] = vDir1[2];
136 }
137 // V1 is now another direction than vDir1
138 // Cross-product
139 dVector3Cross(V1, vDir1, V2);
140 // make unit V2
141 t = dVector3Length(V2);
142 t = radius / t;
143 dVector3Scale(V2, t);
144 // cross again
145 dVector3Cross(V2, vDir1, V1);
146 // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
147 // V1 = first axis
148 // V2 = second axis
149
150 // 3. generate contactpoints
151
152 // Potential contact 1
153 dVector3Add(P, V1, contact->pos);
154 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
155 if(contact->depth > 0)
156 {
157 dVector3Copy(PlaneNormal, contact->normal);
158 contact->g1 = Cylinder;
159 contact->g2 = Plane;
160 GeomCount++;
161 if( GeomCount >= (flags & NUMC_MASK))
162 return GeomCount; // enough contactgeoms
163 contact = (dContactGeom *)((char *)contact + skip);
164 }
165
166 // Potential contact 2
167 dVector3Subtract(P, V1, contact->pos);
168 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
169 if(contact->depth > 0)
170 {
171 dVector3Copy(PlaneNormal, contact->normal);
172 contact->g1 = Cylinder;
173 contact->g2 = Plane;
174 GeomCount++;
175 if( GeomCount >= (flags & NUMC_MASK))
176 return GeomCount; // enough contactgeoms
177 contact = (dContactGeom *)((char *)contact + skip);
178 }
179
180 // Potential contact 3
181 dVector3Add(P, V2, contact->pos);
182 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
183 if(contact->depth > 0)
184 {
185 dVector3Copy(PlaneNormal, contact->normal);
186 contact->g1 = Cylinder;
187 contact->g2 = Plane;
188 GeomCount++;
189 if( GeomCount >= (flags & NUMC_MASK))
190 return GeomCount; // enough contactgeoms
191 contact = (dContactGeom *)((char *)contact + skip);
192 }
193
194 // Potential contact 4
195 dVector3Subtract(P, V2, contact->pos);
196 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
197 if(contact->depth > 0)
198 {
199 dVector3Copy(PlaneNormal, contact->normal);
200 contact->g1 = Cylinder;
201 contact->g2 = Plane;
202 GeomCount++;
203 if( GeomCount >= (flags & NUMC_MASK))
204 return GeomCount; // enough contactgeoms
205 contact = (dContactGeom *)((char *)contact + skip);
206 }
207 }
208 else
209 {
210 dReal t = dVector3Dot(PlaneNormal, vDir1);
211 C[0] = vDir1[0] * t - PlaneNormal[0];
212 C[1] = vDir1[1] * t - PlaneNormal[1];
213 C[2] = vDir1[2] * t - PlaneNormal[2];
214 s = dVector3Length(C);
215 // move C onto the circle
216 s = radius / s;
217 dVector3Scale(C, s);
218
219 // deepest point of disc 1
220 dVector3Add(C, G1Pos1, contact->pos);
221
222 // depth of the deepest point
223 contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
224 if(contact->depth >= 0)
225 {
226 dVector3Copy(PlaneNormal, contact->normal);
227 contact->g1 = Cylinder;
228 contact->g2 = Plane;
229 GeomCount++;
230 if( GeomCount >= (flags & NUMC_MASK))
231 return GeomCount; // enough contactgeoms
232 contact = (dContactGeom *)((char *)contact + skip);
233 }
234
235 // C is still computed
236
237 // deepest point of disc 2
238 dVector3Add(C, G1Pos2, contact->pos);
239
240 // depth of the deepest point
241 contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
242 if(contact->depth >= 0)
243 {
244 dVector3Copy(PlaneNormal, contact->normal);
245 contact->g1 = Cylinder;
246 contact->g2 = Plane;
247 GeomCount++;
248 if( GeomCount >= (flags & NUMC_MASK))
249 return GeomCount; // enough contactgeoms
250 contact = (dContactGeom *)((char *)contact + skip);
251 }
252 }
253 return GeomCount;
254}
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp
new file mode 100644
index 0000000..964c531
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_sphere.cpp
@@ -0,0 +1,264 @@
1/*************************************************************************
2* *
3* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4* All rights reserved. Email: russ@q12.org Web: www.q12.org *
5* *
6* This library is free software; you can redistribute it and/or *
7* modify it under the terms of EITHER: *
8* (1) The GNU Lesser General Public License as published by the Free *
9* Software Foundation; either version 2.1 of the License, or (at *
10* your option) any later version. The text of the GNU Lesser *
11* General Public License is included with this library in the *
12* file LICENSE.TXT. *
13* (2) The BSD-style license that is included with this library in *
14* the file LICENSE-BSD.TXT. *
15* *
16* This library is distributed in the hope that it will be useful, *
17* but WITHOUT ANY WARRANTY; without even the implied warranty of *
18* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20* *
21*************************************************************************/
22
23
24/*******************************************************************
25 * *
26 * cylinder-sphere collider by Christoph Beyer (boernerb@web.de) *
27 * *
28 * In Cylinder/Sphere-collisions, there are three possibilies: *
29 * 1. collision with the cylinder's nappe *
30 * 2. collision with one of the cylinder's disc *
31 * 3. collision with one of the disc's border *
32 * *
33 * This collider computes two distances (s, t) and based on them, *
34 * it decides, which collision we have. *
35 * This collider always generates 1 (or 0, if we have no collison) *
36 * contacts. *
37 * It is able to "separate" cylinder and sphere in all *
38 * configurations, but it never pays attention to velocity. *
39 * So, in extrem situations, "tunneling-effect" is possible. *
40 * *
41 *******************************************************************/
42
43#include <ode/collision.h>
44#include <ode/matrix.h>
45#include <ode/rotation.h>
46#include <ode/odemath.h>
47#include <ode/objects.h>
48#include "collision_kernel.h" // for dxGeom
49#include "collision_util.h"
50
51int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
52 int flags, dContactGeom *contact, int skip)
53{
54 dIASSERT (skip >= (int)sizeof(dContactGeom));
55 dIASSERT (Cylinder->type == dCylinderClass);
56 dIASSERT (Sphere->type == dSphereClass);
57 dIASSERT ((flags & NUMC_MASK) >= 1);
58
59 unsigned char* pContactData = (unsigned char*)contact;
60 int GeomCount = 0; // count of used contacts
61
62#ifdef dSINGLE
63 const dReal toleranz = REAL(0.0001);
64#endif
65#ifdef dDOUBLE
66 const dReal toleranz = REAL(0.0000001);
67#endif
68
69 // get the data from the geoms
70 dReal radius, length;
71 dGeomCylinderGetParams(Cylinder, &radius, &length);
72 dVector3 &cylpos = Cylinder->final_posr->pos;
73 const dReal* pfRot1 = dGeomGetRotation(Cylinder);
74
75 dReal radius2;
76 radius2 = dGeomSphereGetRadius(Sphere);
77 const dReal* SpherePos = dGeomGetPosition(Sphere);
78
79 // G1Pos1 is the middle of the first disc
80 // G1Pos2 is the middle of the second disc
81 // vDir1 is the unit direction of the cylinderaxis
82 dVector3 G1Pos1, G1Pos2, vDir1;
83 vDir1[0] = Cylinder->final_posr->R[2];
84 vDir1[1] = Cylinder->final_posr->R[6];
85 vDir1[2] = Cylinder->final_posr->R[10];
86
87 dReal s;
88 s = length * REAL(0.5); // just a precomputed factor
89 G1Pos2[0] = vDir1[0] * s + cylpos[0];
90 G1Pos2[1] = vDir1[1] * s + cylpos[1];
91 G1Pos2[2] = vDir1[2] * s + cylpos[2];
92
93 G1Pos1[0] = vDir1[0] * -s + cylpos[0];
94 G1Pos1[1] = vDir1[1] * -s + cylpos[1];
95 G1Pos1[2] = vDir1[2] * -s + cylpos[2];
96
97 dVector3 C;
98 dReal t;
99 // Step 1: compute the two distances 's' and 't'
100 // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
101 s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
102 if(s < (-radius2) || s > (length + radius2) )
103 {
104 // Sphere is too far away from the discs
105 // no collision
106 return 0;
107 }
108
109 // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
110 C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
111 C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
112 C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
113 // t is the distance from the Sphere-middle to the cylinder-axis!
114 t = dVector3Length(C);
115 if(t > (radius + radius2) )
116 {
117 // Sphere is too far away from the cylinder axis!
118 // no collision
119 return 0;
120 }
121
122 // decide which kind of collision we have:
123 if(t > radius && (s < 0 || s > length) )
124 {
125 // 3. collision
126 if(s <= 0)
127 {
128 contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
129 if(contact->depth < 0)
130 {
131 // no collision!
132 return 0;
133 }
134 contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
135 contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
136 contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
137 contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
138 contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
139 contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
140 contact->g1 = Cylinder;
141 contact->g2 = Sphere;
142 GeomCount++;
143 return GeomCount;
144 }
145 else
146 {
147 // now s is bigger than length here!
148 contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
149 if(contact->depth < 0)
150 {
151 // no collision!
152 return 0;
153 }
154 contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
155 contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
156 contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
157 contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
158 contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
159 contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
160 contact->g1 = Cylinder;
161 contact->g2 = Sphere;
162 GeomCount++;
163 return GeomCount;
164 }
165 }
166 else if( (radius - t) <= s && (radius - t) <= (length - s) )
167 {
168 // 1. collsision
169 if(t > (radius2 + toleranz))
170 {
171 // cylinder-axis is outside the sphere
172 contact->depth = (radius2 + radius) - t;
173 if(contact->depth < 0)
174 {
175 // should never happen, but just for safeness
176 return 0;
177 }
178 else
179 {
180 C[0] /= t;
181 C[1] /= t;
182 C[2] /= t;
183 contact->pos[0] = C[0] * radius2 + SpherePos[0];
184 contact->pos[1] = C[1] * radius2 + SpherePos[1];
185 contact->pos[2] = C[2] * radius2 + SpherePos[2];
186 contact->normal[0] = C[0];
187 contact->normal[1] = C[1];
188 contact->normal[2] = C[2];
189 contact->g1 = Cylinder;
190 contact->g2 = Sphere;
191 GeomCount++;
192 return GeomCount;
193 }
194 }
195 else
196 {
197 // cylinder-axis is outside of the sphere
198 contact->depth = (radius2 + radius) - t;
199 if(contact->depth < 0)
200 {
201 // should never happen, but just for safeness
202 return 0;
203 }
204 else
205 {
206 contact->pos[0] = C[0] + SpherePos[0];
207 contact->pos[1] = C[1] + SpherePos[1];
208 contact->pos[2] = C[2] + SpherePos[2];
209 contact->normal[0] = C[0] / t;
210 contact->normal[1] = C[1] / t;
211 contact->normal[2] = C[2] / t;
212 contact->g1 = Cylinder;
213 contact->g2 = Sphere;
214 GeomCount++;
215 return GeomCount;
216 }
217 }
218 }
219 else
220 {
221 // 2. collision
222 if(s <= (length * REAL(0.5)) )
223 {
224 // collsision with the first disc
225 contact->depth = s + radius2;
226 if(contact->depth < 0)
227 {
228 // should never happen, but just for safeness
229 return 0;
230 }
231 contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
232 contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
233 contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
234 contact->normal[0] = vDir1[0];
235 contact->normal[1] = vDir1[1];
236 contact->normal[2] = vDir1[2];
237 contact->g1 = Cylinder;
238 contact->g2 = Sphere;
239 GeomCount++;
240 return GeomCount;
241 }
242 else
243 {
244 // collsision with the second disc
245 contact->depth = (radius2 + length - s);
246 if(contact->depth < 0)
247 {
248 // should never happen, but just for safeness
249 return 0;
250 }
251 contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
252 contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
253 contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
254 contact->normal[0] = -vDir1[0];
255 contact->normal[1] = -vDir1[1];
256 contact->normal[2] = -vDir1[2];
257 contact->g1 = Cylinder;
258 contact->g2 = Sphere;
259 GeomCount++;
260 return GeomCount;
261 }
262 }
263 return GeomCount;
264}
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp
new file mode 100644
index 0000000..342be04
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp
@@ -0,0 +1,1145 @@
1/*************************************************************************
2* *
3* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4* All rights reserved. Email: russ@q12.org Web: www.q12.org *
5* *
6* This library is free software; you can redistribute it and/or *
7* modify it under the terms of EITHER: *
8* (1) The GNU Lesser General Public License as published by the Free *
9* Software Foundation; either version 2.1 of the License, or (at *
10* your option) any later version. The text of the GNU Lesser *
11* General Public License is included with this library in the *
12* file LICENSE.TXT. *
13* (2) The BSD-style license that is included with this library in *
14* the file LICENSE-BSD.TXT. *
15* *
16* This library is distributed in the hope that it will be useful, *
17* but WITHOUT ANY WARRANTY; without even the implied warranty of *
18* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20* *
21*************************************************************************/
22
23/*
24 * Cylinder-trimesh collider by Alen Ladavac
25 * Ported to ODE by Nguyen Binh
26 */
27
28
29#include <ode/collision.h>
30#include <ode/matrix.h>
31#include <ode/rotation.h>
32#include <ode/odemath.h>
33#include "collision_util.h"
34
35#define TRIMESH_INTERNAL
36#include "collision_trimesh_internal.h"
37
38#define MAX_REAL dInfinity
39static const int nCYLINDER_AXIS = 2;
40static const int nCYLINDER_CIRCLE_SEGMENTS = 8;
41static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12;
42
43#define OPTIMIZE_CONTACTS 1
44
45// Local contacts data
46typedef struct _sLocalContactData
47{
48 dVector3 vPos;
49 dVector3 vNormal;
50 dReal fDepth;
51 int triIndex;
52 int nFlags; // 0 = filtered out, 1 = OK
53}sLocalContactData;
54
55typedef struct _sCylinderTrimeshColliderData
56{
57 // cylinder data
58 dMatrix3 mCylinderRot;
59 dQuaternion qCylinderRot;
60 dQuaternion qInvCylinderRot;
61 dVector3 vCylinderPos;
62 dVector3 vCylinderAxis;
63 dReal fCylinderRadius;
64 dReal fCylinderSize;
65 dVector3 avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS];
66
67 // mesh data
68 dQuaternion qTrimeshRot;
69 dQuaternion qInvTrimeshRot;
70 dMatrix3 mTrimeshRot;
71 dVector3 vTrimeshPos;
72
73 // global collider data
74 dVector3 vBestPoint;
75 dReal fBestDepth;
76 dReal fBestCenter;
77 dReal fBestrt;
78 int iBestAxis;
79 dVector3 vContactNormal;
80 dVector3 vNormal;
81 dVector3 vE0;
82 dVector3 vE1;
83 dVector3 vE2;
84
85 // ODE stuff
86 dGeomID gCylinder;
87 dxTriMesh* gTrimesh;
88 dContactGeom* gContact;
89 int iFlags;
90 int iSkip;
91 int nContacts;// = 0;
92 sLocalContactData* gLocalContacts;
93} sCylinderTrimeshColliderData;
94
95// Short type name
96typedef sCylinderTrimeshColliderData sData;
97
98// Use to classify contacts to be "near" in position
99static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
100// Use to classify contacts to be "near" in normal direction
101static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4
102
103// If this two contact can be classified as "near"
104inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
105{
106 int bPosNear = 0;
107 int bSameDir = 0;
108 dVector3 vDiff;
109
110 // First check if they are "near" in position
111 dVector3Subtract(c1.vPos,c2.vPos,vDiff);
112 if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
113 &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
114 &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
115 {
116 bPosNear = 1;
117 }
118
119 // Second check if they are "near" in normal direction
120 dVector3Subtract(c1.vNormal,c2.vNormal,vDiff);
121 if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
122 &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
123 &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
124 {
125 bSameDir = 1;
126 }
127
128 // Will be "near" if position and normal direction are "near"
129 return (bPosNear && bSameDir);
130}
131
132inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
133{
134 // The not better will be throw away
135 // You can change the selection criteria here
136 return (c1.fDepth > c2.fDepth);
137}
138
139// iterate through gLocalContacts and filtered out "near contact"
140inline void _OptimizeLocalContacts(sData& cData)
141{
142 int nContacts = cData.nContacts;
143
144 for (int i = 0; i < nContacts-1; i++)
145 {
146 for (int j = i+1; j < nContacts; j++)
147 {
148 if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j]))
149 {
150 // If they are seem to be the same then filtered
151 // out the least penetrate one
152 if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i]))
153 {
154 cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact
155 }
156 else
157 {
158 cData.gLocalContacts[j].nFlags = 0; // filtered 2nd contact
159 }
160
161 // NOTE
162 // There is other way is to add two depth together but
163 // it not work so well. Why???
164 }
165 }
166 }
167}
168
169inline int _ProcessLocalContacts(sData& cData)
170{
171 if (cData.nContacts == 0)
172 {
173 return 0;
174 }
175
176#ifdef OPTIMIZE_CONTACTS
177 if (cData.nContacts > 1 && !(cData.iFlags & CONTACTS_UNIMPORTANT))
178 {
179 // Can be optimized...
180 _OptimizeLocalContacts(cData);
181 }
182#endif
183
184 int iContact = 0;
185 dContactGeom* Contact = 0;
186
187 int nFinalContact = 0;
188
189 for (iContact = 0; iContact < cData.nContacts; iContact ++)
190 {
191 if (1 == cData.gLocalContacts[iContact].nFlags)
192 {
193 Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip);
194 Contact->depth = cData.gLocalContacts[iContact].fDepth;
195 dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal);
196 dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos);
197 Contact->g1 = cData.gCylinder;
198 Contact->g2 = cData.gTrimesh;
199 Contact->side2 = cData.gLocalContacts[iContact].triIndex;
200 dVector3Inv(Contact->normal);
201
202 nFinalContact++;
203 }
204 }
205 // debug
206 //if (nFinalContact != cData.nContacts)
207 //{
208 // printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact);
209 //}
210
211 return nFinalContact;
212}
213
214
215bool _cldTestAxis(sData& cData,
216 const dVector3 &v0,
217 const dVector3 &v1,
218 const dVector3 &v2,
219 dVector3& vAxis,
220 int iAxis,
221 bool bNoFlip = false)
222{
223
224 // calculate length of separating axis vector
225 dReal fL = dVector3Length(vAxis);
226 // if not long enough
227 if ( fL < REAL(1e-5) )
228 {
229 // do nothing
230 return true;
231 }
232
233 // otherwise normalize it
234 vAxis[0] /= fL;
235 vAxis[1] /= fL;
236 vAxis[2] /= fL;
237
238 dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis);
239 // project capsule on vAxis
240 dReal frc;
241
242 if (dFabs(fdot1) > REAL(1.0) )
243 {
244// fdot1 = REAL(1.0);
245 frc = dFabs(cData.fCylinderSize* REAL(0.5));
246 }
247 else
248 {
249 frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1)
250 + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
251 }
252
253 dVector3 vV0;
254 dVector3Subtract(v0,cData.vCylinderPos,vV0);
255 dVector3 vV1;
256 dVector3Subtract(v1,cData.vCylinderPos,vV1);
257 dVector3 vV2;
258 dVector3Subtract(v2,cData.vCylinderPos,vV2);
259
260 // project triangle on vAxis
261 dReal afv[3];
262 afv[0] = dVector3Dot( vV0 , vAxis );
263 afv[1] = dVector3Dot( vV1 , vAxis );
264 afv[2] = dVector3Dot( vV2 , vAxis );
265
266 dReal fMin = MAX_REAL;
267 dReal fMax = -MAX_REAL;
268
269 // for each vertex
270 for(int i = 0; i < 3; i++)
271 {
272 // find minimum
273 if (afv[i]<fMin)
274 {
275 fMin = afv[i];
276 }
277 // find maximum
278 if (afv[i]>fMax)
279 {
280 fMax = afv[i];
281 }
282 }
283
284 // find capsule's center of interval on axis
285 dReal fCenter = (fMin+fMax)* REAL(0.5);
286 // calculate triangles halfinterval
287 dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
288
289 // if they do not overlap,
290 if( dFabs(fCenter) > (frc+fTriangleRadius) )
291 {
292 // exit, we have no intersection
293 return false;
294 }
295
296 // calculate depth
297 dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) );
298
299 // if greater then best found so far
300 if ( fDepth < cData.fBestDepth )
301 {
302 // remember depth
303 cData.fBestDepth = fDepth;
304 cData.fBestCenter = fCenter;
305 cData.fBestrt = frc;
306 dVector3Copy(vAxis,cData.vContactNormal);
307 cData.iBestAxis = iAxis;
308
309 // flip normal if interval is wrong faced
310 if ( fCenter< REAL(0.0) && !bNoFlip)
311 {
312 dVector3Inv(cData.vContactNormal);
313 cData.fBestCenter = -fCenter;
314 }
315 }
316
317 return true;
318}
319
320// intersection test between edge and circle
321bool _cldTestCircleToEdgeAxis(sData& cData,
322 const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
323 const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
324 const dVector3 &vVx0, const dVector3 &vVx1, int iAxis)
325{
326 // calculate direction of edge
327 dVector3 vkl;
328 dVector3Subtract( vVx1 , vVx0 , vkl);
329 dNormalize3(vkl);
330 // starting point of edge
331 dVector3 vol;
332 dVector3Copy(vVx0,vol);
333
334 // calculate angle cosine between cylinder axis and edge
335 dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1);
336
337 // if edge is perpendicular to cylinder axis
338 if(dFabs(fdot2)<REAL(1e-5))
339 {
340 // this can't be separating axis, because edge is parallel to circle plane
341 return true;
342 }
343
344 // find point of intersection between edge line and circle plane
345 dVector3 vTemp;
346 dVector3Subtract(vCenterPoint,vol,vTemp);
347 dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1);
348 dVector3 vpnt;// = vol + vkl * (fdot1/fdot2);
349 vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2;
350 vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2;
351 vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2;
352
353 // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt)
354 dVector3 vTangent;
355 dVector3Subtract(vCenterPoint,vpnt,vTemp);
356 dVector3Cross(vTemp,vCylinderAxis1,vTangent);
357
358 // find vector orthogonal both to tangent and edge direction
359 dVector3 vAxis;
360 dVector3Cross(vTangent,vkl,vAxis);
361
362 // use that vector as separating axis
363 return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis );
364}
365
366// helper for less key strokes
367// r = ( (v1 - v2) cross v3 ) cross v3
368inline void _CalculateAxis(const dVector3& v1,
369 const dVector3& v2,
370 const dVector3& v3,
371 dVector3& r)
372{
373 dVector3 t1;
374 dVector3 t2;
375
376 dVector3Subtract(v1,v2,t1);
377 dVector3Cross(t1,v3,t2);
378 dVector3Cross(t2,v3,r);
379}
380
381bool _cldTestSeparatingAxes(sData& cData,
382 const dVector3 &v0,
383 const dVector3 &v1,
384 const dVector3 &v2)
385{
386
387 // calculate edge vectors
388 dVector3Subtract(v1 ,v0 , cData.vE0);
389 // cData.vE1 has been calculated before -> so save some cycles here
390 dVector3Subtract(v0 ,v2 , cData.vE2);
391
392 // calculate caps centers in absolute space
393 dVector3 vCp0;
394 vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
395 vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
396 vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));
397
398 dVector3 vCp1;
399 vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
400 vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
401 vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));
402
403 // reset best axis
404 cData.iBestAxis = 0;
405 dVector3 vAxis;
406
407 // axis cData.vNormal
408 //vAxis = -cData.vNormal;
409 vAxis[0] = -cData.vNormal[0];
410 vAxis[1] = -cData.vNormal[1];
411 vAxis[2] = -cData.vNormal[2];
412 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true))
413 {
414 return false;
415 }
416
417 // axis CxE0
418 // vAxis = ( cData.vCylinderAxis cross cData.vE0 );
419 dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis);
420 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2))
421 {
422 return false;
423 }
424
425 // axis CxE1
426 // vAxis = ( cData.vCylinderAxis cross cData.vE1 );
427 dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis);
428 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3))
429 {
430 return false;
431 }
432
433 // axis CxE2
434 // vAxis = ( cData.vCylinderAxis cross cData.vE2 );
435 dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis);
436 if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4))
437 {
438 return false;
439 }
440
441 // first vertex on triangle
442 // axis ((V0-Cp0) x C) x C
443 //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
444 _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis);
445 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11))
446 {
447 return false;
448 }
449
450 // second vertex on triangle
451 // axis ((V1-Cp0) x C) x C
452 // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
453 _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis);
454 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12))
455 {
456 return false;
457 }
458
459 // third vertex on triangle
460 // axis ((V2-Cp0) x C) x C
461 //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
462 _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis);
463 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13))
464 {
465 return false;
466 }
467
468 // test cylinder axis
469 // vAxis = cData.vCylinderAxis;
470 dVector3Copy(cData.vCylinderAxis , vAxis);
471 if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14))
472 {
473 return false;
474 }
475
476 // Test top and bottom circle ring of cylinder for separation
477 dVector3 vccATop;
478 vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
479 vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
480 vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));
481
482 dVector3 vccABottom;
483 vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
484 vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
485 vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));
486
487
488 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15))
489 {
490 return false;
491 }
492
493 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16))
494 {
495 return false;
496 }
497
498 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17))
499 {
500 return false;
501 }
502
503 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18))
504 {
505 return false;
506 }
507
508 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19))
509 {
510 return false;
511 }
512
513 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20))
514 {
515 return false;
516 }
517
518 return true;
519}
520
521bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
522{
523 // translate cylinder
524 dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal);
525 dVector3 vN2;
526 vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp;
527 vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp;
528 vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp;
529
530 fTemp = dVector3Length(vN2);
531 if (fTemp < REAL(1e-5))
532 {
533 return false;
534 }
535
536 // Normalize it
537 vN2[0] /= fTemp;
538 vN2[1] /= fTemp;
539 vN2[2] /= fTemp;
540
541 // calculate caps centers in absolute space
542 dVector3 vCposTrans;
543 vCposTrans[0] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius;
544 vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius;
545 vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius;
546
547 dVector3 vCEdgePoint0;
548 vCEdgePoint0[0] = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
549 vCEdgePoint0[1] = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
550 vCEdgePoint0[2] = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));
551
552 dVector3 vCEdgePoint1;
553 vCEdgePoint1[0] = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
554 vCEdgePoint1[1] = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
555 vCEdgePoint1[2] = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));
556
557 // transform cylinder edge points into triangle space
558 vCEdgePoint0[0] -= v0[0];
559 vCEdgePoint0[1] -= v0[1];
560 vCEdgePoint0[2] -= v0[2];
561
562 vCEdgePoint1[0] -= v0[0];
563 vCEdgePoint1[1] -= v0[1];
564 vCEdgePoint1[2] -= v0[2];
565
566 dVector4 plPlane;
567 dVector3 vPlaneNormal;
568
569 // triangle plane
570 //plPlane = Plane4f( -cData.vNormal, 0);
571 vPlaneNormal[0] = -cData.vNormal[0];
572 vPlaneNormal[1] = -cData.vNormal[1];
573 vPlaneNormal[2] = -cData.vNormal[2];
574 dConstructPlane(vPlaneNormal,REAL(0.0),plPlane);
575 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
576 {
577 return false;
578 }
579
580 // plane with edge 0
581 //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), REAL(1e-5));
582 dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal);
583 dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
584 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
585 {
586 return false;
587 }
588
589 // plane with edge 1
590 //dVector3 vTemp = ( cData.vNormal cross cData.vE1 );
591 dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal);
592 fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - REAL(1e-5);
593 //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-REAL(1e-5)));
594 dConstructPlane(vPlaneNormal,-fTemp,plPlane);
595 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
596 {
597 return false;
598 }
599
600 // plane with edge 2
601 // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), REAL(1e-5));
602 dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal);
603 dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
604 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
605 {
606 return false;
607 }
608
609 // return capsule edge points into absolute space
610 vCEdgePoint0[0] += v0[0];
611 vCEdgePoint0[1] += v0[1];
612 vCEdgePoint0[2] += v0[2];
613
614 vCEdgePoint1[0] += v0[0];
615 vCEdgePoint1[1] += v0[1];
616 vCEdgePoint1[2] += v0[2];
617
618 // calculate depths for both contact points
619 dVector3 vTemp;
620 dVector3Subtract(vCEdgePoint0,cData.vCylinderPos, vTemp);
621 dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
622 dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp);
623 dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
624
625 dReal fDepth0 = cData.fBestDepth - (fRestDepth0);
626 dReal fDepth1 = cData.fBestDepth - (fRestDepth1);
627
628 // clamp depths to zero
629 if(fDepth0 < REAL(0.0) )
630 {
631 fDepth0 = REAL(0.0);
632 }
633
634 if(fDepth1<REAL(0.0))
635 {
636 fDepth1 = REAL(0.0);
637 }
638
639 // Generate contact 0
640 {
641 cData.gLocalContacts[cData.nContacts].fDepth = fDepth0;
642 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
643 dVector3Copy(vCEdgePoint0,cData.gLocalContacts[cData.nContacts].vPos);
644 cData.gLocalContacts[cData.nContacts].nFlags = 1;
645 cData.nContacts++;
646 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
647 return true;
648 }
649
650 // Generate contact 1
651 {
652 // generate contacts
653 cData.gLocalContacts[cData.nContacts].fDepth = fDepth1;
654 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
655 dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos);
656 cData.gLocalContacts[cData.nContacts].nFlags = 1;
657 cData.nContacts++;
658 }
659
660 return true;
661}
662
663void _cldClipCylinderToTriangle(sData& cData,const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
664{
665 int i = 0;
666 dVector3 avPoints[3];
667 dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];
668 dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];
669
670 dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);
671 dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);
672
673 // setup array of triangle vertices
674 dVector3Copy(v0,avPoints[0]);
675 dVector3Copy(v1,avPoints[1]);
676 dVector3Copy(v2,avPoints[2]);
677
678 dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
679 dSetZero(vCylinderCircleNormal_Rel,4);
680 // check which circle from cylinder we take for clipping
681 if ( dVector3Dot(cData.vCylinderAxis , cData.vContactNormal) > REAL(0.0))
682 {
683 // get top circle
684 vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
685 vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
686 vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
687
688 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
689 }
690 else
691 {
692 // get bottom circle
693 vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
694 vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
695 vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
696
697 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
698 }
699
700 dVector3 vTemp;
701 dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot);
702 // transform triangle points to space of cylinder circle
703 for(i=0; i<3; i++)
704 {
705 dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp);
706 dQuatTransform(cData.qInvCylinderRot,vTemp,avPoints[i]);
707 }
708
709 int iTmpCounter1 = 0;
710 int iTmpCounter2 = 0;
711 dVector4 plPlane;
712
713 // plane of cylinder that contains circle for intersection
714 //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f );
715 dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
716 dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane);
717
718 // Body of base circle of Cylinder
719 int nCircleSegment = 0;
720 for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++)
721 {
722 dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);
723
724 if (0 == (nCircleSegment % 2))
725 {
726 dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
727 }
728 else
729 {
730 dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
731 }
732
733 dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
734 dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
735 }
736
737 // back transform clipped points to absolute space
738 dReal ftmpdot;
739 dReal fTempDepth;
740 dVector3 vPoint;
741
742 if (nCircleSegment %2)
743 {
744 for( i=0; i<iTmpCounter2; i++)
745 {
746 dQuatTransform(cData.qCylinderRot,avTempArray2[i], vPoint);
747 vPoint[0] += vCylinderCirclePos[0];
748 vPoint[1] += vCylinderCirclePos[1];
749 vPoint[2] += vCylinderCirclePos[2];
750
751 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
752 ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
753 fTempDepth = cData.fBestrt - ftmpdot;
754 // Depth must be positive
755 if (fTempDepth > REAL(0.0))
756 {
757 cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
758 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
759 dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
760 cData.gLocalContacts[cData.nContacts].nFlags = 1;
761 cData.nContacts++;
762 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
763 return;;
764 }
765 }
766 }
767 else
768 {
769 for( i=0; i<iTmpCounter1; i++)
770 {
771 dQuatTransform(cData.qCylinderRot,avTempArray1[i], vPoint);
772 vPoint[0] += vCylinderCirclePos[0];
773 vPoint[1] += vCylinderCirclePos[1];
774 vPoint[2] += vCylinderCirclePos[2];
775
776 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
777 ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
778 fTempDepth = cData.fBestrt - ftmpdot;
779 // Depth must be positive
780 if (fTempDepth > REAL(0.0))
781 {
782 cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
783 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
784 dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
785 cData.gLocalContacts[cData.nContacts].nFlags = 1;
786 cData.nContacts++;
787 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
788 return;;
789 }
790 }
791 }
792}
793
794void TestOneTriangleVsCylinder( sData& cData,
795 const dVector3 &v0,
796 const dVector3 &v1,
797 const dVector3 &v2,
798 const bool bDoubleSided)
799{
800
801 // calculate triangle normal
802 dVector3Subtract( v2 , v1 ,cData.vE1);
803 dVector3 vTemp;
804 dVector3Subtract( v0 , v1 ,vTemp);
805 dVector3Cross(cData.vE1 , vTemp , cData.vNormal );
806
807 dNormalize3( cData.vNormal);
808
809 // create plane from triangle
810 //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 );
811 dReal plDistance = -dVector3Dot(v0, cData.vNormal);
812 dVector4 plTrianglePlane;
813 dConstructPlane( cData.vNormal,plDistance,plTrianglePlane);
814
815 // calculate sphere distance to plane
816 dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane);
817
818 // Sphere must be over positive side of triangle
819 if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided)
820 {
821 // if not don't generate contacts
822 return;
823 }
824
825 dVector3 vPnt0;
826 dVector3 vPnt1;
827 dVector3 vPnt2;
828
829 if (fDistanceCylinderCenterToPlane < REAL(0.0) )
830 {
831 // flip it
832 dVector3Copy(v0 , vPnt0);
833 dVector3Copy(v1 , vPnt2);
834 dVector3Copy(v2 , vPnt1);
835 }
836 else
837 {
838 dVector3Copy(v0 , vPnt0);
839 dVector3Copy(v1 , vPnt1);
840 dVector3Copy(v2 , vPnt2);
841 }
842
843 cData.fBestDepth = MAX_REAL;
844
845 // do intersection test and find best separating axis
846 if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) )
847 {
848 // if not found do nothing
849 return;
850 }
851
852 // if best separation axis is not found
853 if ( cData.iBestAxis == 0 )
854 {
855 // this should not happen (we should already exit in that case)
856 dIASSERT(false);
857 // do nothing
858 return;
859 }
860
861 dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis );
862
863 // choose which clipping method are we going to apply
864 if (dFabs(fdot) < REAL(0.9) )
865 {
866 if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2))
867 {
868 return;
869 }
870 }
871 else
872 {
873 _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2);
874 }
875
876}
877
878void _InitCylinderTrimeshData(sData& cData)
879{
880 // get cylinder information
881 // Rotation
882 const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
883 dMatrix3Copy(pRotCyc,cData.mCylinderRot);
884 dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
885
886 // Position
887 const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
888 dVector3Copy(*pPosCyc,cData.vCylinderPos);
889 // Cylinder axis
890 dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
891 // get cylinder radius and size
892 dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
893
894 // get trimesh position and orientation
895 const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
896 dMatrix3Copy(pRotTris,cData.mTrimeshRot);
897 dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
898
899 // Position
900 const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
901 dVector3Copy(*pPosTris,cData.vTrimeshPos);
902
903
904 // calculate basic angle for 8-gon
905 dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
906 // calculate angle increment
907 dReal fAngleIncrement = fAngle*REAL(2.0);
908
909 // calculate plane normals
910 // axis dependant code
911 for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
912 {
913 cData.avCylinderNormals[i][0] = -dCos(fAngle);
914 cData.avCylinderNormals[i][1] = -dSin(fAngle);
915 cData.avCylinderNormals[i][2] = REAL(0.0);
916
917 fAngle += fAngleIncrement;
918 }
919
920 dSetZero(cData.vBestPoint,4);
921 // reset best depth
922 cData.fBestCenter = REAL(0.0);
923}
924
925#if dTRIMESH_ENABLED
926
927// OPCODE version of cylinder to mesh collider
928#if dTRIMESH_OPCODE
929int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
930{
931 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
932 dIASSERT( o1->type == dCylinderClass );
933 dIASSERT( o2->type == dTriMeshClass );
934 dIASSERT ((flags & NUMC_MASK) >= 1);
935
936 // Main data holder
937 sData cData;
938
939 // Assign ODE stuff
940 cData.gCylinder = o1;
941 cData.gTrimesh = (dxTriMesh*)o2;
942 cData.iFlags = flags;
943 cData.iSkip = skip;
944 cData.gContact = contact;
945 cData.nContacts = 0;
946
947 _InitCylinderTrimeshData(cData);
948
949 OBBCollider& Collider = cData.gTrimesh->_OBBCollider;
950
951 Point cCenter(cData.vCylinderPos[0],cData.vCylinderPos[1],cData.vCylinderPos[2]);
952
953 Point cExtents(cData.fCylinderRadius,cData.fCylinderRadius,cData.fCylinderRadius);
954 cExtents[nCYLINDER_AXIS] = cData.fCylinderSize * REAL(0.5);
955
956 Matrix3x3 obbRot;
957
958 // It is a potential issue to explicitly cast to float
959 // if custom width floating point type is introduced in OPCODE.
960 // It is necessary to make a typedef and cast to it
961 // (e.g. typedef float opc_float;)
962 // However I'm not sure in what header it should be added.
963
964 obbRot[0][0] = /*(float)*/cData.mCylinderRot[0];
965 obbRot[1][0] = /*(float)*/cData.mCylinderRot[1];
966 obbRot[2][0] = /*(float)*/cData.mCylinderRot[2];
967
968 obbRot[0][1] = /*(float)*/cData.mCylinderRot[4];
969 obbRot[1][1] = /*(float)*/cData.mCylinderRot[5];
970 obbRot[2][1] = /*(float)*/cData.mCylinderRot[6];
971
972 obbRot[0][2] = /*(float)*/cData.mCylinderRot[8];
973 obbRot[1][2] = /*(float)*/cData.mCylinderRot[9];
974 obbRot[2][2] = /*(float)*/cData.mCylinderRot[10];
975
976 OBB obbCapsule(cCenter,cExtents,obbRot);
977
978 Matrix4x4 CapsuleMatrix;
979 MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix);
980
981 Matrix4x4 MeshMatrix;
982 MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix);
983
984 // TC results
985 if (cData.gTrimesh->doBoxTC)
986 {
987 dxTriMesh::BoxTC* BoxTC = 0;
988 for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++)
989 {
990 if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder)
991 {
992 BoxTC = &cData.gTrimesh->BoxTCCache[i];
993 break;
994 }
995 }
996 if (!BoxTC)
997 {
998 cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC());
999
1000 BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1];
1001 BoxTC->Geom = cData.gCylinder;
1002 BoxTC->FatCoeff = REAL(1.0);
1003 }
1004
1005 // Intersect
1006 Collider.SetTemporalCoherence(true);
1007 Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix);
1008 }
1009 else
1010 {
1011 Collider.SetTemporalCoherence(false);
1012 Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix);
1013 }
1014
1015 // Retrieve data
1016 int TriCount = Collider.GetNbTouchedPrimitives();
1017 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1018
1019
1020 if (TriCount != 0)
1021 {
1022 if (cData.gTrimesh->ArrayCallback != null)
1023 {
1024 cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount);
1025 }
1026
1027 // allocate buffer for local contacts on stack
1028 cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));
1029
1030 int ctContacts0 = 0;
1031
1032 // loop through all intersecting triangles
1033 for (int i = 0; i < TriCount; i++)
1034 {
1035 const int Triint = Triangles[i];
1036 if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue;
1037
1038
1039 dVector3 dv[3];
1040 FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv);
1041
1042 // test this triangle
1043 TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);
1044
1045 // fill-in tri index for generated contacts
1046 for (; ctContacts0<cData.nContacts; ctContacts0++)
1047 cData.gLocalContacts[ctContacts0].triIndex = Triint;
1048
1049 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1050 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
1051 {
1052 break;
1053 }
1054 }
1055 }
1056
1057 return _ProcessLocalContacts(cData);
1058}
1059#endif
1060
1061// GIMPACT version of cylinder to mesh collider
1062#if dTRIMESH_GIMPACT
1063int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
1064{
1065 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
1066 dIASSERT( o1->type == dCylinderClass );
1067 dIASSERT( o2->type == dTriMeshClass );
1068 dIASSERT ((flags & NUMC_MASK) >= 1);
1069
1070 // Main data holder
1071 sData cData;
1072
1073 // Assign ODE stuff
1074 cData.gCylinder = o1;
1075 cData.gTrimesh = (dxTriMesh*)o2;
1076 cData.iFlags = flags;
1077 cData.iSkip = skip;
1078 cData.gContact = contact;
1079 cData.nContacts = 0;
1080
1081 _InitCylinderTrimeshData(cData);
1082
1083//*****at first , collide box aabb******//
1084
1085 aabb3f test_aabb;
1086
1087 test_aabb.minX = o1->aabb[0];
1088 test_aabb.maxX = o1->aabb[1];
1089 test_aabb.minY = o1->aabb[2];
1090 test_aabb.maxY = o1->aabb[3];
1091 test_aabb.minZ = o1->aabb[4];
1092 test_aabb.maxZ = o1->aabb[5];
1093
1094
1095 GDYNAMIC_ARRAY collision_result;
1096 GIM_CREATE_BOXQUERY_LIST(collision_result);
1097
1098 gim_aabbset_box_collision(&test_aabb, &cData.gTrimesh->m_collision_trimesh.m_aabbset , &collision_result);
1099
1100 if(collision_result.m_size==0)
1101 {
1102 GIM_DYNARRAY_DESTROY(collision_result);
1103 return 0;
1104 }
1105//*****Set globals for box collision******//
1106
1107 int ctContacts0 = 0;
1108 cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));
1109
1110 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
1111 GIM_TRIMESH * ptrimesh = &cData.gTrimesh->m_collision_trimesh;
1112
1113 gim_trimesh_locks_work_data(ptrimesh);
1114
1115
1116 for(unsigned int i=0;i<collision_result.m_size;i++)
1117 {
1118 const int Triint = boxesresult[i];
1119
1120 dVector3 dv[3];
1121 gim_trimesh_get_triangle_vertices(ptrimesh, Triint,dv[0],dv[1],dv[2]);
1122 // test this triangle
1123 TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);
1124
1125 // fill-in triangle index for generated contacts
1126 for (; ctContacts0<cData.nContacts; ctContacts0++)
1127 cData.gLocalContacts[ctContacts0].triIndex = Triint;
1128
1129 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1130 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
1131 {
1132 break;
1133 }
1134 }
1135
1136 gim_trimesh_unlocks_work_data(ptrimesh);
1137 GIM_DYNARRAY_DESTROY(collision_result);
1138
1139 return _ProcessLocalContacts(cData);
1140}
1141#endif
1142
1143#endif // dTRIMESH_ENABLED
1144
1145
diff --git a/libraries/ode-0.9/ode/src/collision_kernel.cpp b/libraries/ode-0.9/ode/src/collision_kernel.cpp
new file mode 100644
index 0000000..b885603
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_kernel.cpp
@@ -0,0 +1,1103 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25core collision functions and data structures, plus part of the public API
26for geometry objects
27
28*/
29
30#include <ode/common.h>
31#include <ode/matrix.h>
32#include <ode/rotation.h>
33#include <ode/objects.h>
34#include <ode/odemath.h>
35#include "collision_kernel.h"
36#include "collision_util.h"
37#include "collision_std.h"
38#include "collision_transform.h"
39#include "collision_trimesh_internal.h"
40
41#if dTRIMESH_GIMPACT
42#include <GIMPACT/gimpact.h>
43#endif
44
45#ifdef _MSC_VER
46#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
47#endif
48
49//****************************************************************************
50// helper functions for dCollide()ing a space with another geom
51
52// this struct records the parameters passed to dCollideSpaceGeom()
53
54// Allocate and free posr - we cache a single posr to avoid thrashing
55static dxPosR* s_cachedPosR = 0;
56
57dxPosR* dAllocPosr()
58{
59 dxPosR* retPosR;
60 if (s_cachedPosR)
61 {
62 retPosR = s_cachedPosR;
63 s_cachedPosR = 0;
64 }
65 else
66 {
67 retPosR = (dxPosR*) dAlloc (sizeof(dxPosR));
68 }
69 return retPosR;
70}
71
72void dFreePosr(dxPosR* oldPosR)
73{
74 if (oldPosR)
75 {
76 if (s_cachedPosR)
77 {
78 dFree(s_cachedPosR, sizeof(dxPosR));
79 }
80 s_cachedPosR = oldPosR;
81 }
82}
83
84void dClearPosrCache(void)
85{
86 if (s_cachedPosR)
87 {
88 dFree(s_cachedPosR, sizeof(dxPosR));
89 s_cachedPosR = 0;
90 }
91}
92
93struct SpaceGeomColliderData {
94 int flags; // space left in contacts array
95 dContactGeom *contact;
96 int skip;
97};
98
99
100static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2)
101{
102 SpaceGeomColliderData *d = (SpaceGeomColliderData*) data;
103 if (d->flags & NUMC_MASK) {
104 int n = dCollide (o1,o2,d->flags,d->contact,d->skip);
105 d->contact = CONTACT (d->contact,d->skip*n);
106 d->flags -= n;
107 }
108}
109
110
111static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags,
112 dContactGeom *contact, int skip)
113{
114 SpaceGeomColliderData data;
115 data.flags = flags;
116 data.contact = contact;
117 data.skip = skip;
118 dSpaceCollide2 (o1,o2,&data,&space_geom_collider);
119 return (flags & NUMC_MASK) - (data.flags & NUMC_MASK);
120}
121
122//****************************************************************************
123// dispatcher for the N^2 collider functions
124
125// function pointers and modes for n^2 class collider functions
126
127struct dColliderEntry {
128 dColliderFn *fn; // collider function, 0 = no function available
129 int reverse; // 1 = reverse o1 and o2
130};
131static dColliderEntry colliders[dGeomNumClasses][dGeomNumClasses];
132static int colliders_initialized = 0;
133
134
135// setCollider() will refuse to write over a collider entry once it has
136// been written.
137
138static void setCollider (int i, int j, dColliderFn *fn)
139{
140 if (colliders[i][j].fn == 0) {
141 colliders[i][j].fn = fn;
142 colliders[i][j].reverse = 0;
143 }
144 if (colliders[j][i].fn == 0) {
145 colliders[j][i].fn = fn;
146 colliders[j][i].reverse = 1;
147 }
148}
149
150
151static void setAllColliders (int i, dColliderFn *fn)
152{
153 for (int j=0; j<dGeomNumClasses; j++) setCollider (i,j,fn);
154}
155
156
157static void initColliders()
158{
159 int i,j;
160
161 if (colliders_initialized) return;
162 colliders_initialized = 1;
163
164 memset (colliders,0,sizeof(colliders));
165
166 // setup space colliders
167 for (i=dFirstSpaceClass; i <= dLastSpaceClass; i++) {
168 for (j=0; j < dGeomNumClasses; j++) {
169 setCollider (i,j,&dCollideSpaceGeom);
170 }
171 }
172
173 setCollider (dSphereClass,dSphereClass,&dCollideSphereSphere);
174 setCollider (dSphereClass,dBoxClass,&dCollideSphereBox);
175 setCollider (dSphereClass,dPlaneClass,&dCollideSpherePlane);
176 setCollider (dBoxClass,dBoxClass,&dCollideBoxBox);
177 setCollider (dBoxClass,dPlaneClass,&dCollideBoxPlane);
178 setCollider (dCapsuleClass,dSphereClass,&dCollideCapsuleSphere);
179 setCollider (dCapsuleClass,dBoxClass,&dCollideCapsuleBox);
180 setCollider (dCapsuleClass,dCapsuleClass,&dCollideCapsuleCapsule);
181 setCollider (dCapsuleClass,dPlaneClass,&dCollideCapsulePlane);
182 setCollider (dRayClass,dSphereClass,&dCollideRaySphere);
183 setCollider (dRayClass,dBoxClass,&dCollideRayBox);
184 setCollider (dRayClass,dCapsuleClass,&dCollideRayCapsule);
185 setCollider (dRayClass,dPlaneClass,&dCollideRayPlane);
186 setCollider (dRayClass,dCylinderClass,&dCollideRayCylinder);
187#if dTRIMESH_ENABLED
188 setCollider (dTriMeshClass,dSphereClass,&dCollideSTL);
189 setCollider (dTriMeshClass,dBoxClass,&dCollideBTL);
190 setCollider (dTriMeshClass,dRayClass,&dCollideRTL);
191 setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL);
192 setCollider (dTriMeshClass,dCapsuleClass,&dCollideCCTL);
193 setCollider (dTriMeshClass,dPlaneClass,&dCollideTrimeshPlane);
194 setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh);
195#endif
196 setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox);
197 setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere);
198 setCollider (dCylinderClass,dPlaneClass,&dCollideCylinderPlane);
199 //setCollider (dCylinderClass,dCylinderClass,&dCollideCylinderCylinder);
200
201//--> Convex Collision
202 setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane);
203 setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex);
204 setCollider (dConvexClass,dBoxClass,&dCollideConvexBox);
205 setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule);
206 setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex);
207 setCollider (dRayClass,dConvexClass,&dCollideRayConvex);
208//<-- Convex Collision
209
210//--> dHeightfield Collision
211 setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield);
212 setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield);
213 setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield);
214 setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield);
215 setCollider (dHeightfieldClass,dCylinderClass,&dCollideHeightfield);
216 setCollider (dHeightfieldClass,dConvexClass,&dCollideHeightfield);
217#if dTRIMESH_ENABLED
218 setCollider (dHeightfieldClass,dTriMeshClass,&dCollideHeightfield);
219#endif
220//<-- dHeightfield Collision
221
222 setAllColliders (dGeomTransformClass,&dCollideTransform);
223}
224
225
226/*
227 * NOTE!
228 * If it is necessary to add special processing mode without contact generation
229 * use NULL contact parameter value as indicator, not zero in flags.
230 */
231int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
232 int skip)
233{
234 dAASSERT(o1 && o2 && contact);
235 dUASSERT(colliders_initialized,"colliders array not initialized");
236 dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number");
237 dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number");
238 // Even though comparison for greater or equal to one is used in all the
239 // other places, here it is more logical to check for greater than zero
240 // because function does not require any specific number of contact slots -
241 // it must be just a positive.
242 dUASSERT((flags & NUMC_MASK) > 0, "no contacts requested");
243
244 // Extra precaution for zero contact count in parameters
245 if ((flags & NUMC_MASK) == 0) return 0;
246 // no contacts if both geoms are the same
247 if (o1 == o2) return 0;
248
249 // no contacts if both geoms on the same body, and the body is not 0
250 if (o1->body == o2->body && o1->body) return 0;
251
252 o1->recomputePosr();
253 o2->recomputePosr();
254
255 dColliderEntry *ce = &colliders[o1->type][o2->type];
256 int count = 0;
257 if (ce->fn) {
258 if (ce->reverse) {
259 count = (*ce->fn) (o2,o1,flags,contact,skip);
260 for (int i=0; i<count; i++) {
261 dContactGeom *c = CONTACT(contact,skip*i);
262 c->normal[0] = -c->normal[0];
263 c->normal[1] = -c->normal[1];
264 c->normal[2] = -c->normal[2];
265 dxGeom *tmp = c->g1;
266 c->g1 = c->g2;
267 c->g2 = tmp;
268 int tmpint = c->side1;
269 c->side1 = c->side2;
270 c->side2 = tmpint;
271 }
272 }
273 else {
274 count = (*ce->fn) (o1,o2,flags,contact,skip);
275 }
276 }
277 return count;
278}
279
280//****************************************************************************
281// dxGeom
282
283dxGeom::dxGeom (dSpaceID _space, int is_placeable)
284{
285 initColliders();
286
287 // setup body vars. invalid type of -1 must be changed by the constructor.
288 type = -1;
289 gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;
290 if (is_placeable) gflags |= GEOM_PLACEABLE;
291 data = 0;
292 body = 0;
293 body_next = 0;
294 if (is_placeable) {
295 final_posr = dAllocPosr();
296 dSetZero (final_posr->pos,4);
297 dRSetIdentity (final_posr->R);
298 }
299 else {
300 final_posr = 0;
301 }
302 offset_posr = 0;
303
304 // setup space vars
305 next = 0;
306 tome = 0;
307 parent_space = 0;
308 dSetZero (aabb,6);
309 category_bits = ~0;
310 collide_bits = ~0;
311
312 // put this geom in a space if required
313 if (_space) dSpaceAdd (_space,this);
314}
315
316
317dxGeom::~dxGeom()
318{
319 if (parent_space) dSpaceRemove (parent_space,this);
320 if ((gflags & GEOM_PLACEABLE) && (!body || (body && offset_posr)))
321 dFreePosr(final_posr);
322 if (offset_posr) dFreePosr(offset_posr);
323 bodyRemove();
324}
325
326
327int dxGeom::AABBTest (dxGeom *o, dReal aabb[6])
328{
329 return 1;
330}
331
332
333void dxGeom::bodyRemove()
334{
335 if (body) {
336 // delete this geom from body list
337 dxGeom **last = &body->geom, *g = body->geom;
338 while (g) {
339 if (g == this) {
340 *last = g->body_next;
341 break;
342 }
343 last = &g->body_next;
344 g = g->body_next;
345 }
346 body = 0;
347 body_next = 0;
348 }
349}
350
351inline void myswap(dReal& a, dReal& b) { dReal t=b; b=a; a=t; }
352
353
354inline void matrixInvert(const dMatrix3& inMat, dMatrix3& outMat)
355{
356 memcpy(outMat, inMat, sizeof(dMatrix3));
357 // swap _12 and _21
358 myswap(outMat[0 + 4*1], outMat[1 + 4*0]);
359 // swap _31 and _13
360 myswap(outMat[2 + 4*0], outMat[0 + 4*2]);
361 // swap _23 and _32
362 myswap(outMat[1 + 4*2], outMat[2 + 4*1]);
363}
364
365void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr)
366{
367 dMatrix3 inv_offset;
368 matrixInvert(offset_posr.R, inv_offset);
369
370 dMULTIPLY0_333(body_posr.R, final_posr.R, inv_offset);
371 dVector3 world_offset;
372 dMULTIPLY0_331(world_offset, body_posr.R, offset_posr.pos);
373 body_posr.pos[0] = final_posr.pos[0] - world_offset[0];
374 body_posr.pos[1] = final_posr.pos[1] - world_offset[1];
375 body_posr.pos[2] = final_posr.pos[2] - world_offset[2];
376}
377
378void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr)
379{
380 dMatrix3 inv_body;
381 matrixInvert(body_posr.R, inv_body);
382
383 dMULTIPLY0_333(offset_posr.R, inv_body, world_posr.R);
384 dVector3 world_offset;
385 world_offset[0] = world_posr.pos[0] - body_posr.pos[0];
386 world_offset[1] = world_posr.pos[1] - body_posr.pos[1];
387 world_offset[2] = world_posr.pos[2] - body_posr.pos[2];
388 dMULTIPLY0_331(offset_posr.pos, inv_body, world_offset);
389}
390
391void dxGeom::computePosr()
392{
393 // should only be recalced if we need to - ie offset from a body
394 dIASSERT(offset_posr);
395 dIASSERT(body);
396
397 dMULTIPLY0_331 (final_posr->pos,body->posr.R,offset_posr->pos);
398 final_posr->pos[0] += body->posr.pos[0];
399 final_posr->pos[1] += body->posr.pos[1];
400 final_posr->pos[2] += body->posr.pos[2];
401 dMULTIPLY0_333 (final_posr->R,body->posr.R,offset_posr->R);
402}
403
404//****************************************************************************
405// misc
406
407dxGeom *dGeomGetBodyNext (dxGeom *geom)
408{
409 return geom->body_next;
410}
411
412//****************************************************************************
413// public API for geometry objects
414
415#define CHECK_NOT_LOCKED(space) \
416 dUASSERT (!(space && space->lock_count), \
417 "invalid operation for geom in locked space");
418
419
420void dGeomDestroy (dxGeom *g)
421{
422 dAASSERT (g);
423 delete g;
424}
425
426
427void dGeomSetData (dxGeom *g, void *data)
428{
429 dAASSERT (g);
430 g->data = data;
431}
432
433
434void *dGeomGetData (dxGeom *g)
435{
436 dAASSERT (g);
437 return g->data;
438}
439
440
441void dGeomSetBody (dxGeom *g, dxBody *b)
442{
443 dAASSERT (g);
444 dUASSERT (b == NULL || (g->gflags & GEOM_PLACEABLE),"geom must be placeable");
445 CHECK_NOT_LOCKED (g->parent_space);
446
447 if (b) {
448 if (!g->body) dFreePosr(g->final_posr);
449 if (g->body != b) {
450 if (g->offset_posr) {
451 dFreePosr(g->offset_posr);
452 g->offset_posr = 0;
453 }
454 g->final_posr = &b->posr;
455 g->bodyRemove();
456 g->bodyAdd (b);
457 }
458 dGeomMoved (g);
459 }
460 else {
461 if (g->body) {
462 if (g->offset_posr)
463 {
464 // if we're offset, we already have our own final position, make sure its updated
465 g->recomputePosr();
466 dFreePosr(g->offset_posr);
467 g->offset_posr = 0;
468 }
469 else
470 {
471 g->final_posr = dAllocPosr();
472 memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3));
473 memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3));
474 }
475 g->bodyRemove();
476 }
477 // dGeomMoved() should not be called if the body is being set to 0, as the
478 // new position of the geom is set to the old position of the body, so the
479 // effective position of the geom remains unchanged.
480 }
481}
482
483
484dBodyID dGeomGetBody (dxGeom *g)
485{
486 dAASSERT (g);
487 return g->body;
488}
489
490
491void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
492{
493 dAASSERT (g);
494 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
495 CHECK_NOT_LOCKED (g->parent_space);
496 if (g->offset_posr) {
497 // move body such that body+offset = position
498 dVector3 world_offset;
499 dMULTIPLY0_331(world_offset, g->body->posr.R, g->offset_posr->pos);
500 dBodySetPosition(g->body,
501 x - world_offset[0],
502 y - world_offset[1],
503 z - world_offset[2]);
504 }
505 else if (g->body) {
506 // this will call dGeomMoved (g), so we don't have to
507 dBodySetPosition (g->body,x,y,z);
508 }
509 else {
510 g->final_posr->pos[0] = x;
511 g->final_posr->pos[1] = y;
512 g->final_posr->pos[2] = z;
513 dGeomMoved (g);
514 }
515}
516
517
518void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
519{
520 dAASSERT (g && R);
521 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
522 CHECK_NOT_LOCKED (g->parent_space);
523 if (g->offset_posr) {
524 g->recomputePosr();
525 // move body such that body+offset = rotation
526 dxPosR new_final_posr;
527 dxPosR new_body_posr;
528 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
529 memcpy(new_final_posr.R, R, sizeof(dMatrix3));
530 getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
531 dBodySetRotation(g->body, new_body_posr.R);
532 dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
533 }
534 else if (g->body) {
535 // this will call dGeomMoved (g), so we don't have to
536 dBodySetRotation (g->body,R);
537 }
538 else {
539 memcpy (g->final_posr->R,R,sizeof(dMatrix3));
540 dGeomMoved (g);
541 }
542}
543
544
545void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
546{
547 dAASSERT (g && quat);
548 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
549 CHECK_NOT_LOCKED (g->parent_space);
550 if (g->offset_posr) {
551 g->recomputePosr();
552 // move body such that body+offset = rotation
553 dxPosR new_final_posr;
554 dxPosR new_body_posr;
555 dQtoR (quat, new_final_posr.R);
556 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
557
558 getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
559 dBodySetRotation(g->body, new_body_posr.R);
560 dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
561 }
562 if (g->body) {
563 // this will call dGeomMoved (g), so we don't have to
564 dBodySetQuaternion (g->body,quat);
565 }
566 else {
567 dQtoR (quat, g->final_posr->R);
568 dGeomMoved (g);
569 }
570}
571
572
573const dReal * dGeomGetPosition (dxGeom *g)
574{
575 dAASSERT (g);
576 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
577 g->recomputePosr();
578 return g->final_posr->pos;
579}
580
581
582void dGeomCopyPosition(dxGeom *g, dVector3 pos)
583{
584 dAASSERT (g);
585 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
586 g->recomputePosr();
587 const dReal* src = g->final_posr->pos;
588 pos[0] = src[0];
589 pos[1] = src[1];
590 pos[2] = src[2];
591}
592
593
594const dReal * dGeomGetRotation (dxGeom *g)
595{
596 dAASSERT (g);
597 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
598 g->recomputePosr();
599 return g->final_posr->R;
600}
601
602
603void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
604{
605 dAASSERT (g);
606 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
607 g->recomputePosr();
608 const dReal* src = g->final_posr->R;
609 R[0] = src[0];
610 R[1] = src[1];
611 R[2] = src[2];
612 R[4] = src[4];
613 R[5] = src[5];
614 R[6] = src[6];
615 R[8] = src[8];
616 R[9] = src[9];
617 R[10] = src[10];
618}
619
620
621void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
622{
623 dAASSERT (g);
624 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
625 if (g->body && !g->offset_posr) {
626 const dReal * body_quat = dBodyGetQuaternion (g->body);
627 quat[0] = body_quat[0];
628 quat[1] = body_quat[1];
629 quat[2] = body_quat[2];
630 quat[3] = body_quat[3];
631 }
632 else {
633 g->recomputePosr();
634 dRtoQ (g->final_posr->R, quat);
635 }
636}
637
638
639void dGeomGetAABB (dxGeom *g, dReal aabb[6])
640{
641 dAASSERT (g);
642 dAASSERT (aabb);
643 g->recomputeAABB();
644 memcpy (aabb,g->aabb,6 * sizeof(dReal));
645}
646
647
648int dGeomIsSpace (dxGeom *g)
649{
650 dAASSERT (g);
651 return IS_SPACE(g);
652}
653
654
655dSpaceID dGeomGetSpace (dxGeom *g)
656{
657 dAASSERT (g);
658 return g->parent_space;
659}
660
661
662int dGeomGetClass (dxGeom *g)
663{
664 dAASSERT (g);
665 return g->type;
666}
667
668
669void dGeomSetCategoryBits (dxGeom *g, unsigned long bits)
670{
671 dAASSERT (g);
672 CHECK_NOT_LOCKED (g->parent_space);
673 g->category_bits = bits;
674}
675
676
677void dGeomSetCollideBits (dxGeom *g, unsigned long bits)
678{
679 dAASSERT (g);
680 CHECK_NOT_LOCKED (g->parent_space);
681 g->collide_bits = bits;
682}
683
684
685unsigned long dGeomGetCategoryBits (dxGeom *g)
686{
687 dAASSERT (g);
688 return g->category_bits;
689}
690
691
692unsigned long dGeomGetCollideBits (dxGeom *g)
693{
694 dAASSERT (g);
695 return g->collide_bits;
696}
697
698
699void dGeomEnable (dxGeom *g)
700{
701 dAASSERT (g);
702 g->gflags |= GEOM_ENABLED;
703}
704
705void dGeomDisable (dxGeom *g)
706{
707 dAASSERT (g);
708 g->gflags &= ~GEOM_ENABLED;
709}
710
711int dGeomIsEnabled (dxGeom *g)
712{
713 dAASSERT (g);
714 return (g->gflags & GEOM_ENABLED) != 0;
715}
716
717
718//****************************************************************************
719// C interface that lets the user make new classes. this interface is a lot
720// more cumbersome than C++ subclassing, which is what is used internally
721// in ODE. this API is mainly to support legacy code.
722
723static int num_user_classes = 0;
724static dGeomClass user_classes [dMaxUserClasses];
725
726
727struct dxUserGeom : public dxGeom {
728 void *user_data;
729
730 dxUserGeom (int class_num);
731 ~dxUserGeom();
732 void computeAABB();
733 int AABBTest (dxGeom *o, dReal aabb[6]);
734};
735
736
737dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1)
738{
739 type = class_num;
740 int size = user_classes[type-dFirstUserClass].bytes;
741 user_data = dAlloc (size);
742 memset (user_data,0,size);
743}
744
745
746dxUserGeom::~dxUserGeom()
747{
748 dGeomClass *c = &user_classes[type-dFirstUserClass];
749 if (c->dtor) c->dtor (this);
750 dFree (user_data,c->bytes);
751}
752
753
754void dxUserGeom::computeAABB()
755{
756 user_classes[type-dFirstUserClass].aabb (this,aabb);
757}
758
759
760int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6])
761{
762 dGeomClass *c = &user_classes[type-dFirstUserClass];
763 if (c->aabb_test) return c->aabb_test (this,o,aabb);
764 else return 1;
765}
766
767
768static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags,
769 dContactGeom *contact, int skip)
770{
771 // this generic collider function is called the first time that a user class
772 // tries to collide against something. it will find out the correct collider
773 // function and then set the colliders array so that the correct function is
774 // called directly the next time around.
775
776 int t1 = o1->type; // note that o1 is a user geom
777 int t2 = o2->type; // o2 *may* be a user geom
778
779 // find the collider function to use. if o1 does not know how to collide with
780 // o2, then o2 might know how to collide with o1 (provided that it is a user
781 // geom).
782 dColliderFn *fn = user_classes[t1-dFirstUserClass].collider (t2);
783 int reverse = 0;
784 if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) {
785 fn = user_classes[t2-dFirstUserClass].collider (t1);
786 reverse = 1;
787 }
788
789 // set the colliders array so that the correct function is called directly
790 // the next time around. note that fn can be 0 here if no collider was found,
791 // which means that dCollide() will always return 0 for this case.
792 colliders[t1][t2].fn = fn;
793 colliders[t1][t2].reverse = reverse;
794 colliders[t2][t1].fn = fn;
795 colliders[t2][t1].reverse = !reverse;
796
797 // now call the collider function indirectly through dCollide(), so that
798 // contact reversing is properly handled.
799 return dCollide (o1,o2,flags,contact,skip);
800}
801
802
803int dCreateGeomClass (const dGeomClass *c)
804{
805 dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class");
806
807 if (num_user_classes >= dMaxUserClasses) {
808 dDebug (0,"too many user classes, you must increase the limit and "
809 "recompile ODE");
810 }
811 user_classes[num_user_classes] = *c;
812 int class_number = num_user_classes + dFirstUserClass;
813 initColliders();
814 setAllColliders (class_number,&dCollideUserGeomWithGeom);
815
816 num_user_classes++;
817 return class_number;
818}
819
820
821void * dGeomGetClassData (dxGeom *g)
822{
823 dUASSERT (g && g->type >= dFirstUserClass &&
824 g->type <= dLastUserClass,"not a custom class");
825 dxUserGeom *user = (dxUserGeom*) g;
826 return user->user_data;
827}
828
829
830dGeomID dCreateGeom (int classnum)
831{
832 dUASSERT (classnum >= dFirstUserClass &&
833 classnum <= dLastUserClass,"not a custom class");
834 return new dxUserGeom (classnum);
835}
836
837
838
839/* ************************************************************************ */
840/* geom offset from body */
841
842void dGeomCreateOffset (dxGeom *g)
843{
844 dAASSERT (g);
845 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
846 dUASSERT (g->body, "geom must be on a body");
847 if (g->offset_posr)
848 {
849 return; // already created
850 }
851 dIASSERT (g->final_posr == &g->body->posr);
852
853 g->final_posr = dAllocPosr();
854 g->offset_posr = dAllocPosr();
855 dSetZero (g->offset_posr->pos,4);
856 dRSetIdentity (g->offset_posr->R);
857
858 g->gflags |= GEOM_POSR_BAD;
859}
860
861void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z)
862{
863 dAASSERT (g);
864 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
865 dUASSERT (g->body, "geom must be on a body");
866 CHECK_NOT_LOCKED (g->parent_space);
867 if (!g->offset_posr)
868 {
869 dGeomCreateOffset(g);
870 }
871 g->offset_posr->pos[0] = x;
872 g->offset_posr->pos[1] = y;
873 g->offset_posr->pos[2] = z;
874 dGeomMoved (g);
875}
876
877void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R)
878{
879 dAASSERT (g && R);
880 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
881 dUASSERT (g->body, "geom must be on a body");
882 CHECK_NOT_LOCKED (g->parent_space);
883 if (!g->offset_posr)
884 {
885 dGeomCreateOffset (g);
886 }
887 memcpy (g->offset_posr->R,R,sizeof(dMatrix3));
888 dGeomMoved (g);
889}
890
891void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat)
892{
893 dAASSERT (g && quat);
894 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
895 dUASSERT (g->body, "geom must be on a body");
896 CHECK_NOT_LOCKED (g->parent_space);
897 if (!g->offset_posr)
898 {
899 dGeomCreateOffset (g);
900 }
901 dQtoR (quat, g->offset_posr->R);
902 dGeomMoved (g);
903}
904
905void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z)
906{
907 dAASSERT (g);
908 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
909 dUASSERT (g->body, "geom must be on a body");
910 CHECK_NOT_LOCKED (g->parent_space);
911 if (!g->offset_posr)
912 {
913 dGeomCreateOffset(g);
914 }
915 dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos);
916 dGeomMoved (g);
917}
918
919void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R)
920{
921 dAASSERT (g && R);
922 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
923 dUASSERT (g->body, "geom must be on a body");
924 CHECK_NOT_LOCKED (g->parent_space);
925 if (!g->offset_posr)
926 {
927 dGeomCreateOffset (g);
928 }
929 g->recomputePosr();
930
931 dxPosR new_final_posr;
932 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
933 memcpy(new_final_posr.R, R, sizeof(dMatrix3));
934
935 getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
936 dGeomMoved (g);
937}
938
939void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat)
940{
941 dAASSERT (g && quat);
942 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
943 dUASSERT (g->body, "geom must be on a body");
944 CHECK_NOT_LOCKED (g->parent_space);
945 if (!g->offset_posr)
946 {
947 dGeomCreateOffset (g);
948 }
949
950 g->recomputePosr();
951
952 dxPosR new_final_posr;
953 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
954 dQtoR (quat, new_final_posr.R);
955
956 getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
957 dGeomMoved (g);
958}
959
960void dGeomClearOffset(dxGeom *g)
961{
962 dAASSERT (g);
963 dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
964 if (g->offset_posr)
965 {
966 dIASSERT(g->body);
967 // no longer need an offset posr
968 dFreePosr(g->offset_posr);
969 g->offset_posr = 0;
970 // the geom will now share the position of the body
971 dFreePosr(g->final_posr);
972 g->final_posr = &g->body->posr;
973 // geom has moved
974 g->gflags &= ~GEOM_POSR_BAD;
975 dGeomMoved (g);
976 }
977}
978
979int dGeomIsOffset(dxGeom *g)
980{
981 dAASSERT (g);
982 return ((0 != g->offset_posr) ? 1 : 0);
983}
984
985static const dVector3 OFFSET_POSITION_ZERO = { 0.0f, 0.0f, 0.0f, 0.0f };
986
987const dReal * dGeomGetOffsetPosition (dxGeom *g)
988{
989 dAASSERT (g);
990 if (g->offset_posr)
991 {
992 return g->offset_posr->pos;
993 }
994 return OFFSET_POSITION_ZERO;
995}
996
997void dGeomCopyOffsetPosition (dxGeom *g, dVector3 pos)
998{
999 dAASSERT (g);
1000 if (g->offset_posr)
1001 {
1002 const dReal* src = g->offset_posr->pos;
1003 pos[0] = src[0];
1004 pos[1] = src[1];
1005 pos[2] = src[2];
1006 }
1007 else
1008 {
1009 pos[0] = 0;
1010 pos[1] = 0;
1011 pos[2] = 0;
1012 }
1013}
1014
1015static const dMatrix3 OFFSET_ROTATION_ZERO =
1016{
1017 1.0f, 0.0f, 0.0f, 0.0f,
1018 0.0f, 1.0f, 0.0f, 0.0f,
1019 0.0f, 0.0f, 1.0f, 0.0f,
1020};
1021
1022const dReal * dGeomGetOffsetRotation (dxGeom *g)
1023{
1024 dAASSERT (g);
1025 if (g->offset_posr)
1026 {
1027 return g->offset_posr->R;
1028 }
1029 return OFFSET_ROTATION_ZERO;
1030}
1031
1032void dGeomCopyOffsetRotation (dxGeom *g, dMatrix3 R)
1033{
1034 dAASSERT (g);
1035 if (g->offset_posr)
1036 {
1037 const dReal* src = g->final_posr->R;
1038 R[0] = src[0];
1039 R[1] = src[1];
1040 R[2] = src[2];
1041 R[4] = src[4];
1042 R[5] = src[5];
1043 R[6] = src[6];
1044 R[8] = src[8];
1045 R[9] = src[9];
1046 R[10] = src[10];
1047 }
1048 else
1049 {
1050 R[0] = OFFSET_ROTATION_ZERO[0];
1051 R[1] = OFFSET_ROTATION_ZERO[1];
1052 R[2] = OFFSET_ROTATION_ZERO[2];
1053 R[4] = OFFSET_ROTATION_ZERO[4];
1054 R[5] = OFFSET_ROTATION_ZERO[5];
1055 R[6] = OFFSET_ROTATION_ZERO[6];
1056 R[8] = OFFSET_ROTATION_ZERO[8];
1057 R[9] = OFFSET_ROTATION_ZERO[9];
1058 R[10] = OFFSET_ROTATION_ZERO[10];
1059 }
1060}
1061
1062void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result)
1063{
1064 dAASSERT (g);
1065 if (g->offset_posr)
1066 {
1067 dRtoQ (g->offset_posr->R, result);
1068 }
1069 else
1070 {
1071 dSetZero (result,4);
1072 result[0] = 1;
1073 }
1074}
1075
1076//****************************************************************************
1077// initialization and shutdown routines - allocate and initialize data,
1078// cleanup before exiting
1079
1080extern void opcode_collider_cleanup();
1081
1082void dInitODE()
1083{
1084#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
1085 gimpact_init();
1086#endif
1087}
1088
1089void dCloseODE()
1090{
1091 colliders_initialized = 0;
1092 num_user_classes = 0;
1093 dClearPosrCache();
1094
1095#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
1096 gimpact_terminate();
1097#endif
1098
1099#if dTRIMESH_ENABLED && dTRIMESH_OPCODE
1100 // Free up static allocations in opcode
1101 opcode_collider_cleanup();
1102#endif
1103}
diff --git a/libraries/ode-0.9/ode/src/collision_kernel.h b/libraries/ode-0.9/ode/src/collision_kernel.h
new file mode 100644
index 0000000..d5a2bc4
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_kernel.h
@@ -0,0 +1,214 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25internal data structures and functions for collision detection.
26
27*/
28
29#ifndef _ODE_COLLISION_KERNEL_H_
30#define _ODE_COLLISION_KERNEL_H_
31
32#include <ode/common.h>
33#include <ode/contact.h>
34#include <ode/collision.h>
35#include "objects.h"
36
37//****************************************************************************
38// constants and macros
39
40// mask for the number-of-contacts field in the dCollide() flags parameter
41#define NUMC_MASK (0xffff)
42
43#define IS_SPACE(geom) \
44 ((geom)->type >= dFirstSpaceClass && (geom)->type <= dLastSpaceClass)
45
46//****************************************************************************
47// geometry object base class
48
49
50// geom flags.
51//
52// GEOM_DIRTY means that the space data structures for this geom are
53// potentially not up to date. NOTE THAT all space parents of a dirty geom
54// are themselves dirty. this is an invariant that must be enforced.
55//
56// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date.
57// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might
58// recalculate its own AABB but does not know how to update the space data
59// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY.
60// the valid combinations are:
61// 0
62// GEOM_DIRTY
63// GEOM_DIRTY|GEOM_AABB_BAD
64// GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD
65
66enum {
67 GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown
68 GEOM_POSR_BAD = 2, // geom's final posr is not valid
69 GEOM_AABB_BAD = 4, // geom's AABB is not valid
70 GEOM_PLACEABLE = 8, // geom is placeable
71 GEOM_ENABLED = 16, // geom is enabled
72
73 // Ray specific
74 RAY_FIRSTCONTACT = 0x10000,
75 RAY_BACKFACECULL = 0x20000,
76 RAY_CLOSEST_HIT = 0x40000
77};
78
79
80// geometry object base class. pos and R will either point to a separately
81// allocated buffer (if body is 0 - pos points to the dxPosR object) or to
82// the pos and R of the body (if body nonzero).
83// a dGeomID is a pointer to this object.
84
85struct dxGeom : public dBase {
86 int type; // geom type number, set by subclass constructor
87 int gflags; // flags used by geom and space
88 void *data; // user-defined data pointer
89 dBodyID body; // dynamics body associated with this object (if any)
90 dxGeom *body_next; // next geom in body's linked list of associated geoms
91 dxPosR *final_posr; // final position of the geom in world coordinates
92 dxPosR *offset_posr; // offset from body in local coordinates
93
94 // information used by spaces
95 dxGeom *next; // next geom in linked list of geoms
96 dxGeom **tome; // linked list backpointer
97 dxSpace *parent_space;// the space this geom is contained in, 0 if none
98 dReal aabb[6]; // cached AABB for this space
99 unsigned long category_bits,collide_bits;
100
101 dxGeom (dSpaceID _space, int is_placeable);
102 virtual ~dxGeom();
103
104
105 // calculate our new final position from our offset and body
106 void computePosr();
107
108 // recalculate our new final position if needed
109 void recomputePosr()
110 {
111 if (gflags & GEOM_POSR_BAD) {
112 computePosr();
113 gflags &= ~GEOM_POSR_BAD;
114 }
115 }
116
117 virtual void computeAABB()=0;
118 // compute the AABB for this object and put it in aabb. this function
119 // always performs a fresh computation, it does not inspect the
120 // GEOM_AABB_BAD flag.
121
122 virtual int AABBTest (dxGeom *o, dReal aabb[6]);
123 // test whether the given AABB object intersects with this object, return
124 // 1=yes, 0=no. this is used as an early-exit test in the space collision
125 // functions. the default implementation returns 1, which is the correct
126 // behavior if no more detailed implementation can be provided.
127
128 // utility functions
129
130 // compute the AABB only if it is not current. this function manipulates
131 // the GEOM_AABB_BAD flag.
132
133 void recomputeAABB() {
134 if (gflags & GEOM_AABB_BAD) {
135 // our aabb functions assume final_posr is up to date
136 recomputePosr();
137 computeAABB();
138 gflags &= ~GEOM_AABB_BAD;
139 }
140 }
141
142 // add and remove this geom from a linked list maintained by a space.
143
144 void spaceAdd (dxGeom **first_ptr) {
145 next = *first_ptr;
146 tome = first_ptr;
147 if (*first_ptr) (*first_ptr)->tome = &next;
148 *first_ptr = this;
149 }
150 void spaceRemove() {
151 if (next) next->tome = tome;
152 *tome = next;
153 }
154
155 // add and remove this geom from a linked list maintained by a body.
156
157 void bodyAdd (dxBody *b) {
158 body = b;
159 body_next = b->geom;
160 b->geom = this;
161 }
162 void bodyRemove();
163};
164
165//****************************************************************************
166// the base space class
167//
168// the contained geoms are divided into two kinds: clean and dirty.
169// the clean geoms have not moved since they were put in the list,
170// and their AABBs are valid. the dirty geoms have changed position, and
171// their AABBs are may not be valid. the two types are distinguished by the
172// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list.
173
174struct dxSpace : public dxGeom {
175 int count; // number of geoms in this space
176 dxGeom *first; // first geom in list
177 int cleanup; // cleanup mode, 1=destroy geoms on exit
178
179 // cached state for getGeom()
180 int current_index; // only valid if current_geom != 0
181 dxGeom *current_geom; // if 0 then there is no information
182
183 // locking stuff. the space is locked when it is currently traversing its
184 // internal data structures, e.g. in collide() and collide2(). operations
185 // that modify the contents of the space are not permitted when the space
186 // is locked.
187 int lock_count;
188
189 dxSpace (dSpaceID _space);
190 ~dxSpace();
191
192 void computeAABB();
193
194 void setCleanup (int mode);
195 int getCleanup();
196 int query (dxGeom *geom);
197 int getNumGeoms();
198 virtual dxGeom *getGeom (int i);
199
200 virtual void add (dxGeom *);
201 virtual void remove (dxGeom *);
202 virtual void dirty (dxGeom *);
203
204 virtual void cleanGeoms()=0;
205 // turn all dirty geoms into clean geoms by computing their AABBs and any
206 // other space data structures that are required. this should clear the
207 // GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms.
208
209 virtual void collide (void *data, dNearCallback *callback)=0;
210 virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0;
211};
212
213
214#endif
diff --git a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp b/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp
new file mode 100644
index 0000000..86a1833
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp
@@ -0,0 +1,584 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// QuadTreeSpace by Erwin de Vries.
24
25#include <ode/common.h>
26#include <ode/matrix.h>
27#include <ode/collision_space.h>
28#include <ode/collision.h>
29#include "collision_kernel.h"
30
31#include "collision_space_internal.h"
32
33
34#define AXIS0 0
35#define AXIS1 1
36#define UP 2
37
38//#define DRAWBLOCKS
39
40const int SPLITAXIS = 2;
41const int SPLITS = SPLITAXIS * SPLITAXIS;
42
43#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED
44
45class Block{
46public:
47 dReal MinX, MaxX;
48 dReal MinZ, MaxZ;
49
50 dGeomID First;
51 int GeomCount;
52
53 Block* Parent;
54 Block* Children;
55
56 void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks);
57
58 void Collide(void* UserData, dNearCallback* Callback);
59 void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback);
60
61 void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback);
62
63 void AddObject(dGeomID Object);
64 void DelObject(dGeomID Object);
65 void Traverse(dGeomID Object);
66
67 bool Inside(const dReal* AABB);
68
69 Block* GetBlock(const dReal* AABB);
70 Block* GetBlockChild(const dReal* AABB);
71};
72
73
74#ifdef DRAWBLOCKS
75#include "..\..\Include\drawstuff\\drawstuff.h"
76
77static void DrawBlock(Block* Block){
78 dVector3 v[8];
79 v[0][AXIS0] = Block->MinX;
80 v[0][UP] = REAL(-1.0);
81 v[0][AXIS1] = Block->MinZ;
82
83 v[1][AXIS0] = Block->MinX;
84 v[1][UP] = REAL(-1.0);
85 v[1][AXIS1] = Block->MaxZ;
86
87 v[2][AXIS0] = Block->MaxX;
88 v[2][UP] = REAL(-1.0);
89 v[2][AXIS1] = Block->MinZ;
90
91 v[3][AXIS0] = Block->MaxX;
92 v[3][UP] = REAL(-1.0);
93 v[3][AXIS1] = Block->MaxZ;
94
95 v[4][AXIS0] = Block->MinX;
96 v[4][UP] = REAL(1.0);
97 v[4][AXIS1] = Block->MinZ;
98
99 v[5][AXIS0] = Block->MinX;
100 v[5][UP] = REAL(1.0);
101 v[5][AXIS1] = Block->MaxZ;
102
103 v[6][AXIS0] = Block->MaxX;
104 v[6][UP] = REAL(1.0);
105 v[6][AXIS1] = Block->MinZ;
106
107 v[7][AXIS0] = Block->MaxX;
108 v[7][UP] = REAL(1.0);
109 v[7][AXIS1] = Block->MaxZ;
110
111 // Bottom
112 dsDrawLine(v[0], v[1]);
113 dsDrawLine(v[1], v[3]);
114 dsDrawLine(v[3], v[2]);
115 dsDrawLine(v[2], v[0]);
116
117 // Top
118 dsDrawLine(v[4], v[5]);
119 dsDrawLine(v[5], v[7]);
120 dsDrawLine(v[7], v[6]);
121 dsDrawLine(v[6], v[4]);
122
123 // Sides
124 dsDrawLine(v[0], v[4]);
125 dsDrawLine(v[1], v[5]);
126 dsDrawLine(v[2], v[6]);
127 dsDrawLine(v[3], v[7]);
128}
129#endif //DRAWBLOCKS
130
131
132void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){
133 GeomCount = 0;
134 First = 0;
135
136 MinX = Center[AXIS0] - Extents[AXIS0];
137 MaxX = Center[AXIS0] + Extents[AXIS0];
138
139 MinZ = Center[AXIS1] - Extents[AXIS1];
140 MaxZ = Center[AXIS1] + Extents[AXIS1];
141
142 this->Parent = Parent;
143 if (Depth > 0){
144 Children = Blocks;
145 Blocks += SPLITS;
146
147 dVector3 ChildExtents;
148 ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS;
149 ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS;
150 ChildExtents[UP] = Extents[UP];
151
152 for (int i = 0; i < SPLITAXIS; i++){
153 for (int j = 0; j < SPLITAXIS; j++){
154 int Index = i * SPLITAXIS + j;
155
156 dVector3 ChildCenter;
157 ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2);
158 ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2);
159 ChildCenter[UP] = Center[UP];
160
161 Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks);
162 }
163 }
164 }
165 else Children = 0;
166}
167
168void Block::Collide(void* UserData, dNearCallback* Callback){
169#ifdef DRAWBLOCKS
170 DrawBlock(this);
171#endif
172 // Collide the local list
173 dxGeom* g = First;
174 while (g){
175 if (GEOM_ENABLED(g)){
176 Collide(g, g->next, UserData, Callback);
177 }
178 g = g->next;
179 }
180
181 // Recurse for children
182 if (Children){
183 for (int i = 0; i < SPLITS; i++){
184 if (Children[i].GeomCount <= 1){ // Early out
185 continue;
186 }
187 Children[i].Collide(UserData, Callback);
188 }
189 }
190}
191
192void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
193#ifdef DRAWBLOCKS
194 DrawBlock(this);
195#endif
196 // Collide against local list
197 while (g2){
198 if (GEOM_ENABLED(g2)){
199 collideAABBs (g1, g2, UserData, Callback);
200 }
201 g2 = g2->next;
202 }
203
204 // Collide against children
205 if (Children){
206 for (int i = 0; i < SPLITS; i++){
207 // Early out for empty blocks
208 if (Children[i].GeomCount == 0){
209 continue;
210 }
211
212 // Does the geom's AABB collide with the block?
213 // Dont do AABB tests for single geom blocks.
214 if (Children[i].GeomCount == 1 && Children[i].First){
215 //
216 }
217 else if (true){
218 if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||
219 g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||
220 g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||
221 g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;
222 }
223 Children[i].Collide(g1, Children[i].First, UserData, Callback);
224 }
225 }
226}
227
228void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){
229 // Collide against local list
230 dxGeom* g2 = First;
231 while (g2){
232 if (GEOM_ENABLED(g2)){
233 collideAABBs (g1, g2, UserData, Callback);
234 }
235 g2 = g2->next;
236 }
237}
238
239void Block::AddObject(dGeomID Object){
240 // Add the geom
241 Object->next = First;
242 First = Object;
243 Object->tome = (dxGeom**)this;
244
245 // Now traverse upwards to tell that we have a geom
246 Block* Block = this;
247 do{
248 Block->GeomCount++;
249 Block = Block->Parent;
250 }
251 while (Block);
252}
253
254void Block::DelObject(dGeomID Object){
255 // Del the geom
256 dxGeom* g = First;
257 dxGeom* Last = 0;
258 while (g){
259 if (g == Object){
260 if (Last){
261 Last->next = g->next;
262 }
263 else First = g->next;
264
265 break;
266 }
267 Last = g;
268 g = g->next;
269 }
270
271 Object->tome = 0;
272
273 // Now traverse upwards to tell that we have lost a geom
274 Block* Block = this;
275 do{
276 Block->GeomCount--;
277 Block = Block->Parent;
278 }
279 while (Block);
280}
281
282void Block::Traverse(dGeomID Object){
283 Block* NewBlock = GetBlock(Object->aabb);
284
285 if (NewBlock != this){
286 // Remove the geom from the old block and add it to the new block.
287 // This could be more optimal, but the loss should be very small.
288 DelObject(Object);
289 NewBlock->AddObject(Object);
290 }
291}
292
293bool Block::Inside(const dReal* AABB){
294 return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ;
295}
296
297Block* Block::GetBlock(const dReal* AABB){
298 if (Inside(AABB)){
299 return GetBlockChild(AABB); // Child or this will have a good block
300 }
301 else if (Parent){
302 return Parent->GetBlock(AABB); // Parent has a good block
303 }
304 else return this; // We are at the root, so we have little choice
305}
306
307Block* Block::GetBlockChild(const dReal* AABB){
308 if (Children){
309 for (int i = 0; i < SPLITS; i++){
310 if (Children[i].Inside(AABB)){
311 return Children[i].GetBlockChild(AABB); // Child will have good block
312 }
313 }
314 }
315 return this; // This is the best block
316}
317
318//****************************************************************************
319// quadtree space
320
321struct dxQuadTreeSpace : public dxSpace{
322 Block* Blocks; // Blocks[0] is the root
323
324 dArray<dxGeom*> DirtyList;
325
326 dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth);
327 ~dxQuadTreeSpace();
328
329 dxGeom* getGeom(int i);
330
331 void add(dxGeom* g);
332 void remove(dxGeom* g);
333 void dirty(dxGeom* g);
334
335 void computeAABB();
336
337 void cleanGeoms();
338 void collide(void* UserData, dNearCallback* Callback);
339 void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
340
341 // Temp data
342 Block* CurrentBlock; // Only used while enumerating
343 int* CurrentChild; // Only used while enumerating
344 int CurrentLevel; // Only used while enumerating
345 dxGeom* CurrentObject; // Only used while enumerating
346 int CurrentIndex;
347};
348
349dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){
350 type = dQuadTreeSpaceClass;
351
352 int BlockCount = 0;
353 for (int i = 0; i <= Depth; i++){
354 BlockCount += (int)pow((dReal)SPLITS, i);
355 }
356
357 Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
358 Block* Blocks = this->Blocks + 1; // This pointer gets modified!
359
360 this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);
361
362 CurrentBlock = 0;
363 CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
364 CurrentLevel = 0;
365 CurrentObject = 0;
366 CurrentIndex = -1;
367
368 // 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
369 aabb[0] = -dInfinity;
370 aabb[1] = dInfinity;
371 aabb[2] = -dInfinity;
372 aabb[3] = dInfinity;
373 aabb[4] = -dInfinity;
374 aabb[5] = dInfinity;
375}
376
377dxQuadTreeSpace::~dxQuadTreeSpace(){
378 int Depth = 0;
379 Block* Current = &Blocks[0];
380 while (Current){
381 Depth++;
382 Current = Current->Children;
383 }
384
385 int BlockCount = 0;
386 for (int i = 0; i < Depth; i++){
387 BlockCount += (int)pow((dReal)SPLITS, i);
388 }
389
390 dFree(Blocks, BlockCount * sizeof(Block));
391 dFree(CurrentChild, (Depth + 1) * sizeof(int));
392}
393
394dxGeom* dxQuadTreeSpace::getGeom(int Index){
395 dUASSERT(Index >= 0 && Index < count, "index out of range");
396
397 //@@@
398 dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
399
400 return 0;
401
402 // This doesnt work
403
404 /*if (CurrentIndex == Index){
405 // Loop through all objects in the local list
406CHILDRECURSE:
407 if (CurrentObject){
408 dGeomID g = CurrentObject;
409 CurrentObject = CurrentObject->next;
410 CurrentIndex++;
411
412#ifdef DRAWBLOCKS
413 DrawBlock(CurrentBlock);
414#endif //DRAWBLOCKS
415 return g;
416 }
417 else{
418 // Now lets loop through our children. Starting at index 0.
419 if (CurrentBlock->Children){
420 CurrentChild[CurrentLevel] = 0;
421PARENTRECURSE:
422 for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
423 if (CurrentBlock->Children[i].GeomCount == 0){
424 continue;
425 }
426 CurrentBlock = &CurrentBlock->Children[i];
427 CurrentObject = CurrentBlock->First;
428
429 i++;
430
431 CurrentLevel++;
432 goto CHILDRECURSE;
433 }
434 }
435 }
436
437 // Now lets go back to the parent so it can continue processing its other children.
438 if (CurrentBlock->Parent){
439 CurrentBlock = CurrentBlock->Parent;
440 CurrentLevel--;
441 goto PARENTRECURSE;
442 }
443 }
444 else{
445 CurrentBlock = &Blocks[0];
446 CurrentLevel = 0;
447 CurrentObject = CurrentObject;
448 CurrentIndex = 0;
449
450 // Other states are already set
451 CurrentObject = CurrentBlock->First;
452 }
453
454
455 if (current_geom && current_index == Index - 1){
456 //current_geom = current_geom->next; // next
457 current_index = Index;
458 return current_geom;
459 }
460 else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow
461 getGeom(i);
462 }*/
463
464 return 0;
465}
466
467void dxQuadTreeSpace::add(dxGeom* g){
468 CHECK_NOT_LOCKED (this);
469 dAASSERT(g);
470 dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");
471
472 g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
473 DirtyList.push(g);
474
475 // add
476 g->parent_space = this;
477 Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block
478 count++;
479
480 // enumerator has been invalidated
481 current_geom = 0;
482
483 dGeomMoved(this);
484}
485
486void dxQuadTreeSpace::remove(dxGeom* g){
487 CHECK_NOT_LOCKED(this);
488 dAASSERT(g);
489 dUASSERT(g->parent_space == this,"object is not in this space");
490
491 // remove
492 ((Block*)g->tome)->DelObject(g);
493 count--;
494
495 for (int i = 0; i < DirtyList.size(); i++){
496 if (DirtyList[i] == g){
497 DirtyList.remove(i);
498 // (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
499 --i;
500 }
501 }
502
503 // safeguard
504 g->next = 0;
505 g->tome = 0;
506 g->parent_space = 0;
507
508 // enumerator has been invalidated
509 current_geom = 0;
510
511 // the bounding box of this space (and that of all the parents) may have
512 // changed as a consequence of the removal.
513 dGeomMoved(this);
514}
515
516void dxQuadTreeSpace::dirty(dxGeom* g){
517 DirtyList.push(g);
518}
519
520void dxQuadTreeSpace::computeAABB(){
521 //
522}
523
524void dxQuadTreeSpace::cleanGeoms(){
525 // compute the AABBs of all dirty geoms, and clear the dirty flags
526 lock_count++;
527
528 for (int i = 0; i < DirtyList.size(); i++){
529 dxGeom* g = DirtyList[i];
530 if (IS_SPACE(g)){
531 ((dxSpace*)g)->cleanGeoms();
532 }
533 g->recomputeAABB();
534 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
535
536 ((Block*)g->tome)->Traverse(g);
537 }
538 DirtyList.setSize(0);
539
540 lock_count--;
541}
542
543void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
544 dAASSERT(Callback);
545
546 lock_count++;
547 cleanGeoms();
548
549 Blocks[0].Collide(UserData, Callback);
550
551 lock_count--;
552}
553
554void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){
555 dAASSERT(g1 && Callback);
556
557 lock_count++;
558 cleanGeoms();
559 g1->recomputeAABB();
560
561 if (g1->parent_space == this){
562 // The block the geom is in
563 Block* CurrentBlock = (Block*)g1->tome;
564
565 // Collide against block and its children
566 CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback);
567
568 // Collide against parents
569 while (true){
570 CurrentBlock = CurrentBlock->Parent;
571 if (!CurrentBlock){
572 break;
573 }
574 CurrentBlock->CollideLocal(g1, UserData, Callback);
575 }
576 }
577 else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback);
578
579 lock_count--;
580}
581
582dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){
583 return new dxQuadTreeSpace(space, Center, Extents, Depth);
584}
diff --git a/libraries/ode-0.9/ode/src/collision_space.cpp b/libraries/ode-0.9/ode/src/collision_space.cpp
new file mode 100644
index 0000000..1a8fc81
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_space.cpp
@@ -0,0 +1,790 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25spaces
26
27*/
28
29#include <ode/common.h>
30#include <ode/matrix.h>
31#include <ode/collision_space.h>
32#include <ode/collision.h>
33#include "collision_kernel.h"
34
35#include "collision_space_internal.h"
36
37#ifdef _MSC_VER
38#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
39#endif
40
41//****************************************************************************
42// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
43// and moving it to the front of the space's list. all the parents of a
44// dirty geom also become dirty.
45
46void dGeomMoved (dxGeom *geom)
47{
48 dAASSERT (geom);
49
50 // if geom is offset, mark it as needing a calculate
51 if (geom->offset_posr) {
52 geom->gflags |= GEOM_POSR_BAD;
53 }
54
55 // from the bottom of the space heirarchy up, process all clean geoms
56 // turning them into dirty geoms.
57 dxSpace *parent = geom->parent_space;
58
59 while (parent && (geom->gflags & GEOM_DIRTY)==0) {
60 CHECK_NOT_LOCKED (parent);
61 geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
62 parent->dirty (geom);
63 geom = parent;
64 parent = parent->parent_space;
65 }
66
67 // all the remaining dirty geoms must have their AABB_BAD flags set, to
68 // ensure that their AABBs get recomputed
69 while (geom) {
70 geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
71 CHECK_NOT_LOCKED (geom->parent_space);
72 geom = geom->parent_space;
73 }
74}
75
76#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED)
77
78//****************************************************************************
79// dxSpace
80
81dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)
82{
83 count = 0;
84 first = 0;
85 cleanup = 1;
86 current_index = 0;
87 current_geom = 0;
88 lock_count = 0;
89}
90
91
92dxSpace::~dxSpace()
93{
94 CHECK_NOT_LOCKED (this);
95 if (cleanup) {
96 // note that destroying each geom will call remove()
97 dxGeom *g,*n;
98 for (g = first; g; g=n) {
99 n = g->next;
100 dGeomDestroy (g);
101 }
102 }
103 else {
104 dxGeom *g,*n;
105 for (g = first; g; g=n) {
106 n = g->next;
107 remove (g);
108 }
109 }
110}
111
112
113void dxSpace::computeAABB()
114{
115 if (first) {
116 int i;
117 dReal a[6];
118 a[0] = dInfinity;
119 a[1] = -dInfinity;
120 a[2] = dInfinity;
121 a[3] = -dInfinity;
122 a[4] = dInfinity;
123 a[5] = -dInfinity;
124 for (dxGeom *g=first; g; g=g->next) {
125 g->recomputeAABB();
126 for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];
127 for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];
128 }
129 memcpy(aabb,a,6*sizeof(dReal));
130 }
131 else {
132 dSetZero (aabb,6);
133 }
134}
135
136
137void dxSpace::setCleanup (int mode)
138{
139 cleanup = (mode != 0);
140}
141
142
143int dxSpace::getCleanup()
144{
145 return cleanup;
146}
147
148
149int dxSpace::query (dxGeom *geom)
150{
151 dAASSERT (geom);
152 return (geom->parent_space == this);
153}
154
155
156int dxSpace::getNumGeoms()
157{
158 return count;
159}
160
161
162// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1
163
164dxGeom *dxSpace::getGeom (int i)
165{
166 dUASSERT (i >= 0 && i < count,"index out of range");
167 if (current_geom && current_index == i-1) {
168 current_geom = current_geom->next;
169 current_index = i;
170 return current_geom;
171 }
172 else {
173 dxGeom *g=first;
174 for (int j=0; j<i; j++) {
175 if (g) g = g->next; else return 0;
176 }
177 current_geom = g;
178 current_index = i;
179 return g;
180 }
181}
182
183
184void dxSpace::add (dxGeom *geom)
185{
186 CHECK_NOT_LOCKED (this);
187 dAASSERT (geom);
188 dUASSERT (geom->parent_space == 0 && geom->next == 0,
189 "geom is already in a space");
190
191 // add
192 geom->parent_space = this;
193 geom->spaceAdd (&first);
194 count++;
195
196 // enumerator has been invalidated
197 current_geom = 0;
198
199 // new geoms are added to the front of the list and are always
200 // considered to be dirty. as a consequence, this space and all its
201 // parents are dirty too.
202 geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
203 dGeomMoved (this);
204}
205
206
207void dxSpace::remove (dxGeom *geom)
208{
209 CHECK_NOT_LOCKED (this);
210 dAASSERT (geom);
211 dUASSERT (geom->parent_space == this,"object is not in this space");
212
213 // remove
214 geom->spaceRemove();
215 count--;
216
217 // safeguard
218 geom->next = 0;
219 geom->tome = 0;
220 geom->parent_space = 0;
221
222 // enumerator has been invalidated
223 current_geom = 0;
224
225 // the bounding box of this space (and that of all the parents) may have
226 // changed as a consequence of the removal.
227 dGeomMoved (this);
228}
229
230
231void dxSpace::dirty (dxGeom *geom)
232{
233 geom->spaceRemove();
234 geom->spaceAdd (&first);
235}
236
237//****************************************************************************
238// simple space - reports all n^2 object intersections
239
240struct dxSimpleSpace : public dxSpace {
241 dxSimpleSpace (dSpaceID _space);
242 void cleanGeoms();
243 void collide (void *data, dNearCallback *callback);
244 void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
245};
246
247
248dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)
249{
250 type = dSimpleSpaceClass;
251}
252
253
254void dxSimpleSpace::cleanGeoms()
255{
256 // compute the AABBs of all dirty geoms, and clear the dirty flags
257 lock_count++;
258 for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
259 if (IS_SPACE(g)) {
260 ((dxSpace*)g)->cleanGeoms();
261 }
262 g->recomputeAABB();
263 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
264 }
265 lock_count--;
266}
267
268
269void dxSimpleSpace::collide (void *data, dNearCallback *callback)
270{
271 dAASSERT (callback);
272
273 lock_count++;
274 cleanGeoms();
275
276 // intersect all bounding boxes
277 for (dxGeom *g1=first; g1; g1=g1->next) {
278 if (GEOM_ENABLED(g1)){
279 for (dxGeom *g2=g1->next; g2; g2=g2->next) {
280 if (GEOM_ENABLED(g2)){
281 collideAABBs (g1,g2,data,callback);
282 }
283 }
284 }
285 }
286
287 lock_count--;
288}
289
290
291void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
292 dNearCallback *callback)
293{
294 dAASSERT (geom && callback);
295
296 lock_count++;
297 cleanGeoms();
298 geom->recomputeAABB();
299
300 // intersect bounding boxes
301 for (dxGeom *g=first; g; g=g->next) {
302 if (GEOM_ENABLED(g)){
303 collideAABBs (g,geom,data,callback);
304 }
305 }
306
307 lock_count--;
308}
309
310//****************************************************************************
311// utility stuff for hash table space
312
313// kind of silly, but oh well...
314#ifndef MAXINT
315#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))
316#endif
317
318
319// prime[i] is the largest prime smaller than 2^i
320#define NUM_PRIMES 31
321static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
322 1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
323 524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
324 67108859L,134217689L,268435399L,536870909L,1073741789L};
325
326
327// an axis aligned bounding box in the hash table
328struct dxAABB {
329 dxAABB *next; // next in the list of all AABBs
330 int level; // the level this is stored in (cell size = 2^level)
331 int dbounds[6]; // AABB bounds, discretized to cell size
332 dxGeom *geom; // corresponding geometry object (AABB stored here)
333 int index; // index of this AABB, starting from 0
334};
335
336
337// a hash table node that represents an AABB that intersects a particular cell
338// at a particular level
339struct Node {
340 Node *next; // next node in hash table collision list, 0 if none
341 int x,y,z; // cell position in space, discretized to cell size
342 dxAABB *aabb; // axis aligned bounding box that intersects this cell
343};
344
345
346// return the `level' of an AABB. the AABB will be put into cells at this
347// level - the cell size will be 2^level. the level is chosen to be the
348// smallest value such that the AABB occupies no more than 8 cells, regardless
349// of its placement. this means that:
350// size/2 < q <= size
351// where q is the maximum AABB dimension.
352
353static int findLevel (dReal bounds[6])
354{
355 if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
356 bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
357 bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
358 return MAXINT;
359 }
360
361 // compute q
362 dReal q,q2;
363 q = bounds[1] - bounds[0]; // x bounds
364 q2 = bounds[3] - bounds[2]; // y bounds
365 if (q2 > q) q = q2;
366 q2 = bounds[5] - bounds[4]; // z bounds
367 if (q2 > q) q = q2;
368
369 // find level such that 0.5 * 2^level < q <= 2^level
370 int level;
371 frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp)
372 return level;
373}
374
375
376// find a virtual memory address for a cell at the given level and x,y,z
377// position.
378// @@@ currently this is not very sophisticated, e.g. the scaling
379// factors could be better designed to avoid collisions, and they should
380// probably depend on the hash table physical size.
381
382static unsigned long getVirtualAddress (int level, int x, int y, int z)
383{
384 return level*1000 + x*100 + y*10 + z;
385}
386
387//****************************************************************************
388// hash space
389
390struct dxHashSpace : public dxSpace {
391 int global_minlevel; // smallest hash table level to put AABBs in
392 int global_maxlevel; // objects that need a level larger than this will be
393 // put in a "big objects" list instead of a hash table
394
395 dxHashSpace (dSpaceID _space);
396 void setLevels (int minlevel, int maxlevel);
397 void getLevels (int *minlevel, int *maxlevel);
398 void cleanGeoms();
399 void collide (void *data, dNearCallback *callback);
400 void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
401};
402
403
404dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space)
405{
406 type = dHashSpaceClass;
407 global_minlevel = -3;
408 global_maxlevel = 10;
409}
410
411
412void dxHashSpace::setLevels (int minlevel, int maxlevel)
413{
414 dAASSERT (minlevel <= maxlevel);
415 global_minlevel = minlevel;
416 global_maxlevel = maxlevel;
417}
418
419
420void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
421{
422 if (minlevel) *minlevel = global_minlevel;
423 if (maxlevel) *maxlevel = global_maxlevel;
424}
425
426
427void dxHashSpace::cleanGeoms()
428{
429 // compute the AABBs of all dirty geoms, and clear the dirty flags
430 lock_count++;
431 for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
432 if (IS_SPACE(g)) {
433 ((dxSpace*)g)->cleanGeoms();
434 }
435 g->recomputeAABB();
436 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
437 }
438 lock_count--;
439}
440
441
442void dxHashSpace::collide (void *data, dNearCallback *callback)
443{
444 dAASSERT(this && callback);
445 dxGeom *geom;
446 dxAABB *aabb;
447 int i,maxlevel;
448
449 // 0 or 1 geoms can't collide with anything
450 if (count < 2) return;
451
452 lock_count++;
453 cleanGeoms();
454
455 // create a list of auxiliary information for all geom axis aligned bounding
456 // boxes. set the level for all AABBs. put AABBs larger than the space's
457 // global_maxlevel in the big_boxes list, check everything else against
458 // that list at the end. for AABBs that are not too big, record the maximum
459 // level that we need.
460
461 int n = 0; // number of AABBs in main list
462 dxAABB *first_aabb = 0; // list of AABBs in hash table
463 dxAABB *big_boxes = 0; // list of AABBs too big for hash table
464 maxlevel = global_minlevel - 1;
465 for (geom = first; geom; geom=geom->next) {
466 if (!GEOM_ENABLED(geom)){
467 continue;
468 }
469 dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB));
470 aabb->geom = geom;
471 // compute level, but prevent cells from getting too small
472 int level = findLevel (geom->aabb);
473 if (level < global_minlevel) level = global_minlevel;
474 if (level <= global_maxlevel) {
475 // aabb goes in main list
476 aabb->next = first_aabb;
477 first_aabb = aabb;
478 aabb->level = level;
479 if (level > maxlevel) maxlevel = level;
480 // cellsize = 2^level
481 dReal cellsize = (dReal) ldexp (1.0,level);
482 // discretize AABB position to cell size
483 for (i=0; i < 6; i++) aabb->dbounds[i] = (int)
484 floor (geom->aabb[i]/cellsize);
485 // set AABB index
486 aabb->index = n;
487 n++;
488 }
489 else {
490 // aabb is too big, put it in the big_boxes list. we don't care about
491 // setting level, dbounds, index, or the maxlevel
492 aabb->next = big_boxes;
493 big_boxes = aabb;
494 }
495 }
496
497 // for `n' objects, an n*n array of bits is used to record if those objects
498 // have been intersection-tested against each other yet. this array can
499 // grow large with high n, but oh well...
500 int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits
501 unsigned char *tested = (unsigned char *) ALLOCA (n * tested_rowsize);
502 memset (tested,0,n * tested_rowsize);
503
504 // create a hash table to store all AABBs. each AABB may take up to 8 cells.
505 // we use chaining to resolve collisions, but we use a relatively large table
506 // to reduce the chance of collisions.
507
508 // compute hash table size sz to be a prime > 8*n
509 for (i=0; i<NUM_PRIMES; i++) {
510 if (prime[i] >= (8*n)) break;
511 }
512 if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless
513 int sz = prime[i];
514
515 // allocate and initialize hash table node pointers
516 Node **table = (Node **) ALLOCA (sizeof(Node*) * sz);
517 for (i=0; i<sz; i++) table[i] = 0;
518
519 // add each AABB to the hash table (may need to add it to up to 8 cells)
520 for (aabb=first_aabb; aabb; aabb=aabb->next) {
521 int *dbounds = aabb->dbounds;
522 for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) {
523 for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) {
524 for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) {
525 // get the hash index
526 unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz;
527 // add a new node to the hash table
528 Node *node = (Node*) ALLOCA (sizeof (Node));
529 node->x = xi;
530 node->y = yi;
531 node->z = zi;
532 node->aabb = aabb;
533 node->next = table[hi];
534 table[hi] = node;
535 }
536 }
537 }
538 }
539
540 // now that all AABBs are loaded into the hash table, we do the actual
541 // collision detection. for all AABBs, check for other AABBs in the
542 // same cells for collisions, and then check for other AABBs in all
543 // intersecting higher level cells.
544
545 int db[6]; // discrete bounds at current level
546 for (aabb=first_aabb; aabb; aabb=aabb->next) {
547 // we are searching for collisions with aabb
548 for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
549 for (int level = aabb->level; level <= maxlevel; level++) {
550 for (int xi = db[0]; xi <= db[1]; xi++) {
551 for (int yi = db[2]; yi <= db[3]; yi++) {
552 for (int zi = db[4]; zi <= db[5]; zi++) {
553 // get the hash index
554 unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
555 // search all nodes at this index
556 Node *node;
557 for (node = table[hi]; node; node=node->next) {
558 // node points to an AABB that may intersect aabb
559 if (node->aabb == aabb) continue;
560 if (node->aabb->level == level &&
561 node->x == xi && node->y == yi && node->z == zi) {
562 // see if aabb and node->aabb have already been tested
563 // against each other
564 unsigned char mask;
565 if (aabb->index <= node->aabb->index) {
566 i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
567 mask = 1 << (node->aabb->index & 7);
568 }
569 else {
570 i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
571 mask = 1 << (aabb->index & 7);
572 }
573 dIASSERT (i >= 0 && i < (tested_rowsize*n));
574 if ((tested[i] & mask)==0) {
575 collideAABBs (aabb->geom,node->aabb->geom,data,callback);
576 }
577 tested[i] |= mask;
578 }
579 }
580 }
581 }
582 }
583 // get the discrete bounds for the next level up
584 for (i=0; i<6; i++) db[i] >>= 1;
585 }
586 }
587
588 // every AABB in the normal list must now be intersected against every
589 // AABB in the big_boxes list. so let's hope there are not too many objects
590 // in the big_boxes list.
591 for (aabb=first_aabb; aabb; aabb=aabb->next) {
592 for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) {
593 collideAABBs (aabb->geom,aabb2->geom,data,callback);
594 }
595 }
596
597 // intersected all AABBs in the big_boxes list together
598 for (aabb=big_boxes; aabb; aabb=aabb->next) {
599 for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) {
600 collideAABBs (aabb->geom,aabb2->geom,data,callback);
601 }
602 }
603
604 lock_count--;
605}
606
607
608void dxHashSpace::collide2 (void *data, dxGeom *geom,
609 dNearCallback *callback)
610{
611 dAASSERT (geom && callback);
612
613 // this could take advantage of the hash structure to avoid
614 // O(n2) complexity, but it does not yet.
615
616 lock_count++;
617 cleanGeoms();
618 geom->recomputeAABB();
619
620 // intersect bounding boxes
621 for (dxGeom *g=first; g; g=g->next) {
622 if (GEOM_ENABLED(g)) collideAABBs (g,geom,data,callback);
623 }
624
625 lock_count--;
626}
627
628//****************************************************************************
629// space functions
630
631dxSpace *dSimpleSpaceCreate (dxSpace *space)
632{
633 return new dxSimpleSpace (space);
634}
635
636
637dxSpace *dHashSpaceCreate (dxSpace *space)
638{
639 return new dxHashSpace (space);
640}
641
642
643void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
644{
645 dAASSERT (space);
646 dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
647 dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
648 dxHashSpace *hspace = (dxHashSpace*) space;
649 hspace->setLevels (minlevel,maxlevel);
650}
651
652
653void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel)
654{
655 dAASSERT (space);
656 dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
657 dxHashSpace *hspace = (dxHashSpace*) space;
658 hspace->getLevels (minlevel,maxlevel);
659}
660
661
662void dSpaceDestroy (dxSpace *space)
663{
664 dAASSERT (space);
665 dUASSERT (dGeomIsSpace(space),"argument not a space");
666 dGeomDestroy (space);
667}
668
669
670void dSpaceSetCleanup (dxSpace *space, int mode)
671{
672 dAASSERT (space);
673 dUASSERT (dGeomIsSpace(space),"argument not a space");
674 space->setCleanup (mode);
675}
676
677
678int dSpaceGetCleanup (dxSpace *space)
679{
680 dAASSERT (space);
681 dUASSERT (dGeomIsSpace(space),"argument not a space");
682 return space->getCleanup();
683}
684
685
686void dSpaceAdd (dxSpace *space, dxGeom *g)
687{
688 dAASSERT (space);
689 dUASSERT (dGeomIsSpace(space),"argument not a space");
690 CHECK_NOT_LOCKED (space);
691 space->add (g);
692}
693
694
695void dSpaceRemove (dxSpace *space, dxGeom *g)
696{
697 dAASSERT (space);
698 dUASSERT (dGeomIsSpace(space),"argument not a space");
699 CHECK_NOT_LOCKED (space);
700 space->remove (g);
701}
702
703
704int dSpaceQuery (dxSpace *space, dxGeom *g)
705{
706 dAASSERT (space);
707 dUASSERT (dGeomIsSpace(space),"argument not a space");
708 return space->query (g);
709}
710
711void dSpaceClean (dxSpace *space){
712 dAASSERT (space);
713 dUASSERT (dGeomIsSpace(space),"argument not a space");
714
715 space->cleanGeoms();
716}
717
718int dSpaceGetNumGeoms (dxSpace *space)
719{
720 dAASSERT (space);
721 dUASSERT (dGeomIsSpace(space),"argument not a space");
722 return space->getNumGeoms();
723}
724
725
726dGeomID dSpaceGetGeom (dxSpace *space, int i)
727{
728 dAASSERT (space);
729 dUASSERT (dGeomIsSpace(space),"argument not a space");
730 return space->getGeom (i);
731}
732
733
734void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
735{
736 dAASSERT (space && callback);
737 dUASSERT (dGeomIsSpace(space),"argument not a space");
738 space->collide (data,callback);
739}
740
741
742void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,
743 dNearCallback *callback)
744{
745 dAASSERT (g1 && g2 && callback);
746 dxSpace *s1,*s2;
747
748 // see if either geom is a space
749 if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;
750 if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;
751
752 // handle the four space/geom cases
753 if (s1) {
754 if (s2) {
755 // g1 and g2 are spaces.
756 if (s1==s2) {
757 // collide a space with itself --> interior collision
758 s1->collide (data,callback);
759 }
760 else {
761 // iterate through the space that has the fewest geoms, calling
762 // collide2 in the other space for each one.
763 if (s1->count < s2->count) {
764 for (dxGeom *g = s1->first; g; g=g->next) {
765 s2->collide2 (data,g,callback);
766 }
767 }
768 else {
769 for (dxGeom *g = s2->first; g; g=g->next) {
770 s1->collide2 (data,g,callback);
771 }
772 }
773 }
774 }
775 else {
776 // g1 is a space, g2 is a geom
777 s1->collide2 (data,g2,callback);
778 }
779 }
780 else {
781 if (s2) {
782 // g1 is a geom, g2 is a space
783 s2->collide2 (data,g1,callback);
784 }
785 else {
786 // g1 and g2 are geoms, call the callback directly
787 callback (data,g1,g2);
788 }
789 }
790}
diff --git a/libraries/ode-0.9/ode/src/collision_space_internal.h b/libraries/ode-0.9/ode/src/collision_space_internal.h
new file mode 100644
index 0000000..004276a
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_space_internal.h
@@ -0,0 +1,84 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25stuff common to all spaces
26
27*/
28
29#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_
30#define _ODE_COLLISION_SPACE_INTERNAL_H_
31
32#define ALLOCA(x) dALLOCA16(x)
33
34#define CHECK_NOT_LOCKED(space) \
35 dUASSERT ((space)==0 || (space)->lock_count==0, \
36 "invalid operation for locked space");
37
38
39// collide two geoms together. for the hash table space, this is
40// called if the two AABBs inhabit the same hash table cells.
41// this only calls the callback function if the AABBs actually
42// intersect. if a geom has an AABB test function, that is called to
43// provide a further refinement of the intersection.
44//
45// NOTE: this assumes that the geom AABBs are valid on entry
46// and that both geoms are enabled.
47
48static void collideAABBs (dxGeom *g1, dxGeom *g2,
49 void *data, dNearCallback *callback)
50{
51 dIASSERT((g1->gflags & GEOM_AABB_BAD)==0);
52 dIASSERT((g2->gflags & GEOM_AABB_BAD)==0);
53
54 // no contacts if both geoms on the same body, and the body is not 0
55 if (g1->body == g2->body && g1->body) return;
56
57 // test if the category and collide bitfields match
58 if ( ((g1->category_bits & g2->collide_bits) ||
59 (g2->category_bits & g1->collide_bits)) == 0) {
60 return;
61 }
62
63 // if the bounding boxes are disjoint then don't do anything
64 dReal *bounds1 = g1->aabb;
65 dReal *bounds2 = g2->aabb;
66 if (bounds1[0] > bounds2[1] ||
67 bounds1[1] < bounds2[0] ||
68 bounds1[2] > bounds2[3] ||
69 bounds1[3] < bounds2[2] ||
70 bounds1[4] > bounds2[5] ||
71 bounds1[5] < bounds2[4]) {
72 return;
73 }
74
75 // check if either object is able to prove that it doesn't intersect the
76 // AABB of the other
77 if (g1->AABBTest (g2,bounds2) == 0) return;
78 if (g2->AABBTest (g1,bounds1) == 0) return;
79
80 // the objects might actually intersect - call the space callback function
81 callback (data,g1,g2);
82}
83
84#endif
diff --git a/libraries/ode-0.9/ode/src/collision_std.h b/libraries/ode-0.9/ode/src/collision_std.h
new file mode 100644
index 0000000..d203ad0
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_std.h
@@ -0,0 +1,172 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25the standard ODE geometry primitives.
26
27*/
28
29#ifndef _ODE_COLLISION_STD_H_
30#define _ODE_COLLISION_STD_H_
31
32#include <set>
33#include <ode/common.h>
34#include "collision_kernel.h"
35
36
37// primitive collision functions - these have the dColliderFn interface, i.e.
38// the same interface as dCollide(). the first and second geom arguments must
39// have the specified types.
40
41int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
42 dContactGeom *contact, int skip);
43int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
44 dContactGeom *contact, int skip);
45int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
46 dContactGeom *contact, int skip);
47int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
48 dContactGeom *contact, int skip);
49int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
50 int flags, dContactGeom *contact, int skip);
51int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags,
52 dContactGeom *contact, int skip);
53int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
54 dContactGeom *contact, int skip);
55int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
56 int flags, dContactGeom *contact, int skip);
57int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
58 dContactGeom *contact, int skip);
59int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
60 dContactGeom *contact, int skip);
61int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
62 dContactGeom *contact, int skip);
63int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
64 int flags, dContactGeom *contact, int skip);
65int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
66 dContactGeom *contact, int skip);
67int dCollideRayCylinder (dxGeom *o1, dxGeom *o2, int flags,
68 dContactGeom *contact, int skip);
69
70// Cylinder - Box/Sphere by (C) CroTeam
71// Ported by Nguyen Binh
72int dCollideCylinderBox(dxGeom *o1, dxGeom *o2,
73 int flags, dContactGeom *contact, int skip);
74int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere,
75 int flags, dContactGeom *contact, int skip);
76int dCollideCylinderPlane(dxGeom *gCylinder, dxGeom *gPlane,
77 int flags, dContactGeom *contact, int skip);
78
79//--> Convex Collision
80int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
81 dContactGeom *contact, int skip);
82int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
83 dContactGeom *contact, int skip);
84int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
85 dContactGeom *contact, int skip);
86int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
87 int flags, dContactGeom *contact, int skip);
88int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
89 dContactGeom *contact, int skip);
90int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
91 dContactGeom *contact, int skip);
92//<-- Convex Collision
93
94// dHeightfield
95int dCollideHeightfield( dxGeom *o1, dxGeom *o2,
96 int flags, dContactGeom *contact, int skip );
97
98//****************************************************************************
99// the basic geometry objects
100
101struct dxSphere : public dxGeom {
102 dReal radius; // sphere radius
103 dxSphere (dSpaceID space, dReal _radius);
104 void computeAABB();
105};
106
107
108struct dxBox : public dxGeom {
109 dVector3 side; // side lengths (x,y,z)
110 dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
111 void computeAABB();
112};
113
114
115struct dxCapsule : public dxGeom {
116 dReal radius,lz; // radius, length along z axis
117 dxCapsule (dSpaceID space, dReal _radius, dReal _length);
118 void computeAABB();
119};
120
121
122struct dxCylinder : public dxGeom {
123 dReal radius,lz; // radius, length along z axis
124 dxCylinder (dSpaceID space, dReal _radius, dReal _length);
125 void computeAABB();
126};
127
128
129struct dxPlane : public dxGeom {
130 dReal p[4];
131 dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
132 void computeAABB();
133};
134
135
136struct dxRay : public dxGeom {
137 dReal length;
138 dxRay (dSpaceID space, dReal _length);
139 void computeAABB();
140};
141
142typedef std::pair<unsigned int,unsigned int> edge; /*!< Used to descrive a convex hull edge, an edge is a pair or indices into the hull's points */
143struct dxConvex : public dxGeom
144{
145
146 dReal *planes; /*!< An array of planes in the form:
147 normal X, normal Y, normal Z,Distance
148 */
149 dReal *points; /*!< An array of points X,Y,Z */
150 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*/
151 unsigned int planecount; /*!< Amount of planes in planes */
152 unsigned int pointcount;/*!< Amount of points in points */
153 dReal saabb[6];/*!< Static AABB */
154 std::set<edge> edges;
155 dxConvex(dSpaceID space,
156 dReal *planes,
157 unsigned int planecount,
158 dReal *points,
159 unsigned int pointcount,
160 unsigned int *polygons);
161 ~dxConvex()
162 {
163 //fprintf(stdout,"dxConvex Destroy\n");
164 }
165 void computeAABB();
166 private:
167 // For Internal Use Only
168 void FillEdges();
169};
170
171
172#endif
diff --git a/libraries/ode-0.9/ode/src/collision_transform.cpp b/libraries/ode-0.9/ode/src/collision_transform.cpp
new file mode 100644
index 0000000..85fd623
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_transform.cpp
@@ -0,0 +1,232 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25geom transform
26
27*/
28
29#include <ode/collision.h>
30#include <ode/matrix.h>
31#include <ode/rotation.h>
32#include <ode/odemath.h>
33#include "collision_transform.h"
34#include "collision_util.h"
35
36#ifdef _MSC_VER
37#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
38#endif
39
40//****************************************************************************
41// dxGeomTransform class
42
43struct dxGeomTransform : public dxGeom {
44 dxGeom *obj; // object that is being transformed
45 int cleanup; // 1 to destroy obj when destroyed
46 int infomode; // 1 to put Tx geom in dContactGeom g1
47
48 // cached final object transform (body tx + relative tx). this is set by
49 // computeAABB(), and it is valid while the AABB is valid.
50 dxPosR transform_posr;
51
52 dxGeomTransform (dSpaceID space);
53 ~dxGeomTransform();
54 void computeAABB();
55 void computeFinalTx();
56};
57/*
58void RunMe()
59{
60 printf("sizeof body = %i\n", sizeof(dxBody));
61 printf("sizeof geom = %i\n", sizeof(dxGeom));
62 printf("sizeof geomtransform = %i\n", sizeof(dxGeomTransform));
63 printf("sizeof posr = %i\n", sizeof(dxPosR));
64}
65*/
66
67dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1)
68{
69 type = dGeomTransformClass;
70 obj = 0;
71 cleanup = 0;
72 infomode = 0;
73 dSetZero (transform_posr.pos,4);
74 dRSetIdentity (transform_posr.R);
75}
76
77
78dxGeomTransform::~dxGeomTransform()
79{
80 if (obj && cleanup) delete obj;
81}
82
83
84void dxGeomTransform::computeAABB()
85{
86 if (!obj) {
87 dSetZero (aabb,6);
88 return;
89 }
90
91 // backup the relative pos and R pointers of the encapsulated geom object
92 dxPosR* posr_bak = obj->final_posr;
93
94 // compute temporary pos and R for the encapsulated geom object
95 computeFinalTx();
96 obj->final_posr = &transform_posr;
97
98 // compute the AABB
99 obj->computeAABB();
100 memcpy (aabb,obj->aabb,6*sizeof(dReal));
101
102 // restore the pos and R
103 obj->final_posr = posr_bak;
104}
105
106
107// utility function for dCollideTransform() : compute final pos and R
108// for the encapsulated geom object
109
110void dxGeomTransform::computeFinalTx()
111{
112 dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos);
113 transform_posr.pos[0] += final_posr->pos[0];
114 transform_posr.pos[1] += final_posr->pos[1];
115 transform_posr.pos[2] += final_posr->pos[2];
116 dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R);
117}
118
119//****************************************************************************
120// collider function:
121// this collides a transformed geom with another geom. the other geom can
122// also be a transformed geom, but this case is not handled specially.
123
124int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
125 dContactGeom *contact, int skip)
126{
127 dIASSERT (skip >= (int)sizeof(dContactGeom));
128 dIASSERT (o1->type == dGeomTransformClass);
129
130 dxGeomTransform *tr = (dxGeomTransform*) o1;
131 if (!tr->obj) return 0;
132 dUASSERT (tr->obj->parent_space==0,
133 "GeomTransform encapsulated object must not be in a space");
134 dUASSERT (tr->obj->body==0,
135 "GeomTransform encapsulated object must not be attached "
136 "to a body");
137
138 // backup the relative pos and R pointers of the encapsulated geom object,
139 // and the body pointer
140 dxPosR *posr_bak = tr->obj->final_posr;
141 dxBody *bodybak = tr->obj->body;
142
143 // compute temporary pos and R for the encapsulated geom object.
144 // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag,
145 // because computeFinalTx() will have already been called in
146 // dxGeomTransform::computeAABB()
147
148 if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx();
149 tr->obj->final_posr = &tr->transform_posr;
150 tr->obj->body = o1->body;
151
152 // do the collision
153 int n = dCollide (tr->obj,o2,flags,contact,skip);
154
155 // if required, adjust the 'g1' values in the generated contacts so that
156 // thay indicated the GeomTransform object instead of the encapsulated
157 // object.
158 if (tr->infomode) {
159 for (int i=0; i<n; i++) {
160 dContactGeom *c = CONTACT(contact,skip*i);
161 c->g1 = o1;
162 }
163 }
164
165 // restore the pos, R and body
166 tr->obj->final_posr = posr_bak;
167 tr->obj->body = bodybak;
168 return n;
169}
170
171//****************************************************************************
172// public API
173
174dGeomID dCreateGeomTransform (dSpaceID space)
175{
176 return new dxGeomTransform (space);
177}
178
179
180void dGeomTransformSetGeom (dGeomID g, dGeomID obj)
181{
182 dUASSERT (g && g->type == dGeomTransformClass,
183 "argument not a geom transform");
184 dxGeomTransform *tr = (dxGeomTransform*) g;
185 if (tr->obj && tr->cleanup) delete tr->obj;
186 tr->obj = obj;
187}
188
189
190dGeomID dGeomTransformGetGeom (dGeomID g)
191{
192 dUASSERT (g && g->type == dGeomTransformClass,
193 "argument not a geom transform");
194 dxGeomTransform *tr = (dxGeomTransform*) g;
195 return tr->obj;
196}
197
198
199void dGeomTransformSetCleanup (dGeomID g, int mode)
200{
201 dUASSERT (g && g->type == dGeomTransformClass,
202 "argument not a geom transform");
203 dxGeomTransform *tr = (dxGeomTransform*) g;
204 tr->cleanup = mode;
205}
206
207
208int dGeomTransformGetCleanup (dGeomID g)
209{
210 dUASSERT (g && g->type == dGeomTransformClass,
211 "argument not a geom transform");
212 dxGeomTransform *tr = (dxGeomTransform*) g;
213 return tr->cleanup;
214}
215
216
217void dGeomTransformSetInfo (dGeomID g, int mode)
218{
219 dUASSERT (g && g->type == dGeomTransformClass,
220 "argument not a geom transform");
221 dxGeomTransform *tr = (dxGeomTransform*) g;
222 tr->infomode = mode;
223}
224
225
226int dGeomTransformGetInfo (dGeomID g)
227{
228 dUASSERT (g && g->type == dGeomTransformClass,
229 "argument not a geom transform");
230 dxGeomTransform *tr = (dxGeomTransform*) g;
231 return tr->infomode;
232}
diff --git a/libraries/ode-0.9/ode/src/collision_transform.h b/libraries/ode-0.9/ode/src/collision_transform.h
new file mode 100644
index 0000000..406a687
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_transform.h
@@ -0,0 +1,40 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25geom transform
26
27*/
28
29#ifndef _ODE_COLLISION_TRANSFORM_H_
30#define _ODE_COLLISION_TRANSFORM_H_
31
32#include <ode/common.h>
33#include "collision_kernel.h"
34
35
36int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
37 dContactGeom *contact, int skip);
38
39
40#endif
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
new file mode 100644
index 0000000..0194d02
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
@@ -0,0 +1,1497 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23
24/*************************************************************************
25 * *
26 * Triangle-box collider by Alen Ladavac and Vedran Klanac. *
27 * Ported to ODE by Oskari Nyman. *
28 * *
29 *************************************************************************/
30
31
32#include <ode/collision.h>
33#include <ode/matrix.h>
34#include <ode/rotation.h>
35#include <ode/odemath.h>
36#include "collision_util.h"
37
38#define TRIMESH_INTERNAL
39#include "collision_trimesh_internal.h"
40
41#if dTRIMESH_ENABLED
42
43
44static void
45GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
46 dxGeom* in_g1, dxGeom* in_g2,
47 const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
48 int& OutTriCount);
49
50
51// largest number, double or float
52#if defined(dSINGLE)
53 #define MAXVALUE FLT_MAX
54#else
55 #define MAXVALUE DBL_MAX
56#endif
57
58
59// dVector3
60// r=a-b
61#define SUBTRACT(a,b,r) do{ \
62 (r)[0]=(a)[0] - (b)[0]; \
63 (r)[1]=(a)[1] - (b)[1]; \
64 (r)[2]=(a)[2] - (b)[2]; }while(0)
65
66
67// dVector3
68// a=b
69#define SET(a,b) do{ \
70 (a)[0]=(b)[0]; \
71 (a)[1]=(b)[1]; \
72 (a)[2]=(b)[2]; }while(0)
73
74
75// dMatrix3
76// a=b
77#define SETM(a,b) do{ \
78 (a)[0]=(b)[0]; \
79 (a)[1]=(b)[1]; \
80 (a)[2]=(b)[2]; \
81 (a)[3]=(b)[3]; \
82 (a)[4]=(b)[4]; \
83 (a)[5]=(b)[5]; \
84 (a)[6]=(b)[6]; \
85 (a)[7]=(b)[7]; \
86 (a)[8]=(b)[8]; \
87 (a)[9]=(b)[9]; \
88 (a)[10]=(b)[10]; \
89 (a)[11]=(b)[11]; }while(0)
90
91
92// dVector3
93// r=a+b
94#define ADD(a,b,r) do{ \
95 (r)[0]=(a)[0] + (b)[0]; \
96 (r)[1]=(a)[1] + (b)[1]; \
97 (r)[2]=(a)[2] + (b)[2]; }while(0)
98
99
100// dMatrix3, int, dVector3
101// v=column a from m
102#define GETCOL(m,a,v) do{ \
103 (v)[0]=(m)[(a)+0]; \
104 (v)[1]=(m)[(a)+4]; \
105 (v)[2]=(m)[(a)+8]; }while(0)
106
107
108// dVector4, dVector3
109// distance between plane p and point v
110#define POINTDISTANCE(p,v) \
111 ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] )
112
113
114// dVector4, dVector3, dReal
115// construct plane from normal and d
116#define CONSTRUCTPLANE(plane,normal,d) do{ \
117 plane[0]=normal[0];\
118 plane[1]=normal[1];\
119 plane[2]=normal[2];\
120 plane[3]=d; }while(0)
121
122
123// dVector3
124// length of vector a
125#define LENGTHOF(a) \
126 dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2])
127
128
129// box data
130static dMatrix3 mHullBoxRot;
131static dVector3 vHullBoxPos;
132static dVector3 vBoxHalfSize;
133
134// mesh data
135static dVector3 vHullDstPos;
136
137// global collider data
138static dVector3 vBestNormal;
139static dReal fBestDepth;
140static int iBestAxis = 0;
141static int iExitAxis = 0;
142static dVector3 vE0, vE1, vE2, vN;
143
144// global info for contact creation
145static int iFlags;
146static dContactGeom *ContactGeoms;
147static int iStride;
148static dxGeom *Geom1;
149static dxGeom *Geom2;
150static int ctContacts = 0;
151
152
153
154// Test normal of mesh face as separating axis for intersection
155static bool _cldTestNormal( dReal fp0, dReal fR, dVector3 vNormal, int iAxis )
156{
157 // calculate overlapping interval of box and triangle
158 dReal fDepth = fR+fp0;
159
160 // if we do not overlap
161 if ( fDepth<0 ) {
162 // do nothing
163 return false;
164 }
165
166 // calculate normal's length
167 dReal fLength = LENGTHOF(vNormal);
168 // if long enough
169 if ( fLength > 0.0f ) {
170
171 dReal fOneOverLength = 1.0f/fLength;
172 // normalize depth
173 fDepth = fDepth*fOneOverLength;
174
175 // get minimum depth
176 if (fDepth<fBestDepth) {
177 vBestNormal[0] = -vNormal[0]*fOneOverLength;
178 vBestNormal[1] = -vNormal[1]*fOneOverLength;
179 vBestNormal[2] = -vNormal[2]*fOneOverLength;
180 iBestAxis = iAxis;
181 //dAASSERT(fDepth>=0);
182 fBestDepth = fDepth;
183 }
184
185 }
186
187 return true;
188}
189
190
191
192
193// Test box axis as separating axis
194static bool _cldTestFace( dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD,
195 dVector3 vNormal, int iAxis )
196{
197 dReal fMin, fMax;
198
199 // find min of triangle interval
200 if ( fp0 < fp1 ) {
201 if ( fp0 < fp2 ) {
202 fMin = fp0;
203 } else {
204 fMin = fp2;
205 }
206 } else {
207 if( fp1 < fp2 ) {
208 fMin = fp1;
209 } else {
210 fMin = fp2;
211 }
212 }
213
214 // find max of triangle interval
215 if ( fp0 > fp1 ) {
216 if ( fp0 > fp2 ) {
217 fMax = fp0;
218 } else {
219 fMax = fp2;
220 }
221 } else {
222 if( fp1 > fp2 ) {
223 fMax = fp1;
224 } else {
225 fMax = fp2;
226 }
227 }
228
229 // calculate minimum and maximum depth
230 dReal fDepthMin = fR - fMin;
231 dReal fDepthMax = fMax + fR;
232
233 // if we dont't have overlapping interval
234 if ( fDepthMin < 0 || fDepthMax < 0 ) {
235 // do nothing
236 return false;
237 }
238
239 dReal fDepth = 0;
240
241 // if greater depth is on negative side
242 if ( fDepthMin > fDepthMax ) {
243 // use smaller depth (one from positive side)
244 fDepth = fDepthMax;
245 // flip normal direction
246 vNormal[0] = -vNormal[0];
247 vNormal[1] = -vNormal[1];
248 vNormal[2] = -vNormal[2];
249 fD = -fD;
250 // if greater depth is on positive side
251 } else {
252 // use smaller depth (one from negative side)
253 fDepth = fDepthMin;
254 }
255
256
257 // if lower depth than best found so far
258 if (fDepth<fBestDepth) {
259 // remember current axis as best axis
260 vBestNormal[0] = vNormal[0];
261 vBestNormal[1] = vNormal[1];
262 vBestNormal[2] = vNormal[2];
263 iBestAxis = iAxis;
264 //dAASSERT(fDepth>=0);
265 fBestDepth = fDepth;
266 }
267
268 return true;
269}
270
271
272
273
274
275// Test cross products of box axis and triangle edges as separating axis
276static bool _cldTestEdge( dReal fp0, dReal fp1, dReal fR, dReal fD,
277 dVector3 vNormal, int iAxis )
278{
279 dReal fMin, fMax;
280
281
282 // ===== Begin Patch by Francisco Leon, 2006/10/28 =====
283
284 // Fixed Null Normal. This prevents boxes passing
285 // through trimeshes at certain contact angles
286
287 fMin = vNormal[0] * vNormal[0] +
288 vNormal[1] * vNormal[1] +
289 vNormal[2] * vNormal[2];
290
291 if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS
292 return true;
293
294 // ===== Ending Patch by Francisco Leon =====
295
296
297 // calculate min and max interval values
298 if ( fp0 < fp1 ) {
299 fMin = fp0;
300 fMax = fp1;
301 } else {
302 fMin = fp1;
303 fMax = fp0;
304 }
305
306 // check if we overlapp
307 dReal fDepthMin = fR - fMin;
308 dReal fDepthMax = fMax + fR;
309
310 // if we don't overlapp
311 if ( fDepthMin < 0 || fDepthMax < 0 ) {
312 // do nothing
313 return false;
314 }
315
316 dReal fDepth;
317
318
319 // if greater depth is on negative side
320 if ( fDepthMin > fDepthMax ) {
321 // use smaller depth (one from positive side)
322 fDepth = fDepthMax;
323 // flip normal direction
324 vNormal[0] = -vNormal[0];
325 vNormal[1] = -vNormal[1];
326 vNormal[2] = -vNormal[2];
327 fD = -fD;
328 // if greater depth is on positive side
329 } else {
330 // use smaller depth (one from negative side)
331 fDepth = fDepthMin;
332 }
333
334 // calculate normal's length
335 dReal fLength = LENGTHOF(vNormal);
336
337 // if long enough
338 if ( fLength > 0.0f ) {
339
340 // normalize depth
341 dReal fOneOverLength = 1.0f/fLength;
342 fDepth = fDepth*fOneOverLength;
343 fD*=fOneOverLength;
344
345
346 // if lower depth than best found so far (favor face over edges)
347 if (fDepth*1.5f<fBestDepth) {
348 // remember current axis as best axis
349 vBestNormal[0] = vNormal[0]*fOneOverLength;
350 vBestNormal[1] = vNormal[1]*fOneOverLength;
351 vBestNormal[2] = vNormal[2]*fOneOverLength;
352 iBestAxis = iAxis;
353 //dAASSERT(fDepth>=0);
354 fBestDepth = fDepth;
355 }
356 }
357
358 return true;
359}
360
361
362
363
364
365// clip polygon with plane and generate new polygon points
366static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn,
367 dVector3 avArrayOut[], int &ctOut,
368 const dVector4 &plPlane )
369{
370 // start with no output points
371 ctOut = 0;
372
373 int i0 = ctIn-1;
374
375 // for each edge in input polygon
376 for (int i1=0; i1<ctIn; i0=i1, i1++) {
377
378
379 // calculate distance of edge points to plane
380 dReal fDistance0 = POINTDISTANCE( plPlane ,avArrayIn[i0] );
381 dReal fDistance1 = POINTDISTANCE( plPlane ,avArrayIn[i1] );
382
383
384 // if first point is in front of plane
385 if( fDistance0 >= 0 ) {
386 // emit point
387 avArrayOut[ctOut][0] = avArrayIn[i0][0];
388 avArrayOut[ctOut][1] = avArrayIn[i0][1];
389 avArrayOut[ctOut][2] = avArrayIn[i0][2];
390 ctOut++;
391 }
392
393 // if points are on different sides
394 if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
395
396 // find intersection point of edge and plane
397 dVector3 vIntersectionPoint;
398 vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
399 vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
400 vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
401
402 // emit intersection point
403 avArrayOut[ctOut][0] = vIntersectionPoint[0];
404 avArrayOut[ctOut][1] = vIntersectionPoint[1];
405 avArrayOut[ctOut][2] = vIntersectionPoint[2];
406 ctOut++;
407 }
408 }
409
410}
411
412
413
414
415static bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
416 // reset best axis
417 iBestAxis = 0;
418 iExitAxis = -1;
419 fBestDepth = MAXVALUE;
420
421 // calculate edges
422 SUBTRACT(v1,v0,vE0);
423 SUBTRACT(v2,v0,vE1);
424 SUBTRACT(vE1,vE0,vE2);
425
426 // calculate poly normal
427 dCROSS(vN,=,vE0,vE1);
428
429 // extract box axes as vectors
430 dVector3 vA0,vA1,vA2;
431 GETCOL(mHullBoxRot,0,vA0);
432 GETCOL(mHullBoxRot,1,vA1);
433 GETCOL(mHullBoxRot,2,vA2);
434
435 // box halfsizes
436 dReal fa0 = vBoxHalfSize[0];
437 dReal fa1 = vBoxHalfSize[1];
438 dReal fa2 = vBoxHalfSize[2];
439
440 // calculate relative position between box and triangle
441 dVector3 vD;
442 SUBTRACT(v0,vHullBoxPos,vD);
443
444 // calculate length of face normal
445 dReal fNLen = LENGTHOF( vN );
446
447 dVector3 vL;
448 dReal fp0, fp1, fp2, fR, fD;
449
450 // Test separating axes for intersection
451 // ************************************************
452 // Axis 1 - Triangle Normal
453 SET(vL,vN);
454 fp0 = dDOT(vL,vD);
455 fp1 = fp0;
456 fp2 = fp0;
457 fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) );
458
459
460 if( !_cldTestNormal( fp0, fR, vL, 1) ) {
461 iExitAxis=1;
462 return false;
463 }
464
465 // ************************************************
466
467 // Test Faces
468 // ************************************************
469 // Axis 2 - Box X-Axis
470 SET(vL,vA0);
471 fD = dDOT(vL,vN)/fNLen;
472 fp0 = dDOT(vL,vD);
473 fp1 = fp0 + dDOT(vA0,vE0);
474 fp2 = fp0 + dDOT(vA0,vE1);
475 fR = fa0;
476
477
478 if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) {
479 iExitAxis=2;
480 return false;
481 }
482 // ************************************************
483
484 // ************************************************
485 // Axis 3 - Box Y-Axis
486 SET(vL,vA1);
487 fD = dDOT(vL,vN)/fNLen;
488 fp0 = dDOT(vL,vD);
489 fp1 = fp0 + dDOT(vA1,vE0);
490 fp2 = fp0 + dDOT(vA1,vE1);
491 fR = fa1;
492
493
494 if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) {
495 iExitAxis=3;
496 return false;
497 }
498
499 // ************************************************
500
501 // ************************************************
502 // Axis 4 - Box Z-Axis
503 SET(vL,vA2);
504 fD = dDOT(vL,vN)/fNLen;
505 fp0 = dDOT(vL,vD);
506 fp1 = fp0 + dDOT(vA2,vE0);
507 fp2 = fp0 + dDOT(vA2,vE1);
508 fR = fa2;
509
510
511 if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) {
512 iExitAxis=4;
513 return false;
514 }
515
516 // ************************************************
517
518 // Test Edges
519 // ************************************************
520 // Axis 5 - Box X-Axis cross Edge0
521 dCROSS(vL,=,vA0,vE0);
522 fD = dDOT(vL,vN)/fNLen;
523 fp0 = dDOT(vL,vD);
524 fp1 = fp0;
525 fp2 = fp0 + dDOT(vA0,vN);
526 fR = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0));
527
528
529 if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) {
530 iExitAxis=5;
531 return false;
532 }
533 // ************************************************
534
535 // ************************************************
536 // Axis 6 - Box X-Axis cross Edge1
537 dCROSS(vL,=,vA0,vE1);
538 fD = dDOT(vL,vN)/fNLen;
539 fp0 = dDOT(vL,vD);
540 fp1 = fp0 - dDOT(vA0,vN);
541 fp2 = fp0;
542 fR = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1));
543
544
545 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) {
546 iExitAxis=6;
547 return false;
548 }
549 // ************************************************
550
551 // ************************************************
552 // Axis 7 - Box X-Axis cross Edge2
553 dCROSS(vL,=,vA0,vE2);
554 fD = dDOT(vL,vN)/fNLen;
555 fp0 = dDOT(vL,vD);
556 fp1 = fp0 - dDOT(vA0,vN);
557 fp2 = fp0 - dDOT(vA0,vN);
558 fR = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2));
559
560
561 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) {
562 iExitAxis=7;
563 return false;
564 }
565
566 // ************************************************
567
568 // ************************************************
569 // Axis 8 - Box Y-Axis cross Edge0
570 dCROSS(vL,=,vA1,vE0);
571 fD = dDOT(vL,vN)/fNLen;
572 fp0 = dDOT(vL,vD);
573 fp1 = fp0;
574 fp2 = fp0 + dDOT(vA1,vN);
575 fR = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0));
576
577
578 if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) {
579 iExitAxis=8;
580 return false;
581 }
582
583 // ************************************************
584
585 // ************************************************
586 // Axis 9 - Box Y-Axis cross Edge1
587 dCROSS(vL,=,vA1,vE1);
588 fD = dDOT(vL,vN)/fNLen;
589 fp0 = dDOT(vL,vD);
590 fp1 = fp0 - dDOT(vA1,vN);
591 fp2 = fp0;
592 fR = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1));
593
594
595 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) {
596 iExitAxis=9;
597 return false;
598 }
599
600 // ************************************************
601
602 // ************************************************
603 // Axis 10 - Box Y-Axis cross Edge2
604 dCROSS(vL,=,vA1,vE2);
605 fD = dDOT(vL,vN)/fNLen;
606 fp0 = dDOT(vL,vD);
607 fp1 = fp0 - dDOT(vA1,vN);
608 fp2 = fp0 - dDOT(vA1,vN);
609 fR = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2));
610
611
612 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) {
613 iExitAxis=10;
614 return false;
615 }
616
617 // ************************************************
618
619 // ************************************************
620 // Axis 11 - Box Z-Axis cross Edge0
621 dCROSS(vL,=,vA2,vE0);
622 fD = dDOT(vL,vN)/fNLen;
623 fp0 = dDOT(vL,vD);
624 fp1 = fp0;
625 fp2 = fp0 + dDOT(vA2,vN);
626 fR = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0));
627
628
629 if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) {
630 iExitAxis=11;
631 return false;
632 }
633 // ************************************************
634
635 // ************************************************
636 // Axis 12 - Box Z-Axis cross Edge1
637 dCROSS(vL,=,vA2,vE1);
638 fD = dDOT(vL,vN)/fNLen;
639 fp0 = dDOT(vL,vD);
640 fp1 = fp0 - dDOT(vA2,vN);
641 fp2 = fp0;
642 fR = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1));
643
644
645 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) {
646 iExitAxis=12;
647 return false;
648 }
649 // ************************************************
650
651 // ************************************************
652 // Axis 13 - Box Z-Axis cross Edge2
653 dCROSS(vL,=,vA2,vE2);
654 fD = dDOT(vL,vN)/fNLen;
655 fp0 = dDOT(vL,vD);
656 fp1 = fp0 - dDOT(vA2,vN);
657 fp2 = fp0 - dDOT(vA2,vN);
658 fR = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2));
659
660
661 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) {
662 iExitAxis=13;
663 return false;
664 }
665
666 // ************************************************
667 return true;
668}
669
670
671
672
673
674// find two closest points on two lines
675static bool _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1,
676 dVector3 vPoint2, dVector3 vLenVec2,
677 dReal &fvalue1, dReal &fvalue2)
678{
679 // calulate denominator
680 dVector3 vp;
681 SUBTRACT(vPoint2,vPoint1,vp);
682 dReal fuaub = dDOT(vLenVec1,vLenVec2);
683 dReal fq1 = dDOT(vLenVec1,vp);
684 dReal fq2 = -dDOT(vLenVec2,vp);
685 dReal fd = 1.0f - fuaub * fuaub;
686
687 // if denominator is positive
688 if (fd > 0.0f) {
689 // calculate points of closest approach
690 fd = 1.0f/fd;
691 fvalue1 = (fq1 + fuaub*fq2)*fd;
692 fvalue2 = (fuaub*fq1 + fq2)*fd;
693 return true;
694 // otherwise
695 } else {
696 // lines are parallel
697 fvalue1 = 0.0f;
698 fvalue2 = 0.0f;
699 return false;
700 }
701
702}
703
704
705
706
707
708// clip and generate contacts
709static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
710 dIASSERT( !(iFlags & CONTACTS_UNIMPORTANT) || ctContacts < (iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results
711
712 // if we have edge/edge intersection
713 if ( iBestAxis > 4 ) {
714
715 dVector3 vub,vPb,vPa;
716
717 SET(vPa,vHullBoxPos);
718
719 // calculate point on box edge
720 for( int i=0; i<3; i++) {
721 dVector3 vRotCol;
722 GETCOL(mHullBoxRot,i,vRotCol);
723 dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
724
725 vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
726 vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
727 vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
728 }
729
730 int iEdge = (iBestAxis-5)%3;
731
732 // decide which edge is on triangle
733 if ( iEdge == 0 ) {
734 SET(vPb,v0);
735 SET(vub,vE0);
736 } else if ( iEdge == 1) {
737 SET(vPb,v2);
738 SET(vub,vE1);
739 } else {
740 SET(vPb,v1);
741 SET(vub,vE2);
742 }
743
744
745 // setup direction parameter for face edge
746 dNormalize3(vub);
747
748 dReal fParam1, fParam2;
749
750 // setup direction parameter for box edge
751 dVector3 vua;
752 int col=(iBestAxis-5)/3;
753 GETCOL(mHullBoxRot,col,vua);
754
755 // find two closest points on both edges
756 _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
757 vPa[0] += vua[0]*fParam1;
758 vPa[1] += vua[1]*fParam1;
759 vPa[2] += vua[2]*fParam1;
760
761 vPb[0] += vub[0]*fParam2;
762 vPb[1] += vub[1]*fParam2;
763 vPb[2] += vub[2]*fParam2;
764
765 // calculate collision point
766 dVector3 vPntTmp;
767 ADD(vPa,vPb,vPntTmp);
768
769 vPntTmp[0]*=0.5f;
770 vPntTmp[1]*=0.5f;
771 vPntTmp[2]*=0.5f;
772
773 // generate contact point between two closest points
774#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
775 dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
776 Contact->depth = fBestDepth;
777 SET(Contact->normal,vBestNormal);
778 SET(Contact->pos,vPntTmp);
779 Contact->g1 = Geom1;
780 Contact->g2 = Geom2;
781 ctContacts++;
782#endif
783 GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
784 vPntTmp, vBestNormal, fBestDepth, ctContacts);
785
786
787 // if triangle is the referent face then clip box to triangle face
788 } else if ( iBestAxis == 1 ) {
789
790
791 dVector3 vNormal2;
792 vNormal2[0]=-vBestNormal[0];
793 vNormal2[1]=-vBestNormal[1];
794 vNormal2[2]=-vBestNormal[2];
795
796
797 // vNr is normal in box frame, pointing from triangle to box
798 dMatrix3 mTransposed;
799 mTransposed[0*4+0]=mHullBoxRot[0*4+0];
800 mTransposed[0*4+1]=mHullBoxRot[1*4+0];
801 mTransposed[0*4+2]=mHullBoxRot[2*4+0];
802
803 mTransposed[1*4+0]=mHullBoxRot[0*4+1];
804 mTransposed[1*4+1]=mHullBoxRot[1*4+1];
805 mTransposed[1*4+2]=mHullBoxRot[2*4+1];
806
807 mTransposed[2*4+0]=mHullBoxRot[0*4+2];
808 mTransposed[2*4+1]=mHullBoxRot[1*4+2];
809 mTransposed[2*4+2]=mHullBoxRot[2*4+2];
810
811 dVector3 vNr;
812 vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2];
813 vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2];
814 vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2];
815
816
817 dVector3 vAbsNormal;
818 vAbsNormal[0] = dFabs( vNr[0] );
819 vAbsNormal[1] = dFabs( vNr[1] );
820 vAbsNormal[2] = dFabs( vNr[2] );
821
822 // get closest face from box
823 int iB0, iB1, iB2;
824 if (vAbsNormal[1] > vAbsNormal[0]) {
825 if (vAbsNormal[1] > vAbsNormal[2]) {
826 iB1 = 0; iB0 = 1; iB2 = 2;
827 } else {
828 iB1 = 0; iB2 = 1; iB0 = 2;
829 }
830 } else {
831
832 if (vAbsNormal[0] > vAbsNormal[2]) {
833 iB0 = 0; iB1 = 1; iB2 = 2;
834 } else {
835 iB1 = 0; iB2 = 1; iB0 = 2;
836 }
837 }
838
839 // Here find center of box face we are going to project
840 dVector3 vCenter;
841 dVector3 vRotCol;
842 GETCOL(mHullBoxRot,iB0,vRotCol);
843
844 if (vNr[iB0] > 0) {
845 vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
846 vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
847 vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
848 } else {
849 vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
850 vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
851 vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
852 }
853
854 // Here find 4 corner points of box
855 dVector3 avPoints[4];
856
857 dVector3 vRotCol2;
858 GETCOL(mHullBoxRot,iB1,vRotCol);
859 GETCOL(mHullBoxRot,iB2,vRotCol2);
860
861 for(int x=0;x<3;x++) {
862 avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
863 avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
864 avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
865 avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
866 }
867
868
869 // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
870 dVector3 avTempArray1[9];
871 dVector3 avTempArray2[9];
872 dVector4 plPlane;
873
874 int iTempCnt1=0;
875 int iTempCnt2=0;
876
877 // zeroify vectors - necessary?
878 for(int i=0; i<9; i++) {
879 avTempArray1[i][0]=0;
880 avTempArray1[i][1]=0;
881 avTempArray1[i][2]=0;
882
883 avTempArray2[i][0]=0;
884 avTempArray2[i][1]=0;
885 avTempArray2[i][2]=0;
886 }
887
888
889 // Normal plane
890 dVector3 vTemp;
891 vTemp[0]=-vN[0];
892 vTemp[1]=-vN[1];
893 vTemp[2]=-vN[2];
894 dNormalize3(vTemp);
895 CONSTRUCTPLANE(plPlane,vTemp,0);
896
897 _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
898
899
900 // Plane p0
901 dVector3 vTemp2;
902 SUBTRACT(v1,v0,vTemp2);
903 dCROSS(vTemp,=,vN,vTemp2);
904 dNormalize3(vTemp);
905 CONSTRUCTPLANE(plPlane,vTemp,0);
906
907 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
908
909
910 // Plane p1
911 SUBTRACT(v2,v1,vTemp2);
912 dCROSS(vTemp,=,vN,vTemp2);
913 dNormalize3(vTemp);
914 SUBTRACT(v0,v2,vTemp2);
915 CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
916
917 _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
918
919
920 // Plane p2
921 SUBTRACT(v0,v2,vTemp2);
922 dCROSS(vTemp,=,vN,vTemp2);
923 dNormalize3(vTemp);
924 CONSTRUCTPLANE(plPlane,vTemp,0);
925
926 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
927
928
929 // END of clipping polygons
930
931
932
933 // for each generated contact point
934 for ( int i=0; i<iTempCnt2; i++ ) {
935 // calculate depth
936 dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);
937
938 // clamp depth to zero
939 if (fTempDepth > 0) {
940 fTempDepth = 0;
941 }
942
943 dVector3 vPntTmp;
944 ADD(avTempArray2[i],v0,vPntTmp);
945
946#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
947 dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
948
949 Contact->depth = -fTempDepth;
950 SET(Contact->normal,vBestNormal);
951 SET(Contact->pos,vPntTmp);
952 Contact->g1 = Geom1;
953 Contact->g2 = Geom2;
954 ctContacts++;
955#endif
956 GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
957 vPntTmp, vBestNormal, -fTempDepth, ctContacts);
958
959 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
960 break;
961 }
962 }
963
964 //dAASSERT(ctContacts>0);
965
966 // if box face is the referent face, then clip triangle on box face
967 } else { // 2 <= if iBestAxis <= 4
968
969 // get normal of box face
970 dVector3 vNormal2;
971 SET(vNormal2,vBestNormal);
972
973 // get indices of box axes in correct order
974 int iA0,iA1,iA2;
975 iA0 = iBestAxis-2;
976 if ( iA0 == 0 ) {
977 iA1 = 1; iA2 = 2;
978 } else if ( iA0 == 1 ) {
979 iA1 = 0; iA2 = 2;
980 } else {
981 iA1 = 0; iA2 = 1;
982 }
983
984 dVector3 avPoints[3];
985 // calculate triangle vertices in box frame
986 SUBTRACT(v0,vHullBoxPos,avPoints[0]);
987 SUBTRACT(v1,vHullBoxPos,avPoints[1]);
988 SUBTRACT(v2,vHullBoxPos,avPoints[2]);
989
990 // CLIP Polygons
991 // define temp data for clipping
992 dVector3 avTempArray1[9];
993 dVector3 avTempArray2[9];
994
995 int iTempCnt1, iTempCnt2;
996
997 // zeroify vectors - necessary?
998 for(int i=0; i<9; i++) {
999 avTempArray1[i][0]=0;
1000 avTempArray1[i][1]=0;
1001 avTempArray1[i][2]=0;
1002
1003 avTempArray2[i][0]=0;
1004 avTempArray2[i][1]=0;
1005 avTempArray2[i][2]=0;
1006 }
1007
1008 // clip triangle with 5 box planes (1 face plane, 4 edge planes)
1009
1010 dVector4 plPlane;
1011
1012 // Normal plane
1013 dVector3 vTemp;
1014 vTemp[0]=-vNormal2[0];
1015 vTemp[1]=-vNormal2[1];
1016 vTemp[2]=-vNormal2[2];
1017 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA0]);
1018
1019 _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane );
1020
1021
1022 // Plane p0
1023 GETCOL(mHullBoxRot,iA1,vTemp);
1024 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
1025
1026 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
1027
1028
1029 // Plane p1
1030 GETCOL(mHullBoxRot,iA1,vTemp);
1031 vTemp[0]=-vTemp[0];
1032 vTemp[1]=-vTemp[1];
1033 vTemp[2]=-vTemp[2];
1034 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
1035
1036 _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
1037
1038
1039 // Plane p2
1040 GETCOL(mHullBoxRot,iA2,vTemp);
1041 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
1042
1043 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
1044
1045
1046 // Plane p3
1047 GETCOL(mHullBoxRot,iA2,vTemp);
1048 vTemp[0]=-vTemp[0];
1049 vTemp[1]=-vTemp[1];
1050 vTemp[2]=-vTemp[2];
1051 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
1052
1053 _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
1054
1055
1056 // for each generated contact point
1057 for ( int i=0; i<iTempCnt1; i++ ) {
1058 // calculate depth
1059 dReal fTempDepth = dDOT(vNormal2,avTempArray1[i])-vBoxHalfSize[iA0];
1060
1061 // clamp depth to zero
1062 if (fTempDepth > 0) {
1063 fTempDepth = 0;
1064 }
1065
1066 // generate contact data
1067 dVector3 vPntTmp;
1068 ADD(avTempArray1[i],vHullBoxPos,vPntTmp);
1069
1070#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
1071 dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
1072
1073 Contact->depth = -fTempDepth;
1074 SET(Contact->normal,vBestNormal);
1075 SET(Contact->pos,vPntTmp);
1076 Contact->g1 = Geom1;
1077 Contact->g2 = Geom2;
1078 ctContacts++;
1079#endif
1080 GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
1081 vPntTmp, vBestNormal, -fTempDepth, ctContacts);
1082
1083 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
1084 break;
1085 }
1086 }
1087
1088 //dAASSERT(ctContacts>0);
1089 }
1090
1091}
1092
1093
1094
1095
1096
1097// test one mesh triangle on intersection with given box
1098static void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser)
1099{
1100 // do intersection test and find best separating axis
1101 if(!_cldTestSeparatingAxes(v0, v1, v2) ) {
1102 // if not found do nothing
1103 return;
1104 }
1105
1106 // if best separation axis is not found
1107 if ( iBestAxis == 0 ) {
1108 // this should not happen (we should already exit in that case)
1109 //dMessage (0, "best separation axis not found");
1110 // do nothing
1111 return;
1112 }
1113
1114 _cldClipping(v0, v1, v2);
1115}
1116
1117
1118
1119
1120
1121// OPCODE version of box to mesh collider
1122#if dTRIMESH_OPCODE
1123int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
1124 dIASSERT (Stride >= (int)sizeof(dContactGeom));
1125 dIASSERT (g1->type == dTriMeshClass);
1126 dIASSERT (BoxGeom->type == dBoxClass);
1127 dIASSERT ((Flags & NUMC_MASK) >= 1);
1128
1129
1130 dxTriMesh* TriMesh = (dxTriMesh*)g1;
1131
1132
1133 // get source hull position, orientation and half size
1134 const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
1135 const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
1136
1137 // to global
1138 SETM(mHullBoxRot,mRotBox);
1139 SET(vHullBoxPos,vPosBox);
1140
1141 dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
1142 vBoxHalfSize[0] *= 0.5f;
1143 vBoxHalfSize[1] *= 0.5f;
1144 vBoxHalfSize[2] *= 0.5f;
1145
1146
1147
1148 // get destination hull position and orientation
1149 const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
1150 const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
1151
1152 // to global
1153 SET(vHullDstPos,vPosMesh);
1154
1155
1156
1157 // global info for contact creation
1158 ctContacts = 0;
1159 iStride=Stride;
1160 iFlags=Flags;
1161 ContactGeoms=Contacts;
1162 Geom1=TriMesh;
1163 Geom2=BoxGeom;
1164
1165
1166
1167 // reset stuff
1168 fBestDepth = MAXVALUE;
1169 vBestNormal[0]=0;
1170 vBestNormal[1]=0;
1171 vBestNormal[2]=0;
1172
1173 OBBCollider& Collider = TriMesh->_OBBCollider;
1174
1175
1176
1177
1178 // Make OBB
1179 OBB Box;
1180 Box.mCenter.x = vPosBox[0];
1181 Box.mCenter.y = vPosBox[1];
1182 Box.mCenter.z = vPosBox[2];
1183
1184 // It is a potential issue to explicitly cast to float
1185 // if custom width floating point type is introduced in OPCODE.
1186 // It is necessary to make a typedef and cast to it
1187 // (e.g. typedef float opc_float;)
1188 // However I'm not sure in what header it should be added.
1189
1190 Box.mExtents.x = /*(float)*/vBoxHalfSize[0];
1191 Box.mExtents.y = /*(float)*/vBoxHalfSize[1];
1192 Box.mExtents.z = /*(float)*/vBoxHalfSize[2];
1193
1194 Box.mRot.m[0][0] = /*(float)*/mRotBox[0];
1195 Box.mRot.m[1][0] = /*(float)*/mRotBox[1];
1196 Box.mRot.m[2][0] = /*(float)*/mRotBox[2];
1197
1198 Box.mRot.m[0][1] = /*(float)*/mRotBox[4];
1199 Box.mRot.m[1][1] = /*(float)*/mRotBox[5];
1200 Box.mRot.m[2][1] = /*(float)*/mRotBox[6];
1201
1202 Box.mRot.m[0][2] = /*(float)*/mRotBox[8];
1203 Box.mRot.m[1][2] = /*(float)*/mRotBox[9];
1204 Box.mRot.m[2][2] = /*(float)*/mRotBox[10];
1205
1206 Matrix4x4 amatrix;
1207 Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix);
1208
1209 Matrix4x4 InvBoxMatrix;
1210 InvertPRMatrix(InvBoxMatrix, BoxMatrix);
1211
1212 // TC results
1213 if (TriMesh->doBoxTC) {
1214 dxTriMesh::BoxTC* BoxTC = 0;
1215 for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
1216 if (TriMesh->BoxTCCache[i].Geom == BoxGeom){
1217 BoxTC = &TriMesh->BoxTCCache[i];
1218 break;
1219 }
1220 }
1221 if (!BoxTC){
1222 TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
1223
1224 BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
1225 BoxTC->Geom = BoxGeom;
1226 BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0
1227 }
1228
1229 // Intersect
1230 Collider.SetTemporalCoherence(true);
1231 Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix));
1232 }
1233 else {
1234 Collider.SetTemporalCoherence(false);
1235 Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null,
1236 &MakeMatrix(vPosMesh, mRotMesh, amatrix));
1237 }
1238
1239 if (! Collider.GetContactStatus()) {
1240 // no collision occurred
1241 return 0;
1242 }
1243
1244 // Retrieve data
1245 int TriCount = Collider.GetNbTouchedPrimitives();
1246 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1247
1248 if (TriCount != 0){
1249 if (TriMesh->ArrayCallback != null){
1250 TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount);
1251 }
1252
1253 int ctContacts0 = 0;
1254
1255 // loop through all intersecting triangles
1256 for (int i = 0; i < TriCount; i++){
1257
1258
1259 const int Triint = Triangles[i];
1260 if (!Callback(TriMesh, BoxGeom, Triint)) continue;
1261
1262
1263 dVector3 dv[3];
1264 FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv);
1265
1266
1267 // test this triangle
1268 _cldTestOneTriangle(dv[0],dv[1],dv[2]);
1269
1270 // fill-in tri index for generated contacts
1271 for (; ctContacts0<ctContacts; ctContacts0++)
1272 SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = Triint;
1273
1274 /*
1275 NOTE by Oleh_Derevenko:
1276 The function continues checking triangles after maximal number
1277 of contacts is reached because it selects maximal penetration depths.
1278 See also comments in GenerateContact()
1279 */
1280 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1281 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
1282 break;
1283 }
1284 }
1285
1286
1287 return ctContacts;
1288}
1289#endif
1290
1291// GIMPACT version of box to mesh collider
1292#if dTRIMESH_GIMPACT
1293int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride)
1294{
1295 dIASSERT (Stride >= (int)sizeof(dContactGeom));
1296 dIASSERT (g1->type == dTriMeshClass);
1297 dIASSERT (BoxGeom->type == dBoxClass);
1298 dIASSERT ((Flags & NUMC_MASK) >= 1);
1299
1300
1301 dxTriMesh* TriMesh = (dxTriMesh*)g1;
1302
1303
1304 // get source hull position, orientation and half size
1305 const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
1306 const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
1307
1308 // to global
1309 SETM(mHullBoxRot,mRotBox);
1310 SET(vHullBoxPos,vPosBox);
1311
1312 dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
1313 vBoxHalfSize[0] *= 0.5f;
1314 vBoxHalfSize[1] *= 0.5f;
1315 vBoxHalfSize[2] *= 0.5f;
1316
1317 // get destination hull position and orientation
1318 /*const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
1319 const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
1320
1321 // to global
1322 SET(vHullDstPos,vPosMesh);*/
1323
1324 // global info for contact creation
1325 ctContacts = 0;
1326 iStride=Stride;
1327 iFlags=Flags;
1328 ContactGeoms=Contacts;
1329 Geom1=TriMesh;
1330 Geom2=BoxGeom;
1331
1332
1333 // reset stuff
1334 fBestDepth = MAXVALUE;
1335 vBestNormal[0]=0;
1336 vBestNormal[1]=0;
1337 vBestNormal[2]=0;
1338
1339
1340//*****at first , collide box aabb******//
1341
1342 GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh;
1343 aabb3f test_aabb;
1344
1345 test_aabb.minX = BoxGeom->aabb[0];
1346 test_aabb.maxX = BoxGeom->aabb[1];
1347 test_aabb.minY = BoxGeom->aabb[2];
1348 test_aabb.maxY = BoxGeom->aabb[3];
1349 test_aabb.minZ = BoxGeom->aabb[4];
1350 test_aabb.maxZ = BoxGeom->aabb[5];
1351
1352 GDYNAMIC_ARRAY collision_result;
1353 GIM_CREATE_BOXQUERY_LIST(collision_result);
1354
1355 gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result);
1356
1357 if(collision_result.m_size==0)
1358 {
1359 GIM_DYNARRAY_DESTROY(collision_result);
1360 return 0;
1361 }
1362//*****Set globals for box collision******//
1363
1364 //collide triangles
1365
1366 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
1367 gim_trimesh_locks_work_data(ptrimesh);
1368
1369 int ctContacts0 = 0;
1370
1371 for(unsigned int i=0;i<collision_result.m_size;i++)
1372 {
1373 dVector3 dv[3];
1374
1375 int Triint = boxesresult[i];
1376 gim_trimesh_get_triangle_vertices(ptrimesh, Triint,dv[0],dv[1],dv[2]);
1377 // test this triangle
1378 _cldTestOneTriangle(dv[0],dv[1],dv[2]);
1379
1380 // fill-in tri index for generated contacts
1381 for (; ctContacts0<ctContacts; ctContacts0++)
1382 SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = Triint;
1383
1384 /*
1385 NOTE by Oleh_Derevenko:
1386 The function continues checking triangles after maximal number
1387 of contacts is reached because it selects maximal penetration depths.
1388 See also comments in GenerateContact()
1389 */
1390 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1391 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
1392 break;
1393 }
1394
1395 gim_trimesh_unlocks_work_data(ptrimesh);
1396 GIM_DYNARRAY_DESTROY(collision_result);
1397
1398 return ctContacts;
1399}
1400#endif
1401
1402
1403// GenerateContact - Written by Jeff Smith (jeff@burri.to)
1404// Generate a "unique" contact. A unique contact has a unique
1405// position or normal. If the potential contact has the same
1406// position and normal as an existing contact, but a larger
1407// penetration depth, this new depth is used instead
1408//
1409static void
1410GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
1411 dxGeom* in_g1, dxGeom* in_g2,
1412 const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
1413 int& OutTriCount)
1414{
1415 /*
1416 NOTE by Oleh_Derevenko:
1417 This function is called after maximal number of contacts has already been
1418 collected because it has a side effect of replacing penetration depth of
1419 existing contact with larger penetration depth of another matching normal contact.
1420 If this logic is not necessary any more, you can bail out on reach of contact
1421 number maximum immediately in dCollideBTL(). You will also need to correct
1422 conditional statements after invocations of GenerateContact() in _cldClipping().
1423 */
1424 do
1425 {
1426 dContactGeom* Contact;
1427 dVector3 diff;
1428
1429 if (!(in_Flags & CONTACTS_UNIMPORTANT))
1430 {
1431 bool duplicate = false;
1432 for (int i=0; i<OutTriCount; i++)
1433 {
1434 Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
1435
1436 // same position?
1437 for (int j=0; j<3; j++)
1438 diff[j] = in_ContactPos[j] - Contact->pos[j];
1439 if (dDOT(diff, diff) < dEpsilon)
1440 {
1441 // same normal?
1442 if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon))
1443 {
1444 if (in_Depth > Contact->depth)
1445 Contact->depth = in_Depth;
1446 duplicate = true;
1447 /*
1448 NOTE by Oleh_Derevenko:
1449 There may be a case when two normals are close to each other but not duplicate
1450 while third normal is detected to be duplicate for both of them.
1451 This is the only reason I can think of, there is no "break" statement.
1452 Perhaps author considered it to be logical that the third normal would
1453 replace the depth in both of initial contacts.
1454 However, I consider it a questionable practice which should not
1455 be applied without deep understanding of underlaying physics.
1456 Even more, is this situation with close normal triplet acceptable at all?
1457 Should not be two initial contacts reduced to one (replaced with the latter)?
1458 If you know the answers for these questions, you may want to change this code.
1459 See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp
1460 */
1461 }
1462 }
1463 }
1464 if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
1465 {
1466 break;
1467 }
1468 }
1469 else
1470 {
1471 dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
1472 }
1473
1474 // Add a new contact
1475 Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
1476
1477 Contact->pos[0] = in_ContactPos[0];
1478 Contact->pos[1] = in_ContactPos[1];
1479 Contact->pos[2] = in_ContactPos[2];
1480 Contact->pos[3] = 0.0;
1481
1482 Contact->normal[0] = in_Normal[0];
1483 Contact->normal[1] = in_Normal[1];
1484 Contact->normal[2] = in_Normal[2];
1485 Contact->normal[3] = 0.0;
1486
1487 Contact->depth = in_Depth;
1488
1489 Contact->g1 = in_g1;
1490 Contact->g2 = in_g2;
1491
1492 OutTriCount++;
1493 }
1494 while (false);
1495}
1496
1497#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp
new file mode 100644
index 0000000..da235e0
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp
@@ -0,0 +1,1181 @@
1/*************************************************************************
2* *
3* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4* All rights reserved. Email: russ@q12.org Web: www.q12.org *
5* *
6* This library is free software; you can redistribute it and/or *
7* modify it under the terms of EITHER: *
8* (1) The GNU Lesser General Public License as published by the Free *
9* Software Foundation; either version 2.1 of the License, or (at *
10* your option) any later version. The text of the GNU Lesser *
11* General Public License is included with this library in the *
12* file LICENSE.TXT. *
13* (2) The BSD-style license that is included with this library in *
14* the file LICENSE-BSD.TXT. *
15* *
16* This library is distributed in the hope that it will be useful, *
17* but WITHOUT ANY WARRANTY; without even the implied warranty of *
18* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20* *
21*************************************************************************/
22
23/*
24 * Triangle-Capsule(Capsule) collider by Alen Ladavac
25 * Ported to ODE by Nguyen Binh
26 */
27
28// NOTES from Nguyen Binh
29// 14 Apr : Seem to be robust
30// There is a problem when you use original Step and set contact friction
31// surface.mu = dInfinity;
32// More description :
33// When I dropped Capsule over the bunny ears, it seems to stuck
34// there for a while. I think the cause is when you set surface.mu = dInfinity;
35// the friction force is too high so it just hang the capsule there.
36// So the good cure for this is to set mu = around 1.5 (in my case)
37// For StepFast1, this become as solid as rock : StepFast1 just approximate
38// friction force.
39
40// NOTES from Croteam's Alen
41//As a side note... there are some extra contacts that can be generated
42//on the edge between two triangles, and if the capsule penetrates deeply into
43//the triangle (usually happens with large mass or low FPS), some such
44//contacts can in some cases push the capsule away from the edge instead of
45//away from the two triangles. This shows up as capsule slowing down a bit
46//when hitting an edge while sliding along a flat tesselated grid of
47//triangles. This is only if capsule is standing upwards.
48
49//Same thing can appear whenever a smooth object (e.g sphere) hits such an
50//edge, and it needs to be solved as a special case probably. This is a
51//problem we are looking forward to address soon.
52
53#include <ode/collision.h>
54#include <ode/matrix.h>
55#include <ode/rotation.h>
56#include <ode/odemath.h>
57#include "collision_util.h"
58
59#define TRIMESH_INTERNAL
60#include "collision_trimesh_internal.h"
61
62#if dTRIMESH_ENABLED
63
64// OPCODE version
65#if dTRIMESH_OPCODE
66// largest number, double or float
67#if defined(dSINGLE)
68#define MAX_REAL FLT_MAX
69#define MIN_REAL (-FLT_MAX)
70#else
71#define MAX_REAL DBL_MAX
72#define MIN_REAL (-DBL_MAX)
73#endif
74
75// To optimize before send contacts to dynamic part
76#define OPTIMIZE_CONTACTS
77
78// dVector3
79// r=a-b
80#define SUBTRACT(a,b,r) \
81 (r)[0]=(a)[0] - (b)[0]; \
82 (r)[1]=(a)[1] - (b)[1]; \
83 (r)[2]=(a)[2] - (b)[2];
84
85
86// dVector3
87// a=b
88#define SET(a,b) \
89 (a)[0]=(b)[0]; \
90 (a)[1]=(b)[1]; \
91 (a)[2]=(b)[2];
92
93
94// dMatrix3
95// a=b
96#define SETM(a,b) \
97 (a)[0]=(b)[0]; \
98 (a)[1]=(b)[1]; \
99 (a)[2]=(b)[2]; \
100 (a)[3]=(b)[3]; \
101 (a)[4]=(b)[4]; \
102 (a)[5]=(b)[5]; \
103 (a)[6]=(b)[6]; \
104 (a)[7]=(b)[7]; \
105 (a)[8]=(b)[8]; \
106 (a)[9]=(b)[9]; \
107 (a)[10]=(b)[10]; \
108 (a)[11]=(b)[11];
109
110
111// dVector3
112// r=a+b
113#define ADD(a,b,r) \
114 (r)[0]=(a)[0] + (b)[0]; \
115 (r)[1]=(a)[1] + (b)[1]; \
116 (r)[2]=(a)[2] + (b)[2];
117
118
119// dMatrix3, int, dVector3
120// v=column a from m
121#define GETCOL(m,a,v) \
122 (v)[0]=(m)[(a)+0]; \
123 (v)[1]=(m)[(a)+4]; \
124 (v)[2]=(m)[(a)+8];
125
126
127// dVector4, dVector3
128// distance between plane p and point v
129#define POINTDISTANCE(p,v) \
130 ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ); \
131
132
133// dVector4, dVector3, dReal
134// construct plane from normal and d
135#define CONSTRUCTPLANE(plane,normal,d) \
136 plane[0]=normal[0];\
137 plane[1]=normal[1];\
138 plane[2]=normal[2];\
139 plane[3]=d;
140
141
142// dVector3
143// length of vector a
144#define LENGTHOF(a) \
145 dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);\
146
147inline dReal _length2OfVector3(dVector3 v)
148{
149 return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] );
150}
151
152
153// Local contacts data
154typedef struct _sLocalContactData
155{
156 dVector3 vPos;
157 dVector3 vNormal;
158 dReal fDepth;
159 int triIndex;
160 int nFlags; // 0 = filtered out, 1 = OK
161}sLocalContactData;
162
163static sLocalContactData *gLocalContacts;
164static unsigned int ctContacts = 0;
165
166// capsule data
167// real time data
168static dMatrix3 mCapsuleRotation;
169static dVector3 vCapsulePosition;
170static dVector3 vCapsuleAxis;
171// static data
172static dReal vCapsuleRadius;
173static dReal fCapsuleSize;
174
175// mesh data
176static dMatrix4 mHullDstPl;
177static dMatrix3 mTriMeshRot;
178static dVector3 mTriMeshPos;
179static dVector3 vE0, vE1, vE2;
180
181// Two geom
182dxGeom* gCylinder;
183dxGeom* gTriMesh;
184
185// global collider data
186static dVector3 vNormal;
187static dReal fBestDepth;
188static dReal fBestCenter;
189static dReal fBestrt;
190static int iBestAxis;
191static dVector3 vN = {0,0,0,0};
192
193static dVector3 vV0;
194static dVector3 vV1;
195static dVector3 vV2;
196
197// ODE contact's specific
198static unsigned int iFlags;
199static dContactGeom *ContactGeoms;
200static int iStride;
201
202// Capsule lie on axis number 3 = (Z axis)
203static const int nCAPSULE_AXIS = 2;
204
205// Use to classify contacts to be "near" in position
206static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
207// Use to classify contacts to be "near" in normal direction
208static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4
209
210
211// If this two contact can be classified as "near"
212inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
213{
214 int bPosNear = 0;
215 int bSameDir = 0;
216 dVector3 vDiff;
217
218 // First check if they are "near" in position
219 SUBTRACT(c1.vPos,c2.vPos,vDiff);
220 if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
221 &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
222 &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
223 {
224 bPosNear = 1;
225 }
226
227 // Second check if they are "near" in normal direction
228 SUBTRACT(c1.vNormal,c2.vNormal,vDiff);
229 if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
230 &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
231 &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
232 {
233 bSameDir = 1;
234 }
235
236 // Will be "near" if position and normal direction are "near"
237 return (bPosNear && bSameDir);
238}
239
240inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
241{
242 // The not better will be throw away
243 // You can change the selection criteria here
244 return (c1.fDepth > c2.fDepth);
245}
246
247// iterate through gLocalContacts and filtered out "near contact"
248inline void _OptimizeLocalContacts()
249{
250 int nContacts = ctContacts;
251
252 for (int i = 0; i < nContacts-1; i++)
253 {
254 for (int j = i+1; j < nContacts; j++)
255 {
256 if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j]))
257 {
258 // If they are seem to be the samed then filtered
259 // out the least penetrate one
260 if (_IsBetter(gLocalContacts[j],gLocalContacts[i]))
261 {
262 gLocalContacts[i].nFlags = 0; // filtered 1st contact
263 }
264 else
265 {
266 gLocalContacts[j].nFlags = 0; // filtered 2nd contact
267 }
268
269 // NOTE
270 // There is other way is to add two depth together but
271 // it not work so well. Why???
272 }
273 }
274 }
275}
276
277inline int _ProcessLocalContacts()
278{
279 if (ctContacts == 0)
280 {
281 return 0;
282 }
283
284#ifdef OPTIMIZE_CONTACTS
285 if (ctContacts > 1 && !(iFlags & CONTACTS_UNIMPORTANT))
286 {
287 // Can be optimized...
288 _OptimizeLocalContacts();
289 }
290#endif
291
292 unsigned int iContact = 0;
293 dContactGeom* Contact = 0;
294
295 unsigned int nFinalContact = 0;
296
297 for (iContact = 0; iContact < ctContacts; iContact ++)
298 {
299 // Ensure that we haven't created too many contacts
300 if( nFinalContact >= (iFlags & NUMC_MASK))
301 {
302 break;
303 }
304
305 if (1 == gLocalContacts[iContact].nFlags)
306 {
307 Contact = SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride);
308 Contact->depth = gLocalContacts[iContact].fDepth;
309 SET(Contact->normal,gLocalContacts[iContact].vNormal);
310 SET(Contact->pos,gLocalContacts[iContact].vPos);
311 Contact->g1 = gTriMesh;
312 Contact->g2 = gCylinder;
313 Contact->side2 = gLocalContacts[iContact].triIndex;
314
315 nFinalContact++;
316 }
317 }
318 // debug
319 //if (nFinalContact != ctContacts)
320 //{
321 // printf("[Info] %d contacts generated,%d filtered.\n",ctContacts,ctContacts-nFinalContact);
322 //}
323
324 return nFinalContact;
325}
326
327BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
328{
329 // calculate distance of edge points to plane
330 dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 );
331 dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 );
332
333 // if both points are behind the plane
334 if ( fDistance0 < 0 && fDistance1 < 0 )
335 {
336 // do nothing
337 return FALSE;
338 // if both points in front of the plane
339 } else if ( fDistance0 > 0 && fDistance1 > 0 )
340 {
341 // accept them
342 return TRUE;
343 // if we have edge/plane intersection
344 } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0))
345 {
346
347 // find intersection point of edge and plane
348 dVector3 vIntersectionPoint;
349 vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
350 vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
351 vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
352
353 // clamp correct edge to intersection point
354 if ( fDistance0 < 0 )
355 {
356 SET(vEpnt0,vIntersectionPoint);
357 } else
358 {
359 SET(vEpnt1,vIntersectionPoint);
360 }
361 return TRUE;
362 }
363 return TRUE;
364}
365
366static BOOL _cldTestAxis(const dVector3 &v0,
367 const dVector3 &v1,
368 const dVector3 &v2,
369 dVector3 vAxis,
370 int iAxis,
371 BOOL bNoFlip = FALSE)
372{
373
374 // calculate length of separating axis vector
375 dReal fL = LENGTHOF(vAxis);
376 // if not long enough
377 // TODO : dReal epsilon please
378 if ( fL < REAL(1e-5) )
379 {
380 // do nothing
381 //iLastOutAxis = 0;
382 return TRUE;
383 }
384
385 // otherwise normalize it
386 dNormalize3(vAxis);
387
388 // project capsule on vAxis
389 dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))*(fCapsuleSize*REAL(0.5)-vCapsuleRadius) + vCapsuleRadius;
390
391 // project triangle on vAxis
392 dReal afv[3];
393 afv[0] = dDOT( vV0 , vAxis );
394 afv[1] = dDOT( vV1 , vAxis );
395 afv[2] = dDOT( vV2 , vAxis );
396
397 dReal fMin = MAX_REAL;
398 dReal fMax = MIN_REAL;
399
400 // for each vertex
401 for(int i=0; i<3; i++)
402 {
403 // find minimum
404 if (afv[i]<fMin)
405 {
406 fMin = afv[i];
407 }
408 // find maximum
409 if (afv[i]>fMax)
410 {
411 fMax = afv[i];
412 }
413 }
414
415 // find triangle's center of interval on axis
416 dReal fCenter = (fMin+fMax)*REAL(0.5);
417 // calculate triangles half interval
418 dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
419
420 // if they do not overlap,
421 if( dFabs(fCenter) > ( frc + fTriangleRadius ) )
422 {
423 // exit, we have no intersection
424 return FALSE;
425 }
426
427 // calculate depth
428 dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius);
429
430 // if greater then best found so far
431 if ( fDepth > fBestDepth )
432 {
433 // remember depth
434 fBestDepth = fDepth;
435 fBestCenter = fCenter;
436 fBestrt = fTriangleRadius;
437
438 vNormal[0] = vAxis[0];
439 vNormal[1] = vAxis[1];
440 vNormal[2] = vAxis[2];
441
442 iBestAxis = iAxis;
443
444 // flip normal if interval is wrong faced
445 if (fCenter<0 && !bNoFlip)
446 {
447 vNormal[0] = -vNormal[0];
448 vNormal[1] = -vNormal[1];
449 vNormal[2] = -vNormal[2];
450
451 fBestCenter = -fCenter;
452 }
453 }
454
455 return TRUE;
456}
457
458// helper for less key strokes
459inline void _CalculateAxis(const dVector3& v1,
460 const dVector3& v2,
461 const dVector3& v3,
462 const dVector3& v4,
463 dVector3& r)
464{
465 dVector3 t1;
466 dVector3 t2;
467
468 SUBTRACT(v1,v2,t1);
469 dCROSS(t2,=,t1,v3);
470 dCROSS(r,=,t2,v4);
471}
472
473static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0,
474 const dVector3 &v1,
475 const dVector3 &v2,
476 uint8 flags)
477{
478 // calculate caps centers in absolute space
479 dVector3 vCp0;
480 vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
481 vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
482 vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
483
484 dVector3 vCp1;
485 vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
486 vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
487 vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
488
489 // reset best axis
490 iBestAxis = 0;
491 // reset best depth
492 fBestDepth = -MAX_REAL;
493 // reset separating axis vector
494 dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
495
496 // Epsilon value for checking axis vector length
497 const dReal fEpsilon = 1e-6f;
498
499 // Translate triangle to Cc cord.
500 SUBTRACT(v0 , vCapsulePosition, vV0);
501 SUBTRACT(v1 , vCapsulePosition, vV1);
502 SUBTRACT(v2 , vCapsulePosition, vV2);
503
504 // We begin to test for 19 separating axis now
505 // I wonder does it help if we employ the method like ISA-GJK???
506 // Or at least we should do experiment and find what axis will
507 // be most likely to be separating axis to check it first.
508
509 // Original
510 // axis vN
511 //vAxis = -vN;
512 vAxis[0] = - vN[0];
513 vAxis[1] = - vN[1];
514 vAxis[2] = - vN[2];
515 if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE))
516 {
517 return FALSE;
518 }
519
520 if (flags & dxTriMeshData::kEdge0)
521 {
522 // axis CxE0 - Edge 0
523 dCROSS(vAxis,=,vCapsuleAxis,vE0);
524 //vAxis = dCROSS( vCapsuleAxis cross vE0 );
525 if( _length2OfVector3( vAxis ) > fEpsilon ) {
526 if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) {
527 return FALSE;
528 }
529 }
530 }
531
532 if (flags & dxTriMeshData::kEdge1)
533 {
534 // axis CxE1 - Edge 1
535 dCROSS(vAxis,=,vCapsuleAxis,vE1);
536 //vAxis = ( vCapsuleAxis cross vE1 );
537 if(_length2OfVector3( vAxis ) > fEpsilon ) {
538 if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) {
539 return FALSE;
540 }
541 }
542 }
543
544 if (flags & dxTriMeshData::kEdge2)
545 {
546 // axis CxE2 - Edge 2
547 //vAxis = ( vCapsuleAxis cross vE2 );
548 dCROSS(vAxis,=,vCapsuleAxis,vE2);
549 if(_length2OfVector3( vAxis ) > fEpsilon ) {
550 if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) {
551 return FALSE;
552 }
553 }
554 }
555
556 if (flags & dxTriMeshData::kEdge0)
557 {
558 // first capsule point
559 // axis ((Cp0-V0) x E0) x E0
560 _CalculateAxis(vCp0,v0,vE0,vE0,vAxis);
561 // vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0;
562 if(_length2OfVector3( vAxis ) > fEpsilon ) {
563 if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) {
564 return FALSE;
565 }
566 }
567 }
568
569 if (flags & dxTriMeshData::kEdge1)
570 {
571 // axis ((Cp0-V1) x E1) x E1
572 _CalculateAxis(vCp0,v1,vE1,vE1,vAxis);
573 //vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1;
574 if(_length2OfVector3( vAxis ) > fEpsilon ) {
575 if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) {
576 return FALSE;
577 }
578 }
579 }
580
581 if (flags & dxTriMeshData::kEdge2)
582 {
583 // axis ((Cp0-V2) x E2) x E2
584 _CalculateAxis(vCp0,v2,vE2,vE2,vAxis);
585 //vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2;
586 if(_length2OfVector3( vAxis ) > fEpsilon ) {
587 if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) {
588 return FALSE;
589 }
590 }
591 }
592
593 if (flags & dxTriMeshData::kEdge0)
594 {
595 // second capsule point
596 // axis ((Cp1-V0) x E0) x E0
597 _CalculateAxis(vCp1,v0,vE0,vE0,vAxis);
598 //vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0;
599 if(_length2OfVector3( vAxis ) > fEpsilon ) {
600 if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) {
601 return FALSE;
602 }
603 }
604 }
605
606 if (flags & dxTriMeshData::kEdge1)
607 {
608 // axis ((Cp1-V1) x E1) x E1
609 _CalculateAxis(vCp1,v1,vE1,vE1,vAxis);
610 //vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1;
611 if(_length2OfVector3( vAxis ) > fEpsilon ) {
612 if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) {
613 return FALSE;
614 }
615 }
616 }
617
618 if (flags & dxTriMeshData::kEdge2)
619 {
620 // axis ((Cp1-V2) x E2) x E2
621 _CalculateAxis(vCp1,v2,vE2,vE2,vAxis);
622 //vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2;
623 if(_length2OfVector3( vAxis ) > fEpsilon ) {
624 if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) {
625 return FALSE;
626 }
627 }
628 }
629
630 if (flags & dxTriMeshData::kVert0)
631 {
632 // first vertex on triangle
633 // axis ((V0-Cp0) x C) x C
634 _CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
635 //vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
636 if(_length2OfVector3( vAxis ) > fEpsilon ) {
637 if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) {
638 return FALSE;
639 }
640 }
641 }
642
643 if (flags & dxTriMeshData::kVert1)
644 {
645 // second vertex on triangle
646 // axis ((V1-Cp0) x C) x C
647 _CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
648 //vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
649 if(_length2OfVector3( vAxis ) > fEpsilon ) {
650 if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) {
651 return FALSE;
652 }
653 }
654 }
655
656 if (flags & dxTriMeshData::kVert2)
657 {
658 // third vertex on triangle
659 // axis ((V2-Cp0) x C) x C
660 _CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
661 //vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
662 if(_length2OfVector3( vAxis ) > fEpsilon ) {
663 if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) {
664 return FALSE;
665 }
666 }
667 }
668
669 // Test as separating axes direction vectors between each triangle
670 // edge and each capsule's cap center
671
672 if (flags & dxTriMeshData::kVert0)
673 {
674 // first triangle vertex and first capsule point
675 //vAxis = v0 - vCp0;
676 SUBTRACT(v0,vCp0,vAxis);
677 if(_length2OfVector3( vAxis ) > fEpsilon ) {
678 if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) {
679 return FALSE;
680 }
681 }
682 }
683
684 if (flags & dxTriMeshData::kVert1)
685 {
686 // second triangle vertex and first capsule point
687 //vAxis = v1 - vCp0;
688 SUBTRACT(v1,vCp0,vAxis);
689 if(_length2OfVector3( vAxis ) > fEpsilon ) {
690 if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) {
691 return FALSE;
692 }
693 }
694 }
695
696 if (flags & dxTriMeshData::kVert2)
697 {
698 // third triangle vertex and first capsule point
699 //vAxis = v2 - vCp0;
700 SUBTRACT(v2,vCp0,vAxis);
701 if(_length2OfVector3( vAxis ) > fEpsilon ) {
702 if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) {
703 return FALSE;
704 }
705 }
706 }
707
708 if (flags & dxTriMeshData::kVert0)
709 {
710 // first triangle vertex and second capsule point
711 //vAxis = v0 - vCp1;
712 SUBTRACT(v0,vCp1,vAxis);
713 if(_length2OfVector3( vAxis ) > fEpsilon ) {
714 if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) {
715 return FALSE;
716 }
717 }
718 }
719
720 if (flags & dxTriMeshData::kVert1)
721 {
722 // second triangle vertex and second capsule point
723 //vAxis = v1 - vCp1;
724 SUBTRACT(v1,vCp1,vAxis);
725 if(_length2OfVector3( vAxis ) > fEpsilon ) {
726 if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) {
727 return FALSE;
728 }
729 }
730 }
731
732 if (flags & dxTriMeshData::kVert2)
733 {
734 // third triangle vertex and second capsule point
735 //vAxis = v2 - vCp1;
736 SUBTRACT(v2,vCp1,vAxis);
737 if(_length2OfVector3( vAxis ) > fEpsilon ) {
738 if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) {
739 return FALSE;
740 }
741 }
742 }
743
744 return TRUE;
745}
746
747// test one mesh triangle on intersection with capsule
748static void _cldTestOneTriangleVSCapsule( const dVector3 &v0,
749 const dVector3 &v1,
750 const dVector3 &v2,
751 uint8 flags)
752{
753
754 // calculate edges
755 SUBTRACT(v1,v0,vE0);
756 SUBTRACT(v2,v1,vE1);
757 SUBTRACT(v0,v2,vE2);
758
759 dVector3 _minus_vE0;
760 SUBTRACT(v0,v1,_minus_vE0);
761
762 // calculate poly normal
763 dCROSS(vN,=,vE1,_minus_vE0);
764 dNormalize3(vN);
765
766 // create plane from triangle
767 dReal plDistance = -dDOT(v0,vN);
768 dVector4 plTrianglePlane;
769 CONSTRUCTPLANE(plTrianglePlane,vN,plDistance);
770
771 // calculate capsule distance to plane
772 dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition);
773
774 // Capsule must be over positive side of triangle
775 if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
776 {
777 // if not don't generate contacts
778 return;
779 }
780
781 dVector3 vPnt0;
782 SET (vPnt0,v0);
783 dVector3 vPnt1;
784 SET (vPnt1,v1);
785 dVector3 vPnt2;
786 SET (vPnt2,v2);
787
788 if (fDistanceCapsuleCenterToPlane < 0 )
789 {
790 SET (vPnt0,v0);
791 SET (vPnt1,v2);
792 SET (vPnt2,v1);
793 }
794
795 // do intersection test and find best separating axis
796 if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags) )
797 {
798 // if not found do nothing
799 return;
800 }
801
802 // if best separation axis is not found
803 if ( iBestAxis == 0 )
804 {
805 // this should not happen (we should already exit in that case)
806 dIASSERT(FALSE);
807 // do nothing
808 return;
809 }
810
811 // calculate caps centers in absolute space
812 dVector3 vCposTrans;
813 vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius;
814 vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius;
815 vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius;
816
817 dVector3 vCEdgePoint0;
818 vCEdgePoint0[0] = vCposTrans[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
819 vCEdgePoint0[1] = vCposTrans[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
820 vCEdgePoint0[2] = vCposTrans[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
821
822 dVector3 vCEdgePoint1;
823 vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
824 vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
825 vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
826
827 // transform capsule edge points into triangle space
828 vCEdgePoint0[0] -= vPnt0[0];
829 vCEdgePoint0[1] -= vPnt0[1];
830 vCEdgePoint0[2] -= vPnt0[2];
831
832 vCEdgePoint1[0] -= vPnt0[0];
833 vCEdgePoint1[1] -= vPnt0[1];
834 vCEdgePoint1[2] -= vPnt0[2];
835
836 dVector4 plPlane;
837 dVector3 _minus_vN;
838 _minus_vN[0] = -vN[0];
839 _minus_vN[1] = -vN[1];
840 _minus_vN[2] = -vN[2];
841 // triangle plane
842 CONSTRUCTPLANE(plPlane,_minus_vN,0);
843 //plPlane = Plane4f( -vN, 0);
844
845 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
846 {
847 return;
848 }
849
850 // plane with edge 0
851 dVector3 vTemp;
852 dCROSS(vTemp,=,vN,vE0);
853 CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
854 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
855 {
856 return;
857 }
858
859 dCROSS(vTemp,=,vN,vE1);
860 CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-REAL(1e-5)));
861 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
862 {
863 return;
864 }
865
866 dCROSS(vTemp,=,vN,vE2);
867 CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
868 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
869 return;
870 }
871
872 // return capsule edge points into absolute space
873 vCEdgePoint0[0] += vPnt0[0];
874 vCEdgePoint0[1] += vPnt0[1];
875 vCEdgePoint0[2] += vPnt0[2];
876
877 vCEdgePoint1[0] += vPnt0[0];
878 vCEdgePoint1[1] += vPnt0[1];
879 vCEdgePoint1[2] += vPnt0[2];
880
881 // calculate depths for both contact points
882 SUBTRACT(vCEdgePoint0,vCapsulePosition,vTemp);
883 dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
884 SUBTRACT(vCEdgePoint1,vCapsulePosition,vTemp);
885 dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
886
887 // clamp depths to zero
888 if(fDepth0 < 0)
889 {
890 fDepth0 = 0.0f;
891 }
892
893 if(fDepth1 < 0 )
894 {
895 fDepth1 = 0.0f;
896 }
897
898 // Cached contacts's data
899 // contact 0
900 dIASSERT(ctContacts < (iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
901 gLocalContacts[ctContacts].fDepth = fDepth0;
902 SET(gLocalContacts[ctContacts].vNormal,vNormal);
903 SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0);
904 gLocalContacts[ctContacts].nFlags = 1;
905 ctContacts++;
906
907 if (ctContacts < (iFlags & NUMC_MASK)) {
908 // contact 1
909 gLocalContacts[ctContacts].fDepth = fDepth1;
910 SET(gLocalContacts[ctContacts].vNormal,vNormal);
911 SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1);
912 gLocalContacts[ctContacts].nFlags = 1;
913 ctContacts++;
914 }
915
916}
917
918// capsule - trimesh by CroTeam
919// Ported by Nguyem Binh
920int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
921{
922 dIASSERT (skip >= (int)sizeof(dContactGeom));
923 dIASSERT (o1->type == dTriMeshClass);
924 dIASSERT (o2->type == dCapsuleClass);
925 dIASSERT ((flags & NUMC_MASK) >= 1);
926
927 dxTriMesh* TriMesh = (dxTriMesh*)o1;
928 gCylinder = o2;
929 gTriMesh = o1;
930
931 const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder);
932 memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
933
934 const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder);
935 memcpy(vCapsulePosition,pDst,sizeof(dVector3));
936
937 vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
938 vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
939 vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
940
941 // Get size of Capsule
942 dGeomCapsuleGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
943 fCapsuleSize += 2*vCapsuleRadius;
944
945 const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
946 memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3));
947
948 const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
949 memcpy(mTriMeshPos,pTriPos,sizeof(dVector3));
950
951 // global info for contact creation
952 iStride =skip;
953 iFlags =flags;
954 ContactGeoms =contact;
955
956 // reset contact counter
957 ctContacts = 0;
958
959 // reset best depth
960 fBestDepth = - MAX_REAL;
961 fBestCenter = 0;
962 fBestrt = 0;
963
964
965
966
967 // reset collision normal
968 vNormal[0] = REAL(0.0);
969 vNormal[1] = REAL(0.0);
970 vNormal[2] = REAL(0.0);
971
972 // Will it better to use LSS here? -> confirm Pierre.
973 OBBCollider& Collider = TriMesh->_OBBCollider;
974
975 // It is a potential issue to explicitly cast to float
976 // if custom width floating point type is introduced in OPCODE.
977 // It is necessary to make a typedef and cast to it
978 // (e.g. typedef float opc_float;)
979 // However I'm not sure in what header it should be added.
980
981 Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]);
982 Point cExtents(/*(float)*/ vCapsuleRadius, /*(float)*/ vCapsuleRadius,/*(float)*/ fCapsuleSize/2);
983
984 Matrix3x3 obbRot;
985
986 obbRot[0][0] = /*(float)*/ mCapsuleRotation[0];
987 obbRot[1][0] = /*(float)*/ mCapsuleRotation[1];
988 obbRot[2][0] = /*(float)*/ mCapsuleRotation[2];
989
990 obbRot[0][1] = /*(float)*/ mCapsuleRotation[4];
991 obbRot[1][1] = /*(float)*/ mCapsuleRotation[5];
992 obbRot[2][1] = /*(float)*/ mCapsuleRotation[6];
993
994 obbRot[0][2] = /*(float)*/ mCapsuleRotation[8];
995 obbRot[1][2] = /*(float)*/ mCapsuleRotation[9];
996 obbRot[2][2] = /*(float)*/ mCapsuleRotation[10];
997
998 OBB obbCapsule(cCenter,cExtents,obbRot);
999
1000 Matrix4x4 CapsuleMatrix;
1001 MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix);
1002
1003 Matrix4x4 MeshMatrix;
1004 MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix);
1005
1006 // TC results
1007 if (TriMesh->doBoxTC) {
1008 dxTriMesh::BoxTC* BoxTC = 0;
1009 for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
1010 if (TriMesh->BoxTCCache[i].Geom == gCylinder){
1011 BoxTC = &TriMesh->BoxTCCache[i];
1012 break;
1013 }
1014 }
1015 if (!BoxTC){
1016 TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
1017
1018 BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
1019 BoxTC->Geom = gCylinder;
1020 BoxTC->FatCoeff = 1.0f;
1021 }
1022
1023 // Intersect
1024 Collider.SetTemporalCoherence(true);
1025 Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix);
1026 }
1027 else {
1028 Collider.SetTemporalCoherence(false);
1029 Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix);
1030 }
1031
1032 if (! Collider.GetContactStatus()) {
1033 // no collision occurred
1034 return 0;
1035 }
1036
1037 // Retrieve data
1038 int TriCount = Collider.GetNbTouchedPrimitives();
1039 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1040
1041 if (TriCount != 0)
1042 {
1043 if (TriMesh->ArrayCallback != null)
1044 {
1045 TriMesh->ArrayCallback(TriMesh, gCylinder, Triangles, TriCount);
1046 }
1047
1048 // allocate buffer for local contacts on stack
1049 gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(iFlags & NUMC_MASK));
1050
1051 unsigned int ctContacts0 = ctContacts;
1052
1053 uint8* UseFlags = TriMesh->Data->UseFlags;
1054
1055 // loop through all intersecting triangles
1056 for (int i = 0; i < TriCount; i++)
1057 {
1058 const int Triint = Triangles[i];
1059 if (!Callback(TriMesh, gCylinder, Triint)) continue;
1060
1061
1062 dVector3 dv[3];
1063 FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv);
1064
1065 uint8 flags = UseFlags ? UseFlags[Triint] : dxTriMeshData::kUseAll;
1066
1067 // test this triangle
1068 _cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags);
1069
1070 // fill-in tri index for generated contacts
1071 for (; ctContacts0<ctContacts; ctContacts0++)
1072 gLocalContacts[ctContacts0].triIndex = Triint;
1073
1074 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1075 if(ctContacts>=(iFlags & NUMC_MASK))
1076 {
1077 break;
1078 }
1079
1080 }
1081 }
1082
1083 return _ProcessLocalContacts();
1084}
1085#endif
1086
1087// GIMPACT version
1088#if dTRIMESH_GIMPACT
1089#define nCAPSULE_AXIS 2
1090// capsule - trimesh By francisco leon
1091int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
1092{
1093 dIASSERT (skip >= (int)sizeof(dContactGeom));
1094 dIASSERT (o1->type == dTriMeshClass);
1095 dIASSERT (o2->type == dCapsuleClass);
1096 dIASSERT ((flags & NUMC_MASK) >= 1);
1097
1098 dxTriMesh* TriMesh = (dxTriMesh*)o1;
1099 dxGeom* gCylinder = o2;
1100
1101 //Get capsule params
1102 dMatrix3 mCapsuleRotation;
1103 dVector3 vCapsulePosition;
1104 dVector3 vCapsuleAxis;
1105 dReal vCapsuleRadius;
1106 dReal fCapsuleSize;
1107 dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
1108 memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
1109 dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
1110 memcpy(vCapsulePosition,pDst,sizeof(dVector3));
1111 //Axis
1112 vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
1113 vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
1114 vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
1115 // Get size of CCylinder
1116 dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
1117 fCapsuleSize*=0.5f;
1118 //Set Capsule params
1119 GIM_CAPSULE_DATA capsule;
1120
1121 capsule.m_radius = vCapsuleRadius;
1122 VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
1123 VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
1124 VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
1125 VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
1126
1127
1128//Create contact list
1129 GDYNAMIC_ARRAY trimeshcontacts;
1130 GIM_CREATE_CONTACT_LIST(trimeshcontacts);
1131
1132 //Collide trimeshe vs capsule
1133 gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
1134
1135
1136 if(trimeshcontacts.m_size == 0)
1137 {
1138 GIM_DYNARRAY_DESTROY(trimeshcontacts);
1139 return 0;
1140 }
1141
1142 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
1143
1144 unsigned contactcount = trimeshcontacts.m_size;
1145 unsigned contactmax = (unsigned)(flags & NUMC_MASK);
1146 if (contactcount > contactmax)
1147 {
1148 contactcount = contactmax;
1149 }
1150
1151 dContactGeom* pcontact;
1152 unsigned i;
1153
1154 for (i=0;i<contactcount;i++)
1155 {
1156 pcontact = SAFECONTACT(flags, contact, i, skip);
1157
1158 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
1159 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
1160 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
1161 pcontact->pos[3] = 1.0f;
1162
1163 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
1164 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
1165 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
1166 pcontact->normal[3] = 0;
1167
1168 pcontact->depth = ptrimeshcontacts->m_depth;
1169 pcontact->g1 = TriMesh;
1170 pcontact->g2 = gCylinder;
1171
1172 ptrimeshcontacts++;
1173 }
1174
1175 GIM_DYNARRAY_DESTROY(trimeshcontacts);
1176
1177 return (int)contactcount;
1178}
1179#endif
1180
1181#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp
new file mode 100644
index 0000000..717c763
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_distance.cpp
@@ -0,0 +1,1255 @@
1// This file contains some code based on the code from Magic Software.
2// That code is available under a Free Source License Agreement
3// that can be found at http://www.magic-software.com/License/free.pdf
4
5#include <ode/common.h>
6#include <ode/odemath.h>
7#include <ode/collision.h>
8#define TRIMESH_INTERNAL
9#include "collision_trimesh_internal.h"
10
11//------------------------------------------------------------------------------
12/**
13 @brief Finds the shortest distance squared between a point and a triangle.
14
15 @param pfSParam Barycentric coordinate of triangle at point closest to p (u)
16 @param pfTParam Barycentric coordinate of triangle at point closest to p (v)
17 @return Shortest distance squared.
18
19 The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
20
21 Taken from:
22 Magic Software, Inc.
23 http://www.magic-software.com
24*/
25dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin,
26 const dVector3 triEdge0, const dVector3 triEdge1,
27 dReal* pfSParam, dReal* pfTParam )
28{
29 dVector3 kDiff;
30 Vector3Subtract( triOrigin, p, kDiff );
31 dReal fA00 = dDOT( triEdge0, triEdge0 );
32 dReal fA01 = dDOT( triEdge0, triEdge1 );
33 dReal fA11 = dDOT( triEdge1, triEdge1 );
34 dReal fB0 = dDOT( kDiff, triEdge0 );
35 dReal fB1 = dDOT( kDiff, triEdge1 );
36 dReal fC = dDOT( kDiff, kDiff );
37 dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
38 dReal fS = fA01*fB1-fA11*fB0;
39 dReal fT = fA01*fB0-fA00*fB1;
40 dReal fSqrDist;
41
42 if ( fS + fT <= fDet )
43 {
44 if ( fS < REAL(0.0) )
45 {
46 if ( fT < REAL(0.0) ) // region 4
47 {
48 if ( fB0 < REAL(0.0) )
49 {
50 fT = REAL(0.0);
51 if ( -fB0 >= fA00 )
52 {
53 fS = REAL(1.0);
54 fSqrDist = fA00+REAL(2.0)*fB0+fC;
55 }
56 else
57 {
58 fS = -fB0/fA00;
59 fSqrDist = fB0*fS+fC;
60 }
61 }
62 else
63 {
64 fS = REAL(0.0);
65 if ( fB1 >= REAL(0.0) )
66 {
67 fT = REAL(0.0);
68 fSqrDist = fC;
69 }
70 else if ( -fB1 >= fA11 )
71 {
72 fT = REAL(1.0);
73 fSqrDist = fA11+REAL(2.0)*fB1+fC;
74 }
75 else
76 {
77 fT = -fB1/fA11;
78 fSqrDist = fB1*fT+fC;
79 }
80 }
81 }
82 else // region 3
83 {
84 fS = REAL(0.0);
85 if ( fB1 >= REAL(0.0) )
86 {
87 fT = REAL(0.0);
88 fSqrDist = fC;
89 }
90 else if ( -fB1 >= fA11 )
91 {
92 fT = REAL(1.0);
93 fSqrDist = fA11+REAL(2.0)*fB1+fC;
94 }
95 else
96 {
97 fT = -fB1/fA11;
98 fSqrDist = fB1*fT+fC;
99 }
100 }
101 }
102 else if ( fT < REAL(0.0) ) // region 5
103 {
104 fT = REAL(0.0);
105 if ( fB0 >= REAL(0.0) )
106 {
107 fS = REAL(0.0);
108 fSqrDist = fC;
109 }
110 else if ( -fB0 >= fA00 )
111 {
112 fS = REAL(1.0);
113 fSqrDist = fA00+REAL(2.0)*fB0+fC;
114 }
115 else
116 {
117 fS = -fB0/fA00;
118 fSqrDist = fB0*fS+fC;
119 }
120 }
121 else // region 0
122 {
123 // minimum at interior point
124 if ( fDet == REAL(0.0) )
125 {
126 fS = REAL(0.0);
127 fT = REAL(0.0);
128 fSqrDist = dInfinity;
129 }
130 else
131 {
132 dReal fInvDet = REAL(1.0)/fDet;
133 fS *= fInvDet;
134 fT *= fInvDet;
135 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
136 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
137 }
138 }
139 }
140 else
141 {
142 dReal fTmp0, fTmp1, fNumer, fDenom;
143
144 if ( fS < REAL(0.0) ) // region 2
145 {
146 fTmp0 = fA01 + fB0;
147 fTmp1 = fA11 + fB1;
148 if ( fTmp1 > fTmp0 )
149 {
150 fNumer = fTmp1 - fTmp0;
151 fDenom = fA00-REAL(2.0)*fA01+fA11;
152 if ( fNumer >= fDenom )
153 {
154 fS = REAL(1.0);
155 fT = REAL(0.0);
156 fSqrDist = fA00+REAL(2.0)*fB0+fC;
157 }
158 else
159 {
160 fS = fNumer/fDenom;
161 fT = REAL(1.0) - fS;
162 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
163 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
164 }
165 }
166 else
167 {
168 fS = REAL(0.0);
169 if ( fTmp1 <= REAL(0.0) )
170 {
171 fT = REAL(1.0);
172 fSqrDist = fA11+REAL(2.0)*fB1+fC;
173 }
174 else if ( fB1 >= REAL(0.0) )
175 {
176 fT = REAL(0.0);
177 fSqrDist = fC;
178 }
179 else
180 {
181 fT = -fB1/fA11;
182 fSqrDist = fB1*fT+fC;
183 }
184 }
185 }
186 else if ( fT < REAL(0.0) ) // region 6
187 {
188 fTmp0 = fA01 + fB1;
189 fTmp1 = fA00 + fB0;
190 if ( fTmp1 > fTmp0 )
191 {
192 fNumer = fTmp1 - fTmp0;
193 fDenom = fA00-REAL(2.0)*fA01+fA11;
194 if ( fNumer >= fDenom )
195 {
196 fT = REAL(1.0);
197 fS = REAL(0.0);
198 fSqrDist = fA11+REAL(2.0)*fB1+fC;
199 }
200 else
201 {
202 fT = fNumer/fDenom;
203 fS = REAL(1.0) - fT;
204 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
205 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
206 }
207 }
208 else
209 {
210 fT = REAL(0.0);
211 if ( fTmp1 <= REAL(0.0) )
212 {
213 fS = REAL(1.0);
214 fSqrDist = fA00+REAL(2.0)*fB0+fC;
215 }
216 else if ( fB0 >= REAL(0.0) )
217 {
218 fS = REAL(0.0);
219 fSqrDist = fC;
220 }
221 else
222 {
223 fS = -fB0/fA00;
224 fSqrDist = fB0*fS+fC;
225 }
226 }
227 }
228 else // region 1
229 {
230 fNumer = fA11 + fB1 - fA01 - fB0;
231 if ( fNumer <= REAL(0.0) )
232 {
233 fS = REAL(0.0);
234 fT = REAL(1.0);
235 fSqrDist = fA11+REAL(2.0)*fB1+fC;
236 }
237 else
238 {
239 fDenom = fA00-REAL(2.0)*fA01+fA11;
240 if ( fNumer >= fDenom )
241 {
242 fS = REAL(1.0);
243 fT = REAL(0.0);
244 fSqrDist = fA00+REAL(2.0)*fB0+fC;
245 }
246 else
247 {
248 fS = fNumer/fDenom;
249 fT = REAL(1.0) - fS;
250 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
251 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
252 }
253 }
254 }
255 }
256
257 if ( pfSParam )
258 *pfSParam = (float)fS;
259
260 if ( pfTParam )
261 *pfTParam = (float)fT;
262
263 return dReal(fabs(fSqrDist));
264}
265
266//------------------------------------------------------------------------------
267/**
268 @brief Finds the shortest distance squared between two line segments.
269 @param pfSegP0 t value for seg1 where the shortest distance between
270 the segments exists.
271 @param pfSegP0 t value for seg2 where the shortest distance between
272 the segments exists.
273 @return Shortest distance squared.
274
275 Taken from:
276 Magic Software, Inc.
277 http://www.magic-software.com
278*/
279dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction,
280 const dVector3 seg2Origin, const dVector3 seg2Direction,
281 dReal* pfSegP0, dReal* pfSegP1 )
282{
283 const dReal gs_fTolerance = 1e-05f;
284 dVector3 kDiff, kNegDiff, seg1NegDirection;
285 Vector3Subtract( seg1Origin, seg2Origin, kDiff );
286 Vector3Negate( kDiff, kNegDiff );
287 dReal fA00 = dDOT( seg1Direction, seg1Direction );
288 Vector3Negate( seg1Direction, seg1NegDirection );
289 dReal fA01 = dDOT( seg1NegDirection, seg2Direction );
290 dReal fA11 = dDOT( seg2Direction, seg2Direction );
291 dReal fB0 = dDOT( kDiff, seg1Direction );
292 dReal fC = dDOT( kDiff, kDiff );
293 dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
294 dReal fB1, fS, fT, fSqrDist, fTmp;
295
296 if ( fDet >= gs_fTolerance )
297 {
298 // line segments are not parallel
299 fB1 = dDOT( kNegDiff, seg2Direction );
300 fS = fA01*fB1-fA11*fB0;
301 fT = fA01*fB0-fA00*fB1;
302
303 if ( fS >= REAL(0.0) )
304 {
305 if ( fS <= fDet )
306 {
307 if ( fT >= REAL(0.0) )
308 {
309 if ( fT <= fDet ) // region 0 (interior)
310 {
311 // minimum at two interior points of 3D lines
312 dReal fInvDet = REAL(1.0)/fDet;
313 fS *= fInvDet;
314 fT *= fInvDet;
315 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
316 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
317 }
318 else // region 3 (side)
319 {
320 fT = REAL(1.0);
321 fTmp = fA01+fB0;
322 if ( fTmp >= REAL(0.0) )
323 {
324 fS = REAL(0.0);
325 fSqrDist = fA11+REAL(2.0)*fB1+fC;
326 }
327 else if ( -fTmp >= fA00 )
328 {
329 fS = REAL(1.0);
330 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
331 }
332 else
333 {
334 fS = -fTmp/fA00;
335 fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
336 }
337 }
338 }
339 else // region 7 (side)
340 {
341 fT = REAL(0.0);
342 if ( fB0 >= REAL(0.0) )
343 {
344 fS = REAL(0.0);
345 fSqrDist = fC;
346 }
347 else if ( -fB0 >= fA00 )
348 {
349 fS = REAL(1.0);
350 fSqrDist = fA00+REAL(2.0)*fB0+fC;
351 }
352 else
353 {
354 fS = -fB0/fA00;
355 fSqrDist = fB0*fS+fC;
356 }
357 }
358 }
359 else
360 {
361 if ( fT >= REAL(0.0) )
362 {
363 if ( fT <= fDet ) // region 1 (side)
364 {
365 fS = REAL(1.0);
366 fTmp = fA01+fB1;
367 if ( fTmp >= REAL(0.0) )
368 {
369 fT = REAL(0.0);
370 fSqrDist = fA00+REAL(2.0)*fB0+fC;
371 }
372 else if ( -fTmp >= fA11 )
373 {
374 fT = REAL(1.0);
375 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
376 }
377 else
378 {
379 fT = -fTmp/fA11;
380 fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
381 }
382 }
383 else // region 2 (corner)
384 {
385 fTmp = fA01+fB0;
386 if ( -fTmp <= fA00 )
387 {
388 fT = REAL(1.0);
389 if ( fTmp >= REAL(0.0) )
390 {
391 fS = REAL(0.0);
392 fSqrDist = fA11+REAL(2.0)*fB1+fC;
393 }
394 else
395 {
396 fS = -fTmp/fA00;
397 fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
398 }
399 }
400 else
401 {
402 fS = REAL(1.0);
403 fTmp = fA01+fB1;
404 if ( fTmp >= REAL(0.0) )
405 {
406 fT = REAL(0.0);
407 fSqrDist = fA00+REAL(2.0)*fB0+fC;
408 }
409 else if ( -fTmp >= fA11 )
410 {
411 fT = REAL(1.0);
412 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
413 }
414 else
415 {
416 fT = -fTmp/fA11;
417 fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
418 }
419 }
420 }
421 }
422 else // region 8 (corner)
423 {
424 if ( -fB0 < fA00 )
425 {
426 fT = REAL(0.0);
427 if ( fB0 >= REAL(0.0) )
428 {
429 fS = REAL(0.0);
430 fSqrDist = fC;
431 }
432 else
433 {
434 fS = -fB0/fA00;
435 fSqrDist = fB0*fS+fC;
436 }
437 }
438 else
439 {
440 fS = REAL(1.0);
441 fTmp = fA01+fB1;
442 if ( fTmp >= REAL(0.0) )
443 {
444 fT = REAL(0.0);
445 fSqrDist = fA00+REAL(2.0)*fB0+fC;
446 }
447 else if ( -fTmp >= fA11 )
448 {
449 fT = REAL(1.0);
450 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
451 }
452 else
453 {
454 fT = -fTmp/fA11;
455 fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
456 }
457 }
458 }
459 }
460 }
461 else
462 {
463 if ( fT >= REAL(0.0) )
464 {
465 if ( fT <= fDet ) // region 5 (side)
466 {
467 fS = REAL(0.0);
468 if ( fB1 >= REAL(0.0) )
469 {
470 fT = REAL(0.0);
471 fSqrDist = fC;
472 }
473 else if ( -fB1 >= fA11 )
474 {
475 fT = REAL(1.0);
476 fSqrDist = fA11+REAL(2.0)*fB1+fC;
477 }
478 else
479 {
480 fT = -fB1/fA11;
481 fSqrDist = fB1*fT+fC;
482 }
483 }
484 else // region 4 (corner)
485 {
486 fTmp = fA01+fB0;
487 if ( fTmp < REAL(0.0) )
488 {
489 fT = REAL(1.0);
490 if ( -fTmp >= fA00 )
491 {
492 fS = REAL(1.0);
493 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
494 }
495 else
496 {
497 fS = -fTmp/fA00;
498 fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
499 }
500 }
501 else
502 {
503 fS = REAL(0.0);
504 if ( fB1 >= REAL(0.0) )
505 {
506 fT = REAL(0.0);
507 fSqrDist = fC;
508 }
509 else if ( -fB1 >= fA11 )
510 {
511 fT = REAL(1.0);
512 fSqrDist = fA11+REAL(2.0)*fB1+fC;
513 }
514 else
515 {
516 fT = -fB1/fA11;
517 fSqrDist = fB1*fT+fC;
518 }
519 }
520 }
521 }
522 else // region 6 (corner)
523 {
524 if ( fB0 < REAL(0.0) )
525 {
526 fT = REAL(0.0);
527 if ( -fB0 >= fA00 )
528 {
529 fS = REAL(1.0);
530 fSqrDist = fA00+REAL(2.0)*fB0+fC;
531 }
532 else
533 {
534 fS = -fB0/fA00;
535 fSqrDist = fB0*fS+fC;
536 }
537 }
538 else
539 {
540 fS = REAL(0.0);
541 if ( fB1 >= REAL(0.0) )
542 {
543 fT = REAL(0.0);
544 fSqrDist = fC;
545 }
546 else if ( -fB1 >= fA11 )
547 {
548 fT = REAL(1.0);
549 fSqrDist = fA11+REAL(2.0)*fB1+fC;
550 }
551 else
552 {
553 fT = -fB1/fA11;
554 fSqrDist = fB1*fT+fC;
555 }
556 }
557 }
558 }
559 }
560 else
561 {
562 // line segments are parallel
563 if ( fA01 > REAL(0.0) )
564 {
565 // direction vectors form an obtuse angle
566 if ( fB0 >= REAL(0.0) )
567 {
568 fS = REAL(0.0);
569 fT = REAL(0.0);
570 fSqrDist = fC;
571 }
572 else if ( -fB0 <= fA00 )
573 {
574 fS = -fB0/fA00;
575 fT = REAL(0.0);
576 fSqrDist = fB0*fS+fC;
577 }
578 else
579 {
580 //fB1 = -kDiff % seg2.m;
581 fB1 = dDOT( kNegDiff, seg2Direction );
582 fS = REAL(1.0);
583 fTmp = fA00+fB0;
584 if ( -fTmp >= fA01 )
585 {
586 fT = REAL(1.0);
587 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1);
588 }
589 else
590 {
591 fT = -fTmp/fA01;
592 fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1));
593 }
594 }
595 }
596 else
597 {
598 // direction vectors form an acute angle
599 if ( -fB0 >= fA00 )
600 {
601 fS = REAL(1.0);
602 fT = REAL(0.0);
603 fSqrDist = fA00+REAL(2.0)*fB0+fC;
604 }
605 else if ( fB0 <= REAL(0.0) )
606 {
607 fS = -fB0/fA00;
608 fT = REAL(0.0);
609 fSqrDist = fB0*fS+fC;
610 }
611 else
612 {
613 fB1 = dDOT( kNegDiff, seg2Direction );
614 fS = REAL(0.0);
615 if ( fB0 >= -fA01 )
616 {
617 fT = REAL(1.0);
618 fSqrDist = fA11+REAL(2.0)*fB1+fC;
619 }
620 else
621 {
622 fT = -fB0/fA01;
623 fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT);
624 }
625 }
626 }
627 }
628
629 if ( pfSegP0 )
630 *pfSegP0 = fS;
631
632 if ( pfSegP1 )
633 *pfSegP1 = fT;
634
635 return dReal(fabs(fSqrDist));
636}
637
638//------------------------------------------------------------------------------
639/**
640 @brief Finds the shortest distance squared between a line segment and
641 a triangle.
642
643 @param pfSegP t value for the line segment where the shortest distance between
644 the segment and the triangle occurs.
645 So the point along the segment that is the shortest distance
646 away from the triangle can be obtained by (seg.end - seg.start) * t.
647 @param pfTriP0 Barycentric coordinate of triangle at point closest to seg (u)
648 @param pfTriP1 Barycentric coordinate of triangle at point closest to seg (v)
649 @return Shortest distance squared.
650
651 The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
652
653 Taken from:
654 Magic Software, Inc.
655 http://www.magic-software.com
656*/
657dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd,
658 const dVector3 triOrigin,
659 const dVector3 triEdge0, const dVector3 triEdge1,
660 dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 )
661{
662 const dReal gs_fTolerance = 1e-06f;
663 dVector3 segDirection, segNegDirection, kDiff, kNegDiff;
664 Vector3Subtract( segEnd, segOrigin, segDirection );
665 Vector3Negate( segDirection, segNegDirection );
666 Vector3Subtract( triOrigin, segOrigin, kDiff );
667 Vector3Negate( kDiff, kNegDiff );
668 dReal fA00 = dDOT( segDirection, segDirection );
669 dReal fA01 = dDOT( segNegDirection, triEdge0 );
670 dReal fA02 = dDOT( segNegDirection, triEdge1 );
671 dReal fA11 = dDOT( triEdge0, triEdge0 );
672 dReal fA12 = dDOT( triEdge0, triEdge1 );
673 dReal fA22 = dDOT( triEdge1, triEdge1 );
674 dReal fB0 = dDOT( kNegDiff, segDirection );
675 dReal fB1 = dDOT( kDiff, triEdge0 );
676 dReal fB2 = dDOT( kDiff, triEdge1 );
677
678 dVector3 kTriSegOrigin, kTriSegDirection, kPt;
679 dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
680
681 // Set up for a relative error test on the angle between ray direction
682 // and triangle normal to determine parallel/nonparallel status.
683 dVector3 kN;
684 dCROSS( kN, =, triEdge0, triEdge1 );
685 dReal fNSqrLen = dDOT( kN, kN );
686 dReal fDot = dDOT( segDirection, kN );
687 bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen);
688
689 if ( bNotParallel )
690 {
691 dReal fCof00 = fA11*fA22-fA12*fA12;
692 dReal fCof01 = fA02*fA12-fA01*fA22;
693 dReal fCof02 = fA01*fA12-fA02*fA11;
694 dReal fCof11 = fA00*fA22-fA02*fA02;
695 dReal fCof12 = fA02*fA01-fA00*fA12;
696 dReal fCof22 = fA00*fA11-fA01*fA01;
697 dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02);
698 dReal fRhs0 = -fB0*fInvDet;
699 dReal fRhs1 = -fB1*fInvDet;
700 dReal fRhs2 = -fB2*fInvDet;
701
702 fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
703 fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
704 fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
705
706 if ( fR < REAL(0.0) )
707 {
708 if ( fS+fT <= REAL(1.0) )
709 {
710 if ( fS < REAL(0.0) )
711 {
712 if ( fT < REAL(0.0) ) // region 4m
713 {
714 // min on face s=0 or t=0 or r=0
715 Vector3Copy( triOrigin, kTriSegOrigin );
716 Vector3Copy( triEdge1, kTriSegDirection );
717 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
718 kTriSegOrigin, kTriSegDirection,
719 &fR, &fT );
720 fS = REAL(0.0);
721 Vector3Copy( triOrigin, kTriSegOrigin );
722 Vector3Copy( triEdge0, kTriSegDirection );
723 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
724 kTriSegOrigin, kTriSegDirection,
725 &fR0, &fS0 );
726 fT0 = REAL(0.0);
727 if ( fSqrDist0 < fSqrDist )
728 {
729 fSqrDist = fSqrDist0;
730 fR = fR0;
731 fS = fS0;
732 fT = fT0;
733 }
734 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
735 &fS0, &fT0 );
736 fR0 = REAL(0.0);
737 if ( fSqrDist0 < fSqrDist )
738 {
739 fSqrDist = fSqrDist0;
740 fR = fR0;
741 fS = fS0;
742 fT = fT0;
743 }
744 }
745 else // region 3m
746 {
747 // min on face s=0 or r=0
748 Vector3Copy( triOrigin, kTriSegOrigin );
749 Vector3Copy( triEdge1, kTriSegDirection );
750 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
751 kTriSegOrigin, kTriSegDirection,
752 &fR,&fT );
753 fS = REAL(0.0);
754 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
755 &fS0, &fT0 );
756 fR0 = REAL(0.0);
757 if ( fSqrDist0 < fSqrDist )
758 {
759 fSqrDist = fSqrDist0;
760 fR = fR0;
761 fS = fS0;
762 fT = fT0;
763 }
764 }
765 }
766 else if ( fT < REAL(0.0) ) // region 5m
767 {
768 // min on face t=0 or r=0
769 Vector3Copy( triOrigin, kTriSegOrigin );
770 Vector3Copy( triEdge0, kTriSegDirection );
771 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
772 kTriSegOrigin, kTriSegDirection,
773 &fR, &fS );
774 fT = REAL(0.0);
775 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
776 &fS0, &fT0 );
777 fR0 = REAL(0.0);
778 if ( fSqrDist0 < fSqrDist )
779 {
780 fSqrDist = fSqrDist0;
781 fR = fR0;
782 fS = fS0;
783 fT = fT0;
784 }
785 }
786 else // region 0m
787 {
788 // min on face r=0
789 fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
790 &fS, &fT );
791 fR = REAL(0.0);
792 }
793 }
794 else
795 {
796 if ( fS < REAL(0.0) ) // region 2m
797 {
798 // min on face s=0 or s+t=1 or r=0
799 Vector3Copy( triOrigin, kTriSegOrigin );
800 Vector3Copy( triEdge1, kTriSegDirection );
801 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
802 kTriSegOrigin, kTriSegDirection,
803 &fR, &fT );
804 fS = REAL(0.0);
805 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
806 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
807 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
808 kTriSegOrigin, kTriSegDirection,
809 &fR0, &fT0 );
810 fS0 = REAL(1.0) - fT0;
811 if ( fSqrDist0 < fSqrDist )
812 {
813 fSqrDist = fSqrDist0;
814 fR = fR0;
815 fS = fS0;
816 fT = fT0;
817 }
818 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
819 &fS0, &fT0 );
820 fR0 = REAL(0.0);
821 if ( fSqrDist0 < fSqrDist )
822 {
823 fSqrDist = fSqrDist0;
824 fR = fR0;
825 fS = fS0;
826 fT = fT0;
827 }
828 }
829 else if ( fT < REAL(0.0) ) // region 6m
830 {
831 // min on face t=0 or s+t=1 or r=0
832 Vector3Copy( triOrigin, kTriSegOrigin );
833 Vector3Copy( triEdge0, kTriSegDirection );
834 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
835 kTriSegOrigin, kTriSegDirection,
836 &fR, &fS );
837 fT = REAL(0.0);
838 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
839 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
840 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
841 kTriSegOrigin, kTriSegDirection,
842 &fR0, &fT0 );
843 fS0 = REAL(1.0) - fT0;
844 if ( fSqrDist0 < fSqrDist )
845 {
846 fSqrDist = fSqrDist0;
847 fR = fR0;
848 fS = fS0;
849 fT = fT0;
850 }
851 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
852 &fS0, &fT0 );
853 fR0 = REAL(0.0);
854 if ( fSqrDist0 < fSqrDist )
855 {
856 fSqrDist = fSqrDist0;
857 fR = fR0;
858 fS = fS0;
859 fT = fT0;
860 }
861 }
862 else // region 1m
863 {
864 // min on face s+t=1 or r=0
865 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
866 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
867 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
868 kTriSegOrigin, kTriSegDirection,
869 &fR, &fT );
870 fS = REAL(1.0) - fT;
871 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
872 &fS0, &fT0 );
873 fR0 = REAL(0.0);
874 if ( fSqrDist0 < fSqrDist )
875 {
876 fSqrDist = fSqrDist0;
877 fR = fR0;
878 fS = fS0;
879 fT = fT0;
880 }
881 }
882 }
883 }
884 else if ( fR <= REAL(1.0) )
885 {
886 if ( fS+fT <= REAL(1.0) )
887 {
888 if ( fS < REAL(0.0) )
889 {
890 if ( fT < REAL(0.0) ) // region 4
891 {
892 // min on face s=0 or t=0
893 Vector3Copy( triOrigin, kTriSegOrigin );
894 Vector3Copy( triEdge1, kTriSegDirection );
895 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
896 kTriSegOrigin, kTriSegDirection,
897 &fR, &fT );
898 fS = REAL(0.0);
899 Vector3Copy( triOrigin, kTriSegOrigin );
900 Vector3Copy( triEdge0, kTriSegDirection );
901 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
902 kTriSegOrigin, kTriSegDirection,
903 &fR0, &fS0 );
904 fT0 = REAL(0.0);
905 if ( fSqrDist0 < fSqrDist )
906 {
907 fSqrDist = fSqrDist0;
908 fR = fR0;
909 fS = fS0;
910 fT = fT0;
911 }
912 }
913 else // region 3
914 {
915 // min on face s=0
916 Vector3Copy( triOrigin, kTriSegOrigin );
917 Vector3Copy( triEdge1, kTriSegDirection );
918 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
919 kTriSegOrigin, kTriSegDirection,
920 &fR, &fT );
921 fS = REAL(0.0);
922 }
923 }
924 else if ( fT < REAL(0.0) ) // region 5
925 {
926 // min on face t=0
927 Vector3Copy( triOrigin, kTriSegOrigin );
928 Vector3Copy( triEdge0, kTriSegDirection );
929 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
930 kTriSegOrigin, kTriSegDirection,
931 &fR, &fS );
932 fT = REAL(0.0);
933 }
934 else // region 0
935 {
936 // global minimum is interior, done
937 fSqrDist = REAL(0.0);
938 }
939 }
940 else
941 {
942 if ( fS < REAL(0.0) ) // region 2
943 {
944 // min on face s=0 or s+t=1
945 Vector3Copy( triOrigin, kTriSegOrigin );
946 Vector3Copy( triEdge1, kTriSegDirection );
947 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
948 kTriSegOrigin, kTriSegDirection,
949 &fR, &fT );
950 fS = REAL(0.0);
951 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
952 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
953 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
954 kTriSegOrigin, kTriSegDirection,
955 &fR0, &fT0 );
956 fS0 = REAL(1.0) - fT0;
957 if ( fSqrDist0 < fSqrDist )
958 {
959 fSqrDist = fSqrDist0;
960 fR = fR0;
961 fS = fS0;
962 fT = fT0;
963 }
964 }
965 else if ( fT < REAL(0.0) ) // region 6
966 {
967 // min on face t=0 or s+t=1
968 Vector3Copy( triOrigin, kTriSegOrigin );
969 Vector3Copy( triEdge0, kTriSegDirection );
970 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
971 kTriSegOrigin, kTriSegDirection,
972 &fR, &fS );
973 fT = REAL(0.0);
974 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
975 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
976 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
977 kTriSegOrigin, kTriSegDirection,
978 &fR0, &fT0 );
979 fS0 = REAL(1.0) - fT0;
980 if ( fSqrDist0 < fSqrDist )
981 {
982 fSqrDist = fSqrDist0;
983 fR = fR0;
984 fS = fS0;
985 fT = fT0;
986 }
987 }
988 else // region 1
989 {
990 // min on face s+t=1
991 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
992 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
993 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
994 kTriSegOrigin, kTriSegDirection,
995 &fR, &fT );
996 fS = REAL(1.0) - fT;
997 }
998 }
999 }
1000 else // fR > 1
1001 {
1002 if ( fS+fT <= REAL(1.0) )
1003 {
1004 if ( fS < REAL(0.0) )
1005 {
1006 if ( fT < REAL(0.0) ) // region 4p
1007 {
1008 // min on face s=0 or t=0 or r=1
1009 Vector3Copy( triOrigin, kTriSegOrigin );
1010 Vector3Copy( triEdge1, kTriSegDirection );
1011 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1012 kTriSegOrigin, kTriSegDirection,
1013 &fR, &fT );
1014 fS = REAL(0.0);
1015 Vector3Copy( triOrigin, kTriSegOrigin );
1016 Vector3Copy( triEdge0, kTriSegDirection );
1017 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1018 kTriSegOrigin, kTriSegDirection,
1019 &fR0, &fS0 );
1020 fT0 = REAL(0.0);
1021 if ( fSqrDist0 < fSqrDist )
1022 {
1023 fSqrDist = fSqrDist0;
1024 fR = fR0;
1025 fS = fS0;
1026 fT = fT0;
1027 }
1028 Vector3Add( segOrigin, segDirection, kPt );
1029 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1030 &fS0, &fT0 );
1031 fR0 = REAL(1.0);
1032 if ( fSqrDist0 < fSqrDist )
1033 {
1034 fSqrDist = fSqrDist0;
1035 fR = fR0;
1036 fS = fS0;
1037 fT = fT0;
1038 }
1039 }
1040 else // region 3p
1041 {
1042 // min on face s=0 or r=1
1043 Vector3Copy( triOrigin, kTriSegOrigin );
1044 Vector3Copy( triEdge1, kTriSegDirection );
1045 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1046 kTriSegOrigin, kTriSegDirection,
1047 &fR, &fT );
1048 fS = REAL(0.0);
1049 Vector3Add( segOrigin, segDirection, kPt );
1050 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1051 &fS0, &fT0 );
1052 fR0 = REAL(1.0);
1053 if ( fSqrDist0 < fSqrDist )
1054 {
1055 fSqrDist = fSqrDist0;
1056 fR = fR0;
1057 fS = fS0;
1058 fT = fT0;
1059 }
1060 }
1061 }
1062 else if ( fT < REAL(0.0) ) // region 5p
1063 {
1064 // min on face t=0 or r=1
1065 Vector3Copy( triOrigin, kTriSegOrigin );
1066 Vector3Copy( triEdge0, kTriSegDirection );
1067 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1068 kTriSegOrigin, kTriSegDirection,
1069 &fR, &fS );
1070 fT = REAL(0.0);
1071 Vector3Add( segOrigin, segDirection, kPt );
1072 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1073 &fS0, &fT0 );
1074 fR0 = REAL(1.0);
1075 if ( fSqrDist0 < fSqrDist )
1076 {
1077 fSqrDist = fSqrDist0;
1078 fR = fR0;
1079 fS = fS0;
1080 fT = fT0;
1081 }
1082 }
1083 else // region 0p
1084 {
1085 // min face on r=1
1086 Vector3Add( segOrigin, segDirection, kPt );
1087 fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1088 &fS, &fT );
1089 fR = REAL(1.0);
1090 }
1091 }
1092 else
1093 {
1094 if ( fS < REAL(0.0) ) // region 2p
1095 {
1096 // min on face s=0 or s+t=1 or r=1
1097 Vector3Copy( triOrigin, kTriSegOrigin );
1098 Vector3Copy( triEdge1, kTriSegDirection );
1099 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1100 kTriSegOrigin, kTriSegDirection,
1101 &fR, &fT );
1102 fS = REAL(0.0);
1103 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1104 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1105 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1106 kTriSegOrigin, kTriSegDirection,
1107 &fR0, &fT0 );
1108 fS0 = REAL(1.0) - fT0;
1109 if ( fSqrDist0 < fSqrDist )
1110 {
1111 fSqrDist = fSqrDist0;
1112 fR = fR0;
1113 fS = fS0;
1114 fT = fT0;
1115 }
1116 Vector3Add( segOrigin, segDirection, kPt );
1117 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1118 &fS0, &fT0 );
1119 fR0 = REAL(1.0);
1120 if ( fSqrDist0 < fSqrDist )
1121 {
1122 fSqrDist = fSqrDist0;
1123 fR = fR0;
1124 fS = fS0;
1125 fT = fT0;
1126 }
1127 }
1128 else if ( fT < REAL(0.0) ) // region 6p
1129 {
1130 // min on face t=0 or s+t=1 or r=1
1131 Vector3Copy( triOrigin, kTriSegOrigin );
1132 Vector3Copy( triEdge0, kTriSegDirection );
1133 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1134 kTriSegOrigin, kTriSegDirection,
1135 &fR, &fS );
1136 fT = REAL(0.0);
1137 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1138 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1139 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1140 kTriSegOrigin, kTriSegDirection,
1141 &fR0, &fT0 );
1142 fS0 = REAL(1.0) - fT0;
1143 if ( fSqrDist0 < fSqrDist )
1144 {
1145 fSqrDist = fSqrDist0;
1146 fR = fR0;
1147 fS = fS0;
1148 fT = fT0;
1149 }
1150 Vector3Add( segOrigin, segDirection, kPt );
1151 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1152 &fS0, &fT0 );
1153 fR0 = REAL(1.0);
1154 if ( fSqrDist0 < fSqrDist )
1155 {
1156 fSqrDist = fSqrDist0;
1157 fR = fR0;
1158 fS = fS0;
1159 fT = fT0;
1160 }
1161 }
1162 else // region 1p
1163 {
1164 // min on face s+t=1 or r=1
1165 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1166 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1167 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1168 kTriSegOrigin, kTriSegDirection,
1169 &fR, &fT );
1170 fS = REAL(1.0) - fT;
1171 Vector3Add( segOrigin, segDirection, kPt );
1172 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1173 &fS0, &fT0 );
1174 fR0 = REAL(1.0);
1175 if ( fSqrDist0 < fSqrDist )
1176 {
1177 fSqrDist = fSqrDist0;
1178 fR = fR0;
1179 fS = fS0;
1180 fT = fT0;
1181 }
1182 }
1183 }
1184 }
1185 }
1186 else
1187 {
1188 // segment and triangle are parallel
1189 Vector3Copy( triOrigin, kTriSegOrigin );
1190 Vector3Copy( triEdge0, kTriSegDirection );
1191 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1192 kTriSegOrigin, kTriSegDirection, &fR, &fS );
1193 fT = REAL(0.0);
1194
1195 Vector3Copy( triEdge1, kTriSegDirection );
1196 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1197 kTriSegOrigin, kTriSegDirection,
1198 &fR0, &fT0 );
1199 fS0 = REAL(0.0);
1200 if ( fSqrDist0 < fSqrDist )
1201 {
1202 fSqrDist = fSqrDist0;
1203 fR = fR0;
1204 fS = fS0;
1205 fT = fT0;
1206 }
1207
1208 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1209 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1210 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1211 kTriSegOrigin, kTriSegDirection, &fR0, &fT0 );
1212 fS0 = REAL(1.0) - fT0;
1213 if ( fSqrDist0 < fSqrDist )
1214 {
1215 fSqrDist = fSqrDist0;
1216 fR = fR0;
1217 fS = fS0;
1218 fT = fT0;
1219 }
1220
1221 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
1222 &fS0, &fT0 );
1223 fR0 = REAL(0.0);
1224 if ( fSqrDist0 < fSqrDist )
1225 {
1226 fSqrDist = fSqrDist0;
1227 fR = fR0;
1228 fS = fS0;
1229 fT = fT0;
1230 }
1231
1232 Vector3Add( segOrigin, segDirection, kPt );
1233 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1234 &fS0, &fT0 );
1235 fR0 = REAL(1.0);
1236 if ( fSqrDist0 < fSqrDist )
1237 {
1238 fSqrDist = fSqrDist0;
1239 fR = fR0;
1240 fS = fS0;
1241 fT = fT0;
1242 }
1243 }
1244
1245 if ( pfSegP )
1246 *pfSegP = fR;
1247
1248 if ( pfTriP0 )
1249 *pfTriP0 = fS;
1250
1251 if ( pfTriP1 )
1252 *pfTriP1 = fT;
1253
1254 return fSqrDist;
1255}
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp
new file mode 100644
index 0000000..229966b
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_gimpact.cpp
@@ -0,0 +1,456 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/collision.h>
24#include <ode/matrix.h>
25#include <ode/rotation.h>
26#include <ode/odemath.h>
27
28#if dTRIMESH_ENABLED
29
30#include "collision_util.h"
31#define TRIMESH_INTERNAL
32#include "collision_trimesh_internal.h"
33
34#if dTRIMESH_GIMPACT
35
36void dxTriMeshData::Preprocess(){ // stub
37}
38
39dTriMeshDataID dGeomTriMeshDataCreate(){
40 return new dxTriMeshData();
41}
42
43void dGeomTriMeshDataDestroy(dTriMeshDataID g){
44 delete g;
45}
46
47void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) { //stub
48}
49
50dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) {
51 return NULL; // stub
52}
53
54void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) { //stub
55}
56
57void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) {
58 dUASSERT(g, "argument not trimesh data");
59 return NULL; // stub
60}
61
62void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
63 const void* Vertices, int VertexStride, int VertexCount,
64 const void* Indices, int IndexCount, int TriStride,
65 const void* Normals)
66{
67 dUASSERT(g, "argument not trimesh data");
68 dIASSERT(Vertices);
69 dIASSERT(Indices);
70
71 g->Build(Vertices, VertexStride, VertexCount,
72 Indices, IndexCount, TriStride,
73 Normals,
74 true);
75}
76
77
78void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
79 const void* Vertices, int VertexStride, int VertexCount,
80 const void* Indices, int IndexCount, int TriStride)
81{
82 dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
83 Indices, IndexCount, TriStride, (void*)NULL);
84}
85
86
87void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
88 const void* Vertices, int VertexStride, int VertexCount,
89 const void* Indices, int IndexCount, int TriStride,
90 const void* Normals)
91{
92 dUASSERT(g, "argument not trimesh data");
93
94 g->Build(Vertices, VertexStride, VertexCount,
95 Indices, IndexCount, TriStride,
96 Normals,
97 false);
98}
99
100
101void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
102 const void* Vertices, int VertexStride, int VertexCount,
103 const void* Indices, int IndexCount, int TriStride) {
104 dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
105 Indices, IndexCount, TriStride, NULL);
106}
107
108
109void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
110 const dReal* Vertices, int VertexCount,
111 const int* Indices, int IndexCount,
112 const int* Normals){
113#ifdef dSINGLE
114 dGeomTriMeshDataBuildSingle1(g,
115 Vertices, 4 * sizeof(dReal), VertexCount,
116 Indices, IndexCount, 3 * sizeof(unsigned int),
117 Normals);
118#else
119 dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount,
120 Indices, IndexCount, 3 * sizeof(unsigned int),
121 Normals);
122#endif
123}
124
125
126void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
127 const dReal* Vertices, int VertexCount,
128 const int* Indices, int IndexCount) {
129 dGeomTriMeshDataBuildSimple1(g,
130 Vertices, VertexCount, Indices, IndexCount,
131 (const int*)NULL);
132}
133
134void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
135{
136 dUASSERT(g, "argument not trimesh data");
137 g->Preprocess();
138}
139
140void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
141{
142 dUASSERT(g, "argument not trimesh data");
143 *buf = NULL;
144 *bufLen = 0;
145}
146
147void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
148{
149 dUASSERT(g, "argument not trimesh data");
150// g->UseFlags = buf;
151}
152
153
154// Trimesh
155
156dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1){
157 type = dTriMeshClass;
158
159 dGeomTriMeshSetData(this,Data);
160
161 /* TC has speed/space 'issues' that don't make it a clear
162 win by default on spheres/boxes. */
163 this->doSphereTC = true;
164 this->doBoxTC = true;
165 this->doCapsuleTC = true;
166
167}
168
169dxTriMesh::~dxTriMesh(){
170
171 //Terminate Trimesh
172 gim_trimesh_destroy(&m_collision_trimesh);
173}
174
175
176void dxTriMesh::ClearTCCache(){
177
178}
179
180
181int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
182 return 1;
183}
184
185
186void dxTriMesh::computeAABB()
187{
188 //update trimesh transform
189 mat4f transform;
190 IDENTIFY_MATRIX_4X4(transform);
191 MakeMatrix(this, transform);
192 gim_trimesh_set_tranform(&m_collision_trimesh,transform);
193
194 //Update trimesh boxes
195 gim_trimesh_update(&m_collision_trimesh);
196
197 memcpy(aabb,&m_collision_trimesh.m_aabbset.m_global_bound,6*sizeof(GREAL));
198}
199
200
201void dxTriMeshData::UpdateData()
202{
203// BVTree.Refit();
204}
205
206
207dGeomID dCreateTriMesh(dSpaceID space,
208 dTriMeshDataID Data,
209 dTriCallback* Callback,
210 dTriArrayCallback* ArrayCallback,
211 dTriRayCallback* RayCallback)
212{
213 dxTriMesh* Geom = new dxTriMesh(space, Data);
214 Geom->Callback = Callback;
215 Geom->ArrayCallback = ArrayCallback;
216 Geom->RayCallback = RayCallback;
217
218 return Geom;
219}
220
221void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
222{
223 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
224 ((dxTriMesh*)g)->Callback = Callback;
225}
226
227dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
228{
229 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
230 return ((dxTriMesh*)g)->Callback;
231}
232
233void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
234{
235 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
236 ((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
237}
238
239dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
240{
241 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
242 return ((dxTriMesh*)g)->ArrayCallback;
243}
244
245void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
246{
247 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
248 ((dxTriMesh*)g)->RayCallback = Callback;
249}
250
251dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
252{
253 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
254 return ((dxTriMesh*)g)->RayCallback;
255}
256
257void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
258{
259 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
260 dxTriMesh* mesh = (dxTriMesh*) g;
261 mesh->Data = Data;
262 // I changed my data -- I know nothing about my own AABB anymore.
263 ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD);
264
265 // GIMPACT only supports stride 12, so we need to catch the error early.
266 dUASSERT
267 (
268 Data->m_VertexStride == 3*sizeof(dReal) && Data->m_TriStride == 3*sizeof(int),
269 "Gimpact trimesh only supports a stride of 3 dReal/int\n"
270 "This means that you cannot use dGeomTriMeshDataBuildSimple() with Gimpact.\n"
271 "Change the stride, or use Opcode trimeshes instead.\n"
272 );
273
274 //Create trimesh
275 if ( Data->m_Vertices )
276 gim_trimesh_create_from_data
277 (
278 &mesh->m_collision_trimesh, // gimpact mesh
279 ( vec3f *)(&Data->m_Vertices[0]), // vertices
280 Data->m_VertexCount, // nr of verts
281 0, // copy verts?
282 ( GUINT *)(&Data->m_Indices[0]), // indices
283 Data->m_TriangleCount*3, // nr of indices
284 0, // copy indices?
285 1 // transformed reply
286 );
287}
288
289dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
290{
291 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
292 return ((dxTriMesh*)g)->Data;
293}
294
295
296
297void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
298{
299 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
300
301 switch (geomClass)
302 {
303 case dSphereClass:
304 ((dxTriMesh*)g)->doSphereTC = (1 == enable);
305 break;
306 case dBoxClass:
307 ((dxTriMesh*)g)->doBoxTC = (1 == enable);
308 break;
309 case dCapsuleClass:
310// case dCCylinderClass:
311 ((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
312 break;
313 }
314}
315
316int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
317{
318 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
319
320 switch (geomClass)
321 {
322 case dSphereClass:
323 if (((dxTriMesh*)g)->doSphereTC)
324 return 1;
325 break;
326 case dBoxClass:
327 if (((dxTriMesh*)g)->doBoxTC)
328 return 1;
329 break;
330 case dCapsuleClass:
331 if (((dxTriMesh*)g)->doCapsuleTC)
332 return 1;
333 break;
334 }
335 return 0;
336}
337
338void dGeomTriMeshClearTCCache(dGeomID g){
339 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
340
341 dxTriMesh* Geom = (dxTriMesh*)g;
342 Geom->ClearTCCache();
343}
344
345/*
346 * returns the TriMeshDataID
347 */
348dTriMeshDataID
349dGeomTriMeshGetTriMeshDataID(dGeomID g)
350{
351 dxTriMesh* Geom = (dxTriMesh*) g;
352 return Geom->Data;
353}
354
355// Getting data
356void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2)
357{
358 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
359
360 dxTriMesh* Geom = (dxTriMesh*)g;
361 gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
362 gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, (*v0),(*v1),(*v2));
363 gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
364
365}
366
367void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
368 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
369
370 dxTriMesh* Geom = (dxTriMesh*)g;
371 dVector3 dv[3];
372 gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
373 gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, dv[0],dv[1],dv[2]);
374 GetPointFromBarycentric(dv, u, v, Out);
375 gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
376}
377
378int dGeomTriMeshGetTriangleCount (dGeomID g)
379{
380 dxTriMesh* Geom = (dxTriMesh*)g;
381 return gim_trimesh_get_triangle_count(&Geom->m_collision_trimesh);
382}
383
384void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
385 dUASSERT(g, "argument not trimesh data");
386 g->UpdateData();
387}
388
389
390//
391// GIMPACT TRIMESH-TRIMESH COLLIDER
392//
393
394int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
395{
396 dIASSERT (Stride >= (int)sizeof(dContactGeom));
397 dIASSERT (g1->type == dTriMeshClass);
398 dIASSERT (g2->type == dTriMeshClass);
399 dIASSERT ((Flags & NUMC_MASK) >= 1);
400
401 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
402 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
403 //Create contact list
404 GDYNAMIC_ARRAY trimeshcontacts;
405 GIM_CREATE_CONTACT_LIST(trimeshcontacts);
406
407 //Collide trimeshes
408 gim_trimesh_trimesh_collision(&TriMesh1->m_collision_trimesh,&TriMesh2->m_collision_trimesh,&trimeshcontacts);
409
410 if(trimeshcontacts.m_size == 0)
411 {
412 GIM_DYNARRAY_DESTROY(trimeshcontacts);
413 return 0;
414 }
415
416 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
417
418
419 unsigned contactcount = trimeshcontacts.m_size;
420 unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
421 if (contactcount > maxcontacts)
422 {
423 contactcount = maxcontacts;
424 }
425
426 dContactGeom* pcontact;
427 unsigned i;
428
429 for (i=0;i<contactcount;i++)
430 {
431 pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
432
433 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
434 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
435 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
436 pcontact->pos[3] = 1.0f;
437
438 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
439 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
440 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
441 pcontact->normal[3] = 0;
442
443 pcontact->depth = ptrimeshcontacts->m_depth;
444 pcontact->g1 = g1;
445 pcontact->g2 = g2;
446
447 ptrimeshcontacts++;
448 }
449
450 GIM_DYNARRAY_DESTROY(trimeshcontacts);
451
452 return (int)contactcount;
453}
454
455#endif // dTRIMESH_GIMPACT
456#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_internal.h b/libraries/ode-0.9/ode/src/collision_trimesh_internal.h
new file mode 100644
index 0000000..bf474a2
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_internal.h
@@ -0,0 +1,495 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// TriMesh code by Erwin de Vries.
24// Modified for FreeSOLID Compatibility by Rodrigo Hernandez
25
26#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
27#define _ODE_COLLISION_TRIMESH_INTERNAL_H_
28
29int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
30int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
31
32int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
33int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
34int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
35int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
36int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
37
38PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags,
39 dContactGeom *contact, int skip )
40{
41 // Swapped case, for code that needs it (heightfield initially)
42 // The other ray-geom colliders take geoms in a swapped order to the
43 // dCollideRTL function which is annoying when using function pointers.
44 return dCollideRTL( trimesh, ray, flags, contact, skip );
45}
46
47//****************************************************************************
48// dxTriMesh class
49
50#ifdef TRIMESH_INTERNAL
51
52#include "collision_kernel.h"
53#include <ode/collision_trimesh.h>
54
55#if dTRIMESH_OPCODE
56#define BAN_OPCODE_AUTOLINK
57#include "Opcode.h"
58using namespace Opcode;
59#endif // dTRIMESH_OPCODE
60
61#if dTRIMESH_GIMPACT
62#include <GIMPACT/gimpact.h>
63#endif
64
65struct dxTriMeshData : public dBase
66{
67 /* Array of flags for which edges and verts should be used on each triangle */
68 enum UseFlags
69 {
70 kEdge0 = 0x1,
71 kEdge1 = 0x2,
72 kEdge2 = 0x4,
73 kVert0 = 0x8,
74 kVert1 = 0x10,
75 kVert2 = 0x20,
76
77 kUseAll = 0xFF,
78 };
79
80 /* Setup the UseFlags array */
81 void Preprocess();
82 /* For when app changes the vertices */
83 void UpdateData();
84
85#if dTRIMESH_OPCODE
86 Model BVTree;
87 MeshInterface Mesh;
88
89 dxTriMeshData();
90 ~dxTriMeshData();
91
92 void Build(const void* Vertices, int VertexStide, int VertexCount,
93 const void* Indices, int IndexCount, int TriStride,
94 const void* Normals,
95 bool Single);
96
97 /* aabb in model space */
98 dVector3 AABBCenter;
99 dVector3 AABBExtents;
100
101 // data for use in collision resolution
102 const void* Normals;
103 uint8* UseFlags;
104#endif // dTRIMESH_OPCODE
105
106#if dTRIMESH_GIMPACT
107 const char* m_Vertices;
108 int m_VertexStride;
109 int m_VertexCount;
110 const char* m_Indices;
111 int m_TriangleCount;
112 int m_TriStride;
113 bool m_single;
114
115 dxTriMeshData()
116 {
117 m_Vertices=NULL;
118 m_VertexStride = 12;
119 m_VertexCount = 0;
120 m_Indices = 0;
121 m_TriangleCount = 0;
122 m_TriStride = 12;
123 m_single = true;
124 }
125
126 void Build(const void* Vertices, int VertexStride, int VertexCount,
127 const void* Indices, int IndexCount, int TriStride,
128 const void* Normals,
129 bool Single)
130 {
131 dIASSERT(Vertices);
132 dIASSERT(Indices);
133 dIASSERT(VertexStride);
134 dIASSERT(TriStride);
135 dIASSERT(IndexCount);
136 m_Vertices=(const char *)Vertices;
137 m_VertexStride = VertexStride;
138 m_VertexCount = VertexCount;
139 m_Indices = (const char *)Indices;
140 m_TriangleCount = IndexCount/3;
141 m_TriStride = TriStride;
142 m_single = Single;
143 }
144
145 inline void GetVertex(unsigned int i, dVector3 Out)
146 {
147 if(m_single)
148 {
149 const float * fverts = (const float * )(m_Vertices + m_VertexStride*i);
150 Out[0] = fverts[0];
151 Out[1] = fverts[1];
152 Out[2] = fverts[2];
153 Out[3] = 1.0f;
154 }
155 else
156 {
157 const double * dverts = (const double * )(m_Vertices + m_VertexStride*i);
158 Out[0] = (float)dverts[0];
159 Out[1] = (float)dverts[1];
160 Out[2] = (float)dverts[2];
161 Out[3] = 1.0f;
162
163 }
164 }
165
166 inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3])
167 {
168 const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle);
169 triindices[0] = ind[0];
170 triindices[1] = ind[1];
171 triindices[2] = ind[2];
172 }
173#endif // dTRIMESH_GIMPACT
174};
175
176
177struct dxTriMesh : public dxGeom{
178 // Callbacks
179 dTriCallback* Callback;
180 dTriArrayCallback* ArrayCallback;
181 dTriRayCallback* RayCallback;
182
183 // Data types
184 dxTriMeshData* Data;
185
186 bool doSphereTC;
187 bool doBoxTC;
188 bool doCapsuleTC;
189
190 // Functions
191 dxTriMesh(dSpaceID Space, dTriMeshDataID Data);
192 ~dxTriMesh();
193
194 void ClearTCCache();
195
196 int AABBTest(dxGeom* g, dReal aabb[6]);
197 void computeAABB();
198
199#if dTRIMESH_OPCODE
200 // Instance data for last transform.
201 dMatrix4 last_trans;
202
203 // Colliders
204 static PlanesCollider _PlanesCollider;
205 static SphereCollider _SphereCollider;
206 static OBBCollider _OBBCollider;
207 static RayCollider _RayCollider;
208 static AABBTreeCollider _AABBTreeCollider;
209 static LSSCollider _LSSCollider;
210
211 // Some constants
212 static CollisionFaces Faces;
213 // Temporal coherence
214 struct SphereTC : public SphereCache{
215 dxGeom* Geom;
216 };
217 dArray<SphereTC> SphereTCCache;
218 static SphereCache defaultSphereCache;
219
220 struct BoxTC : public OBBCache{
221 dxGeom* Geom;
222 };
223 dArray<BoxTC> BoxTCCache;
224 static OBBCache defaultBoxCache;
225
226 struct CapsuleTC : public LSSCache{
227 dxGeom* Geom;
228 };
229 dArray<CapsuleTC> CapsuleTCCache;
230 static LSSCache defaultCapsuleCache;
231#endif // dTRIMESH_OPCODE
232
233#if dTRIMESH_GIMPACT
234 GIM_TRIMESH m_collision_trimesh;
235#endif // dTRIMESH_GIMPACT
236};
237
238#if 0
239#include "collision_kernel.h"
240// Fetches a contact
241inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
242 dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
243 return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
244}
245#endif
246
247#if dTRIMESH_OPCODE
248inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
249 VertexPointers VP;
250 TriMesh->Data->Mesh.GetTriangle(VP, Index);
251 for (int i = 0; i < 3; i++){
252 Out[i][0] = VP.Vertex[i]->x;
253 Out[i][1] = VP.Vertex[i]->y;
254 Out[i][2] = VP.Vertex[i]->z;
255 Out[i][3] = 0;
256 }
257}
258
259inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
260 VertexPointers VP;
261 TriMesh->Data->Mesh.GetTriangle(VP, Index);
262 for (int i = 0; i < 3; i++){
263 dVector3 v;
264 v[0] = VP.Vertex[i]->x;
265 v[1] = VP.Vertex[i]->y;
266 v[2] = VP.Vertex[i]->z;
267 v[3] = 0;
268
269 dMULTIPLY0_331(Out[i], Rotation, v);
270 Out[i][0] += Position[0];
271 Out[i][1] += Position[1];
272 Out[i][2] += Position[2];
273 Out[i][3] = 0;
274 }
275}
276
277inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
278 Out.m[0][0] = (float) Rotation[0];
279 Out.m[1][0] = (float) Rotation[1];
280 Out.m[2][0] = (float) Rotation[2];
281
282 Out.m[0][1] = (float) Rotation[4];
283 Out.m[1][1] = (float) Rotation[5];
284 Out.m[2][1] = (float) Rotation[6];
285
286 Out.m[0][2] = (float) Rotation[8];
287 Out.m[1][2] = (float) Rotation[9];
288 Out.m[2][2] = (float) Rotation[10];
289
290 Out.m[3][0] = (float) Position[0];
291 Out.m[3][1] = (float) Position[1];
292 Out.m[3][2] = (float) Position[2];
293
294 Out.m[0][3] = 0.0f;
295 Out.m[1][3] = 0.0f;
296 Out.m[2][3] = 0.0f;
297 Out.m[3][3] = 1.0f;
298
299 return Out;
300}
301
302inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
303 const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
304 const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
305 return MakeMatrix(Position, Rotation, Out);
306}
307#endif // dTRIMESH_OPCODE
308
309#if dTRIMESH_GIMPACT
310inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
311 // why is this not implemented?
312 dAASSERT(false);
313}
314
315inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
316{
317 m[0][0] = (float) Rotation[0];
318 m[0][1] = (float) Rotation[1];
319 m[0][2] = (float) Rotation[2];
320
321 m[1][0] = (float) Rotation[4];
322 m[1][1] = (float) Rotation[5];
323 m[1][2] = (float) Rotation[6];
324
325 m[2][0] = (float) Rotation[8];
326 m[2][1] = (float) Rotation[9];
327 m[2][2] = (float) Rotation[10];
328
329 m[0][3] = (float) Position[0];
330 m[1][3] = (float) Position[1];
331 m[2][3] = (float) Position[2];
332
333}
334
335inline void MakeMatrix(dxGeom* g, mat4f Out){
336 const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
337 const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
338 MakeMatrix(Position, Rotation, Out);
339}
340#endif // dTRIMESH_GIMPACT
341
342// Outputs a matrix to 3 vectors
343inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
344 Right[0] = Matrix[0 * 4 + 0];
345 Right[1] = Matrix[1 * 4 + 0];
346 Right[2] = Matrix[2 * 4 + 0];
347 Right[3] = REAL(0.0);
348 Up[0] = Matrix[0 * 4 + 1];
349 Up[1] = Matrix[1 * 4 + 1];
350 Up[2] = Matrix[2 * 4 + 1];
351 Up[3] = REAL(0.0);
352 Direction[0] = Matrix[0 * 4 + 2];
353 Direction[1] = Matrix[1 * 4 + 2];
354 Direction[2] = Matrix[2 * 4 + 2];
355 Direction[3] = REAL(0.0);
356}
357
358// Outputs a matrix to 3 vectors
359inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
360 Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
361}
362
363// Finds barycentric
364inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
365 dReal w = REAL(1.0) - u - v;
366
367 Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
368 Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
369 Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
370 Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
371}
372
373// Performs a callback
374inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
375 if (TriMesh->Callback != NULL){
376 return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0);
377 }
378 else return true;
379}
380
381// Some utilities
382template<class T> const T& dcMAX(const T& x, const T& y){
383 return x > y ? x : y;
384}
385
386template<class T> const T& dcMIN(const T& x, const T& y){
387 return x < y ? x : y;
388}
389
390dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin,
391 const dVector3 triEdge1, const dVector3 triEdge2,
392 dReal* pfSParam = 0, dReal* pfTParam = 0 );
393
394dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction,
395 const dVector3 seg2Origin, const dVector3 seg2Direction,
396 dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 );
397
398dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd,
399 const dVector3 triOrigin,
400 const dVector3 triEdge1, const dVector3 triEdge2,
401 dReal* t = 0, dReal* u = 0, dReal* v = 0 );
402
403inline
404void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
405{
406 result[0] = left[0] - right[0];
407 result[1] = left[1] - right[1];
408 result[2] = left[2] - right[2];
409 result[3] = REAL(0.0);
410}
411
412inline
413void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
414{
415 result[0] = left[0] + right[0];
416 result[1] = left[1] + right[1];
417 result[2] = left[2] + right[2];
418 result[3] = REAL(0.0);
419}
420
421inline
422void Vector3Negate( const dVector3 in, dVector3 out )
423{
424 out[0] = -in[0];
425 out[1] = -in[1];
426 out[2] = -in[2];
427 out[3] = REAL(0.0);
428}
429
430inline
431void Vector3Copy( const dVector3 in, dVector3 out )
432{
433 out[0] = in[0];
434 out[1] = in[1];
435 out[2] = in[2];
436 out[3] = REAL(0.0);
437}
438
439inline
440void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
441{
442 out[0] = in[0] * scalar;
443 out[1] = in[1] * scalar;
444 out[2] = in[2] * scalar;
445 out[3] = REAL(0.0);
446}
447
448inline
449void TransformVector3( const dVector3 in,
450 const dMatrix3 orientation, const dVector3 position,
451 dVector3 out )
452{
453 dMULTIPLY0_331( out, orientation, in );
454 out[0] += position[0];
455 out[1] += position[1];
456 out[2] += position[2];
457}
458
459//------------------------------------------------------------------------------
460/**
461 @brief Check for intersection between triangle and capsule.
462
463 @param dist [out] Shortest distance squared between the triangle and
464 the capsule segment (central axis).
465 @param t [out] t value of point on segment that's the shortest distance
466 away from the triangle, the coordinates of this point
467 can be found by (cap.seg.end - cap.seg.start) * t,
468 or cap.seg.ipol(t).
469 @param u [out] Barycentric coord on triangle.
470 @param v [out] Barycentric coord on triangle.
471 @return True if intersection exists.
472
473 The third Barycentric coord is implicit, ie. w = 1.0 - u - v
474 The Barycentric coords give the location of the point on the triangle
475 closest to the capsule (where the distance between the two shapes
476 is the shortest).
477*/
478inline
479bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd,
480 const dReal radius, const dVector3 triOrigin,
481 const dVector3 triEdge0, const dVector3 triEdge1,
482 dReal* dist, dReal* t, dReal* u, dReal* v )
483{
484 dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1,
485 t, u, v );
486
487 if ( dist )
488 *dist = sqrDist;
489
490 return ( sqrDist <= (radius * radius) );
491}
492
493#endif //TRIMESH_INTERNAL
494
495#endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp
new file mode 100644
index 0000000..07f3f8a
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp
@@ -0,0 +1,833 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// TriMesh code by Erwin de Vries.
24
25#include <ode/collision.h>
26#include <ode/matrix.h>
27#include <ode/rotation.h>
28#include <ode/odemath.h>
29#include "collision_util.h"
30#define TRIMESH_INTERNAL
31#include "collision_trimesh_internal.h"
32
33#if dTRIMESH_ENABLED
34#if dTRIMESH_OPCODE
35
36// Trimesh data
37dxTriMeshData::dxTriMeshData() : UseFlags( NULL )
38{
39#if !dTRIMESH_ENABLED
40 dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work");
41#endif
42}
43
44dxTriMeshData::~dxTriMeshData()
45{
46 if ( UseFlags )
47 delete [] UseFlags;
48}
49
50void
51dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount,
52 const void* Indices, int IndexCount, int TriStride,
53 const void* in_Normals,
54 bool Single)
55{
56#if dTRIMESH_ENABLED
57
58 Mesh.SetNbTriangles(IndexCount / 3);
59 Mesh.SetNbVertices(VertexCount);
60 Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices);
61 Mesh.SetStrides(TriStride, VertexStide);
62 Mesh.Single = Single;
63
64 // Build tree
65 BuildSettings Settings;
66 // recommended in Opcode User Manual
67 //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER;
68 // used in ODE, why?
69 //Settings.mRules = SPLIT_BEST_AXIS;
70
71 // best compromise?
72 Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
73
74
75 OPCODECREATE TreeBuilder;
76 TreeBuilder.mIMesh = &Mesh;
77
78 TreeBuilder.mSettings = Settings;
79 TreeBuilder.mNoLeaf = true;
80 TreeBuilder.mQuantized = false;
81
82 TreeBuilder.mKeepOriginal = false;
83 TreeBuilder.mCanRemap = false;
84
85
86
87 BVTree.Build(TreeBuilder);
88
89 // compute model space AABB
90 dVector3 AABBMax, AABBMin;
91 AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity;
92 AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity;
93 if( Single ) {
94 const char* verts = (const char*)Vertices;
95 for( int i = 0; i < VertexCount; ++i ) {
96 const float* v = (const float*)verts;
97 if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
98 if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
99 if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
100 if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
101 if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
102 if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
103 verts += VertexStide;
104 }
105 } else {
106 const char* verts = (const char*)Vertices;
107 for( int i = 0; i < VertexCount; ++i ) {
108 const double* v = (const double*)verts;
109 if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0];
110 if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1];
111 if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2];
112 if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0];
113 if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1];
114 if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2];
115 verts += VertexStide;
116 }
117 }
118 AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5);
119 AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5);
120 AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5);
121 AABBExtents[0] = AABBMax[0] - AABBCenter[0];
122 AABBExtents[1] = AABBMax[1] - AABBCenter[1];
123 AABBExtents[2] = AABBMax[2] - AABBCenter[2];
124
125 // user data (not used by OPCODE)
126 Normals = (dReal *) in_Normals;
127
128 UseFlags = 0;
129
130#endif // dTRIMESH_ENABLED
131}
132
133struct EdgeRecord
134{
135 int VertIdx1; // Index into vertex array for this edges vertices
136 int VertIdx2;
137 int TriIdx; // Index into triangle array for triangle this edge belongs to
138
139 uint8 EdgeFlags;
140 uint8 Vert1Flags;
141 uint8 Vert2Flags;
142 bool Concave;
143};
144
145// Edge comparison function for qsort
146static int EdgeCompare(const void* edge1, const void* edge2)
147{
148 EdgeRecord* e1 = (EdgeRecord*)edge1;
149 EdgeRecord* e2 = (EdgeRecord*)edge2;
150
151 if (e1->VertIdx1 == e2->VertIdx1)
152 return e1->VertIdx2 - e2->VertIdx2;
153 else
154 return e1->VertIdx1 - e2->VertIdx1;
155}
156
157void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const unsigned int* vertIdxs)
158{
159 if (edgeIdx == 0)
160 {
161 edge->EdgeFlags = dxTriMeshData::kEdge0;
162 edge->Vert1Flags = dxTriMeshData::kVert0;
163 edge->Vert2Flags = dxTriMeshData::kVert1;
164 edge->VertIdx1 = vertIdxs[0];
165 edge->VertIdx2 = vertIdxs[1];
166 }
167 else if (edgeIdx == 1)
168 {
169 edge->EdgeFlags = dxTriMeshData::kEdge1;
170 edge->Vert1Flags = dxTriMeshData::kVert1;
171 edge->Vert2Flags = dxTriMeshData::kVert2;
172 edge->VertIdx1 = vertIdxs[1];
173 edge->VertIdx2 = vertIdxs[2];
174 }
175 else if (edgeIdx == 2)
176 {
177 edge->EdgeFlags = dxTriMeshData::kEdge2;
178 edge->Vert1Flags = dxTriMeshData::kVert2;
179 edge->Vert2Flags = dxTriMeshData::kVert0;
180 edge->VertIdx1 = vertIdxs[2];
181 edge->VertIdx2 = vertIdxs[0];
182 }
183
184 // Make sure vert index 1 is less than index 2 (for easier sorting)
185 if (edge->VertIdx1 > edge->VertIdx2)
186 {
187 unsigned int tempIdx = edge->VertIdx1;
188 edge->VertIdx1 = edge->VertIdx2;
189 edge->VertIdx2 = tempIdx;
190
191 uint8 tempFlags = edge->Vert1Flags;
192 edge->Vert1Flags = edge->Vert2Flags;
193 edge->Vert2Flags = tempFlags;
194 }
195
196 edge->TriIdx = triIdx;
197 edge->Concave = false;
198}
199
200#if dTRIMESH_ENABLED
201
202// Get the vertex opposite this edge in the triangle
203inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[])
204{
205 if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) ||
206 (edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0))
207 {
208 return *vertices[2];
209 }
210 else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) ||
211 (edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1))
212 {
213 return *vertices[0];
214 }
215 else
216 return *vertices[1];
217}
218
219#endif // dTRIMESH_ENABLED
220
221void dxTriMeshData::Preprocess()
222{
223
224#if dTRIMESH_ENABLED
225
226 // If this mesh has already been preprocessed, exit
227 if (UseFlags)
228 return;
229
230 udword numTris = Mesh.GetNbTriangles();
231 udword numEdges = numTris * 3;
232
233 UseFlags = new uint8[numTris];
234 memset(UseFlags, 0, sizeof(uint8) * numTris);
235
236 EdgeRecord* records = new EdgeRecord[numEdges];
237
238 // Make a list of every edge in the mesh
239 const IndexedTriangle* tris = Mesh.GetTris();
240 for (unsigned int i = 0; i < numTris; i++)
241 {
242 SetupEdge(&records[i*3], 0, i, tris->mVRef);
243 SetupEdge(&records[i*3+1], 1, i, tris->mVRef);
244 SetupEdge(&records[i*3+2], 2, i, tris->mVRef);
245
246 tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride());
247 }
248
249 // Sort the edges, so the ones sharing the same verts are beside each other
250 qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare);
251
252 // Go through the sorted list of edges and flag all the edges and vertices that we need to use
253 for (unsigned int i = 0; i < numEdges; i++)
254 {
255 EdgeRecord* rec1 = &records[i];
256 EdgeRecord* rec2 = 0;
257 if (i < numEdges - 1)
258 rec2 = &records[i+1];
259
260 if (rec2 &&
261 rec1->VertIdx1 == rec2->VertIdx1 &&
262 rec1->VertIdx2 == rec2->VertIdx2)
263 {
264 VertexPointers vp;
265 Mesh.GetTriangle(vp, rec1->TriIdx);
266
267 // Get the normal of the first triangle
268 Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]);
269 triNorm.Normalize();
270
271 // Get the vert opposite this edge in the first triangle
272 Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex);
273
274 // Get the vert opposite this edge in the second triangle
275 Mesh.GetTriangle(vp, rec2->TriIdx);
276 Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex);
277
278 float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize());
279
280 // We let the dot threshold for concavity get slightly negative to allow for rounding errors
281 static const float kConcaveThresh = -0.000001f;
282
283 // This is a concave edge, leave it for the next pass
284 if (dot >= kConcaveThresh)
285 rec1->Concave = true;
286 // If this is a convex edge, mark its vertices and edge as used
287 else
288 UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
289
290 // Skip the second edge
291 i++;
292 }
293 // This is a boundary edge
294 else
295 {
296 UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
297 }
298 }
299
300 // Go through the list once more, and take any edge we marked as concave and
301 // clear it's vertices flags in any triangles they're used in
302 for (unsigned int i = 0; i < numEdges; i++)
303 {
304 EdgeRecord& er = records[i];
305
306 if (er.Concave)
307 {
308 for (unsigned int j = 0; j < numEdges; j++)
309 {
310 EdgeRecord& curER = records[j];
311
312 if (curER.VertIdx1 == er.VertIdx1 ||
313 curER.VertIdx1 == er.VertIdx2)
314 UseFlags[curER.TriIdx] &= ~curER.Vert1Flags;
315
316 if (curER.VertIdx2 == er.VertIdx1 ||
317 curER.VertIdx2 == er.VertIdx2)
318 UseFlags[curER.TriIdx] &= ~curER.Vert2Flags;
319 }
320 }
321 }
322
323 delete [] records;
324
325#endif // dTRIMESH_ENABLED
326
327}
328
329dTriMeshDataID dGeomTriMeshDataCreate(){
330 return new dxTriMeshData();
331}
332
333void dGeomTriMeshDataDestroy(dTriMeshDataID g){
334 delete g;
335}
336
337
338
339
340void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans )
341{
342 dAASSERT(g)
343 dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
344
345 for (int i=0; i<16; i++)
346 (((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ];
347
348 return;
349}
350
351
352dReal* dGeomTriMeshGetLastTransform( dxGeom* g )
353{
354 dAASSERT(g)
355 dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
356
357 return (dReal*)(((dxTriMesh*)g)->last_trans);
358}
359
360
361
362
363void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
364{
365 dUASSERT(g, "argument not trimesh data");
366
367 switch (data_id)
368 {
369 case TRIMESH_FACE_NORMALS:
370 g->Normals = (dReal *) in_data;
371 break;
372
373 default:
374 dUASSERT(data_id, "invalid data type");
375 break;
376 }
377
378 return;
379}
380
381
382
383void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id)
384{
385 dUASSERT(g, "argument not trimesh data");
386
387 switch (data_id)
388 {
389 case TRIMESH_FACE_NORMALS:
390 return (void *) g->Normals;
391 break;
392
393 default:
394 dUASSERT(data_id, "invalid data type");
395 break;
396 }
397
398 return NULL;
399}
400
401
402void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
403 const void* Vertices, int VertexStride, int VertexCount,
404 const void* Indices, int IndexCount, int TriStride,
405 const void* Normals)
406{
407 dUASSERT(g, "argument not trimesh data");
408
409 g->Build(Vertices, VertexStride, VertexCount,
410 Indices, IndexCount, TriStride,
411 Normals,
412 true);
413}
414
415
416void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
417 const void* Vertices, int VertexStride, int VertexCount,
418 const void* Indices, int IndexCount, int TriStride)
419{
420 dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
421 Indices, IndexCount, TriStride, (void*)NULL);
422}
423
424
425void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
426 const void* Vertices, int VertexStride, int VertexCount,
427 const void* Indices, int IndexCount, int TriStride,
428 const void* Normals)
429{
430 dUASSERT(g, "argument not trimesh data");
431
432 g->Build(Vertices, VertexStride, VertexCount,
433 Indices, IndexCount, TriStride,
434 Normals,
435 false);
436}
437
438
439void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
440 const void* Vertices, int VertexStride, int VertexCount,
441 const void* Indices, int IndexCount, int TriStride) {
442 dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
443 Indices, IndexCount, TriStride, NULL);
444}
445
446
447void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
448 const dReal* Vertices, int VertexCount,
449 const int* Indices, int IndexCount,
450 const int* Normals){
451#ifdef dSINGLE
452 dGeomTriMeshDataBuildSingle1(g,
453 Vertices, 4 * sizeof(dReal), VertexCount,
454 Indices, IndexCount, 3 * sizeof(unsigned int),
455 Normals);
456#else
457 dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount,
458 Indices, IndexCount, 3 * sizeof(unsigned int),
459 Normals);
460#endif
461}
462
463
464void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
465 const dReal* Vertices, int VertexCount,
466 const int* Indices, int IndexCount) {
467 dGeomTriMeshDataBuildSimple1(g,
468 Vertices, VertexCount, Indices, IndexCount,
469 (const int*)NULL);
470}
471
472void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
473{
474 dUASSERT(g, "argument not trimesh data");
475 g->Preprocess();
476}
477
478void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
479{
480 dUASSERT(g, "argument not trimesh data");
481#if dTRIMESH_ENABLED
482 *buf = g->UseFlags;
483 *bufLen = g->Mesh.GetNbTriangles();
484#endif // dTRIMESH_ENABLED
485}
486
487void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
488{
489 dUASSERT(g, "argument not trimesh data");
490 g->UseFlags = buf;
491}
492
493
494#if dTRIMESH_ENABLED
495
496// Trimesh Class Statics
497PlanesCollider dxTriMesh::_PlanesCollider;
498SphereCollider dxTriMesh::_SphereCollider;
499OBBCollider dxTriMesh::_OBBCollider;
500RayCollider dxTriMesh::_RayCollider;
501AABBTreeCollider dxTriMesh::_AABBTreeCollider;
502LSSCollider dxTriMesh::_LSSCollider;
503
504SphereCache dxTriMesh::defaultSphereCache;
505OBBCache dxTriMesh::defaultBoxCache;
506LSSCache dxTriMesh::defaultCapsuleCache;
507
508CollisionFaces dxTriMesh::Faces;
509
510#endif // dTRIMESH_ENABLED
511
512
513dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1)
514{
515 type = dTriMeshClass;
516
517 this->Data = Data;
518
519#if dTRIMESH_ENABLED
520
521 _RayCollider.SetDestination(&Faces);
522
523 _PlanesCollider.SetTemporalCoherence(true);
524
525 _SphereCollider.SetTemporalCoherence(true);
526 _SphereCollider.SetPrimitiveTests(false);
527
528 _OBBCollider.SetTemporalCoherence(true);
529
530 // no first-contact test (i.e. return full contact info)
531 _AABBTreeCollider.SetFirstContact( false );
532 // temporal coherence only works with "first conact" tests
533 _AABBTreeCollider.SetTemporalCoherence(false);
534 // Perform full BV-BV tests (true) or SAT-lite tests (false)
535 _AABBTreeCollider.SetFullBoxBoxTest( true );
536 // Perform full Primitive-BV tests (true) or SAT-lite tests (false)
537 _AABBTreeCollider.SetFullPrimBoxTest( true );
538 _LSSCollider.SetTemporalCoherence(false);
539
540#endif // dTRIMESH_ENABLED
541
542 /* TC has speed/space 'issues' that don't make it a clear
543 win by default on spheres/boxes. */
544 this->doSphereTC = false;
545 this->doBoxTC = false;
546 this->doCapsuleTC = false;
547
548#if dTRIMESH_ENABLED
549
550 const char* msg;
551 if ((msg =_AABBTreeCollider.ValidateSettings()))
552 dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__);
553 _LSSCollider.SetPrimitiveTests(false);
554 _LSSCollider.SetFirstContact(false);
555
556#endif // dTRIMESH_ENABLED
557
558 for (int i=0; i<16; i++)
559 last_trans[i] = REAL( 0.0 );
560}
561
562dxTriMesh::~dxTriMesh(){
563 //
564}
565
566// Cleanup for allocations when shutting down ODE
567void opcode_collider_cleanup()
568{
569#if dTRIMESH_ENABLED
570
571 // Clear TC caches
572 dxTriMesh::Faces.Empty();
573 dxTriMesh::defaultSphereCache.TouchedPrimitives.Empty();
574 dxTriMesh::defaultBoxCache.TouchedPrimitives.Empty();
575 dxTriMesh::defaultCapsuleCache.TouchedPrimitives.Empty();
576
577#endif // dTRIMESH_ENABLED
578}
579
580
581
582void dxTriMesh::ClearTCCache()
583{
584#if dTRIMESH_ENABLED
585 /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches -
586 but the destructor isn't called when doing this, so we would leak.
587 So, call the previous caches' containers' destructors by hand first. */
588 int i, n;
589 n = SphereTCCache.size();
590 for( i = 0; i < n; ++i ) {
591 SphereTCCache[i].~SphereTC();
592 }
593 SphereTCCache.setSize(0);
594 n = BoxTCCache.size();
595 for( i = 0; i < n; ++i ) {
596 BoxTCCache[i].~BoxTC();
597 }
598 BoxTCCache.setSize(0);
599 n = CapsuleTCCache.size();
600 for( i = 0; i < n; ++i ) {
601 CapsuleTCCache[i].~CapsuleTC();
602 }
603 CapsuleTCCache.setSize(0);
604#endif // dTRIMESH_ENABLED
605}
606
607
608int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
609 return 1;
610}
611
612
613void dxTriMesh::computeAABB() {
614 const dxTriMeshData* d = Data;
615 dVector3 c;
616 const dMatrix3& R = final_posr->R;
617 const dVector3& pos = final_posr->pos;
618
619 dMULTIPLY0_331( c, R, d->AABBCenter );
620
621 dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
622 dFabs(R[1] * Data->AABBExtents[1]) +
623 dFabs(R[2] * Data->AABBExtents[2]);
624 dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
625 dFabs(R[5] * Data->AABBExtents[1]) +
626 dFabs(R[6] * Data->AABBExtents[2]);
627 dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
628 dFabs(R[9] * Data->AABBExtents[1]) +
629 dFabs(R[10] * Data->AABBExtents[2]);
630
631 aabb[0] = c[0] + pos[0] - xrange;
632 aabb[1] = c[0] + pos[0] + xrange;
633 aabb[2] = c[1] + pos[1] - yrange;
634 aabb[3] = c[1] + pos[1] + yrange;
635 aabb[4] = c[2] + pos[2] - zrange;
636 aabb[5] = c[2] + pos[2] + zrange;
637}
638
639
640void dxTriMeshData::UpdateData()
641{
642#if dTRIMESH_ENABLED
643 BVTree.Refit();
644#endif // dTRIMESH_ENABLED
645}
646
647
648dGeomID dCreateTriMesh(dSpaceID space,
649 dTriMeshDataID Data,
650 dTriCallback* Callback,
651 dTriArrayCallback* ArrayCallback,
652 dTriRayCallback* RayCallback)
653{
654 dxTriMesh* Geom = new dxTriMesh(space, Data);
655 Geom->Callback = Callback;
656 Geom->ArrayCallback = ArrayCallback;
657 Geom->RayCallback = RayCallback;
658
659 return Geom;
660}
661
662void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
663{
664 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
665 ((dxTriMesh*)g)->Callback = Callback;
666}
667
668dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
669{
670 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
671 return ((dxTriMesh*)g)->Callback;
672}
673
674void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
675{
676 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
677 ((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
678}
679
680dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
681{
682 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
683 return ((dxTriMesh*)g)->ArrayCallback;
684}
685
686void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
687{
688 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
689 ((dxTriMesh*)g)->RayCallback = Callback;
690}
691
692dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
693{
694 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
695 return ((dxTriMesh*)g)->RayCallback;
696}
697
698void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
699{
700 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
701 ((dxTriMesh*)g)->Data = Data;
702 // I changed my data -- I know nothing about my own AABB anymore.
703 ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD);
704}
705
706dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
707{
708 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
709 return ((dxTriMesh*)g)->Data;
710}
711
712
713
714void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
715{
716 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
717
718 switch (geomClass)
719 {
720 case dSphereClass:
721 ((dxTriMesh*)g)->doSphereTC = (1 == enable);
722 break;
723 case dBoxClass:
724 ((dxTriMesh*)g)->doBoxTC = (1 == enable);
725 break;
726 case dCapsuleClass:
727 ((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
728 break;
729 }
730}
731
732int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
733{
734 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
735
736 switch (geomClass)
737 {
738 case dSphereClass:
739 if (((dxTriMesh*)g)->doSphereTC)
740 return 1;
741 break;
742 case dBoxClass:
743 if (((dxTriMesh*)g)->doBoxTC)
744 return 1;
745 break;
746 case dCapsuleClass:
747 if (((dxTriMesh*)g)->doCapsuleTC)
748 return 1;
749 break;
750 }
751 return 0;
752}
753
754void dGeomTriMeshClearTCCache(dGeomID g){
755 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
756
757 dxTriMesh* Geom = (dxTriMesh*)g;
758 Geom->ClearTCCache();
759}
760
761/*
762 * returns the TriMeshDataID
763 */
764dTriMeshDataID
765dGeomTriMeshGetTriMeshDataID(dGeomID g)
766{
767 dxTriMesh* Geom = (dxTriMesh*) g;
768 return Geom->Data;
769}
770
771// Getting data
772void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
773 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
774
775 dxTriMesh* Geom = (dxTriMesh*)g;
776
777 const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
778 const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
779
780 dVector3 v[3];
781 FetchTriangle(Geom, Index, Position, Rotation, v);
782
783 if (v0){
784 (*v0)[0] = v[0][0];
785 (*v0)[1] = v[0][1];
786 (*v0)[2] = v[0][2];
787 (*v0)[3] = v[0][3];
788 }
789 if (v1){
790 (*v1)[0] = v[1][0];
791 (*v1)[1] = v[1][1];
792 (*v1)[2] = v[1][2];
793 (*v1)[3] = v[1][3];
794 }
795 if (v2){
796 (*v2)[0] = v[2][0];
797 (*v2)[1] = v[2][1];
798 (*v2)[2] = v[2][2];
799 (*v2)[3] = v[2][3];
800 }
801}
802
803void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
804 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
805
806 dxTriMesh* Geom = (dxTriMesh*)g;
807
808 const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
809 const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
810
811 dVector3 dv[3];
812 FetchTriangle(Geom, Index, Position, Rotation, dv);
813
814 GetPointFromBarycentric(dv, u, v, Out);
815}
816
817int dGeomTriMeshGetTriangleCount (dGeomID g)
818{
819#if dTRIMESH_ENABLED
820 dxTriMesh* Geom = (dxTriMesh*)g;
821 return Geom->Data->Mesh.GetNbTriangles();
822#else
823 return 0;
824#endif // dTRIMESH_ENABLED
825}
826
827void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
828 dUASSERT(g, "argument not trimesh data");
829 g->UpdateData();
830}
831
832#endif // dTRIMESH_OPCODE
833#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp
new file mode 100644
index 0000000..54034aa
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_plane.cpp
@@ -0,0 +1,213 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// TriMesh - Plane collider by David Walters, July 2006
24
25#include <ode/collision.h>
26#include <ode/matrix.h>
27#include <ode/rotation.h>
28#include <ode/odemath.h>
29
30#if dTRIMESH_ENABLED
31
32#include "collision_util.h"
33#include "collision_std.h"
34
35#define TRIMESH_INTERNAL
36#include "collision_trimesh_internal.h"
37
38#if dTRIMESH_OPCODE
39int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
40{
41 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
42 dIASSERT( o1->type == dTriMeshClass );
43 dIASSERT( o2->type == dPlaneClass );
44 dIASSERT ((flags & NUMC_MASK) >= 1);
45
46 // Alias pointers to the plane and trimesh
47 dxTriMesh* trimesh = (dxTriMesh*)( o1 );
48 dxPlane* plane = (dxPlane*)( o2 );
49
50 int contact_count = 0;
51
52 // Cache the maximum contact count.
53 const int contact_max = ( flags & NUMC_MASK );
54
55 // Cache trimesh position and rotation.
56 const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh );
57 const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh );
58
59 //
60 // For all triangles.
61 //
62
63 // Cache the triangle count.
64 const int tri_count = trimesh->Data->Mesh.GetNbTriangles();
65
66 VertexPointers VP;
67 dReal alpha;
68 dVector3 vertex;
69
70#if !defined(dSINGLE) || 1
71 dVector3 int_vertex; // Intermediate vertex for double precision mode.
72#endif // dSINGLE
73
74 // For each triangle
75 for ( int t = 0; t < tri_count; ++t )
76 {
77 // Get triangle, which should also use callback.
78 trimesh->Data->Mesh.GetTriangle( VP, t );
79
80 // For each vertex.
81 for ( int v = 0; v < 3; ++v )
82 {
83 //
84 // Get Vertex
85 //
86
87#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation
88
89 dMULTIPLY0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) );
90
91#else // dDOUBLE || 1
92
93 // OPCODE data is in single precision format.
94 int_vertex[ 0 ] = VP.Vertex[ v ]->x;
95 int_vertex[ 1 ] = VP.Vertex[ v ]->y;
96 int_vertex[ 2 ] = VP.Vertex[ v ]->z;
97
98 dMULTIPLY0_331( vertex, trimesh_R, int_vertex );
99
100#endif // dSINGLE/dDOUBLE
101
102 vertex[ 0 ] += trimesh_pos[ 0 ];
103 vertex[ 1 ] += trimesh_pos[ 1 ];
104 vertex[ 2 ] += trimesh_pos[ 2 ];
105
106
107 //
108 // Collision?
109 //
110
111 // If alpha < 0 then point is if front of plane. i.e. no contact
112 // If alpha = 0 then the point is on the plane
113 alpha = plane->p[ 3 ] - dDOT( plane->p, vertex );
114
115 // If alpha > 0 the point is behind the plane. CONTACT!
116 if ( alpha > 0 )
117 {
118 // Alias the contact
119 dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip );
120
121 contact->pos[ 0 ] = vertex[ 0 ];
122 contact->pos[ 1 ] = vertex[ 1 ];
123 contact->pos[ 2 ] = vertex[ 2 ];
124
125 contact->normal[ 0 ] = plane->p[ 0 ];
126 contact->normal[ 1 ] = plane->p[ 1 ];
127 contact->normal[ 2 ] = plane->p[ 2 ];
128
129 contact->depth = alpha;
130 contact->g1 = plane;
131 contact->g2 = trimesh;
132
133 ++contact_count;
134
135 // All contact slots are full?
136 if ( contact_count >= contact_max )
137 return contact_count; // <=== STOP HERE
138 }
139 }
140 }
141
142 // Return contact count.
143 return contact_count;
144}
145#endif // dTRIMESH_OPCODE
146
147#if dTRIMESH_GIMPACT
148int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
149{
150 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
151 dIASSERT( o1->type == dTriMeshClass );
152 dIASSERT( o2->type == dPlaneClass );
153 dIASSERT ((flags & NUMC_MASK) >= 1);
154
155 // Alias pointers to the plane and trimesh
156 dxTriMesh* trimesh = (dxTriMesh*)( o1 );
157 vec4f plane;
158 dGeomPlaneGetParams(o2, plane);
159
160 //Find collision
161
162 GDYNAMIC_ARRAY collision_result;
163 GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result);
164
165 gim_trimesh_plane_collision(&trimesh->m_collision_trimesh,plane,&collision_result);
166
167 if(collision_result.m_size == 0 )
168 {
169 GIM_DYNARRAY_DESTROY(collision_result);
170 return 0;
171 }
172
173
174 unsigned int contactcount = collision_result.m_size;
175 unsigned int contactmax = (unsigned int)(flags & NUMC_MASK);
176 if (contactcount > contactmax)
177 {
178 contactcount = contactmax;
179 }
180
181 dContactGeom* pcontact;
182 vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result);
183
184 for(unsigned int i = 0; i < contactcount; i++ )
185 {
186 pcontact = SAFECONTACT(flags, contacts, i, skip);
187
188 pcontact->pos[0] = (*planecontact_results)[0];
189 pcontact->pos[1] = (*planecontact_results)[1];
190 pcontact->pos[2] = (*planecontact_results)[2];
191 pcontact->pos[3] = REAL(1.0);
192
193 pcontact->normal[0] = plane[0];
194 pcontact->normal[1] = plane[1];
195 pcontact->normal[2] = plane[2];
196 pcontact->normal[3] = 0;
197
198 pcontact->depth = (*planecontact_results)[3];
199 pcontact->g1 = o1;
200 pcontact->g2 = o2;
201
202 planecontact_results++;
203 }
204
205 GIM_DYNARRAY_DESTROY(collision_result);
206
207 return (int)contactcount;
208}
209#endif // dTRIMESH_GIMPACT
210
211
212#endif // dTRIMESH_ENABLED
213
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp
new file mode 100644
index 0000000..c0b97ca
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp
@@ -0,0 +1,198 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// TriMesh code by Erwin de Vries.
24
25#include <ode/collision.h>
26#include <ode/matrix.h>
27#include <ode/rotation.h>
28#include <ode/odemath.h>
29
30#if dTRIMESH_ENABLED
31
32#include "collision_util.h"
33
34#define TRIMESH_INTERNAL
35#include "collision_trimesh_internal.h"
36
37#if dTRIMESH_OPCODE
38int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
39 dIASSERT (Stride >= (int)sizeof(dContactGeom));
40 dIASSERT (g1->type == dTriMeshClass);
41 dIASSERT (RayGeom->type == dRayClass);
42 dIASSERT ((Flags & NUMC_MASK) >= 1);
43
44 dxTriMesh* TriMesh = (dxTriMesh*)g1;
45
46 const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
47 const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
48
49 RayCollider& Collider = TriMesh->_RayCollider;
50
51 dReal Length = dGeomRayGetLength(RayGeom);
52
53 int FirstContact, BackfaceCull;
54 dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
55 int ClosestHit = dGeomRayGetClosestHit(RayGeom);
56
57 Collider.SetFirstContact(FirstContact != 0);
58 Collider.SetClosestHit(ClosestHit != 0);
59 Collider.SetCulling(BackfaceCull != 0);
60 Collider.SetMaxDist(Length);
61
62 dVector3 Origin, Direction;
63 dGeomRayGet(RayGeom, Origin, Direction);
64
65 /* Make Ray */
66 Ray WorldRay;
67 WorldRay.mOrig.x = Origin[0];
68 WorldRay.mOrig.y = Origin[1];
69 WorldRay.mOrig.z = Origin[2];
70 WorldRay.mDir.x = Direction[0];
71 WorldRay.mDir.y = Direction[1];
72 WorldRay.mDir.z = Direction[2];
73
74 /* Intersect */
75 Matrix4x4 amatrix;
76 int TriCount = 0;
77 if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
78 TriCount = TriMesh->Faces.GetNbFaces();
79 }
80
81 if (TriCount == 0) {
82 return 0;
83 }
84
85 const CollisionFace* Faces = TriMesh->Faces.GetFaces();
86
87 int OutTriCount = 0;
88 for (int i = 0; i < TriCount; i++) {
89 if (TriMesh->RayCallback == null ||
90 TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
91 Faces[i].mU, Faces[i].mV)) {
92 const int& TriIndex = Faces[i].mFaceID;
93 if (!Callback(TriMesh, RayGeom, TriIndex)) {
94 continue;
95 }
96
97 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
98
99 dVector3 dv[3];
100 FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
101
102 // No sense to save on single type conversion in algorithm of this size.
103 // If there would be a custom typedef for distance type it could be used
104 // instead of dReal. However using float directly is the loss of abstraction
105 // and possible loss of precision in future.
106 /*float*/ dReal T = Faces[i].mDistance;
107 Contact->pos[0] = Origin[0] + (Direction[0] * T);
108 Contact->pos[1] = Origin[1] + (Direction[1] * T);
109 Contact->pos[2] = Origin[2] + (Direction[2] * T);
110 Contact->pos[3] = REAL(0.0);
111
112 dVector3 vu;
113 vu[0] = dv[1][0] - dv[0][0];
114 vu[1] = dv[1][1] - dv[0][1];
115 vu[2] = dv[1][2] - dv[0][2];
116 vu[3] = REAL(0.0);
117
118 dVector3 vv;
119 vv[0] = dv[2][0] - dv[0][0];
120 vv[1] = dv[2][1] - dv[0][1];
121 vv[2] = dv[2][2] - dv[0][2];
122 vv[3] = REAL(0.0);
123
124 dCROSS(Contact->normal, =, vv, vu); // Reversed
125
126 dNormalize3(Contact->normal);
127
128 Contact->depth = T;
129 Contact->g1 = TriMesh;
130 Contact->g2 = RayGeom;
131
132 OutTriCount++;
133
134 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
135 if (OutTriCount >= (Flags & NUMC_MASK)) {
136 break;
137 }
138 }
139 }
140 return OutTriCount;
141}
142#endif // dTRIMESH_OPCODE
143
144#if dTRIMESH_GIMPACT
145int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
146{
147 dIASSERT (Stride >= (int)sizeof(dContactGeom));
148 dIASSERT (g1->type == dTriMeshClass);
149 dIASSERT (RayGeom->type == dRayClass);
150 dIASSERT ((Flags & NUMC_MASK) >= 1);
151
152 dxTriMesh* TriMesh = (dxTriMesh*)g1;
153
154 dReal Length = dGeomRayGetLength(RayGeom);
155 int FirstContact, BackfaceCull;
156 dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
157 int ClosestHit = dGeomRayGetClosestHit(RayGeom);
158 dVector3 Origin, Direction;
159 dGeomRayGet(RayGeom, Origin, Direction);
160
161 char intersect=0;
162 GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;
163
164 if(ClosestHit)
165 {
166 intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
167 }
168 else
169 {
170 intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
171 }
172
173 if(intersect == 0)
174 {
175 return 0;
176 }
177
178 int OutTriCount = 0;
179
180 if(!TriMesh->RayCallback ||
181 TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v))
182 {
183 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride);
184 VEC_COPY(Contact->pos,contact_data.m_point);
185 VEC_COPY(Contact->normal,contact_data.m_normal);
186 Contact->depth = contact_data.tparam;
187 Contact->g1 = TriMesh;
188 Contact->g2 = RayGeom;
189
190 OutTriCount = 1;
191 }
192
193 return OutTriCount;
194}
195#endif // dTRIMESH_GIMPACT
196
197#endif // dTRIMESH_ENABLED
198
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp
new file mode 100644
index 0000000..7ee9b4b
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp
@@ -0,0 +1,573 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// TriMesh code by Erwin de Vries.
24
25#include <ode/collision.h>
26#include <ode/matrix.h>
27#include <ode/rotation.h>
28#include <ode/odemath.h>
29#include "collision_util.h"
30
31#if dTRIMESH_ENABLED
32
33#define TRIMESH_INTERNAL
34#include "collision_trimesh_internal.h"
35
36#if dTRIMESH_OPCODE
37#define MERGECONTACTS
38
39// Ripped from Opcode 1.1.
40static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){
41
42 // now onto the bulk of the collision...
43
44 dVector3 Diff;
45 Diff[0] = Origin[0] - Center[0];
46 Diff[1] = Origin[1] - Center[1];
47 Diff[2] = Origin[2] - Center[2];
48 Diff[3] = Origin[3] - Center[3];
49
50 dReal A00 = dDOT(Edge0, Edge0);
51 dReal A01 = dDOT(Edge0, Edge1);
52 dReal A11 = dDOT(Edge1, Edge1);
53
54 dReal B0 = dDOT(Diff, Edge0);
55 dReal B1 = dDOT(Diff, Edge1);
56
57 dReal C = dDOT(Diff, Diff);
58
59 dReal Det = dFabs(A00 * A11 - A01 * A01);
60 u = A01 * B1 - A11 * B0;
61 v = A01 * B0 - A00 * B1;
62
63 dReal DistSq;
64
65 if (u + v <= Det){
66 if(u < REAL(0.0)){
67 if(v < REAL(0.0)){ // region 4
68 if(B0 < REAL(0.0)){
69 v = REAL(0.0);
70 if (-B0 >= A00){
71 u = REAL(1.0);
72 DistSq = A00 + REAL(2.0) * B0 + C;
73 }
74 else{
75 u = -B0 / A00;
76 DistSq = B0 * u + C;
77 }
78 }
79 else{
80 u = REAL(0.0);
81 if(B1 >= REAL(0.0)){
82 v = REAL(0.0);
83 DistSq = C;
84 }
85 else if(-B1 >= A11){
86 v = REAL(1.0);
87 DistSq = A11 + REAL(2.0) * B1 + C;
88 }
89 else{
90 v = -B1 / A11;
91 DistSq = B1 * v + C;
92 }
93 }
94 }
95 else{ // region 3
96 u = REAL(0.0);
97 if(B1 >= REAL(0.0)){
98 v = REAL(0.0);
99 DistSq = C;
100 }
101 else if(-B1 >= A11){
102 v = REAL(1.0);
103 DistSq = A11 + REAL(2.0) * B1 + C;
104 }
105 else{
106 v = -B1 / A11;
107 DistSq = B1 * v + C;
108 }
109 }
110 }
111 else if(v < REAL(0.0)){ // region 5
112 v = REAL(0.0);
113 if (B0 >= REAL(0.0)){
114 u = REAL(0.0);
115 DistSq = C;
116 }
117 else if (-B0 >= A00){
118 u = REAL(1.0);
119 DistSq = A00 + REAL(2.0) * B0 + C;
120 }
121 else{
122 u = -B0 / A00;
123 DistSq = B0 * u + C;
124 }
125 }
126 else{ // region 0
127 // minimum at interior point
128 if (Det == REAL(0.0)){
129 u = REAL(0.0);
130 v = REAL(0.0);
131 DistSq = FLT_MAX;
132 }
133 else{
134 dReal InvDet = REAL(1.0) / Det;
135 u *= InvDet;
136 v *= InvDet;
137 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
138 }
139 }
140 }
141 else{
142 dReal Tmp0, Tmp1, Numer, Denom;
143
144 if(u < REAL(0.0)){ // region 2
145 Tmp0 = A01 + B0;
146 Tmp1 = A11 + B1;
147 if (Tmp1 > Tmp0){
148 Numer = Tmp1 - Tmp0;
149 Denom = A00 - REAL(2.0) * A01 + A11;
150 if (Numer >= Denom){
151 u = REAL(1.0);
152 v = REAL(0.0);
153 DistSq = A00 + REAL(2.0) * B0 + C;
154 }
155 else{
156 u = Numer / Denom;
157 v = REAL(1.0) - u;
158 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
159 }
160 }
161 else{
162 u = REAL(0.0);
163 if(Tmp1 <= REAL(0.0)){
164 v = REAL(1.0);
165 DistSq = A11 + REAL(2.0) * B1 + C;
166 }
167 else if(B1 >= REAL(0.0)){
168 v = REAL(0.0);
169 DistSq = C;
170 }
171 else{
172 v = -B1 / A11;
173 DistSq = B1 * v + C;
174 }
175 }
176 }
177 else if(v < REAL(0.0)){ // region 6
178 Tmp0 = A01 + B1;
179 Tmp1 = A00 + B0;
180 if (Tmp1 > Tmp0){
181 Numer = Tmp1 - Tmp0;
182 Denom = A00 - REAL(2.0) * A01 + A11;
183 if (Numer >= Denom){
184 v = REAL(1.0);
185 u = REAL(0.0);
186 DistSq = A11 + REAL(2.0) * B1 + C;
187 }
188 else{
189 v = Numer / Denom;
190 u = REAL(1.0) - v;
191 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
192 }
193 }
194 else{
195 v = REAL(0.0);
196 if (Tmp1 <= REAL(0.0)){
197 u = REAL(1.0);
198 DistSq = A00 + REAL(2.0) * B0 + C;
199 }
200 else if(B0 >= REAL(0.0)){
201 u = REAL(0.0);
202 DistSq = C;
203 }
204 else{
205 u = -B0 / A00;
206 DistSq = B0 * u + C;
207 }
208 }
209 }
210 else{ // region 1
211 Numer = A11 + B1 - A01 - B0;
212 if (Numer <= REAL(0.0)){
213 u = REAL(0.0);
214 v = REAL(1.0);
215 DistSq = A11 + REAL(2.0) * B1 + C;
216 }
217 else{
218 Denom = A00 - REAL(2.0) * A01 + A11;
219 if (Numer >= Denom){
220 u = REAL(1.0);
221 v = REAL(0.0);
222 DistSq = A00 + REAL(2.0) * B0 + C;
223 }
224 else{
225 u = Numer / Denom;
226 v = REAL(1.0) - u;
227 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
228 }
229 }
230 }
231 }
232
233 Dist = dSqrt(dFabs(DistSq));
234
235 if (Dist <= Radius){
236 Dist = Radius - Dist;
237 return true;
238 }
239 else return false;
240}
241
242int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
243 dIASSERT (Stride >= (int)sizeof(dContactGeom));
244 dIASSERT (g1->type == dTriMeshClass);
245 dIASSERT (SphereGeom->type == dSphereClass);
246 dIASSERT ((Flags & NUMC_MASK) >= 1);
247
248 dxTriMesh* TriMesh = (dxTriMesh*)g1;
249
250 // Init
251 const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
252 const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
253
254 SphereCollider& Collider = TriMesh->_SphereCollider;
255
256 const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
257 dReal Radius = dGeomSphereGetRadius(SphereGeom);
258
259 // Sphere
260 Sphere Sphere;
261 Sphere.mCenter.x = Position[0];
262 Sphere.mCenter.y = Position[1];
263 Sphere.mCenter.z = Position[2];
264 Sphere.mRadius = Radius;
265
266 Matrix4x4 amatrix;
267
268 // TC results
269 if (TriMesh->doSphereTC) {
270 dxTriMesh::SphereTC* sphereTC = 0;
271 for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
272 if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
273 sphereTC = &TriMesh->SphereTCCache[i];
274 break;
275 }
276 }
277
278 if (!sphereTC){
279 TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
280
281 sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
282 sphereTC->Geom = SphereGeom;
283 }
284
285 // Intersect
286 Collider.SetTemporalCoherence(true);
287 Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
288 &MakeMatrix(TLPosition, TLRotation, amatrix));
289 }
290 else {
291 Collider.SetTemporalCoherence(false);
292 Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
293 &MakeMatrix(TLPosition, TLRotation, amatrix));
294 }
295
296 if (! Collider.GetContactStatus()) {
297 // no collision occurred
298 return 0;
299 }
300
301 // get results
302 int TriCount = Collider.GetNbTouchedPrimitives();
303 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
304
305 if (TriCount != 0){
306 if (TriMesh->ArrayCallback != null){
307 TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
308 }
309
310 int OutTriCount = 0;
311 for (int i = 0; i < TriCount; i++){
312 if (OutTriCount == (Flags & NUMC_MASK)){
313 break;
314 }
315
316 const int TriIndex = Triangles[i];
317
318 dVector3 dv[3];
319 if (!Callback(TriMesh, SphereGeom, TriIndex))
320 continue;
321 FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
322
323 dVector3& v0 = dv[0];
324 dVector3& v1 = dv[1];
325 dVector3& v2 = dv[2];
326
327 dVector3 vu;
328 vu[0] = v1[0] - v0[0];
329 vu[1] = v1[1] - v0[1];
330 vu[2] = v1[2] - v0[2];
331 vu[3] = REAL(0.0);
332
333 dVector3 vv;
334 vv[0] = v2[0] - v0[0];
335 vv[1] = v2[1] - v0[1];
336 vv[2] = v2[2] - v0[2];
337 vv[3] = REAL(0.0);
338
339 // Get plane coefficients
340 dVector4 Plane;
341 dCROSS(Plane, =, vu, vv);
342
343 dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
344 Plane[0] /= Area;
345 Plane[1] /= Area;
346 Plane[2] /= Area;
347
348 Plane[3] = dDOT(Plane, v0);
349
350 /* If the center of the sphere is within the positive halfspace of the
351 * triangle's plane, allow a contact to be generated.
352 * If the center of the sphere made it into the positive halfspace of a
353 * back-facing triangle, then the physics update and/or velocity needs
354 * to be adjusted (penetration has occured anyway).
355 */
356
357 dReal side = dDOT(Plane,Position) - Plane[3];
358
359 if(side < REAL(0.0)) {
360 continue;
361 }
362
363 dReal Depth;
364 dReal u, v;
365 if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
366 continue; // Sphere doesn't hit triangle
367 }
368
369 if (Depth < REAL(0.0)){
370 Depth = REAL(0.0);
371 }
372
373 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
374
375 dReal w = REAL(1.0) - u - v;
376 Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
377 Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
378 Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
379 Contact->pos[3] = REAL(0.0);
380
381 // Using normal as plane (reversed)
382 Contact->normal[0] = -Plane[0];
383 Contact->normal[1] = -Plane[1];
384 Contact->normal[2] = -Plane[2];
385 Contact->normal[3] = REAL(0.0);
386
387 // Depth returned from GetContactData is depth along
388 // contact point - sphere center direction
389 // we'll project it to contact normal
390 dVector3 dir;
391 dir[0] = Position[0]-Contact->pos[0];
392 dir[1] = Position[1]-Contact->pos[1];
393 dir[2] = Position[2]-Contact->pos[2];
394 dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
395 Contact->depth = Depth * dirProj;
396 //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
397 Contact->side1 = TriIndex;
398
399 //Contact->g1 = TriMesh;
400 //Contact->g2 = SphereGeom;
401
402 OutTriCount++;
403 }
404#ifdef MERGECONTACTS // Merge all contacts into 1
405 if (OutTriCount != 0){
406 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
407
408 if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
409 Contact->normal[0] *= Contact->depth;
410 Contact->normal[1] *= Contact->depth;
411 Contact->normal[2] *= Contact->depth;
412 Contact->normal[3] *= Contact->depth;
413
414 for (int i = 1; i < OutTriCount; i++){
415 dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
416
417 Contact->pos[0] += TempContact->pos[0];
418 Contact->pos[1] += TempContact->pos[1];
419 Contact->pos[2] += TempContact->pos[2];
420 Contact->pos[3] += TempContact->pos[3];
421
422 Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
423 Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
424 Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
425 Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
426 }
427
428 Contact->pos[0] /= OutTriCount;
429 Contact->pos[1] /= OutTriCount;
430 Contact->pos[2] /= OutTriCount;
431 Contact->pos[3] /= OutTriCount;
432
433 // Remember to divide in square space.
434 Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
435
436 dNormalize3(Contact->normal);
437 }
438
439 Contact->g1 = TriMesh;
440 Contact->g2 = SphereGeom;
441
442 // TODO:
443 // Side1 now contains index of triangle that gave first hit
444 // Probably we should find index of triangle with deepest penetration
445
446 return 1;
447 }
448 else return 0;
449#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
450 if (OutTriCount != 0){
451 if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
452 dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
453 Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
454 Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
455 Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
456 Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
457
458 for (int i = 1; i < OutTriCount; i++){
459 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
460
461 Normal[0] += Contact->normal[0] * Contact->depth;
462 Normal[1] += Contact->normal[1] * Contact->depth;
463 Normal[2] += Contact->normal[2] * Contact->depth;
464 Normal[3] += Contact->normal[3] * Contact->depth;
465 }
466 dNormalize3(Normal);
467
468 for (int i = 1; i < OutTriCount; i++){
469 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
470
471 Contact->normal[0] = Normal[0];
472 Contact->normal[1] = Normal[1];
473 Contact->normal[2] = Normal[2];
474 Contact->normal[3] = Normal[3];
475
476 Contact->g1 = TriMesh;
477 Contact->g2 = SphereGeom;
478 }
479 }
480 else{
481 SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
482 SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
483 }
484
485 return OutTriCount;
486 }
487 else return 0;
488#else //MERGECONTACTNORMALS // Just gather penetration depths and return
489 for (int i = 0; i < OutTriCount; i++){
490 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
491
492 //Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
493
494 /*Contact->normal[0] /= Contact->depth;
495 Contact->normal[1] /= Contact->depth;
496 Contact->normal[2] /= Contact->depth;
497 Contact->normal[3] /= Contact->depth;*/
498
499 Contact->g1 = TriMesh;
500 Contact->g2 = SphereGeom;
501 }
502
503 return OutTriCount;
504#endif // MERGECONTACTS
505 }
506 else return 0;
507}
508#endif // dTRIMESH_OPCODE
509
510#if dTRIMESH_GIMPACT
511int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride)
512{
513 dIASSERT (Stride >= (int)sizeof(dContactGeom));
514 dIASSERT (g1->type == dTriMeshClass);
515 dIASSERT (SphereGeom->type == dSphereClass);
516 dIASSERT ((Flags & NUMC_MASK) >= 1);
517
518 dxTriMesh* TriMesh = (dxTriMesh*)g1;
519 dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom);
520 dReal Radius = dGeomSphereGetRadius(SphereGeom);
521 //Create contact list
522 GDYNAMIC_ARRAY trimeshcontacts;
523 GIM_CREATE_CONTACT_LIST(trimeshcontacts);
524
525 //Collide trimeshes
526 gim_trimesh_sphere_collision(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
527
528 if(trimeshcontacts.m_size == 0)
529 {
530 GIM_DYNARRAY_DESTROY(trimeshcontacts);
531 return 0;
532 }
533
534 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
535
536 unsigned contactcount = trimeshcontacts.m_size;
537 unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
538 if (contactcount > maxcontacts)
539 {
540 contactcount = maxcontacts;
541 }
542
543 dContactGeom* pcontact;
544 unsigned i;
545
546 for (i=0;i<contactcount;i++)
547 {
548 pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
549
550 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
551 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
552 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
553 pcontact->pos[3] = REAL(1.0);
554
555 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
556 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
557 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
558 pcontact->normal[3] = 0;
559
560 pcontact->depth = ptrimeshcontacts->m_depth;
561 pcontact->g1 = g1;
562 pcontact->g2 = SphereGeom;
563
564 ptrimeshcontacts++;
565 }
566
567 GIM_DYNARRAY_DESTROY(trimeshcontacts);
568
569 return (int)contactcount;
570}
571#endif // dTRIMESH_GIMPACT
572
573#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp
new file mode 100644
index 0000000..c9f209e
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp
@@ -0,0 +1,2033 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// OPCODE TriMesh/TriMesh collision code by Jeff Smith (c) 2004
24
25#ifdef _MSC_VER
26#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
27#endif
28
29#include <ode/collision.h>
30#include <ode/matrix.h>
31#include <ode/rotation.h>
32#include <ode/odemath.h>
33
34// Classic Implementation
35#if !dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER
36
37#if dTRIMESH_ENABLED
38
39#include "collision_util.h"
40#define TRIMESH_INTERNAL
41#include "collision_trimesh_internal.h"
42
43#if dTRIMESH_OPCODE
44
45#define SMALL_ELT REAL(2.5e-4)
46#define EXPANDED_ELT_THRESH REAL(1.0e-3)
47#define DISTANCE_EPSILON REAL(1.0e-8)
48#define VELOCITY_EPSILON REAL(1.0e-5)
49#define TINY_PENETRATION REAL(5.0e-6)
50
51struct LineContactSet
52{
53 enum
54 {
55 MAX_POINTS = 8
56 };
57
58 dVector3 Points[MAX_POINTS];
59 int Count;
60};
61
62
63static void GetTriangleGeometryCallback(udword, VertexPointers&, udword);
64static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*,
65 const dVector3, const dVector3, dReal, int&);
66static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3],
67 dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar,
68 dReal isectpt1[3],dReal isectpt2[3]);
69inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
70static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
71static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3);
72static bool FindTriSolidIntrsection(const dVector3 Tri[3],
73 const dVector4 Planes[6], int numSides,
74 LineContactSet& ClippedPolygon );
75static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );
76static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt,
77 dVector3 in_n, dVector3* in_col_v, dReal &out_depth);
78static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point);
79static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
80 const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
81 dReal *t,dReal *u,dReal *v);
82
83
84
85
86/* some math macros */
87#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
88 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
89 dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; }
90
91#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
92
93#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }
94
95#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }
96
97#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; }
98
99#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }
100
101#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; }
102
103#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]; }
104
105#define LENGTH(x) ((dReal) dSqrt(dDOT(x, x)))
106
107#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];
108
109inline const dReal dMin(const dReal x, const dReal y)
110{
111 return x < y ? x : y;
112}
113
114
115inline void
116SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
117 dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
118 dVector3 n, dVector3 n1, dVector3 n2)
119{
120 if (pen_v == v1) {
121 pen_v = v2;
122 pen_elt = elt_f2;
123 col_v = v1;
124 SET(n, n1);
125 }
126 else {
127 pen_v = v1;
128 pen_elt = elt_f1;
129 col_v = v2;
130 SET(n, n2);
131 }
132}
133
134
135
136
137int
138dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
139{
140 dIASSERT (Stride >= (int)sizeof(dContactGeom));
141 dIASSERT (g1->type == dTriMeshClass);
142 dIASSERT (g2->type == dTriMeshClass);
143 dIASSERT ((Flags & NUMC_MASK) >= 1);
144
145 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
146 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
147
148 dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals;
149 dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals;
150
151 const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1);
152 // TLRotation1 = column-major order
153 const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1);
154
155 const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2);
156 // TLRotation2 = column-major order
157 const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2);
158
159 AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider;
160
161 static BVTCache ColCache;
162 ColCache.Model0 = &TriMesh1->Data->BVTree;
163 ColCache.Model1 = &TriMesh2->Data->BVTree;
164
165 // Collision query
166 Matrix4x4 amatrix, bmatrix;
167 BOOL IsOk = Collider.Collide(ColCache,
168 &MakeMatrix(TLPosition1, TLRotation1, amatrix),
169 &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
170
171
172 // Make "double" versions of these matrices, if appropriate
173 dMatrix4 A, B;
174 dMakeMatrix4(TLPosition1, TLRotation1, A);
175 dMakeMatrix4(TLPosition2, TLRotation2, B);
176
177
178 if (IsOk) {
179 // Get collision status => if true, objects overlap
180 if ( Collider.GetContactStatus() ) {
181 // Number of colliding pairs and list of pairs
182 int TriCount = Collider.GetNbPairs();
183 const Pair* CollidingPairs = Collider.GetPairs();
184
185 if (TriCount > 0) {
186 // step through the pairs, adding contacts
187 int id1, id2;
188 int OutTriCount = 0;
189 dVector3 v1[3], v2[3], CoplanarPt;
190 dVector3 e1, e2, e3, n1, n2, n, ContactNormal;
191 dReal depth;
192 dVector3 orig_pos, old_pos1, old_pos2, elt1, elt2, elt_sum;
193 dVector3 elt_f1[3], elt_f2[3];
194 dReal contact_elt_length = SMALL_ELT;
195 LineContactSet firstClippedTri, secondClippedTri;
196 dVector3 *firstClippedElt = new dVector3[LineContactSet::MAX_POINTS];
197 dVector3 *secondClippedElt = new dVector3[LineContactSet::MAX_POINTS];
198
199
200 // only do these expensive inversions once
201 dMatrix4 InvMatrix1, InvMatrix2;
202 dInvertMatrix4(A, InvMatrix1);
203 dInvertMatrix4(B, InvMatrix2);
204
205
206 for (int i = 0; i < TriCount; i++) {
207
208 id1 = CollidingPairs[i].id0;
209 id2 = CollidingPairs[i].id1;
210
211 // grab the colliding triangles
212 FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1);
213 FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2);
214 // Since we'll be doing matrix transfomrations, we need to
215 // make sure that all vertices have four elements
216 for (int j=0; j<3; j++) {
217 v1[j][3] = 1.0;
218 v2[j][3] = 1.0;
219 }
220
221
222 int IsCoplanar = 0;
223 dReal IsectPt1[3], IsectPt2[3];
224
225 // Sometimes OPCODE makes mistakes, so we look at the return
226 // value for TriTriIntersectWithIsectLine. A retcode of "0"
227 // means no intersection took place
228 if ( TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2],
229 &IsCoplanar,
230 IsectPt1, IsectPt2) ) {
231
232 // Compute the normals of the colliding faces
233 //
234 if (TriNormals1 == NULL) {
235 SUB( e1, v1[1], v1[0] );
236 SUB( e2, v1[2], v1[0] );
237 CROSS( n1, e1, e2 );
238 dNormalize3(n1);
239 }
240 else {
241 // If we were passed normals, we need to adjust them to take into
242 // account the objects' current rotations
243 e1[0] = TriNormals1[id1*3];
244 e1[1] = TriNormals1[id1*3 + 1];
245 e1[2] = TriNormals1[id1*3 + 2];
246 e1[3] = 0.0;
247
248 //dMultiply1(n1, TLRotation1, e1, 3, 3, 1);
249 dMultiply0(n1, TLRotation1, e1, 3, 3, 1);
250 n1[3] = 1.0;
251 }
252
253 if (TriNormals2 == NULL) {
254 SUB( e1, v2[1], v2[0] );
255 SUB( e2, v2[2], v2[0] );
256 CROSS( n2, e1, e2);
257 dNormalize3(n2);
258 }
259 else {
260 // If we were passed normals, we need to adjust them to take into
261 // account the objects' current rotations
262 e2[0] = TriNormals2[id2*3];
263 e2[1] = TriNormals2[id2*3 + 1];
264 e2[2] = TriNormals2[id2*3 + 2];
265 e2[3] = 0.0;
266
267 //dMultiply1(n2, TLRotation2, e2, 3, 3, 1);
268 dMultiply0(n2, TLRotation2, e2, 3, 3, 1);
269 n2[3] = 1.0;
270 }
271
272
273 if (IsCoplanar) {
274 // We can reach this case if the faces are coplanar, OR
275 // if they don't actually intersect. (OPCODE can make
276 // mistakes)
277 if (dFabs(dDOT(n1, n2)) > REAL(0.999)) {
278 // If the faces are coplanar, we declare that the point of
279 // contact is at the average location of the vertices of
280 // both faces
281 dVector3 ContactPt;
282 for (int j=0; j<3; j++) {
283 ContactPt[j] = 0.0;
284 for (int k=0; k<3; k++)
285 ContactPt[j] += v1[k][j] + v2[k][j];
286 ContactPt[j] /= 6.0;
287 }
288 ContactPt[3] = 1.0;
289
290 // and the contact normal is the normal of face 2
291 // (could be face 1, because they are the same)
292 SET(n, n2);
293
294 // and the penetration depth is the co-normal
295 // distance between any two vertices A and B,
296 // i.e. d = DOT(n, (A-B))
297 DEPTH(depth, v1[1], v2[1], n);
298 if (depth < 0)
299 depth *= -1.0;
300
301 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
302 ContactPt, n, depth, OutTriCount);
303 }
304 }
305 else {
306 // Otherwise (in non-co-planar cases), we create a coplanar
307 // point -- the middle of the line of intersection -- that
308 // will be used for various computations down the road
309 for (int j=0; j<3; j++)
310 CoplanarPt[j] = ( (IsectPt1[j] + IsectPt2[j]) / REAL(2.0) );
311 CoplanarPt[3] = 1.0;
312
313 // Find the ELT of the coplanar point
314 //
315 dMultiply1(orig_pos, InvMatrix1, CoplanarPt, 4, 4, 1);
316 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
317 SUB(elt1, CoplanarPt, old_pos1);
318
319 dMultiply1(orig_pos, InvMatrix2, CoplanarPt, 4, 4, 1);
320 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
321 SUB(elt2, CoplanarPt, old_pos2);
322
323 SUB(elt_sum, elt1, elt2); // net motion of the coplanar point
324
325
326 // Calculate how much the vertices of each face moved in the
327 // direction of the opposite face's normal
328 //
329 dReal total_dp1, total_dp2;
330 total_dp1 = 0.0;
331 total_dp2 = 0.0;
332
333 for (int ii=0; ii<3; ii++) {
334 // find the estimated linear translation (ELT) of the vertices
335 // on face 1, wrt to the center of face 2.
336
337 // un-transform this vertex by the current transform
338 dMultiply1(orig_pos, InvMatrix1, v1[ii], 4, 4, 1 );
339
340 // re-transform this vertex by last_trans (to get its old
341 // position)
342 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
343
344 // Then subtract this position from our current one to find
345 // the elapsed linear translation (ELT)
346 for (int k=0; k<3; k++) {
347 elt_f1[ii][k] = (v1[ii][k] - old_pos1[k]) - elt2[k];
348 }
349
350 // Take the dot product of the ELT for each vertex (wrt the
351 // center of face2)
352 total_dp1 += dFabs( dDOT(elt_f1[ii], n2) );
353 }
354
355 for (int ii=0; ii<3; ii++) {
356 // find the estimated linear translation (ELT) of the vertices
357 // on face 2, wrt to the center of face 1.
358 dMultiply1(orig_pos, InvMatrix2, v2[ii], 4, 4, 1);
359 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
360 for (int k=0; k<3; k++) {
361 elt_f2[ii][k] = (v2[ii][k] - old_pos2[k]) - elt1[k];
362 }
363
364 // Take the dot product of the ELT for each vertex (wrt the
365 // center of face2) and add them
366 total_dp2 += dFabs( dDOT(elt_f2[ii], n1) );
367 }
368
369
370 ////////
371 // Estimate the penetration depth.
372 //
373 dReal dp;
374 BOOL badPen = true;
375 dVector3 *pen_v; // the "penetrating vertices"
376 dVector3 *pen_elt; // the elt_f of the penetrating face
377 dVector3 *col_v; // the "collision vertices" (the penetrated face)
378
379 SMULT(n2, n2, -1.0); // SF PATCH #1335183
380 depth = 0.0;
381 if ((total_dp1 > DISTANCE_EPSILON) || (total_dp2 > DISTANCE_EPSILON)) {
382 ////////
383 // Find the collision normal, by finding the face
384 // that is pointed "most" in the direction of travel
385 // of the two triangles
386 //
387 if (total_dp2 > total_dp1) {
388 pen_v = v2;
389 pen_elt = elt_f2;
390 col_v = v1;
391 SET(n, n1);
392 }
393 else {
394 pen_v = v1;
395 pen_elt = elt_f1;
396 col_v = v2;
397 SET(n, n2);
398 }
399 }
400 else {
401 // the total_dp is very small, so let's fall back
402 // to a different test
403 if (LENGTH(elt2) > LENGTH(elt1)) {
404 pen_v = v2;
405 pen_elt = elt_f2;
406 col_v = v1;
407 SET(n, n1);
408 }
409 else {
410 pen_v = v1;
411 pen_elt = elt_f1;
412 col_v = v2;
413 SET(n, n2);
414 }
415 }
416
417
418 for (int j=0; j<3; j++)
419 if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) {
420 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
421 pen_v[j], n, depth, OutTriCount);
422 badPen = false;
423
424 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
425 break;
426 }
427 }
428
429
430 if (badPen) {
431 // try the other normal
432 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
433
434 for (int j=0; j<3; j++)
435 if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) {
436 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
437 pen_v[j], n, depth, OutTriCount);
438 badPen = false;
439
440 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
441 break;
442 }
443 }
444 }
445
446
447
448 ////////////////////////////////////////
449 //
450 // If we haven't found a good penetration, then we're probably straddling
451 // the edge of one of the objects, or the penetraing face is big
452 // enough that all of its vertices are outside the bounds of the
453 // penetrated face.
454 // In these cases, we do a more expensive test. We clip the penetrating
455 // triangle with a solid defined by the penetrated triangle, and repeat
456 // the tests above on this new polygon
457 if (badPen) {
458
459 // Switch pen_v and n back again
460 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
461
462
463 // Find the three sides (no top or bottom) of the solid defined by
464 // the edges of the penetrated triangle.
465
466 // The dVector4 "plane" structures contain the following information:
467 // [0]-[2]: The normal of the face, pointing INWARDS (i.e.
468 // the inverse normal
469 // [3]: The distance between the face and the center of the
470 // solid, along the normal
471 dVector4 SolidPlanes[3];
472 dVector3 tmp1;
473 dVector3 sn;
474
475 for (int j=0; j<3; j++) {
476 e1[j] = col_v[1][j] - col_v[0][j];
477 e2[j] = col_v[0][j] - col_v[2][j];
478 e3[j] = col_v[2][j] - col_v[1][j];
479 }
480
481 // side 1
482 CROSS(sn, e1, n);
483 dNormalize3(sn);
484 SMULT( SolidPlanes[0], sn, -1.0 );
485
486 ADD(tmp1, col_v[0], col_v[1]);
487 SMULT(tmp1, tmp1, 0.5); // center of edge
488 // distance from center to edge along normal
489 SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]);
490
491
492 // side 2
493 CROSS(sn, e2, n);
494 dNormalize3(sn);
495 SMULT( SolidPlanes[1], sn, -1.0 );
496
497 ADD(tmp1, col_v[0], col_v[2]);
498 SMULT(tmp1, tmp1, 0.5); // center of edge
499 // distance from center to edge along normal
500 SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]);
501
502
503 // side 3
504 CROSS(sn, e3, n);
505 dNormalize3(sn);
506 SMULT( SolidPlanes[2], sn, -1.0 );
507
508 ADD(tmp1, col_v[2], col_v[1]);
509 SMULT(tmp1, tmp1, 0.5); // center of edge
510 // distance from center to edge along normal
511 SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]);
512
513
514 FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri);
515
516 for (int j=0; j<firstClippedTri.Count; j++) {
517 firstClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults
518
519 DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], n);
520
521 // if the penetration depth (calculated above) is more than the contact
522 // point's ELT, then we've chosen the wrong face and should switch faces
523 if (pen_v == v1) {
524 dMultiply1(orig_pos, InvMatrix1, firstClippedTri.Points[j], 4, 4, 1);
525 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
526 for (int k=0; k<3; k++) {
527 firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos1[k]) - elt2[k];
528 }
529 }
530 else {
531 dMultiply1(orig_pos, InvMatrix2, firstClippedTri.Points[j], 4, 4, 1);
532 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
533 for (int k=0; k<3; k++) {
534 firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos2[k]) - elt1[k];
535 }
536 }
537
538 if (dp >= 0.0) {
539 contact_elt_length = dFabs(dDOT(firstClippedElt[j], n));
540
541 depth = dp;
542 if (depth == 0.0)
543 depth = dMin(DISTANCE_EPSILON, contact_elt_length);
544
545 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
546 depth = contact_elt_length;
547
548 if (depth <= contact_elt_length) {
549 // Add a contact
550 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
551 firstClippedTri.Points[j], n, depth, OutTriCount);
552 badPen = false;
553
554 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
555 break;
556 }
557 }
558 }
559
560 }
561 }
562
563 if (badPen) {
564 // Switch pen_v and n (again!)
565 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
566
567
568 // Find the three sides (no top or bottom) of the solid created by
569 // the penetrated triangle.
570 // The dVector4 "plane" structures contain the following information:
571 // [0]-[2]: The normal of the face, pointing INWARDS (i.e.
572 // the inverse normal
573 // [3]: The distance between the face and the center of the
574 // solid, along the normal
575 dVector4 SolidPlanes[3];
576 dVector3 tmp1;
577
578 dVector3 sn;
579 for (int j=0; j<3; j++) {
580 e1[j] = col_v[1][j] - col_v[0][j];
581 e2[j] = col_v[0][j] - col_v[2][j];
582 e3[j] = col_v[2][j] - col_v[1][j];
583 }
584
585 // side 1
586 CROSS(sn, e1, n);
587 dNormalize3(sn);
588 SMULT( SolidPlanes[0], sn, -1.0 );
589
590 ADD(tmp1, col_v[0], col_v[1]);
591 SMULT(tmp1, tmp1, 0.5); // center of edge
592 // distance from center to edge along normal
593 SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]);
594
595
596 // side 2
597 CROSS(sn, e2, n);
598 dNormalize3(sn);
599 SMULT( SolidPlanes[1], sn, -1.0 );
600
601 ADD(tmp1, col_v[0], col_v[2]);
602 SMULT(tmp1, tmp1, 0.5); // center of edge
603 // distance from center to edge along normal
604 SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]);
605
606
607 // side 3
608 CROSS(sn, e3, n);
609 dNormalize3(sn);
610 SMULT( SolidPlanes[2], sn, -1.0 );
611
612 ADD(tmp1, col_v[2], col_v[1]);
613 SMULT(tmp1, tmp1, 0.5); // center of edge
614 // distance from center to edge along normal
615 SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]);
616
617 FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri);
618
619 for (int j=0; j<secondClippedTri.Count; j++) {
620 secondClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults
621
622 DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], n);
623
624 if (pen_v == v1) {
625 dMultiply1(orig_pos, InvMatrix1, secondClippedTri.Points[j], 4, 4, 1);
626 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
627 for (int k=0; k<3; k++) {
628 secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos1[k]) - elt2[k];
629 }
630 }
631 else {
632 dMultiply1(orig_pos, InvMatrix2, secondClippedTri.Points[j], 4, 4, 1);
633 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
634 for (int k=0; k<3; k++) {
635 secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos2[k]) - elt1[k];
636 }
637 }
638
639
640 if (dp >= 0.0) {
641 contact_elt_length = dFabs(dDOT(secondClippedElt[j],n));
642
643 depth = dp;
644 if (depth == 0.0)
645 depth = dMin(DISTANCE_EPSILON, contact_elt_length);
646
647 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
648 depth = contact_elt_length;
649
650 if (depth <= contact_elt_length) {
651 // Add a contact
652 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
653 secondClippedTri.Points[j], n, depth, OutTriCount);
654 badPen = false;
655
656 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
657 break;
658 }
659 }
660 }
661
662
663 }
664 }
665
666
667
668 /////////////////
669 // All conventional tests have failed at this point, so now we deal with
670 // cases on a more "heuristic" basis
671 //
672
673 if (badPen) {
674 // Switch pen_v and n (for the fourth time, so they're
675 // what my original guess said they were)
676 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
677
678 if (dFabs(dDOT(n1, n2)) < REAL(0.01)) {
679 // If we reach this point, we have (close to) perpindicular
680 // faces, either resting on each other or sliding in a
681 // direction orthogonal to both surface normals.
682 if (LENGTH(elt_sum) < DISTANCE_EPSILON) {
683 depth = dFabs(dDOT(n, elt_sum));
684
685 if (depth > REAL(1e-12)) {
686 dNormalize3(n);
687 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
688 CoplanarPt, n, depth, OutTriCount);
689 badPen = false;
690 }
691 else {
692 // If the two faces are (nearly) perfectly at rest with
693 // respect to each other, then we ignore the contact,
694 // allowing the objects to slip a little in the hopes
695 // that next frame, they'll give us something to work
696 // with.
697 badPen = false;
698 }
699 }
700 else {
701 // The faces are perpindicular, but moving significantly
702 // This can be sliding, or an unusual edge-straddling
703 // penetration.
704 dVector3 cn;
705
706 CROSS(cn, n1, n2);
707 dNormalize3(cn);
708 SET(n, cn);
709
710 // The shallowest ineterpenetration of the faces
711 // is the depth
712 dVector3 ContactPt;
713 dVector3 dvTmp;
714 dReal rTmp;
715 depth = dInfinity;
716 for (int j=0; j<3; j++) {
717 for (int k=0; k<3; k++) {
718 SUB(dvTmp, col_v[k], pen_v[j]);
719
720 rTmp = dDOT(dvTmp, n);
721 if ( dFabs(rTmp) < dFabs(depth) ) {
722 depth = rTmp;
723 SET( ContactPt, pen_v[j] );
724 contact_elt_length = dFabs(dDOT(pen_elt[j], n));
725 }
726 }
727 }
728 if (depth < 0.0) {
729 SMULT(n, n, -1.0);
730 depth *= -1.0;
731 }
732
733 if ((depth > 0.0) && (depth <= contact_elt_length)) {
734 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
735 ContactPt, n, depth, OutTriCount);
736 badPen = false;
737 }
738
739 }
740 }
741 }
742
743
744 if (badPen) {
745 // Use as the normal the direction of travel, rather than any particular
746 // face normal
747 //
748 dVector3 esn;
749
750 if (pen_v == v1) {
751 SMULT(esn, elt_sum, -1.0);
752 }
753 else {
754 SET(esn, elt_sum);
755 }
756 dNormalize3(esn);
757
758
759 // The shallowest ineterpenetration of the faces
760 // is the depth
761 dVector3 ContactPt;
762 depth = dInfinity;
763 for (int j=0; j<3; j++) {
764 for (int k=0; k<3; k++) {
765 DEPTH(dp, col_v[k], pen_v[j], esn);
766 if ( (ExamineContactPoint(col_v, esn, pen_v[j])) &&
767 ( dFabs(dp) < dFabs(depth)) ) {
768 depth = dp;
769 SET( ContactPt, pen_v[j] );
770 contact_elt_length = dFabs(dDOT(pen_elt[j], esn));
771 }
772 }
773 }
774
775 if ((depth > 0.0) && (depth <= contact_elt_length)) {
776 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
777 ContactPt, esn, depth, OutTriCount);
778 badPen = false;
779 }
780 }
781
782
783 if (badPen) {
784 // If the direction of motion is perpindicular to both normals
785 if ( (dFabs(dDOT(n1, elt_sum)) < REAL(0.01)) && (dFabs(dDOT(n2, elt_sum)) < REAL(0.01)) ) {
786 dVector3 esn;
787 if (pen_v == v1) {
788 SMULT(esn, elt_sum, -1.0);
789 }
790 else {
791 SET(esn, elt_sum);
792 }
793
794 dNormalize3(esn);
795
796
797 // Look at the clipped points again, checking them against this
798 // new normal
799 for (int j=0; j<firstClippedTri.Count; j++) {
800 DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], esn);
801
802 if (dp >= 0.0) {
803 contact_elt_length = dFabs(dDOT(firstClippedElt[j], esn));
804
805 depth = dp;
806 //if (depth == 0.0)
807 //depth = dMin(DISTANCE_EPSILON, contact_elt_length);
808
809 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
810 depth = contact_elt_length;
811
812 if (depth <= contact_elt_length) {
813 // Add a contact
814 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
815 firstClippedTri.Points[j], esn, depth, OutTriCount);
816 badPen = false;
817
818 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
819 break;
820 }
821 }
822 }
823 }
824
825 if (badPen) {
826 // If this test failed, try it with the second set of clipped faces
827 for (int j=0; j<secondClippedTri.Count; j++) {
828 DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], esn);
829
830 if (dp >= 0.0) {
831 contact_elt_length = dFabs(dDOT(secondClippedElt[j], esn));
832
833 depth = dp;
834 //if (depth == 0.0)
835 //depth = dMin(DISTANCE_EPSILON, contact_elt_length);
836
837 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
838 depth = contact_elt_length;
839
840 if (depth <= contact_elt_length) {
841 // Add a contact
842 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
843 secondClippedTri.Points[j], esn, depth, OutTriCount);
844 badPen = false;
845
846 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
847 break;
848 }
849 }
850 }
851 }
852 }
853 }
854 }
855
856
857
858 if (badPen) {
859 // if we have very little motion, we're dealing with resting contact
860 // and shouldn't reference the ELTs at all
861 //
862 if (LENGTH(elt_sum) < VELOCITY_EPSILON) {
863
864 // instead of a "contact_elt_length" threshhold, we'll use an
865 // arbitrary, small one
866 for (int j=0; j<3; j++) {
867 DEPTH(dp, CoplanarPt, pen_v[j], n);
868
869 if (dp == 0.0)
870 dp = TINY_PENETRATION;
871
872 if ( (dp > 0.0) && (dp <= SMALL_ELT)) {
873 // Add a contact
874 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
875 pen_v[j], n, dp, OutTriCount);
876 badPen = false;
877
878 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
879 break;
880 }
881 }
882 }
883
884
885 if (badPen) {
886 // try the other normal
887 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
888
889 for (int j=0; j<3; j++) {
890 DEPTH(dp, CoplanarPt, pen_v[j], n);
891
892 if (dp == 0.0)
893 dp = TINY_PENETRATION;
894
895 if ( (dp > 0.0) && (dp <= SMALL_ELT)) {
896 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
897 pen_v[j], n, dp, OutTriCount);
898 badPen = false;
899
900 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
901 break;
902 }
903 }
904 }
905 }
906
907
908
909 }
910 }
911
912 if (badPen) {
913 // find the nearest existing contact, and replicate it's
914 // normal and depth
915 //
916 dContactGeom* Contact;
917 dVector3 pos_diff;
918 dReal min_dist, dist;
919
920 min_dist = dInfinity;
921 depth = 0.0;
922 for (int j=0; j<OutTriCount; j++) {
923 Contact = SAFECONTACT(Flags, Contacts, j, Stride);
924
925 SUB(pos_diff, Contact->pos, CoplanarPt);
926
927 dist = dDOT(pos_diff, pos_diff);
928 if (dist < min_dist) {
929 min_dist = dist;
930 depth = Contact->depth;
931 SMULT(ContactNormal, Contact->normal, -1.0);
932 }
933 }
934
935 if (depth > 0.0) {
936 // Add a tiny contact at the coplanar point
937 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
938 CoplanarPt, ContactNormal, depth, OutTriCount);
939 badPen = false;
940 }
941 }
942
943
944 if (badPen) {
945 // Add a tiny contact at the coplanar point
946 if (-dDOT(elt_sum, n1) > -dDOT(elt_sum, n2)) {
947 SET(ContactNormal, n1);
948 }
949 else {
950 SET(ContactNormal, n2);
951 }
952
953 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2,
954 CoplanarPt, ContactNormal, TINY_PENETRATION, OutTriCount);
955 badPen = false;
956 }
957
958
959 } // not coplanar (main loop)
960 } // TriTriIntersectWithIsectLine
961
962 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
963 break;
964 }
965 }
966
967 // Free memory
968 delete[] firstClippedElt;
969 delete[] secondClippedElt;
970
971 // Return the number of contacts
972 return OutTriCount;
973 }
974 }
975 }
976
977
978 // There was some kind of failure during the Collide call or
979 // there are no faces overlapping
980 return 0;
981}
982
983
984
985static void
986GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data)
987{
988 dVector3 Out[3];
989
990 FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out);
991
992 for (int i = 0; i < 3; i++)
993 triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]);
994}
995
996
997//
998//
999//
1000#define B11 B[0]
1001#define B12 B[1]
1002#define B13 B[2]
1003#define B14 B[3]
1004#define B21 B[4]
1005#define B22 B[5]
1006#define B23 B[6]
1007#define B24 B[7]
1008#define B31 B[8]
1009#define B32 B[9]
1010#define B33 B[10]
1011#define B34 B[11]
1012#define B41 B[12]
1013#define B42 B[13]
1014#define B43 B[14]
1015#define B44 B[15]
1016
1017#define Binv11 Binv[0]
1018#define Binv12 Binv[1]
1019#define Binv13 Binv[2]
1020#define Binv14 Binv[3]
1021#define Binv21 Binv[4]
1022#define Binv22 Binv[5]
1023#define Binv23 Binv[6]
1024#define Binv24 Binv[7]
1025#define Binv31 Binv[8]
1026#define Binv32 Binv[9]
1027#define Binv33 Binv[10]
1028#define Binv34 Binv[11]
1029#define Binv41 Binv[12]
1030#define Binv42 Binv[13]
1031#define Binv43 Binv[14]
1032#define Binv44 Binv[15]
1033
1034inline void
1035dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B)
1036{
1037 B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0];
1038 B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1];
1039 B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2];
1040
1041 B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0;
1042}
1043
1044
1045static void
1046dInvertMatrix4( dMatrix4& B, dMatrix4& Binv )
1047{
1048 dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43)
1049 -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42)
1050 +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42)
1051 +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41)
1052 -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41)
1053 +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41);
1054
1055 dAASSERT (det != 0.0);
1056
1057 det = 1.0 / det;
1058
1059 Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32)));
1060 Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12)));
1061 Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22)));
1062 Binv14 = 0.0f;
1063 Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33)));
1064 Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13)));
1065 Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23)));
1066 Binv24 = 0.0f;
1067 Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31)));
1068 Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11)));
1069 Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21)));
1070 Binv34 = 0.0f;
1071 Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42)));
1072 Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42)));
1073 Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22)));
1074 Binv44 = 1.0f;
1075}
1076
1077
1078
1079/////////////////////////////////////////////////
1080//
1081// Triangle/Triangle intersection utilities
1082//
1083// From the article "A Fast Triangle-Triangle Intersection Test",
1084// Journal of Graphics Tools, 2(2), 1997
1085//
1086// Some of this functionality is duplicated in OPCODE (see
1087// OPC_TriTriOverlap.h) but we have replicated it here so we don't
1088// have to mess with the internals of OPCODE, as well as so we can
1089// further optimize some of the functions.
1090//
1091// This version computes the line of intersection as well (if they
1092// are not coplanar):
1093// int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3],
1094// dReal U0[3],dReal U1[3],dReal U2[3],
1095// int *coplanar,
1096// dReal isectpt1[3],dReal isectpt2[3]);
1097//
1098// parameters: vertices of triangle 1: V0,V1,V2
1099// vertices of triangle 2: U0,U1,U2
1100//
1101// result : returns 1 if the triangles intersect, otherwise 0
1102// "coplanar" returns whether the tris are coplanar
1103// isectpt1, isectpt2 are the endpoints of the line of
1104// intersection
1105//
1106
1107
1108
1109/* if USE_EPSILON_TEST is true then we do a check:
1110 if |dv|<EPSILON then dv=0.0;
1111 else no check is done (which is less robust)
1112*/
1113#define USE_EPSILON_TEST TRUE
1114#define EPSILON REAL(0.000001)
1115
1116
1117/* sort so that a<=b */
1118#define SORT(a,b) \
1119 if(a>b) \
1120 { \
1121 dReal c; \
1122 c=a; \
1123 a=b; \
1124 b=c; \
1125 }
1126
1127#define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
1128 isect0=VV0+(VV1-VV0)*D0/(D0-D1); \
1129 isect1=VV0+(VV2-VV0)*D0/(D0-D2);
1130
1131
1132#define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
1133 if(D0D1>0.0f) \
1134 { \
1135 /* here we know that D0D2<=0.0 */ \
1136 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
1137 ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
1138 } \
1139 else if(D0D2>0.0f) \
1140 { \
1141 /* here we know that d0d1<=0.0 */ \
1142 ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
1143 } \
1144 else if(D1*D2>0.0f || D0!=0.0f) \
1145 { \
1146 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
1147 ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \
1148 } \
1149 else if(D1!=0.0f) \
1150 { \
1151 ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
1152 } \
1153 else if(D2!=0.0f) \
1154 { \
1155 ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
1156 } \
1157 else \
1158 { \
1159 /* triangles are coplanar */ \
1160 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
1161 }
1162
1163
1164
1165/* this edge to edge test is based on Franlin Antonio's gem:
1166 "Faster Line Segment Intersection", in Graphics Gems III,
1167 pp. 199-202 */
1168#define EDGE_EDGE_TEST(V0,U0,U1) \
1169 Bx=U0[i0]-U1[i0]; \
1170 By=U0[i1]-U1[i1]; \
1171 Cx=V0[i0]-U0[i0]; \
1172 Cy=V0[i1]-U0[i1]; \
1173 f=Ay*Bx-Ax*By; \
1174 d=By*Cx-Bx*Cy; \
1175 if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
1176 { \
1177 e=Ax*Cy-Ay*Cx; \
1178 if(f>0) \
1179 { \
1180 if(e>=0 && e<=f) return 1; \
1181 } \
1182 else \
1183 { \
1184 if(e<=0 && e>=f) return 1; \
1185 } \
1186 }
1187
1188#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
1189{ \
1190 dReal Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
1191 Ax=V1[i0]-V0[i0]; \
1192 Ay=V1[i1]-V0[i1]; \
1193 /* test edge U0,U1 against V0,V1 */ \
1194 EDGE_EDGE_TEST(V0,U0,U1); \
1195 /* test edge U1,U2 against V0,V1 */ \
1196 EDGE_EDGE_TEST(V0,U1,U2); \
1197 /* test edge U2,U1 against V0,V1 */ \
1198 EDGE_EDGE_TEST(V0,U2,U0); \
1199}
1200
1201#define POINT_IN_TRI(V0,U0,U1,U2) \
1202{ \
1203 dReal a,b,c,d0,d1,d2; \
1204 /* is T1 completly inside T2? */ \
1205 /* check if V0 is inside tri(U0,U1,U2) */ \
1206 a=U1[i1]-U0[i1]; \
1207 b=-(U1[i0]-U0[i0]); \
1208 c=-a*U0[i0]-b*U0[i1]; \
1209 d0=a*V0[i0]+b*V0[i1]+c; \
1210 \
1211 a=U2[i1]-U1[i1]; \
1212 b=-(U2[i0]-U1[i0]); \
1213 c=-a*U1[i0]-b*U1[i1]; \
1214 d1=a*V0[i0]+b*V0[i1]+c; \
1215 \
1216 a=U0[i1]-U2[i1]; \
1217 b=-(U0[i0]-U2[i0]); \
1218 c=-a*U2[i0]-b*U2[i1]; \
1219 d2=a*V0[i0]+b*V0[i1]+c; \
1220 if(d0*d1>0.0) \
1221 { \
1222 if(d0*d2>0.0) return 1; \
1223 } \
1224}
1225
1226int coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3],
1227 dReal U0[3],dReal U1[3],dReal U2[3])
1228{
1229 dReal A[3];
1230 short i0,i1;
1231 /* first project onto an axis-aligned plane, that maximizes the area */
1232 /* of the triangles, compute indices: i0,i1. */
1233 A[0]= dFabs(N[0]);
1234 A[1]= dFabs(N[1]);
1235 A[2]= dFabs(N[2]);
1236 if(A[0]>A[1])
1237 {
1238 if(A[0]>A[2])
1239 {
1240 i0=1; /* A[0] is greatest */
1241 i1=2;
1242 }
1243 else
1244 {
1245 i0=0; /* A[2] is greatest */
1246 i1=1;
1247 }
1248 }
1249 else /* A[0]<=A[1] */
1250 {
1251 if(A[2]>A[1])
1252 {
1253 i0=0; /* A[2] is greatest */
1254 i1=1;
1255 }
1256 else
1257 {
1258 i0=0; /* A[1] is greatest */
1259 i1=2;
1260 }
1261 }
1262
1263 /* test all edges of triangle 1 against the edges of triangle 2 */
1264 EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
1265 EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
1266 EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
1267
1268 /* finally, test if tri1 is totally contained in tri2 or vice versa */
1269 POINT_IN_TRI(V0,U0,U1,U2);
1270 POINT_IN_TRI(U0,V0,V1,V2);
1271
1272 return 0;
1273}
1274
1275
1276
1277#define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
1278{ \
1279 if(D0D1>0.0f) \
1280 { \
1281 /* here we know that D0D2<=0.0 */ \
1282 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
1283 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
1284 } \
1285 else if(D0D2>0.0f)\
1286 { \
1287 /* here we know that d0d1<=0.0 */ \
1288 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
1289 } \
1290 else if(D1*D2>0.0f || D0!=0.0f) \
1291 { \
1292 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
1293 A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
1294 } \
1295 else if(D1!=0.0f) \
1296 { \
1297 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
1298 } \
1299 else if(D2!=0.0f) \
1300 { \
1301 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
1302 } \
1303 else \
1304 { \
1305 /* triangles are coplanar */ \
1306 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
1307 } \
1308}
1309
1310
1311
1312
1313/* sort so that a<=b */
1314#define SORT2(a,b,smallest) \
1315 if(a>b) \
1316 { \
1317 dReal c; \
1318 c=a; \
1319 a=b; \
1320 b=c; \
1321 smallest=1; \
1322 } \
1323 else smallest=0;
1324
1325
1326inline void isect2(dReal VTX0[3],dReal VTX1[3],dReal VTX2[3],dReal VV0,dReal VV1,dReal VV2,
1327 dReal D0,dReal D1,dReal D2,dReal *isect0,dReal *isect1,dReal isectpoint0[3],dReal isectpoint1[3])
1328{
1329 dReal tmp=D0/(D0-D1);
1330 dReal diff[3];
1331 *isect0=VV0+(VV1-VV0)*tmp;
1332 SUB(diff,VTX1,VTX0);
1333 MULT(diff,diff,tmp);
1334 ADD(isectpoint0,diff,VTX0);
1335 tmp=D0/(D0-D2);
1336 *isect1=VV0+(VV2-VV0)*tmp;
1337 SUB(diff,VTX2,VTX0);
1338 MULT(diff,diff,tmp);
1339 ADD(isectpoint1,VTX0,diff);
1340}
1341
1342
1343#if 0
1344#define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \
1345 tmp=D0/(D0-D1); \
1346 isect0=VV0+(VV1-VV0)*tmp; \
1347 SUB(diff,VTX1,VTX0); \
1348 MULT(diff,diff,tmp); \
1349 ADD(isectpoint0,diff,VTX0); \
1350 tmp=D0/(D0-D2);
1351/* isect1=VV0+(VV2-VV0)*tmp; \ */
1352/* SUB(diff,VTX2,VTX0); \ */
1353/* MULT(diff,diff,tmp); \ */
1354/* ADD(isectpoint1,VTX0,diff); */
1355#endif
1356
1357inline int compute_intervals_isectline(dReal VERT0[3],dReal VERT1[3],dReal VERT2[3],
1358 dReal VV0,dReal VV1,dReal VV2,dReal D0,dReal D1,dReal D2,
1359 dReal D0D1,dReal D0D2,dReal *isect0,dReal *isect1,
1360 dReal isectpoint0[3],dReal isectpoint1[3])
1361{
1362 if(D0D1>0.0f)
1363 {
1364 /* here we know that D0D2<=0.0 */
1365 /* that is D0, D1 are on the same side, D2 on the other or on the plane */
1366 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
1367 }
1368 else if(D0D2>0.0f)
1369 {
1370 /* here we know that d0d1<=0.0 */
1371 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
1372 }
1373 else if(D1*D2>0.0f || D0!=0.0f)
1374 {
1375 /* here we know that d0d1<=0.0 or that D0!=0.0 */
1376 isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1);
1377 }
1378 else if(D1!=0.0f)
1379 {
1380 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
1381 }
1382 else if(D2!=0.0f)
1383 {
1384 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
1385 }
1386 else
1387 {
1388 /* triangles are coplanar */
1389 return 1;
1390 }
1391 return 0;
1392}
1393
1394#define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \
1395 if(D0D1>0.0f) \
1396 { \
1397 /* here we know that D0D2<=0.0 */ \
1398 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
1399 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
1400 }
1401#if 0
1402 else if(D0D2>0.0f) \
1403 { \
1404 /* here we know that d0d1<=0.0 */ \
1405 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
1406 } \
1407 else if(D1*D2>0.0f || D0!=0.0f) \
1408 { \
1409 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
1410 isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
1411 } \
1412 else if(D1!=0.0f) \
1413 { \
1414 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
1415 } \
1416 else if(D2!=0.0f) \
1417 { \
1418 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
1419 } \
1420 else \
1421 { \
1422 /* triangles are coplanar */ \
1423 coplanar=1; \
1424 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
1425 }
1426#endif
1427
1428
1429
1430static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3],
1431 dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar,
1432 dReal isectpt1[3],dReal isectpt2[3])
1433{
1434 dReal E1[3],E2[3];
1435 dReal N1[3],N2[3],d1,d2;
1436 dReal du0,du1,du2,dv0,dv1,dv2;
1437 dReal D[3];
1438 dReal isect1[2], isect2[2];
1439 dReal isectpointA1[3],isectpointA2[3];
1440 dReal isectpointB1[3],isectpointB2[3];
1441 dReal du0du1,du0du2,dv0dv1,dv0dv2;
1442 short index;
1443 dReal vp0,vp1,vp2;
1444 dReal up0,up1,up2;
1445 dReal b,c,max;
1446 int smallest1,smallest2;
1447
1448 /* compute plane equation of triangle(V0,V1,V2) */
1449 SUB(E1,V1,V0);
1450 SUB(E2,V2,V0);
1451 CROSS(N1,E1,E2);
1452 d1=-DOT(N1,V0);
1453 /* plane equation 1: N1.X+d1=0 */
1454
1455 /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
1456 du0=DOT(N1,U0)+d1;
1457 du1=DOT(N1,U1)+d1;
1458 du2=DOT(N1,U2)+d1;
1459
1460 /* coplanarity robustness check */
1461#if USE_EPSILON_TEST==TRUE
1462 if(dFabs(du0)<EPSILON) du0=0.0;
1463 if(dFabs(du1)<EPSILON) du1=0.0;
1464 if(dFabs(du2)<EPSILON) du2=0.0;
1465#endif
1466 du0du1=du0*du1;
1467 du0du2=du0*du2;
1468
1469 if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
1470 return 0; /* no intersection occurs */
1471
1472 /* compute plane of triangle (U0,U1,U2) */
1473 SUB(E1,U1,U0);
1474 SUB(E2,U2,U0);
1475 CROSS(N2,E1,E2);
1476 d2=-DOT(N2,U0);
1477 /* plane equation 2: N2.X+d2=0 */
1478
1479 /* put V0,V1,V2 into plane equation 2 */
1480 dv0=DOT(N2,V0)+d2;
1481 dv1=DOT(N2,V1)+d2;
1482 dv2=DOT(N2,V2)+d2;
1483
1484#if USE_EPSILON_TEST==TRUE
1485 if(dFabs(dv0)<EPSILON) dv0=0.0;
1486 if(dFabs(dv1)<EPSILON) dv1=0.0;
1487 if(dFabs(dv2)<EPSILON) dv2=0.0;
1488#endif
1489
1490 dv0dv1=dv0*dv1;
1491 dv0dv2=dv0*dv2;
1492
1493 if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
1494 return 0; /* no intersection occurs */
1495
1496 /* compute direction of intersection line */
1497 CROSS(D,N1,N2);
1498
1499 /* compute and index to the largest component of D */
1500 max= dFabs(D[0]);
1501 index=0;
1502 b= dFabs(D[1]);
1503 c= dFabs(D[2]);
1504 if(b>max) max=b,index=1;
1505 if(c>max) max=c,index=2;
1506
1507 /* this is the simplified projection onto L*/
1508 vp0=V0[index];
1509 vp1=V1[index];
1510 vp2=V2[index];
1511
1512 up0=U0[index];
1513 up1=U1[index];
1514 up2=U2[index];
1515
1516 /* compute interval for triangle 1 */
1517 *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
1518 dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
1519 if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);
1520
1521
1522 /* compute interval for triangle 2 */
1523 compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2,
1524 du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);
1525
1526 SORT2(isect1[0],isect1[1],smallest1);
1527 SORT2(isect2[0],isect2[1],smallest2);
1528
1529 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
1530
1531 /* at this point, we know that the triangles intersect */
1532
1533 if(isect2[0]<isect1[0])
1534 {
1535 if(smallest1==0) { SET(isectpt1,isectpointA1); }
1536 else { SET(isectpt1,isectpointA2); }
1537
1538 if(isect2[1]<isect1[1])
1539 {
1540 if(smallest2==0) { SET(isectpt2,isectpointB2); }
1541 else { SET(isectpt2,isectpointB1); }
1542 }
1543 else
1544 {
1545 if(smallest1==0) { SET(isectpt2,isectpointA2); }
1546 else { SET(isectpt2,isectpointA1); }
1547 }
1548 }
1549 else
1550 {
1551 if(smallest2==0) { SET(isectpt1,isectpointB1); }
1552 else { SET(isectpt1,isectpointB2); }
1553
1554 if(isect2[1]>isect1[1])
1555 {
1556 if(smallest1==0) { SET(isectpt2,isectpointA2); }
1557 else { SET(isectpt2,isectpointA1); }
1558 }
1559 else
1560 {
1561 if(smallest2==0) { SET(isectpt2,isectpointB2); }
1562 else { SET(isectpt2,isectpointB1); }
1563 }
1564 }
1565 return 1;
1566}
1567
1568
1569
1570
1571
1572// Find the intersectiojn point between a coplanar line segement,
1573// defined by X1 and X2, and a ray defined by X3 and direction N.
1574//
1575// This forumla for this calculation is:
1576// (c x b) . (a x b)
1577// Q = x1 + a -------------------
1578// | a x b | ^2
1579//
1580// where a = x2 - x1
1581// b = x4 - x3
1582// c = x3 - x1
1583// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt
1584// and x4 is (CoplanarPt - n)
1585static int
1586IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n,
1587 dVector3 out_pt)
1588{
1589 dVector3 a, b, c, x4;
1590
1591 ADD(x4, x3, n); // x4 = x3 + n
1592
1593 SUB(a, x2, x1); // a = x2 - x1
1594 SUB(b, x4, x3);
1595 SUB(c, x3, x1);
1596
1597 dVector3 tmp1, tmp2;
1598 CROSS(tmp1, c, b);
1599 CROSS(tmp2, a, b);
1600
1601 dReal num, denom;
1602 num = dDOT(tmp1, tmp2);
1603 denom = LENGTH( tmp2 );
1604
1605 dReal s;
1606 s = num /(denom*denom);
1607
1608 for (int i=0; i<3; i++)
1609 out_pt[i] = x1[i] + a[i]*s;
1610
1611 // Test if this intersection is "behind" x3, w.r.t. n
1612 SUB(a, x3, out_pt);
1613 if (dDOT(a, n) > 0.0)
1614 return 0;
1615
1616 // Test if this intersection point is outside the edge limits,
1617 // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside
1618 // else outside
1619 SUB(a, out_pt, x1);
1620 SUB(b, out_pt, x2);
1621 if (dDOT(a,b) < 0.0)
1622 return 1;
1623 else
1624 return 0;
1625}
1626
1627
1628// FindTriSolidIntersection - Clips the input trinagle TRI with the
1629// sides of a convex bounding solid, described by PLANES, returning
1630// the (convex) clipped polygon in CLIPPEDPOLYGON
1631//
1632static bool
1633FindTriSolidIntrsection(const dVector3 Tri[3],
1634 const dVector4 Planes[6], int numSides,
1635 LineContactSet& ClippedPolygon )
1636{
1637 // Set up the LineContactSet structure
1638 for (int k=0; k<3; k++) {
1639 SET(ClippedPolygon.Points[k], Tri[k]);
1640 }
1641 ClippedPolygon.Count = 3;
1642
1643 // Clip wrt the sides
1644 for ( int i = 0; i < numSides; i++ )
1645 ClipConvexPolygonAgainstPlane( Planes[i], Planes[i][3], ClippedPolygon );
1646
1647 return (ClippedPolygon.Count > 0);
1648}
1649
1650
1651
1652
1653// ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by
1654// CONTACTS, with a plane (described by N and C). Note: the input
1655// vertices are assumed to be in counterclockwise order.
1656//
1657// This code is taken from The Nebula Device:
1658// http://nebuladevice.sourceforge.net/cgi-bin/twiki/view/Nebula/WebHome
1659// and is licensed under the following license:
1660// http://nebuladevice.sourceforge.net/doc/source/license.txt
1661//
1662static void
1663ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C,
1664 LineContactSet& Contacts )
1665{
1666 // test on which side of line are the vertices
1667 int Positive = 0, Negative = 0, PIndex = -1;
1668 int Quantity = Contacts.Count;
1669
1670 dReal Test[8];
1671 for ( int i = 0; i < Contacts.Count; i++ ) {
1672 // An epsilon is used here because it is possible for the dot product
1673 // and C to be exactly equal to each other (in theory), but differ
1674 // slightly because of floating point problems. Thus, add a little
1675 // to the test number to push actually equal numbers over the edge
1676 // towards the positive. This should probably be somehow a relative
1677 // tolerance, and I don't think multiplying by the constant is the best
1678 // way to do this.
1679 Test[i] = dDOT(N, Contacts.Points[i]) - C + dFabs(C)*REAL(1e-08);
1680
1681 if (Test[i] >= REAL(0.0)) {
1682 Positive++;
1683 if (PIndex < 0) {
1684 PIndex = i;
1685 }
1686 }
1687 else Negative++;
1688 }
1689
1690 if (Positive > 0) {
1691 if (Negative > 0) {
1692 // plane transversely intersects polygon
1693 dVector3 CV[8];
1694 int CQuantity = 0, Cur, Prv;
1695 dReal T;
1696
1697 if (PIndex > 0) {
1698 // first clip vertex on line
1699 Cur = PIndex;
1700 Prv = Cur - 1;
1701 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1702 CV[CQuantity][0] = Contacts.Points[Cur][0]
1703 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1704 CV[CQuantity][1] = Contacts.Points[Cur][1]
1705 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1706 CV[CQuantity][2] = Contacts.Points[Cur][2]
1707 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1708 CV[CQuantity][3] = Contacts.Points[Cur][3]
1709 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1710 CQuantity++;
1711
1712 // vertices on positive side of line
1713 while (Cur < Quantity && Test[Cur] >= REAL(0.0)) {
1714 CV[CQuantity][0] = Contacts.Points[Cur][0];
1715 CV[CQuantity][1] = Contacts.Points[Cur][1];
1716 CV[CQuantity][2] = Contacts.Points[Cur][2];
1717 CV[CQuantity][3] = Contacts.Points[Cur][3];
1718 CQuantity++;
1719 Cur++;
1720 }
1721
1722 // last clip vertex on line
1723 if (Cur < Quantity) {
1724 Prv = Cur - 1;
1725 }
1726 else {
1727 Cur = 0;
1728 Prv = Quantity - 1;
1729 }
1730
1731 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1732 CV[CQuantity][0] = Contacts.Points[Cur][0]
1733 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1734 CV[CQuantity][1] = Contacts.Points[Cur][1]
1735 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1736 CV[CQuantity][2] = Contacts.Points[Cur][2]
1737 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1738 CV[CQuantity][3] = Contacts.Points[Cur][3]
1739 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1740 CQuantity++;
1741 }
1742 else {
1743 // iPIndex is 0
1744 // vertices on positive side of line
1745 Cur = 0;
1746 while (Cur < Quantity && Test[Cur] >= REAL(0.0)) {
1747 CV[CQuantity][0] = Contacts.Points[Cur][0];
1748 CV[CQuantity][1] = Contacts.Points[Cur][1];
1749 CV[CQuantity][2] = Contacts.Points[Cur][2];
1750 CV[CQuantity][3] = Contacts.Points[Cur][3];
1751 CQuantity++;
1752 Cur++;
1753 }
1754
1755 // last clip vertex on line
1756 Prv = Cur - 1;
1757 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1758 CV[CQuantity][0] = Contacts.Points[Cur][0]
1759 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1760 CV[CQuantity][1] = Contacts.Points[Cur][1]
1761 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1762 CV[CQuantity][2] = Contacts.Points[Cur][2]
1763 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1764 CV[CQuantity][3] = Contacts.Points[Cur][3]
1765 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1766 CQuantity++;
1767
1768 // skip vertices on negative side
1769 while (Cur < Quantity && Test[Cur] < REAL(0.0)) {
1770 Cur++;
1771 }
1772
1773 // first clip vertex on line
1774 if (Cur < Quantity) {
1775 Prv = Cur - 1;
1776 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1777 CV[CQuantity][0] = Contacts.Points[Cur][0]
1778 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1779 CV[CQuantity][1] = Contacts.Points[Cur][1]
1780 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1781 CV[CQuantity][2] = Contacts.Points[Cur][2]
1782 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1783 CV[CQuantity][3] = Contacts.Points[Cur][3]
1784 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1785 CQuantity++;
1786
1787 // vertices on positive side of line
1788 while (Cur < Quantity && Test[Cur] >= REAL(0.0)) {
1789 CV[CQuantity][0] = Contacts.Points[Cur][0];
1790 CV[CQuantity][1] = Contacts.Points[Cur][1];
1791 CV[CQuantity][2] = Contacts.Points[Cur][2];
1792 CV[CQuantity][3] = Contacts.Points[Cur][3];
1793 CQuantity++;
1794 Cur++;
1795 }
1796 }
1797 else {
1798 // iCur = 0
1799 Prv = Quantity - 1;
1800 T = Test[0] / (Test[0] - Test[Prv]);
1801 CV[CQuantity][0] = Contacts.Points[0][0]
1802 + T * (Contacts.Points[Prv][0] - Contacts.Points[0][0]);
1803 CV[CQuantity][1] = Contacts.Points[0][1]
1804 + T * (Contacts.Points[Prv][1] - Contacts.Points[0][1]);
1805 CV[CQuantity][2] = Contacts.Points[0][2]
1806 + T * (Contacts.Points[Prv][2] - Contacts.Points[0][2]);
1807 CV[CQuantity][3] = Contacts.Points[0][3]
1808 + T * (Contacts.Points[Prv][3] - Contacts.Points[0][3]);
1809 CQuantity++;
1810 }
1811 }
1812 Quantity = CQuantity;
1813 memcpy( Contacts.Points, CV, CQuantity * sizeof(dVector3) );
1814 }
1815 // else polygon fully on positive side of plane, nothing to do
1816 Contacts.Count = Quantity;
1817 }
1818 else {
1819 Contacts.Count = 0; // This should not happen, but for safety
1820 }
1821
1822}
1823
1824
1825
1826// Determine if a potential collision point is
1827//
1828//
1829static int
1830ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point)
1831{
1832 // Cast a ray from in_point, along the collison normal. Does it intersect the
1833 // collision face.
1834 dReal t, u, v;
1835
1836 if (!RayTriangleIntersect(in_point, in_n, v_col[0], v_col[1], v_col[2],
1837 &t, &u, &v))
1838 return 0;
1839 else
1840 return 1;
1841}
1842
1843
1844
1845// RayTriangleIntersect - If an intersection is found, t contains the
1846// distance along the ray (dir) and u/v contain u/v coordinates into
1847// the triangle. Returns 0 if no hit is found
1848// From "Real-Time Rendering," page 305
1849//
1850static int
1851RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
1852 const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
1853 dReal *t,dReal *u,dReal *v)
1854
1855{
1856 dReal edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
1857 dReal det,inv_det;
1858
1859 // find vectors for two edges sharing vert0
1860 SUB(edge1, vert1, vert0);
1861 SUB(edge2, vert2, vert0);
1862
1863 // begin calculating determinant - also used to calculate U parameter
1864 CROSS(pvec, dir, edge2);
1865
1866 // if determinant is near zero, ray lies in plane of triangle
1867 det = DOT(edge1, pvec);
1868
1869 if ((det > REAL(-0.001)) && (det < REAL(0.001)))
1870 return 0;
1871 inv_det = 1.0 / det;
1872
1873 // calculate distance from vert0 to ray origin
1874 SUB(tvec, orig, vert0);
1875
1876 // calculate U parameter and test bounds
1877 *u = DOT(tvec, pvec) * inv_det;
1878 if ((*u < 0.0) || (*u > 1.0))
1879 return 0;
1880
1881 // prepare to test V parameter
1882 CROSS(qvec, tvec, edge1);
1883
1884 // calculate V parameter and test bounds
1885 *v = DOT(dir, qvec) * inv_det;
1886 if ((*v < 0.0) || ((*u + *v) > 1.0))
1887 return 0;
1888
1889 // calculate t, ray intersects triangle
1890 *t = DOT(edge2, qvec) * inv_det;
1891
1892 return 1;
1893}
1894
1895
1896
1897static bool
1898SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt,
1899 dVector3 in_n, dVector3* in_col_v, dReal &out_depth)
1900{
1901 dReal dp = 0.0;
1902 dReal contact_elt_length;
1903
1904 DEPTH(dp, in_CoplanarPt, in_v, in_n);
1905
1906 if (dp >= 0.0) {
1907 // if the penetration depth (calculated above) is more than
1908 // the contact point's ELT, then we've chosen the wrong face
1909 // and should switch faces
1910 contact_elt_length = dFabs(dDOT(in_elt, in_n));
1911
1912 if (dp == 0.0)
1913 dp = dMin(DISTANCE_EPSILON, contact_elt_length);
1914
1915 if ((contact_elt_length < SMALL_ELT) && (dp < EXPANDED_ELT_THRESH))
1916 dp = contact_elt_length;
1917
1918 if ( (dp > 0.0) && (dp <= contact_elt_length)) {
1919 // Add a contact
1920
1921 if ( ExamineContactPoint(in_col_v, in_n, in_v) ) {
1922 out_depth = dp;
1923 return true;
1924 }
1925 }
1926 }
1927
1928 return false;
1929}
1930
1931
1932
1933
1934// Generate a "unique" contact. A unique contact has a unique
1935// position or normal. If the potential contact has the same
1936// position and normal as an existing contact, but a larger
1937// penetration depth, this new depth is used instead
1938//
1939static void
1940GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
1941 dxTriMesh* in_TriMesh1, dxTriMesh* in_TriMesh2,
1942 const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
1943 int& OutTriCount)
1944{
1945 /*
1946 NOTE by Oleh_Derevenko:
1947 This function is called after maximal number of contacts has already been
1948 collected because it has a side effect of replacing penetration depth of
1949 existing contact with larger penetration depth of another matching normal contact.
1950 If this logic is not necessary any more, you can bail out on reach of contact
1951 number maximum immediately in dCollideTTL(). You will also need to correct
1952 conditional statements after invocations of GenerateContact() in dCollideTTL().
1953 */
1954 dIASSERT(in_Depth >= 0.0);
1955 //if (in_Depth < 0.0) -- the function is always called with depth >= 0
1956 // return;
1957
1958 do
1959 {
1960 dContactGeom* Contact;
1961 dVector3 diff;
1962
1963 if (!(in_Flags & CONTACTS_UNIMPORTANT))
1964 {
1965 bool duplicate = false;
1966
1967 for (int i=0; i<OutTriCount; i++)
1968 {
1969 Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
1970
1971 // same position?
1972 SUB(diff, in_ContactPos, Contact->pos);
1973 if (dDOT(diff, diff) < dEpsilon)
1974 {
1975 // same normal?
1976 if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon))
1977 {
1978 if (in_Depth > Contact->depth) {
1979 Contact->depth = in_Depth;
1980 SMULT( Contact->normal, in_Normal, -1.0);
1981 Contact->normal[3] = 0.0;
1982 }
1983 duplicate = true;
1984 /*
1985 NOTE by Oleh_Derevenko:
1986 There may be a case when two normals are close to each other but no duplicate
1987 while third normal is detected to be duplicate for both of them.
1988 This is the only reason I can think of, there is no "break" statement.
1989 Perhaps author considered it to be logical that the third normal would
1990 replace the depth in both of initial contacts.
1991 However, I consider it a questionable practice which should not
1992 be applied without deep understanding of underlaying physics.
1993 Even more, is this situation with close normal triplet acceptable at all?
1994 Should not be two initial contacts reduced to one (replaced with the latter)?
1995 If you know the answers for these questions, you may want to change this code.
1996 See the same statement in GenerateContact() of collision_trimesh_box.cpp
1997 */
1998 }
1999 }
2000 }
2001
2002 if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
2003 {
2004 break;
2005 }
2006 }
2007 else
2008 {
2009 dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
2010 }
2011
2012 // Add a new contact
2013 Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
2014
2015 SET( Contact->pos, in_ContactPos );
2016 Contact->pos[3] = 0.0;
2017
2018 SMULT( Contact->normal, in_Normal, -1.0);
2019 Contact->normal[3] = 0.0;
2020
2021 Contact->depth = in_Depth;
2022
2023 Contact->g1 = in_TriMesh1;
2024 Contact->g2 = in_TriMesh2;
2025
2026 OutTriCount++;
2027 }
2028 while (false);
2029}
2030
2031#endif // dTRIMESH_OPCODE
2032#endif // !dTRIMESH_USE_NEW_TRIMESH_TRIMESH_COLLIDER
2033#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp
new file mode 100644
index 0000000..c4af41c
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp
@@ -0,0 +1,1304 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// OPCODE TriMesh/TriMesh collision code
24// Written at 2006-10-28 by Francisco León (http://gimpact.sourceforge.net)
25
26#ifdef _MSC_VER
27#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28#endif
29
30#include <ode/collision.h>
31#include <ode/matrix.h>
32#include <ode/rotation.h>
33#include <ode/odemath.h>
34
35// New Implementation
36#if dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER
37
38#if dTRIMESH_ENABLED
39
40#include "collision_util.h"
41#define TRIMESH_INTERNAL
42#include "collision_trimesh_internal.h"
43
44#if dTRIMESH_OPCODE
45
46#define SMALL_ELT REAL(2.5e-4)
47#define EXPANDED_ELT_THRESH REAL(1.0e-3)
48#define DISTANCE_EPSILON REAL(1.0e-8)
49#define VELOCITY_EPSILON REAL(1.0e-5)
50#define TINY_PENETRATION REAL(5.0e-6)
51
52struct LineContactSet
53{
54 enum
55 {
56 MAX_POINTS = 8
57 };
58
59 dVector3 Points[MAX_POINTS];
60 int Count;
61};
62
63
64static void GetTriangleGeometryCallback(udword, VertexPointers&, udword);
65inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
66static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
67static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3);
68static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );
69static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
70 const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
71 dReal *t,dReal *u,dReal *v);
72
73
74///returns the penetration depth
75static dReal MostDeepPoints(
76 LineContactSet & points,
77 const dVector3 plane_normal,
78 dReal plane_dist,
79 LineContactSet & deep_points);
80///returns the penetration depth
81static dReal FindMostDeepPointsInTetra(
82 LineContactSet contact_points,
83 const dVector3 sourcetri[3],///triangle which contains contact_points
84 const dVector3 tetra[4],
85 const dVector4 tetraplanes[4],
86 dVector3 separating_normal,
87 LineContactSet deep_points);
88
89static bool ClipTriByTetra(const dVector3 tri[3],
90 const dVector3 tetra[4],
91 LineContactSet& Contacts);
92static bool TriTriContacts(const dVector3 tr1[3],
93 const dVector3 tr2[3],
94 dxGeom* g1, dxGeom* g2, int Flags,
95 dContactGeom* Contacts, int Stride,
96 int &contactcount);
97
98
99/* some math macros */
100#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
101 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
102 dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; }
103
104#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
105
106#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }
107
108#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }
109
110#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; }
111
112#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }
113
114#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; }
115
116#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]; }
117
118#define LENGTH(x) ((dReal) 1.0f/InvSqrt(dDOT(x, x)))
119
120#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];
121
122inline const dReal dMin(const dReal x, const dReal y)
123{
124 return x < y ? x : y;
125}
126
127
128inline void
129SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
130 dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
131 dVector3 n, dVector3 n1, dVector3 n2)
132{
133 if (pen_v == v1) {
134 pen_v = v2;
135 pen_elt = elt_f2;
136 col_v = v1;
137 SET(n, n1);
138 }
139 else {
140 pen_v = v1;
141 pen_elt = elt_f1;
142 col_v = v2;
143 SET(n, n2);
144 }
145}
146
147///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE///////////////////////////////
148////* Written by Francisco León (http://gimpact.sourceforge.net) *///
149#define CONTACT_DIFF_EPSILON REAL(0.00001)
150#if defined(dDOUBLE)
151#define CONTACT_NORMAL_ZERO REAL(0.0000001)
152#else // if defined(dSINGLE)
153#define CONTACT_NORMAL_ZERO REAL(0.00001)
154#endif
155#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0)
156#define dSQRT3 REAL(1.7320508075688773)
157
158struct CONTACT_KEY
159{
160 dContactGeom * m_contact;
161 unsigned int m_key;
162};
163
164#define MAXCONTACT_X_NODE 4
165struct CONTACT_KEY_HASH_NODE
166{
167 CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE];
168 int m_keycount;
169};
170
171#define CONTACTS_HASHSIZE 256
172CONTACT_KEY_HASH_NODE g_hashcontactset[CONTACTS_HASHSIZE];
173
174
175
176void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact)
177{
178 key.m_contact = contact;
179
180 unsigned int hash=0;
181
182 int i = 0;
183
184 while (true)
185 {
186 dReal coord = contact->pos[i];
187 coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT);
188
189 unsigned int hash_input = ((unsigned int *)&coord)[0];
190 if (sizeof(dReal) / sizeof(unsigned int) != 1)
191 {
192 dIASSERT(sizeof(dReal) / sizeof(unsigned int) == 2);
193 hash_input ^= ((unsigned int *)&coord)[1];
194 }
195
196 hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 );
197 hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 );
198 hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 );
199 hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 );
200
201 if (++i == 3)
202 {
203 break;
204 }
205
206 hash = (hash << 11) | (hash >> 21);
207 }
208
209 key.m_key = hash;
210}
211
212
213static inline unsigned int MakeContactIndex(unsigned int key)
214{
215 dIASSERT(CONTACTS_HASHSIZE == 256);
216
217 unsigned int index = key ^ (key >> 16);
218 index = (index ^ (index >> 8)) & 0xFF;
219 return index;
220}
221
222dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)
223{
224 for(int i=0;i<node->m_keycount;i++)
225 {
226 if(node->m_keyarray[i].m_key == contactkey->m_key)
227 {
228 dContactGeom *contactfound = node->m_keyarray[i].m_contact;
229 if (dDISTANCE(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/)
230 {
231 return contactfound;
232 }
233 }
234 }
235
236 if (node->m_keycount < MAXCONTACT_X_NODE)
237 {
238 node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact;
239 node->m_keyarray[node->m_keycount].m_key = contactkey->m_key;
240 node->m_keycount++;
241 }
242 else
243 {
244 dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled");
245 }
246
247 return contactkey->m_contact;
248}
249
250void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node)
251{
252 dIASSERT(node->m_keycount > 0);
253
254 if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact)
255 {
256 node->m_keycount -= 1;
257 }
258 else
259 {
260 dIASSERT(node->m_keycount == MAXCONTACT_X_NODE);
261 }
262}
263
264void RemoveArbitraryContactFromNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node)
265{
266 dIASSERT(node->m_keycount > 0);
267
268 int keyindex, lastkeyindex = node->m_keycount - 1;
269
270 // Do not check the last contact
271 for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
272 {
273 if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
274 {
275 node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex];
276 break;
277 }
278 }
279
280 dIASSERT(keyindex < lastkeyindex ||
281 node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
282
283 node->m_keycount = lastkeyindex;
284}
285
286void UpdateArbitraryContactInNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node,
287 dContactGeom *pwithcontact)
288{
289 dIASSERT(node->m_keycount > 0);
290
291 int keyindex, lastkeyindex = node->m_keycount - 1;
292
293 // Do not check the last contact
294 for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
295 {
296 if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
297 {
298 break;
299 }
300 }
301
302 dIASSERT(keyindex < lastkeyindex ||
303 node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
304
305 node->m_keyarray[keyindex].m_contact = pwithcontact;
306}
307
308void ClearContactSet()
309{
310 memset(g_hashcontactset,0,sizeof(CONTACT_KEY_HASH_NODE)*CONTACTS_HASHSIZE);
311}
312
313//return true if found
314dContactGeom *InsertContactInSet(const CONTACT_KEY &newkey)
315{
316 unsigned int index = MakeContactIndex(newkey.m_key);
317
318 return AddContactToNode(&newkey, &g_hashcontactset[index]);
319}
320
321void RemoveNewContactFromSet(const CONTACT_KEY &contactkey)
322{
323 unsigned int index = MakeContactIndex(contactkey.m_key);
324
325 RemoveNewContactFromNode(&contactkey, &g_hashcontactset[index]);
326}
327
328void RemoveArbitraryContactFromSet(const CONTACT_KEY &contactkey)
329{
330 unsigned int index = MakeContactIndex(contactkey.m_key);
331
332 RemoveArbitraryContactFromNode(&contactkey, &g_hashcontactset[index]);
333}
334
335void UpdateArbitraryContactInSet(const CONTACT_KEY &contactkey,
336 dContactGeom *pwithcontact)
337{
338 unsigned int index = MakeContactIndex(contactkey.m_key);
339
340 UpdateArbitraryContactInNode(&contactkey, &g_hashcontactset[index], pwithcontact);
341}
342
343bool AllocNewContact(
344 const dVector3 newpoint, dContactGeom *& out_pcontact,
345 int Flags, dContactGeom* Contacts,
346 int Stride, int &contactcount)
347{
348 bool allocated_new = false;
349
350 dContactGeom dLocalContact;
351
352 dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ?
353 SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact;
354
355 pcontact->pos[0] = newpoint[0];
356 pcontact->pos[1] = newpoint[1];
357 pcontact->pos[2] = newpoint[2];
358 pcontact->pos[3] = 1.0f;
359
360 CONTACT_KEY newkey;
361 UpdateContactKey(newkey, pcontact);
362
363 dContactGeom *pcontactfound = InsertContactInSet(newkey);
364 if (pcontactfound == pcontact)
365 {
366 if (pcontactfound != &dLocalContact)
367 {
368 contactcount++;
369 }
370 else
371 {
372 RemoveNewContactFromSet(newkey);
373 pcontactfound = NULL;
374 }
375
376 allocated_new = true;
377 }
378
379 out_pcontact = pcontactfound;
380 return allocated_new;
381}
382
383void FreeExistingContact(dContactGeom *pcontact,
384 int Flags, dContactGeom *Contacts,
385 int Stride, int &contactcount)
386{
387 CONTACT_KEY contactKey;
388 UpdateContactKey(contactKey, pcontact);
389
390 RemoveArbitraryContactFromSet(contactKey);
391
392 int lastContactIndex = contactcount - 1;
393 dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride);
394
395 if (pcontact != plastContact)
396 {
397 *pcontact = *plastContact;
398
399 CONTACT_KEY lastContactKey;
400 UpdateContactKey(lastContactKey, plastContact);
401
402 UpdateArbitraryContactInSet(lastContactKey, pcontact);
403 }
404
405 contactcount = lastContactIndex;
406}
407
408
409dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2,
410 const dVector3 point,
411 dVector3 normal,
412 dReal depth,
413 int Flags,
414 dContactGeom* Contacts, int Stride,
415 int &contactcount)
416{
417 dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code
418
419 dContactGeom * pcontact;
420
421 if (!AllocNewContact(point, pcontact, Flags, Contacts, Stride, contactcount))
422 {
423 const dReal depthDifference = depth - pcontact->depth;
424
425 if (depthDifference > CONTACT_DIFF_EPSILON)
426 {
427 pcontact->normal[0] = normal[0];
428 pcontact->normal[1] = normal[1];
429 pcontact->normal[2] = normal[2];
430 pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
431 pcontact->depth = depth;
432
433 pcontact->g1 = g1;
434 pcontact->g2 = g2;
435 }
436 else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average
437 {
438 if(pcontact->g1 == g2)
439 {
440 MULT(normal,normal, REAL(-1.0));
441 }
442
443 const dReal oldLen = pcontact->normal[3];
444 COMBO(pcontact->normal, normal, oldLen, pcontact->normal);
445
446 const dReal len = LENGTH(pcontact->normal);
447 if (len > CONTACT_NORMAL_ZERO)
448 {
449 MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len);
450 pcontact->normal[3] = len;
451 }
452 else
453 {
454 FreeExistingContact(pcontact, Flags, Contacts, Stride, contactcount);
455 }
456 }
457 }
458 // Contact can be not available if buffer is full
459 else if (pcontact)
460 {
461 pcontact->normal[0] = normal[0];
462 pcontact->normal[1] = normal[1];
463 pcontact->normal[2] = normal[2];
464 pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
465 pcontact->depth = depth;
466 pcontact->g1 = g1;
467 pcontact->g2 = g2;
468 }
469
470 return pcontact;
471}
472
473////////////////////////////////////////////////////////////////////////////////////////////
474
475
476
477
478
479int
480dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
481{
482 dIASSERT (Stride >= (int)sizeof(dContactGeom));
483 dIASSERT (g1->type == dTriMeshClass);
484 dIASSERT (g2->type == dTriMeshClass);
485 dIASSERT ((Flags & NUMC_MASK) >= 1);
486
487 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
488 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
489
490 dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals;
491 dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals;
492
493 const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1);
494 // TLRotation1 = column-major order
495 const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1);
496
497 const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2);
498 // TLRotation2 = column-major order
499 const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2);
500
501 AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider;
502
503
504 static BVTCache ColCache;
505 ColCache.Model0 = &TriMesh1->Data->BVTree;
506 ColCache.Model1 = &TriMesh2->Data->BVTree;
507
508 ////Prepare contact list
509 ClearContactSet();
510
511 // Collision query
512 Matrix4x4 amatrix, bmatrix;
513 BOOL IsOk = Collider.Collide(ColCache,
514 &MakeMatrix(TLPosition1, TLRotation1, amatrix),
515 &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
516
517
518 // Make "double" versions of these matrices, if appropriate
519 dMatrix4 A, B;
520 dMakeMatrix4(TLPosition1, TLRotation1, A);
521 dMakeMatrix4(TLPosition2, TLRotation2, B);
522
523
524
525
526 if (IsOk) {
527 // Get collision status => if true, objects overlap
528 if ( Collider.GetContactStatus() ) {
529 // Number of colliding pairs and list of pairs
530 int TriCount = Collider.GetNbPairs();
531 const Pair* CollidingPairs = Collider.GetPairs();
532
533 if (TriCount > 0) {
534 // step through the pairs, adding contacts
535 int id1, id2;
536 int OutTriCount = 0;
537 dVector3 v1[3], v2[3];
538
539 // only do these expensive inversions once
540 /*dMatrix4 InvMatrix1, InvMatrix2;
541 dInvertMatrix4(A, InvMatrix1);
542 dInvertMatrix4(B, InvMatrix2);*/
543
544
545 for (int i = 0; i < TriCount; i++)
546 {
547 id1 = CollidingPairs[i].id0;
548 id2 = CollidingPairs[i].id1;
549
550 // grab the colliding triangles
551 FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1);
552 FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2);
553 // Since we'll be doing matrix transformations, we need to
554 // make sure that all vertices have four elements
555 for (int j=0; j<3; j++) {
556 v1[j][3] = 1.0;
557 v2[j][3] = 1.0;
558 }
559
560 TriTriContacts(v1,v2,
561 g1, g2, Flags,
562 Contacts,Stride,OutTriCount);
563
564 // Continue loop even after contacts are full
565 // as existing contacts' normals/depths might be updated
566 // Break only if contacts are not important
567 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
568 {
569 break;
570 }
571 }
572
573 // Return the number of contacts
574 return OutTriCount;
575
576 }
577 }
578 }
579
580
581 // There was some kind of failure during the Collide call or
582 // there are no faces overlapping
583 return 0;
584}
585
586
587
588static void
589GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data)
590{
591 dVector3 Out[3];
592
593 FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out);
594
595 for (int i = 0; i < 3; i++)
596 triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]);
597}
598
599
600//
601//
602//
603#define B11 B[0]
604#define B12 B[1]
605#define B13 B[2]
606#define B14 B[3]
607#define B21 B[4]
608#define B22 B[5]
609#define B23 B[6]
610#define B24 B[7]
611#define B31 B[8]
612#define B32 B[9]
613#define B33 B[10]
614#define B34 B[11]
615#define B41 B[12]
616#define B42 B[13]
617#define B43 B[14]
618#define B44 B[15]
619
620#define Binv11 Binv[0]
621#define Binv12 Binv[1]
622#define Binv13 Binv[2]
623#define Binv14 Binv[3]
624#define Binv21 Binv[4]
625#define Binv22 Binv[5]
626#define Binv23 Binv[6]
627#define Binv24 Binv[7]
628#define Binv31 Binv[8]
629#define Binv32 Binv[9]
630#define Binv33 Binv[10]
631#define Binv34 Binv[11]
632#define Binv41 Binv[12]
633#define Binv42 Binv[13]
634#define Binv43 Binv[14]
635#define Binv44 Binv[15]
636
637inline void
638dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B)
639{
640 B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0];
641 B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1];
642 B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2];
643
644 B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0;
645}
646
647
648static void
649dInvertMatrix4( dMatrix4& B, dMatrix4& Binv )
650{
651 dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43)
652 -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42)
653 +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42)
654 +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41)
655 -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41)
656 +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41);
657
658 dAASSERT (det != 0.0);
659
660 det = 1.0 / det;
661
662 Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32)));
663 Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12)));
664 Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22)));
665 Binv14 = 0.0f;
666 Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33)));
667 Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13)));
668 Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23)));
669 Binv24 = 0.0f;
670 Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31)));
671 Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11)));
672 Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21)));
673 Binv34 = 0.0f;
674 Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42)));
675 Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42)));
676 Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22)));
677 Binv44 = 1.0f;
678}
679
680
681
682// Find the intersectiojn point between a coplanar line segement,
683// defined by X1 and X2, and a ray defined by X3 and direction N.
684//
685// This forumla for this calculation is:
686// (c x b) . (a x b)
687// Q = x1 + a -------------------
688// | a x b | ^2
689//
690// where a = x2 - x1
691// b = x4 - x3
692// c = x3 - x1
693// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt
694// and x4 is (CoplanarPt - n)
695static int
696IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n,
697 dVector3 out_pt)
698{
699 dVector3 a, b, c, x4;
700
701 ADD(x4, x3, n); // x4 = x3 + n
702
703 SUB(a, x2, x1); // a = x2 - x1
704 SUB(b, x4, x3);
705 SUB(c, x3, x1);
706
707 dVector3 tmp1, tmp2;
708 CROSS(tmp1, c, b);
709 CROSS(tmp2, a, b);
710
711 dReal num, denom;
712 num = dDOT(tmp1, tmp2);
713 denom = LENGTH( tmp2 );
714
715 dReal s;
716 s = num /(denom*denom);
717
718 for (int i=0; i<3; i++)
719 out_pt[i] = x1[i] + a[i]*s;
720
721 // Test if this intersection is "behind" x3, w.r.t. n
722 SUB(a, x3, out_pt);
723 if (dDOT(a, n) > 0.0)
724 return 0;
725
726 // Test if this intersection point is outside the edge limits,
727 // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside
728 // else outside
729 SUB(a, out_pt, x1);
730 SUB(b, out_pt, x2);
731 if (dDOT(a,b) < 0.0)
732 return 1;
733 else
734 return 0;
735}
736
737
738
739void PlaneClipSegment( const dVector3 s1, const dVector3 s2,
740 const dVector3 N, dReal C, dVector3 clipped)
741{
742 dReal dis1,dis2;
743 dis1 = DOT(s1,N)-C;
744 SUB(clipped,s2,s1);
745 dis2 = DOT(clipped,N);
746 MULT(clipped,clipped,-dis1/dis2);
747 ADD(clipped,clipped,s1);
748 clipped[3] = 1.0f;
749}
750
751/* ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by
752 CONTACTS, with a plane (described by N and C distance from origin).
753 Note: the input vertices are assumed to be in invcounterclockwise order.
754 changed by Francisco Leon (http://gimpact.sourceforge.net) */
755static void
756ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C,
757 LineContactSet& Contacts )
758{
759 int i, vi, prevclassif=32000, classif;
760 /*
761 classif 0 : back, 1 : front
762 */
763
764 dReal d;
765 dVector3 clipped[8];
766 int clippedcount =0;
767
768 if(Contacts.Count==0)
769 {
770 return;
771 }
772 for(i=0;i<=Contacts.Count;i++)
773 {
774 vi = i%Contacts.Count;
775
776 d = DOT(N,Contacts.Points[vi]) - C;
777 ////classify point
778 if(d>REAL(1.0e-8)) classif = 1;
779 else classif = 0;
780
781 if(classif == 0)//back
782 {
783 if(i>0)
784 {
785 if(prevclassif==1)///in front
786 {
787
788 ///add clipped point
789 if(clippedcount<8)
790 {
791 PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi],
792 N,C,clipped[clippedcount]);
793 clippedcount++;
794 }
795 }
796 }
797 ///add point
798 if(clippedcount<8&&i<Contacts.Count)
799 {
800 clipped[clippedcount][0] = Contacts.Points[vi][0];
801 clipped[clippedcount][1] = Contacts.Points[vi][1];
802 clipped[clippedcount][2] = Contacts.Points[vi][2];
803 clipped[clippedcount][3] = 1.0f;
804 clippedcount++;
805 }
806 }
807 else
808 {
809
810 if(i>0)
811 {
812 if(prevclassif==0)
813 {
814 ///add point
815 if(clippedcount<8)
816 {
817 PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi],
818 N,C,clipped[clippedcount]);
819 clippedcount++;
820 }
821 }
822 }
823 }
824
825 prevclassif = classif;
826 }
827
828 if(clippedcount==0)
829 {
830 Contacts.Count = 0;
831 return;
832 }
833 Contacts.Count = clippedcount;
834 memcpy( Contacts.Points, clipped, clippedcount * sizeof(dVector3) );
835 return;
836}
837
838
839bool BuildPlane(const dVector3 s0, const dVector3 s1,const dVector3 s2,
840 dVector3 Normal, dReal & Dist)
841{
842 dVector3 e0,e1;
843 SUB(e0,s1,s0);
844 SUB(e1,s2,s0);
845
846 CROSS(Normal,e0,e1);
847
848 if (!dSafeNormalize3(Normal))
849 {
850 return false;
851 }
852
853 Dist = DOT(Normal,s0);
854 return true;
855
856}
857
858bool BuildEdgesDir(const dVector3 s0, const dVector3 s1,
859 const dVector3 t0, const dVector3 t1,
860 dVector3 crossdir)
861{
862 dVector3 e0,e1;
863
864 SUB(e0,s1,s0);
865 SUB(e1,t1,t0);
866 CROSS(crossdir,e0,e1);
867
868 if (!dSafeNormalize3(crossdir))
869 {
870 return false;
871 }
872 return true;
873}
874
875
876
877bool BuildEdgePlane(
878 const dVector3 s0, const dVector3 s1,
879 const dVector3 normal,
880 dVector3 plane_normal,
881 dReal & plane_dist)
882{
883 dVector3 e0;
884
885 SUB(e0,s1,s0);
886 CROSS(plane_normal,e0,normal);
887 if (!dSafeNormalize3(plane_normal))
888 {
889 return false;
890 }
891 plane_dist = DOT(plane_normal,s0);
892 return true;
893}
894
895
896
897
898/*
899Positive penetration
900Negative number: they are separated
901*/
902dReal IntervalPenetration(dReal &vmin1,dReal &vmax1,
903 dReal &vmin2,dReal &vmax2)
904{
905 if(vmax1<=vmin2)
906 {
907 return -(vmin2-vmax1);
908 }
909 else
910 {
911 if(vmax2<=vmin1)
912 {
913 return -(vmin1-vmax2);
914 }
915 else
916 {
917 if(vmax1<=vmax2)
918 {
919 return vmax1-vmin2;
920 }
921
922 return vmax2-vmin1;
923 }
924
925 }
926 return 0;
927}
928
929void FindInterval(
930 const dVector3 * vertices, int verticecount,
931 dVector3 dir,dReal &vmin,dReal &vmax)
932{
933
934 dReal dist;
935 int i;
936 vmin = DOT(vertices[0],dir);
937 vmax = vmin;
938 for(i=1;i<verticecount;i++)
939 {
940 dist = DOT(vertices[i],dir);
941 if(vmin>dist) vmin=dist;
942 else if(vmax<dist) vmax=dist;
943 }
944}
945
946///returns the penetration depth
947dReal MostDeepPoints(
948 LineContactSet & points,
949 const dVector3 plane_normal,
950 dReal plane_dist,
951 LineContactSet & deep_points)
952{
953 int i;
954 int max_candidates[8];
955 dReal maxdeep=-dInfinity;
956 dReal dist;
957
958 deep_points.Count = 0;
959 for(i=0;i<points.Count;i++)
960 {
961 dist = DOT(plane_normal,points.Points[i]) - plane_dist;
962 dist *= -1.0f;
963 if(dist>maxdeep)
964 {
965 maxdeep = dist;
966 deep_points.Count=1;
967 max_candidates[deep_points.Count-1] = i;
968 }
969 else if(dist+REAL(0.000001)>=maxdeep)
970 {
971 deep_points.Count++;
972 max_candidates[deep_points.Count-1] = i;
973 }
974 }
975
976 for(i=0;i<deep_points.Count;i++)
977 {
978 SET(deep_points.Points[i],points.Points[max_candidates[i]]);
979 }
980 return maxdeep;
981
982}
983
984void ClipPointsByTri(
985 const dVector3 * points, int pointcount,
986 const dVector3 tri[3],
987 const dVector3 triplanenormal,
988 dReal triplanedist,
989 LineContactSet & clipped_points,
990 bool triplane_clips)
991{
992 ///build edges planes
993 int i;
994 dVector4 plane;
995
996 clipped_points.Count = pointcount;
997 memcpy(&clipped_points.Points[0],&points[0],pointcount*sizeof(dVector3));
998 for(i=0;i<3;i++)
999 {
1000 if (BuildEdgePlane(
1001 tri[i],tri[(i+1)%3],triplanenormal,
1002 plane,plane[3]))
1003 {
1004 ClipConvexPolygonAgainstPlane(
1005 plane,
1006 plane[3],
1007 clipped_points);
1008 }
1009 }
1010
1011 if(triplane_clips)
1012 {
1013 ClipConvexPolygonAgainstPlane(
1014 triplanenormal,
1015 triplanedist,
1016 clipped_points);
1017 }
1018}
1019
1020
1021///returns the penetration depth
1022dReal FindTriangleTriangleCollision(
1023 const dVector3 tri1[3],
1024 const dVector3 tri2[3],
1025 dVector3 separating_normal,
1026 LineContactSet & deep_points)
1027{
1028 dReal maxdeep=dInfinity;
1029 dReal dist;
1030 int mostdir=0,mostface = 0, currdir=0;
1031// dReal vmin1,vmax1,vmin2,vmax2;
1032// dVector3 crossdir, pt1,pt2;
1033 dVector4 tri1plane,tri2plane;
1034 separating_normal[3] = 0.0f;
1035 bool bl;
1036 LineContactSet clipped_points1,clipped_points2;
1037 LineContactSet deep_points1,deep_points2;
1038 ////find interval face1
1039
1040 bl = BuildPlane(tri1[0],tri1[1],tri1[2],
1041 tri1plane,tri1plane[3]);
1042 clipped_points1.Count = 0;
1043
1044 if(bl)
1045 {
1046 ClipPointsByTri(
1047 tri2, 3,
1048 tri1,
1049 tri1plane,
1050 tri1plane[3],
1051 clipped_points1,false);
1052
1053
1054
1055 maxdeep = MostDeepPoints(
1056 clipped_points1,
1057 tri1plane,
1058 tri1plane[3],
1059 deep_points1);
1060 SET(separating_normal,tri1plane);
1061
1062 }
1063 currdir++;
1064
1065 ////find interval face2
1066
1067 bl = BuildPlane(tri2[0],tri2[1],tri2[2],
1068 tri2plane,tri2plane[3]);
1069
1070
1071 clipped_points2.Count = 0;
1072 if(bl)
1073 {
1074 ClipPointsByTri(
1075 tri1, 3,
1076 tri2,
1077 tri2plane,
1078 tri2plane[3],
1079 clipped_points2,false);
1080
1081
1082
1083 dist = MostDeepPoints(
1084 clipped_points2,
1085 tri2plane,
1086 tri2plane[3],
1087 deep_points2);
1088
1089
1090
1091 if(dist<maxdeep)
1092 {
1093 maxdeep = dist;
1094 mostdir = currdir;
1095 mostface = 1;
1096 SET(separating_normal,tri2plane);
1097 }
1098 }
1099 currdir++;
1100
1101
1102 ///find edge edge distances
1103 ///test each edge plane
1104
1105 /*for(i=0;i<3;i++)
1106 {
1107
1108
1109 for(j=0;j<3;j++)
1110 {
1111
1112
1113 bl = BuildEdgesDir(
1114 tri1[i],tri1[(i+1)%3],
1115 tri2[j],tri2[(j+1)%3],
1116 crossdir);
1117
1118 ////find plane distance
1119
1120 if(bl)
1121 {
1122 FindInterval(tri1,3,crossdir,vmin1,vmax1);
1123 FindInterval(tri2,3,crossdir,vmin2,vmax2);
1124
1125 dist = IntervalPenetration(
1126 vmin1,
1127 vmax1,
1128 vmin2,
1129 vmax2);
1130 if(dist<maxdeep)
1131 {
1132 maxdeep = dist;
1133 mostdir = currdir;
1134 SET(separating_normal,crossdir);
1135 }
1136 }
1137 currdir++;
1138 }
1139 }*/
1140
1141
1142 ////check most dir for contacts
1143 if(mostdir==0)
1144 {
1145 ///find most deep points
1146 deep_points.Count = deep_points1.Count;
1147 memcpy(
1148 &deep_points.Points[0],
1149 &deep_points1.Points[0],
1150 deep_points1.Count*sizeof(dVector3));
1151
1152 ///invert normal for point to tri1
1153 MULT(separating_normal,separating_normal,-1.0f);
1154 }
1155 else if(mostdir==1)
1156 {
1157 deep_points.Count = deep_points2.Count;
1158 memcpy(
1159 &deep_points.Points[0],
1160 &deep_points2.Points[0],
1161 deep_points2.Count*sizeof(dVector3));
1162
1163 }
1164 /*else
1165 {///edge separation
1166 mostdir -= 2;
1167
1168 //edge 2
1169 j = mostdir%3;
1170 //edge 1
1171 i = mostdir/3;
1172
1173 ///find edge closest points
1174 dClosestLineSegmentPoints(
1175 tri1[i],tri1[(i+1)%3],
1176 tri2[j],tri2[(j+1)%3],
1177 pt1,pt2);
1178 ///find correct direction
1179
1180 SUB(crossdir,pt2,pt1);
1181
1182 vmin1 = LENGTH(crossdir);
1183 if(vmin1<REAL(0.000001))
1184 {
1185
1186 if(mostface==0)
1187 {
1188 vmin1 = DOT(separating_normal,tri1plane);
1189 if(vmin1>0.0)
1190 {
1191 MULT(separating_normal,separating_normal,-1.0f);
1192 deep_points.Count = 1;
1193 SET(deep_points.Points[0],pt2);
1194 }
1195 else
1196 {
1197 deep_points.Count = 1;
1198 SET(deep_points.Points[0],pt2);
1199 }
1200
1201 }
1202 else
1203 {
1204 vmin1 = DOT(separating_normal,tri2plane);
1205 if(vmin1<0.0)
1206 {
1207 MULT(separating_normal,separating_normal,-1.0f);
1208 deep_points.Count = 1;
1209 SET(deep_points.Points[0],pt2);
1210 }
1211 else
1212 {
1213 deep_points.Count = 1;
1214 SET(deep_points.Points[0],pt2);
1215 }
1216
1217 }
1218
1219
1220
1221
1222 }
1223 else
1224 {
1225 MULT(separating_normal,crossdir,1.0f/vmin1);
1226
1227 vmin1 = DOT(separating_normal,tri1plane);
1228 if(vmin1>0.0)
1229 {
1230 MULT(separating_normal,separating_normal,-1.0f);
1231 deep_points.Count = 1;
1232 SET(deep_points.Points[0],pt2);
1233 }
1234 else
1235 {
1236 deep_points.Count = 1;
1237 SET(deep_points.Points[0],pt2);
1238 }
1239
1240
1241 }
1242
1243
1244 }*/
1245 return maxdeep;
1246}
1247
1248
1249
1250///SUPPORT UP TO 8 CONTACTS
1251bool TriTriContacts(const dVector3 tr1[3],
1252 const dVector3 tr2[3],
1253 dxGeom* g1, dxGeom* g2, int Flags,
1254 dContactGeom* Contacts, int Stride,
1255 int &contactcount)
1256{
1257
1258
1259 dVector4 normal;
1260 dReal depth;
1261 ///Test Tri Vs Tri
1262// dContactGeom* pcontact;
1263 int ccount = 0;
1264 LineContactSet contactpoints;
1265 contactpoints.Count = 0;
1266
1267
1268
1269 ///find best direction
1270
1271 depth = FindTriangleTriangleCollision(
1272 tr1,
1273 tr2,
1274 normal,
1275 contactpoints);
1276
1277
1278
1279 if(depth<0.0f) return false;
1280
1281 ccount = 0;
1282 while (ccount<contactpoints.Count)
1283 {
1284 PushNewContact( g1, g2,
1285 contactpoints.Points[ccount],
1286 normal, depth, Flags,
1287 Contacts,Stride,contactcount);
1288
1289 // Continue loop even after contacts are full
1290 // as existing contacts' normals/depths might be updated
1291 // Break only if contacts are not important
1292 if ((contactcount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
1293 {
1294 break;
1295 }
1296
1297 ccount++;
1298 }
1299 return true;
1300}
1301
1302#endif // dTRIMESH_OPCODE
1303#endif // dTRIMESH_USE_NEW_TRIMESH_TRIMESH_COLLIDER
1304#endif // dTRIMESH_ENABLED
diff --git a/libraries/ode-0.9/ode/src/collision_util.cpp b/libraries/ode-0.9/ode/src/collision_util.cpp
new file mode 100644
index 0000000..460cc20
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_util.cpp
@@ -0,0 +1,612 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25some useful collision utility stuff. this includes some API utility
26functions that are defined in the public header files.
27
28*/
29
30#include <ode/common.h>
31#include <ode/collision.h>
32#include <ode/odemath.h>
33#include "collision_util.h"
34
35//****************************************************************************
36
37int dCollideSpheres (dVector3 p1, dReal r1,
38 dVector3 p2, dReal r2, dContactGeom *c)
39{
40 // printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n",
41 // d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
42
43 dReal d = dDISTANCE (p1,p2);
44 if (d > (r1 + r2)) return 0;
45 if (d <= 0) {
46 c->pos[0] = p1[0];
47 c->pos[1] = p1[1];
48 c->pos[2] = p1[2];
49 c->normal[0] = 1;
50 c->normal[1] = 0;
51 c->normal[2] = 0;
52 c->depth = r1 + r2;
53 }
54 else {
55 dReal d1 = dRecip (d);
56 c->normal[0] = (p1[0]-p2[0])*d1;
57 c->normal[1] = (p1[1]-p2[1])*d1;
58 c->normal[2] = (p1[2]-p2[2])*d1;
59 dReal k = REAL(0.5) * (r2 - r1 - d);
60 c->pos[0] = p1[0] + c->normal[0]*k;
61 c->pos[1] = p1[1] + c->normal[1]*k;
62 c->pos[2] = p1[2] + c->normal[2]*k;
63 c->depth = r1 + r2 - d;
64 }
65 return 1;
66}
67
68
69void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
70 const dVector3 pb, const dVector3 ub,
71 dReal *alpha, dReal *beta)
72{
73 dVector3 p;
74 p[0] = pb[0] - pa[0];
75 p[1] = pb[1] - pa[1];
76 p[2] = pb[2] - pa[2];
77 dReal uaub = dDOT(ua,ub);
78 dReal q1 = dDOT(ua,p);
79 dReal q2 = -dDOT(ub,p);
80 dReal d = 1-uaub*uaub;
81 if (d <= REAL(0.0001)) {
82 // @@@ this needs to be made more robust
83 *alpha = 0;
84 *beta = 0;
85 }
86 else {
87 d = dRecip(d);
88 *alpha = (q1 + uaub*q2)*d;
89 *beta = (uaub*q1 + q2)*d;
90 }
91}
92
93
94// given two line segments A and B with endpoints a1-a2 and b1-b2, return the
95// points on A and B that are closest to each other (in cp1 and cp2).
96// in the case of parallel lines where there are multiple solutions, a
97// solution involving the endpoint of at least one line will be returned.
98// this will work correctly for zero length lines, e.g. if a1==a2 and/or
99// b1==b2.
100//
101// the algorithm works by applying the voronoi clipping rule to the features
102// of the line segments. the three features of each line segment are the two
103// endpoints and the line between them. the voronoi clipping rule states that,
104// for feature X on line A and feature Y on line B, the closest points PA and
105// PB between X and Y are globally the closest points if PA is in V(Y) and
106// PB is in V(X), where V(X) is the voronoi region of X.
107
108void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
109 const dVector3 b1, const dVector3 b2,
110 dVector3 cp1, dVector3 cp2)
111{
112 dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n;
113 dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det;
114
115#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2];
116#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];
117
118 // check vertex-vertex features
119
120 SET3 (a1a2,a2,-,a1);
121 SET3 (b1b2,b2,-,b1);
122 SET3 (a1b1,b1,-,a1);
123 da1 = dDOT(a1a2,a1b1);
124 db1 = dDOT(b1b2,a1b1);
125 if (da1 <= 0 && db1 >= 0) {
126 SET2 (cp1,a1);
127 SET2 (cp2,b1);
128 return;
129 }
130
131 SET3 (a1b2,b2,-,a1);
132 da2 = dDOT(a1a2,a1b2);
133 db2 = dDOT(b1b2,a1b2);
134 if (da2 <= 0 && db2 <= 0) {
135 SET2 (cp1,a1);
136 SET2 (cp2,b2);
137 return;
138 }
139
140 SET3 (a2b1,b1,-,a2);
141 da3 = dDOT(a1a2,a2b1);
142 db3 = dDOT(b1b2,a2b1);
143 if (da3 >= 0 && db3 >= 0) {
144 SET2 (cp1,a2);
145 SET2 (cp2,b1);
146 return;
147 }
148
149 SET3 (a2b2,b2,-,a2);
150 da4 = dDOT(a1a2,a2b2);
151 db4 = dDOT(b1b2,a2b2);
152 if (da4 >= 0 && db4 <= 0) {
153 SET2 (cp1,a2);
154 SET2 (cp2,b2);
155 return;
156 }
157
158 // check edge-vertex features.
159 // if one or both of the lines has zero length, we will never get to here,
160 // so we do not have to worry about the following divisions by zero.
161
162 la = dDOT(a1a2,a1a2);
163 if (da1 >= 0 && da3 <= 0) {
164 k = da1 / la;
165 SET3 (n,a1b1,-,k*a1a2);
166 if (dDOT(b1b2,n) >= 0) {
167 SET3 (cp1,a1,+,k*a1a2);
168 SET2 (cp2,b1);
169 return;
170 }
171 }
172
173 if (da2 >= 0 && da4 <= 0) {
174 k = da2 / la;
175 SET3 (n,a1b2,-,k*a1a2);
176 if (dDOT(b1b2,n) <= 0) {
177 SET3 (cp1,a1,+,k*a1a2);
178 SET2 (cp2,b2);
179 return;
180 }
181 }
182
183 lb = dDOT(b1b2,b1b2);
184 if (db1 <= 0 && db2 >= 0) {
185 k = -db1 / lb;
186 SET3 (n,-a1b1,-,k*b1b2);
187 if (dDOT(a1a2,n) >= 0) {
188 SET2 (cp1,a1);
189 SET3 (cp2,b1,+,k*b1b2);
190 return;
191 }
192 }
193
194 if (db3 <= 0 && db4 >= 0) {
195 k = -db3 / lb;
196 SET3 (n,-a2b1,-,k*b1b2);
197 if (dDOT(a1a2,n) <= 0) {
198 SET2 (cp1,a2);
199 SET3 (cp2,b1,+,k*b1b2);
200 return;
201 }
202 }
203
204 // it must be edge-edge
205
206 k = dDOT(a1a2,b1b2);
207 det = la*lb - k*k;
208 if (det <= 0) {
209 // this should never happen, but just in case...
210 SET2(cp1,a1);
211 SET2(cp2,b1);
212 return;
213 }
214 det = dRecip (det);
215 dReal alpha = (lb*da1 - k*db1) * det;
216 dReal beta = ( k*da1 - la*db1) * det;
217 SET3 (cp1,a1,+,alpha*a1a2);
218 SET3 (cp2,b1,+,beta*b1b2);
219
220# undef SET2
221# undef SET3
222}
223
224
225// a simple root finding algorithm is used to find the value of 't' that
226// satisfies:
227// d|D(t)|^2/dt = 0
228// where:
229// |D(t)| = |p(t)-b(t)|
230// where p(t) is a point on the line parameterized by t:
231// p(t) = p1 + t*(p2-p1)
232// and b(t) is that same point clipped to the boundary of the box. in box-
233// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
234// each of which looks like this:
235//
236// t_lo /
237// ______/ -->t
238// / t_hi
239// /
240//
241// t_lo and t_hi are the t values where the line passes through the planes
242// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
243// in a piecewise fashion from t=0 to t=1, stopping at the point where
244// d|D(t)|^2/dt crosses from negative to positive.
245
246void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
247 const dVector3 c, const dMatrix3 R,
248 const dVector3 side,
249 dVector3 lret, dVector3 bret)
250{
251 int i;
252
253 // compute the start and delta of the line p1-p2 relative to the box.
254 // we will do all subsequent computations in this box-relative coordinate
255 // system. we have to do a translation and rotation for each point.
256 dVector3 tmp,s,v;
257 tmp[0] = p1[0] - c[0];
258 tmp[1] = p1[1] - c[1];
259 tmp[2] = p1[2] - c[2];
260 dMULTIPLY1_331 (s,R,tmp);
261 tmp[0] = p2[0] - p1[0];
262 tmp[1] = p2[1] - p1[1];
263 tmp[2] = p2[2] - p1[2];
264 dMULTIPLY1_331 (v,R,tmp);
265
266 // mirror the line so that v has all components >= 0
267 dVector3 sign;
268 for (i=0; i<3; i++) {
269 if (v[i] < 0) {
270 s[i] = -s[i];
271 v[i] = -v[i];
272 sign[i] = -1;
273 }
274 else sign[i] = 1;
275 }
276
277 // compute v^2
278 dVector3 v2;
279 v2[0] = v[0]*v[0];
280 v2[1] = v[1]*v[1];
281 v2[2] = v[2]*v[2];
282
283 // compute the half-sides of the box
284 dReal h[3];
285 h[0] = REAL(0.5) * side[0];
286 h[1] = REAL(0.5) * side[1];
287 h[2] = REAL(0.5) * side[2];
288
289 // region is -1,0,+1 depending on which side of the box planes each
290 // coordinate is on. tanchor is the next t value at which there is a
291 // transition, or the last one if there are no more.
292 int region[3];
293 dReal tanchor[3];
294
295 // Denormals are a problem, because we divide by v[i], and then
296 // multiply that by 0. Alas, infinity times 0 is infinity (!)
297 // We also use v2[i], which is v[i] squared. Here's how the epsilons
298 // are chosen:
299 // float epsilon = 1.175494e-038 (smallest non-denormal number)
300 // double epsilon = 2.225074e-308 (smallest non-denormal number)
301 // For single precision, choose an epsilon such that v[i] squared is
302 // not a denormal; this is for performance.
303 // For double precision, choose an epsilon such that v[i] is not a
304 // denormal; this is for correctness. (Jon Watte on mailinglist)
305
306#if defined( dSINGLE )
307 const dReal tanchor_eps = REAL(1e-19);
308#else
309 const dReal tanchor_eps = REAL(1e-307);
310#endif
311
312 // find the region and tanchor values for p1
313 for (i=0; i<3; i++) {
314 if (v[i] > tanchor_eps) {
315 if (s[i] < -h[i]) {
316 region[i] = -1;
317 tanchor[i] = (-h[i]-s[i])/v[i];
318 }
319 else {
320 region[i] = (s[i] > h[i]);
321 tanchor[i] = (h[i]-s[i])/v[i];
322 }
323 }
324 else {
325 region[i] = 0;
326 tanchor[i] = 2; // this will never be a valid tanchor
327 }
328 }
329
330 // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
331 dReal t=0;
332 dReal dd2dt = 0;
333 for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
334 if (dd2dt >= 0) goto got_answer;
335
336 do {
337 // find the point on the line that is at the next clip plane boundary
338 dReal next_t = 1;
339 for (i=0; i<3; i++) {
340 if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
341 next_t = tanchor[i];
342 }
343
344 // compute d|d|^2/dt for the next t
345 dReal next_dd2dt = 0;
346 for (i=0; i<3; i++) {
347 next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
348 }
349
350 // if the sign of d|d|^2/dt has changed, solution = the crossover point
351 if (next_dd2dt >= 0) {
352 dReal m = (next_dd2dt-dd2dt)/(next_t - t);
353 t -= dd2dt/m;
354 goto got_answer;
355 }
356
357 // advance to the next anchor point / region
358 for (i=0; i<3; i++) {
359 if (tanchor[i] == next_t) {
360 tanchor[i] = (h[i]-s[i])/v[i];
361 region[i]++;
362 }
363 }
364 t = next_t;
365 dd2dt = next_dd2dt;
366 }
367 while (t < 1);
368 t = 1;
369
370 got_answer:
371
372 // compute closest point on the line
373 for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1
374
375 // compute closest point on the box
376 for (i=0; i<3; i++) {
377 tmp[i] = sign[i] * (s[i] + t*v[i]);
378 if (tmp[i] < -h[i]) tmp[i] = -h[i];
379 else if (tmp[i] > h[i]) tmp[i] = h[i];
380 }
381 dMULTIPLY0_331 (s,R,tmp);
382 for (i=0; i<3; i++) bret[i] = s[i] + c[i];
383}
384
385
386// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect
387// or 0 if not.
388
389int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
390 const dVector3 side1, const dVector3 p2,
391 const dMatrix3 R2, const dVector3 side2)
392{
393 // two boxes are disjoint if (and only if) there is a separating axis
394 // perpendicular to a face from one box or perpendicular to an edge from
395 // either box. the following tests are derived from:
396 // "OBB Tree: A Hierarchical Structure for Rapid Interference Detection",
397 // S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
398
399 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
400 // Qij is abs(Rij)
401 dVector3 p,pp;
402 dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
403 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
404
405 // get vector from centers of box 1 to box 2, relative to box 1
406 p[0] = p2[0] - p1[0];
407 p[1] = p2[1] - p1[1];
408 p[2] = p2[2] - p1[2];
409 dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
410
411 // get side lengths / 2
412 A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
413 B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
414
415 // for the following tests, excluding computation of Rij, in the worst case,
416 // 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
417 // notation: R1=[u1 u2 u3], R2=[v1 v2 v3]
418
419 // separating axis = u1,u2,u3
420 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
421 Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
422 if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
423 R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
424 Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
425 if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
426 R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
427 Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
428 if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
429
430 // separating axis = v1,v2,v3
431 if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
432 if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
433 if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
434
435 // separating axis = u1 x (v1,v2,v3)
436 if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0;
437 if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0;
438 if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0;
439
440 // separating axis = u2 x (v1,v2,v3)
441 if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0;
442 if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0;
443 if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0;
444
445 // separating axis = u3 x (v1,v2,v3)
446 if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0;
447 if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0;
448 if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0;
449
450 return 1;
451}
452
453//****************************************************************************
454// other utility functions
455
456void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
457{
458 aabb[0] = -dInfinity;
459 aabb[1] = dInfinity;
460 aabb[2] = -dInfinity;
461 aabb[3] = dInfinity;
462 aabb[4] = -dInfinity;
463 aabb[5] = dInfinity;
464}
465
466
467//****************************************************************************
468// Helpers for Croteam's collider - by Nguyen Binh
469
470int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
471{
472 // calculate distance of edge points to plane
473 dReal fDistance0 = dPointPlaneDistance( vEpnt0 ,plPlane );
474 dReal fDistance1 = dPointPlaneDistance( vEpnt1 ,plPlane );
475
476 // if both points are behind the plane
477 if ( fDistance0 < 0 && fDistance1 < 0 )
478 {
479 // do nothing
480 return 0;
481 // if both points in front of the plane
482 }
483 else if ( fDistance0 > 0 && fDistance1 > 0 )
484 {
485 // accept them
486 return 1;
487 // if we have edge/plane intersection
488 } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0))
489 {
490
491 // find intersection point of edge and plane
492 dVector3 vIntersectionPoint;
493 vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
494 vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
495 vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
496
497 // clamp correct edge to intersection point
498 if ( fDistance0 < 0 )
499 {
500 dVector3Copy(vIntersectionPoint,vEpnt0);
501 } else
502 {
503 dVector3Copy(vIntersectionPoint,vEpnt1);
504 }
505 return 1;
506 }
507 return 1;
508}
509
510// clip polygon with plane and generate new polygon points
511void dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn,
512 dVector3 avArrayOut[], int &ctOut,
513 const dVector4 &plPlane )
514{
515 // start with no output points
516 ctOut = 0;
517
518 int i0 = ctIn-1;
519
520 // for each edge in input polygon
521 for (int i1=0; i1<ctIn; i0=i1, i1++) {
522
523
524 // calculate distance of edge points to plane
525 dReal fDistance0 = dPointPlaneDistance( avArrayIn[i0],plPlane );
526 dReal fDistance1 = dPointPlaneDistance( avArrayIn[i1],plPlane );
527
528 // if first point is in front of plane
529 if( fDistance0 >= 0 ) {
530 // emit point
531 avArrayOut[ctOut][0] = avArrayIn[i0][0];
532 avArrayOut[ctOut][1] = avArrayIn[i0][1];
533 avArrayOut[ctOut][2] = avArrayIn[i0][2];
534 ctOut++;
535 }
536
537 // if points are on different sides
538 if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
539
540 // find intersection point of edge and plane
541 dVector3 vIntersectionPoint;
542 vIntersectionPoint[0]= avArrayIn[i0][0] -
543 (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
544 vIntersectionPoint[1]= avArrayIn[i0][1] -
545 (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
546 vIntersectionPoint[2]= avArrayIn[i0][2] -
547 (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
548
549 // emit intersection point
550 avArrayOut[ctOut][0] = vIntersectionPoint[0];
551 avArrayOut[ctOut][1] = vIntersectionPoint[1];
552 avArrayOut[ctOut][2] = vIntersectionPoint[2];
553 ctOut++;
554 }
555 }
556
557}
558
559void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn,
560 dVector3 avArrayOut[], int &ctOut,
561 const dVector4 &plPlane ,dReal fRadius)
562{
563 // start with no output points
564 ctOut = 0;
565
566 int i0 = ctIn-1;
567
568 // for each edge in input polygon
569 for (int i1=0; i1<ctIn; i0=i1, i1++)
570 {
571 // calculate distance of edge points to plane
572 dReal fDistance0 = dPointPlaneDistance( avArrayIn[i0],plPlane );
573 dReal fDistance1 = dPointPlaneDistance( avArrayIn[i1],plPlane );
574
575 // if first point is in front of plane
576 if( fDistance0 >= 0 )
577 {
578 // emit point
579 if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
580 {
581 avArrayOut[ctOut][0] = avArrayIn[i0][0];
582 avArrayOut[ctOut][1] = avArrayIn[i0][1];
583 avArrayOut[ctOut][2] = avArrayIn[i0][2];
584 ctOut++;
585 }
586 }
587
588 // if points are on different sides
589 if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) )
590 {
591
592 // find intersection point of edge and plane
593 dVector3 vIntersectionPoint;
594 vIntersectionPoint[0]= avArrayIn[i0][0] -
595 (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
596 vIntersectionPoint[1]= avArrayIn[i0][1] -
597 (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
598 vIntersectionPoint[2]= avArrayIn[i0][2] -
599 (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
600
601 // emit intersection point
602 if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
603 {
604 avArrayOut[ctOut][0] = vIntersectionPoint[0];
605 avArrayOut[ctOut][1] = vIntersectionPoint[1];
606 avArrayOut[ctOut][2] = vIntersectionPoint[2];
607 ctOut++;
608 }
609 }
610 }
611}
612
diff --git a/libraries/ode-0.9/ode/src/collision_util.h b/libraries/ode-0.9/ode/src/collision_util.h
new file mode 100644
index 0000000..4c479e3
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_util.h
@@ -0,0 +1,340 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25some useful collision utility stuff.
26
27*/
28
29#ifndef _ODE_COLLISION_UTIL_H_
30#define _ODE_COLLISION_UTIL_H_
31
32#include <ode/common.h>
33#include <ode/contact.h>
34#include <ode/odemath.h>
35#include <ode/rotation.h>
36
37
38// given a pointer `p' to a dContactGeom, return the dContactGeom at
39// p + skip bytes.
40#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
41
42#if 1
43#include "collision_kernel.h"
44// Fetches a contact
45inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
46 dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
47 return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
48}
49#endif
50
51
52// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
53// return 1, else return 0.
54
55int dCollideSpheres (dVector3 p1, dReal r1,
56 dVector3 p2, dReal r2, dContactGeom *c);
57
58
59// given two lines
60// qa = pa + alpha* ua
61// qb = pb + beta * ub
62// where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
63// beta go from [-inf,inf], return alpha and beta such that qa and qb are
64// as close as possible
65
66void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
67 const dVector3 pb, const dVector3 ub,
68 dReal *alpha, dReal *beta);
69
70
71// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
72// vector 'side'), compute the points of closest approach between the box
73// and the line. return these points in 'lret' (the point on the line) and
74// 'bret' (the point on the box). if the line actually penetrates the box
75// then the solution is not unique, but only one solution will be returned.
76// in this case the solution points will coincide.
77
78void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
79 const dVector3 c, const dMatrix3 R,
80 const dVector3 side,
81 dVector3 lret, dVector3 bret);
82
83// 20 Apr 2004
84// Start code by Nguyen Binh
85int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
86// clip polygon with plane and generate new polygon points
87void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );
88
89void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
90
91// Some vector math
92inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
93{
94 c[0] = a[0] - b[0];
95 c[1] = a[1] - b[1];
96 c[2] = a[2] - b[2];
97}
98
99// Some vector math
100inline void dVector3Scale(dVector3& a,dReal nScale)
101{
102 a[0] *= nScale ;
103 a[1] *= nScale ;
104 a[2] *= nScale ;
105}
106
107inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
108{
109 c[0] = a[0] + b[0];
110 c[1] = a[1] + b[1];
111 c[2] = a[2] + b[2];
112}
113
114inline void dVector3Copy(const dVector3& a,dVector3& c)
115{
116 c[0] = a[0];
117 c[1] = a[1];
118 c[2] = a[2];
119}
120
121inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
122{
123 dCROSS(c,=,a,b);
124}
125
126inline dReal dVector3Length(const dVector3& a)
127{
128 return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
129}
130
131inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
132{
133 return dDOT(a,b);
134}
135
136inline void dVector3Inv(dVector3& a)
137{
138 a[0] = -a[0];
139 a[1] = -a[1];
140 a[2] = -a[2];
141}
142
143inline dReal dVector3Length2(const dVector3& a)
144{
145 return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
146}
147
148inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
149{
150 v[0] = m[col + 0];
151 v[1] = m[col + 4];
152 v[2] = m[col + 8];
153}
154
155inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
156{
157 r[0] = v[1] * m[2*4 + col] - v[2] * m[1*4 + col];
158 r[1] = v[2] * m[0*4 + col] - v[0] * m[2*4 + col];
159 r[2] = v[0] * m[1*4 + col] - v[1] * m[0*4 + col];
160}
161
162inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
163{
164 r[0] = v[2] * m[1*4 + col] - v[1] * m[2*4 + col];
165 r[1] = v[0] * m[2*4 + col] - v[2] * m[0*4 + col];
166 r[2] = v[1] * m[0*4 + col] - v[0] * m[1*4 + col];
167}
168
169inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
170{
171 dMULTIPLY0_331(r,m,v);
172}
173
174inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
175{
176 return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
177}
178
179inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
180{
181 plane[0] = normal[0];
182 plane[1] = normal[1];
183 plane[2] = normal[2];
184 plane[3] = distance;
185}
186
187inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
188{
189 dest[0] = source[0];
190 dest[1] = source[1];
191 dest[2] = source[2];
192
193 dest[4] = source[4];
194 dest[5] = source[5];
195 dest[6] = source[6];
196
197 dest[8] = source[8];
198 dest[9] = source[9];
199 dest[10]= source[10];
200}
201
202inline dReal dMatrix3Det( const dMatrix3& mat )
203{
204 dReal det;
205
206 det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
207 - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
208 + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] );
209
210 return( det );
211}
212
213
214inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
215{
216 dReal det = dMatrix3Det( ma );
217
218 if ( dFabs( det ) < REAL(0.0005) )
219 {
220 dRSetIdentity( dst );
221 return;
222 }
223
224 dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det;
225 dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det;
226 dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det;
227
228 dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det;
229 dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det;
230 dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det;
231
232 dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det;
233 dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det;
234 dst[10] = ma[0]*ma[5] - ma[1]*ma[4] / det;
235}
236
237inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
238{
239
240 // Nguyen Binh : this code seem to be the fastest.
241 dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
242 dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
243 dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
244 dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
245
246 dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
247 dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
248 dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
249
250 /*
251 // nVidia SDK implementation
252 dVector3 uv, uuv;
253 dVector3 qvec;
254 qvec[0] = quat[1];
255 qvec[1] = quat[2];
256 qvec[2] = quat[3];
257
258 dVector3Cross(qvec,source,uv);
259 dVector3Cross(qvec,uv,uuv);
260
261 dVector3Scale(uv,REAL(2.0)*quat[0]);
262 dVector3Scale(uuv,REAL(2.0));
263
264 dest[0] = source[0] + uv[0] + uuv[0];
265 dest[1] = source[1] + uv[1] + uuv[1];
266 dest[2] = source[2] + uv[2] + uuv[2];
267 */
268}
269
270inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
271{
272
273 dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
274
275 if (norm > REAL(0.0))
276 {
277 dQuaternion invQuat;
278 invQuat[0] = quat[0] / norm;
279 invQuat[1] = -quat[1] / norm;
280 invQuat[2] = -quat[2] / norm;
281 invQuat[3] = -quat[3] / norm;
282
283 dQuatTransform(invQuat,source,dest);
284
285 }
286 else
287 {
288 // Singular -> return identity
289 dVector3Copy(source,dest);
290 }
291}
292
293inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
294{
295 rY = asin(mRot[0 * 4 + 2]);
296 if (rY < M_PI /2)
297 {
298 if (rY > -M_PI /2)
299 {
300 rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
301 rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
302 }
303 else
304 {
305 // not unique
306 rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
307 rZ = REAL(0.0);
308 }
309 }
310 else
311 {
312 // not unique
313 rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
314 rZ = REAL(0.0);
315 }
316}
317
318inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
319{
320 dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
321
322 if (norm > 0.0f)
323 {
324 dest[0] = source[0] / norm;
325 dest[1] = -source[1] / norm;
326 dest[2] = -source[2] / norm;
327 dest[3] = -source[3] / norm;
328 }
329 else
330 {
331 // Singular -> return identity
332 dest[0] = REAL(1.0);
333 dest[1] = REAL(0.0);
334 dest[2] = REAL(0.0);
335 dest[3] = REAL(0.0);
336 }
337}
338
339
340#endif
diff --git a/libraries/ode-0.9/ode/src/convex.cpp b/libraries/ode-0.9/ode/src/convex.cpp
new file mode 100644
index 0000000..db93beb
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/convex.cpp
@@ -0,0 +1,1294 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24Code for Convex Collision Detection
25By Rodrigo Hernandez
26*/
27//#include <algorithm>
28#include <ode/common.h>
29#include <ode/collision.h>
30#include <ode/matrix.h>
31#include <ode/rotation.h>
32#include <ode/odemath.h>
33#include "collision_kernel.h"
34#include "collision_std.h"
35#include "collision_util.h"
36
37#ifdef _MSC_VER
38#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
39#endif
40
41#if _MSC_VER <= 1200
42#define dMIN(A,B) ((A)>(B) ? B : A)
43#define dMAX(A,B) ((A)>(B) ? A : B)
44#else
45#define dMIN(A,B) std::min(A,B)
46#define dMAX(A,B) std::max(A,B)
47#endif
48
49//****************************************************************************
50// Convex public API
51dxConvex::dxConvex (dSpaceID space,
52 dReal *_planes,
53 unsigned int _planecount,
54 dReal *_points,
55 unsigned int _pointcount,
56 unsigned int *_polygons) :
57 dxGeom (space,1)
58{
59 dAASSERT (_planes != NULL);
60 dAASSERT (_points != NULL);
61 dAASSERT (_polygons != NULL);
62 //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes);
63 type = dConvexClass;
64 planes = _planes;
65 planecount = _planecount;
66 // we need points as well
67 points = _points;
68 pointcount = _pointcount;
69 polygons=_polygons;
70 FillEdges();
71}
72
73
74void dxConvex::computeAABB()
75{
76 // this can, and should be optimized
77 dVector3 point;
78 dMULTIPLY0_331 (point,final_posr->R,points);
79 aabb[0] = point[0]+final_posr->pos[0];
80 aabb[1] = point[0]+final_posr->pos[0];
81 aabb[2] = point[1]+final_posr->pos[1];
82 aabb[3] = point[1]+final_posr->pos[1];
83 aabb[4] = point[2]+final_posr->pos[2];
84 aabb[5] = point[2]+final_posr->pos[2];
85 for(unsigned int i=3;i<(pointcount*3);i+=3)
86 {
87 dMULTIPLY0_331 (point,final_posr->R,&points[i]);
88 aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]);
89 aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]);
90 aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]);
91 aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]);
92 aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]);
93 aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]);
94 }
95}
96
97/*! \brief Populates the edges set, should be called only once whenever
98 the polygon array gets updated */
99void dxConvex::FillEdges()
100{
101 unsigned int *points_in_poly=polygons;
102 unsigned int *index=polygons+1;
103 for(unsigned int i=0;i<planecount;++i)
104 {
105 //fprintf(stdout,"Points in Poly: %d\n",*points_in_poly);
106 for(unsigned int j=0;j<*points_in_poly;++j)
107 {
108 edges.insert(edge(dMIN(index[j],index[(j+1)%*points_in_poly]),
109 dMAX(index[j],index[(j+1)%*points_in_poly])));
110 //fprintf(stdout,"Insert %d-%d\n",index[j],index[(j+1)%*points_in_poly]);
111 }
112 points_in_poly+=(*points_in_poly+1);
113 index=points_in_poly+1;
114 }
115 /*
116 fprintf(stdout,"Edge Count: %d\n",edges.size());
117 for(std::set<edge>::iterator it=edges.begin();
118 it!=edges.end();
119 ++it)
120 {
121 fprintf(stdout,"Edge: %d-%d\n",it->first,it->second);
122 }
123 */
124}
125
126dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount,
127 dReal *_points,
128 unsigned int _pointcount,
129 unsigned int *_polygons)
130{
131 //fprintf(stdout,"dxConvex dCreateConvex\n");
132 return new dxConvex(space,_planes, _planecount,
133 _points,
134 _pointcount,
135 _polygons);
136}
137
138void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount,
139 dReal *_points,
140 unsigned int _pointcount,
141 unsigned int *_polygons)
142{
143 //fprintf(stdout,"dxConvex dGeomSetConvex\n");
144 dUASSERT (g && g->type == dConvexClass,"argument not a convex shape");
145 dxConvex *s = (dxConvex*) g;
146 s->planes = _planes;
147 s->planecount = _planecount;
148 s->points = _points;
149 s->pointcount = _pointcount;
150 s->polygons=_polygons;
151}
152
153//****************************************************************************
154// Helper Inlines
155//
156
157/*! \brief Returns Whether or not the segment ab intersects plane p
158 \param a origin of the segment
159 \param b segment destination
160 \param p plane to test for intersection
161 \param t returns the time "t" in the segment ray that gives us the intersecting
162 point
163 \param q returns the intersection point
164 \return true if there is an intersection, otherwise false.
165*/
166bool IntersectSegmentPlane(dVector3 a,
167 dVector3 b,
168 dVector4 p,
169 dReal &t,
170 dVector3 q)
171{
172 // Compute the t value for the directed line ab intersecting the plane
173 dVector3 ab;
174 ab[0]= b[0] - a[0];
175 ab[1]= b[1] - a[1];
176 ab[2]= b[2] - a[2];
177
178 t = (p[3] - dDOT(p,a)) / dDOT(p,ab);
179
180 // If t in [0..1] compute and return intersection point
181 if (t >= 0.0 && t <= 1.0)
182 {
183 q[0] = a[0] + t * ab[0];
184 q[1] = a[1] + t * ab[1];
185 q[2] = a[2] + t * ab[2];
186 return true;
187 }
188 // Else no intersection
189 return false;
190}
191
192/*! \brief Returns the Closest Point in Ray 1 to Ray 2
193 \param Origin1 The origin of Ray 1
194 \param Direction1 The direction of Ray 1
195 \param Origin1 The origin of Ray 2
196 \param Direction1 The direction of Ray 3
197 \param t the time "t" in Ray 1 that gives us the closest point
198 (closest_point=Origin1+(Direction*t).
199 \return true if there is a closest point, false if the rays are paralell.
200*/
201inline bool ClosestPointInRay(const dVector3 Origin1,
202 const dVector3 Direction1,
203 const dVector3 Origin2,
204 const dVector3 Direction2,
205 dReal& t)
206{
207 dVector3 w = {Origin1[0]-Origin2[0],
208 Origin1[1]-Origin2[1],
209 Origin1[2]-Origin2[2]};
210 dReal a = dDOT(Direction1 , Direction1);
211 dReal b = dDOT(Direction1 , Direction2);
212 dReal c = dDOT(Direction2 , Direction2);
213 dReal d = dDOT(Direction1 , w);
214 dReal e = dDOT(Direction2 , w);
215 dReal denominator = (a*c)-(b*b);
216 if(denominator==0.0f)
217 {
218 return false;
219 }
220 t = ((a*e)-(b*d))/denominator;
221 return true;
222}
223
224/*! \brief Returns the Ray on which 2 planes intersect if they do.
225 \param p1 Plane 1
226 \param p2 Plane 2
227 \param p Contains the origin of the ray upon returning if planes intersect
228 \param d Contains the direction of the ray upon returning if planes intersect
229 \return true if the planes intersect, false if paralell.
230*/
231inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d)
232{
233 // Compute direction of intersection line
234 //Cross(p1, p2,d);
235 dCROSS(d,=,p1,p2);
236
237 // If d is (near) zero, the planes are parallel (and separated)
238 // or coincident, so they're not considered intersecting
239 dReal denom = dDOT(d, d);
240 if (denom < dEpsilon) return false;
241
242 dVector3 n;
243 n[0]=p1[3]*p2[0] - p2[3]*p1[0];
244 n[1]=p1[3]*p2[1] - p2[3]*p1[1];
245 n[2]=p1[3]*p2[2] - p2[3]*p1[2];
246 // Compute point on intersection line
247 //Cross(n, d,p);
248 dCROSS(p,=,n,d);
249 p[0]/=denom;
250 p[1]/=denom;
251 p[2]/=denom;
252 return true;
253}
254
255/*! \brief Finds out if a point lies inside a 2D polygon
256 \param p Point to test
257 \param polygon a pointer to the start of the convex polygon index buffer
258 \param out the closest point in the polygon if the point is not inside
259 \return true if the point lies inside of the polygon, false if not.
260*/
261inline bool IsPointInPolygon(dVector3 p,
262 unsigned int *polygon,
263 dxConvex *convex,
264 dVector3 out)
265{
266 // p is the point we want to check,
267 // polygon is a pointer to the polygon we
268 // are checking against, remember it goes
269 // number of vertices then that many indexes
270 // out returns the closest point on the border of the
271 // polygon if the point is not inside it.
272 size_t pointcount=polygon[0];
273 dVector3 a;
274 dVector3 b;
275 dVector3 c;
276 dVector3 ab;
277 dVector3 ac;
278 dVector3 ap;
279 dVector3 bp;
280 dReal d1;
281 dReal d2;
282 dReal d3;
283 dReal d4;
284 dReal vc;
285 polygon++; // skip past pointcount
286 for(size_t i=0;i<pointcount;++i)
287 {
288 dMULTIPLY0_331 (a,convex->final_posr->R,&convex->points[(polygon[i]*3)]);
289 a[0]=convex->final_posr->pos[0]+a[0];
290 a[1]=convex->final_posr->pos[1]+a[1];
291 a[2]=convex->final_posr->pos[2]+a[2];
292
293 dMULTIPLY0_331 (b,convex->final_posr->R,
294 &convex->points[(polygon[(i+1)%pointcount]*3)]);
295 b[0]=convex->final_posr->pos[0]+b[0];
296 b[1]=convex->final_posr->pos[1]+b[1];
297 b[2]=convex->final_posr->pos[2]+b[2];
298
299 dMULTIPLY0_331 (c,convex->final_posr->R,
300 &convex->points[(polygon[(i+2)%pointcount]*3)]);
301 c[0]=convex->final_posr->pos[0]+c[0];
302 c[1]=convex->final_posr->pos[1]+c[1];
303 c[2]=convex->final_posr->pos[2]+c[2];
304
305 ab[0] = b[0] - a[0];
306 ab[1] = b[1] - a[1];
307 ab[2] = b[2] - a[2];
308 ac[0] = c[0] - a[0];
309 ac[1] = c[1] - a[1];
310 ac[2] = c[2] - a[2];
311 ap[0] = p[0] - a[0];
312 ap[1] = p[1] - a[1];
313 ap[2] = p[2] - a[2];
314 d1 = dDOT(ab,ap);
315 d2 = dDOT(ac,ap);
316 if (d1 <= 0.0f && d2 <= 0.0f)
317 {
318 out[0]=a[0];
319 out[1]=a[1];
320 out[2]=a[2];
321 return false;
322 }
323 bp[0] = p[0] - b[0];
324 bp[1] = p[1] - b[1];
325 bp[2] = p[2] - b[2];
326 d3 = dDOT(ab,bp);
327 d4 = dDOT(ac,bp);
328 if (d3 >= 0.0f && d4 <= d3)
329 {
330 out[0]=b[0];
331 out[1]=b[1];
332 out[2]=b[2];
333 return false;
334 }
335 vc = d1*d4 - d3*d2;
336 if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
337 {
338 dReal v = d1 / (d1 - d3);
339 out[0] = a[0] + (ab[0]*v);
340 out[1] = a[1] + (ab[1]*v);
341 out[2] = a[2] + (ab[2]*v);
342 return false;
343 }
344 }
345 return true;
346}
347
348int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
349 dContactGeom *contact, int skip)
350{
351 dIASSERT (skip >= (int)sizeof(dContactGeom));
352 dIASSERT (o1->type == dConvexClass);
353 dIASSERT (o2->type == dPlaneClass);
354 dIASSERT ((flags & NUMC_MASK) >= 1);
355
356 dxConvex *Convex = (dxConvex*) o1;
357 dxPlane *Plane = (dxPlane*) o2;
358 unsigned int contacts=0;
359 unsigned int maxc = flags & NUMC_MASK;
360 dVector3 v2;
361
362#define LTEQ_ZERO 0x10000000
363#define GTEQ_ZERO 0x20000000
364#define BOTH_SIGNS (LTEQ_ZERO | GTEQ_ZERO)
365 dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later
366
367 unsigned int totalsign = 0;
368 for(unsigned int i=0;i<Convex->pointcount;++i)
369 {
370 dMULTIPLY0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]);
371 dVector3Add(Convex->final_posr->pos, v2, v2);
372
373 unsigned int distance2sign = GTEQ_ZERO;
374 dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D
375 if((distance2 <= REAL(0.0)))
376 {
377 distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS;
378
379 if (contacts != maxc)
380 {
381 dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
382 dVector3Copy(Plane->p, target->normal);
383 dVector3Copy(v2, target->pos);
384 target->depth = -distance2;
385 target->g1 = Convex;
386 target->g2 = Plane;
387 contacts++;
388 }
389 }
390
391 // Take new sign into account
392 totalsign |= distance2sign;
393 // Check if contacts are full and both signs have been already found
394 if ((contacts ^ maxc | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less
395 {
396 break; // Nothing can be changed any more
397 }
398 }
399 if (totalsign == BOTH_SIGNS) return contacts;
400 return 0;
401#undef BOTH_SIGNS
402#undef GTEQ_ZERO
403#undef LTEQ_ZERO
404}
405
406int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
407 dContactGeom *contact, int skip)
408{
409 dIASSERT (skip >= (int)sizeof(dContactGeom));
410 dIASSERT (o1->type == dSphereClass);
411 dIASSERT (o2->type == dConvexClass);
412 dIASSERT ((flags & NUMC_MASK) >= 1);
413
414 dxSphere *Sphere = (dxSphere*) o1;
415 dxConvex *Convex = (dxConvex*) o2;
416 dReal dist,closestdist=dInfinity;
417 dVector4 plane;
418 // dVector3 contactpoint;
419 dVector3 offsetpos,out,temp;
420 unsigned int *pPoly=Convex->polygons;
421 int closestplane;
422 bool sphereinside=true;
423 /*
424 Do a good old sphere vs plane check first,
425 if a collision is found then check if the contact point
426 is within the polygon
427 */
428 // offset the sphere final_posr->position into the convex space
429 offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0];
430 offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1];
431 offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2];
432 //fprintf(stdout,"Begin Check\n");
433 for(unsigned int i=0;i<Convex->planecount;++i)
434 {
435 // apply rotation to the plane
436 dMULTIPLY0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]);
437 plane[3]=(&Convex->planes[(i*4)])[3];
438 // Get the distance from the sphere origin to the plane
439 dist = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D
440 if(dist>0)
441 {
442 // if we get here, we know the center of the sphere is
443 // outside of the convex hull.
444 if(dist<Sphere->radius)
445 {
446 // if we get here we know the sphere surface penetrates
447 // the plane
448 if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,Convex,out))
449 {
450 // finally if we get here we know that the
451 // sphere is directly touching the inside of the polyhedron
452 //fprintf(stdout,"Contact! distance=%f\n",dist);
453 contact->normal[0] = plane[0];
454 contact->normal[1] = plane[1];
455 contact->normal[2] = plane[2];
456 contact->pos[0] = Sphere->final_posr->pos[0]+
457 (-contact->normal[0]*Sphere->radius);
458 contact->pos[1] = Sphere->final_posr->pos[1]+
459 (-contact->normal[1]*Sphere->radius);
460 contact->pos[2] = Sphere->final_posr->pos[2]+
461 (-contact->normal[2]*Sphere->radius);
462 contact->depth = Sphere->radius-dist;
463 contact->g1 = Sphere;
464 contact->g2 = Convex;
465 return 1;
466 }
467 else
468 {
469 // the sphere may not be directly touching
470 // the polyhedron, but it may be touching
471 // a point or an edge, if the distance between
472 // the closest point on the poly (out) and the
473 // center of the sphere is less than the sphere
474 // radius we have a hit.
475 temp[0] = (Sphere->final_posr->pos[0]-out[0]);
476 temp[1] = (Sphere->final_posr->pos[1]-out[1]);
477 temp[2] = (Sphere->final_posr->pos[2]-out[2]);
478 dist=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]);
479 // avoid the sqrt unless really necesary
480 if(dist<(Sphere->radius*Sphere->radius))
481 {
482 // We got an indirect hit
483 dist=dSqrt(dist);
484 contact->normal[0] = temp[0]/dist;
485 contact->normal[1] = temp[1]/dist;
486 contact->normal[2] = temp[2]/dist;
487 contact->pos[0] = Sphere->final_posr->pos[0]+
488 (-contact->normal[0]*Sphere->radius);
489 contact->pos[1] = Sphere->final_posr->pos[1]+
490 (-contact->normal[1]*Sphere->radius);
491 contact->pos[2] = Sphere->final_posr->pos[2]+
492 (-contact->normal[2]*Sphere->radius);
493 contact->depth = Sphere->radius-dist;
494 contact->g1 = Sphere;
495 contact->g2 = Convex;
496 return 1;
497 }
498 }
499 }
500 sphereinside=false;
501 }
502 if(sphereinside)
503 {
504 if(closestdist>dFabs(dist))
505 {
506 closestdist=dFabs(dist);
507 closestplane=i;
508 }
509 }
510 pPoly+=pPoly[0]+1;
511 }
512 if(sphereinside)
513 {
514 // if the center of the sphere is inside
515 // the Convex, we need to pop it out
516 dMULTIPLY0_331(contact->normal,
517 Convex->final_posr->R,
518 &Convex->planes[(closestplane*4)]);
519 contact->pos[0] = Sphere->final_posr->pos[0];
520 contact->pos[1] = Sphere->final_posr->pos[1];
521 contact->pos[2] = Sphere->final_posr->pos[2];
522 contact->depth = closestdist+Sphere->radius;
523 contact->g1 = Sphere;
524 contact->g2 = Convex;
525 return 1;
526 }
527 return 0;
528}
529
530int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
531 dContactGeom *contact, int skip)
532{
533 dIASSERT (skip >= (int)sizeof(dContactGeom));
534 dIASSERT (o1->type == dConvexClass);
535 dIASSERT (o2->type == dBoxClass);
536 dIASSERT ((flags & NUMC_MASK) >= 1);
537
538 dxConvex *Convex = (dxConvex*) o1;
539 dxBox *Box = (dxBox*) o2;
540
541 return 0;
542}
543
544int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
545 int flags, dContactGeom *contact, int skip)
546{
547 dIASSERT (skip >= (int)sizeof(dContactGeom));
548 dIASSERT (o1->type == dConvexClass);
549 dIASSERT (o2->type == dCapsuleClass);
550 dIASSERT ((flags & NUMC_MASK) >= 1);
551
552 dxConvex *Convex = (dxConvex*) o1;
553 dxCapsule *Capsule = (dxCapsule*) o2;
554
555 return 0;
556}
557
558/*!
559 \brief Retrieves the proper convex and plane index between 2 convex objects.
560
561 Seidel's Algorithm does not discriminate between 2 different Convex Hulls,
562 it only cares about planes, so we feed it the planes of Convex 1 followed
563 by the planes of Convex 2 as a single collection of planes,
564 given an index into the single collection,
565 this function determines the correct convex object and index to retrieve
566 the current plane.
567
568 \param c1 Convex 1
569 \param c2 Convex 2
570 \param i Plane Index to retrieve
571 \param index contains the translated index uppon return
572 \return a pointer to the convex object containing plane index "i"
573*/
574inline dxConvex* GetPlaneIndex(dxConvex& c1,
575 dxConvex& c2,
576 unsigned int i,unsigned int& index)
577{
578 if(i>=c1.planecount)
579 {
580 index = i-c1.planecount;
581 return &c2;
582 }
583 index=i;
584 return &c1;
585}
586
587inline void dumpplanes(dxConvex& cvx)
588{
589 // This is just a dummy debug function
590 dVector4 plane;
591 fprintf(stdout,"DUMP PLANES\n");
592 for (unsigned int i=0;i<cvx.planecount;++i)
593 {
594 dMULTIPLY0_331(plane,cvx.final_posr->R,&cvx.planes[(i*4)]);
595 // Translate
596 plane[3]=
597 (cvx.planes[(i*4)+3])+
598 ((plane[0] * cvx.final_posr->pos[0]) +
599 (plane[1] * cvx.final_posr->pos[1]) +
600 (plane[2] * cvx.final_posr->pos[2]));
601 fprintf(stdout,"%f %f %f %f\n",plane[0],plane[1],plane[2],plane[3]);
602 }
603}
604
605// this variable is for debuggin purpuses only, should go once everything works
606static bool hit=false;
607
608/*
609 \brief Tests whether 2 convex objects collide
610
611 Seidel's algorithm is a method to solve linear programs,
612 it finds the optimum vertex "v" of a set of functions,
613 in our case, the set of functions are the plane functions
614 for the 2 convex objects being tested.
615 We don't really care about the value optimum vertex though,
616 but the 2 convex objects only collide if this vertex exists,
617 otherwise, the set of functions is said to be "empty" or "void".
618
619 Seidel's Original algorithm is recursive and not bound to any number
620 of dimensions, the one I present here is Iterative rather than recursive
621 and bound to 3 dimensions, which is what we care about.
622
623 If you're interested (and you should be!) on the algorithm, the paper
624 by Raimund Seidel himself should explain a lot better than I did, you can
625 find it here: http://www.cs.berkeley.edu/~jrs/meshpapers/SeidelLP.pdf
626
627 If posible, read the section about Linear Programming in
628 Christer Ericson's RealTime Collision Detection book.
629
630 \note currently there seem to be some issues with this function since
631 it doesn't detect collisions except for the most simple tests :(.
632*/
633bool SeidelLP(dxConvex& cvx1,dxConvex& cvx2)
634{
635 dVector3 c={1,0,0}; // The Objective vector can be anything
636 dVector3 solution; // We dont really need the solution so its local
637 dxConvex *cvx;
638 unsigned int index;
639 unsigned int planecount=cvx1.planecount+cvx2.planecount;
640 dReal sum,min,max,med;
641 dVector3 c1; // ,c2;
642 dVector4 aoveral,aoveram; // these will contain cached computations
643 unsigned int l,m,n; // l and m are the axes to the zerod dimensions, n is the axe for the last dimension
644 unsigned int i,j,k;
645 dVector4 eq1,eq2,eq3; // cached equations for 3d,2d and 1d respectivelly
646 // Get the support mapping for a HUGE bounding box in direction c
647 solution[0]= (c[0]>0) ? dInfinity : -dInfinity;
648 solution[1]= (c[1]>0) ? dInfinity : -dInfinity;
649 solution[2]= (c[2]>0) ? dInfinity : -dInfinity;
650 for( i=0;i<planecount;++i)
651 {
652 // Isolate plane equation
653 cvx=GetPlaneIndex(cvx1,cvx2,i,index);
654 // Rotate
655 dMULTIPLY0_331(eq1,cvx->final_posr->R,&cvx->planes[(index*4)]);
656
657 // Translate
658 eq1[3]=(cvx->planes[(index*4)+3])+
659 ((eq1[0] * cvx->final_posr->pos[0]) +
660 (eq1[1] * cvx->final_posr->pos[1]) +
661 (eq1[2] * cvx->final_posr->pos[2]));
662 // if(!hit)
663 // {
664 // fprintf(stdout,"Plane I %d: %f %f %f %f\n",i,
665 // cvx->planes[(index*4)+0],
666 // cvx->planes[(index*4)+1],
667 // cvx->planes[(index*4)+2],
668 // cvx->planes[(index*4)+3]);
669 // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",i,
670 // eq1[0],
671 // eq1[1],eq1[2],eq1[3]);
672 // fprintf(stdout,"POS %f,%f,%f\n",
673 // cvx->final_posr->pos[0],
674 // cvx->final_posr->pos[1],
675 // cvx->final_posr->pos[2]);
676 // }
677 // find if the solution is behind the current plane
678 sum=
679 ((eq1[0]*solution[0])+
680 (eq1[1]*solution[1])+
681 (eq1[2]*solution[2]))-eq1[3];
682 // if not we need to discard the current solution
683 if(sum>0)
684 {
685 // go down a dimension by eliminating a variable
686 // first find l
687 l=0;
688 for( j=0;j<3;++j)
689 {
690 if(fabs(eq1[j])>fabs(eq1[l]))
691 {
692 l=j;
693 }
694 }
695 if(eq1[l]==0)
696 {
697 if(!hit)
698 {
699 fprintf(stdout,"Plane %d: %f %f %f %f is invalid\n",i,
700 eq1[0],eq1[1],eq1[2],eq1[3]);
701 }
702 return false;
703 }
704 // then compute a/a[l] c1 and solution
705 aoveral[0]=eq1[0]/eq1[l];
706 aoveral[1]=eq1[1]/eq1[l];
707 aoveral[2]=eq1[2]/eq1[l];
708 aoveral[3]=eq1[3]/eq1[l];
709 c1[0]=c[0]-((c[l]/eq1[l])*eq1[0]);
710 c1[1]=c[1]-((c[l]/eq1[l])*eq1[1]);
711 c1[2]=c[2]-((c[l]/eq1[l])*eq1[2]);
712 solution[0]=solution[0]-((solution[l]/eq1[l])*eq1[0]);
713 solution[1]=solution[1]-((solution[l]/eq1[l])*eq1[1]);
714 solution[2]=solution[2]-((solution[l]/eq1[l])*eq1[2]);
715 // iterate a to get the new equations with the help of a/a[l]
716 for( j=0;j<planecount;++j)
717 {
718 if(i!=j)
719 {
720 cvx=GetPlaneIndex(cvx1,cvx2,j,index);
721 // Rotate
722 dMULTIPLY0_331(eq2,cvx->final_posr->R,&cvx->planes[(index*4)]);
723 // Translate
724 eq2[3]=(cvx->planes[(index*4)+3])+
725 ((eq2[0] * cvx->final_posr->pos[0]) +
726 (eq2[1] * cvx->final_posr->pos[1]) +
727 (eq2[2] * cvx->final_posr->pos[2]));
728
729 // if(!hit)
730 // {
731 // fprintf(stdout,"Plane J %d: %f %f %f %f\n",j,
732 // cvx->planes[(index*4)+0],
733 // cvx->planes[(index*4)+1],
734 // cvx->planes[(index*4)+2],
735 // cvx->planes[(index*4)+3]);
736 // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",j,
737 // eq2[0],
738 // eq2[1],
739 // eq2[2],
740 // eq2[3]);
741 // fprintf(stdout,"POS %f,%f,%f\n",
742 // cvx->final_posr->pos[0],
743 // cvx->final_posr->pos[1],
744 // cvx->final_posr->pos[2]);
745 // }
746
747 // Take The equation down a dimension
748 eq2[0]-=(cvx->planes[(index*4)+l]*aoveral[0]);
749 eq2[1]-=(cvx->planes[(index*4)+l]*aoveral[1]);
750 eq2[2]-=(cvx->planes[(index*4)+l]*aoveral[2]);
751 eq2[3]-=(cvx->planes[(index*4)+l]*aoveral[3]);
752 sum=
753 ((eq2[0]*solution[0])+
754 (eq2[1]*solution[1])+
755 (eq2[2]*solution[2]))-eq2[3];
756 if(sum>0)
757 {
758 m=0;
759 for( k=0;k<3;++k)
760 {
761 if(fabs(eq2[k])>fabs(eq2[m]))
762 {
763 m=k;
764 }
765 }
766 if(eq2[m]==0)
767 {
768 /*
769 if(!hit) fprintf(stdout,
770 "Plane %d: %f %f %f %f is invalid\n",j,
771 eq2[0],eq2[1],eq2[2],eq2[3]);
772 */
773 return false;
774 }
775 // then compute a/a[m] c1 and solution
776 aoveram[0]=eq2[0]/eq2[m];
777 aoveram[1]=eq2[1]/eq2[m];
778 aoveram[2]=eq2[2]/eq2[m];
779 aoveram[3]=eq2[3]/eq2[m];
780 c1[0]=c[0]-((c[m]/eq2[m])*eq2[0]);
781 c1[1]=c[1]-((c[m]/eq2[m])*eq2[1]);
782 c1[2]=c[2]-((c[m]/eq2[m])*eq2[2]);
783 solution[0]=solution[0]-((solution[m]/eq2[m])*eq2[0]);
784 solution[1]=solution[1]-((solution[m]/eq2[m])*eq2[1]);
785 solution[2]=solution[2]-((solution[m]/eq2[m])*eq2[2]);
786 // figure out the value for n by elimination
787 n = (l==0) ? ((m==1)? 2:1):((m==0)?((l==1)?2:1):0);
788 // iterate a to get the new equations with the help of a/a[l]
789 min=-dInfinity;
790 max=med=dInfinity;
791 for(k=0;k<planecount;++k)
792 {
793 if((i!=k)&&(j!=k))
794 {
795 cvx=GetPlaneIndex(cvx1,cvx2,k,index);
796 // Rotate
797 dMULTIPLY0_331(eq3,cvx->final_posr->R,&cvx->planes[(index*4)]);
798 // Translate
799 eq3[3]=(cvx->planes[(index*4)+3])+
800 ((eq3[0] * cvx->final_posr->pos[0]) +
801 (eq3[1] * cvx->final_posr->pos[1]) +
802 (eq3[2] * cvx->final_posr->pos[2]));
803 // if(!hit)
804 // {
805 // fprintf(stdout,"Plane K %d: %f %f %f %f\n",k,
806 // cvx->planes[(index*4)+0],
807 // cvx->planes[(index*4)+1],
808 // cvx->planes[(index*4)+2],
809 // cvx->planes[(index*4)+3]);
810 // fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",k,
811 // eq3[0],
812 // eq3[1],
813 // eq3[2],
814 // eq3[3]);
815 // fprintf(stdout,"POS %f,%f,%f\n",
816 // cvx->final_posr->pos[0],
817 // cvx->final_posr->pos[1],
818 // cvx->final_posr->pos[2]);
819 // }
820
821 // Take the equation Down to 1D
822 eq3[0]-=(cvx->planes[(index*4)+m]*aoveram[0]);
823 eq3[1]-=(cvx->planes[(index*4)+m]*aoveram[1]);
824 eq3[2]-=(cvx->planes[(index*4)+m]*aoveram[2]);
825 eq3[3]-=(cvx->planes[(index*4)+m]*aoveram[3]);
826 if(eq3[n]>0)
827 {
828 max=dMIN(max,eq3[3]/eq3[n]);
829 }
830 else if(eq3[n]<0)
831 {
832 min=dMAX(min,eq3[3]/eq3[n]);
833 }
834 else
835 {
836 med=dMIN(med,eq3[3]);
837 }
838 }
839 }
840 if ((max<min)||(med<0))
841 {
842 // if(!hit) fprintf(stdout,"((max<min)||(med<0)) ((%f<%f)||(%f<0))",max,min,med);
843 if(!hit)
844 {
845 dumpplanes(cvx1);
846 dumpplanes(cvx2);
847 }
848 //fprintf(stdout,"Planes %d %d\n",i,j);
849 return false;
850
851 }
852 // find the value for the solution in one dimension
853 solution[n] = (c1[n]>=0) ? max:min;
854 // lift to 2D
855 solution[m] = (eq2[3]-(eq2[n]*solution[n]))/eq2[m];
856 // lift to 3D
857 solution[l] = (eq1[3]-(eq1[m]*solution[m]+
858 eq1[n]*solution[n]))/eq1[l];
859 }
860 }
861 }
862 }
863 }
864 return true;
865}
866
867/*! \brief A Support mapping function for convex shapes
868 \param dir direction to find the Support Point for
869 \param cvx convex object to find the support point for
870 \param out the support mapping in dir.
871 */
872inline void Support(dVector3 dir,dxConvex& cvx,dVector3 out)
873{
874 unsigned int index = 0;
875 dVector3 point;
876 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
877 point[0]+=cvx.final_posr->pos[0];
878 point[1]+=cvx.final_posr->pos[1];
879 point[2]+=cvx.final_posr->pos[2];
880
881 dReal max = dDOT(point,dir);
882 dReal tmp;
883 for (unsigned int i = 1; i < cvx.pointcount; ++i)
884 {
885 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+(i*3));
886 point[0]+=cvx.final_posr->pos[0];
887 point[1]+=cvx.final_posr->pos[1];
888 point[2]+=cvx.final_posr->pos[2];
889 tmp = dDOT(point, dir);
890 if (tmp > max)
891 {
892 out[0]=point[0];
893 out[1]=point[1];
894 out[2]=point[2];
895 max = tmp;
896 }
897 }
898}
899
900inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)
901{
902 dVector3 point;
903 dReal value;
904 //fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]);
905 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
906 //fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]);
907 point[0]+=cvx.final_posr->pos[0];
908 point[1]+=cvx.final_posr->pos[1];
909 point[2]+=cvx.final_posr->pos[2];
910 max = min = dDOT(axis,cvx.points);
911 for (unsigned int i = 1; i < cvx.pointcount; ++i)
912 {
913 dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+(i*3));
914 point[0]+=cvx.final_posr->pos[0];
915 point[1]+=cvx.final_posr->pos[1];
916 point[2]+=cvx.final_posr->pos[2];
917 value=dDOT(axis,point);
918 if(value<min)
919 {
920 min=value;
921 }
922 else if(value>max)
923 {
924 max=value;
925 }
926 }
927 //fprintf(stdout,"Compute Interval Min Max %f,%f\n",min,max);
928}
929
930/*! \brief Does an axis separation test between the 2 convex shapes
931using faces and edges */
932int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,
933 dContactGeom *contact, int skip)
934{
935 dVector4 plane,savedplane;
936 dReal min1,max1,min2,max2,min_depth=-dInfinity;
937 dVector3 e1,e2,t;
938 int maxc = flags & NUMC_MASK; // this is causing a segfault
939 dIASSERT(maxc != 0);
940 dxConvex *g1,*g2;
941 unsigned int *pPoly;
942 dVector3 v;
943 // Test faces of cvx1 for separation
944 pPoly=cvx1.polygons;
945 for(unsigned int i=0;i<cvx1.planecount;++i)
946 {
947 // -- Apply Transforms --
948 // Rotate
949 dMULTIPLY0_331(plane,cvx1.final_posr->R,cvx1.planes+(i*4));
950 dNormalize3(plane);
951 // Translate
952 plane[3]=
953 (cvx1.planes[(i*4)+3])+
954 ((plane[0] * cvx1.final_posr->pos[0]) +
955 (plane[1] * cvx1.final_posr->pos[1]) +
956 (plane[2] * cvx1.final_posr->pos[2]));
957 ComputeInterval(cvx1,plane,min1,max1);
958 ComputeInterval(cvx2,plane,min2,max2);
959 //fprintf(stdout,"width %f\n",max1-min1);
960 if(max2<min1 || max1<min2) return 0;
961#if 1
962 // this one ON works
963 else if ((max1>min2)&&(max1<max2))
964 {
965 min_depth=max1-min2;
966 savedplane[0]=plane[0];
967 savedplane[1]=plane[1];
968 savedplane[2]=plane[2];
969 savedplane[3]=plane[3];
970 g1=&cvx2; // cvx2 moves
971 g2=&cvx1;
972 }
973#endif
974 pPoly+=pPoly[0]+1;
975 }
976 // Test faces of cvx2 for separation
977 pPoly=cvx2.polygons;
978 for(unsigned int i=0;i<cvx2.planecount;++i)
979 {
980 // fprintf(stdout,"Poly verts %d\n",pPoly[0]);
981 // -- Apply Transforms --
982 // Rotate
983 dMULTIPLY0_331(plane,
984 cvx2.final_posr->R,
985 cvx2.planes+(i*4));
986 dNormalize3(plane);
987 // Translate
988 plane[3]=
989 (cvx2.planes[(i*4)+3])+
990 ((plane[0] * cvx2.final_posr->pos[0]) +
991 (plane[1] * cvx2.final_posr->pos[1]) +
992 (plane[2] * cvx2.final_posr->pos[2]));
993 ComputeInterval(cvx2,plane,min1,max1);
994 ComputeInterval(cvx1,plane,min2,max2);
995 //fprintf(stdout,"width %f\n",max1-min1);
996 if(max2<min1 || max1<min2) return 0;
997#if 1
998 // this one ON does not work
999 else if ((max1>min2)&&(max1<max2))
1000 {
1001 min_depth=max1-min2;
1002 savedplane[0]=plane[0];
1003 savedplane[1]=plane[1];
1004 savedplane[2]=plane[2];
1005 savedplane[3]=plane[3];
1006 g1=&cvx1; // cvx1 moves
1007 g2=&cvx2;
1008 }
1009#endif
1010 pPoly+=pPoly[0]+1;
1011 }
1012 // Test cross products of pairs of edges
1013 for(std::set<edge>::iterator i = cvx1.edges.begin();
1014 i!= cvx1.edges.end();
1015 ++i)
1016 {
1017 // we only need to apply rotation here
1018 dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+(i->first*3));
1019 dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+(i->second*3));
1020 e1[0]-=t[0];
1021 e1[1]-=t[1];
1022 e1[2]-=t[2];
1023 for(std::set<edge>::iterator j = cvx2.edges.begin();
1024 j!= cvx2.edges.end();
1025 ++j)
1026 {
1027 // we only need to apply rotation here
1028 dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+(j->first*3));
1029 dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+(j->second*3));
1030 e2[0]-=t[0];
1031 e2[1]-=t[1];
1032 e2[2]-=t[2];
1033 dCROSS(plane,=,e1,e2);
1034 plane[3]=0;
1035 ComputeInterval(cvx1,plane,min1,max1);
1036 ComputeInterval(cvx2,plane,min2,max2);
1037 if(max2<min1 || max1 < min2) return 0;
1038 }
1039 }
1040/* -- uncomment if you are debugging
1041 static int cvxhit=0;
1042 if(cvxhit<2)
1043 fprintf(stdout,"Plane: %f,%f,%f,%f\n",
1044 (double)savedplane[0],
1045 (double)savedplane[1],
1046 (double)savedplane[2],
1047 (double)savedplane[3]);
1048*/
1049 // If we get here, there was a collision
1050 int contacts=0;
1051 for(unsigned int i=0;i<g1->pointcount;++i)
1052 {
1053 dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]);
1054 dVector3Add(g1->final_posr->pos, v, v);
1055
1056 dReal distance = dVector3Dot(savedplane, v) - savedplane[3]; // Ax + By + Cz - D
1057 if(distance<0)
1058 {
1059 dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);
1060 dVector3Copy(savedplane, target->normal);
1061 dVector3Copy(v, target->pos);
1062 target->depth = -distance;
1063 target->g1 = g1;
1064 target->g2 = g2;
1065/* -- uncomment if you are debugging
1066 if(cvxhit<2)
1067 fprintf(stdout,"Contact: %f,%f,%f depth %f\n",
1068 (double)target->pos[0],
1069 (double)target->pos[1],
1070 (double)target->pos[2],
1071 (double)target->depth);
1072*/
1073 contacts++;
1074 if (contacts==maxc) break;
1075 }
1076 }
1077/* -- uncomment if you are debugging
1078 cvxhit++;
1079*/
1080
1081 return contacts;
1082}
1083
1084int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
1085 dContactGeom *contact, int skip)
1086{
1087 dIASSERT (skip >= (int)sizeof(dContactGeom));
1088 dIASSERT (o1->type == dConvexClass);
1089 dIASSERT (o2->type == dConvexClass);
1090 dIASSERT ((flags & NUMC_MASK) >= 1);
1091
1092// if(!hit) fprintf(stdout,"dCollideConvexConvex\n");
1093 dxConvex *Convex1 = (dxConvex*) o1;
1094 dxConvex *Convex2 = (dxConvex*) o2;
1095 int contacts;
1096 if(contacts=TestConvexIntersection(*Convex1,*Convex2,flags,
1097 contact,skip))
1098 {
1099 //fprintf(stdout,"We have a Hit!\n");
1100 }
1101 return contacts;
1102}
1103
1104#if 0
1105
1106int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
1107 dContactGeom *contact, int skip)
1108{
1109 dIASSERT (skip >= (int)sizeof(dContactGeom));
1110 dIASSERT( o1->type == dRayClass );
1111 dIASSERT( o2->type == dConvexClass );
1112 dIASSERT ((flags & NUMC_MASK) >= 1);
1113
1114 dxRay* ray = (dxRay*) o1;
1115 dxConvex* convex = (dxConvex*) o2;
1116 dVector3 origin,destination,contactpoint,out;
1117 dReal depth;
1118 dVector4 plane;
1119 unsigned int *pPoly=convex->polygons;
1120 // Calculate ray origin and destination
1121 destination[0]=0;
1122 destination[1]=0;
1123 destination[2]= ray->length;
1124 // -- Rotate --
1125 dMULTIPLY0_331(destination,ray->final_posr->R,destination);
1126 origin[0]=ray->final_posr->pos[0];
1127 origin[1]=ray->final_posr->pos[1];
1128 origin[2]=ray->final_posr->pos[2];
1129 destination[0]+=origin[0];
1130 destination[1]+=origin[1];
1131 destination[2]+=origin[2];
1132 for(int i=0;i<convex->planecount;++i)
1133 {
1134 // Rotate
1135 dMULTIPLY0_331(plane,convex->final_posr->R,convex->planes+(i*4));
1136 // Translate
1137 plane[3]=
1138 (convex->planes[(i*4)+3])+
1139 ((plane[0] * convex->final_posr->pos[0]) +
1140 (plane[1] * convex->final_posr->pos[1]) +
1141 (plane[2] * convex->final_posr->pos[2]));
1142 if(IntersectSegmentPlane(origin,
1143 destination,
1144 plane,
1145 depth,
1146 contactpoint))
1147 {
1148 if(IsPointInPolygon(contactpoint,pPoly,convex,out))
1149 {
1150 contact->pos[0]=contactpoint[0];
1151 contact->pos[1]=contactpoint[1];
1152 contact->pos[2]=contactpoint[2];
1153 contact->normal[0]=plane[0];
1154 contact->normal[1]=plane[1];
1155 contact->normal[2]=plane[2];
1156 contact->depth=depth;
1157 contact->g1 = ray;
1158 contact->g2 = convex;
1159 return 1;
1160 }
1161 }
1162 pPoly+=pPoly[0]+1;
1163 }
1164 return 0;
1165}
1166
1167#else
1168
1169// Ray - Convex collider by David Walters, June 2006
1170int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
1171 int flags, dContactGeom *contact, int skip )
1172{
1173 dIASSERT( skip >= (int)sizeof(dContactGeom) );
1174 dIASSERT( o1->type == dRayClass );
1175 dIASSERT( o2->type == dConvexClass );
1176 dIASSERT ((flags & NUMC_MASK) >= 1);
1177
1178 dxRay* ray = (dxRay*) o1;
1179 dxConvex* convex = (dxConvex*) o2;
1180
1181 contact->g1 = ray;
1182 contact->g2 = convex;
1183
1184 dReal alpha, beta, nsign;
1185 int flag;
1186
1187 //
1188 // Compute some useful info
1189 //
1190
1191 flag = 0; // Assume start point is behind all planes.
1192
1193 for ( unsigned int i = 0; i < convex->planecount; ++i )
1194 {
1195 // Alias this plane.
1196 dReal* plane = convex->planes + ( i * 4 );
1197
1198 // If alpha >= 0 then start point is outside of plane.
1199 alpha = dDOT( plane, ray->final_posr->pos ) - plane[3];
1200
1201 // If any alpha is positive, then
1202 // the ray start is _outside_ of the hull
1203 if ( alpha >= 0 )
1204 {
1205 flag = 1;
1206 break;
1207 }
1208 }
1209
1210 // If the ray starts inside the convex hull, then everything is flipped.
1211 nsign = ( flag ) ? REAL( 1.0 ) : REAL( -1.0 );
1212
1213
1214 //
1215 // Find closest contact point
1216 //
1217
1218 // Assume no contacts.
1219 contact->depth = dInfinity;
1220
1221 for ( unsigned int i = 0; i < convex->planecount; ++i )
1222 {
1223 // Alias this plane.
1224 dReal* plane = convex->planes + ( i * 4 );
1225
1226 // If alpha >= 0 then point is outside of plane.
1227 alpha = nsign * ( dDOT( plane, ray->final_posr->pos ) - plane[3] );
1228
1229 // Compute [ plane-normal DOT ray-normal ], (/flip)
1230 beta = dDOT13( plane, ray->final_posr->R+2 ) * nsign;
1231
1232 // Ray is pointing at the plane? ( beta < 0 )
1233 // Ray start to plane is within maximum ray length?
1234 // Ray start to plane is closer than the current best distance?
1235 if ( beta < -dEpsilon &&
1236 alpha >= 0 && alpha <= ray->length &&
1237 alpha < contact->depth )
1238 {
1239 // Compute contact point on convex hull surface.
1240 contact->pos[0] = ray->final_posr->pos[0] + alpha * ray->final_posr->R[0*4+2];
1241 contact->pos[1] = ray->final_posr->pos[1] + alpha * ray->final_posr->R[1*4+2];
1242 contact->pos[2] = ray->final_posr->pos[2] + alpha * ray->final_posr->R[2*4+2];
1243
1244 flag = 0;
1245
1246 // For all _other_ planes.
1247 for ( unsigned int j = 0; j < convex->planecount; ++j )
1248 {
1249 if ( i == j )
1250 continue; // Skip self.
1251
1252 // Alias this plane.
1253 dReal* planej = convex->planes + ( j * 4 );
1254
1255 // If beta >= 0 then start is outside of plane.
1256 beta = dDOT( planej, contact->pos ) - plane[3];
1257
1258 // If any beta is positive, then the contact point
1259 // is not on the surface of the convex hull - it's just
1260 // intersecting some part of its infinite extent.
1261 if ( beta > dEpsilon )
1262 {
1263 flag = 1;
1264 break;
1265 }
1266 }
1267
1268 // Contact point isn't outside hull's surface? then it's a good contact!
1269 if ( flag == 0 )
1270 {
1271 // Store the contact normal, possibly flipped.
1272 contact->normal[0] = nsign * plane[0];
1273 contact->normal[1] = nsign * plane[1];
1274 contact->normal[2] = nsign * plane[2];
1275
1276 // Store depth
1277 contact->depth = alpha;
1278
1279 if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length )
1280 {
1281 // Break on any contact if contacts are not important
1282 break;
1283 }
1284 }
1285 }
1286 }
1287
1288 // Contact?
1289 return ( contact->depth <= ray->length );
1290}
1291
1292#endif
1293
1294//<-- Convex Collision
diff --git a/libraries/ode-0.9/ode/src/cylinder.cpp b/libraries/ode-0.9/ode/src/cylinder.cpp
new file mode 100644
index 0000000..39a6cf3
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/cylinder.cpp
@@ -0,0 +1,100 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::g1 and dContactGeom::g2.
29
30*/
31
32#include <ode/common.h>
33#include <ode/collision.h>
34#include <ode/matrix.h>
35#include <ode/rotation.h>
36#include <ode/odemath.h>
37#include "collision_kernel.h"
38#include "collision_std.h"
39#include "collision_util.h"
40
41#ifdef _MSC_VER
42#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43#endif
44
45// flat cylinder public API
46
47dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) :
48dxGeom (space,1)
49{
50 dAASSERT (_radius > 0 && _length > 0);
51 type = dCylinderClass;
52 radius = _radius;
53 lz = _length;
54}
55
56
57void dxCylinder::computeAABB()
58{
59 const dMatrix3& R = final_posr->R;
60 const dVector3& pos = final_posr->pos;
61
62 dReal xrange = dFabs (R[0] * radius) + dFabs (R[1] * radius) + REAL(0.5)* dFabs (R[2] *
63 lz);
64 dReal yrange = dFabs (R[4] * radius) + dFabs (R[5] * radius) + REAL(0.5)* dFabs (R[6] *
65 lz);
66 dReal zrange = dFabs (R[8] * radius) + dFabs (R[9] * radius) + REAL(0.5)* dFabs (R[10] *
67 lz);
68 aabb[0] = pos[0] - xrange;
69 aabb[1] = pos[0] + xrange;
70 aabb[2] = pos[1] - yrange;
71 aabb[3] = pos[1] + yrange;
72 aabb[4] = pos[2] - zrange;
73 aabb[5] = pos[2] + zrange;
74}
75
76
77dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length)
78{
79 return new dxCylinder (space,radius,length);
80}
81
82void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length)
83{
84 dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
85 dAASSERT (radius > 0 && length > 0);
86 dxCylinder *c = (dxCylinder*) cylinder;
87 c->radius = radius;
88 c->lz = length;
89 dGeomMoved (cylinder);
90}
91
92void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length)
93{
94 dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
95 dxCylinder *c = (dxCylinder*) cylinder;
96 *radius = c->radius;
97 *length = c->lz;
98}
99
100
diff --git a/libraries/ode-0.9/ode/src/error.cpp b/libraries/ode-0.9/ode/src/error.cpp
new file mode 100644
index 0000000..9b33db5
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/error.cpp
@@ -0,0 +1,172 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/error.h>
25
26
27static dMessageFunction *error_function = 0;
28static dMessageFunction *debug_function = 0;
29static dMessageFunction *message_function = 0;
30
31
32extern "C" void dSetErrorHandler (dMessageFunction *fn)
33{
34 error_function = fn;
35}
36
37
38extern "C" void dSetDebugHandler (dMessageFunction *fn)
39{
40 debug_function = fn;
41}
42
43
44extern "C" void dSetMessageHandler (dMessageFunction *fn)
45{
46 message_function = fn;
47}
48
49
50extern "C" dMessageFunction *dGetErrorHandler()
51{
52 return error_function;
53}
54
55
56extern "C" dMessageFunction *dGetDebugHandler()
57{
58 return debug_function;
59}
60
61
62extern "C" dMessageFunction *dGetMessageHandler()
63{
64 return message_function;
65}
66
67
68static void printMessage (int num, const char *msg1, const char *msg2,
69 va_list ap)
70{
71 fflush (stderr);
72 fflush (stdout);
73 if (num) fprintf (stderr,"\n%s %d: ",msg1,num);
74 else fprintf (stderr,"\n%s: ",msg1);
75 vfprintf (stderr,msg2,ap);
76 fprintf (stderr,"\n");
77 fflush (stderr);
78}
79
80//****************************************************************************
81// unix
82
83#ifndef WIN32
84
85extern "C" void dError (int num, const char *msg, ...)
86{
87 va_list ap;
88 va_start (ap,msg);
89 if (error_function) error_function (num,msg,ap);
90 else printMessage (num,"ODE Error",msg,ap);
91 exit (1);
92}
93
94
95extern "C" void dDebug (int num, const char *msg, ...)
96{
97 va_list ap;
98 va_start (ap,msg);
99 if (debug_function) debug_function (num,msg,ap);
100 else printMessage (num,"ODE INTERNAL ERROR",msg,ap);
101 // *((char *)0) = 0; ... commit SEGVicide
102 abort();
103}
104
105
106extern "C" void dMessage (int num, const char *msg, ...)
107{
108 va_list ap;
109 va_start (ap,msg);
110 if (message_function) message_function (num,msg,ap);
111 else printMessage (num,"ODE Message",msg,ap);
112}
113
114#endif
115
116//****************************************************************************
117// windows
118
119#ifdef WIN32
120
121// isn't cygwin annoying!
122#ifdef CYGWIN
123#define _snprintf snprintf
124#define _vsnprintf vsnprintf
125#endif
126
127
128#include "windows.h"
129
130
131extern "C" void dError (int num, const char *msg, ...)
132{
133 va_list ap;
134 va_start (ap,msg);
135 if (error_function) error_function (num,msg,ap);
136 else {
137 char s[1000],title[100];
138 _snprintf (title,sizeof(title),"ODE Error %d",num);
139 _vsnprintf (s,sizeof(s),msg,ap);
140 s[sizeof(s)-1] = 0;
141 MessageBox(0,s,title,MB_OK | MB_ICONWARNING);
142 }
143 exit (1);
144}
145
146
147extern "C" void dDebug (int num, const char *msg, ...)
148{
149 va_list ap;
150 va_start (ap,msg);
151 if (debug_function) debug_function (num,msg,ap);
152 else {
153 char s[1000],title[100];
154 _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num);
155 _vsnprintf (s,sizeof(s),msg,ap);
156 s[sizeof(s)-1] = 0;
157 MessageBox(0,s,title,MB_OK | MB_ICONSTOP);
158 }
159 abort();
160}
161
162
163extern "C" void dMessage (int num, const char *msg, ...)
164{
165 va_list ap;
166 va_start (ap,msg);
167 if (message_function) message_function (num,msg,ap);
168 else printMessage (num,"ODE Message",msg,ap);
169}
170
171
172#endif
diff --git a/libraries/ode-0.9/ode/src/export-dif.cpp b/libraries/ode-0.9/ode/src/export-dif.cpp
new file mode 100644
index 0000000..577d3e1
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/export-dif.cpp
@@ -0,0 +1,568 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24 * Export a DIF (Dynamics Interchange Format) file.
25 */
26
27
28// @@@ TODO:
29// * export all spaces, and geoms in spaces, not just ones attached to bodies
30// (separate export function?)
31// * say the space each geom is in, so reader can construct space heirarchy
32// * limot --> separate out into limits and motors?
33// * make sure ODE-specific parameters divided out
34
35
36#include "ode/ode.h"
37#include "objects.h"
38#include "joint.h"
39#include "collision_kernel.h"
40
41//***************************************************************************
42// utility
43
44struct PrintingContext {
45 FILE *file; // file to write to
46 int precision; // digits of precision to print
47 int indent; // number of levels of indent
48
49 void printIndent();
50 void printReal (dReal x);
51 void print (const char *name, int x);
52 void print (const char *name, dReal x);
53 void print (const char *name, const dReal *x, int n=3);
54 void print (const char *name, const char *x=0);
55 void printNonzero (const char *name, dReal x);
56 void printNonzero (const char *name, const dReal x[3]);
57};
58
59
60void PrintingContext::printIndent()
61{
62 for (int i=0; i<indent; i++) fputc ('\t',file);
63}
64
65
66void PrintingContext::print (const char *name, int x)
67{
68 printIndent();
69 fprintf (file,"%s = %d,\n",name,x);
70}
71
72
73void PrintingContext::printReal (dReal x)
74{
75 if (x==dInfinity) {
76 fprintf (file,"inf");
77 }
78 else if (x==-dInfinity) {
79 fprintf (file,"-inf");
80 }
81 else {
82 fprintf (file,"%.*g",precision,x);
83 }
84}
85
86
87void PrintingContext::print (const char *name, dReal x)
88{
89 printIndent();
90 fprintf (file,"%s = ",name);
91 printReal (x);
92 fprintf (file,",\n");
93}
94
95
96void PrintingContext::print (const char *name, const dReal *x, int n)
97{
98 printIndent();
99 fprintf (file,"%s = {",name);
100 for (int i=0; i<n; i++) {
101 printReal (x[i]);
102 if (i < n-1) fputc (',',file);
103 }
104 fprintf (file,"},\n");
105}
106
107
108void PrintingContext::print (const char *name, const char *x)
109{
110 printIndent();
111 if (x) {
112 fprintf (file,"%s = \"%s\",\n",name,x);
113 }
114 else {
115 fprintf (file,"%s\n",name);
116 }
117}
118
119
120void PrintingContext::printNonzero (const char *name, dReal x)
121{
122 if (x != 0) print (name,x);
123}
124
125
126void PrintingContext::printNonzero (const char *name, const dReal x[3])
127{
128 if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x);
129}
130
131//***************************************************************************
132// joints
133
134
135static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num)
136{
137 if (num >= 0) {
138 c.printIndent();
139 fprintf (c.file,"limit%d = {\n",num);
140 }
141 else {
142 c.print ("limit = {");
143 }
144 c.indent++;
145 c.print ("low_stop",limot.lostop);
146 c.print ("high_stop",limot.histop);
147 c.printNonzero ("bounce",limot.bounce);
148 c.print ("ODE = {");
149 c.indent++;
150 c.printNonzero ("stop_erp",limot.stop_erp);
151 c.printNonzero ("stop_cfm",limot.stop_cfm);
152 c.indent--;
153 c.print ("},");
154 c.indent--;
155 c.print ("},");
156
157 if (num >= 0) {
158 c.printIndent();
159 fprintf (c.file,"motor%d = {\n",num);
160 }
161 else {
162 c.print ("motor = {");
163 }
164 c.indent++;
165 c.printNonzero ("vel",limot.vel);
166 c.printNonzero ("fmax",limot.fmax);
167 c.print ("ODE = {");
168 c.indent++;
169 c.printNonzero ("fudge_factor",limot.fudge_factor);
170 c.printNonzero ("normal_cfm",limot.normal_cfm);
171 c.indent--;
172 c.print ("},");
173 c.indent--;
174 c.print ("},");
175}
176
177
178static const char *getJointName (dxJoint *j)
179{
180 switch (j->vtable->typenum) {
181 case dJointTypeBall: return "ball";
182 case dJointTypeHinge: return "hinge";
183 case dJointTypeSlider: return "slider";
184 case dJointTypeContact: return "contact";
185 case dJointTypeUniversal: return "universal";
186 case dJointTypeHinge2: return "ODE_hinge2";
187 case dJointTypeFixed: return "fixed";
188 case dJointTypeNull: return "null";
189 case dJointTypeAMotor: return "ODE_angular_motor";
190 case dJointTypeLMotor: return "ODE_linear_motor";
191 case dJointTypePR: return "PR";
192 }
193 return "unknown";
194}
195
196
197static void printBall (PrintingContext &c, dxJoint *j)
198{
199 dxJointBall *b = (dxJointBall*) j;
200 c.print ("anchor1",b->anchor1);
201 c.print ("anchor2",b->anchor2);
202}
203
204
205static void printHinge (PrintingContext &c, dxJoint *j)
206{
207 dxJointHinge *h = (dxJointHinge*) j;
208 c.print ("anchor1",h->anchor1);
209 c.print ("anchor2",h->anchor2);
210 c.print ("axis1",h->axis1);
211 c.print ("axis2",h->axis2);
212 c.print ("qrel",h->qrel,4);
213 printLimot (c,h->limot,-1);
214}
215
216
217static void printSlider (PrintingContext &c, dxJoint *j)
218{
219 dxJointSlider *s = (dxJointSlider*) j;
220 c.print ("axis1",s->axis1);
221 c.print ("qrel",s->qrel,4);
222 c.print ("offset",s->offset);
223 printLimot (c,s->limot,-1);
224}
225
226
227static void printContact (PrintingContext &c, dxJoint *j)
228{
229 dxJointContact *ct = (dxJointContact*) j;
230 int mode = ct->contact.surface.mode;
231 c.print ("pos",ct->contact.geom.pos);
232 c.print ("normal",ct->contact.geom.normal);
233 c.print ("depth",ct->contact.geom.depth);
234 //@@@ may want to write the geoms g1 and g2 that are involved, for debugging.
235 // to do this we must have written out all geoms in all spaces, not just
236 // geoms that are attached to bodies.
237 c.print ("mu",ct->contact.surface.mu);
238 if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2);
239 if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce);
240 if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel);
241 if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp);
242 if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm);
243 if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1);
244 if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2);
245 if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1);
246 if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2);
247 int fa = 0; // friction approximation code
248 if (mode & dContactApprox1_1) fa |= 1;
249 if (mode & dContactApprox1_2) fa |= 2;
250 if (fa) c.print ("friction_approximation",fa);
251 if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1);
252}
253
254
255static void printUniversal (PrintingContext &c, dxJoint *j)
256{
257 dxJointUniversal *u = (dxJointUniversal*) j;
258 c.print ("anchor1",u->anchor1);
259 c.print ("anchor2",u->anchor2);
260 c.print ("axis1",u->axis1);
261 c.print ("axis2",u->axis2);
262 c.print ("qrel1",u->qrel1,4);
263 c.print ("qrel2",u->qrel2,4);
264 printLimot (c,u->limot1,1);
265 printLimot (c,u->limot2,2);
266}
267
268
269static void printHinge2 (PrintingContext &c, dxJoint *j)
270{
271 dxJointHinge2 *h = (dxJointHinge2*) j;
272 c.print ("anchor1",h->anchor1);
273 c.print ("anchor2",h->anchor2);
274 c.print ("axis1",h->axis1);
275 c.print ("axis2",h->axis2);
276 c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available
277 c.print ("v2",h->v2);
278 c.print ("susp_erp",h->susp_erp);
279 c.print ("susp_cfm",h->susp_cfm);
280 printLimot (c,h->limot1,1);
281 printLimot (c,h->limot2,2);
282}
283
284static void printPR (PrintingContext &c, dxJoint *j)
285{
286 dxJointPR *pr = (dxJointPR*) j;
287 c.print ("anchor2",pr->anchor2);
288 c.print ("axisR1",pr->axisR1);
289 c.print ("axisR2",pr->axisR2);
290 c.print ("axisP1",pr->axisP1);
291 c.print ("qrel",pr->qrel,4);
292 c.print ("offset",pr->offset);
293 printLimot (c,pr->limotP,1);
294 printLimot (c,pr->limotR,2);
295}
296
297
298static void printFixed (PrintingContext &c, dxJoint *j)
299{
300 dxJointFixed *f = (dxJointFixed*) j;
301 c.print ("qrel",f->qrel);
302 c.print ("offset",f->offset);
303}
304
305static void printLMotor (PrintingContext &c, dxJoint *j)
306{
307 dxJointLMotor *a = (dxJointLMotor*) j;
308 c.print("num", a->num);
309 c.printIndent();
310 fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
311 c.print ("axis1",a->axis[0]);
312 c.print ("axis2",a->axis[1]);
313 c.print ("axis3",a->axis[2]);
314 for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
315}
316
317
318static void printAMotor (PrintingContext &c, dxJoint *j)
319{
320 dxJointAMotor *a = (dxJointAMotor*) j;
321 c.print ("num",a->num);
322 c.print ("mode",a->mode);
323 c.printIndent();
324 fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
325 c.print ("axis1",a->axis[0]);
326 c.print ("axis2",a->axis[1]);
327 c.print ("axis3",a->axis[2]);
328 for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
329 c.print ("angle1",a->angle[0]);
330 c.print ("angle2",a->angle[1]);
331 c.print ("angle3",a->angle[2]);
332}
333
334//***************************************************************************
335// geometry
336
337static void printGeom (PrintingContext &c, dxGeom *g);
338
339static void printSphere (PrintingContext &c, dxGeom *g)
340{
341 c.print ("type","sphere");
342 c.print ("radius",dGeomSphereGetRadius (g));
343}
344
345
346static void printBox (PrintingContext &c, dxGeom *g)
347{
348 dVector3 sides;
349 dGeomBoxGetLengths (g,sides);
350 c.print ("type","box");
351 c.print ("sides",sides);
352}
353
354
355
356static void printCapsule (PrintingContext &c, dxGeom *g)
357{
358 dReal radius,length;
359 dGeomCapsuleGetParams (g,&radius,&length);
360 c.print ("type","capsule");
361 c.print ("radius",radius);
362 c.print ("length",length);
363}
364
365
366static void printPlane (PrintingContext &c, dxGeom *g)
367{
368 dVector4 e;
369 dGeomPlaneGetParams (g,e);
370 c.print ("type","plane");
371 c.print ("normal",e);
372 c.print ("d",e[3]);
373}
374
375
376
377static void printRay (PrintingContext &c, dxGeom *g)
378{
379 dReal length = dGeomRayGetLength (g);
380 c.print ("type","ray");
381 c.print ("length",length);
382}
383
384
385
386static void printGeomTransform (PrintingContext &c, dxGeom *g)
387{
388 dxGeom *g2 = dGeomTransformGetGeom (g);
389 const dReal *pos = dGeomGetPosition (g2);
390 dQuaternion q;
391 dGeomGetQuaternion (g2,q);
392 c.print ("type","transform");
393 c.print ("pos",pos);
394 c.print ("q",q,4);
395 c.print ("geometry = {");
396 c.indent++;
397 printGeom (c,g2);
398 c.indent--;
399 c.print ("}");
400}
401
402
403
404static void printTriMesh (PrintingContext &c, dxGeom *g)
405{
406 c.print ("type","trimesh");
407 //@@@ i don't think that the trimesh accessor functions are really
408 // sufficient to read out all the triangle data, and anyway we
409 // should have a method of not duplicating trimesh data that is
410 // shared.
411}
412
413
414static void printGeom (PrintingContext &c, dxGeom *g)
415{
416 unsigned long category = dGeomGetCategoryBits (g);
417 if (category != (unsigned long)(~0)) {
418 c.printIndent();
419 fprintf (c.file,"category_bits = %lu\n",category);
420 }
421 unsigned long collide = dGeomGetCollideBits (g);
422 if (collide != (unsigned long)(~0)) {
423 c.printIndent();
424 fprintf (c.file,"collide_bits = %lu\n",collide);
425 }
426 if (!dGeomIsEnabled (g)) {
427 c.print ("disabled",1);
428 }
429 switch (g->type) {
430 case dSphereClass: printSphere (c,g); break;
431 case dBoxClass: printBox (c,g); break;
432 case dCapsuleClass: printCapsule (c,g); break;
433 case dPlaneClass: printPlane (c,g); break;
434 case dRayClass: printRay (c,g); break;
435 case dGeomTransformClass: printGeomTransform (c,g); break;
436 case dTriMeshClass: printTriMesh (c,g); break;
437 }
438}
439
440//***************************************************************************
441// world
442
443void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
444{
445 PrintingContext c;
446 c.file = file;
447#if defined(dSINGLE)
448 c.precision = 7;
449#else
450 c.precision = 15;
451#endif
452 c.indent = 1;
453
454 fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
455 c.print ("gravity",w->gravity);
456 c.print ("ODE = {");
457 c.indent++;
458 c.print ("ERP",w->global_erp);
459 c.print ("CFM",w->global_cfm);
460 c.print ("auto_disable = {");
461 c.indent++;
462 c.print ("linear_threshold",w->adis.linear_average_threshold);
463 c.print ("angular_threshold",w->adis.angular_average_threshold);
464 c.print ("average_samples",(int)w->adis.average_samples);
465 c.print ("idle_time",w->adis.idle_time);
466 c.print ("idle_steps",w->adis.idle_steps);
467 fprintf (file,"\t\t},\n\t},\n}\n");
468 c.indent -= 3;
469
470 // bodies
471 int num = 0;
472 fprintf (file,"%sbody = {}\n",prefix);
473 for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
474 b->tag = num;
475 fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
476 c.indent++;
477 c.print ("pos",b->posr.pos);
478 c.print ("q",b->q,4);
479 c.print ("lvel",b->lvel);
480 c.print ("avel",b->avel);
481 c.print ("mass",b->mass.mass);
482 fprintf (file,"\tI = {{");
483 for (int i=0; i<3; i++) {
484 for (int j=0; j<3; j++) {
485 c.printReal (b->mass.I[i*4+j]);
486 if (j < 2) fputc (',',file);
487 }
488 if (i < 2) fprintf (file,"},{");
489 }
490 fprintf (file,"}},\n");
491 c.printNonzero ("com",b->mass.c);
492 c.print ("ODE = {");
493 c.indent++;
494 if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
495 if (b->flags & dxBodyDisabled) c.print ("disabled",1);
496 if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
497 if (b->flags & dxBodyAutoDisable) {
498 c.print ("auto_disable = {");
499 c.indent++;
500 c.print ("linear_threshold",b->adis.linear_average_threshold);
501 c.print ("angular_threshold",b->adis.angular_average_threshold);
502 c.print ("average_samples",(int)b->adis.average_samples);
503 c.print ("idle_time",b->adis.idle_time);
504 c.print ("idle_steps",b->adis.idle_steps);
505 c.print ("time_left",b->adis_timeleft);
506 c.print ("steps_left",b->adis_stepsleft);
507 c.indent--;
508 c.print ("},");
509 }
510 c.printNonzero ("facc",b->facc);
511 c.printNonzero ("tacc",b->tacc);
512 if (b->flags & dxBodyFlagFiniteRotationAxis) {
513 c.print ("finite_rotation_axis",b->finite_rot_axis);
514 }
515 c.indent--;
516 c.print ("},");
517 if (b->geom) {
518 c.print ("geometry = {");
519 c.indent++;
520 for (dxGeom *g=b->geom; g; g=g->body_next) {
521 c.print ("{");
522 c.indent++;
523 printGeom (c,g);
524 c.indent--;
525 c.print ("},");
526 }
527 c.indent--;
528 c.print ("},");
529 }
530 c.indent--;
531 c.print ("}");
532 num++;
533 }
534
535 // joints
536 num = 0;
537 fprintf (file,"%sjoint = {}\n",prefix);
538 for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
539 c.indent++;
540 const char *name = getJointName (j);
541 fprintf (file,
542 "%sjoint[%d] = dynamics.%s_joint {\n"
543 "\tworld = %sworld,\n"
544 "\tbody = {"
545 ,prefix,num,name,prefix);
546
547 if ( j->node[0].body )
548 fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag);
549 if ( j->node[1].body )
550 fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
551
552 switch (j->vtable->typenum) {
553 case dJointTypeBall: printBall (c,j); break;
554 case dJointTypeHinge: printHinge (c,j); break;
555 case dJointTypeSlider: printSlider (c,j); break;
556 case dJointTypeContact: printContact (c,j); break;
557 case dJointTypeUniversal: printUniversal (c,j); break;
558 case dJointTypeHinge2: printHinge2 (c,j); break;
559 case dJointTypeFixed: printFixed (c,j); break;
560 case dJointTypeAMotor: printAMotor (c,j); break;
561 case dJointTypeLMotor: printLMotor (c,j); break;
562 case dJointTypePR: printPR (c,j); break;
563 }
564 c.indent--;
565 c.print ("}");
566 num++;
567 }
568}
diff --git a/libraries/ode-0.9/ode/src/fastdot.c b/libraries/ode-0.9/ode/src/fastdot.c
new file mode 100644
index 0000000..148d2dd
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/fastdot.c
@@ -0,0 +1,30 @@
1/* generated code, do not edit. */
2
3#include "ode/matrix.h"
4
5
6dReal dDot (const dReal *a, const dReal *b, int n)
7{
8 dReal p0,q0,m0,p1,q1,m1,sum;
9 sum = 0;
10 n -= 2;
11 while (n >= 0) {
12 p0 = a[0]; q0 = b[0];
13 m0 = p0 * q0;
14 p1 = a[1]; q1 = b[1];
15 m1 = p1 * q1;
16 sum += m0;
17 sum += m1;
18 a += 2;
19 b += 2;
20 n -= 2;
21 }
22 n += 2;
23 while (n > 0) {
24 sum += (*a) * (*b);
25 a++;
26 b++;
27 n--;
28 }
29 return sum;
30}
diff --git a/libraries/ode-0.9/ode/src/fastldlt.c b/libraries/ode-0.9/ode/src/fastldlt.c
new file mode 100644
index 0000000..df2ea6e
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/fastldlt.c
@@ -0,0 +1,381 @@
1/* generated code, do not edit. */
2
3#include "ode/matrix.h"
4
5/* solve L*X=B, with B containing 1 right hand sides.
6 * L is an n*n lower triangular matrix with ones on the diagonal.
7 * L is stored by rows and its leading dimension is lskip.
8 * B is an n*1 matrix that contains the right hand sides.
9 * B is stored by columns and its leading dimension is also lskip.
10 * B is overwritten with X.
11 * this processes blocks of 2*2.
12 * if this is in the factorizer source file, n must be a multiple of 2.
13 */
14
15static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1)
16{
17 /* declare variables - Z matrix, p and q vectors, etc */
18 dReal Z11,m11,Z21,m21,p1,q1,p2,*ex;
19 const dReal *ell;
20 int i,j;
21 /* compute all 2 x 1 blocks of X */
22 for (i=0; i < n; i+=2) {
23 /* compute all 2 x 1 block of X, from rows i..i+2-1 */
24 /* set the Z matrix to 0 */
25 Z11=0;
26 Z21=0;
27 ell = L + i*lskip1;
28 ex = B;
29 /* the inner loop that computes outer products and adds them to Z */
30 for (j=i-2; j >= 0; j -= 2) {
31 /* compute outer product and add it to the Z matrix */
32 p1=ell[0];
33 q1=ex[0];
34 m11 = p1 * q1;
35 p2=ell[lskip1];
36 m21 = p2 * q1;
37 Z11 += m11;
38 Z21 += m21;
39 /* compute outer product and add it to the Z matrix */
40 p1=ell[1];
41 q1=ex[1];
42 m11 = p1 * q1;
43 p2=ell[1+lskip1];
44 m21 = p2 * q1;
45 /* advance pointers */
46 ell += 2;
47 ex += 2;
48 Z11 += m11;
49 Z21 += m21;
50 /* end of inner loop */
51 }
52 /* compute left-over iterations */
53 j += 2;
54 for (; j > 0; j--) {
55 /* compute outer product and add it to the Z matrix */
56 p1=ell[0];
57 q1=ex[0];
58 m11 = p1 * q1;
59 p2=ell[lskip1];
60 m21 = p2 * q1;
61 /* advance pointers */
62 ell += 1;
63 ex += 1;
64 Z11 += m11;
65 Z21 += m21;
66 }
67 /* finish computing the X(i) block */
68 Z11 = ex[0] - Z11;
69 ex[0] = Z11;
70 p1 = ell[lskip1];
71 Z21 = ex[1] - Z21 - p1*Z11;
72 ex[1] = Z21;
73 /* end of outer loop */
74 }
75}
76
77/* solve L*X=B, with B containing 2 right hand sides.
78 * L is an n*n lower triangular matrix with ones on the diagonal.
79 * L is stored by rows and its leading dimension is lskip.
80 * B is an n*2 matrix that contains the right hand sides.
81 * B is stored by columns and its leading dimension is also lskip.
82 * B is overwritten with X.
83 * this processes blocks of 2*2.
84 * if this is in the factorizer source file, n must be a multiple of 2.
85 */
86
87static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1)
88{
89 /* declare variables - Z matrix, p and q vectors, etc */
90 dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
91 const dReal *ell;
92 int i,j;
93 /* compute all 2 x 2 blocks of X */
94 for (i=0; i < n; i+=2) {
95 /* compute all 2 x 2 block of X, from rows i..i+2-1 */
96 /* set the Z matrix to 0 */
97 Z11=0;
98 Z12=0;
99 Z21=0;
100 Z22=0;
101 ell = L + i*lskip1;
102 ex = B;
103 /* the inner loop that computes outer products and adds them to Z */
104 for (j=i-2; j >= 0; j -= 2) {
105 /* compute outer product and add it to the Z matrix */
106 p1=ell[0];
107 q1=ex[0];
108 m11 = p1 * q1;
109 q2=ex[lskip1];
110 m12 = p1 * q2;
111 p2=ell[lskip1];
112 m21 = p2 * q1;
113 m22 = p2 * q2;
114 Z11 += m11;
115 Z12 += m12;
116 Z21 += m21;
117 Z22 += m22;
118 /* compute outer product and add it to the Z matrix */
119 p1=ell[1];
120 q1=ex[1];
121 m11 = p1 * q1;
122 q2=ex[1+lskip1];
123 m12 = p1 * q2;
124 p2=ell[1+lskip1];
125 m21 = p2 * q1;
126 m22 = p2 * q2;
127 /* advance pointers */
128 ell += 2;
129 ex += 2;
130 Z11 += m11;
131 Z12 += m12;
132 Z21 += m21;
133 Z22 += m22;
134 /* end of inner loop */
135 }
136 /* compute left-over iterations */
137 j += 2;
138 for (; j > 0; j--) {
139 /* compute outer product and add it to the Z matrix */
140 p1=ell[0];
141 q1=ex[0];
142 m11 = p1 * q1;
143 q2=ex[lskip1];
144 m12 = p1 * q2;
145 p2=ell[lskip1];
146 m21 = p2 * q1;
147 m22 = p2 * q2;
148 /* advance pointers */
149 ell += 1;
150 ex += 1;
151 Z11 += m11;
152 Z12 += m12;
153 Z21 += m21;
154 Z22 += m22;
155 }
156 /* finish computing the X(i) block */
157 Z11 = ex[0] - Z11;
158 ex[0] = Z11;
159 Z12 = ex[lskip1] - Z12;
160 ex[lskip1] = Z12;
161 p1 = ell[lskip1];
162 Z21 = ex[1] - Z21 - p1*Z11;
163 ex[1] = Z21;
164 Z22 = ex[1+lskip1] - Z22 - p1*Z12;
165 ex[1+lskip1] = Z22;
166 /* end of outer loop */
167 }
168}
169
170
171void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
172{
173 int i,j;
174 dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
175 if (n < 1) return;
176
177 for (i=0; i<=n-2; i += 2) {
178 /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
179 dSolveL1_2 (A,A+i*nskip1,i,nskip1);
180 /* scale the elements in a 2 x i block at A(i,0), and also */
181 /* compute Z = the outer product matrix that we'll need. */
182 Z11 = 0;
183 Z21 = 0;
184 Z22 = 0;
185 ell = A+i*nskip1;
186 dee = d;
187 for (j=i-6; j >= 0; j -= 6) {
188 p1 = ell[0];
189 p2 = ell[nskip1];
190 dd = dee[0];
191 q1 = p1*dd;
192 q2 = p2*dd;
193 ell[0] = q1;
194 ell[nskip1] = q2;
195 m11 = p1*q1;
196 m21 = p2*q1;
197 m22 = p2*q2;
198 Z11 += m11;
199 Z21 += m21;
200 Z22 += m22;
201 p1 = ell[1];
202 p2 = ell[1+nskip1];
203 dd = dee[1];
204 q1 = p1*dd;
205 q2 = p2*dd;
206 ell[1] = q1;
207 ell[1+nskip1] = q2;
208 m11 = p1*q1;
209 m21 = p2*q1;
210 m22 = p2*q2;
211 Z11 += m11;
212 Z21 += m21;
213 Z22 += m22;
214 p1 = ell[2];
215 p2 = ell[2+nskip1];
216 dd = dee[2];
217 q1 = p1*dd;
218 q2 = p2*dd;
219 ell[2] = q1;
220 ell[2+nskip1] = q2;
221 m11 = p1*q1;
222 m21 = p2*q1;
223 m22 = p2*q2;
224 Z11 += m11;
225 Z21 += m21;
226 Z22 += m22;
227 p1 = ell[3];
228 p2 = ell[3+nskip1];
229 dd = dee[3];
230 q1 = p1*dd;
231 q2 = p2*dd;
232 ell[3] = q1;
233 ell[3+nskip1] = q2;
234 m11 = p1*q1;
235 m21 = p2*q1;
236 m22 = p2*q2;
237 Z11 += m11;
238 Z21 += m21;
239 Z22 += m22;
240 p1 = ell[4];
241 p2 = ell[4+nskip1];
242 dd = dee[4];
243 q1 = p1*dd;
244 q2 = p2*dd;
245 ell[4] = q1;
246 ell[4+nskip1] = q2;
247 m11 = p1*q1;
248 m21 = p2*q1;
249 m22 = p2*q2;
250 Z11 += m11;
251 Z21 += m21;
252 Z22 += m22;
253 p1 = ell[5];
254 p2 = ell[5+nskip1];
255 dd = dee[5];
256 q1 = p1*dd;
257 q2 = p2*dd;
258 ell[5] = q1;
259 ell[5+nskip1] = q2;
260 m11 = p1*q1;
261 m21 = p2*q1;
262 m22 = p2*q2;
263 Z11 += m11;
264 Z21 += m21;
265 Z22 += m22;
266 ell += 6;
267 dee += 6;
268 }
269 /* compute left-over iterations */
270 j += 6;
271 for (; j > 0; j--) {
272 p1 = ell[0];
273 p2 = ell[nskip1];
274 dd = dee[0];
275 q1 = p1*dd;
276 q2 = p2*dd;
277 ell[0] = q1;
278 ell[nskip1] = q2;
279 m11 = p1*q1;
280 m21 = p2*q1;
281 m22 = p2*q2;
282 Z11 += m11;
283 Z21 += m21;
284 Z22 += m22;
285 ell++;
286 dee++;
287 }
288 /* solve for diagonal 2 x 2 block at A(i,i) */
289 Z11 = ell[0] - Z11;
290 Z21 = ell[nskip1] - Z21;
291 Z22 = ell[1+nskip1] - Z22;
292 dee = d + i;
293 /* factorize 2 x 2 block Z,dee */
294 /* factorize row 1 */
295 dee[0] = dRecip(Z11);
296 /* factorize row 2 */
297 sum = 0;
298 q1 = Z21;
299 q2 = q1 * dee[0];
300 Z21 = q2;
301 sum += q1*q2;
302 dee[1] = dRecip(Z22 - sum);
303 /* done factorizing 2 x 2 block */
304 ell[nskip1] = Z21;
305 }
306 /* compute the (less than 2) rows at the bottom */
307 switch (n-i) {
308 case 0:
309 break;
310
311 case 1:
312 dSolveL1_1 (A,A+i*nskip1,i,nskip1);
313 /* scale the elements in a 1 x i block at A(i,0), and also */
314 /* compute Z = the outer product matrix that we'll need. */
315 Z11 = 0;
316 ell = A+i*nskip1;
317 dee = d;
318 for (j=i-6; j >= 0; j -= 6) {
319 p1 = ell[0];
320 dd = dee[0];
321 q1 = p1*dd;
322 ell[0] = q1;
323 m11 = p1*q1;
324 Z11 += m11;
325 p1 = ell[1];
326 dd = dee[1];
327 q1 = p1*dd;
328 ell[1] = q1;
329 m11 = p1*q1;
330 Z11 += m11;
331 p1 = ell[2];
332 dd = dee[2];
333 q1 = p1*dd;
334 ell[2] = q1;
335 m11 = p1*q1;
336 Z11 += m11;
337 p1 = ell[3];
338 dd = dee[3];
339 q1 = p1*dd;
340 ell[3] = q1;
341 m11 = p1*q1;
342 Z11 += m11;
343 p1 = ell[4];
344 dd = dee[4];
345 q1 = p1*dd;
346 ell[4] = q1;
347 m11 = p1*q1;
348 Z11 += m11;
349 p1 = ell[5];
350 dd = dee[5];
351 q1 = p1*dd;
352 ell[5] = q1;
353 m11 = p1*q1;
354 Z11 += m11;
355 ell += 6;
356 dee += 6;
357 }
358 /* compute left-over iterations */
359 j += 6;
360 for (; j > 0; j--) {
361 p1 = ell[0];
362 dd = dee[0];
363 q1 = p1*dd;
364 ell[0] = q1;
365 m11 = p1*q1;
366 Z11 += m11;
367 ell++;
368 dee++;
369 }
370 /* solve for diagonal 1 x 1 block at A(i,i) */
371 Z11 = ell[0] - Z11;
372 dee = d + i;
373 /* factorize 1 x 1 block Z,dee */
374 /* factorize row 1 */
375 dee[0] = dRecip(Z11);
376 /* done factorizing 1 x 1 block */
377 break;
378
379 default: *((char*)0)=0; /* this should never happen! */
380 }
381}
diff --git a/libraries/ode-0.9/ode/src/fastlsolve.c b/libraries/ode-0.9/ode/src/fastlsolve.c
new file mode 100644
index 0000000..0ae99d6
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/fastlsolve.c
@@ -0,0 +1,298 @@
1/* generated code, do not edit. */
2
3#include "ode/matrix.h"
4
5/* solve L*X=B, with B containing 1 right hand sides.
6 * L is an n*n lower triangular matrix with ones on the diagonal.
7 * L is stored by rows and its leading dimension is lskip.
8 * B is an n*1 matrix that contains the right hand sides.
9 * B is stored by columns and its leading dimension is also lskip.
10 * B is overwritten with X.
11 * this processes blocks of 4*4.
12 * if this is in the factorizer source file, n must be a multiple of 4.
13 */
14
15void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1)
16{
17 /* declare variables - Z matrix, p and q vectors, etc */
18 dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
19 const dReal *ell;
20 int lskip2,lskip3,i,j;
21 /* compute lskip values */
22 lskip2 = 2*lskip1;
23 lskip3 = 3*lskip1;
24 /* compute all 4 x 1 blocks of X */
25 for (i=0; i <= n-4; i+=4) {
26 /* compute all 4 x 1 block of X, from rows i..i+4-1 */
27 /* set the Z matrix to 0 */
28 Z11=0;
29 Z21=0;
30 Z31=0;
31 Z41=0;
32 ell = L + i*lskip1;
33 ex = B;
34 /* the inner loop that computes outer products and adds them to Z */
35 for (j=i-12; j >= 0; j -= 12) {
36 /* load p and q values */
37 p1=ell[0];
38 q1=ex[0];
39 p2=ell[lskip1];
40 p3=ell[lskip2];
41 p4=ell[lskip3];
42 /* compute outer product and add it to the Z matrix */
43 Z11 += p1 * q1;
44 Z21 += p2 * q1;
45 Z31 += p3 * q1;
46 Z41 += p4 * q1;
47 /* load p and q values */
48 p1=ell[1];
49 q1=ex[1];
50 p2=ell[1+lskip1];
51 p3=ell[1+lskip2];
52 p4=ell[1+lskip3];
53 /* compute outer product and add it to the Z matrix */
54 Z11 += p1 * q1;
55 Z21 += p2 * q1;
56 Z31 += p3 * q1;
57 Z41 += p4 * q1;
58 /* load p and q values */
59 p1=ell[2];
60 q1=ex[2];
61 p2=ell[2+lskip1];
62 p3=ell[2+lskip2];
63 p4=ell[2+lskip3];
64 /* compute outer product and add it to the Z matrix */
65 Z11 += p1 * q1;
66 Z21 += p2 * q1;
67 Z31 += p3 * q1;
68 Z41 += p4 * q1;
69 /* load p and q values */
70 p1=ell[3];
71 q1=ex[3];
72 p2=ell[3+lskip1];
73 p3=ell[3+lskip2];
74 p4=ell[3+lskip3];
75 /* compute outer product and add it to the Z matrix */
76 Z11 += p1 * q1;
77 Z21 += p2 * q1;
78 Z31 += p3 * q1;
79 Z41 += p4 * q1;
80 /* load p and q values */
81 p1=ell[4];
82 q1=ex[4];
83 p2=ell[4+lskip1];
84 p3=ell[4+lskip2];
85 p4=ell[4+lskip3];
86 /* compute outer product and add it to the Z matrix */
87 Z11 += p1 * q1;
88 Z21 += p2 * q1;
89 Z31 += p3 * q1;
90 Z41 += p4 * q1;
91 /* load p and q values */
92 p1=ell[5];
93 q1=ex[5];
94 p2=ell[5+lskip1];
95 p3=ell[5+lskip2];
96 p4=ell[5+lskip3];
97 /* compute outer product and add it to the Z matrix */
98 Z11 += p1 * q1;
99 Z21 += p2 * q1;
100 Z31 += p3 * q1;
101 Z41 += p4 * q1;
102 /* load p and q values */
103 p1=ell[6];
104 q1=ex[6];
105 p2=ell[6+lskip1];
106 p3=ell[6+lskip2];
107 p4=ell[6+lskip3];
108 /* compute outer product and add it to the Z matrix */
109 Z11 += p1 * q1;
110 Z21 += p2 * q1;
111 Z31 += p3 * q1;
112 Z41 += p4 * q1;
113 /* load p and q values */
114 p1=ell[7];
115 q1=ex[7];
116 p2=ell[7+lskip1];
117 p3=ell[7+lskip2];
118 p4=ell[7+lskip3];
119 /* compute outer product and add it to the Z matrix */
120 Z11 += p1 * q1;
121 Z21 += p2 * q1;
122 Z31 += p3 * q1;
123 Z41 += p4 * q1;
124 /* load p and q values */
125 p1=ell[8];
126 q1=ex[8];
127 p2=ell[8+lskip1];
128 p3=ell[8+lskip2];
129 p4=ell[8+lskip3];
130 /* compute outer product and add it to the Z matrix */
131 Z11 += p1 * q1;
132 Z21 += p2 * q1;
133 Z31 += p3 * q1;
134 Z41 += p4 * q1;
135 /* load p and q values */
136 p1=ell[9];
137 q1=ex[9];
138 p2=ell[9+lskip1];
139 p3=ell[9+lskip2];
140 p4=ell[9+lskip3];
141 /* compute outer product and add it to the Z matrix */
142 Z11 += p1 * q1;
143 Z21 += p2 * q1;
144 Z31 += p3 * q1;
145 Z41 += p4 * q1;
146 /* load p and q values */
147 p1=ell[10];
148 q1=ex[10];
149 p2=ell[10+lskip1];
150 p3=ell[10+lskip2];
151 p4=ell[10+lskip3];
152 /* compute outer product and add it to the Z matrix */
153 Z11 += p1 * q1;
154 Z21 += p2 * q1;
155 Z31 += p3 * q1;
156 Z41 += p4 * q1;
157 /* load p and q values */
158 p1=ell[11];
159 q1=ex[11];
160 p2=ell[11+lskip1];
161 p3=ell[11+lskip2];
162 p4=ell[11+lskip3];
163 /* compute outer product and add it to the Z matrix */
164 Z11 += p1 * q1;
165 Z21 += p2 * q1;
166 Z31 += p3 * q1;
167 Z41 += p4 * q1;
168 /* advance pointers */
169 ell += 12;
170 ex += 12;
171 /* end of inner loop */
172 }
173 /* compute left-over iterations */
174 j += 12;
175 for (; j > 0; j--) {
176 /* load p and q values */
177 p1=ell[0];
178 q1=ex[0];
179 p2=ell[lskip1];
180 p3=ell[lskip2];
181 p4=ell[lskip3];
182 /* compute outer product and add it to the Z matrix */
183 Z11 += p1 * q1;
184 Z21 += p2 * q1;
185 Z31 += p3 * q1;
186 Z41 += p4 * q1;
187 /* advance pointers */
188 ell += 1;
189 ex += 1;
190 }
191 /* finish computing the X(i) block */
192 Z11 = ex[0] - Z11;
193 ex[0] = Z11;
194 p1 = ell[lskip1];
195 Z21 = ex[1] - Z21 - p1*Z11;
196 ex[1] = Z21;
197 p1 = ell[lskip2];
198 p2 = ell[1+lskip2];
199 Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
200 ex[2] = Z31;
201 p1 = ell[lskip3];
202 p2 = ell[1+lskip3];
203 p3 = ell[2+lskip3];
204 Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
205 ex[3] = Z41;
206 /* end of outer loop */
207 }
208 /* compute rows at end that are not a multiple of block size */
209 for (; i < n; i++) {
210 /* compute all 1 x 1 block of X, from rows i..i+1-1 */
211 /* set the Z matrix to 0 */
212 Z11=0;
213 ell = L + i*lskip1;
214 ex = B;
215 /* the inner loop that computes outer products and adds them to Z */
216 for (j=i-12; j >= 0; j -= 12) {
217 /* load p and q values */
218 p1=ell[0];
219 q1=ex[0];
220 /* compute outer product and add it to the Z matrix */
221 Z11 += p1 * q1;
222 /* load p and q values */
223 p1=ell[1];
224 q1=ex[1];
225 /* compute outer product and add it to the Z matrix */
226 Z11 += p1 * q1;
227 /* load p and q values */
228 p1=ell[2];
229 q1=ex[2];
230 /* compute outer product and add it to the Z matrix */
231 Z11 += p1 * q1;
232 /* load p and q values */
233 p1=ell[3];
234 q1=ex[3];
235 /* compute outer product and add it to the Z matrix */
236 Z11 += p1 * q1;
237 /* load p and q values */
238 p1=ell[4];
239 q1=ex[4];
240 /* compute outer product and add it to the Z matrix */
241 Z11 += p1 * q1;
242 /* load p and q values */
243 p1=ell[5];
244 q1=ex[5];
245 /* compute outer product and add it to the Z matrix */
246 Z11 += p1 * q1;
247 /* load p and q values */
248 p1=ell[6];
249 q1=ex[6];
250 /* compute outer product and add it to the Z matrix */
251 Z11 += p1 * q1;
252 /* load p and q values */
253 p1=ell[7];
254 q1=ex[7];
255 /* compute outer product and add it to the Z matrix */
256 Z11 += p1 * q1;
257 /* load p and q values */
258 p1=ell[8];
259 q1=ex[8];
260 /* compute outer product and add it to the Z matrix */
261 Z11 += p1 * q1;
262 /* load p and q values */
263 p1=ell[9];
264 q1=ex[9];
265 /* compute outer product and add it to the Z matrix */
266 Z11 += p1 * q1;
267 /* load p and q values */
268 p1=ell[10];
269 q1=ex[10];
270 /* compute outer product and add it to the Z matrix */
271 Z11 += p1 * q1;
272 /* load p and q values */
273 p1=ell[11];
274 q1=ex[11];
275 /* compute outer product and add it to the Z matrix */
276 Z11 += p1 * q1;
277 /* advance pointers */
278 ell += 12;
279 ex += 12;
280 /* end of inner loop */
281 }
282 /* compute left-over iterations */
283 j += 12;
284 for (; j > 0; j--) {
285 /* load p and q values */
286 p1=ell[0];
287 q1=ex[0];
288 /* compute outer product and add it to the Z matrix */
289 Z11 += p1 * q1;
290 /* advance pointers */
291 ell += 1;
292 ex += 1;
293 }
294 /* finish computing the X(i) block */
295 Z11 = ex[0] - Z11;
296 ex[0] = Z11;
297 }
298}
diff --git a/libraries/ode-0.9/ode/src/fastltsolve.c b/libraries/ode-0.9/ode/src/fastltsolve.c
new file mode 100644
index 0000000..eb950f6
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/fastltsolve.c
@@ -0,0 +1,199 @@
1/* generated code, do not edit. */
2
3#include "ode/matrix.h"
4
5/* solve L^T * x=b, with b containing 1 right hand side.
6 * L is an n*n lower triangular matrix with ones on the diagonal.
7 * L is stored by rows and its leading dimension is lskip.
8 * b is an n*1 matrix that contains the right hand side.
9 * b is overwritten with x.
10 * this processes blocks of 4.
11 */
12
13void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1)
14{
15 /* declare variables - Z matrix, p and q vectors, etc */
16 dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
17 const dReal *ell;
18 int lskip2,lskip3,i,j;
19 /* special handling for L and B because we're solving L1 *transpose* */
20 L = L + (n-1)*(lskip1+1);
21 B = B + n-1;
22 lskip1 = -lskip1;
23 /* compute lskip values */
24 lskip2 = 2*lskip1;
25 lskip3 = 3*lskip1;
26 /* compute all 4 x 1 blocks of X */
27 for (i=0; i <= n-4; i+=4) {
28 /* compute all 4 x 1 block of X, from rows i..i+4-1 */
29 /* set the Z matrix to 0 */
30 Z11=0;
31 Z21=0;
32 Z31=0;
33 Z41=0;
34 ell = L - i;
35 ex = B;
36 /* the inner loop that computes outer products and adds them to Z */
37 for (j=i-4; j >= 0; j -= 4) {
38 /* load p and q values */
39 p1=ell[0];
40 q1=ex[0];
41 p2=ell[-1];
42 p3=ell[-2];
43 p4=ell[-3];
44 /* compute outer product and add it to the Z matrix */
45 m11 = p1 * q1;
46 m21 = p2 * q1;
47 m31 = p3 * q1;
48 m41 = p4 * q1;
49 ell += lskip1;
50 Z11 += m11;
51 Z21 += m21;
52 Z31 += m31;
53 Z41 += m41;
54 /* load p and q values */
55 p1=ell[0];
56 q1=ex[-1];
57 p2=ell[-1];
58 p3=ell[-2];
59 p4=ell[-3];
60 /* compute outer product and add it to the Z matrix */
61 m11 = p1 * q1;
62 m21 = p2 * q1;
63 m31 = p3 * q1;
64 m41 = p4 * q1;
65 ell += lskip1;
66 Z11 += m11;
67 Z21 += m21;
68 Z31 += m31;
69 Z41 += m41;
70 /* load p and q values */
71 p1=ell[0];
72 q1=ex[-2];
73 p2=ell[-1];
74 p3=ell[-2];
75 p4=ell[-3];
76 /* compute outer product and add it to the Z matrix */
77 m11 = p1 * q1;
78 m21 = p2 * q1;
79 m31 = p3 * q1;
80 m41 = p4 * q1;
81 ell += lskip1;
82 Z11 += m11;
83 Z21 += m21;
84 Z31 += m31;
85 Z41 += m41;
86 /* load p and q values */
87 p1=ell[0];
88 q1=ex[-3];
89 p2=ell[-1];
90 p3=ell[-2];
91 p4=ell[-3];
92 /* compute outer product and add it to the Z matrix */
93 m11 = p1 * q1;
94 m21 = p2 * q1;
95 m31 = p3 * q1;
96 m41 = p4 * q1;
97 ell += lskip1;
98 ex -= 4;
99 Z11 += m11;
100 Z21 += m21;
101 Z31 += m31;
102 Z41 += m41;
103 /* end of inner loop */
104 }
105 /* compute left-over iterations */
106 j += 4;
107 for (; j > 0; j--) {
108 /* load p and q values */
109 p1=ell[0];
110 q1=ex[0];
111 p2=ell[-1];
112 p3=ell[-2];
113 p4=ell[-3];
114 /* compute outer product and add it to the Z matrix */
115 m11 = p1 * q1;
116 m21 = p2 * q1;
117 m31 = p3 * q1;
118 m41 = p4 * q1;
119 ell += lskip1;
120 ex -= 1;
121 Z11 += m11;
122 Z21 += m21;
123 Z31 += m31;
124 Z41 += m41;
125 }
126 /* finish computing the X(i) block */
127 Z11 = ex[0] - Z11;
128 ex[0] = Z11;
129 p1 = ell[-1];
130 Z21 = ex[-1] - Z21 - p1*Z11;
131 ex[-1] = Z21;
132 p1 = ell[-2];
133 p2 = ell[-2+lskip1];
134 Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
135 ex[-2] = Z31;
136 p1 = ell[-3];
137 p2 = ell[-3+lskip1];
138 p3 = ell[-3+lskip2];
139 Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
140 ex[-3] = Z41;
141 /* end of outer loop */
142 }
143 /* compute rows at end that are not a multiple of block size */
144 for (; i < n; i++) {
145 /* compute all 1 x 1 block of X, from rows i..i+1-1 */
146 /* set the Z matrix to 0 */
147 Z11=0;
148 ell = L - i;
149 ex = B;
150 /* the inner loop that computes outer products and adds them to Z */
151 for (j=i-4; j >= 0; j -= 4) {
152 /* load p and q values */
153 p1=ell[0];
154 q1=ex[0];
155 /* compute outer product and add it to the Z matrix */
156 m11 = p1 * q1;
157 ell += lskip1;
158 Z11 += m11;
159 /* load p and q values */
160 p1=ell[0];
161 q1=ex[-1];
162 /* compute outer product and add it to the Z matrix */
163 m11 = p1 * q1;
164 ell += lskip1;
165 Z11 += m11;
166 /* load p and q values */
167 p1=ell[0];
168 q1=ex[-2];
169 /* compute outer product and add it to the Z matrix */
170 m11 = p1 * q1;
171 ell += lskip1;
172 Z11 += m11;
173 /* load p and q values */
174 p1=ell[0];
175 q1=ex[-3];
176 /* compute outer product and add it to the Z matrix */
177 m11 = p1 * q1;
178 ell += lskip1;
179 ex -= 4;
180 Z11 += m11;
181 /* end of inner loop */
182 }
183 /* compute left-over iterations */
184 j += 4;
185 for (; j > 0; j--) {
186 /* load p and q values */
187 p1=ell[0];
188 q1=ex[0];
189 /* compute outer product and add it to the Z matrix */
190 m11 = p1 * q1;
191 ell += lskip1;
192 ex -= 1;
193 Z11 += m11;
194 }
195 /* finish computing the X(i) block */
196 Z11 = ex[0] - Z11;
197 ex[0] = Z11;
198 }
199}
diff --git a/libraries/ode-0.9/ode/src/heightfield.cpp b/libraries/ode-0.9/ode/src/heightfield.cpp
new file mode 100644
index 0000000..36755fc
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/heightfield.cpp
@@ -0,0 +1,1812 @@
1// dHeightfield Collider
2// Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com
3// Martijn Buijs 2006 http://home.planet.nl/~buijs512/
4// Based on Terrain & Cone contrib by:
5// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
6// Some code inspired by Magic Software
7
8
9#include <ode/common.h>
10#include <ode/collision.h>
11#include <ode/matrix.h>
12#include <ode/rotation.h>
13#include <ode/odemath.h>
14#include "collision_kernel.h"
15#include "collision_std.h"
16#include "collision_util.h"
17#include "heightfield.h"
18
19
20
21#if dTRIMESH_ENABLED
22#include "collision_trimesh_internal.h"
23#endif // dTRIMESH_ENABLED
24
25#define TERRAINTOL REAL(0.0)
26
27#define dMIN(A,B) ((A)>(B) ? B : A)
28#define dMAX(A,B) ((A)>(B) ? A : B)
29
30
31// Three-way MIN and MAX
32#define dMIN3(A,B,C) ( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) )
33#define dMAX3(A,B,C) ( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) )
34
35#define dOPESIGN(a, op1, op2,b) \
36 (a)[0] op1 op2 ((b)[0]); \
37 (a)[1] op1 op2 ((b)[1]); \
38 (a)[2] op1 op2 ((b)[2]);
39
40#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \
41 \
42 dVector3Copy (MyPoint, myRay.final_posr->pos); \
43 myRay.final_posr->R[2] = MyVector[0]; \
44 myRay.final_posr->R[6] = MyVector[1]; \
45 myRay.final_posr->R[10] = MyVector[2]; \
46 dGeomMoved (&myRay); \
47 }
48
49#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \
50 \
51 MyPlane->p[0] = MyPlaneDef[0]; \
52 MyPlane->p[1] = MyPlaneDef[1]; \
53 MyPlane->p[2] = MyPlaneDef[2]; \
54 MyPlane->p[3] = MyPlaneDef[3]; \
55 dGeomMoved (MyPlane); \
56 }
57//////// Local Build Option ////////////////////////////////////////////////////
58
59// Uncomment this #define to use the (0,0) corner of the geom as the origin,
60// rather than the center. This was the way the original heightfield worked,
61// but as it does not match the way all other geometries work, so for constancy it
62// was changed to work like this.
63
64// #define DHEIGHTFIELD_CORNER_ORIGIN
65
66
67// Uncomment this #define to add heightfield triangles edge colliding
68// Code is not guaranteed and I didn't find the need to add that as
69// colliding planes triangles and edge triangles seems enough.
70// #define _HEIGHTFIELDEDGECOLLIDING
71
72
73//////// dxHeightfieldData /////////////////////////////////////////////////////////////
74
75// dxHeightfieldData constructor
76dxHeightfieldData::dxHeightfieldData()
77{
78 //
79}
80
81
82// build Heightfield data
83void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples,
84 dReal fWidth, dReal fDepth,
85 dReal fScale, dReal fOffset, dReal fThickness,
86 int bWrapMode )
87{
88 dIASSERT( fWidth > REAL( 0.0 ) );
89 dIASSERT( fDepth > REAL( 0.0 ) );
90 dIASSERT( nWidthSamples > 0 );
91 dIASSERT( nDepthSamples > 0 );
92
93 // x,z bounds
94 m_fWidth = fWidth;
95 m_fDepth = fDepth;
96
97 // cache half x,z bounds
98 m_fHalfWidth = fWidth / REAL( 2.0 );
99 m_fHalfDepth = fDepth / REAL( 2.0 );
100
101 // scale and offset
102 m_fScale = fScale;
103 m_fOffset = fOffset;
104
105 // infinite min height bounds
106 m_fThickness = fThickness;
107
108 // number of vertices per side
109 m_nWidthSamples = nWidthSamples;
110 m_nDepthSamples = nDepthSamples;
111
112 m_fSampleWidth = m_fWidth / ( m_nWidthSamples - 1 );
113 m_fSampleDepth = m_fDepth / ( m_nDepthSamples - 1 );
114
115 m_fInvSampleWidth = 1 / m_fSampleWidth;
116 m_fInvSampleDepth = 1 / m_fSampleDepth;
117
118 // finite or repeated terrain?
119 m_bWrapMode = bWrapMode;
120}
121
122
123// recomputes heights bounds
124void dxHeightfieldData::ComputeHeightBounds()
125{
126 static int i;
127 static dReal h;
128 static unsigned char *data_byte;
129 static short *data_short;
130 static float *data_float;
131 static double *data_double;
132
133 switch ( m_nGetHeightMode )
134 {
135
136 // callback
137 case 0:
138 // change nothing, keep using default or user specified bounds
139 return;
140
141 // byte
142 case 1:
143 data_byte = (unsigned char*)m_pHeightData;
144 m_fMinHeight = dInfinity;
145 m_fMaxHeight = -dInfinity;
146
147 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
148 {
149 h = data_byte[i];
150 if (h < m_fMinHeight) m_fMinHeight = h;
151 if (h > m_fMaxHeight) m_fMaxHeight = h;
152 }
153
154 break;
155
156 // short
157 case 2:
158 data_short = (short*)m_pHeightData;
159 m_fMinHeight = dInfinity;
160 m_fMaxHeight = -dInfinity;
161
162 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
163 {
164 h = data_short[i];
165 if (h < m_fMinHeight) m_fMinHeight = h;
166 if (h > m_fMaxHeight) m_fMaxHeight = h;
167 }
168
169 break;
170
171 // float
172 case 3:
173 data_float = (float*)m_pHeightData;
174 m_fMinHeight = dInfinity;
175 m_fMaxHeight = -dInfinity;
176
177 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
178 {
179 h = data_float[i];
180 if (h < m_fMinHeight) m_fMinHeight = h;
181 if (h > m_fMaxHeight) m_fMaxHeight = h;
182 }
183
184 break;
185
186 // double
187 case 4:
188 data_double = (double*)m_pHeightData;
189 m_fMinHeight = dInfinity;
190 m_fMaxHeight = -dInfinity;
191
192 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
193 {
194 h = static_cast< dReal >( data_double[i] );
195 if (h < m_fMinHeight) m_fMinHeight = h;
196 if (h > m_fMaxHeight) m_fMaxHeight = h;
197 }
198
199 break;
200
201 }
202
203 // scale and offset
204 m_fMinHeight *= m_fScale;
205 m_fMaxHeight *= m_fScale;
206 m_fMinHeight += m_fOffset;
207 m_fMaxHeight += m_fOffset;
208
209 // add thickness
210 m_fMinHeight -= m_fThickness;
211}
212
213
214// returns whether point is over terrain Cell triangle?
215bool dxHeightfieldData::IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
216{
217 {
218 const dReal MaxX = CellOrigin[0] + m_fSampleWidth;
219 const dReal TolX = m_fSampleWidth * TERRAINTOL;
220 if ((pos[0]<CellOrigin[0]-TolX) || (pos[0]>MaxX+TolX))
221 return false;
222 }
223
224 {
225 const dReal MaxZ = CellOrigin[2] + m_fSampleDepth;
226 const dReal TolZ = m_fSampleDepth * TERRAINTOL;
227 if ((pos[2]<CellOrigin[2]-TolZ) || (pos[2]>MaxZ+TolZ))
228 return false;
229 }
230
231 // add X percentage position on cell and Z percentage position on cell
232 const dReal pctTotal = (pos[0] - CellOrigin[0]) * m_fInvSampleWidth
233 + (pos[2] - CellOrigin[2]) * m_fInvSampleDepth;
234
235 if (isABC)
236 {
237 if (pctTotal >= REAL(1.0) + TERRAINTOL)
238 return false;
239 else
240 return true;
241 }
242 else if (pctTotal <= REAL(1.0) - TERRAINTOL)
243 {
244 return false;
245 }
246 return true;
247}
248// returns whether point is over terrain Cell triangle?
249bool dxHeightfieldData::IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
250{
251 dReal MaxX, MinX;
252 dReal MaxZ, MinZ;
253 if (isABC)
254 {
255 // point A
256 MinX = CellOrigin[0];
257 MaxX = CellOrigin[0] + m_fSampleWidth;
258
259 MinZ = CellOrigin[2];
260 MaxZ = CellOrigin[2] + m_fSampleDepth;
261 }
262 else
263 {
264 // point D
265 MinX = CellOrigin[0] - m_fSampleWidth;
266 MaxX = CellOrigin[0];
267
268 MinZ = CellOrigin[2] - m_fSampleDepth;
269 MaxZ = CellOrigin[2];
270 }
271
272 // check if inside CELL
273 {
274 const dReal TolX = m_fSampleWidth * TERRAINTOL;
275 if ((pos[0]<MinX-TolX) || (pos[0]>MaxX+TolX))
276 return false;
277 }
278
279 {
280 const dReal TolZ = m_fSampleDepth * TERRAINTOL;
281 if ((pos[2]<MinZ-TolZ) || (pos[2]>MaxZ+TolZ))
282 return false;
283 }
284
285 // Sum up X percentage position on cell and Z percentage position on cell
286 const dReal pctTotal = (pos[0] - MinX) * m_fInvSampleWidth
287 + (pos[2] - MinZ) * m_fInvSampleDepth;
288
289 // check if inside respective Triangle of Cell
290 if (isABC)
291 {
292 if (pctTotal >= REAL(1.0) + TERRAINTOL)
293 return false;
294 else
295 return true;
296 }
297 else if (pctTotal <= REAL(1.0) - TERRAINTOL)
298 {
299 return false;
300 }
301 return true;
302}
303
304
305// returns height at given sample coordinates
306dReal dxHeightfieldData::GetHeight( int x, int z )
307{
308 static dReal h;
309 static unsigned char *data_byte;
310 static short *data_short;
311 static float *data_float;
312 static double *data_double;
313
314 if ( m_bWrapMode == 0 )
315 {
316 // Finite
317 if ( x < 0 ) x = 0;
318 if ( z < 0 ) z = 0;
319 if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1;
320 if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1;
321 }
322 else
323 {
324 // Infinite
325 x %= m_nWidthSamples - 1;
326 z %= m_nDepthSamples - 1;
327 if ( x < 0 ) x += m_nWidthSamples - 1;
328 if ( z < 0 ) z += m_nDepthSamples - 1;
329 }
330
331 switch ( m_nGetHeightMode )
332 {
333
334 // callback (dReal)
335 case 0:
336 h = (*m_pGetHeightCallback)(m_pUserData, x, z);
337 break;
338
339 // byte
340 case 1:
341 data_byte = (unsigned char*)m_pHeightData;
342 h = data_byte[x+(z * m_nWidthSamples)];
343 break;
344
345 // short
346 case 2:
347 data_short = (short*)m_pHeightData;
348 h = data_short[x+(z * m_nWidthSamples)];
349 break;
350
351 // float
352 case 3:
353 data_float = (float*)m_pHeightData;
354 h = data_float[x+(z * m_nWidthSamples)];
355 break;
356
357 // double
358 case 4:
359 data_double = (double*)m_pHeightData;
360 h = static_cast< dReal >( data_double[x+(z * m_nWidthSamples)] );
361 break;
362 }
363
364 return (h * m_fScale) + m_fOffset;
365}
366
367
368// returns height at given coordinates
369dReal dxHeightfieldData::GetHeight( dReal x, dReal z )
370{
371 dReal dnX = dFloor( x * m_fInvSampleWidth );
372 dReal dnZ = dFloor( z * m_fInvSampleDepth );
373
374 dReal dx = ( x - ( dnX * m_fSampleWidth ) ) * m_fInvSampleWidth;
375 dReal dz = ( z - ( dnZ * m_fSampleDepth ) ) * m_fInvSampleDepth;
376
377 int nX = int( dnX );
378 int nZ = int( dnZ );
379
380 //dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) );
381 //dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) );
382
383 dReal y, y0;
384
385 if ( dx + dz < REAL( 1.0 ) )
386 {
387 y0 = GetHeight( nX, nZ );
388
389 y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * dx
390 + ( GetHeight( nX, nZ + 1 ) - y0 ) * dz;
391 }
392 else
393 {
394 y0 = GetHeight( nX + 1, nZ + 1 );
395
396 y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * ( REAL(1.0) - dz ) +
397 ( GetHeight( nX, nZ + 1 ) - y0 ) * ( REAL(1.0) - dx );
398 }
399
400 return y;
401}
402
403
404// dxHeightfieldData destructor
405dxHeightfieldData::~dxHeightfieldData()
406{
407 static unsigned char *data_byte;
408 static short *data_short;
409 static float *data_float;
410 static double *data_double;
411
412 dIASSERT( m_pHeightData );
413
414 if ( m_bCopyHeightData )
415 {
416 switch ( m_nGetHeightMode )
417 {
418
419 // callback
420 case 0:
421 // do nothing
422 break;
423
424 // byte
425 case 1:
426 data_byte = (unsigned char*)m_pHeightData;
427 delete [] data_byte;
428 break;
429
430 // short
431 case 2:
432 data_short = (short*)m_pHeightData;
433 delete [] data_short;
434 break;
435
436 // float
437 case 3:
438 data_float = (float*)m_pHeightData;
439 delete [] data_float;
440 break;
441
442 // double
443 case 4:
444 data_double = (double*)m_pHeightData;
445 delete [] data_double;
446 break;
447
448 }
449 }
450}
451
452
453//////// dxHeightfield /////////////////////////////////////////////////////////////////
454
455
456// dxHeightfield constructor
457dxHeightfield::dxHeightfield( dSpaceID space,
458 dHeightfieldDataID data,
459 int bPlaceable ) :
460 dxGeom( space, bPlaceable ),
461 tempPlaneBuffer(0),
462 tempPlaneInstances(0),
463 tempPlaneBufferSize(0),
464 tempTriangleBuffer(0),
465 tempTriangleBufferSize(0),
466 tempHeightBuffer(0),
467 tempHeightInstances(0),
468 tempHeightBufferSizeX(0),
469 tempHeightBufferSizeZ(0)
470{
471 type = dHeightfieldClass;
472 this->m_p_data = data;
473}
474
475
476// compute axis aligned bounding box
477void dxHeightfield::computeAABB()
478{
479 const dxHeightfieldData *d = m_p_data;
480
481 if ( d->m_bWrapMode == 0 )
482 {
483 // Finite
484 if ( gflags & GEOM_PLACEABLE )
485 {
486 dReal dx[6], dy[6], dz[6];
487
488 // Y-axis
489 dy[0] = ( final_posr->R[ 1] * d->m_fMinHeight );
490 dy[1] = ( final_posr->R[ 5] * d->m_fMinHeight );
491 dy[2] = ( final_posr->R[ 9] * d->m_fMinHeight );
492 dy[3] = ( final_posr->R[ 1] * d->m_fMaxHeight );
493 dy[4] = ( final_posr->R[ 5] * d->m_fMaxHeight );
494 dy[5] = ( final_posr->R[ 9] * d->m_fMaxHeight );
495
496#ifdef DHEIGHTFIELD_CORNER_ORIGIN
497
498 // X-axis
499 dx[0] = 0; dx[3] = ( final_posr->R[ 0] * d->m_fWidth );
500 dx[1] = 0; dx[4] = ( final_posr->R[ 4] * d->m_fWidth );
501 dx[2] = 0; dx[5] = ( final_posr->R[ 8] * d->m_fWidth );
502
503 // Z-axis
504 dz[0] = 0; dz[3] = ( final_posr->R[ 2] * d->m_fDepth );
505 dz[1] = 0; dz[4] = ( final_posr->R[ 6] * d->m_fDepth );
506 dz[2] = 0; dz[5] = ( final_posr->R[10] * d->m_fDepth );
507
508#else // DHEIGHTFIELD_CORNER_ORIGIN
509
510 // X-axis
511 dx[0] = ( final_posr->R[ 0] * -d->m_fHalfWidth );
512 dx[1] = ( final_posr->R[ 4] * -d->m_fHalfWidth );
513 dx[2] = ( final_posr->R[ 8] * -d->m_fHalfWidth );
514 dx[3] = ( final_posr->R[ 0] * d->m_fHalfWidth );
515 dx[4] = ( final_posr->R[ 4] * d->m_fHalfWidth );
516 dx[5] = ( final_posr->R[ 8] * d->m_fHalfWidth );
517
518 // Z-axis
519 dz[0] = ( final_posr->R[ 2] * -d->m_fHalfDepth );
520 dz[1] = ( final_posr->R[ 6] * -d->m_fHalfDepth );
521 dz[2] = ( final_posr->R[10] * -d->m_fHalfDepth );
522 dz[3] = ( final_posr->R[ 2] * d->m_fHalfDepth );
523 dz[4] = ( final_posr->R[ 6] * d->m_fHalfDepth );
524 dz[5] = ( final_posr->R[10] * d->m_fHalfDepth );
525
526#endif // DHEIGHTFIELD_CORNER_ORIGIN
527
528 // X extents
529 aabb[0] = final_posr->pos[0] +
530 dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) );
531 aabb[1] = final_posr->pos[0] +
532 dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) );
533
534 // Y extents
535 aabb[2] = final_posr->pos[1] +
536 dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) );
537 aabb[3] = final_posr->pos[1] +
538 dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) );
539
540 // Z extents
541 aabb[4] = final_posr->pos[2] +
542 dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) );
543 aabb[5] = final_posr->pos[2] +
544 dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) );
545 }
546 else
547 {
548
549#ifdef DHEIGHTFIELD_CORNER_ORIGIN
550
551 aabb[0] = 0; aabb[1] = d->m_fWidth;
552 aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
553 aabb[4] = 0; aabb[5] = d->m_fDepth;
554
555#else // DHEIGHTFIELD_CORNER_ORIGIN
556
557 aabb[0] = -d->m_fHalfWidth; aabb[1] = +d->m_fHalfWidth;
558 aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
559 aabb[4] = -d->m_fHalfDepth; aabb[5] = +d->m_fHalfDepth;
560
561#endif // DHEIGHTFIELD_CORNER_ORIGIN
562
563 }
564 }
565 else
566 {
567 // Infinite
568 if ( gflags & GEOM_PLACEABLE )
569 {
570 aabb[0] = -dInfinity; aabb[1] = +dInfinity;
571 aabb[2] = -dInfinity; aabb[3] = +dInfinity;
572 aabb[4] = -dInfinity; aabb[5] = +dInfinity;
573 }
574 else
575 {
576 aabb[0] = -dInfinity; aabb[1] = +dInfinity;
577 aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
578 aabb[4] = -dInfinity; aabb[5] = +dInfinity;
579 }
580 }
581
582}
583
584
585// dxHeightfield destructor
586dxHeightfield::~dxHeightfield()
587{
588 resetTriangleBuffer();
589 resetPlaneBuffer();
590 resetHeightBuffer();
591}
592
593void dxHeightfield::allocateTriangleBuffer(size_t numTri)
594{
595 size_t alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT);
596 tempTriangleBufferSize = alignedNumTri;
597 tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri];
598}
599
600void dxHeightfield::resetTriangleBuffer()
601{
602 delete[] tempTriangleBuffer;
603}
604
605void dxHeightfield::allocatePlaneBuffer(size_t numTri)
606{
607 size_t alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT);
608 tempPlaneBufferSize = alignedNumTri;
609 tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri];
610 tempPlaneInstances = new HeightFieldPlane[alignedNumTri];
611
612 HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances;
613 for (size_t indexTri = 0; indexTri != alignedNumTri; indexTri++)
614 {
615 tempPlaneBuffer[indexTri] = ptrPlaneMatrix;
616 ptrPlaneMatrix += 1;
617 }
618}
619
620void dxHeightfield::resetPlaneBuffer()
621{
622 delete[] tempPlaneInstances;
623 delete[] tempPlaneBuffer;
624}
625
626void dxHeightfield::allocateHeightBuffer(size_t numX, size_t numZ)
627{
628 size_t alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X);
629 size_t alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z);
630 tempHeightBufferSizeX = alignedNumX;
631 tempHeightBufferSizeZ = alignedNumZ;
632 tempHeightBuffer = new HeightFieldVertex *[alignedNumX];
633 size_t numCells = alignedNumX * alignedNumZ;
634 tempHeightInstances = new HeightFieldVertex [numCells];
635
636 HeightFieldVertex *ptrHeightMatrix = tempHeightInstances;
637 for (size_t indexX = 0; indexX != alignedNumX; indexX++)
638 {
639 tempHeightBuffer[indexX] = ptrHeightMatrix;
640 ptrHeightMatrix += alignedNumZ;
641 }
642}
643
644void dxHeightfield::resetHeightBuffer()
645{
646 delete[] tempHeightInstances;
647 delete[] tempHeightBuffer;
648}
649//////// Heightfield data interface ////////////////////////////////////////////////////
650
651
652dHeightfieldDataID dGeomHeightfieldDataCreate()
653{
654 return new dxHeightfieldData();
655}
656
657
658void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,
659 void* pUserData, dHeightfieldGetHeight* pCallback,
660 dReal width, dReal depth, int widthSamples, int depthSamples,
661 dReal scale, dReal offset, dReal thickness, int bWrap )
662{
663 dUASSERT( d, "argument not Heightfield data" );
664 dIASSERT( pCallback );
665 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
666 dIASSERT( depthSamples >= 2 );
667
668 // callback
669 d->m_nGetHeightMode = 0;
670 d->m_pUserData = pUserData;
671 d->m_pGetHeightCallback = pCallback;
672
673 // set info
674 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
675
676 // default bounds
677 d->m_fMinHeight = -dInfinity;
678 d->m_fMaxHeight = dInfinity;
679}
680
681
682void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d,
683 const unsigned char *pHeightData, int bCopyHeightData,
684 dReal width, dReal depth, int widthSamples, int depthSamples,
685 dReal scale, dReal offset, dReal thickness, int bWrap )
686{
687 dUASSERT( d, "Argument not Heightfield data" );
688 dIASSERT( pHeightData );
689 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
690 dIASSERT( depthSamples >= 2 );
691
692 // set info
693 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
694 d->m_nGetHeightMode = 1;
695 d->m_bCopyHeightData = bCopyHeightData;
696
697 if ( d->m_bCopyHeightData == 0 )
698 {
699 // Data is referenced only.
700 d->m_pHeightData = pHeightData;
701 }
702 else
703 {
704 // We own the height data, allocate storage
705 d->m_pHeightData = new unsigned char[ d->m_nWidthSamples * d->m_nDepthSamples ];
706 dIASSERT( d->m_pHeightData );
707
708 // Copy data.
709 memcpy( (void*)d->m_pHeightData, pHeightData,
710 sizeof( unsigned char ) * d->m_nWidthSamples * d->m_nDepthSamples );
711 }
712
713 // Find height bounds
714 d->ComputeHeightBounds();
715}
716
717
718void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d,
719 const short* pHeightData, int bCopyHeightData,
720 dReal width, dReal depth, int widthSamples, int depthSamples,
721 dReal scale, dReal offset, dReal thickness, int bWrap )
722{
723 dUASSERT( d, "Argument not Heightfield data" );
724 dIASSERT( pHeightData );
725 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
726 dIASSERT( depthSamples >= 2 );
727
728 // set info
729 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
730 d->m_nGetHeightMode = 2;
731 d->m_bCopyHeightData = bCopyHeightData;
732
733 if ( d->m_bCopyHeightData == 0 )
734 {
735 // Data is referenced only.
736 d->m_pHeightData = pHeightData;
737 }
738 else
739 {
740 // We own the height data, allocate storage
741 d->m_pHeightData = new short[ d->m_nWidthSamples * d->m_nDepthSamples ];
742 dIASSERT( d->m_pHeightData );
743
744 // Copy data.
745 memcpy( (void*)d->m_pHeightData, pHeightData,
746 sizeof( short ) * d->m_nWidthSamples * d->m_nDepthSamples );
747 }
748
749 // Find height bounds
750 d->ComputeHeightBounds();
751}
752
753
754void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d,
755 const float *pHeightData, int bCopyHeightData,
756 dReal width, dReal depth, int widthSamples, int depthSamples,
757 dReal scale, dReal offset, dReal thickness, int bWrap )
758{
759 dUASSERT( d, "Argument not Heightfield data" );
760 dIASSERT( pHeightData );
761 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
762 dIASSERT( depthSamples >= 2 );
763
764 // set info
765 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
766 d->m_nGetHeightMode = 3;
767 d->m_bCopyHeightData = bCopyHeightData;
768
769 if ( d->m_bCopyHeightData == 0 )
770 {
771 // Data is referenced only.
772 d->m_pHeightData = pHeightData;
773 }
774 else
775 {
776 // We own the height data, allocate storage
777 d->m_pHeightData = new float[ d->m_nWidthSamples * d->m_nDepthSamples ];
778 dIASSERT( d->m_pHeightData );
779
780 // Copy data.
781 memcpy( (void*)d->m_pHeightData, pHeightData,
782 sizeof( float ) * d->m_nWidthSamples * d->m_nDepthSamples );
783 }
784
785 // Find height bounds
786 d->ComputeHeightBounds();
787}
788
789void dGeomHeightfieldDataBuildDouble( dHeightfieldDataID d,
790 const double *pHeightData, int bCopyHeightData,
791 dReal width, dReal depth, int widthSamples, int depthSamples,
792 dReal scale, dReal offset, dReal thickness, int bWrap )
793{
794 dUASSERT( d, "Argument not Heightfield data" );
795 dIASSERT( pHeightData );
796 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
797 dIASSERT( depthSamples >= 2 );
798
799 // set info
800 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
801 d->m_nGetHeightMode = 4;
802 d->m_bCopyHeightData = bCopyHeightData;
803
804 if ( d->m_bCopyHeightData == 0 )
805 {
806 // Data is referenced only.
807 d->m_pHeightData = pHeightData;
808 }
809 else
810 {
811 // We own the height data, allocate storage
812 d->m_pHeightData = new double[ d->m_nWidthSamples * d->m_nDepthSamples ];
813 dIASSERT( d->m_pHeightData );
814
815 // Copy data.
816 memcpy( (void*)d->m_pHeightData, pHeightData,
817 sizeof( double ) * d->m_nWidthSamples * d->m_nDepthSamples );
818 }
819
820 // Find height bounds
821 d->ComputeHeightBounds();
822}
823
824
825
826
827void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight )
828{
829 dUASSERT(d, "Argument not Heightfield data");
830 d->m_fMinHeight = ( minHeight * d->m_fScale ) + d->m_fOffset - d->m_fThickness;
831 d->m_fMaxHeight = ( maxHeight * d->m_fScale ) + d->m_fOffset;
832}
833
834
835void dGeomHeightfieldDataDestroy( dHeightfieldDataID d )
836{
837 dUASSERT(d, "argument not Heightfield data");
838 delete d;
839}
840
841
842//////// Heightfield geom interface ////////////////////////////////////////////////////
843
844
845dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable )
846{
847 return new dxHeightfield( space, data, bPlaceable );
848}
849
850
851void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d )
852{
853 dxHeightfield* geom = (dxHeightfield*) g;
854 geom->data = d;
855}
856
857
858dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g )
859{
860 dxHeightfield* geom = (dxHeightfield*) g;
861 return geom->m_p_data;
862}
863
864//////// dxHeightfield /////////////////////////////////////////////////////////////////
865
866
867// Typedef for generic 'get point depth' function
868typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z );
869
870
871#define DMESS(A) \
872 dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
873 x,z,A, \
874 pContact->depth, \
875 dGeomSphereGetRadius(o2), \
876 pContact->pos[0], \
877 pContact->pos[1], \
878 pContact->pos[2], \
879 pContact->normal[0], \
880 pContact->normal[1], \
881 pContact->normal[2]);
882
883static inline bool DescendingTriangleSort(const HeightFieldTriangle * const A, const HeightFieldTriangle * const B)
884{
885 return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
886}
887static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B)
888{
889 return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
890}
891
892void dxHeightfield::sortPlanes(const size_t numPlanes)
893{
894 bool has_swapped = true;
895 do
896 {
897 has_swapped = false;//reset flag
898 for (size_t i = 0; i < numPlanes - 1; i++)
899 {
900 //if they are in the wrong order
901 if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1]))
902 {
903 //exchange them
904 HeightFieldPlane * tempPlane = tempPlaneBuffer[i];
905 tempPlaneBuffer[i] = tempPlaneBuffer[i + 1];
906 tempPlaneBuffer[i + 1] = tempPlane;
907
908 //we have swapped at least once, list may not be sorted yet
909 has_swapped = true;
910 }
911 }
912 } //if no swaps were made during this pass, the list has been sorted
913 while (has_swapped);
914}
915
916static inline dReal DistancePointToLine(const dVector3 &_point,
917 const dVector3 &_pt0,
918 const dVector3 &_Edge,
919 const dReal _Edgelength)
920{
921 dVector3 v;
922 dVector3Subtract(_point, _pt0, v);
923 dVector3 s;
924 dVector3Copy (_Edge, s);
925 const dReal dot = dVector3Dot(v, _Edge) / _Edgelength;
926 dVector3Scale(s, dot);
927 dVector3Subtract(v, s, v);
928 return dVector3Length(v);
929}
930
931
932
933
934int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
935 dxGeom* o2, const int numMaxContactsPossible,
936 int flags, dContactGeom* contact,
937 int skip )
938{
939 dContactGeom *pContact = 0;
940 int x, z;
941 // check if not above or inside terrain first
942 // while filling a heightmap partial temporary buffer
943 const unsigned int numX = (maxX - minX) + 1;
944 const unsigned int numZ = (maxZ - minZ) + 1;
945 const dReal minO2Height = o2->aabb[2];
946 const dReal maxO2Height = o2->aabb[3];
947 unsigned int x_local, z_local;
948 dReal maxY = - dInfinity;
949 dReal minY = dInfinity;
950 // localize and const for faster access
951 const dReal cfSampleWidth = m_p_data->m_fSampleWidth;
952 const dReal cfSampleDepth = m_p_data->m_fSampleDepth;
953 {
954 if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ)
955 {
956 resetHeightBuffer();
957 allocateHeightBuffer(numX, numZ);
958 }
959
960 dReal Xpos, Ypos;
961 Xpos = minX * cfSampleWidth;
962
963
964 for ( x = minX, x_local = 0; x_local < numX; x++, x_local++)
965 {
966 const dReal c_Xpos = Xpos;
967 HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
968 Ypos = minZ * cfSampleDepth;
969 for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++)
970 {
971 const dReal h = m_p_data->GetHeight(x, z);
972 HeightFieldRow[z_local].vertex[0] = c_Xpos;
973 HeightFieldRow[z_local].vertex[1] = h;
974 HeightFieldRow[z_local].vertex[2] = Ypos;
975
976
977 maxY = dMAX(maxY, h);
978 minY = dMIN(minY, h);
979
980
981 Ypos += cfSampleDepth;
982 }
983 Xpos += cfSampleWidth;
984 }
985 if (minO2Height - maxY > -dEpsilon )
986 {
987 //totally above heightfield
988 return 0;
989 }
990 if (minY - maxO2Height > -dEpsilon )
991 {
992 // totally under heightfield
993 pContact = CONTACT(contact, 0);
994
995 pContact->pos[0] = o2->final_posr->pos[0];
996 pContact->pos[1] = minY;
997 pContact->pos[2] = o2->final_posr->pos[2];
998
999 pContact->normal[0] = 0;
1000 pContact->normal[1] = - 1;
1001 pContact->normal[2] = 0;
1002
1003 pContact->depth = minY - maxO2Height;
1004
1005 return 1;
1006 }
1007 }
1008 // get All Planes that could collide against.
1009 dColliderFn *geomRayNCollider;
1010 dColliderFn *geomNPlaneCollider;
1011 dGetDepthFn *geomNDepthGetter;
1012
1013 // int max_collisionContact = numMaxContactsPossible; -- not used
1014 switch (o2->type)
1015 {
1016 case dRayClass:
1017 geomRayNCollider = NULL;
1018 geomNPlaneCollider = dCollideRayPlane;
1019 geomNDepthGetter = NULL;
1020 //max_collisionContact = 1;
1021 break;
1022
1023 case dSphereClass:
1024 geomRayNCollider = dCollideRaySphere;
1025 geomNPlaneCollider = dCollideSpherePlane;
1026 geomNDepthGetter = dGeomSpherePointDepth;
1027 //max_collisionContact = 3;
1028 break;
1029
1030 case dBoxClass:
1031 geomRayNCollider = dCollideRayBox;
1032 geomNPlaneCollider = dCollideBoxPlane;
1033 geomNDepthGetter = dGeomBoxPointDepth;
1034 //max_collisionContact = 8;
1035 break;
1036
1037 case dCapsuleClass:
1038 geomRayNCollider = dCollideRayCapsule;
1039 geomNPlaneCollider = dCollideCapsulePlane;
1040 geomNDepthGetter = dGeomCapsulePointDepth;
1041 // max_collisionContact = 3;
1042 break;
1043
1044 case dCylinderClass:
1045 geomRayNCollider = dCollideRayCylinder;
1046 geomNPlaneCollider = dCollideCylinderPlane;
1047 geomNDepthGetter = NULL;// TODO: dGeomCCylinderPointDepth
1048 //max_collisionContact = 3;
1049 break;
1050
1051 case dConvexClass:
1052 geomRayNCollider = dCollideRayConvex;
1053 geomNPlaneCollider = dCollideConvexPlane;
1054 geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth;
1055 //max_collisionContact = 3;
1056 break;
1057
1058#if dTRIMESH_ENABLED
1059
1060 case dTriMeshClass:
1061 geomRayNCollider = dCollideRayTrimesh;
1062 geomNPlaneCollider = dCollideTrimeshPlane;
1063 geomNDepthGetter = NULL;// TODO: dGeomTrimeshPointDepth;
1064 //max_collisionContact = 3;
1065 break;
1066
1067#endif // dTRIMESH_ENABLED
1068
1069 default:
1070 dIASSERT(0); // Shouldn't ever get here.
1071 break;
1072
1073 }
1074
1075 dxPlane myplane(0,0,0,0,0);
1076 dxPlane* sliding_plane = &myplane;
1077 dReal triplane[4];
1078 int i;
1079
1080 // check some trivial case.
1081 // Vector Up plane
1082 if (maxY - minY < dEpsilon)
1083 {
1084 // it's a single plane.
1085 triplane[0] = 0;
1086 triplane[1] = 1;
1087 triplane[2] = 0;
1088 triplane[3] = minY;
1089 dGeomPlaneSetNoNormalize (sliding_plane, triplane);
1090 // find collision and compute contact points
1091 const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip);
1092 dIASSERT(numTerrainContacts <= numMaxContactsPossible);
1093 for (i = 0; i < numTerrainContacts; i++)
1094 {
1095 pContact = CONTACT(contact, i*skip);
1096 dOPESIGN(pContact->normal, =, -, triplane);
1097 }
1098 return numTerrainContacts;
1099 }
1100 // unique plane
1101 {
1102 // check for very simple plane heightfield
1103 dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity;
1104 dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity;
1105
1106
1107 dReal lastXHeight = tempHeightBuffer[0][0].vertex[1];
1108 for ( x_local = 1; x_local < numX; x_local++)
1109 {
1110 HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
1111
1112 const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight;
1113
1114 maxXHeightDelta = dMAX (maxXHeightDelta, deltaX);
1115 minXHeightDelta = dMIN (minXHeightDelta, deltaX);
1116
1117 dReal lastZHeight = HeightFieldRow[0].vertex[1];
1118 for ( z_local = 1; z_local < numZ; z_local++)
1119 {
1120 const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight);
1121
1122 maxZHeightDelta = dMAX (maxZHeightDelta, deltaZ);
1123 minZHeightDelta = dMIN (minZHeightDelta, deltaZ);
1124
1125 }
1126 }
1127
1128 if (maxZHeightDelta - minZHeightDelta < dEpsilon &&
1129 maxXHeightDelta - minXHeightDelta < dEpsilon )
1130 {
1131 // it's a single plane.
1132 const dVector3 &A = tempHeightBuffer[0][0].vertex;
1133 const dVector3 &B = tempHeightBuffer[1][0].vertex;
1134 const dVector3 &C = tempHeightBuffer[0][1].vertex;
1135
1136 // define 2 edges and a point that will define collision plane
1137 {
1138 dVector3 Edge1, Edge2;
1139 dVector3Subtract(C, A, Edge1);
1140 dVector3Subtract(B, A, Edge2);
1141 dVector3Cross(Edge1, Edge2, triplane);
1142 }
1143
1144 // Define Plane
1145 // Normalize plane normal
1146 const dReal dinvlength = REAL(1.0) / dVector3Length(triplane);
1147 triplane[0] *= dinvlength;
1148 triplane[1] *= dinvlength;
1149 triplane[2] *= dinvlength;
1150 // get distance to origin from plane
1151 triplane[3] = dVector3Dot(triplane, A);
1152
1153 dGeomPlaneSetNoNormalize (sliding_plane, triplane);
1154 // find collision and compute contact points
1155 const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip);
1156 dIASSERT(numTerrainContacts <= numMaxContactsPossible);
1157 for (i = 0; i < numTerrainContacts; i++)
1158 {
1159 pContact = CONTACT(contact, i*skip);
1160 dOPESIGN(pContact->normal, =, -, triplane);
1161 }
1162 return numTerrainContacts;
1163 }
1164 }
1165
1166
1167 int numTerrainContacts = 0;
1168 dContactGeom *PlaneContact = m_p_data->m_contacts;
1169
1170 const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2;
1171 if (tempTriangleBufferSize < numTriMax)
1172 {
1173 resetTriangleBuffer();
1174 allocateTriangleBuffer(numTriMax);
1175 }
1176
1177 // Sorting triangle/plane resulting from heightfield zone
1178 // Perhaps that would be necessary in case of too much limited
1179 // maximum contact point...
1180 // or in complex mesh case (trimesh and convex)
1181 // need some test or insights on this before enabling this.
1182 const bool isContactNumPointsLimited =
1183 true;
1184 // (numMaxContacts < 8)
1185 // || o2->type == dConvexClass
1186 // || o2->type == dTriMeshClass
1187 // || (numMaxContacts < (int)numTriMax)
1188
1189
1190
1191 // if small heightfield triangle related to O2 colliding
1192 // or no Triangle colliding at all.
1193 bool needFurtherPasses = (o2->type == dTriMeshClass);
1194 //compute Ratio between Triangle size and O2 aabb size
1195 // no FurtherPasses are needed in ray class
1196 if (o2->type != dRayClass && needFurtherPasses == false)
1197 {
1198 const dReal xratio = (o2->aabb[1] - o2->aabb[0]) * m_p_data->m_fInvSampleWidth;
1199 if (xratio > REAL(1.5))
1200 needFurtherPasses = true;
1201 else
1202 {
1203 const dReal zratio = (o2->aabb[5] - o2->aabb[4]) * m_p_data->m_fInvSampleDepth;
1204 if (zratio > REAL(1.5))
1205 needFurtherPasses = true;
1206 }
1207
1208 }
1209
1210 unsigned int numTri = 0;
1211 HeightFieldVertex *A, *B, *C, *D;
1212 /* (y is up)
1213 A--------B-...x
1214 | /|
1215 | / |
1216 | / |
1217 | / |
1218 | / |
1219 | / |
1220 | / |
1221 |/ |
1222 C--------D
1223 .
1224 .
1225 .
1226 z
1227 */
1228 // keep only triangle that does intersect geom
1229 for ( x = minX, x_local = 0; x < maxX; x++, x_local++)
1230 {
1231 HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
1232 HeightFieldVertex *HeightFieldNextRow = tempHeightBuffer[x_local + 1];
1233
1234 // First A
1235 C = &HeightFieldRow [0];
1236 // First B
1237 D = &HeightFieldNextRow[0];
1238 for ( z = minZ, z_local = 0; z < maxZ; z++, z_local++)
1239 {
1240 A = C;
1241 B = D;
1242
1243 C = &HeightFieldRow [z_local + 1];
1244 D = &HeightFieldNextRow[z_local + 1];
1245
1246 const dReal AHeight = A->vertex[1];
1247 const dReal BHeight = B->vertex[1];
1248 const dReal CHeight = C->vertex[1];
1249 const dReal DHeight = D->vertex[1];
1250
1251 const bool isACollide = 0 < AHeight - minO2Height;
1252 const bool isBCollide = 0 < BHeight - minO2Height;
1253 const bool isCCollide = 0 < CHeight - minO2Height;
1254 const bool isDCollide = 0 < DHeight - minO2Height;
1255
1256 A->state = !(isACollide);
1257 B->state = !(isBCollide);
1258 C->state = !(isCCollide);
1259 D->state = !(isCCollide);
1260
1261 if (isACollide || isBCollide || isCCollide)
1262 {
1263 HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++];
1264
1265 CurrTriUp->state = false;
1266
1267 // changing point order here implies to change it in isOnHeightField
1268 CurrTriUp->vertices[0] = A;
1269 CurrTriUp->vertices[1] = B;
1270 CurrTriUp->vertices[2] = C;
1271
1272 if (isContactNumPointsLimited)
1273 CurrTriUp->setMinMax();
1274 CurrTriUp->isUp = true;
1275 }
1276
1277 if (isBCollide || isCCollide || isDCollide)
1278 {
1279 HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++];
1280
1281 CurrTriDown->state = false;
1282 // changing point order here implies to change it in isOnHeightField
1283
1284 CurrTriDown->vertices[0] = D;
1285 CurrTriDown->vertices[1] = B;
1286 CurrTriDown->vertices[2] = C;
1287
1288
1289 if (isContactNumPointsLimited)
1290 CurrTriDown->setMinMax();
1291 CurrTriDown->isUp = false;
1292 }
1293
1294
1295 if (needFurtherPasses &&
1296 (isBCollide || isCCollide)
1297 &&
1298 (AHeight - CHeight > 0 &&
1299 AHeight - BHeight > 0 &&
1300 DHeight - CHeight > 0 &&
1301 DHeight - BHeight > 0))
1302 {
1303 // That means Edge BC is concave, therefore
1304 // BC Edge and B and C vertices cannot collide
1305
1306 B->state = true;
1307 C->state = true;
1308 }
1309 // should find a way to check other edges (AB, BD, CD) too for concavity
1310 }
1311 }
1312
1313 // at least on triangle should intersect geom
1314 dIASSERT (numTri != 0);
1315 // pass1: VS triangle as Planes
1316 // Group Triangle by same plane definition
1317 // as Terrain often has many triangles using same plane definition
1318 // then collide against that list of triangles.
1319 {
1320
1321 dVector3 Edge1, Edge2;
1322 //compute all triangles normals.
1323 for (unsigned int k = 0; k < numTri; k++)
1324 {
1325 HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
1326
1327 // define 2 edges and a point that will define collision plane
1328 dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1);
1329 dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2);
1330
1331 // find a perpendicular vector to the triangle
1332 if (itTriangle->isUp)
1333 dVector3Cross(Edge1, Edge2, triplane);
1334 else
1335 dVector3Cross(Edge2, Edge1, triplane);
1336
1337 // Define Plane
1338 // Normalize plane normal
1339 const dReal dinvlength = REAL(1.0) / dVector3Length(triplane);
1340 triplane[0] *= dinvlength;
1341 triplane[1] *= dinvlength;
1342 triplane[2] *= dinvlength;
1343 // get distance to origin from plane
1344 triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex);
1345
1346 // saves normal for collision check (planes, triangles, vertices and edges.)
1347 dVector3Copy(triplane, itTriangle->planeDef);
1348 // saves distance for collision check (planes, triangles, vertices and edges.)
1349 itTriangle->planeDef[3] = triplane[3];
1350 }
1351
1352 // group by Triangles by Planes sharing shame plane definition
1353 if (tempPlaneBufferSize < numTri)
1354 {
1355 resetPlaneBuffer();
1356 allocatePlaneBuffer(numTri);
1357 }
1358 unsigned int numPlanes = 0;
1359 for (unsigned int k = 0; k < numTri; k++)
1360 {
1361 HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k];
1362
1363 if (tri_base->state == true)
1364 continue;// already tested or added to plane list.
1365
1366 HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes];
1367 currPlane->resetTriangleListSize(numTri - k);
1368 currPlane->addTriangle(tri_base);
1369 // saves normal for collision check (planes, triangles, vertices and edges.)
1370 dVector3Copy(tri_base->planeDef, currPlane->planeDef);
1371 // saves distance for collision check (planes, triangles, vertices and edges.)
1372 currPlane->planeDef[3]= tri_base->planeDef[3];
1373
1374 const dReal normx = tri_base->planeDef[0];
1375 const dReal normy = tri_base->planeDef[1];
1376 const dReal normz = tri_base->planeDef[2];
1377 const dReal dist = tri_base->planeDef[3];
1378
1379 for (unsigned int m = k + 1; m < numTri; m++)
1380 {
1381
1382 HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m];
1383 if (tri_test->state == true)
1384 continue;// already tested or added to plane list.
1385
1386 // normals and distance are the same.
1387 if (
1388 dFabs(normy - tri_test->planeDef[1]) < dEpsilon &&
1389 dFabs(dist - tri_test->planeDef[3]) < dEpsilon &&
1390 dFabs(normx - tri_test->planeDef[0]) < dEpsilon &&
1391 dFabs(normz - tri_test->planeDef[2]) < dEpsilon
1392 )
1393 {
1394 currPlane->addTriangle (tri_test);
1395 tri_test->state = true;
1396 }
1397 }
1398
1399 tri_base->state = true;
1400 if (isContactNumPointsLimited)
1401 currPlane->setMinMax();
1402
1403 numPlanes++;
1404 }
1405
1406 // sort planes
1407 if (isContactNumPointsLimited)
1408 sortPlanes(numPlanes);
1409
1410#if !defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
1411 /*
1412 Note by Oleh_Derevenko:
1413 It seems to be incorrect to limit contact count by some particular value
1414 since some of them (and even all of them) may be culled in following condition.
1415 However I do not see an easy way to fix this.
1416 If not that culling the flags modification should be changed here and
1417 additionally repeated after some contacts have been generated (in "if (didCollide)").
1418 The maximum of contacts in flags would then be set to minimum of contacts
1419 remaining and HEIGHTFIELDMAXCONTACTPERCELL.
1420 */
1421 int planeTestFlags = (flags & ~NUMC_MASK) | HEIGHTFIELDMAXCONTACTPERCELL;
1422 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1423#else // if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
1424 int numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1425 int planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane;
1426 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1427#endif
1428
1429 for (unsigned int k = 0; k < numPlanes; k++)
1430 {
1431 HeightFieldPlane * const itPlane = tempPlaneBuffer[k];
1432
1433 //set Geom
1434 dGeomPlaneSetNoNormalize (sliding_plane, itPlane->planeDef);
1435 //dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]);
1436 // find collision and compute contact points
1437 bool didCollide = false;
1438 const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, planeTestFlags, PlaneContact, sizeof(dContactGeom));
1439 const size_t planeTriListSize = itPlane->trianglelistCurrentSize;
1440 for (i = 0; i < numPlaneContacts; i++)
1441 {
1442 // Check if contact point found in plane is inside Triangle.
1443 const dVector3 &pCPos = PlaneContact[i].pos;
1444 for (size_t b = 0; planeTriListSize > b; b++)
1445 {
1446 if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0]->vertex,
1447 pCPos,
1448 itPlane->trianglelist[b]->isUp))
1449 {
1450 pContact = CONTACT(contact, numTerrainContacts*skip);
1451 dVector3Copy(pCPos, pContact->pos);
1452 dOPESIGN(pContact->normal, =, -, itPlane->planeDef);
1453 pContact->depth = PlaneContact[i].depth;
1454 numTerrainContacts++;
1455 if ( numTerrainContacts == numMaxContactsPossible )
1456 return numTerrainContacts;
1457
1458 didCollide = true;
1459 break;
1460 }
1461 }
1462 }
1463 if (didCollide)
1464 {
1465#if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
1466 /* Note by Oleh_Derevenko:
1467 This code is not used - see another note above
1468 */
1469 numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1470 planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane;
1471 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1472#endif
1473 for (size_t b = 0; planeTriListSize > b; b++)
1474 {
1475 // flag Triangles Vertices as collided
1476 // to prevent any collision test of those
1477 for (i = 0; i < 3; i++)
1478 itPlane->trianglelist[b]->vertices[i]->state = true;
1479 }
1480 }
1481 else
1482 {
1483 // flag triangle as not collided so that Vertices or Edge
1484 // of that triangles will be checked.
1485 for (size_t b = 0; planeTriListSize > b; b++)
1486 {
1487 itPlane->trianglelist[b]->state = false;
1488 }
1489 }
1490 }
1491 }
1492
1493
1494
1495 // pass2: VS triangle vertices
1496 if (needFurtherPasses)
1497 {
1498 dxRay tempRay(0, 1);
1499 dReal depth;
1500 bool vertexCollided;
1501
1502 // Only one contact is necessary for ray test
1503 int rayTestFlags = (flags & ~NUMC_MASK) | 1;
1504 dIASSERT((1 & ~NUMC_MASK) == 0);
1505 //
1506 // Find Contact Penetration Depth of each vertices
1507 //
1508 for (unsigned int k = 0; k < numTri; k++)
1509 {
1510 const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
1511 if (itTriangle->state == true)
1512 continue;// plane triangle did already collide.
1513
1514 for (size_t i = 0; i < 3; i++)
1515 {
1516 HeightFieldVertex *vertex = itTriangle->vertices[i];
1517 if (vertex->state == true)
1518 continue;// vertice did already collide.
1519
1520 vertexCollided = false;
1521 const dVector3 &triVertex = vertex->vertex;
1522 if ( geomNDepthGetter )
1523 {
1524 depth = geomNDepthGetter( o2,
1525 triVertex[0], triVertex[1], triVertex[2] );
1526 if (depth + dEpsilon < 0)
1527 vertexCollided = true;
1528 }
1529 else
1530 {
1531 // We don't have a GetDepth function, so do a ray cast instead.
1532 // NOTE: This isn't ideal, and a GetDepth function should be
1533 // written for all geom classes.
1534 tempRay.length = (minO2Height - triVertex[1]) * REAL(1000.0);
1535
1536 //dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2],
1537 // - itTriangle->Normal[0], - itTriangle->Normal[1], - itTriangle->Normal[2] );
1538 dGeomRaySetNoNormalize(tempRay, triVertex, itTriangle->planeDef);
1539
1540 if ( geomRayNCollider( &tempRay, o2, rayTestFlags, PlaneContact, sizeof( dContactGeom ) ) )
1541 {
1542 depth = PlaneContact[0].depth;
1543 vertexCollided = true;
1544 }
1545 }
1546 if (vertexCollided)
1547 {
1548 pContact = CONTACT(contact, numTerrainContacts*skip);
1549 //create contact using vertices
1550 dVector3Copy (triVertex, pContact->pos);
1551 //create contact using Plane Normal
1552 dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
1553
1554 pContact->depth = depth;
1555
1556 numTerrainContacts++;
1557 if ( numTerrainContacts == numMaxContactsPossible )
1558 return numTerrainContacts;
1559
1560 vertex->state = true;
1561 }
1562 }
1563 }
1564 }
1565
1566#ifdef _HEIGHTFIELDEDGECOLLIDING
1567 // pass3: VS triangle Edges
1568 if (needFurtherPasses)
1569 {
1570 dVector3 Edge;
1571 dxRay edgeRay(0, 1);
1572
1573 int numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1574 int triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri;
1575 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1576
1577 for (unsigned int k = 0; k < numTri; k++)
1578 {
1579 const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
1580
1581 if (itTriangle->state == true)
1582 continue;// plane did already collide.
1583
1584 for (size_t m = 0; m < 3; m++)
1585 {
1586 const size_t next = (m + 1) % 3;
1587 HeightFieldVertex *vertex0 = itTriangle->vertices[m];
1588 HeightFieldVertex *vertex1 = itTriangle->vertices[next];
1589
1590 // not concave or under the AABB
1591 // nor triangle already collided against vertices
1592 if (vertex0->state == true && vertex1->state == true)
1593 continue;// plane did already collide.
1594
1595 dVector3Subtract(vertex1->vertex, vertex0->vertex, Edge);
1596 edgeRay.length = dVector3Length (Edge);
1597 dGeomRaySetNoNormalize(edgeRay, vertex1->vertex, Edge);
1598 int prevTerrainContacts = numTerrainContacts;
1599 pContact = CONTACT(contact, prevTerrainContacts*skip);
1600 const int numCollision = geomRayNCollider(&edgeRay,o2,triTestFlags,pContact,skip);
1601 dIASSERT(numCollision <= numMaxContactsPerTri);
1602
1603 if (numCollision)
1604 {
1605 numTerrainContacts += numCollision;
1606
1607 do
1608 {
1609 pContact = CONTACT(contact, prevTerrainContacts*skip);
1610
1611 //create contact using Plane Normal
1612 dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
1613
1614 pContact->depth = DistancePointToLine(pContact->pos, vertex1->vertex, Edge, edgeRay.length);
1615 }
1616 while (++prevTerrainContacts != numTerrainContacts);
1617
1618 if ( numTerrainContacts == numMaxContactsPossible )
1619 return numTerrainContacts;
1620
1621 numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1622 triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri;
1623 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1624 }
1625 }
1626
1627 itTriangle->vertices[0]->state = true;
1628 itTriangle->vertices[1]->state = true;
1629 itTriangle->vertices[2]->state = true;
1630 }
1631 }
1632#endif // _HEIGHTFIELDEDGECOLLIDING
1633 return numTerrainContacts;
1634}
1635
1636int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip )
1637{
1638 dIASSERT( skip >= (int)sizeof(dContactGeom) );
1639 dIASSERT( o1->type == dHeightfieldClass );
1640 dIASSERT((flags & NUMC_MASK) >= 1);
1641
1642 int i;
1643
1644 // if ((flags & NUMC_MASK) == 0) -- An assertion check is made on entry
1645 // { flags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); }
1646
1647 int numMaxTerrainContacts = (flags & NUMC_MASK);
1648
1649 dxHeightfield *terrain = (dxHeightfield*) o1;
1650
1651 dVector3 posbak;
1652 dMatrix3 Rbak;
1653 dReal aabbbak[6];
1654 int gflagsbak;
1655 dVector3 pos0,pos1;
1656 dMatrix3 R1;
1657
1658 int numTerrainContacts = 0;
1659
1660 //@@ Should find a way to set reComputeAABB to false in default case
1661 // aka DHEIGHTFIELD_CORNER_ORIGIN not defined and terrain not PLACEABLE
1662 // so that we can free some memory and speed up things a bit
1663 // while saving some precision loss
1664#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1665 const bool reComputeAABB = true;
1666#else
1667 const bool reComputeAABB = ( terrain->gflags & GEOM_PLACEABLE ) ? true : false;
1668#endif //DHEIGHTFIELD_CORNER_ORIGIN
1669
1670 //
1671 // Transform O2 into Heightfield Space
1672 //
1673 if (reComputeAABB)
1674 {
1675 // Backup original o2 position, rotation and AABB.
1676 dVector3Copy( o2->final_posr->pos, posbak );
1677 dMatrix3Copy( o2->final_posr->R, Rbak );
1678 memcpy( aabbbak, o2->aabb, sizeof( dReal ) * 6 );
1679 gflagsbak = o2->gflags;
1680 }
1681
1682 if ( terrain->gflags & GEOM_PLACEABLE )
1683 {
1684 // Transform o2 into heightfield space.
1685 dOP( pos0, -, o2->final_posr->pos, terrain->final_posr->pos );
1686 dMULTIPLY1_331( pos1, terrain->final_posr->R, pos0 );
1687 dMULTIPLY1_333( R1, terrain->final_posr->R, o2->final_posr->R );
1688
1689 // Update o2 with transformed position and rotation.
1690 dVector3Copy( pos1, o2->final_posr->pos );
1691 dMatrix3Copy( R1, o2->final_posr->R );
1692 }
1693
1694#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1695 o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth;
1696 o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth;
1697#endif // DHEIGHTFIELD_CORNER_ORIGIN
1698
1699 // Rebuild AABB for O2
1700 if (reComputeAABB)
1701 o2->computeAABB();
1702
1703 //
1704 // Collide
1705 //
1706
1707 //check if inside boundaries
1708 // using O2 aabb
1709 // aabb[6] is (minx, maxx, miny, maxy, minz, maxz)
1710 const bool wrapped = terrain->m_p_data->m_bWrapMode != 0;
1711
1712 int nMinX;
1713 int nMaxX;
1714 int nMinZ;
1715 int nMaxZ;
1716
1717 if ( !wrapped )
1718 {
1719 if ( o2->aabb[0] > terrain->m_p_data->m_fWidth //MinX
1720 && o2->aabb[4] > terrain->m_p_data->m_fDepth)//MinZ
1721 goto dCollideHeightfieldExit;
1722
1723 if ( o2->aabb[1] < 0 //MaxX
1724 && o2->aabb[5] < 0) //MaxZ
1725 goto dCollideHeightfieldExit;
1726
1727 }
1728
1729 nMinX = int(dFloor(o2->aabb[0] * terrain->m_p_data->m_fInvSampleWidth));
1730 nMaxX = int(dFloor(o2->aabb[1] * terrain->m_p_data->m_fInvSampleWidth)) + 1;
1731 nMinZ = int(dFloor(o2->aabb[4] * terrain->m_p_data->m_fInvSampleDepth));
1732 nMaxZ = int(dFloor(o2->aabb[5] * terrain->m_p_data->m_fInvSampleDepth)) + 1;
1733
1734 if ( !wrapped )
1735 {
1736 nMinX = dMAX( nMinX, 0 );
1737 nMaxX = dMIN( nMaxX, terrain->m_p_data->m_nWidthSamples - 1 );
1738 nMinZ = dMAX( nMinZ, 0 );
1739 nMaxZ = dMIN( nMaxZ, terrain->m_p_data->m_nDepthSamples - 1 );
1740
1741 dIASSERT ((nMinX < nMaxX) || (nMinZ < nMaxZ))
1742 }
1743
1744
1745
1746 numTerrainContacts += terrain->dCollideHeightfieldZone(
1747 nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts,
1748 flags,CONTACT(contact,numTerrainContacts*skip),skip );
1749
1750 dIASSERT( numTerrainContacts <= numMaxTerrainContacts );
1751
1752 dContactGeom *pContact;
1753 for ( i = 0; i < numTerrainContacts; ++i )
1754 {
1755 pContact = CONTACT(contact,i*skip);
1756 pContact->g1 = o1;
1757 pContact->g2 = o2;
1758 }
1759
1760
1761 //------------------------------------------------------------------------------
1762
1763dCollideHeightfieldExit:
1764
1765 if (reComputeAABB)
1766 {
1767 // Restore o2 position, rotation and AABB
1768 dVector3Copy( posbak, o2->final_posr->pos );
1769 dMatrix3Copy( Rbak, o2->final_posr->R );
1770 memcpy( o2->aabb, aabbbak, sizeof(dReal)*6 );
1771 o2->gflags = gflagsbak;
1772
1773 //
1774 // Transform Contacts to World Space
1775 //
1776 if ( terrain->gflags & GEOM_PLACEABLE )
1777 {
1778 for ( i = 0; i < numTerrainContacts; ++i )
1779 {
1780 pContact = CONTACT(contact,i*skip);
1781 dOPE( pos0, =, pContact->pos );
1782
1783#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1784 pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
1785 pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
1786#endif // !DHEIGHTFIELD_CORNER_ORIGIN
1787
1788 dMULTIPLY0_331( pContact->pos, terrain->final_posr->R, pos0 );
1789
1790 dOP( pContact->pos, +, pContact->pos, terrain->final_posr->pos );
1791 dOPE( pos0, =, pContact->normal );
1792
1793 dMULTIPLY0_331( pContact->normal, terrain->final_posr->R, pos0 );
1794 }
1795 }
1796#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1797 else
1798 {
1799 for ( i = 0; i < numTerrainContacts; ++i )
1800 {
1801 pContact = CONTACT(contact,i*skip);
1802 pContact->pos[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
1803 pContact->pos[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
1804 }
1805 }
1806#endif // !DHEIGHTFIELD_CORNER_ORIGIN
1807 }
1808 // Return contact count.
1809 return numTerrainContacts;
1810}
1811
1812
diff --git a/libraries/ode-0.9/ode/src/heightfield.h b/libraries/ode-0.9/ode/src/heightfield.h
new file mode 100644
index 0000000..f4c5952
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/heightfield.h
@@ -0,0 +1,225 @@
1// dHeightfield Collider
2// Martijn Buijs 2006 http://home.planet.nl/~buijs512/
3// Based on Terrain & Cone contrib by:
4// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
5
6#ifndef _DHEIGHTFIELD_H_
7#define _DHEIGHTFIELD_H_
8//------------------------------------------------------------------------------
9
10#include <ode/common.h>
11#include "collision_kernel.h"
12
13
14#define HEIGHTFIELDMAXCONTACTPERCELL 10
15
16//
17// dxHeightfieldData
18//
19// Heightfield Data structure
20//
21struct dxHeightfieldData
22{
23 dReal m_fWidth; // World space heightfield dimension on X axis
24 dReal m_fDepth; // World space heightfield dimension on Z axis
25 dReal m_fSampleWidth; // Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1))
26 dReal m_fSampleDepth; // Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
27 dReal m_fInvSampleWidth; // Cache of inverse Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1))
28 dReal m_fInvSampleDepth; // Cache of inverse Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
29
30 dReal m_fHalfWidth; // Cache of half of m_fWidth
31 dReal m_fHalfDepth; // Cache of half of m_fDepth
32
33 dReal m_fMinHeight; // Min sample height value (scaled and offset)
34 dReal m_fMaxHeight; // Max sample height value (scaled and offset)
35 dReal m_fThickness; // Surface thickness (added to bottom AABB)
36 dReal m_fScale; // Sample value multiplier
37 dReal m_fOffset; // Vertical sample offset
38
39 int m_nWidthSamples; // Vertex count on X axis edge (number of samples)
40 int m_nDepthSamples; // Vertex count on Z axis edge (number of samples)
41 int m_bCopyHeightData; // Do we own the sample data?
42 int m_bWrapMode; // Heightfield wrapping mode (0=finite, 1=infinite)
43 int m_nGetHeightMode; // GetHeight mode ( 0=callback, 1=byte, 2=short, 3=float )
44
45 const void* m_pHeightData; // Sample data array
46 void* m_pUserData; // Callback user data
47
48 dContactGeom m_contacts[HEIGHTFIELDMAXCONTACTPERCELL];
49
50 dHeightfieldGetHeight* m_pGetHeightCallback; // Callback pointer.
51
52 dxHeightfieldData();
53 ~dxHeightfieldData();
54
55 void SetData( int nWidthSamples, int nDepthSamples,
56 dReal fWidth, dReal fDepth,
57 dReal fScale, dReal fOffset,
58 dReal fThickness, int bWrapMode );
59
60 void ComputeHeightBounds();
61
62 bool IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const;
63 bool IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const;
64
65 dReal GetHeight(int x, int z);
66 dReal GetHeight(dReal x, dReal z);
67
68};
69
70class HeightFieldVertex;
71class HeightFieldEdge;
72class HeightFieldTriangle;
73
74class HeightFieldVertex
75{
76public:
77 HeightFieldVertex(){};
78
79 dVector3 vertex;
80 bool state;
81};
82
83class HeightFieldEdge
84{
85public:
86 HeightFieldEdge(){};
87
88 HeightFieldVertex *vertices[2];
89};
90//
91// HeightFieldTriangle
92//
93// HeightFieldTriangle composing heightfield mesh
94//
95class HeightFieldTriangle
96{
97public:
98 HeightFieldTriangle(){};
99
100 inline void setMinMax()
101 {
102 maxAAAB = vertices[0]->vertex[1] > vertices[1]->vertex[1] ? vertices[0]->vertex[1] : vertices[1]->vertex[1];
103 maxAAAB = vertices[2]->vertex[1] > maxAAAB ? vertices[2]->vertex[1] : maxAAAB;
104 };
105
106 HeightFieldVertex *vertices[3];
107 dReal planeDef[4];
108 dReal maxAAAB;
109
110 bool isUp;
111 bool state;
112};
113//
114// HeightFieldTriangle
115//
116// HeightFieldPlane composing heightfield mesh
117//
118class HeightFieldPlane
119{
120public:
121 HeightFieldPlane():
122 trianglelist(0),
123 trianglelistReservedSize(0),
124 trianglelistCurrentSize(0)
125 {
126
127 };
128 ~HeightFieldPlane()
129 {
130 delete [] trianglelist;
131 };
132
133 inline void setMinMax()
134 {
135 const size_t asize = trianglelistCurrentSize;
136 if (asize > 0)
137 {
138 maxAAAB = trianglelist[0]->maxAAAB;
139 for (size_t k = 1; asize > k; k++)
140 {
141 if (trianglelist[k]->maxAAAB > maxAAAB)
142 maxAAAB = trianglelist[k]->maxAAAB;
143 }
144 }
145 };
146
147 void resetTriangleListSize(const size_t newSize)
148 {
149 if (trianglelistReservedSize < newSize)
150 {
151 delete [] trianglelist;
152 trianglelistReservedSize = newSize;
153 trianglelist = new HeightFieldTriangle *[newSize];
154 }
155 trianglelistCurrentSize = 0;
156 }
157
158 void addTriangle(HeightFieldTriangle *tri)
159 {
160 dIASSERT(trianglelistCurrentSize < trianglelistReservedSize);
161
162 trianglelist[trianglelistCurrentSize++] = tri;
163 }
164
165 HeightFieldTriangle **trianglelist;
166 size_t trianglelistReservedSize;
167 size_t trianglelistCurrentSize;
168
169 dReal maxAAAB;
170 dReal planeDef[4];
171};
172//
173// dxHeightfield
174//
175// Heightfield geom structure
176//
177struct dxHeightfield : public dxGeom
178{
179 dxHeightfieldData* m_p_data;
180
181 dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable );
182 ~dxHeightfield();
183
184 void computeAABB();
185
186 int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
187 dxGeom *o2, const int numMaxContacts,
188 int flags, dContactGeom *contact, int skip );
189
190 enum
191 {
192 TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 4,
193 TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X = 4,
194 TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z = 4,
195 TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 1, // Triangles are easy to reallocate and hard to predict
196 };
197
198 static inline size_t AlignBufferSize(size_t value, size_t alignment) { dIASSERT((alignment & (alignment - 1)) == 0); return (value + (alignment - 1)) & ~(alignment - 1); }
199
200 void allocateTriangleBuffer(size_t numTri);
201 void resetTriangleBuffer();
202 void allocatePlaneBuffer(size_t numTri);
203 void resetPlaneBuffer();
204 void allocateHeightBuffer(size_t numX, size_t numZ);
205 void resetHeightBuffer();
206
207 void sortPlanes(const size_t numPlanes);
208
209 HeightFieldPlane **tempPlaneBuffer;
210 HeightFieldPlane *tempPlaneInstances;
211 size_t tempPlaneBufferSize;
212
213 HeightFieldTriangle *tempTriangleBuffer;
214 size_t tempTriangleBufferSize;
215
216 HeightFieldVertex **tempHeightBuffer;
217 HeightFieldVertex *tempHeightInstances;
218 size_t tempHeightBufferSizeX;
219 size_t tempHeightBufferSizeZ;
220
221};
222
223
224//------------------------------------------------------------------------------
225#endif //_DHEIGHTFIELD_H_
diff --git a/libraries/ode-0.9/ode/src/joint.cpp b/libraries/ode-0.9/ode/src/joint.cpp
new file mode 100644
index 0000000..d83294b
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/joint.cpp
@@ -0,0 +1,4065 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25design note: the general principle for giving a joint the option of connecting
26to the static environment (i.e. the absolute frame) is to check the second
27body (joint->node[1].body), and if it is zero then behave as if its body
28transform is the identity.
29
30*/
31
32#include <ode/ode.h>
33#include <ode/odemath.h>
34#include <ode/rotation.h>
35#include <ode/matrix.h>
36#include "joint.h"
37
38//****************************************************************************
39// externs
40
41// extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
42// extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
43
44//****************************************************************************
45// utility
46
47// set three "ball-and-socket" rows in the constraint equation, and the
48// corresponding right hand side.
49
50static inline void setBall (dxJoint *joint, dxJoint::Info2 *info,
51 dVector3 anchor1, dVector3 anchor2)
52{
53 // anchor points in global coordinates with respect to body PORs.
54 dVector3 a1,a2;
55
56 int s = info->rowskip;
57
58 // set jacobian
59 info->J1l[0] = 1;
60 info->J1l[s+1] = 1;
61 info->J1l[2*s+2] = 1;
62 dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1);
63 dCROSSMAT (info->J1a,a1,s,-,+);
64 if (joint->node[1].body) {
65 info->J2l[0] = -1;
66 info->J2l[s+1] = -1;
67 info->J2l[2*s+2] = -1;
68 dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2);
69 dCROSSMAT (info->J2a,a2,s,+,-);
70 }
71
72 // set right hand side
73 dReal k = info->fps * info->erp;
74 if (joint->node[1].body) {
75 for (int j=0; j<3; j++) {
76 info->c[j] = k * (a2[j] + joint->node[1].body->posr.pos[j] -
77 a1[j] - joint->node[0].body->posr.pos[j]);
78 }
79 }
80 else {
81 for (int j=0; j<3; j++) {
82 info->c[j] = k * (anchor2[j] - a1[j] -
83 joint->node[0].body->posr.pos[j]);
84 }
85 }
86}
87
88
89// this is like setBall(), except that `axis' is a unit length vector
90// (in global coordinates) that should be used for the first jacobian
91// position row (the other two row vectors will be derived from this).
92// `erp1' is the erp value to use along the axis.
93
94static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info,
95 dVector3 anchor1, dVector3 anchor2,
96 dVector3 axis, dReal erp1)
97{
98 // anchor points in global coordinates with respect to body PORs.
99 dVector3 a1,a2;
100
101 int i,s = info->rowskip;
102
103 // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
104 // [0 1 0] and [0 0 1], which makes everything much easier.
105 dVector3 q1,q2;
106 dPlaneSpace (axis,q1,q2);
107
108 // set jacobian
109 for (i=0; i<3; i++) info->J1l[i] = axis[i];
110 for (i=0; i<3; i++) info->J1l[s+i] = q1[i];
111 for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i];
112 dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1);
113 dCROSS (info->J1a,=,a1,axis);
114 dCROSS (info->J1a+s,=,a1,q1);
115 dCROSS (info->J1a+2*s,=,a1,q2);
116 if (joint->node[1].body) {
117 for (i=0; i<3; i++) info->J2l[i] = -axis[i];
118 for (i=0; i<3; i++) info->J2l[s+i] = -q1[i];
119 for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i];
120 dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2);
121 dCROSS (info->J2a,= -,a2,axis);
122 dCROSS (info->J2a+s,= -,a2,q1);
123 dCROSS (info->J2a+2*s,= -,a2,q2);
124 }
125
126 // set right hand side - measure error along (axis,q1,q2)
127 dReal k1 = info->fps * erp1;
128 dReal k = info->fps * info->erp;
129
130 for (i=0; i<3; i++) a1[i] += joint->node[0].body->posr.pos[i];
131 if (joint->node[1].body) {
132 for (i=0; i<3; i++) a2[i] += joint->node[1].body->posr.pos[i];
133 info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1));
134 info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1));
135 info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1));
136 }
137 else {
138 info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1));
139 info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1));
140 info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1));
141 }
142}
143
144
145// set three orientation rows in the constraint equation, and the
146// corresponding right hand side.
147
148static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row)
149{
150 int s = info->rowskip;
151 int start_index = start_row * s;
152
153 // 3 rows to make body rotations equal
154 info->J1a[start_index] = 1;
155 info->J1a[start_index + s + 1] = 1;
156 info->J1a[start_index + s*2+2] = 1;
157 if (joint->node[1].body) {
158 info->J2a[start_index] = -1;
159 info->J2a[start_index + s+1] = -1;
160 info->J2a[start_index + s*2+2] = -1;
161 }
162
163 // compute the right hand side. the first three elements will result in
164 // relative angular velocity of the two bodies - this is set to bring them
165 // back into alignment. the correcting angular velocity is
166 // |angular_velocity| = angle/time = erp*theta / stepsize
167 // = (erp*fps) * theta
168 // angular_velocity = |angular_velocity| * u
169 // = (erp*fps) * theta * u
170 // where rotation along unit length axis u by theta brings body 2's frame
171 // to qrel with respect to body 1's frame. using a small angle approximation
172 // for sin(), this gives
173 // angular_velocity = (erp*fps) * 2 * v
174 // where the quaternion of the relative rotation between the two bodies is
175 // q = [cos(theta/2) sin(theta/2)*u] = [s v]
176
177 // get qerr = relative rotation (rotation error) between two bodies
178 dQuaternion qerr,e;
179 if (joint->node[1].body) {
180 dQuaternion qq;
181 dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q);
182 dQMultiply2 (qerr,qq,qrel);
183 }
184 else {
185 dQMultiply3 (qerr,joint->node[0].body->q,qrel);
186 }
187 if (qerr[0] < 0) {
188 qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
189 qerr[2] = -qerr[2];
190 qerr[3] = -qerr[3];
191 }
192 dMULTIPLY0_331 (e,joint->node[0].body->posr.R,qerr+1); // @@@ bad SIMD padding!
193 dReal k = info->fps * info->erp;
194 info->c[start_row] = 2*k * e[0];
195 info->c[start_row+1] = 2*k * e[1];
196 info->c[start_row+2] = 2*k * e[2];
197}
198
199
200// compute anchor points relative to bodies
201
202static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z,
203 dVector3 anchor1, dVector3 anchor2)
204{
205 if (j->node[0].body) {
206 dReal q[4];
207 q[0] = x - j->node[0].body->posr.pos[0];
208 q[1] = y - j->node[0].body->posr.pos[1];
209 q[2] = z - j->node[0].body->posr.pos[2];
210 q[3] = 0;
211 dMULTIPLY1_331 (anchor1,j->node[0].body->posr.R,q);
212 if (j->node[1].body) {
213 q[0] = x - j->node[1].body->posr.pos[0];
214 q[1] = y - j->node[1].body->posr.pos[1];
215 q[2] = z - j->node[1].body->posr.pos[2];
216 q[3] = 0;
217 dMULTIPLY1_331 (anchor2,j->node[1].body->posr.R,q);
218 }
219 else {
220 anchor2[0] = x;
221 anchor2[1] = y;
222 anchor2[2] = z;
223 }
224 }
225 anchor1[3] = 0;
226 anchor2[3] = 0;
227}
228
229
230// compute axes relative to bodies. either axis1 or axis2 can be 0.
231
232static void setAxes (dxJoint *j, dReal x, dReal y, dReal z,
233 dVector3 axis1, dVector3 axis2)
234{
235 if (j->node[0].body) {
236 dReal q[4];
237 q[0] = x;
238 q[1] = y;
239 q[2] = z;
240 q[3] = 0;
241 dNormalize3 (q);
242 if (axis1) {
243 dMULTIPLY1_331 (axis1,j->node[0].body->posr.R,q);
244 axis1[3] = 0;
245 }
246 if (axis2) {
247 if (j->node[1].body) {
248 dMULTIPLY1_331 (axis2,j->node[1].body->posr.R,q);
249 }
250 else {
251 axis2[0] = x;
252 axis2[1] = y;
253 axis2[2] = z;
254 }
255 axis2[3] = 0;
256 }
257 }
258}
259
260
261static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1)
262{
263 if (j->node[0].body) {
264 dMULTIPLY0_331 (result,j->node[0].body->posr.R,anchor1);
265 result[0] += j->node[0].body->posr.pos[0];
266 result[1] += j->node[0].body->posr.pos[1];
267 result[2] += j->node[0].body->posr.pos[2];
268 }
269}
270
271
272static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2)
273{
274 if (j->node[1].body) {
275 dMULTIPLY0_331 (result,j->node[1].body->posr.R,anchor2);
276 result[0] += j->node[1].body->posr.pos[0];
277 result[1] += j->node[1].body->posr.pos[1];
278 result[2] += j->node[1].body->posr.pos[2];
279 }
280 else {
281 result[0] = anchor2[0];
282 result[1] = anchor2[1];
283 result[2] = anchor2[2];
284 }
285}
286
287
288static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1)
289{
290 if (j->node[0].body) {
291 dMULTIPLY0_331 (result,j->node[0].body->posr.R,axis1);
292 }
293}
294
295
296static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2)
297{
298 if (j->node[1].body) {
299 dMULTIPLY0_331 (result,j->node[1].body->posr.R,axis2);
300 }
301 else {
302 result[0] = axis2[0];
303 result[1] = axis2[1];
304 result[2] = axis2[2];
305 }
306}
307
308
309static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis)
310{
311 // the angle between the two bodies is extracted from the quaternion that
312 // represents the relative rotation between them. recall that a quaternion
313 // q is:
314 // [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
315 // where s is a scalar and v is a 3-vector. u is a unit length axis and
316 // theta is a rotation along that axis. we can get theta/2 by:
317 // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
318 // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
319 // |v| = |sin(theta/2)| * |u|
320 // = |sin(theta/2)|
321 // using this value will have a strange effect. recall that there are two
322 // quaternion representations of a given rotation, q and -q. typically as
323 // a body rotates along the axis it will go through a complete cycle using
324 // one representation and then the next cycle will use the other
325 // representation. this corresponds to u pointing in the direction of the
326 // hinge axis and then in the opposite direction. the result is that theta
327 // will appear to go "backwards" every other cycle. here is a fix: if u
328 // points "away" from the direction of the hinge (motor) axis (i.e. more
329 // than 90 degrees) then use -q instead of q. this represents the same
330 // rotation, but results in the cos(theta/2) value being sign inverted.
331
332 // extract the angle from the quaternion. cost2 = cos(theta/2),
333 // sint2 = |sin(theta/2)|
334 dReal cost2 = qrel[0];
335 dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]);
336 dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions
337 (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis
338 (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction
339
340 // the angle we get will be between 0..2*pi, but we want to return angles
341 // between -pi..pi
342 if (theta > M_PI) theta -= 2*M_PI;
343
344 // the angle we've just extracted has the wrong sign
345 theta = -theta;
346
347 return theta;
348}
349
350
351// given two bodies (body1,body2), the hinge axis that they are connected by
352// w.r.t. body1 (axis), and the initial relative orientation between them
353// (q_initial), return the relative rotation angle. the initial relative
354// orientation corresponds to an angle of zero. if body2 is 0 then measure the
355// angle between body1 and the static frame.
356//
357// this will not return the correct angle if the bodies rotate along any axis
358// other than the given hinge axis.
359
360static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis,
361 dQuaternion q_initial)
362{
363 // get qrel = relative rotation between the two bodies
364 dQuaternion qrel;
365 if (body2) {
366 dQuaternion qq;
367 dQMultiply1 (qq,body1->q,body2->q);
368 dQMultiply2 (qrel,qq,q_initial);
369 }
370 else {
371 // pretend body2->q is the identity
372 dQMultiply3 (qrel,body1->q,q_initial);
373 }
374
375 return getHingeAngleFromRelativeQuat (qrel,axis);
376}
377
378//****************************************************************************
379// dxJointLimitMotor
380
381void dxJointLimitMotor::init (dxWorld *world)
382{
383 vel = 0;
384 fmax = 0;
385 lostop = -dInfinity;
386 histop = dInfinity;
387 fudge_factor = 1;
388 normal_cfm = world->global_cfm;
389 stop_erp = world->global_erp;
390 stop_cfm = world->global_cfm;
391 bounce = 0;
392 limit = 0;
393 limit_err = 0;
394}
395
396
397void dxJointLimitMotor::set (int num, dReal value)
398{
399 switch (num) {
400 case dParamLoStop:
401 lostop = value;
402 break;
403 case dParamHiStop:
404 histop = value;
405 break;
406 case dParamVel:
407 vel = value;
408 break;
409 case dParamFMax:
410 if (value >= 0) fmax = value;
411 break;
412 case dParamFudgeFactor:
413 if (value >= 0 && value <= 1) fudge_factor = value;
414 break;
415 case dParamBounce:
416 bounce = value;
417 break;
418 case dParamCFM:
419 normal_cfm = value;
420 break;
421 case dParamStopERP:
422 stop_erp = value;
423 break;
424 case dParamStopCFM:
425 stop_cfm = value;
426 break;
427 }
428}
429
430
431dReal dxJointLimitMotor::get (int num)
432{
433 switch (num) {
434 case dParamLoStop: return lostop;
435 case dParamHiStop: return histop;
436 case dParamVel: return vel;
437 case dParamFMax: return fmax;
438 case dParamFudgeFactor: return fudge_factor;
439 case dParamBounce: return bounce;
440 case dParamCFM: return normal_cfm;
441 case dParamStopERP: return stop_erp;
442 case dParamStopCFM: return stop_cfm;
443 default: return 0;
444 }
445}
446
447
448int dxJointLimitMotor::testRotationalLimit (dReal angle)
449{
450 if (angle <= lostop) {
451 limit = 1;
452 limit_err = angle - lostop;
453 return 1;
454 }
455 else if (angle >= histop) {
456 limit = 2;
457 limit_err = angle - histop;
458 return 1;
459 }
460 else {
461 limit = 0;
462 return 0;
463 }
464}
465
466
467int dxJointLimitMotor::addLimot (dxJoint *joint,
468 dxJoint::Info2 *info, int row,
469 dVector3 ax1, int rotational)
470{
471 int srow = row * info->rowskip;
472
473 // if the joint is powered, or has joint limits, add in the extra row
474 int powered = fmax > 0;
475 if (powered || limit) {
476 dReal *J1 = rotational ? info->J1a : info->J1l;
477 dReal *J2 = rotational ? info->J2a : info->J2l;
478
479 J1[srow+0] = ax1[0];
480 J1[srow+1] = ax1[1];
481 J1[srow+2] = ax1[2];
482 if (joint->node[1].body) {
483 J2[srow+0] = -ax1[0];
484 J2[srow+1] = -ax1[1];
485 J2[srow+2] = -ax1[2];
486 }
487
488 // linear limot torque decoupling step:
489 //
490 // if this is a linear limot (e.g. from a slider), we have to be careful
491 // that the linear constraint forces (+/- ax1) applied to the two bodies
492 // do not create a torque couple. in other words, the points that the
493 // constraint force is applied at must lie along the same ax1 axis.
494 // a torque couple will result in powered or limited slider-jointed free
495 // bodies from gaining angular momentum.
496 // the solution used here is to apply the constraint forces at the point
497 // halfway between the body centers. there is no penalty (other than an
498 // extra tiny bit of computation) in doing this adjustment. note that we
499 // only need to do this if the constraint connects two bodies.
500
501 dVector3 ltd; // Linear Torque Decoupling vector (a torque)
502 if (!rotational && joint->node[1].body) {
503 dVector3 c;
504 c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]);
505 c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]);
506 c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]);
507 dCROSS (ltd,=,c,ax1);
508 info->J1a[srow+0] = ltd[0];
509 info->J1a[srow+1] = ltd[1];
510 info->J1a[srow+2] = ltd[2];
511 info->J2a[srow+0] = ltd[0];
512 info->J2a[srow+1] = ltd[1];
513 info->J2a[srow+2] = ltd[2];
514 }
515
516 // if we're limited low and high simultaneously, the joint motor is
517 // ineffective
518 if (limit && (lostop == histop)) powered = 0;
519
520 if (powered) {
521 info->cfm[row] = normal_cfm;
522 if (! limit) {
523 info->c[row] = vel;
524 info->lo[row] = -fmax;
525 info->hi[row] = fmax;
526 }
527 else {
528 // the joint is at a limit, AND is being powered. if the joint is
529 // being powered into the limit then we apply the maximum motor force
530 // in that direction, because the motor is working against the
531 // immovable limit. if the joint is being powered away from the limit
532 // then we have problems because actually we need *two* lcp
533 // constraints to handle this case. so we fake it and apply some
534 // fraction of the maximum force. the fraction to use can be set as
535 // a fudge factor.
536
537 dReal fm = fmax;
538 if ((vel > 0) || (vel==0 && limit==2)) fm = -fm;
539
540 // if we're powering away from the limit, apply the fudge factor
541 if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor;
542
543 if (rotational) {
544 dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],
545 -fm*ax1[2]);
546 if (joint->node[1].body)
547 dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
548 }
549 else {
550 dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]);
551 if (joint->node[1].body) {
552 dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
553
554 // linear limot torque decoupling step: refer to above discussion
555 dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1],
556 -fm*ltd[2]);
557 dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1],
558 -fm*ltd[2]);
559 }
560 }
561 }
562 }
563
564 if (limit) {
565 dReal k = info->fps * stop_erp;
566 info->c[row] = -k * limit_err;
567 info->cfm[row] = stop_cfm;
568
569 if (lostop == histop) {
570 // limited low and high simultaneously
571 info->lo[row] = -dInfinity;
572 info->hi[row] = dInfinity;
573 }
574 else {
575 if (limit == 1) {
576 // low limit
577 info->lo[row] = 0;
578 info->hi[row] = dInfinity;
579 }
580 else {
581 // high limit
582 info->lo[row] = -dInfinity;
583 info->hi[row] = 0;
584 }
585
586 // deal with bounce
587 if (bounce > 0) {
588 // calculate joint velocity
589 dReal vel;
590 if (rotational) {
591 vel = dDOT(joint->node[0].body->avel,ax1);
592 if (joint->node[1].body)
593 vel -= dDOT(joint->node[1].body->avel,ax1);
594 }
595 else {
596 vel = dDOT(joint->node[0].body->lvel,ax1);
597 if (joint->node[1].body)
598 vel -= dDOT(joint->node[1].body->lvel,ax1);
599 }
600
601 // only apply bounce if the velocity is incoming, and if the
602 // resulting c[] exceeds what we already have.
603 if (limit == 1) {
604 // low limit
605 if (vel < 0) {
606 dReal newc = -bounce * vel;
607 if (newc > info->c[row]) info->c[row] = newc;
608 }
609 }
610 else {
611 // high limit - all those computations are reversed
612 if (vel > 0) {
613 dReal newc = -bounce * vel;
614 if (newc < info->c[row]) info->c[row] = newc;
615 }
616 }
617 }
618 }
619 }
620 return 1;
621 }
622 else return 0;
623}
624
625//****************************************************************************
626// ball and socket
627
628static void ballInit (dxJointBall *j)
629{
630 dSetZero (j->anchor1,4);
631 dSetZero (j->anchor2,4);
632 j->erp = j->world->global_erp;
633 j->cfm = j->world->global_cfm;
634}
635
636
637static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info)
638{
639 info->m = 3;
640 info->nub = 3;
641}
642
643
644static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info)
645{
646 info->erp = joint->erp;
647 info->cfm[0] = joint->cfm;
648 info->cfm[1] = joint->cfm;
649 info->cfm[2] = joint->cfm;
650 setBall (joint,info,joint->anchor1,joint->anchor2);
651}
652
653
654void dJointSetBallAnchor (dJointID j, dReal x, dReal y, dReal z)
655{
656 dxJointBall* joint = (dxJointBall*)j;
657 dUASSERT(joint,"bad joint argument");
658 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
659 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
660}
661
662
663void dJointSetBallAnchor2 (dJointID j, dReal x, dReal y, dReal z)
664{
665 dxJointBall* joint = (dxJointBall*)j;
666 dUASSERT(joint,"bad joint argument");
667 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
668 joint->anchor2[0] = x;
669 joint->anchor2[1] = y;
670 joint->anchor2[2] = z;
671 joint->anchor2[3] = 0;
672
673}
674
675void dJointGetBallAnchor (dJointID j, dVector3 result)
676{
677 dxJointBall* joint = (dxJointBall*)j;
678 dUASSERT(joint,"bad joint argument");
679 dUASSERT(result,"bad result argument");
680 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
681 if (joint->flags & dJOINT_REVERSE)
682 getAnchor2 (joint,result,joint->anchor2);
683 else
684 getAnchor (joint,result,joint->anchor1);
685}
686
687
688void dJointGetBallAnchor2 (dJointID j, dVector3 result)
689{
690 dxJointBall* joint = (dxJointBall*)j;
691 dUASSERT(joint,"bad joint argument");
692 dUASSERT(result,"bad result argument");
693 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
694 if (joint->flags & dJOINT_REVERSE)
695 getAnchor (joint,result,joint->anchor1);
696 else
697 getAnchor2 (joint,result,joint->anchor2);
698}
699
700
701void dxJointBall::set (int num, dReal value)
702{
703 switch (num) {
704 case dParamCFM:
705 cfm = value;
706 break;
707 case dParamERP:
708 erp = value;
709 break;
710 }
711}
712
713
714dReal dxJointBall::get (int num)
715{
716 switch (num) {
717 case dParamCFM:
718 return cfm;
719 case dParamERP:
720 return erp;
721 default:
722 return 0;
723 }
724}
725
726
727void dJointSetBallParam (dJointID j, int parameter, dReal value)
728{
729 dxJointBall* joint = (dxJointBall*)j;
730 dUASSERT(joint,"bad joint argument");
731 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball joint");
732 joint->set (parameter,value);
733}
734
735
736dReal dJointGetBallParam (dJointID j, int parameter)
737{
738 dxJointBall* joint = (dxJointBall*)j;
739 dUASSERT(joint,"bad joint argument");
740 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball joint");
741 return joint->get (parameter);
742}
743
744
745dxJoint::Vtable __dball_vtable = {
746 sizeof(dxJointBall),
747 (dxJoint::init_fn*) ballInit,
748 (dxJoint::getInfo1_fn*) ballGetInfo1,
749 (dxJoint::getInfo2_fn*) ballGetInfo2,
750 dJointTypeBall};
751
752//****************************************************************************
753// hinge
754
755static void hingeInit (dxJointHinge *j)
756{
757 dSetZero (j->anchor1,4);
758 dSetZero (j->anchor2,4);
759 dSetZero (j->axis1,4);
760 j->axis1[0] = 1;
761 dSetZero (j->axis2,4);
762 j->axis2[0] = 1;
763 dSetZero (j->qrel,4);
764 j->limot.init (j->world);
765}
766
767
768static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info)
769{
770 info->nub = 5;
771
772 // see if joint is powered
773 if (j->limot.fmax > 0)
774 info->m = 6; // powered hinge needs an extra constraint row
775 else info->m = 5;
776
777 // see if we're at a joint limit.
778 if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) &&
779 j->limot.lostop <= j->limot.histop) {
780 dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1,
781 j->qrel);
782 if (j->limot.testRotationalLimit (angle)) info->m = 6;
783 }
784}
785
786
787static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info)
788{
789 // set the three ball-and-socket rows
790 setBall (joint,info,joint->anchor1,joint->anchor2);
791
792 // set the two hinge rows. the hinge axis should be the only unconstrained
793 // rotational axis, the angular velocity of the two bodies perpendicular to
794 // the hinge axis should be equal. thus the constraint equations are
795 // p*w1 - p*w2 = 0
796 // q*w1 - q*w2 = 0
797 // where p and q are unit vectors normal to the hinge axis, and w1 and w2
798 // are the angular velocity vectors of the two bodies.
799
800 dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
801 dVector3 p,q; // plane space vectors for ax1
802 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1);
803 dPlaneSpace (ax1,p,q);
804
805 int s3=3*info->rowskip;
806 int s4=4*info->rowskip;
807
808 info->J1a[s3+0] = p[0];
809 info->J1a[s3+1] = p[1];
810 info->J1a[s3+2] = p[2];
811 info->J1a[s4+0] = q[0];
812 info->J1a[s4+1] = q[1];
813 info->J1a[s4+2] = q[2];
814
815 if (joint->node[1].body) {
816 info->J2a[s3+0] = -p[0];
817 info->J2a[s3+1] = -p[1];
818 info->J2a[s3+2] = -p[2];
819 info->J2a[s4+0] = -q[0];
820 info->J2a[s4+1] = -q[1];
821 info->J2a[s4+2] = -q[2];
822 }
823
824 // compute the right hand side of the constraint equation. set relative
825 // body velocities along p and q to bring the hinge back into alignment.
826 // if ax1,ax2 are the unit length hinge axes as computed from body1 and
827 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
828 // if `theta' is the angle between ax1 and ax2, we need an angular velocity
829 // along u to cover angle erp*theta in one step :
830 // |angular_velocity| = angle/time = erp*theta / stepsize
831 // = (erp*fps) * theta
832 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
833 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
834 // ...as ax1 and ax2 are unit length. if theta is smallish,
835 // theta ~= sin(theta), so
836 // angular_velocity = (erp*fps) * (ax1 x ax2)
837 // ax1 x ax2 is in the plane space of ax1, so we project the angular
838 // velocity to p and q to find the right hand side.
839
840 dVector3 ax2,b;
841 if (joint->node[1].body) {
842 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2);
843 }
844 else {
845 ax2[0] = joint->axis2[0];
846 ax2[1] = joint->axis2[1];
847 ax2[2] = joint->axis2[2];
848 }
849 dCROSS (b,=,ax1,ax2);
850 dReal k = info->fps * info->erp;
851 info->c[3] = k * dDOT(b,p);
852 info->c[4] = k * dDOT(b,q);
853
854 // if the hinge is powered, or has joint limits, add in the stuff
855 joint->limot.addLimot (joint,info,5,ax1,1);
856}
857
858
859// compute initial relative rotation body1 -> body2, or env -> body1
860
861static void hingeComputeInitialRelativeRotation (dxJointHinge *joint)
862{
863 if (joint->node[0].body) {
864 if (joint->node[1].body) {
865 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
866 }
867 else {
868 // set joint->qrel to the transpose of the first body q
869 joint->qrel[0] = joint->node[0].body->q[0];
870 for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
871 }
872 }
873}
874
875
876void dJointSetHingeAnchor (dJointID j, dReal x, dReal y, dReal z)
877{
878 dxJointHinge* joint = (dxJointHinge*)j;
879 dUASSERT(joint,"bad joint argument");
880 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
881 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
882 hingeComputeInitialRelativeRotation (joint);
883}
884
885
886void dJointSetHingeAnchorDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz)
887{
888 dxJointHinge* joint = (dxJointHinge*)j;
889 dUASSERT(joint,"bad joint argument");
890 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
891
892 if (joint->node[0].body) {
893 dReal q[4];
894 q[0] = x - joint->node[0].body->posr.pos[0];
895 q[1] = y - joint->node[0].body->posr.pos[1];
896 q[2] = z - joint->node[0].body->posr.pos[2];
897 q[3] = 0;
898 dMULTIPLY1_331 (joint->anchor1,joint->node[0].body->posr.R,q);
899
900 if (joint->node[1].body) {
901 q[0] = x - joint->node[1].body->posr.pos[0];
902 q[1] = y - joint->node[1].body->posr.pos[1];
903 q[2] = z - joint->node[1].body->posr.pos[2];
904 q[3] = 0;
905 dMULTIPLY1_331 (joint->anchor2,joint->node[1].body->posr.R,q);
906 }
907 else {
908 // Move the relative displacement between the passive body and the
909 // anchor in the same direction as the passive body has just moved
910 joint->anchor2[0] = x + dx;
911 joint->anchor2[1] = y + dy;
912 joint->anchor2[2] = z + dz;
913 }
914 }
915 joint->anchor1[3] = 0;
916 joint->anchor2[3] = 0;
917
918 hingeComputeInitialRelativeRotation (joint);
919}
920
921
922
923void dJointSetHingeAxis (dJointID j, dReal x, dReal y, dReal z)
924{
925 dxJointHinge* joint = (dxJointHinge*)j;
926 dUASSERT(joint,"bad joint argument");
927 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
928 setAxes (joint,x,y,z,joint->axis1,joint->axis2);
929 hingeComputeInitialRelativeRotation (joint);
930}
931
932
933void dJointGetHingeAnchor (dJointID j, dVector3 result)
934{
935 dxJointHinge* joint = (dxJointHinge*)j;
936 dUASSERT(joint,"bad joint argument");
937 dUASSERT(result,"bad result argument");
938 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
939 if (joint->flags & dJOINT_REVERSE)
940 getAnchor2 (joint,result,joint->anchor2);
941 else
942 getAnchor (joint,result,joint->anchor1);
943}
944
945
946void dJointGetHingeAnchor2 (dJointID j, dVector3 result)
947{
948 dxJointHinge* joint = (dxJointHinge*)j;
949 dUASSERT(joint,"bad joint argument");
950 dUASSERT(result,"bad result argument");
951 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
952 if (joint->flags & dJOINT_REVERSE)
953 getAnchor (joint,result,joint->anchor1);
954 else
955 getAnchor2 (joint,result,joint->anchor2);
956}
957
958
959void dJointGetHingeAxis (dJointID j, dVector3 result)
960{
961 dxJointHinge* joint = (dxJointHinge*)j;
962 dUASSERT(joint,"bad joint argument");
963 dUASSERT(result,"bad result argument");
964 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
965 getAxis (joint,result,joint->axis1);
966}
967
968
969void dJointSetHingeParam (dJointID j, int parameter, dReal value)
970{
971 dxJointHinge* joint = (dxJointHinge*)j;
972 dUASSERT(joint,"bad joint argument");
973 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
974 joint->limot.set (parameter,value);
975}
976
977
978dReal dJointGetHingeParam (dJointID j, int parameter)
979{
980 dxJointHinge* joint = (dxJointHinge*)j;
981 dUASSERT(joint,"bad joint argument");
982 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
983 return joint->limot.get (parameter);
984}
985
986
987dReal dJointGetHingeAngle (dJointID j)
988{
989 dxJointHinge* joint = (dxJointHinge*)j;
990 dAASSERT(joint);
991 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
992 if (joint->node[0].body) {
993 dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1,
994 joint->qrel);
995 if (joint->flags & dJOINT_REVERSE)
996 return -ang;
997 else
998 return ang;
999 }
1000 else return 0;
1001}
1002
1003
1004dReal dJointGetHingeAngleRate (dJointID j)
1005{
1006 dxJointHinge* joint = (dxJointHinge*)j;
1007 dAASSERT(joint);
1008 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
1009 if (joint->node[0].body) {
1010 dVector3 axis;
1011 dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1);
1012 dReal rate = dDOT(axis,joint->node[0].body->avel);
1013 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
1014 if (joint->flags & dJOINT_REVERSE) rate = - rate;
1015 return rate;
1016 }
1017 else return 0;
1018}
1019
1020
1021void dJointAddHingeTorque (dJointID j, dReal torque)
1022{
1023 dxJointHinge* joint = (dxJointHinge*)j;
1024 dVector3 axis;
1025 dAASSERT(joint);
1026 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
1027
1028 if (joint->flags & dJOINT_REVERSE)
1029 torque = -torque;
1030
1031 getAxis (joint,axis,joint->axis1);
1032 axis[0] *= torque;
1033 axis[1] *= torque;
1034 axis[2] *= torque;
1035
1036 if (joint->node[0].body != 0)
1037 dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]);
1038 if (joint->node[1].body != 0)
1039 dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
1040}
1041
1042
1043dxJoint::Vtable __dhinge_vtable = {
1044 sizeof(dxJointHinge),
1045 (dxJoint::init_fn*) hingeInit,
1046 (dxJoint::getInfo1_fn*) hingeGetInfo1,
1047 (dxJoint::getInfo2_fn*) hingeGetInfo2,
1048 dJointTypeHinge};
1049
1050//****************************************************************************
1051// slider
1052
1053static void sliderInit (dxJointSlider *j)
1054{
1055 dSetZero (j->axis1,4);
1056 j->axis1[0] = 1;
1057 dSetZero (j->qrel,4);
1058 dSetZero (j->offset,4);
1059 j->limot.init (j->world);
1060}
1061
1062
1063dReal dJointGetSliderPosition (dJointID j)
1064{
1065 dxJointSlider* joint = (dxJointSlider*)j;
1066 dUASSERT(joint,"bad joint argument");
1067 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1068
1069 // get axis1 in global coordinates
1070 dVector3 ax1,q;
1071 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1);
1072
1073 if (joint->node[1].body) {
1074 // get body2 + offset point in global coordinates
1075 dMULTIPLY0_331 (q,joint->node[1].body->posr.R,joint->offset);
1076 for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - q[i] -
1077 joint->node[1].body->posr.pos[i];
1078 }
1079 else {
1080 for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] -
1081 joint->offset[i];
1082
1083 }
1084 return dDOT(ax1,q);
1085}
1086
1087
1088dReal dJointGetSliderPositionRate (dJointID j)
1089{
1090 dxJointSlider* joint = (dxJointSlider*)j;
1091 dUASSERT(joint,"bad joint argument");
1092 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1093
1094 // get axis1 in global coordinates
1095 dVector3 ax1;
1096 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1);
1097
1098 if (joint->node[1].body) {
1099 return dDOT(ax1,joint->node[0].body->lvel) -
1100 dDOT(ax1,joint->node[1].body->lvel);
1101 }
1102 else {
1103 return dDOT(ax1,joint->node[0].body->lvel);
1104 }
1105}
1106
1107
1108static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info)
1109{
1110 info->nub = 5;
1111
1112 // see if joint is powered
1113 if (j->limot.fmax > 0)
1114 info->m = 6; // powered slider needs an extra constraint row
1115 else info->m = 5;
1116
1117 // see if we're at a joint limit.
1118 j->limot.limit = 0;
1119 if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) &&
1120 j->limot.lostop <= j->limot.histop) {
1121 // measure joint position
1122 dReal pos = dJointGetSliderPosition (j);
1123 if (pos <= j->limot.lostop) {
1124 j->limot.limit = 1;
1125 j->limot.limit_err = pos - j->limot.lostop;
1126 info->m = 6;
1127 }
1128 else if (pos >= j->limot.histop) {
1129 j->limot.limit = 2;
1130 j->limot.limit_err = pos - j->limot.histop;
1131 info->m = 6;
1132 }
1133 }
1134}
1135
1136
1137static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info)
1138{
1139 int i,s = info->rowskip;
1140 int s3=3*s,s4=4*s;
1141
1142 // pull out pos and R for both bodies. also get the `connection'
1143 // vector pos2-pos1.
1144
1145 dReal *pos1,*pos2,*R1,*R2;
1146 dVector3 c;
1147 pos1 = joint->node[0].body->posr.pos;
1148 R1 = joint->node[0].body->posr.R;
1149 if (joint->node[1].body) {
1150 pos2 = joint->node[1].body->posr.pos;
1151 R2 = joint->node[1].body->posr.R;
1152 for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i];
1153 }
1154 else {
1155 pos2 = 0;
1156 R2 = 0;
1157 }
1158
1159 // 3 rows to make body rotations equal
1160 setFixedOrientation(joint, info, joint->qrel, 0);
1161
1162 // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
1163 // result in three equations, so we project along the planespace vectors
1164 // so that sliding along the slider axis is disregarded. for symmetry we
1165 // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
1166
1167 dVector3 ax1; // joint axis in global coordinates (unit length)
1168 dVector3 p,q; // plane space of ax1
1169 dMULTIPLY0_331 (ax1,R1,joint->axis1);
1170 dPlaneSpace (ax1,p,q);
1171 if (joint->node[1].body) {
1172 dVector3 tmp;
1173 dCROSS (tmp, = REAL(0.5) * ,c,p);
1174 for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i];
1175 for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
1176 dCROSS (tmp, = REAL(0.5) * ,c,q);
1177 for (i=0; i<3; i++) info->J1a[s4+i] = tmp[i];
1178 for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
1179 for (i=0; i<3; i++) info->J2l[s3+i] = -p[i];
1180 for (i=0; i<3; i++) info->J2l[s4+i] = -q[i];
1181 }
1182 for (i=0; i<3; i++) info->J1l[s3+i] = p[i];
1183 for (i=0; i<3; i++) info->J1l[s4+i] = q[i];
1184
1185 // compute last two elements of right hand side. we want to align the offset
1186 // point (in body 2's frame) with the center of body 1.
1187 dReal k = info->fps * info->erp;
1188 if (joint->node[1].body) {
1189 dVector3 ofs; // offset point in global coordinates
1190 dMULTIPLY0_331 (ofs,R2,joint->offset);
1191 for (i=0; i<3; i++) c[i] += ofs[i];
1192 info->c[3] = k * dDOT(p,c);
1193 info->c[4] = k * dDOT(q,c);
1194 }
1195 else {
1196 dVector3 ofs; // offset point in global coordinates
1197 for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i];
1198 info->c[3] = k * dDOT(p,ofs);
1199 info->c[4] = k * dDOT(q,ofs);
1200 }
1201
1202 // if the slider is powered, or has joint limits, add in the extra row
1203 joint->limot.addLimot (joint,info,5,ax1,0);
1204}
1205
1206
1207void dJointSetSliderAxis (dJointID j, dReal x, dReal y, dReal z)
1208{
1209 dxJointSlider* joint = (dxJointSlider*)j;
1210 int i;
1211 dUASSERT(joint,"bad joint argument");
1212 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1213 setAxes (joint,x,y,z,joint->axis1,0);
1214
1215 // compute initial relative rotation body1 -> body2, or env -> body1
1216 // also compute center of body1 w.r.t body 2
1217 if (joint->node[1].body) {
1218 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
1219 dVector3 c;
1220 for (i=0; i<3; i++)
1221 c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i];
1222 dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c);
1223 }
1224 else {
1225 // set joint->qrel to the transpose of the first body's q
1226 joint->qrel[0] = joint->node[0].body->q[0];
1227 for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
1228 for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->posr.pos[i];
1229 }
1230}
1231
1232
1233void dJointSetSliderAxisDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz)
1234{
1235 dxJointSlider* joint = (dxJointSlider*)j;
1236 int i;
1237 dUASSERT(joint,"bad joint argument");
1238 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1239 setAxes (joint,x,y,z,joint->axis1,0);
1240
1241 // compute initial relative rotation body1 -> body2, or env -> body1
1242 // also compute center of body1 w.r.t body 2
1243 if (joint->node[1].body) {
1244 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
1245 dVector3 c;
1246 for (i=0; i<3; i++)
1247 c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i];
1248 dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c);
1249 }
1250 else {
1251 // set joint->qrel to the transpose of the first body's q
1252 joint->qrel[0] = joint->node[0].body->q[0];
1253
1254 for (i=1; i<4; i++)
1255 joint->qrel[i] = -joint->node[0].body->q[i];
1256
1257 joint->offset[0] = joint->node[0].body->posr.pos[0] + dx;
1258 joint->offset[1] = joint->node[0].body->posr.pos[1] + dy;
1259 joint->offset[2] = joint->node[0].body->posr.pos[2] + dz;
1260 }
1261}
1262
1263
1264
1265void dJointGetSliderAxis (dJointID j, dVector3 result)
1266{
1267 dxJointSlider* joint = (dxJointSlider*)j;
1268 dUASSERT(joint,"bad joint argument");
1269 dUASSERT(result,"bad result argument");
1270 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1271 getAxis (joint,result,joint->axis1);
1272}
1273
1274
1275void dJointSetSliderParam (dJointID j, int parameter, dReal value)
1276{
1277 dxJointSlider* joint = (dxJointSlider*)j;
1278 dUASSERT(joint,"bad joint argument");
1279 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1280 joint->limot.set (parameter,value);
1281}
1282
1283
1284dReal dJointGetSliderParam (dJointID j, int parameter)
1285{
1286 dxJointSlider* joint = (dxJointSlider*)j;
1287 dUASSERT(joint,"bad joint argument");
1288 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1289 return joint->limot.get (parameter);
1290}
1291
1292
1293void dJointAddSliderForce (dJointID j, dReal force)
1294{
1295 dxJointSlider* joint = (dxJointSlider*)j;
1296 dVector3 axis;
1297 dUASSERT(joint,"bad joint argument");
1298 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1299
1300 if (joint->flags & dJOINT_REVERSE)
1301 force -= force;
1302
1303 getAxis (joint,axis,joint->axis1);
1304 axis[0] *= force;
1305 axis[1] *= force;
1306 axis[2] *= force;
1307
1308 if (joint->node[0].body != 0)
1309 dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]);
1310 if (joint->node[1].body != 0)
1311 dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
1312
1313 if (joint->node[0].body != 0 && joint->node[1].body != 0) {
1314 // linear torque decoupling:
1315 // we have to compensate the torque, that this slider force may generate
1316 // if body centers are not aligned along the slider axis
1317
1318 dVector3 ltd; // Linear Torque Decoupling vector (a torque)
1319
1320 dVector3 c;
1321 c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]);
1322 c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]);
1323 c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]);
1324 dCROSS (ltd,=,c,axis);
1325
1326 dBodyAddTorque (joint->node[0].body,ltd[0],ltd[1], ltd[2]);
1327 dBodyAddTorque (joint->node[1].body,ltd[0],ltd[1], ltd[2]);
1328 }
1329}
1330
1331
1332dxJoint::Vtable __dslider_vtable = {
1333 sizeof(dxJointSlider),
1334 (dxJoint::init_fn*) sliderInit,
1335 (dxJoint::getInfo1_fn*) sliderGetInfo1,
1336 (dxJoint::getInfo2_fn*) sliderGetInfo2,
1337 dJointTypeSlider};
1338
1339//****************************************************************************
1340// contact
1341
1342static void contactInit (dxJointContact *j)
1343{
1344 // default frictionless contact. hmmm, this info gets overwritten straight
1345 // away anyway, so why bother?
1346#if 0 /* so don't bother ;) */
1347 j->contact.surface.mode = 0;
1348 j->contact.surface.mu = 0;
1349 dSetZero (j->contact.geom.pos,4);
1350 dSetZero (j->contact.geom.normal,4);
1351 j->contact.geom.depth = 0;
1352#endif
1353}
1354
1355
1356static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info)
1357{
1358 // make sure mu's >= 0, then calculate number of constraint rows and number
1359 // of unbounded rows.
1360 int m = 1, nub=0;
1361 if (j->contact.surface.mu < 0) j->contact.surface.mu = 0;
1362 if (j->contact.surface.mode & dContactMu2) {
1363 if (j->contact.surface.mu > 0) m++;
1364 if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0;
1365 if (j->contact.surface.mu2 > 0) m++;
1366 if (j->contact.surface.mu == dInfinity) nub ++;
1367 if (j->contact.surface.mu2 == dInfinity) nub ++;
1368 }
1369 else {
1370 if (j->contact.surface.mu > 0) m += 2;
1371 if (j->contact.surface.mu == dInfinity) nub += 2;
1372 }
1373
1374 j->the_m = m;
1375 info->m = m;
1376 info->nub = nub;
1377}
1378
1379
1380static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info)
1381{
1382 int s = info->rowskip;
1383 int s2 = 2*s;
1384
1385 // get normal, with sign adjusted for body1/body2 polarity
1386 dVector3 normal;
1387 if (j->flags & dJOINT_REVERSE) {
1388 normal[0] = - j->contact.geom.normal[0];
1389 normal[1] = - j->contact.geom.normal[1];
1390 normal[2] = - j->contact.geom.normal[2];
1391 }
1392 else {
1393 normal[0] = j->contact.geom.normal[0];
1394 normal[1] = j->contact.geom.normal[1];
1395 normal[2] = j->contact.geom.normal[2];
1396 }
1397 normal[3] = 0; // @@@ hmmm
1398
1399 // c1,c2 = contact points with respect to body PORs
1400 dVector3 c1,c2;
1401 c1[0] = j->contact.geom.pos[0] - j->node[0].body->posr.pos[0];
1402 c1[1] = j->contact.geom.pos[1] - j->node[0].body->posr.pos[1];
1403 c1[2] = j->contact.geom.pos[2] - j->node[0].body->posr.pos[2];
1404
1405 // set jacobian for normal
1406 info->J1l[0] = normal[0];
1407 info->J1l[1] = normal[1];
1408 info->J1l[2] = normal[2];
1409 dCROSS (info->J1a,=,c1,normal);
1410 if (j->node[1].body) {
1411 c2[0] = j->contact.geom.pos[0] - j->node[1].body->posr.pos[0];
1412 c2[1] = j->contact.geom.pos[1] - j->node[1].body->posr.pos[1];
1413 c2[2] = j->contact.geom.pos[2] - j->node[1].body->posr.pos[2];
1414 info->J2l[0] = -normal[0];
1415 info->J2l[1] = -normal[1];
1416 info->J2l[2] = -normal[2];
1417 dCROSS (info->J2a,= -,c2,normal);
1418 }
1419
1420 // set right hand side and cfm value for normal
1421 dReal erp = info->erp;
1422 if (j->contact.surface.mode & dContactSoftERP)
1423 erp = j->contact.surface.soft_erp;
1424 dReal k = info->fps * erp;
1425 dReal depth = j->contact.geom.depth - j->world->contactp.min_depth;
1426 if (depth < 0) depth = 0;
1427
1428 const dReal maxvel = j->world->contactp.max_vel;
1429 info->c[0] = k*depth;
1430 if (info->c[0] > maxvel)
1431 info->c[0] = maxvel;
1432
1433 if (j->contact.surface.mode & dContactSoftCFM)
1434 info->cfm[0] = j->contact.surface.soft_cfm;
1435
1436 // deal with bounce
1437 if (j->contact.surface.mode & dContactBounce) {
1438 // calculate outgoing velocity (-ve for incoming contact)
1439 dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) +
1440 dDOT(info->J1a,j->node[0].body->avel);
1441 if (j->node[1].body) {
1442 outgoing += dDOT(info->J2l,j->node[1].body->lvel) +
1443 dDOT(info->J2a,j->node[1].body->avel);
1444 }
1445 // only apply bounce if the outgoing velocity is greater than the
1446 // threshold, and if the resulting c[0] exceeds what we already have.
1447 if (j->contact.surface.bounce_vel >= 0 &&
1448 (-outgoing) > j->contact.surface.bounce_vel) {
1449 dReal newc = - j->contact.surface.bounce * outgoing;
1450 if (newc > info->c[0]) info->c[0] = newc;
1451 }
1452 }
1453
1454 // set LCP limits for normal
1455 info->lo[0] = 0;
1456 info->hi[0] = dInfinity;
1457
1458 // now do jacobian for tangential forces
1459 dVector3 t1,t2; // two vectors tangential to normal
1460
1461 // first friction direction
1462 if (j->the_m >= 2) {
1463 if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ?
1464 t1[0] = j->contact.fdir1[0];
1465 t1[1] = j->contact.fdir1[1];
1466 t1[2] = j->contact.fdir1[2];
1467 dCROSS (t2,=,normal,t1);
1468 }
1469 else {
1470 dPlaneSpace (normal,t1,t2);
1471 }
1472 info->J1l[s+0] = t1[0];
1473 info->J1l[s+1] = t1[1];
1474 info->J1l[s+2] = t1[2];
1475 dCROSS (info->J1a+s,=,c1,t1);
1476 if (j->node[1].body) {
1477 info->J2l[s+0] = -t1[0];
1478 info->J2l[s+1] = -t1[1];
1479 info->J2l[s+2] = -t1[2];
1480 dCROSS (info->J2a+s,= -,c2,t1);
1481 }
1482 // set right hand side
1483 if (j->contact.surface.mode & dContactMotion1) {
1484 info->c[1] = j->contact.surface.motion1;
1485 }
1486 // set LCP bounds and friction index. this depends on the approximation
1487 // mode
1488 info->lo[1] = -j->contact.surface.mu;
1489 info->hi[1] = j->contact.surface.mu;
1490 if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0;
1491
1492 // set slip (constraint force mixing)
1493 if (j->contact.surface.mode & dContactSlip1)
1494 info->cfm[1] = j->contact.surface.slip1;
1495 }
1496
1497 // second friction direction
1498 if (j->the_m >= 3) {
1499 info->J1l[s2+0] = t2[0];
1500 info->J1l[s2+1] = t2[1];
1501 info->J1l[s2+2] = t2[2];
1502 dCROSS (info->J1a+s2,=,c1,t2);
1503 if (j->node[1].body) {
1504 info->J2l[s2+0] = -t2[0];
1505 info->J2l[s2+1] = -t2[1];
1506 info->J2l[s2+2] = -t2[2];
1507 dCROSS (info->J2a+s2,= -,c2,t2);
1508 }
1509 // set right hand side
1510 if (j->contact.surface.mode & dContactMotion2) {
1511 info->c[2] = j->contact.surface.motion2;
1512 }
1513 // set LCP bounds and friction index. this depends on the approximation
1514 // mode
1515 if (j->contact.surface.mode & dContactMu2) {
1516 info->lo[2] = -j->contact.surface.mu2;
1517 info->hi[2] = j->contact.surface.mu2;
1518 }
1519 else {
1520 info->lo[2] = -j->contact.surface.mu;
1521 info->hi[2] = j->contact.surface.mu;
1522 }
1523 if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0;
1524
1525 // set slip (constraint force mixing)
1526 if (j->contact.surface.mode & dContactSlip2)
1527 info->cfm[2] = j->contact.surface.slip2;
1528 }
1529}
1530
1531
1532dxJoint::Vtable __dcontact_vtable = {
1533 sizeof(dxJointContact),
1534 (dxJoint::init_fn*) contactInit,
1535 (dxJoint::getInfo1_fn*) contactGetInfo1,
1536 (dxJoint::getInfo2_fn*) contactGetInfo2,
1537 dJointTypeContact};
1538
1539//****************************************************************************
1540// hinge 2. note that this joint must be attached to two bodies for it to work
1541
1542static dReal measureHinge2Angle (dxJointHinge2 *joint)
1543{
1544 dVector3 a1,a2;
1545 dMULTIPLY0_331 (a1,joint->node[1].body->posr.R,joint->axis2);
1546 dMULTIPLY1_331 (a2,joint->node[0].body->posr.R,a1);
1547 dReal x = dDOT(joint->v1,a2);
1548 dReal y = dDOT(joint->v2,a2);
1549 return -dAtan2 (y,x);
1550}
1551
1552
1553static void hinge2Init (dxJointHinge2 *j)
1554{
1555 dSetZero (j->anchor1,4);
1556 dSetZero (j->anchor2,4);
1557 dSetZero (j->axis1,4);
1558 j->axis1[0] = 1;
1559 dSetZero (j->axis2,4);
1560 j->axis2[1] = 1;
1561 j->c0 = 0;
1562 j->s0 = 0;
1563
1564 dSetZero (j->v1,4);
1565 j->v1[0] = 1;
1566 dSetZero (j->v2,4);
1567 j->v2[1] = 1;
1568
1569 j->limot1.init (j->world);
1570 j->limot2.init (j->world);
1571
1572 j->susp_erp = j->world->global_erp;
1573 j->susp_cfm = j->world->global_cfm;
1574
1575 j->flags |= dJOINT_TWOBODIES;
1576}
1577
1578
1579static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info)
1580{
1581 info->m = 4;
1582 info->nub = 4;
1583
1584 // see if we're powered or at a joint limit for axis 1
1585 int atlimit=0;
1586 if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
1587 j->limot1.lostop <= j->limot1.histop) {
1588 dReal angle = measureHinge2Angle (j);
1589 if (j->limot1.testRotationalLimit (angle)) atlimit = 1;
1590 }
1591 if (atlimit || j->limot1.fmax > 0) info->m++;
1592
1593 // see if we're powering axis 2 (we currently never limit this axis)
1594 j->limot2.limit = 0;
1595 if (j->limot2.fmax > 0) info->m++;
1596}
1597
1598
1599// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
1600// relative to body 1 and 2 initially) and then computes the constrained
1601// rotational axis as the cross product of ax1 and ax2.
1602// the sin and cos of the angle between axis 1 and 2 is computed, this comes
1603// from dot and cross product rules.
1604
1605#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \
1606 dVector3 ax1,ax2; \
1607 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); \
1608 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); \
1609 dCROSS (axis,=,ax1,ax2); \
1610 sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \
1611 cos_angle = dDOT (ax1,ax2);
1612
1613
1614static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info)
1615{
1616 // get information we need to set the hinge row
1617 dReal s,c;
1618 dVector3 q;
1619 HINGE2_GET_AXIS_INFO (q,s,c);
1620 dNormalize3 (q); // @@@ quicker: divide q by s ?
1621
1622 // set the three ball-and-socket rows (aligned to the suspension axis ax1)
1623 setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp);
1624
1625 // set the hinge row
1626 int s3=3*info->rowskip;
1627 info->J1a[s3+0] = q[0];
1628 info->J1a[s3+1] = q[1];
1629 info->J1a[s3+2] = q[2];
1630 if (joint->node[1].body) {
1631 info->J2a[s3+0] = -q[0];
1632 info->J2a[s3+1] = -q[1];
1633 info->J2a[s3+2] = -q[2];
1634 }
1635
1636 // compute the right hand side for the constrained rotational DOF.
1637 // axis 1 and axis 2 are separated by an angle `theta'. the desired
1638 // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
1639 // in the joint structure. the correcting angular velocity is:
1640 // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
1641 // = (erp*fps) * (theta0-theta)
1642 // (theta0-theta) can be computed using the following small-angle-difference
1643 // approximation:
1644 // theta0-theta ~= tan(theta0-theta)
1645 // = sin(theta0-theta)/cos(theta0-theta)
1646 // = (c*s0 - s*c0) / (c*c0 + s*s0)
1647 // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1
1648 // where c = cos(theta), s = sin(theta)
1649 // c0 = cos(theta0), s0 = sin(theta0)
1650
1651 dReal k = info->fps * info->erp;
1652 info->c[3] = k * (joint->c0 * s - joint->s0 * c);
1653
1654 // if the axis1 hinge is powered, or has joint limits, add in more stuff
1655 int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
1656
1657 // if the axis2 hinge is powered, add in more stuff
1658 joint->limot2.addLimot (joint,info,row,ax2,1);
1659
1660 // set parameter for the suspension
1661 info->cfm[0] = joint->susp_cfm;
1662}
1663
1664
1665// compute vectors v1 and v2 (embedded in body1), used to measure angle
1666// between body 1 and body 2
1667
1668static void makeHinge2V1andV2 (dxJointHinge2 *joint)
1669{
1670 if (joint->node[0].body) {
1671 // get axis 1 and 2 in global coords
1672 dVector3 ax1,ax2,v;
1673 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1);
1674 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2);
1675
1676 // don't do anything if the axis1 or axis2 vectors are zero or the same
1677 if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) ||
1678 (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) ||
1679 (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return;
1680
1681 // modify axis 2 so it's perpendicular to axis 1
1682 dReal k = dDOT(ax1,ax2);
1683 for (int i=0; i<3; i++) ax2[i] -= k*ax1[i];
1684 dNormalize3 (ax2);
1685
1686 // make v1 = modified axis2, v2 = axis1 x (modified axis2)
1687 dCROSS (v,=,ax1,ax2);
1688 dMULTIPLY1_331 (joint->v1,joint->node[0].body->posr.R,ax2);
1689 dMULTIPLY1_331 (joint->v2,joint->node[0].body->posr.R,v);
1690 }
1691}
1692
1693
1694void dJointSetHinge2Anchor (dJointID j, dReal x, dReal y, dReal z)
1695{
1696 dxJointHinge2* joint = (dxJointHinge2*)j;
1697 dUASSERT(joint,"bad joint argument");
1698 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1699 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
1700 makeHinge2V1andV2 (joint);
1701}
1702
1703
1704void dJointSetHinge2Axis1 (dJointID j, dReal x, dReal y, dReal z)
1705{
1706 dxJointHinge2* joint = (dxJointHinge2*)j;
1707 dUASSERT(joint,"bad joint argument");
1708 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1709 if (joint->node[0].body) {
1710 dReal q[4];
1711 q[0] = x;
1712 q[1] = y;
1713 q[2] = z;
1714 q[3] = 0;
1715 dNormalize3 (q);
1716 dMULTIPLY1_331 (joint->axis1,joint->node[0].body->posr.R,q);
1717 joint->axis1[3] = 0;
1718
1719 // compute the sin and cos of the angle between axis 1 and axis 2
1720 dVector3 ax;
1721 HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
1722 }
1723 makeHinge2V1andV2 (joint);
1724}
1725
1726
1727void dJointSetHinge2Axis2 (dJointID j, dReal x, dReal y, dReal z)
1728{
1729 dxJointHinge2* joint = (dxJointHinge2*)j;
1730 dUASSERT(joint,"bad joint argument");
1731 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1732 if (joint->node[1].body) {
1733 dReal q[4];
1734 q[0] = x;
1735 q[1] = y;
1736 q[2] = z;
1737 q[3] = 0;
1738 dNormalize3 (q);
1739 dMULTIPLY1_331 (joint->axis2,joint->node[1].body->posr.R,q);
1740 joint->axis1[3] = 0;
1741
1742 // compute the sin and cos of the angle between axis 1 and axis 2
1743 dVector3 ax;
1744 HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
1745 }
1746 makeHinge2V1andV2 (joint);
1747}
1748
1749
1750void dJointSetHinge2Param (dJointID j, int parameter, dReal value)
1751{
1752 dxJointHinge2* joint = (dxJointHinge2*)j;
1753 dUASSERT(joint,"bad joint argument");
1754 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1755 if ((parameter & 0xff00) == 0x100) {
1756 joint->limot2.set (parameter & 0xff,value);
1757 }
1758 else {
1759 if (parameter == dParamSuspensionERP) joint->susp_erp = value;
1760 else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value;
1761 else joint->limot1.set (parameter,value);
1762 }
1763}
1764
1765
1766void dJointGetHinge2Anchor (dJointID j, dVector3 result)
1767{
1768 dxJointHinge2* joint = (dxJointHinge2*)j;
1769 dUASSERT(joint,"bad joint argument");
1770 dUASSERT(result,"bad result argument");
1771 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1772 if (joint->flags & dJOINT_REVERSE)
1773 getAnchor2 (joint,result,joint->anchor2);
1774 else
1775 getAnchor (joint,result,joint->anchor1);
1776}
1777
1778
1779void dJointGetHinge2Anchor2 (dJointID j, dVector3 result)
1780{
1781 dxJointHinge2* joint = (dxJointHinge2*)j;
1782 dUASSERT(joint,"bad joint argument");
1783 dUASSERT(result,"bad result argument");
1784 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1785 if (joint->flags & dJOINT_REVERSE)
1786 getAnchor (joint,result,joint->anchor1);
1787 else
1788 getAnchor2 (joint,result,joint->anchor2);
1789}
1790
1791
1792void dJointGetHinge2Axis1 (dJointID j, dVector3 result)
1793{
1794 dxJointHinge2* joint = (dxJointHinge2*)j;
1795 dUASSERT(joint,"bad joint argument");
1796 dUASSERT(result,"bad result argument");
1797 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1798 if (joint->node[0].body) {
1799 dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis1);
1800 }
1801}
1802
1803
1804void dJointGetHinge2Axis2 (dJointID j, dVector3 result)
1805{
1806 dxJointHinge2* joint = (dxJointHinge2*)j;
1807 dUASSERT(joint,"bad joint argument");
1808 dUASSERT(result,"bad result argument");
1809 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1810 if (joint->node[1].body) {
1811 dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis2);
1812 }
1813}
1814
1815
1816dReal dJointGetHinge2Param (dJointID j, int parameter)
1817{
1818 dxJointHinge2* joint = (dxJointHinge2*)j;
1819 dUASSERT(joint,"bad joint argument");
1820 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1821 if ((parameter & 0xff00) == 0x100) {
1822 return joint->limot2.get (parameter & 0xff);
1823 }
1824 else {
1825 if (parameter == dParamSuspensionERP) return joint->susp_erp;
1826 else if (parameter == dParamSuspensionCFM) return joint->susp_cfm;
1827 else return joint->limot1.get (parameter);
1828 }
1829}
1830
1831
1832dReal dJointGetHinge2Angle1 (dJointID j)
1833{
1834 dxJointHinge2* joint = (dxJointHinge2*)j;
1835 dUASSERT(joint,"bad joint argument");
1836 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1837 if (joint->node[0].body) return measureHinge2Angle (joint);
1838 else return 0;
1839}
1840
1841
1842dReal dJointGetHinge2Angle1Rate (dJointID j)
1843{
1844 dxJointHinge2* joint = (dxJointHinge2*)j;
1845 dUASSERT(joint,"bad joint argument");
1846 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1847 if (joint->node[0].body) {
1848 dVector3 axis;
1849 dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1);
1850 dReal rate = dDOT(axis,joint->node[0].body->avel);
1851 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
1852 return rate;
1853 }
1854 else return 0;
1855}
1856
1857
1858dReal dJointGetHinge2Angle2Rate (dJointID j)
1859{
1860 dxJointHinge2* joint = (dxJointHinge2*)j;
1861 dUASSERT(joint,"bad joint argument");
1862 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1863 if (joint->node[0].body && joint->node[1].body) {
1864 dVector3 axis;
1865 dMULTIPLY0_331 (axis,joint->node[1].body->posr.R,joint->axis2);
1866 dReal rate = dDOT(axis,joint->node[0].body->avel);
1867 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
1868 return rate;
1869 }
1870 else return 0;
1871}
1872
1873
1874void dJointAddHinge2Torques (dJointID j, dReal torque1, dReal torque2)
1875{
1876 dxJointHinge2* joint = (dxJointHinge2*)j;
1877 dVector3 axis1, axis2;
1878 dUASSERT(joint,"bad joint argument");
1879 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1880
1881 if (joint->node[0].body && joint->node[1].body) {
1882 dMULTIPLY0_331 (axis1,joint->node[0].body->posr.R,joint->axis1);
1883 dMULTIPLY0_331 (axis2,joint->node[1].body->posr.R,joint->axis2);
1884 axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
1885 axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
1886 axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
1887 dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]);
1888 dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]);
1889 }
1890}
1891
1892
1893dxJoint::Vtable __dhinge2_vtable = {
1894 sizeof(dxJointHinge2),
1895 (dxJoint::init_fn*) hinge2Init,
1896 (dxJoint::getInfo1_fn*) hinge2GetInfo1,
1897 (dxJoint::getInfo2_fn*) hinge2GetInfo2,
1898 dJointTypeHinge2};
1899
1900//****************************************************************************
1901// universal
1902
1903// I just realized that the universal joint is equivalent to a hinge 2 joint with
1904// perfectly stiff suspension. By comparing the hinge 2 implementation to
1905// the universal implementation, you may be able to improve this
1906// implementation (or, less likely, the hinge2 implementation).
1907
1908static void universalInit (dxJointUniversal *j)
1909{
1910 dSetZero (j->anchor1,4);
1911 dSetZero (j->anchor2,4);
1912 dSetZero (j->axis1,4);
1913 j->axis1[0] = 1;
1914 dSetZero (j->axis2,4);
1915 j->axis2[1] = 1;
1916 dSetZero(j->qrel1,4);
1917 dSetZero(j->qrel2,4);
1918 j->limot1.init (j->world);
1919 j->limot2.init (j->world);
1920}
1921
1922
1923static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2)
1924{
1925 // This says "ax1 = joint->node[0].body->posr.R * joint->axis1"
1926 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1);
1927
1928 if (joint->node[1].body) {
1929 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2);
1930 }
1931 else {
1932 ax2[0] = joint->axis2[0];
1933 ax2[1] = joint->axis2[1];
1934 ax2[2] = joint->axis2[2];
1935 }
1936}
1937
1938static void getUniversalAngles(dxJointUniversal *joint, dReal *angle1, dReal *angle2)
1939{
1940 if (joint->node[0].body)
1941 {
1942 // length 1 joint axis in global coordinates, from each body
1943 dVector3 ax1, ax2;
1944 dMatrix3 R;
1945 dQuaternion qcross, qq, qrel;
1946
1947 getUniversalAxes (joint,ax1,ax2);
1948
1949 // It should be possible to get both angles without explicitly
1950 // constructing the rotation matrix of the cross. Basically,
1951 // orientation of the cross about axis1 comes from body 2,
1952 // about axis 2 comes from body 1, and the perpendicular
1953 // axis can come from the two bodies somehow. (We don't really
1954 // want to assume it's 90 degrees, because in general the
1955 // constraints won't be perfectly satisfied, or even very well
1956 // satisfied.)
1957 //
1958 // However, we'd need a version of getHingeAngleFromRElativeQuat()
1959 // that CAN handle when its relative quat is rotated along a direction
1960 // other than the given axis. What I have here works,
1961 // although it's probably much slower than need be.
1962
1963 dRFrom2Axes (R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
1964
1965 dRtoQ (R, qcross);
1966
1967
1968 // This code is essentialy the same as getHingeAngle(), see the comments
1969 // there for details.
1970
1971 // get qrel = relative rotation between node[0] and the cross
1972 dQMultiply1 (qq, joint->node[0].body->q, qcross);
1973 dQMultiply2 (qrel, qq, joint->qrel1);
1974
1975 *angle1 = getHingeAngleFromRelativeQuat(qrel, joint->axis1);
1976
1977 // This is equivalent to
1978 // dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
1979 // You see that the R is constructed from the same 2 axis as for angle1
1980 // but the first and second axis are swapped.
1981 // So we can take the first R and rapply a rotation to it.
1982 // The rotation is around the axis between the 2 axes (ax1 and ax2).
1983 // We do a rotation of 180deg.
1984
1985 dQuaternion qcross2;
1986 // Find the vector between ax1 and ax2 (i.e. in the middle)
1987 // We need to turn around this vector by 180deg
1988
1989 // The 2 axes should be normalize so to find the vector between the 2.
1990 // Add and devide by 2 then normalize or simply normalize
1991 // ax2
1992 // ^
1993 // |
1994 // |
1995 /// *------------> ax1
1996 // We want the vector a 45deg
1997 //
1998 // N.B. We don't need to normalize the ax1 and ax2 since there are
1999 // normalized when we set them.
2000
2001 // We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z]
2002 qrel[0] = 0; // equivalent to cos(Pi/2)
2003 qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1
2004 qrel[2] = ax1[1] + ax2[1];
2005 qrel[3] = ax1[2] + ax2[2];
2006
2007 dReal l = dRecip(sqrt(qrel[1]*qrel[1] + qrel[2]*qrel[2] + qrel[3]*qrel[3]));
2008 qrel[1] *= l;
2009 qrel[2] *= l;
2010 qrel[3] *= l;
2011
2012 dQMultiply0 (qcross2, qrel, qcross);
2013
2014 if (joint->node[1].body) {
2015 dQMultiply1 (qq, joint->node[1].body->q, qcross2);
2016 dQMultiply2 (qrel, qq, joint->qrel2);
2017 }
2018 else {
2019 // pretend joint->node[1].body->q is the identity
2020 dQMultiply2 (qrel, qcross2, joint->qrel2);
2021 }
2022
2023 *angle2 = - getHingeAngleFromRelativeQuat(qrel, joint->axis2);
2024
2025 }
2026 else
2027 {
2028 *angle1 = 0;
2029 *angle2 = 0;
2030 }
2031}
2032
2033static dReal getUniversalAngle1(dxJointUniversal *joint)
2034{
2035 if (joint->node[0].body) {
2036 // length 1 joint axis in global coordinates, from each body
2037 dVector3 ax1, ax2;
2038 dMatrix3 R;
2039 dQuaternion qcross, qq, qrel;
2040
2041 getUniversalAxes (joint,ax1,ax2);
2042
2043 // It should be possible to get both angles without explicitly
2044 // constructing the rotation matrix of the cross. Basically,
2045 // orientation of the cross about axis1 comes from body 2,
2046 // about axis 2 comes from body 1, and the perpendicular
2047 // axis can come from the two bodies somehow. (We don't really
2048 // want to assume it's 90 degrees, because in general the
2049 // constraints won't be perfectly satisfied, or even very well
2050 // satisfied.)
2051 //
2052 // However, we'd need a version of getHingeAngleFromRElativeQuat()
2053 // that CAN handle when its relative quat is rotated along a direction
2054 // other than the given axis. What I have here works,
2055 // although it's probably much slower than need be.
2056
2057 dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
2058 dRtoQ (R,qcross);
2059
2060 // This code is essential the same as getHingeAngle(), see the comments
2061 // there for details.
2062
2063 // get qrel = relative rotation between node[0] and the cross
2064 dQMultiply1 (qq,joint->node[0].body->q,qcross);
2065 dQMultiply2 (qrel,qq,joint->qrel1);
2066
2067 return getHingeAngleFromRelativeQuat(qrel, joint->axis1);
2068 }
2069 return 0;
2070}
2071
2072
2073static dReal getUniversalAngle2(dxJointUniversal *joint)
2074{
2075 if (joint->node[0].body) {
2076 // length 1 joint axis in global coordinates, from each body
2077 dVector3 ax1, ax2;
2078 dMatrix3 R;
2079 dQuaternion qcross, qq, qrel;
2080
2081 getUniversalAxes (joint,ax1,ax2);
2082
2083 // It should be possible to get both angles without explicitly
2084 // constructing the rotation matrix of the cross. Basically,
2085 // orientation of the cross about axis1 comes from body 2,
2086 // about axis 2 comes from body 1, and the perpendicular
2087 // axis can come from the two bodies somehow. (We don't really
2088 // want to assume it's 90 degrees, because in general the
2089 // constraints won't be perfectly satisfied, or even very well
2090 // satisfied.)
2091 //
2092 // However, we'd need a version of getHingeAngleFromRElativeQuat()
2093 // that CAN handle when its relative quat is rotated along a direction
2094 // other than the given axis. What I have here works,
2095 // although it's probably much slower than need be.
2096
2097 dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
2098 dRtoQ(R, qcross);
2099
2100 if (joint->node[1].body) {
2101 dQMultiply1 (qq, joint->node[1].body->q, qcross);
2102 dQMultiply2 (qrel,qq,joint->qrel2);
2103 }
2104 else {
2105 // pretend joint->node[1].body->q is the identity
2106 dQMultiply2 (qrel,qcross, joint->qrel2);
2107 }
2108
2109 return - getHingeAngleFromRelativeQuat(qrel, joint->axis2);
2110 }
2111 return 0;
2112}
2113
2114
2115static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info)
2116{
2117 info->nub = 4;
2118 info->m = 4;
2119
2120 // see if we're powered or at a joint limit.
2121 bool constraint1 = j->limot1.fmax > 0;
2122 bool constraint2 = j->limot2.fmax > 0;
2123
2124 bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
2125 j->limot1.lostop <= j->limot1.histop;
2126 bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) &&
2127 j->limot2.lostop <= j->limot2.histop;
2128
2129 // We need to call testRotationLimit() even if we're motored, since it
2130 // records the result.
2131 if (limiting1 || limiting2) {
2132 dReal angle1, angle2;
2133 getUniversalAngles (j, &angle1, &angle2);
2134 if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true;
2135 if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true;
2136 }
2137 if (constraint1)
2138 info->m++;
2139 if (constraint2)
2140 info->m++;
2141}
2142
2143
2144static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info)
2145{
2146 // set the three ball-and-socket rows
2147 setBall (joint,info,joint->anchor1,joint->anchor2);
2148
2149 // set the universal joint row. the angular velocity about an axis
2150 // perpendicular to both joint axes should be equal. thus the constraint
2151 // equation is
2152 // p*w1 - p*w2 = 0
2153 // where p is a vector normal to both joint axes, and w1 and w2
2154 // are the angular velocity vectors of the two bodies.
2155
2156 // length 1 joint axis in global coordinates, from each body
2157 dVector3 ax1, ax2;
2158 dVector3 ax2_temp;
2159 // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
2160 // about this.
2161 dVector3 p;
2162 dReal k;
2163
2164 getUniversalAxes(joint, ax1, ax2);
2165 k = dDOT(ax1, ax2);
2166 ax2_temp[0] = ax2[0] - k*ax1[0];
2167 ax2_temp[1] = ax2[1] - k*ax1[1];
2168 ax2_temp[2] = ax2[2] - k*ax1[2];
2169 dCROSS(p, =, ax1, ax2_temp);
2170 dNormalize3(p);
2171
2172 int s3=3*info->rowskip;
2173
2174 info->J1a[s3+0] = p[0];
2175 info->J1a[s3+1] = p[1];
2176 info->J1a[s3+2] = p[2];
2177
2178 if (joint->node[1].body) {
2179 info->J2a[s3+0] = -p[0];
2180 info->J2a[s3+1] = -p[1];
2181 info->J2a[s3+2] = -p[2];
2182 }
2183
2184 // compute the right hand side of the constraint equation. set relative
2185 // body velocities along p to bring the axes back to perpendicular.
2186 // If ax1, ax2 are unit length joint axes as computed from body1 and
2187 // body2, we need to rotate both bodies along the axis p. If theta
2188 // is the angle between ax1 and ax2, we need an angular velocity
2189 // along p to cover the angle erp * (theta - Pi/2) in one step:
2190 //
2191 // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
2192 // = (erp*fps) * (theta - Pi/2)
2193 //
2194 // if theta is close to Pi/2,
2195 // theta - Pi/2 ~= cos(theta), so
2196 // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
2197
2198 info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2);
2199
2200 // if the first angle is powered, or has joint limits, add in the stuff
2201 int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
2202
2203 // if the second angle is powered, or has joint limits, add in more stuff
2204 joint->limot2.addLimot (joint,info,row,ax2,1);
2205}
2206
2207
2208static void universalComputeInitialRelativeRotations (dxJointUniversal *joint)
2209{
2210 if (joint->node[0].body) {
2211 dVector3 ax1, ax2;
2212 dMatrix3 R;
2213 dQuaternion qcross;
2214
2215 getUniversalAxes(joint, ax1, ax2);
2216
2217 // Axis 1.
2218 dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
2219 dRtoQ(R, qcross);
2220 dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross);
2221
2222 // Axis 2.
2223 dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
2224 dRtoQ(R, qcross);
2225 if (joint->node[1].body) {
2226 dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross);
2227 }
2228 else {
2229 // set joint->qrel to qcross
2230 for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i];
2231 }
2232 }
2233}
2234
2235
2236void dJointSetUniversalAnchor (dJointID j, dReal x, dReal y, dReal z)
2237{
2238 dxJointUniversal* joint = (dxJointUniversal*)j;
2239 dUASSERT(joint,"bad joint argument");
2240 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2241 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
2242 universalComputeInitialRelativeRotations(joint);
2243}
2244
2245
2246void dJointSetUniversalAxis1 (dJointID j, dReal x, dReal y, dReal z)
2247{
2248 dxJointUniversal* joint = (dxJointUniversal*)j;
2249 dUASSERT(joint,"bad joint argument");
2250 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2251 if (joint->flags & dJOINT_REVERSE)
2252 setAxes (joint,x,y,z,NULL,joint->axis2);
2253 else
2254 setAxes (joint,x,y,z,joint->axis1,NULL);
2255 universalComputeInitialRelativeRotations(joint);
2256}
2257
2258
2259void dJointSetUniversalAxis2 (dJointID j, dReal x, dReal y, dReal z)
2260{
2261 dxJointUniversal* joint = (dxJointUniversal*)j;
2262 dUASSERT(joint,"bad joint argument");
2263 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2264 if (joint->flags & dJOINT_REVERSE)
2265 setAxes (joint,x,y,z,joint->axis1,NULL);
2266 else
2267 setAxes (joint,x,y,z,NULL,joint->axis2);
2268 universalComputeInitialRelativeRotations(joint);
2269}
2270
2271
2272void dJointGetUniversalAnchor (dJointID j, dVector3 result)
2273{
2274 dxJointUniversal* joint = (dxJointUniversal*)j;
2275 dUASSERT(joint,"bad joint argument");
2276 dUASSERT(result,"bad result argument");
2277 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2278 if (joint->flags & dJOINT_REVERSE)
2279 getAnchor2 (joint,result,joint->anchor2);
2280 else
2281 getAnchor (joint,result,joint->anchor1);
2282}
2283
2284
2285void dJointGetUniversalAnchor2 (dJointID j, dVector3 result)
2286{
2287 dxJointUniversal* joint = (dxJointUniversal*)j;
2288 dUASSERT(joint,"bad joint argument");
2289 dUASSERT(result,"bad result argument");
2290 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2291 if (joint->flags & dJOINT_REVERSE)
2292 getAnchor (joint,result,joint->anchor1);
2293 else
2294 getAnchor2 (joint,result,joint->anchor2);
2295}
2296
2297
2298void dJointGetUniversalAxis1 (dJointID j, dVector3 result)
2299{
2300 dxJointUniversal* joint = (dxJointUniversal*)j;
2301 dUASSERT(joint,"bad joint argument");
2302 dUASSERT(result,"bad result argument");
2303 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2304 if (joint->flags & dJOINT_REVERSE)
2305 getAxis2 (joint,result,joint->axis2);
2306 else
2307 getAxis (joint,result,joint->axis1);
2308}
2309
2310
2311void dJointGetUniversalAxis2 (dJointID j, dVector3 result)
2312{
2313 dxJointUniversal* joint = (dxJointUniversal*)j;
2314 dUASSERT(joint,"bad joint argument");
2315 dUASSERT(result,"bad result argument");
2316 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2317 if (joint->flags & dJOINT_REVERSE)
2318 getAxis (joint,result,joint->axis1);
2319 else
2320 getAxis2 (joint,result,joint->axis2);
2321}
2322
2323
2324void dJointSetUniversalParam (dJointID j, int parameter, dReal value)
2325{
2326 dxJointUniversal* joint = (dxJointUniversal*)j;
2327 dUASSERT(joint,"bad joint argument");
2328 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2329 if ((parameter & 0xff00) == 0x100) {
2330 joint->limot2.set (parameter & 0xff,value);
2331 }
2332 else {
2333 joint->limot1.set (parameter,value);
2334 }
2335}
2336
2337
2338dReal dJointGetUniversalParam (dJointID j, int parameter)
2339{
2340 dxJointUniversal* joint = (dxJointUniversal*)j;
2341 dUASSERT(joint,"bad joint argument");
2342 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2343 if ((parameter & 0xff00) == 0x100) {
2344 return joint->limot2.get (parameter & 0xff);
2345 }
2346 else {
2347 return joint->limot1.get (parameter);
2348 }
2349}
2350
2351void dJointGetUniversalAngles (dJointID j, dReal *angle1, dReal *angle2)
2352{
2353 dxJointUniversal* joint = (dxJointUniversal*)j;
2354 dUASSERT(joint,"bad joint argument");
2355 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2356 if (joint->flags & dJOINT_REVERSE)
2357 return getUniversalAngles (joint, angle2, angle1);
2358 else
2359 return getUniversalAngles (joint, angle1, angle2);
2360}
2361
2362
2363dReal dJointGetUniversalAngle1 (dJointID j)
2364{
2365 dxJointUniversal* joint = (dxJointUniversal*)j;
2366 dUASSERT(joint,"bad joint argument");
2367 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2368 if (joint->flags & dJOINT_REVERSE)
2369 return getUniversalAngle2 (joint);
2370 else
2371 return getUniversalAngle1 (joint);
2372}
2373
2374
2375dReal dJointGetUniversalAngle2 (dJointID j)
2376{
2377 dxJointUniversal* joint = (dxJointUniversal*)j;
2378 dUASSERT(joint,"bad joint argument");
2379 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2380 if (joint->flags & dJOINT_REVERSE)
2381 return getUniversalAngle1 (joint);
2382 else
2383 return getUniversalAngle2 (joint);
2384}
2385
2386
2387dReal dJointGetUniversalAngle1Rate (dJointID j)
2388{
2389 dxJointUniversal* joint = (dxJointUniversal*)j;
2390 dUASSERT(joint,"bad joint argument");
2391 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2392
2393 if (joint->node[0].body) {
2394 dVector3 axis;
2395
2396 if (joint->flags & dJOINT_REVERSE)
2397 getAxis2 (joint,axis,joint->axis2);
2398 else
2399 getAxis (joint,axis,joint->axis1);
2400
2401 dReal rate = dDOT(axis, joint->node[0].body->avel);
2402 if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel);
2403 return rate;
2404 }
2405 return 0;
2406}
2407
2408
2409dReal dJointGetUniversalAngle2Rate (dJointID j)
2410{
2411 dxJointUniversal* joint = (dxJointUniversal*)j;
2412 dUASSERT(joint,"bad joint argument");
2413 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2414
2415 if (joint->node[0].body) {
2416 dVector3 axis;
2417
2418 if (joint->flags & dJOINT_REVERSE)
2419 getAxis (joint,axis,joint->axis1);
2420 else
2421 getAxis2 (joint,axis,joint->axis2);
2422
2423 dReal rate = dDOT(axis, joint->node[0].body->avel);
2424 if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel);
2425 return rate;
2426 }
2427 return 0;
2428}
2429
2430
2431void dJointAddUniversalTorques (dJointID j, dReal torque1, dReal torque2)
2432{
2433 dxJointUniversal* joint = (dxJointUniversal*)j;
2434 dVector3 axis1, axis2;
2435 dAASSERT(joint);
2436 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2437
2438 if (joint->flags & dJOINT_REVERSE) {
2439 dReal temp = torque1;
2440 torque1 = - torque2;
2441 torque2 = - temp;
2442 }
2443
2444 getAxis (joint,axis1,joint->axis1);
2445 getAxis2 (joint,axis2,joint->axis2);
2446 axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
2447 axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
2448 axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
2449
2450 if (joint->node[0].body != 0)
2451 dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]);
2452 if (joint->node[1].body != 0)
2453 dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]);
2454}
2455
2456
2457
2458
2459
2460dxJoint::Vtable __duniversal_vtable = {
2461 sizeof(dxJointUniversal),
2462 (dxJoint::init_fn*) universalInit,
2463 (dxJoint::getInfo1_fn*) universalGetInfo1,
2464 (dxJoint::getInfo2_fn*) universalGetInfo2,
2465 dJointTypeUniversal};
2466
2467
2468
2469//****************************************************************************
2470// Prismatic and Rotoide
2471
2472static void PRInit (dxJointPR *j)
2473{
2474 // Default Position
2475 // Z^
2476 // | Body 1 P R Body2
2477 // |+---------+ _ _ +-----------+
2478 // || |----|----(_)--------+ |
2479 // |+---------+ - +-----------+
2480 // |
2481 // X.-----------------------------------------> Y
2482 // N.B. X is comming out of the page
2483 dSetZero (j->anchor2,4);
2484
2485 dSetZero (j->axisR1,4);
2486 j->axisR1[0] = 1;
2487 dSetZero (j->axisR2,4);
2488 j->axisR2[0] = 1;
2489
2490 dSetZero (j->axisP1,4);
2491 j->axisP1[1] = 1;
2492 dSetZero (j->qrel,4);
2493 dSetZero (j->offset,4);
2494
2495 j->limotR.init (j->world);
2496 j->limotP.init (j->world);
2497}
2498
2499
2500dReal dJointGetPRPosition (dJointID j)
2501{
2502 dxJointPR* joint = (dxJointPR*)j;
2503 dUASSERT(joint,"bad joint argument");
2504 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
2505
2506 dVector3 q;
2507 // get the offset in global coordinates
2508 dMULTIPLY0_331 (q,joint->node[0].body->posr.R,joint->offset);
2509
2510 if (joint->node[1].body) {
2511 dVector3 anchor2;
2512
2513 // get the anchor2 in global coordinates
2514 dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2);
2515
2516 q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) -
2517 (joint->node[1].body->posr.pos[0] + anchor2[0]) );
2518 q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) -
2519 (joint->node[1].body->posr.pos[1] + anchor2[1]) );
2520 q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) -
2521 (joint->node[1].body->posr.pos[2] + anchor2[2]) );
2522
2523 }
2524 else {
2525 //N.B. When there is no body 2 the joint->anchor2 is already in
2526 // global coordinates
2527
2528 q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) -
2529 (joint->anchor2[0]) );
2530 q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) -
2531 (joint->anchor2[1]) );
2532 q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) -
2533 (joint->anchor2[2]) );
2534
2535 }
2536
2537 dVector3 axP;
2538 // get prismatic axis in global coordinates
2539 dMULTIPLY0_331 (axP,joint->node[0].body->posr.R,joint->axisP1);
2540
2541 return dDOT(axP, q);
2542}
2543
2544
2545dReal dJointGetPRPositionRate (dJointID j)
2546{
2547 dxJointPR* joint = (dxJointPR*)j;
2548 dUASSERT(joint,"bad joint argument");
2549 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
2550
2551 if (joint->node[0].body) {
2552 // We want to find the rate of change of the prismatic part of the joint
2553 // We can find it by looking at the speed difference between body1 and the
2554 // anchor point.
2555
2556 // r will be used to find the distance between body1 and the anchor point
2557 dVector3 r;
2558 if (joint->node[1].body) {
2559 // Find joint->anchor2 in global coordinates
2560 dVector3 anchor2;
2561 dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2);
2562
2563 r[0] = joint->node[0].body->posr.pos[0] - anchor2[0];
2564 r[1] = joint->node[0].body->posr.pos[1] - anchor2[1];
2565 r[2] = joint->node[0].body->posr.pos[2] - anchor2[2];
2566 }
2567 else {
2568 //N.B. When there is no body 2 the joint->anchor2 is already in
2569 // global coordinates
2570 r[0] = joint->node[0].body->posr.pos[0] - joint->anchor2[0];
2571 r[1] = joint->node[0].body->posr.pos[1] - joint->anchor2[1];
2572 r[2] = joint->node[0].body->posr.pos[2] - joint->anchor2[2];
2573 }
2574
2575 // The body1 can have velocity coming from the rotation of
2576 // the rotoide axis. We need to remove this.
2577
2578 // Take only the angular rotation coming from the rotation
2579 // of the rotoide articulation
2580 // N.B. Body1 and Body2 should have the same rotation along axis
2581 // other than the rotoide axis.
2582 dVector3 angular;
2583 dMULTIPLY0_331 (angular,joint->node[0].body->posr.R,joint->axisR1);
2584 dReal omega = dDOT(angular, joint->node[0].body->avel);
2585 angular[0] *= omega;
2586 angular[1] *= omega;
2587 angular[2] *= omega;
2588
2589 // Find the contribution of the angular rotation to the linear speed
2590 // N.B. We do vel = r X w instead of vel = w x r to have vel negative
2591 // since we want to remove it from the linear velocity of the body
2592 dVector3 lvel1;
2593 dCROSS(lvel1, =, r, angular);
2594
2595 lvel1[0] += joint->node[0].body->lvel[0];
2596 lvel1[1] += joint->node[0].body->lvel[1];
2597 lvel1[2] += joint->node[0].body->lvel[2];
2598
2599 // Since we want rate of change along the prismatic axis
2600 // get axisP1 in global coordinates and get the component
2601 // along this axis only
2602 dVector3 axP1;
2603 dMULTIPLY0_331 (axP1,joint->node[0].body->posr.R,joint->axisP1);
2604 return dDOT(axP1, lvel1);
2605 }
2606
2607 return 0.0;
2608}
2609
2610
2611
2612static void PRGetInfo1 (dxJointPR *j, dxJoint::Info1 *info)
2613{
2614 info->m = 4;
2615 info->nub = 4;
2616
2617 bool added = false;
2618
2619 added = false;
2620 // see if the prismatic articulation is powered
2621 if (j->limotP.fmax > 0)
2622 {
2623 added = true;
2624 (info->m)++; // powered needs an extra constraint row
2625 }
2626
2627 // see if we're at a joint limit.
2628 j->limotP.limit = 0;
2629 if ((j->limotP.lostop > -dInfinity || j->limotP.histop < dInfinity) &&
2630 j->limotP.lostop <= j->limotP.histop) {
2631 // measure joint position
2632 dReal pos = dJointGetPRPosition (j);
2633 if (pos <= j->limotP.lostop) {
2634 j->limotP.limit = 1;
2635 j->limotP.limit_err = pos - j->limotP.lostop;
2636 if (!added)
2637 (info->m)++;
2638 }
2639
2640 if (pos >= j->limotP.histop) {
2641 j->limotP.limit = 2;
2642 j->limotP.limit_err = pos - j->limotP.histop;
2643 if (!added)
2644 (info->m)++;
2645 }
2646 }
2647
2648}
2649
2650
2651
2652static void PRGetInfo2 (dxJointPR *joint, dxJoint::Info2 *info)
2653{
2654 int s = info->rowskip;
2655 int s2= 2*s;
2656 int s3= 3*s;
2657 int s4= 4*s;
2658
2659 dReal k = info->fps * info->erp;
2660
2661
2662 dVector3 q; // plane space of axP and after that axR
2663
2664 // pull out pos and R for both bodies. also get the `connection'
2665 // vector pos2-pos1.
2666
2667 dReal *pos1,*pos2,*R1,*R2;
2668 pos1 = joint->node[0].body->posr.pos;
2669 R1 = joint->node[0].body->posr.R;
2670 if (joint->node[1].body) {
2671 pos2 = joint->node[1].body->posr.pos;
2672 R2 = joint->node[1].body->posr.R;
2673 }
2674 else {
2675 // pos2 = 0; // N.B. We can do that to be safe but it is no necessary
2676 // R2 = 0; // N.B. We can do that to be safe but it is no necessary
2677 }
2678
2679
2680 dVector3 axP; // Axis of the prismatic joint in global frame
2681 dMULTIPLY0_331 (axP, R1, joint->axisP1);
2682
2683 // distance between the body1 and the anchor2 in global frame
2684 // Calculated in the same way as the offset
2685 dVector3 dist;
2686
2687 if (joint->node[1].body)
2688 {
2689 dMULTIPLY0_331 (dist, R2, joint->anchor2);
2690 dist[0] += pos2[0] - pos1[0];
2691 dist[1] += pos2[1] - pos1[1];
2692 dist[2] += pos2[2] - pos1[2];
2693 }
2694 else {
2695 dist[0] = joint->anchor2[0] - pos1[0];
2696 dist[1] = joint->anchor2[1] - pos1[1];
2697 dist[2] = joint->anchor2[2] - pos1[2];
2698 }
2699
2700
2701 // ======================================================================
2702 // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered
2703
2704 // Set the two rotoide rows. The rotoide axis should be the only unconstrained
2705 // rotational axis, the angular velocity of the two bodies perpendicular to
2706 // the rotoide axis should be equal. Thus the constraint equations are
2707 // p*w1 - p*w2 = 0
2708 // q*w1 - q*w2 = 0
2709 // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
2710 // are the angular velocity vectors of the two bodies.
2711 dVector3 ax1;
2712 dMULTIPLY0_331 (ax1, joint->node[0].body->posr.R, joint->axisR1);
2713 dCROSS(q , =, ax1, axP);
2714
2715 info->J1a[0] = axP[0];
2716 info->J1a[1] = axP[1];
2717 info->J1a[2] = axP[2];
2718 info->J1a[s+0] = q[0];
2719 info->J1a[s+1] = q[1];
2720 info->J1a[s+2] = q[2];
2721
2722 if (joint->node[1].body) {
2723 info->J2a[0] = -axP[0];
2724 info->J2a[1] = -axP[1];
2725 info->J2a[2] = -axP[2];
2726 info->J2a[s+0] = -q[0];
2727 info->J2a[s+1] = -q[1];
2728 info->J2a[s+2] = -q[2];
2729 }
2730
2731
2732 // Compute the right hand side of the constraint equation set. Relative
2733 // body velocities along p and q to bring the rotoide back into alignment.
2734 // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
2735 // We need to rotate both bodies along the axis u = (ax1 x ax2).
2736 // if `theta' is the angle between ax1 and ax2, we need an angular velocity
2737 // along u to cover angle erp*theta in one step :
2738 // |angular_velocity| = angle/time = erp*theta / stepsize
2739 // = (erp*fps) * theta
2740 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
2741 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
2742 // ...as ax1 and ax2 are unit length. if theta is smallish,
2743 // theta ~= sin(theta), so
2744 // angular_velocity = (erp*fps) * (ax1 x ax2)
2745 // ax1 x ax2 is in the plane space of ax1, so we project the angular
2746 // velocity to p and q to find the right hand side.
2747
2748 dVector3 ax2;
2749 if (joint->node[1].body) {
2750 dMULTIPLY0_331 (ax2, R2, joint->axisR2);
2751 }
2752 else {
2753 ax2[0] = joint->axisR2[0];
2754 ax2[1] = joint->axisR2[1];
2755 ax2[2] = joint->axisR2[2];
2756 }
2757
2758 dVector3 b;
2759 dCROSS (b,=,ax1, ax2);
2760 info->c[0] = k * dDOT(b, axP);
2761 info->c[1] = k * dDOT(b, q);
2762
2763
2764
2765 // ==========================
2766 // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
2767 // or 5 if rotoide and prismatic powered
2768
2769 // two rows. we want: vel2 = vel1 + w1 x c ... but this would
2770 // result in three equations, so we project along the planespace vectors
2771 // so that sliding along the prismatic axis is disregarded. for symmetry we
2772 // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
2773
2774 // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
2775 // 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
2776 // v_p is speed of prismatic joint (i.e. elongation rate)
2777 // Since the constraints are perpendicular to v_p we have:
2778 // p dot v_p = 0 and q dot v_p = 0
2779 // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
2780 // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
2781 // ==
2782 // 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
2783 // since a . (b x c) = - b . (a x c) = - (a x c) . b
2784 // and a x b = - b x a
2785 // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
2786 // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
2787 // Coeff for 1er line of: J1l => ax1, J2l => -ax1
2788 // Coeff for 2er line of: J1l => q, J2l => -q
2789 // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
2790 // Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q
2791
2792
2793 dCROSS ((info->J1a)+s2, = , dist, ax1);
2794
2795 dCROSS ((info->J1a)+s3, = , dist, q);
2796
2797
2798 info->J1l[s2+0] = ax1[0];
2799 info->J1l[s2+1] = ax1[1];
2800 info->J1l[s2+2] = ax1[2];
2801
2802 info->J1l[s3+0] = q[0];
2803 info->J1l[s3+1] = q[1];
2804 info->J1l[s3+2] = q[2];
2805
2806 if (joint->node[1].body) {
2807 dVector3 anchor2;
2808
2809 // Calculate anchor2 in world coordinate
2810 dMULTIPLY0_331 (anchor2, R2, joint->anchor2);
2811
2812 // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
2813 dCROSS ((info->J2a)+s2, = , ax2, anchor2); // since ax1 == ax2
2814
2815 // The cross product is in reverse order since we want the negative value
2816 dCROSS ((info->J2a)+s3, = , q, anchor2);
2817
2818 info->J2l[s2+0] = -ax1[0];
2819 info->J2l[s2+1] = -ax1[1];
2820 info->J2l[s2+2] = -ax1[2];
2821
2822 info->J2l[s3+0] = -q[0];
2823 info->J2l[s3+1] = -q[1];
2824 info->J2l[s3+2] = -q[2];
2825 }
2826
2827
2828 // We want to make correction for motion not in the line of the axisP
2829 // We calculate the displacement w.r.t. the anchor pt.
2830 //
2831 // compute the elements 2 and 3 of right hand side.
2832 // we want to align the offset point (in body 2's frame) with the center of body 1.
2833 // The position should be the same when we are not along the prismatic axis
2834 dVector3 err;
2835 dMULTIPLY0_331 (err, R1, joint->offset);
2836 err[0] += dist[0];
2837 err[1] += dist[1];
2838 err[2] += dist[2];
2839 info->c[2] = k * dDOT(ax1, err);
2840 info->c[3] = k * dDOT(q, err);
2841
2842 // Here we can't use addLimot because of some assumption in the function
2843 int powered = joint->limotP.fmax > 0;
2844 if (powered || joint->limotP.limit) {
2845 info->J1l[s4+0] = axP[0];
2846 info->J1l[s4+1] = axP[1];
2847 info->J1l[s4+2] = axP[2];
2848 if (joint->node[1].body) {
2849 info->J2l[s4+0] = -axP[0];
2850 info->J2l[s4+1] = -axP[1];
2851 info->J2l[s4+2] = -axP[2];
2852 }
2853 // linear limot torque decoupling step:
2854 //
2855 // if this is a linear limot (e.g. from a slider), we have to be careful
2856 // that the linear constraint forces (+/- ax1) applied to the two bodies
2857 // do not create a torque couple. in other words, the points that the
2858 // constraint force is applied at must lie along the same ax1 axis.
2859 // a torque couple will result in powered or limited slider-jointed free
2860 // bodies from gaining angular momentum.
2861 // the solution used here is to apply the constraint forces at the point
2862 // halfway between the body centers. there is no penalty (other than an
2863 // extra tiny bit of computation) in doing this adjustment. note that we
2864 // only need to do this if the constraint connects two bodies.
2865
2866 dVector3 ltd; // Linear Torque Decoupling vector (a torque)
2867 if (joint->node[1].body) {
2868 dVector3 c;
2869 c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]);
2870 c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]);
2871 c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]);
2872 dReal val = dDOT(q, c);
2873 c[0] -= val * c[0];
2874 c[1] -= val * c[1];
2875 c[2] -= val * c[2];
2876
2877 dCROSS (ltd,=,c,axP);
2878 info->J1a[s4+0] = ltd[0];
2879 info->J1a[s4+1] = ltd[1];
2880 info->J1a[s4+2] = ltd[2];
2881 info->J2a[s4+0] = ltd[0];
2882 info->J2a[s4+1] = ltd[1];
2883 info->J2a[s4+2] = ltd[2];
2884 }
2885
2886 // if we're limited low and high simultaneously, the joint motor is
2887 // ineffective
2888 if (joint->limotP.limit && (joint->limotP.lostop == joint->limotP.histop))
2889 powered = 0;
2890
2891 int row = 4;
2892 if (powered) {
2893 info->cfm[row] = joint->limotP.normal_cfm;
2894 if (!joint->limotP.limit) {
2895 info->c[row] = joint->limotP.vel;
2896 info->lo[row] = -joint->limotP.fmax;
2897 info->hi[row] = joint->limotP.fmax;
2898 }
2899 else {
2900 // the joint is at a limit, AND is being powered. if the joint is
2901 // being powered into the limit then we apply the maximum motor force
2902 // in that direction, because the motor is working against the
2903 // immovable limit. if the joint is being powered away from the limit
2904 // then we have problems because actually we need *two* lcp
2905 // constraints to handle this case. so we fake it and apply some
2906 // fraction of the maximum force. the fraction to use can be set as
2907 // a fudge factor.
2908
2909 dReal fm = joint->limotP.fmax;
2910 dReal vel = joint->limotP.vel;
2911 int limit = joint->limotP.limit;
2912 if ((vel > 0) || (vel==0 && limit==2)) fm = -fm;
2913
2914 // if we're powering away from the limit, apply the fudge factor
2915 if ((limit==1 && vel > 0) || (limit==2 && vel < 0))
2916 fm *= joint->limotP.fudge_factor;
2917
2918
2919 dBodyAddForce (joint->node[0].body,-fm*axP[0],-fm*axP[1],-fm*axP[2]);
2920
2921 if (joint->node[1].body) {
2922 dBodyAddForce (joint->node[1].body,fm*axP[0],fm*axP[1],fm*axP[2]);
2923
2924 // linear limot torque decoupling step: refer to above discussion
2925 dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1],
2926 -fm*ltd[2]);
2927 dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1],
2928 -fm*ltd[2]);
2929 }
2930 }
2931 }
2932
2933 if (joint->limotP.limit) {
2934 dReal k = info->fps * joint->limotP.stop_erp;
2935 info->c[row] = -k * joint->limotP.limit_err;
2936 info->cfm[row] = joint->limotP.stop_cfm;
2937
2938 if (joint->limotP.lostop == joint->limotP.histop) {
2939 // limited low and high simultaneously
2940 info->lo[row] = -dInfinity;
2941 info->hi[row] = dInfinity;
2942 }
2943 else {
2944 if (joint->limotP.limit == 1) {
2945 // low limit
2946 info->lo[row] = 0;
2947 info->hi[row] = dInfinity;
2948 }
2949 else {
2950 // high limit
2951 info->lo[row] = -dInfinity;
2952 info->hi[row] = 0;
2953 }
2954
2955 // deal with bounce
2956 if (joint->limotP.bounce > 0) {
2957 // calculate joint velocity
2958 dReal vel;
2959 vel = dDOT(joint->node[0].body->lvel, axP);
2960 if (joint->node[1].body)
2961 vel -= dDOT(joint->node[1].body->lvel, axP);
2962
2963 // only apply bounce if the velocity is incoming, and if the
2964 // resulting c[] exceeds what we already have.
2965 if (joint->limotP.limit == 1) {
2966 // low limit
2967 if (vel < 0) {
2968 dReal newc = -joint->limotP.bounce * vel;
2969 if (newc > info->c[row]) info->c[row] = newc;
2970 }
2971 }
2972 else {
2973 // high limit - all those computations are reversed
2974 if (vel > 0) {
2975 dReal newc = -joint->limotP.bounce * vel;
2976 if (newc < info->c[row]) info->c[row] = newc;
2977 }
2978 }
2979 }
2980 }
2981 }
2982 }
2983}
2984
2985
2986// compute initial relative rotation body1 -> body2, or env -> body1
2987static void PRComputeInitialRelativeRotation (dxJointPR *joint)
2988{
2989 if (joint->node[0].body) {
2990 if (joint->node[1].body) {
2991 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
2992 }
2993 else {
2994 // set joint->qrel to the transpose of the first body q
2995 joint->qrel[0] = joint->node[0].body->q[0];
2996 for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
2997 // WARNING do we need the - in -joint->node[0].body->q[i]; or not
2998 }
2999 }
3000}
3001
3002void dJointSetPRAnchor (dJointID j, dReal x, dReal y, dReal z)
3003{
3004 dxJointPR* joint = (dxJointPR*)j;
3005 dUASSERT(joint,"bad joint argument");
3006 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3007
3008 dVector3 dummy;
3009 setAnchors (joint,x,y,z,dummy,joint->anchor2);
3010}
3011
3012
3013void dJointSetPRAxis1 (dJointID j, dReal x, dReal y, dReal z)
3014{
3015 dxJointPR* joint = (dxJointPR*)j;
3016 dUASSERT(joint,"bad joint argument");
3017 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3018
3019 setAxes (joint,x,y,z,joint->axisP1, 0);
3020
3021 PRComputeInitialRelativeRotation (joint);
3022
3023 // compute initial relative rotation body1 -> body2, or env -> body1
3024 // also compute distance between anchor of body1 w.r.t center of body 2
3025 dVector3 c;
3026 if (joint->node[1].body) {
3027 dVector3 anchor2;
3028 dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R, joint->anchor2);
3029
3030 c[0] = ( joint->node[1].body->posr.pos[0] + anchor2[0] -
3031 joint->node[0].body->posr.pos[0] );
3032 c[1] = ( joint->node[1].body->posr.pos[1] + anchor2[1] -
3033 joint->node[0].body->posr.pos[1] );
3034 c[2] = ( joint->node[1].body->posr.pos[2] + anchor2[2] -
3035 joint->node[0].body->posr.pos[2] );
3036 }
3037 else if (joint->node[0].body) {
3038 c[0] = joint->anchor2[0] - joint->node[0].body->posr.pos[0];
3039 c[1] = joint->anchor2[1] - joint->node[0].body->posr.pos[1];
3040 c[2] = joint->anchor2[2] - joint->node[0].body->posr.pos[2];
3041 }
3042 else
3043 {
3044 joint->offset[0] = joint->anchor2[0];
3045 joint->offset[1] = joint->anchor2[1];
3046 joint->offset[2] = joint->anchor2[2];
3047
3048 return;
3049 }
3050
3051
3052 dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,c);
3053}
3054
3055
3056void dJointSetPRAxis2 (dJointID j, dReal x, dReal y, dReal z)
3057{
3058 dxJointPR* joint = (dxJointPR*)j;
3059 dUASSERT(joint,"bad joint argument");
3060 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3061 setAxes (joint,x,y,z,joint->axisR1,joint->axisR2);
3062 PRComputeInitialRelativeRotation (joint);
3063}
3064
3065
3066void dJointSetPRParam (dJointID j, int parameter, dReal value)
3067{
3068 dxJointPR* joint = (dxJointPR*)j;
3069 dUASSERT(joint,"bad joint argument");
3070 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3071 if ((parameter & 0xff00) == 0x100) {
3072 joint->limotR.set (parameter,value);
3073 }
3074 else {
3075 joint->limotP.set (parameter & 0xff,value);
3076 }
3077}
3078
3079void dJointGetPRAnchor (dJointID j, dVector3 result)
3080{
3081 dxJointPR* joint = (dxJointPR*)j;
3082 dUASSERT(joint,"bad joint argument");
3083 dUASSERT(result,"bad result argument");
3084 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3085
3086 if (joint->node[1].body)
3087 getAnchor2 (joint,result,joint->anchor2);
3088 else
3089 {
3090 result[0] = joint->anchor2[0];
3091 result[1] = joint->anchor2[1];
3092 result[2] = joint->anchor2[2];
3093 }
3094
3095}
3096
3097void dJointGetPRAxis1 (dJointID j, dVector3 result)
3098{
3099 dxJointPR* joint = (dxJointPR*)j;
3100 dUASSERT(joint,"bad joint argument");
3101 dUASSERT(result,"bad result argument");
3102 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3103 getAxis(joint, result, joint->axisP1);
3104}
3105
3106void dJointGetPRAxis2 (dJointID j, dVector3 result)
3107{
3108 dxJointPR* joint = (dxJointPR*)j;
3109 dUASSERT(joint,"bad joint argument");
3110 dUASSERT(result,"bad result argument");
3111 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3112 getAxis(joint, result, joint->axisR1);
3113}
3114
3115dReal dJointGetPRParam (dJointID j, int parameter)
3116{
3117 dxJointPR* joint = (dxJointPR*)j;
3118 dUASSERT(joint,"bad joint argument");
3119 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not Prismatic and Rotoide");
3120 if ((parameter & 0xff00) == 0x100) {
3121 return joint->limotR.get (parameter & 0xff);
3122 }
3123 else {
3124 return joint->limotP.get (parameter);
3125 }
3126}
3127
3128void dJointAddPRTorque (dJointID j, dReal torque)
3129{
3130 dxJointPR* joint = (dxJointPR*)j;
3131 dVector3 axis;
3132 dAASSERT(joint);
3133 dUASSERT(joint->vtable == &__dPR_vtable,"joint is not a Prismatic and Rotoide");
3134
3135 if (joint->flags & dJOINT_REVERSE)
3136 torque = -torque;
3137
3138 getAxis (joint,axis,joint->axisR1);
3139 axis[0] *= torque;
3140 axis[1] *= torque;
3141 axis[2] *= torque;
3142
3143 if (joint->node[0].body != 0)
3144 dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]);
3145 if (joint->node[1].body != 0)
3146 dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
3147}
3148
3149
3150dxJoint::Vtable __dPR_vtable = {
3151 sizeof(dxJointPR),
3152 (dxJoint::init_fn*) PRInit,
3153 (dxJoint::getInfo1_fn*) PRGetInfo1,
3154 (dxJoint::getInfo2_fn*) PRGetInfo2,
3155 dJointTypePR
3156};
3157
3158
3159//****************************************************************************
3160// angular motor
3161
3162static void amotorInit (dxJointAMotor *j)
3163{
3164 int i;
3165 j->num = 0;
3166 j->mode = dAMotorUser;
3167 for (i=0; i<3; i++) {
3168 j->rel[i] = 0;
3169 dSetZero (j->axis[i],4);
3170 j->limot[i].init (j->world);
3171 j->angle[i] = 0;
3172 }
3173 dSetZero (j->reference1,4);
3174 dSetZero (j->reference2,4);
3175}
3176
3177
3178// compute the 3 axes in global coordinates
3179
3180static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3])
3181{
3182 if (joint->mode == dAMotorEuler) {
3183 // special handling for euler mode
3184 dMULTIPLY0_331 (ax[0],joint->node[0].body->posr.R,joint->axis[0]);
3185 if (joint->node[1].body) {
3186 dMULTIPLY0_331 (ax[2],joint->node[1].body->posr.R,joint->axis[2]);
3187 }
3188 else {
3189 ax[2][0] = joint->axis[2][0];
3190 ax[2][1] = joint->axis[2][1];
3191 ax[2][2] = joint->axis[2][2];
3192 }
3193 dCROSS (ax[1],=,ax[2],ax[0]);
3194 dNormalize3 (ax[1]);
3195 }
3196 else {
3197 for (int i=0; i < joint->num; i++) {
3198 if (joint->rel[i] == 1) {
3199 // relative to b1
3200 dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]);
3201 }
3202 else if (joint->rel[i] == 2) {
3203 // relative to b2
3204 if (joint->node[1].body) { // jds: don't assert, just ignore
3205 dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]);
3206 }
3207 }
3208 else {
3209 // global - just copy it
3210 ax[i][0] = joint->axis[i][0];
3211 ax[i][1] = joint->axis[i][1];
3212 ax[i][2] = joint->axis[i][2];
3213 }
3214 }
3215 }
3216}
3217
3218
3219static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3])
3220{
3221 // assumptions:
3222 // global axes already calculated --> ax
3223 // axis[0] is relative to body 1 --> global ax[0]
3224 // axis[2] is relative to body 2 --> global ax[2]
3225 // ax[1] = ax[2] x ax[0]
3226 // original ax[0] and ax[2] are perpendicular
3227 // reference1 is perpendicular to ax[0] (in body 1 frame)
3228 // reference2 is perpendicular to ax[2] (in body 2 frame)
3229 // all ax[] and reference vectors are unit length
3230
3231 // calculate references in global frame
3232 dVector3 ref1,ref2;
3233 dMULTIPLY0_331 (ref1,joint->node[0].body->posr.R,joint->reference1);
3234 if (joint->node[1].body) {
3235 dMULTIPLY0_331 (ref2,joint->node[1].body->posr.R,joint->reference2);
3236 }
3237 else {
3238 ref2[0] = joint->reference2[0];
3239 ref2[1] = joint->reference2[1];
3240 ref2[2] = joint->reference2[2];
3241 }
3242
3243 // get q perpendicular to both ax[0] and ref1, get first euler angle
3244 dVector3 q;
3245 dCROSS (q,=,ax[0],ref1);
3246 joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1));
3247
3248 // get q perpendicular to both ax[0] and ax[1], get second euler angle
3249 dCROSS (q,=,ax[0],ax[1]);
3250 joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q));
3251
3252 // get q perpendicular to both ax[1] and ax[2], get third euler angle
3253 dCROSS (q,=,ax[1],ax[2]);
3254 joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q));
3255}
3256
3257
3258// set the reference vectors as follows:
3259// * reference1 = current axis[2] relative to body 1
3260// * reference2 = current axis[0] relative to body 2
3261// this assumes that:
3262// * axis[0] is relative to body 1
3263// * axis[2] is relative to body 2
3264
3265static void amotorSetEulerReferenceVectors (dxJointAMotor *j)
3266{
3267 if (j->node[0].body && j->node[1].body) {
3268 dVector3 r; // axis[2] and axis[0] in global coordinates
3269 dMULTIPLY0_331 (r,j->node[1].body->posr.R,j->axis[2]);
3270 dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r);
3271 dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]);
3272 dMULTIPLY1_331 (j->reference2,j->node[1].body->posr.R,r);
3273 }
3274
3275 else { // jds
3276 // else if (j->node[0].body) {
3277 // dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
3278 // dMULTIPLY0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);
3279
3280 // We want to handle angular motors attached to passive geoms
3281 dVector3 r; // axis[2] and axis[0] in global coordinates
3282 r[0] = j->axis[2][0]; r[1] = j->axis[2][1]; r[2] = j->axis[2][2]; r[3] = j->axis[2][3];
3283 dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r);
3284 dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]);
3285 j->reference2[0] += r[0]; j->reference2[1] += r[1];
3286 j->reference2[2] += r[2]; j->reference2[3] += r[3];
3287 }
3288}
3289
3290
3291static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info)
3292{
3293 info->m = 0;
3294 info->nub = 0;
3295
3296 // compute the axes and angles, if in euler mode
3297 if (j->mode == dAMotorEuler) {
3298 dVector3 ax[3];
3299 amotorComputeGlobalAxes (j,ax);
3300 amotorComputeEulerAngles (j,ax);
3301 }
3302
3303 // see if we're powered or at a joint limit for each axis
3304 for (int i=0; i < j->num; i++) {
3305 if (j->limot[i].testRotationalLimit (j->angle[i]) ||
3306 j->limot[i].fmax > 0) {
3307 info->m++;
3308 }
3309 }
3310}
3311
3312
3313static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info)
3314{
3315 int i;
3316
3317 // compute the axes (if not global)
3318 dVector3 ax[3];
3319 amotorComputeGlobalAxes (joint,ax);
3320
3321 // in euler angle mode we do not actually constrain the angular velocity
3322 // along the axes axis[0] and axis[2] (although we do use axis[1]) :
3323 //
3324 // to get constrain w2-w1 along ...not
3325 // ------ --------------------- ------
3326 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
3327 // d(angle[1])/dt = 0 ax[1]
3328 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
3329 //
3330 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
3331 // to prove the result for angle[0], write the expression for angle[0] from
3332 // GetInfo1 then take the derivative. to prove this for angle[2] it is
3333 // easier to take the euler rate expression for d(angle[2])/dt with respect
3334 // to the components of w and set that to 0.
3335
3336 dVector3 *axptr[3];
3337 axptr[0] = &ax[0];
3338 axptr[1] = &ax[1];
3339 axptr[2] = &ax[2];
3340
3341 dVector3 ax0_cross_ax1;
3342 dVector3 ax1_cross_ax2;
3343 if (joint->mode == dAMotorEuler) {
3344 dCROSS (ax0_cross_ax1,=,ax[0],ax[1]);
3345 axptr[2] = &ax0_cross_ax1;
3346 dCROSS (ax1_cross_ax2,=,ax[1],ax[2]);
3347 axptr[0] = &ax1_cross_ax2;
3348 }
3349
3350 int row=0;
3351 for (i=0; i < joint->num; i++) {
3352 row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1);
3353 }
3354}
3355
3356
3357void dJointSetAMotorNumAxes (dJointID j, int num)
3358{
3359 dxJointAMotor* joint = (dxJointAMotor*)j;
3360 dAASSERT(joint && num >= 0 && num <= 3);
3361 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3362 if (joint->mode == dAMotorEuler) {
3363 joint->num = 3;
3364 }
3365 else {
3366 if (num < 0) num = 0;
3367 if (num > 3) num = 3;
3368 joint->num = num;
3369 }
3370}
3371
3372
3373void dJointSetAMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z)
3374{
3375 dxJointAMotor* joint = (dxJointAMotor*)j;
3376 dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
3377 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3378 dUASSERT(!(!joint->node[1].body && (joint->flags & dJOINT_REVERSE) && rel == 1),"no first body, can't set axis rel=1");
3379 dUASSERT(!(!joint->node[1].body && !(joint->flags & dJOINT_REVERSE) && rel == 2),"no second body, can't set axis rel=2");
3380 if (anum < 0) anum = 0;
3381 if (anum > 2) anum = 2;
3382
3383 // adjust rel to match the internal body order
3384 if (!joint->node[1].body && rel==2) rel = 1;
3385
3386 joint->rel[anum] = rel;
3387
3388 // x,y,z is always in global coordinates regardless of rel, so we may have
3389 // to convert it to be relative to a body
3390 dVector3 r;
3391 r[0] = x;
3392 r[1] = y;
3393 r[2] = z;
3394 r[3] = 0;
3395 if (rel > 0) {
3396 if (rel==1) {
3397 dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r);
3398 }
3399 else {
3400 // don't assert; handle the case of attachment to a bodiless geom
3401 if (joint->node[1].body) { // jds
3402 dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r);
3403 }
3404 else {
3405 joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1];
3406 joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3];
3407 }
3408 }
3409 }
3410 else {
3411 joint->axis[anum][0] = r[0];
3412 joint->axis[anum][1] = r[1];
3413 joint->axis[anum][2] = r[2];
3414 }
3415 dNormalize3 (joint->axis[anum]);
3416 if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint);
3417}
3418
3419
3420void dJointSetAMotorAngle (dJointID j, int anum, dReal angle)
3421{
3422 dxJointAMotor* joint = (dxJointAMotor*)j;
3423 dAASSERT(joint && anum >= 0 && anum < 3);
3424 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3425 if (joint->mode == dAMotorUser) {
3426 if (anum < 0) anum = 0;
3427 if (anum > 3) anum = 3;
3428 joint->angle[anum] = angle;
3429 }
3430}
3431
3432
3433void dJointSetAMotorParam (dJointID j, int parameter, dReal value)
3434{
3435 dxJointAMotor* joint = (dxJointAMotor*)j;
3436 dAASSERT(joint);
3437 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3438 int anum = parameter >> 8;
3439 if (anum < 0) anum = 0;
3440 if (anum > 2) anum = 2;
3441 parameter &= 0xff;
3442 joint->limot[anum].set (parameter, value);
3443}
3444
3445
3446void dJointSetAMotorMode (dJointID j, int mode)
3447{
3448 dxJointAMotor* joint = (dxJointAMotor*)j;
3449 dAASSERT(joint);
3450 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3451 joint->mode = mode;
3452 if (joint->mode == dAMotorEuler) {
3453 joint->num = 3;
3454 amotorSetEulerReferenceVectors (joint);
3455 }
3456}
3457
3458
3459int dJointGetAMotorNumAxes (dJointID j)
3460{
3461 dxJointAMotor* joint = (dxJointAMotor*)j;
3462 dAASSERT(joint);
3463 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3464 return joint->num;
3465}
3466
3467
3468void dJointGetAMotorAxis (dJointID j, int anum, dVector3 result)
3469{
3470 dxJointAMotor* joint = (dxJointAMotor*)j;
3471 dAASSERT(joint && anum >= 0 && anum < 3);
3472 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3473 if (anum < 0) anum = 0;
3474 if (anum > 2) anum = 2;
3475 if (joint->rel[anum] > 0) {
3476 if (joint->rel[anum]==1) {
3477 dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis[anum]);
3478 }
3479 else {
3480 if (joint->node[1].body) { // jds
3481 dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis[anum]);
3482 }
3483 else {
3484 result[0] = joint->axis[anum][0]; result[1] = joint->axis[anum][1];
3485 result[2] = joint->axis[anum][2]; result[3] = joint->axis[anum][3];
3486 }
3487 }
3488 }
3489 else {
3490 result[0] = joint->axis[anum][0];
3491 result[1] = joint->axis[anum][1];
3492 result[2] = joint->axis[anum][2];
3493 }
3494}
3495
3496
3497int dJointGetAMotorAxisRel (dJointID j, int anum)
3498{
3499 dxJointAMotor* joint = (dxJointAMotor*)j;
3500 dAASSERT(joint && anum >= 0 && anum < 3);
3501 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3502 if (anum < 0) anum = 0;
3503 if (anum > 2) anum = 2;
3504 return joint->rel[anum];
3505}
3506
3507
3508dReal dJointGetAMotorAngle (dJointID j, int anum)
3509{
3510 dxJointAMotor* joint = (dxJointAMotor*)j;
3511 dAASSERT(joint && anum >= 0 && anum < 3);
3512 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3513 if (anum < 0) anum = 0;
3514 if (anum > 3) anum = 3;
3515 return joint->angle[anum];
3516}
3517
3518
3519dReal dJointGetAMotorAngleRate (dJointID j, int anum)
3520{
3521 dxJointAMotor* joint = (dxJointAMotor*)j;
3522 // @@@
3523 dDebug (0,"not yet implemented");
3524 return 0;
3525}
3526
3527
3528dReal dJointGetAMotorParam (dJointID j, int parameter)
3529{
3530 dxJointAMotor* joint = (dxJointAMotor*)j;
3531 dAASSERT(joint);
3532 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3533 int anum = parameter >> 8;
3534 if (anum < 0) anum = 0;
3535 if (anum > 2) anum = 2;
3536 parameter &= 0xff;
3537 return joint->limot[anum].get (parameter);
3538}
3539
3540
3541int dJointGetAMotorMode (dJointID j)
3542{
3543 dxJointAMotor* joint = (dxJointAMotor*)j;
3544 dAASSERT(joint);
3545 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3546 return joint->mode;
3547}
3548
3549
3550void dJointAddAMotorTorques (dJointID j, dReal torque1, dReal torque2, dReal torque3)
3551{
3552 dxJointAMotor* joint = (dxJointAMotor*)j;
3553 dVector3 axes[3];
3554 dAASSERT(joint);
3555 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
3556
3557 if (joint->num == 0)
3558 return;
3559 dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints");
3560
3561 amotorComputeGlobalAxes (joint,axes);
3562 axes[0][0] *= torque1;
3563 axes[0][1] *= torque1;
3564 axes[0][2] *= torque1;
3565 if (joint->num >= 2) {
3566 axes[0][0] += axes[1][0] * torque2;
3567 axes[0][1] += axes[1][1] * torque2;
3568 axes[0][2] += axes[1][2] * torque2;
3569 if (joint->num >= 3) {
3570 axes[0][0] += axes[2][0] * torque3;
3571 axes[0][1] += axes[2][1] * torque3;
3572 axes[0][2] += axes[2][2] * torque3;
3573 }
3574 }
3575
3576 if (joint->node[0].body != 0)
3577 dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]);
3578 if (joint->node[1].body != 0)
3579 dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]);
3580}
3581
3582
3583dxJoint::Vtable __damotor_vtable = {
3584 sizeof(dxJointAMotor),
3585 (dxJoint::init_fn*) amotorInit,
3586 (dxJoint::getInfo1_fn*) amotorGetInfo1,
3587 (dxJoint::getInfo2_fn*) amotorGetInfo2,
3588 dJointTypeAMotor};
3589
3590
3591
3592//****************************************************************************
3593// lmotor joint
3594static void lmotorInit (dxJointLMotor *j)
3595{
3596 int i;
3597 j->num = 0;
3598 for (i=0;i<3;i++) {
3599 dSetZero(j->axis[i],4);
3600 j->limot[i].init(j->world);
3601 }
3602}
3603
3604static void lmotorComputeGlobalAxes (dxJointLMotor *joint, dVector3 ax[3])
3605{
3606 for (int i=0; i< joint->num; i++) {
3607 if (joint->rel[i] == 1) {
3608 dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]);
3609 }
3610 else if (joint->rel[i] == 2) {
3611 if (joint->node[1].body) { // jds: don't assert, just ignore
3612 dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]);
3613 }
3614 } else {
3615 ax[i][0] = joint->axis[i][0];
3616 ax[i][1] = joint->axis[i][1];
3617 ax[i][2] = joint->axis[i][2];
3618 }
3619 }
3620}
3621
3622static void lmotorGetInfo1 (dxJointLMotor *j, dxJoint::Info1 *info)
3623{
3624 info->m = 0;
3625 info->nub = 0;
3626 for (int i=0; i < j->num; i++) {
3627 if (j->limot[i].fmax > 0) {
3628 info->m++;
3629 }
3630 }
3631}
3632
3633static void lmotorGetInfo2 (dxJointLMotor *joint, dxJoint::Info2 *info)
3634{
3635 int row=0;
3636 dVector3 ax[3];
3637 lmotorComputeGlobalAxes(joint, ax);
3638
3639 for (int i=0;i<joint->num;i++) {
3640 row += joint->limot[i].addLimot(joint,info,row,ax[i], 0);
3641 }
3642}
3643
3644void dJointSetLMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z)
3645{
3646 dxJointLMotor* joint = (dxJointLMotor*)j;
3647//for now we are ignoring rel!
3648 dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
3649 dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor");
3650 if (anum < 0) anum = 0;
3651 if (anum > 2) anum = 2;
3652
3653 if (!joint->node[1].body && rel==2) rel = 1; //ref 1
3654
3655 joint->rel[anum] = rel;
3656
3657 dVector3 r;
3658 r[0] = x;
3659 r[1] = y;
3660 r[2] = z;
3661 r[3] = 0;
3662 if (rel > 0) {
3663 if (rel==1) {
3664 dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r);
3665 } else {
3666 //second body has to exists thanks to ref 1 line
3667 dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r);
3668 }
3669 } else {
3670 joint->axis[anum][0] = r[0];
3671 joint->axis[anum][1] = r[1];
3672 joint->axis[anum][2] = r[2];
3673 }
3674
3675 dNormalize3 (joint->axis[anum]);
3676}
3677
3678void dJointSetLMotorNumAxes (dJointID j, int num)
3679{
3680 dxJointLMotor* joint = (dxJointLMotor*)j;
3681 dAASSERT(joint && num >= 0 && num <= 3);
3682 dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor");
3683 if (num < 0) num = 0;
3684 if (num > 3) num = 3;
3685 joint->num = num;
3686}
3687
3688void dJointSetLMotorParam (dJointID j, int parameter, dReal value)
3689{
3690 dxJointLMotor* joint = (dxJointLMotor*)j;
3691 dAASSERT(joint);
3692 dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor");
3693 int anum = parameter >> 8;
3694 if (anum < 0) anum = 0;
3695 if (anum > 2) anum = 2;
3696 parameter &= 0xff;
3697 joint->limot[anum].set (parameter, value);
3698}
3699
3700int dJointGetLMotorNumAxes (dJointID j)
3701{
3702 dxJointLMotor* joint = (dxJointLMotor*)j;
3703 dAASSERT(joint);
3704 dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor");
3705 return joint->num;
3706}
3707
3708
3709void dJointGetLMotorAxis (dJointID j, int anum, dVector3 result)
3710{
3711 dxJointLMotor* joint = (dxJointLMotor*)j;
3712 dAASSERT(joint && anum >= 0 && anum < 3);
3713 dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor");
3714 if (anum < 0) anum = 0;
3715 if (anum > 2) anum = 2;
3716 result[0] = joint->axis[anum][0];
3717 result[1] = joint->axis[anum][1];
3718 result[2] = joint->axis[anum][2];
3719}
3720
3721dReal dJointGetLMotorParam (dJointID j, int parameter)
3722{
3723 dxJointLMotor* joint = (dxJointLMotor*)j;
3724 dAASSERT(joint);
3725 dUASSERT(joint->vtable == &__dlmotor_vtable,"joint is not an lmotor");
3726 int anum = parameter >> 8;
3727 if (anum < 0) anum = 0;
3728 if (anum > 2) anum = 2;
3729 parameter &= 0xff;
3730 return joint->limot[anum].get (parameter);
3731}
3732
3733dxJoint::Vtable __dlmotor_vtable = {
3734 sizeof(dxJointLMotor),
3735 (dxJoint::init_fn*) lmotorInit,
3736 (dxJoint::getInfo1_fn*) lmotorGetInfo1,
3737 (dxJoint::getInfo2_fn*) lmotorGetInfo2,
3738 dJointTypeLMotor
3739};
3740
3741
3742//****************************************************************************
3743// fixed joint
3744
3745static void fixedInit (dxJointFixed *j)
3746{
3747 dSetZero (j->offset,4);
3748 dSetZero (j->qrel,4);
3749 j->erp = j->world->global_erp;
3750 j->cfm = j->world->global_cfm;
3751}
3752
3753
3754static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info)
3755{
3756 info->m = 6;
3757 info->nub = 6;
3758}
3759
3760
3761static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info)
3762{
3763 int s = info->rowskip;
3764
3765 // Three rows for orientation
3766 setFixedOrientation(joint, info, joint->qrel, 3);
3767
3768 // Three rows for position.
3769 // set jacobian
3770 info->J1l[0] = 1;
3771 info->J1l[s+1] = 1;
3772 info->J1l[2*s+2] = 1;
3773
3774 info->erp = joint->erp;
3775 info->cfm[0] = joint->cfm;
3776 info->cfm[1] = joint->cfm;
3777 info->cfm[2] = joint->cfm;
3778
3779 dVector3 ofs;
3780 dMULTIPLY0_331 (ofs,joint->node[0].body->posr.R,joint->offset);
3781 if (joint->node[1].body) {
3782 dCROSSMAT (info->J1a,ofs,s,+,-);
3783 info->J2l[0] = -1;
3784 info->J2l[s+1] = -1;
3785 info->J2l[2*s+2] = -1;
3786 }
3787
3788 // set right hand side for the first three rows (linear)
3789 dReal k = info->fps * info->erp;
3790 if (joint->node[1].body) {
3791 for (int j=0; j<3; j++)
3792 info->c[j] = k * (joint->node[1].body->posr.pos[j] -
3793 joint->node[0].body->posr.pos[j] + ofs[j]);
3794 }
3795 else {
3796 for (int j=0; j<3; j++)
3797 info->c[j] = k * (joint->offset[j] - joint->node[0].body->posr.pos[j]);
3798 }
3799}
3800
3801
3802void dJointSetFixed (dJointID j)
3803{
3804 dxJointFixed* joint = (dxJointFixed*)j;
3805 dUASSERT(joint,"bad joint argument");
3806 dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed");
3807 int i;
3808
3809 // This code is taken from sJointSetSliderAxis(), we should really put the
3810 // common code in its own function.
3811 // compute the offset between the bodies
3812 if (joint->node[0].body) {
3813 if (joint->node[1].body) {
3814 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
3815 dReal ofs[4];
3816 for (i=0; i<4; i++) ofs[i] = joint->node[0].body->posr.pos[i];
3817 for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->posr.pos[i];
3818 dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,ofs);
3819 }
3820 else {
3821 // set joint->qrel to the transpose of the first body's q
3822 joint->qrel[0] = joint->node[0].body->q[0];
3823 for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
3824 for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->posr.pos[i];
3825 }
3826 }
3827}
3828
3829void dxJointFixed::set (int num, dReal value)
3830{
3831 switch (num) {
3832 case dParamCFM:
3833 cfm = value;
3834 break;
3835 case dParamERP:
3836 erp = value;
3837 break;
3838 }
3839}
3840
3841
3842dReal dxJointFixed::get (int num)
3843{
3844 switch (num) {
3845 case dParamCFM:
3846 return cfm;
3847 case dParamERP:
3848 return erp;
3849 default:
3850 return 0;
3851 }
3852}
3853
3854
3855void dJointSetFixedParam (dJointID j, int parameter, dReal value)
3856{
3857 dxJointFixed* joint = (dxJointFixed*)j;
3858 dUASSERT(joint,"bad joint argument");
3859 dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed joint");
3860 joint->set (parameter,value);
3861}
3862
3863
3864dReal dJointGetFixedParam (dJointID j, int parameter)
3865{
3866 dxJointFixed* joint = (dxJointFixed*)j;
3867 dUASSERT(joint,"bad joint argument");
3868 dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed joint");
3869 return joint->get (parameter);
3870}
3871
3872
3873dxJoint::Vtable __dfixed_vtable = {
3874 sizeof(dxJointFixed),
3875 (dxJoint::init_fn*) fixedInit,
3876 (dxJoint::getInfo1_fn*) fixedGetInfo1,
3877 (dxJoint::getInfo2_fn*) fixedGetInfo2,
3878 dJointTypeFixed};
3879
3880//****************************************************************************
3881// null joint
3882
3883static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info)
3884{
3885 info->m = 0;
3886 info->nub = 0;
3887}
3888
3889
3890static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info)
3891{
3892 dDebug (0,"this should never get called");
3893}
3894
3895
3896dxJoint::Vtable __dnull_vtable = {
3897 sizeof(dxJointNull),
3898 (dxJoint::init_fn*) 0,
3899 (dxJoint::getInfo1_fn*) nullGetInfo1,
3900 (dxJoint::getInfo2_fn*) nullGetInfo2,
3901 dJointTypeNull};
3902
3903
3904
3905
3906/*
3907 This code is part of the Plane2D ODE joint
3908 by psero@gmx.de
3909 Wed Apr 23 18:53:43 CEST 2003
3910
3911 Add this code to the file: ode/src/joint.cpp
3912*/
3913
3914
3915# define VoXYZ(v1, o1, x, y, z) \
3916 ( \
3917 (v1)[0] o1 (x), \
3918 (v1)[1] o1 (y), \
3919 (v1)[2] o1 (z) \
3920 )
3921
3922static dReal Midentity[3][3] =
3923 {
3924 { 1, 0, 0 },
3925 { 0, 1, 0 },
3926 { 0, 0, 1, }
3927 };
3928
3929
3930
3931static void plane2dInit (dxJointPlane2D *j)
3932/*********************************************/
3933{
3934 /* MINFO ("plane2dInit ()"); */
3935 j->motor_x.init (j->world);
3936 j->motor_y.init (j->world);
3937 j->motor_angle.init (j->world);
3938}
3939
3940
3941
3942static void plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info)
3943/***********************************************************************/
3944{
3945 /* MINFO ("plane2dGetInfo1 ()"); */
3946
3947 info->nub = 3;
3948 info->m = 3;
3949
3950 if (j->motor_x.fmax > 0)
3951 j->row_motor_x = info->m ++;
3952 if (j->motor_y.fmax > 0)
3953 j->row_motor_y = info->m ++;
3954 if (j->motor_angle.fmax > 0)
3955 j->row_motor_angle = info->m ++;
3956}
3957
3958
3959
3960static void plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info)
3961/***************************************************************************/
3962{
3963 int r0 = 0,
3964 r1 = info->rowskip,
3965 r2 = 2 * r1;
3966 dReal eps = info->fps * info->erp;
3967
3968 /* MINFO ("plane2dGetInfo2 ()"); */
3969
3970/*
3971 v = v1, w = omega1
3972 (v2, omega2 not important (== static environment))
3973
3974 constraint equations:
3975 xz = 0
3976 wx = 0
3977 wy = 0
3978
3979 <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 )
3980 ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 )
3981 ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 )
3982 J1/J1l Omega1/J1a
3983*/
3984
3985 // fill in linear and angular coeff. for left hand side:
3986
3987 VoXYZ (&info->J1l[r0], =, 0, 0, 1);
3988 VoXYZ (&info->J1l[r1], =, 0, 0, 0);
3989 VoXYZ (&info->J1l[r2], =, 0, 0, 0);
3990
3991 VoXYZ (&info->J1a[r0], =, 0, 0, 0);
3992 VoXYZ (&info->J1a[r1], =, 1, 0, 0);
3993 VoXYZ (&info->J1a[r2], =, 0, 1, 0);
3994
3995 // error correction (against drift):
3996
3997 // a) linear vz, so that z (== pos[2]) == 0
3998 info->c[0] = eps * -joint->node[0].body->posr.pos[2];
3999
4000# if 0
4001 // b) angular correction? -> left to application !!!
4002 dReal *body_z_axis = &joint->node[0].body->R[8];
4003 info->c[1] = eps * +atan2 (body_z_axis[1], body_z_axis[2]); // wx error
4004 info->c[2] = eps * -atan2 (body_z_axis[0], body_z_axis[2]); // wy error
4005# endif
4006
4007 // if the slider is powered, or has joint limits, add in the extra row:
4008
4009 if (joint->row_motor_x > 0)
4010 joint->motor_x.addLimot (
4011 joint, info, joint->row_motor_x, Midentity[0], 0);
4012
4013 if (joint->row_motor_y > 0)
4014 joint->motor_y.addLimot (
4015 joint, info, joint->row_motor_y, Midentity[1], 0);
4016
4017 if (joint->row_motor_angle > 0)
4018 joint->motor_angle.addLimot (
4019 joint, info, joint->row_motor_angle, Midentity[2], 1);
4020}
4021
4022
4023
4024dxJoint::Vtable __dplane2d_vtable =
4025{
4026 sizeof (dxJointPlane2D),
4027 (dxJoint::init_fn*) plane2dInit,
4028 (dxJoint::getInfo1_fn*) plane2dGetInfo1,
4029 (dxJoint::getInfo2_fn*) plane2dGetInfo2,
4030 dJointTypePlane2D
4031};
4032
4033
4034void dJointSetPlane2DXParam (dxJoint *joint,
4035 int parameter, dReal value)
4036{
4037 dUASSERT (joint, "bad joint argument");
4038 dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
4039 dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint );
4040 joint2d->motor_x.set (parameter, value);
4041}
4042
4043
4044void dJointSetPlane2DYParam (dxJoint *joint,
4045 int parameter, dReal value)
4046{
4047 dUASSERT (joint, "bad joint argument");
4048 dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
4049 dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint );
4050 joint2d->motor_y.set (parameter, value);
4051}
4052
4053
4054
4055void dJointSetPlane2DAngleParam (dxJoint *joint,
4056 int parameter, dReal value)
4057{
4058 dUASSERT (joint, "bad joint argument");
4059 dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
4060 dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint );
4061 joint2d->motor_angle.set (parameter, value);
4062}
4063
4064
4065
diff --git a/libraries/ode-0.9/ode/src/joint.h b/libraries/ode-0.9/ode/src/joint.h
new file mode 100644
index 0000000..27c013e
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/joint.h
@@ -0,0 +1,346 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_JOINT_H_
24#define _ODE_JOINT_H_
25
26
27#include "objects.h"
28#include <ode/contact.h>
29#include "obstack.h"
30
31
32// joint flags
33enum {
34 // if this flag is set, the joint was allocated in a joint group
35 dJOINT_INGROUP = 1,
36
37 // if this flag is set, the joint was attached with arguments (0,body).
38 // our convention is to treat all attaches as (body,0), i.e. so node[0].body
39 // is always nonzero, so this flag records the fact that the arguments were
40 // swapped.
41 dJOINT_REVERSE = 2,
42
43 // if this flag is set, the joint can not have just one body attached to it,
44 // it must have either zero or two bodies attached.
45 dJOINT_TWOBODIES = 4
46};
47
48
49// there are two of these nodes in the joint, one for each connection to a
50// body. these are node of a linked list kept by each body of it's connecting
51// joints. but note that the body pointer in each node points to the body that
52// makes use of the *other* node, not this node. this trick makes it a bit
53// easier to traverse the body/joint graph.
54
55struct dxJointNode {
56 dxJoint *joint; // pointer to enclosing dxJoint object
57 dxBody *body; // *other* body this joint is connected to
58 dxJointNode *next; // next node in body's list of connected joints
59};
60
61
62struct dxJoint : public dObject {
63 // naming convention: the "first" body this is connected to is node[0].body,
64 // and the "second" body is node[1].body. if this joint is only connected
65 // to one body then the second body is 0.
66
67 // info returned by getInfo1 function. the constraint dimension is m (<=6).
68 // i.e. that is the total number of rows in the jacobian. `nub' is the
69 // number of unbounded variables (which have lo,hi = -/+ infinity).
70
71 struct Info1 {
72 int m,nub;
73 };
74
75 // info returned by getInfo2 function
76
77 struct Info2 {
78 // integrator parameters: frames per second (1/stepsize), default error
79 // reduction parameter (0..1).
80 dReal fps,erp;
81
82 // for the first and second body, pointers to two (linear and angular)
83 // n*3 jacobian sub matrices, stored by rows. these matrices will have
84 // been initialized to 0 on entry. if the second body is zero then the
85 // J2xx pointers may be 0.
86 dReal *J1l,*J1a,*J2l,*J2a;
87
88 // elements to jump from one row to the next in J's
89 int rowskip;
90
91 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
92 // "constraint force mixing" vector. c is set to zero on entry, cfm is
93 // set to a constant value (typically very small or zero) value on entry.
94 dReal *c,*cfm;
95
96 // lo and hi limits for variables (set to -/+ infinity on entry).
97 dReal *lo,*hi;
98
99 // findex vector for variables. see the LCP solver interface for a
100 // description of what this does. this is set to -1 on entry.
101 // note that the returned indexes are relative to the first index of
102 // the constraint.
103 int *findex;
104 };
105
106 // virtual function table: size of the joint structure, function pointers.
107 // we do it this way instead of using C++ virtual functions because
108 // sometimes we need to allocate joints ourself within a memory pool.
109
110 typedef void init_fn (dxJoint *joint);
111 typedef void getInfo1_fn (dxJoint *joint, Info1 *info);
112 typedef void getInfo2_fn (dxJoint *joint, Info2 *info);
113 struct Vtable {
114 int size;
115 init_fn *init;
116 getInfo1_fn *getInfo1;
117 getInfo2_fn *getInfo2;
118 int typenum; // a dJointTypeXXX type number
119 };
120
121 Vtable *vtable; // virtual function table
122 int flags; // dJOINT_xxx flags
123 dxJointNode node[2]; // connections to bodies. node[1].body can be 0
124 dJointFeedback *feedback; // optional feedback structure
125 dReal lambda[6]; // lambda generated by last step
126};
127
128
129// joint group. NOTE: any joints in the group that have their world destroyed
130// will have their world pointer set to 0.
131
132struct dxJointGroup : public dBase {
133 int num; // number of joints on the stack
134 dObStack stack; // a stack of (possibly differently sized) dxJoint
135}; // objects.
136
137
138// common limit and motor information for a single joint axis of movement
139struct dxJointLimitMotor {
140 dReal vel,fmax; // powered joint: velocity, max force
141 dReal lostop,histop; // joint limits, relative to initial position
142 dReal fudge_factor; // when powering away from joint limits
143 dReal normal_cfm; // cfm to use when not at a stop
144 dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit
145 dReal bounce; // restitution factor
146 // variables used between getInfo1() and getInfo2()
147 int limit; // 0=free, 1=at lo limit, 2=at hi limit
148 dReal limit_err; // if at limit, amount over limit
149
150 void init (dxWorld *);
151 void set (int num, dReal value);
152 dReal get (int num);
153 int testRotationalLimit (dReal angle);
154 int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row,
155 dVector3 ax1, int rotational);
156};
157
158
159// ball and socket
160
161struct dxJointBall : public dxJoint {
162 dVector3 anchor1; // anchor w.r.t first body
163 dVector3 anchor2; // anchor w.r.t second body
164 dReal erp; // error reduction
165 dReal cfm; // constraint force mix in
166 void set (int num, dReal value);
167 dReal get (int num);
168};
169extern struct dxJoint::Vtable __dball_vtable;
170
171
172// hinge
173
174struct dxJointHinge : public dxJoint {
175 dVector3 anchor1; // anchor w.r.t first body
176 dVector3 anchor2; // anchor w.r.t second body
177 dVector3 axis1; // axis w.r.t first body
178 dVector3 axis2; // axis w.r.t second body
179 dQuaternion qrel; // initial relative rotation body1 -> body2
180 dxJointLimitMotor limot; // limit and motor information
181};
182extern struct dxJoint::Vtable __dhinge_vtable;
183
184
185// universal
186
187struct dxJointUniversal : public dxJoint {
188 dVector3 anchor1; // anchor w.r.t first body
189 dVector3 anchor2; // anchor w.r.t second body
190 dVector3 axis1; // axis w.r.t first body
191 dVector3 axis2; // axis w.r.t second body
192 dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece
193 dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2
194 dxJointLimitMotor limot1; // limit and motor information for axis1
195 dxJointLimitMotor limot2; // limit and motor information for axis2
196};
197extern struct dxJoint::Vtable __duniversal_vtable;
198
199
200/**
201 * The axisP must be perpendicular to axis2
202 * <PRE>
203 * +-------------+
204 * | x |
205 * +------------\+
206 * Prismatic articulation .. ..
207 * | .. ..
208 * \/ .. ..
209 * +--------------+ --| __.. .. anchor2
210 * | x | .....|.......(__) ..
211 * +--------------+ --| ^ <
212 * |----------------------->|
213 * Offset |--- Rotoide articulation
214 * </PRE>
215 */
216struct dxJointPR : public dxJoint {
217
218 dVector3 anchor2; ///< @brief Position of the rotoide articulation
219 ///< w.r.t second body.
220 ///< @note Position of body 2 in world frame +
221 ///< anchor2 in world frame give the position
222 ///< of the rotoide articulation
223 dVector3 axisR1; ///< axis of the rotoide articulation w.r.t first body.
224 ///< @note This is considered as axis1 from the parameter
225 ///< view.
226 dVector3 axisR2; ///< axis of the rotoide articulation w.r.t second body.
227 ///< @note This is considered also as axis1 from the
228 ///< parameter view
229 dVector3 axisP1; ///< axis for the prismatic articulation w.r.t first body.
230 ///< @note This is considered as axis2 in from the parameter
231 ///< view
232 dQuaternion qrel; ///< initial relative rotation body1 -> body2.
233 dVector3 offset; ///< @brief vector between the body1 and the rotoide
234 ///< articulation.
235 ///<
236 ///< Going from the first to the second in the frame
237 ///< of body1.
238 ///< That should be aligned with body1 center along axisP
239 ///< This is calculated whe the axis are set.
240 dxJointLimitMotor limotR; ///< limit and motor information for the rotoide articulation.
241 dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation.
242};
243extern struct dxJoint::Vtable __dPR_vtable;
244
245
246
247// slider. if body2 is 0 then qrel is the absolute rotation of body1 and
248// offset is the position of body1 center along axis1.
249
250struct dxJointSlider : public dxJoint {
251 dVector3 axis1; // axis w.r.t first body
252 dQuaternion qrel; // initial relative rotation body1 -> body2
253 dVector3 offset; // point relative to body2 that should be
254 // aligned with body1 center along axis1
255 dxJointLimitMotor limot; // limit and motor information
256};
257extern struct dxJoint::Vtable __dslider_vtable;
258
259
260// contact
261
262struct dxJointContact : public dxJoint {
263 int the_m; // number of rows computed by getInfo1
264 dContact contact;
265};
266extern struct dxJoint::Vtable __dcontact_vtable;
267
268
269// hinge 2
270
271struct dxJointHinge2 : public dxJoint {
272 dVector3 anchor1; // anchor w.r.t first body
273 dVector3 anchor2; // anchor w.r.t second body
274 dVector3 axis1; // axis 1 w.r.t first body
275 dVector3 axis2; // axis 2 w.r.t second body
276 dReal c0,s0; // cos,sin of desired angle between axis 1,2
277 dVector3 v1,v2; // angle ref vectors embedded in first body
278 dxJointLimitMotor limot1; // limit+motor info for axis 1
279 dxJointLimitMotor limot2; // limit+motor info for axis 2
280 dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm)
281};
282extern struct dxJoint::Vtable __dhinge2_vtable;
283
284
285// angular motor
286
287struct dxJointAMotor : public dxJoint {
288 int num; // number of axes (0..3)
289 int mode; // a dAMotorXXX constant
290 int rel[3]; // what the axes are relative to (global,b1,b2)
291 dVector3 axis[3]; // three axes
292 dxJointLimitMotor limot[3]; // limit+motor info for axes
293 dReal angle[3]; // user-supplied angles for axes
294 // these vectors are used for calculating euler angles
295 dVector3 reference1; // original axis[2], relative to body 1
296 dVector3 reference2; // original axis[0], relative to body 2
297};
298extern struct dxJoint::Vtable __damotor_vtable;
299
300
301struct dxJointLMotor : public dxJoint {
302 int num;
303 int rel[3];
304 dVector3 axis[3];
305 dxJointLimitMotor limot[3];
306};
307
308extern struct dxJoint::Vtable __dlmotor_vtable;
309
310
311// 2d joint, constrains to z == 0
312
313struct dxJointPlane2D : public dxJoint
314{
315 int row_motor_x;
316 int row_motor_y;
317 int row_motor_angle;
318 dxJointLimitMotor motor_x;
319 dxJointLimitMotor motor_y;
320 dxJointLimitMotor motor_angle;
321};
322
323extern struct dxJoint::Vtable __dplane2d_vtable;
324
325
326// fixed
327
328struct dxJointFixed : public dxJoint {
329 dQuaternion qrel; // initial relative rotation body1 -> body2
330 dVector3 offset; // relative offset between the bodies
331 dReal erp; // error reduction parameter
332 dReal cfm; // constraint force mix-in
333 void set (int num, dReal value);
334 dReal get (int num);
335};
336extern struct dxJoint::Vtable __dfixed_vtable;
337
338
339// null joint, for testing only
340
341struct dxJointNull : public dxJoint {
342};
343extern struct dxJoint::Vtable __dnull_vtable;
344
345
346#endif
diff --git a/libraries/ode-0.9/ode/src/lcp.cpp b/libraries/ode-0.9/ode/src/lcp.cpp
new file mode 100644
index 0000000..d03d3e8
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/lcp.cpp
@@ -0,0 +1,2007 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25
26THE ALGORITHM
27-------------
28
29solve A*x = b+w, with x and w subject to certain LCP conditions.
30each x(i),w(i) must lie on one of the three line segments in the following
31diagram. each line segment corresponds to one index set :
32
33 w(i)
34 /|\ | :
35 | | :
36 | |i in N :
37 w>0 | |state[i]=0 :
38 | | :
39 | | : i in C
40 w=0 + +-----------------------+
41 | : |
42 | : |
43 w<0 | : |i in N
44 | : |state[i]=1
45 | : |
46 | : |
47 +-------|-----------|-----------|----------> x(i)
48 lo 0 hi
49
50the Dantzig algorithm proceeds as follows:
51 for i=1:n
52 * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or
53 negative towards the line. as this is done, the other (x(j),w(j))
54 for j<i are constrained to be on the line. if any (x,w) reaches the
55 end of a line segment then it is switched between index sets.
56 * i is added to the appropriate index set depending on what line segment
57 it hits.
58
59we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit
60simpler, because the starting point for x(i),w(i) is always on the dotted
61line x=0 and x will only ever increase in one direction, so it can only hit
62two out of the three line segments.
63
64
65NOTES
66-----
67
68this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".
69the implementation is split into an LCP problem object (dLCP) and an LCP
70driver function. most optimization occurs in the dLCP object.
71
72a naive implementation of the algorithm requires either a lot of data motion
73or a lot of permutation-array lookup, because we are constantly re-ordering
74rows and columns. to avoid this and make a more optimized algorithm, a
75non-trivial data structure is used to represent the matrix A (this is
76implemented in the fast version of the dLCP object).
77
78during execution of this algorithm, some indexes in A are clamped (set C),
79some are non-clamped (set N), and some are "don't care" (where x=0).
80A,x,b,w (and other problem vectors) are permuted such that the clamped
81indexes are first, the unclamped indexes are next, and the don't-care
82indexes are last. this permutation is recorded in the array `p'.
83initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,
84the corresponding elements of p are swapped.
85
86because the C and N elements are grouped together in the rows of A, we can do
87lots of work with a fast dot product function. if A,x,etc were not permuted
88and we only had a permutation array, then those dot products would be much
89slower as we would have a permutation array lookup in some inner loops.
90
91A is accessed through an array of row pointers, so that element (i,j) of the
92permuted matrix is A[i][j]. this makes row swapping fast. for column swapping
93we still have to actually move the data.
94
95during execution of this algorithm we maintain an L*D*L' factorization of
96the clamped submatrix of A (call it `AC') which is the top left nC*nC
97submatrix of A. there are two ways we could arrange the rows/columns in AC.
98
99(1) AC is always permuted such that L*D*L' = AC. this causes a problem
100 when a row/column is removed from C, because then all the rows/columns of A
101 between the deleted index and the end of C need to be rotated downward.
102 this results in a lot of data motion and slows things down.
103(2) L*D*L' is actually a factorization of a *permutation* of AC (which is
104 itself a permutation of the underlying A). this is what we do - the
105 permutation is recorded in the vector C. call this permutation A[C,C].
106 when a row/column is removed from C, all we have to do is swap two
107 rows/columns and manipulate C.
108
109*/
110
111#include <ode/common.h>
112#include "lcp.h"
113#include <ode/matrix.h>
114#include <ode/misc.h>
115#include "mat.h" // for testing
116#include <ode/timer.h> // for testing
117
118//***************************************************************************
119// code generation parameters
120
121// LCP debugging (mosty for fast dLCP) - this slows things down a lot
122//#define DEBUG_LCP
123
124//#define dLCP_SLOW // use slow dLCP object
125#define dLCP_FAST // use fast dLCP object
126
127// option 1 : matrix row pointers (less data copying)
128#define ROWPTRS
129#define ATYPE dReal **
130#define AROW(i) (A[i])
131
132// option 2 : no matrix row pointers (slightly faster inner loops)
133//#define NOROWPTRS
134//#define ATYPE dReal *
135//#define AROW(i) (A+(i)*nskip)
136
137// use protected, non-stack memory allocation system
138
139#ifdef dUSE_MALLOC_FOR_ALLOCA
140extern unsigned int dMemoryFlag;
141
142#define ALLOCA(t,v,s) t* v = (t*) malloc(s)
143#define UNALLOCA(t) free(t)
144
145#else
146
147#define ALLOCA(t,v,s) t* v =(t*)dALLOCA16(s)
148#define UNALLOCA(t) /* nothing */
149
150#endif
151
152#define NUB_OPTIMIZATIONS
153
154//***************************************************************************
155
156// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
157// A is nskip. this only references and swaps the lower triangle.
158// if `do_fast_row_swaps' is nonzero and row pointers are being used, then
159// rows will be swapped by exchanging row pointers. otherwise the data will
160// be copied.
161
162static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip,
163 int do_fast_row_swaps)
164{
165 int i;
166 dAASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
167 nskip >= n && i1 < i2);
168
169# ifdef ROWPTRS
170 for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1];
171 for (i=i1+1; i<i2; i++) A[i][i1] = A[i2][i];
172 A[i1][i2] = A[i1][i1];
173 A[i1][i1] = A[i2][i1];
174 A[i2][i1] = A[i2][i2];
175 // swap rows, by swapping row pointers
176 if (do_fast_row_swaps) {
177 dReal *tmpp;
178 tmpp = A[i1];
179 A[i1] = A[i2];
180 A[i2] = tmpp;
181 }
182 else {
183 ALLOCA (dReal,tmprow,n * sizeof(dReal));
184
185#ifdef dUSE_MALLOC_FOR_ALLOCA
186 if (tmprow == NULL) {
187 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
188 return;
189 }
190#endif
191
192 memcpy (tmprow,A[i1],n * sizeof(dReal));
193 memcpy (A[i1],A[i2],n * sizeof(dReal));
194 memcpy (A[i2],tmprow,n * sizeof(dReal));
195 UNALLOCA(tmprow);
196 }
197 // swap columns the hard way
198 for (i=i2+1; i<n; i++) {
199 dReal tmp = A[i][i1];
200 A[i][i1] = A[i][i2];
201 A[i][i2] = tmp;
202 }
203# else
204 dReal tmp;
205 ALLOCA (dReal,tmprow,n * sizeof(dReal));
206
207#ifdef dUSE_MALLOC_FOR_ALLOCA
208 if (tmprow == NULL) {
209 return;
210 }
211#endif
212
213 if (i1 > 0) {
214 memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal));
215 memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal));
216 memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal));
217 }
218 for (i=i1+1; i<i2; i++) {
219 tmp = A[i2*nskip+i];
220 A[i2*nskip+i] = A[i*nskip+i1];
221 A[i*nskip+i1] = tmp;
222 }
223 tmp = A[i1*nskip+i1];
224 A[i1*nskip+i1] = A[i2*nskip+i2];
225 A[i2*nskip+i2] = tmp;
226 for (i=i2+1; i<n; i++) {
227 tmp = A[i*nskip+i1];
228 A[i*nskip+i1] = A[i*nskip+i2];
229 A[i*nskip+i2] = tmp;
230 }
231 UNALLOCA(tmprow);
232# endif
233
234}
235
236
237// swap two indexes in the n*n LCP problem. i1 must be <= i2.
238
239static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo,
240 dReal *hi, int *p, int *state, int *findex,
241 int n, int i1, int i2, int nskip,
242 int do_fast_row_swaps)
243{
244 dReal tmp;
245 int tmpi;
246 dIASSERT (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n &&
247 i1 <= i2);
248 if (i1==i2) return;
249 swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
250#ifdef dUSE_MALLOC_FOR_ALLOCA
251 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
252 return;
253#endif
254 tmp = x[i1];
255 x[i1] = x[i2];
256 x[i2] = tmp;
257 tmp = b[i1];
258 b[i1] = b[i2];
259 b[i2] = tmp;
260 tmp = w[i1];
261 w[i1] = w[i2];
262 w[i2] = tmp;
263 tmp = lo[i1];
264 lo[i1] = lo[i2];
265 lo[i2] = tmp;
266 tmp = hi[i1];
267 hi[i1] = hi[i2];
268 hi[i2] = tmp;
269 tmpi = p[i1];
270 p[i1] = p[i2];
271 p[i2] = tmpi;
272 tmpi = state[i1];
273 state[i1] = state[i2];
274 state[i2] = tmpi;
275 if (findex) {
276 tmpi = findex[i1];
277 findex[i1] = findex[i2];
278 findex[i2] = tmpi;
279 }
280}
281
282
283// for debugging - check that L,d is the factorization of A[C,C].
284// A[C,C] has size nC*nC and leading dimension nskip.
285// L has size nC*nC and leading dimension nskip.
286// d has size nC.
287
288#ifdef DEBUG_LCP
289
290static void checkFactorization (ATYPE A, dReal *_L, dReal *_d,
291 int nC, int *C, int nskip)
292{
293 int i,j;
294 if (nC==0) return;
295
296 // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C]
297 dMatrix A1 (nC,nC);
298 for (i=0; i<nC; i++) {
299 for (j=0; j<=i; j++) A1(i,j) = A1(j,i) = AROW(i)[j];
300 }
301 dMatrix A2 = A1.select (nC,C,nC,C);
302
303 // printf ("A1=\n"); A1.print(); printf ("\n");
304 // printf ("A2=\n"); A2.print(); printf ("\n");
305
306 // compute A3 = L*D*L'
307 dMatrix L (nC,nC,_L,nskip,1);
308 dMatrix D (nC,nC);
309 for (i=0; i<nC; i++) D(i,i) = 1/_d[i];
310 L.clearUpperTriangle();
311 for (i=0; i<nC; i++) L(i,i) = 1;
312 dMatrix A3 = L * D * L.transpose();
313
314 // printf ("L=\n"); L.print(); printf ("\n");
315 // printf ("D=\n"); D.print(); printf ("\n");
316 // printf ("A3=\n"); A2.print(); printf ("\n");
317
318 // compare A2 and A3
319 dReal diff = A2.maxDifference (A3);
320 if (diff > 1e-8)
321 dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff);
322}
323
324#endif
325
326
327// for debugging
328
329#ifdef DEBUG_LCP
330
331static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C)
332{
333 int j,k;
334 dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n);
335 for (k=0; k<i; k++) dIASSERT (p[k] >= 0 && p[k] < i);
336 for (k=i; k<n; k++) dIASSERT (p[k] == k);
337 for (j=0; j<nC; j++) {
338 int C_is_bad = 1;
339 for (k=0; k<nC; k++) if (C[k]==j) C_is_bad = 0;
340 dIASSERT (C_is_bad==0);
341 }
342}
343
344#endif
345
346//***************************************************************************
347// dLCP manipulator object. this represents an n*n LCP problem.
348//
349// two index sets C and N are kept. each set holds a subset of
350// the variable indexes 0..n-1. an index can only be in one set.
351// initially both sets are empty.
352//
353// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
354
355#ifdef dLCP_SLOW
356
357// simple but slow implementation of dLCP, for testing the LCP drivers.
358
359#include "array.h"
360
361struct dLCP {
362 int n,nub,nskip;
363 dReal *Adata,*x,*b,*w,*lo,*hi; // LCP problem data
364 ATYPE A; // A rows
365 dArray<int> C,N; // index sets
366 int last_i_for_solve1; // last i value given to solve1
367
368 dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
369 dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
370 dReal *_Dell, dReal *_ell, dReal *_tmp,
371 int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
372 // the constructor is given an initial problem description (A,x,b,w) and
373 // space for other working data (which the caller may allocate on the stack).
374 // some of this data is specific to the fast dLCP implementation.
375 // the matrices A and L have size n*n, vectors have size n*1.
376 // A represents a symmetric matrix but only the lower triangle is valid.
377 // `nub' is the number of unbounded indexes at the start. all the indexes
378 // 0..nub-1 will be put into C.
379
380 ~dLCP();
381
382 int getNub() { return nub; }
383 // return the value of `nub'. the constructor may want to change it,
384 // so the caller should find out its new value.
385
386 // transfer functions: transfer index i to the given set (C or N). indexes
387 // less than `nub' can never be given. A,x,b,w,etc may be permuted by these
388 // functions, the caller must be robust to this.
389
390 void transfer_i_to_C (int i);
391 // this assumes C and N span 1:i-1. this also assumes that solve1() has
392 // been recently called for the same i without any other transfer
393 // functions in between (thereby allowing some data reuse for the fast
394 // implementation).
395 void transfer_i_to_N (int i);
396 // this assumes C and N span 1:i-1.
397 void transfer_i_from_N_to_C (int i);
398 void transfer_i_from_C_to_N (int i);
399
400 int numC();
401 int numN();
402 // return the number of indexes in set C/N
403
404 int indexC (int i);
405 int indexN (int i);
406 // return index i in set C/N.
407
408 // accessor and arithmetic functions. Aij translates as A(i,j), etc.
409 // make sure that only the lower triangle of A is ever referenced.
410
411 dReal Aii (int i);
412 dReal AiC_times_qC (int i, dReal *q);
413 dReal AiN_times_qN (int i, dReal *q); // for all Nj
414 void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj
415 void pN_plusequals_ANi (dReal *p, int i, int sign=1);
416 // for all Nj. sign = +1,-1. assumes i > maximum index in N.
417 void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q);
418 void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj
419 void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
420 // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1.
421 // the fast version of this function computes some data that is needed by
422 // transfer_i_to_C(). if only_transfer is nonzero then this function
423 // *only* computes that data, it does not set a(C).
424
425 void unpermute();
426 // call this at the end of the LCP function. if the x/w values have been
427 // permuted then this will unscramble them.
428};
429
430
431dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
432 dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
433 dReal *_Dell, dReal *_ell, dReal *_tmp,
434 int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
435{
436 dUASSERT (_findex==0,"slow dLCP object does not support findex array");
437
438 n = _n;
439 nub = _nub;
440 Adata = _Adata;
441 A = 0;
442 x = _x;
443 b = _b;
444 w = _w;
445 lo = _lo;
446 hi = _hi;
447 nskip = dPAD(n);
448 dSetZero (x,n);
449 last_i_for_solve1 = -1;
450
451 int i,j;
452 C.setSize (n);
453 N.setSize (n);
454 for (i=0; i<n; i++) {
455 C[i] = 0;
456 N[i] = 0;
457 }
458
459# ifdef ROWPTRS
460 // make matrix row pointers
461 A = Arows;
462 for (i=0; i<n; i++) A[i] = Adata + i*nskip;
463# else
464 A = Adata;
465# endif
466
467 // lets make A symmetric
468 for (i=0; i<n; i++) {
469 for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
470 }
471
472 // if nub>0, put all indexes 0..nub-1 into C and solve for x
473 if (nub > 0) {
474 for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
475 dFactorLDLT (_L,_d,nub,nskip);
476 memcpy (x,b,nub*sizeof(dReal));
477 dSolveLDLT (_L,_d,x,nub,nskip);
478 dSetZero (_w,nub);
479 for (i=0; i<nub; i++) C[i] = 1;
480 }
481}
482
483
484dLCP::~dLCP()
485{
486}
487
488
489void dLCP::transfer_i_to_C (int i)
490{
491 if (i < nub) dDebug (0,"bad i");
492 if (C[i]) dDebug (0,"i already in C");
493 if (N[i]) dDebug (0,"i already in N");
494 for (int k=0; k<i; k++) {
495 if (!(C[k] ^ N[k])) dDebug (0,"assumptions for C and N violated");
496 }
497 for (int k=i; k<n; k++)
498 if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
499 if (i != last_i_for_solve1) dDebug (0,"assumptions for i violated");
500 last_i_for_solve1 = -1;
501 C[i] = 1;
502}
503
504
505void dLCP::transfer_i_to_N (int i)
506{
507 if (i < nub) dDebug (0,"bad i");
508 if (C[i]) dDebug (0,"i already in C");
509 if (N[i]) dDebug (0,"i already in N");
510 for (int k=0; k<i; k++)
511 if (!C[k] && !N[k]) dDebug (0,"assumptions for C and N violated");
512 for (int k=i; k<n; k++)
513 if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
514 last_i_for_solve1 = -1;
515 N[i] = 1;
516}
517
518
519void dLCP::transfer_i_from_N_to_C (int i)
520{
521 if (i < nub) dDebug (0,"bad i");
522 if (C[i]) dDebug (0,"i already in C");
523 if (!N[i]) dDebug (0,"i not in N");
524 last_i_for_solve1 = -1;
525 N[i] = 0;
526 C[i] = 1;
527}
528
529
530void dLCP::transfer_i_from_C_to_N (int i)
531{
532 if (i < nub) dDebug (0,"bad i");
533 if (N[i]) dDebug (0,"i already in N");
534 if (!C[i]) dDebug (0,"i not in C");
535 last_i_for_solve1 = -1;
536 C[i] = 0;
537 N[i] = 1;
538}
539
540
541int dLCP::numC()
542{
543 int i,count=0;
544 for (i=0; i<n; i++) if (C[i]) count++;
545 return count;
546}
547
548
549int dLCP::numN()
550{
551 int i,count=0;
552 for (i=0; i<n; i++) if (N[i]) count++;
553 return count;
554}
555
556
557int dLCP::indexC (int i)
558{
559 int k,count=0;
560 for (k=0; k<n; k++) {
561 if (C[k]) {
562 if (count==i) return k;
563 count++;
564 }
565 }
566 dDebug (0,"bad index C (%d)",i);
567 return 0;
568}
569
570
571int dLCP::indexN (int i)
572{
573 int k,count=0;
574 for (k=0; k<n; k++) {
575 if (N[k]) {
576 if (count==i) return k;
577 count++;
578 }
579 }
580 dDebug (0,"bad index into N");
581 return 0;
582}
583
584
585dReal dLCP::Aii (int i)
586{
587 return AROW(i)[i];
588}
589
590
591dReal dLCP::AiC_times_qC (int i, dReal *q)
592{
593 dReal sum = 0;
594 for (int k=0; k<n; k++) if (C[k]) sum += AROW(i)[k] * q[k];
595 return sum;
596}
597
598
599dReal dLCP::AiN_times_qN (int i, dReal *q)
600{
601 dReal sum = 0;
602 for (int k=0; k<n; k++) if (N[k]) sum += AROW(i)[k] * q[k];
603 return sum;
604}
605
606
607void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
608{
609 dReal sum;
610 for (int ii=0; ii<n; ii++) if (N[ii]) {
611 sum = 0;
612 for (int jj=0; jj<n; jj++) if (C[jj]) sum += AROW(ii)[jj] * q[jj];
613 p[ii] = sum;
614 }
615}
616
617
618void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
619{
620 int k;
621 for (k=0; k<n; k++) if (N[k] && k >= i) dDebug (0,"N assumption violated");
622 if (sign > 0) {
623 for (k=0; k<n; k++) if (N[k]) p[k] += AROW(i)[k];
624 }
625 else {
626 for (k=0; k<n; k++) if (N[k]) p[k] -= AROW(i)[k];
627 }
628}
629
630
631void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
632{
633 for (int k=0; k<n; k++) if (C[k]) p[k] += s*q[k];
634}
635
636
637void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
638{
639 for (int k=0; k<n; k++) if (N[k]) p[k] += s*q[k];
640}
641
642
643void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
644{
645
646 ALLOCA (dReal,AA,n*nskip*sizeof(dReal));
647#ifdef dUSE_MALLOC_FOR_ALLOCA
648 if (AA == NULL) {
649 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
650 return;
651 }
652#endif
653 ALLOCA (dReal,dd,n*sizeof(dReal));
654#ifdef dUSE_MALLOC_FOR_ALLOCA
655 if (dd == NULL) {
656 UNALLOCA(AA);
657 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
658 return;
659 }
660#endif
661 ALLOCA (dReal,bb,n*sizeof(dReal));
662#ifdef dUSE_MALLOC_FOR_ALLOCA
663 if (bb == NULL) {
664 UNALLOCA(AA);
665 UNALLOCA(dd);
666 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
667 return;
668 }
669#endif
670
671 int ii,jj,AAi,AAj;
672
673 last_i_for_solve1 = i;
674 AAi = 0;
675 for (ii=0; ii<n; ii++) if (C[ii]) {
676 AAj = 0;
677 for (jj=0; jj<n; jj++) if (C[jj]) {
678 AA[AAi*nskip+AAj] = AROW(ii)[jj];
679 AAj++;
680 }
681 bb[AAi] = AROW(i)[ii];
682 AAi++;
683 }
684 if (AAi==0) {
685 UNALLOCA (AA);
686 UNALLOCA (dd);
687 UNALLOCA (bb);
688 return;
689 }
690
691 dFactorLDLT (AA,dd,AAi,nskip);
692 dSolveLDLT (AA,dd,bb,AAi,nskip);
693
694 AAi=0;
695 if (dir > 0) {
696 for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++];
697 }
698 else {
699 for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++];
700 }
701
702 UNALLOCA (AA);
703 UNALLOCA (dd);
704 UNALLOCA (bb);
705}
706
707
708void dLCP::unpermute()
709{
710}
711
712#endif // dLCP_SLOW
713
714//***************************************************************************
715// fast implementation of dLCP. see the above definition of dLCP for
716// interface comments.
717//
718// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is
719// permuted as the other vectors/matrices are permuted.
720//
721// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have
722// contiguous indexes. the don't-care indexes follow N.
723//
724// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are
725// added or removed from the set C the factorization is updated.
726// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.
727// the leading dimension of the matrix L is always `nskip'.
728//
729// at the start there may be other indexes that are unbounded but are not
730// included in `nub'. dLCP will permute the matrix so that absolutely all
731// unbounded vectors are at the start. thus there may be some initial
732// permutation.
733//
734// the algorithms here assume certain patterns, particularly with respect to
735// index transfer.
736
737#ifdef dLCP_FAST
738
739struct dLCP {
740 int n,nskip,nub;
741 ATYPE A; // A rows
742 dReal *Adata,*x,*b,*w,*lo,*hi; // permuted LCP problem data
743 dReal *L,*d; // L*D*L' factorization of set C
744 dReal *Dell,*ell,*tmp;
745 int *state,*findex,*p,*C;
746 int nC,nN; // size of each index set
747
748 dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
749 dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
750 dReal *_Dell, dReal *_ell, dReal *_tmp,
751 int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
752 int getNub() { return nub; }
753 void transfer_i_to_C (int i);
754 void transfer_i_to_N (int i)
755 { nN++; } // because we can assume C and N span 1:i-1
756 void transfer_i_from_N_to_C (int i);
757 void transfer_i_from_C_to_N (int i);
758 int numC() { return nC; }
759 int numN() { return nN; }
760 int indexC (int i) { return i; }
761 int indexN (int i) { return i+nC; }
762 dReal Aii (int i) { return AROW(i)[i]; }
763 dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); }
764 dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); }
765 void pN_equals_ANC_times_qC (dReal *p, dReal *q);
766 void pN_plusequals_ANi (dReal *p, int i, int sign=1);
767 void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
768 { for (int i=0; i<nC; i++) p[i] += s*q[i]; }
769 void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
770 { for (int i=0; i<nN; i++) p[i+nC] += s*q[i+nC]; }
771 void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
772 void unpermute();
773};
774
775
776dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
777 dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
778 dReal *_Dell, dReal *_ell, dReal *_tmp,
779 int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
780{
781 n = _n;
782 nub = _nub;
783 Adata = _Adata;
784 A = 0;
785 x = _x;
786 b = _b;
787 w = _w;
788 lo = _lo;
789 hi = _hi;
790 L = _L;
791 d = _d;
792 Dell = _Dell;
793 ell = _ell;
794 tmp = _tmp;
795 state = _state;
796 findex = _findex;
797 p = _p;
798 C = _C;
799 nskip = dPAD(n);
800 dSetZero (x,n);
801
802 int k;
803
804# ifdef ROWPTRS
805 // make matrix row pointers
806 A = Arows;
807 for (k=0; k<n; k++) A[k] = Adata + k*nskip;
808# else
809 A = Adata;
810# endif
811
812 nC = 0;
813 nN = 0;
814 for (k=0; k<n; k++) p[k]=k; // initially unpermuted
815
816 /*
817 // for testing, we can do some random swaps in the area i > nub
818 if (nub < n) {
819 for (k=0; k<100; k++) {
820 int i1,i2;
821 do {
822 i1 = dRandInt(n-nub)+nub;
823 i2 = dRandInt(n-nub)+nub;
824 }
825 while (i1 > i2);
826 //printf ("--> %d %d\n",i1,i2);
827 swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0);
828 }
829 }
830 */
831
832 // permute the problem so that *all* the unbounded variables are at the
833 // start, i.e. look for unbounded variables not included in `nub'. we can
834 // potentially push up `nub' this way and get a bigger initial factorization.
835 // note that when we swap rows/cols here we must not just swap row pointers,
836 // as the initial factorization relies on the data being all in one chunk.
837 // variables that have findex >= 0 are *not* considered to be unbounded even
838 // if lo=-inf and hi=inf - this is because these limits may change during the
839 // solution process.
840
841 for (k=nub; k<n; k++) {
842 if (findex && findex[k] >= 0) continue;
843 if (lo[k]==-dInfinity && hi[k]==dInfinity) {
844 swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0);
845 nub++;
846 }
847 }
848
849 // if there are unbounded variables at the start, factorize A up to that
850 // point and solve for x. this puts all indexes 0..nub-1 into C.
851 if (nub > 0) {
852 for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal));
853 dFactorLDLT (L,d,nub,nskip);
854 memcpy (x,b,nub*sizeof(dReal));
855 dSolveLDLT (L,d,x,nub,nskip);
856 dSetZero (w,nub);
857 for (k=0; k<nub; k++) C[k] = k;
858 nC = nub;
859 }
860
861 // permute the indexes > nub such that all findex variables are at the end
862 if (findex) {
863 int num_at_end = 0;
864 for (k=n-1; k >= nub; k--) {
865 if (findex[k] >= 0) {
866 swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1);
867 num_at_end++;
868 }
869 }
870 }
871
872 // print info about indexes
873 /*
874 for (k=0; k<n; k++) {
875 if (k<nub) printf ("C");
876 else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c");
877 else printf (".");
878 }
879 printf ("\n");
880 */
881}
882
883
884void dLCP::transfer_i_to_C (int i)
885{
886 int j;
887 if (nC > 0) {
888 // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
889 for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
890 d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
891 }
892 else {
893 d[0] = dRecip (AROW(i)[i]);
894 }
895 swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
896 C[nC] = nC;
897 nC++;
898
899# ifdef DEBUG_LCP
900 checkFactorization (A,L,d,nC,C,nskip);
901 if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C);
902# endif
903}
904
905
906void dLCP::transfer_i_from_N_to_C (int i)
907{
908 int j;
909 if (nC > 0) {
910 dReal *aptr = AROW(i);
911# ifdef NUB_OPTIMIZATIONS
912 // if nub>0, initial part of aptr unpermuted
913 for (j=0; j<nub; j++) Dell[j] = aptr[j];
914 for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
915# else
916 for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
917# endif
918 dSolveL1 (L,Dell,nC,nskip);
919 for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
920 for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
921 d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
922 }
923 else {
924 d[0] = dRecip (AROW(i)[i]);
925 }
926 swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
927 C[nC] = nC;
928 nN--;
929 nC++;
930
931 // @@@ TO DO LATER
932 // if we just finish here then we'll go back and re-solve for
933 // delta_x. but actually we can be more efficient and incrementally
934 // update delta_x here. but if we do this, we wont have ell and Dell
935 // to use in updating the factorization later.
936
937# ifdef DEBUG_LCP
938 checkFactorization (A,L,d,nC,C,nskip);
939# endif
940}
941
942
943void dLCP::transfer_i_from_C_to_N (int i)
944{
945 // remove a row/column from the factorization, and adjust the
946 // indexes (black magic!)
947 int j,k;
948 for (j=0; j<nC; j++) if (C[j]==i) {
949 dLDLTRemove (A,C,L,d,n,nC,j,nskip);
950 for (k=0; k<nC; k++) if (C[k]==nC-1) {
951 C[k] = C[j];
952 if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
953 break;
954 }
955 dIASSERT (k < nC);
956 break;
957 }
958 dIASSERT (j < nC);
959 swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);
960 nC--;
961 nN++;
962
963# ifdef DEBUG_LCP
964 checkFactorization (A,L,d,nC,C,nskip);
965# endif
966}
967
968
969void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
970{
971 // we could try to make this matrix-vector multiplication faster using
972 // outer product matrix tricks, e.g. with the dMultidotX() functions.
973 // but i tried it and it actually made things slower on random 100x100
974 // problems because of the overhead involved. so we'll stick with the
975 // simple method for now.
976 for (int i=0; i<nN; i++) p[i+nC] = dDot (AROW(i+nC),q,nC);
977}
978
979
980void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
981{
982 dReal *aptr = AROW(i)+nC;
983 if (sign > 0) {
984 for (int i=0; i<nN; i++) p[i+nC] += aptr[i];
985 }
986 else {
987 for (int i=0; i<nN; i++) p[i+nC] -= aptr[i];
988 }
989}
990
991
992void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
993{
994 // the `Dell' and `ell' that are computed here are saved. if index i is
995 // later added to the factorization then they can be reused.
996 //
997 // @@@ question: do we need to solve for entire delta_x??? yes, but
998 // only if an x goes below 0 during the step.
999
1000 int j;
1001 if (nC > 0) {
1002 dReal *aptr = AROW(i);
1003# ifdef NUB_OPTIMIZATIONS
1004 // if nub>0, initial part of aptr[] is guaranteed unpermuted
1005 for (j=0; j<nub; j++) Dell[j] = aptr[j];
1006 for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
1007# else
1008 for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
1009# endif
1010 dSolveL1 (L,Dell,nC,nskip);
1011 for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
1012
1013 if (!only_transfer) {
1014 for (j=0; j<nC; j++) tmp[j] = ell[j];
1015 dSolveL1T (L,tmp,nC,nskip);
1016 if (dir > 0) {
1017 for (j=0; j<nC; j++) a[C[j]] = -tmp[j];
1018 }
1019 else {
1020 for (j=0; j<nC; j++) a[C[j]] = tmp[j];
1021 }
1022 }
1023 }
1024}
1025
1026
1027void dLCP::unpermute()
1028{
1029 // now we have to un-permute x and w
1030 int j;
1031 ALLOCA (dReal,tmp,n*sizeof(dReal));
1032#ifdef dUSE_MALLOC_FOR_ALLOCA
1033 if (tmp == NULL) {
1034 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1035 return;
1036 }
1037#endif
1038 memcpy (tmp,x,n*sizeof(dReal));
1039 for (j=0; j<n; j++) x[p[j]] = tmp[j];
1040 memcpy (tmp,w,n*sizeof(dReal));
1041 for (j=0; j<n; j++) w[p[j]] = tmp[j];
1042
1043 UNALLOCA (tmp);
1044}
1045
1046#endif // dLCP_FAST
1047
1048//***************************************************************************
1049// an unoptimized Dantzig LCP driver routine for the basic LCP problem.
1050// must have lo=0, hi=dInfinity, and nub=0.
1051
1052void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
1053 dReal *w, int nub, dReal *lo, dReal *hi)
1054{
1055 dAASSERT (n>0 && A && x && b && w && nub == 0);
1056
1057 int i,k;
1058 int nskip = dPAD(n);
1059 ALLOCA (dReal,L,n*nskip*sizeof(dReal));
1060#ifdef dUSE_MALLOC_FOR_ALLOCA
1061 if (L == NULL) {
1062 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1063 return;
1064 }
1065#endif
1066 ALLOCA (dReal,d,n*sizeof(dReal));
1067#ifdef dUSE_MALLOC_FOR_ALLOCA
1068 if (d == NULL) {
1069 UNALLOCA(L);
1070 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1071 return;
1072 }
1073#endif
1074 ALLOCA (dReal,delta_x,n*sizeof(dReal));
1075#ifdef dUSE_MALLOC_FOR_ALLOCA
1076 if (delta_x == NULL) {
1077 UNALLOCA(d);
1078 UNALLOCA(L);
1079 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1080 return;
1081 }
1082#endif
1083 ALLOCA (dReal,delta_w,n*sizeof(dReal));
1084#ifdef dUSE_MALLOC_FOR_ALLOCA
1085 if (delta_w == NULL) {
1086 UNALLOCA(delta_x);
1087 UNALLOCA(d);
1088 UNALLOCA(L);
1089 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1090 return;
1091 }
1092#endif
1093 ALLOCA (dReal,Dell,n*sizeof(dReal));
1094#ifdef dUSE_MALLOC_FOR_ALLOCA
1095 if (Dell == NULL) {
1096 UNALLOCA(delta_w);
1097 UNALLOCA(delta_x);
1098 UNALLOCA(d);
1099 UNALLOCA(L);
1100 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1101 return;
1102 }
1103#endif
1104 ALLOCA (dReal,ell,n*sizeof(dReal));
1105#ifdef dUSE_MALLOC_FOR_ALLOCA
1106 if (ell == NULL) {
1107 UNALLOCA(Dell);
1108 UNALLOCA(delta_w);
1109 UNALLOCA(delta_x);
1110 UNALLOCA(d);
1111 UNALLOCA(L);
1112 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1113 return;
1114 }
1115#endif
1116 ALLOCA (dReal,tmp,n*sizeof(dReal));
1117#ifdef dUSE_MALLOC_FOR_ALLOCA
1118 if (tmp == NULL) {
1119 UNALLOCA(ell);
1120 UNALLOCA(Dell);
1121 UNALLOCA(delta_w);
1122 UNALLOCA(delta_x);
1123 UNALLOCA(d);
1124 UNALLOCA(L);
1125 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1126 return;
1127 }
1128#endif
1129 ALLOCA (dReal*,Arows,n*sizeof(dReal*));
1130#ifdef dUSE_MALLOC_FOR_ALLOCA
1131 if (Arows == NULL) {
1132 UNALLOCA(tmp);
1133 UNALLOCA(ell);
1134 UNALLOCA(Dell);
1135 UNALLOCA(delta_w);
1136 UNALLOCA(delta_x);
1137 UNALLOCA(d);
1138 UNALLOCA(L);
1139 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1140 return;
1141 }
1142#endif
1143 ALLOCA (int,p,n*sizeof(int));
1144#ifdef dUSE_MALLOC_FOR_ALLOCA
1145 if (p == NULL) {
1146 UNALLOCA(Arows);
1147 UNALLOCA(tmp);
1148 UNALLOCA(ell);
1149 UNALLOCA(Dell);
1150 UNALLOCA(delta_w);
1151 UNALLOCA(delta_x);
1152 UNALLOCA(d);
1153 UNALLOCA(L);
1154 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1155 return;
1156 }
1157#endif
1158 ALLOCA (int,C,n*sizeof(int));
1159#ifdef dUSE_MALLOC_FOR_ALLOCA
1160 if (C == NULL) {
1161 UNALLOCA(p);
1162 UNALLOCA(Arows);
1163 UNALLOCA(tmp);
1164 UNALLOCA(ell);
1165 UNALLOCA(Dell);
1166 UNALLOCA(delta_w);
1167 UNALLOCA(delta_x);
1168 UNALLOCA(d);
1169 UNALLOCA(L);
1170 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1171 return;
1172 }
1173#endif
1174 ALLOCA (int,dummy,n*sizeof(int));
1175#ifdef dUSE_MALLOC_FOR_ALLOCA
1176 if (dummy == NULL) {
1177 UNALLOCA(C);
1178 UNALLOCA(p);
1179 UNALLOCA(Arows);
1180 UNALLOCA(tmp);
1181 UNALLOCA(ell);
1182 UNALLOCA(Dell);
1183 UNALLOCA(delta_w);
1184 UNALLOCA(delta_x);
1185 UNALLOCA(d);
1186 UNALLOCA(L);
1187 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1188 return;
1189 }
1190#endif
1191
1192
1193 dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
1194 nub = lcp.getNub();
1195
1196 for (i=0; i<n; i++) {
1197 w[i] = lcp.AiC_times_qC (i,x) - b[i];
1198 if (w[i] >= 0) {
1199 lcp.transfer_i_to_N (i);
1200 }
1201 else {
1202 for (;;) {
1203 // compute: delta_x(C) = -A(C,C)\A(C,i)
1204 dSetZero (delta_x,n);
1205 lcp.solve1 (delta_x,i);
1206#ifdef dUSE_MALLOC_FOR_ALLOCA
1207 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1208 UNALLOCA(dummy);
1209 UNALLOCA(C);
1210 UNALLOCA(p);
1211 UNALLOCA(Arows);
1212 UNALLOCA(tmp);
1213 UNALLOCA(ell);
1214 UNALLOCA(Dell);
1215 UNALLOCA(delta_w);
1216 UNALLOCA(delta_x);
1217 UNALLOCA(d);
1218 UNALLOCA(L);
1219 return;
1220 }
1221#endif
1222 delta_x[i] = 1;
1223
1224 // compute: delta_w = A*delta_x
1225 dSetZero (delta_w,n);
1226 lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
1227 lcp.pN_plusequals_ANi (delta_w,i);
1228 delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
1229
1230 // find index to switch
1231 int si = i; // si = switch index
1232 int si_in_N = 0; // set to 1 if si in N
1233 dReal s = -w[i]/delta_w[i];
1234
1235 if (s <= 0) {
1236 dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
1237 if (i < (n-1)) {
1238 dSetZero (x+i,n-i);
1239 dSetZero (w+i,n-i);
1240 }
1241 goto done;
1242 }
1243
1244 for (k=0; k < lcp.numN(); k++) {
1245 if (delta_w[lcp.indexN(k)] < 0) {
1246 dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
1247 if (s2 < s) {
1248 s = s2;
1249 si = lcp.indexN(k);
1250 si_in_N = 1;
1251 }
1252 }
1253 }
1254 for (k=0; k < lcp.numC(); k++) {
1255 if (delta_x[lcp.indexC(k)] < 0) {
1256 dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
1257 if (s2 < s) {
1258 s = s2;
1259 si = lcp.indexC(k);
1260 si_in_N = 0;
1261 }
1262 }
1263 }
1264
1265 // apply x = x + s * delta_x
1266 lcp.pC_plusequals_s_times_qC (x,s,delta_x);
1267 x[i] += s;
1268 lcp.pN_plusequals_s_times_qN (w,s,delta_w);
1269 w[i] += s * delta_w[i];
1270
1271 // switch indexes between sets if necessary
1272 if (si==i) {
1273 w[i] = 0;
1274 lcp.transfer_i_to_C (i);
1275 break;
1276 }
1277 if (si_in_N) {
1278 w[si] = 0;
1279 lcp.transfer_i_from_N_to_C (si);
1280 }
1281 else {
1282 x[si] = 0;
1283 lcp.transfer_i_from_C_to_N (si);
1284 }
1285 }
1286 }
1287 }
1288
1289 done:
1290 lcp.unpermute();
1291
1292 UNALLOCA (L);
1293 UNALLOCA (d);
1294 UNALLOCA (delta_x);
1295 UNALLOCA (delta_w);
1296 UNALLOCA (Dell);
1297 UNALLOCA (ell);
1298 UNALLOCA (tmp);
1299 UNALLOCA (Arows);
1300 UNALLOCA (p);
1301 UNALLOCA (C);
1302 UNALLOCA (dummy);
1303}
1304
1305//***************************************************************************
1306// an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
1307
1308void dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
1309 dReal *w, int nub, dReal *lo, dReal *hi, int *findex)
1310{
1311 dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n);
1312
1313 int i,k,hit_first_friction_index = 0;
1314 int nskip = dPAD(n);
1315
1316 // if all the variables are unbounded then we can just factor, solve,
1317 // and return
1318 if (nub >= n) {
1319 dFactorLDLT (A,w,n,nskip); // use w for d
1320 dSolveLDLT (A,w,b,n,nskip);
1321 memcpy (x,b,n*sizeof(dReal));
1322 dSetZero (w,n);
1323
1324 return;
1325 }
1326# ifndef dNODEBUG
1327 // check restrictions on lo and hi
1328 for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
1329# endif
1330 ALLOCA (dReal,L,n*nskip*sizeof(dReal));
1331#ifdef dUSE_MALLOC_FOR_ALLOCA
1332 if (L == NULL) {
1333 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1334 return;
1335 }
1336#endif
1337 ALLOCA (dReal,d,n*sizeof(dReal));
1338#ifdef dUSE_MALLOC_FOR_ALLOCA
1339 if (d == NULL) {
1340 UNALLOCA(L);
1341 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1342 return;
1343 }
1344#endif
1345 ALLOCA (dReal,delta_x,n*sizeof(dReal));
1346#ifdef dUSE_MALLOC_FOR_ALLOCA
1347 if (delta_x == NULL) {
1348 UNALLOCA(d);
1349 UNALLOCA(L);
1350 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1351 return;
1352 }
1353#endif
1354 ALLOCA (dReal,delta_w,n*sizeof(dReal));
1355#ifdef dUSE_MALLOC_FOR_ALLOCA
1356 if (delta_w == NULL) {
1357 UNALLOCA(delta_x);
1358 UNALLOCA(d);
1359 UNALLOCA(L);
1360 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1361 return;
1362 }
1363#endif
1364 ALLOCA (dReal,Dell,n*sizeof(dReal));
1365#ifdef dUSE_MALLOC_FOR_ALLOCA
1366 if (Dell == NULL) {
1367 UNALLOCA(delta_w);
1368 UNALLOCA(delta_x);
1369 UNALLOCA(d);
1370 UNALLOCA(L);
1371 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1372 return;
1373 }
1374#endif
1375 ALLOCA (dReal,ell,n*sizeof(dReal));
1376#ifdef dUSE_MALLOC_FOR_ALLOCA
1377 if (ell == NULL) {
1378 UNALLOCA(Dell);
1379 UNALLOCA(delta_w);
1380 UNALLOCA(delta_x);
1381 UNALLOCA(d);
1382 UNALLOCA(L);
1383 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1384 return;
1385 }
1386#endif
1387 ALLOCA (dReal*,Arows,n*sizeof(dReal*));
1388#ifdef dUSE_MALLOC_FOR_ALLOCA
1389 if (Arows == NULL) {
1390 UNALLOCA(ell);
1391 UNALLOCA(Dell);
1392 UNALLOCA(delta_w);
1393 UNALLOCA(delta_x);
1394 UNALLOCA(d);
1395 UNALLOCA(L);
1396 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1397 return;
1398 }
1399#endif
1400 ALLOCA (int,p,n*sizeof(int));
1401#ifdef dUSE_MALLOC_FOR_ALLOCA
1402 if (p == NULL) {
1403 UNALLOCA(Arows);
1404 UNALLOCA(ell);
1405 UNALLOCA(Dell);
1406 UNALLOCA(delta_w);
1407 UNALLOCA(delta_x);
1408 UNALLOCA(d);
1409 UNALLOCA(L);
1410 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1411 return;
1412 }
1413#endif
1414 ALLOCA (int,C,n*sizeof(int));
1415#ifdef dUSE_MALLOC_FOR_ALLOCA
1416 if (C == NULL) {
1417 UNALLOCA(p);
1418 UNALLOCA(Arows);
1419 UNALLOCA(ell);
1420 UNALLOCA(Dell);
1421 UNALLOCA(delta_w);
1422 UNALLOCA(delta_x);
1423 UNALLOCA(d);
1424 UNALLOCA(L);
1425 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1426 return;
1427 }
1428#endif
1429
1430 int dir;
1431 dReal dirf;
1432
1433 // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
1434 ALLOCA (int,state,n*sizeof(int));
1435#ifdef dUSE_MALLOC_FOR_ALLOCA
1436 if (state == NULL) {
1437 UNALLOCA(C);
1438 UNALLOCA(p);
1439 UNALLOCA(Arows);
1440 UNALLOCA(ell);
1441 UNALLOCA(Dell);
1442 UNALLOCA(delta_w);
1443 UNALLOCA(delta_x);
1444 UNALLOCA(d);
1445 UNALLOCA(L);
1446 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1447 return;
1448 }
1449#endif
1450
1451 // create LCP object. note that tmp is set to delta_w to save space, this
1452 // optimization relies on knowledge of how tmp is used, so be careful!
1453 dLCP *lcp=new dLCP(n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
1454 nub = lcp->getNub();
1455
1456 // loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the
1457 // LCP conditions then i is added to the appropriate index set. otherwise
1458 // x(i),w(i) is driven either +ve or -ve to force it to the valid region.
1459 // as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
1460 // while driving x(i) we maintain the LCP conditions on the other variables
1461 // 0..i-1. we do this by watching out for other x(i),w(i) values going
1462 // outside the valid region, and then switching them between index sets
1463 // when that happens.
1464
1465 for (i=nub; i<n; i++) {
1466 // the index i is the driving index and indexes i+1..n-1 are "dont care",
1467 // i.e. when we make changes to the system those x's will be zero and we
1468 // don't care what happens to those w's. in other words, we only consider
1469 // an (i+1)*(i+1) sub-problem of A*x=b+w.
1470
1471 // if we've hit the first friction index, we have to compute the lo and
1472 // hi values based on the values of x already computed. we have been
1473 // permuting the indexes, so the values stored in the findex vector are
1474 // no longer valid. thus we have to temporarily unpermute the x vector.
1475 // for the purposes of this computation, 0*infinity = 0 ... so if the
1476 // contact constraint's normal force is 0, there should be no tangential
1477 // force applied.
1478
1479 if (hit_first_friction_index == 0 && findex && findex[i] >= 0) {
1480 // un-permute x into delta_w, which is not being used at the moment
1481 for (k=0; k<n; k++) delta_w[p[k]] = x[k];
1482
1483 // set lo and hi values
1484 for (k=i; k<n; k++) {
1485 dReal wfk = delta_w[findex[k]];
1486 if (wfk == 0) {
1487 hi[k] = 0;
1488 lo[k] = 0;
1489 }
1490 else {
1491 hi[k] = dFabs (hi[k] * wfk);
1492 lo[k] = -hi[k];
1493 }
1494 }
1495 hit_first_friction_index = 1;
1496 }
1497
1498 // thus far we have not even been computing the w values for indexes
1499 // greater than i, so compute w[i] now.
1500 w[i] = lcp->AiC_times_qC (i,x) + lcp->AiN_times_qN (i,x) - b[i];
1501
1502 // if lo=hi=0 (which can happen for tangential friction when normals are
1503 // 0) then the index will be assigned to set N with some state. however,
1504 // set C's line has zero size, so the index will always remain in set N.
1505 // with the "normal" switching logic, if w changed sign then the index
1506 // would have to switch to set C and then back to set N with an inverted
1507 // state. this is pointless, and also computationally expensive. to
1508 // prevent this from happening, we use the rule that indexes with lo=hi=0
1509 // will never be checked for set changes. this means that the state for
1510 // these indexes may be incorrect, but that doesn't matter.
1511
1512 // see if x(i),w(i) is in a valid region
1513 if (lo[i]==0 && w[i] >= 0) {
1514 lcp->transfer_i_to_N (i);
1515 state[i] = 0;
1516 }
1517 else if (hi[i]==0 && w[i] <= 0) {
1518 lcp->transfer_i_to_N (i);
1519 state[i] = 1;
1520 }
1521 else if (w[i]==0) {
1522 // this is a degenerate case. by the time we get to this test we know
1523 // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
1524 // and similarly that hi > 0. this means that the line segment
1525 // corresponding to set C is at least finite in extent, and we are on it.
1526 // NOTE: we must call lcp->solve1() before lcp->transfer_i_to_C()
1527 lcp->solve1 (delta_x,i,0,1);
1528
1529#ifdef dUSE_MALLOC_FOR_ALLOCA
1530 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1531 UNALLOCA(state);
1532 UNALLOCA(C);
1533 UNALLOCA(p);
1534 UNALLOCA(Arows);
1535 UNALLOCA(ell);
1536 UNALLOCA(Dell);
1537 UNALLOCA(delta_w);
1538 UNALLOCA(delta_x);
1539 UNALLOCA(d);
1540 UNALLOCA(L);
1541 return;
1542 }
1543#endif
1544
1545 lcp->transfer_i_to_C (i);
1546 }
1547 else {
1548 // we must push x(i) and w(i)
1549 for (;;) {
1550 // find direction to push on x(i)
1551 if (w[i] <= 0) {
1552 dir = 1;
1553 dirf = REAL(1.0);
1554 }
1555 else {
1556 dir = -1;
1557 dirf = REAL(-1.0);
1558 }
1559
1560 // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
1561 lcp->solve1 (delta_x,i,dir);
1562
1563#ifdef dUSE_MALLOC_FOR_ALLOCA
1564 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1565 UNALLOCA(state);
1566 UNALLOCA(C);
1567 UNALLOCA(p);
1568 UNALLOCA(Arows);
1569 UNALLOCA(ell);
1570 UNALLOCA(Dell);
1571 UNALLOCA(delta_w);
1572 UNALLOCA(delta_x);
1573 UNALLOCA(d);
1574 UNALLOCA(L);
1575 return;
1576 }
1577#endif
1578
1579 // note that delta_x[i] = dirf, but we wont bother to set it
1580
1581 // compute: delta_w = A*delta_x ... note we only care about
1582 // delta_w(N) and delta_w(i), the rest is ignored
1583 lcp->pN_equals_ANC_times_qC (delta_w,delta_x);
1584 lcp->pN_plusequals_ANi (delta_w,i,dir);
1585 delta_w[i] = lcp->AiC_times_qC (i,delta_x) + lcp->Aii(i)*dirf;
1586
1587 // find largest step we can take (size=s), either to drive x(i),w(i)
1588 // to the valid LCP region or to drive an already-valid variable
1589 // outside the valid region.
1590
1591 int cmd = 1; // index switching command
1592 int si = 0; // si = index to switch if cmd>3
1593 dReal s = -w[i]/delta_w[i];
1594 if (dir > 0) {
1595 if (hi[i] < dInfinity) {
1596 dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i)
1597 if (s2 < s) {
1598 s = s2;
1599 cmd = 3;
1600 }
1601 }
1602 }
1603 else {
1604 if (lo[i] > -dInfinity) {
1605 dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i)
1606 if (s2 < s) {
1607 s = s2;
1608 cmd = 2;
1609 }
1610 }
1611 }
1612
1613 for (k=0; k < lcp->numN(); k++) {
1614 if ((state[lcp->indexN(k)]==0 && delta_w[lcp->indexN(k)] < 0) ||
1615 (state[lcp->indexN(k)]!=0 && delta_w[lcp->indexN(k)] > 0)) {
1616 // don't bother checking if lo=hi=0
1617 if (lo[lcp->indexN(k)] == 0 && hi[lcp->indexN(k)] == 0) continue;
1618 dReal s2 = -w[lcp->indexN(k)] / delta_w[lcp->indexN(k)];
1619 if (s2 < s) {
1620 s = s2;
1621 cmd = 4;
1622 si = lcp->indexN(k);
1623 }
1624 }
1625 }
1626
1627 for (k=nub; k < lcp->numC(); k++) {
1628 if (delta_x[lcp->indexC(k)] < 0 && lo[lcp->indexC(k)] > -dInfinity) {
1629 dReal s2 = (lo[lcp->indexC(k)]-x[lcp->indexC(k)]) /
1630 delta_x[lcp->indexC(k)];
1631 if (s2 < s) {
1632 s = s2;
1633 cmd = 5;
1634 si = lcp->indexC(k);
1635 }
1636 }
1637 if (delta_x[lcp->indexC(k)] > 0 && hi[lcp->indexC(k)] < dInfinity) {
1638 dReal s2 = (hi[lcp->indexC(k)]-x[lcp->indexC(k)]) /
1639 delta_x[lcp->indexC(k)];
1640 if (s2 < s) {
1641 s = s2;
1642 cmd = 6;
1643 si = lcp->indexC(k);
1644 }
1645 }
1646 }
1647
1648 //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
1649 // "C->NL","C->NH"};
1650 //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
1651
1652 // if s <= 0 then we've got a problem. if we just keep going then
1653 // we're going to get stuck in an infinite loop. instead, just cross
1654 // our fingers and exit with the current solution.
1655 if (s <= 0) {
1656 dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
1657 if (i < (n-1)) {
1658 dSetZero (x+i,n-i);
1659 dSetZero (w+i,n-i);
1660 }
1661 goto done;
1662 }
1663
1664 // apply x = x + s * delta_x
1665 lcp->pC_plusequals_s_times_qC (x,s,delta_x);
1666 x[i] += s * dirf;
1667
1668 // apply w = w + s * delta_w
1669 lcp->pN_plusequals_s_times_qN (w,s,delta_w);
1670 w[i] += s * delta_w[i];
1671
1672 // switch indexes between sets if necessary
1673 switch (cmd) {
1674 case 1: // done
1675 w[i] = 0;
1676 lcp->transfer_i_to_C (i);
1677 break;
1678 case 2: // done
1679 x[i] = lo[i];
1680 state[i] = 0;
1681 lcp->transfer_i_to_N (i);
1682 break;
1683 case 3: // done
1684 x[i] = hi[i];
1685 state[i] = 1;
1686 lcp->transfer_i_to_N (i);
1687 break;
1688 case 4: // keep going
1689 w[si] = 0;
1690 lcp->transfer_i_from_N_to_C (si);
1691 break;
1692 case 5: // keep going
1693 x[si] = lo[si];
1694 state[si] = 0;
1695 lcp->transfer_i_from_C_to_N (si);
1696 break;
1697 case 6: // keep going
1698 x[si] = hi[si];
1699 state[si] = 1;
1700 lcp->transfer_i_from_C_to_N (si);
1701 break;
1702 }
1703
1704 if (cmd <= 3) break;
1705 }
1706 }
1707 }
1708
1709 done:
1710 lcp->unpermute();
1711 delete lcp;
1712
1713 UNALLOCA (L);
1714 UNALLOCA (d);
1715 UNALLOCA (delta_x);
1716 UNALLOCA (delta_w);
1717 UNALLOCA (Dell);
1718 UNALLOCA (ell);
1719 UNALLOCA (Arows);
1720 UNALLOCA (p);
1721 UNALLOCA (C);
1722 UNALLOCA (state);
1723}
1724
1725//***************************************************************************
1726// accuracy and timing test
1727
1728extern "C" ODE_API void dTestSolveLCP()
1729{
1730 int n = 100;
1731 int i,nskip = dPAD(n);
1732#ifdef dDOUBLE
1733 const dReal tol = REAL(1e-9);
1734#endif
1735#ifdef dSINGLE
1736 const dReal tol = REAL(1e-4);
1737#endif
1738 printf ("dTestSolveLCP()\n");
1739
1740 ALLOCA (dReal,A,n*nskip*sizeof(dReal));
1741#ifdef dUSE_MALLOC_FOR_ALLOCA
1742 if (A == NULL) {
1743 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1744 return;
1745 }
1746#endif
1747 ALLOCA (dReal,x,n*sizeof(dReal));
1748#ifdef dUSE_MALLOC_FOR_ALLOCA
1749 if (x == NULL) {
1750 UNALLOCA (A);
1751 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1752 return;
1753 }
1754#endif
1755 ALLOCA (dReal,b,n*sizeof(dReal));
1756#ifdef dUSE_MALLOC_FOR_ALLOCA
1757 if (b == NULL) {
1758 UNALLOCA (x);
1759 UNALLOCA (A);
1760 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1761 return;
1762 }
1763#endif
1764 ALLOCA (dReal,w,n*sizeof(dReal));
1765#ifdef dUSE_MALLOC_FOR_ALLOCA
1766 if (w == NULL) {
1767 UNALLOCA (b);
1768 UNALLOCA (x);
1769 UNALLOCA (A);
1770 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1771 return;
1772 }
1773#endif
1774 ALLOCA (dReal,lo,n*sizeof(dReal));
1775#ifdef dUSE_MALLOC_FOR_ALLOCA
1776 if (lo == NULL) {
1777 UNALLOCA (w);
1778 UNALLOCA (b);
1779 UNALLOCA (x);
1780 UNALLOCA (A);
1781 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1782 return;
1783 }
1784#endif
1785 ALLOCA (dReal,hi,n*sizeof(dReal));
1786#ifdef dUSE_MALLOC_FOR_ALLOCA
1787 if (hi == NULL) {
1788 UNALLOCA (lo);
1789 UNALLOCA (w);
1790 UNALLOCA (b);
1791 UNALLOCA (x);
1792 UNALLOCA (A);
1793 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1794 return;
1795 }
1796#endif
1797
1798 ALLOCA (dReal,A2,n*nskip*sizeof(dReal));
1799#ifdef dUSE_MALLOC_FOR_ALLOCA
1800 if (A2 == NULL) {
1801 UNALLOCA (hi);
1802 UNALLOCA (lo);
1803 UNALLOCA (w);
1804 UNALLOCA (b);
1805 UNALLOCA (x);
1806 UNALLOCA (A);
1807 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1808 return;
1809 }
1810#endif
1811 ALLOCA (dReal,b2,n*sizeof(dReal));
1812#ifdef dUSE_MALLOC_FOR_ALLOCA
1813 if (b2 == NULL) {
1814 UNALLOCA (A2);
1815 UNALLOCA (hi);
1816 UNALLOCA (lo);
1817 UNALLOCA (w);
1818 UNALLOCA (b);
1819 UNALLOCA (x);
1820 UNALLOCA (A);
1821 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1822 return;
1823 }
1824#endif
1825 ALLOCA (dReal,lo2,n*sizeof(dReal));
1826#ifdef dUSE_MALLOC_FOR_ALLOCA
1827 if (lo2 == NULL) {
1828 UNALLOCA (b2);
1829 UNALLOCA (A2);
1830 UNALLOCA (hi);
1831 UNALLOCA (lo);
1832 UNALLOCA (w);
1833 UNALLOCA (b);
1834 UNALLOCA (x);
1835 UNALLOCA (A);
1836 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1837 return;
1838 }
1839#endif
1840 ALLOCA (dReal,hi2,n*sizeof(dReal));
1841#ifdef dUSE_MALLOC_FOR_ALLOCA
1842 if (hi2 == NULL) {
1843 UNALLOCA (lo2);
1844 UNALLOCA (b2);
1845 UNALLOCA (A2);
1846 UNALLOCA (hi);
1847 UNALLOCA (lo);
1848 UNALLOCA (w);
1849 UNALLOCA (b);
1850 UNALLOCA (x);
1851 UNALLOCA (A);
1852 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1853 return;
1854 }
1855#endif
1856 ALLOCA (dReal,tmp1,n*sizeof(dReal));
1857#ifdef dUSE_MALLOC_FOR_ALLOCA
1858 if (tmp1 == NULL) {
1859 UNALLOCA (hi2);
1860 UNALLOCA (lo2);
1861 UNALLOCA (b2);
1862 UNALLOCA (A2);
1863 UNALLOCA (hi);
1864 UNALLOCA (lo);
1865 UNALLOCA (w);
1866 UNALLOCA (b);
1867 UNALLOCA (x);
1868 UNALLOCA (A);
1869 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1870 return;
1871 }
1872#endif
1873 ALLOCA (dReal,tmp2,n*sizeof(dReal));
1874#ifdef dUSE_MALLOC_FOR_ALLOCA
1875 if (tmp2 == NULL) {
1876 UNALLOCA (tmp1);
1877 UNALLOCA (hi2);
1878 UNALLOCA (lo2);
1879 UNALLOCA (b2);
1880 UNALLOCA (A2);
1881 UNALLOCA (hi);
1882 UNALLOCA (lo);
1883 UNALLOCA (w);
1884 UNALLOCA (b);
1885 UNALLOCA (x);
1886 UNALLOCA (A);
1887 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1888 return;
1889 }
1890#endif
1891
1892 double total_time = 0;
1893 for (int count=0; count < 1000; count++) {
1894
1895 // form (A,b) = a random positive definite LCP problem
1896 dMakeRandomMatrix (A2,n,n,1.0);
1897 dMultiply2 (A,A2,A2,n,n,n);
1898 dMakeRandomMatrix (x,n,1,1.0);
1899 dMultiply0 (b,A,x,n,n,1);
1900 for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
1901
1902 // choose `nub' in the range 0..n-1
1903 int nub = 50; //dRandInt (n);
1904
1905 // make limits
1906 for (i=0; i<nub; i++) lo[i] = -dInfinity;
1907 for (i=0; i<nub; i++) hi[i] = dInfinity;
1908 //for (i=nub; i<n; i++) lo[i] = 0;
1909 //for (i=nub; i<n; i++) hi[i] = dInfinity;
1910 //for (i=nub; i<n; i++) lo[i] = -dInfinity;
1911 //for (i=nub; i<n; i++) hi[i] = 0;
1912 for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
1913 for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
1914
1915 // set a few limits to lo=hi=0
1916 /*
1917 for (i=0; i<10; i++) {
1918 int j = dRandInt (n-nub) + nub;
1919 lo[j] = 0;
1920 hi[j] = 0;
1921 }
1922 */
1923
1924 // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
1925 // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
1926 // ensure that it doesn't get referenced (if it does, the answer will be
1927 // wrong).
1928
1929 memcpy (A2,A,n*nskip*sizeof(dReal));
1930 dClearUpperTriangle (A2,n);
1931 memcpy (b2,b,n*sizeof(dReal));
1932 memcpy (lo2,lo,n*sizeof(dReal));
1933 memcpy (hi2,hi,n*sizeof(dReal));
1934 dSetZero (x,n);
1935 dSetZero (w,n);
1936
1937 dStopwatch sw;
1938 dStopwatchReset (&sw);
1939 dStopwatchStart (&sw);
1940
1941 dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);
1942#ifdef dUSE_MALLOC_FOR_ALLOCA
1943 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1944 UNALLOCA (tmp2);
1945 UNALLOCA (tmp1);
1946 UNALLOCA (hi2);
1947 UNALLOCA (lo2);
1948 UNALLOCA (b2);
1949 UNALLOCA (A2);
1950 UNALLOCA (hi);
1951 UNALLOCA (lo);
1952 UNALLOCA (w);
1953 UNALLOCA (b);
1954 UNALLOCA (x);
1955 UNALLOCA (A);
1956 return;
1957 }
1958#endif
1959
1960 dStopwatchStop (&sw);
1961 double time = dStopwatchTime(&sw);
1962 total_time += time;
1963 double average = total_time / double(count+1) * 1000.0;
1964
1965 // check the solution
1966
1967 dMultiply0 (tmp1,A,x,n,n,1);
1968 for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
1969 dReal diff = dMaxDifference (tmp1,tmp2,n,1);
1970 // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
1971 // diff > tol ? "FAILED" : "passed");
1972 if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
1973 int n1=0,n2=0,n3=0;
1974 for (i=0; i<n; i++) {
1975 if (x[i]==lo[i] && w[i] >= 0) {
1976 n1++; // ok
1977 }
1978 else if (x[i]==hi[i] && w[i] <= 0) {
1979 n2++; // ok
1980 }
1981 else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
1982 n3++; // ok
1983 }
1984 else {
1985 dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
1986 x[i],w[i],lo[i],hi[i]);
1987 }
1988 }
1989
1990 // pacifier
1991 printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
1992 printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
1993 }
1994
1995 UNALLOCA (A);
1996 UNALLOCA (x);
1997 UNALLOCA (b);
1998 UNALLOCA (w);
1999 UNALLOCA (lo);
2000 UNALLOCA (hi);
2001 UNALLOCA (A2);
2002 UNALLOCA (b2);
2003 UNALLOCA (lo2);
2004 UNALLOCA (hi2);
2005 UNALLOCA (tmp1);
2006 UNALLOCA (tmp2);
2007}
diff --git a/libraries/ode-0.9/ode/src/lcp.h b/libraries/ode-0.9/ode/src/lcp.h
new file mode 100644
index 0000000..484902c
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/lcp.h
@@ -0,0 +1,58 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
26satisfies one of
27 (1) x = lo, w >= 0
28 (2) x = hi, w <= 0
29 (3) lo < x < hi, w = 0
30A is a matrix of dimension n*n, everything else is a vector of size n*1.
31lo and hi can be +/- dInfinity as needed. the first `nub' variables are
32unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
33
34we restrict lo(i) <= 0 and hi(i) >= 0.
35
36the original data (A,b) may be modified by this function.
37
38if the `findex' (friction index) parameter is nonzero, it points to an array
39of index values. in this case constraints that have findex[i] >= 0 are
40special. all non-special constraints are solved for, then the lo and hi values
41for the special constraints are set:
42 hi[i] = abs( hi[i] * x[findex[i]] )
43 lo[i] = -hi[i]
44and the solution continues. this mechanism allows a friction approximation
45to be implemented. the first `nub' variables are assumed to have findex < 0.
46
47*/
48
49
50#ifndef _ODE_LCP_H_
51#define _ODE_LCP_H_
52
53
54void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w,
55 int nub, dReal *lo, dReal *hi, int *findex);
56
57
58#endif
diff --git a/libraries/ode-0.9/ode/src/mass.cpp b/libraries/ode-0.9/ode/src/mass.cpp
new file mode 100644
index 0000000..7d0b1c2
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/mass.cpp
@@ -0,0 +1,529 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/mass.h>
25#include <ode/odemath.h>
26#include <ode/matrix.h>
27
28// Local dependencies
29#include "collision_kernel.h"
30
31#define SQR(x) ((x)*(x)) //!< Returns x square
32#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
33
34#define _I(i,j) I[(i)*4+(j)]
35
36
37// return 1 if ok, 0 if bad
38
39int dMassCheck (const dMass *m)
40{
41 int i;
42
43 if (m->mass <= 0) {
44 dDEBUGMSG ("mass must be > 0");
45 return 0;
46 }
47 if (!dIsPositiveDefinite (m->I,3)) {
48 dDEBUGMSG ("inertia must be positive definite");
49 return 0;
50 }
51
52 // verify that the center of mass position is consistent with the mass
53 // and inertia matrix. this is done by checking that the inertia around
54 // the center of mass is also positive definite. from the comment in
55 // dMassTranslate(), if the body is translated so that its center of mass
56 // is at the point of reference, then the new inertia is:
57 // I + mass*crossmat(c)^2
58 // note that requiring this to be positive definite is exactly equivalent
59 // to requiring that the spatial inertia matrix
60 // [ mass*eye(3,3) M*crossmat(c)^T ]
61 // [ M*crossmat(c) I ]
62 // is positive definite, given that I is PD and mass>0. see the theorem
63 // about partitioned PD matrices for proof.
64
65 dMatrix3 I2,chat;
66 dSetZero (chat,12);
67 dCROSSMAT (chat,m->c,4,+,-);
68 dMULTIPLY0_333 (I2,chat,chat);
69 for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i];
70 for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i];
71 for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i];
72 if (!dIsPositiveDefinite (I2,3)) {
73 dDEBUGMSG ("center of mass inconsistent with mass parameters");
74 return 0;
75 }
76 return 1;
77}
78
79
80void dMassSetZero (dMass *m)
81{
82 dAASSERT (m);
83 m->mass = REAL(0.0);
84 dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
85 dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
86}
87
88
89void dMassSetParameters (dMass *m, dReal themass,
90 dReal cgx, dReal cgy, dReal cgz,
91 dReal I11, dReal I22, dReal I33,
92 dReal I12, dReal I13, dReal I23)
93{
94 dAASSERT (m);
95 dMassSetZero (m);
96 m->mass = themass;
97 m->c[0] = cgx;
98 m->c[1] = cgy;
99 m->c[2] = cgz;
100 m->_I(0,0) = I11;
101 m->_I(1,1) = I22;
102 m->_I(2,2) = I33;
103 m->_I(0,1) = I12;
104 m->_I(0,2) = I13;
105 m->_I(1,2) = I23;
106 m->_I(1,0) = I12;
107 m->_I(2,0) = I13;
108 m->_I(2,1) = I23;
109 dMassCheck (m);
110}
111
112
113void dMassSetSphere (dMass *m, dReal density, dReal radius)
114{
115 dMassSetSphereTotal (m, (REAL(4.0)/REAL(3.0)) * M_PI *
116 radius*radius*radius * density, radius);
117}
118
119
120void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius)
121{
122 dAASSERT (m);
123 dMassSetZero (m);
124 m->mass = total_mass;
125 dReal II = REAL(0.4) * total_mass * radius*radius;
126 m->_I(0,0) = II;
127 m->_I(1,1) = II;
128 m->_I(2,2) = II;
129
130# ifndef dNODEBUG
131 dMassCheck (m);
132# endif
133}
134
135
136void dMassSetCapsule (dMass *m, dReal density, int direction,
137 dReal radius, dReal length)
138{
139 dReal M1,M2,Ia,Ib;
140 dAASSERT (m);
141 dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
142 dMassSetZero (m);
143 M1 = M_PI*radius*radius*length*density; // cylinder mass
144 M2 = (REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density; // total cap mass
145 m->mass = M1+M2;
146 Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) +
147 M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length);
148 Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius;
149 m->_I(0,0) = Ia;
150 m->_I(1,1) = Ia;
151 m->_I(2,2) = Ia;
152 m->_I(direction-1,direction-1) = Ib;
153
154# ifndef dNODEBUG
155 dMassCheck (m);
156# endif
157}
158
159
160void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction,
161 dReal a, dReal b)
162{
163 dMassSetCapsule (m, 1.0, direction, a, b);
164 dMassAdjust (m, total_mass);
165}
166
167
168void dMassSetCylinder (dMass *m, dReal density, int direction,
169 dReal radius, dReal length)
170{
171 dMassSetCylinderTotal (m, M_PI*radius*radius*length*density,
172 direction, radius, length);
173}
174
175void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction,
176 dReal radius, dReal length)
177{
178 dReal r2,I;
179 dAASSERT (m);
180 dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
181 dMassSetZero (m);
182 r2 = radius*radius;
183 m->mass = total_mass;
184 I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length);
185 m->_I(0,0) = I;
186 m->_I(1,1) = I;
187 m->_I(2,2) = I;
188 m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2;
189
190# ifndef dNODEBUG
191 dMassCheck (m);
192# endif
193}
194
195
196void dMassSetBox (dMass *m, dReal density,
197 dReal lx, dReal ly, dReal lz)
198{
199 dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz);
200}
201
202
203void dMassSetBoxTotal (dMass *m, dReal total_mass,
204 dReal lx, dReal ly, dReal lz)
205{
206 dAASSERT (m);
207 dMassSetZero (m);
208 m->mass = total_mass;
209 m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz);
210 m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz);
211 m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly);
212
213# ifndef dNODEBUG
214 dMassCheck (m);
215# endif
216}
217
218
219
220
221
222
223#if dTRIMESH_ENABLED
224
225/*
226 * dMassSetTrimesh, implementation by Gero Mueller.
227 * Based on Brian Mirtich, "Fast and Accurate Computation of
228 * Polyhedral Mass Properties," journal of graphics tools, volume 1,
229 * number 2, 1996.
230*/
231void dMassSetTrimesh( dMass *m, dReal density, dGeomID g )
232{
233 dAASSERT (m);
234 dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
235
236 dMassSetZero (m);
237
238 unsigned int triangles = dGeomTriMeshGetTriangleCount( g );
239
240 dReal nx, ny, nz;
241 unsigned int i, A, B, C;
242 // face integrals
243 dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
244
245 // projection integrals
246 dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
247
248 dReal T0 = 0;
249 dReal T1[3] = {0., 0., 0.};
250 dReal T2[3] = {0., 0., 0.};
251 dReal TP[3] = {0., 0., 0.};
252
253 for( i = 0; i < triangles; i++ )
254 {
255 dVector3 v0, v1, v2;
256 dGeomTriMeshGetTriangle( g, i, &v0, &v1, &v2);
257
258 dVector3 n, a, b;
259 dOP( a, -, v1, v0 );
260 dOP( b, -, v2, v0 );
261 dCROSS( n, =, b, a );
262 nx = fabs(n[0]);
263 ny = fabs(n[1]);
264 nz = fabs(n[2]);
265
266 if( nx > ny && nx > nz )
267 C = 0;
268 else
269 C = (ny > nz) ? 1 : 2;
270
271 A = (C + 1) % 3;
272 B = (A + 1) % 3;
273
274 // calculate face integrals
275 {
276 dReal w;
277 dReal k1, k2, k3, k4;
278
279 //compProjectionIntegrals(f);
280 {
281 dReal a0, a1, da;
282 dReal b0, b1, db;
283 dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
284 dReal a1_2, a1_3, b1_2, b1_3;
285 dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
286 dReal Cab, Kab, Caab, Kaab, Cabb, Kabb;
287
288 P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
289
290 for( int j = 0; j < 3; j++)
291 {
292 switch(j)
293 {
294 case 0:
295 a0 = v0[A];
296 b0 = v0[B];
297 a1 = v1[A];
298 b1 = v1[B];
299 break;
300 case 1:
301 a0 = v1[A];
302 b0 = v1[B];
303 a1 = v2[A];
304 b1 = v2[B];
305 break;
306 case 2:
307 a0 = v2[A];
308 b0 = v2[B];
309 a1 = v0[A];
310 b1 = v0[B];
311 break;
312 }
313 da = a1 - a0;
314 db = b1 - b0;
315 a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
316 b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
317 a1_2 = a1 * a1; a1_3 = a1_2 * a1;
318 b1_2 = b1 * b1; b1_3 = b1_2 * b1;
319
320 C1 = a1 + a0;
321 Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
322 Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
323 Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
324 Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
325 Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
326 Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
327
328 P1 += db*C1;
329 Pa += db*Ca;
330 Paa += db*Caa;
331 Paaa += db*Caaa;
332 Pb += da*Cb;
333 Pbb += da*Cbb;
334 Pbbb += da*Cbbb;
335 Pab += db*(b1*Cab + b0*Kab);
336 Paab += db*(b1*Caab + b0*Kaab);
337 Pabb += da*(a1*Cabb + a0*Kabb);
338 }
339
340 P1 /= 2.0;
341 Pa /= 6.0;
342 Paa /= 12.0;
343 Paaa /= 20.0;
344 Pb /= -6.0;
345 Pbb /= -12.0;
346 Pbbb /= -20.0;
347 Pab /= 24.0;
348 Paab /= 60.0;
349 Pabb /= -60.0;
350 }
351
352 w = - dDOT(n, v0);
353
354 k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
355
356 Fa = k1 * Pa;
357 Fb = k1 * Pb;
358 Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
359
360 Faa = k1 * Paa;
361 Fbb = k1 * Pbb;
362 Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb +
363 w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
364
365 Faaa = k1 * Paaa;
366 Fbbb = k1 * Pbbb;
367 Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
368 + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
369 + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
370 + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
371
372 Faab = k1 * Paab;
373 Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
374 Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
375 + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
376 }
377
378
379 T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));
380
381 T1[A] += n[A] * Faa;
382 T1[B] += n[B] * Fbb;
383 T1[C] += n[C] * Fcc;
384 T2[A] += n[A] * Faaa;
385 T2[B] += n[B] * Fbbb;
386 T2[C] += n[C] * Fccc;
387 TP[A] += n[A] * Faab;
388 TP[B] += n[B] * Fbbc;
389 TP[C] += n[C] * Fcca;
390 }
391
392 T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
393 T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
394 TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
395
396 m->mass = density * T0;
397 m->_I(0,0) = density * (T2[1] + T2[2]);
398 m->_I(1,1) = density * (T2[2] + T2[0]);
399 m->_I(2,2) = density * (T2[0] + T2[1]);
400 m->_I(0,1) = - density * TP[0];
401 m->_I(1,0) = - density * TP[0];
402 m->_I(2,1) = - density * TP[1];
403 m->_I(1,2) = - density * TP[1];
404 m->_I(2,0) = - density * TP[2];
405 m->_I(0,2) = - density * TP[2];
406
407 // Added to address SF bug 1729095
408 dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 );
409
410# ifndef dNODEBUG
411 dMassCheck (m);
412# endif
413}
414
415
416void dMassSetTrimeshTotal( dMass *m, dReal total_mass, dGeomID g)
417{
418 dAASSERT( m );
419 dUASSERT( g && g->type == dTriMeshClass, "argument not a trimesh" );
420 dMassSetTrimesh( m, 1.0, g );
421 dMassAdjust( m, total_mass );
422}
423
424#endif // dTRIMESH_ENABLED
425
426
427
428
429void dMassAdjust (dMass *m, dReal newmass)
430{
431 dAASSERT (m);
432 dReal scale = newmass / m->mass;
433 m->mass = newmass;
434 for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale;
435
436# ifndef dNODEBUG
437 dMassCheck (m);
438# endif
439}
440
441
442void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
443{
444 // if the body is translated by `a' relative to its point of reference,
445 // the new inertia about the point of reference is:
446 //
447 // I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
448 //
449 // where c is the existing center of mass and I is the old inertia.
450
451 int i,j;
452 dMatrix3 ahat,chat,t1,t2;
453 dReal a[3];
454
455 dAASSERT (m);
456
457 // adjust inertia matrix
458 dSetZero (chat,12);
459 dCROSSMAT (chat,m->c,4,+,-);
460 a[0] = x + m->c[0];
461 a[1] = y + m->c[1];
462 a[2] = z + m->c[2];
463 dSetZero (ahat,12);
464 dCROSSMAT (ahat,a,4,+,-);
465 dMULTIPLY0_333 (t1,ahat,ahat);
466 dMULTIPLY0_333 (t2,chat,chat);
467 for (i=0; i<3; i++) for (j=0; j<3; j++)
468 m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
469
470 // ensure perfect symmetry
471 m->_I(1,0) = m->_I(0,1);
472 m->_I(2,0) = m->_I(0,2);
473 m->_I(2,1) = m->_I(1,2);
474
475 // adjust center of mass
476 m->c[0] += x;
477 m->c[1] += y;
478 m->c[2] += z;
479
480# ifndef dNODEBUG
481 dMassCheck (m);
482# endif
483}
484
485
486void dMassRotate (dMass *m, const dMatrix3 R)
487{
488 // if the body is rotated by `R' relative to its point of reference,
489 // the new inertia about the point of reference is:
490 //
491 // R * I * R'
492 //
493 // where I is the old inertia.
494
495 dMatrix3 t1;
496 dReal t2[3];
497
498 dAASSERT (m);
499
500 // rotate inertia matrix
501 dMULTIPLY2_333 (t1,m->I,R);
502 dMULTIPLY0_333 (m->I,R,t1);
503
504 // ensure perfect symmetry
505 m->_I(1,0) = m->_I(0,1);
506 m->_I(2,0) = m->_I(0,2);
507 m->_I(2,1) = m->_I(1,2);
508
509 // rotate center of mass
510 dMULTIPLY0_331 (t2,R,m->c);
511 m->c[0] = t2[0];
512 m->c[1] = t2[1];
513 m->c[2] = t2[2];
514
515# ifndef dNODEBUG
516 dMassCheck (m);
517# endif
518}
519
520
521void dMassAdd (dMass *a, const dMass *b)
522{
523 int i;
524 dAASSERT (a && b);
525 dReal denom = dRecip (a->mass + b->mass);
526 for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
527 a->mass += b->mass;
528 for (i=0; i<12; i++) a->I[i] += b->I[i];
529}
diff --git a/libraries/ode-0.9/ode/src/mat.cpp b/libraries/ode-0.9/ode/src/mat.cpp
new file mode 100644
index 0000000..6e635dc
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/mat.cpp
@@ -0,0 +1,230 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/misc.h>
25#include <ode/matrix.h>
26#include <ode/error.h>
27#include <ode/memory.h>
28#include "mat.h"
29
30
31dMatrix::dMatrix()
32{
33 n = 0;
34 m = 0;
35 data = 0;
36}
37
38
39dMatrix::dMatrix (int rows, int cols)
40{
41 if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
42 n = rows;
43 m = cols;
44 data = (dReal*) dAlloc (n*m*sizeof(dReal));
45 dSetZero (data,n*m);
46}
47
48
49dMatrix::dMatrix (const dMatrix &a)
50{
51 n = a.n;
52 m = a.m;
53 data = (dReal*) dAlloc (n*m*sizeof(dReal));
54 memcpy (data,a.data,n*m*sizeof(dReal));
55}
56
57
58dMatrix::dMatrix (int rows, int cols,
59 dReal *_data, int rowskip, int colskip)
60{
61 if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
62 n = rows;
63 m = cols;
64 data = (dReal*) dAlloc (n*m*sizeof(dReal));
65 for (int i=0; i<n; i++) {
66 for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip];
67 }
68}
69
70
71dMatrix::~dMatrix()
72{
73 if (data) dFree (data,n*m*sizeof(dReal));
74}
75
76
77dReal & dMatrix::operator () (int i, int j)
78{
79 if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)");
80 return data [i*m+j];
81}
82
83
84void dMatrix::operator= (const dMatrix &a)
85{
86 if (data) dFree (data,n*m*sizeof(dReal));
87 n = a.n;
88 m = a.m;
89 if (n > 0 && m > 0) {
90 data = (dReal*) dAlloc (n*m*sizeof(dReal));
91 memcpy (data,a.data,n*m*sizeof(dReal));
92 }
93 else data = 0;
94}
95
96
97void dMatrix::operator= (dReal a)
98{
99 for (int i=0; i<n*m; i++) data[i] = a;
100}
101
102
103dMatrix dMatrix::transpose()
104{
105 dMatrix r (m,n);
106 for (int i=0; i<n; i++) {
107 for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j];
108 }
109 return r;
110}
111
112
113dMatrix dMatrix::select (int np, int *p, int nq, int *q)
114{
115 if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes");
116 dMatrix r (np,nq);
117 for (int i=0; i<np; i++) {
118 for (int j=0; j<nq; j++) {
119 if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m)
120 dDebug (0,"Matrix select, bad index arrays");
121 r.data[i*nq+j] = data[p[i]*m+q[j]];
122 }
123 }
124 return r;
125}
126
127
128dMatrix dMatrix::operator + (const dMatrix &a)
129{
130 if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes");
131 dMatrix r (n,m);
132 for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i];
133 return r;
134}
135
136
137dMatrix dMatrix::operator - (const dMatrix &a)
138{
139 if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes");
140 dMatrix r (n,m);
141 for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i];
142 return r;
143}
144
145
146dMatrix dMatrix::operator - ()
147{
148 dMatrix r (n,m);
149 for (int i=0; i<n*m; i++) r.data[i] = -data[i];
150 return r;
151}
152
153
154dMatrix dMatrix::operator * (const dMatrix &a)
155{
156 if (m != a.n) dDebug (0,"matrix *, mismatched sizes");
157 dMatrix r (n,a.m);
158 for (int i=0; i<n; i++) {
159 for (int j=0; j<a.m; j++) {
160 dReal sum = 0;
161 for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j];
162 r.data [i*a.m+j] = sum;
163 }
164 }
165 return r;
166}
167
168
169void dMatrix::operator += (const dMatrix &a)
170{
171 if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes");
172 for (int i=0; i<n*m; i++) data[i] += a.data[i];
173}
174
175
176void dMatrix::operator -= (const dMatrix &a)
177{
178 if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes");
179 for (int i=0; i<n*m; i++) data[i] -= a.data[i];
180}
181
182
183void dMatrix::clearUpperTriangle()
184{
185 if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices");
186 for (int i=0; i<n; i++) {
187 for (int j=i+1; j<m; j++) data[i*m+j] = 0;
188 }
189}
190
191
192void dMatrix::clearLowerTriangle()
193{
194 if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices");
195 for (int i=0; i<n; i++) {
196 for (int j=0; j<i; j++) data[i*m+j] = 0;
197 }
198}
199
200
201void dMatrix::makeRandom (dReal range)
202{
203 for (int i=0; i<n; i++) {
204 for (int j=0; j<m; j++)
205 data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
206 }
207}
208
209
210void dMatrix::print (char *fmt, FILE *f)
211{
212 for (int i=0; i<n; i++) {
213 for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]);
214 fprintf (f,"\n");
215 }
216}
217
218
219dReal dMatrix::maxDifference (const dMatrix &a)
220{
221 if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes");
222 dReal max = 0;
223 for (int i=0; i<n; i++) {
224 for (int j=0; j<m; j++) {
225 dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]);
226 if (diff > max) max = diff;
227 }
228 }
229 return max;
230}
diff --git a/libraries/ode-0.9/ode/src/mat.h b/libraries/ode-0.9/ode/src/mat.h
new file mode 100644
index 0000000..2814a01
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/mat.h
@@ -0,0 +1,71 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// matrix class. this is mostly for convenience in the testing code, it is
24// not optimized at all. correctness is much more importance here.
25
26#ifndef _ODE_MAT_H_
27#define _ODE_MAT_H_
28
29#include <ode/common.h>
30
31
32class dMatrix {
33 int n,m; // matrix dimension, n,m >= 0
34 dReal *data; // if nonzero, n*m elements allocated on the heap
35
36public:
37 // constructors, destructors
38 dMatrix(); // make default 0x0 matrix
39 dMatrix (int rows, int cols); // construct zero matrix of given size
40 dMatrix (const dMatrix &); // construct copy of given matrix
41 // create copy of given data - element (i,j) is data[i*rowskip+j*colskip]
42 dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip);
43 ~dMatrix(); // destructor
44
45 // data movement
46 dReal & operator () (int i, int j); // reference an element
47 void operator= (const dMatrix &); // matrix = matrix
48 void operator= (dReal); // matrix = scalar
49 dMatrix transpose(); // return transposed matrix
50 // return a permuted submatrix of this matrix, made up of the rows in p
51 // and the columns in q. p has np elements, q has nq elements.
52 dMatrix select (int np, int *p, int nq, int *q);
53
54 // operators
55 dMatrix operator + (const dMatrix &);
56 dMatrix operator - (const dMatrix &);
57 dMatrix operator - ();
58 dMatrix operator * (const dMatrix &);
59 void operator += (const dMatrix &);
60 void operator -= (const dMatrix &);
61
62 // utility
63 void clearUpperTriangle();
64 void clearLowerTriangle();
65 void makeRandom (dReal range);
66 void print (char *fmt = "%10.4f ", FILE *f=stdout);
67 dReal maxDifference (const dMatrix &);
68};
69
70
71#endif
diff --git a/libraries/ode-0.9/ode/src/matrix.cpp b/libraries/ode-0.9/ode/src/matrix.cpp
new file mode 100644
index 0000000..16afe91
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/matrix.cpp
@@ -0,0 +1,358 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/common.h>
24#include <ode/matrix.h>
25
26// misc defines
27#define ALLOCA dALLOCA16
28
29
30void dSetZero (dReal *a, int n)
31{
32 dAASSERT (a && n >= 0);
33 while (n > 0) {
34 *(a++) = 0;
35 n--;
36 }
37}
38
39
40void dSetValue (dReal *a, int n, dReal value)
41{
42 dAASSERT (a && n >= 0);
43 while (n > 0) {
44 *(a++) = value;
45 n--;
46 }
47}
48
49
50void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
51{
52 int i,j,k,qskip,rskip,rpad;
53 dAASSERT (A && B && C && p>0 && q>0 && r>0);
54 qskip = dPAD(q);
55 rskip = dPAD(r);
56 rpad = rskip - r;
57 dReal sum;
58 const dReal *b,*c,*bb;
59 bb = B;
60 for (i=p; i; i--) {
61 for (j=0 ; j<r; j++) {
62 c = C + j;
63 b = bb;
64 sum = 0;
65 for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c);
66 *(A++) = sum;
67 }
68 A += rpad;
69 bb += qskip;
70 }
71}
72
73
74void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
75{
76 int i,j,k,pskip,rskip;
77 dReal sum;
78 dAASSERT (A && B && C && p>0 && q>0 && r>0);
79 pskip = dPAD(p);
80 rskip = dPAD(r);
81 for (i=0; i<p; i++) {
82 for (j=0; j<r; j++) {
83 sum = 0;
84 for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip];
85 A[i*rskip+j] = sum;
86 }
87 }
88}
89
90
91void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
92{
93 int i,j,k,z,rpad,qskip;
94 dReal sum;
95 const dReal *bb,*cc;
96 dAASSERT (A && B && C && p>0 && q>0 && r>0);
97 rpad = dPAD(r) - r;
98 qskip = dPAD(q);
99 bb = B;
100 for (i=p; i; i--) {
101 cc = C;
102 for (j=r; j; j--) {
103 z = 0;
104 sum = 0;
105 for (k=q; k; k--,z++) sum += bb[z] * cc[z];
106 *(A++) = sum;
107 cc += qskip;
108 }
109 A += rpad;
110 bb += qskip;
111 }
112}
113
114
115int dFactorCholesky (dReal *A, int n)
116{
117 int i,j,k,nskip;
118 dReal sum,*a,*b,*aa,*bb,*cc,*recip;
119 dAASSERT (n > 0 && A);
120 nskip = dPAD (n);
121 recip = (dReal*) ALLOCA (n * sizeof(dReal));
122 aa = A;
123 for (i=0; i<n; i++) {
124 bb = A;
125 cc = A + i*nskip;
126 for (j=0; j<i; j++) {
127 sum = *cc;
128 a = aa;
129 b = bb;
130 for (k=j; k; k--) sum -= (*(a++))*(*(b++));
131 *cc = sum * recip[j];
132 bb += nskip;
133 cc++;
134 }
135 sum = *cc;
136 a = aa;
137 for (k=i; k; k--, a++) sum -= (*a)*(*a);
138 if (sum <= REAL(0.0)) return 0;
139 *cc = dSqrt(sum);
140 recip[i] = dRecip (*cc);
141 aa += nskip;
142 }
143 return 1;
144}
145
146
147void dSolveCholesky (const dReal *L, dReal *b, int n)
148{
149 int i,k,nskip;
150 dReal sum,*y;
151 dAASSERT (n > 0 && L && b);
152 nskip = dPAD (n);
153 y = (dReal*) ALLOCA (n*sizeof(dReal));
154 for (i=0; i<n; i++) {
155 sum = 0;
156 for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k];
157 y[i] = (b[i]-sum)/L[i*nskip+i];
158 }
159 for (i=n-1; i >= 0; i--) {
160 sum = 0;
161 for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k];
162 b[i] = (y[i]-sum)/L[i*nskip+i];
163 }
164}
165
166
167int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n)
168{
169 int i,j,nskip;
170 dReal *L,*x;
171 dAASSERT (n > 0 && A && Ainv);
172 nskip = dPAD (n);
173 L = (dReal*) ALLOCA (nskip*n*sizeof(dReal));
174 memcpy (L,A,nskip*n*sizeof(dReal));
175 x = (dReal*) ALLOCA (n*sizeof(dReal));
176 if (dFactorCholesky (L,n)==0) return 0;
177 dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0
178 for (i=0; i<n; i++) {
179 for (j=0; j<n; j++) x[j] = 0;
180 x[i] = 1;
181 dSolveCholesky (L,x,n);
182 for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j];
183 }
184 return 1;
185}
186
187
188int dIsPositiveDefinite (const dReal *A, int n)
189{
190 dReal *Acopy;
191 dAASSERT (n > 0 && A);
192 int nskip = dPAD (n);
193 Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal));
194 memcpy (Acopy,A,nskip*n * sizeof(dReal));
195 return dFactorCholesky (Acopy,n);
196}
197
198
199/***** this has been replaced by a faster version
200void dSolveL1T (const dReal *L, dReal *b, int n, int nskip)
201{
202 int i,j;
203 dAASSERT (L && b && n >= 0 && nskip >= n);
204 dReal sum;
205 for (i=n-2; i>=0; i--) {
206 sum = 0;
207 for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j];
208 b[i] -= sum;
209 }
210}
211*/
212
213
214void dVectorScale (dReal *a, const dReal *d, int n)
215{
216 dAASSERT (a && d && n >= 0);
217 for (int i=0; i<n; i++) a[i] *= d[i];
218}
219
220
221void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip)
222{
223 dAASSERT (L && d && b && n > 0 && nskip >= n);
224 dSolveL1 (L,b,n,nskip);
225 dVectorScale (b,d,n);
226 dSolveL1T (L,b,n,nskip);
227}
228
229
230void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip)
231{
232 int j,p;
233 dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee;
234 dAASSERT (L && d && a && n > 0 && nskip >= n);
235
236 if (n < 2) return;
237 W1 = (dReal*) ALLOCA (n*sizeof(dReal));
238 W2 = (dReal*) ALLOCA (n*sizeof(dReal));
239
240 W1[0] = 0;
241 W2[0] = 0;
242 for (j=1; j<n; j++) W1[j] = W2[j] = a[j] * M_SQRT1_2;
243 W11 = (REAL(0.5)*a[0]+1)*M_SQRT1_2;
244 W21 = (REAL(0.5)*a[0]-1)*M_SQRT1_2;
245
246 alpha1=1;
247 alpha2=1;
248
249 dee = d[0];
250 alphanew = alpha1 + (W11*W11)*dee;
251 dee /= alphanew;
252 gamma1 = W11 * dee;
253 dee *= alpha1;
254 alpha1 = alphanew;
255 alphanew = alpha2 - (W21*W21)*dee;
256 dee /= alphanew;
257 gamma2 = W21 * dee;
258 alpha2 = alphanew;
259 k1 = REAL(1.0) - W21*gamma1;
260 k2 = W21*gamma1*W11 - W21;
261 for (p=1; p<n; p++) {
262 Wp = W1[p];
263 ell = L[p*nskip];
264 W1[p] = Wp - W11*ell;
265 W2[p] = k1*Wp + k2*ell;
266 }
267
268 for (j=1; j<n; j++) {
269 dee = d[j];
270 alphanew = alpha1 + (W1[j]*W1[j])*dee;
271 dee /= alphanew;
272 gamma1 = W1[j] * dee;
273 dee *= alpha1;
274 alpha1 = alphanew;
275 alphanew = alpha2 - (W2[j]*W2[j])*dee;
276 dee /= alphanew;
277 gamma2 = W2[j] * dee;
278 dee *= alpha2;
279 d[j] = dee;
280 alpha2 = alphanew;
281
282 k1 = W1[j];
283 k2 = W2[j];
284 for (p=j+1; p<n; p++) {
285 ell = L[p*nskip+j];
286 Wp = W1[p] - k1 * ell;
287 ell += gamma1 * Wp;
288 W1[p] = Wp;
289 Wp = W2[p] - k2 * ell;
290 ell -= gamma2 * Wp;
291 W2[p] = Wp;
292 L[p*nskip+j] = ell;
293 }
294 }
295}
296
297
298// macros for dLDLTRemove() for accessing A - either access the matrix
299// directly or access it via row pointers. we are only supposed to reference
300// the lower triangle of A (it is symmetric), but indexes i and j come from
301// permutation vectors so they are not predictable. so do a test on the
302// indexes - this should not slow things down too much, as we don't do this
303// in an inner loop.
304
305#define _GETA(i,j) (A[i][j])
306//#define _GETA(i,j) (A[(i)*nskip+(j)])
307#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i))
308
309
310void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
311 int n1, int n2, int r, int nskip)
312{
313 int i;
314 dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
315 n1 >= n2 && nskip >= n1);
316 #ifndef dNODEBUG
317 for (i=0; i<n2; i++) dIASSERT(p[i] >= 0 && p[i] < n1);
318 #endif
319
320 if (r==n2-1) {
321 return; // deleting last row/col is easy
322 }
323 else if (r==0) {
324 dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal));
325 for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]);
326 a[0] += REAL(1.0);
327 dLDLTAddTL (L,d,a,n2,nskip);
328 }
329 else {
330 dReal *t = (dReal*) ALLOCA (r * sizeof(dReal));
331 dReal *a = (dReal*) ALLOCA ((n2-r) * sizeof(dReal));
332 for (i=0; i<r; i++) t[i] = L[r*nskip+i] / d[i];
333 for (i=0; i<(n2-r); i++)
334 a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]);
335 a[0] += REAL(1.0);
336 dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip);
337 }
338
339 // snip out row/column r from L and d
340 dRemoveRowCol (L,n2,nskip,r);
341 if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal));
342}
343
344
345void dRemoveRowCol (dReal *A, int n, int nskip, int r)
346{
347 int i;
348 dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n);
349 if (r >= n-1) return;
350 if (r > 0) {
351 for (i=0; i<r; i++)
352 memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal));
353 for (i=r; i<(n-1); i++)
354 memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal));
355 }
356 for (i=r; i<(n-1); i++)
357 memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal));
358}
diff --git a/libraries/ode-0.9/ode/src/memory.cpp b/libraries/ode-0.9/ode/src/memory.cpp
new file mode 100644
index 0000000..60bb5b6
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/memory.cpp
@@ -0,0 +1,87 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/memory.h>
25#include <ode/error.h>
26
27
28static dAllocFunction *allocfn = 0;
29static dReallocFunction *reallocfn = 0;
30static dFreeFunction *freefn = 0;
31
32
33
34void dSetAllocHandler (dAllocFunction *fn)
35{
36 allocfn = fn;
37}
38
39
40void dSetReallocHandler (dReallocFunction *fn)
41{
42 reallocfn = fn;
43}
44
45
46void dSetFreeHandler (dFreeFunction *fn)
47{
48 freefn = fn;
49}
50
51
52dAllocFunction *dGetAllocHandler()
53{
54 return allocfn;
55}
56
57
58dReallocFunction *dGetReallocHandler()
59{
60 return reallocfn;
61}
62
63
64dFreeFunction *dGetFreeHandler()
65{
66 return freefn;
67}
68
69
70void * dAlloc (size_t size)
71{
72 if (allocfn) return allocfn (size); else return malloc (size);
73}
74
75
76void * dRealloc (void *ptr, size_t oldsize, size_t newsize)
77{
78 if (reallocfn) return reallocfn (ptr,oldsize,newsize);
79 else return realloc (ptr,newsize);
80}
81
82
83void dFree (void *ptr, size_t size)
84{
85 if (!ptr) return;
86 if (freefn) freefn (ptr,size); else free (ptr);
87}
diff --git a/libraries/ode-0.9/ode/src/misc.cpp b/libraries/ode-0.9/ode/src/misc.cpp
new file mode 100644
index 0000000..1a6f263
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/misc.cpp
@@ -0,0 +1,169 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/misc.h>
25#include <ode/matrix.h>
26
27//****************************************************************************
28// random numbers
29
30static unsigned long seed = 0;
31
32unsigned long dRand()
33{
34 seed = (1664525L*seed + 1013904223L) & 0xffffffff;
35 return seed;
36}
37
38
39unsigned long dRandGetSeed()
40{
41 return seed;
42}
43
44
45void dRandSetSeed (unsigned long s)
46{
47 seed = s;
48}
49
50
51int dTestRand()
52{
53 unsigned long oldseed = seed;
54 int ret = 1;
55 seed = 0;
56 if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
57 dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
58 dRand() != 0x6252e503) ret = 0;
59 seed = oldseed;
60 return ret;
61}
62
63
64// adam's all-int straightforward(?) dRandInt (0..n-1)
65int dRandInt (int n)
66{
67 // seems good; xor-fold and modulus
68 const unsigned long un = n;
69 unsigned long r = dRand();
70
71 // note: probably more aggressive than it needs to be -- might be
72 // able to get away without one or two of the innermost branches.
73 if (un <= 0x00010000UL) {
74 r ^= (r >> 16);
75 if (un <= 0x00000100UL) {
76 r ^= (r >> 8);
77 if (un <= 0x00000010UL) {
78 r ^= (r >> 4);
79 if (un <= 0x00000004UL) {
80 r ^= (r >> 2);
81 if (un <= 0x00000002UL) {
82 r ^= (r >> 1);
83 }
84 }
85 }
86 }
87 }
88
89 return (int) (r % un);
90}
91
92
93dReal dRandReal()
94{
95 return ((dReal) dRand()) / ((dReal) 0xffffffff);
96}
97
98//****************************************************************************
99// matrix utility stuff
100
101void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f)
102{
103 int i,j;
104 int skip = dPAD(m);
105 for (i=0; i<n; i++) {
106 for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);
107 fprintf (f,"\n");
108 }
109}
110
111
112void dMakeRandomVector (dReal *A, int n, dReal range)
113{
114 int i;
115 for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
116}
117
118
119void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
120{
121 int i,j;
122 int skip = dPAD(m);
123 dSetZero (A,n*skip);
124 for (i=0; i<n; i++) {
125 for (j=0; j<m; j++) A[i*skip+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
126 }
127}
128
129
130void dClearUpperTriangle (dReal *A, int n)
131{
132 int i,j;
133 int skip = dPAD(n);
134 for (i=0; i<n; i++) {
135 for (j=i+1; j<n; j++) A[i*skip+j] = 0;
136 }
137}
138
139
140dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
141{
142 int i,j;
143 int skip = dPAD(m);
144 dReal diff,max;
145 max = 0;
146 for (i=0; i<n; i++) {
147 for (j=0; j<m; j++) {
148 diff = dFabs(A[i*skip+j] - B[i*skip+j]);
149 if (diff > max) max = diff;
150 }
151 }
152 return max;
153}
154
155
156dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
157{
158 int i,j;
159 int skip = dPAD(n);
160 dReal diff,max;
161 max = 0;
162 for (i=0; i<n; i++) {
163 for (j=0; j<=i; j++) {
164 diff = dFabs(A[i*skip+j] - B[i*skip+j]);
165 if (diff > max) max = diff;
166 }
167 }
168 return max;
169}
diff --git a/libraries/ode-0.9/ode/src/objects.h b/libraries/ode-0.9/ode/src/objects.h
new file mode 100644
index 0000000..f286e2d
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/objects.h
@@ -0,0 +1,138 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// object, body, and world structs.
24
25
26#ifndef _ODE_OBJECT_H_
27#define _ODE_OBJECT_H_
28
29#include <ode/common.h>
30#include <ode/memory.h>
31#include <ode/mass.h>
32#include "array.h"
33
34
35// some body flags
36
37enum {
38 dxBodyFlagFiniteRotation = 1, // use finite rotations
39 dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis
40 dxBodyDisabled = 4, // body is disabled
41 dxBodyNoGravity = 8, // body is not influenced by gravity
42 dxBodyAutoDisable = 16 // enable auto-disable on body
43};
44
45
46// base class that does correct object allocation / deallocation
47
48struct dBase {
49 void *operator new (size_t size) { return dAlloc (size); }
50 void operator delete (void *ptr, size_t size) { dFree (ptr,size); }
51 void *operator new[] (size_t size) { return dAlloc (size); }
52 void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); }
53};
54
55
56// base class for bodies and joints
57
58struct dObject : public dBase {
59 dxWorld *world; // world this object is in
60 dObject *next; // next object of this type in list
61 dObject **tome; // pointer to previous object's next ptr
62 void *userdata; // user settable data
63 int tag; // used by dynamics algorithms
64};
65
66
67// auto disable parameters
68struct dxAutoDisable {
69 dReal idle_time; // time the body needs to be idle to auto-disable it
70 int idle_steps; // steps the body needs to be idle to auto-disable it
71 dReal linear_average_threshold; // linear (squared) average velocity threshold
72 dReal angular_average_threshold; // angular (squared) average velocity threshold
73 unsigned int average_samples; // size of the average_lvel and average_avel buffers
74};
75
76
77// quick-step parameters
78struct dxQuickStepParameters {
79 int num_iterations; // number of SOR iterations to perform
80 dReal w; // the SOR over-relaxation parameter
81};
82
83
84// contact generation parameters
85struct dxContactParameters {
86 dReal max_vel; // maximum correcting velocity
87 dReal min_depth; // thickness of 'surface layer'
88};
89
90
91
92// position vector and rotation matrix for geometry objects that are not
93// connected to bodies.
94
95struct dxPosR {
96 dVector3 pos;
97 dMatrix3 R;
98};
99
100struct dxBody : public dObject {
101 dxJointNode *firstjoint; // list of attached joints
102 int flags; // some dxBodyFlagXXX flags
103 dGeomID geom; // first collision geom associated with body
104 dMass mass; // mass parameters about POR
105 dMatrix3 invI; // inverse of mass.I
106 dReal invMass; // 1 / mass.mass
107 dxPosR posr; // position and orientation of point of reference
108 dQuaternion q; // orientation quaternion
109 dVector3 lvel,avel; // linear and angular velocity of POR
110 dVector3 facc,tacc; // force and torque accumulators
111 dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none
112
113 // auto-disable information
114 dxAutoDisable adis; // auto-disable parameters
115 dReal adis_timeleft; // time left to be idle
116 int adis_stepsleft; // steps left to be idle
117 dVector3* average_lvel_buffer; // buffer for the linear average velocity calculation
118 dVector3* average_avel_buffer; // buffer for the angular average velocity calculation
119 unsigned int average_counter; // counter/index to fill the average-buffers
120 int average_ready; // indicates ( with = 1 ), if the Body's buffers are ready for average-calculations
121};
122
123
124struct dxWorld : public dBase {
125 dxBody *firstbody; // body linked list
126 dxJoint *firstjoint; // joint linked list
127 int nb,nj; // number of bodies and joints in lists
128 dVector3 gravity; // gravity vector (m/s/s)
129 dReal global_erp; // global error reduction parameter
130 dReal global_cfm; // global costraint force mixing parameter
131 dxAutoDisable adis; // auto-disable parameters
132 int adis_flag; // auto-disable flag for new bodies
133 dxQuickStepParameters qs;
134 dxContactParameters contactp;
135};
136
137
138#endif
diff --git a/libraries/ode-0.9/ode/src/obstack.cpp b/libraries/ode-0.9/ode/src/obstack.cpp
new file mode 100644
index 0000000..0840396
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/obstack.cpp
@@ -0,0 +1,130 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/common.h>
24#include <ode/error.h>
25#include <ode/memory.h>
26#include "obstack.h"
27
28//****************************************************************************
29// macros and constants
30
31#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \
32 ofs = (size_t) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) );
33
34#define MAX_ALLOC_SIZE \
35 ((size_t)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1))
36
37//****************************************************************************
38// dObStack
39
40dObStack::dObStack()
41{
42 first = 0;
43 last = 0;
44 current_arena = 0;
45 current_ofs = 0;
46}
47
48
49dObStack::~dObStack()
50{
51 // free all arenas
52 Arena *a,*nexta;
53 a = first;
54 while (a) {
55 nexta = a->next;
56 dFree (a,dOBSTACK_ARENA_SIZE);
57 a = nexta;
58 }
59}
60
61
62void *dObStack::alloc (int num_bytes)
63{
64 if ((size_t)num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large");
65
66 // allocate or move to a new arena if necessary
67 if (!first) {
68 // allocate the first arena if necessary
69 first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
70 first->next = 0;
71 first->used = sizeof (Arena);
72 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
73 }
74 else {
75 // we already have one or more arenas, see if a new arena must be used
76 if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) {
77 if (!last->next) {
78 last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
79 last->next->next = 0;
80 }
81 last = last->next;
82 last->used = sizeof (Arena);
83 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
84 }
85 }
86
87 // allocate an area in the arena
88 char *c = ((char*) last) + last->used;
89 last->used += num_bytes;
90 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
91 return c;
92}
93
94
95void dObStack::freeAll()
96{
97 last = first;
98 if (first) {
99 first->used = sizeof(Arena);
100 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
101 }
102}
103
104
105void *dObStack::rewind()
106{
107 current_arena = first;
108 current_ofs = sizeof (Arena);
109 if (current_arena) {
110 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs)
111 return ((char*) current_arena) + current_ofs;
112 }
113 else return 0;
114}
115
116
117void *dObStack::next (int num_bytes)
118{
119 // this functions like alloc, except that no new storage is ever allocated
120 if (!current_arena) return 0;
121 current_ofs += num_bytes;
122 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
123 if (current_ofs >= current_arena->used) {
124 current_arena = current_arena->next;
125 if (!current_arena) return 0;
126 current_ofs = sizeof (Arena);
127 ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
128 }
129 return ((char*) current_arena) + current_ofs;
130}
diff --git a/libraries/ode-0.9/ode/src/obstack.h b/libraries/ode-0.9/ode/src/obstack.h
new file mode 100644
index 0000000..b5f715b
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/obstack.h
@@ -0,0 +1,68 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_OBSTACK_H_
24#define _ODE_OBSTACK_H_
25
26#include "objects.h"
27
28// each obstack Arena pointer points to a block of this many bytes
29#define dOBSTACK_ARENA_SIZE 16384
30
31
32struct dObStack : public dBase {
33 struct Arena {
34 Arena *next; // next arena in linked list
35 size_t used; // total number of bytes used in this arena, counting
36 }; // this header
37
38 Arena *first; // head of the arena linked list. 0 if no arenas yet
39 Arena *last; // arena where blocks are currently being allocated
40
41 // used for iterator
42 Arena *current_arena;
43 size_t current_ofs;
44
45 dObStack();
46 ~dObStack();
47
48 void *alloc (int num_bytes);
49 // allocate a block in the last arena, allocating a new arena if necessary.
50 // it is a runtime error if num_bytes is larger than the arena size.
51
52 void freeAll();
53 // free all blocks in all arenas. this does not deallocate the arenas
54 // themselves, so future alloc()s will reuse them.
55
56 void *rewind();
57 // rewind the obstack iterator, and return the address of the first
58 // allocated block. return 0 if there are no allocated blocks.
59
60 void *next (int num_bytes);
61 // return the address of the next allocated block. 'num_bytes' is the size
62 // of the previous block. this returns null if there are no more arenas.
63 // the sequence of 'num_bytes' parameters passed to next() during a
64 // traversal of the list must exactly match the parameters passed to alloc().
65};
66
67
68#endif
diff --git a/libraries/ode-0.9/ode/src/ode.cpp b/libraries/ode-0.9/ode/src/ode.cpp
new file mode 100644
index 0000000..46f559a
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/ode.cpp
@@ -0,0 +1,1732 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifdef _MSC_VER
24#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
25#endif
26
27// this source file is mostly concerned with the data structures, not the
28// numerics.
29
30#include "objects.h"
31#include <ode/ode.h>
32#include "joint.h"
33#include <ode/odemath.h>
34#include <ode/matrix.h>
35#include "step.h"
36#include "quickstep.h"
37#include "util.h"
38#include <ode/memory.h>
39#include <ode/error.h>
40
41// misc defines
42#define ALLOCA dALLOCA16
43
44//****************************************************************************
45// utility
46
47static inline void initObject (dObject *obj, dxWorld *w)
48{
49 obj->world = w;
50 obj->next = 0;
51 obj->tome = 0;
52 obj->userdata = 0;
53 obj->tag = 0;
54}
55
56
57// add an object `obj' to the list who's head pointer is pointed to by `first'.
58
59static inline void addObjectToList (dObject *obj, dObject **first)
60{
61 obj->next = *first;
62 obj->tome = first;
63 if (*first) (*first)->tome = &obj->next;
64 (*first) = obj;
65}
66
67
68// remove the object from the linked list
69
70static inline void removeObjectFromList (dObject *obj)
71{
72 if (obj->next) obj->next->tome = obj->tome;
73 *(obj->tome) = obj->next;
74 // safeguard
75 obj->next = 0;
76 obj->tome = 0;
77}
78
79
80// remove the joint from neighbour lists of all connected bodies
81
82static void removeJointReferencesFromAttachedBodies (dxJoint *j)
83{
84 for (int i=0; i<2; i++) {
85 dxBody *body = j->node[i].body;
86 if (body) {
87 dxJointNode *n = body->firstjoint;
88 dxJointNode *last = 0;
89 while (n) {
90 if (n->joint == j) {
91 if (last) last->next = n->next;
92 else body->firstjoint = n->next;
93 break;
94 }
95 last = n;
96 n = n->next;
97 }
98 }
99 }
100 j->node[0].body = 0;
101 j->node[0].next = 0;
102 j->node[1].body = 0;
103 j->node[1].next = 0;
104}
105
106//****************************************************************************
107// debugging
108
109// see if an object list loops on itself (if so, it's bad).
110
111static int listHasLoops (dObject *first)
112{
113 if (first==0 || first->next==0) return 0;
114 dObject *a=first,*b=first->next;
115 int skip=0;
116 while (b) {
117 if (a==b) return 1;
118 b = b->next;
119 if (skip) a = a->next;
120 skip ^= 1;
121 }
122 return 0;
123}
124
125
126// check the validity of the world data structures
127
128static void checkWorld (dxWorld *w)
129{
130 dxBody *b;
131 dxJoint *j;
132
133 // check there are no loops
134 if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
135 if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
136
137 // check lists are well formed (check `tome' pointers)
138 for (b=w->firstbody; b; b=(dxBody*)b->next) {
139 if (b->next && b->next->tome != &b->next)
140 dDebug (0,"bad tome pointer in body list");
141 }
142 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
143 if (j->next && j->next->tome != &j->next)
144 dDebug (0,"bad tome pointer in joint list");
145 }
146
147 // check counts
148 int n = 0;
149 for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
150 if (w->nb != n) dDebug (0,"body count incorrect");
151 n = 0;
152 for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
153 if (w->nj != n) dDebug (0,"joint count incorrect");
154
155 // set all tag values to a known value
156 static int count = 0;
157 count++;
158 for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
159 for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
160
161 // check all body/joint world pointers are ok
162 for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
163 dDebug (0,"bad world pointer in body list");
164 for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
165 dDebug (0,"bad world pointer in joint list");
166
167 /*
168 // check for half-connected joints - actually now these are valid
169 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
170 if (j->node[0].body || j->node[1].body) {
171 if (!(j->node[0].body && j->node[1].body))
172 dDebug (0,"half connected joint found");
173 }
174 }
175 */
176
177 // check that every joint node appears in the joint lists of both bodies it
178 // attaches
179 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
180 for (int i=0; i<2; i++) {
181 if (j->node[i].body) {
182 int ok = 0;
183 for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
184 if (n->joint == j) ok = 1;
185 }
186 if (ok==0) dDebug (0,"joint not in joint list of attached body");
187 }
188 }
189 }
190
191 // check all body joint lists (correct body ptrs)
192 for (b=w->firstbody; b; b=(dxBody*)b->next) {
193 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
194 if (&n->joint->node[0] == n) {
195 if (n->joint->node[1].body != b)
196 dDebug (0,"bad body pointer in joint node of body list (1)");
197 }
198 else {
199 if (n->joint->node[0].body != b)
200 dDebug (0,"bad body pointer in joint node of body list (2)");
201 }
202 if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
203 }
204 }
205
206 // check all body pointers in joints, check they are distinct
207 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
208 if (j->node[0].body && (j->node[0].body == j->node[1].body))
209 dDebug (0,"non-distinct body pointers in joint");
210 if ((j->node[0].body && j->node[0].body->tag != count) ||
211 (j->node[1].body && j->node[1].body->tag != count))
212 dDebug (0,"bad body pointer in joint");
213 }
214}
215
216
217void dWorldCheck (dxWorld *w)
218{
219 checkWorld (w);
220}
221
222//****************************************************************************
223// body
224dxWorld* dBodyGetWorld (dxBody* b)
225{
226 dAASSERT (b);
227 return b->world;
228}
229
230dxBody *dBodyCreate (dxWorld *w)
231{
232 dAASSERT (w);
233 dxBody *b = new dxBody;
234 initObject (b,w);
235 b->firstjoint = 0;
236 b->flags = 0;
237 b->geom = 0;
238 b->average_lvel_buffer = 0;
239 b->average_avel_buffer = 0;
240 dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
241 dSetZero (b->invI,4*3);
242 b->invI[0] = 1;
243 b->invI[5] = 1;
244 b->invI[10] = 1;
245 b->invMass = 1;
246 dSetZero (b->posr.pos,4);
247 dSetZero (b->q,4);
248 b->q[0] = 1;
249 dRSetIdentity (b->posr.R);
250 dSetZero (b->lvel,4);
251 dSetZero (b->avel,4);
252 dSetZero (b->facc,4);
253 dSetZero (b->tacc,4);
254 dSetZero (b->finite_rot_axis,4);
255 addObjectToList (b,(dObject **) &w->firstbody);
256 w->nb++;
257
258 // set auto-disable parameters
259 b->average_avel_buffer = b->average_lvel_buffer = 0; // no buffer at beginnin
260 dBodySetAutoDisableDefaults (b); // must do this after adding to world
261 b->adis_stepsleft = b->adis.idle_steps;
262 b->adis_timeleft = b->adis.idle_time;
263 b->average_counter = 0;
264 b->average_ready = 0; // average buffer not filled on the beginning
265 dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples);
266
267 return b;
268}
269
270
271void dBodyDestroy (dxBody *b)
272{
273 dAASSERT (b);
274
275 // all geoms that link to this body must be notified that the body is about
276 // to disappear. note that the call to dGeomSetBody(geom,0) will result in
277 // dGeomGetBodyNext() returning 0 for the body, so we must get the next body
278 // before setting the body to 0.
279 dxGeom *next_geom = 0;
280 for (dxGeom *geom = b->geom; geom; geom = next_geom) {
281 next_geom = dGeomGetBodyNext (geom);
282 dGeomSetBody (geom,0);
283 }
284
285 // detach all neighbouring joints, then delete this body.
286 dxJointNode *n = b->firstjoint;
287 while (n) {
288 // sneaky trick to speed up removal of joint references (black magic)
289 n->joint->node[(n == n->joint->node)].body = 0;
290
291 dxJointNode *next = n->next;
292 n->next = 0;
293 removeJointReferencesFromAttachedBodies (n->joint);
294 n = next;
295 }
296 removeObjectFromList (b);
297 b->world->nb--;
298
299 // delete the average buffers
300 if(b->average_lvel_buffer)
301 {
302 delete[] (b->average_lvel_buffer);
303 b->average_lvel_buffer = 0;
304 }
305 if(b->average_avel_buffer)
306 {
307 delete[] (b->average_avel_buffer);
308 b->average_avel_buffer = 0;
309 }
310
311 delete b;
312}
313
314
315void dBodySetData (dBodyID b, void *data)
316{
317 dAASSERT (b);
318 b->userdata = data;
319}
320
321
322void *dBodyGetData (dBodyID b)
323{
324 dAASSERT (b);
325 return b->userdata;
326}
327
328
329void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
330{
331 dAASSERT (b);
332 b->posr.pos[0] = x;
333 b->posr.pos[1] = y;
334 b->posr.pos[2] = z;
335
336 // notify all attached geoms that this body has moved
337 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
338 dGeomMoved (geom);
339}
340
341
342void dBodySetRotation (dBodyID b, const dMatrix3 R)
343{
344 dAASSERT (b && R);
345 dQuaternion q;
346 dRtoQ (R,q);
347 dNormalize4 (q);
348 b->q[0] = q[0];
349 b->q[1] = q[1];
350 b->q[2] = q[2];
351 b->q[3] = q[3];
352 dQtoR (b->q,b->posr.R);
353
354 // notify all attached geoms that this body has moved
355 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
356 dGeomMoved (geom);
357}
358
359
360void dBodySetQuaternion (dBodyID b, const dQuaternion q)
361{
362 dAASSERT (b && q);
363 b->q[0] = q[0];
364 b->q[1] = q[1];
365 b->q[2] = q[2];
366 b->q[3] = q[3];
367 dNormalize4 (b->q);
368 dQtoR (b->q,b->posr.R);
369
370 // notify all attached geoms that this body has moved
371 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
372 dGeomMoved (geom);
373}
374
375
376void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
377{
378 dAASSERT (b);
379 b->lvel[0] = x;
380 b->lvel[1] = y;
381 b->lvel[2] = z;
382}
383
384
385void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
386{
387 dAASSERT (b);
388 b->avel[0] = x;
389 b->avel[1] = y;
390 b->avel[2] = z;
391}
392
393
394const dReal * dBodyGetPosition (dBodyID b)
395{
396 dAASSERT (b);
397 return b->posr.pos;
398}
399
400
401void dBodyCopyPosition (dBodyID b, dVector3 pos)
402{
403 dAASSERT (b);
404 dReal* src = b->posr.pos;
405 pos[0] = src[0];
406 pos[1] = src[1];
407 pos[2] = src[2];
408}
409
410
411const dReal * dBodyGetRotation (dBodyID b)
412{
413 dAASSERT (b);
414 return b->posr.R;
415}
416
417
418void dBodyCopyRotation (dBodyID b, dMatrix3 R)
419{
420 dAASSERT (b);
421 const dReal* src = b->posr.R;
422 R[0] = src[0];
423 R[1] = src[1];
424 R[2] = src[2];
425 R[3] = src[3];
426 R[4] = src[4];
427 R[5] = src[5];
428 R[6] = src[6];
429 R[7] = src[7];
430 R[8] = src[8];
431 R[9] = src[9];
432 R[10] = src[10];
433 R[11] = src[11];
434}
435
436
437const dReal * dBodyGetQuaternion (dBodyID b)
438{
439 dAASSERT (b);
440 return b->q;
441}
442
443
444void dBodyCopyQuaternion (dBodyID b, dQuaternion quat)
445{
446 dAASSERT (b);
447 dReal* src = b->q;
448 quat[0] = src[0];
449 quat[1] = src[1];
450 quat[2] = src[2];
451 quat[3] = src[3];
452}
453
454
455const dReal * dBodyGetLinearVel (dBodyID b)
456{
457 dAASSERT (b);
458 return b->lvel;
459}
460
461
462const dReal * dBodyGetAngularVel (dBodyID b)
463{
464 dAASSERT (b);
465 return b->avel;
466}
467
468
469void dBodySetMass (dBodyID b, const dMass *mass)
470{
471 dAASSERT (b && mass );
472 dIASSERT(dMassCheck(mass));
473
474 // The centre of mass must be at the origin.
475 // Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it.
476 dUASSERT( fabs( mass->c[0] ) <= dEpsilon &&
477 fabs( mass->c[1] ) <= dEpsilon &&
478 fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." )
479
480 memcpy (&b->mass,mass,sizeof(dMass));
481 if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {
482 dDEBUGMSG ("inertia must be positive definite!");
483 dRSetIdentity (b->invI);
484 }
485 b->invMass = dRecip(b->mass.mass);
486}
487
488
489void dBodyGetMass (dBodyID b, dMass *mass)
490{
491 dAASSERT (b && mass);
492 memcpy (mass,&b->mass,sizeof(dMass));
493}
494
495
496void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
497{
498 dAASSERT (b);
499 b->facc[0] += fx;
500 b->facc[1] += fy;
501 b->facc[2] += fz;
502}
503
504
505void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
506{
507 dAASSERT (b);
508 b->tacc[0] += fx;
509 b->tacc[1] += fy;
510 b->tacc[2] += fz;
511}
512
513
514void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
515{
516 dAASSERT (b);
517 dVector3 t1,t2;
518 t1[0] = fx;
519 t1[1] = fy;
520 t1[2] = fz;
521 t1[3] = 0;
522 dMULTIPLY0_331 (t2,b->posr.R,t1);
523 b->facc[0] += t2[0];
524 b->facc[1] += t2[1];
525 b->facc[2] += t2[2];
526}
527
528
529void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
530{
531 dAASSERT (b);
532 dVector3 t1,t2;
533 t1[0] = fx;
534 t1[1] = fy;
535 t1[2] = fz;
536 t1[3] = 0;
537 dMULTIPLY0_331 (t2,b->posr.R,t1);
538 b->tacc[0] += t2[0];
539 b->tacc[1] += t2[1];
540 b->tacc[2] += t2[2];
541}
542
543
544void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
545 dReal px, dReal py, dReal pz)
546{
547 dAASSERT (b);
548 b->facc[0] += fx;
549 b->facc[1] += fy;
550 b->facc[2] += fz;
551 dVector3 f,q;
552 f[0] = fx;
553 f[1] = fy;
554 f[2] = fz;
555 q[0] = px - b->posr.pos[0];
556 q[1] = py - b->posr.pos[1];
557 q[2] = pz - b->posr.pos[2];
558 dCROSS (b->tacc,+=,q,f);
559}
560
561
562void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
563 dReal px, dReal py, dReal pz)
564{
565 dAASSERT (b);
566 dVector3 prel,f,p;
567 f[0] = fx;
568 f[1] = fy;
569 f[2] = fz;
570 f[3] = 0;
571 prel[0] = px;
572 prel[1] = py;
573 prel[2] = pz;
574 prel[3] = 0;
575 dMULTIPLY0_331 (p,b->posr.R,prel);
576 b->facc[0] += f[0];
577 b->facc[1] += f[1];
578 b->facc[2] += f[2];
579 dCROSS (b->tacc,+=,p,f);
580}
581
582
583void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
584 dReal px, dReal py, dReal pz)
585{
586 dAASSERT (b);
587 dVector3 frel,f;
588 frel[0] = fx;
589 frel[1] = fy;
590 frel[2] = fz;
591 frel[3] = 0;
592 dMULTIPLY0_331 (f,b->posr.R,frel);
593 b->facc[0] += f[0];
594 b->facc[1] += f[1];
595 b->facc[2] += f[2];
596 dVector3 q;
597 q[0] = px - b->posr.pos[0];
598 q[1] = py - b->posr.pos[1];
599 q[2] = pz - b->posr.pos[2];
600 dCROSS (b->tacc,+=,q,f);
601}
602
603
604void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
605 dReal px, dReal py, dReal pz)
606{
607 dAASSERT (b);
608 dVector3 frel,prel,f,p;
609 frel[0] = fx;
610 frel[1] = fy;
611 frel[2] = fz;
612 frel[3] = 0;
613 prel[0] = px;
614 prel[1] = py;
615 prel[2] = pz;
616 prel[3] = 0;
617 dMULTIPLY0_331 (f,b->posr.R,frel);
618 dMULTIPLY0_331 (p,b->posr.R,prel);
619 b->facc[0] += f[0];
620 b->facc[1] += f[1];
621 b->facc[2] += f[2];
622 dCROSS (b->tacc,+=,p,f);
623}
624
625
626const dReal * dBodyGetForce (dBodyID b)
627{
628 dAASSERT (b);
629 return b->facc;
630}
631
632
633const dReal * dBodyGetTorque (dBodyID b)
634{
635 dAASSERT (b);
636 return b->tacc;
637}
638
639
640void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
641{
642 dAASSERT (b);
643 b->facc[0] = x;
644 b->facc[1] = y;
645 b->facc[2] = z;
646}
647
648
649void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
650{
651 dAASSERT (b);
652 b->tacc[0] = x;
653 b->tacc[1] = y;
654 b->tacc[2] = z;
655}
656
657
658void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
659 dVector3 result)
660{
661 dAASSERT (b);
662 dVector3 prel,p;
663 prel[0] = px;
664 prel[1] = py;
665 prel[2] = pz;
666 prel[3] = 0;
667 dMULTIPLY0_331 (p,b->posr.R,prel);
668 result[0] = p[0] + b->posr.pos[0];
669 result[1] = p[1] + b->posr.pos[1];
670 result[2] = p[2] + b->posr.pos[2];
671}
672
673
674void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
675 dVector3 result)
676{
677 dAASSERT (b);
678 dVector3 prel,p;
679 prel[0] = px;
680 prel[1] = py;
681 prel[2] = pz;
682 prel[3] = 0;
683 dMULTIPLY0_331 (p,b->posr.R,prel);
684 result[0] = b->lvel[0];
685 result[1] = b->lvel[1];
686 result[2] = b->lvel[2];
687 dCROSS (result,+=,b->avel,p);
688}
689
690
691void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
692 dVector3 result)
693{
694 dAASSERT (b);
695 dVector3 p;
696 p[0] = px - b->posr.pos[0];
697 p[1] = py - b->posr.pos[1];
698 p[2] = pz - b->posr.pos[2];
699 p[3] = 0;
700 result[0] = b->lvel[0];
701 result[1] = b->lvel[1];
702 result[2] = b->lvel[2];
703 dCROSS (result,+=,b->avel,p);
704}
705
706
707void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
708 dVector3 result)
709{
710 dAASSERT (b);
711 dVector3 prel;
712 prel[0] = px - b->posr.pos[0];
713 prel[1] = py - b->posr.pos[1];
714 prel[2] = pz - b->posr.pos[2];
715 prel[3] = 0;
716 dMULTIPLY1_331 (result,b->posr.R,prel);
717}
718
719
720void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
721 dVector3 result)
722{
723 dAASSERT (b);
724 dVector3 p;
725 p[0] = px;
726 p[1] = py;
727 p[2] = pz;
728 p[3] = 0;
729 dMULTIPLY0_331 (result,b->posr.R,p);
730}
731
732
733void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
734 dVector3 result)
735{
736 dAASSERT (b);
737 dVector3 p;
738 p[0] = px;
739 p[1] = py;
740 p[2] = pz;
741 p[3] = 0;
742 dMULTIPLY1_331 (result,b->posr.R,p);
743}
744
745
746void dBodySetFiniteRotationMode (dBodyID b, int mode)
747{
748 dAASSERT (b);
749 b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
750 if (mode) {
751 b->flags |= dxBodyFlagFiniteRotation;
752 if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
753 b->finite_rot_axis[2] != 0) {
754 b->flags |= dxBodyFlagFiniteRotationAxis;
755 }
756 }
757}
758
759
760void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
761{
762 dAASSERT (b);
763 b->finite_rot_axis[0] = x;
764 b->finite_rot_axis[1] = y;
765 b->finite_rot_axis[2] = z;
766 if (x != 0 || y != 0 || z != 0) {
767 dNormalize3 (b->finite_rot_axis);
768 b->flags |= dxBodyFlagFiniteRotationAxis;
769 }
770 else {
771 b->flags &= ~dxBodyFlagFiniteRotationAxis;
772 }
773}
774
775
776int dBodyGetFiniteRotationMode (dBodyID b)
777{
778 dAASSERT (b);
779 return ((b->flags & dxBodyFlagFiniteRotation) != 0);
780}
781
782
783void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
784{
785 dAASSERT (b);
786 result[0] = b->finite_rot_axis[0];
787 result[1] = b->finite_rot_axis[1];
788 result[2] = b->finite_rot_axis[2];
789}
790
791
792int dBodyGetNumJoints (dBodyID b)
793{
794 dAASSERT (b);
795 int count=0;
796 for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
797 return count;
798}
799
800
801dJointID dBodyGetJoint (dBodyID b, int index)
802{
803 dAASSERT (b);
804 int i=0;
805 for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
806 if (i == index) return n->joint;
807 }
808 return 0;
809}
810
811
812void dBodyEnable (dBodyID b)
813{
814 dAASSERT (b);
815 b->flags &= ~dxBodyDisabled;
816 b->adis_stepsleft = b->adis.idle_steps;
817 b->adis_timeleft = b->adis.idle_time;
818 // no code for average-processing needed here
819}
820
821
822void dBodyDisable (dBodyID b)
823{
824 dAASSERT (b);
825 b->flags |= dxBodyDisabled;
826}
827
828
829int dBodyIsEnabled (dBodyID b)
830{
831 dAASSERT (b);
832 return ((b->flags & dxBodyDisabled) == 0);
833}
834
835
836void dBodySetGravityMode (dBodyID b, int mode)
837{
838 dAASSERT (b);
839 if (mode) b->flags &= ~dxBodyNoGravity;
840 else b->flags |= dxBodyNoGravity;
841}
842
843
844int dBodyGetGravityMode (dBodyID b)
845{
846 dAASSERT (b);
847 return ((b->flags & dxBodyNoGravity) == 0);
848}
849
850
851// body auto-disable functions
852
853dReal dBodyGetAutoDisableLinearThreshold (dBodyID b)
854{
855 dAASSERT(b);
856 return dSqrt (b->adis.linear_average_threshold);
857}
858
859
860void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold)
861{
862 dAASSERT(b);
863 b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
864}
865
866
867dReal dBodyGetAutoDisableAngularThreshold (dBodyID b)
868{
869 dAASSERT(b);
870 return dSqrt (b->adis.angular_average_threshold);
871}
872
873
874void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold)
875{
876 dAASSERT(b);
877 b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
878}
879
880
881int dBodyGetAutoDisableAverageSamplesCount (dBodyID b)
882{
883 dAASSERT(b);
884 return b->adis.average_samples;
885}
886
887
888void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count)
889{
890 dAASSERT(b);
891 b->adis.average_samples = average_samples_count;
892 // update the average buffers
893 if(b->average_lvel_buffer)
894 {
895 delete[] b->average_lvel_buffer;
896 b->average_lvel_buffer = 0;
897 }
898 if(b->average_avel_buffer)
899 {
900 delete[] b->average_avel_buffer;
901 b->average_avel_buffer = 0;
902 }
903 if(b->adis.average_samples > 0)
904 {
905 b->average_lvel_buffer = new dVector3[b->adis.average_samples];
906 b->average_avel_buffer = new dVector3[b->adis.average_samples];
907 }
908 else
909 {
910 b->average_lvel_buffer = 0;
911 b->average_avel_buffer = 0;
912 }
913 // new buffer is empty
914 b->average_counter = 0;
915 b->average_ready = 0;
916}
917
918
919int dBodyGetAutoDisableSteps (dBodyID b)
920{
921 dAASSERT(b);
922 return b->adis.idle_steps;
923}
924
925
926void dBodySetAutoDisableSteps (dBodyID b, int steps)
927{
928 dAASSERT(b);
929 b->adis.idle_steps = steps;
930}
931
932
933dReal dBodyGetAutoDisableTime (dBodyID b)
934{
935 dAASSERT(b);
936 return b->adis.idle_time;
937}
938
939
940void dBodySetAutoDisableTime (dBodyID b, dReal time)
941{
942 dAASSERT(b);
943 b->adis.idle_time = time;
944}
945
946
947int dBodyGetAutoDisableFlag (dBodyID b)
948{
949 dAASSERT(b);
950 return ((b->flags & dxBodyAutoDisable) != 0);
951}
952
953
954void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable)
955{
956 dAASSERT(b);
957 if (!do_auto_disable)
958 {
959 b->flags &= ~dxBodyAutoDisable;
960 // (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag
961 b->flags &= ~dxBodyDisabled;
962 b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world);
963 b->adis.idle_time = dWorldGetAutoDisableTime(b->world);
964 // resetting the average calculations too
965 dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) );
966 }
967 else
968 {
969 b->flags |= dxBodyAutoDisable;
970 }
971}
972
973
974void dBodySetAutoDisableDefaults (dBodyID b)
975{
976 dAASSERT(b);
977 dWorldID w = b->world;
978 dAASSERT(w);
979 b->adis = w->adis;
980 dBodySetAutoDisableFlag (b, w->adis_flag);
981}
982
983//****************************************************************************
984// joints
985
986static void dJointInit (dxWorld *w, dxJoint *j)
987{
988 dIASSERT (w && j);
989 initObject (j,w);
990 j->vtable = 0;
991 j->flags = 0;
992 j->node[0].joint = j;
993 j->node[0].body = 0;
994 j->node[0].next = 0;
995 j->node[1].joint = j;
996 j->node[1].body = 0;
997 j->node[1].next = 0;
998 dSetZero (j->lambda,6);
999 addObjectToList (j,(dObject **) &w->firstjoint);
1000 w->nj++;
1001}
1002
1003
1004static dxJoint *createJoint (dWorldID w, dJointGroupID group,
1005 dxJoint::Vtable *vtable)
1006{
1007 dIASSERT (w && vtable);
1008 dxJoint *j;
1009 if (group) {
1010 j = (dxJoint*) group->stack.alloc (vtable->size);
1011 group->num++;
1012 }
1013 else j = (dxJoint*) dAlloc (vtable->size);
1014 dJointInit (w,j);
1015 j->vtable = vtable;
1016 if (group) j->flags |= dJOINT_INGROUP;
1017 if (vtable->init) vtable->init (j);
1018 j->feedback = 0;
1019 return j;
1020}
1021
1022
1023dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
1024{
1025 dAASSERT (w);
1026 return createJoint (w,group,&__dball_vtable);
1027}
1028
1029
1030dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
1031{
1032 dAASSERT (w);
1033 return createJoint (w,group,&__dhinge_vtable);
1034}
1035
1036
1037dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
1038{
1039 dAASSERT (w);
1040 return createJoint (w,group,&__dslider_vtable);
1041}
1042
1043
1044dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
1045 const dContact *c)
1046{
1047 dAASSERT (w && c);
1048 dxJointContact *j = (dxJointContact *)
1049 createJoint (w,group,&__dcontact_vtable);
1050 j->contact = *c;
1051 return j;
1052}
1053
1054
1055dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
1056{
1057 dAASSERT (w);
1058 return createJoint (w,group,&__dhinge2_vtable);
1059}
1060
1061
1062dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
1063{
1064 dAASSERT (w);
1065 return createJoint (w,group,&__duniversal_vtable);
1066}
1067
1068dxJoint * dJointCreatePR (dWorldID w, dJointGroupID group)
1069{
1070 dAASSERT (w);
1071 return createJoint (w,group,&__dPR_vtable);
1072}
1073
1074dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
1075{
1076 dAASSERT (w);
1077 return createJoint (w,group,&__dfixed_vtable);
1078}
1079
1080
1081dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
1082{
1083 dAASSERT (w);
1084 return createJoint (w,group,&__dnull_vtable);
1085}
1086
1087
1088dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
1089{
1090 dAASSERT (w);
1091 return createJoint (w,group,&__damotor_vtable);
1092}
1093
1094dxJoint * dJointCreateLMotor (dWorldID w, dJointGroupID group)
1095{
1096 dAASSERT (w);
1097 return createJoint (w,group,&__dlmotor_vtable);
1098}
1099
1100dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group)
1101{
1102 dAASSERT (w);
1103 return createJoint (w,group,&__dplane2d_vtable);
1104}
1105
1106void dJointDestroy (dxJoint *j)
1107{
1108 dAASSERT (j);
1109 if (j->flags & dJOINT_INGROUP) return;
1110 removeJointReferencesFromAttachedBodies (j);
1111 removeObjectFromList (j);
1112 j->world->nj--;
1113 dFree (j,j->vtable->size);
1114}
1115
1116
1117dJointGroupID dJointGroupCreate (int max_size)
1118{
1119 // not any more ... dUASSERT (max_size > 0,"max size must be > 0");
1120 dxJointGroup *group = new dxJointGroup;
1121 group->num = 0;
1122 return group;
1123}
1124
1125
1126void dJointGroupDestroy (dJointGroupID group)
1127{
1128 dAASSERT (group);
1129 dJointGroupEmpty (group);
1130 delete group;
1131}
1132
1133
1134void dJointGroupEmpty (dJointGroupID group)
1135{
1136 // the joints in this group are detached starting from the most recently
1137 // added (at the top of the stack). this helps ensure that the various
1138 // linked lists are not traversed too much, as the joints will hopefully
1139 // be at the start of those lists.
1140 // if any group joints have their world pointer set to 0, their world was
1141 // previously destroyed. no special handling is required for these joints.
1142
1143 dAASSERT (group);
1144 int i;
1145 dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*));
1146 dxJoint *j = (dxJoint*) group->stack.rewind();
1147 for (i=0; i < group->num; i++) {
1148 jlist[i] = j;
1149 j = (dxJoint*) (group->stack.next (j->vtable->size));
1150 }
1151 for (i=group->num-1; i >= 0; i--) {
1152 if (jlist[i]->world) {
1153 removeJointReferencesFromAttachedBodies (jlist[i]);
1154 removeObjectFromList (jlist[i]);
1155 jlist[i]->world->nj--;
1156 }
1157 }
1158 group->num = 0;
1159 group->stack.freeAll();
1160}
1161
1162
1163void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
1164{
1165 // check arguments
1166 dUASSERT (joint,"bad joint argument");
1167 dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
1168 dxWorld *world = joint->world;
1169 dUASSERT ( (!body1 || body1->world == world) &&
1170 (!body2 || body2->world == world),
1171 "joint and bodies must be in same world");
1172
1173 // check if the joint can not be attached to just one body
1174 dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
1175 ((body1 != 0) ^ (body2 != 0))),
1176 "joint can not be attached to just one body");
1177
1178 // remove any existing body attachments
1179 if (joint->node[0].body || joint->node[1].body) {
1180 removeJointReferencesFromAttachedBodies (joint);
1181 }
1182
1183 // if a body is zero, make sure that it is body2, so 0 --> node[1].body
1184 if (body1==0) {
1185 body1 = body2;
1186 body2 = 0;
1187 joint->flags |= dJOINT_REVERSE;
1188 }
1189 else {
1190 joint->flags &= (~dJOINT_REVERSE);
1191 }
1192
1193 // attach to new bodies
1194 joint->node[0].body = body1;
1195 joint->node[1].body = body2;
1196 if (body1) {
1197 joint->node[1].next = body1->firstjoint;
1198 body1->firstjoint = &joint->node[1];
1199 }
1200 else joint->node[1].next = 0;
1201 if (body2) {
1202 joint->node[0].next = body2->firstjoint;
1203 body2->firstjoint = &joint->node[0];
1204 }
1205 else {
1206 joint->node[0].next = 0;
1207 }
1208}
1209
1210
1211void dJointSetData (dxJoint *joint, void *data)
1212{
1213 dAASSERT (joint);
1214 joint->userdata = data;
1215}
1216
1217
1218void *dJointGetData (dxJoint *joint)
1219{
1220 dAASSERT (joint);
1221 return joint->userdata;
1222}
1223
1224
1225int dJointGetType (dxJoint *joint)
1226{
1227 dAASSERT (joint);
1228 return joint->vtable->typenum;
1229}
1230
1231
1232dBodyID dJointGetBody (dxJoint *joint, int index)
1233{
1234 dAASSERT (joint);
1235 if (index == 0 || index == 1) {
1236 if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body;
1237 else return joint->node[index].body;
1238 }
1239 else return 0;
1240}
1241
1242
1243void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
1244{
1245 dAASSERT (joint);
1246 joint->feedback = f;
1247}
1248
1249
1250dJointFeedback *dJointGetFeedback (dxJoint *joint)
1251{
1252 dAASSERT (joint);
1253 return joint->feedback;
1254}
1255
1256
1257
1258dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2)
1259{
1260 dAASSERT (in_b1 || in_b2);
1261
1262 dBodyID b1, b2;
1263
1264 if (in_b1 == 0) {
1265 b1 = in_b2;
1266 b2 = in_b1;
1267 }
1268 else {
1269 b1 = in_b1;
1270 b2 = in_b2;
1271 }
1272
1273 // look through b1's neighbour list for b2
1274 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1275 if (n->body == b2) return n->joint;
1276 }
1277
1278 return 0;
1279}
1280
1281
1282
1283int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list)
1284{
1285 dAASSERT (in_b1 || in_b2);
1286
1287
1288 dBodyID b1, b2;
1289
1290 if (in_b1 == 0) {
1291 b1 = in_b2;
1292 b2 = in_b1;
1293 }
1294 else {
1295 b1 = in_b1;
1296 b2 = in_b2;
1297 }
1298
1299 // look through b1's neighbour list for b2
1300 int numConnectingJoints = 0;
1301 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1302 if (n->body == b2)
1303 out_list[numConnectingJoints++] = n->joint;
1304 }
1305
1306 return numConnectingJoints;
1307}
1308
1309
1310int dAreConnected (dBodyID b1, dBodyID b2)
1311{
1312 dAASSERT (b1 && b2);
1313 // look through b1's neighbour list for b2
1314 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1315 if (n->body == b2) return 1;
1316 }
1317 return 0;
1318}
1319
1320
1321int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type)
1322{
1323 dAASSERT (b1 && b2);
1324 // look through b1's neighbour list for b2
1325 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1326 if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1;
1327 }
1328 return 0;
1329}
1330
1331//****************************************************************************
1332// world
1333
1334dxWorld * dWorldCreate()
1335{
1336 dxWorld *w = new dxWorld;
1337 w->firstbody = 0;
1338 w->firstjoint = 0;
1339 w->nb = 0;
1340 w->nj = 0;
1341 dSetZero (w->gravity,4);
1342 w->global_erp = REAL(0.2);
1343#if defined(dSINGLE)
1344 w->global_cfm = 1e-5f;
1345#elif defined(dDOUBLE)
1346 w->global_cfm = 1e-10;
1347#else
1348 #error dSINGLE or dDOUBLE must be defined
1349#endif
1350
1351 w->adis.idle_steps = 10;
1352 w->adis.idle_time = 0;
1353 w->adis_flag = 0;
1354 w->adis.average_samples = 1; // Default is 1 sample => Instantaneous velocity
1355 w->adis.angular_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
1356 w->adis.linear_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
1357
1358 w->qs.num_iterations = 20;
1359 w->qs.w = REAL(1.3);
1360
1361 w->contactp.max_vel = dInfinity;
1362 w->contactp.min_depth = 0;
1363
1364 return w;
1365}
1366
1367
1368void dWorldDestroy (dxWorld *w)
1369{
1370 // delete all bodies and joints
1371 dAASSERT (w);
1372 dxBody *nextb, *b = w->firstbody;
1373 while (b) {
1374 nextb = (dxBody*) b->next;
1375 if(b->average_lvel_buffer)
1376 {
1377 delete[] (b->average_lvel_buffer);
1378 b->average_lvel_buffer = 0;
1379 }
1380 if(b->average_avel_buffer)
1381 {
1382 delete[] (b->average_avel_buffer);
1383 b->average_avel_buffer = 0;
1384 }
1385 dBodyDestroy(b); // calling here dBodyDestroy for correct destroying! (i.e. the average buffers)
1386 b = nextb;
1387 }
1388 dxJoint *nextj, *j = w->firstjoint;
1389 while (j) {
1390 nextj = (dxJoint*)j->next;
1391 if (j->flags & dJOINT_INGROUP) {
1392 // the joint is part of a group, so "deactivate" it instead
1393 j->world = 0;
1394 j->node[0].body = 0;
1395 j->node[0].next = 0;
1396 j->node[1].body = 0;
1397 j->node[1].next = 0;
1398 dMessage (0,"warning: destroying world containing grouped joints");
1399 }
1400 else {
1401 dFree (j,j->vtable->size);
1402 }
1403 j = nextj;
1404 }
1405 delete w;
1406}
1407
1408
1409void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
1410{
1411 dAASSERT (w);
1412 w->gravity[0] = x;
1413 w->gravity[1] = y;
1414 w->gravity[2] = z;
1415}
1416
1417
1418void dWorldGetGravity (dWorldID w, dVector3 g)
1419{
1420 dAASSERT (w);
1421 g[0] = w->gravity[0];
1422 g[1] = w->gravity[1];
1423 g[2] = w->gravity[2];
1424}
1425
1426
1427void dWorldSetERP (dWorldID w, dReal erp)
1428{
1429 dAASSERT (w);
1430 w->global_erp = erp;
1431}
1432
1433
1434dReal dWorldGetERP (dWorldID w)
1435{
1436 dAASSERT (w);
1437 return w->global_erp;
1438}
1439
1440
1441void dWorldSetCFM (dWorldID w, dReal cfm)
1442{
1443 dAASSERT (w);
1444 w->global_cfm = cfm;
1445}
1446
1447
1448dReal dWorldGetCFM (dWorldID w)
1449{
1450 dAASSERT (w);
1451 return w->global_cfm;
1452}
1453
1454
1455void dWorldStep (dWorldID w, dReal stepsize)
1456{
1457 dUASSERT (w,"bad world argument");
1458 dUASSERT (stepsize > 0,"stepsize must be > 0");
1459 dxProcessIslands (w,stepsize,&dInternalStepIsland);
1460}
1461
1462
1463void dWorldQuickStep (dWorldID w, dReal stepsize)
1464{
1465 dUASSERT (w,"bad world argument");
1466 dUASSERT (stepsize > 0,"stepsize must be > 0");
1467 dxProcessIslands (w,stepsize,&dxQuickStepper);
1468}
1469
1470
1471void dWorldImpulseToForce (dWorldID w, dReal stepsize,
1472 dReal ix, dReal iy, dReal iz,
1473 dVector3 force)
1474{
1475 dAASSERT (w);
1476 stepsize = dRecip(stepsize);
1477 force[0] = stepsize * ix;
1478 force[1] = stepsize * iy;
1479 force[2] = stepsize * iz;
1480 // @@@ force[3] = 0;
1481}
1482
1483
1484// world auto-disable functions
1485
1486dReal dWorldGetAutoDisableLinearThreshold (dWorldID w)
1487{
1488 dAASSERT(w);
1489 return dSqrt (w->adis.linear_average_threshold);
1490}
1491
1492
1493void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_average_threshold)
1494{
1495 dAASSERT(w);
1496 w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
1497}
1498
1499
1500dReal dWorldGetAutoDisableAngularThreshold (dWorldID w)
1501{
1502 dAASSERT(w);
1503 return dSqrt (w->adis.angular_average_threshold);
1504}
1505
1506
1507void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_average_threshold)
1508{
1509 dAASSERT(w);
1510 w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
1511}
1512
1513
1514int dWorldGetAutoDisableAverageSamplesCount (dWorldID w)
1515{
1516 dAASSERT(w);
1517 return w->adis.average_samples;
1518}
1519
1520
1521void dWorldSetAutoDisableAverageSamplesCount (dWorldID w, unsigned int average_samples_count)
1522{
1523 dAASSERT(w);
1524 w->adis.average_samples = average_samples_count;
1525}
1526
1527
1528int dWorldGetAutoDisableSteps (dWorldID w)
1529{
1530 dAASSERT(w);
1531 return w->adis.idle_steps;
1532}
1533
1534
1535void dWorldSetAutoDisableSteps (dWorldID w, int steps)
1536{
1537 dAASSERT(w);
1538 w->adis.idle_steps = steps;
1539}
1540
1541
1542dReal dWorldGetAutoDisableTime (dWorldID w)
1543{
1544 dAASSERT(w);
1545 return w->adis.idle_time;
1546}
1547
1548
1549void dWorldSetAutoDisableTime (dWorldID w, dReal time)
1550{
1551 dAASSERT(w);
1552 w->adis.idle_time = time;
1553}
1554
1555
1556int dWorldGetAutoDisableFlag (dWorldID w)
1557{
1558 dAASSERT(w);
1559 return w->adis_flag;
1560}
1561
1562
1563void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable)
1564{
1565 dAASSERT(w);
1566 w->adis_flag = (do_auto_disable != 0);
1567}
1568
1569
1570void dWorldSetQuickStepNumIterations (dWorldID w, int num)
1571{
1572 dAASSERT(w);
1573 w->qs.num_iterations = num;
1574}
1575
1576
1577int dWorldGetQuickStepNumIterations (dWorldID w)
1578{
1579 dAASSERT(w);
1580 return w->qs.num_iterations;
1581}
1582
1583
1584void dWorldSetQuickStepW (dWorldID w, dReal param)
1585{
1586 dAASSERT(w);
1587 w->qs.w = param;
1588}
1589
1590
1591dReal dWorldGetQuickStepW (dWorldID w)
1592{
1593 dAASSERT(w);
1594 return w->qs.w;
1595}
1596
1597
1598void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel)
1599{
1600 dAASSERT(w);
1601 w->contactp.max_vel = vel;
1602}
1603
1604
1605dReal dWorldGetContactMaxCorrectingVel (dWorldID w)
1606{
1607 dAASSERT(w);
1608 return w->contactp.max_vel;
1609}
1610
1611
1612void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth)
1613{
1614 dAASSERT(w);
1615 w->contactp.min_depth = depth;
1616}
1617
1618
1619dReal dWorldGetContactSurfaceLayer (dWorldID w)
1620{
1621 dAASSERT(w);
1622 return w->contactp.min_depth;
1623}
1624
1625//****************************************************************************
1626// testing
1627
1628#define NUM 100
1629
1630#define DO(x)
1631
1632
1633extern "C" void dTestDataStructures()
1634{
1635 int i;
1636 DO(printf ("testDynamicsStuff()\n"));
1637
1638 dBodyID body [NUM];
1639 int nb = 0;
1640 dJointID joint [NUM];
1641 int nj = 0;
1642
1643 for (i=0; i<NUM; i++) body[i] = 0;
1644 for (i=0; i<NUM; i++) joint[i] = 0;
1645
1646 DO(printf ("creating world\n"));
1647 dWorldID w = dWorldCreate();
1648 checkWorld (w);
1649
1650 for (;;) {
1651 if (nb < NUM && dRandReal() > 0.5) {
1652 DO(printf ("creating body\n"));
1653 body[nb] = dBodyCreate (w);
1654 DO(printf ("\t--> %p\n",body[nb]));
1655 nb++;
1656 checkWorld (w);
1657 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1658 }
1659 if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
1660 dBodyID b1 = body [dRand() % nb];
1661 dBodyID b2 = body [dRand() % nb];
1662 if (b1 != b2) {
1663 DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
1664 joint[nj] = dJointCreateBall (w,0);
1665 DO(printf ("\t-->%p\n",joint[nj]));
1666 checkWorld (w);
1667 dJointAttach (joint[nj],b1,b2);
1668 nj++;
1669 checkWorld (w);
1670 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1671 }
1672 }
1673 if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
1674 dBodyID b1 = body [dRand() % nb];
1675 dBodyID b2 = body [dRand() % nb];
1676 if (b1 != b2) {
1677 int k = dRand() % nj;
1678 DO(printf ("reattaching joint %p\n",joint[k]));
1679 dJointAttach (joint[k],b1,b2);
1680 checkWorld (w);
1681 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1682 }
1683 }
1684 if (nb > 0 && dRandReal() > 0.5) {
1685 int k = dRand() % nb;
1686 DO(printf ("destroying body %p\n",body[k]));
1687 dBodyDestroy (body[k]);
1688 checkWorld (w);
1689 for (; k < (NUM-1); k++) body[k] = body[k+1];
1690 nb--;
1691 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1692 }
1693 if (nj > 0 && dRandReal() > 0.5) {
1694 int k = dRand() % nj;
1695 DO(printf ("destroying joint %p\n",joint[k]));
1696 dJointDestroy (joint[k]);
1697 checkWorld (w);
1698 for (; k < (NUM-1); k++) joint[k] = joint[k+1];
1699 nj--;
1700 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1701 }
1702 }
1703
1704 /*
1705 printf ("creating world\n");
1706 dWorldID w = dWorldCreate();
1707 checkWorld (w);
1708 printf ("creating body\n");
1709 dBodyID b1 = dBodyCreate (w);
1710 checkWorld (w);
1711 printf ("creating body\n");
1712 dBodyID b2 = dBodyCreate (w);
1713 checkWorld (w);
1714 printf ("creating joint\n");
1715 dJointID j = dJointCreateBall (w);
1716 checkWorld (w);
1717 printf ("attaching joint\n");
1718 dJointAttach (j,b1,b2);
1719 checkWorld (w);
1720 printf ("destroying joint\n");
1721 dJointDestroy (j);
1722 checkWorld (w);
1723 printf ("destroying body\n");
1724 dBodyDestroy (b1);
1725 checkWorld (w);
1726 printf ("destroying body\n");
1727 dBodyDestroy (b2);
1728 checkWorld (w);
1729 printf ("destroying world\n");
1730 dWorldDestroy (w);
1731 */
1732}
diff --git a/libraries/ode-0.9/ode/src/odemath.cpp b/libraries/ode-0.9/ode/src/odemath.cpp
new file mode 100644
index 0000000..11bc84e
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/odemath.cpp
@@ -0,0 +1,178 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/common.h>
24#include <ode/odemath.h>
25
26// get some math functions under windows
27#ifdef WIN32
28#include <float.h>
29#ifndef CYGWIN // added by andy for cygwin
30#undef copysign
31#define copysign(a,b) ((dReal)_copysign(a,b))
32#endif // added by andy for cygwin
33#endif
34
35#undef dNormalize3
36#undef dNormalize4
37
38
39// this may be called for vectors `a' with extremely small magnitude, for
40// example the result of a cross product on two nearly perpendicular vectors.
41// we must be robust to these small vectors. to prevent numerical error,
42// first find the component a[i] with the largest magnitude and then scale
43// all the components by 1/a[i]. then we can compute the length of `a' and
44// scale the components by 1/l. this has been verified to work with vectors
45// containing the smallest representable numbers.
46
47int dSafeNormalize3 (dVector3 a)
48{
49 dReal a0,a1,a2,aa0,aa1,aa2,l;
50 dAASSERT (a);
51 a0 = a[0];
52 a1 = a[1];
53 a2 = a[2];
54 aa0 = dFabs(a0);
55 aa1 = dFabs(a1);
56 aa2 = dFabs(a2);
57 if (aa1 > aa0) {
58 if (aa2 > aa1) {
59 goto aa2_largest;
60 }
61 else { // aa1 is largest
62 a0 /= aa1;
63 a2 /= aa1;
64 l = dRecipSqrt (a0*a0 + a2*a2 + 1);
65 a[0] = a0*l;
66 a[1] = dCopySign(l,a1);
67 a[2] = a2*l;
68 }
69 }
70 else {
71 if (aa2 > aa0) {
72 aa2_largest: // aa2 is largest
73 a0 /= aa2;
74 a1 /= aa2;
75 l = dRecipSqrt (a0*a0 + a1*a1 + 1);
76 a[0] = a0*l;
77 a[1] = a1*l;
78 a[2] = dCopySign(l,a2);
79 }
80 else { // aa0 is largest
81 if (aa0 <= 0) {
82 a[0] = 1; // if all a's are zero, this is where we'll end up.
83 a[1] = 0; // return a default unit length vector.
84 a[2] = 0;
85 return 0;
86 }
87 a1 /= aa0;
88 a2 /= aa0;
89 l = dRecipSqrt (a1*a1 + a2*a2 + 1);
90 a[0] = dCopySign(l,a0);
91 a[1] = a1*l;
92 a[2] = a2*l;
93 }
94 }
95 return 1;
96}
97
98/* OLD VERSION */
99/*
100void dNormalize3 (dVector3 a)
101{
102 dIASSERT (a);
103 dReal l = dDOT(a,a);
104 if (l > 0) {
105 l = dRecipSqrt(l);
106 a[0] *= l;
107 a[1] *= l;
108 a[2] *= l;
109 }
110 else {
111 a[0] = 1;
112 a[1] = 0;
113 a[2] = 0;
114 }
115}
116*/
117
118void dNormalize3(dVector3 a)
119{
120 _dNormalize3(a);
121}
122
123
124int dSafeNormalize4 (dVector4 a)
125{
126 dAASSERT (a);
127 dReal l = dDOT(a,a)+a[3]*a[3];
128 if (l > 0) {
129 l = dRecipSqrt(l);
130 a[0] *= l;
131 a[1] *= l;
132 a[2] *= l;
133 a[3] *= l;
134 return 1;
135 }
136 else {
137 a[0] = 1;
138 a[1] = 0;
139 a[2] = 0;
140 a[3] = 0;
141 return 0;
142 }
143}
144
145void dNormalize4(dVector4 a)
146{
147 _dNormalize4(a);
148}
149
150
151void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
152{
153 dAASSERT (n && p && q);
154 if (dFabs(n[2]) > M_SQRT1_2) {
155 // choose p in y-z plane
156 dReal a = n[1]*n[1] + n[2]*n[2];
157 dReal k = dRecipSqrt (a);
158 p[0] = 0;
159 p[1] = -n[2]*k;
160 p[2] = n[1]*k;
161 // set q = n x p
162 q[0] = a*k;
163 q[1] = -n[0]*p[2];
164 q[2] = n[0]*p[1];
165 }
166 else {
167 // choose p in x-y plane
168 dReal a = n[0]*n[0] + n[1]*n[1];
169 dReal k = dRecipSqrt (a);
170 p[0] = -n[1]*k;
171 p[1] = n[0]*k;
172 p[2] = 0;
173 // set q = n x p
174 q[0] = -n[2]*p[1];
175 q[1] = n[2]*p[0];
176 q[2] = a*k;
177 }
178}
diff --git a/libraries/ode-0.9/ode/src/plane.cpp b/libraries/ode-0.9/ode/src/plane.cpp
new file mode 100644
index 0000000..e3afb49
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/plane.cpp
@@ -0,0 +1,145 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::g1 and dContactGeom::g2.
29
30*/
31
32#include <ode/common.h>
33#include <ode/collision.h>
34#include <ode/matrix.h>
35#include <ode/rotation.h>
36#include <ode/odemath.h>
37#include "collision_kernel.h"
38#include "collision_std.h"
39#include "collision_util.h"
40
41#ifdef _MSC_VER
42#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43#endif
44
45//****************************************************************************
46// plane public API
47
48static void make_sure_plane_normal_has_unit_length (dxPlane *g)
49{
50 dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2];
51 if (l > 0) {
52 l = dRecipSqrt(l);
53 g->p[0] *= l;
54 g->p[1] *= l;
55 g->p[2] *= l;
56 g->p[3] *= l;
57 }
58 else {
59 g->p[0] = 1;
60 g->p[1] = 0;
61 g->p[2] = 0;
62 g->p[3] = 0;
63 }
64}
65
66
67dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) :
68 dxGeom (space,0)
69{
70 type = dPlaneClass;
71 p[0] = a;
72 p[1] = b;
73 p[2] = c;
74 p[3] = d;
75 make_sure_plane_normal_has_unit_length (this);
76}
77
78
79void dxPlane::computeAABB()
80{
81 aabb[0] = -dInfinity;
82 aabb[1] = dInfinity;
83 aabb[2] = -dInfinity;
84 aabb[3] = dInfinity;
85 aabb[4] = -dInfinity;
86 aabb[5] = dInfinity;
87
88 // Planes that have normal vectors aligned along an axis can use a
89 // less comprehensive (half space) bounding box.
90
91 if ( p[1] == 0.0f && p[2] == 0.0f ) {
92 // normal aligned with x-axis
93 aabb[0] = (p[0] > 0) ? -dInfinity : -p[3];
94 aabb[1] = (p[0] > 0) ? p[3] : dInfinity;
95 } else
96 if ( p[0] == 0.0f && p[2] == 0.0f ) {
97 // normal aligned with y-axis
98 aabb[2] = (p[1] > 0) ? -dInfinity : -p[3];
99 aabb[3] = (p[1] > 0) ? p[3] : dInfinity;
100 } else
101 if ( p[0] == 0.0f && p[1] == 0.0f ) {
102 // normal aligned with z-axis
103 aabb[4] = (p[2] > 0) ? -dInfinity : -p[3];
104 aabb[5] = (p[2] > 0) ? p[3] : dInfinity;
105 }
106}
107
108
109dGeomID dCreatePlane (dSpaceID space,
110 dReal a, dReal b, dReal c, dReal d)
111{
112 return new dxPlane (space,a,b,c,d);
113}
114
115
116void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d)
117{
118 dUASSERT (g && g->type == dPlaneClass,"argument not a plane");
119 dxPlane *p = (dxPlane*) g;
120 p->p[0] = a;
121 p->p[1] = b;
122 p->p[2] = c;
123 p->p[3] = d;
124 make_sure_plane_normal_has_unit_length (p);
125 dGeomMoved (g);
126}
127
128
129void dGeomPlaneGetParams (dGeomID g, dVector4 result)
130{
131 dUASSERT (g && g->type == dPlaneClass,"argument not a plane");
132 dxPlane *p = (dxPlane*) g;
133 result[0] = p->p[0];
134 result[1] = p->p[1];
135 result[2] = p->p[2];
136 result[3] = p->p[3];
137}
138
139
140dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z)
141{
142 dUASSERT (g && g->type == dPlaneClass,"argument not a plane");
143 dxPlane *p = (dxPlane*) g;
144 return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z;
145}
diff --git a/libraries/ode-0.9/ode/src/quickstep.cpp b/libraries/ode-0.9/ode/src/quickstep.cpp
new file mode 100644
index 0000000..07aa9f7
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/quickstep.cpp
@@ -0,0 +1,880 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include "objects.h"
24#include "joint.h"
25#include <ode/config.h>
26#include <ode/odemath.h>
27#include <ode/rotation.h>
28#include <ode/timer.h>
29#include <ode/error.h>
30#include <ode/matrix.h>
31#include <ode/misc.h>
32#include "lcp.h"
33#include "util.h"
34
35#define ALLOCA dALLOCA16
36
37typedef const dReal *dRealPtr;
38typedef dReal *dRealMutablePtr;
39#define dRealArray(name,n) dReal name[n];
40#define dRealAllocaArray(name,n) dReal *name = (dReal*) ALLOCA ((n)*sizeof(dReal));
41
42//***************************************************************************
43// configuration
44
45// for the SOR and CG methods:
46// uncomment the following line to use warm starting. this definitely
47// help for motor-driven joints. unfortunately it appears to hurt
48// with high-friction contacts using the SOR method. use with care
49
50//#define WARM_STARTING 1
51
52
53// for the SOR method:
54// uncomment the following line to determine a new constraint-solving
55// order for each iteration. however, the qsort per iteration is expensive,
56// and the optimal order is somewhat problem dependent.
57// @@@ try the leaf->root ordering.
58
59//#define REORDER_CONSTRAINTS 1
60
61
62// for the SOR method:
63// uncomment the following line to randomly reorder constraint rows
64// during the solution. depending on the situation, this can help a lot
65// or hardly at all, but it doesn't seem to hurt.
66
67#define RANDOMLY_REORDER_CONSTRAINTS 1
68
69//****************************************************************************
70// special matrix multipliers
71
72// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q)
73static void Multiply1_12q1 (dReal *A, dReal *B, dReal *C, int q)
74{
75 int k;
76 dReal sum;
77 dIASSERT (q>0 && A && B && C);
78 sum = 0;
79 for (k=0; k<q; k++) sum += B[k*12] * C[k];
80 A[0] = sum;
81 sum = 0;
82 for (k=0; k<q; k++) sum += B[1+k*12] * C[k];
83 A[1] = sum;
84 sum = 0;
85 for (k=0; k<q; k++) sum += B[2+k*12] * C[k];
86 A[2] = sum;
87 sum = 0;
88 for (k=0; k<q; k++) sum += B[3+k*12] * C[k];
89 A[3] = sum;
90 sum = 0;
91 for (k=0; k<q; k++) sum += B[4+k*12] * C[k];
92 A[4] = sum;
93 sum = 0;
94 for (k=0; k<q; k++) sum += B[5+k*12] * C[k];
95 A[5] = sum;
96}
97
98//***************************************************************************
99// testing stuff
100
101#ifdef TIMING
102#define IFTIMING(x) x
103#else
104#define IFTIMING(x) /* */
105#endif
106
107//***************************************************************************
108// various common computations involving the matrix J
109
110// compute iMJ = inv(M)*J'
111
112static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
113 dxBody * const *body, dRealPtr invI)
114{
115 int i,j;
116 dRealMutablePtr iMJ_ptr = iMJ;
117 dRealMutablePtr J_ptr = J;
118 for (i=0; i<m; i++) {
119 int b1 = jb[i*2];
120 int b2 = jb[i*2+1];
121 dReal k = body[b1]->invMass;
122 for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
123 dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
124 if (b2 >= 0) {
125 k = body[b2]->invMass;
126 for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
127 dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
128 }
129 J_ptr += 12;
130 iMJ_ptr += 12;
131 }
132}
133
134
135// compute out = inv(M)*J'*in.
136
137static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
138 dRealMutablePtr in, dRealMutablePtr out)
139{
140 int i,j;
141 dSetZero (out,6*nb);
142 dRealPtr iMJ_ptr = iMJ;
143 for (i=0; i<m; i++) {
144 int b1 = jb[i*2];
145 int b2 = jb[i*2+1];
146 dRealMutablePtr out_ptr = out + b1*6;
147 for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
148 iMJ_ptr += 6;
149 if (b2 >= 0) {
150 out_ptr = out + b2*6;
151 for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
152 }
153 iMJ_ptr += 6;
154 }
155}
156
157
158// compute out = J*in.
159
160static void multiply_J (int m, dRealMutablePtr J, int *jb,
161 dRealMutablePtr in, dRealMutablePtr out)
162{
163 int i,j;
164 dRealPtr J_ptr = J;
165 for (i=0; i<m; i++) {
166 int b1 = jb[i*2];
167 int b2 = jb[i*2+1];
168 dReal sum = 0;
169 dRealMutablePtr in_ptr = in + b1*6;
170 for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
171 J_ptr += 6;
172 if (b2 >= 0) {
173 in_ptr = in + b2*6;
174 for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
175 }
176 J_ptr += 6;
177 out[i] = sum;
178 }
179}
180
181
182// compute out = (J*inv(M)*J' + cfm)*in.
183// use z as an nb*6 temporary.
184
185static void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
186 dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out)
187{
188 multiply_invM_JT (m,nb,iMJ,jb,in,z);
189 multiply_J (m,J,jb,z,out);
190
191 // add cfm
192 for (int i=0; i<m; i++) out[i] += cfm[i] * in[i];
193}
194
195//***************************************************************************
196// conjugate gradient method with jacobi preconditioner
197// THIS IS EXPERIMENTAL CODE that doesn't work too well, so it is ifdefed out.
198//
199// adding CFM seems to be critically important to this method.
200
201#if 0
202
203static inline dReal dot (int n, dRealPtr x, dRealPtr y)
204{
205 dReal sum=0;
206 for (int i=0; i<n; i++) sum += x[i]*y[i];
207 return sum;
208}
209
210
211// x = y + z*alpha
212
213static inline void add (int n, dRealMutablePtr x, dRealPtr y, dRealPtr z, dReal alpha)
214{
215 for (int i=0; i<n; i++) x[i] = y[i] + z[i]*alpha;
216}
217
218
219static void CG_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
220 dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
221 dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
222 dxQuickStepParameters *qs)
223{
224 int i,j;
225 const int num_iterations = qs->num_iterations;
226
227 // precompute iMJ = inv(M)*J'
228 dRealAllocaArray (iMJ,m*12);
229 compute_invM_JT (m,J,iMJ,jb,body,invI);
230
231 dReal last_rho = 0;
232 dRealAllocaArray (r,m);
233 dRealAllocaArray (z,m);
234 dRealAllocaArray (p,m);
235 dRealAllocaArray (q,m);
236
237 // precompute 1 / diagonals of A
238 dRealAllocaArray (Ad,m);
239 dRealPtr iMJ_ptr = iMJ;
240 dRealPtr J_ptr = J;
241 for (i=0; i<m; i++) {
242 dReal sum = 0;
243 for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
244 if (jb[i*2+1] >= 0) {
245 for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
246 }
247 iMJ_ptr += 12;
248 J_ptr += 12;
249 Ad[i] = REAL(1.0) / (sum + cfm[i]);
250 }
251
252#ifdef WARM_STARTING
253 // compute residual r = b - A*lambda
254 multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
255 for (i=0; i<m; i++) r[i] = b[i] - r[i];
256#else
257 dSetZero (lambda,m);
258 memcpy (r,b,m*sizeof(dReal)); // residual r = b - A*lambda
259#endif
260
261 for (int iteration=0; iteration < num_iterations; iteration++) {
262 for (i=0; i<m; i++) z[i] = r[i]*Ad[i]; // z = inv(M)*r
263 dReal rho = dot (m,r,z); // rho = r'*z
264
265 // @@@
266 // we must check for convergence, otherwise rho will go to 0 if
267 // we get an exact solution, which will introduce NaNs into the equations.
268 if (rho < 1e-10) {
269 printf ("CG returned at iteration %d\n",iteration);
270 break;
271 }
272
273 if (iteration==0) {
274 memcpy (p,z,m*sizeof(dReal)); // p = z
275 }
276 else {
277 add (m,p,z,p,rho/last_rho); // p = z + (rho/last_rho)*p
278 }
279
280 // compute q = (J*inv(M)*J')*p
281 multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,p,q);
282
283 dReal alpha = rho/dot (m,p,q); // alpha = rho/(p'*q)
284 add (m,lambda,lambda,p,alpha); // lambda = lambda + alpha*p
285 add (m,r,r,q,-alpha); // r = r - alpha*q
286 last_rho = rho;
287 }
288
289 // compute fc = inv(M)*J'*lambda
290 multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
291
292#if 0
293 // measure solution error
294 multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
295 dReal error = 0;
296 for (i=0; i<m; i++) error += dFabs(r[i] - b[i]);
297 printf ("lambda error = %10.6e\n",error);
298#endif
299}
300
301#endif
302
303//***************************************************************************
304// SOR-LCP method
305
306// nb is the number of bodies in the body array.
307// J is an m*12 matrix of constraint rows
308// jb is an array of first and second body numbers for each constraint row
309// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
310//
311// this returns lambda and fc (the constraint force).
312// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
313//
314// b, lo and hi are modified on exit
315
316
317struct IndexError {
318 dReal error; // error to sort on
319 int findex;
320 int index; // row index
321};
322
323
324#ifdef REORDER_CONSTRAINTS
325
326static int compare_index_error (const void *a, const void *b)
327{
328 const IndexError *i1 = (IndexError*) a;
329 const IndexError *i2 = (IndexError*) b;
330 if (i1->findex < 0 && i2->findex >= 0) return -1;
331 if (i1->findex >= 0 && i2->findex < 0) return 1;
332 if (i1->error < i2->error) return -1;
333 if (i1->error > i2->error) return 1;
334 return 0;
335}
336
337#endif
338
339
340static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
341 dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
342 dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
343 dxQuickStepParameters *qs)
344{
345 const int num_iterations = qs->num_iterations;
346 const dReal sor_w = qs->w; // SOR over-relaxation parameter
347
348 int i,j;
349
350#ifdef WARM_STARTING
351 // for warm starting, this seems to be necessary to prevent
352 // jerkiness in motor-driven joints. i have no idea why this works.
353 for (i=0; i<m; i++) lambda[i] *= 0.9;
354#else
355 dSetZero (lambda,m);
356#endif
357
358#ifdef REORDER_CONSTRAINTS
359 // the lambda computed at the previous iteration.
360 // this is used to measure error for when we are reordering the indexes.
361 dRealAllocaArray (last_lambda,m);
362#endif
363
364 // a copy of the 'hi' vector in case findex[] is being used
365 dRealAllocaArray (hicopy,m);
366 memcpy (hicopy,hi,m*sizeof(dReal));
367
368 // precompute iMJ = inv(M)*J'
369 dRealAllocaArray (iMJ,m*12);
370 compute_invM_JT (m,J,iMJ,jb,body,invI);
371
372 // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
373 // as we change lambda.
374#ifdef WARM_STARTING
375 multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
376#else
377 dSetZero (fc,nb*6);
378#endif
379
380 // precompute 1 / diagonals of A
381 dRealAllocaArray (Ad,m);
382 dRealPtr iMJ_ptr = iMJ;
383 dRealMutablePtr J_ptr = J;
384 for (i=0; i<m; i++) {
385 dReal sum = 0;
386 for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
387 if (jb[i*2+1] >= 0) {
388 for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
389 }
390 iMJ_ptr += 12;
391 J_ptr += 12;
392 Ad[i] = sor_w / (sum + cfm[i]);
393 }
394
395 // scale J and b by Ad
396 J_ptr = J;
397 for (i=0; i<m; i++) {
398 for (j=0; j<12; j++) {
399 J_ptr[0] *= Ad[i];
400 J_ptr++;
401 }
402 b[i] *= Ad[i];
403
404 // scale Ad by CFM. N.B. this should be done last since it is used above
405 Ad[i] *= cfm[i];
406 }
407
408 // order to solve constraint rows in
409 IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
410
411#ifndef REORDER_CONSTRAINTS
412 // make sure constraints with findex < 0 come first.
413 j=0;
414 int k=1;
415
416 // Fill the array from both ends
417 for (i=0; i<m; i++)
418 if (findex[i] < 0)
419 order[j++].index = i; // Place them at the front
420 else
421 order[m-k++].index = i; // Place them at the end
422
423 dIASSERT ((j+k-1)==m); // -1 since k was started at 1 and not 0
424#endif
425
426 for (int iteration=0; iteration < num_iterations; iteration++) {
427
428#ifdef REORDER_CONSTRAINTS
429 // constraints with findex < 0 always come first.
430 if (iteration < 2) {
431 // for the first two iterations, solve the constraints in
432 // the given order
433 for (i=0; i<m; i++) {
434 order[i].error = i;
435 order[i].findex = findex[i];
436 order[i].index = i;
437 }
438 }
439 else {
440 // sort the constraints so that the ones converging slowest
441 // get solved last. use the absolute (not relative) error.
442 for (i=0; i<m; i++) {
443 dReal v1 = dFabs (lambda[i]);
444 dReal v2 = dFabs (last_lambda[i]);
445 dReal max = (v1 > v2) ? v1 : v2;
446 if (max > 0) {
447 //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
448 order[i].error = dFabs(lambda[i]-last_lambda[i]);
449 }
450 else {
451 order[i].error = dInfinity;
452 }
453 order[i].findex = findex[i];
454 order[i].index = i;
455 }
456 }
457 qsort (order,m,sizeof(IndexError),&compare_index_error);
458
459 //@@@ potential optimization: swap lambda and last_lambda pointers rather
460 // than copying the data. we must make sure lambda is properly
461 // returned to the caller
462 memcpy (last_lambda,lambda,m*sizeof(dReal));
463#endif
464#ifdef RANDOMLY_REORDER_CONSTRAINTS
465 if ((iteration & 7) == 0) {
466 for (i=1; i<m; ++i) {
467 IndexError tmp = order[i];
468 int swapi = dRandInt(i+1);
469 order[i] = order[swapi];
470 order[swapi] = tmp;
471 }
472 }
473#endif
474
475 for (int i=0; i<m; i++) {
476 // @@@ potential optimization: we could pre-sort J and iMJ, thereby
477 // linearizing access to those arrays. hmmm, this does not seem
478 // like a win, but we should think carefully about our memory
479 // access pattern.
480
481 int index = order[i].index;
482 J_ptr = J + index*12;
483 iMJ_ptr = iMJ + index*12;
484
485 // set the limits for this constraint. note that 'hicopy' is used.
486 // this is the place where the QuickStep method differs from the
487 // direct LCP solving method, since that method only performs this
488 // limit adjustment once per time step, whereas this method performs
489 // once per iteration per constraint row.
490 // the constraints are ordered so that all lambda[] values needed have
491 // already been computed.
492 if (findex[index] >= 0) {
493 hi[index] = dFabs (hicopy[index] * lambda[findex[index]]);
494 lo[index] = -hi[index];
495 }
496
497 int b1 = jb[index*2];
498 int b2 = jb[index*2+1];
499 dReal delta = b[index] - lambda[index]*Ad[index];
500 dRealMutablePtr fc_ptr = fc + 6*b1;
501
502 // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
503 delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
504 fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
505 fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
506 // @@@ potential optimization: handle 1-body constraints in a separate
507 // loop to avoid the cost of test & jump?
508 if (b2 >= 0) {
509 fc_ptr = fc + 6*b2;
510 delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
511 fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
512 fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
513 }
514
515 // compute lambda and clamp it to [lo,hi].
516 // @@@ potential optimization: does SSE have clamping instructions
517 // to save test+jump penalties here?
518 dReal new_lambda = lambda[index] + delta;
519 if (new_lambda < lo[index]) {
520 delta = lo[index]-lambda[index];
521 lambda[index] = lo[index];
522 }
523 else if (new_lambda > hi[index]) {
524 delta = hi[index]-lambda[index];
525 lambda[index] = hi[index];
526 }
527 else {
528 lambda[index] = new_lambda;
529 }
530
531 //@@@ a trick that may or may not help
532 //dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations));
533 //delta *= ramp;
534
535 // update fc.
536 // @@@ potential optimization: SIMD for this and the b2 >= 0 case
537 fc_ptr = fc + 6*b1;
538 fc_ptr[0] += delta * iMJ_ptr[0];
539 fc_ptr[1] += delta * iMJ_ptr[1];
540 fc_ptr[2] += delta * iMJ_ptr[2];
541 fc_ptr[3] += delta * iMJ_ptr[3];
542 fc_ptr[4] += delta * iMJ_ptr[4];
543 fc_ptr[5] += delta * iMJ_ptr[5];
544 // @@@ potential optimization: handle 1-body constraints in a separate
545 // loop to avoid the cost of test & jump?
546 if (b2 >= 0) {
547 fc_ptr = fc + 6*b2;
548 fc_ptr[0] += delta * iMJ_ptr[6];
549 fc_ptr[1] += delta * iMJ_ptr[7];
550 fc_ptr[2] += delta * iMJ_ptr[8];
551 fc_ptr[3] += delta * iMJ_ptr[9];
552 fc_ptr[4] += delta * iMJ_ptr[10];
553 fc_ptr[5] += delta * iMJ_ptr[11];
554 }
555 }
556 }
557}
558
559
560void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
561 dxJoint * const *_joint, int nj, dReal stepsize)
562{
563 int i,j;
564 IFTIMING(dTimerStart("preprocessing");)
565
566 dReal stepsize1 = dRecip(stepsize);
567
568 // number all bodies in the body list - set their tag values
569 for (i=0; i<nb; i++) body[i]->tag = i;
570
571 // make a local copy of the joint array, because we might want to modify it.
572 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
573 // but not the joint array, because the caller might need it unchanged).
574 //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
575 dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
576 memcpy (joint,_joint,nj * sizeof(dxJoint*));
577
578 // for all bodies, compute the inertia tensor and its inverse in the global
579 // frame, and compute the rotational force and add it to the torque
580 // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
581 //dRealAllocaArray (I,3*4*nb); // need to remember all I's for feedback purposes only
582 dRealAllocaArray (invI,3*4*nb);
583 for (i=0; i<nb; i++) {
584 dMatrix3 tmp;
585
586 // compute inverse inertia tensor in global frame
587 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);
588 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);
589#ifdef dGYROSCOPIC
590 dMatrix3 I;
591 // compute inertia tensor in global frame
592 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
593 //dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);
594 dMULTIPLY0_333 (I,body[i]->posr.R,tmp);
595 // compute rotational force
596 //dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
597 dMULTIPLY0_331 (tmp,I,body[i]->avel);
598 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
599#endif
600 }
601
602 // add the gravity force to all bodies
603 for (i=0; i<nb; i++) {
604 if ((body[i]->flags & dxBodyNoGravity)==0) {
605 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
606 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
607 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
608 }
609 }
610
611 // get joint information (m = total constraint dimension, nub = number of unbounded variables).
612 // joints with m=0 are inactive and are removed from the joints array
613 // entirely, so that the code that follows does not consider them.
614 //@@@ do we really need to save all the info1's
615 dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
616 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
617 joint[j]->vtable->getInfo1 (joint[j],info+i);
618 dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
619 if (info[i].m > 0) {
620 joint[i] = joint[j];
621 i++;
622 }
623 }
624 nj = i;
625
626 // create the row offset array
627 int m = 0;
628 int *ofs = (int*) ALLOCA (nj*sizeof(int));
629 for (i=0; i<nj; i++) {
630 ofs[i] = m;
631 m += info[i].m;
632 }
633
634 // if there are constraints, compute the constraint force
635 dRealAllocaArray (J,m*12);
636 int *jb = (int*) ALLOCA (m*2*sizeof(int));
637 if (m > 0) {
638 // create a constraint equation right hand side vector `c', a constraint
639 // force mixing vector `cfm', and LCP low and high bound vectors, and an
640 // 'findex' vector.
641 dRealAllocaArray (c,m);
642 dRealAllocaArray (cfm,m);
643 dRealAllocaArray (lo,m);
644 dRealAllocaArray (hi,m);
645 int *findex = (int*) ALLOCA (m*sizeof(int));
646 dSetZero (c,m);
647 dSetValue (cfm,m,world->global_cfm);
648 dSetValue (lo,m,-dInfinity);
649 dSetValue (hi,m, dInfinity);
650 for (i=0; i<m; i++) findex[i] = -1;
651
652 // get jacobian data from constraints. an m*12 matrix will be created
653 // to store the two jacobian blocks from each constraint. it has this
654 // format:
655 //
656 // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ .
657 // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows)
658 // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
659 // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
660 // etc...
661 //
662 // (lll) = linear jacobian data
663 // (aaa) = angular jacobian data
664 //
665 IFTIMING (dTimerNow ("create J");)
666 dSetZero (J,m*12);
667 dxJoint::Info2 Jinfo;
668 Jinfo.rowskip = 12;
669 Jinfo.fps = stepsize1;
670 Jinfo.erp = world->global_erp;
671 int mfb = 0; // number of rows of Jacobian we will have to save for joint feedback
672 for (i=0; i<nj; i++) {
673 Jinfo.J1l = J + ofs[i]*12;
674 Jinfo.J1a = Jinfo.J1l + 3;
675 Jinfo.J2l = Jinfo.J1l + 6;
676 Jinfo.J2a = Jinfo.J1l + 9;
677 Jinfo.c = c + ofs[i];
678 Jinfo.cfm = cfm + ofs[i];
679 Jinfo.lo = lo + ofs[i];
680 Jinfo.hi = hi + ofs[i];
681 Jinfo.findex = findex + ofs[i];
682 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
683 // adjust returned findex values for global index numbering
684 for (j=0; j<info[i].m; j++) {
685 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
686 }
687 if (joint[i]->feedback)
688 mfb += info[i].m;
689 }
690
691 // we need a copy of Jacobian for joint feedbacks
692 // because it gets destroyed by SOR solver
693 // instead of saving all Jacobian, we can save just rows
694 // for joints, that requested feedback (which is normaly much less)
695 dRealAllocaArray (Jcopy,mfb*12);
696 if (mfb > 0) {
697 mfb = 0;
698 for (i=0; i<nj; i++)
699 if (joint[i]->feedback) {
700 memcpy(Jcopy+mfb*12, J+ofs[i]*12, info[i].m*12*sizeof(dReal));
701 mfb += info[i].m;
702 }
703 }
704
705
706 // create an array of body numbers for each joint row
707 int *jb_ptr = jb;
708 for (i=0; i<nj; i++) {
709 int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->tag) : -1;
710 int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->tag) : -1;
711 for (j=0; j<info[i].m; j++) {
712 jb_ptr[0] = b1;
713 jb_ptr[1] = b2;
714 jb_ptr += 2;
715 }
716 }
717 dIASSERT (jb_ptr == jb+2*m);
718
719 // compute the right hand side `rhs'
720 IFTIMING (dTimerNow ("compute rhs");)
721 dRealAllocaArray (tmp1,nb*6);
722 // put v/h + invM*fe into tmp1
723 for (i=0; i<nb; i++) {
724 dReal body_invMass = body[i]->invMass;
725 for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1;
726 dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
727 for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1;
728 }
729
730 // put J*tmp1 into rhs
731 dRealAllocaArray (rhs,m);
732 multiply_J (m,J,jb,tmp1,rhs);
733
734 // complete rhs
735 for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
736
737 // scale CFM
738 for (i=0; i<m; i++) cfm[i] *= stepsize1;
739
740 // load lambda from the value saved on the previous iteration
741 dRealAllocaArray (lambda,m);
742#ifdef WARM_STARTING
743 dSetZero (lambda,m); //@@@ shouldn't be necessary
744 for (i=0; i<nj; i++) {
745 memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(dReal));
746 }
747#endif
748
749 // solve the LCP problem and get lambda and invM*constraint_force
750 IFTIMING (dTimerNow ("solving LCP problem");)
751 dRealAllocaArray (cforce,nb*6);
752 SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,&world->qs);
753
754#ifdef WARM_STARTING
755 // save lambda for the next iteration
756 //@@@ note that this doesn't work for contact joints yet, as they are
757 // recreated every iteration
758 for (i=0; i<nj; i++) {
759 memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(dReal));
760 }
761#endif
762
763 // note that the SOR method overwrites rhs and J at this point, so
764 // they should not be used again.
765
766 // add stepsize * cforce to the body velocity
767 for (i=0; i<nb; i++) {
768 for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
769 for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
770 }
771
772 // if joint feedback is requested, compute the constraint force.
773 // BUT: cforce is inv(M)*J'*lambda, whereas we want just J'*lambda,
774 // so we must compute M*cforce.
775 // @@@ if any joint has a feedback request we compute the entire
776 // adjusted cforce, which is not the most efficient way to do it.
777 /*for (j=0; j<nj; j++) {
778 if (joint[j]->feedback) {
779 // compute adjusted cforce
780 for (i=0; i<nb; i++) {
781 dReal k = body[i]->mass.mass;
782 cforce [i*6+0] *= k;
783 cforce [i*6+1] *= k;
784 cforce [i*6+2] *= k;
785 dVector3 tmp;
786 dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3);
787 cforce [i*6+3] = tmp[0];
788 cforce [i*6+4] = tmp[1];
789 cforce [i*6+5] = tmp[2];
790 }
791 // compute feedback for this and all remaining joints
792 for (; j<nj; j++) {
793 dJointFeedback *fb = joint[j]->feedback;
794 if (fb) {
795 int b1 = joint[j]->node[0].body->tag;
796 memcpy (fb->f1,cforce+b1*6,3*sizeof(dReal));
797 memcpy (fb->t1,cforce+b1*6+3,3*sizeof(dReal));
798 if (joint[j]->node[1].body) {
799 int b2 = joint[j]->node[1].body->tag;
800 memcpy (fb->f2,cforce+b2*6,3*sizeof(dReal));
801 memcpy (fb->t2,cforce+b2*6+3,3*sizeof(dReal));
802 }
803 }
804 }
805 }
806 }*/
807
808 if (mfb > 0) {
809 // straightforward computation of joint constraint forces:
810 // multiply related lambdas with respective J' block for joints
811 // where feedback was requested
812 mfb = 0;
813 for (i=0; i<nj; i++) {
814 if (joint[i]->feedback) {
815 dJointFeedback *fb = joint[i]->feedback;
816 dReal data[6];
817 Multiply1_12q1 (data, Jcopy+mfb*12, lambda+ofs[i], info[i].m);
818 fb->f1[0] = data[0];
819 fb->f1[1] = data[1];
820 fb->f1[2] = data[2];
821 fb->t1[0] = data[3];
822 fb->t1[1] = data[4];
823 fb->t1[2] = data[5];
824 if (joint[i]->node[1].body)
825 {
826 Multiply1_12q1 (data, Jcopy+mfb*12+6, lambda+ofs[i], info[i].m);
827 fb->f2[0] = data[0];
828 fb->f2[1] = data[1];
829 fb->f2[2] = data[2];
830 fb->t2[0] = data[3];
831 fb->t2[1] = data[4];
832 fb->t2[2] = data[5];
833 }
834 mfb += info[i].m;
835 }
836 }
837 }
838 }
839
840 // compute the velocity update:
841 // add stepsize * invM * fe to the body velocity
842
843 IFTIMING (dTimerNow ("compute velocity update");)
844 for (i=0; i<nb; i++) {
845 dReal body_invMass = body[i]->invMass;
846 for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j];
847 for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;
848 dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc);
849 }
850
851#if 0
852 // check that the updated velocity obeys the constraint (this check needs unmodified J)
853 dRealAllocaArray (vel,nb*6);
854 for (i=0; i<nb; i++) {
855 for (j=0; j<3; j++) vel[i*6+j] = body[i]->lvel[j];
856 for (j=0; j<3; j++) vel[i*6+3+j] = body[i]->avel[j];
857 }
858 dRealAllocaArray (tmp,m);
859 multiply_J (m,J,jb,vel,tmp);
860 dReal error = 0;
861 for (i=0; i<m; i++) error += dFabs(tmp[i]);
862 printf ("velocity error = %10.6e\n",error);
863#endif
864
865 // update the position and orientation from the new linear/angular velocity
866 // (over the given timestep)
867 IFTIMING (dTimerNow ("update position");)
868 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
869
870 IFTIMING (dTimerNow ("tidy up");)
871
872 // zero all force accumulators
873 for (i=0; i<nb; i++) {
874 dSetZero (body[i]->facc,3);
875 dSetZero (body[i]->tacc,3);
876 }
877
878 IFTIMING (dTimerEnd();)
879 IFTIMING (if (m > 0) dTimerReport (stdout,1);)
880}
diff --git a/libraries/ode-0.9/ode/src/quickstep.h b/libraries/ode-0.9/ode/src/quickstep.h
new file mode 100644
index 0000000..43863e7
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/quickstep.h
@@ -0,0 +1,33 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_QUICK_STEP_H_
24#define _ODE_QUICK_STEP_H_
25
26#include <ode/common.h>
27
28
29void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
30 dxJoint * const *_joint, int nj, dReal stepsize);
31
32
33#endif
diff --git a/libraries/ode-0.9/ode/src/ray.cpp b/libraries/ode-0.9/ode/src/ray.cpp
new file mode 100644
index 0000000..adf61ac
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/ray.cpp
@@ -0,0 +1,686 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::g1 and dContactGeom::g2.
29
30*/
31
32#include <ode/common.h>
33#include <ode/collision.h>
34#include <ode/matrix.h>
35#include <ode/rotation.h>
36#include <ode/odemath.h>
37#include "collision_kernel.h"
38#include "collision_std.h"
39#include "collision_util.h"
40
41#ifdef _MSC_VER
42#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43#endif
44
45//****************************************************************************
46// ray public API
47
48dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1)
49{
50 type = dRayClass;
51 length = _length;
52}
53
54
55void dxRay::computeAABB()
56{
57 dVector3 e;
58 e[0] = final_posr->pos[0] + final_posr->R[0*4+2]*length;
59 e[1] = final_posr->pos[1] + final_posr->R[1*4+2]*length;
60 e[2] = final_posr->pos[2] + final_posr->R[2*4+2]*length;
61
62 if (final_posr->pos[0] < e[0]){
63 aabb[0] = final_posr->pos[0];
64 aabb[1] = e[0];
65 }
66 else{
67 aabb[0] = e[0];
68 aabb[1] = final_posr->pos[0];
69 }
70
71 if (final_posr->pos[1] < e[1]){
72 aabb[2] = final_posr->pos[1];
73 aabb[3] = e[1];
74 }
75 else{
76 aabb[2] = e[1];
77 aabb[3] = final_posr->pos[1];
78 }
79
80 if (final_posr->pos[2] < e[2]){
81 aabb[4] = final_posr->pos[2];
82 aabb[5] = e[2];
83 }
84 else{
85 aabb[4] = e[2];
86 aabb[5] = final_posr->pos[2];
87 }
88}
89
90
91dGeomID dCreateRay (dSpaceID space, dReal length)
92{
93 return new dxRay (space,length);
94}
95
96
97void dGeomRaySetLength (dGeomID g, dReal length)
98{
99 dUASSERT (g && g->type == dRayClass,"argument not a ray");
100 dxRay *r = (dxRay*) g;
101 r->length = length;
102 dGeomMoved (g);
103}
104
105
106dReal dGeomRayGetLength (dGeomID g)
107{
108 dUASSERT (g && g->type == dRayClass,"argument not a ray");
109 dxRay *r = (dxRay*) g;
110 return r->length;
111}
112
113
114void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz,
115 dReal dx, dReal dy, dReal dz)
116{
117 dUASSERT (g && g->type == dRayClass,"argument not a ray");
118 g->recomputePosr();
119 dReal* rot = g->final_posr->R;
120 dReal* pos = g->final_posr->pos;
121 dVector3 n;
122 pos[0] = px;
123 pos[1] = py;
124 pos[2] = pz;
125
126 n[0] = dx;
127 n[1] = dy;
128 n[2] = dz;
129 dNormalize3(n);
130 rot[0*4+2] = n[0];
131 rot[1*4+2] = n[1];
132 rot[2*4+2] = n[2];
133 dGeomMoved (g);
134}
135
136
137void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir)
138{
139 dUASSERT (g && g->type == dRayClass,"argument not a ray");
140 g->recomputePosr();
141 start[0] = g->final_posr->pos[0];
142 start[1] = g->final_posr->pos[1];
143 start[2] = g->final_posr->pos[2];
144 dir[0] = g->final_posr->R[0*4+2];
145 dir[1] = g->final_posr->R[1*4+2];
146 dir[2] = g->final_posr->R[2*4+2];
147}
148
149
150void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull)
151{
152 dUASSERT (g && g->type == dRayClass,"argument not a ray");
153
154 if (FirstContact){
155 g->gflags |= RAY_FIRSTCONTACT;
156 }
157 else g->gflags &= ~RAY_FIRSTCONTACT;
158
159 if (BackfaceCull){
160 g->gflags |= RAY_BACKFACECULL;
161 }
162 else g->gflags &= ~RAY_BACKFACECULL;
163}
164
165
166void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull)
167{
168 dUASSERT (g && g->type == dRayClass,"argument not a ray");
169
170 (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0);
171 (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0);
172}
173
174
175void dGeomRaySetClosestHit (dxGeom *g, int closestHit)
176{
177 dUASSERT (g && g->type == dRayClass,"argument not a ray");
178 if (closestHit){
179 g->gflags |= RAY_CLOSEST_HIT;
180 }
181 else g->gflags &= ~RAY_CLOSEST_HIT;
182}
183
184
185int dGeomRayGetClosestHit (dxGeom *g)
186{
187 dUASSERT (g && g->type == dRayClass,"argument not a ray");
188 return ((g->gflags & RAY_CLOSEST_HIT) != 0);
189}
190
191
192
193// if mode==1 then use the sphere exit contact, not the entry contact
194
195static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius,
196 dContactGeom *contact, int mode)
197{
198 dVector3 q;
199 q[0] = ray->final_posr->pos[0] - sphere_pos[0];
200 q[1] = ray->final_posr->pos[1] - sphere_pos[1];
201 q[2] = ray->final_posr->pos[2] - sphere_pos[2];
202 dReal B = dDOT14(q,ray->final_posr->R+2);
203 dReal C = dDOT(q,q) - radius*radius;
204 // note: if C <= 0 then the start of the ray is inside the sphere
205 dReal k = B*B - C;
206 if (k < 0) return 0;
207 k = dSqrt(k);
208 dReal alpha;
209 if (mode && C >= 0) {
210 alpha = -B + k;
211 if (alpha < 0) return 0;
212 }
213 else {
214 alpha = -B - k;
215 if (alpha < 0) {
216 alpha = -B + k;
217 if (alpha < 0) return 0;
218 }
219 }
220 if (alpha > ray->length) return 0;
221 contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
222 contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
223 contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
224 dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0);
225 contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]);
226 contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]);
227 contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]);
228 dNormalize3 (contact->normal);
229 contact->depth = alpha;
230 return 1;
231}
232
233
234int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
235 dContactGeom *contact, int skip)
236{
237 dIASSERT (skip >= (int)sizeof(dContactGeom));
238 dIASSERT (o1->type == dRayClass);
239 dIASSERT (o2->type == dSphereClass);
240 dIASSERT ((flags & NUMC_MASK) >= 1);
241
242 dxRay *ray = (dxRay*) o1;
243 dxSphere *sphere = (dxSphere*) o2;
244 contact->g1 = ray;
245 contact->g2 = sphere;
246 return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0);
247}
248
249
250int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
251 dContactGeom *contact, int skip)
252{
253 dIASSERT (skip >= (int)sizeof(dContactGeom));
254 dIASSERT (o1->type == dRayClass);
255 dIASSERT (o2->type == dBoxClass);
256 dIASSERT ((flags & NUMC_MASK) >= 1);
257
258 dxRay *ray = (dxRay*) o1;
259 dxBox *box = (dxBox*) o2;
260
261 contact->g1 = ray;
262 contact->g2 = box;
263
264 int i;
265
266 // compute the start and delta of the ray relative to the box.
267 // we will do all subsequent computations in this box-relative coordinate
268 // system. we have to do a translation and rotation for each point.
269 dVector3 tmp,s,v;
270 tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
271 tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
272 tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
273 dMULTIPLY1_331 (s,box->final_posr->R,tmp);
274 tmp[0] = ray->final_posr->R[0*4+2];
275 tmp[1] = ray->final_posr->R[1*4+2];
276 tmp[2] = ray->final_posr->R[2*4+2];
277 dMULTIPLY1_331 (v,box->final_posr->R,tmp);
278
279 // mirror the line so that v has all components >= 0
280 dVector3 sign;
281 for (i=0; i<3; i++) {
282 if (v[i] < 0) {
283 s[i] = -s[i];
284 v[i] = -v[i];
285 sign[i] = 1;
286 }
287 else sign[i] = -1;
288 }
289
290 // compute the half-sides of the box
291 dReal h[3];
292 h[0] = REAL(0.5) * box->side[0];
293 h[1] = REAL(0.5) * box->side[1];
294 h[2] = REAL(0.5) * box->side[2];
295
296 // do a few early exit tests
297 if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] ||
298 (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] ||
299 (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] ||
300 (v[0] == 0 && v[1] == 0 && v[2] == 0)) {
301 return 0;
302 }
303
304 // compute the t=[lo..hi] range for where s+v*t intersects the box
305 dReal lo = -dInfinity;
306 dReal hi = dInfinity;
307 int nlo = 0, nhi = 0;
308 for (i=0; i<3; i++) {
309 if (v[i] != 0) {
310 dReal k = (-h[i] - s[i])/v[i];
311 if (k > lo) {
312 lo = k;
313 nlo = i;
314 }
315 k = (h[i] - s[i])/v[i];
316 if (k < hi) {
317 hi = k;
318 nhi = i;
319 }
320 }
321 }
322
323 // check if the ray intersects
324 if (lo > hi) return 0;
325 dReal alpha;
326 int n;
327 if (lo >= 0) {
328 alpha = lo;
329 n = nlo;
330 }
331 else {
332 alpha = hi;
333 n = nhi;
334 }
335 if (alpha < 0 || alpha > ray->length) return 0;
336 contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
337 contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
338 contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
339 contact->normal[0] = box->final_posr->R[0*4+n] * sign[n];
340 contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
341 contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
342 contact->depth = alpha;
343 return 1;
344}
345
346
347int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
348 int flags, dContactGeom *contact, int skip)
349{
350 dIASSERT (skip >= (int)sizeof(dContactGeom));
351 dIASSERT (o1->type == dRayClass);
352 dIASSERT (o2->type == dCapsuleClass);
353 dIASSERT ((flags & NUMC_MASK) >= 1);
354
355 dxRay *ray = (dxRay*) o1;
356 dxCapsule *ccyl = (dxCapsule*) o2;
357
358 contact->g1 = ray;
359 contact->g2 = ccyl;
360 dReal lz2 = ccyl->lz * REAL(0.5);
361
362 // compute some useful info
363 dVector3 cs,q,r;
364 dReal C,k;
365 cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0];
366 cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1];
367 cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2];
368 k = dDOT41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis
369 q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0];
370 q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1];
371 q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2];
372 C = dDOT(q,q) - ccyl->radius*ccyl->radius;
373 // if C < 0 then ray start position within infinite extension of cylinder
374
375 // see if ray start position is inside the capped cylinder
376 int inside_ccyl = 0;
377 if (C < 0) {
378 if (k < -lz2) k = -lz2;
379 else if (k > lz2) k = lz2;
380 r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
381 r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
382 r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
383 if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) +
384 (ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) +
385 (ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) {
386 inside_ccyl = 1;
387 }
388 }
389
390 // compute ray collision with infinite cylinder, except for the case where
391 // the ray is outside the capped cylinder but within the infinite cylinder
392 // (it that case the ray can only hit endcaps)
393 if (!inside_ccyl && C < 0) {
394 // set k to cap position to check
395 if (k < 0) k = -lz2; else k = lz2;
396 }
397 else {
398 dReal uv = dDOT44(ccyl->final_posr->R+2,ray->final_posr->R+2);
399 r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2];
400 r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2];
401 r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2];
402 dReal A = dDOT(r,r);
403 dReal B = 2*dDOT(q,r);
404 k = B*B-4*A*C;
405 if (k < 0) {
406 // the ray does not intersect the infinite cylinder, but if the ray is
407 // inside and parallel to the cylinder axis it may intersect the end
408 // caps. set k to cap position to check.
409 if (!inside_ccyl) return 0;
410 if (uv < 0) k = -lz2; else k = lz2;
411 }
412 else {
413 k = dSqrt(k);
414 A = dRecip (2*A);
415 dReal alpha = (-B-k)*A;
416 if (alpha < 0) {
417 alpha = (-B+k)*A;
418 if (alpha < 0) return 0;
419 }
420 if (alpha > ray->length) return 0;
421
422 // the ray intersects the infinite cylinder. check to see if the
423 // intersection point is between the caps
424 contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
425 contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
426 contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
427 q[0] = contact->pos[0] - ccyl->final_posr->pos[0];
428 q[1] = contact->pos[1] - ccyl->final_posr->pos[1];
429 q[2] = contact->pos[2] - ccyl->final_posr->pos[2];
430 k = dDOT14(q,ccyl->final_posr->R+2);
431 dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0);
432 if (k >= -lz2 && k <= lz2) {
433 contact->normal[0] = nsign * (contact->pos[0] -
434 (ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]));
435 contact->normal[1] = nsign * (contact->pos[1] -
436 (ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]));
437 contact->normal[2] = nsign * (contact->pos[2] -
438 (ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]));
439 dNormalize3 (contact->normal);
440 contact->depth = alpha;
441 return 1;
442 }
443
444 // the infinite cylinder intersection point is not between the caps.
445 // set k to cap position to check.
446 if (k < 0) k = -lz2; else k = lz2;
447 }
448 }
449
450 // check for ray intersection with the caps. k must indicate the cap
451 // position to check
452 q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
453 q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
454 q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
455 return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl);
456}
457
458
459int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
460 dContactGeom *contact, int skip)
461{
462 dIASSERT (skip >= (int)sizeof(dContactGeom));
463 dIASSERT (o1->type == dRayClass);
464 dIASSERT (o2->type == dPlaneClass);
465 dIASSERT ((flags & NUMC_MASK) >= 1);
466
467 dxRay *ray = (dxRay*) o1;
468 dxPlane *plane = (dxPlane*) o2;
469
470 dReal alpha = plane->p[3] - dDOT (plane->p,ray->final_posr->pos);
471 // note: if alpha > 0 the starting point is below the plane
472 dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
473 dReal k = dDOT14(plane->p,ray->final_posr->R+2);
474 if (k==0) return 0; // ray parallel to plane
475 alpha /= k;
476 if (alpha < 0 || alpha > ray->length) return 0;
477 contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
478 contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
479 contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
480 contact->normal[0] = nsign*plane->p[0];
481 contact->normal[1] = nsign*plane->p[1];
482 contact->normal[2] = nsign*plane->p[2];
483 contact->depth = alpha;
484 contact->g1 = ray;
485 contact->g2 = plane;
486 return 1;
487}
488
489// Ray - Cylinder collider by David Walters (June 2006)
490int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
491{
492 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
493 dIASSERT( o1->type == dRayClass );
494 dIASSERT( o2->type == dCylinderClass );
495 dIASSERT( (flags & NUMC_MASK) >= 1 );
496
497 dxRay* ray = (dxRay*)( o1 );
498 dxCylinder* cyl = (dxCylinder*)( o2 );
499
500 // Fill in contact information.
501 contact->g1 = ray;
502 contact->g2 = cyl;
503
504 const dReal half_length = cyl->lz * REAL( 0.5 );
505
506 //
507 // Compute some useful info
508 //
509
510 dVector3 q, r;
511 dReal d, C, k;
512
513 // Vector 'r', line segment from C to R (ray start) ( r = R - C )
514 r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
515 r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
516 r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];
517
518 // Distance that ray start is along cyl axis ( Z-axis direction )
519 d = dDOT41( cyl->final_posr->R + 2, r );
520
521 //
522 // Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
523 //
524 // Point on axis ( in world space ): cp = ( d * Cz ) + C
525 //
526 // Line 'q' from R to cp: q = cp - R
527 // q = ( d * Cz ) + C - R
528 // q = ( d * Cz ) - ( R - C )
529
530 q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
531 q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
532 q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];
533
534
535 // Compute square length of 'q'. Subtract from radius squared to
536 // get square distance 'C' between the line q and the radius.
537
538 // if C < 0 then ray start position is within infinite extension of cylinder
539
540 C = dDOT( q, q ) - ( cyl->radius * cyl->radius );
541
542 // Compute the projection of ray direction normal onto cylinder direction normal.
543 dReal uv = dDOT44( cyl->final_posr->R+2, ray->final_posr->R+2 );
544
545
546
547 //
548 // Find ray collision with infinite cylinder
549 //
550
551 // Compute vector from end of ray direction normal to projection on cylinder direction normal.
552 r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
553 r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
554 r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];
555
556
557 // Quadratic Formula Magic
558 // Compute discriminant 'k':
559
560 // k < 0 : No intersection
561 // k = 0 : Tangent
562 // k > 0 : Intersection
563
564 dReal A = dDOT( r, r );
565 dReal B = 2 * dDOT( q, r );
566
567 k = B*B - 4*A*C;
568
569
570
571
572 //
573 // Collision with Flat Caps ?
574 //
575
576 // No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
577 if ( k < dEpsilon && C <= 0 )
578 {
579 // The ray does not intersect the edge of the infinite cylinder,
580 // but the ray start is inside and so must run parallel to the axis.
581 // It may yet intersect an end cap. The following cases are valid:
582
583 // -ve-cap , -half centre +half , +ve-cap
584 // <<================|-------------------|------------->>>---|================>>
585 // | |
586 // | d-------------------> 1.
587 // 2. d------------------> |
588 // 3. <------------------d |
589 // | <-------------------d 4.
590 // | |
591 // <<================|-------------------|------------->>>---|===============>>
592
593 // Negative if the ray and cylinder axes point in opposite directions.
594 const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
595
596 // Negative if the ray start is inside the cylinder
597 const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );
598
599 // Ray and Cylinder axes run in the same direction ( cases 1, 2 )
600 // Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
601 if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
602 ( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
603 {
604 return 0; // No intersection with caps or curved surface.
605 }
606
607 // Compute depth (distance from ray to cylinder)
608 contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );
609
610 // Compute contact point.
611 contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
612 contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
613 contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );
614
615 // Compute reflected contact normal.
616 contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
617 contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
618 contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );
619
620 // Contact!
621 return 1;
622 }
623
624
625
626 //
627 // Collision with Curved Edge ?
628 //
629
630 if ( k > 0 )
631 {
632 // Finish off quadratic formula to get intersection co-efficient
633 k = dSqrt( k );
634 A = dRecip( 2 * A );
635
636 // Compute distance along line to contact point.
637 dReal alpha = ( -B - k ) * A;
638 if ( alpha < 0 )
639 {
640 // Flip in the other direction.
641 alpha = ( -B + k ) * A;
642 }
643
644 // Intersection point is within ray length?
645 if ( alpha >= 0 && alpha <= ray->length )
646 {
647 // The ray intersects the infinite cylinder!
648
649 // Compute contact point.
650 contact->pos[0] = ray->final_posr->pos[0] + ( alpha * ray->final_posr->R[0*4+2] );
651 contact->pos[1] = ray->final_posr->pos[1] + ( alpha * ray->final_posr->R[1*4+2] );
652 contact->pos[2] = ray->final_posr->pos[2] + ( alpha * ray->final_posr->R[2*4+2] );
653
654 // q is the vector from the cylinder centre to the contact point.
655 q[0] = contact->pos[0] - cyl->final_posr->pos[0];
656 q[1] = contact->pos[1] - cyl->final_posr->pos[1];
657 q[2] = contact->pos[2] - cyl->final_posr->pos[2];
658
659 // Compute the distance along the cylinder axis of this contact point.
660 d = dDOT14( q, cyl->final_posr->R+2 );
661
662 // Check to see if the intersection point is between the flat end caps
663 if ( d >= -half_length && d <= +half_length )
664 {
665 // Flip the normal if the start point is inside the cylinder.
666 const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
667
668 // Compute contact normal.
669 contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
670 contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
671 contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
672 dNormalize3( contact->normal );
673
674 // Store depth.
675 contact->depth = alpha;
676
677 // Contact!
678 return 1;
679 }
680 }
681 }
682
683 // No contact with anything.
684 return 0;
685}
686
diff --git a/libraries/ode-0.9/ode/src/rotation.cpp b/libraries/ode-0.9/ode/src/rotation.cpp
new file mode 100644
index 0000000..adb933b
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/rotation.cpp
@@ -0,0 +1,316 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the
26"rotation axis" and s is the "rotation angle".
27
28*/
29
30#include <ode/rotation.h>
31#include <ode/odemath.h>
32
33
34#define _R(i,j) R[(i)*4+(j)]
35
36#define SET_3x3_IDENTITY \
37 _R(0,0) = REAL(1.0); \
38 _R(0,1) = REAL(0.0); \
39 _R(0,2) = REAL(0.0); \
40 _R(0,3) = REAL(0.0); \
41 _R(1,0) = REAL(0.0); \
42 _R(1,1) = REAL(1.0); \
43 _R(1,2) = REAL(0.0); \
44 _R(1,3) = REAL(0.0); \
45 _R(2,0) = REAL(0.0); \
46 _R(2,1) = REAL(0.0); \
47 _R(2,2) = REAL(1.0); \
48 _R(2,3) = REAL(0.0);
49
50
51void dRSetIdentity (dMatrix3 R)
52{
53 dAASSERT (R);
54 SET_3x3_IDENTITY;
55}
56
57
58void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
59 dReal angle)
60{
61 dAASSERT (R);
62 dQuaternion q;
63 dQFromAxisAndAngle (q,ax,ay,az,angle);
64 dQtoR (q,R);
65}
66
67
68void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
69{
70 dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
71 dAASSERT (R);
72 sphi = dSin(phi);
73 cphi = dCos(phi);
74 stheta = dSin(theta);
75 ctheta = dCos(theta);
76 spsi = dSin(psi);
77 cpsi = dCos(psi);
78 _R(0,0) = cpsi*ctheta;
79 _R(0,1) = spsi*ctheta;
80 _R(0,2) =-stheta;
81 _R(0,3) = REAL(0.0);
82 _R(1,0) = cpsi*stheta*sphi - spsi*cphi;
83 _R(1,1) = spsi*stheta*sphi + cpsi*cphi;
84 _R(1,2) = ctheta*sphi;
85 _R(1,3) = REAL(0.0);
86 _R(2,0) = cpsi*stheta*cphi + spsi*sphi;
87 _R(2,1) = spsi*stheta*cphi - cpsi*sphi;
88 _R(2,2) = ctheta*cphi;
89 _R(2,3) = REAL(0.0);
90}
91
92
93void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
94 dReal bx, dReal by, dReal bz)
95{
96 dReal l,k;
97 dAASSERT (R);
98 l = dSqrt (ax*ax + ay*ay + az*az);
99 if (l <= REAL(0.0)) {
100 dDEBUGMSG ("zero length vector");
101 return;
102 }
103 l = dRecip(l);
104 ax *= l;
105 ay *= l;
106 az *= l;
107 k = ax*bx + ay*by + az*bz;
108 bx -= k*ax;
109 by -= k*ay;
110 bz -= k*az;
111 l = dSqrt (bx*bx + by*by + bz*bz);
112 if (l <= REAL(0.0)) {
113 dDEBUGMSG ("zero length vector");
114 return;
115 }
116 l = dRecip(l);
117 bx *= l;
118 by *= l;
119 bz *= l;
120 _R(0,0) = ax;
121 _R(1,0) = ay;
122 _R(2,0) = az;
123 _R(0,1) = bx;
124 _R(1,1) = by;
125 _R(2,1) = bz;
126 _R(0,2) = - by*az + ay*bz;
127 _R(1,2) = - bz*ax + az*bx;
128 _R(2,2) = - bx*ay + ax*by;
129 _R(0,3) = REAL(0.0);
130 _R(1,3) = REAL(0.0);
131 _R(2,3) = REAL(0.0);
132}
133
134
135void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az)
136{
137 dVector3 n,p,q;
138 n[0] = ax;
139 n[1] = ay;
140 n[2] = az;
141 dNormalize3 (n);
142 dPlaneSpace (n,p,q);
143 _R(0,0) = p[0];
144 _R(1,0) = p[1];
145 _R(2,0) = p[2];
146 _R(0,1) = q[0];
147 _R(1,1) = q[1];
148 _R(2,1) = q[2];
149 _R(0,2) = n[0];
150 _R(1,2) = n[1];
151 _R(2,2) = n[2];
152 _R(0,3) = REAL(0.0);
153 _R(1,3) = REAL(0.0);
154 _R(2,3) = REAL(0.0);
155}
156
157
158void dQSetIdentity (dQuaternion q)
159{
160 dAASSERT (q);
161 q[0] = 1;
162 q[1] = 0;
163 q[2] = 0;
164 q[3] = 0;
165}
166
167
168void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
169 dReal angle)
170{
171 dAASSERT (q);
172 dReal l = ax*ax + ay*ay + az*az;
173 if (l > REAL(0.0)) {
174 angle *= REAL(0.5);
175 q[0] = dCos (angle);
176 l = dSin(angle) * dRecipSqrt(l);
177 q[1] = ax*l;
178 q[2] = ay*l;
179 q[3] = az*l;
180 }
181 else {
182 q[0] = 1;
183 q[1] = 0;
184 q[2] = 0;
185 q[3] = 0;
186 }
187}
188
189
190void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
191{
192 dAASSERT (qa && qb && qc);
193 qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
194 qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
195 qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
196 qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
197}
198
199
200void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
201{
202 dAASSERT (qa && qb && qc);
203 qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
204 qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
205 qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
206 qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
207}
208
209
210void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
211{
212 dAASSERT (qa && qb && qc);
213 qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
214 qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
215 qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
216 qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
217}
218
219
220void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
221{
222 dAASSERT (qa && qb && qc);
223 qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
224 qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
225 qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
226 qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
227}
228
229
230// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction
231// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
232// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
233// University, 1997.
234
235void dRfromQ (dMatrix3 R, const dQuaternion q)
236{
237 dAASSERT (q && R);
238 // q = (s,vx,vy,vz)
239 dReal qq1 = 2*q[1]*q[1];
240 dReal qq2 = 2*q[2]*q[2];
241 dReal qq3 = 2*q[3]*q[3];
242 _R(0,0) = 1 - qq2 - qq3;
243 _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
244 _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
245 _R(0,3) = REAL(0.0);
246 _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
247 _R(1,1) = 1 - qq1 - qq3;
248 _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
249 _R(1,3) = REAL(0.0);
250 _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
251 _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
252 _R(2,2) = 1 - qq1 - qq2;
253 _R(2,3) = REAL(0.0);
254}
255
256
257void dQfromR (dQuaternion q, const dMatrix3 R)
258{
259 dAASSERT (q && R);
260 dReal tr,s;
261 tr = _R(0,0) + _R(1,1) + _R(2,2);
262 if (tr >= 0) {
263 s = dSqrt (tr + 1);
264 q[0] = REAL(0.5) * s;
265 s = REAL(0.5) * dRecip(s);
266 q[1] = (_R(2,1) - _R(1,2)) * s;
267 q[2] = (_R(0,2) - _R(2,0)) * s;
268 q[3] = (_R(1,0) - _R(0,1)) * s;
269 }
270 else {
271 // find the largest diagonal element and jump to the appropriate case
272 if (_R(1,1) > _R(0,0)) {
273 if (_R(2,2) > _R(1,1)) goto case_2;
274 goto case_1;
275 }
276 if (_R(2,2) > _R(0,0)) goto case_2;
277 goto case_0;
278
279 case_0:
280 s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
281 q[1] = REAL(0.5) * s;
282 s = REAL(0.5) * dRecip(s);
283 q[2] = (_R(0,1) + _R(1,0)) * s;
284 q[3] = (_R(2,0) + _R(0,2)) * s;
285 q[0] = (_R(2,1) - _R(1,2)) * s;
286 return;
287
288 case_1:
289 s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
290 q[2] = REAL(0.5) * s;
291 s = REAL(0.5) * dRecip(s);
292 q[3] = (_R(1,2) + _R(2,1)) * s;
293 q[1] = (_R(0,1) + _R(1,0)) * s;
294 q[0] = (_R(0,2) - _R(2,0)) * s;
295 return;
296
297 case_2:
298 s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
299 q[3] = REAL(0.5) * s;
300 s = REAL(0.5) * dRecip(s);
301 q[1] = (_R(2,0) + _R(0,2)) * s;
302 q[2] = (_R(1,2) + _R(2,1)) * s;
303 q[0] = (_R(1,0) - _R(0,1)) * s;
304 return;
305 }
306}
307
308
309void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q)
310{
311 dAASSERT (w && q && dq);
312 dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
313 dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
314 dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
315 dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
316}
diff --git a/libraries/ode-0.9/ode/src/scrapbook.cpp b/libraries/ode-0.9/ode/src/scrapbook.cpp
new file mode 100644
index 0000000..2621814
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/scrapbook.cpp
@@ -0,0 +1,485 @@
1
2/*
3
4this is code that was once useful but has now been obseleted.
5
6this file should not be compiled as part of ODE!
7
8*/
9
10//***************************************************************************
11// intersect a line segment with a plane
12
13extern "C" int dClipLineToBox (const dVector3 p1, const dVector3 p2,
14 const dVector3 p, const dMatrix3 R,
15 const dVector3 side)
16{
17 // compute the start and end of the line (p1 and p2) relative to the box.
18 // we will do all subsequent computations in this box-relative coordinate
19 // system. we have to do a translation and rotation for each point.
20 dVector3 tmp,s,e;
21 tmp[0] = p1[0] - p[0];
22 tmp[1] = p1[1] - p[1];
23 tmp[2] = p1[2] - p[2];
24 dMULTIPLY1_331 (s,R,tmp);
25 tmp[0] = p2[0] - p[0];
26 tmp[1] = p2[1] - p[1];
27 tmp[2] = p2[2] - p[2];
28 dMULTIPLY1_331 (e,R,tmp);
29
30 // compute the vector 'v' from the start point to the end point
31 dVector3 v;
32 v[0] = e[0] - s[0];
33 v[1] = e[1] - s[1];
34 v[2] = e[2] - s[2];
35
36 // a point on the line is defined by the parameter 't'. t=0 corresponds
37 // to the start of the line, t=1 corresponds to the end of the line.
38 // we will clip the line to the box by finding the range of t where a
39 // point on the line is inside the box. the currently known bounds for
40 // t and tlo..thi.
41 dReal tlo=0,thi=1;
42
43 // clip in the X/Y/Z direction
44 for (int i=0; i<3; i++) {
45 // first adjust s,e for the current t range. this is redundant for the
46 // first iteration, but never mind.
47 e[i] = s[i] + thi*v[i];
48 s[i] = s[i] + tlo*v[i];
49 // compute where t intersects the positive and negative sides.
50 dReal tp = ( side[i] - s[i])/v[i]; // @@@ handle case where denom=0
51 dReal tm = (-side[i] - s[i])/v[i];
52 // handle 9 intersection cases
53 if (s[i] <= -side[i]) {
54 tlo = tm;
55 if (e[i] <= -side[i]) return 0;
56 else if (e[i] >= side[i]) thi = tp;
57 }
58 else if (s[i] <= side[i]) {
59 if (e[i] <= -side[i]) thi = tm;
60 else if (e[i] >= side[i]) thi = tp;
61 }
62 else {
63 tlo = tp;
64 if (e[i] <= -side[i]) thi = tm;
65 else if (e[i] >= side[i]) return 0;
66 }
67 }
68
69 //... @@@ AT HERE @@@
70
71 return 1;
72}
73
74
75//***************************************************************************
76// a nice try at C-B collision. unfortunately it doesn't work. the logic
77// for testing for line-box intersection is correct, but unfortunately the
78// closest-point distance estimates are often too large. as a result contact
79// points are placed incorrectly.
80
81
82int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags,
83 dContactGeom *contact, int skip)
84{
85 int i;
86
87 dIASSERT (skip >= (int)sizeof(dContactGeom));
88 dIASSERT (o1->_class->num == dCCylinderClass);
89 dIASSERT (o2->_class->num == dBoxClass);
90 contact->g1 = const_cast<dxGeom*> (o1);
91 contact->g2 = const_cast<dxGeom*> (o2);
92 dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1);
93 dxBox *box = (dxBox*) CLASSDATA(o2);
94
95 // get p1,p2 = cylinder axis endpoints, get radius
96 dVector3 p1,p2;
97 dReal clen = cyl->lz * REAL(0.5);
98 p1[0] = o1->pos[0] + clen * o1->R[2];
99 p1[1] = o1->pos[1] + clen * o1->R[6];
100 p1[2] = o1->pos[2] + clen * o1->R[10];
101 p2[0] = o1->pos[0] - clen * o1->R[2];
102 p2[1] = o1->pos[1] - clen * o1->R[6];
103 p2[2] = o1->pos[2] - clen * o1->R[10];
104 dReal radius = cyl->radius;
105
106 // copy out box center, rotation matrix, and side array
107 dReal *c = o2->pos;
108 dReal *R = o2->R;
109 dReal *side = box->side;
110
111 // compute the start and end of the line (p1 and p2) relative to the box.
112 // we will do all subsequent computations in this box-relative coordinate
113 // system. we have to do a translation and rotation for each point.
114 dVector3 tmp3,s,e;
115 tmp3[0] = p1[0] - c[0];
116 tmp3[1] = p1[1] - c[1];
117 tmp3[2] = p1[2] - c[2];
118 dMULTIPLY1_331 (s,R,tmp3);
119 tmp3[0] = p2[0] - c[0];
120 tmp3[1] = p2[1] - c[1];
121 tmp3[2] = p2[2] - c[2];
122 dMULTIPLY1_331 (e,R,tmp3);
123
124 // compute the vector 'v' from the start point to the end point
125 dVector3 v;
126 v[0] = e[0] - s[0];
127 v[1] = e[1] - s[1];
128 v[2] = e[2] - s[2];
129
130 // compute the half-sides of the box
131 dReal S0 = side[0] * REAL(0.5);
132 dReal S1 = side[1] * REAL(0.5);
133 dReal S2 = side[2] * REAL(0.5);
134
135 // compute the size of the bounding box around the line segment
136 dReal B0 = dFabs (v[0]);
137 dReal B1 = dFabs (v[1]);
138 dReal B2 = dFabs (v[2]);
139
140 // for all 6 separation axes, measure the penetration depth. if any depth is
141 // less than 0 then the objects don't penetrate at all so we can just
142 // return 0. find the axis with the smallest depth, and record its normal.
143
144 // note: normalR is set to point to a column of R if that is the smallest
145 // depth normal so far. otherwise normalR is 0 and normalC is set to a
146 // vector relative to the box. invert_normal is 1 if the sign of the normal
147 // should be flipped.
148
149 dReal depth,trial_depth,tmp,length;
150 const dReal *normalR=0;
151 dVector3 normalC;
152 int invert_normal = 0;
153 int code = 0; // 0=no contact, 1-3=face contact, 4-6=edge contact
154
155 depth = dInfinity;
156
157 // look at face-normal axes
158
159#undef TEST
160#define TEST(center,depth_expr,norm,contact_code) \
161 tmp = (center); \
162 trial_depth = radius + REAL(0.5) * ((depth_expr) - dFabs(tmp)); \
163 if (trial_depth < 0) return 0; \
164 if (trial_depth < depth) { \
165 depth = trial_depth; \
166 normalR = (norm); \
167 invert_normal = (tmp < 0); \
168 code = contact_code; \
169 }
170
171 TEST (s[0]+e[0], side[0] + B0, R+0, 1);
172 TEST (s[1]+e[1], side[1] + B1, R+1, 2);
173 TEST (s[2]+e[2], side[2] + B2, R+2, 3);
174
175 // look at v x box-edge axes
176
177#undef TEST
178#define TEST(box_radius,line_offset,nx,ny,nz,contact_code) \
179 tmp = (line_offset); \
180 trial_depth = (box_radius) - dFabs(tmp); \
181 length = dSqrt ((nx)*(nx) + (ny)*(ny) + (nz)*(nz)); \
182 if (length > 0) { \
183 length = dRecip(length); \
184 trial_depth = trial_depth * length + radius; \
185 if (trial_depth < 0) return 0; \
186 if (trial_depth < depth) { \
187 depth = trial_depth; \
188 normalR = 0; \
189 normalC[0] = (nx)*length; \
190 normalC[1] = (ny)*length; \
191 normalC[2] = (nz)*length; \
192 invert_normal = (tmp < 0); \
193 code = contact_code; \
194 } \
195 }
196
197 TEST (B2*S1+B1*S2,v[1]*s[2]-v[2]*s[1], 0,-v[2],v[1], 4);
198 TEST (B2*S0+B0*S2,v[2]*s[0]-v[0]*s[2], v[2],0,-v[0], 5);
199 TEST (B1*S0+B0*S1,v[0]*s[1]-v[1]*s[0], -v[1],v[0],0, 6);
200
201#undef TEST
202
203 // if we get to this point, the box and ccylinder interpenetrate.
204 // compute the normal in global coordinates.
205 dReal *normal = contact[0].normal;
206 if (normalR) {
207 normal[0] = normalR[0];
208 normal[1] = normalR[4];
209 normal[2] = normalR[8];
210 }
211 else {
212 dMULTIPLY0_331 (normal,R,normalC);
213 }
214 if (invert_normal) {
215 normal[0] = -normal[0];
216 normal[1] = -normal[1];
217 normal[2] = -normal[2];
218 }
219
220 // set the depth
221 contact[0].depth = depth;
222
223 if (code == 0) {
224 return 0; // should never get here
225 }
226 else if (code >= 4) {
227 // handle edge contacts
228 // find an endpoint q1 on the intersecting edge of the box
229 dVector3 q1;
230 dReal sign[3];
231 for (i=0; i<3; i++) q1[i] = c[i];
232 sign[0] = (dDOT14(normal,R+0) > 0) ? REAL(1.0) : REAL(-1.0);
233 for (i=0; i<3; i++) q1[i] += sign[0] * S0 * R[i*4];
234 sign[1] = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
235 for (i=0; i<3; i++) q1[i] += sign[1] * S1 * R[i*4+1];
236 sign[2] = (dDOT14(normal,R+2) > 0) ? REAL(1.0) : REAL(-1.0);
237 for (i=0; i<3; i++) q1[i] += sign[2] * S2 * R[i*4+2];
238
239 // find the other endpoint q2 of the intersecting edge
240 dVector3 q2;
241 for (i=0; i<3; i++)
242 q2[i] = q1[i] - R[code-4 + i*4] * (sign[code-4] * side[code-4]);
243
244 // determine the closest point between the box edge and the line segment
245 dVector3 cp1,cp2;
246 dClosestLineSegmentPoints (q1,q2, p1,p2, cp1,cp2);
247 for (i=0; i<3; i++) contact[0].pos[i] = cp1[i] - REAL(0.5)*normal[i]*depth;
248 return 1;
249 }
250 else {
251 // handle face contacts.
252 // @@@ temporary: make deepest vertex on the line the contact point.
253 // @@@ this kind of works, but we sometimes need two contact points for
254 // @@@ stability.
255
256 // compute 'v' in global coordinates
257 dVector3 gv;
258 for (i=0; i<3; i++) gv[i] = p2[i] - p1[i];
259
260 if (dDOT (normal,gv) > 0) {
261 for (i=0; i<3; i++)
262 contact[0].pos[i] = p1[i] + (depth*REAL(0.5)-radius)*normal[i];
263 }
264 else {
265 for (i=0; i<3; i++)
266 contact[0].pos[i] = p2[i] + (depth*REAL(0.5)-radius)*normal[i];
267 }
268 return 1;
269 }
270}
271
272//***************************************************************************
273// this function works, it's just not being used for anything at the moment:
274
275// given a box (R,side), `R' is the rotation matrix for the box, and `side'
276// is a vector of x/y/z side lengths, return the size of the interval of the
277// box projected along the given axis. if the axis has unit length then the
278// return value will be the actual diameter, otherwise the result will be
279// scaled by the axis length.
280
281static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side,
282 const dVector3 axis)
283{
284 dVector3 q;
285 dMULTIPLY1_331 (q,R,axis); // transform axis to body-relative
286 return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + dFabs(q[2])*side[2];
287}
288
289//***************************************************************************
290// the old capped cylinder to capped cylinder collision code. this fails to
291// detect cap-to-cap contact points when the cylinder axis are aligned, but
292// other that that it is pretty robust.
293
294// this returns at most one contact point when the two cylinder's axes are not
295// aligned, and at most two (for stability) when they are aligned.
296// the algorithm minimizes the distance between two "sample spheres" that are
297// positioned along the cylinder axes according to:
298// sphere1 = pos1 + alpha1 * axis1
299// sphere2 = pos2 + alpha2 * axis2
300// alpha1 and alpha2 are limited to +/- half the length of the cylinders.
301// the algorithm works by finding a solution that has both alphas free, or
302// a solution that has one or both alphas fixed to the ends of the cylinder.
303
304int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2,
305 int flags, dContactGeom *contact, int skip)
306{
307 int i;
308 const dReal tolerance = REAL(1e-5);
309
310 dIASSERT (skip >= (int)sizeof(dContactGeom));
311 dIASSERT (o1->type == dCCylinderClass);
312 dIASSERT (o2->type == dCCylinderClass);
313 dxCCylinder *cyl1 = (dxCCylinder*) o1;
314 dxCCylinder *cyl2 = (dxCCylinder*) o2;
315
316 contact->g1 = o1;
317 contact->g2 = o2;
318
319 // copy out some variables, for convenience
320 dReal lz1 = cyl1->lz * REAL(0.5);
321 dReal lz2 = cyl2->lz * REAL(0.5);
322 dReal *pos1 = o1->pos;
323 dReal *pos2 = o2->pos;
324 dReal axis1[3],axis2[3];
325 axis1[0] = o1->R[2];
326 axis1[1] = o1->R[6];
327 axis1[2] = o1->R[10];
328 axis2[0] = o2->R[2];
329 axis2[1] = o2->R[6];
330 axis2[2] = o2->R[10];
331
332 dReal alpha1,alpha2,sphere1[3],sphere2[3];
333 int fix1 = 0; // 0 if alpha1 is free, +/-1 to fix at +/- lz1
334 int fix2 = 0; // 0 if alpha2 is free, +/-1 to fix at +/- lz2
335
336 for (int count=0; count<9; count++) {
337 // find a trial solution by fixing or not fixing the alphas
338 if (fix1) {
339 if (fix2) {
340 // alpha1 and alpha2 are fixed, so the solution is easy
341 if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1;
342 if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2;
343 for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
344 for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
345 }
346 else {
347 // fix alpha1 but let alpha2 be free
348 if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1;
349 for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
350 alpha2 = (axis2[0]*(sphere1[0]-pos2[0]) +
351 axis2[1]*(sphere1[1]-pos2[1]) +
352 axis2[2]*(sphere1[2]-pos2[2]));
353 for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
354 }
355 }
356 else {
357 if (fix2) {
358 // fix alpha2 but let alpha1 be free
359 if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2;
360 for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
361 alpha1 = (axis1[0]*(sphere2[0]-pos1[0]) +
362 axis1[1]*(sphere2[1]-pos1[1]) +
363 axis1[2]*(sphere2[2]-pos1[2]));
364 for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
365 }
366 else {
367 // let alpha1 and alpha2 be free
368 // compute determinant of d(d^2)\d(alpha) jacobian
369 dReal a1a2 = dDOT (axis1,axis2);
370 dReal det = REAL(1.0)-a1a2*a1a2;
371 if (det < tolerance) {
372 // the cylinder axes (almost) parallel, so we will generate up to two
373 // contacts. the solution matrix is rank deficient so alpha1 and
374 // alpha2 are related by:
375 // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2)
376 // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2)
377 // first compute where the two cylinders overlap in alpha1 space:
378 if (a1a2 < 0) {
379 axis2[0] = -axis2[0];
380 axis2[1] = -axis2[1];
381 axis2[2] = -axis2[2];
382 }
383 dReal q[3];
384 for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
385 dReal k = dDOT (axis1,q);
386 dReal a1lo = -lz1;
387 dReal a1hi = lz1;
388 dReal a2lo = -lz2 - k;
389 dReal a2hi = lz2 - k;
390 dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
391 dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
392 if (lo <= hi) {
393 int num_contacts = flags & NUMC_MASK;
394 if (num_contacts >= 2 && lo < hi) {
395 // generate up to two contacts. if one of those contacts is
396 // not made, fall back on the one-contact strategy.
397 for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
398 for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
399 int n1 = dCollideSpheres (sphere1,cyl1->radius,
400 sphere2,cyl2->radius,contact);
401 if (n1) {
402 for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
403 for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
404 dContactGeom *c2 = CONTACT(contact,skip);
405 int n2 = dCollideSpheres (sphere1,cyl1->radius,
406 sphere2,cyl2->radius, c2);
407 if (n2) {
408 c2->g1 = o1;
409 c2->g2 = o2;
410 return 2;
411 }
412 }
413 }
414
415 // just one contact to generate, so put it in the middle of
416 // the range
417 alpha1 = (lo + hi) * REAL(0.5);
418 alpha2 = alpha1 + k;
419 for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
420 for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
421 return dCollideSpheres (sphere1,cyl1->radius,
422 sphere2,cyl2->radius,contact);
423 }
424 else return 0;
425 }
426 det = REAL(1.0)/det;
427 dReal delta[3];
428 for (i=0; i<3; i++) delta[i] = pos1[i] - pos2[i];
429 dReal q1 = dDOT (delta,axis1);
430 dReal q2 = dDOT (delta,axis2);
431 alpha1 = det*(a1a2*q2-q1);
432 alpha2 = det*(q2-a1a2*q1);
433 for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
434 for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
435 }
436 }
437
438 // if the alphas are outside their allowed ranges then fix them and
439 // try again
440 if (fix1==0) {
441 if (alpha1 < -lz1) {
442 fix1 = -1;
443 continue;
444 }
445 if (alpha1 > lz1) {
446 fix1 = 1;
447 continue;
448 }
449 }
450 if (fix2==0) {
451 if (alpha2 < -lz2) {
452 fix2 = -1;
453 continue;
454 }
455 if (alpha2 > lz2) {
456 fix2 = 1;
457 continue;
458 }
459 }
460
461 // unfix the alpha variables if the local distance gradient indicates
462 // that we are not yet at the minimum
463 dReal tmp[3];
464 for (i=0; i<3; i++) tmp[i] = sphere1[i] - sphere2[i];
465 if (fix1) {
466 dReal gradient = dDOT (tmp,axis1);
467 if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) {
468 fix1 = 0;
469 continue;
470 }
471 }
472 if (fix2) {
473 dReal gradient = -dDOT (tmp,axis2);
474 if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) {
475 fix2 = 0;
476 continue;
477 }
478 }
479 return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
480 }
481 // if we go through the loop too much, then give up. we should NEVER get to
482 // this point (i hope).
483 dMessage (0,"dCollideCC(): too many iterations");
484 return 0;
485}
diff --git a/libraries/ode-0.9/ode/src/sphere.cpp b/libraries/ode-0.9/ode/src/sphere.cpp
new file mode 100644
index 0000000..fa25aac
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/sphere.cpp
@@ -0,0 +1,241 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25standard ODE geometry primitives: public API and pairwise collision functions.
26
27the rule is that only the low level primitive collision functions should set
28dContactGeom::g1 and dContactGeom::g2.
29
30*/
31
32#include <ode/common.h>
33#include <ode/collision.h>
34#include <ode/matrix.h>
35#include <ode/rotation.h>
36#include <ode/odemath.h>
37#include "collision_kernel.h"
38#include "collision_std.h"
39#include "collision_util.h"
40
41#ifdef _MSC_VER
42#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43#endif
44
45
46//****************************************************************************
47// sphere public API
48
49dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1)
50{
51 dAASSERT (_radius > 0);
52 type = dSphereClass;
53 radius = _radius;
54}
55
56
57void dxSphere::computeAABB()
58{
59 aabb[0] = final_posr->pos[0] - radius;
60 aabb[1] = final_posr->pos[0] + radius;
61 aabb[2] = final_posr->pos[1] - radius;
62 aabb[3] = final_posr->pos[1] + radius;
63 aabb[4] = final_posr->pos[2] - radius;
64 aabb[5] = final_posr->pos[2] + radius;
65}
66
67
68dGeomID dCreateSphere (dSpaceID space, dReal radius)
69{
70 return new dxSphere (space,radius);
71}
72
73
74void dGeomSphereSetRadius (dGeomID g, dReal radius)
75{
76 dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
77 dAASSERT (radius > 0);
78 dxSphere *s = (dxSphere*) g;
79 s->radius = radius;
80 dGeomMoved (g);
81}
82
83
84dReal dGeomSphereGetRadius (dGeomID g)
85{
86 dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
87 dxSphere *s = (dxSphere*) g;
88 return s->radius;
89}
90
91
92dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
93{
94 dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
95 g->recomputePosr();
96
97 dxSphere *s = (dxSphere*) g;
98 dReal * pos = s->final_posr->pos;
99 return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) +
100 (y-pos[1])*(y-pos[1]) +
101 (z-pos[2])*(z-pos[2]));
102}
103
104//****************************************************************************
105// pairwise collision functions for standard geom types
106
107int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
108 dContactGeom *contact, int skip)
109{
110 dIASSERT (skip >= (int)sizeof(dContactGeom));
111 dIASSERT (o1->type == dSphereClass);
112 dIASSERT (o2->type == dSphereClass);
113 dIASSERT ((flags & NUMC_MASK) >= 1);
114
115 dxSphere *sphere1 = (dxSphere*) o1;
116 dxSphere *sphere2 = (dxSphere*) o2;
117
118 contact->g1 = o1;
119 contact->g2 = o2;
120
121 return dCollideSpheres (o1->final_posr->pos,sphere1->radius,
122 o2->final_posr->pos,sphere2->radius,contact);
123}
124
125
126int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
127 dContactGeom *contact, int skip)
128{
129 dIASSERT (skip >= (int)sizeof(dContactGeom));
130 dIASSERT (o1->type == dSphereClass);
131 dIASSERT (o2->type == dBoxClass);
132 dIASSERT ((flags & NUMC_MASK) >= 1);
133
134 // this is easy. get the sphere center `p' relative to the box, and then clip
135 // that to the boundary of the box (call that point `q'). if q is on the
136 // boundary of the box and |p-q| is <= sphere radius, they touch.
137 // if q is inside the box, the sphere is inside the box, so set a contact
138 // normal to push the sphere to the closest box face.
139
140 dVector3 l,t,p,q,r;
141 dReal depth;
142 int onborder = 0;
143
144 dxSphere *sphere = (dxSphere*) o1;
145 dxBox *box = (dxBox*) o2;
146
147 contact->g1 = o1;
148 contact->g2 = o2;
149
150 p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
151 p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
152 p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];
153
154 l[0] = box->side[0]*REAL(0.5);
155 t[0] = dDOT14(p,o2->final_posr->R);
156 if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
157 if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; }
158
159 l[1] = box->side[1]*REAL(0.5);
160 t[1] = dDOT14(p,o2->final_posr->R+1);
161 if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
162 if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; }
163
164 t[2] = dDOT14(p,o2->final_posr->R+2);
165 l[2] = box->side[2]*REAL(0.5);
166 if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
167 if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; }
168
169 if (!onborder) {
170 // sphere center inside box. find closest face to `t'
171 dReal min_distance = l[0] - dFabs(t[0]);
172 int mini = 0;
173 for (int i=1; i<3; i++) {
174 dReal face_distance = l[i] - dFabs(t[i]);
175 if (face_distance < min_distance) {
176 min_distance = face_distance;
177 mini = i;
178 }
179 }
180 // contact position = sphere center
181 contact->pos[0] = o1->final_posr->pos[0];
182 contact->pos[1] = o1->final_posr->pos[1];
183 contact->pos[2] = o1->final_posr->pos[2];
184 // contact normal points to closest face
185 dVector3 tmp;
186 tmp[0] = 0;
187 tmp[1] = 0;
188 tmp[2] = 0;
189 tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
190 dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp);
191 // contact depth = distance to wall along normal plus radius
192 contact->depth = min_distance + sphere->radius;
193 return 1;
194 }
195
196 t[3] = 0; //@@@ hmmm
197 dMULTIPLY0_331 (q,o2->final_posr->R,t);
198 r[0] = p[0] - q[0];
199 r[1] = p[1] - q[1];
200 r[2] = p[2] - q[2];
201 depth = sphere->radius - dSqrt(dDOT(r,r));
202 if (depth < 0) return 0;
203 contact->pos[0] = q[0] + o2->final_posr->pos[0];
204 contact->pos[1] = q[1] + o2->final_posr->pos[1];
205 contact->pos[2] = q[2] + o2->final_posr->pos[2];
206 contact->normal[0] = r[0];
207 contact->normal[1] = r[1];
208 contact->normal[2] = r[2];
209 dNormalize3 (contact->normal);
210 contact->depth = depth;
211 return 1;
212}
213
214
215int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
216 dContactGeom *contact, int skip)
217{
218 dIASSERT (skip >= (int)sizeof(dContactGeom));
219 dIASSERT (o1->type == dSphereClass);
220 dIASSERT (o2->type == dPlaneClass);
221 dIASSERT ((flags & NUMC_MASK) >= 1);
222
223 dxSphere *sphere = (dxSphere*) o1;
224 dxPlane *plane = (dxPlane*) o2;
225
226 contact->g1 = o1;
227 contact->g2 = o2;
228 dReal k = dDOT (o1->final_posr->pos,plane->p);
229 dReal depth = plane->p[3] - k + sphere->radius;
230 if (depth >= 0) {
231 contact->normal[0] = plane->p[0];
232 contact->normal[1] = plane->p[1];
233 contact->normal[2] = plane->p[2];
234 contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius;
235 contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
236 contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
237 contact->depth = depth;
238 return 1;
239 }
240 else return 0;
241}
diff --git a/libraries/ode-0.9/ode/src/stack.cpp b/libraries/ode-0.9/ode/src/stack.cpp
new file mode 100644
index 0000000..e062f92
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/stack.cpp
@@ -0,0 +1,114 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23@@@ this file should not be compiled any more @@@
24
25#include <string.h>
26#include <errno.h>
27#include "stack.h"
28#include "ode/error.h"
29#include "ode/config.h"
30
31//****************************************************************************
32// unix version that uses mmap(). some systems have anonymous mmaps and some
33// need to mmap /dev/zero.
34
35#ifndef WIN32
36
37#include <unistd.h>
38#include <sys/mman.h>
39#include <sys/types.h>
40#include <sys/stat.h>
41#include <fcntl.h>
42
43
44void dStack::init (int max_size)
45{
46 if (sizeof(long int) != sizeof(char*)) dDebug (0,"internal");
47 if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0");
48
49#ifndef MMAP_ANONYMOUS
50 static int dev_zero_fd = -1; // cached file descriptor for /dev/zero
51 if (dev_zero_fd < 0) dev_zero_fd = open ("/dev/zero", O_RDWR);
52 if (dev_zero_fd < 0) dError (0,"can't open /dev/zero (%s)",strerror(errno));
53 base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, MAP_PRIVATE,
54 dev_zero_fd,0);
55#else
56 base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE,
57 MAP_PRIVATE | MAP_ANON,0,0);
58#endif
59
60 if (int(base) == -1) dError (0,"Stack::init(), mmap() failed, "
61 "max_size=%d (%s)",max_size,strerror(errno));
62 size = max_size;
63 pointer = base;
64 frame = 0;
65}
66
67
68void dStack::destroy()
69{
70 munmap (base,size);
71 base = 0;
72 size = 0;
73 pointer = 0;
74 frame = 0;
75}
76
77#endif
78
79//****************************************************************************
80
81#ifdef WIN32
82
83#include "windows.h"
84
85
86void dStack::init (int max_size)
87{
88 if (sizeof(LPVOID) != sizeof(char*)) dDebug (0,"internal");
89 if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0");
90 base = (char*) VirtualAlloc (NULL,max_size,MEM_RESERVE,PAGE_READWRITE);
91 if (base == 0) dError (0,"Stack::init(), VirtualAlloc() failed, "
92 "max_size=%d",max_size);
93 size = max_size;
94 pointer = base;
95 frame = 0;
96 committed = 0;
97
98 // get page size
99 SYSTEM_INFO info;
100 GetSystemInfo (&info);
101 pagesize = info.dwPageSize;
102}
103
104
105void dStack::destroy()
106{
107 VirtualFree (base,0,MEM_RELEASE);
108 base = 0;
109 size = 0;
110 pointer = 0;
111 frame = 0;
112}
113
114#endif
diff --git a/libraries/ode-0.9/ode/src/stack.h b/libraries/ode-0.9/ode/src/stack.h
new file mode 100644
index 0000000..5afff41
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/stack.h
@@ -0,0 +1,138 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/* this comes from the `reuse' library. copy any changes back to the source.
24
25these stack allocation functions are a replacement for alloca(), except that
26they allocate memory from a separate pool.
27
28advantages over alloca():
29 - consecutive allocations are guaranteed to be contiguous with increasing
30 address.
31 - functions can allocate stack memory that is returned to the caller,
32 in other words pushing and popping stack frames is optional.
33
34disadvantages compared to alloca():
35 - less portable
36 - slightly slower, although still orders of magnitude faster than malloc().
37 - longjmp() and exceptions do not deallocate stack memory (but who cares?).
38
39just like alloca():
40 - using too much stack memory does not fail gracefully, it fails with a
41 segfault.
42
43*/
44
45
46#ifndef _ODE_STACK_H_
47#define _ODE_STACK_H_
48
49
50#ifdef WIN32
51#include "windows.h"
52#endif
53
54
55struct dStack {
56 char *base; // bottom of the stack
57 int size; // maximum size of the stack
58 char *pointer; // current top of the stack
59 char *frame; // linked list of stack frame ptrs
60# ifdef WIN32 // stuff for windows:
61 int pagesize; // - page size - this is ASSUMED to be a power of 2
62 int committed; // - bytes committed in allocated region
63#endif
64
65 // initialize the stack. `max_size' is the maximum size that the stack can
66 // reach. on unix and windows a `virtual' memory block of this size is
67 // mapped into the address space but does not actually consume physical
68 // memory until it is referenced - so it is safe to set this to a high value.
69
70 void init (int max_size);
71
72
73 // destroy the stack. this unmaps any virtual memory that was allocated.
74
75 void destroy();
76
77
78 // allocate `size' bytes from the stack and return a pointer to the allocated
79 // memory. `size' must be >= 0. the returned pointer will be aligned to the
80 // size of a long int.
81
82 char * alloc (int size)
83 {
84 char *ret = pointer;
85 pointer += ((size-1) | (sizeof(long int)-1) )+1;
86# ifdef WIN32
87 // for windows we need to commit pages as they are required
88 if ((pointer-base) > committed) {
89 committed = ((pointer-base-1) | (pagesize-1))+1; // round up to pgsize
90 VirtualAlloc (base,committed,MEM_COMMIT,PAGE_READWRITE);
91 }
92# endif
93 return ret;
94 }
95
96
97 // return the address that will be returned by the next call to alloc()
98
99 char *nextAlloc()
100 {
101 return pointer;
102 }
103
104
105 // push and pop the current size of the stack. pushFrame() saves the current
106 // frame pointer on the stack, and popFrame() retrieves it. a typical
107 // stack-using function will bracket alloc() calls with pushFrame() and
108 // popFrame(). both functions return the current stack pointer - this should
109 // be the same value for the two bracketing calls. calling popFrame() too
110 // many times will result in a segfault.
111
112 char * pushFrame()
113 {
114 char *newframe = pointer;
115 char **addr = (char**) alloc (sizeof(char*));
116 *addr = frame;
117 frame = newframe;
118 return newframe;
119
120 /* OLD CODE
121 *((char**)pointer) = frame;
122 frame = pointer;
123 char *ret = pointer;
124 pointer += sizeof(char*);
125 return ret;
126 */
127 }
128
129 char * popFrame()
130 {
131 pointer = frame;
132 frame = *((char**)pointer);
133 return pointer;
134 }
135};
136
137
138#endif
diff --git a/libraries/ode-0.9/ode/src/step.cpp b/libraries/ode-0.9/ode/src/step.cpp
new file mode 100644
index 0000000..19d0473
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/step.cpp
@@ -0,0 +1,1795 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include "objects.h"
24#include "joint.h"
25#include <ode/config.h>
26#include <ode/odemath.h>
27#include <ode/rotation.h>
28#include <ode/timer.h>
29#include <ode/error.h>
30#include <ode/matrix.h>
31#include "lcp.h"
32#include "util.h"
33
34//****************************************************************************
35// misc defines
36
37#define FAST_FACTOR
38//#define TIMING
39
40// memory allocation system
41#ifdef dUSE_MALLOC_FOR_ALLOCA
42unsigned int dMemoryFlag;
43#define REPORT_OUT_OF_MEMORY fprintf(stderr, "Insufficient memory to complete rigid body simulation. Results will not be accurate.\n")
44
45#define ALLOCA(t,v,s) t* v=(t*)malloc(s)
46#define UNALLOCA(t) free(t)
47
48#else
49#define ALLOCA(t,v,s) t* v=(t*)dALLOCA16(s)
50#define UNALLOCA(t) /* nothing */
51#endif
52
53
54//****************************************************************************
55// debugging - comparison of various vectors and matrices produced by the
56// slow and fast versions of the stepper.
57
58//#define COMPARE_METHODS
59
60#ifdef COMPARE_METHODS
61#include "testing.h"
62dMatrixComparison comparator;
63#endif
64
65// undef to use the fast decomposition
66#define DIRECT_CHOLESKY
67#undef REPORT_ERROR
68
69//****************************************************************************
70// special matrix multipliers
71
72// this assumes the 4th and 8th rows of B and C are zero.
73
74static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
75 int p, int r, int Askip)
76{
77 int i,j;
78 dReal sum,*bb,*cc;
79 dIASSERT (p>0 && r>0 && A && B && C);
80 bb = B;
81 for (i=p; i; i--) {
82 cc = C;
83 for (j=r; j; j--) {
84 sum = bb[0]*cc[0];
85 sum += bb[1]*cc[1];
86 sum += bb[2]*cc[2];
87 sum += bb[4]*cc[4];
88 sum += bb[5]*cc[5];
89 sum += bb[6]*cc[6];
90 *(A++) = sum;
91 cc += 8;
92 }
93 A += Askip - r;
94 bb += 8;
95 }
96}
97
98
99// this assumes the 4th and 8th rows of B and C are zero.
100
101static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
102 int p, int r, int Askip)
103{
104 int i,j;
105 dReal sum,*bb,*cc;
106 dIASSERT (p>0 && r>0 && A && B && C);
107 bb = B;
108 for (i=p; i; i--) {
109 cc = C;
110 for (j=r; j; j--) {
111 sum = bb[0]*cc[0];
112 sum += bb[1]*cc[1];
113 sum += bb[2]*cc[2];
114 sum += bb[4]*cc[4];
115 sum += bb[5]*cc[5];
116 sum += bb[6]*cc[6];
117 *(A++) += sum;
118 cc += 8;
119 }
120 A += Askip - r;
121 bb += 8;
122 }
123}
124
125
126// this assumes the 4th and 8th rows of B are zero.
127
128static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
129{
130 int i;
131 dIASSERT (p>0 && A && B && C);
132 dReal sum;
133 for (i=p; i; i--) {
134 sum = B[0]*C[0];
135 sum += B[1]*C[1];
136 sum += B[2]*C[2];
137 sum += B[4]*C[4];
138 sum += B[5]*C[5];
139 sum += B[6]*C[6];
140 *(A++) = sum;
141 B += 8;
142 }
143}
144
145
146// this assumes the 4th and 8th rows of B are zero.
147
148static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
149{
150 int i;
151 dIASSERT (p>0 && A && B && C);
152 dReal sum;
153 for (i=p; i; i--) {
154 sum = B[0]*C[0];
155 sum += B[1]*C[1];
156 sum += B[2]*C[2];
157 sum += B[4]*C[4];
158 sum += B[5]*C[5];
159 sum += B[6]*C[6];
160 *(A++) += sum;
161 B += 8;
162 }
163}
164
165
166// this assumes the 4th and 8th rows of B are zero.
167
168static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
169{
170 int k;
171 dReal sum;
172 dIASSERT (q>0 && A && B && C);
173 sum = 0;
174 for (k=0; k<q; k++) sum += B[k*8] * C[k];
175 A[0] += sum;
176 sum = 0;
177 for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
178 A[1] += sum;
179 sum = 0;
180 for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
181 A[2] += sum;
182 sum = 0;
183 for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
184 A[4] += sum;
185 sum = 0;
186 for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
187 A[5] += sum;
188 sum = 0;
189 for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
190 A[6] += sum;
191}
192
193
194// this assumes the 4th and 8th rows of B are zero.
195
196static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
197{
198 int k;
199 dReal sum;
200 dIASSERT (q>0 && A && B && C);
201 sum = 0;
202 for (k=0; k<q; k++) sum += B[k*8] * C[k];
203 A[0] = sum;
204 sum = 0;
205 for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
206 A[1] = sum;
207 sum = 0;
208 for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
209 A[2] = sum;
210 sum = 0;
211 for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
212 A[4] = sum;
213 sum = 0;
214 for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
215 A[5] = sum;
216 sum = 0;
217 for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
218 A[6] = sum;
219}
220
221//****************************************************************************
222// the slow, but sure way
223// note that this does not do any joint feedback!
224
225// given lists of bodies and joints that form an island, perform a first
226// order timestep.
227//
228// `body' is the body array, `nb' is the size of the array.
229// `_joint' is the body array, `nj' is the size of the array.
230
231void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
232 dxJoint * const *_joint, int nj, dReal stepsize)
233{
234 int i,j,k;
235 int n6 = 6*nb;
236
237#ifdef TIMING
238 dTimerStart("preprocessing");
239#endif
240
241 // number all bodies in the body list - set their tag values
242 for (i=0; i<nb; i++) body[i]->tag = i;
243
244 // make a local copy of the joint array, because we might want to modify it.
245 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
246 // but not the joint array, because the caller might need it unchanged).
247 ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*));
248#ifdef dUSE_MALLOC_FOR_ALLOCA
249 if (joint == NULL) {
250 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
251 return;
252 }
253#endif
254 memcpy (joint,_joint,nj * sizeof(dxJoint*));
255
256 // for all bodies, compute the inertia tensor and its inverse in the global
257 // frame, and compute the rotational force and add it to the torque
258 // accumulator.
259 // @@@ check computation of rotational force.
260 ALLOCA(dReal,I,3*nb*4*sizeof(dReal));
261#ifdef dUSE_MALLOC_FOR_ALLOCA
262 if (I == NULL) {
263 UNALLOCA(joint);
264 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
265 return;
266 }
267#endif
268 ALLOCA(dReal,invI,3*nb*4*sizeof(dReal));
269#ifdef dUSE_MALLOC_FOR_ALLOCA
270 if (invI == NULL) {
271 UNALLOCA(I);
272 UNALLOCA(joint);
273 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
274 return;
275 }
276#endif
277
278 //dSetZero (I,3*nb*4);
279 //dSetZero (invI,3*nb*4);
280 for (i=0; i<nb; i++) {
281 dReal tmp[12];
282 // compute inertia tensor in global frame
283 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
284 dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);
285 // compute inverse inertia tensor in global frame
286 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);
287 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);
288 // compute rotational force
289 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
290 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
291 }
292
293 // add the gravity force to all bodies
294 for (i=0; i<nb; i++) {
295 if ((body[i]->flags & dxBodyNoGravity)==0) {
296 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
297 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
298 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
299 }
300 }
301
302 // get m = total constraint dimension, nub = number of unbounded variables.
303 // create constraint offset array and number-of-rows array for all joints.
304 // the constraints are re-ordered as follows: the purely unbounded
305 // constraints, the mixed unbounded + LCP constraints, and last the purely
306 // LCP constraints.
307 //
308 // joints with m=0 are inactive and are removed from the joints array
309 // entirely, so that the code that follows does not consider them.
310 int m = 0;
311 ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1));
312#ifdef dUSE_MALLOC_FOR_ALLOCA
313 if (info == NULL) {
314 UNALLOCA(invI);
315 UNALLOCA(I);
316 UNALLOCA(joint);
317 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
318 return;
319 }
320#endif
321
322 ALLOCA(int,ofs,nj*sizeof(int));
323#ifdef dUSE_MALLOC_FOR_ALLOCA
324 if (ofs == NULL) {
325 UNALLOCA(info);
326 UNALLOCA(invI);
327 UNALLOCA(I);
328 UNALLOCA(joint);
329 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
330 return;
331 }
332#endif
333
334 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
335 joint[j]->vtable->getInfo1 (joint[j],info+i);
336 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
337 info[i].nub >= 0 && info[i].nub <= info[i].m);
338 if (info[i].m > 0) {
339 joint[i] = joint[j];
340 i++;
341 }
342 }
343 nj = i;
344
345 // the purely unbounded constraints
346 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
347 ofs[i] = m;
348 m += info[i].m;
349 }
350 int nub = m;
351 // the mixed unbounded + LCP constraints
352 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
353 ofs[i] = m;
354 m += info[i].m;
355 }
356 // the purely LCP constraints
357 for (i=0; i<nj; i++) if (info[i].nub == 0) {
358 ofs[i] = m;
359 m += info[i].m;
360 }
361
362 // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
363 // parameters
364#ifdef TIMING
365 dTimerNow ("create mass matrix");
366#endif
367 int nskip = dPAD (n6);
368 ALLOCA(dReal, invM, n6*nskip*sizeof(dReal));
369#ifdef dUSE_MALLOC_FOR_ALLOCA
370 if (invM == NULL) {
371 UNALLOCA(ofs);
372 UNALLOCA(info);
373 UNALLOCA(invI);
374 UNALLOCA(I);
375 UNALLOCA(joint);
376 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
377 return;
378 }
379#endif
380
381 dSetZero (invM,n6*nskip);
382 for (i=0; i<nb; i++) {
383 dReal *MM = invM+(i*6)*nskip+(i*6);
384 MM[0] = body[i]->invMass;
385 MM[nskip+1] = body[i]->invMass;
386 MM[2*nskip+2] = body[i]->invMass;
387 MM += 3*nskip+3;
388 for (j=0; j<3; j++) for (k=0; k<3; k++) {
389 MM[j*nskip+k] = invI[i*12+j*4+k];
390 }
391 }
392
393 // assemble some body vectors: fe = external forces, v = velocities
394 ALLOCA(dReal,fe,n6*sizeof(dReal));
395#ifdef dUSE_MALLOC_FOR_ALLOCA
396 if (fe == NULL) {
397 UNALLOCA(invM);
398 UNALLOCA(ofs);
399 UNALLOCA(info);
400 UNALLOCA(invI);
401 UNALLOCA(I);
402 UNALLOCA(joint);
403 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
404 return;
405 }
406#endif
407
408 ALLOCA(dReal,v,n6*sizeof(dReal));
409#ifdef dUSE_MALLOC_FOR_ALLOCA
410 if (v == NULL) {
411 UNALLOCA(fe);
412 UNALLOCA(invM);
413 UNALLOCA(ofs);
414 UNALLOCA(info);
415 UNALLOCA(invI);
416 UNALLOCA(I);
417 UNALLOCA(joint);
418 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
419 return;
420 }
421#endif
422
423 //dSetZero (fe,n6);
424 //dSetZero (v,n6);
425 for (i=0; i<nb; i++) {
426 for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
427 for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
428 for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
429 for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
430 }
431
432 // this will be set to the velocity update
433 ALLOCA(dReal,vnew,n6*sizeof(dReal));
434#ifdef dUSE_MALLOC_FOR_ALLOCA
435 if (vnew == NULL) {
436 UNALLOCA(v);
437 UNALLOCA(fe);
438 UNALLOCA(invM);
439 UNALLOCA(ofs);
440 UNALLOCA(info);
441 UNALLOCA(invI);
442 UNALLOCA(I);
443 UNALLOCA(joint);
444 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
445 return;
446 }
447#endif
448 dSetZero (vnew,n6);
449
450 // if there are constraints, compute cforce
451 if (m > 0) {
452 // create a constraint equation right hand side vector `c', a constraint
453 // force mixing vector `cfm', and LCP low and high bound vectors, and an
454 // 'findex' vector.
455 ALLOCA(dReal,c,m*sizeof(dReal));
456#ifdef dUSE_MALLOC_FOR_ALLOCA
457 if (c == NULL) {
458 UNALLOCA(vnew);
459 UNALLOCA(v);
460 UNALLOCA(fe);
461 UNALLOCA(invM);
462 UNALLOCA(ofs);
463 UNALLOCA(info);
464 UNALLOCA(invI);
465 UNALLOCA(I);
466 UNALLOCA(joint);
467 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
468 return;
469 }
470#endif
471 ALLOCA(dReal,cfm,m*sizeof(dReal));
472#ifdef dUSE_MALLOC_FOR_ALLOCA
473 if (cfm == NULL) {
474 UNALLOCA(c);
475 UNALLOCA(vnew);
476 UNALLOCA(v);
477 UNALLOCA(fe);
478 UNALLOCA(invM);
479 UNALLOCA(ofs);
480 UNALLOCA(info);
481 UNALLOCA(invI);
482 UNALLOCA(I);
483 UNALLOCA(joint);
484 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
485 return;
486 }
487#endif
488 ALLOCA(dReal,lo,m*sizeof(dReal));
489#ifdef dUSE_MALLOC_FOR_ALLOCA
490 if (lo == NULL) {
491 UNALLOCA(cfm);
492 UNALLOCA(c);
493 UNALLOCA(vnew);
494 UNALLOCA(v);
495 UNALLOCA(fe);
496 UNALLOCA(invM);
497 UNALLOCA(ofs);
498 UNALLOCA(info);
499 UNALLOCA(invI);
500 UNALLOCA(I);
501 UNALLOCA(joint);
502 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
503 return;
504 }
505#endif
506 ALLOCA(dReal,hi,m*sizeof(dReal));
507#ifdef dUSE_MALLOC_FOR_ALLOCA
508 if (hi == NULL) {
509 UNALLOCA(lo);
510 UNALLOCA(cfm);
511 UNALLOCA(c);
512 UNALLOCA(vnew);
513 UNALLOCA(v);
514 UNALLOCA(fe);
515 UNALLOCA(invM);
516 UNALLOCA(ofs);
517 UNALLOCA(info);
518 UNALLOCA(invI);
519 UNALLOCA(I);
520 UNALLOCA(joint);
521 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
522 return;
523 }
524#endif
525 ALLOCA(int,findex,m*sizeof(int));
526#ifdef dUSE_MALLOC_FOR_ALLOCA
527 if (findex == NULL) {
528 UNALLOCA(hi);
529 UNALLOCA(lo);
530 UNALLOCA(cfm);
531 UNALLOCA(c);
532 UNALLOCA(vnew);
533 UNALLOCA(v);
534 UNALLOCA(fe);
535 UNALLOCA(invM);
536 UNALLOCA(ofs);
537 UNALLOCA(info);
538 UNALLOCA(invI);
539 UNALLOCA(I);
540 UNALLOCA(joint);
541 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
542 return;
543 }
544#endif
545 dSetZero (c,m);
546 dSetValue (cfm,m,world->global_cfm);
547 dSetValue (lo,m,-dInfinity);
548 dSetValue (hi,m, dInfinity);
549 for (i=0; i<m; i++) findex[i] = -1;
550
551 // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
552 // data. also fill the c vector.
553# ifdef TIMING
554 dTimerNow ("create J");
555# endif
556 ALLOCA(dReal,J,m*nskip*sizeof(dReal));
557#ifdef dUSE_MALLOC_FOR_ALLOCA
558 if (J == NULL) {
559 UNALLOCA(findex);
560 UNALLOCA(hi);
561 UNALLOCA(lo);
562 UNALLOCA(cfm);
563 UNALLOCA(c);
564 UNALLOCA(vnew);
565 UNALLOCA(v);
566 UNALLOCA(fe);
567 UNALLOCA(invM);
568 UNALLOCA(ofs);
569 UNALLOCA(info);
570 UNALLOCA(invI);
571 UNALLOCA(I);
572 UNALLOCA(joint);
573 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
574 return;
575 }
576#endif
577 dSetZero (J,m*nskip);
578 dxJoint::Info2 Jinfo;
579 Jinfo.rowskip = nskip;
580 Jinfo.fps = dRecip(stepsize);
581 Jinfo.erp = world->global_erp;
582 for (i=0; i<nj; i++) {
583 Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
584 Jinfo.J1a = Jinfo.J1l + 3;
585 if (joint[i]->node[1].body) {
586 Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
587 Jinfo.J2a = Jinfo.J2l + 3;
588 }
589 else {
590 Jinfo.J2l = 0;
591 Jinfo.J2a = 0;
592 }
593 Jinfo.c = c + ofs[i];
594 Jinfo.cfm = cfm + ofs[i];
595 Jinfo.lo = lo + ofs[i];
596 Jinfo.hi = hi + ofs[i];
597 Jinfo.findex = findex + ofs[i];
598 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
599 // adjust returned findex values for global index numbering
600 for (j=0; j<info[i].m; j++) {
601 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
602 }
603 }
604
605 // compute A = J*invM*J'
606# ifdef TIMING
607 dTimerNow ("compute A");
608# endif
609 ALLOCA(dReal,JinvM,m*nskip*sizeof(dReal));
610#ifdef dUSE_MALLOC_FOR_ALLOCA
611 if (JinvM == NULL) {
612 UNALLOCA(J);
613 UNALLOCA(findex);
614 UNALLOCA(hi);
615 UNALLOCA(lo);
616 UNALLOCA(cfm);
617 UNALLOCA(c);
618 UNALLOCA(vnew);
619 UNALLOCA(v);
620 UNALLOCA(fe);
621 UNALLOCA(invM);
622 UNALLOCA(ofs);
623 UNALLOCA(info);
624 UNALLOCA(invI);
625 UNALLOCA(I);
626 UNALLOCA(joint);
627 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
628 return;
629 }
630#endif
631 //dSetZero (JinvM,m*nskip);
632 dMultiply0 (JinvM,J,invM,m,n6,n6);
633 int mskip = dPAD(m);
634 ALLOCA(dReal,A,m*mskip*sizeof(dReal));
635#ifdef dUSE_MALLOC_FOR_ALLOCA
636 if (A == NULL) {
637 UNALLOCA(JinvM);
638 UNALLOCA(J);
639 UNALLOCA(findex);
640 UNALLOCA(hi);
641 UNALLOCA(lo);
642 UNALLOCA(cfm);
643 UNALLOCA(c);
644 UNALLOCA(vnew);
645 UNALLOCA(v);
646 UNALLOCA(fe);
647 UNALLOCA(invM);
648 UNALLOCA(ofs);
649 UNALLOCA(info);
650 UNALLOCA(invI);
651 UNALLOCA(I);
652 UNALLOCA(joint);
653 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
654 return;
655 }
656#endif
657 //dSetZero (A,m*mskip);
658 dMultiply2 (A,JinvM,J,m,n6,m);
659
660 // add cfm to the diagonal of A
661 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
662
663# ifdef COMPARE_METHODS
664 comparator.nextMatrix (A,m,m,1,"A");
665# endif
666
667 // compute `rhs', the right hand side of the equation J*a=c
668# ifdef TIMING
669 dTimerNow ("compute rhs");
670# endif
671 ALLOCA(dReal,tmp1,n6*sizeof(dReal));
672#ifdef dUSE_MALLOC_FOR_ALLOCA
673 if (tmp1 == NULL) {
674 UNALLOCA(A);
675 UNALLOCA(JinvM);
676 UNALLOCA(J);
677 UNALLOCA(findex);
678 UNALLOCA(hi);
679 UNALLOCA(lo);
680 UNALLOCA(cfm);
681 UNALLOCA(c);
682 UNALLOCA(vnew);
683 UNALLOCA(v);
684 UNALLOCA(fe);
685 UNALLOCA(invM);
686 UNALLOCA(ofs);
687 UNALLOCA(info);
688 UNALLOCA(invI);
689 UNALLOCA(I);
690 UNALLOCA(joint);
691 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
692 return;
693 }
694#endif
695 //dSetZero (tmp1,n6);
696 dMultiply0 (tmp1,invM,fe,n6,n6,1);
697 for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
698 ALLOCA(dReal,rhs,m*sizeof(dReal));
699#ifdef dUSE_MALLOC_FOR_ALLOCA
700 if (rhs == NULL) {
701 UNALLOCA(tmp1);
702 UNALLOCA(A);
703 UNALLOCA(JinvM);
704 UNALLOCA(J);
705 UNALLOCA(findex);
706 UNALLOCA(hi);
707 UNALLOCA(lo);
708 UNALLOCA(cfm);
709 UNALLOCA(c);
710 UNALLOCA(vnew);
711 UNALLOCA(v);
712 UNALLOCA(fe);
713 UNALLOCA(invM);
714 UNALLOCA(ofs);
715 UNALLOCA(info);
716 UNALLOCA(invI);
717 UNALLOCA(I);
718 UNALLOCA(joint);
719 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
720 return;
721 }
722#endif
723 //dSetZero (rhs,m);
724 dMultiply0 (rhs,J,tmp1,m,n6,1);
725 for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
726
727# ifdef COMPARE_METHODS
728 comparator.nextMatrix (c,m,1,0,"c");
729 comparator.nextMatrix (rhs,m,1,0,"rhs");
730# endif
731
732
733
734
735
736#ifndef DIRECT_CHOLESKY
737 // solve the LCP problem and get lambda.
738 // this will destroy A but that's okay
739# ifdef TIMING
740 dTimerNow ("solving LCP problem");
741# endif
742 ALLOCA(dReal,lambda,m*sizeof(dReal));
743#ifdef dUSE_MALLOC_FOR_ALLOCA
744 if (lambda == NULL) {
745 UNALLOCA(rhs);
746 UNALLOCA(tmp1);
747 UNALLOCA(A);
748 UNALLOCA(JinvM);
749 UNALLOCA(J);
750 UNALLOCA(findex);
751 UNALLOCA(hi);
752 UNALLOCA(lo);
753 UNALLOCA(cfm);
754 UNALLOCA(c);
755 UNALLOCA(vnew);
756 UNALLOCA(v);
757 UNALLOCA(fe);
758 UNALLOCA(invM);
759 UNALLOCA(ofs);
760 UNALLOCA(info);
761 UNALLOCA(invI);
762 UNALLOCA(I);
763 UNALLOCA(joint);
764 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
765 return;
766 }
767#endif
768 ALLOCA(dReal,residual,m*sizeof(dReal));
769#ifdef dUSE_MALLOC_FOR_ALLOCA
770 if (residual == NULL) {
771 UNALLOCA(lambda);
772 UNALLOCA(rhs);
773 UNALLOCA(tmp1);
774 UNALLOCA(A);
775 UNALLOCA(JinvM);
776 UNALLOCA(J);
777 UNALLOCA(findex);
778 UNALLOCA(hi);
779 UNALLOCA(lo);
780 UNALLOCA(cfm);
781 UNALLOCA(c);
782 UNALLOCA(vnew);
783 UNALLOCA(v);
784 UNALLOCA(fe);
785 UNALLOCA(invM);
786 UNALLOCA(ofs);
787 UNALLOCA(info);
788 UNALLOCA(invI);
789 UNALLOCA(I);
790 UNALLOCA(joint);
791 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
792 return;
793 }
794#endif
795 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
796 UNALLOCA(residual);
797 UNALLOCA(lambda);
798
799#ifdef dUSE_MALLOC_FOR_ALLOCA
800 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
801 return;
802#endif
803
804
805#else
806
807 // OLD WAY - direct factor and solve
808
809 // factorize A (L*L'=A)
810# ifdef TIMING
811 dTimerNow ("factorize A");
812# endif
813 ALLOCA(dReal,L,m*mskip*sizeof(dReal));
814#ifdef dUSE_MALLOC_FOR_ALLOCA
815 if (L == NULL) {
816 UNALLOCA(rhs);
817 UNALLOCA(tmp1);
818 UNALLOCA(A);
819 UNALLOCA(JinvM);
820 UNALLOCA(J);
821 UNALLOCA(findex);
822 UNALLOCA(hi);
823 UNALLOCA(lo);
824 UNALLOCA(cfm);
825 UNALLOCA(c);
826 UNALLOCA(vnew);
827 UNALLOCA(v);
828 UNALLOCA(fe);
829 UNALLOCA(invM);
830 UNALLOCA(ofs);
831 UNALLOCA(info);
832 UNALLOCA(invI);
833 UNALLOCA(I);
834 UNALLOCA(joint);
835 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
836 return;
837 }
838#endif
839 memcpy (L,A,m*mskip*sizeof(dReal));
840 if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
841
842 // compute lambda
843# ifdef TIMING
844 dTimerNow ("compute lambda");
845# endif
846 ALLOCA(dReal,lambda,m*sizeof(dReal));
847#ifdef dUSE_MALLOC_FOR_ALLOCA
848 if (lambda == NULL) {
849 UNALLOCA(L);
850 UNALLOCA(rhs);
851 UNALLOCA(tmp1);
852 UNALLOCA(A);
853 UNALLOCA(JinvM);
854 UNALLOCA(J);
855 UNALLOCA(findex);
856 UNALLOCA(hi);
857 UNALLOCA(lo);
858 UNALLOCA(cfm);
859 UNALLOCA(c);
860 UNALLOCA(vnew);
861 UNALLOCA(v);
862 UNALLOCA(fe);
863 UNALLOCA(invM);
864 UNALLOCA(ofs);
865 UNALLOCA(info);
866 UNALLOCA(invI);
867 UNALLOCA(I);
868 UNALLOCA(joint);
869 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
870 return;
871 }
872#endif
873 memcpy (lambda,rhs,m * sizeof(dReal));
874 dSolveCholesky (L,lambda,m);
875#endif
876
877# ifdef COMPARE_METHODS
878 comparator.nextMatrix (lambda,m,1,0,"lambda");
879# endif
880
881 // compute the velocity update `vnew'
882# ifdef TIMING
883 dTimerNow ("compute velocity update");
884# endif
885 dMultiply1 (tmp1,J,lambda,n6,m,1);
886 for (i=0; i<n6; i++) tmp1[i] += fe[i];
887 dMultiply0 (vnew,invM,tmp1,n6,n6,1);
888 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
889
890#ifdef REPORT_ERROR
891 // see if the constraint has worked: compute J*vnew and make sure it equals
892 // `c' (to within a certain tolerance).
893# ifdef TIMING
894 dTimerNow ("verify constraint equation");
895# endif
896 dMultiply0 (tmp1,J,vnew,m,n6,1);
897 dReal err = 0;
898 for (i=0; i<m; i++) {
899 err += dFabs(tmp1[i]-c[i]);
900 }
901 printf ("total constraint error=%.6e\n",err);
902#endif
903
904 UNALLOCA(c);
905 UNALLOCA(cfm);
906 UNALLOCA(lo);
907 UNALLOCA(hi);
908 UNALLOCA(findex);
909 UNALLOCA(J);
910 UNALLOCA(JinvM);
911 UNALLOCA(A);
912 UNALLOCA(tmp1);
913 UNALLOCA(rhs);
914 UNALLOCA(lambda);
915 UNALLOCA(L);
916 }
917 else {
918 // no constraints
919 dMultiply0 (vnew,invM,fe,n6,n6,1);
920 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
921 }
922
923#ifdef COMPARE_METHODS
924 comparator.nextMatrix (vnew,n6,1,0,"vnew");
925#endif
926
927 // apply the velocity update to the bodies
928#ifdef TIMING
929 dTimerNow ("update velocity");
930#endif
931 for (i=0; i<nb; i++) {
932 for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
933 for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
934 }
935
936 // update the position and orientation from the new linear/angular velocity
937 // (over the given timestep)
938#ifdef TIMING
939 dTimerNow ("update position");
940#endif
941 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
942
943#ifdef TIMING
944 dTimerNow ("tidy up");
945#endif
946
947 // zero all force accumulators
948 for (i=0; i<nb; i++) {
949 body[i]->facc[0] = 0;
950 body[i]->facc[1] = 0;
951 body[i]->facc[2] = 0;
952 body[i]->facc[3] = 0;
953 body[i]->tacc[0] = 0;
954 body[i]->tacc[1] = 0;
955 body[i]->tacc[2] = 0;
956 body[i]->tacc[3] = 0;
957 }
958
959#ifdef TIMING
960 dTimerEnd();
961 if (m > 0) dTimerReport (stdout,1);
962#endif
963
964 UNALLOCA(joint);
965 UNALLOCA(I);
966 UNALLOCA(invI);
967 UNALLOCA(info);
968 UNALLOCA(ofs);
969 UNALLOCA(invM);
970 UNALLOCA(fe);
971 UNALLOCA(v);
972 UNALLOCA(vnew);
973}
974
975//****************************************************************************
976// an optimized version of dInternalStepIsland1()
977
978void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
979 dxJoint * const *_joint, int nj, dReal stepsize)
980{
981 int i,j,k;
982#ifdef TIMING
983 dTimerStart("preprocessing");
984#endif
985
986 dReal stepsize1 = dRecip(stepsize);
987
988 // number all bodies in the body list - set their tag values
989 for (i=0; i<nb; i++) body[i]->tag = i;
990
991 // make a local copy of the joint array, because we might want to modify it.
992 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
993 // but not the joint array, because the caller might need it unchanged).
994 ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*));
995#ifdef dUSE_MALLOC_FOR_ALLOCA
996 if (joint == NULL) {
997 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
998 return;
999 }
1000#endif
1001 memcpy (joint,_joint,nj * sizeof(dxJoint*));
1002
1003 // for all bodies, compute the inertia tensor and its inverse in the global
1004 // frame, and compute the rotational force and add it to the torque
1005 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
1006 // @@@ check computation of rotational force.
1007#ifdef dGYROSCOPIC
1008 ALLOCA(dReal,I,3*nb*4*sizeof(dReal));
1009# ifdef dUSE_MALLOC_FOR_ALLOCA
1010 if (I == NULL) {
1011 UNALLOCA(joint);
1012 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1013 return;
1014 }
1015# endif
1016#else
1017 dReal *I = NULL;
1018#endif // for dGYROSCOPIC
1019
1020 ALLOCA(dReal,invI,3*nb*4*sizeof(dReal));
1021#ifdef dUSE_MALLOC_FOR_ALLOCA
1022 if (invI == NULL) {
1023 UNALLOCA(I);
1024 UNALLOCA(joint);
1025 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1026 return;
1027 }
1028#endif
1029
1030 //dSetZero (I,3*nb*4);
1031 //dSetZero (invI,3*nb*4);
1032 for (i=0; i<nb; i++) {
1033 dReal tmp[12];
1034
1035 // compute inverse inertia tensor in global frame
1036 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);
1037 dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);
1038#ifdef dGYROSCOPIC
1039 // compute inertia tensor in global frame
1040 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
1041 dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);
1042
1043 // compute rotational force
1044 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
1045 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
1046#endif
1047 }
1048
1049 // add the gravity force to all bodies
1050 for (i=0; i<nb; i++) {
1051 if ((body[i]->flags & dxBodyNoGravity)==0) {
1052 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
1053 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
1054 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
1055 }
1056 }
1057
1058 // get m = total constraint dimension, nub = number of unbounded variables.
1059 // create constraint offset array and number-of-rows array for all joints.
1060 // the constraints are re-ordered as follows: the purely unbounded
1061 // constraints, the mixed unbounded + LCP constraints, and last the purely
1062 // LCP constraints. this assists the LCP solver to put all unbounded
1063 // variables at the start for a quick factorization.
1064 //
1065 // joints with m=0 are inactive and are removed from the joints array
1066 // entirely, so that the code that follows does not consider them.
1067 // also number all active joints in the joint list (set their tag values).
1068 // inactive joints receive a tag value of -1.
1069
1070 int m = 0;
1071 ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1));
1072#ifdef dUSE_MALLOC_FOR_ALLOCA
1073 if (info == NULL) {
1074 UNALLOCA(invI);
1075 UNALLOCA(I);
1076 UNALLOCA(joint);
1077 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1078 return;
1079 }
1080#endif
1081 ALLOCA(int,ofs,nj*sizeof(int));
1082#ifdef dUSE_MALLOC_FOR_ALLOCA
1083 if (ofs == NULL) {
1084 UNALLOCA(info);
1085 UNALLOCA(invI);
1086 UNALLOCA(I);
1087 UNALLOCA(joint);
1088 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1089 return;
1090 }
1091#endif
1092 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
1093 joint[j]->vtable->getInfo1 (joint[j],info+i);
1094 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
1095 info[i].nub >= 0 && info[i].nub <= info[i].m);
1096 if (info[i].m > 0) {
1097 joint[i] = joint[j];
1098 joint[i]->tag = i;
1099 i++;
1100 }
1101 else {
1102 joint[j]->tag = -1;
1103 }
1104 }
1105 nj = i;
1106
1107 // the purely unbounded constraints
1108 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
1109 ofs[i] = m;
1110 m += info[i].m;
1111 }
1112 int nub = m;
1113 // the mixed unbounded + LCP constraints
1114 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
1115 ofs[i] = m;
1116 m += info[i].m;
1117 }
1118 // the purely LCP constraints
1119 for (i=0; i<nj; i++) if (info[i].nub == 0) {
1120 ofs[i] = m;
1121 m += info[i].m;
1122 }
1123
1124 // this will be set to the force due to the constraints
1125 ALLOCA(dReal,cforce,nb*8*sizeof(dReal));
1126#ifdef dUSE_MALLOC_FOR_ALLOCA
1127 if (cforce == NULL) {
1128 UNALLOCA(ofs);
1129 UNALLOCA(info);
1130 UNALLOCA(invI);
1131 UNALLOCA(I);
1132 UNALLOCA(joint);
1133 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1134 return;
1135 }
1136#endif
1137 dSetZero (cforce,nb*8);
1138
1139 // if there are constraints, compute cforce
1140 if (m > 0) {
1141 // create a constraint equation right hand side vector `c', a constraint
1142 // force mixing vector `cfm', and LCP low and high bound vectors, and an
1143 // 'findex' vector.
1144 ALLOCA(dReal,c,m*sizeof(dReal));
1145#ifdef dUSE_MALLOC_FOR_ALLOCA
1146 if (c == NULL) {
1147 UNALLOCA(cforce);
1148 UNALLOCA(ofs);
1149 UNALLOCA(info);
1150 UNALLOCA(invI);
1151 UNALLOCA(I);
1152 UNALLOCA(joint);
1153 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1154 return;
1155 }
1156#endif
1157 ALLOCA(dReal,cfm,m*sizeof(dReal));
1158#ifdef dUSE_MALLOC_FOR_ALLOCA
1159 if (cfm == NULL) {
1160 UNALLOCA(c);
1161 UNALLOCA(cforce);
1162 UNALLOCA(ofs);
1163 UNALLOCA(info);
1164 UNALLOCA(invI);
1165 UNALLOCA(I);
1166 UNALLOCA(joint);
1167 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1168 return;
1169 }
1170#endif
1171 ALLOCA(dReal,lo,m*sizeof(dReal));
1172#ifdef dUSE_MALLOC_FOR_ALLOCA
1173 if (lo == NULL) {
1174 UNALLOCA(cfm);
1175 UNALLOCA(c);
1176 UNALLOCA(cforce);
1177 UNALLOCA(ofs);
1178 UNALLOCA(info);
1179 UNALLOCA(invI);
1180 UNALLOCA(I);
1181 UNALLOCA(joint);
1182 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1183 return;
1184 }
1185#endif
1186 ALLOCA(dReal,hi,m*sizeof(dReal));
1187#ifdef dUSE_MALLOC_FOR_ALLOCA
1188 if (hi == NULL) {
1189 UNALLOCA(lo);
1190 UNALLOCA(cfm);
1191 UNALLOCA(c);
1192 UNALLOCA(cforce);
1193 UNALLOCA(ofs);
1194 UNALLOCA(info);
1195 UNALLOCA(invI);
1196 UNALLOCA(I);
1197 UNALLOCA(joint);
1198 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1199 return;
1200 }
1201#endif
1202 ALLOCA(int,findex,m*sizeof(int));
1203#ifdef dUSE_MALLOC_FOR_ALLOCA
1204 if (findex == NULL) {
1205 UNALLOCA(hi);
1206 UNALLOCA(lo);
1207 UNALLOCA(cfm);
1208 UNALLOCA(c);
1209 UNALLOCA(cforce);
1210 UNALLOCA(ofs);
1211 UNALLOCA(info);
1212 UNALLOCA(invI);
1213 UNALLOCA(I);
1214 UNALLOCA(joint);
1215 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1216 return;
1217 }
1218#endif
1219 dSetZero (c,m);
1220 dSetValue (cfm,m,world->global_cfm);
1221 dSetValue (lo,m,-dInfinity);
1222 dSetValue (hi,m, dInfinity);
1223 for (i=0; i<m; i++) findex[i] = -1;
1224
1225 // get jacobian data from constraints. a (2*m)x8 matrix will be created
1226 // to store the two jacobian blocks from each constraint. it has this
1227 // format:
1228 //
1229 // l l l 0 a a a 0 \ .
1230 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
1231 // l l l 0 a a a 0 /
1232 // l l l 0 a a a 0 \ .
1233 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
1234 // l l l 0 a a a 0 /
1235 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
1236 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
1237 // etc...
1238 //
1239 // (lll) = linear jacobian data
1240 // (aaa) = angular jacobian data
1241 //
1242# ifdef TIMING
1243 dTimerNow ("create J");
1244# endif
1245 ALLOCA(dReal,J,2*m*8*sizeof(dReal));
1246#ifdef dUSE_MALLOC_FOR_ALLOCA
1247 if (J == NULL) {
1248 UNALLOCA(findex);
1249 UNALLOCA(hi);
1250 UNALLOCA(lo);
1251 UNALLOCA(cfm);
1252 UNALLOCA(c);
1253 UNALLOCA(cforce);
1254 UNALLOCA(ofs);
1255 UNALLOCA(info);
1256 UNALLOCA(invI);
1257 UNALLOCA(I);
1258 UNALLOCA(joint);
1259 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1260 return;
1261 }
1262#endif
1263 dSetZero (J,2*m*8);
1264 dxJoint::Info2 Jinfo;
1265 Jinfo.rowskip = 8;
1266 Jinfo.fps = stepsize1;
1267 Jinfo.erp = world->global_erp;
1268 for (i=0; i<nj; i++) {
1269 Jinfo.J1l = J + 2*8*ofs[i];
1270 Jinfo.J1a = Jinfo.J1l + 4;
1271 Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
1272 Jinfo.J2a = Jinfo.J2l + 4;
1273 Jinfo.c = c + ofs[i];
1274 Jinfo.cfm = cfm + ofs[i];
1275 Jinfo.lo = lo + ofs[i];
1276 Jinfo.hi = hi + ofs[i];
1277 Jinfo.findex = findex + ofs[i];
1278 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
1279 // adjust returned findex values for global index numbering
1280 for (j=0; j<info[i].m; j++) {
1281 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
1282 }
1283 }
1284
1285 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
1286 // format as J so we just go through the constraints in J multiplying by
1287 // the appropriate scalars and matrices.
1288# ifdef TIMING
1289 dTimerNow ("compute A");
1290# endif
1291 ALLOCA(dReal,JinvM,2*m*8*sizeof(dReal));
1292#ifdef dUSE_MALLOC_FOR_ALLOCA
1293 if (JinvM == NULL) {
1294 UNALLOCA(J);
1295 UNALLOCA(findex);
1296 UNALLOCA(hi);
1297 UNALLOCA(lo);
1298 UNALLOCA(cfm);
1299 UNALLOCA(c);
1300 UNALLOCA(cforce);
1301 UNALLOCA(ofs);
1302 UNALLOCA(info);
1303 UNALLOCA(invI);
1304 UNALLOCA(I);
1305 UNALLOCA(joint);
1306 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1307 return;
1308 }
1309#endif
1310 dSetZero (JinvM,2*m*8);
1311 for (i=0; i<nj; i++) {
1312 int b = joint[i]->node[0].body->tag;
1313 dReal body_invMass = body[b]->invMass;
1314 dReal *body_invI = invI + b*12;
1315 dReal *Jsrc = J + 2*8*ofs[i];
1316 dReal *Jdst = JinvM + 2*8*ofs[i];
1317 for (j=info[i].m-1; j>=0; j--) {
1318 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
1319 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
1320 Jsrc += 8;
1321 Jdst += 8;
1322 }
1323 if (joint[i]->node[1].body) {
1324 b = joint[i]->node[1].body->tag;
1325 body_invMass = body[b]->invMass;
1326 body_invI = invI + b*12;
1327 for (j=info[i].m-1; j>=0; j--) {
1328 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
1329 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
1330 Jsrc += 8;
1331 Jdst += 8;
1332 }
1333 }
1334 }
1335
1336 // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
1337 // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
1338 // if joints i and j have at least one body in common. this fact suggests
1339 // the algorithm used to fill A:
1340 //
1341 // for b = all bodies
1342 // n = number of joints attached to body b
1343 // for i = 1..n
1344 // for j = i+1..n
1345 // ii = actual joint number for i
1346 // jj = actual joint number for j
1347 // // (ii,jj) will be set to all pairs of joints around body b
1348 // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
1349 //
1350 // this algorithm catches all pairs of joints that have at least one body
1351 // in common. it does not compute the diagonal blocks of A however -
1352 // another similar algorithm does that.
1353
1354 int mskip = dPAD(m);
1355 ALLOCA(dReal,A,m*mskip*sizeof(dReal));
1356#ifdef dUSE_MALLOC_FOR_ALLOCA
1357 if (A == NULL) {
1358 UNALLOCA(JinvM);
1359 UNALLOCA(J);
1360 UNALLOCA(findex);
1361 UNALLOCA(hi);
1362 UNALLOCA(lo);
1363 UNALLOCA(cfm);
1364 UNALLOCA(c);
1365 UNALLOCA(cforce);
1366 UNALLOCA(ofs);
1367 UNALLOCA(info);
1368 UNALLOCA(invI);
1369 UNALLOCA(I);
1370 UNALLOCA(joint);
1371 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1372 return;
1373 }
1374#endif
1375 dSetZero (A,m*mskip);
1376 for (i=0; i<nb; i++) {
1377 for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
1378 for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
1379 // get joint numbers and ensure ofs[j1] >= ofs[j2]
1380 int j1 = n1->joint->tag;
1381 int j2 = n2->joint->tag;
1382 if (ofs[j1] < ofs[j2]) {
1383 int tmp = j1;
1384 j1 = j2;
1385 j2 = tmp;
1386 }
1387
1388 // if either joint was tagged as -1 then it is an inactive (m=0)
1389 // joint that should not be considered
1390 if (j1==-1 || j2==-1) continue;
1391
1392 // determine if body i is the 1st or 2nd body of joints j1 and j2
1393 int jb1 = (joint[j1]->node[1].body == body[i]);
1394 int jb2 = (joint[j2]->node[1].body == body[i]);
1395 // jb1/jb2 must be 0 for joints with only one body
1396 dIASSERT(joint[j1]->node[1].body || jb1==0);
1397 dIASSERT(joint[j2]->node[1].body || jb2==0);
1398
1399 // set block of A
1400 MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
1401 JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
1402 J + 2*8*ofs[j2] + jb2*8*info[j2].m,
1403 info[j1].m,info[j2].m, mskip);
1404 }
1405 }
1406 }
1407 // compute diagonal blocks of A
1408 for (i=0; i<nj; i++) {
1409 Multiply2_p8r (A + ofs[i]*(mskip+1),
1410 JinvM + 2*8*ofs[i],
1411 J + 2*8*ofs[i],
1412 info[i].m,info[i].m, mskip);
1413 if (joint[i]->node[1].body) {
1414 MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
1415 JinvM + 2*8*ofs[i] + 8*info[i].m,
1416 J + 2*8*ofs[i] + 8*info[i].m,
1417 info[i].m,info[i].m, mskip);
1418 }
1419 }
1420
1421 // add cfm to the diagonal of A
1422 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
1423
1424# ifdef COMPARE_METHODS
1425 comparator.nextMatrix (A,m,m,1,"A");
1426# endif
1427
1428 // compute the right hand side `rhs'
1429# ifdef TIMING
1430 dTimerNow ("compute rhs");
1431# endif
1432 ALLOCA(dReal,tmp1,nb*8*sizeof(dReal));
1433#ifdef dUSE_MALLOC_FOR_ALLOCA
1434 if (tmp1 == NULL) {
1435 UNALLOCA(A);
1436 UNALLOCA(JinvM);
1437 UNALLOCA(J);
1438 UNALLOCA(findex);
1439 UNALLOCA(hi);
1440 UNALLOCA(lo);
1441 UNALLOCA(cfm);
1442 UNALLOCA(c);
1443 UNALLOCA(cforce);
1444 UNALLOCA(ofs);
1445 UNALLOCA(info);
1446 UNALLOCA(invI);
1447 UNALLOCA(I);
1448 UNALLOCA(joint);
1449 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1450 return;
1451 }
1452#endif
1453 //dSetZero (tmp1,nb*8);
1454 // put v/h + invM*fe into tmp1
1455 for (i=0; i<nb; i++) {
1456 dReal body_invMass = body[i]->invMass;
1457 dReal *body_invI = invI + i*12;
1458 for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
1459 body[i]->lvel[j] * stepsize1;
1460 dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
1461 for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
1462 }
1463 // put J*tmp1 into rhs
1464 ALLOCA(dReal,rhs,m*sizeof(dReal));
1465#ifdef dUSE_MALLOC_FOR_ALLOCA
1466 if (rhs == NULL) {
1467 UNALLOCA(tmp1);
1468 UNALLOCA(A);
1469 UNALLOCA(JinvM);
1470 UNALLOCA(J);
1471 UNALLOCA(findex);
1472 UNALLOCA(hi);
1473 UNALLOCA(lo);
1474 UNALLOCA(cfm);
1475 UNALLOCA(c);
1476 UNALLOCA(cforce);
1477 UNALLOCA(ofs);
1478 UNALLOCA(info);
1479 UNALLOCA(invI);
1480 UNALLOCA(I);
1481 UNALLOCA(joint);
1482 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1483 return;
1484 }
1485#endif
1486 //dSetZero (rhs,m);
1487 for (i=0; i<nj; i++) {
1488 dReal *JJ = J + 2*8*ofs[i];
1489 Multiply0_p81 (rhs+ofs[i],JJ,
1490 tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
1491 if (joint[i]->node[1].body) {
1492 MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
1493 tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
1494 }
1495 }
1496 // complete rhs
1497 for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
1498
1499# ifdef COMPARE_METHODS
1500 comparator.nextMatrix (c,m,1,0,"c");
1501 comparator.nextMatrix (rhs,m,1,0,"rhs");
1502# endif
1503
1504 // solve the LCP problem and get lambda.
1505 // this will destroy A but that's okay
1506# ifdef TIMING
1507 dTimerNow ("solving LCP problem");
1508# endif
1509 ALLOCA(dReal,lambda,m*sizeof(dReal));
1510#ifdef dUSE_MALLOC_FOR_ALLOCA
1511 if (lambda == NULL) {
1512 UNALLOCA(rhs);
1513 UNALLOCA(tmp1);
1514 UNALLOCA(A);
1515 UNALLOCA(JinvM);
1516 UNALLOCA(J);
1517 UNALLOCA(findex);
1518 UNALLOCA(hi);
1519 UNALLOCA(lo);
1520 UNALLOCA(cfm);
1521 UNALLOCA(c);
1522 UNALLOCA(cforce);
1523 UNALLOCA(ofs);
1524 UNALLOCA(info);
1525 UNALLOCA(invI);
1526 UNALLOCA(I);
1527 UNALLOCA(joint);
1528 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1529 return;
1530 }
1531#endif
1532 ALLOCA(dReal,residual,m*sizeof(dReal));
1533#ifdef dUSE_MALLOC_FOR_ALLOCA
1534 if (residual == NULL) {
1535 UNALLOCA(lambda);
1536 UNALLOCA(rhs);
1537 UNALLOCA(tmp1);
1538 UNALLOCA(A);
1539 UNALLOCA(JinvM);
1540 UNALLOCA(J);
1541 UNALLOCA(findex);
1542 UNALLOCA(hi);
1543 UNALLOCA(lo);
1544 UNALLOCA(cfm);
1545 UNALLOCA(c);
1546 UNALLOCA(cforce);
1547 UNALLOCA(ofs);
1548 UNALLOCA(info);
1549 UNALLOCA(invI);
1550 UNALLOCA(I);
1551 UNALLOCA(joint);
1552 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1553 return;
1554 }
1555#endif
1556 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
1557
1558#ifdef dUSE_MALLOC_FOR_ALLOCA
1559 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
1560 return;
1561#endif
1562
1563
1564// OLD WAY - direct factor and solve
1565//
1566// // factorize A (L*L'=A)
1567//# ifdef TIMING
1568// dTimerNow ("factorize A");
1569//# endif
1570// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
1571// memcpy (L,A,m*mskip*sizeof(dReal));
1572//# ifdef FAST_FACTOR
1573// dFastFactorCholesky (L,m); // does not report non positive definiteness
1574//# else
1575// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
1576//# endif
1577//
1578// // compute lambda
1579//# ifdef TIMING
1580// dTimerNow ("compute lambda");
1581//# endif
1582// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
1583// memcpy (lambda,rhs,m * sizeof(dReal));
1584// dSolveCholesky (L,lambda,m);
1585
1586# ifdef COMPARE_METHODS
1587 comparator.nextMatrix (lambda,m,1,0,"lambda");
1588# endif
1589
1590 // compute the constraint force `cforce'
1591# ifdef TIMING
1592 dTimerNow ("compute constraint force");
1593# endif
1594 // compute cforce = J'*lambda
1595 for (i=0; i<nj; i++) {
1596 dReal *JJ = J + 2*8*ofs[i];
1597 dxBody* b1 = joint[i]->node[0].body;
1598 dxBody* b2 = joint[i]->node[1].body;
1599 dJointFeedback *fb = joint[i]->feedback;
1600
1601 if (fb) {
1602 // the user has requested feedback on the amount of force that this
1603 // joint is applying to the bodies. we use a slightly slower
1604 // computation that splits out the force components and puts them
1605 // in the feedback structure.
1606 dReal data1[8],data2[8];
1607 Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
1608 dReal *cf1 = cforce + 8*b1->tag;
1609 cf1[0] += (fb->f1[0] = data1[0]);
1610 cf1[1] += (fb->f1[1] = data1[1]);
1611 cf1[2] += (fb->f1[2] = data1[2]);
1612 cf1[4] += (fb->t1[0] = data1[4]);
1613 cf1[5] += (fb->t1[1] = data1[5]);
1614 cf1[6] += (fb->t1[2] = data1[6]);
1615 if (b2){
1616 Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
1617 dReal *cf2 = cforce + 8*b2->tag;
1618 cf2[0] += (fb->f2[0] = data2[0]);
1619 cf2[1] += (fb->f2[1] = data2[1]);
1620 cf2[2] += (fb->f2[2] = data2[2]);
1621 cf2[4] += (fb->t2[0] = data2[4]);
1622 cf2[5] += (fb->t2[1] = data2[5]);
1623 cf2[6] += (fb->t2[2] = data2[6]);
1624 }
1625 }
1626 else {
1627 // no feedback is required, let's compute cforce the faster way
1628 MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
1629 if (b2) {
1630 MultiplyAdd1_8q1 (cforce + 8*b2->tag,
1631 JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
1632 }
1633 }
1634 }
1635 UNALLOCA(c);
1636 UNALLOCA(cfm);
1637 UNALLOCA(lo);
1638 UNALLOCA(hi);
1639 UNALLOCA(findex);
1640 UNALLOCA(J);
1641 UNALLOCA(JinvM);
1642 UNALLOCA(A);
1643 UNALLOCA(tmp1);
1644 UNALLOCA(rhs);
1645 UNALLOCA(lambda);
1646 UNALLOCA(residual);
1647 }
1648
1649 // compute the velocity update
1650#ifdef TIMING
1651 dTimerNow ("compute velocity update");
1652#endif
1653
1654 // add fe to cforce
1655 for (i=0; i<nb; i++) {
1656 for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
1657 for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
1658 }
1659 // multiply cforce by stepsize
1660 for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
1661 // add invM * cforce to the body velocity
1662 for (i=0; i<nb; i++) {
1663 dReal body_invMass = body[i]->invMass;
1664 dReal *body_invI = invI + i*12;
1665 for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
1666 dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
1667 }
1668
1669 // update the position and orientation from the new linear/angular velocity
1670 // (over the given timestep)
1671# ifdef TIMING
1672 dTimerNow ("update position");
1673# endif
1674 for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
1675
1676#ifdef COMPARE_METHODS
1677 ALLOCA(dReal,tmp, ALLOCA (nb*6*sizeof(dReal));
1678#ifdef dUSE_MALLOC_FOR_ALLOCA
1679 if (tmp == NULL) {
1680 UNALLOCA(cforce);
1681 UNALLOCA(ofs);
1682 UNALLOCA(info);
1683 UNALLOCA(invI);
1684 UNALLOCA(I);
1685 UNALLOCA(joint);
1686 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1687 return;
1688 }
1689#endif
1690 for (i=0; i<nb; i++) {
1691 for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
1692 for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
1693 }
1694 comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
1695 UNALLOCA(tmp);
1696#endif
1697
1698#ifdef TIMING
1699 dTimerNow ("tidy up");
1700#endif
1701
1702 // zero all force accumulators
1703 for (i=0; i<nb; i++) {
1704 body[i]->facc[0] = 0;
1705 body[i]->facc[1] = 0;
1706 body[i]->facc[2] = 0;
1707 body[i]->facc[3] = 0;
1708 body[i]->tacc[0] = 0;
1709 body[i]->tacc[1] = 0;
1710 body[i]->tacc[2] = 0;
1711 body[i]->tacc[3] = 0;
1712 }
1713
1714#ifdef TIMING
1715 dTimerEnd();
1716 if (m > 0) dTimerReport (stdout,1);
1717#endif
1718
1719 UNALLOCA(joint);
1720 UNALLOCA(I);
1721 UNALLOCA(invI);
1722 UNALLOCA(info);
1723 UNALLOCA(ofs);
1724 UNALLOCA(cforce);
1725}
1726
1727//****************************************************************************
1728
1729void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
1730 dxJoint * const *joint, int nj, dReal stepsize)
1731{
1732
1733#ifdef dUSE_MALLOC_FOR_ALLOCA
1734 dMemoryFlag = d_MEMORY_OK;
1735#endif
1736
1737#ifndef COMPARE_METHODS
1738 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1739
1740#ifdef dUSE_MALLOC_FOR_ALLOCA
1741 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1742 REPORT_OUT_OF_MEMORY;
1743 return;
1744 }
1745#endif
1746
1747#endif
1748
1749#ifdef COMPARE_METHODS
1750 int i;
1751
1752 // save body state
1753 ALLOCA(dxBody,state,nb*sizeof(dxBody));
1754#ifdef dUSE_MALLOC_FOR_ALLOCA
1755 if (state == NULL) {
1756 dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
1757 REPORT_OUT_OF_MEMORY;
1758 return;
1759 }
1760#endif
1761 for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
1762
1763 // take slow step
1764 comparator.reset();
1765 dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
1766 comparator.end();
1767#ifdef dUSE_MALLOC_FOR_ALLOCA
1768 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1769 UNALLOCA(state);
1770 REPORT_OUT_OF_MEMORY;
1771 return;
1772 }
1773#endif
1774
1775 // restore state
1776 for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
1777
1778 // take fast step
1779 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1780 comparator.end();
1781#ifdef dUSE_MALLOC_FOR_ALLOCA
1782 if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
1783 UNALLOCA(state);
1784 REPORT_OUT_OF_MEMORY;
1785 return;
1786 }
1787#endif
1788
1789 //comparator.dump();
1790 //_exit (1);
1791 UNALLOCA(state);
1792#endif
1793}
1794
1795
diff --git a/libraries/ode-0.9/ode/src/step.h b/libraries/ode-0.9/ode/src/step.h
new file mode 100644
index 0000000..4947253
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/step.h
@@ -0,0 +1,36 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_STEP_H_
24#define _ODE_STEP_H_
25
26#include <ode/common.h>
27
28
29void dInternalStepIsland (dxWorld *world,
30 dxBody * const *body, int nb,
31 dxJoint * const *joint, int nj,
32 dReal stepsize);
33
34
35
36#endif
diff --git a/libraries/ode-0.9/ode/src/stepfast.cpp b/libraries/ode-0.9/ode/src/stepfast.cpp
new file mode 100755
index 0000000..35c45db
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/stepfast.cpp
@@ -0,0 +1,1139 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * Fast iterative solver, David Whittaker. Email: david@csworkbench.com *
7 * *
8 * This library is free software; you can redistribute it and/or *
9 * modify it under the terms of EITHER: *
10 * (1) The GNU Lesser General Public License as published by the Free *
11 * Software Foundation; either version 2.1 of the License, or (at *
12 * your option) any later version. The text of the GNU Lesser *
13 * General Public License is included with this library in the *
14 * file LICENSE.TXT. *
15 * (2) The BSD-style license that is included with this library in *
16 * the file LICENSE-BSD.TXT. *
17 * *
18 * This library is distributed in the hope that it will be useful, *
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
21 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
22 * *
23 *************************************************************************/
24
25// This is the StepFast code by David Whittaker. This code is faster, but
26// sometimes less stable than, the original "big matrix" code.
27// Refer to the user's manual for more information.
28// Note that this source file duplicates a lot of stuff from step.cpp,
29// eventually we should move the common code to a third file.
30
31#include "objects.h"
32#include "joint.h"
33#include <ode/config.h>
34#include <ode/objects.h>
35#include <ode/odemath.h>
36#include <ode/rotation.h>
37#include <ode/timer.h>
38#include <ode/error.h>
39#include <ode/matrix.h>
40#include <ode/misc.h>
41#include "lcp.h"
42#include "step.h"
43#include "util.h"
44
45
46// misc defines
47
48#define ALLOCA dALLOCA16
49
50#define RANDOM_JOINT_ORDER
51//#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate)
52#define SLOW_LCP //use the old LCP solver
53//#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work
54//#define TIMING
55
56
57static int autoEnableDepth = 2;
58
59void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth)
60{
61 if (autodepth > 0)
62 autoEnableDepth = autodepth;
63 else
64 autoEnableDepth = 0;
65}
66
67int dWorldGetAutoEnableDepthSF1 (dxWorld *world)
68{
69 return autoEnableDepth;
70}
71
72//little bit of math.... the _sym_ functions assume the return matrix will be symmetric
73static void
74Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
75{
76 int i, j;
77 dReal sum, *aa, *ad, *bb, *cc;
78 dIASSERT (p > 0 && A && B && C);
79 bb = B;
80 for (i = 0; i < p; i++)
81 {
82 //aa is going accross the matrix, ad down
83 aa = ad = A;
84 cc = C;
85 for (j = i; j < p; j++)
86 {
87 sum = bb[0] * cc[0];
88 sum += bb[1] * cc[1];
89 sum += bb[2] * cc[2];
90 sum += bb[4] * cc[4];
91 sum += bb[5] * cc[5];
92 sum += bb[6] * cc[6];
93 *(aa++) = *ad = sum;
94 ad += Askip;
95 cc += 8;
96 }
97 bb += 8;
98 A += Askip + 1;
99 C += 8;
100 }
101}
102
103static void
104MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
105{
106 int i, j;
107 dReal sum, *aa, *ad, *bb, *cc;
108 dIASSERT (p > 0 && A && B && C);
109 bb = B;
110 for (i = 0; i < p; i++)
111 {
112 //aa is going accross the matrix, ad down
113 aa = ad = A;
114 cc = C;
115 for (j = i; j < p; j++)
116 {
117 sum = bb[0] * cc[0];
118 sum += bb[1] * cc[1];
119 sum += bb[2] * cc[2];
120 sum += bb[4] * cc[4];
121 sum += bb[5] * cc[5];
122 sum += bb[6] * cc[6];
123 *(aa++) += sum;
124 *ad += sum;
125 ad += Askip;
126 cc += 8;
127 }
128 bb += 8;
129 A += Askip + 1;
130 C += 8;
131 }
132}
133
134
135// this assumes the 4th and 8th rows of B are zero.
136
137static void
138Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p)
139{
140 int i;
141 dIASSERT (p > 0 && A && B && C);
142 dReal sum;
143 for (i = p; i; i--)
144 {
145 sum = B[0] * C[0];
146 sum += B[1] * C[1];
147 sum += B[2] * C[2];
148 sum += B[4] * C[4];
149 sum += B[5] * C[5];
150 sum += B[6] * C[6];
151 *(A++) = sum;
152 B += 8;
153 }
154}
155
156
157// this assumes the 4th and 8th rows of B are zero.
158
159static void
160MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p)
161{
162 int i;
163 dIASSERT (p > 0 && A && B && C);
164 dReal sum;
165 for (i = p; i; i--)
166 {
167 sum = B[0] * C[0];
168 sum += B[1] * C[1];
169 sum += B[2] * C[2];
170 sum += B[4] * C[4];
171 sum += B[5] * C[5];
172 sum += B[6] * C[6];
173 *(A++) += sum;
174 B += 8;
175 }
176}
177
178
179// this assumes the 4th and 8th rows of B are zero.
180
181static void
182Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q)
183{
184 int k;
185 dReal sum;
186 dIASSERT (q > 0 && A && B && C);
187 sum = 0;
188 for (k = 0; k < q; k++)
189 sum += B[k * 8] * C[k];
190 A[0] = sum;
191 sum = 0;
192 for (k = 0; k < q; k++)
193 sum += B[1 + k * 8] * C[k];
194 A[1] = sum;
195 sum = 0;
196 for (k = 0; k < q; k++)
197 sum += B[2 + k * 8] * C[k];
198 A[2] = sum;
199 sum = 0;
200 for (k = 0; k < q; k++)
201 sum += B[4 + k * 8] * C[k];
202 A[4] = sum;
203 sum = 0;
204 for (k = 0; k < q; k++)
205 sum += B[5 + k * 8] * C[k];
206 A[5] = sum;
207 sum = 0;
208 for (k = 0; k < q; k++)
209 sum += B[6 + k * 8] * C[k];
210 A[6] = sum;
211}
212
213//****************************************************************************
214// body rotation
215
216// return sin(x)/x. this has a singularity at 0 so special handling is needed
217// for small arguments.
218
219static inline dReal
220sinc (dReal x)
221{
222 // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
223 // is actually accurate to one LS bit within this range if double precision
224 // is being used - so don't worry!
225 if (dFabs (x) < 1.0e-4)
226 return REAL (1.0) - x * x * REAL (0.166666666666666666667);
227 else
228 return dSin (x) / x;
229}
230
231
232// given a body b, apply its linear and angular rotation over the time
233// interval h, thereby adjusting its position and orientation.
234
235static inline void
236moveAndRotateBody (dxBody * b, dReal h)
237{
238 int j;
239
240 // handle linear velocity
241 for (j = 0; j < 3; j++)
242 b->posr.pos[j] += h * b->lvel[j];
243
244 if (b->flags & dxBodyFlagFiniteRotation)
245 {
246 dVector3 irv; // infitesimal rotation vector
247 dQuaternion q; // quaternion for finite rotation
248
249 if (b->flags & dxBodyFlagFiniteRotationAxis)
250 {
251 // split the angular velocity vector into a component along the finite
252 // rotation axis, and a component orthogonal to it.
253 dVector3 frv, irv; // finite rotation vector
254 dReal k = dDOT (b->finite_rot_axis, b->avel);
255 frv[0] = b->finite_rot_axis[0] * k;
256 frv[1] = b->finite_rot_axis[1] * k;
257 frv[2] = b->finite_rot_axis[2] * k;
258 irv[0] = b->avel[0] - frv[0];
259 irv[1] = b->avel[1] - frv[1];
260 irv[2] = b->avel[2] - frv[2];
261
262 // make a rotation quaternion q that corresponds to frv * h.
263 // compare this with the full-finite-rotation case below.
264 h *= REAL (0.5);
265 dReal theta = k * h;
266 q[0] = dCos (theta);
267 dReal s = sinc (theta) * h;
268 q[1] = frv[0] * s;
269 q[2] = frv[1] * s;
270 q[3] = frv[2] * s;
271 }
272 else
273 {
274 // make a rotation quaternion q that corresponds to w * h
275 dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]);
276 h *= REAL (0.5);
277 dReal theta = wlen * h;
278 q[0] = dCos (theta);
279 dReal s = sinc (theta) * h;
280 q[1] = b->avel[0] * s;
281 q[2] = b->avel[1] * s;
282 q[3] = b->avel[2] * s;
283 }
284
285 // do the finite rotation
286 dQuaternion q2;
287 dQMultiply0 (q2, q, b->q);
288 for (j = 0; j < 4; j++)
289 b->q[j] = q2[j];
290
291 // do the infitesimal rotation if required
292 if (b->flags & dxBodyFlagFiniteRotationAxis)
293 {
294 dReal dq[4];
295 dWtoDQ (irv, b->q, dq);
296 for (j = 0; j < 4; j++)
297 b->q[j] += h * dq[j];
298 }
299 }
300 else
301 {
302 // the normal way - do an infitesimal rotation
303 dReal dq[4];
304 dWtoDQ (b->avel, b->q, dq);
305 for (j = 0; j < 4; j++)
306 b->q[j] += h * dq[j];
307 }
308
309 // normalize the quaternion and convert it to a rotation matrix
310 dNormalize4 (b->q);
311 dQtoR (b->q, b->posr.R);
312
313 // notify all attached geoms that this body has moved
314 for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
315 dGeomMoved (geom);
316}
317
318//****************************************************************************
319//This is an implementation of the iterated/relaxation algorithm.
320//Here is a quick overview of the algorithm per Sergi Valverde's posts to the
321//mailing list:
322//
323// for i=0..N-1 do
324// for c = 0..C-1 do
325// Solve constraint c-th
326// Apply forces to constraint bodies
327// next
328// next
329// Integrate bodies
330
331void
332dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize)
333{
334 int i, j, k;
335# ifdef TIMING
336 dTimerNow ("constraint preprocessing");
337# endif
338
339 dReal stepsize1 = dRecip (stepsize);
340
341 int m = info.m;
342 // nothing to do if no constraints.
343 if (m <= 0)
344 return;
345
346 int nub = 0;
347 if (info.nub == info.m)
348 nub = m;
349
350 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
351 // format as J so we just go through the constraints in J multiplying by
352 // the appropriate scalars and matrices.
353# ifdef TIMING
354 dTimerNow ("compute A");
355# endif
356 dReal JinvM[2 * 6 * 8];
357 //dSetZero (JinvM, 2 * m * 8);
358
359 dReal *Jsrc = Jinfo.J1l;
360 dReal *Jdst = JinvM;
361 if (body[0])
362 {
363 for (j = m - 1; j >= 0; j--)
364 {
365 for (k = 0; k < 3; k++)
366 Jdst[k] = Jsrc[k] * body[0]->invMass;
367 dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]);
368 Jsrc += 8;
369 Jdst += 8;
370 }
371 }
372 if (body[1])
373 {
374 Jsrc = Jinfo.J2l;
375 Jdst = JinvM + 8 * m;
376 for (j = m - 1; j >= 0; j--)
377 {
378 for (k = 0; k < 3; k++)
379 Jdst[k] = Jsrc[k] * body[1]->invMass;
380 dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]);
381 Jsrc += 8;
382 Jdst += 8;
383 }
384 }
385
386
387 // now compute A = JinvM * J'.
388 int mskip = dPAD (m);
389 dReal A[6 * 8];
390 //dSetZero (A, 6 * 8);
391
392 if (body[0]) {
393 Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip);
394 if (body[1])
395 MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l,
396 m, mskip);
397 } else {
398 if (body[1])
399 Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l,
400 m, mskip);
401 }
402
403 // add cfm to the diagonal of A
404 for (i = 0; i < m; i++)
405 A[i * mskip + i] += Jinfo.cfm[i] * stepsize1;
406
407 // compute the right hand side `rhs'
408# ifdef TIMING
409 dTimerNow ("compute rhs");
410# endif
411 dReal tmp1[16];
412 //dSetZero (tmp1, 16);
413 // put v/h + invM*fe into tmp1
414 for (i = 0; i < 2; i++)
415 {
416 if (!body[i])
417 continue;
418 for (j = 0; j < 3; j++)
419 tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1;
420 dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc);
421 for (j = 0; j < 3; j++)
422 tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1;
423 }
424 // put J*tmp1 into rhs
425 dReal rhs[6];
426 //dSetZero (rhs, 6);
427
428 if (body[0]) {
429 Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m);
430 if (body[1])
431 MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
432 } else {
433 if (body[1])
434 Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
435 }
436
437 // complete rhs
438 for (i = 0; i < m; i++)
439 rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i];
440
441#ifdef SLOW_LCP
442 // solve the LCP problem and get lambda.
443 // this will destroy A but that's okay
444# ifdef TIMING
445 dTimerNow ("solving LCP problem");
446# endif
447 dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal));
448 dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal));
449 dReal lo[6], hi[6];
450 memcpy (lo, Jinfo.lo, m * sizeof (dReal));
451 memcpy (hi, Jinfo.hi, m * sizeof (dReal));
452 dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex);
453#endif
454
455 // LCP Solver replacement:
456 // This algorithm goes like this:
457 // Do a straightforward LDLT factorization of the matrix A, solving for
458 // A*x = rhs
459 // For each x[i] that is outside of the bounds of lo[i] and hi[i],
460 // clamp x[i] into that range.
461 // Substitute into A the now known x's
462 // subtract the residual away from the rhs.
463 // Remove row and column i from L, updating the factorization
464 // place the known x's at the end of the array, keeping up with location in p
465 // Repeat until all constraints have been clamped or all are within bounds
466 //
467 // This is probably only faster in the single joint case where only one repeat is
468 // the norm.
469
470#ifdef FAST_FACTOR
471 // factorize A (L*D*L'=A)
472# ifdef TIMING
473 dTimerNow ("factorize A");
474# endif
475 dReal d[6];
476 dReal L[6 * 8];
477 memcpy (L, A, m * mskip * sizeof (dReal));
478 dFactorLDLT (L, d, m, mskip);
479
480 // compute lambda
481# ifdef TIMING
482 dTimerNow ("compute lambda");
483# endif
484
485 int left = m; //constraints left to solve.
486 int remove[6];
487 dReal lambda[6];
488 dReal x[6];
489 int p[6];
490 for (i = 0; i < 6; i++)
491 p[i] = i;
492 while (true)
493 {
494 memcpy (x, rhs, left * sizeof (dReal));
495 dSolveLDLT (L, d, x, left, mskip);
496
497 int fixed = 0;
498 for (i = 0; i < left; i++)
499 {
500 j = p[i];
501 remove[i] = false;
502 // This isn't the exact same use of findex as dSolveLCP.... since x[findex]
503 // may change after I've already clamped x[i], but it should be close
504 if (Jinfo.findex[j] > -1)
505 {
506 dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]);
507 if (x[i] > f)
508 x[i] = f;
509 else if (x[i] < -f)
510 x[i] = -f;
511 else
512 continue;
513 }
514 else
515 {
516 if (x[i] > Jinfo.hi[j])
517 x[i] = Jinfo.hi[j];
518 else if (x[i] < Jinfo.lo[j])
519 x[i] = Jinfo.lo[j];
520 else
521 continue;
522 }
523 remove[i] = true;
524 fixed++;
525 }
526 if (fixed == 0 || fixed == left) //no change or all constraints solved
527 break;
528
529 for (i = 0; i < left; i++) //sub in to right hand side.
530 if (remove[i])
531 for (j = 0; j < left; j++)
532 if (!remove[j])
533 rhs[j] -= A[j * mskip + i] * x[i];
534
535 for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables
536 {
537 if (remove[r])
538 {
539 //dRemoveLDLT adapted for use without row pointers.
540 if (r == left - 1)
541 {
542 left--;
543 continue; // deleting last row/col is easy
544 }
545 else if (r == 0)
546 {
547 dReal a[6];
548 for (i = 0; i < left; i++)
549 a[i] = -A[i * mskip];
550 a[0] += REAL (1.0);
551 dLDLTAddTL (L, d, a, left, mskip);
552 }
553 else
554 {
555 dReal t[6];
556 dReal a[6];
557 for (i = 0; i < r; i++)
558 t[i] = L[r * mskip + i] / d[i];
559 for (i = 0; i < left - r; i++)
560 a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r];
561 a[0] += REAL (1.0);
562 dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip);
563 }
564
565 dRemoveRowCol (L, left, mskip, r);
566 //end dRemoveLDLT
567
568 left--;
569 if (r < (left - 1))
570 {
571 dReal tx = x[r];
572 memmove (d + r, d + r + 1, (left - r) * sizeof (dReal));
573 memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal));
574 //x will get written over by rhs anyway, no need to move it around
575 //just store the fixed value we just discovered in it.
576 x[left] = tx;
577 for (i = 0; i < m; i++)
578 if (p[i] > r && p[i] <= left)
579 p[i]--;
580 p[r] = left;
581 }
582 }
583 }
584 }
585
586 for (i = 0; i < m; i++)
587 lambda[i] = x[p[i]];
588# endif
589 // compute the constraint force `cforce'
590# ifdef TIMING
591 dTimerNow ("compute constraint force");
592#endif
593
594 // compute cforce = J'*lambda
595 dJointFeedback *fb = joint->feedback;
596 dReal cforce[16];
597 //dSetZero (cforce, 16);
598
599 if (fb)
600 {
601 // the user has requested feedback on the amount of force that this
602 // joint is applying to the bodies. we use a slightly slower
603 // computation that splits out the force components and puts them
604 // in the feedback structure.
605 dReal data1[8], data2[8];
606 if (body[0])
607 {
608 Multiply1_8q1 (data1, Jinfo.J1l, lambda, m);
609 dReal *cf1 = cforce;
610 cf1[0] = (fb->f1[0] = data1[0]);
611 cf1[1] = (fb->f1[1] = data1[1]);
612 cf1[2] = (fb->f1[2] = data1[2]);
613 cf1[4] = (fb->t1[0] = data1[4]);
614 cf1[5] = (fb->t1[1] = data1[5]);
615 cf1[6] = (fb->t1[2] = data1[6]);
616 }
617 if (body[1])
618 {
619 Multiply1_8q1 (data2, Jinfo.J2l, lambda, m);
620 dReal *cf2 = cforce + 8;
621 cf2[0] = (fb->f2[0] = data2[0]);
622 cf2[1] = (fb->f2[1] = data2[1]);
623 cf2[2] = (fb->f2[2] = data2[2]);
624 cf2[4] = (fb->t2[0] = data2[4]);
625 cf2[5] = (fb->t2[1] = data2[5]);
626 cf2[6] = (fb->t2[2] = data2[6]);
627 }
628 }
629 else
630 {
631 // no feedback is required, let's compute cforce the faster way
632 if (body[0])
633 Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m);
634 if (body[1])
635 Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m);
636 }
637
638 for (i = 0; i < 2; i++)
639 {
640 if (!body[i])
641 continue;
642 for (j = 0; j < 3; j++)
643 {
644 body[i]->facc[j] += cforce[i * 8 + j];
645 body[i]->tacc[j] += cforce[i * 8 + 4 + j];
646 }
647 }
648}
649
650void
651dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations)
652{
653# ifdef TIMING
654 dTimerNow ("preprocessing");
655# endif
656 dxBody *bodyPair[2], *body;
657 dReal *GIPair[2], *GinvIPair[2];
658 dxJoint *joint;
659 int iter, b, j, i;
660 dReal ministep = stepsize / maxiterations;
661
662 // make a local copy of the joint array, because we might want to modify it.
663 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
664 // but not the joint array, because the caller might need it unchanged).
665 dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *));
666 memcpy (joints, _joints, nj * sizeof (dxJoint *));
667
668 // get m = total constraint dimension, nub = number of unbounded variables.
669 // create constraint offset array and number-of-rows array for all joints.
670 // the constraints are re-ordered as follows: the purely unbounded
671 // constraints, the mixed unbounded + LCP constraints, and last the purely
672 // LCP constraints. this assists the LCP solver to put all unbounded
673 // variables at the start for a quick factorization.
674 //
675 // joints with m=0 are inactive and are removed from the joints array
676 // entirely, so that the code that follows does not consider them.
677 // also number all active joints in the joint list (set their tag values).
678 // inactive joints receive a tag value of -1.
679
680 int m = 0;
681 dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1));
682 int *ofs = (int *) ALLOCA (nj * sizeof (int));
683 for (i = 0, j = 0; j < nj; j++)
684 { // i=dest, j=src
685 joints[j]->vtable->getInfo1 (joints[j], info + i);
686 dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
687 if (info[i].m > 0)
688 {
689 joints[i] = joints[j];
690 joints[i]->tag = i;
691 i++;
692 }
693 else
694 {
695 joints[j]->tag = -1;
696 }
697 }
698 nj = i;
699
700 // the purely unbounded constraints
701 for (i = 0; i < nj; i++)
702 {
703 ofs[i] = m;
704 m += info[i].m;
705 }
706 dReal *c = NULL;
707 dReal *cfm = NULL;
708 dReal *lo = NULL;
709 dReal *hi = NULL;
710 int *findex = NULL;
711
712 dReal *J = NULL;
713 dxJoint::Info2 * Jinfo = NULL;
714
715 if (m)
716 {
717 // create a constraint equation right hand side vector `c', a constraint
718 // force mixing vector `cfm', and LCP low and high bound vectors, and an
719 // 'findex' vector.
720 c = (dReal *) ALLOCA (m * sizeof (dReal));
721 cfm = (dReal *) ALLOCA (m * sizeof (dReal));
722 lo = (dReal *) ALLOCA (m * sizeof (dReal));
723 hi = (dReal *) ALLOCA (m * sizeof (dReal));
724 findex = (int *) ALLOCA (m * sizeof (int));
725 dSetZero (c, m);
726 dSetValue (cfm, m, world->global_cfm);
727 dSetValue (lo, m, -dInfinity);
728 dSetValue (hi, m, dInfinity);
729 for (i = 0; i < m; i++)
730 findex[i] = -1;
731
732 // get jacobian data from constraints. a (2*m)x8 matrix will be created
733 // to store the two jacobian blocks from each constraint. it has this
734 // format:
735 //
736 // l l l 0 a a a 0 \ .
737 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
738 // l l l 0 a a a 0 /
739 // l l l 0 a a a 0 \ .
740 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
741 // l l l 0 a a a 0 /
742 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
743 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
744 // etc...
745 //
746 // (lll) = linear jacobian data
747 // (aaa) = angular jacobian data
748 //
749# ifdef TIMING
750 dTimerNow ("create J");
751# endif
752 J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal));
753 dSetZero (J, 2 * m * 8);
754 Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2));
755 for (i = 0; i < nj; i++)
756 {
757 Jinfo[i].rowskip = 8;
758 Jinfo[i].fps = dRecip (stepsize);
759 Jinfo[i].erp = world->global_erp;
760 Jinfo[i].J1l = J + 2 * 8 * ofs[i];
761 Jinfo[i].J1a = Jinfo[i].J1l + 4;
762 Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m;
763 Jinfo[i].J2a = Jinfo[i].J2l + 4;
764 Jinfo[i].c = c + ofs[i];
765 Jinfo[i].cfm = cfm + ofs[i];
766 Jinfo[i].lo = lo + ofs[i];
767 Jinfo[i].hi = hi + ofs[i];
768 Jinfo[i].findex = findex + ofs[i];
769 //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i);
770 }
771
772 }
773
774 dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
775 dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
776 dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
777 dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
778 for (b = 0; b < nb; b++)
779 {
780 for (i = 0; i < 4; i++)
781 {
782 saveFacc[b * 4 + i] = bodies[b]->facc[i];
783 saveTacc[b * 4 + i] = bodies[b]->tacc[i];
784 }
785 bodies[b]->tag = b;
786 }
787
788 for (iter = 0; iter < maxiterations; iter++)
789 {
790# ifdef TIMING
791 dTimerNow ("applying inertia and gravity");
792# endif
793 dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
794
795 for (b = 0; b < nb; b++)
796 {
797 body = bodies[b];
798
799 // for all bodies, compute the inertia tensor and its inverse in the global
800 // frame, and compute the rotational force and add it to the torque
801 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
802 // @@@ check computation of rotational force.
803
804 // compute inertia tensor in global frame
805 dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R);
806 dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp);
807 // compute inverse inertia tensor in global frame
808 dMULTIPLY2_333 (tmp, body->invI, body->posr.R);
809 dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp);
810
811 for (i = 0; i < 4; i++)
812 body->tacc[i] = saveTacc[b * 4 + i];
813#ifdef dGYROSCOPIC
814 // compute rotational force
815 dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel);
816 dCROSS (body->tacc, -=, body->avel, tmp);
817#endif
818
819 // add the gravity force to all bodies
820 if ((body->flags & dxBodyNoGravity) == 0)
821 {
822 body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0];
823 body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1];
824 body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2];
825 body->facc[3] = 0;
826 } else {
827 body->facc[0] = saveFacc[b * 4 + 0];
828 body->facc[1] = saveFacc[b * 4 + 1];
829 body->facc[2] = saveFacc[b * 4 + 2];
830 body->facc[3] = 0;
831 }
832
833 }
834
835#ifdef RANDOM_JOINT_ORDER
836#ifdef TIMING
837 dTimerNow ("randomizing joint order");
838#endif
839 //randomize the order of the joints by looping through the array
840 //and swapping the current joint pointer with a random one before it.
841 for (j = 0; j < nj; j++)
842 {
843 joint = joints[j];
844 dxJoint::Info1 i1 = info[j];
845 dxJoint::Info2 i2 = Jinfo[j];
846 const int r = dRandInt(j+1);
847 dIASSERT (r < nj);
848 joints[j] = joints[r];
849 info[j] = info[r];
850 Jinfo[j] = Jinfo[r];
851 joints[r] = joint;
852 info[r] = i1;
853 Jinfo[r] = i2;
854 }
855#endif
856
857 //now iterate through the random ordered joint array we created.
858 for (j = 0; j < nj; j++)
859 {
860#ifdef TIMING
861 dTimerNow ("setting up joint");
862#endif
863 joint = joints[j];
864 bodyPair[0] = joint->node[0].body;
865 bodyPair[1] = joint->node[1].body;
866
867 if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled))
868 bodyPair[0] = 0;
869 if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled))
870 bodyPair[1] = 0;
871
872 //if this joint is not connected to any enabled bodies, skip it.
873 if (!bodyPair[0] && !bodyPair[1])
874 continue;
875
876 if (bodyPair[0])
877 {
878 GIPair[0] = globalI + bodyPair[0]->tag * 12;
879 GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12;
880 }
881 if (bodyPair[1])
882 {
883 GIPair[1] = globalI + bodyPair[1]->tag * 12;
884 GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12;
885 }
886
887 joints[j]->vtable->getInfo2 (joints[j], Jinfo + j);
888
889 //dInternalStepIslandFast is an exact copy of the old routine with one
890 //modification: the calculated forces are added back to the facc and tacc
891 //vectors instead of applying them to the bodies and moving them.
892 if (info[j].m > 0)
893 {
894 dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep);
895 }
896 }
897 // }
898# ifdef TIMING
899 dTimerNow ("moving bodies");
900# endif
901 //Now we can simulate all the free floating bodies, and move them.
902 for (b = 0; b < nb; b++)
903 {
904 body = bodies[b];
905
906 for (i = 0; i < 4; i++)
907 {
908 body->facc[i] *= ministep;
909 body->tacc[i] *= ministep;
910 }
911
912 //apply torque
913 dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc);
914
915 //apply force
916 for (i = 0; i < 3; i++)
917 body->lvel[i] += body->invMass * body->facc[i];
918
919 //move It!
920 moveAndRotateBody (body, ministep);
921 }
922 }
923 for (b = 0; b < nb; b++)
924 for (j = 0; j < 4; j++)
925 bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
926}
927
928
929#ifdef NO_ISLANDS
930
931// Since the iterative algorithm doesn't care about islands of bodies, this is a
932// faster algorithm that just sends it all the joints and bodies in one array.
933// It's downfall is it's inability to handle disabled bodies as well as the old one.
934static void
935processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
936{
937 // nothing to do if no bodies
938 if (world->nb <= 0)
939 return;
940
941 dInternalHandleAutoDisabling (world,stepsize);
942
943# ifdef TIMING
944 dTimerStart ("creating joint and body arrays");
945# endif
946 dxBody **bodies, *body;
947 dxJoint **joints, *joint;
948 joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *));
949 bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *));
950
951 int nj = 0;
952 for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next)
953 joints[nj++] = joint;
954
955 int nb = 0;
956 for (body = world->firstbody; body; body = (dxBody *) body->next)
957 bodies[nb++] = body;
958
959 dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations);
960# ifdef TIMING
961 dTimerEnd ();
962 dTimerReport (stdout, 1);
963# endif
964}
965
966#else
967
968//****************************************************************************
969// island processing
970
971// this groups all joints and bodies in a world into islands. all objects
972// in an island are reachable by going through connected bodies and joints.
973// each island can be simulated separately.
974// note that joints that are not attached to anything will not be included
975// in any island, an so they do not affect the simulation.
976//
977// this function starts new island from unvisited bodies. however, it will
978// never start a new islands from a disabled body. thus islands of disabled
979// bodies will not be included in the simulation. disabled bodies are
980// re-enabled if they are found to be part of an active island.
981
982static void
983processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
984{
985#ifdef TIMING
986 dTimerStart ("Island Setup");
987#endif
988 dxBody *b, *bb, **body;
989 dxJoint *j, **joint;
990
991 // nothing to do if no bodies
992 if (world->nb <= 0)
993 return;
994
995 dInternalHandleAutoDisabling (world,stepsize);
996
997 // make arrays for body and joint lists (for a single island) to go into
998 body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *));
999 joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *));
1000 int bcount = 0; // number of bodies in `body'
1001 int jcount = 0; // number of joints in `joint'
1002 int tbcount = 0;
1003 int tjcount = 0;
1004
1005 // set all body/joint tags to 0
1006 for (b = world->firstbody; b; b = (dxBody *) b->next)
1007 b->tag = 0;
1008 for (j = world->firstjoint; j; j = (dxJoint *) j->next)
1009 j->tag = 0;
1010
1011 // allocate a stack of unvisited bodies in the island. the maximum size of
1012 // the stack can be the lesser of the number of bodies or joints, because
1013 // new bodies are only ever added to the stack by going through untagged
1014 // joints. all the bodies in the stack must be tagged!
1015 int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
1016 dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *));
1017 int *autostack = (int *) ALLOCA (stackalloc * sizeof (int));
1018
1019 for (bb = world->firstbody; bb; bb = (dxBody *) bb->next)
1020 {
1021#ifdef TIMING
1022 dTimerNow ("Island Processing");
1023#endif
1024 // get bb = the next enabled, untagged body, and tag it
1025 if (bb->tag || (bb->flags & dxBodyDisabled))
1026 continue;
1027 bb->tag = 1;
1028
1029 // tag all bodies and joints starting from bb.
1030 int stacksize = 0;
1031 int autoDepth = autoEnableDepth;
1032 b = bb;
1033 body[0] = bb;
1034 bcount = 1;
1035 jcount = 0;
1036 goto quickstart;
1037 while (stacksize > 0)
1038 {
1039 b = stack[--stacksize]; // pop body off stack
1040 autoDepth = autostack[stacksize];
1041 body[bcount++] = b; // put body on body list
1042 quickstart:
1043
1044 // traverse and tag all body's joints, add untagged connected bodies
1045 // to stack
1046 for (dxJointNode * n = b->firstjoint; n; n = n->next)
1047 {
1048 if (!n->joint->tag)
1049 {
1050 int thisDepth = autoEnableDepth;
1051 n->joint->tag = 1;
1052 joint[jcount++] = n->joint;
1053 if (n->body && !n->body->tag)
1054 {
1055 if (n->body->flags & dxBodyDisabled)
1056 thisDepth = autoDepth - 1;
1057 if (thisDepth < 0)
1058 continue;
1059 n->body->flags &= ~dxBodyDisabled;
1060 n->body->tag = 1;
1061 autostack[stacksize] = thisDepth;
1062 stack[stacksize++] = n->body;
1063 }
1064 }
1065 }
1066 dIASSERT (stacksize <= world->nb);
1067 dIASSERT (stacksize <= world->nj);
1068 }
1069
1070 // now do something with body and joint lists
1071 dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations);
1072
1073 // what we've just done may have altered the body/joint tag values.
1074 // we must make sure that these tags are nonzero.
1075 // also make sure all bodies are in the enabled state.
1076 int i;
1077 for (i = 0; i < bcount; i++)
1078 {
1079 body[i]->tag = 1;
1080 body[i]->flags &= ~dxBodyDisabled;
1081 }
1082 for (i = 0; i < jcount; i++)
1083 joint[i]->tag = 1;
1084
1085 tbcount += bcount;
1086 tjcount += jcount;
1087 }
1088
1089#ifdef TIMING
1090 dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount);
1091#endif
1092
1093 // if debugging, check that all objects (except for disabled bodies,
1094 // unconnected joints, and joints that are connected to disabled bodies)
1095 // were tagged.
1096# ifndef dNODEBUG
1097 for (b = world->firstbody; b; b = (dxBody *) b->next)
1098 {
1099 if (b->flags & dxBodyDisabled)
1100 {
1101 if (b->tag)
1102 dDebug (0, "disabled body tagged");
1103 }
1104 else
1105 {
1106 if (!b->tag)
1107 dDebug (0, "enabled body not tagged");
1108 }
1109 }
1110 for (j = world->firstjoint; j; j = (dxJoint *) j->next)
1111 {
1112 if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0))
1113 {
1114 if (!j->tag)
1115 dDebug (0, "attached enabled joint not tagged");
1116 }
1117 else
1118 {
1119 if (j->tag)
1120 dDebug (0, "unattached or disabled joint tagged");
1121 }
1122 }
1123# endif
1124
1125# ifdef TIMING
1126 dTimerEnd ();
1127 dTimerReport (stdout, 1);
1128# endif
1129}
1130
1131#endif
1132
1133
1134void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations)
1135{
1136 dUASSERT (w, "bad world argument");
1137 dUASSERT (stepsize > 0, "stepsize must be > 0");
1138 processIslandsFast (w, stepsize, maxiterations);
1139}
diff --git a/libraries/ode-0.9/ode/src/testing.cpp b/libraries/ode-0.9/ode/src/testing.cpp
new file mode 100644
index 0000000..a22f865
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/testing.cpp
@@ -0,0 +1,243 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <ode/config.h>
24#include <ode/misc.h>
25#include <ode/memory.h>
26#include "testing.h"
27
28#ifdef dDOUBLE
29static const dReal tol = 1.0e-9;
30#else
31static const dReal tol = 1.0e-5f;
32#endif
33
34
35// matrix header on the stack
36
37struct dMatrixComparison::dMatInfo {
38 int n,m; // size of matrix
39 char name[128]; // name of the matrix
40 dReal *data; // matrix data
41 int size; // size of `data'
42};
43
44
45
46dMatrixComparison::dMatrixComparison()
47{
48 afterfirst = 0;
49 index = 0;
50}
51
52
53dMatrixComparison::~dMatrixComparison()
54{
55 reset();
56}
57
58
59dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
60 char *name, ...)
61{
62 if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
63 int num = n*dPAD(m);
64
65 if (afterfirst==0) {
66 dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
67 mi->n = n;
68 mi->m = m;
69 mi->size = num * sizeof(dReal);
70 mi->data = (dReal*) dAlloc (mi->size);
71 memcpy (mi->data,A,mi->size);
72
73 va_list ap;
74 va_start (ap,name);
75 vsprintf (mi->name,name,ap);
76 if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
77
78 mat.push (mi);
79 return 0;
80 }
81 else {
82 if (lower_tri && n != m)
83 dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
84 if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
85 dMatInfo *mp = mat[index];
86 index++;
87
88 dMatInfo mi;
89 va_list ap;
90 va_start (ap,name);
91 vsprintf (mi.name,name,ap);
92 if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
93
94 if (strcmp(mp->name,mi.name) != 0)
95 dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
96 mp->name,mi.name);
97 if (mp->n != n || mp->m != m)
98 dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
99 mp->n,mp->m,n,m);
100
101 dReal maxdiff;
102 if (lower_tri) {
103 maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
104 }
105 else {
106 maxdiff = dMaxDifference (A,mp->data,n,m);
107 }
108 if (maxdiff > tol)
109 dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
110 "error=%.4e)",n,m,mi.name,maxdiff);
111 return maxdiff;
112 }
113}
114
115
116void dMatrixComparison::end()
117{
118 if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
119 afterfirst = 1;
120 index = 0;
121}
122
123
124void dMatrixComparison::reset()
125{
126 for (int i=0; i<mat.size(); i++) {
127 dFree (mat[i]->data,mat[i]->size);
128 dFree (mat[i],sizeof(dMatInfo));
129 }
130 mat.setSize (0);
131 afterfirst = 0;
132 index = 0;
133}
134
135
136void dMatrixComparison::dump()
137{
138 for (int i=0; i<mat.size(); i++)
139 printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
140}
141
142//****************************************************************************
143// unit test
144
145#include <setjmp.h>
146
147static jmp_buf jump_buffer;
148
149static void myDebug (int num, const char *msg, va_list ap)
150{
151 // printf ("(Error %d: ",num);
152 // vprintf (msg,ap);
153 // printf (")\n");
154 longjmp (jump_buffer,1);
155}
156
157
158extern "C" ODE_API void dTestMatrixComparison()
159{
160 volatile int i;
161 printf ("dTestMatrixComparison()\n");
162 dMessageFunction *orig_debug = dGetDebugHandler();
163
164 dMatrixComparison mc;
165 dReal A[50*50];
166
167 // make first sequence
168 unsigned long seed = dRandGetSeed();
169 for (i=1; i<49; i++) {
170 dMakeRandomMatrix (A,i,i+1,1.0);
171 mc.nextMatrix (A,i,i+1,0,"A%d",i);
172 }
173 mc.end();
174
175 //mc.dump();
176
177 // test identical sequence
178 dSetDebugHandler (&myDebug);
179 dRandSetSeed (seed);
180 if (setjmp (jump_buffer)) {
181 printf ("\tFAILED (1)\n");
182 }
183 else {
184 for (i=1; i<49; i++) {
185 dMakeRandomMatrix (A,i,i+1,1.0);
186 mc.nextMatrix (A,i,i+1,0,"A%d",i);
187 }
188 mc.end();
189 printf ("\tpassed (1)\n");
190 }
191 dSetDebugHandler (orig_debug);
192
193 // test broken sequences (with matrix error)
194 dRandSetSeed (seed);
195 volatile int passcount = 0;
196 for (i=1; i<49; i++) {
197 if (setjmp (jump_buffer)) {
198 passcount++;
199 }
200 else {
201 dSetDebugHandler (&myDebug);
202 dMakeRandomMatrix (A,i,i+1,1.0);
203 A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
204 mc.nextMatrix (A,i,i+1,0,"A%d",i);
205 dSetDebugHandler (orig_debug);
206 }
207 }
208 mc.end();
209 printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
210
211 // test broken sequences (with name error)
212 dRandSetSeed (seed);
213 passcount = 0;
214 for (i=1; i<49; i++) {
215 if (setjmp (jump_buffer)) {
216 passcount++;
217 }
218 else {
219 dSetDebugHandler (&myDebug);
220 dMakeRandomMatrix (A,i,i+1,1.0);
221 mc.nextMatrix (A,i,i+1,0,"B%d",i);
222 dSetDebugHandler (orig_debug);
223 }
224 }
225 mc.end();
226 printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
227
228 // test identical sequence again
229 dSetDebugHandler (&myDebug);
230 dRandSetSeed (seed);
231 if (setjmp (jump_buffer)) {
232 printf ("\tFAILED (4)\n");
233 }
234 else {
235 for (i=1; i<49; i++) {
236 dMakeRandomMatrix (A,i,i+1,1.0);
237 mc.nextMatrix (A,i,i+1,0,"A%d",i);
238 }
239 mc.end();
240 printf ("\tpassed (4)\n");
241 }
242 dSetDebugHandler (orig_debug);
243}
diff --git a/libraries/ode-0.9/ode/src/testing.h b/libraries/ode-0.9/ode/src/testing.h
new file mode 100644
index 0000000..4d19ff3
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/testing.h
@@ -0,0 +1,65 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/* stuff used for testing */
24
25#ifndef _ODE_TESTING_H_
26#define _ODE_TESTING_H_
27
28#include <ode/common.h>
29#include "array.h"
30
31
32// compare a sequence of named matrices/vectors, i.e. to make sure that two
33// different pieces of code are giving the same results.
34
35class dMatrixComparison {
36 struct dMatInfo;
37 dArray<dMatInfo*> mat;
38 int afterfirst,index;
39
40public:
41 dMatrixComparison();
42 ~dMatrixComparison();
43
44 dReal nextMatrix (dReal *A, int n, int m, int lower_tri, char *name, ...);
45 // add a new n*m matrix A to the sequence. the name of the matrix is given
46 // by the printf-style arguments (name,...). if this is the first sequence
47 // then this object will simply record the matrices and return 0.
48 // if this the second or subsequent sequence then this object will compare
49 // the matrices with the first sequence, and report any differences.
50 // the matrix error will be returned. if `lower_tri' is 1 then only the
51 // lower triangle of the matrix (including the diagonal) will be compared
52 // (the matrix must be square).
53
54 void end();
55 // end a sequence.
56
57 void reset();
58 // restarts the object, so the next sequence will be the first sequence.
59
60 void dump();
61 // print out info about all the matrices in the sequence
62};
63
64
65#endif
diff --git a/libraries/ode-0.9/ode/src/timer.cpp b/libraries/ode-0.9/ode/src/timer.cpp
new file mode 100644
index 0000000..f754bf1
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/timer.cpp
@@ -0,0 +1,421 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25TODO
26----
27
28* gettimeofday() and the pentium time stamp counter return the real time,
29 not the process time. fix this somehow!
30
31*/
32
33#include <ode/common.h>
34#include <ode/timer.h>
35
36// misc defines
37#define ALLOCA dALLOCA16
38
39//****************************************************************************
40// implementation for windows based on the multimedia performance counter.
41
42#ifdef WIN32
43
44#include "windows.h"
45
46static inline void getClockCount (unsigned long cc[2])
47{
48 LARGE_INTEGER a;
49 QueryPerformanceCounter (&a);
50 cc[0] = a.LowPart;
51 cc[1] = a.HighPart;
52}
53
54
55static inline void serialize()
56{
57}
58
59
60static inline double loadClockCount (unsigned long cc[2])
61{
62 LARGE_INTEGER a;
63 a.LowPart = cc[0];
64 a.HighPart = cc[1];
65 return double(a.QuadPart);
66}
67
68
69double dTimerResolution()
70{
71 return 1.0/dTimerTicksPerSecond();
72}
73
74
75double dTimerTicksPerSecond()
76{
77 static int query=0;
78 static double hz=0.0;
79 if (!query) {
80 LARGE_INTEGER a;
81 QueryPerformanceFrequency (&a);
82 hz = double(a.QuadPart);
83 query = 1;
84 }
85 return hz;
86}
87
88#endif
89
90//****************************************************************************
91// implementation based on the pentium time stamp counter. the timer functions
92// can be serializing or non-serializing. serializing will ensure that all
93// instructions have executed and data has been written back before the cpu
94// time stamp counter is read. the CPUID instruction is used to serialize.
95
96#if defined(PENTIUM) && !defined(WIN32)
97
98// we need to know the clock rate so that the timing function can report
99// accurate times. this number only needs to be set accurately if we're
100// doing performance tests and care about real-world time numbers - otherwise,
101// just ignore this. i have not worked out how to determine this number
102// automatically yet.
103
104#define PENTIUM_HZ (500e6)
105
106static inline void getClockCount (unsigned long cc[2])
107{
108#ifndef X86_64_SYSTEM
109 asm volatile (
110 "rdtsc\n"
111 "movl %%eax,(%%esi)\n"
112 "movl %%edx,4(%%esi)\n"
113 : : "S" (cc) : "%eax","%edx","cc","memory");
114#else
115 asm volatile (
116 "rdtsc\n"
117 "movl %%eax,(%%rsi)\n"
118 "movl %%edx,4(%%rsi)\n"
119 : : "S" (cc) : "%eax","%edx","cc","memory");
120#endif
121}
122
123
124static inline void serialize()
125{
126#ifndef X86_64_SYSTEM
127 asm volatile (
128 "mov $0,%%eax\n"
129 "push %%ebx\n"
130 "cpuid\n"
131 "pop %%ebx\n"
132 : : : "%eax","%ecx","%edx","cc","memory");
133#else
134 asm volatile (
135 "mov $0,%%rax\n"
136 "push %%rbx\n"
137 "cpuid\n"
138 "pop %%rbx\n"
139 : : : "%rax","%rcx","%rdx","cc","memory");
140#endif
141}
142
143
144static inline double loadClockCount (unsigned long a[2])
145{
146 double ret;
147#ifndef X86_64_SYSTEM
148 asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) :
149 "cc","memory");
150#else
151 asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) :
152 "cc","memory");
153#endif
154 return ret;
155}
156
157
158double dTimerResolution()
159{
160 return 1.0/PENTIUM_HZ;
161}
162
163
164double dTimerTicksPerSecond()
165{
166 return PENTIUM_HZ;
167}
168
169#endif
170
171//****************************************************************************
172// otherwise, do the implementation based on gettimeofday().
173
174#if !defined(PENTIUM) && !defined(WIN32)
175
176#ifndef macintosh
177
178#include <sys/time.h>
179#include <unistd.h>
180
181
182static inline void getClockCount (unsigned long cc[2])
183{
184 struct timeval tv;
185 gettimeofday (&tv,0);
186 cc[0] = tv.tv_usec;
187 cc[1] = tv.tv_sec;
188}
189
190#else // macintosh
191
192#include <MacTypes.h>
193#include <Timer.h>
194
195static inline void getClockCount (unsigned long cc[2])
196{
197 UnsignedWide ms;
198 Microseconds (&ms);
199 cc[1] = ms.lo / 1000000;
200 cc[0] = ms.lo - ( cc[1] * 1000000 );
201}
202
203#endif
204
205
206static inline void serialize()
207{
208}
209
210
211static inline double loadClockCount (unsigned long a[2])
212{
213 return a[1]*1.0e6 + a[0];
214}
215
216
217double dTimerResolution()
218{
219 unsigned long cc1[2],cc2[2];
220 getClockCount (cc1);
221 do {
222 getClockCount (cc2);
223 }
224 while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
225 do {
226 getClockCount (cc1);
227 }
228 while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
229 double t1 = loadClockCount (cc1);
230 double t2 = loadClockCount (cc2);
231 return (t1-t2) / dTimerTicksPerSecond();
232}
233
234
235double dTimerTicksPerSecond()
236{
237 return 1000000;
238}
239
240#endif
241
242//****************************************************************************
243// stop watches
244
245void dStopwatchReset (dStopwatch *s)
246{
247 s->time = 0;
248 s->cc[0] = 0;
249 s->cc[1] = 0;
250}
251
252
253void dStopwatchStart (dStopwatch *s)
254{
255 serialize();
256 getClockCount (s->cc);
257}
258
259
260void dStopwatchStop (dStopwatch *s)
261{
262 unsigned long cc[2];
263 serialize();
264 getClockCount (cc);
265 double t1 = loadClockCount (s->cc);
266 double t2 = loadClockCount (cc);
267 s->time += t2-t1;
268}
269
270
271double dStopwatchTime (dStopwatch *s)
272{
273 return s->time / dTimerTicksPerSecond();
274}
275
276//****************************************************************************
277// code timers
278
279// maximum number of events to record
280#define MAXNUM 100
281
282static int num = 0; // number of entries used in event array
283static struct {
284 unsigned long cc[2]; // clock counts
285 double total_t; // total clocks used in this slot.
286 double total_p; // total percentage points used in this slot.
287 int count; // number of times this slot has been updated.
288 char *description; // pointer to static string
289} event[MAXNUM];
290
291
292// make sure all slot totals and counts reset to 0 at start
293
294static void initSlots()
295{
296 static int initialized=0;
297 if (!initialized) {
298 for (int i=0; i<MAXNUM; i++) {
299 event[i].count = 0;
300 event[i].total_t = 0;
301 event[i].total_p = 0;
302 }
303 initialized = 1;
304 }
305}
306
307
308void dTimerStart (const char *description)
309{
310 initSlots();
311 event[0].description = const_cast<char*> (description);
312 num = 1;
313 serialize();
314 getClockCount (event[0].cc);
315}
316
317
318void dTimerNow (const char *description)
319{
320 if (num < MAXNUM) {
321 // do not serialize
322 getClockCount (event[num].cc);
323 event[num].description = const_cast<char*> (description);
324 num++;
325 }
326}
327
328
329void dTimerEnd()
330{
331 if (num < MAXNUM) {
332 serialize();
333 getClockCount (event[num].cc);
334 event[num].description = "TOTAL";
335 num++;
336 }
337}
338
339//****************************************************************************
340// print report
341
342static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt)
343{
344 if (a >= 0.999999) {
345 fprintf (f,fmt,a);
346 return;
347 }
348 a *= 1000.0;
349 if (a >= 0.999999) {
350 fprintf (f,fmt,a);
351 fprintf (f,"m");
352 return;
353 }
354 a *= 1000.0;
355 if (a >= 0.999999) {
356 fprintf (f,fmt,a);
357 fprintf (f,"u");
358 return;
359 }
360 a *= 1000.0;
361 fprintf (f,fmt,a);
362 fprintf (f,"n");
363}
364
365
366void dTimerReport (FILE *fout, int average)
367{
368 int i;
369 size_t maxl;
370 double ccunit = 1.0/dTimerTicksPerSecond();
371 fprintf (fout,"\nTimer Report (");
372 fprintDoubleWithPrefix (fout,ccunit,"%.2f ");
373 fprintf (fout,"s resolution)\n------------\n");
374 if (num < 1) return;
375
376 // get maximum description length
377 maxl = 0;
378 for (i=0; i<num; i++) {
379 size_t l = strlen (event[i].description);
380 if (l > maxl) maxl = l;
381 }
382
383 // calculate total time
384 double t1 = loadClockCount (event[0].cc);
385 double t2 = loadClockCount (event[num-1].cc);
386 double total = t2 - t1;
387 if (total <= 0) total = 1;
388
389 // compute time difference for all slots except the last one. update totals
390 double *times = (double*) ALLOCA (num * sizeof(double));
391 for (i=0; i < (num-1); i++) {
392 double t1 = loadClockCount (event[i].cc);
393 double t2 = loadClockCount (event[i+1].cc);
394 times[i] = t2 - t1;
395 event[i].count++;
396 event[i].total_t += times[i];
397 event[i].total_p += times[i]/total * 100.0;
398 }
399
400 // print report (with optional averages)
401 for (i=0; i<num; i++) {
402 double t,p;
403 if (i < (num-1)) {
404 t = times[i];
405 p = t/total * 100.0;
406 }
407 else {
408 t = total;
409 p = 100.0;
410 }
411 fprintf (fout,"%-*s %7.2fms %6.2f%%",maxl,event[i].description,
412 t*ccunit * 1000.0, p);
413 if (average && i < (num-1)) {
414 fprintf (fout," (avg %7.2fms %6.2f%%)",
415 (event[i].total_t / event[i].count)*ccunit * 1000.0,
416 event[i].total_p / event[i].count);
417 }
418 fprintf (fout,"\n");
419 }
420 fprintf (fout,"\n");
421}
diff --git a/libraries/ode-0.9/ode/src/util.cpp b/libraries/ode-0.9/ode/src/util.cpp
new file mode 100644
index 0000000..c8d6230
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/util.cpp
@@ -0,0 +1,374 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include "ode/ode.h"
24#include "objects.h"
25#include "joint.h"
26#include "util.h"
27
28#define ALLOCA dALLOCA16
29
30//****************************************************************************
31// Auto disabling
32
33void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
34{
35 dxBody *bb;
36 for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
37 {
38 // don't freeze objects mid-air (patch 1586738)
39 if ( bb->firstjoint == NULL ) continue;
40
41 // nothing to do unless this body is currently enabled and has
42 // the auto-disable flag set
43 if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
44
45 // if sampling / threshold testing is disabled, we can never sleep.
46 if ( bb->adis.average_samples == 0 ) continue;
47
48 //
49 // see if the body is idle
50 //
51
52#ifndef dNODEBUG
53 // sanity check
54 if ( bb->average_counter >= bb->adis.average_samples )
55 {
56 dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
57
58 // something is going wrong, reset the average-calculations
59 bb->average_ready = 0; // not ready for average calculation
60 bb->average_counter = 0; // reset the buffer index
61 }
62#endif // dNODEBUG
63
64 // sample the linear and angular velocity
65 bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
66 bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
67 bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
68 bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
69 bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
70 bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
71 bb->average_counter++;
72
73 // buffer ready test
74 if ( bb->average_counter >= bb->adis.average_samples )
75 {
76 bb->average_counter = 0; // fill the buffer from the beginning
77 bb->average_ready = 1; // this body is ready now for average calculation
78 }
79
80 int idle = 0; // Assume it's in motion unless we have samples to disprove it.
81
82 // enough samples?
83 if ( bb->average_ready )
84 {
85 idle = 1; // Initial assumption: IDLE
86
87 // the sample buffers are filled and ready for calculation
88 dVector3 average_lvel, average_avel;
89
90 // Store first velocity samples
91 average_lvel[0] = bb->average_lvel_buffer[0][0];
92 average_avel[0] = bb->average_avel_buffer[0][0];
93 average_lvel[1] = bb->average_lvel_buffer[0][1];
94 average_avel[1] = bb->average_avel_buffer[0][1];
95 average_lvel[2] = bb->average_lvel_buffer[0][2];
96 average_avel[2] = bb->average_avel_buffer[0][2];
97
98 // If we're not in "instantaneous mode"
99 if ( bb->adis.average_samples > 1 )
100 {
101 // add remaining velocities together
102 for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
103 {
104 average_lvel[0] += bb->average_lvel_buffer[i][0];
105 average_avel[0] += bb->average_avel_buffer[i][0];
106 average_lvel[1] += bb->average_lvel_buffer[i][1];
107 average_avel[1] += bb->average_avel_buffer[i][1];
108 average_lvel[2] += bb->average_lvel_buffer[i][2];
109 average_avel[2] += bb->average_avel_buffer[i][2];
110 }
111
112 // make average
113 dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
114
115 average_lvel[0] *= r1;
116 average_avel[0] *= r1;
117 average_lvel[1] *= r1;
118 average_avel[1] *= r1;
119 average_lvel[2] *= r1;
120 average_avel[2] *= r1;
121 }
122
123 // threshold test
124 dReal av_lspeed, av_aspeed;
125 av_lspeed = dDOT( average_lvel, average_lvel );
126 if ( av_lspeed > bb->adis.linear_average_threshold )
127 {
128 idle = 0; // average linear velocity is too high for idle
129 }
130 else
131 {
132 av_aspeed = dDOT( average_avel, average_avel );
133 if ( av_aspeed > bb->adis.angular_average_threshold )
134 {
135 idle = 0; // average angular velocity is too high for idle
136 }
137 }
138 }
139
140 // if it's idle, accumulate steps and time.
141 // these counters won't overflow because this code doesn't run for disabled bodies.
142 if (idle) {
143 bb->adis_stepsleft--;
144 bb->adis_timeleft -= stepsize;
145 }
146 else {
147 // Reset countdowns
148 bb->adis_stepsleft = bb->adis.idle_steps;
149 bb->adis_timeleft = bb->adis.idle_time;
150 }
151
152 // disable the body if it's idle for a long enough time
153 if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
154 {
155 bb->flags |= dxBodyDisabled; // set the disable flag
156
157 // disabling bodies should also include resetting the velocity
158 // should prevent jittering in big "islands"
159 bb->lvel[0] = 0;
160 bb->lvel[1] = 0;
161 bb->lvel[2] = 0;
162 bb->avel[0] = 0;
163 bb->avel[1] = 0;
164 bb->avel[2] = 0;
165 }
166 }
167}
168
169
170//****************************************************************************
171// body rotation
172
173// return sin(x)/x. this has a singularity at 0 so special handling is needed
174// for small arguments.
175
176static inline dReal sinc (dReal x)
177{
178 // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
179 // is actually accurate to one LS bit within this range if double precision
180 // is being used - so don't worry!
181 if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
182 else return dSin(x)/x;
183}
184
185
186// given a body b, apply its linear and angular rotation over the time
187// interval h, thereby adjusting its position and orientation.
188
189void dxStepBody (dxBody *b, dReal h)
190{
191 int j;
192
193 // handle linear velocity
194 for (j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
195
196 if (b->flags & dxBodyFlagFiniteRotation) {
197 dVector3 irv; // infitesimal rotation vector
198 dQuaternion q; // quaternion for finite rotation
199
200 if (b->flags & dxBodyFlagFiniteRotationAxis) {
201 // split the angular velocity vector into a component along the finite
202 // rotation axis, and a component orthogonal to it.
203 dVector3 frv; // finite rotation vector
204 dReal k = dDOT (b->finite_rot_axis,b->avel);
205 frv[0] = b->finite_rot_axis[0] * k;
206 frv[1] = b->finite_rot_axis[1] * k;
207 frv[2] = b->finite_rot_axis[2] * k;
208 irv[0] = b->avel[0] - frv[0];
209 irv[1] = b->avel[1] - frv[1];
210 irv[2] = b->avel[2] - frv[2];
211
212 // make a rotation quaternion q that corresponds to frv * h.
213 // compare this with the full-finite-rotation case below.
214 h *= REAL(0.5);
215 dReal theta = k * h;
216 q[0] = dCos(theta);
217 dReal s = sinc(theta) * h;
218 q[1] = frv[0] * s;
219 q[2] = frv[1] * s;
220 q[3] = frv[2] * s;
221 }
222 else {
223 // make a rotation quaternion q that corresponds to w * h
224 dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
225 b->avel[2]*b->avel[2]);
226 h *= REAL(0.5);
227 dReal theta = wlen * h;
228 q[0] = dCos(theta);
229 dReal s = sinc(theta) * h;
230 q[1] = b->avel[0] * s;
231 q[2] = b->avel[1] * s;
232 q[3] = b->avel[2] * s;
233 }
234
235 // do the finite rotation
236 dQuaternion q2;
237 dQMultiply0 (q2,q,b->q);
238 for (j=0; j<4; j++) b->q[j] = q2[j];
239
240 // do the infitesimal rotation if required
241 if (b->flags & dxBodyFlagFiniteRotationAxis) {
242 dReal dq[4];
243 dWtoDQ (irv,b->q,dq);
244 for (j=0; j<4; j++) b->q[j] += h * dq[j];
245 }
246 }
247 else {
248 // the normal way - do an infitesimal rotation
249 dReal dq[4];
250 dWtoDQ (b->avel,b->q,dq);
251 for (j=0; j<4; j++) b->q[j] += h * dq[j];
252 }
253
254 // normalize the quaternion and convert it to a rotation matrix
255 dNormalize4 (b->q);
256 dQtoR (b->q,b->posr.R);
257
258 // notify all attached geoms that this body has moved
259 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
260 dGeomMoved (geom);
261}
262
263//****************************************************************************
264// island processing
265
266// this groups all joints and bodies in a world into islands. all objects
267// in an island are reachable by going through connected bodies and joints.
268// each island can be simulated separately.
269// note that joints that are not attached to anything will not be included
270// in any island, an so they do not affect the simulation.
271//
272// this function starts new island from unvisited bodies. however, it will
273// never start a new islands from a disabled body. thus islands of disabled
274// bodies will not be included in the simulation. disabled bodies are
275// re-enabled if they are found to be part of an active island.
276
277void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
278{
279 dxBody *b,*bb,**body;
280 dxJoint *j,**joint;
281
282 // nothing to do if no bodies
283 if (world->nb <= 0) return;
284
285 // handle auto-disabling of bodies
286 dInternalHandleAutoDisabling (world,stepsize);
287
288 // make arrays for body and joint lists (for a single island) to go into
289 body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
290 joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
291 int bcount = 0; // number of bodies in `body'
292 int jcount = 0; // number of joints in `joint'
293
294 // set all body/joint tags to 0
295 for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
296 for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
297
298 // allocate a stack of unvisited bodies in the island. the maximum size of
299 // the stack can be the lesser of the number of bodies or joints, because
300 // new bodies are only ever added to the stack by going through untagged
301 // joints. all the bodies in the stack must be tagged!
302 int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
303 dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
304
305 for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
306 // get bb = the next enabled, untagged body, and tag it
307 if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
308 bb->tag = 1;
309
310 // tag all bodies and joints starting from bb.
311 int stacksize = 0;
312 b = bb;
313 body[0] = bb;
314 bcount = 1;
315 jcount = 0;
316 goto quickstart;
317 while (stacksize > 0) {
318 b = stack[--stacksize]; // pop body off stack
319 body[bcount++] = b; // put body on body list
320 quickstart:
321
322 // traverse and tag all body's joints, add untagged connected bodies
323 // to stack
324 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
325 if (!n->joint->tag) {
326 n->joint->tag = 1;
327 joint[jcount++] = n->joint;
328 if (n->body && !n->body->tag) {
329 n->body->tag = 1;
330 stack[stacksize++] = n->body;
331 }
332 }
333 }
334 dIASSERT(stacksize <= world->nb);
335 dIASSERT(stacksize <= world->nj);
336 }
337
338 // now do something with body and joint lists
339 stepper (world,body,bcount,joint,jcount,stepsize);
340
341 // what we've just done may have altered the body/joint tag values.
342 // we must make sure that these tags are nonzero.
343 // also make sure all bodies are in the enabled state.
344 int i;
345 for (i=0; i<bcount; i++) {
346 body[i]->tag = 1;
347 body[i]->flags &= ~dxBodyDisabled;
348 }
349 for (i=0; i<jcount; i++) joint[i]->tag = 1;
350 }
351
352 // if debugging, check that all objects (except for disabled bodies,
353 // unconnected joints, and joints that are connected to disabled bodies)
354 // were tagged.
355# ifndef dNODEBUG
356 for (b=world->firstbody; b; b=(dxBody*)b->next) {
357 if (b->flags & dxBodyDisabled) {
358 if (b->tag) dDebug (0,"disabled body tagged");
359 }
360 else {
361 if (!b->tag) dDebug (0,"enabled body not tagged");
362 }
363 }
364 for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
365 if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
366 (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
367 if (!j->tag) dDebug (0,"attached enabled joint not tagged");
368 }
369 else {
370 if (j->tag) dDebug (0,"unattached or disabled joint tagged");
371 }
372 }
373# endif
374}
diff --git a/libraries/ode-0.9/ode/src/util.h b/libraries/ode-0.9/ode/src/util.h
new file mode 100644
index 0000000..a8e6390
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/util.h
@@ -0,0 +1,38 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_UTIL_H_
24#define _ODE_UTIL_H_
25
26#include "objects.h"
27
28
29void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize);
30void dxStepBody (dxBody *b, dReal h);
31
32typedef void (*dstepper_fn_t) (dxWorld *world, dxBody * const *body, int nb,
33 dxJoint * const *_joint, int nj, dReal stepsize);
34
35void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper);
36
37
38#endif
generated by cgit v1.1 at 2024-09-18 02:50:16 +0000