gpuray_glsl

view vmath/quat.cc @ 5:000017955721

find changesets by author, revision, files, or words in the commit message
fixed the cone normal
author John Tsiombikas <nuclear@member.fsf.org>
date Tue, 11 Nov 2014 20:25:59 +0200
parents
children
line source
1 #include "quat.h"
2 #include "vmath.h"
3
4 Quaternion::Quaternion() {
5 s = 1.0;
6 v.x = v.y = v.z = 0.0;
7 }
8
9 Quaternion::Quaternion(scalar_t s, const Vector3 &v) {
10 this->s = s;
11 this->v = v;
12 }
13
14 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) {
15 v.x = x;
16 v.y = y;
17 v.z = z;
18 this->s = s;
19 }
20
21 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) {
22 set_rotation(axis, angle);
23 }
24
25 Quaternion::Quaternion(const quat_t &quat)
26 {
27 v.x = quat.x;
28 v.y = quat.y;
29 v.z = quat.z;
30 s = quat.w;
31 }
32
33 Quaternion Quaternion::operator +(const Quaternion &quat) const
34 {
35 return Quaternion(s + quat.s, v + quat.v);
36 }
37
38 Quaternion Quaternion::operator -(const Quaternion &quat) const
39 {
40 return Quaternion(s - quat.s, v - quat.v);
41 }
42
43 Quaternion Quaternion::operator -() const
44 {
45 return Quaternion(-s, -v);
46 }
47
48 /** Quaternion Multiplication:
49 * Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2]
50 */
51 Quaternion Quaternion::operator *(const Quaternion &quat) const {
52 Quaternion newq;
53 newq.s = s * quat.s - dot_product(v, quat.v);
54 newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
55 return newq;
56 }
57
58 void Quaternion::operator +=(const Quaternion &quat) {
59 *this = Quaternion(s + quat.s, v + quat.v);
60 }
61
62 void Quaternion::operator -=(const Quaternion &quat) {
63 *this = Quaternion(s - quat.s, v - quat.v);
64 }
65
66 void Quaternion::operator *=(const Quaternion &quat) {
67 *this = *this * quat;
68 }
69
70 void Quaternion::reset_identity() {
71 s = 1.0;
72 v.x = v.y = v.z = 0.0;
73 }
74
75 Quaternion Quaternion::conjugate() const {
76 return Quaternion(s, -v);
77 }
78
79 scalar_t Quaternion::length() const {
80 return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
81 }
82
83 /** Q * ~Q = ||Q||^2 */
84 scalar_t Quaternion::length_sq() const {
85 return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
86 }
87
88 void Quaternion::normalize() {
89 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
90 v.x /= len;
91 v.y /= len;
92 v.z /= len;
93 s /= len;
94 }
95
96 Quaternion Quaternion::normalized() const {
97 Quaternion nq = *this;
98 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
99 nq.v.x /= len;
100 nq.v.y /= len;
101 nq.v.z /= len;
102 nq.s /= len;
103 return nq;
104 }
105
106 /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
107 Quaternion Quaternion::inverse() const {
108 Quaternion inv = conjugate();
109 scalar_t lensq = length_sq();
110 inv.v /= lensq;
111 inv.s /= lensq;
112
113 return inv;
114 }
115
116
117 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) {
118 scalar_t half_angle = angle / 2.0;
119 s = cos(half_angle);
120 v = axis * sin(half_angle);
121 }
122
123 void Quaternion::rotate(const Vector3 &axis, scalar_t angle) {
124 Quaternion q;
125 scalar_t half_angle = angle / 2.0;
126 q.s = cos(half_angle);
127 q.v = axis * sin(half_angle);
128
129 *this *= q;
130 }
131
132 void Quaternion::rotate(const Quaternion &q) {
133 *this = q * *this * q.conjugate();
134 }
135
136 Matrix3x3 Quaternion::get_rotation_matrix() const {
137 return Matrix3x3(
138 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,
139 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,
140 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);
141 }
142
143
144 /** Spherical linear interpolation (slerp) */
145 Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, scalar_t t) {
146 Quaternion q1;
147 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;
148
149 if(dot < 0.0) {
150 /* make sure we interpolate across the shortest arc */
151 q1 = -quat1;
152 dot = -dot;
153 } else {
154 q1 = quat1;
155 }
156
157 /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
158 * floating point imprecisions
159 */
160 if(dot < -1.0) dot = -1.0;
161 if(dot > 1.0) dot = 1.0;
162
163 scalar_t angle = acos(dot);
164 scalar_t a, b;
165
166 scalar_t sin_angle = sin(angle);
167 if(fabs(sin_angle) < SMALL_NUMBER) {
168 /* for very small angles or completely opposite orientations
169 * use linear interpolation to avoid div/zero (in the first case it makes sense,
170 * the second case is pretty much undefined anyway I guess ...
171 */
172 a = 1.0f - t;
173 b = t;
174 } else {
175 a = sin((1.0f - t) * angle) / sin_angle;
176 b = sin(t * angle) / sin_angle;
177 }
178
179 scalar_t x = q1.v.x * a + q2.v.x * b;
180 scalar_t y = q1.v.y * a + q2.v.y * b;
181 scalar_t z = q1.v.z * a + q2.v.z * b;
182 scalar_t s = q1.s * a + q2.s * b;
183
184 return Quaternion(s, Vector3(x, y, z));
185 }
186
187
188 std::ostream &operator <<(std::ostream &out, const Quaternion &q) {
189 out << "(" << q.s << ", " << q.v << ")";
190 return out;
191 }