3dphotoshoot

annotate libs/vmath/quat.cc @ 27:3d082c566b53

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