Commit e2eb3c42 authored by isucan's avatar isucan
Browse files

patches from mark moll

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@165 253336fb-580f-4252-a368-f3cef5a2a82b
parent 0157dfeb
......@@ -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)
......
......@@ -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)
{
......
......@@ -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)
......
......@@ -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;
}
......
......@@ -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;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment