Commit 25a89cca authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Clean code and fix bugs. Test are passing.

parent f1c3c6c2
......@@ -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})
......
......@@ -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]);
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -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
{
......
template <typename Derived>
IVector3& operator=(const FclType<Derived>& other)
{
const Vec3f& tmp = other.derived();
setValue (tmp);
return *this;
}
#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
......@@ -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)
{
......
......@@ -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
......
......@@ -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];
......
<
......@@ -46,4 +46,3 @@ if (FCL_HAVE_OCTOMAP)