istereo

annotate libs/vmath/matrix_c.c @ 28:c0ae8e668447

find changesets by author, revision, files, or words in the commit message
added vmath library
author John Tsiombikas <nuclear@mutantstargoat.com>
date Thu, 08 Sep 2011 08:30:42 +0300
parents
children ff055bff6a15
rev   line source
nuclear@28 1 #include <stdio.h>
nuclear@28 2 #include "matrix.h"
nuclear@28 3 #include "vector.h"
nuclear@28 4 #include "quat.h"
nuclear@28 5
nuclear@28 6 void m3_to_m4(mat4_t dest, mat3_t src)
nuclear@28 7 {
nuclear@28 8 int i, j;
nuclear@28 9
nuclear@28 10 memset(dest, 0, sizeof(mat4_t));
nuclear@28 11 for(i=0; i<3; i++) {
nuclear@28 12 for(j=0; j<3; j++) {
nuclear@28 13 dest[i][j] = src[i][j];
nuclear@28 14 }
nuclear@28 15 }
nuclear@28 16 dest[3][3] = 1.0;
nuclear@28 17 }
nuclear@28 18
nuclear@28 19 void m3_print(FILE *fp, mat3_t m)
nuclear@28 20 {
nuclear@28 21 int i;
nuclear@28 22 for(i=0; i<3; i++) {
nuclear@28 23 fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]);
nuclear@28 24 }
nuclear@28 25 }
nuclear@28 26
nuclear@28 27 /* C matrix 4x4 functions */
nuclear@28 28 void m4_to_m3(mat3_t dest, mat4_t src)
nuclear@28 29 {
nuclear@28 30 int i, j;
nuclear@28 31 for(i=0; i<3; i++) {
nuclear@28 32 for(j=0; j<3; j++) {
nuclear@28 33 dest[i][j] = src[i][j];
nuclear@28 34 }
nuclear@28 35 }
nuclear@28 36 }
nuclear@28 37
nuclear@28 38 void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
nuclear@28 39 {
nuclear@28 40 mat4_t tm;
nuclear@28 41 m4_identity(tm);
nuclear@28 42 tm[0][3] = x;
nuclear@28 43 tm[1][3] = y;
nuclear@28 44 tm[2][3] = z;
nuclear@28 45 m4_mult(m, m, tm);
nuclear@28 46 }
nuclear@28 47
nuclear@28 48 void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
nuclear@28 49 {
nuclear@28 50 m4_rotate_x(m, x);
nuclear@28 51 m4_rotate_y(m, y);
nuclear@28 52 m4_rotate_z(m, z);
nuclear@28 53 }
nuclear@28 54
nuclear@28 55 void m4_rotate_x(mat4_t m, scalar_t angle)
nuclear@28 56 {
nuclear@28 57 mat4_t rm;
nuclear@28 58 m4_identity(rm);
nuclear@28 59 rm[1][1] = cos(angle); rm[1][2] = -sin(angle);
nuclear@28 60 rm[2][1] = sin(angle); rm[2][2] = cos(angle);
nuclear@28 61 m4_mult(m, m, rm);
nuclear@28 62 }
nuclear@28 63
nuclear@28 64 void m4_rotate_y(mat4_t m, scalar_t angle)
nuclear@28 65 {
nuclear@28 66 mat4_t rm;
nuclear@28 67 m4_identity(rm);
nuclear@28 68 rm[0][0] = cos(angle); rm[0][2] = sin(angle);
nuclear@28 69 rm[2][0] = -sin(angle); rm[2][2] = cos(angle);
nuclear@28 70 m4_mult(m, m, rm);
nuclear@28 71 }
nuclear@28 72
nuclear@28 73 void m4_rotate_z(mat4_t m, scalar_t angle)
nuclear@28 74 {
nuclear@28 75 mat4_t rm;
nuclear@28 76 m4_identity(rm);
nuclear@28 77 rm[0][0] = cos(angle); rm[0][1] = -sin(angle);
nuclear@28 78 rm[1][0] = sin(angle); rm[1][1] = cos(angle);
nuclear@28 79 m4_mult(m, m, rm);
nuclear@28 80 }
nuclear@28 81
nuclear@28 82 void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
nuclear@28 83 {
nuclear@28 84 mat4_t xform;
nuclear@28 85 scalar_t sina = sin(angle);
nuclear@28 86 scalar_t cosa = cos(angle);
nuclear@28 87 scalar_t one_minus_cosa = 1.0 - cosa;
nuclear@28 88 scalar_t nxsq = x * x;
nuclear@28 89 scalar_t nysq = y * y;
nuclear@28 90 scalar_t nzsq = z * z;
nuclear@28 91
nuclear@28 92 m4_identity(xform);
nuclear@28 93 xform[0][0] = nxsq + (1.0 - nxsq) * cosa;
nuclear@28 94 xform[0][1] = x * y * one_minus_cosa - z * sina;
nuclear@28 95 xform[0][2] = x * z * one_minus_cosa + y * sina;
nuclear@28 96 xform[1][0] = x * y * one_minus_cosa + z * sina;
nuclear@28 97 xform[1][1] = nysq + (1.0 - nysq) * cosa;
nuclear@28 98 xform[1][2] = y * z * one_minus_cosa - x * sina;
nuclear@28 99 xform[2][0] = x * z * one_minus_cosa - y * sina;
nuclear@28 100 xform[2][1] = y * z * one_minus_cosa + x * sina;
nuclear@28 101 xform[2][2] = nzsq + (1.0 - nzsq) * cosa;
nuclear@28 102
nuclear@28 103 m4_mult(m, m, xform);
nuclear@28 104 }
nuclear@28 105
nuclear@28 106 void m4_rotate_quat(mat4_t m, quat_t q)
nuclear@28 107 {
nuclear@28 108 mat4_t rm;
nuclear@28 109 quat_to_mat4(rm, q);
nuclear@28 110 m4_mult(m, m, rm);
nuclear@28 111 }
nuclear@28 112
nuclear@28 113 void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
nuclear@28 114 {
nuclear@28 115 mat4_t sm;
nuclear@28 116 m4_identity(sm);
nuclear@28 117 sm[0][0] = x;
nuclear@28 118 sm[1][1] = y;
nuclear@28 119 sm[2][2] = z;
nuclear@28 120 m4_mult(m, m, sm);
nuclear@28 121 }
nuclear@28 122
nuclear@28 123 void m4_transpose(mat4_t res, mat4_t m)
nuclear@28 124 {
nuclear@28 125 int i, j;
nuclear@28 126 mat4_t tmp;
nuclear@28 127 m4_copy(tmp, m);
nuclear@28 128
nuclear@28 129 for(i=0; i<4; i++) {
nuclear@28 130 for(j=0; j<4; j++) {
nuclear@28 131 res[i][j] = tmp[j][i];
nuclear@28 132 }
nuclear@28 133 }
nuclear@28 134 }
nuclear@28 135
nuclear@28 136 scalar_t m4_determinant(mat4_t m)
nuclear@28 137 {
nuclear@28 138 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@28 139 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@28 140 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@28 141
nuclear@28 142 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@28 143 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@28 144 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@28 145
nuclear@28 146 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@28 147 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@28 148 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@28 149
nuclear@28 150 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@28 151 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@28 152 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@28 153
nuclear@28 154 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
nuclear@28 155 }
nuclear@28 156
nuclear@28 157 void m4_adjoint(mat4_t res, mat4_t m)
nuclear@28 158 {
nuclear@28 159 int i, j;
nuclear@28 160 mat4_t coef;
nuclear@28 161
nuclear@28 162 coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@28 163 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@28 164 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@28 165 coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@28 166 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@28 167 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@28 168 coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@28 169 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@28 170 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@28 171 coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@28 172 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@28 173 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@28 174
nuclear@28 175 coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@28 176 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@28 177 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@28 178 coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@28 179 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@28 180 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@28 181 coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@28 182 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@28 183 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@28 184 coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@28 185 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@28 186 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@28 187
nuclear@28 188 coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@28 189 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
nuclear@28 190 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
nuclear@28 191 coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@28 192 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@28 193 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
nuclear@28 194 coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
nuclear@28 195 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@28 196 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@28 197 coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
nuclear@28 198 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
nuclear@28 199 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@28 200
nuclear@28 201 coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@28 202 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
nuclear@28 203 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
nuclear@28 204 coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@28 205 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@28 206 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
nuclear@28 207 coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
nuclear@28 208 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@28 209 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@28 210 coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
nuclear@28 211 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
nuclear@28 212 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@28 213
nuclear@28 214 m4_transpose(res, coef);
nuclear@28 215
nuclear@28 216 for(i=0; i<4; i++) {
nuclear@28 217 for(j=0; j<4; j++) {
nuclear@28 218 res[i][j] = j % 2 ? -res[i][j] : res[i][j];
nuclear@28 219 if(i % 2) res[i][j] = -res[i][j];
nuclear@28 220 }
nuclear@28 221 }
nuclear@28 222 }
nuclear@28 223
nuclear@28 224 void m4_inverse(mat4_t res, mat4_t m)
nuclear@28 225 {
nuclear@28 226 int i, j;
nuclear@28 227 mat4_t adj;
nuclear@28 228 scalar_t det;
nuclear@28 229
nuclear@28 230 m4_adjoint(adj, m);
nuclear@28 231 det = m4_determinant(m);
nuclear@28 232
nuclear@28 233 for(i=0; i<4; i++) {
nuclear@28 234 for(j=0; j<4; j++) {
nuclear@28 235 res[i][j] = adj[i][j] / det;
nuclear@28 236 }
nuclear@28 237 }
nuclear@28 238 }
nuclear@28 239
nuclear@28 240 void m4_print(FILE *fp, mat4_t m)
nuclear@28 241 {
nuclear@28 242 int i;
nuclear@28 243 for(i=0; i<4; i++) {
nuclear@28 244 fprintf(fp, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2], (float)m[i][3]);
nuclear@28 245 }
nuclear@28 246 }