3dphotoshoot
view libs/vmath/quat.cc @ 15:2d48f65da357
assman
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sun, 07 Jun 2015 20:40:37 +0300 |
| parents | |
| children |
line source
1 /*
2 libvmath - a vector math library
3 Copyright (C) 2004-2015 John Tsiombikas <nuclear@member.fsf.org>
5 This program is free software: you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as published
7 by the Free Software Foundation, either version 3 of the License, or
8 (at your option) any later version.
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
15 You should have received a copy of the GNU Lesser General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 */
18 #include "quat.h"
19 #include "vmath.h"
21 Quaternion::Quaternion()
22 {
23 s = 1.0;
24 v.x = v.y = v.z = 0.0;
25 }
27 Quaternion::Quaternion(scalar_t s, const Vector3 &v)
28 {
29 this->s = s;
30 this->v = v;
31 }
33 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z)
34 {
35 v.x = x;
36 v.y = y;
37 v.z = z;
38 this->s = s;
39 }
41 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle)
42 {
43 set_rotation(axis, angle);
44 }
46 Quaternion::Quaternion(const quat_t &quat)
47 {
48 v.x = quat.x;
49 v.y = quat.y;
50 v.z = quat.z;
51 s = quat.w;
52 }
54 Quaternion Quaternion::operator +(const Quaternion &quat) const
55 {
56 return Quaternion(s + quat.s, v + quat.v);
57 }
59 Quaternion Quaternion::operator -(const Quaternion &quat) const
60 {
61 return Quaternion(s - quat.s, v - quat.v);
62 }
64 Quaternion Quaternion::operator -() const
65 {
66 return Quaternion(-s, -v);
67 }
69 /** Quaternion Multiplication:
70 * Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2]
71 */
72 Quaternion Quaternion::operator *(const Quaternion &quat) const
73 {
74 Quaternion newq;
75 newq.s = s * quat.s - dot_product(v, quat.v);
76 newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
77 return newq;
78 }
80 void Quaternion::operator +=(const Quaternion &quat)
81 {
82 *this = Quaternion(s + quat.s, v + quat.v);
83 }
85 void Quaternion::operator -=(const Quaternion &quat)
86 {
87 *this = Quaternion(s - quat.s, v - quat.v);
88 }
90 void Quaternion::operator *=(const Quaternion &quat)
91 {
92 *this = *this * quat;
93 }
95 void Quaternion::reset_identity()
96 {
97 s = 1.0;
98 v.x = v.y = v.z = 0.0;
99 }
101 Quaternion Quaternion::conjugate() const
102 {
103 return Quaternion(s, -v);
104 }
106 scalar_t Quaternion::length() const
107 {
108 return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
109 }
111 /** Q * ~Q = ||Q||^2 */
112 scalar_t Quaternion::length_sq() const
113 {
114 return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
115 }
117 void Quaternion::normalize()
118 {
119 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
120 v.x /= len;
121 v.y /= len;
122 v.z /= len;
123 s /= len;
124 }
126 Quaternion Quaternion::normalized() const
127 {
128 Quaternion nq = *this;
129 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
130 nq.v.x /= len;
131 nq.v.y /= len;
132 nq.v.z /= len;
133 nq.s /= len;
134 return nq;
135 }
137 /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
138 Quaternion Quaternion::inverse() const
139 {
140 Quaternion inv = conjugate();
141 scalar_t lensq = length_sq();
142 inv.v /= lensq;
143 inv.s /= lensq;
145 return inv;
146 }
149 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle)
150 {
151 scalar_t half_angle = angle / 2.0;
152 s = cos(half_angle);
153 v = axis * sin(half_angle);
154 }
156 void Quaternion::rotate(const Vector3 &axis, scalar_t angle)
157 {
158 Quaternion q;
159 scalar_t half_angle = angle / 2.0;
160 q.s = cos(half_angle);
161 q.v = axis * sin(half_angle);
163 *this *= q;
164 }
166 void Quaternion::rotate(const Quaternion &q)
167 {
168 *this = q * *this * q.conjugate();
169 }
171 Matrix3x3 Quaternion::get_rotation_matrix() const
172 {
173 return Matrix3x3(
174 1.0 - 2.0 * v.y*v.y - 2.0 * v.z*v.z, 2.0 * v.x * v.y - 2.0 * s * v.z, 2.0 * v.z * v.x + 2.0 * s * v.y,
175 2.0 * v.x * v.y + 2.0 * s * v.z, 1.0 - 2.0 * v.x*v.x - 2.0 * v.z*v.z, 2.0 * v.y * v.z - 2.0 * s * v.x,
176 2.0 * v.z * v.x - 2.0 * s * v.y, 2.0 * v.y * v.z + 2.0 * s * v.x, 1.0 - 2.0 * v.x*v.x - 2.0 * v.y*v.y);
177 }
180 /** Spherical linear interpolation (slerp) */
181 Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, scalar_t t)
182 {
183 Quaternion q1 = quat1;
184 scalar_t dot = q1.s * q2.s + q1.v.x * q2.v.x + q1.v.y * q2.v.y + q1.v.z * q2.v.z;
186 if(dot < 0.0) {
187 /* make sure we interpolate across the shortest arc */
188 q1 = -quat1;
189 dot = -dot;
190 }
192 /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
193 * floating point imprecisions
194 */
195 if(dot < -1.0) dot = -1.0;
196 if(dot > 1.0) dot = 1.0;
198 scalar_t angle = acos(dot);
199 scalar_t a, b;
201 scalar_t sin_angle = sin(angle);
202 if(fabs(sin_angle) < SMALL_NUMBER) {
203 /* for very small angles or completely opposite orientations
204 * use linear interpolation to avoid div/zero (in the first case it makes sense,
205 * the second case is pretty much undefined anyway I guess ...
206 */
207 a = 1.0f - t;
208 b = t;
209 } else {
210 a = sin((1.0f - t) * angle) / sin_angle;
211 b = sin(t * angle) / sin_angle;
212 }
214 scalar_t x = q1.v.x * a + q2.v.x * b;
215 scalar_t y = q1.v.y * a + q2.v.y * b;
216 scalar_t z = q1.v.z * a + q2.v.z * b;
217 scalar_t s = q1.s * a + q2.s * b;
219 return Quaternion(s, Vector3(x, y, z));
220 }
223 /*
224 std::ostream &operator <<(std::ostream &out, const Quaternion &q)
225 {
226 out << "(" << q.s << ", " << q.v << ")";
227 return out;
228 }
229 */