cleaned up the quaternion implementation prior to making pixwerx rely on it

src/core/include/pw/core/math.hpp

@@ -8,7 +8,19 @@ namespace pw {
 const static double __PW_PI = 3.1415926535897932384626433832795028841971693993751058209;
 
 template <typename T>
-inline const T Pi() { return static_cast<T>(__PW_PI); }
+inline const T pi() { return static_cast<T>(__PW_PI); }
+
+
+
+template <typename T>
+inline static double rad_to_deg(const T& angle_in_radian) {
+    return static_cast<T>(angle_in_radian * T(180.) / pi<T>());
+}
+
+template <typename T>
+inline static double deg_to_rad(const T& angle_in_degree) {
+    return static_cast<T>(angle_in_degree * pi<T>() / T(180.));
+}
 
 }
 

src/core/include/pw/core/matrix.hpp

@@ -42,6 +42,7 @@ class matrix : public matrixbase<T> {
 public:
 
     using typename matrixbase<T>::value_type;
+    using typename matrixbase<T>::size_type;
 
     matrix();
 
@@ -59,6 +60,12 @@ public:
     inline matrix& operator += (const matrix& other) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) += other.at(i); return *this; }
     inline matrix& operator -= (const matrix& other) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) -= other.at(i); return *this; }
 
+    inline const matrix normalized() const {
+        const T one_over_n = T(1) / this->norm();
+        return *this * one_over_n;
+    }
+
+
     inline const matrix
     get_inverse() const
     {
@@ -281,7 +288,7 @@ T matrix<R,C,T>::squared_norm() const
 
     for (unsigned int r = 0; r < R; ++r)
         for (unsigned int c = 0; c < C; ++c)
-            res += ((*this)(r,c) * (*this)(r,c));
+            res += ((*this).at(r,c) * (*this).at(r,c));
     return res;
 }
 
@@ -316,24 +323,22 @@ class matrix44 : public matrix<4,4,T>
 {
 public:
 
-    matrix44()
-    {
-    }
+    using matrix<4,4,T>::matrix;
+
 
     matrix44(const matrix<4,4,T>& i)
     {
         *this = i;
     }
 
-    matrix44&
-    operator = (const matrix<4,4,T>& rhs)
-    {
+    matrix44& operator = (const matrix<4,4,T>& rhs) {
         if (this != &rhs){
             this->at(0,0) = rhs.at(0,0);this->at(0,1) = rhs.at(0,1);this->at(0,2) = rhs.at(0,2);this->at(0,3) = rhs.at(0,3);
             this->at(1,0) = rhs.at(1,0);this->at(1,1) = rhs.at(1,1);this->at(1,2) = rhs.at(1,2);this->at(1,3) = rhs.at(1,3);
             this->at(2,0) = rhs.at(2,0);this->at(2,1) = rhs.at(2,1);this->at(2,2) = rhs.at(2,2);this->at(2,3) = rhs.at(2,3);
             this->at(3,0) = rhs.at(3,0);this->at(3,1) = rhs.at(3,1);this->at(3,2) = rhs.at(3,2);this->at(3,3) = rhs.at(3,3);
         }
+
         return *this;
     }
 

src/core/include/pw/core/matrixbase.hpp

@@ -32,20 +32,12 @@ namespace pw {
  * \brief base class for all matrix and vector operations
  */
 template <typename T> class matrixbase {
-protected:
-
-    T* _data;
-    int _data_offset;
-
-    int _cols;
-    int _rows;
-
-    int _row_stride;
-    int _col_stride;
+public:
 
     typedef T value_type;
+    typedef int offset_type;
+    typedef unsigned int size_type;
 
-public:
 
 	//! assignment constructor
     explicit matrixbase(int rows, int cols,T* ptr,bool row_major,int data_offset = 0)
@@ -58,7 +50,7 @@ public:
 	{
 	}
 
-    inline const T get_element(int e) const { return this->at(e); }
+    inline const T& get_element(int e) const { return this->at(e); }
     inline void set_element(int e,const T &v) { this->at(e) = v; }
 
     //! return number of rows
@@ -89,7 +81,10 @@ public:
 
     //! fill data
     inline matrixbase& fill(const T& val) {
-        for (unsigned int i = 0; i < this->cells(); ++i) this->at(i) = val; return *this;
+        for (unsigned int r = 0; r < this->rows(); r++)
+            for (unsigned int c = 0; c < this->cols(); c++)
+                this->at(r,c) = val;
+        return *this;
     }
 
     //! set identity
@@ -122,6 +117,18 @@ public:
 
     //! get the offset for data in the matrix
     inline int get_data_offset(int r,int c) const { return (r * _row_stride + c * _col_stride) + _data_offset; }
+
+protected:
+
+    T* _data;
+    int _data_offset;
+
+    int _cols;
+    int _rows;
+
+    int _row_stride;
+    int _col_stride;
+
 };
 
 }

src/core/include/pw/core/quaternion.hpp

@@ -37,24 +37,23 @@ namespace pw {
 template <typename T>
 class quaternion {
 
-    static constexpr T _sqrt90 = noexcept(std::sqrt(0.5));
-
-protected:
-
-    vector4<T> _q;
+    static const T _sqrt90;
 
 public:
 
+    typedef vector4<T> coefficient_type;
     typedef T value_type;
 
     quaternion() { *this = identity(); }
 
-    quaternion(const T& x,const T& y,const T& z,const T& w) {
-        this->set(x,y,z,w);
-    }
+    quaternion(const T& x,const T& y,const T& z,const T& w)
+        : _q(coefficient_type(x,y,z,w)) {}
 
-    inline
-    void set(const T& x,const T& y,const T& z,const T& w) {
+    quaternion(const coefficient_type& vec) { *this = vec; }
+
+    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);
     }
 
@@ -63,21 +62,69 @@ public:
     inline void set_z(const T& v) { z() = v; }
     inline void set_w(const T& v) { w() = v; }
 
-
-
-    inline const vector4<T> 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.y(); }
+    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.x(); }
+    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()
+                    );
+    }
+
+    //! 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(this->_q - rhs._q);
+    }
+
+    //! squared norm
+    inline const T squared_norm() const { return _q.squared_norm(); }
+
+    //! norm
+    inline const T norm() const { return _q.norm(); }
+
+    //! dot product
+    inline const T dot(const quaternion& other) const { return _q.dot(other._q); }
+
+    //! conjugate
+    inline quaternion conjugate() const { return quaternion(-x(),-y(),-z(),w()); }
+
+    //! compute inverse
+    inline quaternion inverse() const {
+        const T one_over_squared_norm = T(1) / squared_norm();
+        return conjugate() * one_over_squared_norm;
+    }
+
+    //! compute normalized
+    inline quaternion normalized() const {
+        return quaternion(_q.normalized());
+    }
+
+    inline void normalize() { *this = this->normalized(); }
+
     //! conversion from a matrix
     inline static const quaternion from_matrix(const matrix<4,4,T> &m);
 
@@ -85,15 +132,15 @@ public:
     const matrix<4,4,T> to_matrix() const;
 
     //! return identiy quaternion
-    static quaternion<T> identity();
+    static const quaternion<T> identity();
 
-    static quaternion<T> rotate_180_degree_around_x(); ///< rotate 180 degree around X axis
-    static quaternion<T> rotate_180_degree_around_y(); ///< rotate 180 degree around Y axis
-    static 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 quaternion<T> rotate_90_degree_around_x(bool negative = false);
-    static quaternion<T> rotate_90_degree_around_y(bool negative = false);
-    static 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) {
@@ -110,8 +157,20 @@ public:
                              );
 
     }
+
+    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;
 };
 
+template <typename T>
+const T quaternion<T>::_sqrt90 = std::sqrt(0.5);
+
 
 template <typename T>
 const quaternion<T> quaternion<T>::from_matrix(const matrix<4,4,T> &m) {
@@ -161,46 +220,94 @@ const matrix<4,4,T> quaternion<T>::to_matrix() const {
 }
 
 template <typename T>
-quaternion<T> quaternion<T>::identity()
+const quaternion<T> quaternion<T>::identity()
 {
     return quaternion<T>(0,0,0,1);
 }
 
 template <typename T>
-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);
 }
 template <typename T>
-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);
 }
 
 template <typename T>
-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);
 }
 
 template <typename T>
-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>
-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);
 }
 
 template <typename T>
-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);
 }
 
+template <typename T>
+const quaternion<T> quaternion<T>::lerp(const quaternion<T>& qa,const quaternion<T>& qb,const T& t) {
+    return quaternion<T>(qa + (qb - qa) * t);
+}
+
+template <typename T>
+const quaternion<T> quaternion<T>::normalized_lerp(const quaternion<T>& qa,const quaternion<T>& qb,const T& t) {
+    return quaternion<T>::lerp(qa,qb,t).normalized();
+}
+
+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;
+
+    // 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);
+
+    return qm;
+}
 
 //
 //

src/core/include/pw/core/vector.hpp

@@ -33,6 +33,10 @@ namespace pw {
 template <unsigned int components,typename T>
 class vector : public matrix<components,1,T> {
 public:
+
+    using typename matrix<components,1,T>::value_type;
+    using matrix<components,1,T>::operator = ;
+
     vector() : matrix<components,1,T>() {}
 
     vector(const vector& other) : matrix<components,1,T>(other) {}
@@ -43,22 +47,20 @@ public:
     const T& operator()(unsigned int c) const { return matrixbase<T>::at(c); }
 
 
-    T dot(const vector<components,T>& other) const
-    {
+    const T dot(const vector<components,T>& other) const {
         T res = 0;
         for (unsigned int i = 0; i < components; i++) res += (*this)(i) * other(i);
         return res;
     }
 
-#if OLD_TACITPIXEL
-
-    T getAngle(const vector<components,T>& other) const
-    {
+    const T angle_to(const vector<components,T>& other) const {
+        using std::acos;
         vector<components,T> nself(*this); vector<components,T> nothr = other;
         nself.normalize(); nothr.normalize();
         return acos( nothr.dot(nself) );
     }
-#endif
+
+
 };
 
 
@@ -72,7 +74,7 @@ public:
 
     vector3() : vector<3,T>() {}
 
-    vector3(const vector<3,T>& other) : matrix<3,1,T>(other) {}
+    vector3(const vector<3,T>& other) : vector<3, T> (other) {}
 
     vector3(T c1, T c2, T c3) { this->set(c1,c2,c3); }
 
@@ -145,11 +147,7 @@ public:
 template <typename T> class vector4 : public vector<4,T> {
 public:
 
-    vector4() {}
-
-    vector4(const vector<3,T>& rv,T pad = T(0)) {
-        this->set(rv(0),rv(1),rv(2),pad);
-    }
+    using vector<4,T>::vector;
 
     vector4(const T& v1,const T& v2,const T& v3,const T& v4) {
         this->set(v1,v2,v3,v4);
@@ -162,12 +160,9 @@ public:
         (*this)(3) = v4;
     }
 
-
     inline const vector3<T> xyz() const { return vector3<T>(x(),y(),z()); }
     inline const vector2<T> xy() const { return vector2<T>(x(),y()); }
 
-
-
     const T& x() const { return (*this)(0); }
     T& x() { return (*this)(0); }
 

src/core/tests/pwcore_test_quaternion.cpp

@@ -5,11 +5,26 @@
 
 int main(int argc,char **argv) {
 
-    pw::quaternion<float> qf = pw::quaternionf::rotate_180_degree_around_x();
+    pw::quaternion<float> qf = pw::quaternionf::rotate_90_degree_around_x();
 
     std::cout << "qf = " << pw::serialize::matrix(qf.as_vector()) << std::endl;
     std::cout << "qf.matrix() = " << pw::serialize::matrix(qf.to_matrix()) << std::endl;
 
+    std::cout << "qf.squared_norm() = " << qf.squared_norm() << std::endl;
+    std::cout << "qf.dot(qf) = " << qf.dot(qf) << std::endl;
+    std::cout << "qf.conjugate() = " << pw::serialize::matrix(qf.conjugate().as_vector()) << std::endl;
+
+    pw::quaternionf qi = qf.inverse();
+    std::cout << "qf.inverse() (qi) = " << pw::serialize::matrix(qi.as_vector()) << std::endl;
+
+
+    pw::quaternionf qmid = pw::quaternionf::normalized_lerp(qi,qf,0.5f);
+
+    std::cout << "qmid.dot() (half between qf and qi) = " << pw::rad_to_deg(std::acos(qmid.dot(qf))) << std::endl;
+
+
+
+
 
     return 0;
 }

src/core/tests/pwcore_test_vector.cpp

@@ -24,6 +24,8 @@ int main(int argc,char **argv) {
 
     std::cout << "v3 = " << pw::serialize::matrix(v3) << std::endl;
 
+    std::cout << "v3.normalized() = " << pw::serialize::matrix(v3.normalized()) << std::endl;
+
 
     return 0;
 }

src/scene/include/pw/scene/component.hpp

@@ -5,9 +5,12 @@
 
 #include <string>
 #include <vector>
+#include <memory>
 
 namespace pw {
 
+class node;
+
 /**
  * @brief components represent attributes of the scene nodes
  */
@@ -19,8 +22,15 @@ public:
 
     //! only very few components can be attached multiple times
     virtual bool singular() const { return true; }
+
+    explicit component(node* n = nullptr);
+    component(const component& other);
+    virtual ~component();
+
 protected:
 
+    node* _node;
+
     std::string _name;
 };
 

src/scene/include/pw/scene/node.hpp

@@ -19,6 +19,8 @@ public:
     typedef std::vector<ref> ref_array;
     typedef std::vector<node*> ptr_array;
 
+    friend class component;
+
     //! standard c'tor
     node(const std::string& name = "");
 
@@ -51,8 +53,13 @@ public:
 
     virtual ~node();
 
+    const component::array& components() const;
+
 protected:
 
+    void register_component(component* c);
+    void unregister_component(component* c);
+
     ref_array _children;
     ptr_array _parents;
 

src/scene/include/pw/scene/transform.hpp

@@ -0,0 +1,27 @@
+#ifndef PW_SCENE_TRANSFORM_HPP
+#define PW_SCENE_TRANSFORM_HPP
+
+#include <pw/core/matrix.hpp>
+#include <pw/scene/component.hpp>
+
+namespace pw {
+
+class transform : public component {
+public:
+    using component::component;
+
+    const matrix44d& local() const;
+    void set_local(const matrix44d &local);
+
+protected:
+
+    matrix44d _local;
+    matrix44d _global;
+
+    std::vector<ref> _path;
+
+};
+
+}
+
+#endif

src/scene/src/CMakeLists.txt

@@ -2,12 +2,14 @@
 set(hdrs
     ../include/pw/scene/component.hpp
     ../include/pw/scene/node.hpp
+	../include/pw/scene/transform.hpp
     )
 
 set(srcs
     node.cpp
     nodepath.cpp
     component.cpp
+	transform.cpp
     )
 
 add_library(pwscene

src/scene/src/component.cpp

@@ -3,24 +3,39 @@
 
 #include <pw/core/vector.hpp>
 #include <pw/core/quaternion.hpp>
+#include "pw/scene/node.hpp"
 
 #include <string>
 
+#include <iostream>
+
 namespace pw {
 
 
-class transform : public component {
 
-    matrix44d _local;
-    matrix44d _global;
+component::component(node *n)
+    : _node(n)
+{
+    if (_node != nullptr) _node->register_component(this);
+}
 
-};
+component::component(const component &other)
+    : _node(other._node)
+{
+}
 
-class trs : public transform {
+component::~component() {
+    if (_node != nullptr) _node->unregister_component(this);
+}
+
+
+
+//class trs : public transform {
+
+//    vector3d _position;
+//    vector3d _scale;
+//    quaterniond _rotation;
+//};
 
-    vector3d _position;
-    vector3d _scale;
-    quaterniond _rotation;
-};
 
 }

src/scene/src/node.cpp

@@ -34,6 +34,24 @@ node::~node()
     _parents.clear();
 }
 
+void node::register_component(component *c) {
+    _components.push_back(std::shared_ptr<component>(c));
+}
+
+void node::unregister_component(component *c)
+{
+//    component::array::iterator it = _components.end();
+
+//    while ((it = std::find(_components.begin(),_components.end(),std::shared_ptr<component>(c))) != _components.end()) {
+//        _components.erase(it);
+//    }
+}
+
+const component::array& node::components() const
+{
+    return _components;
+}
+
 
 
 }

src/scene/src/nodepath.cpp

@@ -0,0 +1,6 @@
+namespace pw {
+
+
+
+
+}

src/scene/src/transform.cpp

@@ -0,0 +1,16 @@
+#include "pw/scene/transform.hpp"
+
+namespace pw {
+
+const matrix44d& transform::local() const
+{
+    return _local;
+}
+
+void transform::set_local(const matrix44d &local)
+{
+    // TODO need to rebuild the transforms: both -> global down and global up
+    _local = local;
+}
+
+}

src/scene/tests/pwscene_test_node.cpp

@@ -1,6 +1,7 @@
 #include <pw/core/vector.hpp>
 #include <pw/core/serialize.hpp>
 #include <pw/scene/node.hpp>
+#include <pw/scene/transform.hpp>
 
 #include <iostream>
 
@@ -24,6 +25,13 @@ int main(int argc,char **argv) {
     std::cout << "s root: " << n->children()[0]->is_root() << std::endl;
 
 
+    // thats the only and most ugly way to register a component
+    new component(n.get());
+
+    new transform(n.get());
+
+    std::cout << "n components: " << n->components().size() << std::endl;
+
 
     return 0;
 }

src/scripting/src/script.cpp

@@ -44,7 +44,7 @@ void lua_state::load_modules() {
 
     typedef double Scalar;
 
-    _namespace.set("pi",pw::Pi<Scalar>());
+    _namespace.set("pi",pw::pi<Scalar>());
 
     using namespace pw;