Mat3D.h

/*
 FILE: Mat3D.h
 ABOUT: C++ 3D Martix Tool
 AUTHOR: Kevin Gliewe
 EMAIL: [email protected]
 DATE: 2014-07-5
 */


#pragma once

#include <math.h>
#include <string.h>
#include "Vector3D.h"


class Mat3D
{
public:

	static const Mat3D ZERO;
	static const Mat3D UNIT;

    Vector3D x_axis; //!< The 1st row, which is usually used as the x-axis.
    Vector3D y_axis; //!< The 2nd row, which is usually used as the y-axis.
    Vector3D z_axis; //!< The 3rd row, which is usually used as the z-axis.
    Vector3D trans; //!< The 4th row, which is usually used as the translation.

    Mat3D() {
    	this->x_axis = Vector3D();
    	this->y_axis = Vector3D();
    	this->z_axis = Vector3D();
    	this->trans = Vector3D();
    }

    Mat3D(Vector3D x_axis_, Vector3D y_axis_, Vector3D z_axis_, Vector3D trans_){
		x_axis = x_axis_;
		y_axis = y_axis_;
		z_axis = z_axis_;
		trans = trans_;
    }

    Mat3D(Mat3D* other){
		x_axis = other->x_axis.copy();
		y_axis = other->y_axis.copy();
		z_axis = other->z_axis.copy();
		trans = other->trans.copy();
    }

    float getX() { return this->trans.X; }
    float getY() { return this->trans.Y; }
    float getZ() { return this->trans.Z; }

    void setX(float val) { this->trans.X = val; }
    void setY(float val) { this->trans.Y = val; }
    void setZ(float val) { this->trans.Z = val; }

    float getRX() {
    	/*Vector3D is = this->y_axis+this->z_axis;
    	Vector3D from = Vector3D(0,1,1); // y+z
    	return is.getAngleXRad(from);*/
    	return this->z_axis.getAngleXRad(Vector3D(0,0,1));
    }

    float getRY() {
    	/*Vector3D is = this->x_axis+this->z_axis;
    	Vector3D from = Vector3D(1,0,1); // x+z
    	return is.getAngleYRad(from);*/
    	return this->y_axis.getAngleYRad(Vector3D(1,0,0));
    }

    float getRZ() {
    	/*Vector3D is = this->x_axis+this->y_axis;
    	Vector3D from = Vector3D(1,1,0); // x+y
    	return is.getAngleZRad(from);*/
    	return this->x_axis.getAngleZRad(Vector3D(0,1,0));
    }

    void set(Vector3D x_axis_, Vector3D y_axis_, Vector3D z_axis_, Vector3D trans_){
		x_axis = x_axis_;
		y_axis = y_axis_;
		z_axis = z_axis_;
		trans = trans_;
    }

    void set(Mat3D mat){
		x_axis = mat.x_axis;
		y_axis = mat.y_axis;
		z_axis = mat.z_axis;
		trans = mat.trans;
    }

    bool isUnit(){
        return ( //
            x_axis.X == 1.0f && x_axis.Y == 0.0f && x_axis.Z == 0.0f && //
            y_axis.X == 0.0f && y_axis.Y == 1.0f && y_axis.Z == 0.0f && //
            z_axis.X == 0.0f && z_axis.Y == 0.0f && z_axis.Z == 1.0f && //
            trans.X == 0.0f && trans.Y == 0.0f && trans.Z == 0.0f);
    }

    /*void rotateX(float deg) {
        float sinf = sin(deg);
        float cosf = cos(deg);

        Mat3D newM = Mat3D(
        		Vector3D::X_UNIT,
        		Vector3D(0.0f, cosf, sinf),
        		Vector3D(0.0f, -sinf, cosf),
        		Vector3D::ZERO);

        newM = newM.toGlobalFrom(*this);

        this->set(newM);
    }

    void rotateY(float deg) {
    	float sinf = sin(deg);
    	float cosf = cos(deg);

    	Mat3D newM = Mat3D(
    			Vector3D(cosf, 0.0f, -sinf),
    			Vector3D::Y_UNIT,
    			Vector3D(sinf, 0.0f, cosf),
    			Vector3D::ZERO);

    	newM = newM.toGlobalFrom(*this);

    	this->set(newM);
    }

    void rotateZ(float deg) {
       	float sinf = sin(deg);
		float cosf = cos(deg);

		Mat3D newM = Mat3D(
				Vector3D(cosf, sinf, 0.0f),
				Vector3D(-sinf, cosf, 0.0f),
				Vector3D::Z_UNIT,
				Vector3D::ZERO);

		newM = newM.toGlobalFrom(*this);

		this->set(newM);
    }*/

    void scale(float x_scale, float y_scale, float z_scale = 1.0f){
    	this->x_axis = this->x_axis*x_scale;
    	this->y_axis = this->y_axis*y_scale;
    	this->z_axis = this->z_axis*z_scale;
    }

    void scale(Vector3D scale){
    	this->x_axis = this->x_axis*scale.X;
    	this->y_axis = this->y_axis*scale.Y;
    	this->z_axis = this->z_axis*scale.Z;
    }

    void translate(float x, float y, float z = 0.0f){
    	this->translate(Vector3D(x,y,z));
    }

    void translate(Vector3D trans){
    	this->trans = this->trans + trans;
    }

    Mat3D copy() {
    	return Mat3D(this);
    }

    Mat3D orthonormal(){
    	Vector3D new_z_axis = z_axis.unit();
    	Vector3D new_x_axis = y_axis.cross(z_axis).unit();
    	Vector3D new_y_axis = new_z_axis.cross(new_x_axis);

        return Mat3D(new_x_axis, new_y_axis, new_z_axis, trans);
    }

    Mat3D toLocalOf(Mat3D mat) {
        float rsq_xa = 1.0f / mat.x_axis.sqLength();
        float rsq_ya = 1.0f / mat.y_axis.sqLength();
        float rsq_za = 1.0f / mat.z_axis.sqLength();
        Vector3D vec = trans - mat.trans;

        return Mat3D( //
			Vector3D(x_axis.dot(mat.x_axis) * rsq_xa, x_axis.dot(mat.y_axis) * rsq_ya, x_axis.dot(mat.z_axis) * rsq_za), //
			Vector3D(y_axis.dot(mat.x_axis) * rsq_xa, y_axis.dot(mat.y_axis) * rsq_ya, y_axis.dot(mat.z_axis) * rsq_za), //
			Vector3D(z_axis.dot(mat.x_axis) * rsq_xa, z_axis.dot(mat.y_axis) * rsq_ya, z_axis.dot(mat.z_axis) * rsq_za), //
			Vector3D(vec.dot(mat.x_axis) * rsq_xa, vec.dot(mat.y_axis) * rsq_ya, vec.dot(mat.z_axis) * rsq_za));
    }

    Mat3D toGlobalFrom(Mat3D mat) {
        return Mat3D( //
            toGlobalVFrom_noTrans(x_axis, mat), //
            toGlobalVFrom_noTrans(y_axis, mat), //
            toGlobalVFrom_noTrans(z_axis, mat), //
            toGlobalVFrom(trans, mat));
    }

    Mat3D toLocalOf_noTrans(Mat3D mat){
        float rsq_xa = 1.0f / mat.x_axis.sqLength();
        float rsq_ya = 1.0f / mat.y_axis.sqLength();
        float rsq_za = 1.0f / mat.z_axis.sqLength();

        return Mat3D( //
			Vector3D(x_axis.dot(mat.x_axis) * rsq_xa, x_axis.dot(mat.y_axis) * rsq_ya, x_axis.dot(mat.z_axis) * rsq_za), //
			Vector3D(y_axis.dot(mat.x_axis) * rsq_xa, y_axis.dot(mat.y_axis) * rsq_ya, y_axis.dot(mat.z_axis) * rsq_za), //
			Vector3D(z_axis.dot(mat.x_axis) * rsq_xa, z_axis.dot(mat.y_axis) * rsq_ya, z_axis.dot(mat.z_axis) * rsq_za), //
			Vector3D::ZERO);
    }

    Mat3D toGlobalFrom_noTrans(Mat3D mat){
        return Mat3D( //
            toGlobalVFrom_noTrans(x_axis, mat), //
            toGlobalVFrom_noTrans(y_axis, mat), //
            toGlobalVFrom_noTrans(z_axis, mat), //
            Vector3D::ZERO);
    }

    std::string toString() {
        std::stringstream strs;
        strs << "|" << this->x_axis.X << "\t," << this->y_axis.X << "\t," << this->z_axis.X << "\t," << this->trans.X <<"\t|\n";
        strs << "|" << this->x_axis.Y << "\t," << this->y_axis.Y << "\t," << this->z_axis.Y << "\t," << this->trans.Y <<"\t|\n";
        strs << "|" << this->x_axis.Z << "\t," << this->y_axis.Z << "\t," << this->z_axis.Z << "\t," << this->trans.Z <<"\t|";
        std::string str = strs.str();
        return str;
    }

    void toR32x16(float* r32x16) {
        r32x16[0] = x_axis.X;
        r32x16[1] = x_axis.Y;
        r32x16[2] = x_axis.Z;
        r32x16[3] = 0.0f;

        r32x16[4] = y_axis.X;
        r32x16[5] = y_axis.Y;
        r32x16[6] = y_axis.Z;
        r32x16[7] = 0.0f;

        r32x16[8] = z_axis.X;
        r32x16[9] = z_axis.Y;
        r32x16[10] = z_axis.Z;
        r32x16[11] = 0.0f;

        r32x16[12] = trans.X;
        r32x16[13] = trans.Y;
        r32x16[14] = trans.Z;
        r32x16[15] = 1.0f;
    }

    static Mat3D fromR32x16(const float* r32x16) {
        return Mat3D( //
			Vector3D(r32x16[0], r32x16[1], r32x16[2]), //
			Vector3D(r32x16[4], r32x16[5], r32x16[6]), //
			Vector3D(r32x16[8], r32x16[9], r32x16[10]), //
			Vector3D(r32x16[12], r32x16[13], r32x16[14]));
    }

    /* VECTOR3D */
    static Vector3D toLocalVOf(Vector3D v, Mat3D mat)
    {
    	Vector3D vec = v - mat.trans;

        return Vector3D( //
            vec.dot(mat.x_axis) / mat.x_axis.sqLength(), //
            vec.dot(mat.y_axis) / mat.y_axis.sqLength(), //
            vec.dot(mat.z_axis) / mat.z_axis.sqLength());
    }


    static Vector3D toGlobalVFrom(Vector3D v, Mat3D mat)
    {
        return mat.x_axis * v.X + mat.y_axis * v.Y + mat.z_axis * v.Z + mat.trans;
    }


    static Vector3D toLocalVOf_noTrans(Vector3D v, Mat3D mat)
    {
        return Vector3D( //
            v.dot(mat.x_axis) / mat.x_axis.sqLength(), //
            v.dot(mat.y_axis) / mat.y_axis.sqLength(), //
            v.dot(mat.z_axis) / mat.z_axis.sqLength());
    }
    static Vector3D toGlobalVFrom_noTrans(Vector3D v, Mat3D mat) {
    	return mat.x_axis * v.X + mat.y_axis * v.Y + mat.z_axis * v.Z;
    }
};

const Mat3D Mat3D::ZERO(Vector3D::ZERO, Vector3D::ZERO, Vector3D::ZERO, Vector3D::ZERO);
const Mat3D Mat3D::UNIT(Vector3D::X_UNIT, Vector3D::Y_UNIT, Vector3D::Z_UNIT, Vector3D::ZERO);