vectors.hpp

/*
  Copyright ©2013 The Regents of the University of California
  (Regents). All Rights Reserved. Permission to use, copy, modify, and
  distribute this software and its documentation for educational,
  research, and not-for-profit purposes, without fee and without a
  signed licensing agreement, is hereby granted, provided that the
  above copyright notice, this paragraph and the following two
  paragraphs appear in all copies, modifications, and
  distributions. Contact The Office of Technology Licensing, UC
  Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620,
  (510) 643-7201, for commercial licensing opportunities.

  IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT,
  INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
  LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
  DOCUMENTATION, EVEN IF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY
  OF SUCH DAMAGE.

  REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  FOR A PARTICULAR PURPOSE. THE SOFTWARE AND ACCOMPANYING
  DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS
  IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT,
  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
*/

#ifndef VECTORS_HPP
#define VECTORS_HPP

#include "winport.hpp" // aa: windows bindings, etc

#include <cmath>
#include <iostream>

// aa: if defined, AVX SIMD doubles will be used for vector math.
//#define _AVX

// aa: force alignment for global new/delete operators (in all files that include this one)
// aa: force alignment for global new/delete operators (in all files that include this one)
#if defined(_WIN32)
#define __align(sz) __declspec(align(sz))
inline void* malloc_align(size_t size, size_t alignment = 32) { return _aligned_malloc(size, alignment); }
inline void aligned_free(void *ptr)    { _aligned_free(ptr); }
#else
// This is for Linux, Apple OS will require a separate treatment...
#define __align(sz) __attribute__((aligned(sz)))
#include <stdlib.h>
#include <assert.h>
inline void* malloc_align(size_t size, size_t alignment = 32) {
	void* ptr;
	if(0 != posix_memalign(&ptr, alignment, size)) {
		ptr = NULL;
	}
	assert(ptr!=NULL);
	return ptr;
}
inline void aligned_free(void *ptr)    { free(ptr); }
#endif

#if defined(_AVX)
inline void* operator new(size_t sz)      { return malloc_align(sz); }
inline void* operator new[](size_t sz)    { return malloc_align(sz); }
inline void  operator delete(void *ptr)   { aligned_free(ptr); }
inline void  operator delete[](void *ptr) { aligned_free(ptr); }
#endif

inline double sq (double x) {return x*x;}

template <bool cond> struct static_assertion_failure;
template <> struct static_assertion_failure<true> {void operator() () {}};
#define static_assert(cond) static_assertion_failure<cond>();

#define tpl template <int n, typename T>
#define VecnT Vec<n,T>

template <int n, typename T=double> class Vec {
private:
#if defined(_AVX)
  __align(32) T c[n%4? 4*(1+n/4): n];
#else
  T c[n];
#endif
public:
    Vec () {for (int i = 0; i < n; i++) c[i] = 0;}
    explicit Vec (T x) {for (int i = 0; i < n; i++) c[i] = x;}
    explicit Vec (T x, T y) {static_assert(n==2); c[0] = x; c[1] = y;}
    explicit Vec (T x, T y, T z) {static_assert(n==3); c[0] = x; c[1] = y; c[2] = z;}
    explicit Vec (T x, T y, T z, T w) {static_assert(n==4); c[0] = x; c[1] = y; c[2] = z; c[3] = w;}
    T &operator[] (int i) {return c[i];}
    const T &operator[] (int i) const {return c[i];}
    T &operator() (int i) {return c[i];}
    const T &operator() (int i) const {return c[i];}
};
tpl VecnT operator+ (const VecnT &u) {return u;}
tpl VecnT operator+ (const VecnT &u, const VecnT &v) {VecnT w; for (int i = 0; i < n; i++) w[i] = u[i] + v[i]; return w;}
tpl VecnT &operator+= (VecnT &u, const VecnT &v) {return u = u + v;}
tpl VecnT operator- (const VecnT &u) {VecnT v; for (int i = 0; i < n; i++) v[i] = -u[i]; return v;}
tpl VecnT operator- (const VecnT &u, const VecnT &v) {return u + (-v);}
tpl VecnT &operator-= (VecnT &u, const VecnT &v) {return u = u - v;}
tpl VecnT operator* (const T &a, const VecnT &u) {VecnT v; for (int i = 0; i < n; i++) v[i] = a*u[i]; return v;}
tpl VecnT operator* (const VecnT &u, const T &a) {return a*u;}
tpl VecnT &operator*= (VecnT &u, const T &a) {return u = u*a;}
tpl VecnT operator/ (const VecnT &u, const T &a) {return u*(1/a);}
tpl VecnT &operator/= (VecnT &u, const T &a) {return u = u/a;}
tpl bool operator== (const VecnT &u, const VecnT &v) {for(int i=0; i<n; ++i) if(u[i] != v[i]) return false; return true;}
tpl bool operator!= (const VecnT &u, const VecnT &v) {return !(u==v);}
tpl T dot (const VecnT &u, const VecnT &v) {T d = 0; for (int i = 0; i < n; i++) d += u[i]*v[i]; return d;}
tpl T norm2 (const VecnT &u) {return dot(u,u);}
tpl T norm (const VecnT &u) {return sqrt(norm2(u));}
tpl VecnT normalize (const VecnT &u) {T m = norm(u); return m==0 ? VecnT(0) : u/m;}
tpl std::ostream &operator<< (std::ostream &out, const VecnT &u) {out << "("; for (int i = 0; i < n; i++) out << (i==0?"":", ") << u[i]; out << ")"; return out;}
template <typename T> Vec<3,T> cross (const Vec<3,T> &u, const Vec<3,T> &v) {Vec<3,T> w; w[0] = u[1]*v[2] - u[2]*v[1]; w[1] = u[2]*v[0] - u[0]*v[2]; w[2] = u[0]*v[1] - u[1]*v[0]; return w;}
template <typename T> T stp (const Vec<3,T> &u, const Vec<3,T> &v, const Vec<3,T> &w) {return dot(u,cross(v,w));}
template <int m, int n, typename T> Vec<m,T> project (const VecnT &u) {Vec<m,T> v; for (int i = 0; i < m; i++) v[i] = (i<n) ? u[i] : 0; return v;}
template <typename T> Vec<2,T> perp (const Vec<2,T> &u) {return Vec<2,T>(-u[1],u[0]);}

#if defined(_AVX)
#if !defined(_WIN32)
#include <x86intrin.h>
#endif
template<> inline Vec<3,double> operator+<3,double>(const Vec<3,double> &u, const Vec<3,double> &v) { __m256d r = _mm256_add_pd((__m256d&)u, (__m256d&)v); return (Vec<3,double>&)r; }
template<> inline Vec<3,double> operator-<3,double>(const Vec<3,double> &u, const Vec<3,double> &v) { __m256d r = _mm256_sub_pd((__m256d&)u, (__m256d&)v); return (Vec<3,double>&)r; }
template<> inline Vec<3,double> operator*<3,double>(const double &a, const Vec<3,double> &v) { __m256d r = _mm256_mul_pd(_mm256_set1_pd(a), (__m256d&)v); return (Vec<3,double>&)r; }
template<> inline Vec<3,double> operator/<3,double>(const Vec<3,double> &u, const double &a) { __m256d r = _mm256_div_pd((__m256d&)u, _mm256_set1_pd(a)); return (Vec<3,double>&)r; }

template<> inline Vec<3,double>& operator+=<3,double>(Vec<3,double> &r, const Vec<3,double> &v) { (__m256d&)r = _mm256_add_pd((__m256d&)r, (__m256d&)v); return r; }
template<> inline Vec<3,double>& operator-=<3,double>(Vec<3,double> &r, const Vec<3,double> &v) { (__m256d&)r = _mm256_sub_pd((__m256d&)r, (__m256d&)v); return r; }

// There are no base templates for these functions, we will define inline versions.
inline Vec<3,double>& operator*=(Vec<3,double> &r, const Vec<3,double> &v)      { (__m256d&)r = _mm256_mul_pd((__m256d&)r, (__m256d&)v); return r; }
inline Vec<3,double>& operator/=(Vec<3,double> &r, const Vec<3,double> &v)      { (__m256d&)r = _mm256_div_pd((__m256d&)r, (__m256d&)v); return r; }
inline Vec<3,double>  operator*(const Vec<3,double> &u, const Vec<3,double> &v) { __m256d r = _mm256_mul_pd((__m256d&)u, (__m256d&)v); return (Vec<3,double>&)r; }
inline Vec<3,double>  operator/(const Vec<3,double> &u, const Vec<3,double> &v) { __m256d r = _mm256_div_pd((__m256d&)u, (__m256d&)v); return (Vec<3,double>&)r; }
#endif

#undef tpl
#undef VecnT

typedef Vec<2> Vec2;
typedef Vec<3> Vec3;

#define tpl template <int m, int n, typename T>
#define MatmnT Mat<m,n,T>
#define MatnmT Mat<n,m,T>
#define MatnnT Mat<n,n,T>
#define VecmT Vec<m,T>
#define VecnT Vec<n,T>

//aa: transposed matrix functionality
template <int m, int n, typename T=double> class MatTransposed;

template <int m, int n, typename T=double> class Mat {
private:
    VecmT c[n];
public:
    Mat () {for (int j = 0; j < n; j++) c[j] = VecmT(0);}
    explicit Mat (T x) {for (int j = 0; j < n; j++) {c[j] = VecmT(0); if (j < m) c[j][j] = x;}}
    explicit Mat (VecmT x, VecmT y) {static_assert(n==2); c[0] = x; c[1] = y;}
    explicit Mat (VecmT x, VecmT y, VecmT z) {static_assert(n==3); c[0] = x; c[1] = y; c[2] = z;}
    explicit Mat (VecmT x, VecmT y, VecmT z, VecmT w) {static_assert(n==4); c[0] = x; c[1] = y; c[2] = z; c[3] = w;}
//aa:    static Mat rows (VecnT x, VecnT y) {return Mat<n,2,T>(x,y).t();}
//aa:    static Mat rows (VecnT x, VecnT y, VecnT z) {return Mat<n,3,T>(x,y,z).t();}
	static Mat rows (VecnT x, VecnT y) { Mat<2,n,T> M; for(int i = 0; i < n; i++) { M.col(i)[0] = x[i]; M.col(i)[1] = y[i]; } return M; }
    static Mat rows (VecnT x, VecnT y, VecnT z) { Mat<3,n,T> M; for(int i = 0; i < n; i++) { M.col(i)[0] = x[i]; M.col(i)[1] = y[i]; M.col(i)[2] = z[i];} return M; }
    static Mat rows (VecnT x, VecnT y, VecnT z, VecnT w) { Mat<4,n,T> M; for(int i = 0; i < n; i++) { M.col(i)[0] = x[i]; M.col(i)[1] = y[i]; M.col(i)[2] = z[i]; M.col(i)[3] = w[i];} return M; }
    VecnT row (int i) const {VecnT R; for(int col = 0; col < n; ++col) { R[col] = c[col][i]; } return R; }


    T &operator() (int i, int j) {return c[j][i];}
    const T &operator() (int i, int j) const {return c[j][i];}
    VecmT &col (int j) {return c[j];}
    const VecmT &col (int j) const {return c[j];}
    MatnmT t () const {return transpose(*this);}
	// const MatTransposed<m,n,T>& t () const {return reinterpret_cast<const MatTransposed<m,n,T>&>(*this);}
    MatmnT inv () const {return inverse(*this);}
};
tpl MatmnT operator+ (const MatmnT &A) {return A;}
tpl MatmnT operator+ (const MatmnT &A, const MatmnT &B) {MatmnT C; for (int j = 0; j < n; j++) C.col(j) = A.col(j) + B.col(j); return C;}
tpl MatmnT &operator+= (MatmnT &A, const MatmnT &B) {return A = A + B;}
tpl MatmnT operator- (const MatmnT &A) {MatmnT B; for (int j = 0; j < n; j++) B.col(j) = -A.col(j); return B;}
tpl MatmnT operator- (const MatmnT &A, const MatmnT &B) {return A + (-B);}
tpl MatmnT &operator-= (MatmnT &A, const MatmnT &B) {return A = A - B;}
tpl MatmnT operator* (const T &a, const MatmnT &A) {MatmnT B; for (int j = 0; j < n; j++) B.col(j) = a*A.col(j); return B;}
tpl MatmnT operator* (const MatmnT &A, const T &a) {return a*A;}
tpl MatmnT &operator*= (MatmnT &A, const T &a) {return A = A*a;}
tpl MatmnT operator/ (const MatmnT &A, const T &a) {return A*(1/a);}
tpl MatmnT &operator/= (MatmnT &A, const T &a) {return A = A/a;}
tpl VecmT operator* (const MatmnT &A, const VecnT &u) {VecmT v = VecmT(0); for (int j = 0; j < n; j++) v += A.col(j)*u[j]; return v;}
template <int m, int n, int o, typename T> Mat<m,o,T> operator* (const Mat<m,n,T> &A, const Mat<n,o,T> &B) {Mat<m,o,T> C; for (int k = 0; k < o; k++) C.col(k) = A*B.col(k); return C;}
tpl MatmnT *operator*= (const MatmnT &A, const MatnnT &B) {return A = A*B;}
tpl MatnmT transpose (const MatmnT &A) {MatnmT B; for (int i = 0; i < m; i++) for (int j = 0; j < n; j++) B(j,i) = A(i,j); return B;}
template <int n, typename T> VecnT diag (const MatnnT &A) {VecnT u; for (int j = 0; j < n; j++) u[j] = A(j,j); return u;}
template <int n, typename T> T trace (const MatnnT &A) {T t = 0; for (int j = 0; j < n; j++) t += A(j,j); return t;}
template <typename T> T det (const Mat<2,2,T> &A) {return A(0,0)*A(1,1) - A(0,1)*A(1,0);}
template <typename T> T det (const Mat<3,3,T> &A) {return stp(A.col(0), A.col(1), A.col(2));}
template <typename T> Mat<2,2,T> inverse (const Mat<2,2,T> &A) {return Mat<2,2,T>(Vec<2,T>(A(1,1), -A(1,0)), Vec<2,T>(-A(0,1), A(0,0)))/det(A);}
template <typename T> T wedge (const Vec<2,T> &u, const Vec<2,T> &v) {return u[0]*v[1] - u[1]*v[0];}
template <typename T> Mat<3,3,T> inverse (const Mat<3,3,T> &A) {return Mat<3,3,T>(cross(A.col(1),A.col(2)), cross(A.col(2),A.col(0)), cross(A.col(0),A.col(1))).t()/det(A);}
template <int n, typename T> MatnnT diag (const VecnT &u) {MatnnT A = MatnnT(0); for (int j = 0; j < n; j++) A(j,j) = u[j]; return A;}
tpl MatmnT outer (const VecmT &u, const VecnT &v) {MatmnT A; for (int j = 0; j < n; j++) A.col(j) = u*v[j]; return A;}
tpl std::ostream &operator<< (std::ostream &out, const MatmnT &A) {MatnmT At = transpose(A); out << "(" << std::endl; for (int i = 0; i < m; i++) out << "    " << At.col(i) << (i+1==m?"":",") << std::endl; out << ")"; return out;}

// Frobenius norm
tpl T norm2_F (const MatmnT &A) {T a = 0; for (int j = 0; j < n; j++) a += norm2(A.col(j)); return a;}
tpl T norm_F (const MatmnT &A) {return sqrt(norm2_F(A));}

template <int m1, int n1, int m2, int n2, typename T> Mat<m1,n1,T> project (const Mat<m2,n2,T> &A) {Mat<m1,n1,T> B; for (int j = 0; j < n1; j++) B.col(j) = (j<n2) ? project<m1>(A.col(j)) : Vec<m1,T>(0); return B;}

#undef tpl
#undef MatmnT
#undef MatnnT
#undef VecmT
#undef VecnT

typedef Mat<2,2> Mat2x2;
typedef Mat<3,3> Mat3x3;
typedef Mat<3,2> Mat3x2;
typedef Mat<2,3> Mat2x3;

template <int n> struct Eig {
    Mat<n,n> Q;
    Vec<n> l;
};

template <int n> Eig<n> eigen_decomposition (const Mat<n,n> &A);
template<> Eig<2> eigen_decomposition<2>(const Mat2x2 &A);

template <int m, int n> struct SVD {
    Mat<m,m> U;
    Vec<n> s;
    Mat<n,n> Vt;
};

template <int m, int n> SVD<m,n> singular_value_decomposition (const Mat<m,n> &A);
template<> SVD<3,2> singular_value_decomposition<3,2> (const Mat<3,2> &A);


template <int m, int n, int o, typename T> Mat<m,o,T> operator* (const Mat<m,n,T> &A, const MatTransposed<o,n,T> &B);
template <int m, int n, typename T> Vec<n, T> operator* (const MatTransposed<m,n,T> &A, const Vec<m, T> &u);
template <int m, int n, int o, typename T> Mat<m,o,T> operator* (const MatTransposed<n,m,T> &A, const Mat<n,o,T> &B);

//aa: transposed matrix functionality
template <int m, int n, typename T> class MatTransposed : protected Mat<m,n,T> {
	friend Vec<n,T> operator*<> (const MatTransposed<m,n,T> &A, const Vec<m,T> &u);
	template <int m1, int n1, int o, typename T1>  friend Mat<m1,o,T1> operator* (const Mat<m1,n1,T1> &A, const MatTransposed<o,n1,T1> &B);
	template <int m1, int n1, int o, typename T1> friend Mat<m1,o,T1> operator* (const MatTransposed<n1,m1,T1> &A, const Mat<n1,o,T1> &B);
public:

    const Mat<m,n,T>& t () const {return static_cast<const Mat<m, n, T>&>(*this);}
};

template <int m, int n, typename T> Vec<n, T> operator* (const MatTransposed<m,n,T> &A, const Vec<m, T> &u)
{
	Vec<n,T> v;
	for (int j = 0; j < n; j++) v[j] = dot(A.col(j), u);
	return v;
}

template <int m, int n, int o, typename T> Mat<m,o,T> operator* (const Mat<m,n,T> &A, const MatTransposed<o,n,T> &B)
{
	Mat<m,o,T> C;
	for (int k = 0; k < o; k++) {
		//C.col(k) = 0; //!!!
		C.col(k) = A.col(0)*B.col(0)[k];
		for(int i = 1; i < n; i++) {
			C.col(k) += A.col(i)*B.col(i)[k];
		}
	}
	return C;
}

template <int m, int n, int o, typename T> Mat<m,o,T> operator* (const MatTransposed<n,m,T> &A, const Mat<n,o,T> &B)
{
	Mat<m,o,T> C;
	for (int k = 0; k < o; k++) {
		for(int j = 0; j < m; j++)
			C.col(k)[j] = dot(A.col(j), B.col(k)); //!!!
	}
	return C;
}

/*
template <int m, int n, int o, typename T> Mat<m,o,T> operator* (const MatTransposed<n,m,T> &A, const MatTransposed<o,n,T> &B)
{
	Mat<m,o,T> C;
	for (int k = 0; k < o; k++) {
		for(int j = 0; j < m; j++) {
			C.col(k)[j] = 0; //!!!
			for(int i = 0; i < n; i++)
				C.col(k)[j] += A.col(j)[i]*B.col(i)[k];
		}
	}
	return C;
}
*/
#undef static_assert

#endif