vector.h Example File
demos/boxes/vector.h

 /****************************************************************************
 **
 ** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
 ** All rights reserved.
 ** Contact: Nokia Corporation (qt-info@nokia.com)
 **
 ** This file is part of the demonstration applications of the Qt Toolkit.
 **
 ** $QT_BEGIN_LICENSE:LGPL$
 ** Commercial Usage
 ** Licensees holding valid Qt Commercial licenses may use this file in
 ** accordance with the Qt Commercial License Agreement provided with the
 ** Software or, alternatively, in accordance with the terms contained in
 ** a written agreement between you and Nokia.
 **
 ** GNU Lesser General Public License Usage
 ** Alternatively, this file may be used under the terms of the GNU Lesser
 ** General Public License version 2.1 as published by the Free Software
 ** Foundation and appearing in the file LICENSE.LGPL included in the
 ** packaging of this file.  Please review the following information to
 ** ensure the GNU Lesser General Public License version 2.1 requirements
 ** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
 **
 ** In addition, as a special exception, Nokia gives you certain additional
 ** rights.  These rights are described in the Nokia Qt LGPL Exception
 ** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
 **
 ** GNU General Public License Usage
 ** Alternatively, this file may be used under the terms of the GNU
 ** General Public License version 3.0 as published by the Free Software
 ** Foundation and appearing in the file LICENSE.GPL included in the
 ** packaging of this file.  Please review the following information to
 ** ensure the GNU General Public License version 3.0 requirements will be
 ** met: http://www.gnu.org/copyleft/gpl.html.
 **
 ** If you have questions regarding the use of this file, please contact
 ** Nokia at qt-info@nokia.com.
 ** $QT_END_LICENSE$
 **
 ****************************************************************************/

 #ifndef VECTOR_H
 #define VECTOR_H

 #include <cassert>
 #include <cmath>
 #include <iostream>

 namespace gfx
 {

 template<class T, int n>
 struct Vector
 {
     // Keep the Vector struct a plain old data (POD) struct by avoiding constructors

     static Vector vector(T x)
     {
         Vector result;
         for (int i = 0; i < n; ++i)
             result.v[i] = x;
         return result;
     }

     // Use only for 2D vectors
     static Vector vector(T x, T y)
     {
         assert(n == 2);
         Vector result;
         result.v[0] = x;
         result.v[1] = y;
         return result;
     }

     // Use only for 3D vectors
     static Vector vector(T x, T y, T z)
     {
         assert(n == 3);
         Vector result;
         result.v[0] = x;
         result.v[1] = y;
         result.v[2] = z;
         return result;
     }

     // Use only for 4D vectors
     static Vector vector(T x, T y, T z, T w)
     {
         assert(n == 4);
         Vector result;
         result.v[0] = x;
         result.v[1] = y;
         result.v[2] = z;
         result.v[3] = w;
         return result;
     }

     // Pass 'n' arguments to this function.
     static Vector vector(T *v)
     {
         Vector result;
         for (int i = 0; i < n; ++i)
             result.v[i] = v[i];
         return result;
     }

     T &operator [] (int i) {return v[i];}
     T operator [] (int i) const {return v[i];}

 #define VECTOR_BINARY_OP(op, arg, rhs)      \
     Vector operator op (arg) const          \
     {                                       \
         Vector result;                      \
         for (int i = 0; i < n; ++i)         \
             result.v[i] = v[i] op rhs;      \
         return result;                      \
     }

     VECTOR_BINARY_OP(+, const Vector &u, u.v[i])
     VECTOR_BINARY_OP(-, const Vector &u, u.v[i])
     VECTOR_BINARY_OP(*, const Vector &u, u.v[i])
     VECTOR_BINARY_OP(/, const Vector &u, u.v[i])
     VECTOR_BINARY_OP(+, T s, s)
     VECTOR_BINARY_OP(-, T s, s)
     VECTOR_BINARY_OP(*, T s, s)
     VECTOR_BINARY_OP(/, T s, s)
 #undef VECTOR_BINARY_OP

     Vector operator - () const
     {
         Vector result;
         for (int i = 0; i < n; ++i)
             result.v[i] = -v[i];
         return result;
     }

 #define VECTOR_ASSIGN_OP(op, arg, rhs)      \
     Vector &operator op (arg)               \
     {                                       \
         for (int i = 0; i < n; ++i)         \
             v[i] op rhs;                    \
         return *this;                       \
     }

     VECTOR_ASSIGN_OP(+=, const Vector &u, u.v[i])
     VECTOR_ASSIGN_OP(-=, const Vector &u, u.v[i])
     VECTOR_ASSIGN_OP(=, T s, s)
     VECTOR_ASSIGN_OP(*=, T s, s)
     VECTOR_ASSIGN_OP(/=, T s, s)
 #undef VECTOR_ASSIGN_OP

     static T dot(const Vector &u, const Vector &v)
     {
         T sum(0);
         for (int i = 0; i < n; ++i)
             sum += u.v[i] * v.v[i];
         return sum;
     }

     static Vector cross(const Vector &u, const Vector &v)
     {
         assert(n == 3);
         return vector(u.v[1] * v.v[2] - u.v[2] * v.v[1],
                       u.v[2] * v.v[0] - u.v[0] * v.v[2],
                       u.v[0] * v.v[1] - u.v[1] * v.v[0]);
     }

     T sqrNorm() const
     {
         return dot(*this, *this);
     }

     // requires floating point type T
     void normalize()
     {
         T s = sqrNorm();
         if (s != 0)
             *this /= sqrt(s);
     }

     // requires floating point type T
     Vector normalized() const
     {
         T s = sqrNorm();
         if (s == 0)
             return *this;
         return *this / sqrt(s);
     }

     T *bits() {return v;}
     const T *bits() const {return v;}

     T v[n];
 };

 #define SCALAR_VECTOR_BINARY_OP(op)                     \
 template<class T, int n>                                \
 Vector<T, n> operator op (T s, const Vector<T, n>& u)   \
 {                                                       \
     Vector<T, n> result;                                \
     for (int i = 0; i < n; ++i)                         \
         result[i] = s op u[i];                          \
     return result;                                      \
 }

 SCALAR_VECTOR_BINARY_OP(+)
 SCALAR_VECTOR_BINARY_OP(-)
 SCALAR_VECTOR_BINARY_OP(*)
 SCALAR_VECTOR_BINARY_OP(/)
 #undef SCALAR_VECTOR_BINARY_OP

 template<class T, int n>
 std::ostream &operator << (std::ostream &os, const Vector<T, n> &v)
 {
     assert(n > 0);
     os << "[" << v[0];
     for (int i = 1; i < n; ++i)
         os << ", " << v[i];
     os << "]";
     return os;
 }

 typedef Vector<float, 2> Vector2f;
 typedef Vector<float, 3> Vector3f;
 typedef Vector<float, 4> Vector4f;

 template<class T, int rows, int cols>
 struct Matrix
 {
     // Keep the Matrix struct a plain old data (POD) struct by avoiding constructors

     static Matrix matrix(T x)
     {
         Matrix result;
         for (int i = 0; i < rows; ++i) {
             for (int j = 0; j < cols; ++j)
                 result.v[i][j] = x;
         }
         return result;
     }

     static Matrix matrix(T *m)
     {
         Matrix result;
         for (int i = 0; i < rows; ++i) {
             for (int j = 0; j < cols; ++j) {
                 result.v[i][j] = *m;
                 ++m;
             }
         }
         return result;
     }

     T &operator () (int i, int j) {return v[i][j];}
     T operator () (int i, int j) const {return v[i][j];}
     Vector<T, cols> &operator [] (int i) {return v[i];}
     const Vector<T, cols> &operator [] (int i) const {return v[i];}

     // TODO: operators, methods

     Vector<T, rows> operator * (const Vector<T, cols> &u) const
     {
         Vector<T, rows> result;
         for (int i = 0; i < rows; ++i)
             result[i] = Vector<T, cols>::dot(v[i], u);
         return result;
     }

     template<int k>
     Matrix<T, rows, k> operator * (const Matrix<T, cols, k> &m)
     {
         Matrix<T, rows, k> result;
         for (int i = 0; i < rows; ++i)
             result[i] = v[i] * m;
         return result;
     }

     T* bits() {return reinterpret_cast<T *>(this);}
     const T* bits() const {return reinterpret_cast<const T *>(this);}

     // Simple Gauss elimination.
     // TODO: Optimize and improve stability.
     Matrix inverse(bool *ok = 0) const
     {
         assert(rows == cols);
         Matrix rhs = identity();
         Matrix lhs(*this);
         T temp;
         // Down
         for (int i = 0; i < rows; ++i) {
             // Pivoting
             int pivot = i;
             for (int j = i; j < rows; ++j) {
                 if (qAbs(lhs(j, i)) > lhs(pivot, i))
                     pivot = j;
             }
                         // TODO: fuzzy compare.
             if (lhs(pivot, i) == T(0)) {
                 if (ok)
                     *ok = false;
                 return rhs;
             }
             if (pivot != i) {
                 for (int j = i; j < cols; ++j) {
                     temp = lhs(pivot, j);
                     lhs(pivot, j) = lhs(i, j);
                     lhs(i, j) = temp;
                 }
                 for (int j = 0; j < cols; ++j) {
                     temp = rhs(pivot, j);
                     rhs(pivot, j) = rhs(i, j);
                     rhs(i, j) = temp;
                 }
             }

             // Normalize i-th row
             rhs[i] /= lhs(i, i);
             for (int j = cols - 1; j > i; --j)
                 lhs(i, j) /= lhs(i, i);

                         // Eliminate non-zeros in i-th column below the i-th row.
             for (int j = i + 1; j < rows; ++j) {
                 rhs[j] -= lhs(j, i) * rhs[i];
                 for (int k = i + 1; k < cols; ++k)
                     lhs(j, k) -= lhs(j, i) * lhs(i, k);
             }
         }
         // Up
         for (int i = rows - 1; i > 0; --i) {
             for (int j = i - 1; j >= 0; --j)
                 rhs[j] -= lhs(j, i) * rhs[i];
         }
         if (ok)
             *ok = true;
         return rhs;
     }

     Matrix<T, cols, rows> transpose() const
     {
         Matrix<T, cols, rows> result;
         for (int i = 0; i < rows; ++i) {
             for (int j = 0; j < cols; ++j)
                 result.v[j][i] = v[i][j];
         }
         return result;
     }

     static Matrix identity()
     {
         Matrix result = matrix(T(0));
         for (int i = 0; i < rows && i < cols; ++i)
             result.v[i][i] = T(1);
         return result;
     }

     Vector<T, cols> v[rows];
 };

 template<class T, int rows, int cols>
 Vector<T, cols> operator * (const Vector<T, rows> &u, const Matrix<T, rows, cols> &m)
 {
     Vector<T, cols> result = Vector<T, cols>::vector(T(0));
     for (int i = 0; i < rows; ++i)
         result += m[i] * u[i];
     return result;
 }

 template<class T, int rows, int cols>
 std::ostream &operator << (std::ostream &os, const Matrix<T, rows, cols> &m)
 {
     assert(rows > 0);
     os << "[" << m[0];
     for (int i = 1; i < rows; ++i)
         os << ", " << m[i];
     os << "]";
     return os;
 }

 typedef Matrix<float, 2, 2> Matrix2x2f;
 typedef Matrix<float, 3, 3> Matrix3x3f;
 typedef Matrix<float, 4, 4> Matrix4x4f;

 template<class T>
 struct Quaternion
 {
     // Keep the Quaternion struct a plain old data (POD) struct by avoiding constructors

     static Quaternion quaternion(T s, T x, T y, T z)
     {
         Quaternion result;
         result.scalar = s;
         result.vector[0] = x;
         result.vector[1] = y;
         result.vector[2] = z;
         return result;
     }

     static Quaternion quaternion(T s, const Vector<T, 3> &v)
     {
         Quaternion result;
         result.scalar = s;
         result.vector = v;
         return result;
     }

     static Quaternion identity()
     {
         return quaternion(T(1), T(0), T(0), T(0));
     }

     // assumes that all the elements are packed tightly
     T& operator [] (int i) {return reinterpret_cast<T *>(this)[i];}
     T operator [] (int i) const {return reinterpret_cast<const T *>(this)[i];}

 #define QUATERNION_BINARY_OP(op, arg, rhs)  \
     Quaternion operator op (arg) const      \
     {                                       \
         Quaternion result;                  \
         for (int i = 0; i < 4; ++i)         \
             result[i] = (*this)[i] op rhs;  \
         return result;                      \
     }

     QUATERNION_BINARY_OP(+, const Quaternion &q, q[i])
     QUATERNION_BINARY_OP(-, const Quaternion &q, q[i])
     QUATERNION_BINARY_OP(*, T s, s)
     QUATERNION_BINARY_OP(/, T s, s)
 #undef QUATERNION_BINARY_OP

     Quaternion operator - () const
     {
         return Quaternion(-scalar, -vector);
     }

     Quaternion operator * (const Quaternion &q) const
     {
         Quaternion result;
         result.scalar = scalar * q.scalar - Vector<T, 3>::dot(vector, q.vector);
         result.vector = scalar * q.vector + vector * q.scalar + Vector<T, 3>::cross(vector, q.vector);
         return result;
     }

     Quaternion operator * (const Vector<T, 3> &v) const
     {
         Quaternion result;
         result.scalar = -Vector<T, 3>::dot(vector, v);
         result.vector = scalar * v + Vector<T, 3>::cross(vector, v);
         return result;
     }

     friend Quaternion operator * (const Vector<T, 3> &v, const Quaternion &q)
     {
         Quaternion result;
         result.scalar = -Vector<T, 3>::dot(v, q.vector);
         result.vector = v * q.scalar + Vector<T, 3>::cross(v, q.vector);
         return result;
     }

 #define QUATERNION_ASSIGN_OP(op, arg, rhs)  \
     Quaternion &operator op (arg)           \
     {                                       \
         for (int i = 0; i < 4; ++i)         \
             (*this)[i] op rhs;              \
         return *this;                       \
     }

     QUATERNION_ASSIGN_OP(+=, const Quaternion &q, q[i])
     QUATERNION_ASSIGN_OP(-=, const Quaternion &q, q[i])
     QUATERNION_ASSIGN_OP(=, T s, s)
     QUATERNION_ASSIGN_OP(*=, T s, s)
     QUATERNION_ASSIGN_OP(/=, T s, s)
 #undef QUATERNION_ASSIGN_OP

     Quaternion& operator *= (const Quaternion &q)
     {
         Quaternion result;
         result.scalar = scalar * q.scalar - Vector<T, 3>::dot(vector, q.vector);
         result.vector = scalar * q.vector + vector * q.scalar + Vector<T, 3>::cross(vector, q.vector);
         return (*this = result);
     }

     Quaternion& operator *= (const Vector<T, 3> &v)
     {
         Quaternion result;
         result.scalar = -Vector<T, 3>::dot(vector, v);
         result.vector = scalar * v + Vector<T, 3>::cross(vector, v);
         return (*this = result);
     }

     Quaternion conjugate() const
     {
         return quaternion(scalar, -vector);
     }

     T sqrNorm() const
     {
         return scalar * scalar + vector.sqrNorm();
     }

     Quaternion inverse() const
     {
         return conjugate() / sqrNorm();
     }

     // requires floating point type T
     Quaternion normalized() const
     {
         T s = sqrNorm();
         if (s == 0)
             return *this;
         return *this / sqrt(s);
     }

     void matrix(Matrix<T, 3, 3>& m) const
     {
         T bb = vector[0] * vector[0];
         T cc = vector[1] * vector[1];
         T dd = vector[2] * vector[2];
         T diag = scalar * scalar - bb - cc - dd;
         T ab = scalar * vector[0];
         T ac = scalar * vector[1];
         T ad = scalar * vector[2];
         T bc = vector[0] * vector[1];
         T cd = vector[1] * vector[2];
         T bd = vector[2] * vector[0];
         m(0, 0) = diag + 2 * bb;
         m(0, 1) = 2 * (bc - ad);
         m(0, 2) = 2 * (ac + bd);
         m(1, 0) = 2 * (ad + bc);
         m(1, 1) = diag + 2 * cc;
         m(1, 2) = 2 * (cd - ab);
         m(2, 0) = 2 * (bd - ac);
         m(2, 1) = 2 * (ab + cd);
         m(2, 2) = diag + 2 * dd;
     }

     void matrix(Matrix<T, 4, 4>& m) const
     {
         T bb = vector[0] * vector[0];
         T cc = vector[1] * vector[1];
         T dd = vector[2] * vector[2];
         T diag = scalar * scalar - bb - cc - dd;
         T ab = scalar * vector[0];
         T ac = scalar * vector[1];
         T ad = scalar * vector[2];
         T bc = vector[0] * vector[1];
         T cd = vector[1] * vector[2];
         T bd = vector[2] * vector[0];
         m(0, 0) = diag + 2 * bb;
         m(0, 1) = 2 * (bc - ad);
         m(0, 2) = 2 * (ac + bd);
         m(0, 3) = 0;
         m(1, 0) = 2 * (ad + bc);
         m(1, 1) = diag + 2 * cc;
         m(1, 2) = 2 * (cd - ab);
         m(1, 3) = 0;
         m(2, 0) = 2 * (bd - ac);
         m(2, 1) = 2 * (ab + cd);
         m(2, 2) = diag + 2 * dd;
         m(2, 3) = 0;
         m(3, 0) = 0;
         m(3, 1) = 0;
         m(3, 2) = 0;
         m(3, 3) = 1;
     }

     // assumes that 'this' is normalized
     Vector<T, 3> transform(const Vector<T, 3> &v) const
     {
         Matrix<T, 3, 3> m;
         matrix(m);
         return v * m;
     }

     // assumes that all the elements are packed tightly
     T* bits() {return reinterpret_cast<T *>(this);}
     const T* bits() const {return reinterpret_cast<const T *>(this);}

     // requires floating point type T
     static Quaternion rotation(T angle, const Vector<T, 3> &unitAxis)
     {
         T s = sin(angle / 2);
         T c = cos(angle / 2);
         return quaternion(c, unitAxis * s);
     }

     T scalar;
     Vector<T, 3> vector;
 };

 template<class T>
 Quaternion<T> operator * (T s, const Quaternion<T>& q)
 {
     return Quaternion<T>::quaternion(s * q.scalar, s * q.vector);
 }

 typedef Quaternion<float> Quaternionf;

 } // end namespace gfx

 #endif