nuclear@10: /*
nuclear@10: libvmath - a vector math library
nuclear@10: Copyright (C) 2004-2015 John Tsiombikas <nuclear@member.fsf.org>
nuclear@10: 
nuclear@10: This program is free software: you can redistribute it and/or modify
nuclear@10: it under the terms of the GNU Lesser General Public License as published
nuclear@10: by the Free Software Foundation, either version 3 of the License, or
nuclear@10: (at your option) any later version.
nuclear@10: 
nuclear@10: This program is distributed in the hope that it will be useful,
nuclear@10: but WITHOUT ANY WARRANTY; without even the implied warranty of
nuclear@10: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
nuclear@10: GNU Lesser General Public License for more details.
nuclear@10: 
nuclear@10: You should have received a copy of the GNU Lesser General Public License
nuclear@10: along with this program.  If not, see <http://www.gnu.org/licenses/>.
nuclear@10: */
nuclear@10: 
nuclear@10: 
nuclear@10: #include <stdio.h>
nuclear@10: #include <math.h>
nuclear@10: #include "quat.h"
nuclear@10: 
nuclear@10: void quat_print(FILE *fp, quat_t q)
nuclear@10: {
nuclear@10: 	fprintf(fp, "([ %.4f %.4f %.4f ] %.4f)", q.x, q.y, q.z, q.w);
nuclear@10: }
nuclear@10: 
nuclear@10: quat_t quat_rotate(quat_t q, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
nuclear@10: {
nuclear@10: 	quat_t rq;
nuclear@10: 	scalar_t half_angle = angle * 0.5;
nuclear@10: 	scalar_t sin_half = sin(half_angle);
nuclear@10: 
nuclear@10: 	rq.w = cos(half_angle);
nuclear@10: 	rq.x = x * sin_half;
nuclear@10: 	rq.y = y * sin_half;
nuclear@10: 	rq.z = z * sin_half;
nuclear@10: 
nuclear@10: 	return quat_mul(q, rq);
nuclear@10: }
nuclear@10: 
nuclear@10: quat_t quat_rotate_quat(quat_t q, quat_t rotq)
nuclear@10: {
nuclear@10: 	return quat_mul(quat_mul(rotq, q), quat_conjugate(rotq));
nuclear@10: }
nuclear@10: 
nuclear@10: quat_t quat_slerp(quat_t q1, quat_t q2, scalar_t t)
nuclear@10: {
nuclear@10: 	quat_t res;
nuclear@10: 	scalar_t a, b, angle, sin_angle, dot;
nuclear@10: 
nuclear@10: 	dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
nuclear@10: 	if(dot < 0.0) {
nuclear@10: 		/* make sure we interpolate across the shortest arc */
nuclear@10: 		q1.x = -q1.x;
nuclear@10: 		q1.y = -q1.y;
nuclear@10: 		q1.z = -q1.z;
nuclear@10: 		q1.w = -q1.w;
nuclear@10: 		dot = -dot;
nuclear@10: 	}
nuclear@10: 
nuclear@10: 	/* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
nuclear@10: 	 * floating point imprecisions
nuclear@10: 	 */
nuclear@10: 	if(dot < -1.0) dot = -1.0;
nuclear@10: 	if(dot > 1.0) dot = 1.0;
nuclear@10: 
nuclear@10: 	angle = acos(dot);
nuclear@10: 	sin_angle = sin(angle);
nuclear@10: 
nuclear@10: 	if(fabs(sin_angle) < SMALL_NUMBER) {
nuclear@10: 		/* for very small angles or completely opposite orientations
nuclear@10: 		 * use linear interpolation to avoid div/zero (in the first case it makes sense,
nuclear@10: 		 * the second case is pretty much undefined anyway I guess ...
nuclear@10: 		 */
nuclear@10: 		a = 1.0f - t;
nuclear@10: 		b = t;
nuclear@10: 	} else {
nuclear@10: 		a = sin((1.0f - t) * angle) / sin_angle;
nuclear@10: 		b = sin(t * angle) / sin_angle;
nuclear@10: 	}
nuclear@10: 
nuclear@10: 	res.x = q1.x * a + q2.x * b;
nuclear@10: 	res.y = q1.y * a + q2.y * b;
nuclear@10: 	res.z = q1.z * a + q2.z * b;
nuclear@10: 	res.w = q1.w * a + q2.w * b;
nuclear@10: 	return res;
nuclear@10: }