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
f77bad18
Commit
f77bad18
authored
May 09, 2017
by
Joseph Mirabel
Committed by
Joseph Mirabel
May 09, 2017
Browse files
Fix usage of quaternions
parent
caeec612
Changes
11
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/collision_object.h
View file @
f77bad18
...
...
@@ -199,7 +199,7 @@ public:
/// @brief compute the AABB in world space
inline
void
computeAABB
()
{
if
(
t
.
getQuatRotation
()
.
isIdentity
(
))
if
(
isQuatIdentity
(
t
.
getQuatRotation
()))
{
aabb
=
translate
(
cgeom
->
aabb_local
,
t
.
getTranslation
());
}
...
...
include/hpp/fcl/math/transform.h
View file @
f77bad18
...
...
@@ -45,243 +45,18 @@
namespace
fcl
{
/*
/// @brief Quaternion used locally by InterpMotion
class Quaternion3f
{
private:
typedef Eigen::Matrix<FCL_REAL, 4, 1, Eigen::DontAlign> Vec4f;
typedef typename Vec4f:: FixedSegmentReturnType<3>::Type XYZ_t;
typedef typename Vec4f::ConstFixedSegmentReturnType<3>::Type XYZConst_t;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
enum {
W = 0,
X = 1,
Y = 2,
Z = 3
};
/// @brief Default quaternion is identity rotation
Quaternion3f()
{
data[W] = 1;
data[X] = 0;
data[Y] = 0;
data[Z] = 0;
}
/// @brief Construct quaternion from 4D vector
template <typename Derived>
Quaternion3f(const Eigen::MatrixBase<Derived>& other) :
data (other.derived())
{}
Quaternion3f(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
{
data[W] = w;
data[X] = x;
data[Y] = y;
data[Z] = z;
}
template<typename Derived>
Quaternion3f(FCL_REAL _w, const Eigen::MatrixBase<Derived>& _vec)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
w() = _w;
vec().noalias() = _vec;
}
/// @brief Whether the rotation is identity
bool isIdentity() const
{
return (data[W] == 1) && (data[X] == 0) && (data[Y] == 0) && (data[Z] == 0);
}
/// @brief Matrix to quaternion
void fromRotation(const Matrix3f& R);
/// @brief Quaternion to matrix
void toRotation(Matrix3f& R) const;
/// @brief Euler to quaternion
void fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c);
/// @brief Quaternion to Euler
void toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const;
/// @brief Axes to quaternion
void fromAxes(const Matrix3f& axes);
/// @brief Axes to matrix
void toAxes(Matrix3f& axis) const;
/// @brief Axis and angle to quaternion
template<typename Derived>
inline void fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
FCL_REAL half_angle = 0.5 * angle;
FCL_REAL sn = sin((double)half_angle);
data[W] = cos((double)half_angle);
data.segment<3>(X).noalias() = sn*axis;
}
/// @brief Quaternion to axis and angle
void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
/// @brief Dot product between quaternions
inline FCL_REAL dot(const Quaternion3f& other) const
{
return data.dot(other.data);
}
/// @brief addition
inline const Eigen::CwiseBinaryOp<
Eigen::internal::scalar_sum_op <FCL_REAL>, const Vec4f, const Vec4f>
operator + (const Quaternion3f& other) const
{
return Eigen::CwiseBinaryOp< Eigen::internal::scalar_sum_op <FCL_REAL>,
const Vec4f, const Vec4f>
(data, other.data);
}
inline const Quaternion3f& operator += (const Quaternion3f& other)
{
data += other.data;
return *this;
}
/// @brief multiplication
inline Quaternion3f operator * (const Quaternion3f& other) const
{
return Quaternion3f(w() * other.w() - vec().dot(other.vec()),
w() * other.vec() + other.w() * vec() + vec().cross(other.vec()));
}
const Quaternion3f& operator *= (const Quaternion3f& other);
/// @brief division
Quaternion3f operator - () const;
/// @brief scalar multiplication
Quaternion3f operator * (FCL_REAL t) const;
const Quaternion3f& operator *= (FCL_REAL t);
/// @brief conjugate
inline Quaternion3f conj() const
{
return Quaternion3f (w(), -vec());
}
/// @brief inverse
inline Quaternion3f inverse() const {
Quaternion3f inv = conj();
inv.normalize();
return inv;
}
inline void normalize () {
FCL_REAL n = data.squaredNorm();
if (n > 0) data *= 1/sqrt(n);
}
/// @brief rotate a vector
template<typename Derived>
inline const quaternion_transform_return_type<Derived>
transform(const Eigen::MatrixBase<Derived>& v) const;
bool operator == (const Quaternion3f& other) const
{
return (data == other.data);
}
bool operator != (const Quaternion3f& other) const
{
return !(*this == other);
}
inline FCL_REAL& w() { return data[W]; }
inline FCL_REAL& x() { return data[X]; }
inline FCL_REAL& y() { return data[Y]; }
inline FCL_REAL& z() { return data[Z]; }
inline const FCL_REAL& w() const { return data[W]; }
inline const FCL_REAL& x() const { return data[X]; }
inline const FCL_REAL& y() const { return data[Y]; }
inline const FCL_REAL& z() const { return data[Z]; }
private:
FCL_REAL operator [] (std::size_t i) const
{
return data[i];
}
inline XYZ_t vec() { return data.segment<3>(X); }
inline XYZConst_t vec() const { return data.segment<3>(X); }
Vec4f data;
template<typename Derived>
friend struct quaternion_transform_return_type;
};
template<typename RhsType> struct quaternion_transform_return_type :
quaternion_transform_return_type_traits<RhsType>::type
{
typedef quaternion_transform_return_type_traits<RhsType> quat_traits;
typedef typename quat_traits::type Base;
EIGEN_GENERIC_PUBLIC_INTERFACE(quaternion_transform_return_type)
EIGEN_STRONG_INLINE quaternion_transform_return_type(const Quaternion3f& q, const Eigen::MatrixBase<RhsType>& v) :
Base (2*q.vec().dot(v) * q.vec(), ((q.w()*q.w() - q.vec().dot(q.vec()))*v + 2*q.w()*m_uCrossV)),
m_uCrossV (q.vec().cross(v))
{}
typename quat_traits::Cross_t m_uCrossV;
};
template<typename LhsType, typename RhsType> struct translate_return_type :
translate_return_type_traits<LhsType, RhsType>::type
{
typedef translate_return_type_traits<LhsType, RhsType> trans_traits;
typedef typename trans_traits::type Base;
EIGEN_GENERIC_PUBLIC_INTERFACE(translate_return_type)
EIGEN_STRONG_INLINE translate_return_type(const quaternion_transform_return_type<LhsType>& rv, const RhsType& T) :
Base (rv, T)
{}
};
template<typename Derived, typename OtherDerived>
const translate_return_type<Derived, OtherDerived>
operator+ (const quaternion_transform_return_type<Derived>& rv,
const Eigen::MatrixBase<OtherDerived>& T)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3);
return translate_return_type<Derived, OtherDerived>(rv, T.derived());
}
typedef
Eigen
::
Quaternion
<
FCL_REAL
>
Quaternion3f
;
template<typename Derived>
const quaternion_transform_return_type<Derived>
Quaternion3f::transform (const Eigen::MatrixBase<Derived>& v) const
inline
bool
isQuatIdentity
(
const
Quaternion3f
&
q
)
{
return
quaternion_transform_return_type<Derived> (*this, v.derived())
;
return
(
q
.
w
()
==
1
||
q
.
w
()
==
-
1
)
&&
q
.
x
()
==
0
&&
q
.
y
()
==
0
&&
q
.
z
()
==
0
;
}
static inline std::ostream& operator << (std::ostream& o
, const Quaternion3f& q)
inline
bool
areQuatEquals
(
const
Quaternion3f
&
q1
,
const
Quaternion3f
&
q
2
)
{
o << "(" << q.w() << " " <<
q.x()
<< " " <<
q.y()
<< " " <<
q.z()
<< ")";
return o
;
return
(
q1
.
w
()
==
q2
.
w
()
&&
q
1
.
x
()
==
q2
.
x
()
&&
q
1
.
y
()
==
q2
.
y
()
&&
q
1
.
z
()
==
q2
.
z
())
||
(
q1
.
w
()
==
-
q2
.
w
()
&&
q1
.
x
()
==
-
q2
.
x
()
&&
q1
.
y
()
==
-
q2
.
y
()
&&
q1
.
z
()
==
-
q2
.
z
())
;
}
*/
typedef
Eigen
::
Quaternion
<
FCL_REAL
>
Quaternion3f
;
/// @brief Simple transform class used locally by InterpMotion
class
Transform3f
...
...
@@ -313,7 +88,7 @@ public:
R
(
R_
),
T
(
T_
)
{
q
.
fromRotat
ion
(
R_
);
q
=
Quatern
ion
3f
(
R_
);
}
/// @brief Construct transform from rotation and translation
...
...
@@ -324,21 +99,24 @@ public:
}
/// @brief Construct transform from rotation
Transform3f
(
const
Matrix3f
&
R_
)
:
matrix_set
(
true
),
Transform3f
(
const
Matrix3f
&
R_
)
:
matrix_set
(
true
),
T
(
Vec3f
::
Zero
()),
R
(
R_
)
{
q
.
fromRotat
ion
(
R_
);
q
=
Quatern
ion
3f
(
R_
);
}
/// @brief Construct transform from rotation
Transform3f
(
const
Quaternion3f
&
q_
)
:
matrix_set
(
false
),
T
(
Vec3f
::
Zero
()),
q
(
q_
)
{
}
/// @brief Construct transform from translation
Transform3f
(
const
Vec3f
&
T_
)
:
matrix_set
(
true
),
T
(
T_
)
T
(
T_
),
q
(
Quaternion3f
::
Identity
())
{
R
.
setIdentity
();
}
...
...
@@ -385,7 +163,7 @@ public:
{
R
.
noalias
()
=
R_
;
T
.
noalias
()
=
T_
;
q
.
fromRotat
ion
(
R_
);
q
=
Quatern
ion
3f
(
R_
);
matrix_set
=
true
;
}
...
...
@@ -403,7 +181,7 @@ public:
{
R
.
noalias
()
=
R_
;
matrix_set
=
true
;
q
.
fromRotat
ion
(
R
);
q
=
Quatern
ion
3f
(
R
);
}
/// @brief set transform from translation
...
...
@@ -424,30 +202,30 @@ public:
template
<
typename
Derived
>
inline
Vec3f
transform
(
const
Eigen
::
MatrixBase
<
Derived
>&
v
)
const
{
return
q
.
transform
(
v
)
+
T
;
return
q
*
v
+
T
;
}
/// @brief inverse transform
inline
Transform3f
&
inverse
()
{
matrix_set
=
false
;
q
=
q
.
conj
();
T
=
q
.
transform
(
-
T
).
eval
(
);
q
=
q
.
conj
ugate
();
T
=
q
*
(
-
T
);
return
*
this
;
}
/// @brief inverse the transform and multiply with another
inline
Transform3f
inverseTimes
(
const
Transform3f
&
other
)
const
{
const
Quaternion3f
&
q_inv
=
q
.
conj
();
return
Transform3f
(
q_inv
*
other
.
q
,
q_inv
.
transform
(
other
.
T
-
T
));
const
Quaternion3f
&
q_inv
=
q
.
conj
ugate
();
return
Transform3f
(
q_inv
*
other
.
q
,
q_inv
*
(
other
.
T
-
T
));
}
/// @brief multiply with another transform
inline
const
Transform3f
&
operator
*=
(
const
Transform3f
&
other
)
{
matrix_set
=
false
;
T
+=
q
.
transform
(
other
.
T
).
eval
()
;
T
+=
q
*
other
.
T
;
q
*=
other
.
q
;
return
*
this
;
}
...
...
@@ -456,13 +234,13 @@ public:
inline
Transform3f
operator
*
(
const
Transform3f
&
other
)
const
{
Quaternion3f
q_new
=
q
*
other
.
q
;
return
Transform3f
(
q_new
,
q
.
transform
(
other
.
T
)
+
T
);
return
Transform3f
(
q_new
,
q
*
other
.
T
+
T
);
}
/// @brief check whether the transform is identity
inline
bool
isIdentity
()
const
{
return
q
.
isIdentity
()
&&
T
.
isZero
();
return
is
Quat
Identity
(
q
)
&&
T
.
isZero
();
}
/// @brief set the transform to be identity transform
...
...
@@ -470,13 +248,13 @@ public:
{
R
.
setIdentity
();
T
.
setZero
();
q
=
Quaternion3f
();
q
.
setIdentity
();
matrix_set
=
true
;
}
bool
operator
==
(
const
Transform3f
&
other
)
const
{
return
(
q
==
other
.
getQuatRotation
())
&&
(
T
==
other
.
getTranslation
());
return
areQuatEquals
(
q
,
other
.
getQuatRotation
())
&&
(
T
==
other
.
getTranslation
());
}
bool
operator
!=
(
const
Transform3f
&
other
)
const
...
...
src/BV/OBB.cpp
View file @
f77bad18
...
...
@@ -107,16 +107,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
{
OBB
b
;
b
.
To
=
(
b1
.
To
+
b2
.
To
)
*
0.5
;
Quaternion3f
q0
,
q1
;
q0
.
fromAxes
(
b1
.
axes
);
q1
.
fromAxes
(
b2
.
axes
);
Quaternion3f
q0
(
b1
.
axes
),
q1
(
b2
.
axes
);
if
(
q0
.
dot
(
q1
)
<
0
)
q1
=
-
q
1
;
q1
.
coeffs
()
*
=
-
1
;
Quaternion3f
q
=
q0
+
q1
;
FCL_REAL
inv_length
=
1.0
/
std
::
sqrt
(
q
.
dot
(
q
));
q
=
q
*
inv_length
;
q
.
toAxes
(
b
.
axes
);
Quaternion3f
q
((
q0
.
coeffs
()
+
q1
.
coeffs
()).
normalized
());
b
.
axes
=
q
.
toRotationMatrix
();
Vec3f
vertex
[
8
],
diff
;
...
...
src/broadphase/broadphase_dynamic_AABB_tree.cpp
View file @
f77bad18
...
...
@@ -330,7 +330,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c
bool
collisionRecurse
(
DynamicAABBTreeCollisionManager
::
DynamicAABBNode
*
root1
,
const
OcTree
*
tree2
,
const
OcTree
::
OcTreeNode
*
root2
,
const
AABB
&
root2_bv
,
const
Transform3f
&
tf2
,
void
*
cdata
,
CollisionCallBack
callback
)
{
if
(
tf2
.
getQuatRotation
()
.
isIdentity
(
))
if
(
isQuatIdentity
(
tf2
.
getQuatRotation
()))
return
collisionRecurse_
(
root1
,
tree2
,
root2
,
root2_bv
,
tf2
.
getTranslation
(),
cdata
,
callback
);
else
// has rotation
return
collisionRecurse_
(
root1
,
tree2
,
root2
,
root2_bv
,
tf2
,
cdata
,
callback
);
...
...
@@ -417,7 +417,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c
bool
distanceRecurse
(
DynamicAABBTreeCollisionManager
::
DynamicAABBNode
*
root1
,
const
OcTree
*
tree2
,
const
OcTree
::
OcTreeNode
*
root2
,
const
AABB
&
root2_bv
,
const
Transform3f
&
tf2
,
void
*
cdata
,
DistanceCallBack
callback
,
FCL_REAL
&
min_dist
)
{
if
(
tf2
.
getQuatRotation
()
.
isIdentity
(
))
if
(
isQuatIdentity
(
tf2
.
getQuatRotation
()))
return
distanceRecurse_
(
root1
,
tree2
,
root2
,
root2_bv
,
tf2
.
getTranslation
(),
cdata
,
callback
,
min_dist
);
else
return
distanceRecurse_
(
root1
,
tree2
,
root2
,
root2_bv
,
tf2
,
cdata
,
callback
,
min_dist
);
...
...
src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
View file @
f77bad18
...
...
@@ -634,7 +634,7 @@ bool selfDistanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode*
#if FCL_HAVE_OCTOMAP
bool
collisionRecurse
(
DynamicAABBTreeCollisionManager_Array
::
DynamicAABBNode
*
nodes1
,
size_t
root1_id
,
const
OcTree
*
tree2
,
const
OcTree
::
OcTreeNode
*
root2
,
const
AABB
&
root2_bv
,
const
Transform3f
&
tf2
,
void
*
cdata
,
CollisionCallBack
callback
)
{
if
(
tf2
.
getQuatRotation
()
.
isIdentity
(
))
if
(
isQuatIdentity
(
tf2
.
getQuatRotation
()))
return
collisionRecurse_
(
nodes1
,
root1_id
,
tree2
,
root2
,
root2_bv
,
tf2
.
getTranslation
(),
cdata
,
callback
);
else
return
collisionRecurse_
(
nodes1
,
root1_id
,
tree2
,
root2
,
root2_bv
,
tf2
,
cdata
,
callback
);
...
...
@@ -643,7 +643,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no
bool
distanceRecurse
(
DynamicAABBTreeCollisionManager_Array
::
DynamicAABBNode
*
nodes1
,
size_t
root1_id
,
const
OcTree
*
tree2
,
const
OcTree
::
OcTreeNode
*
root2
,
const
AABB
&
root2_bv
,
const
Transform3f
&
tf2
,
void
*
cdata
,
DistanceCallBack
callback
,
FCL_REAL
&
min_dist
)
{
if
(
tf2
.
getQuatRotation
()
.
isIdentity
(
))
if
(
isQuatIdentity
(
tf2
.
getQuatRotation
()))
return
distanceRecurse_
(
nodes1
,
root1_id
,
tree2
,
root2
,
root2_bv
,
tf2
.
getTranslation
(),
cdata
,
callback
,
min_dist
);
else
return
distanceRecurse_
(
nodes1
,
root1_id
,
tree2
,
root2
,
root2_bv
,
tf2
,
cdata
,
callback
,
min_dist
);
...
...
src/math/transform.cpp
View file @
f77bad18
...
...
@@ -42,240 +42,18 @@
namespace
fcl
{
void
Quaternion3f
::
fromRotation
(
const
Matrix3f
&
R
)
{
const
int
next
[
3
]
=
{
1
,
2
,
0
};
FCL_REAL
trace
=
R
(
0
,
0
)
+
R
(
1
,
1
)
+
R
(
2
,
2
);
FCL_REAL
root
;
if
(
trace
>
0.0
)
{
// |w| > 1/2, may as well choose w > 1/2
root
=
sqrt
(
trace
+
1.0
);
// 2w
data
[
W
]
=
0.5
*
root
;
root
=
0.5
/
root
;
// 1/(4w)
data
[
X
]
=
(
R
(
2
,
1
)
-
R
(
1
,
2
))
*
root
;
data
[
Y
]
=
(
R
(
0
,
2
)
-
R
(
2
,
0
))
*
root
;
data
[
Z
]
=
(
R
(
1
,
0
)
-
R
(
0
,
1
))
*
root
;
}
else
{
// |w| <= 1/2
int
i
=
0
;
if
(
R
(
1
,
1
)
>
R
(
0
,
0
))
{
i
=
1
;
}
if
(
R
(
2
,
2
)
>
R
(
i
,
i
))
{
i
=
2
;
}
int
j
=
next
[
i
];
int
k
=
next
[
j
];
root
=
sqrt
(
R
(
i
,
i
)
-
R
(
j
,
j
)
-
R
(
k
,
k
)
+
1.0
);
FCL_REAL
*
quat
[
3
]
=
{
&
data
[
X
],
&
data
[
Y
],
&
data
[
Z
]
};
*
quat
[
i
]
=
0.5
*
root
;
root
=
0.5
/
root
;
data
[
W
]
=
(
R
(
k
,
j
)
-
R
(
j
,
k
))
*
root
;
*
quat
[
j
]
=
(
R
(
j
,
i
)
+
R
(
i
,
j
))
*
root
;
*
quat
[
k
]
=
(
R
(
k
,
i
)
+
R
(
i
,
k
))
*
root
;
}
}
void
Quaternion3f
::
toRotation
(
Matrix3f
&
R
)
const
{
assert
(
.99
<
data
[
W
]
*
data
[
W
]
+
data
[
X
]
*
data
[
X
]
+
data
[
Y
]
*
data
[
Y
]
+
data
[
Z
]
*
data
[
Z
]);
assert
(
data
[
W
]
*
data
[
W
]
+
data
[
X
]
*
data
[
X
]
+
data
[
Y
]
*
data
[
Y
]
+
data
[
Z
]
*
data
[
Z
]
<
1.01
);
FCL_REAL
twoX
=
2.0
*
data
[
X
];
FCL_REAL
twoY
=
2.0
*
data
[
Y
];
FCL_REAL
twoZ
=
2.0
*
data
[
Z
];
FCL_REAL
twoWX
=
twoX
*
data
[
W
];
FCL_REAL
twoWY
=
twoY
*
data
[
W
];
FCL_REAL
twoWZ
=
twoZ
*
data
[
W
];
FCL_REAL
twoXX
=
twoX
*
data
[
X
];
FCL_REAL
twoXY
=
twoY
*
data
[
X
];
FCL_REAL
twoXZ
=
twoZ
*
data
[
X
];
FCL_REAL
twoYY
=
twoY
*
data
[
Y
];
FCL_REAL
twoYZ
=
twoZ
*
data
[
Y
];
FCL_REAL
twoZZ
=
twoZ
*
data
[
Z
];
R
(
0
,
0
)
=
1.0
-
(
twoYY
+
twoZZ
);
R
(
0
,
1
)
=
twoXY
-
twoWZ
;
R
(
0
,
2
)
=
twoXZ
+
twoWY
;
R
(
1
,
0
)
=
twoXY
+
twoWZ
;
R
(
1
,
1
)
=
1.0
-
(
twoXX
+
twoZZ
);
R
(
1
,
2
)
=
twoYZ
-
twoWX
;
R
(
2
,
0
)
=
twoXZ
-
twoWY
;
R
(
2
,
1
)
=
twoYZ
+
twoWX
;
R
(
2
,
2
)
=
1.0
-
(
twoXX
+
twoYY
);
}
void
Quaternion3f
::
fromAxes
(
const
Matrix3f
&
axes
)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
const
int
next
[
3
]
=
{
1
,
2
,
0
};
FCL_REAL
trace
=
axes
.
trace
();
FCL_REAL
root
;
if
(
trace
>
0.0
)
{
// |w| > 1/2, may as well choose w > 1/2
root
=
sqrt
(
trace
+
1.0
);
// 2w
data
[
W
]
=
0.5
*
root
;
root
=
0.5
/
root
;
// 1/(4w)
data
[
X
]
=
(
axes
(
1
,
2
)
-
axes
(
2
,
1
))
*
root
;
data
[
Y
]
=
(
axes
(
2
,
0
)
-
axes
(
0
,
2
))
*
root
;
data
[
Z
]
=
(
axes
(
0
,
1
)
-
axes
(
1
,
0
))
*
root
;
}
else
{
// |w| <= 1/2
int
i
=
0
;
if
(
axes
(
1
,
1
)
>
axes
(
0
,
0
))
{
i
=
1
;
}
if
(
axes
(
2
,
2
)
>
axes
(
i
,
i
))
{
i
=
2
;
}
int
j
=
next
[
i
];
int
k
=
next
[
j
];
root
=
sqrt
(
axes
(
i
,
i
)
-
axes
(
j
,
j
)
-
axes
(
k
,
k
)
+
1.0
);
FCL_REAL
*
quat
[
3
]
=
{
&
data
[
X
],
&
data
[
Y
],
&
data
[
Z
]
};
*
quat
[
i
]
=
0.5
*
root
;
root
=
0.5
/
root
;
data
[
W
]
=
(
axes
(
j
,
k
)
-
axes
(
k
,
j
))
*
root
;
*
quat
[
j
]
=
(
axes
(
i
,
j
)
+
axes
(
j
,
i
))
*
root
;
*
quat
[
k
]
=
(
axes
(
i
,
k
)
+
axes
(
k
,
i
))
*
root
;
}
}
void
Quaternion3f
::
toAxes
(
Matrix3f
&
axes
)
const
{
FCL_REAL
twoX
=
2.0
*
data
[
X
];
FCL_REAL
twoY
=
2.0
*
data
[
Y
];
FCL_REAL
twoZ
=
2.0
*
data
[
Z
];
FCL_REAL
twoWX
=
twoX
*
data
[
W
];
FCL_REAL
twoWY
=
twoY
*
data
[
W
];
FCL_REAL
twoWZ
=
twoZ
*
data
[
W
];
FCL_REAL
twoXX
=
twoX
*
data
[
X
];
FCL_REAL
twoXY
=
twoY
*
data
[
X
];
FCL_REAL
twoXZ
=
twoZ
*
data
[
X
];
FCL_REAL
twoYY
=
twoY
*
data
[
Y
];
FCL_REAL
twoYZ
=
twoZ
*
data
[
Y
];
FCL_REAL
twoZZ
=
twoZ
*
data
[
Z
];
axes
<<
1.0
-
(
twoYY
+
twoZZ
),
twoXY
+
twoWZ
,
twoXZ
-
twoWY
,
twoXY
-
twoWZ
,
1.0
-
(
twoXX
+
twoZZ
),
twoYZ
+
twoWX
,
twoXZ
+
twoWY
,
twoYZ
-
twoWX
,
1.0
-
(
twoXX
+
twoYY
);
}
void
Quaternion3f
::
toAxisAngle
(
Vec3f
&
axis
,
FCL_REAL
&
angle
)
const
{
FCL_REAL
sqr_length
=
data
.
squaredNorm
();
if
(
sqr_length
>
0
)
{
angle
=
2.0
*
acos
((
double
)
data
[
W
]);
double
inv_length
=
1.0
/
sqrt
(
sqr_length
);
axis
.
noalias
()
=
inv_length
*
data
.
segment
<
3
>
(
X
);
}
else
{
angle
=
0
;
axis
[
0
]
=
1
;
axis
[
1
]
=
0
;
axis
[
2
]
=
0
;
}
}
const
Quaternion3f
&
Quaternion3f
::
operator
*=
(
const
Quaternion3f
&
other
)
{
FCL_REAL
a
=
data
[
W
]
*
other
.
data
[
W
]
-
data
[
X
]
*
other
.
data
[
X
]
-
data
[
Y
]
*
other
.
data
[
Y
]
-
data
[
Z
]
*
other
.
data
[
Z
];