diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index bd9bd47241c63e4b8a35463ea6da49b4fea106a8..c2717d0c81b4cc469bb224946e03121192892bd1 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 c72b61fe097179f6cc61053c7a8fb10f854a4d3b..944e5aa45b093579cc8a14c2c15bb017b462aa43 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 a0526a4a8931d7e3bc73871b747e08b1adc68397..825078091284fd104379e62778ec70e44b89c44f 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 7ed1001ea3cc4e94838624b985c864815d78a3c5..c2c619ba6707ac1b43960a6c7bdec7c519fcde49 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 d4130bbf3a40f2273b358a1ec4bb56e243d304e9..0c674991014cbf3962ea5b5dc84378e87e23ee6f 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]));
+}
+
+
}