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
405a4b83
Commit
405a4b83
authored
Jun 10, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jun 14, 2016
Browse files
Clean code and remove unused files.
parent
f1923507
Changes
18
Expand all
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
405a4b83
...
...
@@ -48,9 +48,8 @@ setup_project()
set
(
FCL_HAVE_SSE FALSE CACHE BOOL
"Enable SSE vectorization"
)
# add_optional_dependency("eigen3 >= 3.0.0")
add_required_dependency
(
"eigen3 >= 3.0.0"
)
set
(
FCL_HAVE_EIGEN TRUE
CACHE BOOL
"Use eigen matrix type when possible"
)
set
(
FCL_HAVE_EIGEN TRUE
)
include_directories
(
${
EIGEN3_INCLUDE_DIRS
}
)
# Required dependencies
...
...
include/hpp/fcl/ccd/interval_matrix.h
View file @
405a4b83
...
...
@@ -99,10 +99,6 @@ 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 @
405a4b83
...
...
@@ -157,9 +157,15 @@ 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
template
<
typename
Derived
>
IVector3
&
operator
=
(
const
Eigen
::
MatrixBase
<
Derived
>&
other
)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Derived
,
3
);
\
setValue
(
other
.
derived
());
// const Vec3f& tmp (other);
// setValue (tmp);
return
*
this
;
}
};
IVector3
bound
(
const
IVector3
&
i
,
const
Vec3f
&
v
);
...
...
include/hpp/fcl/ccd/taylor_vector.h
View file @
405a4b83
...
...
@@ -40,10 +40,8 @@
#include
<hpp/fcl/ccd/interval_vector.h>
#include
<hpp/fcl/ccd/taylor_model.h>
#if FCL_HAVE_EIGEN
#include
<hpp/fcl/eigen/taylor_operator.h>
#endif
#include
<hpp/fcl/math/vec_3f.h>
#include
<hpp/fcl/math/matrix_3f.h>
namespace
fcl
{
...
...
@@ -106,8 +104,20 @@ public:
const
boost
::
shared_ptr
<
TimeInterval
>&
getTimeInterval
()
const
;
};
class
TMatrix3
;
void
generateTVector3ForLinearFunc
(
TVector3
&
v
,
const
Vec3f
&
position
,
const
Vec3f
&
velocity
);
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
Eigen
::
MatrixBase
<
Derived
>&
v
,
const
TaylorModel
&
a
)
{
const
typename
TaylorReturnType
<
Derived
::
ColsAtCompileTime
>::
eigen_type
b
=
v
.
derived
();
return
b
*
a
;
}
TVector3
operator
*
(
const
Vec3f
&
v
,
const
TaylorModel
&
a
);
TVector3
operator
+
(
const
Vec3f
&
v1
,
const
TVector3
&
v2
);
...
...
include/hpp/fcl/eigen/math_details.h
deleted
100644 → 0
View file @
f1923507
This diff is collapsed.
Click to expand it.
include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
deleted
100644 → 0
View file @
f1923507
include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
deleted
100644 → 0
View file @
f1923507
template
<
typename
Derived
>
IVector3
&
operator
=
(
const
Eigen
::
MatrixBase
<
Derived
>&
other
)
{
const
Vec3f
&
tmp
(
other
);
setValue
(
tmp
);
return
*
this
;
}
include/hpp/fcl/eigen/product.h
deleted
100644 → 0
View file @
f1923507
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
typename
internal
::
remove_fcl
<
Derived
>::
type
EDerived
;
typedef
typename
internal
::
remove_fcl
<
OtherDerived
>::
type
EOtherDerived
;
typedef
FclOp
<
typename
internal
::
deduce_fcl_type
<
EDerived
,
EOtherDerived
,
COEFWISE
>::
Type
>
ProductType
;
typedef
FclOp
<
typename
ProductReturnType
<
Transpose
<
Derived
>
,
EOtherDerived
>::
Type
>
TransposeTimesType
;
typedef
FclOp
<
typename
ProductReturnType
<
EDerived
,
Transpose
<
EOtherDerived
>
>::
Type
>
TimesTransposeType
;
typedef
FclOp
<
typename
ProductReturnType
<
ProductType
,
Transpose
<
EDerived
>
>::
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 typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::ProductType \
operator*(const MatrixBase<OtherDerived>& other) const \
{ \
return FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::run (*this, other.derived()); \
}
include/hpp/fcl/eigen/taylor_operator.h
deleted
100644 → 0
View file @
f1923507
#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
Eigen
::
MatrixBase
<
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
deleted
100644 → 0
View file @
f1923507
This diff is collapsed.
Click to expand it.
include/hpp/fcl/math/math_details.h
deleted
100644 → 0
View file @
f1923507
/*
* 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_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
<cstring>
namespace
fcl
{
namespace
details
{
template
<
typename
T
>
struct
Vec3Data
{
typedef
T
meta_type
;
T
vs
[
3
];
Vec3Data
()
{
setValue
(
0
);
}
Vec3Data
(
T
x
)
{
setValue
(
x
);
}
Vec3Data
(
T
*
x
)
{
memcpy
(
vs
,
x
,
sizeof
(
T
)
*
3
);
}
Vec3Data
(
T
x
,
T
y
,
T
z
)
{
setValue
(
x
,
y
,
z
);
}
inline
void
setValue
(
T
x
,
T
y
,
T
z
)
{
vs
[
0
]
=
x
;
vs
[
1
]
=
y
;
vs
[
2
]
=
z
;
}
inline
void
setValue
(
T
x
)
{
vs
[
0
]
=
x
;
vs
[
1
]
=
x
;
vs
[
2
]
=
x
;
}
inline
Vec3Data
<
T
>&
ubound
(
const
Vec3Data
<
T
>&
u
)
{
vs
[
0
]
=
std
::
min
(
vs
[
0
],
u
.
vs
[
0
]);
vs
[
1
]
=
std
::
min
(
vs
[
1
],
u
.
vs
[
1
]);
vs
[
2
]
=
std
::
min
(
vs
[
2
],
u
.
vs
[
2
]);
return
*
this
;
}
inline
Vec3Data
<
T
>&
lbound
(
const
Vec3Data
<
T
>&
l
)
{
vs
[
0
]
=
std
::
max
(
vs
[
0
],
l
.
vs
[
0
]);
vs
[
1
]
=
std
::
max
(
vs
[
1
],
l
.
vs
[
1
]);
vs
[
2
]
=
std
::
max
(
vs
[
2
],
l
.
vs
[
2
]);
return
*
this
;
}
T
operator
[]
(
size_t
i
)
const
{
return
vs
[
i
];
}
T
&
operator
[]
(
size_t
i
)
{
return
vs
[
i
];
}
inline
Vec3Data
<
T
>
operator
+
(
const
Vec3Data
<
T
>&
other
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
+
other
.
vs
[
0
],
vs
[
1
]
+
other
.
vs
[
1
],
vs
[
2
]
+
other
.
vs
[
2
]);
}
inline
Vec3Data
<
T
>
operator
-
(
const
Vec3Data
<
T
>&
other
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
-
other
.
vs
[
0
],
vs
[
1
]
-
other
.
vs
[
1
],
vs
[
2
]
-
other
.
vs
[
2
]);
}
inline
Vec3Data
<
T
>
operator
*
(
const
Vec3Data
<
T
>&
other
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
*
other
.
vs
[
0
],
vs
[
1
]
*
other
.
vs
[
1
],
vs
[
2
]
*
other
.
vs
[
2
]);
}
inline
Vec3Data
<
T
>
operator
/
(
const
Vec3Data
<
T
>&
other
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
/
other
.
vs
[
0
],
vs
[
1
]
/
other
.
vs
[
1
],
vs
[
2
]
/
other
.
vs
[
2
]);
}
inline
Vec3Data
<
T
>&
operator
+=
(
const
Vec3Data
<
T
>&
other
)
{
vs
[
0
]
+=
other
.
vs
[
0
];
vs
[
1
]
+=
other
.
vs
[
1
];
vs
[
2
]
+=
other
.
vs
[
2
];
return
*
this
;
}
inline
Vec3Data
<
T
>&
operator
-=
(
const
Vec3Data
<
T
>&
other
)
{
vs
[
0
]
-=
other
.
vs
[
0
];
vs
[
1
]
-=
other
.
vs
[
1
];
vs
[
2
]
-=
other
.
vs
[
2
];
return
*
this
;
}
inline
Vec3Data
<
T
>&
operator
*=
(
const
Vec3Data
<
T
>&
other
)
{
vs
[
0
]
*=
other
.
vs
[
0
];
vs
[
1
]
*=
other
.
vs
[
1
];
vs
[
2
]
*=
other
.
vs
[
2
];
return
*
this
;
}
inline
Vec3Data
<
T
>&
operator
/=
(
const
Vec3Data
<
T
>&
other
)
{
vs
[
0
]
/=
other
.
vs
[
0
];
vs
[
1
]
/=
other
.
vs
[
1
];
vs
[
2
]
/=
other
.
vs
[
2
];
return
*
this
;
}
inline
Vec3Data
<
T
>
operator
+
(
T
t
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
+
t
,
vs
[
1
]
+
t
,
vs
[
2
]
+
t
);
}
inline
Vec3Data
<
T
>
operator
-
(
T
t
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
-
t
,
vs
[
1
]
-
t
,
vs
[
2
]
-
t
);
}
inline
Vec3Data
<
T
>
operator
*
(
T
t
)
const
{
return
Vec3Data
<
T
>
(
vs
[
0
]
*
t
,
vs
[
1
]
*
t
,
vs
[
2
]
*
t
);
}
inline
Vec3Data
<
T
>
operator
/
(
T
t
)
const
{
T
inv_t
=
1.0
/
t
;
return
Vec3Data
<
T
>
(
vs
[
0
]
*
inv_t
,
vs
[
1
]
*
inv_t
,
vs
[
2
]
*
inv_t
);
}
inline
Vec3Data
<
T
>&
operator
+=
(
T
t
)
{
vs
[
0
]
+=
t
;
vs
[
1
]
+=
t
;
vs
[
2
]
+=
t
;
return
*
this
;
}
inline
Vec3Data
<
T
>&
operator
-=
(
T
t
)
{
vs
[
0
]
-=
t
;
vs
[
1
]
-=
t
;
vs
[
2
]
-=
t
;
return
*
this
;
}
inline
Vec3Data
<
T
>&
operator
*=
(
T
t
)
{
vs
[
0
]
*=
t
;
vs
[
1
]
*=
t
;
vs
[
2
]
*=
t
;
return
*
this
;
}
inline
Vec3Data
<
T
>&
operator
/=
(
T
t
)
{
T
inv_t
=
1.0
/
t
;
vs
[
0
]
*=
inv_t
;
vs
[
1
]
*=
inv_t
;
vs
[
2
]
*=
inv_t
;
return
*
this
;
}
inline
Vec3Data
<
T
>
operator
-
()
const
{
return
Vec3Data
<
T
>
(
-
vs
[
0
],
-
vs
[
1
],
-
vs
[
2
]);
}
};
template
<
typename
T
>
static
inline
Vec3Data
<
T
>
cross_prod
(
const
Vec3Data
<
T
>&
l
,
const
Vec3Data
<
T
>&
r
)
{
return
Vec3Data
<
T
>
(
l
.
vs
[
1
]
*
r
.
vs
[
2
]
-
l
.
vs
[
2
]
*
r
.
vs
[
1
],
l
.
vs
[
2
]
*
r
.
vs
[
0
]
-
l
.
vs
[
0
]
*
r
.
vs
[
2
],
l
.
vs
[
0
]
*
r
.
vs
[
1
]
-
l
.
vs
[
1
]
*
r
.
vs
[
0
]);
}
template
<
typename
T
>
static
inline
T
dot_prod3
(
const
Vec3Data
<
T
>&
l
,
const
Vec3Data
<
T
>&
r
)
{
return
l
.
vs
[
0
]
*
r
.
vs
[
0
]
+
l
.
vs
[
1
]
*
r
.
vs
[
1
]
+
l
.
vs
[
2
]
*
r
.
vs
[
2
];
}
template
<
typename
T
>
static
inline
Vec3Data
<
T
>
min
(
const
Vec3Data
<
T
>&
x
,
const
Vec3Data
<
T
>&
y
)
{
return
Vec3Data
<
T
>
(
std
::
min
(
x
.
vs
[
0
],
y
.
vs
[
0
]),
std
::
min
(
x
.
vs
[
1
],
y
.
vs
[
1
]),
std
::
min
(
x
.
vs
[
2
],
y
.
vs
[
2
]));
}
template
<
typename
T
>
static
inline
Vec3Data
<
T
>
max
(
const
Vec3Data
<
T
>&
x
,
const
Vec3Data
<
T
>&
y
)
{
return
Vec3Data
<
T
>
(
std
::
max
(
x
.
vs
[
0
],
y
.
vs
[
0
]),
std
::
max
(
x
.
vs
[
1
],
y
.
vs
[
1
]),
std
::
max
(
x
.
vs
[
2
],
y
.
vs
[
2
]));
}
template
<
typename
T
>
static
inline
Vec3Data
<
T
>
abs
(
const
Vec3Data
<
T
>&
x
)
{
return
Vec3Data
<
T
>
(
std
::
abs
(
x
.
vs
[
0
]),
std
::
abs
(
x
.
vs
[
1
]),
std
::
abs
(
x
.
vs
[
2
]));
}
template
<
typename
T
>
static
inline
bool
equal
(
const
Vec3Data
<
T
>&
x
,
const
Vec3Data
<
T
>&
y
,
T
epsilon
)
{
return
((
x
.
vs
[
0
]
-
y
.
vs
[
0
]
<
epsilon
)
&&
(
x
.
vs
[
0
]
-
y
.
vs
[
0
]
>
-
epsilon
)
&&
(
x
.
vs
[
1
]
-
y
.
vs
[
1
]
<
epsilon
)
&&
(
x
.
vs
[
1
]
-
y
.
vs
[
1
]
>
-
epsilon
)
&&
(
x
.
vs
[
2
]
-
y
.
vs
[
2
]
<
epsilon
)
&&
(
x
.
vs
[
2
]
-
y
.
vs
[
2
]
>
-
epsilon
));
}
template
<
typename
T
>
struct
Matrix3Data
{
typedef
T
meta_type
;
typedef
Vec3Data
<
T
>
vector_type
;
Vec3Data
<
T
>
rs
[
3
];
Matrix3Data
()
{};
Matrix3Data
(
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
);
}
Matrix3Data
(
const
Vec3Data
<
T
>&
v1
,
const
Vec3Data
<
T
>&
v2
,
const
Vec3Data
<
T
>&
v3
)
{
rs
[
0
]
=
v1
;
rs
[
1
]
=
v2
;
rs
[
2
]
=
v3
;
}
Matrix3Data
(
const
Matrix3Data
<
T
>&
other
)
{
rs
[
0
]
=
other
.
rs
[
0
];
rs
[
1
]
=
other
.
rs
[
1
];
rs
[
2
]
=
other
.
rs
[
2
];
}
inline
Vec3Data
<
T
>
getColumn
(
size_t
i
)
const
{
return
Vec3Data
<
T
>
(
rs
[
0
][
i
],
rs
[
1
][
i
],
rs
[
2
][
i
]);
}
inline
const
Vec3Data
<
T
>&
getRow
(
size_t
i
)
const
{
return
rs
[
i
];
}
inline
T
operator
()
(
size_t
i
,
size_t
j
)
const
{
return
rs
[
i
][
j
];
}
inline
T
&
operator
()
(
size_t
i
,
size_t
j
)
{
return
rs
[
i
][
j
];
}
inline
Vec3Data
<
T
>
operator
*
(
const
Vec3Data
<
T
>&
v
)
const
{
return
Vec3Data
<
T
>
(
dot_prod3
(
rs
[
0
],
v
),
dot_prod3
(
rs
[
1
],
v
),
dot_prod3
(
rs
[
2
],
v
));
}
inline
Matrix3Data
<
T
>
operator
*
(
const
Matrix3Data
<
T
>&
other
)
const
{
return
Matrix3Data
<
T
>
(
other
.
transposeDotX
(
rs
[
0
]),
other
.
transposeDotY
(
rs
[
0
]),
other
.
transposeDotZ
(
rs
[
0
]),
other
.
transposeDotX
(
rs
[
1
]),
other
.
transposeDotY
(
rs
[
1
]),
other
.
transposeDotZ
(
rs
[
1
]),
other
.
transposeDotX
(
rs
[
2
]),
other
.
transposeDotY
(
rs
[
2
]),
other
.
transposeDotZ
(
rs
[
2
]));
}
inline
Matrix3Data
<
T
>
operator
+
(
const
Matrix3Data
<
T
>&
other
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
]
+
other
.
rs
[
0
],
rs
[
1
]
+
other
.
rs
[
1
],
rs
[
2
]
+
other
.
rs
[
2
]);
}
inline
Matrix3Data
<
T
>
operator
-
(
const
Matrix3Data
<
T
>&
other
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
]
-
other
.
rs
[
0
],
rs
[
1
]
-
other
.
rs
[
1
],
rs
[
2
]
-
other
.
rs
[
2
]);
}
inline
Matrix3Data
<
T
>
operator
+
(
T
c
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
]
+
c
,
rs
[
1
]
+
c
,
rs
[
2
]
+
c
);
}
inline
Matrix3Data
<
T
>
operator
-
(
T
c
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
]
-
c
,
rs
[
1
]
-
c
,
rs
[
2
]
-
c
);
}
inline
Matrix3Data
<
T
>
operator
*
(
T
c
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
]
*
c
,
rs
[
1
]
*
c
,
rs
[
2
]
*
c
);
}
inline
Matrix3Data
<
T
>
operator
/
(
T
c
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
]
/
c
,
rs
[
1
]
/
c
,
rs
[
2
]
/
c
);
}
inline
Matrix3Data
<
T
>&
operator
*=
(
const
Matrix3Data
<
T
>&
other
)
{
rs
[
0
].
setValue
(
other
.
transposeDotX
(
rs
[
0
]),
other
.
transposeDotY
(
rs
[
0
]),
other
.
transposeDotZ
(
rs
[
0
]));
rs
[
1
].
setValue
(
other
.
transposeDotX
(
rs
[
1
]),
other
.
transposeDotY
(
rs
[
1
]),
other
.
transposeDotZ
(
rs
[
1
]));
rs
[
2
].
setValue
(
other
.
transposeDotX
(
rs
[
2
]),
other
.
transposeDotY
(
rs
[
2
]),
other
.
transposeDotZ
(
rs
[
2
]));
return
*
this
;
}
inline
Matrix3Data
<
T
>&
operator
+=
(
const
Matrix3Data
<
T
>&
other
)
{
rs
[
0
]
+=
other
.
rs
[
0
];
rs
[
1
]
+=
other
.
rs
[
1
];
rs
[
2
]
+=
other
.
rs
[
2
];
return
*
this
;
}
inline
Matrix3Data
<
T
>&
operator
-=
(
const
Matrix3Data
<
T
>&
other
)
{
rs
[
0
]
-=
other
.
rs
[
0
];
rs
[
1
]
-=
other
.
rs
[
1
];
rs
[
2
]
-=
other
.
rs
[
2
];
return
*
this
;
}
inline
Matrix3Data
<
T
>&
operator
+=
(
T
c
)
{
rs
[
0
]
+=
c
;
rs
[
1
]
+=
c
;
rs
[
2
]
+=
c
;
return
*
this
;
}
inline
Matrix3Data
<
T
>&
operator
-
(
T
c
)
{
rs
[
0
]
-=
c
;
rs
[
1
]
-=
c
;
rs
[
2
]
-=
c
;
return
*
this
;
}
inline
Matrix3Data
<
T
>&
operator
*
(
T
c
)
{
rs
[
0
]
*=
c
;
rs
[
1
]
*=
c
;
rs
[
2
]
*=
c
;
return
*
this
;
}
inline
Matrix3Data
<
T
>&
operator
/
(
T
c
)
{
rs
[
0
]
/=
c
;
rs
[
1
]
/=
c
;
rs
[
2
]
/=
c
;
return
*
this
;
}
void
setIdentity
()
{
setValue
((
T
)
1
,
(
T
)
0
,
(
T
)
0
,
(
T
)
0
,
(
T
)
1
,
(
T
)
0
,
(
T
)
0
,
(
T
)
0
,
(
T
)
1
);
}
void
setZero
()
{
setValue
((
T
)
0
);
}
static
const
Matrix3Data
<
T
>&
getIdentity
()
{
static
const
Matrix3Data
<
T
>
I
((
T
)
1
,
(
T
)
0
,
(
T
)
0
,
(
T
)
0
,
(
T
)
1
,
(
T
)
0
,
(
T
)
0
,
(
T
)
0
,
(
T
)
1
);
return
I
;
}
T
determinant
()
const
{
return
dot_prod3
(
rs
[
0
],
cross_prod
(
rs
[
1
],
rs
[
2
]));
}
Matrix3Data
<
T
>&
transpose
()
{
register
T
tmp
=
rs
[
0
][
1
];
rs
[
0
][
1
]
=
rs
[
1
][
0
];
rs
[
1
][
0
]
=
tmp
;
tmp
=
rs
[
0
][
2
];
rs
[
0
][
2
]
=
rs
[
2
][
0
];
rs
[
2
][
0
]
=
tmp
;
tmp
=
rs
[
2
][
1
];
rs
[
2
][
1
]
=
rs
[
1
][
2
];
rs
[
1
][
2
]
=
tmp
;
return
*
this
;
}
Matrix3Data
<
T
>&
inverse
()
{
T
det
=
determinant
();
register
T
inrsdet
=
1
/
det
;
setValue
((
rs
[
1
][
1
]
*
rs
[
2
][
2
]
-
rs
[
1
][
2
]
*
rs
[
2
][
1
])
*
inrsdet
,
(
rs
[
0
][
2
]
*
rs
[
2
][
1
]
-
rs
[
0
][
1
]
*
rs
[
2
][
2
])
*
inrsdet
,
(
rs
[
0
][
1
]
*
rs
[
1
][
2
]
-
rs
[
0
][
2
]
*
rs
[
1
][
1
])
*
inrsdet
,
(
rs
[
1
][
2
]
*
rs
[
2
][
0
]
-
rs
[
1
][
0
]
*
rs
[
2
][
2
])
*
inrsdet
,
(
rs
[
0
][
0
]
*
rs
[
2
][
2
]
-
rs
[
0
][
2
]
*
rs
[
2
][
0
])
*
inrsdet
,
(
rs
[
0
][
2
]
*
rs
[
1
][
0
]
-
rs
[
0
][
0
]
*
rs
[
1
][
2
])
*
inrsdet
,
(
rs
[
1
][
0
]
*
rs
[
2
][
1
]
-
rs
[
1
][
1
]
*
rs
[
2
][
0
])
*
inrsdet
,
(
rs
[
0
][
1
]
*
rs
[
2
][
0
]
-
rs
[
0
][
0
]
*
rs
[
2
][
1
])
*
inrsdet
,
(
rs
[
0
][
0
]
*
rs
[
1
][
1
]
-
rs
[
0
][
1
]
*
rs
[
1
][
0
])
*
inrsdet
);
return
*
this
;
}
Matrix3Data
<
T
>
transposeTimes
(
const
Matrix3Data
<
T
>&
m
)
const
{
return
Matrix3Data
<
T
>
(
rs
[
0
][
0
]
*
m
.
rs
[
0
][
0
]
+
rs
[
1
][
0
]
*
m
.
rs
[
1
][
0
]
+
rs
[
2
][
0
]
*
m
.
rs
[
2
][
0
],
rs
[
0
][
0
]
*
m
.
rs
[
0
][
1
]
+
rs
[
1
][
0
]
*
m
.
rs
[
1
][
1
]
+
rs
[
2
][
0
]
*
m
.
rs
[
2
][
1
],
rs
[
0
][
0
]
*
m
.
rs
[
0
][
2
]
+
rs
[
1
][
0
]
*
m
.
rs
[
1
][
2
]
+
rs
[
2
][
0
]
*
m
.
rs
[
2
][
2
],
rs
[
0
][
1
]
*
m
.
rs
[
0
][
0
]
+
rs
[
1
][
1
]
*
m
.
rs
[
1
][
0
]
+
rs
[
2
][
1
]
*
m
.
rs
[
2
][
0
],
rs
[
0
][
1
]
*
m
.
rs
[
0
][
1
]
+
rs
[
1
][
1
]
*
m
.
rs
[
1
][
1
]
+
rs
[
2
][
1
]
*
m
.
rs
[
2
][
1
],
rs
[
0
][
1
]
*
m
.
rs
[
0
][
2
]
+
rs
[
1
][
1
]
*
m
.
rs
[
1
][
2
]
+
rs
[
2
][
1
]
*
m
.
rs
[
2
][
2
],
rs
[
0
][
2
]
*
m
.
rs
[
0
][
0
]
+
rs
[
1
][
2
]
*
m
.
rs
[
1
][
0
]
+
rs
[
2
][
2
]
*
m
.
rs
[
2
][
0
],
rs
[
0
][
2
]
*
m
.
rs
[
0
][
1
]
+
rs
[
1
][
2
]
*
m
.
rs
[
1
][
1
]
+
rs
[
2
][
2
]
*
m
.
rs
[
2
][
1
],
rs
[
0
][
2
]
*
m
.
rs
[
0
][
2
]
+
rs
[
1
][
2
]
*
m
.
rs
[
1
][
2
]
+
rs
[
2
][
2
]
*
m
.
rs
[
2
][
2
]);
}
Matrix3Data
<
T
>
timesTranspose
(
const
Matrix3Data
<
T
>&
m
)
const
{
return
Matrix3Data
<
T
>
(
dot_prod3
(
rs
[
0
],
m
[
0
]),
dot_prod3
(
rs
[
0
],
m
[
1
]),
dot_prod3
(
rs
[
0
],
m
[
2
]),
dot_prod3
(
rs
[
1
],
m
[
0
]),
dot_prod3
(
rs
[
1
],
m
[
1
]),
dot_prod3
(
rs
[
1
],
m
[
2
]),
dot_prod3
(
rs
[
2
],
m
[
0
]),
dot_prod3
(
rs
[
2
],
m
[
1
]),
dot_prod3
(
rs
[
2
],
m
[
2
]));
}
Vec3Data
<
T
>
transposeTimes
(
const
Vec3Data
<
T
>&
v
)
const
{
return
Vec3Data
<
T
>
(
transposeDotX
(
v
),
transposeDotY
(
v
),
transposeDotZ
(
v
));
}
inline
T
transposeDotX
(
const
Vec3Data
<
T
>&
v
)
const
{
return
rs
[
0
][
0
]
*
v
[
0
]
+
rs
[
1
][
0
]
*
v
[
1
]
+
rs
[
2
][
0
]
*
v
[
2
];
}