Skip to content

Libs/Mesh/PreviewMeshQC/math3d.h

Classes

Name
class vec2d
class vec3d
class vec6d
class mat3d
class matrix
class quatd
class GLCOLOR

Types

Name
typedef unsigned char uchar

Functions

Name
quatd operator*(const double a, const quatd & q)

Types Documentation

typedef uchar

typedef unsigned char uchar;

Functions Documentation

function operator*

inline quatd operator*(
    const double a,
    const quatd & q
)

Source code

#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);
    }
};

Updated on 2022-07-23 at 17:50:04 -0600