fiddling ...
This commit is contained in:
parent
bf834a33e0
commit
b2172d88fb
3 changed files with 203 additions and 194 deletions
|
|
@ -31,19 +31,34 @@
|
||||||
|
|
||||||
namespace pw {
|
namespace pw {
|
||||||
|
|
||||||
|
template <std::size_t R,std::size_t C,bool RowMajor = false>
|
||||||
|
struct matrixtraits_
|
||||||
|
{
|
||||||
|
//! rows
|
||||||
|
inline std::size_t rows() const { return R; }
|
||||||
|
|
||||||
|
//! return number of columns
|
||||||
|
inline std::size_t cols() const { return C; }
|
||||||
|
};
|
||||||
|
|
||||||
template <typename T, std::size_t R,std::size_t C,bool RowMajor = false>
|
template <typename T, std::size_t R,std::size_t C,bool RowMajor = false>
|
||||||
struct matrix_ : matrixbase_<T, matrix_<T, R, C>> {
|
struct matrix_ : matrixbase_<T, matrix_<T, R, C>>,matrixtraits_<R,C,RowMajor>
|
||||||
|
{
|
||||||
|
|
||||||
T data[R*C];
|
T data[R*C];
|
||||||
|
|
||||||
typedef matrixbase_<T, matrix_<T, R, C>> Base;
|
// typedef matrixbase_<T, matrix_<T, R, C>> Base;
|
||||||
using Base::Base;
|
using matrixbase_<T, matrix_<T, R, C>>::matrixbase_;
|
||||||
|
using matrixtraits_<R,C,RowMajor>::rows;
|
||||||
|
using matrixtraits_<R,C,RowMajor>::cols;
|
||||||
|
|
||||||
|
|
||||||
matrix_(const matrix_& other)
|
matrix_(const matrix_& other)
|
||||||
{
|
{
|
||||||
*this = other;
|
*this = other;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
explicit matrix_(std::initializer_list<T> args)
|
explicit matrix_(std::initializer_list<T> args)
|
||||||
{
|
{
|
||||||
typename std::initializer_list<T>::iterator it = args.begin();
|
typename std::initializer_list<T>::iterator it = args.begin();
|
||||||
|
|
@ -64,14 +79,9 @@ struct matrix_ : matrixbase_<T, matrix_<T, R, C>> {
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
//! rows
|
|
||||||
inline std::size_t rows() const { return R; }
|
|
||||||
|
|
||||||
//! return number of columns
|
// //! get cell count
|
||||||
inline std::size_t cols() const { return C; }
|
// inline std::size_t coefficients() const { return this->size(); }
|
||||||
|
|
||||||
//! get cell count
|
|
||||||
inline std::size_t coefficients() const { return this->size(); }
|
|
||||||
|
|
||||||
inline size_t offset(size_t r,size_t c) const {
|
inline size_t offset(size_t r,size_t c) const {
|
||||||
return (RowMajor) ? r * C + c : c * R + r;
|
return (RowMajor) ? r * C + c : c * R + r;
|
||||||
|
|
|
||||||
|
|
@ -33,79 +33,79 @@ namespace pw {
|
||||||
template <typename T>
|
template <typename T>
|
||||||
struct quaternion_ : vector4_<T> {
|
struct quaternion_ : vector4_<T> {
|
||||||
|
|
||||||
typedef vector4_<T> Base;
|
typedef vector4_<T> Base;
|
||||||
|
|
||||||
using Base::Base;
|
using Base::Base;
|
||||||
using Base::x;
|
using Base::x;
|
||||||
using Base::y;
|
using Base::y;
|
||||||
using Base::z;
|
using Base::z;
|
||||||
using Base::w;
|
using Base::w;
|
||||||
// using Base::lerp;
|
// using Base::lerp;
|
||||||
// using Base::operator*;
|
// using Base::operator*;
|
||||||
// using Base::operator/;
|
using Base::operator/;
|
||||||
|
|
||||||
inline const quaternion_ operator * (const quaternion_& rhs) const {
|
inline const quaternion_ operator * (const quaternion_& rhs) const {
|
||||||
return quaternion_(
|
return quaternion_(
|
||||||
rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(),
|
rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(),
|
||||||
rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(),
|
rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(),
|
||||||
rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(),
|
rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(),
|
||||||
rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z()
|
rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z()
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
//! conjugate
|
//! conjugate
|
||||||
inline quaternion_ conjugate() const { return quaternion_( { -x(),-y(),-z(),w() } ); }
|
inline quaternion_ conjugate() const { return quaternion_( { -x(),-y(),-z(),w() } ); }
|
||||||
|
|
||||||
//! compute inverse
|
//! compute inverse
|
||||||
inline auto inverse() const {
|
inline quaternion_ inverse() const {
|
||||||
return conjugate() / this->norm();
|
return conjugate() / this->norm();
|
||||||
}
|
}
|
||||||
|
|
||||||
const matrix4x4_<T> to_matrix() const {
|
const matrix4x4_<T> to_matrix() const {
|
||||||
|
|
||||||
matrix4x4_<T> m; m.set_identity();
|
matrix4x4_<T> m; m.set_identity();
|
||||||
|
|
||||||
T xx = x() * x();
|
T xx = x() * x();
|
||||||
T xy = x() * y();
|
T xy = x() * y();
|
||||||
T xz = x() * z();
|
T xz = x() * z();
|
||||||
T xw = x() * w();
|
T xw = x() * w();
|
||||||
|
|
||||||
T yy = y() * y();
|
T yy = y() * y();
|
||||||
T yz = y() * z();
|
T yz = y() * z();
|
||||||
T yw = y() * w();
|
T yw = y() * w();
|
||||||
|
|
||||||
T zz = z() * z();
|
T zz = z() * z();
|
||||||
T zw = z() * w();
|
T zw = z() * w();
|
||||||
|
|
||||||
m(0,0) = 1 - 2 * ( yy + zz );
|
m(0,0) = 1 - 2 * ( yy + zz );
|
||||||
m(0,1) = 2 * ( xy - zw );
|
m(0,1) = 2 * ( xy - zw );
|
||||||
m(0,2) = 2 * ( xz + yw );
|
m(0,2) = 2 * ( xz + yw );
|
||||||
|
|
||||||
m(1,0) = 2 * ( xy + zw );
|
m(1,0) = 2 * ( xy + zw );
|
||||||
m(1,1) = 1 - 2 * ( xx + zz );
|
m(1,1) = 1 - 2 * ( xx + zz );
|
||||||
m(1,2) = 2 * ( yz - xw );
|
m(1,2) = 2 * ( yz - xw );
|
||||||
|
|
||||||
m(2,0) = 2 * ( xz - yw );
|
m(2,0) = 2 * ( xz - yw );
|
||||||
m(2,1) = 2 * ( yz + xw );
|
m(2,1) = 2 * ( yz + xw );
|
||||||
m(2,2) = 1 - 2 * ( xx + yy );
|
m(2,2) = 1 - 2 * ( xx + yy );
|
||||||
|
|
||||||
return m;
|
return m;
|
||||||
}
|
}
|
||||||
|
|
||||||
static quaternion_<T> from_matrix(const matrix_<T,4,4> &m) {
|
static quaternion_<T> from_matrix(const matrix_<T,4,4> &m) {
|
||||||
using std::sqrt;
|
using std::sqrt;
|
||||||
const T wtemp = sqrt(T(1) + m(0,0) + m(1,1) + m(2,2)) / T(2);
|
const T wtemp = sqrt(T(1) + m(0,0) + m(1,1) + m(2,2)) / T(2);
|
||||||
const T w4 = T(4.0) * wtemp;
|
const T w4 = T(4.0) * wtemp;
|
||||||
return quaternion_<T>(
|
return quaternion_<T>(
|
||||||
(m(2,1) - m(1,2)) / w4,
|
(m(2,1) - m(1,2)) / w4,
|
||||||
(m(0,2) - m(2,0)) / w4,
|
(m(0,2) - m(2,0)) / w4,
|
||||||
(m(1,0) - m(0,1)) / w4,
|
(m(1,0) - m(0,1)) / w4,
|
||||||
wtemp);
|
wtemp);
|
||||||
}
|
}
|
||||||
|
|
||||||
static const quaternion_ normalized_lerp(const quaternion_ &a,const quaternion_ &b,const T &t) {
|
static const quaternion_ normalized_lerp(const quaternion_ &a,const quaternion_ &b,const T &t) {
|
||||||
return quaternion_(lerp(a,b,t).normalized());
|
return quaternion_(lerp(a,b,t).normalized());
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
//
|
//
|
||||||
|
|
@ -129,129 +129,129 @@ typedef quaternion_<double> quaterniond;
|
||||||
template <typename T>
|
template <typename T>
|
||||||
class quaternion_ {
|
class quaternion_ {
|
||||||
|
|
||||||
static const T _sqrt90;
|
static const T _sqrt90;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
typedef vector4_<T> coefficient_type;
|
typedef vector4_<T> coefficient_type;
|
||||||
typedef T value_type;
|
typedef T value_type;
|
||||||
|
|
||||||
quaternion_() { *this = identity(); }
|
quaternion_() { *this = identity(); }
|
||||||
|
|
||||||
quaternion_(const T& x,const T& y,const T& z,const T& w)
|
quaternion_(const T& x,const T& y,const T& z,const T& w)
|
||||||
: _q(coefficient_type(x,y,z,w)) {}
|
: _q(coefficient_type(x,y,z,w)) {}
|
||||||
|
|
||||||
quaternion_(const coefficient_type& vec) { *this = vec; }
|
quaternion_(const coefficient_type& vec) { *this = vec; }
|
||||||
|
|
||||||
inline quaternion_& operator = (const coefficient_type& vec) { _q = vec; return *this; }
|
inline quaternion_& operator = (const coefficient_type& vec) { _q = vec; return *this; }
|
||||||
|
|
||||||
inline void set(const T& x,const T& y,const T& z,const T& w) {
|
inline void set(const T& x,const T& y,const T& z,const T& w) {
|
||||||
_q.set(x,y,z,w);
|
_q.set(x,y,z,w);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void set_x(const T& v) { x() = v; }
|
inline void set_x(const T& v) { x() = v; }
|
||||||
inline void set_y(const T& v) { y() = v; }
|
inline void set_y(const T& v) { y() = v; }
|
||||||
inline void set_z(const T& v) { z() = v; }
|
inline void set_z(const T& v) { z() = v; }
|
||||||
inline void set_w(const T& v) { w() = v; }
|
inline void set_w(const T& v) { w() = v; }
|
||||||
|
|
||||||
inline const coefficient_type& as_vector() const { return _q; }
|
inline const coefficient_type& as_vector() const { return _q; }
|
||||||
|
|
||||||
inline T& x() { return _q.x(); }
|
inline T& x() { return _q.x(); }
|
||||||
inline T& y() { return _q.x(); }
|
inline T& y() { return _q.x(); }
|
||||||
inline T& z() { return _q.z(); }
|
inline T& z() { return _q.z(); }
|
||||||
inline T& w() { return _q.w(); }
|
inline T& w() { return _q.w(); }
|
||||||
|
|
||||||
inline const T& x() const { return _q.z(); }
|
inline const T& x() const { return _q.z(); }
|
||||||
inline const T& y() const { return _q.y(); }
|
inline const T& y() const { return _q.y(); }
|
||||||
inline const T& z() const { return _q.z(); }
|
inline const T& z() const { return _q.z(); }
|
||||||
inline const T& w() const { return _q.w(); }
|
inline const T& w() const { return _q.w(); }
|
||||||
|
|
||||||
|
|
||||||
//! multiplication
|
//! multiplication
|
||||||
inline const quaternion_ operator * (const quaternion_& rhs) const {
|
inline const quaternion_ operator * (const quaternion_& rhs) const {
|
||||||
return quaternion_(
|
return quaternion_(
|
||||||
rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(),
|
rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(),
|
||||||
rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(),
|
rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(),
|
||||||
rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(),
|
rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(),
|
||||||
rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z()
|
rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z()
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
//! multiply with scalar
|
//! multiply with scalar
|
||||||
inline const quaternion_ operator * (const T& s) const {
|
inline const quaternion_ operator * (const T& s) const {
|
||||||
return quaternion_(x()*s,y()*s,z()*s,w()*s);
|
return quaternion_(x()*s,y()*s,z()*s,w()*s);
|
||||||
}
|
}
|
||||||
|
|
||||||
//! addition
|
//! addition
|
||||||
inline const quaternion_ operator + (const quaternion_& rhs) const {
|
inline const quaternion_ operator + (const quaternion_& rhs) const {
|
||||||
return quaternion_(coefficient_type(this->_q + rhs._q));
|
return quaternion_(coefficient_type(this->_q + rhs._q));
|
||||||
}
|
}
|
||||||
|
|
||||||
//! addition
|
//! addition
|
||||||
inline const quaternion_ operator - (const quaternion_& rhs) const {
|
inline const quaternion_ operator - (const quaternion_& rhs) const {
|
||||||
return quaternion_(this->_q - rhs._q);
|
return quaternion_(this->_q - rhs._q);
|
||||||
}
|
}
|
||||||
|
|
||||||
//! squared norm
|
//! squared norm
|
||||||
inline const T squared_norm() const { return _q.squared_norm(); }
|
inline const T squared_norm() const { return _q.squared_norm(); }
|
||||||
|
|
||||||
//! norm
|
//! norm
|
||||||
inline const T norm() const { return _q.norm(); }
|
inline const T norm() const { return _q.norm(); }
|
||||||
|
|
||||||
//! dot product
|
//! dot product
|
||||||
inline const T dot(const quaternion_& other) const { return dot(_q,other._q); }
|
inline const T dot(const quaternion_& other) const { return dot(_q,other._q); }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//! compute normalized
|
//! compute normalized
|
||||||
inline quaternion_ normalized() const {
|
inline quaternion_ normalized() const {
|
||||||
return quaternion_(_q.normalized());
|
return quaternion_(_q.normalized());
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void normalize() { *this = this->normalized(); }
|
inline void normalize() { *this = this->normalized(); }
|
||||||
|
|
||||||
//! conversion from a matrix
|
//! conversion from a matrix
|
||||||
inline static const quaternion_ from_matrix(const matrix_<T,4,4> &m);
|
inline static const quaternion_ from_matrix(const matrix_<T,4,4> &m);
|
||||||
|
|
||||||
//! conversion to a matrix
|
//! conversion to a matrix
|
||||||
const matrix_<T,4,4> to_matrix() const;
|
const matrix_<T,4,4> to_matrix() const;
|
||||||
|
|
||||||
//! return identiy quaternion
|
//! return identiy quaternion
|
||||||
static const quaternion_<T> identity();
|
static const quaternion_<T> identity();
|
||||||
|
|
||||||
static const quaternion_<T> rotate_180_degree_around_x(); ///< rotate 180 degree around X axis
|
static const quaternion_<T> rotate_180_degree_around_x(); ///< rotate 180 degree around X axis
|
||||||
static const quaternion_<T> rotate_180_degree_around_y(); ///< rotate 180 degree around Y axis
|
static const quaternion_<T> rotate_180_degree_around_y(); ///< rotate 180 degree around Y axis
|
||||||
static const quaternion_<T> rotate_180_degree_around_z(); ///< rotate 180 degree around Z axis
|
static const quaternion_<T> rotate_180_degree_around_z(); ///< rotate 180 degree around Z axis
|
||||||
|
|
||||||
static const quaternion_<T> rotate_90_degree_around_x(bool negative = false);
|
static const quaternion_<T> rotate_90_degree_around_x(bool negative = false);
|
||||||
static const quaternion_<T> rotate_90_degree_around_y(bool negative = false);
|
static const quaternion_<T> rotate_90_degree_around_y(bool negative = false);
|
||||||
static const quaternion_<T> rotate_90_degree_around_z(bool negative = false);
|
static const quaternion_<T> rotate_90_degree_around_z(bool negative = false);
|
||||||
|
|
||||||
template <typename AxisAngleType>
|
template <typename AxisAngleType>
|
||||||
static const quaternion_<T> from_axisangle(const AxisAngleType &aa) {
|
static const quaternion_<T> from_axisangle(const AxisAngleType &aa) {
|
||||||
|
|
||||||
using std::sin;
|
using std::sin;
|
||||||
using std::cos;
|
using std::cos;
|
||||||
|
|
||||||
const T sinHalfAngle(sin(aa.angle() * T(0.5) ));
|
const T sinHalfAngle(sin(aa.angle() * T(0.5) ));
|
||||||
|
|
||||||
return quaternion_<T>(aa.axis().x() * sinHalfAngle, // x
|
return quaternion_<T>(aa.axis().x() * sinHalfAngle, // x
|
||||||
aa.axis().y() * sinHalfAngle, // y
|
aa.axis().y() * sinHalfAngle, // y
|
||||||
aa.axis().z() * sinHalfAngle, // z
|
aa.axis().z() * sinHalfAngle, // z
|
||||||
cos(aa.angle() * 0.5) // w
|
cos(aa.angle() * 0.5) // w
|
||||||
);
|
);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const quaternion_<T> lerp(const quaternion_& qa,const quaternion_& qb,const T& t);
|
static const quaternion_<T> lerp(const quaternion_& qa,const quaternion_& qb,const T& t);
|
||||||
static const quaternion_<T> normalized_lerp(const quaternion_& qa,const quaternion_& qb,const T& t);
|
static const quaternion_<T> normalized_lerp(const quaternion_& qa,const quaternion_& qb,const T& t);
|
||||||
static const quaternion_<T> slerp(const quaternion_& qa,const quaternion_& qb,const T& t);
|
static const quaternion_<T> slerp(const quaternion_& qa,const quaternion_& qb,const T& t);
|
||||||
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
coefficient_type _q;
|
coefficient_type _q;
|
||||||
};
|
};
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
|
|
@ -265,42 +265,42 @@ const T quaternion_<T>::_sqrt90 = std::sqrt(0.5);
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::identity()
|
const quaternion_<T> quaternion_<T>::identity()
|
||||||
{
|
{
|
||||||
return quaternion_<T>(0,0,0,1);
|
return quaternion_<T>(0,0,0,1);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::rotate_180_degree_around_x()
|
const quaternion_<T> quaternion_<T>::rotate_180_degree_around_x()
|
||||||
{
|
{
|
||||||
return quaternion_<T>(1,0,0,0);
|
return quaternion_<T>(1,0,0,0);
|
||||||
}
|
}
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::rotate_180_degree_around_y()
|
const quaternion_<T> quaternion_<T>::rotate_180_degree_around_y()
|
||||||
{
|
{
|
||||||
return quaternion_<T>(0,1,0,0);
|
return quaternion_<T>(0,1,0,0);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::rotate_180_degree_around_z()
|
const quaternion_<T> quaternion_<T>::rotate_180_degree_around_z()
|
||||||
{
|
{
|
||||||
return quaternion_<T>(0,0,1,0);
|
return quaternion_<T>(0,0,1,0);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::rotate_90_degree_around_x(bool negative/* = false*/)
|
const quaternion_<T> quaternion_<T>::rotate_90_degree_around_x(bool negative/* = false*/)
|
||||||
{
|
{
|
||||||
return quaternion_<T>((negative) ? - _sqrt90 : _sqrt90,0,0,_sqrt90);
|
return quaternion_<T>((negative) ? - _sqrt90 : _sqrt90,0,0,_sqrt90);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::rotate_90_degree_around_y(bool negative/* = false*/)
|
const quaternion_<T> quaternion_<T>::rotate_90_degree_around_y(bool negative/* = false*/)
|
||||||
{
|
{
|
||||||
return quaternion_<T>(0, (negative) ? -_sqrt90 : _sqrt90,0,_sqrt90);
|
return quaternion_<T>(0, (negative) ? -_sqrt90 : _sqrt90,0,_sqrt90);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::rotate_90_degree_around_z(bool negative/* = false*/)
|
const quaternion_<T> quaternion_<T>::rotate_90_degree_around_z(bool negative/* = false*/)
|
||||||
{
|
{
|
||||||
return quaternion_<T>(0,0,(negative) ? -_sqrt90 : _sqrt90, _sqrt90);
|
return quaternion_<T>(0,0,(negative) ? -_sqrt90 : _sqrt90, _sqrt90);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -308,40 +308,40 @@ const quaternion_<T> quaternion_<T>::rotate_90_degree_around_z(bool negative/* =
|
||||||
template <typename T>
|
template <typename T>
|
||||||
const quaternion_<T> quaternion_<T>::slerp(const quaternion_<T>& qa,const quaternion_<T>& qb,const T& t)
|
const quaternion_<T> quaternion_<T>::slerp(const quaternion_<T>& qa,const quaternion_<T>& qb,const T& t)
|
||||||
{
|
{
|
||||||
using std::abs;
|
using std::abs;
|
||||||
using std::sqrt;
|
using std::sqrt;
|
||||||
using std::acos;
|
using std::acos;
|
||||||
|
|
||||||
// quaternion to return
|
// quaternion to return
|
||||||
quaternion_ qm;
|
quaternion_ qm;
|
||||||
// Calculate angle between them.
|
// Calculate angle between them.
|
||||||
double cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z();
|
double cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z();
|
||||||
// if qa=qb or qa=-qb then theta = 0 and we can return qa
|
// if qa=qb or qa=-qb then theta = 0 and we can return qa
|
||||||
if (abs(cosHalfTheta) >= T(1.)) {
|
if (abs(cosHalfTheta) >= T(1.)) {
|
||||||
return qa;
|
return qa;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate temporary values.
|
// Calculate temporary values.
|
||||||
double halfTheta = acos(cosHalfTheta);
|
double halfTheta = acos(cosHalfTheta);
|
||||||
double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
|
double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
|
||||||
// if theta = 180 degrees then result is not fully defined
|
// if theta = 180 degrees then result is not fully defined
|
||||||
// we could rotate around any axis normal to qa or qb
|
// we could rotate around any axis normal to qa or qb
|
||||||
if (::std::abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
|
if (::std::abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
|
||||||
qm.w() = (qa.w() * 0.5 + qb.w() * 0.5);
|
qm.w() = (qa.w() * 0.5 + qb.w() * 0.5);
|
||||||
qm.x() = (qa.x() * 0.5 + qb.x() * 0.5);
|
qm.x() = (qa.x() * 0.5 + qb.x() * 0.5);
|
||||||
qm.y() = (qa.y() * 0.5 + qb.y() * 0.5);
|
qm.y() = (qa.y() * 0.5 + qb.y() * 0.5);
|
||||||
qm.z() = (qa.z() * 0.5 + qb.z() * 0.5);
|
qm.z() = (qa.z() * 0.5 + qb.z() * 0.5);
|
||||||
return qm;
|
return qm;
|
||||||
}
|
}
|
||||||
double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
|
double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
|
||||||
double ratioB = sin(t * halfTheta) / sinHalfTheta;
|
double ratioB = sin(t * halfTheta) / sinHalfTheta;
|
||||||
//calculate Quaternion.
|
//calculate Quaternion.
|
||||||
qm.w() = (qa.w() * ratioA + qb.w() * ratioB);
|
qm.w() = (qa.w() * ratioA + qb.w() * ratioB);
|
||||||
qm.x() = (qa.x() * ratioA + qb.x() * ratioB);
|
qm.x() = (qa.x() * ratioA + qb.x() * ratioB);
|
||||||
qm.y() = (qa.y() * ratioA + qb.y() * ratioB);
|
qm.y() = (qa.y() * ratioA + qb.y() * ratioB);
|
||||||
qm.z() = (qa.z() * ratioA + qb.z() * ratioB);
|
qm.z() = (qa.z() * ratioA + qb.z() * ratioB);
|
||||||
|
|
||||||
return qm;
|
return qm;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -27,22 +27,21 @@
|
||||||
|
|
||||||
namespace pw {
|
namespace pw {
|
||||||
|
|
||||||
|
|
||||||
template <typename T, std::size_t N>
|
template <typename T, std::size_t N>
|
||||||
struct vector_ : matrix_<T, N, 1,false>
|
struct vector_ : matrix_<T, N, 1,false>
|
||||||
{
|
{
|
||||||
typedef matrix_<T, N, 1, false> Base;
|
typedef matrix_<T, N, 1, false> Base;
|
||||||
|
|
||||||
using typename Base::value_type;
|
using typename Base::value_type;
|
||||||
using Base::Base;
|
using Base::Base;
|
||||||
|
|
||||||
static T dot(const vector_ &a,const vector_ &b) {
|
static T dot(const Base &a,const Base &b) {
|
||||||
vector_ r; for (size_t i = 0;i < N;i++) r[i] = a[i] * b[i];
|
vector_ r; for (size_t i = 0;i < N;i++) r[i] = a[i] * b[i];
|
||||||
return std::accumulate(std::begin(r), std::end(r), T(0));
|
return std::accumulate(std::begin(r), std::end(r), T(0));
|
||||||
}
|
}
|
||||||
|
|
||||||
static T angle_between(const vector_ &a,const vector_ &b) {
|
static T angle_between(const Base &a,const Base &b) {
|
||||||
return std::acos( dot( a.normalized(), b.normalized() ) );
|
return std::acos( dot( a.normalized(), b.normalized() ) );
|
||||||
}
|
}
|
||||||
|
|
||||||
static const vector_ lerp(const vector_ &a,const vector_ &b,const T& t) {
|
static const vector_ lerp(const vector_ &a,const vector_ &b,const T& t) {
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue