Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <iostream>
- #include <cmath>
- #include <cstring>
- using namespace std;
- #define M_PI 3.14159265358979323846 /* pi */
- // matrix.c
- void gl_print_matrix(const float *m);
- #define X _v[0]
- #define Y _v[1]
- #define Z _v[2]
- #define W _v[3]
- // Matrix & Vertex
- class Vector3 {
- public:
- Vector3() { }
- Vector3(float x, float y, float z) {
- X = x;
- Y = y;
- Z = z;
- }
- void normalize();
- float getLength() const { return sqrt(X * X + Y * Y + Z * Z); }
- bool operator==(const Vector3 &other) const {
- return X == other.X && Y == other.Y && Z == other.Z;
- }
- bool operator!=(const Vector3 &other) const {
- return X != other.X || Y != other.Y || Z != other.Z;
- }
- Vector3 operator-() const {
- return Vector3(-X, -Y, -Z);
- }
- Vector3 operator*(float factor) const {
- return Vector3(X * factor, Y * factor, Z * factor);
- }
- Vector3 operator+(const Vector3 &other) const {
- return Vector3(X + other.X, Y + other.Y, Z + other.Z);
- }
- Vector3 operator-(const Vector3 &other) const {
- return Vector3(X - other.X, Y - other.Y, Z - other.Z);
- }
- Vector3 &operator*=(float factor) {
- X *= factor;
- Y *= factor;
- Z *= factor;
- return *this;
- }
- Vector3 &operator+=(float value) {
- X += value;
- Y += value;
- Z += value;
- return *this;
- }
- Vector3 &operator-=(float value) {
- X -= value;
- Y -= value;
- Z -= value;
- return *this;
- }
- float _v[3];
- };
- class Vector4 {
- public:
- Vector4() { }
- Vector4(const Vector3 &vec, float w);
- Vector4(float x, float y, float z, float w) {
- X = x;
- Y = y;
- Z = z;
- W = w;
- }
- bool operator==(const Vector4 &other) const {
- return X == other.X && Y == other.Y && Z == other.Z && W == other.W;
- }
- bool operator!=(const Vector4 &other) const {
- return X != other.X || Y != other.Y || Z != other.Z || W != other.W;
- }
- Vector4 operator-() const {
- return Vector4(-X, -Y, -Z, -W);
- }
- Vector4 operator*(float factor) const {
- return Vector4(X * factor, Y * factor, Z * factor,W * factor);
- }
- Vector4 operator+(const Vector4 &other) const {
- return Vector4(X + other.X, Y + other.Y, Z + other.Z, W + other.W);
- }
- Vector4 operator-(const Vector4 &other) const {
- return Vector4(X - other.X, Y - other.Y, Z - other.Z, W - other.W);
- }
- Vector4 &operator*=(float factor) {
- X *= factor;
- Y *= factor;
- Z *= factor;
- W *= factor;
- return *this;
- }
- Vector4 &operator+=(float value) {
- X += value;
- Y += value;
- Z += value;
- W += value;
- return *this;
- }
- Vector4 &operator-=(float value) {
- X -= value;
- Y -= value;
- Z -= value;
- W -= value;
- return *this;
- }
- float _v[4];
- };
- class Matrix4 {
- public:
- Matrix4() { }
- bool isIdentity() const;
- inline Matrix4 operator+(const Matrix4 &b) const {
- Matrix4 result;
- for (int i = 0; i < 4; i++) {
- for (int j = 0; j < 4; j++) {
- result._m[i][j] = _m[i][j] + b._m[i][j];
- }
- }
- return result;
- }
- inline Matrix4 operator-(const Matrix4 &b) const {
- Matrix4 result;
- for (int i = 0; i < 4; i++) {
- for (int j = 0; j < 4; j++) {
- result._m[i][j] = _m[i][j] - b._m[i][j];
- }
- }
- return result;
- }
- inline Matrix4 operator*(const Matrix4 &b) const {
- Matrix4 result;
- for (int i = 0; i < 4; i++) {
- for (int j = 0; j < 4; j++) {
- float s = 0.0;
- for (int k = 0; k < 4; k++)
- s += _m[i][k] * b._m[k][j];
- result._m[i][j] = s;
- }
- }
- return result;
- }
- inline Matrix4 &operator*=(const Matrix4 &b) {
- Matrix4 a = *this;
- float s;
- for (int i = 0; i < 4; i++) {
- for (int j = 0; j < 4; j++) {
- s = 0.0;
- for (int k = 0; k < 4; k++)
- s += a._m[i][k] * b._m[k][j];
- this->_m[i][j] = s;
- }
- }
- return *this;
- }
- void scale(float x, float y, float z);
- void translate(float x, float y, float z);
- void identity();
- void rotation(float t, int);
- void invert();
- void transpose();
- Matrix4 inverseOrtho() const;
- Matrix4 getCopy();
- Matrix4 transpose() const;
- Matrix4 inverse() const;
- static Matrix4 frustum(float left, float right, float bottom, float top, float nearp, float farp);
- float _m[4][4];
- };
- Matrix4 Matrix4::getCopy() {
- return *this;
- }
- int MatrixInverse(float *m) {
- double inv[16];
- inv[0] = m[5] * m[10] * m[15] -
- m[5] * m[11] * m[14] -
- m[9] * m[6] * m[15] +
- m[9] * m[7] * m[14] +
- m[13] * m[6] * m[11] -
- m[13] * m[7] * m[10];
- inv[4] = -m[4] * m[10] * m[15] +
- m[4] * m[11] * m[14] +
- m[8] * m[6] * m[15] -
- m[8] * m[7] * m[14] -
- m[12] * m[6] * m[11] +
- m[12] * m[7] * m[10];
- inv[8] = m[4] * m[9] * m[15] -
- m[4] * m[11] * m[13] -
- m[8] * m[5] * m[15] +
- m[8] * m[7] * m[13] +
- m[12] * m[5] * m[11] -
- m[12] * m[7] * m[9];
- inv[12] = -m[4] * m[9] * m[14] +
- m[4] * m[10] * m[13] +
- m[8] * m[5] * m[14] -
- m[8] * m[6] * m[13] -
- m[12] * m[5] * m[10] +
- m[12] * m[6] * m[9];
- inv[1] = -m[1] * m[10] * m[15] +
- m[1] * m[11] * m[14] +
- m[9] * m[2] * m[15] -
- m[9] * m[3] * m[14] -
- m[13] * m[2] * m[11] +
- m[13] * m[3] * m[10];
- inv[5] = m[0] * m[10] * m[15] -
- m[0] * m[11] * m[14] -
- m[8] * m[2] * m[15] +
- m[8] * m[3] * m[14] +
- m[12] * m[2] * m[11] -
- m[12] * m[3] * m[10];
- inv[9] = -m[0] * m[9] * m[15] +
- m[0] * m[11] * m[13] +
- m[8] * m[1] * m[15] -
- m[8] * m[3] * m[13] -
- m[12] * m[1] * m[11] +
- m[12] * m[3] * m[9];
- inv[13] = m[0] * m[9] * m[14] -
- m[0] * m[10] * m[13] -
- m[8] * m[1] * m[14] +
- m[8] * m[2] * m[13] +
- m[12] * m[1] * m[10] -
- m[12] * m[2] * m[9];
- inv[2] = m[1] * m[6] * m[15] -
- m[1] * m[7] * m[14] -
- m[5] * m[2] * m[15] +
- m[5] * m[3] * m[14] +
- m[13] * m[2] * m[7] -
- m[13] * m[3] * m[6];
- inv[6] = -m[0] * m[6] * m[15] +
- m[0] * m[7] * m[14] +
- m[4] * m[2] * m[15] -
- m[4] * m[3] * m[14] -
- m[12] * m[2] * m[7] +
- m[12] * m[3] * m[6];
- inv[10] = m[0] * m[5] * m[15] -
- m[0] * m[7] * m[13] -
- m[4] * m[1] * m[15] +
- m[4] * m[3] * m[13] +
- m[12] * m[1] * m[7] -
- m[12] * m[3] * m[5];
- inv[14] = -m[0] * m[5] * m[14] +
- m[0] * m[6] * m[13] +
- m[4] * m[1] * m[14] -
- m[4] * m[2] * m[13] -
- m[12] * m[1] * m[6] +
- m[12] * m[2] * m[5];
- inv[3] = -m[1] * m[6] * m[11] +
- m[1] * m[7] * m[10] +
- m[5] * m[2] * m[11] -
- m[5] * m[3] * m[10] -
- m[9] * m[2] * m[7] +
- m[9] * m[3] * m[6];
- inv[7] = m[0] * m[6] * m[11] -
- m[0] * m[7] * m[10] -
- m[4] * m[2] * m[11] +
- m[4] * m[3] * m[10] +
- m[8] * m[2] * m[7] -
- m[8] * m[3] * m[6];
- inv[11] = -m[0] * m[5] * m[11] +
- m[0] * m[7] * m[9] +
- m[4] * m[1] * m[11] -
- m[4] * m[3] * m[9] -
- m[8] * m[1] * m[7] +
- m[8] * m[3] * m[5];
- inv[15] = m[0] * m[5] * m[10] -
- m[0] * m[6] * m[9] -
- m[4] * m[1] * m[10] +
- m[4] * m[2] * m[9] +
- m[8] * m[1] * m[6] -
- m[8] * m[2] * m[5];
- double det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
- if (det == 0)
- return false;
- det = 1.0 / det;
- for (int i = 0; i < 16; i++) {
- m[i] = inv[i] * det;
- }
- return true;
- }
- void Vector3::normalize() {
- float n = sqrt(X * X + Y * Y + Z * Z);
- if (n != 0) {
- X /= n;
- Y /= n;
- Z /= n;
- }
- }
- Vector4::Vector4(const Vector3 &vec, float w) {
- X = vec.X;
- Y = vec.Y;
- Z = vec.Z;
- W = w;
- }
- void Matrix4::identity() {
- memset(_m, 0, sizeof(_m));
- _m[0][0] = 1.0f;
- _m[1][1] = 1.0f;
- _m[2][2] = 1.0f;
- _m[3][3] = 1.0f;
- }
- Matrix4 Matrix4::transpose() const {
- Matrix4 a;
- a._m[0][0] = this->_m[0][0];
- a._m[0][1] = this->_m[1][0];
- a._m[0][2] = this->_m[2][0];
- a._m[0][3] = this->_m[3][0];
- a._m[1][0] = this->_m[0][1];
- a._m[1][1] = this->_m[1][1];
- a._m[1][2] = this->_m[2][1];
- a._m[1][3] = this->_m[3][1];
- a._m[2][0] = this->_m[0][2];
- a._m[2][1] = this->_m[1][2];
- a._m[2][2] = this->_m[2][2];
- a._m[2][3] = this->_m[3][2];
- a._m[3][0] = this->_m[0][3];
- a._m[3][1] = this->_m[1][3];
- a._m[3][2] = this->_m[2][3];
- a._m[3][3] = this->_m[3][3];
- return a;
- }
- void Matrix4::transpose() {
- Matrix4 tmp = *this;
- this->_m[0][0] = tmp._m[0][0];
- this->_m[0][1] = tmp._m[1][0];
- this->_m[0][2] = tmp._m[2][0];
- this->_m[0][3] = tmp._m[3][0];
- this->_m[1][0] = tmp._m[0][1];
- this->_m[1][1] = tmp._m[1][1];
- this->_m[1][2] = tmp._m[2][1];
- this->_m[1][3] = tmp._m[3][1];
- this->_m[2][0] = tmp._m[0][2];
- this->_m[2][1] = tmp._m[1][2];
- this->_m[2][2] = tmp._m[2][2];
- this->_m[2][3] = tmp._m[3][2];
- this->_m[3][0] = tmp._m[0][3];
- this->_m[3][1] = tmp._m[1][3];
- this->_m[3][2] = tmp._m[2][3];
- this->_m[3][3] = tmp._m[3][3];
- }
- Matrix4 Matrix4::inverseOrtho() const {
- Matrix4 a;
- for (int i = 0; i < 3; i++) {
- for (int j = 0; j < 3; j++) {
- a._m[i][j] = this->_m[j][i];
- }
- }
- a._m[3][0] = 0.0f;
- a._m[3][1] = 0.0f;
- a._m[3][2] = 0.0f;
- a._m[3][3] = 1.0f;
- for (int i = 0; i < 3; i++) {
- float s = 0;
- for (int j = 0; j < 3; j++) {
- s -= this->_m[j][i] * this->_m[j][3];
- }
- a._m[i][3] = s;
- }
- return a;
- }
- Matrix4 Matrix4::inverse() const {
- Matrix4 result = *this;
- MatrixInverse((float *)result._m);
- return result;
- }
- void Matrix4::rotation(float t, int u) {
- float s, c;
- int v, w;
- identity();
- if ((v = u + 1) > 2)
- v = 0;
- if ((w = v + 1) > 2)
- w = 0;
- s = sin(t);
- c = cos(t);
- _m[v][v] = c;
- _m[v][w] = -s;
- _m[w][v] = s;
- _m[w][w] = c;
- }
- bool Matrix4::isIdentity() const {
- //NOTE: This might need to be implemented in a fault-tolerant way.
- for (int i = 0; i < 4; i++) {
- for (int j = 0; j < 4; j++) {
- if (i == j) {
- if (_m[i][j] != 1.0) {
- return false;
- }
- } else if (_m[i][j] != 0.0) {
- return false;
- }
- }
- }
- return true;
- }
- void Matrix4::invert() {
- MatrixInverse((float *)this->_m);
- }
- Matrix4 Matrix4::frustum(float left, float right, float bottom, float top, float nearp, float farp) {
- float x, y, A, B, C, D;
- x = (float)((2.0 * nearp) / (right - left));
- y = (float)((2.0 * nearp) / (top - bottom));
- A = (right + left) / (right - left);
- B = (top + bottom) / (top - bottom);
- C = -(farp + nearp) / (farp - nearp);
- D = (float)(-(2.0 * farp * nearp) / (farp - nearp));
- Matrix4 m;
- m._m[0][0] = x; m._m[0][1] = 0; m._m[0][2] = A; m._m[0][3] = 0;
- m._m[1][0] = 0; m._m[1][1] = y; m._m[1][2] = B; m._m[1][3] = 0;
- m._m[2][0] = 0; m._m[2][1] = 0; m._m[2][2] = C; m._m[2][3] = D;
- m._m[3][0] = 0; m._m[3][1] = 0; m._m[3][2] = -1; m._m[3][3] = 0;
- return m;
- }
- void Matrix4::translate(float x, float y, float z) {
- _m[0][3] += _m[0][0] * x + _m[0][1] * y + _m[0][2] * z;
- _m[1][3] += _m[1][0] * x + _m[1][1] * y + _m[1][2] * z;
- _m[2][3] += _m[2][0] * x + _m[2][1] * y + _m[2][2] * z;
- _m[3][3] += _m[3][0] * x + _m[3][1] * y + _m[3][2] * z;
- }
- void Matrix4::scale(float x, float y, float z) {
- _m[0][0] *= x; _m[0][1] *= y; _m[0][2] *= z;
- _m[1][0] *= x; _m[1][1] *= y; _m[1][2] *= z;
- _m[2][0] *= x; _m[2][1] *= y; _m[2][2] *= z;
- _m[3][0] *= x; _m[3][1] *= y; _m[3][2] *= z;
- }
- /*void gl_print_matrix(const float *m) {
- for (int i = 0; i < 4; i++) {
- fprintf(stderr, "%f\t%f\t%f\t%f\n", m[12+i], m[8+i], m[4+i], m[i]);
- }
- fprintf(stderr, "\n");
- }*/
- void gl_print_matrix(const float *m) {
- for (int i = 0; i < 16; i+=4) {
- fprintf(stderr, "%f\t%f\t%f\t%f\n", m[i], m[1+i], m[2+i], m[3+i]);
- }
- fprintf(stderr, "\n");
- }
- void glopLoadMatrix(float *q, Matrix4 *m) {
- for (int i = 0; i < 4; i++) {
- m->_m[0][i] = q[0];
- m->_m[1][i] = q[1];
- m->_m[2][i] = q[2];
- m->_m[3][i] = q[3];
- q += 4;
- }
- }
- void glopRotate(float *u, Matrix4 *w, float angle) {
- Matrix4 m;
- int dir_code;
- angle = (float)(angle * (float)M_PI / 180.0);
- // simple case detection
- dir_code = ((u[0] != 0) << 2) | ((u[1] != 0) << 1) | (u[2] != 0);
- switch (dir_code) {
- case 0:
- m.identity();
- break;
- case 4:
- if (u[0] < 0) angle = -angle;
- m.rotation(angle, 0);
- break;
- case 2:
- if (u[1] < 0) angle = -angle;
- m.rotation(angle, 1);
- break;
- case 1:
- if (u[2] < 0) angle = -angle;
- m.rotation(angle, 2);
- break;
- default: {
- float cost, sint;
- // normalize vector
- float len = u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
- if (len == 0.0f)
- return;
- len = 1.0f / sqrt(len);
- u[0] *= len;
- u[1] *= len;
- u[2] *= len;
- // store cos and sin values
- cost = cos(angle);
- sint = sin(angle);
- // fill in the values
- m._m[3][0] = 0.0f;
- m._m[3][2] = 0.0f;
- m._m[0][3] = 0.0f;
- m._m[1][3] = 0.0f;
- m._m[2][3] = 0.0f;
- m._m[3][3] = 1.0f;
- // do the math
- m._m[0][0] = u[0] * u[0] * (1 - cost) + cost;
- m._m[1][0] = u[1] * u[0] * (1 - cost) - u[2] * sint;
- m._m[2][0] = u[2] * u[0] * (1 - cost) + u[1] * sint;
- m._m[0][1] = u[0] * u[1] * (1 - cost) + u[2] * sint;
- m._m[1][1] = u[1] * u[1] * (1 - cost) + cost;
- m._m[2][1] = u[2] * u[1] * (1 - cost) - u[0] * sint;
- m._m[0][2] = u[0] * u[2] * (1 - cost) - u[1] * sint;
- m._m[1][2] = u[1] * u[2] * (1 - cost) + u[0] * sint;
- m._m[2][2] = u[2] * u[2] * (1 - cost) + cost;
- }
- }
- //gl_print_matrix(*m._m);
- *w *= m;
- }
- int main() {
- float f[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
- Matrix4 mtx;
- glopLoadMatrix(f, &mtx);
- gl_print_matrix(*mtx._m);
- Matrix4 mtx2 = mtx.getCopy();
- mtx2.translate(10,20,30);
- gl_print_matrix(*mtx2._m);
- Matrix4 mtx3 = mtx.getCopy();
- mtx3.scale(10,20,30);
- gl_print_matrix(*mtx3._m);
- /*Matrix4 mtx4 = mtx.getCopy();
- int derp = MatrixInverse(*mtx4._m);
- fprintf(stderr, "%i\n", derp);
- if (derp !=0.f) gl_print_matrix(*mtx4._m);*/
- Matrix4 mtx5 = mtx.getCopy();
- float vec[3] = {0.5,0.8,1};
- glopRotate(vec, &mtx5, 30);
- gl_print_matrix(*mtx5._m);
- Matrix4 mtx6 = mtx.getCopy();
- float vec2[3] = {0,0,1};
- glopRotate(vec2, &mtx6, 30);
- gl_print_matrix(*mtx6._m);
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment