Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
H
hpp-fcl
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Guilhem Saurel
hpp-fcl
Commits
e516fa72
Commit
e516fa72
authored
9 years ago
by
Joseph Mirabel
Committed by
Joseph Mirabel
9 years ago
Browse files
Options
Downloads
Patches
Plain Diff
Improve Eigen integration
parent
0d64ce1e
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
include/hpp/fcl/eigen/math_details.h
+298
-0
298 additions, 0 deletions
include/hpp/fcl/eigen/math_details.h
with
298 additions
and
0 deletions
include/hpp/fcl/eigen/math_details.h
+
298
−
0
View file @
e516fa72
...
...
@@ -303,6 +303,133 @@ static inline bool equal(const eigen_wrapper_v4<T>& x, const eigen_wrapper_v4<T>
return
((
x
.
v
-
y
.
v
).
cwiseAbs
().
array
()
<
epsilon
).
all
();
}
template
<
typename
T
>
struct
eigen_v3
:
Eigen
::
Matrix
<
T
,
3
,
1
>
{
typedef
T
meta_type
;
typedef
Eigen
::
Matrix
<
T
,
3
,
1
>
Base
;
eigen_v3
(
void
)
:
Base
()
{
this
->
setZero
();
}
// This constructor allows you to construct MyVectorType from Eigen expressions
template
<
typename
OtherDerived
>
eigen_v3
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
:
Base
(
other
)
{}
// This method allows you to assign Eigen expressions to MyVectorType
template
<
typename
OtherDerived
>
eigen_v3
&
operator
=
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
{
this
->
Base
::
operator
=
(
other
);
return
*
this
;
}
eigen_v3
(
T
x
)
:
Base
(
x
,
x
,
x
)
{}
eigen_v3
(
T
*
x
)
:
Base
(
*
x
,
*
x
,
*
x
)
{}
eigen_v3
(
T
x
,
T
y
,
T
z
)
:
Base
(
x
,
y
,
z
)
{}
inline
void
setValue
(
T
x
,
T
y
,
T
z
)
{
this
->
operator
[]
(
0
)
=
x
;
this
->
operator
[]
(
1
)
=
y
;
this
->
operator
[]
(
2
)
=
z
;
}
inline
void
setValue
(
T
x
)
{
this
->
setConstant
(
x
);
}
inline
void
negate
()
{
this
->
operator
*=
(
-
1
);
}
template
<
typename
OtherDerived
>
inline
eigen_v3
<
T
>&
ubound
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
u
)
{
*
this
=
this
->
cwiseMin
(
u
);
return
*
this
;
}
template
<
typename
OtherDerived
>
inline
eigen_v3
<
T
>&
lbound
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
l
)
{
*
this
=
this
->
cwiseMax
(
l
);
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[0] - other.v[0], v[1] - other.v[1], v[2] - other.v[2]); }
// inline eigen_wrapper_v3<T> operator * (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v[0] * other.v[0], v[1] * other.v[1], v[2] * other.v[2]); }
// inline eigen_v3<T> operator / (const eigen_v3<T>& other) const { return eigen_v3<T>(this->array().cwiseQuotient (other)); }
inline
eigen_v3
<
T
>&
operator
+=
(
const
eigen_v3
<
T
>&
other
)
{
return
static_cast
<
eigen_v3
<
T
>&>
(
this
->
Base
::
operator
+=
(
other
));
}
inline
eigen_v3
<
T
>&
operator
-=
(
const
eigen_v3
<
T
>&
other
)
{
return
static_cast
<
eigen_v3
<
T
>&>
(
this
->
Base
::
operator
-=
(
other
));
}
inline
eigen_v3
<
T
>&
operator
*=
(
const
eigen_v3
<
T
>&
other
)
{
return
static_cast
<
eigen_v3
<
T
>&>
(
this
->
array
().
operator
*=
(
other
));
}
inline
eigen_v3
<
T
>&
operator
/=
(
const
eigen_v3
<
T
>&
other
)
{
return
static_cast
<
eigen_v3
<
T
>&>
(
this
->
array
().
operator
/=
(
other
));
}
// inline eigen_wrapper_v4<T>& operator /= (const eigen_wrapper_v4<T>& other) { = d().cwiseQuotient (other.d()); return *this; }
// inline eigen_wrapper_v3<T>& operator *= (const eigen_wrapper_v3<T>& other) { return this->Base::operator*= (other); }
// inline eigen_wrapper_v3<T>& operator /= (const eigen_wrapper_v3<T>& other) { return this->Base::operator/= (other); }
// 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) 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_v3
<
T
>&
operator
+=
(
T
t
)
{
this
->
array
()
+=
t
;
return
*
this
;
}
inline
eigen_v3
<
T
>&
operator
-=
(
T
t
)
{
this
->
array
()
-=
t
;
return
*
this
;
}
inline
eigen_v3
<
T
>&
operator
*=
(
T
t
)
{
this
->
array
()
*=
t
;
return
*
this
;
}
inline
eigen_v3
<
T
>&
operator
/=
(
T
t
)
{
this
->
array
()
/=
t
;
return
*
this
;
}
// inline eigen_wrapper_v3<T> operator - () const { return eigen_wrapper_v3<T>(-v); }
};
template
<
typename
T
>
static
inline
eigen_v3
<
T
>
cross_prod
(
const
eigen_v3
<
T
>&
l
,
const
eigen_v3
<
T
>&
r
)
{
return
l
.
cross
(
r
);
}
template
<
typename
T
>
static
inline
T
dot_prod3
(
const
eigen_v3
<
T
>&
l
,
const
eigen_v3
<
T
>&
r
)
{
return
l
.
dot
(
r
);
}
template
<
typename
T
>
static
inline
eigen_v3
<
T
>
min
(
const
eigen_v3
<
T
>&
x
,
const
eigen_v3
<
T
>&
y
)
{
return
x
.
cwiseMin
(
y
);
}
template
<
typename
T
>
static
inline
eigen_v3
<
T
>
max
(
const
eigen_v3
<
T
>&
x
,
const
eigen_v3
<
T
>&
y
)
{
return
x
.
cwiseMax
(
y
);
}
template
<
typename
T
>
static
inline
eigen_v3
<
T
>
abs
(
const
eigen_v3
<
T
>&
x
)
{
return
x
.
cwiseAbs
();
}
template
<
typename
T
>
static
inline
bool
equal
(
const
eigen_v3
<
T
>&
x
,
const
eigen_v3
<
T
>&
y
,
T
epsilon
)
{
return
((
x
-
y
).
cwiseAbs
().
array
()
<
epsilon
).
all
();
}
template
<
typename
T
>
struct
eigen_wrapper_m3
...
...
@@ -474,6 +601,177 @@ eigen_wrapper_m3<T> inverse(const eigen_wrapper_m3<T>& m)
return
eigen_wrapper_m3
<
T
>
(
m
.
m
.
inverse
().
eval
());
}
template
<
typename
T
>
struct
eigen_m3
:
Eigen
::
Matrix
<
T
,
3
,
3
>
{
typedef
T
meta_type
;
typedef
eigen_v3
<
T
>
vector_type
;
typedef
Eigen
::
Matrix
<
T
,
3
,
3
>
Base
;
typedef
typename
Base
::
ColXpr
ColXpr
;
typedef
typename
Base
::
ConstColXpr
ConstColXpr
;
typedef
typename
Base
::
RowXpr
RowXpr
;
typedef
typename
Base
::
ConstRowXpr
ConstRowXpr
;
eigen_m3
(
void
)
:
Base
()
{
}
// This constructor allows you to construct MyVectorType from Eigen expressions
template
<
typename
OtherDerived
>
eigen_m3
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
:
Base
(
other
)
{}
// This method allows you to assign Eigen expressions to MyVectorType
template
<
typename
OtherDerived
>
eigen_m3
&
operator
=
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
{
this
->
Base
::
operator
=
(
other
);
return
*
this
;
}
eigen_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_m3
(
const
vector_type
&
v1
,
const
vector_type
&
v2
,
const
vector_type
&
v3
)
{
this
->
row
(
1
)
=
v1
;
this
->
row
(
2
)
=
v2
;
this
->
row
(
3
)
=
v3
;
}
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
eigen_m3
<
T
>&
operator
*=
(
const
eigen_m3
<
T
>&
other
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
*=
(
other
));
}
inline
eigen_m3
<
T
>&
operator
+=
(
const
eigen_m3
<
T
>&
other
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
+=
(
other
));
}
inline
eigen_m3
<
T
>&
operator
-=
(
const
eigen_m3
<
T
>&
other
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
-=
(
other
));
}
inline
eigen_m3
<
T
>&
operator
+=
(
T
c
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
+=
(
c
));
}
inline
eigen_m3
<
T
>&
operator
-=
(
T
c
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
-=
(
c
));
}
inline
eigen_m3
<
T
>&
operator
*=
(
T
c
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
*=
(
c
));
}
inline
eigen_m3
<
T
>&
operator
/=
(
T
c
)
{
return
static_cast
<
eigen_m3
<
T
>&>
(
this
->
operator
/=
(
c
));
}
static
const
eigen_m3
<
T
>&
getIdentity
()
{
static
const
eigen_m3
<
T
>
I
(
Base
::
Identity
());
return
I
;
}
eigen_m3
<
T
>&
transpose
()
{
this
->
transposeInPlace
();
return
*
this
;
}
eigen_m3
<
T
>&
inverse
()
{
this
->
Base
::
operator
=
(
this
->
inverse
().
eval
());
// m = m.inverse().eval();
return
*
this
;
}
template
<
typename
OtherDerived
>
const
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
timesTranspose
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
operator
*
(
other
.
Base
::
transpose
());
}
template
<
typename
OtherDerived
>
inline
T
transposeDotX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
col
(
0
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
transposeDotY
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
col
(
1
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
transposeDotZ
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
col
(
2
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
transposeDot
(
size_t
i
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
col
(
i
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
dotX
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
row
(
0
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
dotY
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
row
(
1
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
dotZ
(
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
row
(
2
).
dot
(
other
);
}
template
<
typename
OtherDerived
>
inline
T
dot
(
size_t
i
,
const
Eigen
::
MatrixBase
<
OtherDerived
>&
other
)
const
{
return
this
->
row
(
i
).
dot
(
other
);
}
inline
void
setValue
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
{
*
this
<<
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
;
}
inline
void
setValue
(
T
x
)
{
this
->
setConstant
(
x
);
}
};
template
<
typename
T
>
eigen_m3
<
T
>
abs
(
const
eigen_m3
<
T
>&
m
)
{
return
eigen_m3
<
T
>
(
m
.
cwiseAbs
());
}
template
<
typename
T
>
eigen_m3
<
T
>
transpose
(
const
eigen_m3
<
T
>&
m
)
{
return
eigen_m3
<
T
>
(
m
.
eigen_m3
<
T
>::
Base
::
transpose
());
}
template
<
typename
T
>
eigen_m3
<
T
>
inverse
(
const
eigen_m3
<
T
>&
m
)
{
return
eigen_m3
<
T
>
(
m
.
eigen_m3
<
T
>::
Base
::
inverse
().
eval
());
}
}
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment