Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
25a89cca
Commit
25a89cca
authored
Mar 15, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 15, 2016
Browse files
Clean code and fix bugs. Test are passing.
parent
f1c3c6c2
Changes
12
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
25a89cca
...
...
@@ -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
View file @
25a89cca
...
...
@@ -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
View file @
25a89cca
...
...
@@ -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
View file @
25a89cca
...
...
@@ -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
View file @
25a89cca
...
...
@@ -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-matrix.h
0 → 100644
View file @
25a89cca
include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
0 → 100644
View file @
25a89cca
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
0 → 100644
View file @
25a89cca
#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
View file @
25a89cca
...
...
@@ -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
.
d
ot
(
y
.
cross
(
z
));
return
x
.
d
erived
().
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
View file @
25a89cca
...
...
@@ -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
View file @
25a89cca
...
...
@@ -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
View file @
25a89cca
...
...
@@ -46,4 +46,3 @@ if (FCL_HAVE_OCTOMAP)