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
0d64ce1e
Commit
0d64ce1e
authored
Mar 11, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 11, 2016
Browse files
Add math details based on Eigen library.
parent
77eb79e4
Changes
7
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
0d64ce1e
...
...
@@ -2,7 +2,7 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2014 CNRS-LAAS
# Author: Florent Lamiraux
# Author: Florent Lamiraux
, Joseph Mirabel
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
...
...
@@ -35,6 +35,7 @@
cmake_minimum_required
(
VERSION 2.8
)
set
(
CXX_DISABLE_WERROR TRUE
)
include
(
cmake/base.cmake
)
include
(
cmake/eigen.cmake
)
include
(
cmake/boost.cmake
)
set
(
PROJECT_NAME hpp-fcl
)
...
...
@@ -45,6 +46,17 @@ set(PROJECT_URL "http://github.com/humanoid-path-planner/hpp-fcl")
setup_project
()
set
(
FCL_HAVE_SSE FALSE CACHE BOOL
"Enable SSE vectorization"
)
set
(
FCL_HAVE_EIGEN FALSE CACHE BOOL
"Use eigen wrappers"
)
add_optional_dependency
(
"eigen3 >= 3.0.0"
)
if
(
EIGEN3_FOUND
)
set
(
FCL_HAVE_EIGEN TRUE
)
if
(
FCL_HAVE_EIGEN
)
include_directories
(
${
EIGEN3_INCLUDE_DIRS
}
)
endif
(
FCL_HAVE_EIGEN
)
endif
(
EIGEN3_FOUND
)
# Required dependencies
add_required_dependency
(
"ccd >= 1.4"
)
set
(
BOOST_COMPONENTS
...
...
@@ -156,6 +168,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/collision_object.h
include/hpp/fcl/octree.h
include/hpp/fcl/fwd.hh
include/hpp/fcl/eigen/math_details.h
)
add_subdirectory
(
src
)
...
...
include/hpp/fcl/config-fcl.hh.in
View file @
0d64ce1e
...
...
@@ -37,6 +37,7 @@
# include "config.h"
#cmakedefine01 FCL_HAVE_EIGEN
#cmakedefine01 FCL_HAVE_SSE
#cmakedefine01 FCL_HAVE_OCTOMAP
#cmakedefine01 FCL_HAVE_FLANN
...
...
include/hpp/fcl/eigen/math_details.h
0 → 100644
View file @
0d64ce1e
/*
* 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.
*/
#ifndef FCL_EIGEN_DETAILS_H
#define FCL_EIGEN_DETAILS_H
#include
<Eigen/Dense>
#include
<Eigen/Geometry>
namespace
fcl
{
namespace
details
{
template
<
typename
T
>
struct
eigen_wrapper_v3
{
typedef
T
meta_type
;
typedef
Eigen
::
Matrix
<
T
,
3
,
1
>
vector_type
;
vector_type
v
;
eigen_wrapper_v3
()
{
setValue
(
0
);
}
template
<
typename
Derived
>
eigen_wrapper_v3
(
const
Eigen
::
MatrixBase
<
Derived
>&
value
)
:
v
(
value
)
{}
eigen_wrapper_v3
(
T
x
)
:
v
(
vector_type
::
Constant
(
x
))
{}
eigen_wrapper_v3
(
T
*
x
)
:
v
(
vector_type
::
Constant
(
*
x
))
{}
eigen_wrapper_v3
(
T
x
,
T
y
,
T
z
)
:
v
(
x
,
y
,
z
)
{}
inline
void
setValue
(
T
x
,
T
y
,
T
z
)
{
v
<<
x
,
y
,
z
;
}
inline
void
setValue
(
T
x
)
{
v
.
setConstant
(
x
);
}
inline
void
negate
()
{
v
*=
-
1
;
}
inline
eigen_wrapper_v3
<
T
>&
ubound
(
const
eigen_wrapper_v3
<
T
>&
u
)
{
v
=
v
.
cwiseMin
(
u
.
v
);
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
lbound
(
const
eigen_wrapper_v3
<
T
>&
l
)
{
v
=
v
.
cwiseMax
(
l
.
v
);
return
*
this
;
}
T
operator
[]
(
size_t
i
)
const
{
return
v
[
i
];
}
T
&
operator
[]
(
size_t
i
)
{
return
v
[
i
];
}
inline
eigen_wrapper_v3
<
T
>
operator
+
(
const
eigen_wrapper_v3
<
T
>&
other
)
const
{
return
eigen_wrapper_v3
<
T
>
(
v
+
other
.
v
);
}
inline
eigen_wrapper_v3
<
T
>
operator
-
(
const
eigen_wrapper_v3
<
T
>&
other
)
const
{
return
eigen_wrapper_v3
<
T
>
(
v
-
other
.
v
);
}
inline
eigen_wrapper_v3
<
T
>
operator
*
(
const
eigen_wrapper_v3
<
T
>&
other
)
const
{
return
eigen_wrapper_v3
<
T
>
(
v
.
cwiseProduct
(
other
.
v
));
}
inline
eigen_wrapper_v3
<
T
>
operator
/
(
const
eigen_wrapper_v3
<
T
>&
other
)
const
{
return
(
eigen_wrapper_v3
<
T
>
(
v
)
/=
other
);
}
inline
eigen_wrapper_v3
<
T
>&
operator
+=
(
const
eigen_wrapper_v3
<
T
>&
other
)
{
v
+=
other
.
v
;
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
operator
-=
(
const
eigen_wrapper_v3
<
T
>&
other
)
{
v
-=
other
.
v
;
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
operator
*=
(
const
eigen_wrapper_v3
<
T
>&
other
)
{
v
=
v
.
cwiseProduct
(
other
.
v
);
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
operator
/=
(
const
eigen_wrapper_v3
<
T
>&
other
)
{
v
=
v
.
cwiseQuotient
(
other
.
v
);
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>
operator
+
(
T
t
)
const
{
return
eigen_wrapper_v3
<
T
>
((
v
.
array
()
+
t
).
matrix
());
}
inline
eigen_wrapper_v3
<
T
>
operator
-
(
T
t
)
const
{
return
eigen_wrapper_v3
<
T
>
((
v
.
array
()
-
t
).
matrix
());
}
inline
eigen_wrapper_v3
<
T
>
operator
*
(
T
t
)
const
{
return
eigen_wrapper_v3
<
T
>
(
v
*
t
);
}
inline
eigen_wrapper_v3
<
T
>
operator
/
(
T
t
)
const
{
return
eigen_wrapper_v3
<
T
>
(
v
/
t
);
}
inline
eigen_wrapper_v3
<
T
>&
operator
+=
(
T
t
)
{
v
.
array
()
+=
t
;
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
operator
-=
(
T
t
)
{
v
.
array
()
-=
t
;
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
operator
*=
(
T
t
)
{
v
.
array
()
*=
t
;
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>&
operator
/=
(
T
t
)
{
v
.
array
()
/=
t
;
return
*
this
;
}
inline
eigen_wrapper_v3
<
T
>
operator
-
()
const
{
return
eigen_wrapper_v3
<
T
>
(
-
v
);
}
};
template
<
typename
T
>
static
inline
eigen_wrapper_v3
<
T
>
cross_prod
(
const
eigen_wrapper_v3
<
T
>&
l
,
const
eigen_wrapper_v3
<
T
>&
r
)
{
return
eigen_wrapper_v3
<
T
>
(
l
.
v
.
cross
(
r
.
v
));
}
template
<
typename
T
>
static
inline
T
dot_prod3
(
const
eigen_wrapper_v3
<
T
>&
l
,
const
eigen_wrapper_v3
<
T
>&
r
)
{
return
l
.
v
.
dot
(
r
.
v
);
}
template
<
typename
T
>
static
inline
eigen_wrapper_v3
<
T
>
min
(
const
eigen_wrapper_v3
<
T
>&
x
,
const
eigen_wrapper_v3
<
T
>&
y
)
{
return
eigen_wrapper_v3
<
T
>
(
x
.
v
.
cwiseMin
(
y
.
v
));
}
template
<
typename
T
>
static
inline
eigen_wrapper_v3
<
T
>
max
(
const
eigen_wrapper_v3
<
T
>&
x
,
const
eigen_wrapper_v3
<
T
>&
y
)
{
return
eigen_wrapper_v3
<
T
>
(
x
.
v
.
cwiseMax
(
y
.
v
));
}
template
<
typename
T
>
static
inline
eigen_wrapper_v3
<
T
>
abs
(
const
eigen_wrapper_v3
<
T
>&
x
)
{
return
eigen_wrapper_v3
<
T
>
(
x
.
v
.
cwiseAbs
());
}
template
<
typename
T
>
static
inline
bool
equal
(
const
eigen_wrapper_v3
<
T
>&
x
,
const
eigen_wrapper_v3
<
T
>&
y
,
T
epsilon
)
{
return
((
x
.
v
-
y
.
v
).
cwiseAbs
().
array
()
<
epsilon
).
all
();
}
namespace
internal
{
template
<
typename
Derived
,
int
Size
>
struct
assign
{
static
Eigen
::
Matrix
<
typename
Derived
::
Scalar
,
4
,
1
>
run
(
const
Eigen
::
MatrixBase
<
Derived
>&
value
)
{
assert
(
false
);
}
};
template
<
typename
Derived
>
struct
assign
<
Derived
,
3
>
{
static
Eigen
::
Matrix
<
typename
Derived
::
Scalar
,
4
,
1
>
run
(
const
Eigen
::
MatrixBase
<
Derived
>&
value
)
{
return
(
Eigen
::
Matrix
<
typename
Derived
::
Scalar
,
4
,
1
>
()
<<
value
,
0
).
finished
();
}
};
template
<
typename
Derived
>
struct
assign
<
Derived
,
4
>
{
static
Eigen
::
Matrix
<
typename
Derived
::
Scalar
,
4
,
1
>
run
(
const
Eigen
::
MatrixBase
<
Derived
>&
value
)
{
return
value
;
}
};
}
template
<
typename
T
>
struct
eigen_wrapper_v4
{
typedef
T
meta_type
;
typedef
Eigen
::
Matrix
<
T
,
4
,
1
>
vector_type
;
vector_type
v
;
eigen_wrapper_v4
()
{
setValue
(
0
);
}
template
<
typename
Derived
>
eigen_wrapper_v4
(
const
Eigen
::
MatrixBase
<
Derived
>&
value
)
:
v
(
internal
::
assign
<
Derived
,
Derived
::
SizeAtCompileTime
>::
run
(
value
))
{}
eigen_wrapper_v4
(
T
x
)
:
v
(
vector_type
::
Constant
(
x
))
{
v
[
3
]
=
0
;
}
eigen_wrapper_v4
(
T
*
x
)
:
v
(
vector_type
::
Constant
(
*
x
))
{
v
[
3
]
=
0
;
}
eigen_wrapper_v4
(
T
x
,
T
y
,
T
z
)
:
v
(
x
,
y
,
z
,
0
)
{}
inline
typename
vector_type
::
template
FixedSegmentReturnType
<
3
>
::
Type
d
()
{
return
v
.
template
head
<
3
>
();
}
inline
typename
vector_type
::
template
ConstFixedSegmentReturnType
<
3
>
::
Type
d
()
const
{
return
v
.
template
head
<
3
>
();
}
inline
void
setValue
(
T
x
,
T
y
,
T
z
,
T
w
=
0
)
{
v
<<
x
,
y
,
z
,
w
;
}
inline
void
setValue
(
T
x
)
{
d
().
setConstant
(
x
);
v
[
3
]
=
0
;
}
inline
void
negate
()
{
v
*=
-
1
;
}
inline
eigen_wrapper_v4
<
T
>&
ubound
(
const
eigen_wrapper_v4
<
T
>&
u
)
{
v
=
v
.
cwiseMin
(
u
.
v
);
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
lbound
(
const
eigen_wrapper_v4
<
T
>&
l
)
{
v
=
v
.
cwiseMax
(
l
.
v
);
return
*
this
;
}
T
operator
[]
(
size_t
i
)
const
{
return
v
[
i
];
}
T
&
operator
[]
(
size_t
i
)
{
return
v
[
i
];
}
inline
eigen_wrapper_v4
<
T
>
operator
+
(
const
eigen_wrapper_v4
<
T
>&
other
)
const
{
return
eigen_wrapper_v4
<
T
>
(
v
+
other
.
v
);
}
inline
eigen_wrapper_v4
<
T
>
operator
-
(
const
eigen_wrapper_v4
<
T
>&
other
)
const
{
return
eigen_wrapper_v4
<
T
>
(
v
-
other
.
v
);
}
inline
eigen_wrapper_v4
<
T
>
operator
*
(
const
eigen_wrapper_v4
<
T
>&
other
)
const
{
return
eigen_wrapper_v4
<
T
>
(
v
.
cwiseProduct
(
other
.
v
));
}
inline
eigen_wrapper_v4
<
T
>
operator
/
(
const
eigen_wrapper_v4
<
T
>&
other
)
const
{
return
(
eigen_wrapper_v4
<
T
>
(
v
)
/=
other
);
}
inline
eigen_wrapper_v4
<
T
>&
operator
+=
(
const
eigen_wrapper_v4
<
T
>&
other
)
{
v
+=
other
.
v
;
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
operator
-=
(
const
eigen_wrapper_v4
<
T
>&
other
)
{
v
-=
other
.
v
;
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
operator
*=
(
const
eigen_wrapper_v4
<
T
>&
other
)
{
v
=
v
.
cwiseProduct
(
other
.
v
);
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
operator
/=
(
const
eigen_wrapper_v4
<
T
>&
other
)
{
d
()
=
d
().
cwiseQuotient
(
other
.
d
());
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>
operator
+
(
T
t
)
const
{
return
eigen_wrapper_v4
<
T
>
((
d
().
array
()
+
t
).
matrix
());
}
inline
eigen_wrapper_v4
<
T
>
operator
-
(
T
t
)
const
{
return
eigen_wrapper_v4
<
T
>
((
d
().
array
()
-
t
).
matrix
());
}
inline
eigen_wrapper_v4
<
T
>
operator
*
(
T
t
)
const
{
return
eigen_wrapper_v4
<
T
>
(
v
*
t
);
}
inline
eigen_wrapper_v4
<
T
>
operator
/
(
T
t
)
const
{
return
eigen_wrapper_v4
<
T
>
(
v
/
t
);
}
inline
eigen_wrapper_v4
<
T
>&
operator
+=
(
T
t
)
{
d
().
array
()
+=
t
;
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
operator
-=
(
T
t
)
{
d
().
array
()
-=
t
;
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
operator
*=
(
T
t
)
{
v
.
array
()
*=
t
;
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>&
operator
/=
(
T
t
)
{
v
.
array
()
/=
t
;
return
*
this
;
}
inline
eigen_wrapper_v4
<
T
>
operator
-
()
const
{
return
eigen_wrapper_v4
<
T
>
(
-
v
);
}
}
__attribute__
((
aligned
));
template
<
typename
T
>
static
inline
eigen_wrapper_v4
<
T
>
cross_prod
(
const
eigen_wrapper_v4
<
T
>&
l
,
const
eigen_wrapper_v4
<
T
>&
r
)
{
return
eigen_wrapper_v4
<
T
>
(
l
.
v
.
cross3
(
r
.
v
));
}
template
<
typename
T
>
static
inline
T
dot_prod3
(
const
eigen_wrapper_v4
<
T
>&
l
,
const
eigen_wrapper_v4
<
T
>&
r
)
{
return
l
.
v
.
dot
(
r
.
v
);
}
template
<
typename
T
>
static
inline
eigen_wrapper_v4
<
T
>
min
(
const
eigen_wrapper_v4
<
T
>&
x
,
const
eigen_wrapper_v4
<
T
>&
y
)
{
return
eigen_wrapper_v4
<
T
>
(
x
.
v
.
cwiseMin
(
y
.
v
));
}
template
<
typename
T
>
static
inline
eigen_wrapper_v4
<
T
>
max
(
const
eigen_wrapper_v4
<
T
>&
x
,
const
eigen_wrapper_v4
<
T
>&
y
)
{
return
eigen_wrapper_v4
<
T
>
(
x
.
v
.
cwiseMax
(
y
.
v
));
}
template
<
typename
T
>
static
inline
eigen_wrapper_v4
<
T
>
abs
(
const
eigen_wrapper_v4
<
T
>&
x
)
{
return
eigen_wrapper_v4
<
T
>
(
x
.
v
.
cwiseAbs
());
}
template
<
typename
T
>
static
inline
bool
equal
(
const
eigen_wrapper_v4
<
T
>&
x
,
const
eigen_wrapper_v4
<
T
>&
y
,
T
epsilon
)
{
return
((
x
.
v
-
y
.
v
).
cwiseAbs
().
array
()
<
epsilon
).
all
();
}
template
<
typename
T
>
struct
eigen_wrapper_m3
{
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
;
typedef
typename
matrix_type
::
ConstRowXpr
ConstRowXpr
;
matrix_type
m
;
eigen_wrapper_m3
()
{};
template
<
typename
Derived
>
eigen_wrapper_m3
(
const
Eigen
::
MatrixBase
<
Derived
>&
matrix
)
:
m
(
matrix
)
{}
eigen_wrapper_m3
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
{
setValue
(
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
);
}
eigen_wrapper_m3
(
const
vector_type
&
v1
,
const
vector_type
&
v2
,
const
vector_type
&
v3
)
{
m
<<
v1
.
v
,
v2
.
v
,
v3
.
v
;
}
eigen_wrapper_m3
(
const
eigen_wrapper_m3
<
T
>&
other
)
{
m
=
other
.
m
;
}
inline
inner_col_type
getColumn
(
size_t
i
)
const
{
return
m
.
col
(
i
);
}
inline
ConstRowXpr
getRow
(
size_t
i
)
const
{
return
m
.
row
(
i
);
}
inline
T
operator
()
(
size_t
i
,
size_t
j
)
const
{
return
m
(
i
,
j
);
}
inline
T
&
operator
()
(
size_t
i
,
size_t
j
)
{
return
m
(
i
,
j
);
}
inline
vector_type
operator
*
(
const
vector_type
&
v
)
const
{
return
vector_type
(
m
*
v
.
v
);
}
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
-
(
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
*=
(
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
;
}
void
setIdentity
()
{
m
.
setIdentity
();
}
void
setZero
()
{
m
.
setZero
();
}
static
const
eigen_wrapper_m3
<
T
>&
getIdentity
()
{
static
const
eigen_wrapper_m3
<
T
>
I
(
matrix_type
::
Identity
());
return
I
;
}
T
determinant
()
const
{
return
m
.
determinant
();
}
eigen_wrapper_m3
<
T
>&
transpose
()
{
m
.
transposeInPlace
();
return
*
this
;
}
eigen_wrapper_m3
<
T
>&
inverse
()
{
m
=
m
.
inverse
().
eval
();
return
*
this
;
}
eigen_wrapper_m3
<
T
>
transposeTimes
(
const
eigen_wrapper_m3
<
T
>&
other
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
.
transpose
()
*
other
.
m
);
}
eigen_wrapper_m3
<
T
>
timesTranspose
(
const
eigen_wrapper_m3
<
T
>&
other
)
const
{
return
eigen_wrapper_m3
<
T
>
(
m
*
other
.
transpose
());
}
vector_type
transposeTimes
(
const
vector_type
&
other
)
const
{
return
vector_type
(
m
.
transpose
()
*
other
.
v
);
}
inline
T
transposeDotX
(
const
vector_type
&
other
)
const
{
return
m
.
col
(
0
).
dot
(
other
.
v
);
}
inline
T
transposeDotY
(
const
vector_type
&
other
)
const
{
return
m
.
col
(
1
).
dot
(
other
.
v
);
}
inline
T
transposeDotZ
(
const
vector_type
&
other
)
const
{
return
m
.
col
(
2
).
dot
(
other
.
v
);
}
inline
T
transposeDot
(
size_t
i
,
const
vector_type
&
other
)
const
{
return
m
.
col
(
i
).
dot
(
other
.
v
);
}
inline
T
dotX
(
const
vector_type
&
other
)
const
{
return
m
.
row
(
0
).
dot
(
other
.
v
);
}
inline
T
dotY
(
const
vector_type
&
other
)
const
{
return
m
.
row
(
1
).
dot
(
other
.
v
);
}
inline
T
dotZ
(
const
vector_type
&
other
)
const
{
return
m
.
row
(
2
).
dot
(
other
.
v
);
}
inline
T
dot
(
size_t
i
,
const
vector_type
&
other
)
const
{
return
m
.
row
(
i
).
dot
(
other
.
v
);
}
inline
void
setValue
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
{
m
<<
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
;
}
inline
void
setValue
(
T
x
)
{
m
.
setConstant
(
x
);
}
};
template
<
typename
T
>
eigen_wrapper_m3
<
T
>
abs
(
const
eigen_wrapper_m3
<
T
>&
m
)
{
return
eigen_wrapper_m3
<
T
>
(
m
.
m
.
cwiseAbs
());
}
template
<
typename
T
>
eigen_wrapper_m3
<
T
>
transpose
(
const
eigen_wrapper_m3
<
T
>&
m
)
{
return
eigen_wrapper_m3
<
T
>
(
m
.
m
.
transpose
());
}
template
<
typename
T
>
eigen_wrapper_m3
<
T
>
inverse
(
const
eigen_wrapper_m3
<
T
>&
m
)
{
return
eigen_wrapper_m3
<
T
>
(
m
.
m
.
inverse
().
eval
());
}
}
}
#endif
include/hpp/fcl/math/matrix_3f.h
View file @
0d64ce1e
...
...
@@ -466,7 +466,9 @@ typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename
}
#if FCL_HAVE_SSE
#if FCL_HAVE_EIGEN
typedef
Matrix3fX
<
details
::
eigen_wrapper_m3
<
FCL_REAL
>
>
Matrix3f
;
#elif FCL_HAVE_SSE
typedef
Matrix3fX
<
details
::
sse_meta_f12
>
Matrix3f
;
#else
typedef
Matrix3fX
<
details
::
Matrix3Data
<
FCL_REAL
>
>
Matrix3f
;
...
...
include/hpp/fcl/math/vec_3f.h
View file @
0d64ce1e
...
...
@@ -42,7 +42,9 @@
#include
<hpp/fcl/data_types.h>
#include
<hpp/fcl/math/math_details.h>
#if FCL_HAVE_SSE
#if FCL_HAVE_EIGEN
#include
<hpp/fcl/eigen/math_details.h>
#elif FCL_HAVE_SSE
#include
<hpp/fcl/simd/math_simd_details.h>
#endif
...
...
@@ -233,7 +235,9 @@ void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
}
}
#if FCL_HAVE_SSE
#if FCL_HAVE_EIGEN
typedef
Vec3fX
<
details
::
eigen_wrapper_v3
<
FCL_REAL
>
>
Vec3f
;
#elif FCL_HAVE_SSE
typedef
Vec3fX
<
details
::
sse_meta_f4
>
Vec3f
;
#else
typedef
Vec3fX
<
details
::
Vec3Data
<
FCL_REAL
>
>
Vec3f
;
...
...
test/CMakeLists.txt
View file @
0d64ce1e
...
...
@@ -37,4 +37,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
)
test/test_fcl_math.cpp
View file @
0d64ce1e
...
...
@@ -37,6 +37,9 @@
#define BOOST_TEST_MODULE "FCL_MATH"
#include
<boost/test/included/unit_test.hpp>
#if FCL_HAVE_EIGEN
#include
<hpp/fcl/eigen/math_details.h>
#endif
#if FCL_HAVE_SSE
#include
<hpp/fcl/simd/math_simd_details.h>
#endif
...
...
@@ -163,6 +166,471 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
BOOST_CHECK
(
v1
.
dot
(
v2
)
==
26
);
}
#if FCL_HAVE_EIGEN
BOOST_AUTO_TEST_CASE
(
vec_test_eigen_vec32
)
{
typedef
Vec3fX
<
details
::
eigen_wrapper_v3
<
float
>
>
Vec3f32
;