From a1d8ec4bc976d0989b758d298f935b55dbec8a37 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 25 Jun 2012 09:06:16 +0000
Subject: [PATCH] sphere-sphere, box-box special implementation. seems the
precision is lower than standard GJK. So now is still not used.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@106 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
trunk/fcl/include/fcl/matrix_3f.h | 10 ++++-
.../fcl/include/fcl/narrowphase/narrowphase.h | 41 ++++++++++++++++++-
trunk/fcl/src/BV/OBB.cpp | 2 +-
trunk/fcl/src/matrix_3f.cpp | 15 +++----
5 files changed, 58 insertions(+), 12 deletions(-)
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index bd9bd472..c2717d0c 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp src/narrowphase.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index c72b61fe..944e5aa4 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -137,7 +137,6 @@ namespace fcl
BVH_REAL determinant() const;
Matrix3f transpose() const;
Matrix3f inverse() const;
- Matrix3f abs() const;
Matrix3f transposeTimes(const Matrix3f& m) const;
Matrix3f timesTranspose(const Matrix3f& m) const;
@@ -171,6 +170,11 @@ namespace fcl
return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
}
+ inline BVH_REAL transposeDot(size_t i, const Vec3f& v) const
+ {
+ return v_[0][i] * v[0] + v_[1][i] * v[1] + v_[2][i] * v[2];
+ }
+
inline void setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
@@ -179,7 +183,7 @@ namespace fcl
v_[1].setValue(yx, yy, yz);
v_[2].setValue(zx, zy, zz);
}
-
+
inline void setValue(BVH_REAL x)
{
v_[0].setValue(x);
@@ -191,6 +195,8 @@ namespace fcl
void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, Matrix3f& R, Vec3f& T);
void matEigen(const Matrix3f& R, BVH_REAL dout[3], Vec3f vout[3]);
+
+ Matrix3f abs(const Matrix3f& R);
}
#endif
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index a0526a4a..82507809 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -41,6 +41,7 @@
#include "fcl/narrowphase/gjk_libccd.h"
+
namespace fcl
{
@@ -192,6 +193,30 @@ struct GJKSolver_libccd
+template<>
+bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
+ const Sphere& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+/*
+template<>
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+
+template<>
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
+ const Box& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+*/
+
+template<>
+bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
+ const Sphere& s2, const SimpleTransform& tf2,
+ BVH_REAL* dist) const;
struct GJKSolver_indep
{
@@ -453,8 +478,22 @@ struct GJKSolver_indep
BVH_REAL gjk_max_iterations;
};
+template<>
+bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
+ const Sphere& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
-
+/*
+template<>
+bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
+ const Box& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+*/
+
+template<>
+bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
+ const Sphere& s2, const SimpleTransform& tf2,
+ BVH_REAL* dist) const;
}
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index 7ed1001e..c2c619ba 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -113,7 +113,7 @@ bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const V
register BVH_REAL t, s;
const BVH_REAL reps = 1e-6;
- Matrix3f Bf = B.abs();
+ Matrix3f Bf = abs(B);
Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
diff --git a/trunk/fcl/src/matrix_3f.cpp b/trunk/fcl/src/matrix_3f.cpp
index d4130bbf..0c674991 100644
--- a/trunk/fcl/src/matrix_3f.cpp
+++ b/trunk/fcl/src/matrix_3f.cpp
@@ -71,13 +71,6 @@ Matrix3f Matrix3f::transpose() const
v_[0][2], v_[1][2], v_[2][2]);
}
-Matrix3f Matrix3f::abs() const
-{
- return Matrix3f(fabs(v_[0][0]), fabs(v_[0][1]), fabs(v_[0][2]),
- fabs(v_[1][0]), fabs(v_[1][1]), fabs(v_[1][2]),
- fabs(v_[2][0]), fabs(v_[2][1]), fabs(v_[2][2]));
-}
-
Matrix3f Matrix3f::inverse() const
{
BVH_REAL det = determinant();
@@ -235,5 +228,13 @@ void matEigen(const Matrix3f& m, BVH_REAL dout[3], Vec3f vout[3])
}
+Matrix3f abs(const Matrix3f& R)
+{
+ return Matrix3f(fabs(R.v_[0][0]), fabs(R.v_[0][1]), fabs(R.v_[0][2]),
+ fabs(R.v_[1][0]), fabs(R.v_[1][1]), fabs(R.v_[1][2]),
+ fabs(R.v_[2][0]), fabs(R.v_[2][1]), fabs(R.v_[2][2]));
+}
+
+
}
--
GitLab