1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/quat.cc	Sun May 31 00:40:26 2015 +0300
     1.3 @@ -0,0 +1,229 @@
     1.4 +/*
     1.5 +libvmath - a vector math library
     1.6 +Copyright (C) 2004-2015 John Tsiombikas <nuclear@member.fsf.org>
     1.7 +
     1.8 +This program is free software: you can redistribute it and/or modify
     1.9 +it under the terms of the GNU Lesser General Public License as published
    1.10 +by the Free Software Foundation, either version 3 of the License, or
    1.11 +(at your option) any later version.
    1.12 +
    1.13 +This program is distributed in the hope that it will be useful,
    1.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
    1.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    1.16 +GNU Lesser General Public License for more details.
    1.17 +
    1.18 +You should have received a copy of the GNU Lesser General Public License
    1.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
    1.20 +*/
    1.21 +#include "quat.h"
    1.22 +#include "vmath.h"
    1.23 +
    1.24 +Quaternion::Quaternion()
    1.25 +{
    1.26 +	s = 1.0;
    1.27 +	v.x = v.y = v.z = 0.0;
    1.28 +}
    1.29 +
    1.30 +Quaternion::Quaternion(scalar_t s, const Vector3 &v)
    1.31 +{
    1.32 +	this->s = s;
    1.33 +	this->v = v;
    1.34 +}
    1.35 +
    1.36 +Quaternion::Quaternion(scalar_t s, scalar_t x, scalar_t y, scalar_t z)
    1.37 +{
    1.38 +	v.x = x;
    1.39 +	v.y = y;
    1.40 +	v.z = z;
    1.41 +	this->s = s;
    1.42 +}
    1.43 +
    1.44 +Quaternion::Quaternion(const Vector3 &axis, scalar_t angle)
    1.45 +{
    1.46 +	set_rotation(axis, angle);
    1.47 +}
    1.48 +
    1.49 +Quaternion::Quaternion(const quat_t &quat)
    1.50 +{
    1.51 +	v.x = quat.x;
    1.52 +	v.y = quat.y;
    1.53 +	v.z = quat.z;
    1.54 +	s = quat.w;
    1.55 +}
    1.56 +
    1.57 +Quaternion Quaternion::operator +(const Quaternion &quat) const
    1.58 +{
    1.59 +	return Quaternion(s + quat.s, v + quat.v);
    1.60 +}
    1.61 +
    1.62 +Quaternion Quaternion::operator -(const Quaternion &quat) const
    1.63 +{
    1.64 +	return Quaternion(s - quat.s, v - quat.v);
    1.65 +}
    1.66 +
    1.67 +Quaternion Quaternion::operator -() const
    1.68 +{
    1.69 +	return Quaternion(-s, -v);
    1.70 +}
    1.71 +
    1.72 +/** Quaternion Multiplication:
    1.73 + * Q1*Q2 = [s1*s2 - v1.v2,  s1*v2 + s2*v1 + v1(x)v2]
    1.74 + */
    1.75 +Quaternion Quaternion::operator *(const Quaternion &quat) const
    1.76 +{
    1.77 +	Quaternion newq;
    1.78 +	newq.s = s * quat.s - dot_product(v, quat.v);
    1.79 +	newq.v = quat.v * s + v * quat.s + cross_product(v, quat.v);
    1.80 +	return newq;
    1.81 +}
    1.82 +
    1.83 +void Quaternion::operator +=(const Quaternion &quat)
    1.84 +{
    1.85 +	*this = Quaternion(s + quat.s, v + quat.v);
    1.86 +}
    1.87 +
    1.88 +void Quaternion::operator -=(const Quaternion &quat)
    1.89 +{
    1.90 +	*this = Quaternion(s - quat.s, v - quat.v);
    1.91 +}
    1.92 +
    1.93 +void Quaternion::operator *=(const Quaternion &quat)
    1.94 +{
    1.95 +	*this = *this * quat;
    1.96 +}
    1.97 +
    1.98 +void Quaternion::reset_identity()
    1.99 +{
   1.100 +	s = 1.0;
   1.101 +	v.x = v.y = v.z = 0.0;
   1.102 +}
   1.103 +
   1.104 +Quaternion Quaternion::conjugate() const
   1.105 +{
   1.106 +	return Quaternion(s, -v);
   1.107 +}
   1.108 +
   1.109 +scalar_t Quaternion::length() const
   1.110 +{
   1.111 +	return (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   1.112 +}
   1.113 +
   1.114 +/** Q * ~Q = ||Q||^2 */
   1.115 +scalar_t Quaternion::length_sq() const
   1.116 +{
   1.117 +	return v.x*v.x + v.y*v.y + v.z*v.z + s*s;
   1.118 +}
   1.119 +
   1.120 +void Quaternion::normalize()
   1.121 +{
   1.122 +	scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   1.123 +	v.x /= len;
   1.124 +	v.y /= len;
   1.125 +	v.z /= len;
   1.126 +	s /= len;
   1.127 +}
   1.128 +
   1.129 +Quaternion Quaternion::normalized() const
   1.130 +{
   1.131 +	Quaternion nq = *this;
   1.132 +	scalar_t len = (scalar_t)sqrt(v.x*v.x + v.y*v.y + v.z*v.z + s*s);
   1.133 +	nq.v.x /= len;
   1.134 +	nq.v.y /= len;
   1.135 +	nq.v.z /= len;
   1.136 +	nq.s /= len;
   1.137 +	return nq;
   1.138 +}
   1.139 +
   1.140 +/** Quaternion Inversion: Q^-1 = ~Q / ||Q||^2 */
   1.141 +Quaternion Quaternion::inverse() const
   1.142 +{
   1.143 +	Quaternion inv = conjugate();
   1.144 +	scalar_t lensq = length_sq();
   1.145 +	inv.v /= lensq;
   1.146 +	inv.s /= lensq;
   1.147 +
   1.148 +	return inv;
   1.149 +}
   1.150 +
   1.151 +
   1.152 +void Quaternion::set_rotation(const Vector3 &axis, scalar_t angle)
   1.153 +{
   1.154 +	scalar_t half_angle = angle / 2.0;
   1.155 +	s = cos(half_angle);
   1.156 +	v = axis * sin(half_angle);
   1.157 +}
   1.158 +
   1.159 +void Quaternion::rotate(const Vector3 &axis, scalar_t angle)
   1.160 +{
   1.161 +	Quaternion q;
   1.162 +	scalar_t half_angle = angle / 2.0;
   1.163 +	q.s = cos(half_angle);
   1.164 +	q.v = axis * sin(half_angle);
   1.165 +
   1.166 +	*this *= q;
   1.167 +}
   1.168 +
   1.169 +void Quaternion::rotate(const Quaternion &q)
   1.170 +{
   1.171 +	*this = q * *this * q.conjugate();
   1.172 +}
   1.173 +
   1.174 +Matrix3x3 Quaternion::get_rotation_matrix() const
   1.175 +{
   1.176 +	return Matrix3x3(
   1.177 +			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.178 +			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.179 +			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.180 +}
   1.181 +
   1.182 +
   1.183 +/** Spherical linear interpolation (slerp) */
   1.184 +Quaternion slerp(const Quaternion &quat1, const Quaternion &q2, scalar_t t)
   1.185 +{
   1.186 +	Quaternion q1 = quat1;
   1.187 +	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.188 +
   1.189 +	if(dot < 0.0) {
   1.190 +		/* make sure we interpolate across the shortest arc */
   1.191 +		q1 = -quat1;
   1.192 +		dot = -dot;
   1.193 +	}
   1.194 +
   1.195 +	/* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
   1.196 +	 * floating point imprecisions
   1.197 +	 */
   1.198 +	if(dot < -1.0) dot = -1.0;
   1.199 +	if(dot > 1.0) dot = 1.0;
   1.200 +
   1.201 +	scalar_t angle = acos(dot);
   1.202 +	scalar_t a, b;
   1.203 +
   1.204 +	scalar_t sin_angle = sin(angle);
   1.205 +	if(fabs(sin_angle) < SMALL_NUMBER) {
   1.206 +		/* for very small angles or completely opposite orientations
   1.207 +		 * use linear interpolation to avoid div/zero (in the first case it makes sense,
   1.208 +		 * the second case is pretty much undefined anyway I guess ...
   1.209 +		 */
   1.210 +		a = 1.0f - t;
   1.211 +		b = t;
   1.212 +	} else {
   1.213 +		a = sin((1.0f - t) * angle) / sin_angle;
   1.214 +		b = sin(t * angle) / sin_angle;
   1.215 +	}
   1.216 +
   1.217 +	scalar_t x = q1.v.x * a + q2.v.x * b;
   1.218 +	scalar_t y = q1.v.y * a + q2.v.y * b;
   1.219 +	scalar_t z = q1.v.z * a + q2.v.z * b;
   1.220 +	scalar_t s = q1.s * a + q2.s * b;
   1.221 +
   1.222 +	return Quaternion(s, Vector3(x, y, z));
   1.223 +}
   1.224 +
   1.225 +
   1.226 +/*
   1.227 +std::ostream &operator <<(std::ostream &out, const Quaternion &q)
   1.228 +{
   1.229 +	out << "(" << q.s << ", " << q.v << ")";
   1.230 +	return out;
   1.231 +}
   1.232 +*/