#pragma once
#include <math.h>
#include <assert.h>
#include <vector>
using namespace std;
//-----------------------------------------------------------------------------
// class vec2d defines a 2D vector
class vec2d
{
public:
vec2d() { x = y = 0.0; }
vec2d(double X, double Y) { x = X; y = Y; }
vec2d operator - () { return vec2d(-x, -y); }
vec2d operator - (const vec2d& r) { return vec2d(x - r.x, y - r.y); }
vec2d operator + (const vec2d& r) { return vec2d(x + r.x, y + r.y); }
vec2d operator * (double g) { return vec2d(x*g, y*g); }
double operator * (const vec2d& r) { return (x*r.x + y*r.y); }
double norm() { return sqrt(x*x + y*y); }
double unit() { double R = sqrt(x*x + y*y); if (R != 0) {x /= R; y/= R; }; return R; }
bool operator == (const vec2d& r) const { return (x==r.x)&&(y==r.y); }
public:
double x, y;
};
//-----------------------------------------------------------------------------
// class vec3d defines a 3D vector
//
class vec3d
{
public:
vec3d() { x = y = z = 0; }
vec3d(double rx, double ry, double rz) { x = rx; y = ry; z = rz; }
vec3d(const vec2d& r) { x = r.x; y = r.y; z = 0; }
vec3d operator + (const vec3d& v) const { return vec3d( x + v.x, y + v.y, z + v.z); }
vec3d operator - (const vec3d& v) const { return vec3d( x - v.x, y - v.y, z - v.z); }
vec3d operator ^ (const vec3d& v) const
{
return vec3d( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
}
double operator * (const vec3d& v) const { return (x*v.x + y*v.y + z*v.z); }
vec3d operator * (const double g) const { return vec3d(x*g, y*g, z*g); }
vec3d operator / (const double g) const { return vec3d(x/g, y/g, z/g); }
const vec3d& operator += (const vec3d& v) { x += v.x; y += v.y; z += v.z; return (*this); }
const vec3d& operator -= (const vec3d& v) { x -= v.x; y -= v.y; z -= v.z; return (*this); }
const vec3d& operator /= (const double f) { x /= f; y /= f; z /= f; return (*this); }
const vec3d& operator /= (const int n) { x /= n; y /= n; z /= n; return (*this); }
const vec3d& operator *= (const double f) { x*=f; y*=f; z*=f; return (*this); }
vec3d operator - () const { return vec3d(-x, -y, -z); }
double Length() const { return (double) sqrt(x*x + y*y + z*z); }
double SqrLength() const { return x*x + y*y + z*z; }
vec3d& Normalize()
{
double L = Length();
if (L != 0) { x /= L; y /= L; z /= L; }
return (*this);
}
public:
double x, y, z;
};
// vec6d
class vec6d
{
public:
vec6d() { x = y = z = xy = yz = xz = 0; }
public:
double x, y, z;
double xy, yz, xz;
};
// mat3d
class mat3d
{
public:
mat3d()
{
zero();
}
mat3d(double a00, double a01, double a02, double a10, double a11, double a12, double a20, double a21, double a22);
double* operator [] (int i) { return m_data[i]; }
double& operator () (int i, int j) { return m_data[i][j]; }
mat3d operator * (mat3d& m)
{
mat3d a;
int k;
for (k=0; k<3; k++)
{
a[0][0] += m_data[0][k]*m[k][0]; a[0][1] += m_data[0][k]*m[k][1]; a[0][2] += m_data[0][k]*m[k][2];
a[1][0] += m_data[1][k]*m[k][0]; a[1][1] += m_data[1][k]*m[k][1]; a[1][2] += m_data[1][k]*m[k][2];
a[2][0] += m_data[2][k]*m[k][0]; a[2][1] += m_data[2][k]*m[k][1]; a[2][2] += m_data[2][k]*m[k][2];
}
return a;
}
mat3d& operator *=(mat3d& m)
{
mat3d a;
int k;
for (k=0; k<3; k++)
{
a[0][0] += m_data[0][k]*m[k][0]; a[0][1] += m_data[0][k]*m[k][1]; a[0][2] += m_data[0][k]*m[k][2];
a[1][0] += m_data[1][k]*m[k][0]; a[1][1] += m_data[1][k]*m[k][1]; a[1][2] += m_data[1][k]*m[k][2];
a[2][0] += m_data[2][k]*m[k][0]; a[2][1] += m_data[2][k]*m[k][1]; a[2][2] += m_data[2][k]*m[k][2];
}
m_data[0][0] = a.m_data[0][0]; m_data[0][1] = a.m_data[0][1]; m_data[0][2] = a.m_data[0][2];
m_data[1][0] = a.m_data[1][0]; m_data[1][1] = a.m_data[1][1]; m_data[1][2] = a.m_data[1][2];
m_data[2][0] = a.m_data[2][0]; m_data[2][1] = a.m_data[2][1]; m_data[2][2] = a.m_data[2][2];
return (*this);
}
mat3d& operator +=(mat3d& m)
{
m_data[0][0] += m[0][0]; m_data[0][1] += m[0][1]; m_data[0][2] += m[0][2];
m_data[1][0] += m[1][0]; m_data[1][1] += m[1][1]; m_data[1][2] += m[1][2];
m_data[2][0] += m[2][0]; m_data[2][1] += m[2][1]; m_data[2][2] += m[2][2];
return (*this);
}
mat3d& operator -=(mat3d& m)
{
m_data[0][0] -= m[0][0]; m_data[0][1] -= m[0][1]; m_data[0][2] -= m[0][2];
m_data[1][0] -= m[1][0]; m_data[1][1] -= m[1][1]; m_data[1][2] -= m[1][2];
m_data[2][0] -= m[2][0]; m_data[2][1] -= m[2][1]; m_data[2][2] -= m[2][2];
return (*this);
}
mat3d& operator /=(const double f)
{
m_data[0][0] /= f; m_data[0][1] /= f; m_data[0][2] /= f;
m_data[1][0] /= f; m_data[1][1] /= f; m_data[1][2] /= f;
m_data[2][0] /= f; m_data[2][1] /= f; m_data[2][2] /= f;
return (*this);
}
vec3d operator * (vec3d b)
{
vec3d r;
r.x = m_data[0][0]*b.x + m_data[0][1]*b.y + m_data[0][2]*b.z;
r.y = m_data[1][0]*b.x + m_data[1][1]*b.y + m_data[1][2]*b.z;
r.z = m_data[2][0]*b.x + m_data[2][1]*b.y + m_data[2][2]*b.z;
return r;
}
double det() const
{
double det = 0;
det += m_data[0][0]*m_data[1][1]*m_data[2][2];
det += m_data[0][1]*m_data[1][2]*m_data[2][0];
det += m_data[0][2]*m_data[1][0]*m_data[2][1];
det -= m_data[0][2]*m_data[1][1]*m_data[2][0];
det -= m_data[0][1]*m_data[1][0]*m_data[2][2];
det -= m_data[0][0]*m_data[1][2]*m_data[2][1];
return det;
}
double Invert();
mat3d inverse() const;
void zero()
{
m_data[0][0] = m_data[0][1] = m_data[0][2] = 0;
m_data[1][0] = m_data[1][1] = m_data[1][2] = 0;
m_data[2][0] = m_data[2][1] = m_data[2][2] = 0;
}
void unit()
{
m_data[0][0] = m_data[1][1] = m_data[2][2] = 1;
m_data[0][1] = m_data[0][2] = m_data[1][2] = 0;
m_data[1][0] = m_data[2][0] = m_data[2][1] = 0;
}
mat3d transpose();
protected:
double m_data[3][3];
};
// matrix
class matrix
{
public:
matrix(int r, int c);
~matrix() { delete [] d; }
void zero();
double* operator [] (int i) { return d + i*m_nc; }
double& operator () (int i, int j) { return d[i*m_nc + j]; }
bool solve(vector<double>& x, vector<double>& b);
bool lsq_solve(vector<double>& x, vector<double>& b);
bool eigen_vectors(matrix& Eigen,vector<double>& eigen_values);
int Rows() { return m_nr; }
void mult_transpose(vector<double>& x, vector<double>& y);
void mult_transpose_self(matrix& AAt);
private:
double* d;
int m_nr, m_nc;
int m_ne;
};
// quatd
class quatd
{
public:
// constructors
quatd () { x = y = z = 0; w = 1; }
quatd( const double angle, vec3d v)
{
w = (double) cos(angle * 0.5);
double sina = (double) sin(angle * 0.5);
v.Normalize();
x = v.x*sina;
y = v.y*sina;
z = v.z*sina;
}
quatd (vec3d v1, vec3d v2)
{
vec3d n = v1^v2;
n.Normalize();
double d = v1*v2;
double sina = (double) sqrt((1.0-d)*0.5);
double cosa = (double) sqrt((1.0+d)*0.5);
w = cosa;
x = n.x*sina;
y = n.y*sina;
z = n.z*sina;
}
quatd(const double qx, const double qy, const double qz, const double qw = 1.0)
{
w = qw;
x = qx;
y = qy;
z = qz;
}
bool operator != (const quatd& q) { return ((x!=q.x) || (y!=q.y) || (z!=q.z) || (w!=q.w)); }
quatd operator - () { return quatd(-x, -y, -z, -w); }
// addition and substraction
quatd operator + (const quatd& q) const
{
return quatd(x + q.x, y + q.y, z + q.z, w + q.w);
}
quatd operator - (const quatd& q) const
{
return quatd(x - q.x, y - q.y, z - q.z, w - q.w);
}
quatd& operator += (const quatd& q)
{
x += q.x;
y += q.y;
z += q.z;
w += q.w;
return *this;
}
quatd& operator -= (const quatd& q)
{
x -= q.x;
y -= q.y;
z -= q.z;
w -= q.w;
return *this;
}
// multiplication
quatd operator * (const quatd& q) const
{
double qw = w*q.w - x*q.x - y*q.y - z*q.z;
double qx = w*q.x + x*q.w + y*q.z - z*q.y;
double qy = w*q.y + y*q.w + z*q.x - x*q.z;
double qz = w*q.z + z*q.w + x*q.y - y*q.x;
return quatd(qx, qy, qz, qw);
}
quatd& operator *= (const quatd& q)
{
double qw = w*q.w - x*q.x - y*q.y - z*q.z;
double qx = w*q.x + x*q.w + y*q.z - z*q.y;
double qy = w*q.y + y*q.w + z*q.x - x*q.z;
double qz = w*q.z + z*q.w + x*q.y - y*q.x;
x = qx;
y = qy;
z = qz;
w = qw;
return *this;
}
quatd operator*(const double a) const
{
return quatd(x*a, y*a, z*a, w*a);
}
// division
quatd operator / (const double a) const
{
return quatd(x/a, y/a, z/a, w/a);
}
quatd& operator /= (const double a)
{
x /= a;
y /= a;
z /= a;
w /= a;
return *this;
}
// Special ops
quatd Conjugate() const { return quatd(-x, -y, -z, w); }
double Norm() const { return w*w + x*x + y*y + z*z; }
void MakeUnit()
{
double N = (double) sqrt(w*w + x*x + y*y + z*z);
if (N != 0)
{
x /= N;
y /= N;
z /= N;
w /= N;
}
else w = 1.f;
}
quatd Inverse() const
{
double N = w*w + x*x + y*y + z*z;
return quatd(-x/N, -y/N, -z/N, w/N);
}
double DotProduct(const quatd& q) const
{
return w*q.w + x*q.x + y*q.y + z*q.z;
}
vec3d GetVector() const
{
return vec3d(x, y, z).Normalize();
}
double GetAngle() const
{
return (double)(acos(w)*2.0);
}
/* quatd& MultiplyAngle(double fa)
{
double angle = fa*acos(w)*2.0;
w = cos(angle * 0.5);
double sina = sin(angle * 0.5);
x *= sina;
y *= sina;
z *= sina;
}
*/
// use only when *this is unit vector
void RotateVector(vec3d& v) const
{
if ((w == 0) || ((x==0) && (y==0) && (z==0))) return;
// v*q^-1
double qw = v.x*x + v.y*y + v.z*z;
double qx = v.x*w - v.y*z + v.z*y;
double qy = v.y*w - v.z*x + v.x*z;
double qz = v.z*w - v.x*y + v.y*x;
// q* (v* q^-1)
v.x = (double) (w*qx + x*qw + y*qz - z*qy);
v.y = (double) (w*qy + y*qw + z*qx - x*qz);
v.z = (double) (w*qz + z*qw + x*qy - y*qx);
}
// use only when *this is unit vector
vec3d operator * (const vec3d& r)
{
vec3d n = r;
// v*q^-1
double qw = n.x*x + n.y*y + n.z*z;
double qx = n.x*w - n.y*z + n.z*y;
double qy = n.y*w - n.z*x + n.x*z;
double qz = n.z*w - n.x*y + n.y*x;
// q* (v* q^-1)
n.x = (w*qx + x*qw + y*qz - z*qy);
n.y = (w*qy + y*qw + z*qx - x*qz);
n.z = (w*qz + z*qw + x*qy - y*qx);
return n;
}
mat3d operator * (mat3d m)
{
mat3d a;
double qw, qx, qy, qz;
for (int i=0; i<3; ++i)
{
// v*q^-1
qw = m[0][i]*x + m[1][i]*y + m[2][i]*z;
qx = m[0][i]*w - m[1][i]*z + m[2][i]*y;
qy = m[1][i]*w - m[2][i]*x + m[0][i]*z;
qz = m[2][i]*w - m[0][i]*y + m[1][i]*x;
// q* (v* q^-1)
a[0][i] = (w*qx + x*qw + y*qz - z*qy);
a[1][i] = (w*qy + y*qw + z*qx - x*qz);
a[2][i] = (w*qz + z*qw + x*qy - y*qx);
}
return a;
}
void RotateVectorP(double* v, double* r) const
{
static double fx, fy, fz, fw;
static double qw, qx, qy, qz;
fx = (double) x;
fy = (double) y;
fz = (double) z;
fw = (double) w;
qw = v[0]*fx + v[1]*fy + v[2]*fz;
qx = v[0]*fw - v[1]*fz + v[2]*fy;
qy = v[1]*fw - v[2]*fx + v[0]*fz;
qz = v[2]*fw - v[0]*fy + v[1]*fx;
r[0] = (double) (fw*qx + fx*qw + fy*qz - fz*qy);
r[1] = (double) (fw*qy + fy*qw + fz*qx - fx*qz);
r[2] = (double) (fw*qz + fz*qw + fx*qy - fy*qx);
}
static double dot(quatd &q1, quatd &q2)
{ return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w; }
static quatd lerp(quatd &q1, quatd &q2, double t)
{ quatd q = (q1*(1-t) + q2*t); q.MakeUnit(); return q; }
static quatd slerp(quatd &q1, quatd &q2, double t) ;
public:
double x, y, z;
double w;
};
inline quatd operator * (const double a, const quatd& q)
{
return q*a;
}
typedef unsigned char uchar;
class GLCOLOR
{
public:
uchar a, b, g, r;
public:
GLCOLOR() : a(255), b(0), g(0), r(0){}
GLCOLOR(uchar ur, uchar ug, uchar ub, uchar ua = 255)
{
r = ur; g = ug; b = ub; a = ua;
}
GLCOLOR operator * (double f)
{
return GLCOLOR((uchar) (r*f), (uchar) (g*f), (uchar) (b*f));
}
GLCOLOR operator + (GLCOLOR& c)
{
return GLCOLOR(r+c.r, g+c.g, b+c.b);
}
};