al_Vec.hpp

#ifndef INCLUDE_AL_VEC_HPP
#define INCLUDE_AL_VEC_HPP
 
/*  Allocore --
  Multimedia / virtual environment application class library
 
  Copyright (C) 2009. AlloSphere Research Group, Media Arts & Technology, UCSB.
  Copyright (C) 2012. The Regents of the University of California.
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
 
    Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.
 
    Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
 
    Neither the name of the University of California nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.
 
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.
 
 
  File description:
  Generic fixed-size n-vector
 
  File author(s):
  Lance Putnam, 2010, putnam.lance@gmail.com
  Graham Wakefield, 2010, grrrwaaa@gmail.com
*/
 
#include <stdio.h>
#include <cmath>
#include <ostream>
#include "al/math/al_Constants.hpp"
 
namespace al {
 
template <int N, class T>
class Vec;
 
typedef Vec<2, float> Vec2f;   
typedef Vec<2, double> Vec2d;  
typedef Vec<2, int> Vec2i;     
typedef Vec<3, float> Vec3f;   
typedef Vec<3, double> Vec3d;  
typedef Vec<3, int> Vec3i;     
typedef Vec<4, float> Vec4f;   
typedef Vec<4, double> Vec4d;  
typedef Vec<4, int> Vec4i;     
 
// Forward iterates from 0 to n-1. Current index is 'i'.
#define IT(n) for (int i = 0; i < (n); ++i)
 
template <int N, class T>
struct VecElems {
  T x, y, z, w;
 private:
  T data[N - 4];
};
 
template <class T>
struct VecElems<0, T> {
  static T x;
};
template <class T>
T VecElems<0, T>::x = 0;
 
template <class T>
struct VecElems<1, T> {
  T x;
};
template <class T>
struct VecElems<2, T> {
  T x, y;
};
template <class T>
struct VecElems<3, T> {
  T x, y, z;
 
  Vec<3, T> cross(const Vec<3, T>& b) const {
    return Vec<3, T>(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
  }
};
template <class T>
struct VecElems<4, T> {
  T x, y, z, w;
 
  Vec<4, T> cross(const Vec<4, T>& b) const {
    return Vec<3, T>(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
  }
};
 
 
template <int N, class T>
class Vec : public VecElems<N, T> {
 public:
  using VecElems<N, T>::x;
 
  typedef T value_type;
 
  Vec(const T& v = T()) { set(v); }
 
  Vec(const T& v1, const T& v2) { set(v1, v2); }
 
  Vec(const T& v1, const T& v2, const T& v3) { set(v1, v2, v3); }
 
  Vec(const T& v1, const T& v2, const T& v3, const T& v4) {
    set(v1, v2, v3, v4);
  }
 
  template <int N2, class T2>
  Vec(const Vec<N2, T2>& v) {
    set(v);
  }
 
  template <class Tv, class Ts>
  Vec(const Vec<N - 1, Tv>& v, const Ts& s) {
    set(v, s);
  }
 
  template <class T2>
  Vec(const T2* v, int stride = 1) {
    set(v, stride);
  }
 
  //--------------------------------------------------------------------------
  // Memory Operations
 
  static int size() { return N; }
 
  static Vec& pun(T* src) { return *(Vec*)(src); }
  static const Vec& pun(const T* src) { return *(const Vec*)(src); }
 
  template <class V>
  V& as() {
    return *(V*)(elems());
  }
 
  template <class V>
  const V& as() const {
    return *(const V*)(elems());
  }
 
  const T* elems() const { return &x; }
 
  T* elems() { return &x; }
 
  T* begin() { return elems(); }
  const T* begin() const { return elems(); }
  T* end() { return elems() + N; }
  const T* end() const { return elems() + N; }
 
  T& operator[](size_t i) { return elems()[i]; }
 
  const T& operator[](size_t i) const { return elems()[i]; }
 
  bool operator==(const Vec& v) const {
    IT(N) {
      if ((*this)[i] != v[i]) return false;
    }
    return true;
  }
 
  bool operator==(const T& v) const {
    IT(N) {
      if ((*this)[i] != v) return false;
    }
    return true;
  }
 
  bool operator!=(const Vec& v) const { return !(*this == v); }
 
  bool operator!=(const T& v) const { return !(*this == v); }
 
  Vec<2, T> get(int i0, int i1) const {
    return Vec<2, T>((*this)[i0], (*this)[i1]);
  }
 
  Vec<3, T> get(int i0, int i1, int i2) const {
    return Vec<3, T>((*this)[i0], (*this)[i1], (*this)[i2]);
  }
 
  Vec<4, T> get(int i0, int i1, int i2, int i3) const {
    return Vec<4, T>((*this)[i0], (*this)[i1], (*this)[i2], (*this)[i3]);
  }
 
  template <int M>
  Vec<M, T> sub(int begin = 0) const {
    return Vec<M, T>(elems() + begin);
  }
 
  template <int M>
  Vec<M, T>& sub(int begin = 0) {
    return *(Vec<M, T>*)(elems() + begin);
  }
 
  //--------------------------------------------------------------------------
  // Basic Arithmetic Operations
 
  Vec& operator=(const Vec& v) { return set(v); }
  Vec& operator=(const T& v) { return set(v); }
  Vec& operator+=(const Vec& v) {
    IT(N)(*this)[i] += v[i];
    return *this;
  }
  Vec& operator+=(const T& v) {
    IT(N)(*this)[i] += v;
    return *this;
  }
  Vec& operator-=(const Vec& v) {
    IT(N)(*this)[i] -= v[i];
    return *this;
  }
  Vec& operator-=(const T& v) {
    IT(N)(*this)[i] -= v;
    return *this;
  }
  Vec& operator*=(const Vec& v) {
    IT(N)(*this)[i] *= v[i];
    return *this;
  }
  Vec& operator*=(const T& v) {
    IT(N)(*this)[i] *= v;
    return *this;
  }
  Vec& operator/=(const Vec& v) {
    IT(N)(*this)[i] /= v[i];
    return *this;
  }
  Vec& operator/=(const T& v) {
    IT(N)(*this)[i] /= v;
    return *this;
  }
 
  Vec operator+(const Vec& v) const { return Vec(*this) += v; }
  Vec operator+(const T& v) const { return Vec(*this) += v; }
  Vec operator-(const Vec& v) const { return Vec(*this) -= v; }
  Vec operator-(const T& v) const { return Vec(*this) -= v; }
  Vec operator*(const Vec& v) const { return Vec(*this) *= v; }
  Vec operator*(const T& v) const { return Vec(*this) *= v; }
  Vec operator/(const Vec& v) const { return Vec(*this) /= v; }
  Vec operator/(const T& v) const { return Vec(*this) /= v; }
  Vec operator-() const { return Vec(*this).negate(); }
  bool operator>(const Vec& v) const { return magSqr() > v.magSqr(); }
  bool operator<(const Vec& v) const { return magSqr() < v.magSqr(); }
 
  template <class T2>
  Vec& set(const Vec<N, T2>& v) {
    IT(N) { (*this)[i] = T(v[i]); }
    return *this;
  }
 
  template <int N2, class T2>
  Vec& set(const Vec<N2, T2>& v) {
    IT(N < N2 ? N : N2) { (*this)[i] = T(v[i]); }
    return *this;
  }
 
  template <class Tv, class Ts>
  Vec& set(const Vec<N - 1, Tv>& v, const Ts& s) {
    (*this)[N - 1] = s;
    return set(v);
  }
 
  Vec& set(const T& v) {
    IT(N) { (*this)[i] = v; }
    return *this;
  }
 
  template <class T2>
  Vec& set(const T2* v) {
    IT(N) { (*this)[i] = T(v[i]); }
    return *this;
  }
 
  template <class T2>
  Vec& set(const T2* v, int stride) {
    IT(N) { (*this)[i] = T(v[i * stride]); }
    return *this;
  }
 
  Vec& set(const T& v1, const T& v2) { return set(v1, v2, v1, v1, v1, v1); }
 
  Vec& set(const T& v1, const T& v2, const T& v3) {
    return set(v1, v2, v3, v1, v1, v1);
  }
 
  Vec& set(const T& v1, const T& v2, const T& v3, const T& v4) {
    return set(v1, v2, v3, v4, v1, v1);
  }
 
  Vec& set(const T& v1, const T& v2, const T& v3, const T& v4, const T& v5) {
    return set(v1, v2, v3, v4, v5, v1);
  }
 
  Vec& set(const T& v1, const T& v2, const T& v3, const T& v4, const T& v5,
           const T& v6) {
    switch (N) {
      default:
        (*this)[5] = v6;
      case 5:
        (*this)[4] = v5;
      case 4:
        (*this)[3] = v4;
      case 3:
        (*this)[2] = v3;
      case 2:
        (*this)[1] = v2;
      case 1:
        (*this)[0] = v1;
    }
    return *this;
  }
 
  Vec& zero() { return set(T(0)); }
 
  //--------------------------------------------------------------------------
  // Linear Operations
 
 
  template <int Dimension>
  Vec by(T shift) const {
    Vec res(*this);
    res[Dimension] += shift;
    return res;
  }
 
  Vec byx(T shift) const { return by<0>(shift); }
 
  Vec byy(T shift) const { return by<1>(shift); }
 
  Vec byz(T shift) const { return by<2>(shift); }
 
  template <class U>
  T dot(const Vec<N, U>& v) const {
    T r = (*this)[0] * v[0];
    for (int i = 1; i < N; ++i) {
      r += (*this)[i] * v[i];
    }
    return r;
  }
 
  T mag() const { return std::sqrt(magSqr()); }
 
  T magSqr() const { return dot(*this); }
 
 
  T norm(const T& p) const {
    T r = std::pow(std::abs((*this)[0]), p);
    for (int i = 1; i < N; ++i) {
      r += std::pow(std::abs((*this)[i]), p);
    }
    return std::pow(r, T(1) / p);
  }
 
  T norm1() const { return sumAbs(); }
 
  T norm2() const { return mag(); }
 
  T product() const {
    T r = (*this)[0];
    for (int i = 1; i < N; ++i) {
      r *= (*this)[i];
    }
    return r;
  }
 
  T sum() const {
    T r = (*this)[0];
    for (int i = 1; i < N; ++i) {
      r += (*this)[i];
    }
    return r;
  }
 
  T sumAbs() const {
    T r = std::abs((*this)[0]);
    for (int i = 1; i < N; ++i) {
      r += std::abs((*this)[i]);
    }
    return r;
  }
 
  Vec& lerp(const Vec& target, T amt) {
    return (*this) += (target - (*this)) * amt;
  }
 
  Vec& mag(T v);
 
  Vec& negate() {
    IT(N) { (*this)[i] = -(*this)[i]; }
    return *this;
  }
 
 
  Vec& normalize(T scale = T(1));
 
 
  Vec normalized(T scale = T(1)) const { return Vec(*this).normalize(scale); }
 
  Vec projection(const Vec& u) const { return dot(u) * u; }
 
 
  Vec rejection(const Vec& u) const { return (*this) - projection(u); }
 
  Vec& reflect(const Vec& u) { return (*this) -= ((T(2) * dot(u)) * u); }
 
 
  Vec& rotate(double angle, int dim1 = 0, int dim2 = 1) {
    double a = cos(angle);
    double b = sin(angle);
    T t = (*this)[dim1];
    T u = (*this)[dim2];
    (*this)[dim1] = t * a - u * b;
    (*this)[dim2] = t * b + u * a;
    return *this;
  }
 
  void print(FILE* out = stdout) const;
};
 
// -----------------------------------------------------------------------------
// The following are functions that either cannot be defined as class methods
// (due to syntax rules or specialization) or simply are not object oriented.
 
// Non-member binary arithmetic operations
template <int N, class T>
inline Vec<N, T> operator+(const T& s, const Vec<N, T>& v) {
  return v + s;
}
 
template <int N, class T>
inline Vec<N, T> operator-(const T& s, const Vec<N, T>& v) {
  return -v + s;
}
 
template <int N, class T>
inline Vec<N, T> operator*(const T& s, const Vec<N, T>& v) {
  return v * s;
}
 
template <int N, class T>
inline Vec<N, T> operator/(const T& s, const Vec<N, T>& v) {
  Vec<N, T> r;
  IT(N) { r[i] = s / v[i]; }
  return r;
}
 
// Specialized vector functions
 
template <int N, class T>
inline T abs(const Vec<N, T>& v) {
  return v.mag();
}
 
template <int N1, class T1, int N2, class T2>
inline Vec<N1 + N2, T1> concat(const Vec<N1, T1>& a, const Vec<N2, T2>& b) {
  Vec<N1 + N2, T1> r;
  r.set(a.elems());
  for (int i = 0; i < N2; ++i) r[i + N1] = T1(b[i]);
  return r;
}
 
template <int M, int N, class T>
inline Vec<M, T> sub(const Vec<N, T>& v, int begin = 0) {
  return v.template sub<M>(begin);
}
 
template <class T>
inline void cross(Vec<3, T>& r, const Vec<3, T>& a, const Vec<3, T>& b) {
  //  r[0] = a[1]*b[2] - a[2]*b[1];
  //  r[1] = a[2]*b[0] - a[0]*b[2];
  //  r[2] = a[0]*b[1] - a[1]*b[0];
  r = a.cross(b);
}
 
template <class T>
inline Vec<3, T> cross(const Vec<3, T>& a, const Vec<3, T>& b) {
  Vec<3, T> r;
  cross(r, a, b);
  return r;
}
 
template <class T>
inline void cross(Vec<3, T>& r, const Vec<4, T>& a, const Vec<4, T>& b) {
  //  r[0] = a[1]*b[2] - a[2]*b[1];
  //  r[1] = a[2]*b[0] - a[0]*b[2];
  //  r[2] = a[0]*b[1] - a[1]*b[0];
  r = a.cross(b);
}
 
template <class T>
inline Vec<3, T> cross(const Vec<4, T>& a, const Vec<4, T>& b) {
  Vec<3, T> r;
  cross(r, a, b);
  return r;
}
 
 
template <class T>
void rotate(Vec<3, T>& vec, const Vec<3, T>& normal, double cosAng,
            double sinAng) {
  T c = cosAng;
  T s = sinAng;
 
  // Rodrigues' rotation formula:
  vec = vec * c + cross(normal, vec) * s +
        normal * (normal.dot(vec) * (T(1) - c));
}
 
 
template <class T>
void rotate(Vec<3, T>& vec, const Vec<3, T>& normal, double angle) {
  rotate(vec, normal, cos(angle), sin(angle));
}
 
template <int N, class T>
inline T angle(const Vec<N, T>& a, const Vec<N, T>& b) {
  T cosAng = a.dot(b) / sqrt(a.magSqr() * b.magSqr());
  if (cosAng >= T(1)) {
    return T(0);
  } else if (cosAng <= T(-1)) {
    return T(M_PI);
  }
  return std::acos(cosAng);
}
 
template <int N, class T>
inline void centroid3(Vec<N, T>& c, const Vec<N, T>& p1, const Vec<N, T>& p2,
                      const Vec<N, T>& p3) {
  static const T _1_3 = T(1) / T(3);
  c = (p1 + p2 + p3) * _1_3;
}
 
template <int N, class T>
Vec<N, T> closestPointOnLineSegment(const Vec<N, T>& a, const Vec<N, T>& b,
                                    const Vec<N, T>& p) {
  auto ab = b - a;
  auto dot = (p - a).dot(ab);  // projection of ap onto ab
  auto magAB = ab.magSqr();
  auto frac =
      dot / magAB;  // normalized distance along ab from a to the closest point
 
  // check if p projection is beyond endpoints of ab
  if (frac < 0.) return a;
  if (frac > 1.) return b;
 
  return a + ab * frac;
}
 
template <int N, class T, class U>
inline T dist(const Vec<N, T>& a, const Vec<N, U>& b) {
  return (a - b).mag();
}
 
template <int N, class T>
inline Vec<N, T> lerp(const Vec<N, T>& input, const Vec<N, T>& target, T amt) {
  return Vec<N, T>(input).lerp(target, amt);
}
 
template <class T>
inline void normal(Vec<3, T>& n, const Vec<3, T>& p1, const Vec<3, T>& p2,
                   const Vec<3, T>& p3) {
  cross(n, p2 - p1, p3 - p1);
  n.normalize();
}
 
template <int N, class T>
inline Vec<N, T> min(const Vec<N, T>& a, const Vec<N, T>& b) {
  Vec<N, T> r;
  IT(N) { r[i] = a[i] > b[i] ? b[i] : a[i]; }
  return r;
}
 
template <int N, class T>
inline Vec<N, T> max(const Vec<N, T>& a, const Vec<N, T>& b) {
  Vec<N, T> r;
  IT(N) { r[i] = a[i] < b[i] ? b[i] : a[i]; }
  return r;
}
 
// Implementation --------------------------------------------------------------
 
template <int N, class T>
Vec<N, T>& Vec<N, T>::mag(T v) {
  T m = mag();
  if (m > T(1e-20)) {
    (*this) *= (v / m);
  } else {
    set(T(0));
    (*this)[0] = v;
  }
  return *this;
}
 
template <int N, class T>
Vec<N, T>& Vec<N, T>::normalize(T scale) {
  return mag(scale);
}
 
template <typename T>
const char* typeString();
#define TypeString(A)                  \
  template <>                          \
  inline const char* typeString<A>() { \
    return #A;                         \
  }
TypeString(char) TypeString(unsigned char) TypeString(int)
    TypeString(unsigned int) TypeString(float) TypeString(double)
        TypeString(long double)
#undef TypeString
 
            template <int N, class T>
            void Vec<N, T>::print(FILE* out) const {
  fprintf(out, "{");
  if (size()) {
    fprintf(out, "%g", (double)((*this)[0]));
    for (int i = 1; i < N; ++i) fprintf(out, ", %g", (double)((*this)[i]));
  }
  fprintf(out, "}");
}
 
// Pretty-printing by Matt:
//
template <int N, class T>
std::ostream& operator<<(std::ostream& out, const Vec<N, T>& v) {
  //  out << "Vec<" << N << "," << typeString<T>() << "> = {";
  //  if(v.size()){
  //    out << v[0];
  //    for (int i = 1; i < N; ++i)
  //      out << ", " << v[i];
  //  }
  //  out << "}" << std::endl;
  out << "{";
  if (v.size()) {
    out << v[0];
    for (int i = 1; i < N; ++i) out << ", " << v[i];
  }
  out << "}";
  return out;
}
 
#undef IT
}  // namespace al
#endif