3dphotoshoot

annotate libs/vmath/matrix_c.c @ 11:ad49e1f9b627

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