[ode] 01/09: Imported Upstream version 0.14
Leopold Palomo-Avellaneda
leo at alaxarxa.net
Thu Apr 28 13:21:24 UTC 2016
This is an automated email from the git hooks/post-receive script.
lepalom-guest pushed a commit to branch master
in repository ode.
commit b5757a6919ca6fcf2541aa6975e55188433563a8
Author: Leopold Palomo-Avellaneda <leopold.palomo at upc.edu>
Date: Wed Apr 27 08:31:36 2016 +0200
Imported Upstream version 0.14
---
.hg_archival.txt | 5 +-
.hgtags | 2 +
CHANGELOG.txt | 15 ++
GIMPACT/include/GIMPACT/gim_math.h | 3 +
build/premake4.lua | 9 +-
configure.ac | 18 +-
drawstuff/src/drawstuff.cpp | 28 +--
include/drawstuff/drawstuff.h | 12 +-
include/ode/collision.h | 14 +-
libccd/configure.ac | 2 +-
ode/demo/demo_moving_trimesh.cpp | 98 +++++++-
ode/demo/demo_trimesh.cpp | 70 +++++-
ode/src/Makefile.am | 6 +-
ode/src/collision_convex_trimesh.cpp | 99 ++++++++
ode/src/collision_cylinder_trimesh.cpp | 57 ++---
ode/src/collision_kernel.cpp | 1 +
ode/src/collision_libccd.cpp | 167 ++++++++++++-
ode/src/collision_libccd.h | 2 +
ode/src/collision_std.h | 12 +-
ode/src/collision_trimesh_box.cpp | 380 +++++++++++++-----------------
ode/src/collision_trimesh_ccylinder.cpp | 92 ++------
ode/src/collision_trimesh_colliders.h | 1 +
ode/src/collision_trimesh_disabled.cpp | 1 -
ode/src/collision_trimesh_gimpact.cpp | 34 +--
ode/src/collision_trimesh_internal.h | 362 +++++++++++++++++++++++-----
ode/src/collision_trimesh_plane.cpp | 35 +--
ode/src/collision_trimesh_ray.cpp | 24 +-
ode/src/collision_trimesh_sphere.cpp | 63 ++---
ode/src/collision_trimesh_trimesh.cpp | 9 +-
ode/src/collision_trimesh_trimesh_new.cpp | 23 +-
ode/src/collision_util.h | 72 ++++--
ode/src/convex.cpp | 46 ++--
ode/src/quickstep.cpp | 52 ++--
33 files changed, 1172 insertions(+), 642 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index c4436b8..482ca15 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,4 @@
repo: 5ec20c9e20a4a77b0aa9c21aa4c7a1e1f60978f8
-node: bd71868b92674fae9b9e41e61ca8875134e8249c
+node: a77b1d06e3a62b0116e7d3c4be4c6beaac8726ea
branch: default
-latesttag: 0.13.1
-latesttagdistance: 19
+tag: 0.14
diff --git a/.hgtags b/.hgtags
index 58414a0..ed70b58 100644
--- a/.hgtags
+++ b/.hgtags
@@ -10,3 +10,5 @@
c528f26e7eb8e95aa14d68e0c76f2acf0bca2ddf 0.0.1
f89ea1ff3fc1b669d5f7b8d8475580b00f28ba9c 0.10.1
f6383a55d1a34cf18200dcc38b19536387cee7f5 0.13.1
+4cf81eb385f506785054e0773d94f89eff7484f9 0.13.2
+7b0c5226eb9c3090e1b8a63aa202f2d8588fea6b 0.14
diff --git a/CHANGELOG.txt b/CHANGELOG.txt
index 720b50e..0eb99a3 100644
--- a/CHANGELOG.txt
+++ b/CHANGELOG.txt
@@ -8,6 +8,21 @@ the rules for this file:
* keep the format consistent (79 char width, M/D/Y date format).
------------------------------------------------------------------------------
+11/28/2015 Oleh Derevenko
+ * Convex-Trimesh collider added (libccd+GIMPACT only)(by Piotr Piastucki)
+ * dCreateConvex() and dGeomSetConvex() public APIs changed to expect
+ their parameter arrays as const pointers
+
+11/01/2015 Oleh Derevenko
+ * OPCODE mesh colliders' input coordinates have been offset to
+ mesh-relative frames to decrease potential computational errors
+ (suggested by luckytrashsc2 at g***l.com)
+
+08/05/2015 Oleh Derevenko
+ * Implemented change to return highest depth contacts subset for GIMPACT
+ in cases if contacts count exceeds requested maximum (as suggested in
+ the issue #36 by Piotr Piastucki)
+
11/17/2014 Daniel K. O.
* Added support for using libccd from the system (if found via
pkg-config)
diff --git a/GIMPACT/include/GIMPACT/gim_math.h b/GIMPACT/include/GIMPACT/gim_math.h
index d348de4..97fdad2 100644
--- a/GIMPACT/include/GIMPACT/gim_math.h
+++ b/GIMPACT/include/GIMPACT/gim_math.h
@@ -59,6 +59,9 @@ Constants starting with G_
#define GINT32 int32_t
#define GUINT32 uint32_t
+#ifdef GPTR
+#undef GPTR
+#endif
#define GPTR void*
/*! Constants for integers*/
diff --git a/build/premake4.lua b/build/premake4.lua
index 76c7309..f77706e 100644
--- a/build/premake4.lua
+++ b/build/premake4.lua
@@ -4,7 +4,7 @@
-- For more information on Premake: http://industriousone.com/premake
----------------------------------------------------------------------
- ode_version = "0.13.1"
+ ode_version = "0.14"
----------------------------------------------------------------------
-- Demo list: add/remove demos from here and the rest of the build
@@ -349,7 +349,8 @@
"../ode/src/collision_trimesh_ray.cpp",
"../ode/src/collision_trimesh_sphere.cpp",
"../ode/src/collision_trimesh_trimesh.cpp",
- "../ode/src/collision_trimesh_plane.cpp"
+ "../ode/src/collision_trimesh_plane.cpp",
+ "../ode/src/collision_convex_trimesh.cpp"
}
configuration { "not no-trimesh", "with-gimpact or all-collis-libs" }
@@ -360,7 +361,9 @@
configuration { "with-libccd" }
files { "../libccd/src/ccd/*.h", "../libccd/src/*.c" }
- defines { "dLIBCCD_ENABLED", "dLIBCCD_CYL_CYL" }
+ defines { "dLIBCCD_ENABLED", "dLIBCCD_INTERNAL",
+ "dLIBCCD_BOX_CYL", "dLIBCCD_CYL_CYL", "dLIBCCD_CAP_CYL", "dLIBCCD_CONVEX_BOX",
+ "dLIBCCD_CONVEX_CAP", "dLIBCCD_CONVEX_CYL", "dLIBCCD_CONVEX_SPHERE", "dLIBCCD_CONVEX_CONVEX" }
configuration { "not with-libccd" }
excludes { "../ode/src/collision_libccd.cpp", "../ode/src/collision_libccd.h" }
diff --git a/configure.ac b/configure.ac
index a606561..d2d7108 100644
--- a/configure.ac
+++ b/configure.ac
@@ -1,6 +1,6 @@
dnl AC_INIT does not take a macro as a version nr: set it separately! - Bram
-AC_INIT([ODE],[0.13.1],[ode at ode.org])
-ODE_VERSION=0.13.1
+AC_INIT([ODE],[0.14],[ode at ode.org])
+ODE_VERSION=0.14
AC_SUBST(ODE_VERSION)
# Those are instructions from the Libtool manual:
@@ -22,9 +22,9 @@ AC_SUBST(ODE_VERSION)
#
# 6. If any interfaces have been removed since the last public release,
# then set AGE to 0.
-CURRENT=5
+CURRENT=6
REVISION=0
-AGE=1
+AGE=0
AC_ARG_ENABLE(version-info,
AS_HELP_STRING([--disable-version-info],
@@ -440,7 +440,7 @@ libccd_source=internal
AC_ARG_WITH(libccd,
[AS_HELP_STRING([--with-libccd=@<:@internal|system@:>@],
- [use the specified libccd @<:@default=internal@:>@])],
+ [use the specified libccd @<:@default=system@:>@])],
[libccd_source=$withval],
[libccd_source=system])
@@ -452,7 +452,13 @@ then
fi
fi
-AC_CONFIG_SUBDIRS([libccd])
+if test x$use_libccd = xyes
+then
+ if test x$libccd_source = xinternal
+ then
+ AC_CONFIG_SUBDIRS([libccd])
+ fi
+fi
AM_CONDITIONAL(LIBCCD, test x$use_libccd != xno)
AM_CONDITIONAL(LIBCCD_INTERNAL, test x$libccd_source = xinternal)
diff --git a/drawstuff/src/drawstuff.cpp b/drawstuff/src/drawstuff.cpp
index 544c3cf..ae55c1b 100644
--- a/drawstuff/src/drawstuff.cpp
+++ b/drawstuff/src/drawstuff.cpp
@@ -394,10 +394,9 @@ static void setShadowTransform()
glMultMatrixf (matrix);
}
-static void drawConvex (float *_planes,unsigned int _planecount,
- float *_points,
- unsigned int /*_pointcount*/,
- unsigned int *_polygons)
+static void drawConvex (const float *_planes, unsigned int _planecount,
+ const float *_points, unsigned int /*_pointcount*/,
+ const unsigned int *_polygons)
{
unsigned int polyindex=0;
for(unsigned int i=0;i<_planecount;++i)
@@ -419,10 +418,9 @@ static void drawConvex (float *_planes,unsigned int _planecount,
}
}
-static void drawConvexD (double *_planes,unsigned int _planecount,
- double *_points,
- unsigned int /*_pointcount*/,
- unsigned int *_polygons)
+static void drawConvexD (const double *_planes, unsigned int _planecount,
+ const double *_points, unsigned int /*_pointcount*/,
+ const unsigned int *_polygons)
{
unsigned int polyindex=0;
for(unsigned int i=0;i<_planecount;++i)
@@ -1379,10 +1377,9 @@ extern "C" void dsDrawBox (const float pos[3], const float R[12],
}
extern "C" void dsDrawConvex (const float pos[3], const float R[12],
- float *_planes,unsigned int _planecount,
- float *_points,
- unsigned int _pointcount,
- unsigned int *_polygons)
+ const float *_planes,unsigned int _planecount,
+ const float *_points, unsigned int _pointcount,
+ const unsigned int *_polygons)
{
if (current_state != 2) dsError ("drawing function called outside simulation loop");
setupDrawingMode();
@@ -1537,10 +1534,9 @@ extern "C" void dsDrawBoxD (const double pos[3], const double R[12],
}
extern "C" void dsDrawConvexD (const double pos[3], const double R[12],
- double *_planes,unsigned int _planecount,
- double *_points,
- unsigned int _pointcount,
- unsigned int *_polygons)
+ const double *_planes, unsigned int _planecount,
+ const double *_points, unsigned int _pointcount,
+ const unsigned int *_polygons)
{
if (current_state != 2) dsError ("drawing function called outside simulation loop");
setupDrawingMode();
diff --git a/include/drawstuff/drawstuff.h b/include/drawstuff/drawstuff.h
index 88222aa..64d53c8 100644
--- a/include/drawstuff/drawstuff.h
+++ b/include/drawstuff/drawstuff.h
@@ -253,11 +253,11 @@ DS_API void dsDrawLine (const float pos1[3], const float pos2[3]);
* @ingroup drawstuff
*/
DS_API void dsDrawConvex(const float pos[3], const float R[12],
- float *_planes,
+ const float *_planes,
unsigned int _planecount,
- float *_points,
+ const float *_points,
unsigned int _pointcount,
- unsigned int *_polygons);
+ const unsigned int *_polygons);
/* these drawing functions are identical to the ones above, except they take
* double arrays for `pos' and `R'.
@@ -274,11 +274,11 @@ DS_API void dsDrawCapsuleD (const double pos[3], const double R[12],
float length, float radius);
DS_API void dsDrawLineD (const double pos1[3], const double pos2[3]);
DS_API void dsDrawConvexD(const double pos[3], const double R[12],
- double *_planes,
+ const double *_planes,
unsigned int _planecount,
- double *_points,
+ const double *_points,
unsigned int _pointcount,
- unsigned int *_polygons);
+ const unsigned int *_polygons);
/**
* @brief Set the quality with which curved objects are rendered.
diff --git a/include/ode/collision.h b/include/ode/collision.h
index 3bd1183..fef6e7b 100644
--- a/include/ode/collision.h
+++ b/include/ode/collision.h
@@ -963,16 +963,18 @@ ODE_API dReal dGeomSpherePointDepth (dGeomID sphere, dReal x, dReal y, dReal z);
/*--> Convex Functions*/
ODE_API dGeomID dCreateConvex (dSpaceID space,
- dReal *_planes,
+ const dReal *_planes,
unsigned int _planecount,
- dReal *_points,
- unsigned int _pointcount,unsigned int *_polygons);
+ const dReal *_points,
+ unsigned int _pointcount,
+ const unsigned int *_polygons);
ODE_API void dGeomSetConvex (dGeomID g,
- dReal *_planes,
+ const dReal *_planes,
unsigned int _count,
- dReal *_points,
- unsigned int _pointcount,unsigned int *_polygons);
+ const dReal *_points,
+ unsigned int _pointcount,
+ const unsigned int *_polygons);
/*<-- Convex Functions*/
/**
diff --git a/libccd/configure.ac b/libccd/configure.ac
index 7e86a8c..5a12836 100644
--- a/libccd/configure.ac
+++ b/libccd/configure.ac
@@ -5,7 +5,7 @@
AC_INIT([libccd], [1.0], [danfis at danfis.cz])
AC_CONFIG_SRCDIR([src/ccd.c])
AC_CONFIG_HEADERS([src/config.h])
-AM_INIT_AUTOMAKE
+AM_INIT_AUTOMAKE(foreign)
# Checks for programs.
AC_PROG_CXX
diff --git a/ode/demo/demo_moving_trimesh.cpp b/ode/demo/demo_moving_trimesh.cpp
index 68a0aa0..20ff167 100644
--- a/ode/demo/demo_moving_trimesh.cpp
+++ b/ode/demo/demo_moving_trimesh.cpp
@@ -29,6 +29,84 @@
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
+//<---- Convex Object
+static const dReal planes[] = // planes for a cube
+{
+ 1.0f ,0.0f ,0.0f ,0.25f,
+ 0.0f ,1.0f ,0.0f ,0.25f,
+ 0.0f ,0.0f ,1.0f ,0.25f,
+ 0.0f ,0.0f ,-1.0f,0.25f,
+ 0.0f ,-1.0f,0.0f ,0.25f,
+ -1.0f,0.0f ,0.0f ,0.25f
+ /*
+ 1.0f ,0.0f ,0.0f ,2.0f,
+ 0.0f ,1.0f ,0.0f ,1.0f,
+ 0.0f ,0.0f ,1.0f ,1.0f,
+ 0.0f ,0.0f ,-1.0f,1.0f,
+ 0.0f ,-1.0f,0.0f ,1.0f,
+ -1.0f,0.0f ,0.0f ,0.0f
+ */
+};
+static const unsigned int planecount=6;
+
+static const dReal points[] = // points for a cube
+{
+ 0.25f,0.25f,0.25f,
+ -0.25f,0.25f,0.25f,
+
+ 0.25f,-0.25f,0.25f,
+ -0.25f,-0.25f,0.25f,
+
+ 0.25f,0.25f,-0.25f,
+ -0.25f,0.25f,-0.25f,
+
+ 0.25f,-0.25f,-0.25f,
+ -0.25f,-0.25f,-0.25f,
+};
+static const unsigned int pointcount=8;
+
+static const unsigned int polygons[] = //Polygons for a cube (6 squares)
+ {
+ 4,0,2,6,4, // positive X
+ 4,1,0,4,5, // positive Y
+ 4,0,1,3,2, // positive Z
+ 4,3,1,5,7, // negative X
+ 4,2,3,7,6, // negative Y
+ 4,5,4,6,7, // negative Z
+ };
+//----> Convex Object
+
+int tmTriangles[] =
+{
+ 0,2,6,
+ 0,6,4,
+ 1,0,4,
+ 1,4,5,
+ 0,1,3,
+ 0,3,2,
+ 3,1,5,
+ 3,5,7,
+ 2,3,7,
+ 2,7,6,
+ 5,4,6,
+ 5,6,7
+};
+
+float tmVertices[] =
+{
+ 0.25f,0.25f,0.25f, // point 0
+ -0.25f,0.25f,0.25f, // point 1
+
+ 0.25f,-0.25f,0.25f, // point 2
+ -0.25f,-0.25f,0.25f,// point 3
+
+ 0.25f,0.25f,-0.25f, // point 4
+ -0.25f,0.25f,-0.25f,// point 5
+
+ 0.25f,-0.25f,-0.25f,// point 6
+ -0.25f,-0.25f,-0.25f,// point 7
+};
+
// select correct drawing functions
#ifdef dDOUBLE
@@ -38,6 +116,7 @@
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
+#define dsDrawConvex dsDrawConvexD
#endif
@@ -128,6 +207,7 @@ static void start()
printf (" y for cylinder.\n");
printf (" c for capsule.\n");
printf (" x for a composite object.\n");
+ printf (" v for a convex object.\n");
printf (" m for a trimesh.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
@@ -155,7 +235,7 @@ static void command (int cmd)
bool setBody = false;
cmd = locase (cmd);
- if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
+ if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') {
if (num < NUM) {
i = num;
num++;
@@ -203,6 +283,15 @@ static void command (int cmd)
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
+ } else if (cmd == 'v') {
+
+ dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
+ obj[i].geom[0] = dCreateConvex(space,
+ planes,
+ planecount,
+ points,
+ pointcount,
+ polygons);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
@@ -340,6 +429,13 @@ void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
+ } else if (type == dConvexClass) {
+ //dVector3 sides={0.50,0.50,0.50};
+ dsDrawConvex(pos,R,planes,
+ planecount,
+ points,
+ pointcount,
+ polygons);
}
if (show_aabb) {
diff --git a/ode/demo/demo_trimesh.cpp b/ode/demo/demo_trimesh.cpp
index 2b03e7b..70ccac3 100644
--- a/ode/demo/demo_trimesh.cpp
+++ b/ode/demo/demo_trimesh.cpp
@@ -30,6 +30,53 @@
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
+//<---- Convex Object
+static const dReal planes[] = // planes for a cube
+{
+ 1.0f ,0.0f ,0.0f ,0.25f,
+ 0.0f ,1.0f ,0.0f ,0.25f,
+ 0.0f ,0.0f ,1.0f ,0.25f,
+ 0.0f ,0.0f ,-1.0f,0.25f,
+ 0.0f ,-1.0f,0.0f ,0.25f,
+ -1.0f,0.0f ,0.0f ,0.25f
+ /*
+ 1.0f ,0.0f ,0.0f ,2.0f,
+ 0.0f ,1.0f ,0.0f ,1.0f,
+ 0.0f ,0.0f ,1.0f ,1.0f,
+ 0.0f ,0.0f ,-1.0f,1.0f,
+ 0.0f ,-1.0f,0.0f ,1.0f,
+ -1.0f,0.0f ,0.0f ,0.0f
+ */
+};
+static const unsigned int planecount=6;
+
+static const dReal points[] = // points for a cube
+{
+ 0.25f,0.25f,0.25f,
+ -0.25f,0.25f,0.25f,
+
+ 0.25f,-0.25f,0.25f,
+ -0.25f,-0.25f,0.25f,
+
+ 0.25f,0.25f,-0.25f,
+ -0.25f,0.25f,-0.25f,
+
+ 0.25f,-0.25f,-0.25f,
+ -0.25f,-0.25f,-0.25f,
+};
+static const unsigned int pointcount=8;
+
+static const unsigned int polygons[] = //Polygons for a cube (6 squares)
+ {
+ 4,0,2,6,4, // positive X
+ 4,1,0,4,5, // positive Y
+ 4,0,1,3,2, // positive Z
+ 4,3,1,5,7, // negative X
+ 4,2,3,7,6, // negative Y
+ 4,5,4,6,7, // negative Z
+ };
+//----> Convex Object
+
// select correct drawing functions
#ifdef dDOUBLE
@@ -39,6 +86,7 @@
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
+#define dsDrawConvex dsDrawConvexD
#endif
@@ -143,6 +191,7 @@ static void start()
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for cylinder.\n");
+ printf( " v for a convex.\n" );
printf (" x for a composite object.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
@@ -170,7 +219,7 @@ static void command (int cmd)
bool setBody = false;
cmd = locase (cmd);
- if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
+ if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
@@ -284,7 +333,17 @@ static void command (int cmd)
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
- }
+ } else if (cmd == 'v') {
+
+ dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
+
+ obj[i].geom[0] = dCreateConvex(space,
+ planes,
+ planecount,
+ points,
+ pointcount,
+ polygons);
+ }
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
@@ -339,6 +398,13 @@ void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
+ } else if (type == dConvexClass) {
+ //dVector3 sides={0.50,0.50,0.50};
+ dsDrawConvex(pos,R,planes,
+ planecount,
+ points,
+ pointcount,
+ polygons);
}
/*
// cylinder option not yet implemented
diff --git a/ode/src/Makefile.am b/ode/src/Makefile.am
index 370bd98..4c984ac 100644
--- a/ode/src/Makefile.am
+++ b/ode/src/Makefile.am
@@ -108,7 +108,8 @@ libode_la_SOURCES += collision_trimesh_trimesh.cpp \
collision_trimesh_distance.cpp \
collision_trimesh_internal.h \
collision_cylinder_trimesh.cpp \
- collision_trimesh_plane.cpp
+ collision_trimesh_plane.cpp \
+ collision_convex_trimesh.cpp
endif
@@ -133,7 +134,8 @@ libode_la_SOURCES+= collision_trimesh_trimesh.cpp \
collision_trimesh_distance.cpp \
collision_trimesh_internal.h \
collision_cylinder_trimesh.cpp \
- collision_trimesh_plane.cpp
+ collision_trimesh_plane.cpp \
+ collision_convex_trimesh.cpp
endif
diff --git a/ode/src/collision_convex_trimesh.cpp b/ode/src/collision_convex_trimesh.cpp
new file mode 100644
index 0000000..781d9b8
--- /dev/null
+++ b/ode/src/collision_convex_trimesh.cpp
@@ -0,0 +1,99 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+
+typedef struct _sLocalContactData
+{
+ dVector3 vPos;
+ dVector3 vNormal;
+ dReal fDepth;
+ int triIndex;
+ int nFlags; // 0 = filtered out, 1 = OK
+}sLocalContactData;
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_std.h"
+#include "collision_trimesh_internal.h"
+#if dLIBCCD_ENABLED
+#include "collision_libccd.h"
+#endif
+
+int dCollideConvexTrimesh( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
+{
+ int contactcount = 0;
+ dIASSERT( skip >= (int)sizeof( dContactGeom ) );
+ dIASSERT( o1->type == dConvexClass );
+ dIASSERT( o2->type == dTriMeshClass );
+ dIASSERT ((flags & NUMC_MASK) >= 1);
+
+#if dLIBCCD_ENABLED
+
+#if dTRIMESH_OPCODE
+
+
+
+#elif dTRIMESH_GIMPACT
+ dxTriMesh *trimesh = (dxTriMesh *)o2;
+
+ aabb3f test_aabb;
+ test_aabb.minX = o1->aabb[0];
+ test_aabb.maxX = o1->aabb[1];
+ test_aabb.minY = o1->aabb[2];
+ test_aabb.maxY = o1->aabb[3];
+ test_aabb.minZ = o1->aabb[4];
+ test_aabb.maxZ = o1->aabb[5];
+
+ GDYNAMIC_ARRAY collision_result;
+ GIM_CREATE_BOXQUERY_LIST(collision_result);
+
+ gim_aabbset_box_collision(&test_aabb, &trimesh->m_collision_trimesh.m_aabbset, &collision_result);
+
+ if (collision_result.m_size != 0)
+ {
+ GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
+ GIM_TRIMESH * ptrimesh = &trimesh->m_collision_trimesh;
+ gim_trimesh_locks_work_data(ptrimesh);
+
+ contactcount = dCollideConvexTrimeshTrianglesCCD(o1, o2, (int *)boxesresult, collision_result.m_size, flags, contacts, skip);
+
+ gim_trimesh_unlocks_work_data(ptrimesh);
+ }
+
+ GIM_DYNARRAY_DESTROY(collision_result);
+#endif // dTRIMESH_GIMPACT
+
+#endif // dLIBCCD_ENABLED
+
+ return contactcount;
+}
+
+#endif // dTRIMESH_ENABLED
+
diff --git a/ode/src/collision_cylinder_trimesh.cpp b/ode/src/collision_cylinder_trimesh.cpp
index 9759a80..857a1e3 100644
--- a/ode/src/collision_cylinder_trimesh.cpp
+++ b/ode/src/collision_cylinder_trimesh.cpp
@@ -971,48 +971,35 @@ static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider,
sCylinderTrimeshColliderData &cData, dxGeom *Cylinder, dxTriMesh *Trimesh,
OBBCache &BoxCache)
{
- const dVector3 &vCylinderPos = cData.m_vCylinderPos;
-
- Point cCenter(vCylinderPos[0],vCylinderPos[1],vCylinderPos[2]);
-
- Point cExtents(cData.m_fCylinderRadius,cData.m_fCylinderRadius,cData.m_fCylinderRadius);
- cExtents[nCYLINDER_AXIS] = cData.m_fCylinderSize * REAL(0.5);
-
- Matrix3x3 obbRot;
+ Matrix4x4 MeshMatrix;
+ const dVector3 vZeroVector3 = { REAL(0.0), };
+ MakeMatrix(vZeroVector3, cData.m_mTrimeshRot, MeshMatrix);
+ const dVector3 &vCylinderPos = cData.m_vCylinderPos;
const dMatrix3 &mCylinderRot = cData.m_mCylinderRot;
- // It is a potential issue to explicitly cast to float
- // if custom width floating point type is introduced in OPCODE.
- // It is necessary to make a typedef and cast to it
- // (e.g. typedef float opc_float;)
- // However I'm not sure in what header it should be added.
+ dVector3 vCylinderOffsetPos;
+ dSubtractVectors3(vCylinderOffsetPos, vCylinderPos, cData.m_vTrimeshPos);
- obbRot[0][0] = /*(float)*/mCylinderRot[0];
- obbRot[1][0] = /*(float)*/mCylinderRot[1];
- obbRot[2][0] = /*(float)*/mCylinderRot[2];
+ const dReal fCylinderRadius = cData.m_fCylinderRadius, fCylinderHalfAxis = cData.m_fCylinderSize * REAL(0.5);
- obbRot[0][1] = /*(float)*/mCylinderRot[4];
- obbRot[1][1] = /*(float)*/mCylinderRot[5];
- obbRot[2][1] = /*(float)*/mCylinderRot[6];
-
- obbRot[0][2] = /*(float)*/mCylinderRot[8];
- obbRot[1][2] = /*(float)*/mCylinderRot[9];
- obbRot[2][2] = /*(float)*/mCylinderRot[10];
-
- OBB obbCapsule(cCenter,cExtents,obbRot);
-
- Matrix4x4 CapsuleMatrix;
- MakeMatrix(vCylinderPos, mCylinderRot, CapsuleMatrix);
-
- Matrix4x4 MeshMatrix;
- MakeMatrix(cData.m_vTrimeshPos, cData.m_mTrimeshRot, MeshMatrix);
+ OBB obbCylinder;
+ obbCylinder.mCenter.Set(vCylinderOffsetPos[0], vCylinderOffsetPos[1], vCylinderOffsetPos[2]);
+ obbCylinder.mExtents.Set(
+ 0 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius,
+ 1 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius,
+ 2 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius);
+ obbCylinder.mRot.Set(
+ mCylinderRot[0], mCylinderRot[4], mCylinderRot[8],
+ mCylinderRot[1], mCylinderRot[5], mCylinderRot[9],
+ mCylinderRot[2], mCylinderRot[6], mCylinderRot[10]);
// TC results
if (Trimesh->doBoxTC)
{
dxTriMesh::BoxTC* BoxTC = 0;
- for (int i = 0; i < Trimesh->BoxTCCache.size(); i++)
+ const int iBoxCacheSize = Trimesh->BoxTCCache.size();
+ for (int i = 0; i != iBoxCacheSize; i++)
{
if (Trimesh->BoxTCCache[i].Geom == Cylinder)
{
@@ -1031,12 +1018,12 @@ static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider,
// Intersect
Collider.SetTemporalCoherence(true);
- Collider.Collide(*BoxTC, obbCapsule, Trimesh->Data->BVTree, null, &MeshMatrix);
+ Collider.Collide(*BoxTC, obbCylinder, Trimesh->Data->BVTree, null, &MeshMatrix);
}
else
{
Collider.SetTemporalCoherence(false);
- Collider.Collide(BoxCache, obbCapsule, Trimesh->Data->BVTree, null,&MeshMatrix);
+ Collider.Collide(BoxCache, obbCylinder, Trimesh->Data->BVTree, null, &MeshMatrix);
}
}
@@ -1070,7 +1057,7 @@ int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *con
{
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
- if (Trimesh->ArrayCallback != null)
+ if (Trimesh->ArrayCallback != NULL)
{
Trimesh->ArrayCallback(Trimesh, Cylinder, Triangles, TriCount);
}
diff --git a/ode/src/collision_kernel.cpp b/ode/src/collision_kernel.cpp
index 2ce5eef..b9f9dce 100644
--- a/ode/src/collision_kernel.cpp
+++ b/ode/src/collision_kernel.cpp
@@ -201,6 +201,7 @@ static void setAllColliders (int i, dColliderFn *fn)
setCollider (dTriMeshClass,dCapsuleClass,&dCollideCCTL);
setCollider (dTriMeshClass,dPlaneClass,&dCollideTrimeshPlane);
setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh);
+ setCollider (dConvexClass,dTriMeshClass,&dCollideConvexTrimesh);
#endif
#ifdef dLIBCCD_BOX_CYL
diff --git a/ode/src/collision_libccd.cpp b/ode/src/collision_libccd.cpp
index 3e67651..9084fbe 100644
--- a/ode/src/collision_libccd.cpp
+++ b/ode/src/collision_libccd.cpp
@@ -27,6 +27,9 @@
#include "odemath.h"
#include "collision_libccd.h"
#include "collision_std.h"
+#if dTRIMESH_ENABLED
+#include "collision_util.h"
+#endif
struct _ccd_obj_t {
@@ -65,6 +68,12 @@ struct _ccd_convex_t {
};
typedef struct _ccd_convex_t ccd_convex_t;
+struct _ccd_triangle_t {
+ ccd_obj_t o;
+ ccd_vec3_t vertices[3];
+};
+typedef struct _ccd_triangle_t ccd_triangle_t;
+
/** Transforms geom to ccd struct */
static void ccdGeomToObj(const dGeomID g, ccd_obj_t *);
static void ccdGeomToBox(const dGeomID g, ccd_box_t *);
@@ -243,7 +252,7 @@ static void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t
ccd_vec3_t dir, p;
ccd_real_t maxdot, dot;
size_t i;
- dReal *curp;
+ const dReal *curp;
ccdVec3Copy(&dir, _dir);
ccdQuatRotVec(&dir, &c->o.rot_inv);
@@ -427,3 +436,159 @@ int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *con
&c1, ccdSupportConvex, ccdCenter,
&c2, ccdSupportConvex, ccdCenter);
}
+
+
+#if dTRIMESH_ENABLED
+
+static
+void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
+{
+ const ccd_triangle_t* o = (ccd_triangle_t *) obj;
+ ccd_real_t maxdot, dot;
+ maxdot = -CCD_REAL_MAX;
+ for (int i = 0; i < 3; i++) {
+ dot = ccdVec3Dot(_dir, &o->vertices[i]);
+ if (dot > maxdot) {
+ ccdVec3Copy(v, &o->vertices[i]);
+ maxdot = dot;
+ }
+ }
+}
+
+const static float CONTACT_DEPTH_EPSILON = 0.0001f;
+const static float CONTACT_POS_EPSILON = 0.0001f;
+const static float CONTACT_PERTURBATION_ANGLE = 0.001f;
+
+static
+int addUniqueContact(dContactGeom *contacts, dContactGeom *c, int contactcount, int maxcontacts, int flags, int skip)
+{
+ dReal minDepth = dInfinity;
+ int index = contactcount;
+ for (int k = 0; k < contactcount; k++) {
+ dContactGeom* pc = SAFECONTACT(flags, contacts, k, skip);
+ if (fabs(c->pos[0] - pc->pos[0]) < CONTACT_POS_EPSILON
+ && fabs(c->pos[1] - pc->pos[1]) < CONTACT_POS_EPSILON
+ && fabs(c->pos[2] - pc->pos[2]) < CONTACT_POS_EPSILON) {
+ index = pc->depth + CONTACT_DEPTH_EPSILON < c->depth ? k : maxcontacts;
+ break;
+ }
+ if (contactcount == maxcontacts && pc->depth < minDepth && pc->depth < c->depth) {
+ minDepth = pc->depth;
+ index = k;
+ }
+ }
+ if (index < maxcontacts) {
+ dContactGeom* contact = SAFECONTACT(flags, contacts, index, skip);
+ contact->g1 = c->g1;
+ contact->g2 = c->g2;
+ contact->depth = c->depth;
+ contact->side1 = c->side1;
+ contact->side2 = c->side2;
+ contact->pos[0] = c->pos[0];
+ contact->pos[1] = c->pos[1];
+ contact->pos[2] = c->pos[2];
+ contact->normal[0] = c->normal[0];
+ contact->normal[1] = c->normal[1];
+ contact->normal[2] = c->normal[2];
+ contactcount = index == contactcount ? contactcount + 1 : contactcount;
+ }
+ return contactcount;
+}
+
+static
+int addPerturbedContacts(dxGeom* o1, dxGeom* o2, int flags, dContactGeom *contacts, int skip,
+ ccd_convex_t* c1, ccd_triangle_t* c2, dVector3* triangle, dContactGeom* contact, int contactcount)
+{
+ int maxcontacts = (flags & 0xffff);
+ dVector3 upAxis, cross;
+ dVector3 pos;
+
+ pos[0] = contact->pos[0];
+ pos[1] = contact->pos[1];
+ pos[2] = contact->pos[2];
+
+ upAxis[0] = 0;
+ upAxis[1] = 1;
+ upAxis[2] = 0;
+ if (fabs(dVector3Dot(contact->normal, upAxis)) > 0.7) {
+ upAxis[0] = 0;
+ upAxis[1] = 0;
+ upAxis[2] = 1;
+ }
+
+ dVector3Cross(contact->normal, upAxis, cross);
+ if (dSafeNormalize3(cross)) {
+
+ dVector3Cross(cross, contact->normal, upAxis);
+ if (dSafeNormalize3(upAxis)) {
+
+ for (int k = 0; k < 4; k++) {
+ dContactGeom perturbedContact;
+ dQuaternion q1, q2, qr;
+ dQFromAxisAndAngle(q1, upAxis[0], upAxis[1], upAxis[2], k % 2 == 0 ? CONTACT_PERTURBATION_ANGLE : -CONTACT_PERTURBATION_ANGLE);
+ dQFromAxisAndAngle(q2, cross[0], cross[1], cross[2], k / 2 == 0 ? CONTACT_PERTURBATION_ANGLE : -CONTACT_PERTURBATION_ANGLE);
+ dQMultiply0(qr, q1, q2);
+
+ for (int j = 0; j < 3; j++) {
+ dVector3 p, perturbed;
+ dVector3Subtract(triangle[j], pos, p);
+ dQuatTransform(qr, p, perturbed);
+ dVector3Add(perturbed, pos, perturbed);
+
+ c2->vertices[j].v[0] = perturbed[0];
+ c2->vertices[j].v[1] = perturbed[1];
+ c2->vertices[j].v[2] = perturbed[2];
+ }
+
+ if (ccdCollide(o1, o2, flags, &perturbedContact, skip, c1, ccdSupportConvex, ccdCenter, c2, ccdSupportTriangle, ccdCenter) == 1) {
+ perturbedContact.side2 = contact->side2;
+ contactcount = addUniqueContact(contacts, &perturbedContact, contactcount, maxcontacts, flags, skip);
+ }
+ }
+ }
+ }
+
+ return contactcount;
+}
+
+/*extern */
+int dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, int* indices, int numindices, int flags, dContactGeom *contacts, int skip)
+{
+ ccd_convex_t c1;
+ ccd_triangle_t c2;
+ dVector3 triangle[3];
+ int maxcontacts = (flags & 0xffff);
+ int contactcount = 0;
+ ccdGeomToConvex(o1, &c1);
+ ccdGeomToObj(o2, (ccd_obj_t *)&c2);
+
+ for (int i = 0; i < numindices; i++) {
+ dContactGeom tempcontact;
+ dGeomTriMeshGetTriangle(o2, indices[i], &triangle[0], &triangle[1], &triangle[2]);
+
+ for (int j = 0; j < 3; j++) {
+ c2.vertices[j].v[0] = triangle[j][0];
+ c2.vertices[j].v[1] = triangle[j][1];
+ c2.vertices[j].v[2] = triangle[j][2];
+ }
+
+ if (ccdCollide(o1, o2, flags, &tempcontact, skip, &c1, ccdSupportConvex, ccdCenter, &c2, ccdSupportTriangle, ccdCenter) == 1) {
+ tempcontact.side2 = i;
+ contactcount = addUniqueContact(contacts, &tempcontact, contactcount, maxcontacts, flags, skip);
+
+ if ((flags & CONTACTS_UNIMPORTANT)) {
+ break;
+ }
+ }
+ }
+
+ if (contactcount == 1 && !(flags & CONTACTS_UNIMPORTANT)) {
+ dContactGeom* contact = SAFECONTACT(flags, contacts, 0, skip);
+ dGeomTriMeshGetTriangle(o2, contact->side2, &triangle[0], &triangle[1], &triangle[2]);
+ contactcount = addPerturbedContacts(o1, o2, flags, contacts, skip, &c1, &c2, triangle, contact, contactcount);
+ }
+
+ return contactcount;
+}
+
+#endif // dTRIMESH_ENABLED
diff --git a/ode/src/collision_libccd.h b/ode/src/collision_libccd.h
index 039341c..49d794d 100644
--- a/ode/src/collision_libccd.h
+++ b/ode/src/collision_libccd.h
@@ -39,4 +39,6 @@ int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *con
int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, int* indices, int numindices, int flags, dContactGeom *contact, int skip);
+
#endif /* _LIBCCD_COLLISION_H_ */
diff --git a/ode/src/collision_std.h b/ode/src/collision_std.h
index 322d64e..710e580 100644
--- a/ode/src/collision_std.h
+++ b/ode/src/collision_std.h
@@ -140,21 +140,21 @@ struct dxRay : public dxGeom {
struct dxConvex : public dxGeom
{
- dReal *planes; /*!< An array of planes in the form:
+ const dReal *planes; /*!< An array of planes in the form:
normal X, normal Y, normal Z,Distance
*/
- dReal *points; /*!< An array of points X,Y,Z */
- unsigned int *polygons; /*! An array of indices to the points of each polygon, it should be the number of vertices followed by that amount of indices to "points" in counter clockwise order*/
+ const dReal *points; /*!< An array of points X,Y,Z */
+ const unsigned int *polygons; /*! An array of indices to the points of each polygon, it should be the number of vertices followed by that amount of indices to "points" in counter clockwise order*/
unsigned int planecount; /*!< Amount of planes in planes */
unsigned int pointcount;/*!< Amount of points in points */
unsigned int edgecount;/*!< Amount of edges in convex */
dReal saabb[6];/*!< Static AABB */
dxConvex(dSpaceID space,
- dReal *planes,
+ const dReal *planes,
unsigned int planecount,
- dReal *points,
+ const dReal *points,
unsigned int pointcount,
- unsigned int *polygons);
+ const unsigned int *polygons);
~dxConvex()
{
if((edgecount!=0)&&(edges!=NULL)) delete[] edges;
diff --git a/ode/src/collision_trimesh_box.cpp b/ode/src/collision_trimesh_box.cpp
index f258549..483c393 100644
--- a/ode/src/collision_trimesh_box.cpp
+++ b/ode/src/collision_trimesh_box.cpp
@@ -101,8 +101,7 @@ struct sTrimeshBoxColliderData
void SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom,
int Flags, dContactGeom* Contacts, int Stride);
- int TestCollisionForSingleTriangle(int ctContacts0, int Triint,
- dVector3 dv[3], bool &bOutFinishSearching);
+ void TestCollisionForSingleTriangle(int Triint, dVector3 dv[3], bool &bOutFinishSearching);
bool _cldTestNormal(dReal fp0, dReal fR, dVector3 vNormal, int iAxis);
bool _cldTestFace(dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD,
@@ -111,7 +110,9 @@ struct sTrimeshBoxColliderData
dVector3 vNormal, int iAxis);
bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2);
void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex);
- void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex);
+ bool _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex);
+
+ void GenerateContact(int TriIndex, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth);
// box data
dMatrix3 m_mHullBoxRot;
@@ -737,19 +738,7 @@ void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v
vPntTmp[2]*=0.5f;
// generate contact point between two closest points
-#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
- dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride);
- Contact->depth = m_fBestDepth;
- SET(Contact->normal,m_vBestNormal);
- SET(Contact->pos,vPntTmp);
- Contact->g1 = Geom1;
- Contact->g2 = Geom2;
- Contact->side1 = TriIndex;
- Contact->side2 = -1;
- m_ctContacts++;
-#endif
- GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex,
- vPntTmp, m_vBestNormal, m_fBestDepth, m_ctContacts);
+ GenerateContact(TriIndex, vPntTmp, m_vBestNormal, m_fBestDepth);
// if triangle is the referent face then clip box to triangle face
@@ -904,20 +893,7 @@ void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v
dVector3 vPntTmp;
ADD(avTempArray2[i],v0,vPntTmp);
-#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
- dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride);
-
- Contact->depth = -fTempDepth;
- SET(Contact->normal,m_vBestNormal);
- SET(Contact->pos,vPntTmp);
- Contact->g1 = Geom1;
- Contact->g2 = Geom2;
- Contact->side1 = TriIndex;
- Contact->side2 = -1;
- m_ctContacts++;
-#endif
- GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex,
- vPntTmp, m_vBestNormal, -fTempDepth, m_ctContacts);
+ GenerateContact(TriIndex, vPntTmp, m_vBestNormal, -fTempDepth);
if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
break;
@@ -1028,20 +1004,7 @@ void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v
dVector3 vPntTmp;
ADD(avTempArray1[i],m_vHullBoxPos,vPntTmp);
-#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
- dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride);
-
- Contact->depth = -fTempDepth;
- SET(Contact->normal,m_vBestNormal);
- SET(Contact->pos,vPntTmp);
- Contact->g1 = Geom1;
- Contact->g2 = Geom2;
- Contact->side1 = TriIndex;
- Contact->side2 = -1;
- m_ctContacts++;
-#endif
- GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex,
- vPntTmp, m_vBestNormal, -fTempDepth, m_ctContacts);
+ GenerateContact(TriIndex, vPntTmp, m_vBestNormal, -fTempDepth);
if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
break;
@@ -1052,31 +1015,115 @@ void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v
}
}
+// GenerateContact - Written by Jeff Smith (jeff at burri.to)
+// Generate a "unique" contact. A unique contact has a unique
+// position or normal. If the potential contact has the same
+// position and normal as an existing contact, but a larger
+// penetration depth, this new depth is used instead
+//
+void sTrimeshBoxColliderData::GenerateContact(int TriIndex, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth)
+{
+ int TriCount = m_ctContacts;
+ do
+ {
+ dContactGeom* TgtContact = NULL;
+ bool deeper = false;
+ if (!(m_iFlags & CONTACTS_UNIMPORTANT))
+ {
+ dReal MinDepth = dInfinity;
+ dContactGeom* MinContact = NULL;
+ bool duplicate = false;
+ for (int i = 0; i < TriCount; i++)
+ {
+ dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, i, m_iStride);
-// test one mesh triangle on intersection with given box
-void sTrimeshBoxColliderData::_cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex)//, void *pvUser)
-{
- // do intersection test and find best separating axis
- if(!_cldTestSeparatingAxes(v0, v1, v2)) {
- // if not found do nothing
- return;
- }
+ // same position?
+ dVector3 diff;
+ dSubtractVectors3(diff, in_ContactPos, Contact->pos);
- // if best separation axis is not found
- if (m_iBestAxis == 0) {
- // this should not happen (we should already exit in that case)
- //dMessage (0, "best separation axis not found");
- // do nothing
- return;
- }
+ if (dCalcVectorDot3(diff, diff) < dEpsilon)
+ {
+ // same normal?
+ if (REAL(1.0) - dCalcVectorDot3(in_Normal, Contact->normal) < dEpsilon)
+ {
+ if (in_Depth > Contact->depth)
+ {
+ Contact->depth = in_Depth;
+ Contact->side1 = TriIndex;
+ }
- _cldClipping(v0, v1, v2, TriIndex);
+ duplicate = true;
+ break;
+ }
+ }
+
+ if (Contact->depth < MinDepth)
+ {
+ MinDepth = Contact->depth;
+ MinContact = Contact;
+ }
+ }
+ if (duplicate)
+ {
+ break;
+ }
+
+ if (TriCount == (m_iFlags & NUMC_MASK))
+ {
+ if (!(MinDepth < in_Depth))
+ {
+ break;
+ }
+
+ TgtContact = MinContact;
+ deeper = true;
+ }
+ }
+ else
+ {
+ dIASSERT(TriCount < (m_iFlags & NUMC_MASK));
+ }
+
+ if (!deeper)
+ {
+ // Add a new contact
+ TgtContact = SAFECONTACT(m_iFlags, m_ContactGeoms, TriCount, m_iStride);
+ TriCount++;
+
+ TgtContact->pos[3] = 0.0;
+
+ TgtContact->normal[3] = 0.0;
+
+ TgtContact->g1 = m_Geom1;
+ TgtContact->g2 = m_Geom2;
+
+ TgtContact->side2 = -1;
+ }
+
+ TgtContact->pos[0] = in_ContactPos[0];
+ TgtContact->pos[1] = in_ContactPos[1];
+ TgtContact->pos[2] = in_ContactPos[2];
+
+ TgtContact->normal[0] = in_Normal[0];
+ TgtContact->normal[1] = in_Normal[1];
+ TgtContact->normal[2] = in_Normal[2];
+
+ TgtContact->depth = in_Depth;
+
+ TgtContact->side1 = TriIndex;
+
+ m_ctContacts = TriCount;
+ }
+ while (false);
}
+
+
+
void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom,
int Flags, dContactGeom* Contacts, int Stride)
{
@@ -1114,82 +1161,82 @@ void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Bo
m_vBestNormal[2]=0;
}
-int sTrimeshBoxColliderData::TestCollisionForSingleTriangle(int ctContacts0, int Triint,
- dVector3 dv[3], bool &bOutFinishSearching)
+void sTrimeshBoxColliderData::TestCollisionForSingleTriangle(int Triint, dVector3 dv[3], bool &bOutFinishSearching)
{
+ bool finish = false;
+
// test this triangle
- _cldTestOneTriangle(dv[0],dv[1],dv[2],Triint);
+ if (_cldTestOneTriangle(dv[0], dv[1], dv[2], Triint))
+ {
+ /*
+ NOTE by Oleh_Derevenko:
+ The function continues checking triangles after maximal number
+ of contacts is reached because it selects maximal penetration depths.
+ See also comments in GenerateContact()
+ */
+ finish = ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)));
+ }
- // fill-in tri index for generated contacts
- for (; ctContacts0 < m_ctContacts; ctContacts0++) {
- dContactGeom* pContact = SAFECONTACT(m_iFlags, m_ContactGeoms, ctContacts0, m_iStride);
- pContact->side1 = Triint;
- pContact->side2 = -1;
+ bOutFinishSearching = finish;
+}
+
+// test one mesh triangle on intersection with given box
+bool sTrimeshBoxColliderData::_cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex)//, void *pvUser)
+{
+ // do intersection test and find best separating axis
+ if (!_cldTestSeparatingAxes(v0, v1, v2)) {
+ // if not found do nothing
+ return false;
}
- /*
- NOTE by Oleh_Derevenko:
- The function continues checking triangles after maximal number
- of contacts is reached because it selects maximal penetration depths.
- See also comments in GenerateContact()
- */
- bOutFinishSearching = ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)));
+ // if best separation axis is not found
+ if (m_iBestAxis == 0) {
+ // this should not happen (we should already exit in that case)
+ //dMessage (0, "best separation axis not found");
+ // do nothing
+ return false;
+ }
- return ctContacts0;
+ _cldClipping(v0, v1, v2, TriIndex);
+ return true;
}
+
// OPCODE version of box to mesh collider
#if dTRIMESH_OPCODE
static void dQueryBTLPotentialCollisionTriangles(OBBCollider &Collider,
const sTrimeshBoxColliderData &cData, dxTriMesh *TriMesh, dxGeom *BoxGeom,
OBBCache &BoxCache)
{
+ // get destination hull position and orientation
+ const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
+ const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+
+ Matrix4x4 MeshMatrix;
+ const dVector3 vZeroVector3 = { REAL(0.0), };
+ MakeMatrix(vZeroVector3, mRotMesh, MeshMatrix);
+
// get source hull position, orientation and half size
const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
+ dVector3 vOffsetPosBox;
+ dSubtractVectors3(vOffsetPosBox, vPosBox, vPosMesh);
+
// Make OBB
OBB Box;
- Box.mCenter.x = vPosBox[0];
- Box.mCenter.y = vPosBox[1];
- Box.mCenter.z = vPosBox[2];
-
- // It is a potential issue to explicitly cast to float
- // if custom width floating point type is introduced in OPCODE.
- // It is necessary to make a typedef and cast to it
- // (e.g. typedef float opc_float;)
- // However I'm not sure in what header it should be added.
-
- Box.mExtents.x = /*(float)*/cData.m_vBoxHalfSize[0];
- Box.mExtents.y = /*(float)*/cData.m_vBoxHalfSize[1];
- Box.mExtents.z = /*(float)*/cData.m_vBoxHalfSize[2];
-
- Box.mRot.m[0][0] = /*(float)*/mRotBox[0];
- Box.mRot.m[1][0] = /*(float)*/mRotBox[1];
- Box.mRot.m[2][0] = /*(float)*/mRotBox[2];
-
- Box.mRot.m[0][1] = /*(float)*/mRotBox[4];
- Box.mRot.m[1][1] = /*(float)*/mRotBox[5];
- Box.mRot.m[2][1] = /*(float)*/mRotBox[6];
-
- Box.mRot.m[0][2] = /*(float)*/mRotBox[8];
- Box.mRot.m[1][2] = /*(float)*/mRotBox[9];
- Box.mRot.m[2][2] = /*(float)*/mRotBox[10];
-
- Matrix4x4 amatrix;
- Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix);
-
- Matrix4x4 InvBoxMatrix;
- InvertPRMatrix(InvBoxMatrix, BoxMatrix);
-
- // get destination hull position and orientation
- const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
- const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+ Box.mCenter.Set(vOffsetPosBox[0], vOffsetPosBox[1], vOffsetPosBox[2]);
+ Box.mExtents.Set(cData.m_vBoxHalfSize[0], cData.m_vBoxHalfSize[1], cData.m_vBoxHalfSize[2]);
+ Box.mRot.Set(
+ mRotBox[0], mRotBox[4], mRotBox[8],
+ mRotBox[1], mRotBox[5], mRotBox[9],
+ mRotBox[2], mRotBox[6], mRotBox[10]);
// TC results
if (TriMesh->doBoxTC) {
dxTriMesh::BoxTC* BoxTC = 0;
- for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
+ const int iBoxCacheSize = TriMesh->BoxTCCache.size();
+ for (int i = 0; i != iBoxCacheSize; i++){
if (TriMesh->BoxTCCache[i].Geom == BoxGeom){
BoxTC = &TriMesh->BoxTCCache[i];
break;
@@ -1205,12 +1252,11 @@ static void dQueryBTLPotentialCollisionTriangles(OBBCollider &Collider,
// Intersect
Collider.SetTemporalCoherence(true);
- Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix));
+ Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MeshMatrix);
}
else {
Collider.SetTemporalCoherence(false);
- Collider.Collide(BoxCache, Box, TriMesh->Data->BVTree, null,
- &MakeMatrix(vPosMesh, mRotMesh, amatrix));
+ Collider.Collide(BoxCache, Box, TriMesh->Data->BVTree, null, &MeshMatrix);
}
}
@@ -1251,8 +1297,6 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
- int ctContacts0 = 0;
-
// loop through all intersecting triangles
for (int i = 0; i < TriCount; i++){
const int Triint = Triangles[i];
@@ -1262,7 +1306,7 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv);
bool bFinishSearching;
- ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching);
+ cData.TestCollisionForSingleTriangle(Triint, dv, bFinishSearching);
if (bFinishSearching) {
break;
@@ -1322,8 +1366,6 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
gim_trimesh_locks_work_data(ptrimesh);
- int ctContacts0 = 0;
-
for(unsigned int i=0;i<collision_result.m_size;i++)
{
dVector3 dv[3];
@@ -1332,7 +1374,7 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
gim_trimesh_get_triangle_vertices(ptrimesh, Triint, dv[0], dv[1], dv[2]);
bool bFinishSearching;
- ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching);
+ cData.TestCollisionForSingleTriangle(Triint, dv, bFinishSearching);
if (bFinishSearching)
{
@@ -1348,101 +1390,5 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts,
#endif
-// GenerateContact - Written by Jeff Smith (jeff at burri.to)
-// Generate a "unique" contact. A unique contact has a unique
-// position or normal. If the potential contact has the same
-// position and normal as an existing contact, but a larger
-// penetration depth, this new depth is used instead
-//
-static void
-GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
- dxGeom* in_g1, dxGeom* in_g2, int TriIndex,
- const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
- int& OutTriCount)
-{
- /*
- NOTE by Oleh_Derevenko:
- This function is called after maximal number of contacts has already been
- collected because it has a side effect of replacing penetration depth of
- existing contact with larger penetration depth of another matching normal contact.
- If this logic is not necessary any more, you can bail out on reach of contact
- number maximum immediately in dCollideBTL(). You will also need to correct
- conditional statements after invocations of GenerateContact() in _cldClipping().
- */
- do
- {
- dContactGeom* Contact;
- dVector3 diff;
-
- if (!(in_Flags & CONTACTS_UNIMPORTANT))
- {
- bool duplicate = false;
- for (int i=0; i<OutTriCount; i++)
- {
- Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
-
- // same position?
- for (int j=0; j<3; j++)
- diff[j] = in_ContactPos[j] - Contact->pos[j];
- if (dCalcVectorDot3(diff, diff) < dEpsilon)
- {
- // same normal?
- if (REAL(1.0) - dFabs(dCalcVectorDot3(in_Normal, Contact->normal)) < dEpsilon)
- {
- if (in_Depth > Contact->depth)
- Contact->depth = in_Depth;
- duplicate = true;
- /*
- NOTE by Oleh_Derevenko:
- There may be a case when two normals are close to each other but not duplicate
- while third normal is detected to be duplicate for both of them.
- This is the only reason I can think of, there is no "break" statement.
- Perhaps author considered it to be logical that the third normal would
- replace the depth in both of initial contacts.
- However, I consider it a questionable practice which should not
- be applied without deep understanding of underlaying physics.
- Even more, is this situation with close normal triplet acceptable at all?
- Should not be two initial contacts reduced to one (replaced with the latter)?
- If you know the answers for these questions, you may want to change this code.
- See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp
- */
- }
- }
- }
- if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
- {
- break;
- }
- }
- else
- {
- dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
- }
-
- // Add a new contact
- Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
-
- Contact->pos[0] = in_ContactPos[0];
- Contact->pos[1] = in_ContactPos[1];
- Contact->pos[2] = in_ContactPos[2];
- Contact->pos[3] = 0.0;
-
- Contact->normal[0] = in_Normal[0];
- Contact->normal[1] = in_Normal[1];
- Contact->normal[2] = in_Normal[2];
- Contact->normal[3] = 0.0;
-
- Contact->depth = in_Depth;
-
- Contact->g1 = in_g1;
- Contact->g2 = in_g2;
-
- Contact->side1 = TriIndex;
- Contact->side2 = -1;
-
- OutTriCount++;
- }
- while (false);
-}
#endif // dTRIMESH_ENABLED
diff --git a/ode/src/collision_trimesh_ccylinder.cpp b/ode/src/collision_trimesh_ccylinder.cpp
index 5e15637..de1fb83 100644
--- a/ode/src/collision_trimesh_ccylinder.cpp
+++ b/ode/src/collision_trimesh_ccylinder.cpp
@@ -167,7 +167,7 @@ struct sTrimeshCapsuleColliderData
// mesh data
// dMatrix4 mHullDstPl;
dMatrix3 m_mTriMeshRot;
- dVector3 m_mTriMeshPos;
+ dVector3 m_vTriMeshPos;
dVector3 m_vE0, m_vE1, m_vE2;
// global collider data
@@ -930,7 +930,7 @@ void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom
memcpy(m_mTriMeshRot, pTriRot, sizeof(dMatrix3));
const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
- memcpy(m_mTriMeshPos, pTriPos, sizeof(dVector3));
+ memcpy(m_vTriMeshPos, pTriPos, sizeof(dVector3));
// global info for contact creation
m_iStride =skip;
@@ -971,45 +971,34 @@ static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
const sTrimeshCapsuleColliderData &cData, dxTriMesh *TriMesh, dxGeom *Capsule,
OBBCache &BoxCache)
{
- // It is a potential issue to explicitly cast to float
- // if custom width floating point type is introduced in OPCODE.
- // It is necessary to make a typedef and cast to it
- // (e.g. typedef float opc_float;)
- // However I'm not sure in what header it should be added.
-
- const dVector3 &vCapsulePosition = cData.m_vCapsulePosition;
-
- Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]);
- Point cExtents(/*(float)*/ cData.m_vCapsuleRadius, /*(float)*/ cData.m_vCapsuleRadius,/*(float)*/ cData.m_fCapsuleSize/2);
-
- Matrix3x3 obbRot;
-
- const dMatrix3 &mCapsuleRotation = cData.m_mCapsuleRotation;
-
- obbRot[0][0] = /*(float)*/ mCapsuleRotation[0];
- obbRot[1][0] = /*(float)*/ mCapsuleRotation[1];
- obbRot[2][0] = /*(float)*/ mCapsuleRotation[2];
-
- obbRot[0][1] = /*(float)*/ mCapsuleRotation[4];
- obbRot[1][1] = /*(float)*/ mCapsuleRotation[5];
- obbRot[2][1] = /*(float)*/ mCapsuleRotation[6];
+ Matrix4x4 MeshMatrix;
+ const dVector3 vZeroVector3 = { REAL(0.0), };
+ MakeMatrix(vZeroVector3, cData.m_mTriMeshRot, MeshMatrix);
- obbRot[0][2] = /*(float)*/ mCapsuleRotation[8];
- obbRot[1][2] = /*(float)*/ mCapsuleRotation[9];
- obbRot[2][2] = /*(float)*/ mCapsuleRotation[10];
+ const dVector3 &vCapsulePos = cData.m_vCapsulePosition;
+ const dMatrix3 &mCapsuleRot = cData.m_mCapsuleRotation;
- OBB obbCapsule(cCenter,cExtents,obbRot);
+ dVector3 vCapsuleOffsetPos;
+ dSubtractVectors3(vCapsuleOffsetPos, vCapsulePos, cData.m_vTriMeshPos);
- Matrix4x4 CapsuleMatrix;
- MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix);
+ const dReal fCapsuleRadius = cData.m_vCapsuleRadius, fCapsuleHalfAxis = cData.m_fCapsuleSize * REAL(0.5);
- Matrix4x4 MeshMatrix;
- MakeMatrix(cData.m_mTriMeshPos, cData.m_mTriMeshRot, MeshMatrix);
+ OBB obbCapsule;
+ obbCapsule.mCenter.Set(vCapsuleOffsetPos[0], vCapsuleOffsetPos[1], vCapsuleOffsetPos[2]);
+ obbCapsule.mExtents.Set(
+ 0 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius,
+ 1 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius,
+ 2 == nCAPSULE_AXIS ? fCapsuleHalfAxis : fCapsuleRadius);
+ obbCapsule.mRot.Set(
+ mCapsuleRot[0], mCapsuleRot[4], mCapsuleRot[8],
+ mCapsuleRot[1], mCapsuleRot[5], mCapsuleRot[9],
+ mCapsuleRot[2], mCapsuleRot[6], mCapsuleRot[10]);
// TC results
if (TriMesh->doBoxTC) {
dxTriMesh::BoxTC* BoxTC = 0;
- for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
+ const int iBoxCacheSize = TriMesh->BoxTCCache.size();
+ for (int i = 0; i != iBoxCacheSize; i++){
if (TriMesh->BoxTCCache[i].Geom == Capsule){
BoxTC = &TriMesh->BoxTCCache[i];
break;
@@ -1029,7 +1018,7 @@ static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
}
else {
Collider.SetTemporalCoherence(false);
- Collider.Collide(BoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix);
+ Collider.Collide(BoxCache, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix);
}
}
@@ -1087,7 +1076,7 @@ int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int s
if (!Callback(TriMesh, Capsule, Triint)) continue;
dVector3 dv[3];
- FetchTriangle(TriMesh, Triint, cData.m_mTriMeshPos, cData.m_mTriMeshRot, dv);
+ FetchTriangle(TriMesh, Triint, cData.m_vTriMeshPos, cData.m_mTriMeshRot, dv);
uint8 flags = UseFlags ? UseFlags[Triint] : (uint8)dxTriMeshData::kUseAll;
@@ -1167,39 +1156,10 @@ int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int s
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
-
unsigned contactcount = trimeshcontacts.m_size;
- unsigned contactmax = (unsigned)(flags & NUMC_MASK);
- if (contactcount > contactmax)
- {
- contactcount = contactmax;
- }
-
- dContactGeom* pcontact;
- unsigned i;
-
- for (i=0;i<contactcount;i++)
- {
- pcontact = SAFECONTACT(flags, contact, i, skip);
- pcontact->pos[0] = ptrimeshcontacts->m_point[0];
- pcontact->pos[1] = ptrimeshcontacts->m_point[1];
- pcontact->pos[2] = ptrimeshcontacts->m_point[2];
- pcontact->pos[3] = 1.0f;
-
- pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
- pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
- pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
- pcontact->normal[3] = 0;
-
- pcontact->depth = ptrimeshcontacts->m_depth;
- pcontact->g1 = TriMesh;
- pcontact->g2 = gCylinder;
- pcontact->side1 = ptrimeshcontacts->m_feature1;
- pcontact->side2 = -1;
-
- ptrimeshcontacts++;
- }
+ dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, TriMesh, gCylinder, -1);
+ contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contact, skip);
GIM_DYNARRAY_DESTROY(trimeshcontacts);
diff --git a/ode/src/collision_trimesh_colliders.h b/ode/src/collision_trimesh_colliders.h
index a800df8..9452f90 100644
--- a/ode/src/collision_trimesh_colliders.h
+++ b/ode/src/collision_trimesh_colliders.h
@@ -32,6 +32,7 @@ int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int sk
int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideConvexTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
ODE_PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags,
dContactGeom *contact, int skip )
diff --git a/ode/src/collision_trimesh_disabled.cpp b/ode/src/collision_trimesh_disabled.cpp
index fa3c493..93d0d94 100644
--- a/ode/src/collision_trimesh_disabled.cpp
+++ b/ode/src/collision_trimesh_disabled.cpp
@@ -37,7 +37,6 @@ bool dxTriMesh::controlGeometry(int controlClass, int controlCode, void *dataVal
return dxGeom::controlGeometry(controlClass, controlCode, dataValue, dataSize);
}
-int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]) { return 0; }
void dxTriMesh::computeAABB() { dSetZero (aabb,6); }
static dMatrix4 identity = {
diff --git a/ode/src/collision_trimesh_gimpact.cpp b/ode/src/collision_trimesh_gimpact.cpp
index 4e7204f..9dd7f08 100644
--- a/ode/src/collision_trimesh_gimpact.cpp
+++ b/ode/src/collision_trimesh_gimpact.cpp
@@ -465,40 +465,10 @@ int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int S
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
-
-
unsigned contactcount = trimeshcontacts.m_size;
- unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
- if (contactcount > maxcontacts)
- {
- contactcount = maxcontacts;
- }
-
- dContactGeom* pcontact;
- unsigned i;
- for (i=0;i<contactcount;i++)
- {
- pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
-
- pcontact->pos[0] = ptrimeshcontacts->m_point[0];
- pcontact->pos[1] = ptrimeshcontacts->m_point[1];
- pcontact->pos[2] = ptrimeshcontacts->m_point[2];
- pcontact->pos[3] = 1.0f;
-
- pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
- pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
- pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
- pcontact->normal[3] = 0;
-
- pcontact->depth = ptrimeshcontacts->m_depth;
- pcontact->g1 = g1;
- pcontact->g2 = g2;
- pcontact->side1 = ptrimeshcontacts->m_feature1;
- pcontact->side2 = ptrimeshcontacts->m_feature2;
-
- ptrimeshcontacts++;
- }
+ dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, g1, g2);
+ contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, Flags, Contacts, Stride);
GIM_DYNARRAY_DESTROY(trimeshcontacts);
diff --git a/ode/src/collision_trimesh_internal.h b/ode/src/collision_trimesh_internal.h
index 15e79cc..66563b9 100644
--- a/ode/src/collision_trimesh_internal.h
+++ b/ode/src/collision_trimesh_internal.h
@@ -28,12 +28,14 @@
#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
#define _ODE_COLLISION_TRIMESH_INTERNAL_H_
+
//****************************************************************************
// dxTriMesh class
#include "collision_kernel.h"
#include "collision_trimesh_colliders.h"
+#include "collision_util.h"
#include <ode/collision_trimesh.h>
#if dTRIMESH_OPCODE
@@ -50,7 +52,11 @@ using namespace Opcode;
#include "odetls.h"
#endif
+#include "util.h"
+#ifndef ALLOCA
+#define ALLOCA(x) dALLOCA16(x)
+#endif
#if dTRIMESH_OPCODE
@@ -179,7 +185,8 @@ struct TrimeshCollidersCache
#if dTLS_ENABLED
-inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
+static inline
+TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
{
EODETLSKIND tkTLSKind = (EODETLSKIND)uiTLSKind;
return COdeTls::GetTrimeshCollidersCache(tkTLSKind);
@@ -188,7 +195,8 @@ inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
#else // dTLS_ENABLED
-inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
+static inline
+TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
{
(void)uiTLSKind; // unused
@@ -376,23 +384,17 @@ struct dxTriMesh : public dxGeom{
#endif // dTRIMESH_GIMPACT
};
-#if 0
-#include "collision_kernel.h"
-// Fetches a contact
-inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
- dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
- return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
-}
-#endif
#if dTRIMESH_OPCODE
-inline unsigned FetchTriangleCount(dxTriMesh* TriMesh)
+static inline
+unsigned FetchTriangleCount(dxTriMesh* TriMesh)
{
return TriMesh->Data->Mesh.GetNbTriangles();
}
-inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
+static inline
+void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
VertexPointers VP;
ConversionArea VC;
TriMesh->Data->Mesh.GetTriangle(VP, Index, VC);
@@ -411,44 +413,31 @@ inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position
}
}
-inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
+static inline
+void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
FetchTriangle(TriMesh, Index, Position, Rotation, Out);
}
-inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
- Out.m[0][0] = (float) Rotation[0];
- Out.m[1][0] = (float) Rotation[1];
- Out.m[2][0] = (float) Rotation[2];
-
- Out.m[0][1] = (float) Rotation[4];
- Out.m[1][1] = (float) Rotation[5];
- Out.m[2][1] = (float) Rotation[6];
-
- Out.m[0][2] = (float) Rotation[8];
- Out.m[1][2] = (float) Rotation[9];
- Out.m[2][2] = (float) Rotation[10];
-
- Out.m[3][0] = (float) Position[0];
- Out.m[3][1] = (float) Position[1];
- Out.m[3][2] = (float) Position[2];
-
- Out.m[0][3] = 0.0f;
- Out.m[1][3] = 0.0f;
- Out.m[2][3] = 0.0f;
- Out.m[3][3] = 1.0f;
-
- return Out;
+static inline
+Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
+ return Out.Set(
+ Rotation[0], Rotation[4], Rotation[8], 0.0f,
+ Rotation[1], Rotation[5], Rotation[9], 0.0f,
+ Rotation[2], Rotation[6], Rotation[10],0.0f,
+ Position[0], Position[1], Position[2], 1.0f);
}
-inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
+static inline
+Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
return MakeMatrix(Position, Rotation, Out);
}
#endif // dTRIMESH_OPCODE
+
#if dTRIMESH_GIMPACT
#ifdef dDOUBLE
@@ -464,7 +453,8 @@ inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
(b)[3] = 0; \
}
-inline void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3) {
+static inline
+void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3) {
vec3f src1, src2, src3;
gim_trimesh_get_triangle_vertices(trimesh, triangle_index, src1, src2, src3);
@@ -478,14 +468,16 @@ inline void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32
#define gim_trimesh_get_triangle_vertices gim_trimesh_get_triangle_verticesODE
-inline int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
+static inline
+int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
vec3f dir_vec3f = { dir[ 0 ], dir[ 1 ], dir[ 2 ] };
vec3f origin_vec3f = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
return gim_trimesh_ray_closest_collision( mesh, origin_vec3f, dir_vec3f, tmax, contact );
}
-inline int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
+static inline
+int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
vec3f dir_vec3f = { dir[ 0 ], dir[ 1 ], dir[ 2 ] };
vec3f origin_vec3f = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
@@ -523,18 +515,21 @@ inline int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVe
#endif // dDouble
-inline unsigned FetchTriangleCount(dxTriMesh* TriMesh)
+static inline
+unsigned FetchTriangleCount(dxTriMesh* TriMesh)
{
return gim_trimesh_get_triangle_count(&TriMesh->m_collision_trimesh);
}
-inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
+static inline
+void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
gim_trimesh_locks_work_data(&TriMesh->m_collision_trimesh);
gim_trimesh_get_triangle_vertices(&TriMesh->m_collision_trimesh, (GUINT32)Index, Out[0], Out[1], Out[2]);
gim_trimesh_unlocks_work_data(&TriMesh->m_collision_trimesh);
}
-inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
+static inline
+void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
{
m[0][0] = (float) Rotation[0];
m[0][1] = (float) Rotation[1];
@@ -554,15 +549,269 @@ inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m
}
-inline void MakeMatrix(dxGeom* g, mat4f Out){
+static inline
+void MakeMatrix(dxGeom* g, mat4f Out){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
MakeMatrix(Position, Rotation, Out);
}
+
+
+struct dxGIMCContactAccessor
+{
+ dxGIMCContactAccessor(GIM_CONTACT *ptrimeshcontacts, dGeomID g1, dGeomID g2) : m_ptrimeshcontacts(ptrimeshcontacts), m_g1(g1), m_g2(g2), m_gotside2ovr(false), m_side2ovr() {}
+ dxGIMCContactAccessor(GIM_CONTACT *ptrimeshcontacts, dGeomID g1, dGeomID g2, int side2ovr) : m_ptrimeshcontacts(ptrimeshcontacts), m_g1(g1), m_g2(g2), m_gotside2ovr(true), m_side2ovr(side2ovr) {}
+
+ dReal RetrieveDepthByIndex(unsigned index) const { return m_ptrimeshcontacts[index].m_depth; }
+
+ void ExportContactGeomByIndex(dContactGeom *pcontact, unsigned index) const
+ {
+ const GIM_CONTACT *ptrimeshcontact = m_ptrimeshcontacts + index;
+ pcontact->pos[0] = ptrimeshcontact->m_point[0];
+ pcontact->pos[1] = ptrimeshcontact->m_point[1];
+ pcontact->pos[2] = ptrimeshcontact->m_point[2];
+ pcontact->pos[3] = REAL(1.0);
+
+ pcontact->normal[0] = ptrimeshcontact->m_normal[0];
+ pcontact->normal[1] = ptrimeshcontact->m_normal[1];
+ pcontact->normal[2] = ptrimeshcontact->m_normal[2];
+ pcontact->normal[3] = 0;
+
+ pcontact->depth = ptrimeshcontact->m_depth;
+ pcontact->g1 = m_g1;
+ pcontact->g2 = m_g2;
+ pcontact->side1 = ptrimeshcontact->m_feature1;
+ pcontact->side2 = !m_gotside2ovr ? ptrimeshcontact->m_feature2 : m_side2ovr;
+ }
+
+ const GIM_CONTACT *m_ptrimeshcontacts;
+ dGeomID m_g1, m_g2;
+ bool m_gotside2ovr;
+ int m_side2ovr;
+};
+
+struct dxPlaneContactAccessor
+{
+ dxPlaneContactAccessor(const vec4f *planecontact_results, const dReal *plane, dGeomID g1, dGeomID g2) : m_planecontact_results(planecontact_results), m_plane(plane), m_g1(g1), m_g2(g2) {}
+
+ dReal RetrieveDepthByIndex(unsigned index) const { return m_planecontact_results[index][3]; }
+
+ void ExportContactGeomByIndex(dContactGeom *pcontact, unsigned index) const
+ {
+ const vec4f *planecontact = m_planecontact_results + index;
+
+ pcontact->pos[0] = (*planecontact)[0];
+ pcontact->pos[1] = (*planecontact)[1];
+ pcontact->pos[2] = (*planecontact)[2];
+ pcontact->pos[3] = REAL(1.0);
+
+ const dReal *plane = m_plane;
+ pcontact->normal[0] = plane[0];
+ pcontact->normal[1] = plane[1];
+ pcontact->normal[2] = plane[2];
+ pcontact->normal[3] = 0;
+
+ pcontact->depth = (*planecontact)[3];
+ pcontact->g1 = m_g1; // trimesh geom
+ pcontact->g2 = m_g2; // plane geom
+ pcontact->side1 = -1; // note: don't have the triangle index, but OPCODE *does* do this properly
+ pcontact->side2 = -1;
+ }
+
+ const vec4f *m_planecontact_results;
+ const dReal *m_plane;
+ dGeomID m_g1, m_g2;
+};
+
+struct dxGImpactContactsExportHelper
+{
+public:
+ template<class dxGImpactContactAccessor>
+ static unsigned ExportMaxDepthGImpactContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+ int Flags, dContactGeom* Contacts, int Stride)
+ {
+ unsigned result;
+
+ unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
+ if (contactcount > maxcontacts)
+ {
+ ExportExcesssiveContacts(srccontacts, contactcount, Flags, Contacts, Stride);
+ result = maxcontacts;
+ }
+ else
+ {
+ ExportFitContacts(srccontacts, contactcount, Flags, Contacts, Stride);
+ result = contactcount;
+ }
+
+ return result;
+ }
+
+private:
+ template<class dxGImpactContactAccessor>
+ static void ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+ int Flags, dContactGeom* Contacts, int Stride)
+ {
+ unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
+ dReal marginaldepth = FindContactsMarginalDepth(srccontacts, contactcount, maxcontacts);
+
+ unsigned contactshead = 0, contacttail = maxcontacts;
+ for (unsigned i = 0; i < contactcount; i++)
+ {
+ dReal depth = srccontacts.RetrieveDepthByIndex(i);
+
+ if (depth > marginaldepth)
+ {
+ dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contactshead, Stride);
+ srccontacts.ExportContactGeomByIndex(pcontact, i);
+
+ if (++contactshead == maxcontacts)
+ {
+ break;
+ }
+ }
+ else if (depth == marginaldepth && contactshead < contacttail)
+ {
+ --contacttail;
+
+ dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contacttail, Stride);
+ srccontacts.ExportContactGeomByIndex(pcontact, i);
+ }
+ }
+ }
+
+ template<class dxGImpactContactAccessor>
+ static void ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+ int Flags, dContactGeom* Contacts, int Stride)
+ {
+ for (unsigned i = 0; i < contactcount; i++)
+ {
+ dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+ srccontacts.ExportContactGeomByIndex(pcontact, i);
+ }
+ }
+
+ template<class dxGImpactContactAccessor>
+ static dReal FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts)
+ {
+ dReal result;
+
+ dReal *pdepths = (dReal *)ALLOCA(contactcount * sizeof(dReal));
+ unsigned marginindex = 0;
+ unsigned highindex = marginindex;
+
+ dReal firstdepth = srccontacts.RetrieveDepthByIndex(0);
+ dReal mindepth = firstdepth, maxdepth = firstdepth;
+ dIASSERT(contactcount > 1);
+
+ for (unsigned i = 1; i < contactcount; i++)
+ {
+ dReal depth = srccontacts.RetrieveDepthByIndex(i);
+
+ if (depth < firstdepth)
+ {
+ dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth;
+ if (depth < mindepth) { mindepth = depth; }
+ }
+ else if (depth > firstdepth)
+ {
+ pdepths[highindex++] = depth;
+ if (maxdepth < depth) { maxdepth = depth; }
+ }
+ }
+
+ unsigned countabove = highindex - marginindex;
+ if (maxcontacts < countabove)
+ {
+ result = FindContactsMarginalDepth(pdepths + marginindex, countabove, maxcontacts, firstdepth, maxdepth);
+ }
+ else if (maxcontacts == countabove)
+ {
+ result = dNextAfter(firstdepth, dInfinity);
+ }
+ else
+ {
+ unsigned countbelow = marginindex;
+ if (maxcontacts <= contactcount - countbelow)
+ {
+ result = firstdepth;
+ }
+ else
+ {
+ result = FindContactsMarginalDepth(pdepths, countbelow, maxcontacts - (contactcount - countbelow), mindepth, firstdepth);
+ }
+ }
+
+ return result;
+ }
+
+ static dReal FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth)
+ {
+ dReal result;
+
+ while (true)
+ {
+ dReal firstdepth = 0.5 * (mindepth + maxdepth);
+ dReal lowdepth = maxdepth, highdepth = mindepth;
+
+ unsigned marginindex = 0;
+ unsigned highindex = marginindex;
+ dIASSERT(contactcount != 0);
+
+ for (unsigned i = 0; i < contactcount; i++)
+ {
+ dReal depth = pdepths[i];
+
+ if (depth < firstdepth)
+ {
+ dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth;
+ if (highdepth < depth) { highdepth = depth; }
+ }
+ else if (depth > firstdepth)
+ {
+ pdepths[highindex++] = depth;
+ if (depth < lowdepth) { lowdepth = depth; }
+ }
+ }
+
+ unsigned countabove = highindex - marginindex;
+ if (maxcontacts < countabove)
+ {
+ contactcount = countabove;
+ pdepths += marginindex;
+ mindepth = lowdepth;
+ }
+ else if (maxcontacts == countabove)
+ {
+ result = dNextAfter(firstdepth, dInfinity);
+ break;
+ }
+ else
+ {
+ unsigned countbelow = marginindex;
+ if (maxcontacts <= contactcount - countbelow)
+ {
+ result = firstdepth;
+ break;
+ }
+
+ maxcontacts -= contactcount - countbelow;
+ contactcount = countbelow;
+ maxdepth = highdepth;
+ }
+ }
+
+ return result;
+ }
+};
+
+
#endif // dTRIMESH_GIMPACT
// Outputs a matrix to 3 vectors
-inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
+static inline
+void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
Right[0] = Matrix[0 * 4 + 0];
Right[1] = Matrix[1 * 4 + 0];
Right[2] = Matrix[2 * 4 + 0];
@@ -578,12 +827,14 @@ inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVecto
}
// Outputs a matrix to 3 vectors
-inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
+static inline
+void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
}
// Finds barycentric
-inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
+static inline
+void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
dReal w = REAL(1.0) - u - v;
Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
@@ -593,7 +844,8 @@ inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVec
}
// Performs a callback
-inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
+static
+bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
if (TriMesh->Callback != NULL){
return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0);
}
@@ -622,7 +874,7 @@ dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd,
const dVector3 triEdge1, const dVector3 triEdge2,
dReal* t = 0, dReal* u = 0, dReal* v = 0 );
-inline
+static inline
void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
{
result[0] = left[0] - right[0];
@@ -631,7 +883,7 @@ void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result
result[3] = REAL(0.0);
}
-inline
+static inline
void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
{
result[0] = left[0] + right[0];
@@ -640,7 +892,7 @@ void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
result[3] = REAL(0.0);
}
-inline
+static inline
void Vector3Negate( const dVector3 in, dVector3 out )
{
out[0] = -in[0];
@@ -649,7 +901,7 @@ void Vector3Negate( const dVector3 in, dVector3 out )
out[3] = REAL(0.0);
}
-inline
+static inline
void Vector3Copy( const dVector3 in, dVector3 out )
{
out[0] = in[0];
@@ -658,7 +910,7 @@ void Vector3Copy( const dVector3 in, dVector3 out )
out[3] = REAL(0.0);
}
-inline
+static inline
void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
{
out[0] = in[0] * scalar;
@@ -667,7 +919,7 @@ void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
out[3] = REAL(0.0);
}
-inline
+static inline
void TransformVector3( const dVector3 in,
const dMatrix3 orientation, const dVector3 position,
dVector3 out )
@@ -697,7 +949,7 @@ void TransformVector3( const dVector3 in,
closest to the capsule (where the distance between the two shapes
is the shortest).
*/
-inline
+static inline
bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd,
const dReal radius, const dVector3 triOrigin,
const dVector3 triEdge0, const dVector3 triEdge1,
diff --git a/ode/src/collision_trimesh_plane.cpp b/ode/src/collision_trimesh_plane.cpp
index 708d932..0615538 100644
--- a/ode/src/collision_trimesh_plane.cpp
+++ b/ode/src/collision_trimesh_plane.cpp
@@ -196,38 +196,11 @@ int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* conta
}
+ vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f, collision_result);
unsigned int contactcount = collision_result.m_size;
- unsigned int contactmax = (unsigned int)(flags & NUMC_MASK);
- if (contactcount > contactmax)
- {
- contactcount = contactmax;
- }
-
- dContactGeom* pcontact;
- vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result);
-
- for(unsigned int i = 0; i < contactcount; i++ )
- {
- pcontact = SAFECONTACT(flags, contacts, i, skip);
-
- pcontact->pos[0] = (*planecontact_results)[0];
- pcontact->pos[1] = (*planecontact_results)[1];
- pcontact->pos[2] = (*planecontact_results)[2];
- pcontact->pos[3] = REAL(1.0);
-
- pcontact->normal[0] = plane[0];
- pcontact->normal[1] = plane[1];
- pcontact->normal[2] = plane[2];
- pcontact->normal[3] = 0;
-
- pcontact->depth = (*planecontact_results)[3];
- pcontact->g1 = o1; // trimesh geom
- pcontact->g2 = o2; // plane geom
- pcontact->side1 = -1; // note: don't have the triangle index, but OPCODE *does* do this properly
- pcontact->side2 = -1;
-
- planecontact_results++;
- }
+
+ dxPlaneContactAccessor contactaccessor(planecontact_results, plane, o1, o2);
+ contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contacts, skip);
GIM_DYNARRAY_DESTROY(collision_result);
diff --git a/ode/src/collision_trimesh_ray.cpp b/ode/src/collision_trimesh_ray.cpp
index 7ae1653..27d6ee1 100644
--- a/ode/src/collision_trimesh_ray.cpp
+++ b/ode/src/collision_trimesh_ray.cpp
@@ -42,9 +42,6 @@ int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts,
dxTriMesh* TriMesh = (dxTriMesh*)g1;
- const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
- const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
-
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
@@ -61,22 +58,27 @@ int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts,
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
+ const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+ const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+ Matrix4x4 MeshMatrix;
+ const dVector3 ZeroVector3 = { REAL(0.0), };
+ MakeMatrix(ZeroVector3, TLRotation, MeshMatrix);
+
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
+ dVector3 OffsetOrigin;
+ dSubtractVectors3(OffsetOrigin, Origin, TLPosition);
+
/* Make Ray */
Ray WorldRay;
- WorldRay.mOrig.x = Origin[0];
- WorldRay.mOrig.y = Origin[1];
- WorldRay.mOrig.z = Origin[2];
- WorldRay.mDir.x = Direction[0];
- WorldRay.mDir.y = Direction[1];
- WorldRay.mDir.z = Direction[2];
+ WorldRay.mOrig.Set(OffsetOrigin[0], OffsetOrigin[1], OffsetOrigin[2]);
+ WorldRay.mDir.Set(Direction[0], Direction[1], Direction[2]);
/* Intersect */
- Matrix4x4 amatrix;
int TriCount = 0;
- if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
+ if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MeshMatrix)) {
TriCount = pccColliderCache->Faces.GetNbFaces();
}
diff --git a/ode/src/collision_trimesh_sphere.cpp b/ode/src/collision_trimesh_sphere.cpp
index 5a9a678..1170884 100644
--- a/ode/src/collision_trimesh_sphere.cpp
+++ b/ode/src/collision_trimesh_sphere.cpp
@@ -249,38 +249,42 @@ int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contact
dxTriMesh* TriMesh = (dxTriMesh*)g1;
- // Init
- const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
- const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
-
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
SphereCollider& Collider = pccColliderCache->_SphereCollider;
+ const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+ const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+ Matrix4x4 MeshMatrix;
+ const dVector3 ZeroVector3 = { REAL(0.0), };
+ MakeMatrix(ZeroVector3, TLRotation, MeshMatrix);
+
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
+ dVector3 OffsetPosition;
+ dSubtractVectors3(OffsetPosition, Position, TLPosition);
+
// Sphere
Sphere Sphere;
- Sphere.mCenter.x = Position[0];
- Sphere.mCenter.y = Position[1];
- Sphere.mCenter.z = Position[2];
+ Sphere.mCenter.Set(OffsetPosition[0], OffsetPosition[1], OffsetPosition[2]);
Sphere.mRadius = Radius;
- Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
- for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
+ const int sphereCacheSize = TriMesh->SphereTCCache.size();
+ for (int i = 0; i != sphereCacheSize; i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
- if (!sphereTC){
+ if (!sphereTC) {
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
@@ -289,13 +293,11 @@ int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contact
// Intersect
Collider.SetTemporalCoherence(true);
- Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
- &MakeMatrix(TLPosition, TLRotation, amatrix));
+ Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, &MeshMatrix);
}
else {
Collider.SetTemporalCoherence(false);
- Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
- &MakeMatrix(TLPosition, TLRotation, amatrix));
+ Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, &MeshMatrix);
}
if (! Collider.GetContactStatus()) {
@@ -566,39 +568,10 @@ int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contact
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
-
unsigned contactcount = trimeshcontacts.m_size;
- unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
- if (contactcount > maxcontacts)
- {
- contactcount = maxcontacts;
- }
-
- dContactGeom* pcontact;
- unsigned i;
-
- for (i=0;i<contactcount;i++)
- {
- pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
- pcontact->pos[0] = ptrimeshcontacts->m_point[0];
- pcontact->pos[1] = ptrimeshcontacts->m_point[1];
- pcontact->pos[2] = ptrimeshcontacts->m_point[2];
- pcontact->pos[3] = REAL(1.0);
-
- pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
- pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
- pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
- pcontact->normal[3] = 0;
-
- pcontact->depth = ptrimeshcontacts->m_depth;
- pcontact->g1 = g1;
- pcontact->g2 = SphereGeom;
- pcontact->side1 = ptrimeshcontacts->m_feature1;
- pcontact->side2 = -1;
-
- ptrimeshcontacts++;
- }
+ dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, g1, SphereGeom, -1);
+ contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, Flags, Contacts, Stride);
GIM_DYNARRAY_DESTROY(trimeshcontacts);
diff --git a/ode/src/collision_trimesh_trimesh.cpp b/ode/src/collision_trimesh_trimesh.cpp
index 37072e8..f2398a9 100644
--- a/ode/src/collision_trimesh_trimesh.cpp
+++ b/ode/src/collision_trimesh_trimesh.cpp
@@ -174,9 +174,12 @@ dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Strid
// Collision query
Matrix4x4 amatrix, bmatrix;
- BOOL IsOk = Collider.Collide(ColCache,
- &MakeMatrix(TLPosition1, TLRotation1, amatrix),
- &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
+ dVector3 TLOffsetPosition1 = { REAL(0.0), };
+ dVector3 TLOffsetPosition2;
+ dSubtractVectors3(TLOffsetPosition2, TLPosition2, TLPosition1);
+ MakeMatrix(TLOffsetPosition1, TLRotation1, amatrix);
+ MakeMatrix(TLOffsetPosition2, TLRotation2, bmatrix);
+ BOOL IsOk = Collider.Collide(ColCache, &amatrix, &bmatrix);
// Make "double" versions of these matrices, if appropriate
diff --git a/ode/src/collision_trimesh_trimesh_new.cpp b/ode/src/collision_trimesh_trimesh_new.cpp
index 118df99..3541e8d 100644
--- a/ode/src/collision_trimesh_trimesh_new.cpp
+++ b/ode/src/collision_trimesh_trimesh_new.cpp
@@ -508,17 +508,12 @@ dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Strid
// Collision query
Matrix4x4 amatrix, bmatrix;
- BOOL IsOk = Collider.Collide(ColCache,
- &MakeMatrix(TLPosition1, TLRotation1, amatrix),
- &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
-
-
- // Make "double" versions of these matrices, if appropriate
- dMatrix4 A, B;
- dMakeMatrix4(TLPosition1, TLRotation1, A);
- dMakeMatrix4(TLPosition2, TLRotation2, B);
-
-
+ dVector3 TLOffsetPosition1 = { REAL(0.0), };
+ dVector3 TLOffsetPosition2;
+ dSubtractVectors3(TLOffsetPosition2, TLPosition2, TLPosition1);
+ MakeMatrix(TLOffsetPosition1, TLRotation1, amatrix);
+ MakeMatrix(TLOffsetPosition2, TLRotation2, bmatrix);
+ bool IsOk = Collider.Collide(ColCache, &amatrix, &bmatrix);
if (IsOk) {
@@ -534,12 +529,6 @@ dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Strid
int OutTriCount = 0;
dVector3 v1[3], v2[3];
- // only do these expensive inversions once
- /*dMatrix4 InvMatrix1, InvMatrix2;
- dInvertMatrix4(A, InvMatrix1);
- dInvertMatrix4(B, InvMatrix2);*/
-
-
for (int i = 0; i < TriCount; i++)
{
id1 = CollidingPairs[i].id0;
diff --git a/ode/src/collision_util.h b/ode/src/collision_util.h
index adaee69..c88fcb4 100644
--- a/ode/src/collision_util.h
+++ b/ode/src/collision_util.h
@@ -42,7 +42,8 @@ some useful collision utility stuff.
#if 1
#include "collision_kernel.h"
// Fetches a contact
-static inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
+static inline
+dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
}
@@ -89,82 +90,98 @@ void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArr
void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
// Some vector math
-static inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
+static inline
+void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
{
dSubtractVectors3(c, a, b);
}
-static inline void dVector3Scale(dVector3& a,dReal nScale)
+static inline
+void dVector3Scale(dVector3& a,dReal nScale)
{
dScaleVector3(a, nScale);
}
-static inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
+static inline
+void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
{
dAddVectors3(c, a, b);
}
-static inline void dVector3Copy(const dVector3& a,dVector3& c)
+static inline
+void dVector3Copy(const dVector3& a,dVector3& c)
{
dCopyVector3(c, a);
}
-static inline void dVector4Copy(const dVector4& a,dVector4& c)
+static inline
+void dVector4Copy(const dVector4& a,dVector4& c)
{
dCopyVector4(c, a);
}
-static inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
+static inline
+void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
{
dCalcVectorCross3(c, a, b);
}
-static inline dReal dVector3Length(const dVector3& a)
+static inline
+dReal dVector3Length(const dVector3& a)
{
return dCalcVectorLength3(a);
}
-static inline dReal dVector3LengthSquare(const dVector3& a)
+static inline
+dReal dVector3LengthSquare(const dVector3& a)
{
return dCalcVectorLengthSquare3(a);
}
-static inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
+static inline
+dReal dVector3Dot(const dVector3& a,const dVector3& b)
{
return dCalcVectorDot3(a, b);
}
-static inline void dVector3Inv(dVector3& a)
+static inline
+void dVector3Inv(dVector3& a)
{
dNegateVector3(a);
}
-static inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
+static inline
+void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
{
dGetMatrixColumn3(v, m, col);
}
-static inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
+static inline
+void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
{
dCalcVectorCross3_114(r, v, m + col);
}
-static inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
+static inline
+void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
{
dCalcVectorCross3_141(r, m + col, v);
}
-static inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
+static inline
+void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
{
dMultiply0_331(r, m, v);
}
-static inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
+static inline
+dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
{
return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
}
-static inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
+static inline
+void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
{
plane[0] = normal[0];
plane[1] = normal[1];
@@ -172,12 +189,14 @@ static inline void dConstructPlane(const dVector3& normal,const dReal& distance,
plane[3] = distance;
}
-static inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
+static inline
+void dMatrix3Copy(const dReal* source,dMatrix3& dest)
{
dCopyMatrix4x3(dest, source);
}
-static inline dReal dMatrix3Det( const dMatrix3& mat )
+static inline
+dReal dMatrix3Det( const dMatrix3& mat )
{
dReal det;
@@ -189,7 +208,8 @@ static inline dReal dMatrix3Det( const dMatrix3& mat )
}
-inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
+static inline
+void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
{
dReal det = dMatrix3Det( ma );
@@ -214,7 +234,8 @@ inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
dst[10] = ( ma[0]*ma[5] - ma[1]*ma[4] ) * detRecip;
}
-inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
+static inline
+void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
{
// Nguyen Binh : this code seem to be the fastest.
@@ -247,7 +268,8 @@ inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVecto
*/
}
-inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
+static inline
+void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
{
dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
@@ -270,7 +292,8 @@ inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVe
}
}
-inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
+static inline
+void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
{
rY = asin(mRot[0 * 4 + 2]);
if (rY < M_PI /2)
@@ -295,7 +318,8 @@ inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal
}
}
-inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
+static inline
+void dQuatInv(const dQuaternion& source, dQuaternion& dest)
{
dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
diff --git a/ode/src/convex.cpp b/ode/src/convex.cpp
index 6684fea..36f5726 100644
--- a/ode/src/convex.cpp
+++ b/ode/src/convex.cpp
@@ -48,11 +48,11 @@ By Rodrigo Hernandez
//****************************************************************************
// Convex public API
dxConvex::dxConvex (dSpaceID space,
- dReal *_planes,
+ const dReal *_planes,
unsigned int _planecount,
- dReal *_points,
+ const dReal *_points,
unsigned int _pointcount,
- unsigned int *_polygons) :
+ const unsigned int *_polygons) :
dxGeom (space,1)
{
dAASSERT (_planes != NULL);
@@ -71,8 +71,8 @@ dxConvex::dxConvex (dSpaceID space,
#ifndef dNODEBUG
// Check for properly build polygons by calculating the determinant
// of the 3x3 matrix composed of the first 3 points in the polygon.
- unsigned int *points_in_poly=polygons;
- unsigned int *index=polygons+1;
+ const unsigned int *points_in_poly=polygons;
+ const unsigned int *index=polygons+1;
for(unsigned int i=0;i<planecount;++i)
{
@@ -123,8 +123,8 @@ void dxConvex::computeAABB()
/*! \brief Populates the edges set, should be called only once whenever the polygon array gets updated */
void dxConvex::FillEdges()
{
- unsigned int *points_in_poly=polygons;
- unsigned int *index=polygons+1;
+ const unsigned int *points_in_poly=polygons;
+ const unsigned int *index=polygons+1;
if (edges!=NULL) delete[] edges;
edgecount = 0;
edge e;
@@ -205,10 +205,10 @@ void dxConvex::GetFaceNormal(int i, dVector3 normal)
}
#endif
-dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount,
- dReal *_points,
+dGeomID dCreateConvex (dSpaceID space,const dReal *_planes,unsigned int _planecount,
+ const dReal *_points,
unsigned int _pointcount,
- unsigned int *_polygons)
+ const unsigned int *_polygons)
{
//fprintf(stdout,"dxConvex dCreateConvex\n");
return new dxConvex(space,_planes, _planecount,
@@ -217,10 +217,10 @@ dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount,
_polygons);
}
-void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount,
- dReal *_points,
+void dGeomSetConvex (dGeomID g,const dReal *_planes,unsigned int _planecount,
+ const dReal *_points,
unsigned int _pointcount,
- unsigned int *_polygons)
+ const unsigned int *_polygons)
{
//fprintf(stdout,"dxConvex dGeomSetConvex\n");
dUASSERT (g && g->type == dConvexClass,"argument not a convex shape");
@@ -508,7 +508,7 @@ inline bool IsPointInConvex(dVector3 p,
\return true if the point lies inside of the polygon, false if not.
*/
inline bool IsPointInPolygon(dVector3 p,
- unsigned int *polygon,
+ const unsigned int *polygon,
dReal *plane,
dxConvex *convex,
dVector3 out)
@@ -660,7 +660,7 @@ int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
dVector4 plane;
// dVector3 contactpoint;
dVector3 offsetpos,out,temp;
- unsigned int *pPoly=Convex->polygons;
+ const unsigned int *pPoly=Convex->polygons;
int closestplane=-1;
bool sphereinside=true;
/*
@@ -857,7 +857,7 @@ bool CheckEdgeIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,int& curc,
e2[0]+=cvx1.final_posr->pos[0];
e2[1]+=cvx1.final_posr->pos[1];
e2[2]+=cvx1.final_posr->pos[2];
- unsigned int* pPoly=cvx2.polygons;
+ const unsigned int* pPoly=cvx2.polygons;
for(size_t j=0;j<cvx2.planecount;++j)
{
// Rotate
@@ -1134,11 +1134,11 @@ int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,
// cvx1 MUST always be in contact->g1 and cvx2 in contact->g2
// This was learned the hard way :(
unsigned int incident_side;
- unsigned int* pIncidentPoly;
- unsigned int* pIncidentPoints;
+ const unsigned int* pIncidentPoly;
+ const unsigned int* pIncidentPoints;
unsigned int reference_side;
- unsigned int* pReferencePoly;
- unsigned int* pReferencePoints;
+ const unsigned int* pReferencePoly;
+ const unsigned int* pReferencePoints;
dVector4 plane,rplane,iplane;
dVector3 tmp;
dVector3 dist,p;
@@ -1485,7 +1485,7 @@ int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
for ( unsigned int i = 0; i < convex->planecount; ++i )
{
// Alias this plane.
- dReal* plane = convex->planes + ( i * 4 );
+ const dReal* plane = convex->planes + ( i * 4 );
// If alpha >= 0 then start point is outside of plane.
alpha = dCalcVectorDot3( plane, ray->final_posr->pos ) - plane[3];
@@ -1513,7 +1513,7 @@ int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
for ( unsigned int i = 0; i < convex->planecount; ++i )
{
// Alias this plane.
- dReal* plane = convex->planes + ( i * 4 );
+ const dReal* plane = convex->planes + ( i * 4 );
// If alpha >= 0 then point is outside of plane.
alpha = nsign * ( dCalcVectorDot3( plane, ray->final_posr->pos ) - plane[3] );
@@ -1542,7 +1542,7 @@ int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
continue; // Skip self.
// Alias this plane.
- dReal* planej = convex->planes + ( j * 4 );
+ const dReal* planej = convex->planes + ( j * 4 );
// If beta >= 0 then start is outside of plane.
beta = dCalcVectorDot3( planej, contact->pos ) - plane[3];
diff --git a/ode/src/quickstep.cpp b/ode/src/quickstep.cpp
index 1a6e28c..fd5a745 100644
--- a/ode/src/quickstep.cpp
+++ b/ode/src/quickstep.cpp
@@ -349,14 +349,13 @@ struct dxQuickStepperStage5CallContext
struct dxQuickStepperStage4CallContext
{
void Initialize(const dxStepperProcessingCallContext *callContext, const dxQuickStepperLocalContext *localContext,
- dReal *lambda, dReal *cforce, dReal *iMJ, dReal *Ad, IndexError *order, dReal *last_lambda, atomicord32 *bi_links_or_mi_levels, atomicord32 *mi_links)
+ dReal *lambda, dReal *cforce, dReal *iMJ, IndexError *order, dReal *last_lambda, atomicord32 *bi_links_or_mi_levels, atomicord32 *mi_links)
{
m_stepperCallContext = callContext;
m_localContext = localContext;
m_lambda = lambda;
m_cforce = cforce;
m_iMJ = iMJ;
- m_Ad = Ad;
m_order = order;
m_last_lambda = last_lambda;
m_bi_links_or_mi_levels = bi_links_or_mi_levels;
@@ -419,7 +418,6 @@ struct dxQuickStepperStage4CallContext
dReal *m_lambda;
dReal *m_cforce;
dReal *m_iMJ;
- dReal *m_Ad;
IndexError *m_order;
dReal *m_last_lambda;
atomicord32 *m_bi_links_or_mi_levels;
@@ -1418,7 +1416,6 @@ void dxQuickStepIsland_Stage3(dxQuickStepperStage3CallContext *stage3CallContext
unsigned int nb = callContext->m_islandBodiesCount;
dReal *cforce = memarena->AllocateArray<dReal>((size_t)nb*6);
dReal *iMJ = memarena->AllocateArray<dReal>((size_t)m*12);
- dReal *Ad = memarena->AllocateArray<dReal>(m);
// order to solve constraint rows in
IndexError *order = memarena->AllocateArray<IndexError>(m);
dReal *last_lambda = NULL;
@@ -1441,7 +1438,7 @@ void dxQuickStepIsland_Stage3(dxQuickStepperStage3CallContext *stage3CallContext
dIASSERT(singleThreadedExecution);
#endif
dxQuickStepperStage4CallContext *stage4CallContext = (dxQuickStepperStage4CallContext *)memarena->AllocateBlock(sizeof(dxQuickStepperStage4CallContext));
- stage4CallContext->Initialize(callContext, localContext, lambda, cforce, iMJ, Ad, order, last_lambda, bi_links_or_mi_levels, mi_links);
+ stage4CallContext->Initialize(callContext, localContext, lambda, cforce, iMJ, order, last_lambda, bi_links_or_mi_levels, mi_links);
if (singleThreadedExecution)
{
@@ -1847,7 +1844,6 @@ void dxQuickStepIsland_Stage4LCP_AdComputation(dxQuickStepperStage4CallContext *
const dReal sor_w = qs->w; // SOR over-relaxation parameter
dReal *iMJ = stage4CallContext->m_iMJ;
- dReal *Ad = stage4CallContext->m_Ad;
const unsigned int step_size = dxQUICKSTEPISLAND_STAGE4LCP_AD_STEP;
unsigned int m_steps = (m + (step_size - 1)) / step_size;
@@ -1877,7 +1873,9 @@ void dxQuickStepIsland_Stage4LCP_AdComputation(dxQuickStepperStage4CallContext *
// NOTE: This may seem unnecessary but it's indeed an optimization
// to move multiplication by Ad[i] and cfm[i] out of iteration loop.
- // scale J and b by Ad
+ // scale cfm, J and b by Ad
+ cfm[mi] = cfm_i * Ad_i;
+
unsigned int l = lend;
do {
J_ptr[l - 1] *= Ad_i;
@@ -1885,8 +1883,6 @@ void dxQuickStepIsland_Stage4LCP_AdComputation(dxQuickStepperStage4CallContext *
while (--l != 0);
rhs[mi] *= Ad_i;
- // scale Ad by CFM. N.B. this should be done last since it is used above
- Ad[mi] = Ad_i * cfm_i;
if (++mi == miend) {
break;
@@ -2408,39 +2404,38 @@ void dxQuickStepIsland_Stage4LCP_IterationStep(dxQuickStepperStage4CallContext *
IndexError *order = stage4CallContext->m_order;
unsigned int index = order[i].index;
+ size_t index_offset = (size_t)index*12;
dReal *fc_ptr1;
- dReal *fc_ptr2;
+ dReal *fc_ptr2 = NULL;
dReal delta;
- {
- int *jb = localContext->m_jb;
- int b1 = jb[(size_t)index*2];
- int b2 = jb[(size_t)index*2+1];
- dReal *fc = stage4CallContext->m_cforce;
- fc_ptr1 = fc + 6*(size_t)(unsigned)b1;
- fc_ptr2 = (b2 != -1) ? fc + 6*(size_t)(unsigned)b2 : NULL;
- }
-
dReal *lambda = stage4CallContext->m_lambda;
dReal old_lambda = lambda[index];
- size_t index_offset = (size_t)index*12;
{
- dReal *rhs = localContext->m_rhs;
- dReal *Ad = stage4CallContext->m_Ad;
- delta = rhs[index] - old_lambda*Ad[index];
+ const dReal *rhs = localContext->m_rhs;
+ const dReal *cfm = localContext->m_cfm;
+ delta = rhs[index] - old_lambda * cfm[index];
+
+ dReal *fc = stage4CallContext->m_cforce;
+
+ int *jb = localContext->m_jb;
+ int b2 = jb[(size_t)index*2+1];
+ int b1 = jb[(size_t)index*2];
dReal *J = localContext->m_J;
const dReal *J_ptr = J + index_offset;
// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
- delta -=fc_ptr1[0] * J_ptr[0] + fc_ptr1[1] * J_ptr[1] +
+ fc_ptr1 = fc + 6*(size_t)(unsigned)b1;
+ delta -= fc_ptr1[0] * J_ptr[0] + fc_ptr1[1] * J_ptr[1] +
fc_ptr1[2] * J_ptr[2] + fc_ptr1[3] * J_ptr[3] +
fc_ptr1[4] * J_ptr[4] + fc_ptr1[5] * J_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
- if (fc_ptr2) {
- delta -=fc_ptr2[0] * J_ptr[6] + fc_ptr2[1] * J_ptr[7] +
+ if (b2 != -1) {
+ fc_ptr2 = fc + 6*(size_t)(unsigned)b2;
+ delta -= fc_ptr2[0] * J_ptr[6] + fc_ptr2[1] * J_ptr[7] +
fc_ptr2[2] * J_ptr[8] + fc_ptr2[3] * J_ptr[9] +
fc_ptr2[4] * J_ptr[10] + fc_ptr2[5] * J_ptr[11];
}
@@ -2473,11 +2468,11 @@ void dxQuickStepIsland_Stage4LCP_IterationStep(dxQuickStepperStage4CallContext *
// to save test+jump penalties here?
dReal new_lambda = old_lambda + delta;
if (new_lambda < lo_act) {
- delta = lo_act-old_lambda;
+ delta = lo_act - old_lambda;
lambda[index] = lo_act;
}
else if (new_lambda > hi_act) {
- delta = hi_act-old_lambda;
+ delta = hi_act - old_lambda;
lambda[index] = hi_act;
}
else {
@@ -2928,7 +2923,6 @@ size_t dxEstimateQuickStepMemoryRequirements (dxBody * const *body,
sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for lambda
sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * 6 * nb); // for cforce
sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
- sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Ad
sub3_res1 += dEFFICIENT_SIZE(sizeof(IndexError) * m); // for order
#if CONSTRAINTS_REORDERING_METHOD == REORDERING_METHOD__BY_ERROR
sub3_res1 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for last_lambda
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-games/ode.git
More information about the Pkg-games-commits
mailing list