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
699fb081
Commit
699fb081
authored
Mar 15, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 15, 2016
Browse files
Work in progress
parent
d447c557
Changes
10
Expand all
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/eigen/math_details.h
View file @
699fb081
...
...
@@ -436,6 +436,7 @@ static inline bool equal(const eigen_v3<T>& x, const eigen_v3<T>& y, T epsilon)
template
<
typename
T
>
struct
eigen_wrapper_m3
{
typedef
T
meta_type
;
typedef
eigen_wrapper_v3
<
T
>
vector_type
;
typedef
Eigen
::
Matrix
<
T
,
3
,
3
,
Eigen
::
RowMajor
>
matrix_type
;
typedef
Eigen
::
Matrix
<
T
,
3
,
1
>
inner_col_type
;
...
...
@@ -477,19 +478,19 @@ struct eigen_wrapper_m3
inline
eigen_wrapper_m3
<
T
>
operator
+
(
const
eigen_wrapper_m3
<
T
>&
other
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
+
other
.
m
);
}
inline
eigen_wrapper_m3
<
T
>
operator
-
(
const
eigen_wrapper_m3
<
T
>&
other
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
-
other
.
m
);
}
inline
eigen_wrapper_m3
<
T
>
operator
+
(
meta_type
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
+
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
-
(
meta_type
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
-
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
*
(
meta_type
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
*
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
/
(
meta_type
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
/
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
+
(
T
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
+
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
-
(
T
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
-
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
*
(
T
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
*
c
);
}
inline
eigen_wrapper_m3
<
T
>
operator
/
(
T
c
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
/
c
);
}
inline
eigen_wrapper_m3
<
T
>&
operator
*=
(
const
eigen_wrapper_m3
<
T
>&
other
)
{
m
*=
other
.
m
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
+=
(
const
eigen_wrapper_m3
<
T
>&
other
)
{
m
+=
other
.
m
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
-=
(
const
eigen_wrapper_m3
<
T
>&
other
)
{
m
-=
other
.
m
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
+=
(
meta_type
c
)
{
m
.
array
()
+=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
-=
(
meta_type
c
)
{
m
.
array
()
-=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
*=
(
meta_type
c
)
{
m
.
array
()
*=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
/=
(
meta_type
c
)
{
m
.
array
()
/=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
+=
(
T
c
)
{
m
.
array
()
+=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
-=
(
T
c
)
{
m
.
array
()
-=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
*=
(
T
c
)
{
m
.
array
()
*=
c
;
return
*
this
;
}
inline
eigen_wrapper_m3
<
T
>&
operator
/=
(
T
c
)
{
m
.
array
()
/=
c
;
return
*
this
;
}
void
setIdentity
()
{
m
.
setIdentity
();
}
...
...
@@ -678,14 +679,14 @@ struct eigen_m3 :
}
template
<
typename
OtherDerived
>
const
Eigen
::
ProductReturnType
<
eigen_m3
<
T
>
,
OtherDerived
>::
Type
const
typename
Eigen
::
ProductReturnType
<
eigen_m3
<
T
>
,
OtherDerived
>::
Type
transposeTimes
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
Base
::
transpose
()
*
other
;
}
template
<
typename
OtherDerived
>
const
Eigen
::
ProductReturnType
<
eigen_m3
<
T
>
,
OtherDerived
>::
Type
const
typename
Eigen
::
ProductReturnType
<
eigen_m3
<
T
>
,
OtherDerived
>::
Type
timesTranspose
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
operator
*
(
other
.
Base
::
transpose
());
...
...
include/hpp/fcl/eigen/matrix_3fx.h
View file @
699fb081
...
...
@@ -43,35 +43,52 @@ namespace fcl
/// @brief Matrix2 class wrapper. the core data is in the template parameter class
template
<
typename
T
>
class
Matrix3fX
:
Eigen
::
Matrix
<
T
,
3
,
1
>
class
Matrix3fX
:
public
Eigen
::
Matrix
<
T
,
3
,
3
,
Eigen
::
RowMajor
>
{
public:
// typedef typename T::vector_type S;
Matrix3fX
()
{}
Matrix3fX
(
U
xx
,
U
xy
,
U
xz
,
U
yx
,
U
yy
,
U
yz
,
U
zx
,
U
zy
,
U
zz
)
:
data
(
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
)
{}
typedef
Eigen
::
Matrix
<
T
,
3
,
3
,
Eigen
::
RowMajor
>
Base
;
typedef
typename
Base
::
ColXpr
ColXpr
;
typedef
typename
Base
::
ConstColXpr
ConstColXpr
;
typedef
typename
Base
::
RowXpr
RowXpr
;
typedef
typename
Base
::
ConstRowXpr
ConstRowXpr
;
Matrix3fX
(
const
Vec3fX
<
S
>&
v1
,
const
Vec3fX
<
S
>&
v2
,
const
Vec3fX
<
S
>&
v3
)
:
data
(
v1
.
data
,
v2
.
data
,
v3
.
data
)
{}
Matrix3fX
(
const
Matrix3fX
<
T
>&
other
)
:
data
(
other
.
data
)
{}
Matrix3fX
(
void
)
:
Base
(
)
{}
Matrix3fX
(
const
T
&
data_
)
:
data
(
data_
)
{}
inline
Vec3fX
<
S
>
getColumn
(
size_t
i
)
const
{
return
Vec3fX
<
S
>
(
data
.
getColumn
(
i
));
}
// This constructor allows you to construct MyVectorType from Eigen expressions
template
<
typename
OtherDerived
>
Matrix3fX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
:
Base
(
other
)
{}
inline
Vec3fX
<
S
>
getRow
(
size_t
i
)
const
{
return
Vec3fX
<
S
>
(
data
.
getRow
(
i
));
}
// This method allows you to assign Eigen expressions to MyVectorType
template
<
typename
OtherDerived
>
Matrix3fX
&
operator
=
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
{
this
->
Base
::
operator
=
(
other
);
return
*
this
;
}
Matrix3fX
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
:
Base
((
Base
()
<<
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
).
finished
())
{}
template
<
typename
OtherDerived
>
Matrix3fX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v1
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v2
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v3
)
:
Base
((
Base
()
<<
v1
,
v2
,
v3
).
finished
())
{}
inline
ColXpr
getColumn
(
size_t
i
)
{
return
this
->
col
(
i
);
}
inline
RowXpr
getRow
(
size_t
i
)
{
return
this
->
row
(
i
);
}
inline
ConstColXpr
getColumn
(
size_t
i
)
const
{
return
this
->
col
(
i
);
}
inline
ConstRowXpr
getRow
(
size_t
i
)
const
{
return
this
->
row
(
i
);
}
/*
inline U operator () (size_t i, size_t j) const
{
return data(i, j);
...
...
@@ -168,6 +185,7 @@ public:
{
data.setIdentity();
}
// */
inline
bool
isIdentity
()
const
{
...
...
@@ -177,10 +195,12 @@ public:
data
(
2
,
0
)
==
0
&&
data
(
2
,
1
)
==
0
&&
data
(
2
,
2
)
==
1
;
}
/*
inline void setZero()
{
data.setZero();
}
// */
/// @brief Set the matrix from euler angles YPR around ZYX axes
/// @param eulerX Roll about X axis
...
...
@@ -218,150 +238,175 @@ public:
setEulerZYX
(
roll
,
pitch
,
yaw
);
}
/*
inline U determinant() const
{
return data.determinant();
}
// */
Matrix3fX
<
T
>&
transpose
()
{
data
.
transpose
();
this
->
transpose
InPlace
();
return
*
this
;
}
Matrix3fX
<
T
>&
inverse
()
{
data
.
inverse
();
*
this
=
this
->
inverse
().
eval
();
return
*
this
;
}
Matrix3fX
<
T
>&
abs
()
{
data
.
a
bs
();
*
this
=
this
->
cwiseA
bs
();
return
*
this
;
}
static
const
Matrix3fX
<
T
>&
getIdentity
()
{
static
const
Matrix3fX
<
T
>
I
((
U
)
1
,
(
U
)
0
,
(
U
)
0
,
(
U
)
0
,
(
U
)
1
,
(
U
)
0
,
(
U
)
0
,
(
U
)
0
,
(
U
)
1
);
static
const
Matrix3fX
<
T
>
I
((
T
)
1
,
(
T
)
0
,
(
T
)
0
,
(
T
)
0
,
(
T
)
1
,
(
T
)
0
,
(
T
)
0
,
(
T
)
0
,
(
T
)
1
);
return
I
;
}
Matrix3fX
<
T
>
transposeTimes
(
const
Matrix3fX
<
T
>&
other
)
const
template
<
typename
OtherDerived
>
const
typename
Eigen
::
ProductReturnType
<
Eigen
::
Transpose
<
Matrix3fX
<
T
>
>
,
OtherDerived
>::
Type
transposeTimes
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
Matrix3fX
<
T
>
(
data
.
transposeTimes
(
other
.
data
))
;
return
this
->
Base
::
transpose
()
*
other
;
}
Matrix3fX
<
T
>
timesTranspose
(
const
Matrix3fX
<
T
>&
other
)
const
template
<
typename
OtherDerived
>
const
typename
Eigen
::
ProductReturnType
<
Matrix3fX
<
T
>
,
Eigen
::
Transpose
<
OtherDerived
>
>
::
Type
timesTranspose
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
Matrix3fX
<
T
>
(
data
.
timesTranspose
(
other
.
data
)
);
return
*
this
*
other
.
transpose
(
);
}
/*
Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
{
return Vec3fX<S>(data.transposeTimes(v.data));
}
// */
Matrix3fX
<
T
>
tensorTransform
(
const
Matrix3fX
<
T
>&
m
)
const
template
<
typename
OtherDerived
>
const
Eigen
::
ProductReturnType
<
Eigen
::
ProductReturnType
<
Matrix3fX
<
T
>
,
OtherDerived
>::
Type
,
Eigen
::
Transpose
<
Matrix3fX
<
T
>
>
>
::
Type
tensorTransform
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
m
)
const
{
Matrix3fX
<
T
>
res
(
*
this
);
res
*=
m
;
return
res
.
timesTranspose
(
*
this
);
return
(
*
this
*
m
)
*
this
->
Base
::
transpose
();
// Matrix3fX<T> res(*this);
// res *= m;
// return res.timesTranspose(*this);
}
// (1 0 0)^T (*this)^T v
inline
U
transposeDotX
(
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
transposeDotX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
transposeDot
(
0
,
v
.
data
);
return
transposeDot
(
0
,
v
);
}
// (0 1 0)^T (*this)^T v
inline
U
transposeDotY
(
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
transposeDotY
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
transposeDot
(
1
,
v
.
data
);
return
transposeDot
(
1
,
v
);
}
// (0 0 1)^T (*this)^T v
inline
U
transposeDotZ
(
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
transposeDotZ
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
transposeDot
(
2
,
v
.
data
);
return
transposeDot
(
2
,
v
);
}
// (\delta_{i3})^T (*this)^T v
inline
U
transposeDot
(
size_t
i
,
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
transposeDot
(
size_t
i
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
transposeDot
(
i
,
v
.
data
);
return
this
->
col
(
i
).
dot
(
v
);
}
// (1 0 0)^T (*this) v
inline
U
dotX
(
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
dotX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
dot
(
0
,
v
.
data
);
return
dot
(
0
,
v
);
}
// (0 1 0)^T (*this) v
inline
U
dotY
(
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
dotY
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
dot
(
1
,
v
.
data
);
return
dot
(
1
,
v
);
}
// (0 0 1)^T (*this) v
inline
U
dotZ
(
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
dotZ
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
dot
(
2
,
v
.
data
);
return
dot
(
2
,
v
);
}
// (\delta_{i3})^T (*this) v
inline
U
dot
(
size_t
i
,
const
Vec3fX
<
S
>&
v
)
const
template
<
typename
OtherDerived
>
inline
T
dot
(
size_t
i
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
data
.
dot
(
i
,
v
.
data
);
return
this
->
row
(
i
).
dot
(
v
);
}
inline
void
setValue
(
U
xx
,
U
xy
,
U
xz
,
U
yx
,
U
yy
,
U
yz
,
U
zx
,
U
zy
,
U
zz
)
inline
void
setValue
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
{
data
.
setValue
(
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
)
;
(
*
this
)(
0
,
0
)
=
xx
;
(
*
this
)(
0
,
1
)
=
xy
;
(
*
this
)(
0
,
2
)
=
xz
;
(
*
this
)(
1
,
0
)
=
yx
;
(
*
this
)(
1
,
1
)
=
yy
;
(
*
this
)(
1
,
2
)
=
yz
;
(
*
this
)(
2
,
0
)
=
zx
;
(
*
this
)(
2
,
1
)
=
zy
;
(
*
this
)(
2
,
2
)
=
zz
;
}
inline
void
setValue
(
U
x
)
inline
void
setValue
(
T
x
)
{
data
.
setValue
(
x
);
this
->
setConstant
(
x
);
}
};
template
<
typename
T
>
void
hat
(
Matrix3fX
<
T
>&
mat
,
const
Vec3fX
<
typename
T
::
vector_type
>&
vec
)
template
<
typename
T
,
typename
OtherDerived
>
void
hat
(
Matrix3fX
<
T
>&
mat
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
vec
)
{
mat
.
setValue
(
0
,
-
vec
[
2
],
vec
[
1
],
vec
[
2
],
0
,
-
vec
[
0
],
-
vec
[
1
],
vec
[
0
],
0
);
}
template
<
typename
T
>
void
relativeTransform
(
const
Matrix3fX
<
T
>&
R1
,
const
Vec3fX
<
typename
T
::
vector_type
>&
t1
,
const
Matrix3fX
<
T
>&
R2
,
const
Vec3fX
<
typename
T
::
vector_type
>&
t2
,
Matrix3fX
<
T
>&
R
,
Vec3fX
<
typename
T
::
vector_type
>&
t
)
template
<
typename
T
,
typename
OtherDerived
>
void
relativeTransform
(
const
Matrix3fX
<
T
>&
R1
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
t1
,
const
Matrix3fX
<
T
>&
R2
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
t2
,
Matrix3fX
<
T
>&
R
,
Eigen
::
MatrixBase
<
OtherDerived
>&
t
)
{
R
=
R1
.
transposeTimes
(
R2
);
t
=
R1
.
transposeTimes
(
t2
-
t1
);
}
/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
template
<
typename
T
>
void
eigen
(
const
Matrix3fX
<
T
>&
m
,
typename
T
::
meta_type
dout
[
3
],
Vec3fX
<
typename
T
::
vector_type
>
vout
[
3
])
template
<
typename
T
,
typename
Derived
>
void
eigen
(
const
Matrix3fX
<
T
>&
m
,
T
dout
[
3
],
const
Eigen
::
MatrixBase
<
Derived
>
vout
[
3
])
{
Matrix3fX
<
T
>
R
(
m
);
int
n
=
3
;
int
j
,
iq
,
ip
,
i
;
typename
T
::
meta_type
tresh
,
theta
,
tau
,
t
,
sm
,
s
,
h
,
g
,
c
;
T
tresh
,
theta
,
tau
,
t
,
sm
,
s
,
h
,
g
,
c
;
int
nrot
;
typename
T
::
meta_type
b
[
3
];
typename
T
::
meta_type
z
[
3
];
typename
T
::
meta_type
v
[
3
][
3
]
=
{{
1
,
0
,
0
},
{
0
,
1
,
0
},
{
0
,
0
,
1
}};
typename
T
::
meta_type
d
[
3
];
T
b
[
3
];
T
z
[
3
];
T
v
[
3
][
3
]
=
{{
1
,
0
,
0
},
{
0
,
1
,
0
},
{
0
,
0
,
1
}};
T
d
[
3
];
for
(
ip
=
0
;
ip
<
n
;
++
ip
)
{
...
...
@@ -439,25 +484,26 @@ void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename
}
template
<
typename
T
>
Matrix3fX
<
T
>
abs
(
const
Matrix3fX
<
T
>&
R
)
const
typename
CwiseUnaryOp
<
internal
::
scalar_abs_op
<
Scalar
>
,
const
Matrix3fX
<
T
>
>
abs
(
const
Matrix3fX
<
T
>&
R
)
{
return
Matrix3fX
<
T
>
(
abs
(
R
.
data
)
);
return
R
.
cwiseAbs
(
);
}
template
<
typename
T
>
Matrix3fX
<
T
>
transpose
(
const
Matrix3fX
<
T
>&
R
)
typename
Eigen
::
Transpose
<
Matrix3fX
<
T
>
>
transpose
(
const
Matrix3fX
<
T
>&
R
)
{
return
Matrix3fX
<
T
>
(
transpose
(
R
.
data
)
);
return
R
.
Matrix3fX
<
T
>
::
Base
::
transpose
();
}
template
<
typename
T
>
Matrix3fX
<
T
>
inverse
(
const
Matrix3fX
<
T
>&
R
)
{
return
Matrix3fX
<
T
>
(
inverse
(
R
.
data
)
);
return
R
.
Matrix3fX
<
T
>
::
Base
::
inverse
().
eval
(
);
}
template
<
typename
T
>
typename
T
::
meta_type
quadraticForm
(
const
Matrix3fX
<
T
>&
R
,
const
Vec3fX
<
typename
T
::
vector_type
>&
v
)
template
<
typename
T
,
typename
Derived
>
T
quadraticForm
(
const
Matrix3fX
<
T
>&
R
,
const
Eigen
::
MatrixBase
<
Derived
>&
v
)
{
return
v
.
dot
(
R
*
v
);
}
...
...
include/hpp/fcl/eigen/product.h
0 → 100644
View file @
699fb081
namespace
internal
{
template
<
typename
Derived
,
typename
OtherDerived
,
bool
coefwise
>
struct
deduce_fcl_type
{};
template
<
typename
Derived
,
typename
OtherDerived
>
struct
deduce_fcl_type
<
Derived
,
OtherDerived
,
false
>
{
typedef
typename
ProductReturnType
<
Derived
,
OtherDerived
>::
Type
Type
;
};
template
<
typename
Derived
,
typename
OtherDerived
>
struct
deduce_fcl_type
<
Derived
,
OtherDerived
,
true
>
{
typedef
CwiseBinaryOp
<
scalar_multiple_op
<
typename
Derived
::
Scalar
>
,
const
Derived
,
const
OtherDerived
>
Type
;
};
}
template
<
typename
Derived
,
typename
OtherDerived
>
struct
FclProduct
{
enum
{
COEFWISE
=
Derived
::
ColsAtCompileTime
==
1
&&
OtherDerived
::
ColsAtCompileTime
==
1
};
typedef
FclOp
<
typename
internal
::
deduce_fcl_type
<
Derived
,
OtherDerived
,
COEFWISE
>::
Type
>
ProductType
;
typedef
FclOp
<
typename
ProductReturnType
<
Transpose
<
Derived
>
,
OtherDerived
>::
Type
>
TransposeTimesType
;
typedef
FclOp
<
typename
ProductReturnType
<
Derived
,
Transpose
<
OtherDerived
>
>::
Type
>
TimesTransposeType
;
typedef
FclOp
<
typename
ProductReturnType
<
ProductType
,
Transpose
<
Derived
>
>::
Type
>
TensorTransformType
;
static
EIGEN_STRONG_INLINE
ProductType
run
(
const
Derived
&
l
,
const
OtherDerived
&
r
)
{
return
ProductType
(
l
,
r
);
}
};
#define FCL_EIGEN_MAKE_PRODUCT_OPERATOR() \
template <typename OtherDerived> \
EIGEN_STRONG_INLINE const typename FclProduct<const FCL_EIGEN_CURRENT_CLASS, const OtherDerived>::ProductType \
operator*(const MatrixBase<OtherDerived>& other) const \
{ \
return FclProduct<const FCL_EIGEN_CURRENT_CLASS, const OtherDerived>::run (*this, other.derived()); \
}
include/hpp/fcl/eigen/vec_3fx.h
View file @
699fb081
This diff is collapsed.
Click to expand it.
include/hpp/fcl/math/math_details.h
View file @
699fb081
...
...
@@ -36,6 +36,9 @@
#ifndef FCL_MATH_DETAILS_H
#define FCL_MATH_DETAILS_H
#if FCL_HAVE_EIGEN
# error "This file should not be included when compiling with Eigen library"
#endif
#include
<cmath>
#include
<algorithm>
...
...
include/hpp/fcl/math/matrix_3f.h
View file @
699fb081
...
...
@@ -51,9 +51,9 @@ namespace fcl
#if FCL_HAVE_EIGEN
# if FCL_USE_NATIVE_EIGEN
typedef
Matrix
3fX
<
FCL_REAL
>
Matrix3f
;
typedef
Eigen
::
Fcl
Matrix
<
FCL_REAL
,
3
,
0
>
Matrix3f
;
# else
typedef
Matrix3fX
<
details
::
eigen_m3
<
FCL_REAL
>
>
Matrix3f
;
typedef
Matrix3fX
<
details
::
eigen_
wrapper_
m3
<
FCL_REAL
>
>
Matrix3f
;
# endif
#elif FCL_HAVE_SSE
typedef
Matrix3fX
<
details
::
sse_meta_f12
>
Matrix3f
;
...
...
include/hpp/fcl/math/vec_3f.h
View file @
699fb081
...
...
@@ -41,12 +41,6 @@
#include
<hpp/fcl/config-fcl.hh>
#include
<hpp/fcl/data_types.h>
#if FCL_USE_NATIVE_EIGEN
# include <hpp/fcl/eigen/vec_3fx.h>
#else
# include <hpp/fcl/math/vec_3fx.h>
#endif
#if FCL_HAVE_EIGEN
# if ! FCL_USE_NATIVE_EIGEN
# include <hpp/fcl/eigen/math_details.h>
...
...
@@ -57,6 +51,12 @@
# include <hpp/fcl/math/math_details.h>
#endif
#if FCL_USE_NATIVE_EIGEN
# include <hpp/fcl/eigen/vec_3fx.h>
#else
# include <hpp/fcl/math/vec_3fx.h>
#endif
#include
<cmath>
#include
<iostream>
#include
<limits>
...
...
@@ -67,9 +67,9 @@ namespace fcl
#if FCL_HAVE_EIGEN
# if FCL_USE_NATIVE_EIGEN
typedef
Vec3fX
<
FCL_REAL
>
Vec3f
;
typedef
Eigen
::
FclMatrix
<
FCL_REAL
,
1
,
0
>
Vec3f
;
# else
typedef
Vec3fX
<
details
::
eigen_v3
<
FCL_REAL
>
>
Vec3f
;
typedef
Vec3fX
<
details
::
eigen_
wrapper_
v3
<
FCL_REAL
>
>
Vec3f
;
# endif
#elif FCL_HAVE_SSE
typedef
Vec3fX
<
details
::
sse_meta_f4
>
Vec3f
;
...
...
include/hpp/fcl/math/vec_3fx.h
View file @
699fb081
...
...
@@ -41,8 +41,6 @@
#include
<hpp/fcl/config-fcl.hh>
#include
<hpp/fcl/data_types.h>
#include
<hpp/fcl/math/math_details.h>
#include
<cmath>
#include
<iostream>
#include
<limits>
...
...
test/CMakeLists.txt
View file @
699fb081
config_files
(
fcl_resources/config.h
)
macro
(
add_fcl_template_test test_name
)
add_executable
(
${
ARGV
}
)
target_link_libraries
(
${
test_name
}
${
Boost_LIBRARIES
}
)
add_test
(
${
test_name
}
${
EXECUTABLE_OUTPUT_PATH
}
/
${
test_name
}
)
endmacro
(
add_fcl_template_test
)
macro
(
add_fcl_test test_name
)
add_executable
(
${
ARGV
}
)
target_link_libraries
(
${
test_name
}
...
...
@@ -37,5 +45,5 @@ add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
if
(
FCL_HAVE_OCTOMAP
)
add_fcl_test
(
test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp
)
endif
()
add_fcl_test
(
test_fcl_math test_fcl_math.cpp
)
add_fcl_template_test
(
test_fcl_eigen test_fcl_eigen.cpp
)
test/test_fcl_eigen.cpp
0 → 100644
View file @
699fb081
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#define BOOST_TEST_MODULE "FCL_EIGEN"
#include
<boost/test/included/unit_test.hpp>
#include
<hpp/fcl/config-fcl.hh>