gpuray_glsl

annotate vmath/quat.cc @ 3:297dbc5080c4

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