quaternion: update look, from_to, rotate
This commit is contained in:
parent
1af6ed4ca8
commit
db076ad6f4
104
quaternion.cpp
104
quaternion.cpp
@ -34,7 +34,7 @@ template<> const quaternion<4, double> quaternion<4, double>::IDENTITY = { 1, 0,
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
template <size_t S, typename T>
|
||||
quaternion<S,T>
|
||||
quaternion<S,T>::rotation (const T radians, const vector<3,T> axis)
|
||||
quaternion<S,T>::angle_axis (const T radians, const vector<3,T> axis)
|
||||
{
|
||||
CHECK (is_normalised (axis));
|
||||
|
||||
@ -50,19 +50,56 @@ quaternion<S,T>::rotation (const T radians, const vector<3,T> axis)
|
||||
//-----------------------------------------------------------------------------
|
||||
template <size_t S, typename T>
|
||||
quaternion<S,T>
|
||||
quaternion<S,T>::rotation (const vector<3,T> src, const vector<3,T> dst)
|
||||
quaternion<S,T>::from_euler (vector<3,T> angles)
|
||||
{
|
||||
auto v = util::cross (src, dst);
|
||||
auto half = angles / 2;
|
||||
|
||||
auto c = cos (half);
|
||||
auto s = sin (half);
|
||||
|
||||
return {
|
||||
std::acos (dot (src, dst)),
|
||||
v.x,
|
||||
v.y,
|
||||
v.z
|
||||
c.x * c.y * c.z - s.x * s.y * s.z,
|
||||
s.x * c.y * c.z + c.x * s.y * s.z,
|
||||
c.x * s.y * c.z - s.x * c.y * s.z,
|
||||
c.x * c.y * s.z + s.x * s.y * c.z,
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// vector-to-vector rotation algorithm from:
|
||||
// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
|
||||
template <size_t S, typename T>
|
||||
quaternion<S,T>
|
||||
quaternion<S,T>::from_to (const vector<3,T> u, const vector<3,T> v)
|
||||
{
|
||||
CHECK (is_normalised (u));
|
||||
CHECK (is_normalised (v));
|
||||
|
||||
auto norm_u_norm_v = std::sqrt(dot(u, u) * dot(v, v));
|
||||
auto real_part = norm_u_norm_v + dot(u, v);
|
||||
util::vector<3,T> w;
|
||||
|
||||
if (real_part < 1.e-6f * norm_u_norm_v)
|
||||
{
|
||||
/* If u and v are exactly opposite, rotate 180 degrees
|
||||
* around an arbitrary orthogonal axis. Axis normalisation
|
||||
* can happen later, when we normalise the quaternion. */
|
||||
real_part = 0.0f;
|
||||
w = std::abs(u.x) > std::abs(u.z) ?
|
||||
util::vector3<T> (-u.y, u.x, 0.f) :
|
||||
util::vector3<T> (0.f, -u.z, u.y);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Otherwise, build quaternion the standard way. */
|
||||
w = cross(u, v);
|
||||
}
|
||||
|
||||
return normalised (util::quaternion<4,T> (real_part, w.x, w.y, w.z));
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
template <typename T>
|
||||
quaternion<4,T>
|
||||
@ -72,6 +109,7 @@ util::conjugate (quaternion<4,T> q)
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
template quaternion<4,float> util::conjugate (quaternion<4,float>);
|
||||
|
||||
|
||||
@ -130,6 +168,58 @@ quaternion<S, T>::as_matrix (void) const
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
|
||||
template <typename T>
|
||||
util::vector3<T>
|
||||
util::rotate (vector3<T> v, quaternion<4,T> q)
|
||||
{
|
||||
CHECK (is_normalised (v));
|
||||
|
||||
#if 1
|
||||
util::vector3<T> u { q.x, q.y, q.z };
|
||||
return v + 2 * cross (u, cross (u, v) + q.w * v);
|
||||
#else
|
||||
// Verbosely:
|
||||
quaternionf p { 0, v.x, v.y, v.z };
|
||||
auto p_ = q * p * conjugate (q);
|
||||
return { p_.x, p_.y, p_.z };
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
template util::vector3f util::rotate (util::vector3f, util::quaternionf);
|
||||
template util::vector3d util::rotate (util::vector3d, util::quaterniond);
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
template <size_t S, typename T>
|
||||
quaternion<S,T>
|
||||
quaternion<S,T>::look (vector<3,T> fwd, vector<3,T> up)
|
||||
{
|
||||
CHECK (is_normalised (fwd));
|
||||
CHECK (is_normalised (up));
|
||||
|
||||
constexpr util::vector3<T> RH_FWD { 0, 0, -1 };
|
||||
constexpr util::vector3<T> RH_UP { 0, 1, 0 };
|
||||
|
||||
// rotate the right-handed fwd to face the given fwd
|
||||
auto q1 = from_to (RH_FWD, fwd);
|
||||
|
||||
// can't retain the up direction if fwd is the same as up
|
||||
if (norm2 (cross (fwd, up)) < 1e-6)
|
||||
return q1;
|
||||
|
||||
// find the right-handed up rotated to the given fwd
|
||||
auto new_up = rotate (RH_UP, q1);
|
||||
|
||||
// rotate first to the new forward, then align the right handed up with
|
||||
// the given up.
|
||||
return from_to (new_up, up) * q1;
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
template <size_t S, typename T>
|
||||
std::ostream&
|
||||
|
@ -41,8 +41,11 @@ namespace util {
|
||||
|
||||
using coord::base<S,T,::util::quaternion,::util::coord::wxyz,::util::coord::abcd>::base;
|
||||
|
||||
static quaternion rotation (T radians, vector<3,T> axis);
|
||||
static quaternion rotation (vector<3,T> src, vector<3,T> dst);
|
||||
static quaternion angle_axis (T radians, vector<3,T> axis);
|
||||
static quaternion from_euler (vector<3,T>);
|
||||
|
||||
static quaternion from_to (vector<3,T>, vector<3,T>);
|
||||
static quaternion look (vector<3,T> fwd, vector<3,T> up);
|
||||
|
||||
matrix4<T> as_matrix (void) const;
|
||||
|
||||
@ -62,6 +65,11 @@ namespace util {
|
||||
operator/ (const quaternion<S,T>, const quaternion<S,T>);
|
||||
|
||||
typedef quaternion<4,float> quaternionf;
|
||||
typedef quaternion<4,double> quaterniond;
|
||||
|
||||
template <typename T>
|
||||
vector3<T>
|
||||
rotate (vector3<T>, quaternion<4,T>);
|
||||
|
||||
template <size_t S, typename T>
|
||||
std::ostream&
|
||||
|
@ -54,7 +54,7 @@ test_mq_axis (util::TAP::logger &tap)
|
||||
|
||||
for (auto t: TESTS) {
|
||||
auto m = util::matrix4f::rotation (1, t.euler);
|
||||
auto q = util::quaternionf::rotation (1, t.euler);
|
||||
auto q = util::quaternionf::angle_axis (1, t.euler);
|
||||
|
||||
auto diff = sum (abs (m - q.as_matrix ()));
|
||||
tap.expect_le (diff, 1e-6f, "matrix/quaternion rotation identities, %s", t.msg);
|
||||
@ -83,9 +83,9 @@ test_mq_euler (util::TAP::logger &tap)
|
||||
util::matrix4f::rotation (t.euler[1], { 0, 1, 0 }) *
|
||||
util::matrix4f::rotation (t.euler[2], { 0, 0, 1 });
|
||||
auto q = (
|
||||
util::quaternionf::rotation (t.euler[0], { 1, 0, 0 }) *
|
||||
util::quaternionf::rotation (t.euler[1], { 0, 1, 0 }) *
|
||||
util::quaternionf::rotation (t.euler[2], { 0, 0, 1 })
|
||||
util::quaternionf::angle_axis (t.euler[0], { 1, 0, 0 }) *
|
||||
util::quaternionf::angle_axis (t.euler[1], { 0, 1, 0 }) *
|
||||
util::quaternionf::angle_axis (t.euler[2], { 0, 0, 1 })
|
||||
).as_matrix ();
|
||||
|
||||
auto diff = util::sum (abs (m - q));
|
||||
|
@ -192,9 +192,9 @@ main (void)
|
||||
constexpr auto PI2 = 2 * util::PI<float>;
|
||||
|
||||
auto matrix = (
|
||||
util::quaternionf::rotation (t.euler[2], { 0, 0, 1 }) *
|
||||
util::quaternionf::rotation (t.euler[1], { 0, 1, 0 }) *
|
||||
util::quaternionf::rotation (t.euler[0], { 1, 0, 0 })
|
||||
util::quaternionf::angle_axis (t.euler[2], { 0, 0, 1 }) *
|
||||
util::quaternionf::angle_axis (t.euler[1], { 0, 1, 0 }) *
|
||||
util::quaternionf::angle_axis (t.euler[0], { 1, 0, 0 })
|
||||
).as_matrix ();
|
||||
|
||||
auto euler = to_euler (matrix);
|
||||
|
@ -76,7 +76,7 @@ main (void)
|
||||
for (size_t i = 0; i < elems (ROTATIONS); ++i) {
|
||||
const auto &r = ROTATIONS[i];
|
||||
|
||||
auto q = quaternionf::rotation (r.mag, r.axis).as_matrix ();
|
||||
auto q = quaternionf::angle_axis (r.mag, r.axis).as_matrix ();
|
||||
auto m = util::matrix4f::rotation (r.mag, r.axis);
|
||||
auto diff = util::abs (q - m);
|
||||
|
||||
@ -87,7 +87,7 @@ main (void)
|
||||
auto m = util::matrix4f::IDENTITY;
|
||||
|
||||
for (auto r: ROTATIONS) {
|
||||
q = q.rotation (r.mag, r.axis) * q;
|
||||
q = q.angle_axis (r.mag, r.axis) * q;
|
||||
m = m.rotation (r.mag, r.axis) * m;
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user