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
9e9f5359
Commit
9e9f5359
authored
Mar 15, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 15, 2016
Browse files
Remove unused file and fix CMakeLists.txt
parent
25a89cca
Changes
2
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
9e9f5359
...
...
@@ -169,6 +169,11 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/octree.h
include/hpp/fcl/fwd.hh
include/hpp/fcl/eigen/math_details.h
include/hpp/fcl/eigen/vec_3fx.h
include/hpp/fcl/eigen/product.h
include/hpp/fcl/eigen/taylor_operator.h
include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
)
add_subdirectory
(
src
)
...
...
include/hpp/fcl/eigen/matrix_3fx.h
deleted
100644 → 0
View file @
25a89cca
/*
* 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.
*/
/** \author Jia Pan */
#ifndef FCL_EIGEN_MATRIX_3F_H
#define FCL_EIGEN_MATRIX_3F_H
namespace
fcl
{
/// @brief Matrix2 class wrapper. the core data is in the template parameter class
template
<
typename
T
>
class
Matrix3fX
:
public
Eigen
::
Matrix
<
T
,
3
,
3
,
Eigen
::
RowMajor
>
{
public:
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
(
void
)
:
Base
()
{}
// This constructor allows you to construct MyVectorType from Eigen expressions
template
<
typename
OtherDerived
>
Matrix3fX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
:
Base
(
other
)
{}
// 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);
}
inline U& operator () (size_t i, size_t j)
{
return data(i, j);
}
inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
{
return Vec3fX<S>(data * v.data);
}
inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
{
return Matrix3fX<T>(data * m.data);
}
inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
{
return Matrix3fX<T>(data + other.data);
}
inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
{
return Matrix3fX<T>(data - other.data);
}
inline Matrix3fX<T> operator + (U c) const
{
return Matrix3fX<T>(data + c);
}
inline Matrix3fX<T> operator - (U c) const
{
return Matrix3fX<T>(data - c);
}
inline Matrix3fX<T> operator * (U c) const
{
return Matrix3fX<T>(data * c);
}
inline Matrix3fX<T> operator / (U c) const
{
return Matrix3fX<T>(data / c);
}
inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
{
data *= other.data;
return *this;
}
inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
{
data += other.data;
return *this;
}
inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
{
data -= other.data;
return *this;
}
inline Matrix3fX<T>& operator += (U c)
{
data += c;
return *this;
}
inline Matrix3fX<T>& operator -= (U c)
{
data -= c;
return *this;
}
inline Matrix3fX<T>& operator *= (U c)
{
data *= c;
return *this;
}
inline Matrix3fX<T>& operator /= (U c)
{
data /= c;
return *this;
}
inline void setIdentity()
{
data.setIdentity();
}
// */
inline
bool
isIdentity
()
const
{
return
data
(
0
,
0
)
==
1
&&
data
(
0
,
1
)
==
0
&&
data
(
0
,
2
)
==
0
&&
data
(
1
,
0
)
==
0
&&
data
(
1
,
1
)
==
1
&&
data
(
1
,
2
)
==
0
&&
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
/// @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
(
FCL_REAL
eulerX
,
FCL_REAL
eulerY
,
FCL_REAL
eulerZ
)
{
FCL_REAL
ci
(
cos
(
eulerX
));
FCL_REAL
cj
(
cos
(
eulerY
));
FCL_REAL
ch
(
cos
(
eulerZ
));
FCL_REAL
si
(
sin
(
eulerX
));
FCL_REAL
sj
(
sin
(
eulerY
));
FCL_REAL
sh
(
sin
(
eulerZ
));
FCL_REAL
cc
=
ci
*
ch
;
FCL_REAL
cs
=
ci
*
sh
;
FCL_REAL
sc
=
si
*
ch
;
FCL_REAL
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
(
FCL_REAL
yaw
,
FCL_REAL
pitch
,
FCL_REAL
roll
)
{
setEulerZYX
(
roll
,
pitch
,
yaw
);
}
/*
inline U determinant() const
{
return data.determinant();
}
// */
Matrix3fX
<
T
>&
transpose
()
{
this
->
transposeInPlace
();
return
*
this
;
}
Matrix3fX
<
T
>&
inverse
()
{
*
this
=
this
->
inverse
().
eval
();
return
*
this
;
}
Matrix3fX
<
T
>&
abs
()
{
*
this
=
this
->
cwiseAbs
();
return
*
this
;
}
static
const
Matrix3fX
<
T
>&
getIdentity
()
{
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
;
}
template
<
typename
OtherDerived
>
const
typename
Eigen
::
ProductReturnType
<
Eigen
::
Transpose
<
Matrix3fX
<
T
>
>
,
OtherDerived
>::
Type
transposeTimes
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
Base
::
transpose
()
*
other
;
}
template
<
typename
OtherDerived
>
const
typename
Eigen
::
ProductReturnType
<
Matrix3fX
<
T
>
,
Eigen
::
Transpose
<
OtherDerived
>
>
::
Type
timesTranspose
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
*
this
*
other
.
transpose
();
}
/*
Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
{
return Vec3fX<S>(data.transposeTimes(v.data));
}
// */
template
<
typename
OtherDerived
>
const
Eigen
::
ProductReturnType
<
Eigen
::
ProductReturnType
<
Matrix3fX
<
T
>
,
OtherDerived
>::
Type
,
Eigen
::
Transpose
<
Matrix3fX
<
T
>
>
>
::
Type
tensorTransform
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
m
)
const
{
return
(
*
this
*
m
)
*
this
->
Base
::
transpose
();
// Matrix3fX<T> res(*this);
// res *= m;
// return res.timesTranspose(*this);
}
// (1 0 0)^T (*this)^T v
template
<
typename
OtherDerived
>
inline
T
transposeDotX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
transposeDot
(
0
,
v
);
}
// (0 1 0)^T (*this)^T v
template
<
typename
OtherDerived
>
inline
T
transposeDotY
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
transposeDot
(
1
,
v
);
}
// (0 0 1)^T (*this)^T v
template
<
typename
OtherDerived
>
inline
T
transposeDotZ
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
transposeDot
(
2
,
v
);
}
// (\delta_{i3})^T (*this)^T v
template
<
typename
OtherDerived
>
inline
T
transposeDot
(
size_t
i
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
this
->
col
(
i
).
dot
(
v
);
}
// (1 0 0)^T (*this) v
template
<
typename
OtherDerived
>
inline
T
dotX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
dot
(
0
,
v
);
}
// (0 1 0)^T (*this) v
template
<
typename
OtherDerived
>
inline
T
dotY
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
dot
(
1
,
v
);
}
// (0 0 1)^T (*this) v
template
<
typename
OtherDerived
>
inline
T
dotZ
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
dot
(
2
,
v
);
}
// (\delta_{i3})^T (*this) v
template
<
typename
OtherDerived
>
inline
T
dot
(
size_t
i
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
v
)
const
{
return
this
->
row
(
i
).
dot
(
v
);
}
inline
void
setValue
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
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
(
T
x
)
{
this
->
setConstant
(
x
);
}
};
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
,
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
,
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
;
T
tresh
,
theta
,
tau
,
t
,
sm
,
s
,
h
,
g
,
c
;
int
nrot
;
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
)
{
b
[
ip
]
=
d
[
ip
]
=
R
(
ip
,
ip
);
z
[
ip
]
=
0
;
}
nrot
=
0
;
for
(
i
=
0
;
i
<
50
;
++
i
)
{
sm
=
0
;
for
(
ip
=
0
;
ip
<
n
;
++
ip
)
for
(
iq
=
ip
+
1
;
iq
<
n
;
++
iq
)
sm
+=
std
::
abs
(
R
(
ip
,
iq
));
if
(
sm
==
0.0
)
{
vout
[
0
].
setValue
(
v
[
0
][
0
],
v
[
0
][
1
],
v
[
0
][
2
]);
vout
[
1
].
setValue
(
v
[
1
][
0
],
v
[
1
][
1
],
v
[
1
][
2
]);
vout
[
2
].
setValue
(
v
[
2
][
0
],
v
[
2
][
1
],
v
[
2
][
2
]);
dout
[
0
]
=
d
[
0
];
dout
[
1
]
=
d
[
1
];
dout
[
2
]
=
d
[
2
];
return
;
}
if
(
i
<
3
)
tresh
=
0.2
*
sm
/
(
n
*
n
);
else
tresh
=
0.0
;
for
(
ip
=
0
;
ip
<
n
;
++
ip
)
{
for
(
iq
=
ip
+
1
;
iq
<
n
;
++
iq
)
{
g
=
100.0
*
std
::
abs
(
R
(
ip
,
iq
));
if
(
i
>
3
&&
std
::
abs
(
d
[
ip
])
+
g
==
std
::
abs
(
d
[
ip
])
&&
std
::
abs
(
d
[
iq
])
+
g
==
std
::
abs
(
d
[
iq
]))
R
(
ip
,
iq
)
=
0.0
;
else
if
(
std
::
abs
(
R
(
ip
,
iq
))
>
tresh
)
{
h
=
d
[
iq
]
-
d
[
ip
];
if
(
std
::
abs
(
h
)
+
g
==
std
::
abs
(
h
))
t
=
(
R
(
ip
,
iq
))
/
h
;
else
{
theta
=
0.5
*
h
/
(
R
(
ip
,
iq
));
t
=
1.0
/
(
std
::
abs
(
theta
)
+
std
::
sqrt
(
1.0
+
theta
*
theta
));
if
(
theta
<
0.0
)
t
=
-
t
;
}
c
=
1.0
/
std
::
sqrt
(
1
+
t
*
t
);
s
=
t
*
c
;
tau
=
s
/
(
1.0
+
c
);
h
=
t
*
R
(
ip
,
iq
);
z
[
ip
]
-=
h
;
z
[
iq
]
+=
h
;
d
[
ip
]
-=
h
;
d
[
iq
]
+=
h
;
R
(
ip
,
iq
)
=
0.0
;
for
(
j
=
0
;
j
<
ip
;
++
j
)
{
g
=
R
(
j
,
ip
);
h
=
R
(
j
,
iq
);
R
(
j
,
ip
)
=
g
-
s
*
(
h
+
g
*
tau
);
R
(
j
,
iq
)
=
h
+
s
*
(
g
-
h
*
tau
);
}
for
(
j
=
ip
+
1
;
j
<
iq
;
++
j
)
{
g
=
R
(
ip
,
j
);
h
=
R
(
j
,
iq
);
R
(
ip
,
j
)
=
g
-
s
*
(
h
+
g
*
tau
);
R
(
j
,
iq
)
=
h
+
s
*
(
g
-
h
*
tau
);
}
for
(
j
=
iq
+
1
;
j
<
n
;
++
j
)
{
g
=
R
(
ip
,
j
);
h
=
R
(
iq
,
j
);
R
(
ip
,
j
)
=
g
-
s
*
(
h
+
g
*
tau
);
R
(
iq
,
j
)
=
h
+
s
*
(
g
-
h
*
tau
);
}
for
(
j
=
0
;
j
<
n
;
++
j
)
{
g
=
v
[
j
][
ip
];
h
=
v
[
j
][
iq
];
v
[
j
][
ip
]
=
g
-
s
*
(
h
+
g
*
tau
);
v
[
j
][
iq
]
=
h
+
s
*
(
g
-
h
*
tau
);
}
nrot
++
;
}
}
}
for
(
ip
=
0
;
ip
<
n
;
++
ip
)
{
b
[
ip
]
+=
z
[
ip
];
d
[
ip
]
=
b
[
ip
];
z
[
ip
]
=
0.0
;
}
}
std
::
cerr
<<
"eigen: too many iterations in Jacobi transform."
<<
std
::
endl
;
return
;
}
template
<
typename
T
>
const
typename
CwiseUnaryOp
<
internal
::
scalar_abs_op
<
Scalar
>
,
const
Matrix3fX
<
T
>
>
abs
(
const
Matrix3fX
<
T
>&
R
)
{
return
R
.
cwiseAbs
();
}
template
<
typename
T
>
typename
Eigen
::
Transpose
<
Matrix3fX
<
T
>
>
transpose
(
const
Matrix3fX
<
T
>&
R
)
{
return
R
.
Matrix3fX
<
T
>::
Base
::
transpose
();
}
template
<
typename
T
>
Matrix3fX
<
T
>
inverse
(
const
Matrix3fX
<
T
>&
R
)
{
return
R
.
Matrix3fX
<
T
>::
Base
::
inverse
().
eval
();
}
template
<
typename
T
,
typename
Derived
>
T
quadraticForm
(
const
Matrix3fX
<
T
>&
R
,
const
Eigen
::
MatrixBase
<
Derived
>&
v
)
{
return
v
.
dot
(
R
*
v
);
}
}
#endif
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment