//---------------------------------------------------------------------------
#ifndef LinearAlgebra_templCodeH
#define LinearAlgebra_templCodeH
//---------------------------------------------------------------------------
//#include "GlobalDefinitions.h"
//---------------------------------------------------------------------------
#include "LinearAlgebra.h"
//---------------------------------------------------------------------------
#include <math.h>
#include <string>
//---------------------------------------------------------------------------

   using namespace std;



// ---------------------------------------------------------------------------
//
//                                 StaticVector
//
// ---------------------------------------------------------------------------



template <class FloatType, unsigned dim>
inline FloatType& StaticVector<FloatType, dim>::operator[](const unsigned &index) {
   #ifdef RANGE_CHECK
   if (index >= dim) {
      throw ERangeCheck("StaticVector::operator[] - index exceeds bounds");
   }
   #endif
   return v[index];
}

template <class FloatType, unsigned dim>
inline const FloatType& StaticVector<FloatType, dim>::operator[](const unsigned &index) const{
   #ifdef RANGE_CHECK
   if (index >= dim) {
      throw ERangeCheck("StaticVector::operator[] - index exceeds bounds");
   }
   #endif
   return v[index];
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator+(const StaticVector<FloatType, dim> &op) const {
   StaticVector<FloatType, dim> result;
   for (unsigned i=0; i<dim; i++) {
      result[i] = v[i] + op.v[i];
   }
   return result;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator-(const StaticVector<FloatType, dim> &op) const {
   StaticVector<FloatType, dim> result;
   for (unsigned i=0; i<dim; i++) {
      result[i] = v[i] - op.v[i];
   }
   return result;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator-() const {
   StaticVector<FloatType, dim> result;
   for (unsigned i=0; i<dim; i++) {
      result[i] = -v[i];
   }
   return result;
}

template <class FloatType, unsigned dim>
inline FloatType StaticVector<FloatType, dim>::operator*(const StaticVector<FloatType, dim> op) const {
   FloatType result = 0.0;
   for (unsigned i=0; i<dim; i++) {
      result += v[i] * op.v[i];
   }
   return result;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator*(const FloatType &s) const {
   StaticVector<FloatType, dim> result;
   for (unsigned i=0; i<dim; i++) {
      result[i] = s * v[i];
   }
   return result;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator/(const FloatType &s) const {
   StaticVector<FloatType, dim> result;
   for (unsigned i=0; i<dim; i++) {
      result[i] = v[i] / s;
   }
   return result;
}

template <class FloatType, unsigned dim>
StaticVector<FloatType, dim> StaticVector<FloatType, dim>::crossProduct(const StaticVector<FloatType, dim> &op) const {
   throw PException("StaticVector<FloatType,dim>::crossProduct() is only defined for dim == 3");
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::componentProduct(const StaticVector<FloatType, dim> &op) const {
   StaticVector<FloatType, dim> result;
   for (unsigned i=0; i<dim; i++) {
      result[i] = v[i] * op.v[i];
   }
   return result;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator+=(const StaticVector<FloatType, dim> &op) {
   for (unsigned i=0; i<dim; i++) {
      v[i] += op.v[i];
   }
   return *this;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator-=(const StaticVector<FloatType, dim> &op) {
   for (unsigned i=0; i<dim; i++) {
      v[i] -= op.v[i];
   }
   return *this;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator*=(const FloatType &s) {
   for (unsigned i=0; i<dim; i++) {
      v[i] *= s;
   }
   return *this;
}

template <class FloatType, unsigned dim>
inline StaticVector<FloatType, dim> StaticVector<FloatType, dim>::operator/=(const FloatType &s) {
   for (unsigned i=0; i<dim; i++) {
      v[i] /= s;
   }
   return *this;
}

template <class FloatType, unsigned dim>
inline bool StaticVector<FloatType, dim>::operator==(const StaticVector<FloatType, dim> op) const {
   for (unsigned i=0; i<dim; i++) {
      if (v[i] != op.v[i]) return false;
   }
   return true;
}

template <class FloatType, unsigned dim>
inline bool StaticVector<FloatType, dim>::operator!=(const StaticVector<FloatType, dim> op) const {
   for (unsigned i=0; i<dim; i++) {
      if (v[i] != op.v[i]) return true;
   }
   return false;
}

template <class FloatType, unsigned dim>
inline FloatType* StaticVector<FloatType, dim>::data() {
   return &(v[0]);
}

template <class FloatType, unsigned dim>
inline const FloatType* StaticVector<FloatType, dim>::data() const {
   return &(v[0]);
}

template <class FloatType, unsigned dim>
inline void StaticVector<FloatType, dim>::normalize() {
   FloatType norm = 0.0;
   for (int i=0; i<dim; i++) {
      norm += v[i] * v[i];
   }
   if ( norm > 1e-7 )
      (*this) = (*this) / sqrt( norm );
}

template <class FloatType, unsigned dim>
inline FloatType StaticVector<FloatType, dim>::getSqrNorm() const {
   FloatType norm = (FloatType)0.0;
   for (int i=0; i<dim; i++) {
      norm += v[i] * v[i];
   }
   return norm;
}

template<>
inline StaticVector<float, 3> StaticVector<float, 3>::crossProduct(const StaticVector<float, 3> &op) const {
   StaticVector<float, 3> result;
   result.v[0] = v[1]*op.v[2] - v[2]*op.v[1];
   result.v[1] = v[2]*op.v[0] - v[0]*op.v[2];
   result.v[2] = v[0]*op.v[1] - v[1]*op.v[0];
   return result;
}

template<>
inline StaticVector<double, 3> StaticVector<double, 3>::crossProduct(const StaticVector<double, 3> &op) const {
   StaticVector<double, 3> result;
   result.v[0] = v[1]*op.v[2] - v[2]*op.v[1];
   result.v[1] = v[2]*op.v[0] - v[0]*op.v[2];
   result.v[2] = v[0]*op.v[1] - v[1]*op.v[0];
   return result;
}

template<>
inline StaticVector<int, 3> StaticVector<int, 3>::crossProduct(const StaticVector<int, 3> &op) const {
   StaticVector<int, 3> result;
   result.v[0] = v[1]*op.v[2] - v[2]*op.v[1];
   result.v[1] = v[2]*op.v[0] - v[0]*op.v[2];
   result.v[2] = v[0]*op.v[1] - v[1]*op.v[0];
   return result;
}


// ---- helper functions


template <class FloatTypeDest, class FloatTypeSource, int Dim>
StaticVector<FloatTypeDest, Dim> convertVector(const StaticVector<FloatTypeSource, Dim> &source) {
   StaticVector<FloatTypeDest, Dim> result;
   for (int i=0; i<Dim; i++) {
      result[i] = (FloatTypeDest)source[i];
   }
   return result;
}



// ---------------------------------------------------------------------------
//
//                                 StaticMatrix
//
// ---------------------------------------------------------------------------



template <class FloatType, unsigned columns, unsigned rows>
inline StaticVector<FloatType, rows>& StaticMatrix<FloatType, columns, rows>::operator[](const unsigned &index) {
   #ifdef RANGE_CHECK
   if (index >= columns) {
      throw ERangeCheck("StaticMatrix::operator[] - index exceeds bounds");
   }
   #endif
   return theColumns[index];
}

template <class FloatType, unsigned columns, unsigned rows>
inline const StaticVector<FloatType, rows>& StaticMatrix<FloatType, columns, rows>::operator[](const unsigned &index) const {
   #ifdef RANGE_CHECK
   if (index >= columns) {
      throw ERangeCheck("StaticMatrix::operator[] - index exceeds bounds");
   }
   #endif
   return theColumns[index];
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator+(const StaticMatrix<FloatType, columns, rows> &op) const {
   StaticMatrix<FloatType, columns, rows> result;
   for (unsigned i=0; i<columns; i++) {
      result[i] = theColumns[i] + op.theColumns[i];
   }
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator-(const StaticMatrix<FloatType, columns, rows> &op) const {
   StaticMatrix<FloatType, columns, rows> result;
   for (unsigned i=0; i<columns; i++) {
      result[i] = theColumns[i] - op.theColumns[i];
   }
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator-() const {
   StaticMatrix<FloatType, columns, rows> result;
   for (unsigned i=0; i<columns; i++) {
      result[i] = -theColumns[i];
   }
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator*(const FloatType &s) const {
   StaticMatrix<FloatType, columns, rows> result;
   for (unsigned i=0; i<columns; i++) {
      result[i] = theColumns[i] * s;
   }
   return result;
}


/* This:    colums: columns
            rows: rows
   Operand: colums: colums2
            rows: columns
   Result:  colums: columns2
            rows: rows
*/
template <class FloatType, unsigned columns, unsigned rows>
template <unsigned columns2>
StaticMatrix<FloatType, columns2, rows> StaticMatrix<FloatType, columns, rows>::operator*( const StaticMatrix<FloatType, columns2, columns>& mat ) const
{
   StaticMatrix<FloatType, columns2, rows> res;
   for ( unsigned char row = 0; row < rows; row++ ){
      for ( unsigned char column = 0; column < columns2; column++ ){
         res[column][row] = 0;
         for ( unsigned char k = 0; k < columns; k++ )
            res[column][row] += theColumns[k][row] * mat[column][k];
      }
   }
   return res;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator/(const FloatType &s) const {
   StaticMatrix<FloatType, columns, rows> result;
   for (unsigned i=0; i<columns; i++) {
      result[i] = theColumns[i] / s;
   }
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator+=(const StaticMatrix<FloatType, columns, rows> &op) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i] += op.theColumns[i];
   }
   return *this;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator-=(const StaticMatrix<FloatType, columns, rows> &op) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i] -= op.theColumns[i];
   }
   return *this;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator*=(const FloatType &op) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i] *= op;
   }
   return *this;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator*=(const StaticMatrix<FloatType, columns, rows> &op) {
   StaticMatrix<FloatType, columns, rows> result = (*this)*op;
   *this = result;
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator/=(const FloatType &op) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i] /= op;
   }
   return *this;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows> StaticMatrix<FloatType, columns, rows>::operator=(const StaticMatrix<FloatType, columns, rows> &op) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i] = op.theColumns[i];
   }
   return *this;
}

template <class FloatType, unsigned columns, unsigned rows>
inline bool StaticMatrix<FloatType, columns, rows>::operator==(const StaticMatrix<FloatType, columns, rows> &op) const {
   for (unsigned i=0; i<columns; i++) {
      if (theColumns[i] != op.theColumns[i]) return false;
   }
   return true;
}

template <class FloatType, unsigned columns, unsigned rows>
inline bool StaticMatrix<FloatType, columns, rows>::operator!=(const StaticMatrix<FloatType, columns, rows> &op) const {
   for (unsigned i=0; i<columns; i++) {
      if (theColumns[i] != op.theColumns[i]) return true;
   }
   return false;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticVector<FloatType, rows> StaticMatrix<FloatType, columns, rows>::operator*(const StaticVector<FloatType, columns> &v) const {
   StaticVector<FloatType, rows> result;
   for (unsigned y=0; y<rows; y++) {
      result[y] = 0.0;
      for (unsigned x=0; x<columns; x++) {
         result[y] += theColumns[x][y]*v[x];
      }
   }
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, columns, rows>::StaticMatrix() {
   for (unsigned j=0; j<columns; j++) {
      for (unsigned i=0; i<rows; i++) {
         if (i == j)
            theColumns[j][i] = 1.0;
         else
            theColumns[j][i] = 0.0;
      }
   }
}


template <class FloatType, unsigned columns, unsigned rows>
inline FloatType* StaticMatrix<FloatType, columns, rows>::data() {
   return &(theColumns[0][0]);
}

template <class FloatType, unsigned columns, unsigned rows>
inline const FloatType* StaticMatrix<FloatType, columns, rows>::data() const {
   return &(theColumns[0][0]);
}

template <class FloatType, unsigned columns, unsigned rows>
inline void StaticMatrix<FloatType, columns, rows>::changeRows(unsigned row1, unsigned row2) {
   StaticVector<FloatType, columns> h;
   unsigned i;
   for (i=0; i<columns; i++) {
      h[i] = theColumns[i][row2];
   }
   for (i=0; i<columns; i++) {
      theColumns[i][row2] = theColumns[i][row1];
   }
   for (i=0; i<columns; i++) {
      theColumns[i][row1] = h[i];
   }
}

template <class FloatType, unsigned columns, unsigned rows>
inline void StaticMatrix<FloatType, columns, rows>::multRow(unsigned row, FloatType value) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i][row] *= value;
   }
}

template <class FloatType, unsigned columns, unsigned rows>
inline void StaticMatrix<FloatType, columns, rows>::combineRows(unsigned row, unsigned with, FloatType by) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i][row] += theColumns[i][with] * by;
   }
}

template <class FloatType, unsigned columns, unsigned rows>
inline void StaticMatrix<FloatType, columns, rows>::addRows(unsigned row, unsigned with) {
   for (unsigned i=0; i<columns; i++) {
      theColumns[i][row] += theColumns[i][with];
   }
}

template <class FloatType, unsigned columns, unsigned rows>
inline StaticMatrix<FloatType, rows, columns> StaticMatrix<FloatType, columns, rows>::transpose() const {
   StaticMatrix<FloatType, rows, columns> result;
   for (unsigned r=0; r<rows; r++) {
      for (unsigned c=0; c<columns; c++) {
         result[r][c] = theColumns[c][r];
      }
   }
   return result;
}

template <class FloatType, unsigned columns, unsigned rows>
inline FloatType StaticMatrix<FloatType, columns, rows>::getDeterminant() const {
   if ( ( rows == 2 ) && ( columns == 2 ) )
      return (*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0];
   else if ( ( rows == 3 ) && ( columns == 3 ) )
      return (*this)[0][0] * ( (*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1] ) -
             (*this)[0][1] * ( (*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0] ) +
             (*this)[0][2] * ( (*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0] );
   else if ( rows == 1 && columns == 1)
      return (*this)[0][0];
   else
      // limitiation: does not work for dimensions higher larger 3
   {

   }
}

/**
 *  Helping structure for jacobi iteration
 */
#define STATIC_MATRIX_ROTATE(a,i,j,k,l) g=a[i][j];\
                                        h=a[k][l];\
                                        a[i][j]=g-s*(h+g*tau);\
                                        a[k][l]=h+s*(g-h*tau);

template <class FloatType, unsigned columns, unsigned rows>
void StaticMatrix<FloatType, columns, rows>::eigenSort( StaticVector<FloatType,rows>& eigenValues, StaticMatrix<FloatType,rows,columns>& eigenVectors ) const
{
   int i,j,k;
   FloatType p;
   for ( i = 0; i < rows-1; i++ ){
      p = eigenValues[k=i];
      for( j = i+1; j < rows; j++ )
         if( eigenValues[j] >= p ) p = eigenValues[k=j];
      if( k != i ){
         eigenValues[k] = eigenValues[i];
         eigenValues[i] = p;
         for( j = 0; j < rows; j++ ){
            p = eigenVectors[j][i];
            eigenVectors[j][i] = eigenVectors[j][k];
            eigenVectors[j][k] = p;
         }
      }
   }
}


// --- explicit specialization for dim 2,3,4 cases:


#ifdef LINEAR_ALGEBRA_EXPLICIT_LOOP_UNROOLING


template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator+(const StaticVector<float, 4> &op) const {
   StaticVector<float, 4> result;
   result.v[0] = v[0] + op.v[0];
   result.v[1] = v[1] + op.v[1];
   result.v[2] = v[2] + op.v[2];
   result.v[3] = v[3] + op.v[3];
   return result;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator+(const StaticVector<float, 3> &op) const {
   StaticVector<float, 3> result;
   result.v[0] = v[0] + op.v[0];
   result.v[1] = v[1] + op.v[1];
   result.v[2] = v[2] + op.v[2];
   return result;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator+(const StaticVector<float, 2> &op) const {
   StaticVector<float, 2> result;
   result.v[0] = v[0] + op.v[0];
   result.v[1] = v[1] + op.v[1];
   return result;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator-(const StaticVector<float, 4> &op) const {
   StaticVector<float, 4> result;
   result.v[0] = v[0] - op.v[0];
   result.v[1] = v[1] - op.v[1];
   result.v[2] = v[2] - op.v[2];
   result.v[3] = v[3] - op.v[3];
   return result;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator-(const StaticVector<float, 3> &op) const {
   StaticVector<float, 3> result;
   result.v[0] = v[0] - op.v[0];
   result.v[1] = v[1] - op.v[1];
   result.v[2] = v[2] - op.v[2];
   return result;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator-(const StaticVector<float, 2> &op) const {
   StaticVector<float, 2> result;
   result.v[0] = v[0] - op.v[0];
   result.v[1] = v[1] - op.v[1];
   return result;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator-() const {
   StaticVector<float, 4> result;
   result.v[0] = -v[0];
   result.v[1] = -v[1];
   result.v[2] = -v[2];
   result.v[3] = -v[3];
   return result;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator-() const {
   StaticVector<float, 3> result;
   result.v[0] = -v[0];
   result.v[1] = -v[1];
   result.v[2] = -v[2];
   return result;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator-() const {
   StaticVector<float, 2> result;
   result.v[0] = -v[0];
   result.v[1] = -v[1];
   return result;
}

template<>
inline float StaticVector<float, 4>::operator*(const StaticVector<float, 4> op) const {
   float result = 0.0;
   result += v[0] * op.v[0];
   result += v[1] * op.v[1];
   result += v[2] * op.v[2];
   result += v[3] * op.v[3];
   return result;
}
template<>
inline float StaticVector<float, 3>::operator*(const StaticVector<float, 3> op) const {
   float result = 0.0;
   result += v[0] * op.v[0];
   result += v[1] * op.v[1];
   result += v[2] * op.v[2];
   return result;
}
template<>
inline float StaticVector<float, 2>::operator*(const StaticVector<float, 2> op) const {
   float result = 0.0;
   result += v[0] * op.v[0];
   result += v[1] * op.v[1];
   return result;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator*(const float &s) const {
   StaticVector<float, 4> result;
   result.v[0] = s * v[0];
   result.v[1] = s * v[1];
   result.v[2] = s * v[2];
   result.v[3] = s * v[3];
   return result;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator*(const float &s) const {
   StaticVector<float, 3> result;
   result.v[0] = s * v[0];
   result.v[1] = s * v[1];
   result.v[2] = s * v[2];
   return result;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator*(const float &s) const {
   StaticVector<float, 2> result;
   result.v[0] = s * v[0];
   result.v[1] = s * v[1];
   return result;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator/(const float &s) const {
   StaticVector<float, 4> result;
   result.v[0] = v[0] / s;
   result.v[1] = v[1] / s;
   result.v[2] = v[2] / s;
   result.v[3] = v[3] / s;
   return result;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator/(const float &s) const {
   StaticVector<float, 3> result;
   result.v[0] = v[0] / s;
   result.v[1] = v[1] / s;
   result.v[2] = v[2] / s;
   return result;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator/(const float &s) const {
   StaticVector<float, 2> result;
   result.v[0] = v[0] / s;
   result.v[1] = v[1] / s;
   return result;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::componentProduct(const StaticVector<float, 4> &op) const {
   StaticVector<float, 4> result;
   result.v[0] = v[0] * op.v[0];
   result.v[1] = v[1] * op.v[1];
   result.v[2] = v[2] * op.v[2];
   result.v[3] = v[3] * op.v[3];
   return result;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::componentProduct(const StaticVector<float, 3> &op) const {
   StaticVector<float, 3> result;
   result.v[0] = v[0] * op.v[0];
   result.v[1] = v[1] * op.v[1];
   result.v[2] = v[2] * op.v[2];
   return result;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::componentProduct(const StaticVector<float, 2> &op) const {
   StaticVector<float, 2> result;
   result.v[0] = v[0] * op.v[0];
   result.v[1] = v[1] * op.v[1];
   return result;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator+=(const StaticVector<float, 4> &op) {
   v[0] += op.v[0];
   v[1] += op.v[1];
   v[2] += op.v[2];
   v[3] += op.v[3];
   return *this;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator+=(const StaticVector<float, 3> &op) {
   v[0] += op.v[0];
   v[1] += op.v[1];
   v[2] += op.v[2];
   return *this;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator+=(const StaticVector<float, 2> &op) {
   v[0] += op.v[0];
   v[1] += op.v[1];
   return *this;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator-=(const StaticVector<float, 4> &op) {
   v[0] -= op.v[0];
   v[1] -= op.v[1];
   v[2] -= op.v[2];
   v[3] -= op.v[3];
   return *this;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator-=(const StaticVector<float, 3> &op) {
   v[0] -= op.v[0];
   v[1] -= op.v[1];
   v[2] -= op.v[2];
   return *this;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator-=(const StaticVector<float, 2> &op) {
   v[0] -= op.v[0];
   v[1] -= op.v[1];
   return *this;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator*=(const float &s) {
   v[0] *= s;
   v[1] *= s;
   v[2] *= s;
   v[3] *= s;
   return *this;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator*=(const float &s) {
   v[0] *= s;
   v[1] *= s;
   v[2] *= s;
   return *this;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator*=(const float &s) {
   v[0] *= s;
   v[1] *= s;
   return *this;
}

template<>
inline StaticVector<float, 4> StaticVector<float, 4>::operator/=(const float &s) {
   v[0] /= s;
   v[1] /= s;
   v[2] /= s;
   v[3] /= s;
   return *this;
}
template<>
inline StaticVector<float, 3> StaticVector<float, 3>::operator/=(const float &s) {
   v[0] /= s;
   v[1] /= s;
   v[2] /= s;
   return *this;
}
template<>
inline StaticVector<float, 2> StaticVector<float, 2>::operator/=(const float &s) {
   v[0] /= s;
   v[1] /= s;
   return *this;
}

template<>
inline bool StaticVector<float, 4>::operator==(const StaticVector<float, 4> op) const {
   if (v[0] != op.v[0]) return false;
   if (v[1] != op.v[1]) return false;
   if (v[2] != op.v[2]) return false;
   if (v[3] != op.v[3]) return false;
   return true;
}
template<>
inline bool StaticVector<float, 3>::operator==(const StaticVector<float, 3> op) const {
   if (v[0] != op.v[0]) return false;
   if (v[1] != op.v[1]) return false;
   if (v[2] != op.v[2]) return false;
   return true;
}
template<>
inline bool StaticVector<float, 2>::operator==(const StaticVector<float, 2> op) const {
   if (v[0] != op.v[0]) return false;
   if (v[1] != op.v[1]) return false;
   return true;
}

template<>
inline bool StaticVector<float, 4>::operator!=(const StaticVector<float, 4> op) const {
   if (v[0] != op.v[0]) return true;
   if (v[1] != op.v[1]) return true;
   if (v[2] != op.v[2]) return true;
   if (v[3] != op.v[3]) return true;
   return false;
}
template<>
inline bool StaticVector<float, 3>::operator!=(const StaticVector<float, 3> op) const {
   if (v[0] != op.v[0]) return true;
   if (v[1] != op.v[1]) return true;
   if (v[2] != op.v[2]) return true;
   return false;
}
template<>
inline bool StaticVector<float, 2>::operator!=(const StaticVector<float, 2> op) const {
   if (v[0] != op.v[0]) return true;
   if (v[1] != op.v[1]) return true;
   return false;
}



template<>
inline StaticMatrix<float, 2, 2>::StaticMatrix() {
   theColumns[0][0] = 1.0f; theColumns[0][1] = 0.0f;
   theColumns[1][0] = 0.0f; theColumns[1][1] = 1.0f;
}

template<>
inline StaticMatrix<float, 3, 3>::StaticMatrix() {
   theColumns[0][0] = 1.0f; theColumns[0][1] = 0.0f; theColumns[0][2] = 0.0f;
   theColumns[1][0] = 0.0f; theColumns[1][1] = 1.0f; theColumns[1][2] = 0.0f;
   theColumns[2][0] = 0.0f; theColumns[2][1] = 0.0f; theColumns[2][2] = 1.0f;
}

template<>
inline StaticMatrix<float, 4, 4>::StaticMatrix() {
   theColumns[0][0] = 1.0f; theColumns[0][1] = 0.0f; theColumns[0][2] = 0.0f; theColumns[0][3] = 0.0f;
   theColumns[1][0] = 0.0f; theColumns[1][1] = 1.0f; theColumns[1][2] = 0.0f; theColumns[1][3] = 0.0f;
   theColumns[2][0] = 0.0f; theColumns[2][1] = 0.0f; theColumns[2][2] = 1.0f; theColumns[2][3] = 0.0f;
   theColumns[3][0] = 0.0f; theColumns[3][1] = 0.0f; theColumns[3][2] = 0.0f; theColumns[3][3] = 1.0f;
}

template<>
inline StaticMatrix<float, 2, 3>::StaticMatrix() {
   theColumns[0][0] = 0.0f; theColumns[0][1] = 0.0f;
   theColumns[1][0] = 0.0f; theColumns[1][1] = 0.0f;
   theColumns[2][0] = 0.0f; theColumns[2][1] = 0.0f;
}

template<>
inline StaticMatrix<float, 2, 2> StaticMatrix<float, 2, 2>::operator+(const StaticMatrix<float, 2, 2> &op) const {
   StaticMatrix<float, 2, 2> result;
   result.theColumns[0][0] = theColumns[0][0] + op.theColumns[0][0];
   result.theColumns[0][1] = theColumns[0][1] + op.theColumns[0][1];
   result.theColumns[1][0] = theColumns[1][0] + op.theColumns[1][0];
   result.theColumns[1][1] = theColumns[1][1] + op.theColumns[1][1];
   return result;
}

template<>
inline StaticMatrix<float, 2, 2> StaticMatrix<float, 2, 2>::operator*(const float &s) const {
   StaticMatrix<float, 2, 2> result;
   result.theColumns[0][0] = theColumns[0][0] * s;
   result.theColumns[0][1] = theColumns[0][1] * s;
   result.theColumns[1][0] = theColumns[1][0] * s;
   result.theColumns[1][1] = theColumns[1][1] * s;
   return result;
}

template<>
inline StaticVector<float, 4> StaticMatrix<float, 4, 4>::operator*(const StaticVector<float, 4> &v) const {
   StaticVector<float, 4> result;
   result[0] = theColumns[0][0]*v[0] + theColumns[1][0]*v[1] + theColumns[2][0]*v[2] + theColumns[3][0]*v[3];
   result[1] = theColumns[0][1]*v[0] + theColumns[1][1]*v[1] + theColumns[2][1]*v[2] + theColumns[3][1]*v[3];
   result[2] = theColumns[0][2]*v[0] + theColumns[1][2]*v[1] + theColumns[2][2]*v[2] + theColumns[3][2]*v[3];
   result[3] = theColumns[0][3]*v[0] + theColumns[1][3]*v[1] + theColumns[2][3]*v[2] + theColumns[3][3]*v[3];
   return result;
}
template<>
inline StaticVector<float, 3> StaticMatrix<float, 3, 3>::operator*(const StaticVector<float, 3> &v) const {
   StaticVector<float, 3> result;
   result[0] = theColumns[0][0]*v[0] + theColumns[1][0]*v[1] + theColumns[2][0]*v[2];
   result[1] = theColumns[0][1]*v[0] + theColumns[1][1]*v[1] + theColumns[2][1]*v[2];
   result[2] = theColumns[0][2]*v[0] + theColumns[1][2]*v[1] + theColumns[2][2]*v[2];
   return result;
}
template<>
inline StaticVector<float, 2> StaticMatrix<float, 2, 2>::operator*(const StaticVector<float, 2> &v) const {
   StaticVector<float, 2> result;
   result[0] = theColumns[0][0]*v[0] + theColumns[1][0]*v[1];
   result[1] = theColumns[0][1]*v[0] + theColumns[1][1]*v[1];
   return result;
}
template<>
inline StaticVector<float, 2> StaticMatrix<float, 3, 2>::operator*(const StaticVector<float, 3> &v) const {
   StaticVector<float, 2> result;
   result[0] = theColumns[0][0]*v[0] + theColumns[1][0]*v[1] + theColumns[2][0]*v[2];
   result[1] = theColumns[0][1]*v[0] + theColumns[1][1]*v[1] + theColumns[2][1]*v[2];
   return result;
}

template<>
inline float normQuad(StaticVector<float, 3> v) {
   return v[0]*v[0]+v[1]*v[1]+v[2]*v[2];
};

template<>
inline float normQuad(StaticVector<float, 2> v) {
   return v[0]*v[0]+v[1]*v[1];
};


// --- partial specialization (efficiency)


template <class FloatTypeDest, class FloatTypeSource>
StaticVector<FloatTypeDest, 2> convertVector(const StaticVector<FloatTypeSource, 2> &source) {
   result[0] = (FloatTypeDest)source[0];
   result[1] = (FloatTypeDest)source[1];
   return result;
}

template <class FloatTypeDest, class FloatTypeSource>
StaticVector<FloatTypeDest, 3> convertVector(const StaticVector<FloatTypeSource, 3> &source) {
   result[0] = (FloatTypeDest)source[0];
   result[1] = (FloatTypeDest)source[1];
   result[2] = (FloatTypeDest)source[2];
   return result;
}

template <class FloatTypeDest, class FloatTypeSource>
StaticVector<FloatTypeDest, 4> convertVector(const StaticVector<FloatTypeSource, 4> &source) {
   result[0] = (FloatTypeDest)source[0];
   result[1] = (FloatTypeDest)source[1];
   result[2] = (FloatTypeDest)source[2];
   result[3] = (FloatTypeDest)source[3];
   return result;
}

template<>
inline float norm(StaticVector<float, 3> v) {
   return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
};

template<>
inline float norm(StaticVector<float, 2> v) {
   return sqrt(v[0]*v[0]+v[1]*v[1]);
};

template<>
inline StaticVector<float,1> makeVector<float,1>( const float * data )
{
   StaticVector<float,1> ret;
   ret[0] = data[0];
   return ret;
}


template<>
inline StaticVector<float,2> makeVector<float,2>( const float * data )
{
   StaticVector<float,2> ret;
   ret[0] = data[0];
   ret[1] = data[1];
   return ret;
}

template<>
inline StaticVector<float,3> makeVector<float,3>( const float * data )
{
   StaticVector<float,3> ret;
   ret[0] = data[0];
   ret[1] = data[1];
   ret[2] = data[2];
   return ret;
}

template<>
inline StaticVector<float,4> makeVector<float,4>( const float * data )
{
   StaticVector<float,4> ret;
   ret[0] = data[0];
   ret[1] = data[1];
   ret[2] = data[2];
   ret[3] = data[3];
   return ret;
}


#endif  // end of explicit loop unrolling


#endif
