1 /**
  2  Copyright (c) 2010-2012 cocos2d-x.org
  3  Copyright (c) 2008, Luke Benstead.
  4  All rights reserved.
  5 
  6  Redistribution and use in source and binary forms, with or without modification,
  7  are permitted provided that the following conditions are met:
  8 
  9  * Redistributions of source code must retain the above copyright notice,
 10  this list of conditions and the following disclaimer.
 11  * Redistributions in binary form must reproduce the above copyright notice,
 12  this list of conditions and the following disclaimer in the documentation
 13  and/or other materials provided with the distribution.
 14 
 15  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 16  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 17  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 18  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 19  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 20  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 21  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 22  ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 23  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 24  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 25  */
 26 
 27 /*
 28  A 4x4 matrix
 29 
 30  mat =
 31  | 0   4   8  12 |
 32  | 1   5   9  13 |
 33  | 2   6  10  14 |
 34  | 3   7  11  15 |
 35  */
 36 cc.kmMat4 = function () {
 37     this.mat = new Float32Array([0, 0, 0, 0,
 38         0, 0, 0, 0,
 39         0, 0, 0, 0,
 40         0, 0, 0, 0]);
 41 };
 42 
 43 /**
 44  * Fills a kmMat4 structure with the values from a 16 element array of floats
 45  * @Params pOut - A pointer to the destination matrix
 46  * @Params pMat - A 16 element array of floats
 47  * @Return Returns pOut so that the call can be nested
 48  */
 49 cc.kmMat4Fill = function (pOut, pMat) {
 50     pOut.mat[0] = pOut.mat[1] = pOut.mat[2] =pOut.mat[3] =
 51         pOut.mat[4] =pOut.mat[5] =pOut.mat[6] =pOut.mat[7] =
 52             pOut.mat[8] =pOut.mat[9] =pOut.mat[10] =pOut.mat[11] =
 53                 pOut.mat[12] =pOut.mat[13] =pOut.mat[14] =pOut.mat[15] =pMat;
 54 };
 55 
 56 /**
 57  * Sets pOut to an identity matrix returns pOut
 58  * @Params pOut - A pointer to the matrix to set to identity
 59  * @Return Returns pOut so that the call can be nested
 60  */
 61 cc.kmMat4Identity = function (pOut) {
 62     pOut.mat[1] = pOut.mat[2] = pOut.mat[3]
 63         = pOut.mat[4] = pOut.mat[6] = pOut.mat[7]
 64         = pOut.mat[8] = pOut.mat[9] = pOut.mat[11]
 65         = pOut.mat[12] = pOut.mat[13] = pOut.mat[14] = 0;
 66     pOut.mat[0] = pOut.mat[5] = pOut.mat[10] = pOut.mat[15] = 1.0;
 67     return pOut;
 68 };
 69 
 70 cc.kmMat4._get = function (pIn, row, col) {
 71     return pIn.mat[row + 4 * col];
 72 };
 73 
 74 cc.kmMat4._set = function (pIn, row, col, value) {
 75     pIn.mat[row + 4 * col] = value;
 76 };
 77 
 78 cc.kmMat4._swap = function (pIn, r1, c1, r2, c2) {
 79     var tmp = cc.kmMat4._get(pIn, r1, c1);
 80     cc.kmMat4._set(pIn, r1, c1, cc.kmMat4._get(pIn, r2, c2));
 81     cc.kmMat4._set(pIn, r2, c2, tmp);
 82 };
 83 
 84 //Returns an upper and a lower triangular matrix which are L and R in the Gauss algorithm
 85 cc.kmMat4._gaussj = function (a, b) {
 86     var i, icol = 0, irow = 0, j, k, l, ll, n = 4, m = 4;
 87     var big, dum, pivinv;
 88     var indxc = [0, 0, 0, 0];
 89     var indxr = [0, 0, 0, 0];
 90     var ipiv = [0, 0, 0, 0];
 91 
 92     /*    for (j = 0; j < n; j++) {
 93      ipiv[j] = 0;
 94      }*/
 95 
 96     for (i = 0; i < n; i++) {
 97         big = 0.0;
 98         for (j = 0; j < n; j++) {
 99             if (ipiv[j] != 1) {
100                 for (k = 0; k < n; k++) {
101                     if (ipiv[k] == 0) {
102                         if (Math.abs(cc.kmMat4._get(a, j, k)) >= big) {
103                             big = Math.abs(cc.kmMat4._get(a, j, k));
104                             irow = j;
105                             icol = k;
106                         }
107                     }
108                 }
109             }
110         }
111         ++(ipiv[icol]);
112         if (irow != icol) {
113             for (l = 0; l < n; l++)
114                 cc.kmMat4._swap(a, irow, l, icol, l);
115             for (l = 0; l < m; l++)
116                 cc.kmMat4._swap(b, irow, l, icol, l);
117         }
118         indxr[i] = irow;
119         indxc[i] = icol;
120         if (cc.kmMat4._get(a, icol, icol) == 0.0)
121             return cc.KM_FALSE;
122 
123         pivinv = 1.0 / cc.kmMat4._get(a, icol, icol);
124         cc.kmMat4._set(a, icol, icol, 1.0);
125         for (l = 0; l < n; l++)
126             cc.kmMat4._set(a, icol, l, cc.kmMat4._get(a, icol, l) * pivinv);
127 
128         for (l = 0; l < m; l++)
129             cc.kmMat4._set(b, icol, l, cc.kmMat4._get(b, icol, l) * pivinv);
130 
131         for (ll = 0; ll < n; ll++) {
132             if (ll != icol) {
133                 dum = cc.kmMat4._get(a, ll, icol);
134                 cc.kmMat4._set(a, ll, icol, 0.0);
135                 for (l = 0; l < n; l++)
136                     cc.kmMat4._set(a, ll, l, cc.kmMat4._get(a, ll, l) - cc.kmMat4._get(a, icol, l) * dum);
137 
138                 for (l = 0; l < m; l++)
139                     cc.kmMat4._set(b, ll, l, cc.kmMat4._get(a, ll, l) - cc.kmMat4._get(b, icol, l) * dum);
140             }
141         }
142     }
143 //    This is the end of the main loop over columns of the reduction. It only remains to unscram-
144 //    ble the solution in view of the column interchanges. We do this by interchanging pairs of
145 //    columns in the reverse order that the permutation was built up.
146     for (l = n - 1; l >= 0; l--) {
147         if (indxr[l] != indxc[l]) {
148             for (k = 0; k < n; k++)
149                 cc.kmMat4._swap(a, k, indxr[l], k, indxc[l]);
150         }
151     }
152     return cc.KM_TRUE;
153 };
154 
155 cc.kmMat4._identity =
156     new Float32Array([1.0, 0.0, 0.0, 0.0,
157         0.0, 1.0, 0.0, 0.0,
158         0.0, 0.0, 1.0, 0.0,
159         0.0, 0.0, 0.0, 1.0]);
160 
161 /**
162  * Calculates the inverse of pM and stores the result in
163  * pOut.
164  * @Return Returns NULL if there is no inverse, else pOut
165  */
166 cc.kmMat4Inverse = function (pOut, pM) {
167     var inv = new cc.kmMat4();
168     var tmp = new cc.kmMat4();
169 
170     cc.kmMat4Assign(inv, pM);
171     cc.kmMat4Identity(tmp);
172 
173     if (cc.kmMat4._gaussj(inv, tmp) == cc.KM_FALSE)
174         return null;
175 
176     cc.kmMat4Assign(pOut, inv);
177     return pOut;
178 };
179 
180 /**
181  * Returns KM_TRUE if pIn is an identity matrix
182  * KM_FALSE otherwise
183  */
184 cc.kmMat4IsIdentity = function (pIn) {
185     for (var i = 0; i < 16; i++) {
186         if (cc.kmMat4._identity[i] != pIn.mat[i])
187             return false;
188     }
189     return true;
190 };
191 
192 /**
193  * Sets pOut to the transpose of pIn, returns pOut
194  */
195 cc.kmMat4Transpose = function (pOut, pIn) {
196     var x, z, outArr = pOut.mat,inArr = pIn.mat;
197     for (z = 0; z < 4; ++z) {
198         for (x = 0; x < 4; ++x)
199             outArr[(z * 4) + x] = inArr[(x * 4) + z];
200     }
201     return pOut;
202 };
203 
204 /**
205  * Multiplies pM1 with pM2, stores the result in pOut, returns pOut
206  */
207 cc.kmMat4Multiply = function (pOut, pM1, pM2) {
208     // Cache the matrix values (makes for huge speed increases!)
209     var  outArray = pOut.mat;
210     var a00 = pM1.mat[0], a01 = pM1.mat[1], a02 = pM1.mat[2], a03 = pM1.mat[3];
211     var a10 = pM1.mat[4], a11 = pM1.mat[5], a12 = pM1.mat[6], a13 = pM1.mat[7];
212     var a20 = pM1.mat[8], a21 = pM1.mat[9], a22 = pM1.mat[10], a23 = pM1.mat[11];
213     var a30 = pM1.mat[12], a31 = pM1.mat[13], a32 = pM1.mat[14], a33 = pM1.mat[15];
214 
215     var b00 = pM2.mat[0], b01 = pM2.mat[1], b02 = pM2.mat[2], b03 = pM2.mat[3];
216     var b10 = pM2.mat[4], b11 = pM2.mat[5], b12 = pM2.mat[6], b13 = pM2.mat[7];
217     var b20 = pM2.mat[8], b21 = pM2.mat[9], b22 = pM2.mat[10], b23 = pM2.mat[11];
218     var b30 = pM2.mat[12], b31 = pM2.mat[13], b32 = pM2.mat[14], b33 = pM2.mat[15];
219 
220     outArray[0] = b00 * a00 + b01 * a10 + b02 * a20 + b03 * a30;
221     outArray[1] = b00 * a01 + b01 * a11 + b02 * a21 + b03 * a31;
222     outArray[2] = b00 * a02 + b01 * a12 + b02 * a22 + b03 * a32;
223     outArray[3] = b00 * a03 + b01 * a13 + b02 * a23 + b03 * a33;
224     outArray[4] = b10 * a00 + b11 * a10 + b12 * a20 + b13 * a30;
225     outArray[5] = b10 * a01 + b11 * a11 + b12 * a21 + b13 * a31;
226     outArray[6] = b10 * a02 + b11 * a12 + b12 * a22 + b13 * a32;
227     outArray[7] = b10 * a03 + b11 * a13 + b12 * a23 + b13 * a33;
228     outArray[8] = b20 * a00 + b21 * a10 + b22 * a20 + b23 * a30;
229     outArray[9] = b20 * a01 + b21 * a11 + b22 * a21 + b23 * a31;
230     outArray[10] = b20 * a02 + b21 * a12 + b22 * a22 + b23 * a32;
231     outArray[11] = b20 * a03 + b21 * a13 + b22 * a23 + b23 * a33;
232     outArray[12] = b30 * a00 + b31 * a10 + b32 * a20 + b33 * a30;
233     outArray[13] = b30 * a01 + b31 * a11 + b32 * a21 + b33 * a31;
234     outArray[14] = b30 * a02 + b31 * a12 + b32 * a22 + b33 * a32;
235     outArray[15] = b30 * a03 + b31 * a13 + b32 * a23 + b33 * a33;
236     return pOut;
237 };
238 
239 cc.getMat4MultiplyValue = function (pM1, pM2) {
240     var m1 = pM1.mat, m2 = pM2.mat;
241     var mat = new Float32Array(16);
242 
243     mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3];
244     mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3];
245     mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3];
246     mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3];
247 
248     mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7];
249     mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7];
250     mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7];
251     mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7];
252 
253     mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11];
254     mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11];
255     mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11];
256     mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11];
257 
258     mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15];
259     mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15];
260     mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
261     mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
262 
263     return mat;
264 };
265 
266 cc.getMat4MultiplyWithMat4 = function (pM1, pM2, swapMat) {
267     var m1 = pM1.mat, m2 = pM2.mat;
268     var mat = swapMat.mat;
269 
270     mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3];
271     mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3];
272     mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3];
273     mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3];
274 
275     mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7];
276     mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7];
277     mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7];
278     mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7];
279 
280     mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11];
281     mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11];
282     mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11];
283     mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11];
284 
285     mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15];
286     mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15];
287     mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
288     mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
289 
290     return swapMat.mat;
291 };
292 
293 /**
294  * Assigns the value of pIn to pOut
295  */
296 cc.kmMat4Assign = function (pOut, pIn) {
297     if(pOut == pIn) {
298         cc.log("cc.kmMat4Assign(): pOut equals pIn");
299         return pOut;
300     }
301 
302     var outArr = pOut.mat;
303     var inArr = pIn.mat;
304 
305     outArr[0] = inArr[0];
306     outArr[1] = inArr[1];
307     outArr[2] = inArr[2];
308     outArr[3] = inArr[3];
309 
310     outArr[4] = inArr[4];
311     outArr[5] = inArr[5];
312     outArr[6] = inArr[6];
313     outArr[7] = inArr[7];
314 
315     outArr[8] = inArr[8];
316     outArr[9] = inArr[9];
317     outArr[10] = inArr[10];
318     outArr[11] = inArr[11];
319 
320     outArr[12] = inArr[12];
321     outArr[13] = inArr[13];
322     outArr[14] = inArr[14];
323     outArr[15] = inArr[15];
324     return pOut;
325 };
326 
327 /**
328  * Returns KM_TRUE if the 2 matrices are equal (approximately)
329  */
330 cc.kmMat4AreEqual = function (pMat1, pMat2) {
331     if(pMat1 == pMat2){
332         cc.log("cc.kmMat4AreEqual(): pMat1 and pMat2 are same object.");
333         return true;
334     }
335 
336     for (var i = 0; i < 16; i++) {
337         if (!(pMat1.mat[i] + cc.kmEpsilon > pMat2.mat[i] &&
338             pMat1.mat[i] - cc.kmEpsilon < pMat2.mat[i])) {
339             return false;
340         }
341     }
342     return true;
343 };
344 
345 /**
346  * Builds an X-axis rotation matrix and stores it in pOut, returns pOut
347  */
348 cc.kmMat4RotationX = function (pOut, radians) {
349     /*
350      |  1  0       0       0 |
351      M = |  0  cos(A) -sin(A)  0 |
352      |  0  sin(A)  cos(A)  0 |
353      |  0  0       0       1 |
354 
355      */
356 
357     pOut.mat[0] = 1.0;
358     pOut.mat[1] = 0.0;
359     pOut.mat[2] = 0.0;
360     pOut.mat[3] = 0.0;
361 
362     pOut.mat[4] = 0.0;
363     pOut.mat[5] = Math.cos(radians);
364     pOut.mat[6] = Math.sin(radians);
365     pOut.mat[7] = 0.0;
366 
367     pOut.mat[8] = 0.0;
368     pOut.mat[9] = -Math.sin(radians);
369     pOut.mat[10] = Math.cos(radians);
370     pOut.mat[11] = 0.0;
371 
372     pOut.mat[12] = 0.0;
373     pOut.mat[13] = 0.0;
374     pOut.mat[14] = 0.0;
375     pOut.mat[15] = 1.0;
376 
377     return pOut;
378 };
379 
380 /**
381  * Builds a rotation matrix using the rotation around the Y-axis
382  * The result is stored in pOut, pOut is returned.
383  */
384 cc.kmMat4RotationY = function (pOut, radians) {
385     /*
386      |  cos(A)  0   sin(A)  0 |
387      M = |  0       1   0       0 |
388      | -sin(A)  0   cos(A)  0 |
389      |  0       0   0       1 |
390      */
391     pOut.mat[0] = Math.cos(radians);
392     pOut.mat[1] = 0.0;
393     pOut.mat[2] = -Math.sin(radians);
394     pOut.mat[3] = 0.0;
395 
396     pOut.mat[4] = 0.0;
397     pOut.mat[5] = 1.0;
398     pOut.mat[6] = 0.0;
399     pOut.mat[7] = 0.0;
400 
401     pOut.mat[8] = Math.sin(radians);
402     pOut.mat[9] = 0.0;
403     pOut.mat[10] = Math.cos(radians);
404     pOut.mat[11] = 0.0;
405 
406     pOut.mat[12] = 0.0;
407     pOut.mat[13] = 0.0;
408     pOut.mat[14] = 0.0;
409     pOut.mat[15] = 1.0;
410 
411     return pOut;
412 };
413 
414 /**
415  * Builds a rotation matrix around the Z-axis. The resulting
416  * matrix is stored in pOut. pOut is returned.
417  */
418 cc.kmMat4RotationZ = function (pOut, radians) {
419     /*
420      |  cos(A)  -sin(A)   0   0 |
421      M = |  sin(A)   cos(A)   0   0 |
422      |  0        0        1   0 |
423      |  0        0        0   1 |
424      */
425     pOut.mat[0] = Math.cos(radians);
426     pOut.mat[1] = Math.sin(radians);
427     pOut.mat[2] = 0.0;
428     pOut.mat[3] = 0.0;
429 
430     pOut.mat[4] = -Math.sin(radians);
431     pOut.mat[5] = Math.cos(radians);
432     pOut.mat[6] = 0.0;
433     pOut.mat[7] = 0.0;
434 
435     pOut.mat[8] = 0.0;
436     pOut.mat[9] = 0.0;
437     pOut.mat[10] = 1.0;
438     pOut.mat[11] = 0.0;
439 
440     pOut.mat[12] = 0.0;
441     pOut.mat[13] = 0.0;
442     pOut.mat[14] = 0.0;
443     pOut.mat[15] = 1.0;
444 
445     return pOut;
446 };
447 
448 /**
449  * Builds a rotation matrix from pitch, yaw and roll. The resulting
450  * matrix is stored in pOut and pOut is returned
451  */
452 cc.kmMat4RotationPitchYawRoll = function (pOut, pitch, yaw, roll) {
453     var cr = Math.cos(pitch);
454     var sr = Math.sin(pitch);
455     var cp = Math.cos(yaw);
456     var sp = Math.sin(yaw);
457     var cy = Math.cos(roll);
458     var sy = Math.sin(roll);
459     var srsp = sr * sp;
460     var crsp = cr * sp;
461 
462     pOut.mat[0] = cp * cy;
463     pOut.mat[4] = cp * sy;
464     pOut.mat[8] = -sp;
465 
466     pOut.mat[1] = srsp * cy - cr * sy;
467     pOut.mat[5] = srsp * sy + cr * cy;
468     pOut.mat[9] = sr * cp;
469 
470     pOut.mat[2] = crsp * cy + sr * sy;
471     pOut.mat[6] = crsp * sy - sr * cy;
472     pOut.mat[10] = cr * cp;
473 
474     pOut.mat[3] = pOut.mat[7] = pOut.mat[11] = 0.0;
475     pOut.mat[15] = 1.0;
476 
477     return pOut;
478 };
479 
480 /** Converts a quaternion to a rotation matrix,
481  * the result is stored in pOut, returns pOut
482  */
483 cc.kmMat4RotationQuaternion = function (pOut, pQ) {
484     pOut.mat[0] = 1.0 - 2.0 * (pQ.y * pQ.y + pQ.z * pQ.z );
485     pOut.mat[1] = 2.0 * (pQ.x * pQ.y + pQ.z * pQ.w);
486     pOut.mat[2] = 2.0 * (pQ.x * pQ.z - pQ.y * pQ.w);
487     pOut.mat[3] = 0.0;
488 
489     // Second row
490     pOut.mat[4] = 2.0 * ( pQ.x * pQ.y - pQ.z * pQ.w );
491     pOut.mat[5] = 1.0 - 2.0 * ( pQ.x * pQ.x + pQ.z * pQ.z );
492     pOut.mat[6] = 2.0 * (pQ.z * pQ.y + pQ.x * pQ.w );
493     pOut.mat[7] = 0.0;
494 
495     // Third row
496     pOut.mat[8] = 2.0 * ( pQ.x * pQ.z + pQ.y * pQ.w );
497     pOut.mat[9] = 2.0 * ( pQ.y * pQ.z - pQ.x * pQ.w );
498     pOut.mat[10] = 1.0 - 2.0 * ( pQ.x * pQ.x + pQ.y * pQ.y );
499     pOut.mat[11] = 0.0;
500 
501     // Fourth row
502     pOut.mat[12] = 0;
503     pOut.mat[13] = 0;
504     pOut.mat[14] = 0;
505     pOut.mat[15] = 1.0;
506 
507     return pOut;
508 };
509 
510 /** Build a 4x4 OpenGL transformation matrix using a 3x3 rotation matrix,
511  * and a 3d vector representing a translation. Assign the result to pOut,
512  * pOut is also returned.
513  */
514 cc.kmMat4RotationTranslation = function (pOut, rotation, translation) {
515     pOut.mat[0] = rotation.mat[0];
516     pOut.mat[1] = rotation.mat[1];
517     pOut.mat[2] = rotation.mat[2];
518     pOut.mat[3] = 0.0;
519 
520     pOut.mat[4] = rotation.mat[3];
521     pOut.mat[5] = rotation.mat[4];
522     pOut.mat[6] = rotation.mat[5];
523     pOut.mat[7] = 0.0;
524 
525     pOut.mat[8] = rotation.mat[6];
526     pOut.mat[9] = rotation.mat[7];
527     pOut.mat[10] = rotation.mat[8];
528     pOut.mat[11] = 0.0;
529 
530     pOut.mat[12] = translation.x;
531     pOut.mat[13] = translation.y;
532     pOut.mat[14] = translation.z;
533     pOut.mat[15] = 1.0;
534 
535     return pOut;
536 };
537 
538 /** Builds a scaling matrix */
539 cc.kmMat4Scaling = function (pOut, x, y, z) {
540     pOut.mat[0] = x;
541     pOut.mat[5] = y;
542     pOut.mat[10] = z;
543     pOut.mat[15] = 1.0;
544     pOut.mat[1] = pOut.mat[2] = pOut.mat[3] =
545         pOut.mat[4] = pOut.mat[6] = pOut.mat[7] =
546             pOut.mat[8] = pOut.mat[9] = pOut.mat[11] =
547                 pOut.mat[12] = pOut.mat[13] = pOut.mat[14] = 0;
548     return pOut;
549 };
550 
551 /**
552  * Builds a translation matrix. All other elements in the matrix
553  * will be set to zero except for the diagonal which is set to 1.0
554  */
555 cc.kmMat4Translation = function (pOut, x, y, z) {
556     //FIXME: Write a test for this
557     pOut.mat[0] = pOut.mat[5] = pOut.mat[10] = pOut.mat[15] = 1.0;
558     pOut.mat[1] = pOut.mat[2] = pOut.mat[3] =
559         pOut.mat[4] = pOut.mat[6] = pOut.mat[7] =
560             pOut.mat[8] = pOut.mat[9] = pOut.mat[11] = 0.0;
561     pOut.mat[12] = x;
562     pOut.mat[13] = y;
563     pOut.mat[14] = z;
564     return pOut;
565 };
566 
567 /**
568  * Get the up vector from a matrix. pIn is the matrix you
569  * wish to extract the vector from. pOut is a pointer to the
570  * kmVec3 structure that should hold the resulting vector
571  */
572 cc.kmMat4GetUpVec3 = function (pOut, pIn) {
573     pOut.x = pIn.mat[4];
574     pOut.y = pIn.mat[5];
575     pOut.z = pIn.mat[6];
576     cc.kmVec3Normalize(pOut, pOut);
577     return pOut;
578 };
579 
580 /** Extract the right vector from a 4x4 matrix. The result is
581  * stored in pOut. Returns pOut.
582  */
583 cc.kmMat4GetRightVec3 = function (pOut, pIn) {
584     pOut.x = pIn.mat[0];
585     pOut.y = pIn.mat[1];
586     pOut.z = pIn.mat[2];
587     cc.kmVec3Normalize(pOut, pOut);
588     return pOut;
589 };
590 
591 /**
592  * Extract the forward vector from a 4x4 matrix. The result is
593  * stored in pOut. Returns pOut.
594  */
595 cc.kmMat4GetForwardVec3 = function (pOut, pIn) {
596     pOut.x = pIn.mat[8];
597     pOut.y = pIn.mat[9];
598     pOut.z = pIn.mat[10];
599     cc.kmVec3Normalize(pOut, pOut);
600     return pOut;
601 };
602 
603 /**
604  * Creates a perspective projection matrix in the
605  * same way as gluPerspective
606  */
607 cc.kmMat4PerspectiveProjection = function (pOut, fovY, aspect, zNear, zFar) {
608     var r = cc.kmDegreesToRadians(fovY / 2);
609     var deltaZ = zFar - zNear;
610     var s = Math.sin(r);
611 
612     if (deltaZ == 0 || s == 0 || aspect == 0)
613         return null;
614 
615     //cos(r) / sin(r) = cot(r)
616     var cotangent = Math.cos(r) / s;
617 
618     cc.kmMat4Identity(pOut);
619     pOut.mat[0] = cotangent / aspect;
620     pOut.mat[5] = cotangent;
621     pOut.mat[10] = -(zFar + zNear) / deltaZ;
622     pOut.mat[11] = -1;
623     pOut.mat[14] = -2 * zNear * zFar / deltaZ;
624     pOut.mat[15] = 0;
625 
626     return pOut;
627 };
628 
629 /** Creates an orthographic projection matrix like glOrtho */
630 cc.kmMat4OrthographicProjection = function (pOut, left, right, bottom, top, nearVal, farVal) {
631     cc.kmMat4Identity(pOut);
632     pOut.mat[0] = 2 / (right - left);
633     pOut.mat[5] = 2 / (top - bottom);
634     pOut.mat[10] = -2 / (farVal - nearVal);
635     pOut.mat[12] = -((right + left) / (right - left));
636     pOut.mat[13] = -((top + bottom) / (top - bottom));
637     pOut.mat[14] = -((farVal + nearVal) / (farVal - nearVal));
638     return pOut;
639 };
640 
641 /**
642  * Builds a translation matrix in the same way as gluLookAt()
643  * the resulting matrix is stored in pOut. pOut is returned.
644  */
645 cc.kmMat4LookAt = function (pOut, pEye, pCenter, pUp) {
646     var f = new cc.kmVec3(), up = new cc.kmVec3(), s = new cc.kmVec3(), u = new cc.kmVec3();
647     var translate = new cc.kmMat4();
648 
649     cc.kmVec3Subtract(f, pCenter, pEye);
650     cc.kmVec3Normalize(f, f);
651 
652     cc.kmVec3Assign(up, pUp);
653     cc.kmVec3Normalize(up, up);
654 
655     cc.kmVec3Cross(s, f, up);
656     cc.kmVec3Normalize(s, s);
657 
658     cc.kmVec3Cross(u, s, f);
659     cc.kmVec3Normalize(s, s);
660 
661     cc.kmMat4Identity(pOut);
662 
663     pOut.mat[0] = s.x;
664     pOut.mat[4] = s.y;
665     pOut.mat[8] = s.z;
666 
667     pOut.mat[1] = u.x;
668     pOut.mat[5] = u.y;
669     pOut.mat[9] = u.z;
670 
671     pOut.mat[2] = -f.x;
672     pOut.mat[6] = -f.y;
673     pOut.mat[10] = -f.z;
674 
675     cc.kmMat4Translation(translate, -pEye.x, -pEye.y, -pEye.z);
676     cc.kmMat4Multiply(pOut, pOut, translate);
677 
678     return pOut;
679 };
680 
681 /**
682  * Build a rotation matrix from an axis and an angle. Result is stored in pOut.
683  * pOut is returned.
684  */
685 cc.kmMat4RotationAxisAngle = function (pOut, axis, radians) {
686     var rcos = Math.cos(radians);
687     var rsin = Math.sin(radians);
688 
689     var normalizedAxis = new cc.kmVec3();
690     cc.kmVec3Normalize(normalizedAxis, axis);
691 
692     pOut.mat[0] = rcos + normalizedAxis.x * normalizedAxis.x * (1 - rcos);
693     pOut.mat[1] = normalizedAxis.z * rsin + normalizedAxis.y * normalizedAxis.x * (1 - rcos);
694     pOut.mat[2] = -normalizedAxis.y * rsin + normalizedAxis.z * normalizedAxis.x * (1 - rcos);
695     pOut.mat[3] = 0.0;
696 
697     pOut.mat[4] = -normalizedAxis.z * rsin + normalizedAxis.x * normalizedAxis.y * (1 - rcos);
698     pOut.mat[5] = rcos + normalizedAxis.y * normalizedAxis.y * (1 - rcos);
699     pOut.mat[6] = normalizedAxis.x * rsin + normalizedAxis.z * normalizedAxis.y * (1 - rcos);
700     pOut.mat[7] = 0.0;
701 
702     pOut.mat[8] = normalizedAxis.y * rsin + normalizedAxis.x * normalizedAxis.z * (1 - rcos);
703     pOut.mat[9] = -normalizedAxis.x * rsin + normalizedAxis.y * normalizedAxis.z * (1 - rcos);
704     pOut.mat[10] = rcos + normalizedAxis.z * normalizedAxis.z * (1 - rcos);
705     pOut.mat[11] = 0.0;
706 
707     pOut.mat[12] = 0.0;
708     pOut.mat[13] = 0.0;
709     pOut.mat[14] = 0.0;
710     pOut.mat[15] = 1.0;
711 
712     return pOut;
713 };
714 
715 /**
716  * Extract a 3x3 rotation matrix from the input 4x4 transformation.
717  * Stores the result in pOut, returns pOut
718  */
719 cc.kmMat4ExtractRotation = function (pOut, pIn) {
720     pOut.mat[0] = pIn.mat[0];
721     pOut.mat[1] = pIn.mat[1];
722     pOut.mat[2] = pIn.mat[2];
723 
724     pOut.mat[3] = pIn.mat[4];
725     pOut.mat[4] = pIn.mat[5];
726     pOut.mat[5] = pIn.mat[6];
727 
728     pOut.mat[6] = pIn.mat[8];
729     pOut.mat[7] = pIn.mat[9];
730     pOut.mat[8] = pIn.mat[10];
731 
732     return pOut;
733 };
734 
735 cc.kmMat4ExtractPlane = function (pOut, pIn, plane) {
736     switch (plane) {
737         case cc.KM_PLANE_RIGHT:
738             pOut.a = pIn.mat[3] - pIn.mat[0];
739             pOut.b = pIn.mat[7] - pIn.mat[4];
740             pOut.c = pIn.mat[11] - pIn.mat[8];
741             pOut.d = pIn.mat[15] - pIn.mat[12];
742             break;
743         case cc.KM_PLANE_LEFT:
744             pOut.a = pIn.mat[3] + pIn.mat[0];
745             pOut.b = pIn.mat[7] + pIn.mat[4];
746             pOut.c = pIn.mat[11] + pIn.mat[8];
747             pOut.d = pIn.mat[15] + pIn.mat[12];
748             break;
749         case cc.KM_PLANE_BOTTOM:
750             pOut.a = pIn.mat[3] + pIn.mat[1];
751             pOut.b = pIn.mat[7] + pIn.mat[5];
752             pOut.c = pIn.mat[11] + pIn.mat[9];
753             pOut.d = pIn.mat[15] + pIn.mat[13];
754             break;
755         case cc.KM_PLANE_TOP:
756             pOut.a = pIn.mat[3] - pIn.mat[1];
757             pOut.b = pIn.mat[7] - pIn.mat[5];
758             pOut.c = pIn.mat[11] - pIn.mat[9];
759             pOut.d = pIn.mat[15] - pIn.mat[13];
760             break;
761         case cc.KM_PLANE_FAR:
762             pOut.a = pIn.mat[3] - pIn.mat[2];
763             pOut.b = pIn.mat[7] - pIn.mat[6];
764             pOut.c = pIn.mat[11] - pIn.mat[10];
765             pOut.d = pIn.mat[15] - pIn.mat[14];
766             break;
767         case cc.KM_PLANE_NEAR:
768             pOut.a = pIn.mat[3] + pIn.mat[2];
769             pOut.b = pIn.mat[7] + pIn.mat[6];
770             pOut.c = pIn.mat[11] + pIn.mat[10];
771             pOut.d = pIn.mat[15] + pIn.mat[14];
772             break;
773         default:
774             cc.log("cc.kmMat4ExtractPlane(): Invalid plane index");
775             break;
776     }
777 
778     var t = Math.sqrt(pOut.a * pOut.a +
779         pOut.b * pOut.b +
780         pOut.c * pOut.c);
781     pOut.a /= t;
782     pOut.b /= t;
783     pOut.c /= t;
784     pOut.d /= t;
785 
786     return pOut;
787 };
788 
789 /**
790  * Take the rotation from a 4x4 transformation matrix, and return it as an axis and an angle (in radians)
791  * returns the output axis.
792  */
793 cc.kmMat4RotationToAxisAngle = function (pAxis, radians, pIn) {
794     /*Surely not this easy?*/
795     var temp = new cc.kmQuaternion();
796     var rotation = new cc.kmMat3();
797     cc.kmMat4ExtractRotation(rotation, pIn);
798     cc.kmQuaternionRotationMatrix(temp, rotation);
799     cc.kmQuaternionToAxisAngle(temp, pAxis, radians);
800     return pAxis;
801 };
802 
803 
804 
805 
806 
807