From e2eb3c42b16abe3bfef142a4700f16f36b69ae3d Mon Sep 17 00:00:00 2001
From: isucan <isucan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Thu, 23 Aug 2012 10:55:25 +0000
Subject: [PATCH] patches from mark moll
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@165 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 9 +++----
trunk/fcl/include/fcl/ccd/motion.h | 2 +-
.../fcl/include/fcl/simd/math_simd_details.h | 24 ++++---------------
trunk/fcl/src/BV/OBBRSS.cpp | 1 +
trunk/fcl/src/BV/kIOS.cpp | 3 ++-
5 files changed, 14 insertions(+), 25 deletions(-)
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 0d75d702..24d288ac 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 124bb268..0fd9f917 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 404f9381..8248ab59 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 3a5fc39a..7a703b97 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 2dbdce96..347d8c49 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;
}
--
GitLab