Clean code and fix bugs. Test are passing.

CMakeLists.txt

@@ -50,7 +50,7 @@ set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
 set(FCL_USE_NATIVE_EIGEN FALSE CACHE BOOL "Use native eigen matrix type when possible")
 
 add_optional_dependency("eigen3 >= 3.0.0")
-set(FCL_HAVE_EIGEN EIGEN3_FOUND CACHE BOOL "Use eigen wrappers")
+set(FCL_HAVE_EIGEN ${EIGEN3_FOUND} CACHE BOOL "Use eigen wrappers")
 if (EIGEN3_FOUND)
   if (FCL_HAVE_EIGEN)
     include_directories(${EIGEN3_INCLUDE_DIRS})

include/hpp/fcl/BV/BV.h

@@ -283,7 +283,7 @@ public:
 
     const Matrix3f& R = tf1.getRotation();
     bool left_hand = (id[0] == (id[1] + 1) % 3);
-    bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
+    if (left_hand) bv2.axis[0] = -R.getColumn(id[0]); else bv2.axis[0] = R.getColumn(id[0]);
     bv2.axis[1] = R.getColumn(id[1]);
     bv2.axis[2] = R.getColumn(id[2]);    
   }

include/hpp/fcl/ccd/interval_matrix.h

@@ -99,6 +99,10 @@ struct IMatrix3
   IMatrix3& rotationConstrain();
 
   void print() const;
+
+#ifdef FCL_CCD_INTERVALMATRIX_PLUGIN
+# include FCL_CCD_INTERVALMATRIX_PLUGIN
+#endif
 };
 
 IMatrix3 rotationConstrain(const IMatrix3& m);

include/hpp/fcl/ccd/interval_vector.h

@@ -156,6 +156,10 @@ struct IVector3
 
   bool overlap(const IVector3& v) const;
   bool contain(const IVector3& v) const;
+
+#ifdef FCL_CCD_INTERVALVECTOR_PLUGIN
+# include FCL_CCD_INTERVALVECTOR_PLUGIN
+#endif
 };
 
 IVector3 bound(const IVector3& i, const Vec3f& v);

include/hpp/fcl/ccd/taylor_vector.h

@@ -41,6 +41,10 @@
 #include <hpp/fcl/ccd/interval_vector.h>
 #include <hpp/fcl/ccd/taylor_model.h>
 
+#if FCL_USE_NATIVE_EIGEN
+#include <hpp/fcl/eigen/taylor_operator.h>
+#endif
+
 namespace fcl
 {
 

include/hpp/fcl/eigen/plugins/ccd/interval-vector.h

+template <typename Derived>
+IVector3& operator=(const FclType<Derived>& other)
+{
+  const Vec3f& tmp = other.derived();
+  setValue (tmp);
+  return *this;
+}

include/hpp/fcl/eigen/taylor_operator.h

+#ifndef FCL_CCD_TAYLOR_OPERATOR_H
+#define FCL_CCD_TAYLOR_OPERATOR_H
+
+#include <hpp/fcl/math/vec_3f.h>
+#include <hpp/fcl/math/matrix_3f.h>
+
+namespace fcl {
+
+class TVector3;
+class TMatrix3;
+
+template<int Col> struct TaylorReturnType {};
+template<> struct TaylorReturnType<1> { typedef TVector3 type; typedef Vec3f    eigen_type; };
+template<> struct TaylorReturnType<3> { typedef TMatrix3 type; typedef Matrix3f eigen_type; };
+
+template<typename Derived>
+typename TaylorReturnType<Derived::ColsAtCompileTime>::type operator * (const FclType<Derived>& v, const TaylorModel& a)
+{
+  const typename TaylorReturnType<Derived::ColsAtCompileTime>::eigen_type b = v.derived();
+  return b * a;
+}
+
+}
+
+#endif // FCL_CCD_TAYLOR_OPERATOR_H

include/hpp/fcl/eigen/vec_3fx.h

@@ -41,12 +41,16 @@
 #include <hpp/fcl/config-fcl.hh>
 #include <hpp/fcl/data_types.h>
 
-#include <hpp/fcl/eigen/math_details.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
 
 #include <cmath>
 #include <iostream>
 #include <limits>
 
+#define FCL_CCD_INTERVALVECTOR_PLUGIN <hpp/fcl/eigen/plugins/ccd/interval-vector.h>
+#define FCL_CCD_MATRIXVECTOR_PLUGIN <hpp/fcl/eigen/plugins/ccd/interval-matrix.h>
+
 #define FCL_EIGEN_EXPOSE_PARENT_TYPE(Type) typedef typename Base::Type Type;
 
 #define FCL_EIGEN_MAKE_CWISE_BINARY_OP(METHOD,FUNCTOR) \
@@ -114,6 +118,26 @@
   FCL_EIGEN_MATRIX_DOT_AXIS(NAME,Y,1)\
   FCL_EIGEN_MATRIX_DOT_AXIS(NAME,Z,2)
 
+#define FCL_EIGEN_MAKE_CROSS() \
+  template<typename OtherDerived> \
+  EIGEN_STRONG_INLINE typename BinaryReturnType<FCL_EIGEN_CURRENT_CLASS,OtherDerived>::Cross \
+  cross (const MatrixBase<OtherDerived>& other) const { return this->Base::cross (other); }
+
+#define FCL_EIGEN_MAKE_DOT() \
+  template <typename OtherDerived> \
+  EIGEN_STRONG_INLINE Scalar dot (const MatrixBase<OtherDerived>& other) const \
+  { return this->Base::dot (other.derived()); }
+
+namespace fcl {
+template <typename Derived>
+class FclType
+{
+  public:
+    Derived& derived () { return static_cast<Derived&> (*this); }
+    const Derived& derived () const { return static_cast<const Derived&> (*this); }
+};
+}
+
 namespace Eigen {
   template <typename T> class FclOp;
   template <typename T, int Cols, int Options> class FclMatrix;
@@ -123,24 +147,6 @@ namespace Eigen {
   namespace internal {
 
     template<typename T> struct traits< FclOp<T> > : traits <T> {};
-    // template<typename T>
-      // struct traits< FclOp<T> >
-      // {
-        // typedef T Scalar;
-        // typedef FclOp<T> This;
-        // typedef traits<typename This::Base> traits_base;
-        // typedef typename traits_base::StorageKind StorageKind;
-        // typedef typename traits_base::Index Index;
-        // typedef typename traits_base::XprKind XprKind;
-        // enum {
-          // RowsAtCompileTime = traits_base::RowsAtCompileTime,
-          // ColsAtCompileTime = traits_base::ColsAtCompileTime,
-          // MaxRowsAtCompileTime = traits_base::MaxRowsAtCompileTime,
-          // MaxColsAtCompileTime = traits_base::MaxColsAtCompileTime,
-          // Flags = traits_base::Flags,
-          // CoeffReadCost = traits_base::CoeffReadCost
-        // };
-      // };
     template<typename T, int Cols, int _Options>
       struct traits< FclMatrix<T, Cols, _Options> >
       {
@@ -197,13 +203,14 @@ namespace Eigen {
         const CwiseBinaryOp<internal::scalar_max_op<Scalar>,
               const Derived, const OtherDerived>
                 > Max;
+      typedef FclMatrix <Scalar, 1, 0> Cross;
     };
 
 #define FCL_EIGEN_CURRENT_CLASS FclMatrix
 
 /// @brief Vector3 class wrapper. The core data is in the template parameter class.
 template <typename T, int Cols, int _Options>
-class FclMatrix : public Matrix <T, 3, Cols, _Options>
+class FclMatrix : public Matrix <T, 3, Cols, _Options>, public ::fcl::FclType<FclMatrix <T, Cols, _Options> >
 {
 public:
   typedef Matrix <T, 3, Cols, _Options> Base;
@@ -213,7 +220,11 @@ public:
   FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstColXpr)
   FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstRowXpr)
 
-  FclMatrix(void): Base() {}
+  // Compatibility with other Matrix3fX and Vec3f
+  typedef T U;
+  typedef T meta_type;
+
+  FclMatrix(void): Base(Base::Zero()) {}
 
   // This constructor allows you to construct MyVectorType from Eigen expressions
   template<typename OtherDerived>
@@ -239,6 +250,16 @@ public:
     setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz);
   }
 
+  template <typename Vector>
+  FclMatrix(const ::fcl::FclType<Vector>& r0,
+            const ::fcl::FclType<Vector>& r1,
+            const ::fcl::FclType<Vector>& r2) : Base()
+  {
+    this->row(0) = r0.derived();
+    this->row(1) = r1.derived();
+    this->row(2) = r2.derived();
+  }
+
   /// @brief create vector (x, x, x)
   FclMatrix(T x) : Base(Base::Constant (x)) {}
 
@@ -259,6 +280,8 @@ public:
   FCL_EIGEN_MATRIX_DOT(dot,row)
   FCL_EIGEN_MATRIX_DOT(transposeDot,col)
 
+  FCL_EIGEN_MAKE_DOT()
+
   FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator+,internal::scalar_sum_op)
   FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator-,internal::scalar_difference_op)
   // Map this to scalar product or matrix product depending on the Cols.
@@ -283,10 +306,10 @@ public:
   FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator-=)
   FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator*=)
   FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator/=)
-  inline const typename UnaryReturnType<FclMatrix>::Opposite operator - () const { return typename UnaryReturnType<FclMatrix>::Opposite(*this); }
+  inline const typename UnaryReturnType<FclMatrix>::Opposite operator-() const { return typename UnaryReturnType<FclMatrix>::Opposite(*this); }
   // There is no class for cross 
   // inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
-  // inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
+  FCL_EIGEN_MAKE_CROSS()
   inline FclMatrix& normalize()
   {
     T sqr_length = this->squaredNorm();
@@ -299,7 +322,7 @@ public:
     T sqr_length = this->squaredNorm();
     if(sqr_length > 0)
     {
-      this->operator/= ((T)sqrt(sqr_length));
+      this->operator/= ((T)std::sqrt(sqr_length));
       *signal = true;
     }
     else
@@ -405,13 +428,58 @@ public:
     const typename FclProduct<const FclMatrix,const OtherDerived>::ProductType left (*this, other.derived());
     return typename FclProduct<const FclMatrix,const OtherDerived>::TensorTransformType (left, t);
   }
+
+  static const FclMatrix& getIdentity()
+  {
+    static const FclMatrix I((T)1, (T)0, (T)0,
+                             (T)0, (T)1, (T)0,
+                             (T)0, (T)0, (T)1);
+    return I;
+  }
+
+  /// @brief Set the matrix from euler angles YPR around ZYX axes
+  /// @param eulerX Roll about X axis
+  /// @param eulerY Pitch around Y axis
+  /// @param eulerZ Yaw aboud Z axis
+  ///  
+  /// These angles are used to produce a rotation matrix. The euler
+  /// angles are applied in ZYX order. I.e a vector is first rotated 
+  /// about X then Y and then Z
+  inline void setEulerZYX(Scalar eulerX, Scalar eulerY, Scalar eulerZ)
+  {
+    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(FclMatrix, 3, 3);
+    Scalar ci(std::cos(eulerX));
+    Scalar cj(std::cos(eulerY));
+    Scalar ch(std::cos(eulerZ));
+    Scalar si(std::sin(eulerX));
+    Scalar sj(std::sin(eulerY));
+    Scalar sh(std::sin(eulerZ));
+    Scalar cc = ci * ch;
+    Scalar cs = ci * sh;
+    Scalar sc = si * ch;
+    Scalar ss = si * sh;
+
+    setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+             cj * sh, sj * ss + cc, sj * cs - sc, 
+             -sj,     cj * si,      cj * ci);
+
+  }
+
+  /// @brief Set the matrix from euler angles using YPR around YXZ respectively
+  /// @param yaw Yaw about Y axis
+  /// @param pitch Pitch about X axis
+  /// @param roll Roll about Z axis 
+  void setEulerYPR(Scalar yaw, Scalar pitch, Scalar roll)
+  {
+    setEulerZYX(roll, pitch, yaw);
+  }
 };
 
 #undef FCL_EIGEN_CURRENT_CLASS
 #define FCL_EIGEN_CURRENT_CLASS FclOp
 
 template <typename EigenOp>
-class FclOp : public EigenOp
+class FclOp : public EigenOp, public ::fcl::FclType<FclOp <EigenOp> >
 {
 public:
   typedef typename internal::traits<EigenOp>::Scalar T;
@@ -430,6 +498,10 @@ public:
   FclOp (Lhs& lhs, const Rhs& rhs)
     : Base (lhs, rhs) {}
 
+  template<typename OtherEigenOp>
+  FclOp (const FclOp<OtherEigenOp>& other)
+    : Base (other) {}
+
   template<typename XprType>
   FclOp (XprType& xpr)
     : Base (xpr) {}
@@ -437,14 +509,13 @@ public:
   Base& base () { return *this; }
   const Base& base () const { return *this; }
 
-  // template<typename Lhs, typename Rhs, typename BinaryOp>
-  // FclOp (const CwiseBinaryOp<BinaryOp, Lhs, Rhs>& o)
-    // : Base (o.lhs(), o.rhs(), o.functor()) {}
 
   FCL_EIGEN_MAKE_GET_COL_ROW()
   FCL_EIGEN_MATRIX_DOT(dot,row)
   FCL_EIGEN_MATRIX_DOT(transposeDot,col)
 
+  FCL_EIGEN_MAKE_DOT()
+
   FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator+,internal::scalar_sum_op)
   FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator-,internal::scalar_difference_op)
   // Map this to scalar product or matrix product depending on the Cols.
@@ -461,6 +532,9 @@ public:
   }
   FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator*,internal::scalar_multiple_op)
   FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator/,internal::scalar_quotient1_op)
+  inline const typename UnaryReturnType<const FclOp>::Opposite operator-() const { return typename UnaryReturnType<const FclOp>::Opposite(*this); }
+
+  FCL_EIGEN_MAKE_CROSS()
 
   inline const typename UnaryReturnType<EigenOp>::Normalize
     normalize() const
@@ -492,19 +566,8 @@ public:
     return typename UnaryReturnType<EigenOp>::Abs (this->derived());
   }
 
-  inline T length() const { return this->norm(); }
-  // inline T norm() const { return sqrt(details::dot_prod3(data, data)); }
-  inline T sqrLength() const { return this->squaredNorm(); }
-  // inline T squaredNorm() const { return details::dot_prod3(data, data); }
-  /* Useless
-  inline void setValue(T x, T y, T z) {
-    this->m_storage.data()[0] = x;
-    this->m_storage.data()[1] = y;
-    this->m_storage.data()[2] = z;
-  }
-  inline void setValue(T x) { this->setConstant (x); }
-  inline void setZero () {data.setValue (0); }
-  //*/
+  inline Scalar length() const { return this->norm(); }
+  inline Scalar sqrLength() const { return this->squaredNorm(); }
 
   template <typename Derived>
   inline bool equal(const Eigen::MatrixBase<Derived>& other, T epsilon = std::numeric_limits<T>::epsilon() * 100) const
@@ -536,11 +599,7 @@ public:
   bool isZero() const
   {
     return this->isZero ();
-    // (this->m_storage.data()[0] == 0)
-      // &&   (this->m_storage.data()[1] == 0)
-      // &&   (this->m_storage.data()[2] == 0);
   }
-  // */
 
   const FclOp<Transpose<const FclOp> > transpose () const {
     EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(EigenOp, 3, 3);
@@ -549,6 +608,7 @@ public:
 
   // const FclOp<internal::inverse_impl<FclOp> > inverse () const { return FclOp<Transpose<FclOp> >(*this); }
 };
+
 }
 
 namespace fcl
@@ -564,54 +624,49 @@ static inline Eigen::FclMatrix<T, 1, _Options> normalize(const Eigen::FclMatrix<
     return v;
 }
 
-template<typename T, int _Options>
-static inline T triple(
-    const Eigen::FclMatrix<T, 1, _Options>& x,
-    const Eigen::FclMatrix<T, 1, _Options>& y,
-    const Eigen::FclMatrix<T, 1, _Options>& z)
+template<typename Derived>
+static inline typename Derived::Scalar triple(
+    const FclType<Derived>& x,
+    const FclType<Derived>& y,
+    const FclType<Derived>& z)
 {
-  return x.dot(y.cross(z));
+  return x.derived().dot(y.derived().cross(z.derived()));
 }
 
-// template<typename T, int _Options>
-// std::ostream& operator << (std::ostream& out, const Eigen::FclMatrix<T>& x)
-// {
-  // out << x[0] << " " << x[1] << " " << x[2];
-  // return out;
-// }
 
-template<typename T, int _Options>
-static inline const typename Eigen::BinaryReturnType<
-  const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options>
-  >::Min
- min(const Eigen::FclMatrix<T, 1, _Options>& x, const Eigen::FclMatrix<T, 1, _Options>& y)
+template<typename Derived, typename OtherDerived>
+static inline const typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min
+ min(const FclType<Derived>& x, const FclType<OtherDerived>& y)
 {
-  return typename Eigen::BinaryReturnType<const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options> >::Min (x, y);
+  return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min (x.derived(), y.derived());
 }
 
-template<typename T, int _Options>
-static inline const typename Eigen::BinaryReturnType<
-  const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options>
-  >::Max
- max(const Eigen::FclMatrix<T, 1, _Options>& x, const Eigen::FclMatrix<T, 1, _Options>& y)
+template<typename Derived, typename OtherDerived>
+static inline const typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max
+ max(const FclType<Derived>& x, const FclType<OtherDerived>& y)
 {
-  return typename Eigen::BinaryReturnType<const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options> >::Max (x, y);
+  return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max (x.derived(), y.derived());
 }
 
-template<typename T, int _Cols, int _Options>
-// static inline Vec3fX<T> abs(const Vec3fX<T>& x)
-static inline const typename Eigen::UnaryReturnType<const Eigen::FclMatrix<T, _Cols, _Options> >
-abs(const Eigen::FclMatrix<T, _Cols, _Options>& x)
+template<typename Derived>
+static inline const typename Eigen::UnaryReturnType<const Derived>::Abs
+abs(const FclType<Derived>& x)
 {
-  return typename Eigen::UnaryReturnType<const Eigen::FclMatrix<T, _Cols, _Options> >::Abs (x);
+  return typename Eigen::UnaryReturnType<const Derived>::Abs (x.derived());
 }
 
-template<typename T, int _Options>
+template<typename Derived>
 void generateCoordinateSystem(
-    const Eigen::FclMatrix<T, 1, _Options>& w,
-    const Eigen::FclMatrix<T, 1, _Options>& u,
-    const Eigen::FclMatrix<T, 1, _Options>& v)
+    FclType<Derived>& _w,
+    FclType<Derived>& _u,
+    FclType<Derived>& _v)
 {
+  typedef typename Derived::Scalar T;
+
+  Derived& w = _w.derived();
+  Derived& u = _u.derived();
+  Derived& v = _v.derived();
+
   T inv_length;
   if(std::abs(w[0]) >= std::abs(w[1]))
   {
@@ -662,10 +717,10 @@ void relativeTransform(const Matrix& R1, const Vector& t1,
 
 /// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
 template<typename Matrix, typename Vector>
-void eigen(const Matrix& m, typename Matrix::Scalar dout[3], Vector vout[3])
+void eigen(const FclType<Matrix>& m, typename Matrix::Scalar dout[3], Vector* vout)
 {
   typedef typename Matrix::Scalar Scalar;
-  Matrix R(m);
+  Matrix R(m.derived());
   int n = 3;
   int j, iq, ip, i;
   Scalar tresh, theta, tau, t, sm, s, h, g, c;
@@ -750,12 +805,6 @@ void eigen(const Matrix& m, typename Matrix::Scalar dout[3], Vector vout[3])
   return;
 }
 
-// template<typename >
-// Matrix abs(const Matrix& R) 
-// {
-  // return R.abs());
-// }
-
 template<typename T, int _Options>
 Eigen::FclOp<Eigen::Transpose<const Eigen::FclMatrix<T,3,_Options> > > transpose(const Eigen::FclMatrix<T, 3, _Options>& R)
 {

include/hpp/fcl/math/matrix_3f.h

@@ -40,9 +40,7 @@
 
 #include <hpp/fcl/math/vec_3f.h>
 
-#if FCL_USE_NATIVE_EIGEN
-# include <hpp/fcl/eigen/matrix_3fx.h>
-#else
+#if ! FCL_USE_NATIVE_EIGEN
 # include <hpp/fcl/math/matrix_3fx.h>
 #endif
 

src/narrowphase/narrowphase.cpp

@@ -1882,7 +1882,7 @@ bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1,
   FCL_REAL depth = - std::abs(signed_dist) + s1.radius;
   if(depth >= 0)
   {
-    if(normal) *normal = (signed_dist > 0) ? -new_s2.n : new_s2.n;
+    if(normal) if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n;
     if(penetration_depth) *penetration_depth = depth;
     if(contact_points) *contact_points = center - new_s2.n * signed_dist;
 
@@ -1958,7 +1958,7 @@ bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
 
   // compute the contact point by project the deepest point onto the plane
   if(penetration_depth) *penetration_depth = depth;
-  if(normal) *normal = (signed_dist > 0) ? -new_s2.n : new_s2.n;
+  if(normal) if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n;
   if(contact_points) *contact_points = p - new_s2.n * new_s2.signedDistance(p);
 
   return true;
@@ -2025,13 +2025,13 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
       {
         if(penetration_depth) *penetration_depth = abs_d1 + s1.radius;
         if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
-        if(normal) *normal = (d1 < 0) ? -new_s2.n : new_s2.n;
+        if(normal) if (d1 < 0) *normal = -new_s2.n; else *normal = new_s2.n;
       }
       else
       {
         if(penetration_depth) *penetration_depth = abs_d2 + s1.radius;
         if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
-        if(normal) *normal = (d2 < 0) ? -new_s2.n : new_s2.n;
+        if(normal) if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n;
       }
       return true;
     }
@@ -2062,7 +2062,7 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
         }
       }
         
-      if(normal) *normal = (d1 < 0) ? new_s2.n : -new_s2.n;
+      if(normal) if (d1 < 0) *normal = new_s2.n; else *normal = -new_s2.n;
       return true;
     }
   }
@@ -2117,7 +2117,7 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
       else
       {
         if(penetration_depth) *penetration_depth = depth;
-        if(normal) *normal = (d < 0) ? new_s2.n : -new_s2.n;
+        if(normal) if (d < 0) *normal = new_s2.n; else *normal = -new_s2.n;
         if(contact_points) *contact_points = T - new_s2.n * d;
         return true;
       }
@@ -2161,13 +2161,13 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
         {
           if(penetration_depth) *penetration_depth = abs_d2;
           if(contact_points) *contact_points = c2 - new_s2.n * d2;
-          if(normal) *normal = (d2 < 0) ? -new_s2.n : new_s2.n;
+          if(normal) if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n;
         }
         else
         {
           if(penetration_depth) *penetration_depth = abs_d1;
           if(contact_points) *contact_points = c1 - new_s2.n * d1;
-          if(normal) *normal = (d1 < 0) ? -new_s2.n : new_s2.n;
+          if(normal) if (d1 < 0) *normal = -new_s2.n; else *normal = new_s2.n;
         }
         return true;
       }
@@ -2197,7 +2197,7 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
     else
     {
       if(penetration_depth) *penetration_depth = depth;
-      if(normal) *normal = (d < 0) ? new_s2.n : -new_s2.n;
+      if(normal) if (d < 0) *normal = new_s2.n; else *normal = -new_s2.n;
       if(contact_points) *contact_points = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d;
       return true;
     }
@@ -2248,7 +2248,7 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
       }
 
       if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative);
-      if(normal) *normal = (d_positive > d_negative) ? -new_s2.n : new_s2.n;
+      if(normal) if (d_positive > d_negative) *normal = -new_s2.n; else *normal = new_s2.n;
       if(contact_points)
       {
         Vec3f p[2];
@@ -2367,7 +2367,7 @@ bool planeTriangleIntersect(const Plane& s1, const Transform3f& tf1,
     }
 
     if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative);
-    if(normal) *normal = (d_positive > d_negative) ? new_s1.n : -new_s1.n;
+    if(normal) if (d_positive > d_negative) *normal = new_s1.n; else *normal = -new_s1.n;
     if(contact_points)
     {
       Vec3f p[2];

test/CMakeLists.txt

@@ -46,4 +46,3 @@ if (FCL_HAVE_OCTOMAP)