pip/libs/main/math/piquaternion.cpp

/*
	PIP - Platform Independent Primitives
	Class for quaternions
	Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "piquaternion.h"


PIQuaternion::PIQuaternion(const PIMathVectorT3d & u, double a) {
	q[0] = a;
	q[1] = u[0];
	q[2] = u[1];
	q[3] = u[2];
}


PIMathVectorT3d PIQuaternion::eyler() const {
	PIMathMatrixT33d rmat = rotationMatrix();
	double angle_y = asin(rmat[0][2]), angle_x, angle_z;
	double c =  cos(angle_y);
	if (fabs(c) < 1.E-5) {
		angle_x = 0;
		angle_z = atan2(rmat[1][0], rmat[1][1]);
	} else {
		angle_x = -atan2(-rmat[1][2] / c, rmat[2][2] / c);
		angle_z = atan2(-rmat[0][1] / c, rmat[0][0] / c);
	}
	if (angle_z < 0) angle_z = 2*M_PI + angle_z;
	return PIMathVectorT3d({angle_x,angle_y,angle_z});
}


PIMathMatrixT33d PIQuaternion::rotationMatrix() const {
	PIMathMatrixT33d ret;
	double xx = q[1] * q[1];
	double xy = q[1] * q[2];
	double xz = q[1] * q[3];
	double xw = q[1] * q[0];
	double yy = q[2] * q[2];
	double yz = q[2] * q[3];
	double yw = q[2] * q[0];
	double zz = q[3] * q[3];
	double zw = q[3] * q[0];
	ret[0][0] = 1. - 2. * (yy + zz);
	ret[0][1] = 2. * (xy - zw);
	ret[0][2] = 2. * (xz + yw);
	ret[1][0] = 2. * (xy + zw);
	ret[1][1] = 1. - 2. * (xx + zz);
	ret[1][2] = 2. * (yz - xw);
	ret[2][0] = 2. * (xz - yw);
	ret[2][1] = 2. * (yz + xw);
	ret[2][2] = 1. - 2. * (xx + yy);
	return ret;
}


void PIQuaternion::axis(PIMathVectorT3d * ret) const {
	if (!ret) return;
	ret[0] = vector();
	
}


PIQuaternion PIQuaternion::rotated(const PIMathVectorT3d & u, double a) const {
	PIQuaternion ret(*this);
	ret.rotate(u, a);
	return ret;
}


void PIQuaternion::rotate(const PIMathVectorT3d & u, double a) {
	PIQuaternion v(u * sin(a / 2.), cos(a / 2.));
	*this = (v * (*this) * v.conjugate());
}


void PIQuaternion::normalize() {
	double d = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	if (d != 0.)
		for (int i = 0; i < 4; ++i)
			q[i] /= d;
}


PIQuaternion PIQuaternion::fromEyler(double ax, double ay, double az) {
	PIQuaternion q_heading;
	PIQuaternion q_pinch;
	PIQuaternion q_bank;
	PIQuaternion q_tmp;
	q_heading.q[0] = 0;
	q_heading.q[1] = 0;
	q_heading.q[2] = sin(ax / 2);
	q_heading.q[3] = -cos(ax / 2);
	q_pinch.q[0] = 0;
	q_pinch.q[1] = sin(ay / 2);
	q_pinch.q[2] = 0;
	q_pinch.q[3] = -cos(ay / 2);
	az = M_PI - az;
	q_bank.q[0] = sin(az / 2);
	q_bank.q[1] = 0;
	q_bank.q[2] = 0;
	q_bank.q[3] = cos(az / 2);
	q_tmp = q_heading * q_pinch;
	q_tmp = q_tmp * q_bank;
	q_tmp.normalize();
	return q_tmp;
}


PIQuaternion PIQuaternion::fromAngles(double ax, double ay, double az) {
	double c1 = cos(ay/2);
	double s1 = sin(ay/2);
	double c2 = cos(az/2);
	double s2 = sin(az/2);
	double c3 = cos(ax/2);
	double s3 = sin(ax/2);
	double c1c2 = c1*c2;
	double s1s2 = s1*s2;
	double a = c1c2*c3 - s1s2*s3;
	double x = c1c2*s3 + s1s2*c3;
	double y = s1*c2*c3 + c1*s2*s3;
	double z = c1*s2*c3 - s1*c2*s3;
	PIQuaternion res;
	res.q[0] = a;
	res.q[1] = x;
	res.q[2] = y;
	res.q[3] = z;
	return res;
}

PIQuaternion PIQuaternion::fromAngles2(double ax, double ay, double az) {
	double om = PIMathVectorT3d({ax,ay,az}).length();
	if (om == 0.) return PIQuaternion(PIMathVectorT3d(), 1.);
	PIQuaternion res;
	res.q[0] = cos(om/2);
	res.q[1] = ax/om * sin(om/2);
	res.q[2] = ay/om * sin(om/2);
	res.q[3] = az/om * sin(om/2);
	return res;
}


PIQuaternion PIQuaternion::fromRotationMatrix(const PIMathMatrixT33d & m) {
	PIQuaternion ret;
	float tr = m[0][0] + m[1][1] + m[2][2];
	if (tr > 0.0f) {
		ret.q[0] = tr + 1.0f;
		ret.q[1] = m[2][1] - m[1][2];
		ret.q[2] = m[0][2] - m[2][0];
		ret.q[3] = m[1][0] - m[0][1];
	} else {
		if ((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])) {
			ret.q[0] = m[2][1] - m[1][2];
			ret.q[1] = 1.0f + m[0][0] - m[1][1] - m[2][2];
			ret.q[2] = m[0][1] + m[1][0];
			ret.q[3] = m[0][2] + m[2][0];
		} else {
			if (m[1][1] > m[2][2]) {
				ret.q[0] = m[0][2] - m[2][0];
				ret.q[1] = m[0][1] + m[1][0];
				ret.q[2] = 1.0f + m[1][1] - m[0][0] - m[2][2];
				ret.q[3] = m[1][2] + m[2][1];
			} else {
				ret.q[0] = m[1][0] - m[0][1];
				ret.q[1] = m[0][2] + m[2][0];
				ret.q[2] = m[1][2] + m[2][1];
				ret.q[3] = 1.0f + m[2][2] - m[0][0] - m[1][1];
			}
		}
	}
	ret.normalize();
	return ret;
}


PIQuaternion operator*(const PIQuaternion & q0, const PIQuaternion & q1) {
	PIMathVectorT3d v0(q0.vector()), v1(q1.vector());
	double r0 = q0.q[0] * q1.q[0] - v0.dot(v1);
	PIMathVectorT3d qv = v1*q0.q[0] + v0*q1.q[0] + v0.cross(v1);
	PIQuaternion ret;
	ret.q[0] = r0;
	ret.q[1] = qv[0];
	ret.q[2] = qv[1];
	ret.q[3] = qv[2];
	return ret;
}


PIQuaternion operator *(const double &a, const PIQuaternion &q1) {
	PIQuaternion ret(q1);
	ret.q[0] *= a;
	ret.q[1] *= a;
	ret.q[2] *= a;
	ret.q[3] *= a;
	return ret;
}