56 lines
1.5 KiB
C++
56 lines
1.5 KiB
C++
|
// This file is part of libigl, a simple c++ geometry processing library.
|
||
|
|
//
|
||
|
|
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
|
||
|
|
//
|
||
|
|
// This Source Code Form is subject to the terms of the Mozilla Public License
|
||
|
|
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
|
||
|
|
// obtain one at http://mozilla.org/MPL/2.0/.
|
||
|
|
#include "rotate_by_quat.h"
|
||
|
|
|
||
|
|
#include "quat_conjugate.h"
|
||
|
|
#include "quat_mult.h"
|
||
|
|
#include "normalize_quat.h"
|
||
|
|
#include <cassert>
|
||
|
|
|
||
|
|
template <typename Q_type>
|
||
|
|
IGL_INLINE void igl::rotate_by_quat(
|
||
|
|
const Q_type *v,
|
||
|
|
const Q_type *q,
|
||
|
|
Q_type *out)
|
||
|
|
{
|
||
|
|
// Quaternion form of v, copy data in v, (as a result out can be same pointer
|
||
|
|
// as v)
|
||
|
|
Q_type quat_v[4] = {v[0],v[1],v[2],0};
|
||
|
|
|
||
|
|
// normalize input
|
||
|
|
Q_type normalized_q[4];
|
||
|
|
|
||
|
|
#ifndef NDEBUG
|
||
|
|
bool normalized =
|
||
|
|
#endif
|
||
|
|
igl::normalize_quat<Q_type>(q,normalized_q);
|
||
|
|
#ifndef NDEBUG
|
||
|
|
assert(normalized);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
// Conjugate of q
|
||
|
|
Q_type q_conj[4];
|
||
|
|
igl::quat_conjugate<Q_type>(normalized_q,q_conj);
|
||
|
|
|
||
|
|
// Rotate of vector v by quaternion q is:
|
||
|
|
// q*v*conj(q)
|
||
|
|
// Compute q*v
|
||
|
|
Q_type q_mult_quat_v[4];
|
||
|
|
igl::quat_mult<Q_type>(normalized_q,quat_v,q_mult_quat_v);
|
||
|
|
// Compute (q*v) * conj(q)
|
||
|
|
igl::quat_mult<Q_type>(q_mult_quat_v,q_conj,out);
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef IGL_STATIC_LIBRARY
|
||
|
|
// Explicit template instantiation
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template void igl::rotate_by_quat<double>(double const*, double const*, double*);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template void igl::rotate_by_quat<float>(float const*, float const*, float*);
|
||
|
|
#endif
|