diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 0d75d70213fb6c9666a2612a2fa2a62076effe3d..24d288ac3a9d85c51496c149c27ba634ac5f54da 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -27,10 +27,11 @@ pkg_check_modules(CCD REQUIRED ccd)
include_directories(${CCD_INCLUDE_DIRS})
link_directories(${CCD_LIBRARY_DIRS})
-pkg_check_modules(OCTOMAP REQUIRED octomap)
-include_directories(${OCTOMAP_INCLUDE_DIRS})
-link_directories(${OCTOMAP_LIBRARY_DIRS})
-
+pkg_check_modules(OCTOMAP QUIET octomap)
+if(OCTOMAP_FOUND)
+ include_directories(${OCTOMAP_INCLUDE_DIRS})
+ link_directories(${OCTOMAP_LIBRARY_DIRS})
+endif()
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/BV/kDOP.cpp src/traversal/traversal_node_base.cpp src/traversal/traversal_node_bvhs.cpp src/intersect.cpp src/ccd/motion.cpp src/BVH/BV_fitter.cpp src/BVH/BV_splitter.cpp src/BVH/BVH_model.cpp src/BVH/BVH_utility.cpp src/math/transform.cpp src/traversal/traversal_node_setup.cpp src/shape/geometric_shapes.cpp src/shape/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal/traversal_recurse.cpp src/broadphase/broadphase_bruteforce.cpp src/broadphase/broadphase_spatialhash.cpp src/broadphase/broadphase_SaP.cpp src/broadphase/broadphase_SSaP.cpp src/broadphase/broadphase_interval_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree_array.cpp src/collision.cpp src/collision_func_matrix.cpp src/broadphase/interval_tree.cpp src/ccd/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/broadphase/hierarchy_tree.cpp src/profile.cpp src/collision_data.cpp)
diff --git a/trunk/fcl/include/fcl/ccd/motion.h b/trunk/fcl/include/fcl/ccd/motion.h
index 124bb268dc761c8a6bc77ef7433d863bc593b280..0fd9f91703ff796b77f4221f95f9f6d85fc014aa 100644
--- a/trunk/fcl/include/fcl/ccd/motion.h
+++ b/trunk/fcl/include/fcl/ccd/motion.h
@@ -525,7 +525,7 @@ public:
InterpMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2,
const Vec3f& O) : tf1(R1, T1),
- tf2(T2, T2),
+ tf2(R2, T2),
tf(tf1),
reference_p(O)
{
diff --git a/trunk/fcl/include/fcl/simd/math_simd_details.h b/trunk/fcl/include/fcl/simd/math_simd_details.h
index 404f9381c35fb5590699d6084c17ed51320fc2e6..8248ab597d3b46f76c944cf6b7cbf30b1554757c 100644
--- a/trunk/fcl/include/fcl/simd/math_simd_details.h
+++ b/trunk/fcl/include/fcl/simd/math_simd_details.h
@@ -73,25 +73,11 @@ static inline __m128 vec_sel(__m128 a, __m128 b, unsigned int mask)
return vec_sel(a, b, _mm_set1_ps(*(float*)&mask));
}
-static inline __m128 vec_splat(__m128 a, int e)
-{
- return _mm_shuffle_ps(a, a, _MM_SHUFFLE(e, e, e, e));
-}
-
-static inline __m128d vec_splat(__m128d a, int e)
-{
- return _mm_shuffle_pd(a, a, _MM_SHUFFLE2(e, e));
-}
+#define vec_splat(a, e) _mm_shuffle_ps((a), (a), _MM_SHUFFLE((e), (e), (e), (e)))
+#define vec_splatd(a, e) _mm_shuffle_pd((a), (a), _MM_SHUFFLE2((e), (e)))
-static inline __m128 _mm_ror_ps(__m128 x, int e)
-{
- return (e % 4) ? _mm_shuffle_ps(x, x, _MM_SHUFFLE((e+3)%4, (e+2)%4, (e+1)%4, e%4)) : x;
-}
-
-static inline __m128 _mm_rol_ps(__m128 x, int e)
-{
- return (e % 4) ? _mm_shuffle_ps(x, x, _MM_SHUFFLE((7-e)%4, (6-e)%4, (5-e)%4, (4-e)%4)) : x;
-}
+#define _mm_ror_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE(((e)+3)%4, ((e)+2)%4, ((e)+1)%4, (e)%4)) : (x))
+#define _mm_rol_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE((7-(e))%4, (6-(e))%4, (5-(e))%4, (4-(e))%4)) : x)
static inline __m128 newtonraphson_rsqrt4(const __m128 v)
{
@@ -327,7 +313,7 @@ static inline __m128d dot_prod3(__m128d x0, __m128d x1, __m128d y0, __m128d y1)
{
register __m128d m1 = _mm_mul_pd(x0, y0);
register __m128d m2 = _mm_mul_pd(x1, y1);
- return _mm_add_pd(_mm_add_pd(vec_splat(m1, 0), vec_splat(m1, 1)), vec_splat(m2, 0));
+ return _mm_add_pd(_mm_add_pd(vec_splatd(m1, 0), vec_splatd(m1, 1)), vec_splatd(m2, 0));
}
static inline double dot_prod3(const sse_meta_d4& x, const sse_meta_d4& y)
diff --git a/trunk/fcl/src/BV/OBBRSS.cpp b/trunk/fcl/src/BV/OBBRSS.cpp
index 3a5fc39af8732f206ef1571696eda94c3885e0e0..7a703b97c095134330e69b57794b21ad19a56999 100644
--- a/trunk/fcl/src/BV/OBBRSS.cpp
+++ b/trunk/fcl/src/BV/OBBRSS.cpp
@@ -54,6 +54,7 @@ OBBRSS translate(const OBBRSS& bv, const Vec3f& t)
OBBRSS res(bv);
res.obb.To += t;
res.rss.Tr += t;
+ return res;
}
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/trunk/fcl/src/BV/kIOS.cpp
index 2dbdce96cdba5e08c6d0e01cf86f50afe5923108..347d8c49ac17d7af5298d3c4b38069d2e1cbde37 100644
--- a/trunk/fcl/src/BV/kIOS.cpp
+++ b/trunk/fcl/src/BV/kIOS.cpp
@@ -88,7 +88,7 @@ kIOS& kIOS::operator += (const Vec3f& p)
}
obb += p;
-
+ return *this;
}
kIOS kIOS::operator + (const kIOS& other) const
@@ -205,6 +205,7 @@ kIOS translate(const kIOS& bv, const Vec3f& t)
}
translate(res.obb, t);
+ return res;
}