From 8c15d35fcae9a3d056183105d749aba1377038c5 Mon Sep 17 00:00:00 2001
From: mmoll <mmoll@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Sat, 25 Aug 2012 03:43:23 +0000
Subject: [PATCH] fix compilation issues with SSE, properly set SSE flags for
gcc and clang, add option to disable SSE to create more portable binaries
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@177 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 18 ++++++++----------
.../include/fcl/broadphase/broadphase_SaP.h | 4 ++--
trunk/fcl/include/fcl/math/matrix_3f.h | 9 ++++++---
.../fcl/traversal/traversal_node_shapes.h | 2 +-
trunk/fcl/src/BV/OBB.cpp | 2 +-
trunk/fcl/src/BV/RSS.cpp | 2 +-
trunk/fcl/src/BVH/BV_fitter.cpp | 16 ++++++++--------
trunk/fcl/src/broadphase/broadphase_SaP.cpp | 18 +++++++++---------
trunk/fcl/src/ccd/taylor_model.cpp | 1 +
trunk/fcl/src/narrowphase/gjk.cpp | 2 ++
10 files changed, 39 insertions(+), 35 deletions(-)
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index ed92ad62..2a862dbe 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -14,17 +14,15 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
include(FCLVersion)
-# Check for SSE flags
+# Whether to enable SSE
+option(FCL_USE_SSE "Whether FCL should SSE instructions" ON)
set(FCL_HAVE_SSE 0)
-include(FCLCheckSSE)
-fcl_check_for_sse()
-if (SSE_FLAGS)
- message(STATUS "FCL uses SSE")
- add_definitions(${SSE_FLAGS})
- set(FCL_HAVE_SSE 0)
- # waiting for Jia to fix this; not using sse anyway
-else()
- message(STATUS "FCL does not use SSE")
+if(FCL_USE_SSE)
+ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ set(FCL_HAVE_SSE 1)
+ add_definitions(-march=native)
+ endif()
+ # TODO: do something similar for other compilers
endif()
# Find Octomap (optional)
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h b/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h
index 165fbd57..8fe62b35 100644
--- a/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_SaP.h
@@ -163,13 +163,13 @@ protected:
else return aabb->cached.min_;
}
- inline FCL_REAL getVal(size_t i) const
+ inline Vec3f::U getVal(size_t i) const
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
}
- inline FCL_REAL& getVal(size_t i)
+ inline Vec3f::U& getVal(size_t i)
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
diff --git a/trunk/fcl/include/fcl/math/matrix_3f.h b/trunk/fcl/include/fcl/math/matrix_3f.h
index 36fc4352..f711924b 100644
--- a/trunk/fcl/include/fcl/math/matrix_3f.h
+++ b/trunk/fcl/include/fcl/math/matrix_3f.h
@@ -407,8 +407,11 @@ typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename
}
-typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
-//typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
+#if FCL_HAVE_SSE
+ typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
+#else
+ typedef Matrix3fX<details::Vec3Data<FCL_REAL> > Matrix3f;
+#endif
static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
{
@@ -428,7 +431,7 @@ public:
Matrix3f Sigma;
/// @brief Variations along the eign axes
- FCL_REAL sigma[3];
+ Matrix3f::U sigma[3];
/// @brief Eigen axes of the variation matrix
Vec3f axis[3];
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
index f015079d..3bb3cc3d 100644
--- a/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_shapes.h
@@ -148,7 +148,7 @@ public:
void leafTesting(int, int) const
{
FCL_REAL distance;
- !nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance);
+ nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance);
result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE);
}
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index d83f8533..6b1246a0 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -74,7 +74,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
computeVertices(b2, vertex + 8);
Matrix3f M;
Vec3f E[3];
- FCL_REAL s[3] = {0, 0, 0};
+ Matrix3f::U s[3] = {0, 0, 0};
// obb axes
Vec3f& R0 = b.axis[0];
diff --git a/trunk/fcl/src/BV/RSS.cpp b/trunk/fcl/src/BV/RSS.cpp
index 363e752d..4c29a127 100644
--- a/trunk/fcl/src/BV/RSS.cpp
+++ b/trunk/fcl/src/BV/RSS.cpp
@@ -1076,7 +1076,7 @@ RSS RSS::operator + (const RSS& other) const
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- FCL_REAL s[3] = {0, 0, 0};
+ Matrix3f::U s[3] = {0, 0, 0};
getCovariance(v, NULL, NULL, NULL, 16, M);
eigen(M, s, E);
diff --git a/trunk/fcl/src/BVH/BV_fitter.cpp b/trunk/fcl/src/BVH/BV_fitter.cpp
index 7455f53b..e01e3643 100644
--- a/trunk/fcl/src/BVH/BV_fitter.cpp
+++ b/trunk/fcl/src/BVH/BV_fitter.cpp
@@ -48,7 +48,7 @@ static const double invCosA = 2.0 / sqrt(3.0);
static const double sinA = 0.5;
static const double cosA = sqrt(3.0) / 2.0;
-static inline void axisFromEigen(Vec3f eigenV[3], FCL_REAL eigenS[3], Vec3f axis[3])
+static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::U eigenS[3], Vec3f axis[3])
{
int min, mid, max;
if(eigenS[0] > eigenS[1]) { max = 0; min = 1; }
@@ -137,7 +137,7 @@ void fitn(Vec3f* ps, int n, OBB& bv)
{
Matrix3f M;
Vec3f E[3];
- FCL_REAL s[3] = {0, 0, 0}; // three eigen values
+ Matrix3f::U s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
@@ -223,7 +223,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
{
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- FCL_REAL s[3] = {0, 0, 0};
+ Matrix3f::U s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
@@ -340,7 +340,7 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
{
Matrix3f M;
Vec3f E[3];
- FCL_REAL s[3] = {0, 0, 0}; // three eigen values;
+ Matrix3f::U s[3] = {0, 0, 0}; // three eigen values;
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
@@ -522,7 +522,7 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- FCL_REAL s[3]; // three eigen values
+ Matrix3f::U s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
@@ -540,7 +540,7 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
OBBRSS bv;
Matrix3f M;
Vec3f E[3];
- FCL_REAL s[3];
+ Matrix3f::U s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
@@ -571,7 +571,7 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- FCL_REAL s[3]; // three eigen values
+ Matrix3f::U s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
@@ -599,7 +599,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- FCL_REAL s[3];
+ Matrix3f::U s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
diff --git a/trunk/fcl/src/broadphase/broadphase_SaP.cpp b/trunk/fcl/src/broadphase/broadphase_SaP.cpp
index 432a94b3..c22af618 100644
--- a/trunk/fcl/src/broadphase/broadphase_SaP.cpp
+++ b/trunk/fcl/src/broadphase/broadphase_SaP.cpp
@@ -117,9 +117,9 @@ void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& o
for(size_t coord = 0; coord < 3; ++coord)
{
std::sort(endpoints.begin(), endpoints.end(),
- boost::bind(std::less<FCL_REAL>(),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
+ boost::bind(std::less<Vec3f::U>(),
+ boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
+ boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
endpoints[0]->prev[coord] = NULL;
endpoints[0]->next[coord] = endpoints[1];
@@ -508,9 +508,9 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionC
// compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<FCL_REAL>(),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+ boost::bind(std::less<Vec3f::U>(),
+ boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
EndPoint* end_pos = NULL;
if(res_it != velist[axis].end())
@@ -574,9 +574,9 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<FCL_REAL>(),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+ boost::bind(std::less<Vec3f::U>(),
+ boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
EndPoint* end_pos = NULL;
if(res_it != velist[axis].end())
diff --git a/trunk/fcl/src/ccd/taylor_model.cpp b/trunk/fcl/src/ccd/taylor_model.cpp
index 769985ab..0851ed88 100644
--- a/trunk/fcl/src/ccd/taylor_model.cpp
+++ b/trunk/fcl/src/ccd/taylor_model.cpp
@@ -98,6 +98,7 @@ TaylorModel TaylorModel::operator + (FCL_REAL d) const
TaylorModel& TaylorModel::operator += (FCL_REAL d)
{
coeffs_[0] += d;
+ return *this;
}
TaylorModel TaylorModel::operator + (const TaylorModel& other) const
diff --git a/trunk/fcl/src/narrowphase/gjk.cpp b/trunk/fcl/src/narrowphase/gjk.cpp
index 2813b2f1..8419d010 100644
--- a/trunk/fcl/src/narrowphase/gjk.cpp
+++ b/trunk/fcl/src/narrowphase/gjk.cpp
@@ -158,6 +158,8 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
return Vec3f(0, 0, 0);
}
break;
+ default:
+ ; // nothing
}
return Vec3f(0, 0, 0);
--
GitLab