matrix4x4.c (12867B)
1 /* 2 * Copyright (c) 2009, Mozilla Corp 3 * Copyright (c) 2012, Google, Inc. 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions are met: 8 * * Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * * Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * * Neither the name of the <organization> nor the 14 * names of its contributors may be used to endorse or promote products 15 * derived from this software without specific prior written permission. 16 * 17 * THIS SOFTWARE IS PROVIDED BY <copyright holder> ''AS IS'' AND ANY 18 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 * DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY 21 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 */ 28 29 /* 30 * Based on sample code from the OpenGL(R) ES 2.0 Programming Guide, which carriers 31 * the following header: 32 * 33 * Book: OpenGL(R) ES 2.0 Programming Guide 34 * Authors: Aaftab Munshi, Dan Ginsburg, Dave Shreiner 35 * ISBN-10: 0321502795 36 * ISBN-13: 9780321502797 37 * Publisher: Addison-Wesley Professional 38 * URLs: http://safari.informit.com/9780321563835 39 * http://www.opengles-book.com 40 */ 41 42 /* 43 * Ported from JavaScript to C by Behdad Esfahbod, 2012. 44 * Added MultMatrix. Converting from fixed-function OpenGL matrix 45 * operations to these functions should be as simple as renaming the 46 * 'gl' prefix to 'm4' and adding the matrix argument to the call. 47 * 48 * The C version lives at http://code.google.com/p/matrix4x4-c/ 49 */ 50 51 #include "matrix4x4.h" 52 #include <math.h> 53 54 /* 55 * A simple 4x4 matrix utility implementation 56 */ 57 58 59 float * 60 m4LoadIdentity (float *mat) { 61 unsigned int i; 62 for (i = 0; i < 16; i++) 63 mat[i] = 0; 64 mat[0*4+0] = 1.0; 65 mat[1*4+1] = 1.0; 66 mat[2*4+2] = 1.0; 67 mat[3*4+3] = 1.0; 68 return mat; 69 } 70 71 /* Copies other matrix into mat */ 72 float * 73 m4Copy (float *mat, const float *other) { 74 unsigned int i; 75 for (i = 0; i < 16; i++) { 76 mat[i] = other[i]; 77 } 78 return mat; 79 } 80 81 float * 82 m4Multiply (float *mat, const float *right) { 83 float tmp[16]; 84 unsigned int i; 85 86 for (i = 0; i < 4; i++) { 87 tmp[i*4+0] = 88 (mat[i*4+0] * right[0*4+0]) + 89 (mat[i*4+1] * right[1*4+0]) + 90 (mat[i*4+2] * right[2*4+0]) + 91 (mat[i*4+3] * right[3*4+0]) ; 92 93 tmp[i*4+1] = 94 (mat[i*4+0] * right[0*4+1]) + 95 (mat[i*4+1] * right[1*4+1]) + 96 (mat[i*4+2] * right[2*4+1]) + 97 (mat[i*4+3] * right[3*4+1]) ; 98 99 tmp[i*4+2] = 100 (mat[i*4+0] * right[0*4+2]) + 101 (mat[i*4+1] * right[1*4+2]) + 102 (mat[i*4+2] * right[2*4+2]) + 103 (mat[i*4+3] * right[3*4+2]) ; 104 105 tmp[i*4+3] = 106 (mat[i*4+0] * right[0*4+3]) + 107 (mat[i*4+1] * right[1*4+3]) + 108 (mat[i*4+2] * right[2*4+3]) + 109 (mat[i*4+3] * right[3*4+3]) ; 110 } 111 112 return m4Copy (mat, tmp); 113 } 114 115 float 116 m4Get (float *mat, unsigned int row, unsigned int col) { 117 return mat[4*row+col]; 118 } 119 120 float * 121 m4MultMatrix (float *mat, const float *left) { 122 float tmp[16]; 123 return m4Copy (mat, m4Multiply (m4Copy (tmp, left), mat)); 124 } 125 126 float * 127 m4Scale (float *mat, float sx, float sy, float sz) { 128 mat[0*4+0] *= sx; 129 mat[0*4+1] *= sx; 130 mat[0*4+2] *= sx; 131 mat[0*4+3] *= sx; 132 133 mat[1*4+0] *= sy; 134 mat[1*4+1] *= sy; 135 mat[1*4+2] *= sy; 136 mat[1*4+3] *= sy; 137 138 mat[2*4+0] *= sz; 139 mat[2*4+1] *= sz; 140 mat[2*4+2] *= sz; 141 mat[2*4+3] *= sz; 142 143 return mat; 144 } 145 146 float * 147 m4Translate (float *mat, float tx, float ty, float tz) { 148 mat[3*4+0] += mat[0*4+0] * tx + mat[1*4+0] * ty + mat[2*4+0] * tz; 149 mat[3*4+1] += mat[0*4+1] * tx + mat[1*4+1] * ty + mat[2*4+1] * tz; 150 mat[3*4+2] += mat[0*4+2] * tx + mat[1*4+2] * ty + mat[2*4+2] * tz; 151 mat[3*4+3] += mat[0*4+3] * tx + mat[1*4+3] * ty + mat[2*4+3] * tz; 152 153 return mat; 154 } 155 156 float * 157 m4Rotate (float *mat, float angle, float x, float y, float z) { 158 float mag = sqrt(x*x + y*y + z*z); 159 float sinAngle = sin(angle * M_PI / 180.0); 160 float cosAngle = cos(angle * M_PI / 180.0); 161 162 float xx, yy, zz, xy, yz, zx, xs, ys, zs; 163 float oneMinusCos; 164 165 float rotMat[16]; 166 167 if (mag <= 0) 168 return mat; 169 170 m4LoadIdentity (rotMat); 171 172 x /= mag; 173 y /= mag; 174 z /= mag; 175 176 xx = x * x; 177 yy = y * y; 178 zz = z * z; 179 xy = x * y; 180 yz = y * z; 181 zx = z * x; 182 xs = x * sinAngle; 183 ys = y * sinAngle; 184 zs = z * sinAngle; 185 oneMinusCos = 1.0 - cosAngle; 186 187 rotMat[0*4+0] = (oneMinusCos * xx) + cosAngle; 188 rotMat[0*4+1] = (oneMinusCos * xy) - zs; 189 rotMat[0*4+2] = (oneMinusCos * zx) + ys; 190 rotMat[0*4+3] = 0.0; 191 192 rotMat[1*4+0] = (oneMinusCos * xy) + zs; 193 rotMat[1*4+1] = (oneMinusCos * yy) + cosAngle; 194 rotMat[1*4+2] = (oneMinusCos * yz) - xs; 195 rotMat[1*4+3] = 0.0; 196 197 rotMat[2*4+0] = (oneMinusCos * zx) - ys; 198 rotMat[2*4+1] = (oneMinusCos * yz) + xs; 199 rotMat[2*4+2] = (oneMinusCos * zz) + cosAngle; 200 rotMat[2*4+3] = 0.0; 201 202 rotMat[3*4+0] = 0.0; 203 rotMat[3*4+1] = 0.0; 204 rotMat[3*4+2] = 0.0; 205 rotMat[3*4+3] = 1.0; 206 207 return m4Copy (mat, m4Multiply (rotMat, mat)); 208 } 209 210 float * 211 m4Frustum (float *mat, float left, float right, float bottom, float top, float nearZ, float farZ) { 212 float deltaX = right - left; 213 float deltaY = top - bottom; 214 float deltaZ = farZ - nearZ; 215 216 float frust[16]; 217 218 if ( (nearZ <= 0.0) || (farZ <= 0.0) || 219 (deltaX <= 0.0) || (deltaY <= 0.0) || (deltaZ <= 0.0) ) 220 return mat; 221 222 m4LoadIdentity (frust); 223 224 frust[0*4+0] = 2.0 * nearZ / deltaX; 225 frust[0*4+1] = frust[0*4+2] = frust[0*4+3] = 0.0; 226 227 frust[1*4+1] = 2.0 * nearZ / deltaY; 228 frust[1*4+0] = frust[1*4+2] = frust[1*4+3] = 0.0; 229 230 frust[2*4+0] = (right + left) / deltaX; 231 frust[2*4+1] = (top + bottom) / deltaY; 232 frust[2*4+2] = -(nearZ + farZ) / deltaZ; 233 frust[2*4+3] = -1.0; 234 235 frust[3*4+2] = -2.0 * nearZ * farZ / deltaZ; 236 frust[3*4+0] = frust[3*4+1] = frust[3*4+3] = 0.0; 237 238 return m4Copy (mat, m4Multiply (frust, mat)); 239 } 240 241 float * 242 m4Perspective (float *mat, float fovy, float aspect, float nearZ, float farZ) { 243 float frustumH = tan(fovy / 360.0 * M_PI) * nearZ; 244 float frustumW = frustumH * aspect; 245 246 return m4Frustum(mat, -frustumW, frustumW, -frustumH, frustumH, nearZ, farZ); 247 } 248 249 float * 250 m4Ortho (float *mat, float left, float right, float bottom, float top, float nearZ, float farZ) { 251 float deltaX = right - left; 252 float deltaY = top - bottom; 253 float deltaZ = farZ - nearZ; 254 255 float ortho[16]; 256 257 if ( (deltaX == 0.0) || (deltaY == 0.0) || (deltaZ == 0.0) ) 258 return mat; 259 260 m4LoadIdentity (ortho); 261 262 ortho[0*4+0] = 2.0 / deltaX; 263 ortho[3*4+0] = -(right + left) / deltaX; 264 ortho[1*4+1] = 2.0 / deltaY; 265 ortho[3*4+1] = -(top + bottom) / deltaY; 266 ortho[2*4+2] = -2.0 / deltaZ; 267 ortho[3*4+2] = -(nearZ + farZ) / deltaZ; 268 269 return m4Copy (mat, m4Multiply (ortho, mat)); 270 } 271 272 /* In-place inversion */ 273 float * 274 m4Invert (float *mat) { 275 float tmp_0 = m4Get(mat,2,2) * m4Get(mat,3,3); 276 float tmp_1 = m4Get(mat,3,2) * m4Get(mat,2,3); 277 float tmp_2 = m4Get(mat,1,2) * m4Get(mat,3,3); 278 float tmp_3 = m4Get(mat,3,2) * m4Get(mat,1,3); 279 float tmp_4 = m4Get(mat,1,2) * m4Get(mat,2,3); 280 float tmp_5 = m4Get(mat,2,2) * m4Get(mat,1,3); 281 float tmp_6 = m4Get(mat,0,2) * m4Get(mat,3,3); 282 float tmp_7 = m4Get(mat,3,2) * m4Get(mat,0,3); 283 float tmp_8 = m4Get(mat,0,2) * m4Get(mat,2,3); 284 float tmp_9 = m4Get(mat,2,2) * m4Get(mat,0,3); 285 float tmp_10 = m4Get(mat,0,2) * m4Get(mat,1,3); 286 float tmp_11 = m4Get(mat,1,2) * m4Get(mat,0,3); 287 float tmp_12 = m4Get(mat,2,0) * m4Get(mat,3,1); 288 float tmp_13 = m4Get(mat,3,0) * m4Get(mat,2,1); 289 float tmp_14 = m4Get(mat,1,0) * m4Get(mat,3,1); 290 float tmp_15 = m4Get(mat,3,0) * m4Get(mat,1,1); 291 float tmp_16 = m4Get(mat,1,0) * m4Get(mat,2,1); 292 float tmp_17 = m4Get(mat,2,0) * m4Get(mat,1,1); 293 float tmp_18 = m4Get(mat,0,0) * m4Get(mat,3,1); 294 float tmp_19 = m4Get(mat,3,0) * m4Get(mat,0,1); 295 float tmp_20 = m4Get(mat,0,0) * m4Get(mat,2,1); 296 float tmp_21 = m4Get(mat,2,0) * m4Get(mat,0,1); 297 float tmp_22 = m4Get(mat,0,0) * m4Get(mat,1,1); 298 float tmp_23 = m4Get(mat,1,0) * m4Get(mat,0,1); 299 300 float t0 = ((tmp_0 * m4Get(mat,1,1) + tmp_3 * m4Get(mat,2,1) + tmp_4 * m4Get(mat,3,1)) - 301 (tmp_1 * m4Get(mat,1,1) + tmp_2 * m4Get(mat,2,1) + tmp_5 * m4Get(mat,3,1))); 302 float t1 = ((tmp_1 * m4Get(mat,0,1) + tmp_6 * m4Get(mat,2,1) + tmp_9 * m4Get(mat,3,1)) - 303 (tmp_0 * m4Get(mat,0,1) + tmp_7 * m4Get(mat,2,1) + tmp_8 * m4Get(mat,3,1))); 304 float t2 = ((tmp_2 * m4Get(mat,0,1) + tmp_7 * m4Get(mat,1,1) + tmp_10 * m4Get(mat,3,1)) - 305 (tmp_3 * m4Get(mat,0,1) + tmp_6 * m4Get(mat,1,1) + tmp_11 * m4Get(mat,3,1))); 306 float t3 = ((tmp_5 * m4Get(mat,0,1) + tmp_8 * m4Get(mat,1,1) + tmp_11 * m4Get(mat,2,1)) - 307 (tmp_4 * m4Get(mat,0,1) + tmp_9 * m4Get(mat,1,1) + tmp_10 * m4Get(mat,2,1))); 308 309 float d = 1.0 / (m4Get(mat,0,0) * t0 + m4Get(mat,1,0) * t1 + m4Get(mat,2,0) * t2 + m4Get(mat,3,0) * t3); 310 311 float out_00 = d * t0; 312 float out_01 = d * t1; 313 float out_02 = d * t2; 314 float out_03 = d * t3; 315 316 float out_10 = d * ((tmp_1 * m4Get(mat,1,0) + tmp_2 * m4Get(mat,2,0) + tmp_5 * m4Get(mat,3,0)) - 317 (tmp_0 * m4Get(mat,1,0) + tmp_3 * m4Get(mat,2,0) + tmp_4 * m4Get(mat,3,0))); 318 float out_11 = d * ((tmp_0 * m4Get(mat,0,0) + tmp_7 * m4Get(mat,2,0) + tmp_8 * m4Get(mat,3,0)) - 319 (tmp_1 * m4Get(mat,0,0) + tmp_6 * m4Get(mat,2,0) + tmp_9 * m4Get(mat,3,0))); 320 float out_12 = d * ((tmp_3 * m4Get(mat,0,0) + tmp_6 * m4Get(mat,1,0) + tmp_11 * m4Get(mat,3,0)) - 321 (tmp_2 * m4Get(mat,0,0) + tmp_7 * m4Get(mat,1,0) + tmp_10 * m4Get(mat,3,0))); 322 float out_13 = d * ((tmp_4 * m4Get(mat,0,0) + tmp_9 * m4Get(mat,1,0) + tmp_10 * m4Get(mat,2,0)) - 323 (tmp_5 * m4Get(mat,0,0) + tmp_8 * m4Get(mat,1,0) + tmp_11 * m4Get(mat,2,0))); 324 325 float out_20 = d * ((tmp_12 * m4Get(mat,1,3) + tmp_15 * m4Get(mat,2,3) + tmp_16 * m4Get(mat,3,3)) - 326 (tmp_13 * m4Get(mat,1,3) + tmp_14 * m4Get(mat,2,3) + tmp_17 * m4Get(mat,3,3))); 327 float out_21 = d * ((tmp_13 * m4Get(mat,0,3) + tmp_18 * m4Get(mat,2,3) + tmp_21 * m4Get(mat,3,3)) - 328 (tmp_12 * m4Get(mat,0,3) + tmp_19 * m4Get(mat,2,3) + tmp_20 * m4Get(mat,3,3))); 329 float out_22 = d * ((tmp_14 * m4Get(mat,0,3) + tmp_19 * m4Get(mat,1,3) + tmp_22 * m4Get(mat,3,3)) - 330 (tmp_15 * m4Get(mat,0,3) + tmp_18 * m4Get(mat,1,3) + tmp_23 * m4Get(mat,3,3))); 331 float out_23 = d * ((tmp_17 * m4Get(mat,0,3) + tmp_20 * m4Get(mat,1,3) + tmp_23 * m4Get(mat,2,3)) - 332 (tmp_16 * m4Get(mat,0,3) + tmp_21 * m4Get(mat,1,3) + tmp_22 * m4Get(mat,2,3))); 333 334 float out_30 = d * ((tmp_14 * m4Get(mat,2,2) + tmp_17 * m4Get(mat,3,2) + tmp_13 * m4Get(mat,1,2)) - 335 (tmp_16 * m4Get(mat,3,2) + tmp_12 * m4Get(mat,1,2) + tmp_15 * m4Get(mat,2,2))); 336 float out_31 = d * ((tmp_20 * m4Get(mat,3,2) + tmp_12 * m4Get(mat,0,2) + tmp_19 * m4Get(mat,2,2)) - 337 (tmp_18 * m4Get(mat,2,2) + tmp_21 * m4Get(mat,3,2) + tmp_13 * m4Get(mat,0,2))); 338 float out_32 = d * ((tmp_18 * m4Get(mat,1,2) + tmp_23 * m4Get(mat,3,2) + tmp_15 * m4Get(mat,0,2)) - 339 (tmp_22 * m4Get(mat,3,2) + tmp_14 * m4Get(mat,0,2) + tmp_19 * m4Get(mat,1,2))); 340 float out_33 = d * ((tmp_22 * m4Get(mat,2,2) + tmp_16 * m4Get(mat,0,2) + tmp_21 * m4Get(mat,1,2)) - 341 (tmp_20 * m4Get(mat,1,2) + tmp_23 * m4Get(mat,2,2) + tmp_17 * m4Get(mat,0,2))); 342 343 mat[0*4+0] = out_00; 344 mat[0*4+1] = out_01; 345 mat[0*4+2] = out_02; 346 mat[0*4+3] = out_03; 347 mat[1*4+0] = out_10; 348 mat[1*4+1] = out_11; 349 mat[1*4+2] = out_12; 350 mat[1*4+3] = out_13; 351 mat[2*4+0] = out_20; 352 mat[2*4+1] = out_21; 353 mat[2*4+2] = out_22; 354 mat[2*4+3] = out_23; 355 mat[3*4+0] = out_30; 356 mat[3*4+1] = out_31; 357 mat[3*4+2] = out_32; 358 mat[3*4+3] = out_33; 359 return mat; 360 } 361 362 /* Puts the inverse of other matrix into mat */ 363 float * 364 m4Inverse (float *mat, const float *other) { 365 m4Copy (mat, other); 366 m4Invert (mat); 367 return mat; 368 } 369 370 /* In-place transpose */ 371 float * 372 m4Transpose (float *mat) { 373 float tmp = mat[0*4+1]; 374 mat[0*4+1] = mat[1*4+0]; 375 mat[1*4+0] = tmp; 376 377 tmp = mat[0*4+2]; 378 mat[0*4+2] = mat[2*4+0]; 379 mat[2*4+0] = tmp; 380 381 tmp = mat[0*4+3]; 382 mat[0*4+3] = mat[3*4+0]; 383 mat[3*4+0] = tmp; 384 385 tmp = mat[1*4+2]; 386 mat[1*4+2] = mat[2*4+1]; 387 mat[2*4+1] = tmp; 388 389 tmp = mat[1*4+3]; 390 mat[1*4+3] = mat[3*4+1]; 391 mat[3*4+1] = tmp; 392 393 tmp = mat[2*4+3]; 394 mat[2*4+3] = mat[3*4+2]; 395 mat[3*4+2] = tmp; 396 397 return mat; 398 }