clray

annotate src/matrix.cc @ 61:0f174fd60f19

find changesets by author, revision, files, or words in the commit message
added readme file and license
author John Tsiombikas <nuclear@member.fsf.org>
date Mon, 28 Dec 2015 10:27:23 +0200
parents
children
rev   line source
nuclear@13 1 #include <string.h>
nuclear@13 2 #include "matrix.h"
nuclear@13 3
nuclear@13 4 #define M(x, y) ((y) * 4 + (x))
nuclear@13 5
nuclear@13 6 Matrix4x4::Matrix4x4()
nuclear@13 7 {
nuclear@13 8 memset(m, 0, sizeof m);
nuclear@13 9 m[0] = m[5] = m[10] = m[15] = 1.0;
nuclear@13 10 }
nuclear@13 11
nuclear@13 12 Matrix4x4::Matrix4x4(const float *mat)
nuclear@13 13 {
nuclear@13 14 memcpy(m, mat, sizeof m);
nuclear@13 15 }
nuclear@13 16
nuclear@13 17 Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03,
nuclear@13 18 float m10, float m11, float m12, float m13,
nuclear@13 19 float m20, float m21, float m22, float m23,
nuclear@13 20 float m30, float m31, float m32, float m33)
nuclear@13 21 {
nuclear@13 22 m[M(0, 0)] = m00; m[M(0, 1)] = m01; m[M(0, 2)] = m02; m[M(0, 3)] = m03;
nuclear@13 23 m[M(1, 0)] = m10; m[M(1, 1)] = m11; m[M(1, 2)] = m12; m[M(1, 3)] = m13;
nuclear@13 24 m[M(2, 0)] = m20; m[M(2, 1)] = m21; m[M(2, 2)] = m22; m[M(2, 3)] = m23;
nuclear@13 25 m[M(3, 0)] = m30; m[M(3, 1)] = m31; m[M(3, 2)] = m32; m[M(3, 3)] = m33;
nuclear@13 26 }
nuclear@13 27
nuclear@13 28 Matrix4x4::Matrix4x4(const Matrix4x4 &mat)
nuclear@13 29 {
nuclear@13 30 memcpy(m, mat.m, sizeof m);
nuclear@13 31 }
nuclear@13 32
nuclear@13 33 Matrix4x4 &Matrix4x4::operator =(const Matrix4x4 &mat)
nuclear@13 34 {
nuclear@13 35 memcpy(m, mat.m, sizeof m);
nuclear@13 36 return *this;
nuclear@13 37 }
nuclear@13 38
nuclear@13 39 void Matrix4x4::identity()
nuclear@13 40 {
nuclear@13 41 memset(m, 0, sizeof m);
nuclear@13 42 m[0] = m[5] = m[10] = m[15] = 1.0;
nuclear@13 43 }
nuclear@13 44
nuclear@13 45 float Matrix4x4::determinant() const
nuclear@13 46 {
nuclear@13 47 float det11 = (m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13 48 (m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
nuclear@13 49 (m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
nuclear@13 50
nuclear@13 51 float det12 = (m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13 52 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13 53 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
nuclear@13 54
nuclear@13 55 float det13 = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
nuclear@13 56 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13 57 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13 58
nuclear@13 59 float det14 = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
nuclear@13 60 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
nuclear@13 61 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13 62
nuclear@13 63 return m[M(0, 0)] * det11 - m[M(0, 1)] * det12 + m[M(0, 2)] * det13 - m[M(0, 3)] * det14;
nuclear@13 64 }
nuclear@13 65
nuclear@13 66 Matrix4x4 Matrix4x4::adjoint() const
nuclear@13 67 {
nuclear@13 68 Matrix4x4 coef;
nuclear@13 69
nuclear@13 70 coef.m[M(0, 0)] = (m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13 71 (m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
nuclear@13 72 (m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
nuclear@13 73 coef.m[M(0, 1)] = (m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13 74 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13 75 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
nuclear@13 76 coef.m[M(0, 2)] = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
nuclear@13 77 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13 78 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13 79 coef.m[M(0, 3)] = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
nuclear@13 80 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
nuclear@13 81 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13 82
nuclear@13 83 coef.m[M(1, 0)] = (m[M(0, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13 84 (m[M(0, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
nuclear@13 85 (m[M(0, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
nuclear@13 86 coef.m[M(1, 1)] = (m[M(0, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13 87 (m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13 88 (m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
nuclear@13 89 coef.m[M(1, 2)] = (m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
nuclear@13 90 (m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13 91 (m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13 92 coef.m[M(1, 3)] = (m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
nuclear@13 93 (m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
nuclear@13 94 (m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13 95
nuclear@13 96 coef.m[M(2, 0)] = (m[M(0, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(1, 3)])) -
nuclear@13 97 (m[M(0, 2)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) +
nuclear@13 98 (m[M(0, 3)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)]));
nuclear@13 99 coef.m[M(2, 1)] = (m[M(0, 0)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(1, 3)])) -
nuclear@13 100 (m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
nuclear@13 101 (m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)]));
nuclear@13 102 coef.m[M(2, 2)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) -
nuclear@13 103 (m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
nuclear@13 104 (m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
nuclear@13 105 coef.m[M(2, 3)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)])) -
nuclear@13 106 (m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)])) +
nuclear@13 107 (m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
nuclear@13 108
nuclear@13 109 coef.m[M(3, 0)] = (m[M(0, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(2, 2)] * m[M(1, 3)])) -
nuclear@13 110 (m[M(0, 2)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) +
nuclear@13 111 (m[M(0, 3)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)]));
nuclear@13 112 coef.m[M(3, 1)] = (m[M(0, 0)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(2, 2)] * m[M(1, 3)])) -
nuclear@13 113 (m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
nuclear@13 114 (m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)]));
nuclear@13 115 coef.m[M(3, 2)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) -
nuclear@13 116 (m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
nuclear@13 117 (m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
nuclear@13 118 coef.m[M(3, 3)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)])) -
nuclear@13 119 (m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)])) +
nuclear@13 120 (m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
nuclear@13 121
nuclear@13 122 coef.transpose();
nuclear@13 123
nuclear@13 124 for(int i=0; i<4; i++) {
nuclear@13 125 for(int j=0; j<4; j++) {
nuclear@13 126 coef.m[M(i, j)] = j % 2 ? -coef.m[M(i, j)] : coef.m[M(i, j)];
nuclear@13 127 if(i % 2) coef.m[M(i, j)] = -coef.m[M(i, j)];
nuclear@13 128 }
nuclear@13 129 }
nuclear@13 130
nuclear@13 131 return coef;
nuclear@13 132 }
nuclear@13 133
nuclear@13 134 void Matrix4x4::invert()
nuclear@13 135 {
nuclear@13 136 Matrix4x4 adj = adjoint();
nuclear@13 137
nuclear@13 138 float det = determinant();
nuclear@13 139
nuclear@13 140 for(int i=0; i<16; i++) {
nuclear@13 141 m[i] = adj.m[i] / det;
nuclear@13 142 }
nuclear@13 143 }
nuclear@13 144
nuclear@13 145 void Matrix4x4::transpose()
nuclear@13 146 {
nuclear@13 147 float tmp[16];
nuclear@13 148
nuclear@13 149 memcpy(tmp, m, sizeof tmp);
nuclear@13 150 for(int i=0; i<4; i++) {
nuclear@13 151 for(int j=0; j<4; j++) {
nuclear@13 152 m[M(i, j)] = tmp[M(j, i)];
nuclear@13 153 }
nuclear@13 154 }
nuclear@13 155 }