dungeon_crawler

view prototype/vmath/quat.cc @ 80:a373b36ffc17

find changesets by author, revision, files, or words in the commit message
better
author John Tsiombikas <nuclear@member.fsf.org>
date Sat, 27 Oct 2012 01:59:39 +0300
parents
children
line source
1 #include "quat.h"
2
3 Quaternion::Quaternion() {
4 s = 1.0;
5 v.x = v.y = v.z = 0.0;
6 }
7
8 Quaternion::Quaternion(scalar_t s, const Vector3 &v) {
9 this->s = s;
10 this->v = v;
11 }
12
13 Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) {
14 v.x = x;
15 v.y = y;
16 v.z = z;
17 this->s = s;
18 }
19
20 Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) {
21 set_rotation(axis, angle);
22 }
23
24 Quaternion::Quaternion(const quat_t &quat)
25 {
26 v.x = quat.x;
27 v.y = quat.y;
28 v.z = quat.z;
29 s = quat.w;
30 }
31
32 Quaternion Quaternion::operator +(const Quaternion &quat) const {
33 return Quaternion(s + quat.s, v + quat.v);
34 }
35
36 Quaternion Quaternion::operator -(const Quaternion &quat) const {
37 return Quaternion(s - quat.s, v - quat.v);
38 }
39
40 Quaternion Quaternion::operator -() const {
41 return Quaternion(-s, -v);
42 }
43
44 /** Quaternion Multiplication:
45 * Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2]
46 */
47 Quaternion Quaternion::operator *(const Quaternion &quat) const {
48 Quaternion newq;
49 newq.s = s * quat.s - dot_product(v, quat.v);
50 newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
51 return newq;
52 }
53
54 void Quaternion::operator +=(const Quaternion &quat) {
55 *this = Quaternion(s + quat.s, v + quat.v);
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 = *this * quat;
64 }
65
66 void Quaternion::reset_identity() {
67 s = 1.0;
68 v.x = v.y = v.z = 0.0;
69 }
70
71 Quaternion Quaternion::conjugate() const {
72 return Quaternion(s, -v);
73 }
74
75 scalar_t Quaternion::length() const {
76 return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
77 }
78
79 /** Q * ~Q = ||Q||^2 */
80 scalar_t Quaternion::length_sq() const {
81 return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
82 }
83
84 void Quaternion::normalize() {
85 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
86 v.x /= len;
87 v.y /= len;
88 v.z /= len;
89 s /= len;
90 }
91
92 Quaternion Quaternion::normalized() const {
93 Quaternion nq = *this;
94 scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
95 nq.v.x /= len;
96 nq.v.y /= len;
97 nq.v.z /= len;
98 nq.s /= len;
99 return nq;
100 }
101
102 /** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
103 Quaternion Quaternion::inverse() const {
104 Quaternion inv = conjugate();
105 scalar_t lensq = length_sq();
106 inv.v /= lensq;
107 inv.s /= lensq;
108
109 return inv;
110 }
111
112
113 void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) {
114 scalar_t half_angle = angle / 2.0;
115 s = cos(half_angle);
116 v = axis * sin(half_angle);
117 }
118
119 void Quaternion::rotate(const Vector3 &axis, scalar_t angle) {
120 Quaternion q;
121 scalar_t half_angle = angle / 2.0;
122 q.s = cos(half_angle);
123 q.v = axis * sin(half_angle);
124
125 *this *= q;
126 }
127
128 void Quaternion::rotate(const Quaternion &q) {
129 *this = q * *this * q.conjugate();
130 }
131
132 Matrix3x3 Quaternion::get_rotation_matrix() const {
133 return Matrix3x3( 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,
134 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,
135 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);
136 }
137
138
139 /** Spherical linear interpolation (slerp) */
140 Quaternion slerp(const Quaternion &q1, const Quaternion &q2, scalar_t t) {
141 scalar_t angle = acos(q1.s * q2.s + q1.v.x * q2.v.x + q1.v.y * q2.v.y + q1.v.z * q2.v.z);
142 scalar_t a = sin((1.0f - t) * angle);
143 scalar_t b = sin(t * angle);
144 scalar_t c = sin(angle);
145
146 scalar_t x = (q1.v.x * a + q2.v.x * b) / c;
147 scalar_t y = (q1.v.y * a + q2.v.y * b) / c;
148 scalar_t z = (q1.v.z * a + q2.v.z * b) / c;
149 scalar_t s = (q1.s * a + q2.s * b) / c;
150
151 return Quaternion(s, Vector3(x, y, z)).normalized();
152 }
153
154
155
156 std::ostream &operator <<(std::ostream &out, const Quaternion &q) {
157 out << "(" << q.s << ", " << q.v << ")";
158 return out;
159 }