gpuray_glsl
diff vmath/quat.cc @ 0:f234630e38ff
initial commit
author | John Tsiombikas <nuclear@member.fsf.org> |
---|---|
date | Sun, 09 Nov 2014 13:03:36 +0200 |
parents | |
children |
line diff
1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000 1.2 +++ b/vmath/quat.cc Sun Nov 09 13:03:36 2014 +0200 1.3 @@ -0,0 +1,191 @@ 1.4 +#include "quat.h" 1.5 +#include "vmath.h" 1.6 + 1.7 +Quaternion::Quaternion() { 1.8 + s = 1.0; 1.9 + v.x = v.y = v.z = 0.0; 1.10 +} 1.11 + 1.12 +Quaternion::Quaternion(scalar_t s, const Vector3 &v) { 1.13 + this->s = s; 1.14 + this->v = v; 1.15 +} 1.16 + 1.17 +Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z) { 1.18 + v.x = x; 1.19 + v.y = y; 1.20 + v.z = z; 1.21 + this->s = s; 1.22 +} 1.23 + 1.24 +Quaternion::Quaternion(const Vector3 &axis, scalar_t angle) { 1.25 + set_rotation(axis, angle); 1.26 +} 1.27 + 1.28 +Quaternion::Quaternion(const quat_t &quat) 1.29 +{ 1.30 + v.x = quat.x; 1.31 + v.y = quat.y; 1.32 + v.z = quat.z; 1.33 + s = quat.w; 1.34 +} 1.35 + 1.36 +Quaternion Quaternion::operator +(const Quaternion &quat) const 1.37 +{ 1.38 + return Quaternion(s + quat.s, v + quat.v); 1.39 +} 1.40 + 1.41 +Quaternion Quaternion::operator -(const Quaternion &quat) const 1.42 +{ 1.43 + return Quaternion(s - quat.s, v - quat.v); 1.44 +} 1.45 + 1.46 +Quaternion Quaternion::operator -() const 1.47 +{ 1.48 + return Quaternion(-s, -v); 1.49 +} 1.50 + 1.51 +/** Quaternion Multiplication: 1.52 + * Q1*Q2 = [s1*s2 - v1.v2, s1*v2 + s2*v1 + v1(x)v2] 1.53 + */ 1.54 +Quaternion Quaternion::operator *(const Quaternion &quat) const { 1.55 + Quaternion newq; 1.56 + newq.s = s * quat.s - dot_product(v, quat.v); 1.57 + newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v); 1.58 + return newq; 1.59 +} 1.60 + 1.61 +void Quaternion::operator +=(const Quaternion &quat) { 1.62 + *this = Quaternion(s + quat.s, v + quat.v); 1.63 +} 1.64 + 1.65 +void Quaternion::operator -=(const Quaternion &quat) { 1.66 + *this = Quaternion(s - quat.s, v - quat.v); 1.67 +} 1.68 + 1.69 +void Quaternion::operator *=(const Quaternion &quat) { 1.70 + *this = *this * quat; 1.71 +} 1.72 + 1.73 +void Quaternion::reset_identity() { 1.74 + s = 1.0; 1.75 + v.x = v.y = v.z = 0.0; 1.76 +} 1.77 + 1.78 +Quaternion Quaternion::conjugate() const { 1.79 + return Quaternion(s, -v); 1.80 +} 1.81 + 1.82 +scalar_t Quaternion::length() const { 1.83 + return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s); 1.84 +} 1.85 + 1.86 +/** Q * ~Q = ||Q||^2 */ 1.87 +scalar_t Quaternion::length_sq() const { 1.88 + return v.x*v.x + v.y*v.y + v.z*v.z + s*s; 1.89 +} 1.90 + 1.91 +void Quaternion::normalize() { 1.92 + scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s); 1.93 + v.x /= len; 1.94 + v.y /= len; 1.95 + v.z /= len; 1.96 + s /= len; 1.97 +} 1.98 + 1.99 +Quaternion Quaternion::normalized() const { 1.100 + Quaternion nq = *this; 1.101 + scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s); 1.102 + nq.v.x /= len; 1.103 + nq.v.y /= len; 1.104 + nq.v.z /= len; 1.105 + nq.s /= len; 1.106 + return nq; 1.107 +} 1.108 + 1.109 +/** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */ 1.110 +Quaternion Quaternion::inverse() const { 1.111 + Quaternion inv = conjugate(); 1.112 + scalar_t lensq = length_sq(); 1.113 + inv.v /= lensq; 1.114 + inv.s /= lensq; 1.115 + 1.116 + return inv; 1.117 +} 1.118 + 1.119 + 1.120 +void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle) { 1.121 + scalar_t half_angle = angle / 2.0; 1.122 + s = cos(half_angle); 1.123 + v = axis * sin(half_angle); 1.124 +} 1.125 + 1.126 +void Quaternion::rotate(const Vector3 &axis, scalar_t angle) { 1.127 + Quaternion q; 1.128 + scalar_t half_angle = angle / 2.0; 1.129 + q.s = cos(half_angle); 1.130 + q.v = axis * sin(half_angle); 1.131 + 1.132 + *this *= q; 1.133 +} 1.134 + 1.135 +void Quaternion::rotate(const Quaternion &q) { 1.136 + *this = q * *this * q.conjugate(); 1.137 +} 1.138 + 1.139 +Matrix3x3 Quaternion::get_rotation_matrix() const { 1.140 + return Matrix3x3( 1.141 + 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, 1.142 + 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, 1.143 + 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); 1.144 +} 1.145 + 1.146 + 1.147 +/** Spherical linear interpolation (slerp) */ 1.148 +Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, scalar_t t) { 1.149 + Quaternion q1; 1.150 + 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; 1.151 + 1.152 + if(dot < 0.0) { 1.153 + /* make sure we interpolate across the shortest arc */ 1.154 + q1 = -quat1; 1.155 + dot = -dot; 1.156 + } else { 1.157 + q1 = quat1; 1.158 + } 1.159 + 1.160 + /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to 1.161 + * floating point imprecisions 1.162 + */ 1.163 + if(dot < -1.0) dot = -1.0; 1.164 + if(dot > 1.0) dot = 1.0; 1.165 + 1.166 + scalar_t angle = acos(dot); 1.167 + scalar_t a, b; 1.168 + 1.169 + scalar_t sin_angle = sin(angle); 1.170 + if(fabs(sin_angle) < SMALL_NUMBER) { 1.171 + /* for very small angles or completely opposite orientations 1.172 + * use linear interpolation to avoid div/zero (in the first case it makes sense, 1.173 + * the second case is pretty much undefined anyway I guess ... 1.174 + */ 1.175 + a = 1.0f - t; 1.176 + b = t; 1.177 + } else { 1.178 + a = sin((1.0f - t) * angle) / sin_angle; 1.179 + b = sin(t * angle) / sin_angle; 1.180 + } 1.181 + 1.182 + scalar_t x = q1.v.x * a + q2.v.x * b; 1.183 + scalar_t y = q1.v.y * a + q2.v.y * b; 1.184 + scalar_t z = q1.v.z * a + q2.v.z * b; 1.185 + scalar_t s = q1.s * a + q2.s * b; 1.186 + 1.187 + return Quaternion(s, Vector3(x, y, z)); 1.188 +} 1.189 + 1.190 + 1.191 +std::ostream &operator <<(std::ostream &out, const Quaternion &q) { 1.192 + out << "(" << q.s << ", " << q.v << ")"; 1.193 + return out; 1.194 +}