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