SHS/pip
4
3
Fork 0
Code Issues 67 Pull Requests 1 Actions Releases Wiki Activity

code format

Browse Source
This commit is contained in:
Пелипенко Иван
2022-12-14 14:13:52 +03:00
parent 430a41fefc
commit c2b8a8d6da
297 changed files with 27331 additions and 24162 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
libs/main/math/piquaternion.cpp
View File

@@ -1,20 +1,20 @@
/*
PIP - Platform Independent Primitives
Class for quaternions
Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru
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 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.
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/>.
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"
@@ -30,8 +30,8 @@ PIQuaternion::PIQuaternion(const PIMathVectorT3d & u, double a) {
PIMathVectorT3d PIQuaternion::eyler() const {
PIMathMatrixT33d rmat = rotationMatrix();
double angle_y = asin(rmat[0][2]), angle_x, angle_z;
double c = cos(angle_y);
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]);
@@ -39,8 +39,8 @@ PIMathVectorT3d PIQuaternion::eyler() const {
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});
if (angle_z < 0) angle_z = 2 * M_PI + angle_z;
return PIMathVectorT3d({angle_x, angle_y, angle_z});
}
@@ -71,7 +71,6 @@ PIMathMatrixT33d PIQuaternion::rotationMatrix() const {
void PIQuaternion::axis(PIMathVectorT3d * ret) const {
if (!ret) return;
ret[0] = vector();
}
@@ -89,7 +88,7 @@ void PIQuaternion::rotate(const PIMathVectorT3d & u, double a) {
void PIQuaternion::normalize() {
double d = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
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;
@@ -105,35 +104,35 @@ PIQuaternion PIQuaternion::fromEyler(double ax, double ay, double az) {
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_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;
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;
@@ -143,13 +142,13 @@ PIQuaternion PIQuaternion::fromAngles(double ax, double ay, double az) {
}
PIQuaternion PIQuaternion::fromAngles2(double ax, double ay, double az) {
double om = PIMathVectorT3d({ax,ay,az}).length();
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);
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;
}
@@ -189,8 +188,8 @@ PIQuaternion PIQuaternion::fromRotationMatrix(const PIMathMatrixT33d & m) {
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);
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];
@@ -200,7 +199,7 @@ PIQuaternion operator*(const PIQuaternion & q0, const PIQuaternion & q1) {
}
PIQuaternion operator *(const double &a, const PIQuaternion &q1) {
PIQuaternion operator*(const double & a, const PIQuaternion & q1) {
PIQuaternion ret(q1);
ret.q[0] *= a;
ret.q[1] *= a;
@@ -208,4 +207,3 @@ PIQuaternion operator *(const double &a, const PIQuaternion &q1) {
ret.q[3] *= a;
return ret;
}