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