istereo2

annotate libs/vmath/matrix_c.c @ 13:ea928c313344

find changesets by author, revision, files, or words in the commit message
foo
author John Tsiombikas <nuclear@member.fsf.org>
date Mon, 28 Sep 2015 19:04:50 +0300
parents
children
rev   line source
nuclear@2 1 /*
nuclear@2 2 libvmath - a vector math library
nuclear@2 3 Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
nuclear@2 4
nuclear@2 5 This program is free software: you can redistribute it and/or modify
nuclear@2 6 it under the terms of the GNU Lesser General Public License as published
nuclear@2 7 by the Free Software Foundation, either version 3 of the License, or
nuclear@2 8 (at your option) any later version.
nuclear@2 9
nuclear@2 10 This program is distributed in the hope that it will be useful,
nuclear@2 11 but WITHOUT ANY WARRANTY; without even the implied warranty of
nuclear@2 12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
nuclear@2 13 GNU Lesser General Public License for more details.
nuclear@2 14
nuclear@2 15 You should have received a copy of the GNU Lesser General Public License
nuclear@2 16 along with this program. If not, see <http://www.gnu.org/licenses/>.
nuclear@2 17 */
nuclear@2 18
nuclear@2 19
nuclear@2 20 #include <stdio.h>
nuclear@2 21 #include "matrix.h"
nuclear@2 22 #include "vector.h"
nuclear@2 23 #include "quat.h"
nuclear@2 24
nuclear@2 25 void m3_to_m4(mat4_t dest, mat3_t src)
nuclear@2 26 {
nuclear@2 27 int i, j;
nuclear@2 28
nuclear@2 29 memset(dest, 0, sizeof(mat4_t));
nuclear@2 30 for(i=0; i<3; i++) {
nuclear@2 31 for(j=0; j<3; j++) {
nuclear@2 32 dest[i][j] = src[i][j];
nuclear@2 33 }
nuclear@2 34 }
nuclear@2 35 dest[3][3] = 1.0;
nuclear@2 36 }
nuclear@2 37
nuclear@2 38 void m3_print(FILE *fp, mat3_t m)
nuclear@2 39 {
nuclear@2 40 int i;
nuclear@2 41 for(i=0; i<3; i++) {
nuclear@2 42 fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]);
nuclear@2 43 }
nuclear@2 44 }
nuclear@2 45
nuclear@2 46 /* C matrix 4x4 functions */
nuclear@2 47 void m4_to_m3(mat3_t dest, mat4_t src)
nuclear@2 48 {
nuclear@2 49 int i, j;
nuclear@2 50 for(i=0; i<3; i++) {
nuclear@2 51 for(j=0; j<3; j++) {
nuclear@2 52 dest[i][j] = src[i][j];
nuclear@2 53 }
nuclear@2 54 }
nuclear@2 55 }
nuclear@2 56
nuclear@2 57 void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
nuclear@2 58 {
nuclear@2 59 mat4_t tm;
nuclear@2 60 m4_identity(tm);
nuclear@2 61 tm[0][3] = x;
nuclear@2 62 tm[1][3] = y;
nuclear@2 63 tm[2][3] = z;
nuclear@2 64 m4_mult(m, m, tm);
nuclear@2 65 }
nuclear@2 66
nuclear@2 67 void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
nuclear@2 68 {
nuclear@2 69 m4_rotate_x(m, x);
nuclear@2 70 m4_rotate_y(m, y);
nuclear@2 71 m4_rotate_z(m, z);
nuclear@2 72 }
nuclear@2 73
nuclear@2 74 void m4_rotate_x(mat4_t m, scalar_t angle)
nuclear@2 75 {
nuclear@2 76 mat4_t rm;
nuclear@2 77 m4_identity(rm);
nuclear@2 78 rm[1][1] = cos(angle); rm[1][2] = -sin(angle);
nuclear@2 79 rm[2][1] = sin(angle); rm[2][2] = cos(angle);
nuclear@2 80 m4_mult(m, m, rm);
nuclear@2 81 }
nuclear@2 82
nuclear@2 83 void m4_rotate_y(mat4_t m, scalar_t angle)
nuclear@2 84 {
nuclear@2 85 mat4_t rm;
nuclear@2 86 m4_identity(rm);
nuclear@2 87 rm[0][0] = cos(angle); rm[0][2] = sin(angle);
nuclear@2 88 rm[2][0] = -sin(angle); rm[2][2] = cos(angle);
nuclear@2 89 m4_mult(m, m, rm);
nuclear@2 90 }
nuclear@2 91
nuclear@2 92 void m4_rotate_z(mat4_t m, scalar_t angle)
nuclear@2 93 {
nuclear@2 94 mat4_t rm;
nuclear@2 95 m4_identity(rm);
nuclear@2 96 rm[0][0] = cos(angle); rm[0][1] = -sin(angle);
nuclear@2 97 rm[1][0] = sin(angle); rm[1][1] = cos(angle);
nuclear@2 98 m4_mult(m, m, rm);
nuclear@2 99 }
nuclear@2 100
nuclear@2 101 void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
nuclear@2 102 {
nuclear@2 103 mat4_t xform;
nuclear@2 104 scalar_t sina = sin(angle);
nuclear@2 105 scalar_t cosa = cos(angle);
nuclear@2 106 scalar_t one_minus_cosa = 1.0 - cosa;
nuclear@2 107 scalar_t nxsq = x * x;
nuclear@2 108 scalar_t nysq = y * y;
nuclear@2 109 scalar_t nzsq = z * z;
nuclear@2 110
nuclear@2 111 m4_identity(xform);
nuclear@2 112 xform[0][0] = nxsq + (1.0 - nxsq) * cosa;
nuclear@2 113 xform[0][1] = x * y * one_minus_cosa - z * sina;
nuclear@2 114 xform[0][2] = x * z * one_minus_cosa + y * sina;
nuclear@2 115 xform[1][0] = x * y * one_minus_cosa + z * sina;
nuclear@2 116 xform[1][1] = nysq + (1.0 - nysq) * cosa;
nuclear@2 117 xform[1][2] = y * z * one_minus_cosa - x * sina;
nuclear@2 118 xform[2][0] = x * z * one_minus_cosa - y * sina;
nuclear@2 119 xform[2][1] = y * z * one_minus_cosa + x * sina;
nuclear@2 120 xform[2][2] = nzsq + (1.0 - nzsq) * cosa;
nuclear@2 121
nuclear@2 122 m4_mult(m, m, xform);
nuclear@2 123 }
nuclear@2 124
nuclear@2 125 void m4_rotate_quat(mat4_t m, quat_t q)
nuclear@2 126 {
nuclear@2 127 mat4_t rm;
nuclear@2 128 quat_to_mat4(rm, q);
nuclear@2 129 m4_mult(m, m, rm);
nuclear@2 130 }
nuclear@2 131
nuclear@2 132 void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
nuclear@2 133 {
nuclear@2 134 mat4_t sm;
nuclear@2 135 m4_identity(sm);
nuclear@2 136 sm[0][0] = x;
nuclear@2 137 sm[1][1] = y;
nuclear@2 138 sm[2][2] = z;
nuclear@2 139 m4_mult(m, m, sm);
nuclear@2 140 }
nuclear@2 141
nuclear@2 142 void m4_transpose(mat4_t res, mat4_t m)
nuclear@2 143 {
nuclear@2 144 int i, j;
nuclear@2 145 mat4_t tmp;
nuclear@2 146 m4_copy(tmp, m);
nuclear@2 147
nuclear@2 148 for(i=0; i<4; i++) {
nuclear@2 149 for(j=0; j<4; j++) {
nuclear@2 150 res[i][j] = tmp[j][i];
nuclear@2 151 }
nuclear@2 152 }
nuclear@2 153 }
nuclear@2 154
nuclear@2 155 scalar_t m4_determinant(mat4_t m)
nuclear@2 156 {
nuclear@2 157 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@2 158 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@2 159 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@2 160
nuclear@2 161 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@2 162 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@2 163 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@2 164
nuclear@2 165 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@2 166 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@2 167 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@2 168
nuclear@2 169 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@2 170 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@2 171 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@2 172
nuclear@2 173 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
nuclear@2 174 }
nuclear@2 175
nuclear@2 176 void m4_adjoint(mat4_t res, mat4_t m)
nuclear@2 177 {
nuclear@2 178 int i, j;
nuclear@2 179 mat4_t coef;
nuclear@2 180
nuclear@2 181 coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@2 182 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@2 183 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@2 184 coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@2 185 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@2 186 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@2 187 coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@2 188 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@2 189 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@2 190 coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@2 191 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@2 192 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@2 193
nuclear@2 194 coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@2 195 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@2 196 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@2 197 coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@2 198 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@2 199 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@2 200 coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@2 201 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@2 202 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@2 203 coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@2 204 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@2 205 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@2 206
nuclear@2 207 coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@2 208 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
nuclear@2 209 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
nuclear@2 210 coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@2 211 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@2 212 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
nuclear@2 213 coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
nuclear@2 214 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@2 215 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@2 216 coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
nuclear@2 217 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
nuclear@2 218 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@2 219
nuclear@2 220 coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@2 221 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
nuclear@2 222 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
nuclear@2 223 coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@2 224 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@2 225 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
nuclear@2 226 coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
nuclear@2 227 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@2 228 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@2 229 coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
nuclear@2 230 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
nuclear@2 231 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@2 232
nuclear@2 233 m4_transpose(res, coef);
nuclear@2 234
nuclear@2 235 for(i=0; i<4; i++) {
nuclear@2 236 for(j=0; j<4; j++) {
nuclear@2 237 res[i][j] = j % 2 ? -res[i][j] : res[i][j];
nuclear@2 238 if(i % 2) res[i][j] = -res[i][j];
nuclear@2 239 }
nuclear@2 240 }
nuclear@2 241 }
nuclear@2 242
nuclear@2 243 void m4_inverse(mat4_t res, mat4_t m)
nuclear@2 244 {
nuclear@2 245 int i, j;
nuclear@2 246 mat4_t adj;
nuclear@2 247 scalar_t det;
nuclear@2 248
nuclear@2 249 m4_adjoint(adj, m);
nuclear@2 250 det = m4_determinant(m);
nuclear@2 251
nuclear@2 252 for(i=0; i<4; i++) {
nuclear@2 253 for(j=0; j<4; j++) {
nuclear@2 254 res[i][j] = adj[i][j] / det;
nuclear@2 255 }
nuclear@2 256 }
nuclear@2 257 }
nuclear@2 258
nuclear@2 259 void m4_print(FILE *fp, mat4_t m)
nuclear@2 260 {
nuclear@2 261 int i;
nuclear@2 262 for(i=0; i<4; i++) {
nuclear@2 263 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@2 264 }
nuclear@2 265 }