dbf-halloween2015

annotate libs/vmath/quat.cc @ 1:c3f5c32cb210

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