1 /*
  2   Copyright (c) 2008 Seneca College
  3   Licenced under the MIT License (http://www.c3dl.org/index.php/mit-license/)
  4 */
  5 // Written By:	Mark Paruzel
  6 // Date:		April 20, 2008
  7 // Project:		Canvas 3D Library
  8 
  9 // -----------------------------------------------------------------------------
 10 // NOTE: This group of functions act uppon an array of four values which
 11 //       represent a Quaternion in 3D space. The values of the array each hold 
 12 //       the values of the W, X, Y, and Z values of a quaternion.
 13 // -----------------------------------------------------------------------------
 14 
 15 /**
 16 	Is the object 'quat' a valid Quaternion?
 17 
 18 	@param {Array} quat
 19 
 20 	@returns {boolean} True if 'quat' is a valid Quaternion, false 
 21 	otherwise.
 22 */
 23 c3dl.isValidQuat = function(quat)
 24 {
 25 	// All math types are arrays instead of objects for performance reasons.
 26 	if (quat instanceof Array)
 27 	{
 28 		// Must be 4 values long
 29 		if (quat.length == 4)
 30 		{
 31 			for (var i = 0; i < 4; i++)
 32 			{
 33 				// Check for bad values
 34 				if (isNaN(quat[i])) 
 35 					return false;
 36 			}
 37 
 38 			return true;			
 39 		}
 40 	}
 41 
 42 	// if not instance of an Array
 43 	return false;
 44 }
 45 
 46 /**
 47 	Make a Quaternion from the arguments.
 48 
 49 	@param {float} newW
 50 	@param {float} newX
 51 	@param {float} newY
 52 	@param {float} newZ
 53 
 54 	@returns {Array} A Quaternion defined by the passed 
 55 	in arguments.
 56 */
 57 c3dl.makeQuat = function(newW, newX, newY, newZ)
 58 {
 59     // Quat = W + X * i + Y * j + Z * k 
 60 	var quat = [];
 61 	
 62 	quat[0] = !isNaN(newW) ? parseFloat(newW) : 0.0; // W
 63 	quat[1] = !isNaN(newX) ? parseFloat(newX) : 0.0; // X
 64 	quat[2] = !isNaN(newY) ? parseFloat(newY) : 0.0; // Y
 65 	quat[3] = !isNaN(newZ) ? parseFloat(newZ) : 0.0; // Z
 66 
 67 	return quat;
 68 }
 69 
 70 
 71 /**
 72 	Convert a Quaternion to a Matrix.
 73 	
 74 	@param {Array} quat
 75 	@param {Array} dest	
 76 
 77 	@returns {Array} A matrix
 78 */
 79 c3dl.quatToMatrix = function(quat, dest)
 80 {
 81 	if (!c3dl.isValidQuat(quat))
 82 	{
 83 		c3dl.debug.logWarning('quatToMatrix() called with the first parameter not a quaternion');
 84 		return null;
 85 	}
 86 	
 87 	var quatToMatrixTx  = 2.0 * quat[1];
 88 	var quatToMatrixTy  = 2.0 * quat[2];
 89 	var quatToMatrixTz  = 2.0 * quat[3];
 90 	var quatToMatrixTwx = quatToMatrixTx * quat[0];
 91 	var quatToMatrixTwy = quatToMatrixTy * quat[0];
 92 	var quatToMatrixTwz = quatToMatrixTz * quat[0];
 93 	var quatToMatrixTxx = quatToMatrixTx * quat[1];
 94 	var quatToMatrixTxy = quatToMatrixTy * quat[1];
 95 	var quatToMatrixTxz = quatToMatrixTz * quat[1];
 96 	var quatToMatrixTyy = quatToMatrixTy * quat[2];
 97 	var quatToMatrixTyz = quatToMatrixTz * quat[2];
 98 	var quatToMatrixTzz = quatToMatrixTz * quat[3];
 99 
100 	if (dest)
101 	{
102 		dest[0] = 1.0 - (quatToMatrixTyy + quatToMatrixTzz);
103 		dest[1] = quatToMatrixTxy - quatToMatrixTwz;
104 		dest[2] = quatToMatrixTxz + quatToMatrixTwy;
105 		dest[3] = 0.0;
106 		dest[4] = quatToMatrixTxy + quatToMatrixTwz;
107 		dest[5] = 1.0 - (quatToMatrixTxx + quatToMatrixTzz);
108 		dest[6] = quatToMatrixTyz - quatToMatrixTwx;
109 		dest[7] = 0.0;
110 		dest[8] = quatToMatrixTxz - quatToMatrixTwy;
111 		dest[9] = quatToMatrixTyz + quatToMatrixTwx;
112 		dest[10] = 1.0 - (quatToMatrixTxx + quatToMatrixTyy);
113 		dest[11] = 0.0;
114 		dest[12] = 0.0;
115 		dest[13] = 0.0;
116 		dest[14] = 0.0;
117 		dest[15] = 1.0;
118 		
119 		return dest;
120 	}
121 	else
122 	{
123 		// Setup a new Matrix out of this quaternion
124 		var newMat = new Array(16);
125 
126 		newMat[0] = 1.0 - (quatToMatrixTyy + quatToMatrixTzz);
127 		newMat[1] = quatToMatrixTxy + quatToMatrixTwz;
128 		newMat[2] = quatToMatrixTxz - quatToMatrixTwy;
129 		newMat[3] = 0.0;
130 
131 		newMat[4] = quatToMatrixTxy - quatToMatrixTwz;
132 		newMat[5] = 1.0 - (quatToMatrixTxx + quatToMatrixTzz);
133 		newMat[6] = quatToMatrixTyz + quatToMatrixTwx;
134 		newMat[7] = 0.0;
135 
136 		newMat[8] = quatToMatrixTxz + quatToMatrixTwy;
137 		newMat[9] = quatToMatrixTyz - quatToMatrixTwx;
138 		newMat[10] = 1.0 - (quatToMatrixTxx + quatToMatrixTyy);
139 		newMat[11] = 0.0;
140 		
141 		newMat[12] = 0.0;
142 		newMat[13] = 0.0;
143 		newMat[14] = 0.0;
144 		newMat[15] = 1.0;
145 		return newMat;
146 	}
147 }
148 
149 
150 /**
151 	@param {Array} axisVec
152 	@param {float} angleScalar
153 */
154 c3dl.quatToAxisAngle = function(axisVec, angleScalar)
155 {
156 	axisVec = makeVector();
157 	
158 	// Get the Length squared
159 	var sqLength = c3dl.quatLengthSq();
160 	
161 	// If length is very small, its basically touching a world Axis
162 	if (sqLength > c3dl.TOLERANCE)
163 	{
164 		var invLength = c3dl.invSqrt(sqLength);
165 		
166 		// Set the Angle
167 		angleScalar = 2.0 * Math.acos(quat[0]);
168 		
169 		// Set the Vector
170 		axisVec = [quat[1] * invLength, quat[2] * invLength, quat[3] * invLength];
171 	}
172 	else
173 	{
174 		// Set world Axis
175 		angleScalar = 0.0;
176 		axisVec = [1.0, 0.0, 0.0];
177 	}	
178 }
179 
180 
181 /**
182 	Convert an axis-angle vector to a Quaternion.
183 
184 	@param {Array} axisVec
185 	@param {float} angleScalar
186 	@param {Array} dest
187 
188 	@returns {Array}
189 
190 */
191 c3dl.axisAngleToQuathalfAngle;
192 c3dl.axisAngleToQuats;
193 c3dl.axisAngleToQuat = function(axisVec, angleScalar, dest)
194 {
195 	if (!c3dl.isValidVector(axisVec))
196 	{
197 		c3dl.debug.logWarning('axisAngleToQuat() called with the first parameter not a vector');
198 		return null;
199 	}
200 	
201 	if (isNaN(angleScalar))
202 	{
203 		c3dl.debug.logWarning('axisAngleToQuat() called with the second parameter not a number');
204 		return null;
205 	}
206 	
207 	// q = cos(A/2) + sin(A/2) * (x*i + y*j + z*k)
208 	c3dl.axisAngleToQuathalfAngle = 0.5 * angleScalar;
209 	c3dl.axisAngleToQuats = Math.sin(c3dl.axisAngleToQuathalfAngle);
210 	
211 	if (dest)
212 	{
213 		dest[0] = Math.cos(c3dl.axisAngleToQuathalfAngle);
214 		dest[1] = c3dl.axisAngleToQuats * axisVec[0];
215 		dest[2] = c3dl.axisAngleToQuats * axisVec[1];
216 		dest[3] = c3dl.axisAngleToQuats * axisVec[2];
217 		return dest;
218 	}
219 	else
220 	{
221 		var quat = c3dl.makeQuat(Math.cos(c3dl.axisAngleToQuathalfAngle),
222 							c3dl.axisAngleToQuats * axisVec[0],
223 							c3dl.axisAngleToQuats * axisVec[1],
224 							c3dl.axisAngleToQuats * axisVec[2]);
225 		return quat;
226 	}
227 }
228 
229 
230 /**
231 	Convert a Matrix to a Quaternion.
232 
233 	@param {Array} newMat
234 
235 	@returns {Array}
236 */
237 c3dl.matrixToQuat = function(newMat)
238 {
239 	if (c3dl.isValidMatrix(newMat))
240 	{
241 		var quat = c3dl.makeQuat();
242 		var trace = newMat[0] + newMat[5] + newMat[10] + 1;
243 		var sqTrace;
244 		var s;
245 		
246 		if (trace > 0.0)
247 		{
248 			sqTrace = Math.sqrt(trace);
249 			
250 			s = 0.5 / sqTrace;
251 			quat[0] = 0.25 / s;
252 			quat[1] = (newMat[6] - newMat[9]) * s;
253 			quat[2] = (newMat[8] - newMat[2]) * s;
254 			quat[3] = (newMat[1] - newMat[4]) * s;
255 			
256 		}else{
257 			if (newMat[0] > newMat[5] && newMat[0] > newMat[10])
258 			{
259 				s = 2.0 * Math.sqrt(1.0 + newMat[0] - newMat[5] - newMat[10]);
260 				quat[1] = 0.25 * s;
261 				quat[2] = (newMat[1] - newMat[4]) / s;
262 				quat[3] = (newMat[2] - newMat[8]) / s;
263 				quat[0] = (newMat[9] - newMat[6]) / s;
264 			}else if(newMat[5] > newMat[10])
265 			{
266 				s = 2.0 * Math.sqrt(1.0 + newMat[5] - newMat[0] - newMat[10]);
267 				quat[1] = (newMat[1] - newMat[4]) / s;
268 				quat[2] = 0.25 * s;
269 				quat[3] = (newMat[9] - newMat[6]) / s;
270 				quat[0] = (newMat[2] - newMat[8]) / s;
271 			}else{
272 				s = 2.0 * Math.sqrt(1.0 + newMat[10] - newMat[0] - newMat[5]);
273 				quat[1] = (newMat[2] - newMat[8]) / s;
274 				quat[2] = (newMat[9] - newMat[6]) / s;
275 				quat[3] = 0.25 * s;
276 				quat[0] = (newMat[1] - newMat[4]) / s;
277 			}
278 		}
279 		
280 		return quat;
281 	}
282 
283 	c3dl.debug.logWarning('matrixToQuat() called with a parameter that\'s not a matrix');
284 	return null;
285 }
286 
287 
288 /**
289 	@param {Array} quat
290 
291 	@returns {float}
292 */
293 c3dl.quatLengthSq = function(quat)
294 {
295 	if (c3dl.isValidQuat(quat))
296 	{
297 		return quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3];
298 	}
299 	
300 	c3dl.debug.logWarning('quatLengthSq() called with a parameter that\'s not a quaternion');
301 	return null;
302 }
303 
304 
305 /**
306 	Get the length of Quaternion 'quat'.
307 
308 	@param {Array} quat
309 
310 	@returns {float} The length 'quat' or null if 'quat' was not a 
311 	valid Quaternion.
312 */
313 c3dl.quatLength = function(quat)
314 {
315 	if (c3dl.isValidQuat(quat))
316 	{
317 		return Math.sqrt(c3dl.quatLengthSq(quat));
318 	}
319 	
320 	c3dl.debug.logWarning('quatLength() called with a parameter that\'s not a quaternion');
321 	return null;
322 }
323 
324 
325 /**
326 	Add two quaternions.
327 
328 	@param {Array} quatOne
329 	@param {Array} quatTwo
330 
331 	@returns {Array} The result of adding quatOne and quatTwo.
332 */
333 c3dl.addQuats = function(quatOne, quatTwo)
334 {
335 	if (c3dl.isValidQuat(quatOne) && c3dl.isValidQuat(quatTwo))
336 	{
337 		var quat = c3dl.makeQuat();
338 		
339 		for (var i = 0; i < 4; i++)
340 			quat[i] = quatOne[i] + quatTwo[i];
341 	
342 		return quat;
343 	}
344 	
345 	c3dl.debug.logWarning('addQuats() called with parameters that are not quaternions');
346 	return null;	
347 }
348 
349 
350 /**
351 	Subtract Quaternion 'quatTwo' from 'quatOne'.
352 
353 	@param {Array} quatOne
354 	@param {Array} quatTwo
355 
356 	@returns {Array}
357 */
358 c3dl.subtractQuats = function(quatOne, quatTwo)
359 {
360 	if (c3dl.isValidQuat(quatOne) && c3dl.isValidQuat(quatTwo))
361 	{
362 		var quat = c3dl.makeQuat();
363 		
364 		for (var i = 0; i < 4; i++)
365 			quat[i] = quatOne[i] - quatTwo[i];
366 	
367 		return quat;
368 	}
369 	
370 	c3dl.debug.logWarning('addQuats() called with parameters that are not quaternions');
371 	return null;	
372 }
373 
374 
375 /**
376 	Multiply the Quaternion 'quatOne' with a scalar.
377 
378 	@param {Array} quatOne
379 	@param {float} scalar
380 
381 	@return {Array}
382 */
383 c3dl.multiplyQuatByScalar = function(quatOne, scalar)
384 {
385 	if (c3dl.isValidQuat(quatOne) && !isNaN(scalar))
386 	{
387 		var quat = c3dl.makeQuat();
388 		
389 		for (var i = 0; i < 4; i++)
390 			quat[i] = quatOne[i] * scalar; //!! Mark: is this supposed to be a multiply?
391 	
392 		return quat;
393 	}
394 	
395 	c3dl.debug.logWarning('addQuats() called with parameters that are not valid');
396 	return null;	
397 }
398 
399 
400 /**
401 	Get the conjugate of Quaternion 'quat'.
402 
403 	@param {Array} quat
404 
405 	return {Array}
406 */
407 c3dl.getQuatConjugate = function(quat)
408 {
409 	if (c3dl.isValidQuat(quat))
410 	{
411 		var nQt = c3dl.makeQuat();
412 		
413 		nQt = [quat[0], -quat[1], -quat[2], -quat[3]];
414 		return nQt;
415 	}
416 	
417 	c3dl.debug.logWarning('getQuatConjugate() called with a parameter that\'s not a quaternion');
418 	return null;
419 }
420 
421 
422 /**
423 	Dot Product
424 
425 	@param {Array} quatOne
426 	@param {Array} quatTwo
427 
428 	@returns {float}
429 */
430 c3dl.quatDotProduct = function(quatOne, quatTwo)
431 {
432 	if (c3dl.isValidQuat(quatOne) && c3dl.isValidQuat(quatTwo))
433 	{
434 		return quatOne[0] * quatTwo[0] + 
435 			   quatOne[1] * quatTwo[1] + 
436 			   quatOne[2] * quatTwo[2] + 
437 			   quatOne[3] * quatTwo[3];
438 	}
439 	
440 	c3dl.debug.logWarning('quatDotProduct() called with parameters that are not valid');
441 	return null;
442 }
443 
444 /**
445 	Get the normalized Quaternion of quat.
446 
447 	@param {Array} quat
448 
449 	@return {Array}
450 */
451 c3dl.normalizeQuat = function(quat)
452 {
453 	if (c3dl.isValidQuat(quat))
454 	{
455 		var newQuat = c3dl.makeQuat();
456 		var len = c3dl.quatLength(quat);
457 		var invLen = 1.0 / len;
458 		
459 		if (len > 0.001)
460 		{
461 			newQuat[0] = quat[0] * invLen;
462 			newQuat[1] = quat[1] * invLen;
463 			newQuat[2] = quat[2] * invLen;
464 			newQuat[3] = quat[3] * invLen;
465 		}
466 		else
467 		{
468 			// If Normalization Cannot be done
469 			newQuat[0] = 0.0;
470 			newQuat[1] = 0.0;
471 			newQuat[2] = 0.0;
472 			newQuat[3] = 0.0;
473 		}
474 		
475 		return newQuat;
476 	}
477 	
478 	c3dl.debug.logWarning('normalizeQuat() called with parameters that are not valid');
479 	return null;
480 }
481 
482 
483 /**
484 	Get the inverse of the Quaternion quat.
485 
486 	@param {Array} quat
487 
488 	@return {Array}
489 */
490 c3dl.inverseQuat = function(quat)
491 {
492 	if (c3dl.isValidQuat(quat))
493 	{
494 		var invQuat = c3dl.makeQuat();
495 		var norm = 0.0;
496 
497 		for (var i = 0; i < 4; i++)
498 		{
499 			norm += quat[i] * quat[i];
500 		}
501 		
502 		if (norm > 0.0)
503 		{
504 			var invNorm = 1.0 / norm;
505 			invQuat[0] = quat[0] * invNorm;
506 			invQuat[1] = -quat[1] * invNorm;
507 			invQuat[2] = -quat[2] * invNorm;
508 			invQuat[3] = -quat[3] * invNorm;
509 		}
510 
511 		return invQuat;
512 	}
513 
514 	c3dl.debug.logWarning('inverseQuat() called with parameters that are not valid');
515 	return null;	
516 }
517