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