dungeon_crawler

annotate prototype/vmath/quat.cc @ 1:96de911d05d4

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