1///////////////////////////////////////////////////////////////////////////////////////////////////
2// OpenGL Mathematics Copyright (c) 2005 - 2014 G-Truc Creation (www.g-truc.net)
3///////////////////////////////////////////////////////////////////////////////////////////////////
4// Created : 2005-12-21
5// Updated : 2008-11-27
6// Licence : This source is under MIT License
7// File    : glm/gtx/quaternion.inl
8///////////////////////////////////////////////////////////////////////////////////////////////////
9
10#include <limits>
11
12namespace glm
13{
14	template <typename T, precision P>
15	GLM_FUNC_QUALIFIER detail::tvec3<T, P> cross
16	(
17		detail::tvec3<T, P> const & v,
18		detail::tquat<T, P> const & q
19	)
20	{
21		return inverse(q) * v;
22	}
23
24	template <typename T, precision P>
25	GLM_FUNC_QUALIFIER detail::tvec3<T, P> cross
26	(
27		detail::tquat<T, P> const & q,
28		detail::tvec3<T, P> const & v
29	)
30	{
31		return q * v;
32	}
33
34	template <typename T, precision P>
35	GLM_FUNC_QUALIFIER detail::tquat<T, P> squad
36	(
37		detail::tquat<T, P> const & q1,
38		detail::tquat<T, P> const & q2,
39		detail::tquat<T, P> const & s1,
40		detail::tquat<T, P> const & s2,
41		T const & h)
42	{
43		return mix(mix(q1, q2, h), mix(s1, s2, h), T(2) * (T(1) - h) * h);
44	}
45
46	template <typename T, precision P>
47	GLM_FUNC_QUALIFIER detail::tquat<T, P> intermediate
48	(
49		detail::tquat<T, P> const & prev,
50		detail::tquat<T, P> const & curr,
51		detail::tquat<T, P> const & next
52	)
53	{
54		detail::tquat<T, P> invQuat = inverse(curr);
55		return exp((log(next + invQuat) + log(prev + invQuat)) / T(-4)) * curr;
56	}
57
58	template <typename T, precision P>
59	GLM_FUNC_QUALIFIER detail::tquat<T, P> exp
60	(
61		detail::tquat<T, P> const & q
62	)
63	{
64		detail::tvec3<T, P> u(q.x, q.y, q.z);
65		float Angle = glm::length(u);
66		detail::tvec3<T, P> v(u / Angle);
67		return detail::tquat<T, P>(cos(Angle), sin(Angle) * v);
68	}
69
70	template <typename T, precision P>
71	GLM_FUNC_QUALIFIER detail::tquat<T, P> log
72	(
73		detail::tquat<T, P> const & q
74	)
75	{
76		if((q.x == static_cast<T>(0)) && (q.y == static_cast<T>(0)) && (q.z == static_cast<T>(0)))
77		{
78			if(q.w > T(0))
79				return detail::tquat<T, P>(log(q.w), T(0), T(0), T(0));
80			else if(q.w < T(0))
81				return detail::tquat<T, P>(log(-q.w), T(3.1415926535897932384626433832795), T(0),T(0));
82			else
83				return detail::tquat<T, P>(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity());
84		}
85		else
86		{
87			T Vec3Len = sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
88			T QuatLen = sqrt(Vec3Len * Vec3Len + q.w * q.w);
89			T t = atan(Vec3Len, T(q.w)) / Vec3Len;
90			return detail::tquat<T, P>(t * q.x, t * q.y, t * q.z, log(QuatLen));
91		}
92	}
93
94	template <typename T, precision P>
95	GLM_FUNC_QUALIFIER detail::tquat<T, P> pow
96	(
97		detail::tquat<T, P> const & x,
98		T const & y
99	)
100	{
101		if(abs(x.w) > T(0.9999))
102			return x;
103		float Angle = acos(y);
104		float NewAngle = Angle * y;
105		float Div = sin(NewAngle) / sin(Angle);
106		return detail::tquat<T, P>(
107			cos(NewAngle),
108			x.x * Div,
109			x.y * Div,
110			x.z * Div);
111	}
112
113	//template <typename T, precision P>
114	//GLM_FUNC_QUALIFIER detail::tquat<T, P> sqrt
115	//(
116	//	detail::tquat<T, P> const & q
117	//)
118	//{
119	//	T q0 = static_cast<T>(1) - dot(q, q);
120	//	return T(2) * (T(1) + q0) * q;
121	//}
122
123	template <typename T, precision P>
124	GLM_FUNC_QUALIFIER detail::tvec3<T, P> rotate
125	(
126		detail::tquat<T, P> const & q,
127		detail::tvec3<T, P> const & v
128	)
129	{
130		return q * v;
131	}
132
133	template <typename T, precision P>
134	GLM_FUNC_QUALIFIER detail::tvec4<T, P> rotate
135	(
136		detail::tquat<T, P> const & q,
137		detail::tvec4<T, P> const & v
138	)
139	{
140		return q * v;
141	}
142
143	template <typename T, precision P>
144	GLM_FUNC_QUALIFIER T extractRealComponent
145	(
146		detail::tquat<T, P> const & q
147	)
148	{
149		T w = static_cast<T>(1.0) - q.x * q.x - q.y * q.y - q.z * q.z;
150		if(w < T(0))
151			return T(0);
152		else
153			return -sqrt(w);
154	}
155
156	template <typename T, precision P>
157	GLM_FUNC_QUALIFIER T length2
158	(
159		detail::tquat<T, P> const & q
160	)
161	{
162		return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
163	}
164
165	template <typename T, precision P>
166	GLM_FUNC_QUALIFIER detail::tquat<T, P> shortMix
167	(
168		detail::tquat<T, P> const & x,
169		detail::tquat<T, P> const & y,
170		T const & a
171	)
172	{
173		if(a <= T(0)) return x;
174		if(a >= T(1)) return y;
175
176		T fCos = dot(x, y);
177		detail::tquat<T, P> y2(y); //BUG!!! tquat<T> y2;
178		if(fCos < T(0))
179		{
180			y2 = -y;
181			fCos = -fCos;
182		}
183
184		//if(fCos > 1.0f) // problem
185		T k0, k1;
186		if(fCos > T(0.9999))
187		{
188			k0 = static_cast<T>(1) - a;
189			k1 = static_cast<T>(0) + a; //BUG!!! 1.0f + a;
190		}
191		else
192		{
193			T fSin = sqrt(T(1) - fCos * fCos);
194			T fAngle = atan(fSin, fCos);
195			T fOneOverSin = static_cast<T>(1) / fSin;
196			k0 = sin((T(1) - a) * fAngle) * fOneOverSin;
197			k1 = sin((T(0) + a) * fAngle) * fOneOverSin;
198		}
199
200		return detail::tquat<T, P>(
201			k0 * x.w + k1 * y2.w,
202			k0 * x.x + k1 * y2.x,
203			k0 * x.y + k1 * y2.y,
204			k0 * x.z + k1 * y2.z);
205	}
206
207	template <typename T, precision P>
208	GLM_FUNC_QUALIFIER detail::tquat<T, P> fastMix
209	(
210		detail::tquat<T, P> const & x,
211		detail::tquat<T, P> const & y,
212		T const & a
213	)
214	{
215		return glm::normalize(x * (T(1) - a) + (y * a));
216	}
217
218	template <typename T, precision P>
219	GLM_FUNC_QUALIFIER detail::tquat<T, P> rotation
220	(
221		detail::tvec3<T, P> const & orig,
222		detail::tvec3<T, P> const & dest
223	)
224	{
225		T cosTheta = dot(orig, dest);
226		detail::tvec3<T, P> rotationAxis;
227
228		if(cosTheta < T(-1) + epsilon<T>())
229		{
230			// special case when vectors in opposite directions :
231			// there is no "ideal" rotation axis
232			// So guess one; any will do as long as it's perpendicular to start
233			// This implementation favors a rotation around the Up axis (Y),
234			// since it's often what you want to do.
235			rotationAxis = cross(detail::tvec3<T, P>(0, 0, 1), orig);
236			if(length2(rotationAxis) < epsilon<T>()) // bad luck, they were parallel, try again!
237				rotationAxis = cross(detail::tvec3<T, P>(1, 0, 0), orig);
238
239			rotationAxis = normalize(rotationAxis);
240			return angleAxis(pi<T>(), rotationAxis);
241		}
242
243		// Implementation from Stan Melax's Game Programming Gems 1 article
244		rotationAxis = cross(orig, dest);
245
246		T s = sqrt((T(1) + cosTheta) * T(2));
247		T invs = static_cast<T>(1) / s;
248
249		return detail::tquat<T, P>(
250			s * T(0.5f),
251			rotationAxis.x * invs,
252			rotationAxis.y * invs,
253			rotationAxis.z * invs);
254	}
255
256}//namespace glm
257