| #include "Rotation3.h"
|
| #include <cmath>
|
| #include "macros.h"
|
|
|
| typedef Rotation3::real real;
|
|
|
| real Rotation3::matrix11() const {
|
| return sqr(quat[0]) + sqr(quat[1]) - sqr(quat[2]) - sqr(quat[3]);
|
| }
|
| real Rotation3::matrix12() const {
|
| return 2*(quat[1]*quat[2] - quat[0]*quat[3]);
|
| }
|
| real Rotation3::matrix13() const{
|
| return 2*(quat[2]*quat[3] + quat[0]*quat[2]);
|
| }
|
| real Rotation3::matrix21() const {
|
| return 2*(quat[1]*quat[2] + quat[0]*quat[3]);
|
| }
|
| real Rotation3::matrix22() const {
|
| return sqr(quat[0]) - sqr(quat[1]) + sqr(quat[2]) - sqr(quat[3]);
|
| }
|
| real Rotation3::matrix23() const {
|
| return 2*(quat[2]*quat[3] - quat[0]*quat[1]);
|
| }
|
| real Rotation3::matrix31() const {
|
| return 2*(quat[1]*quat[3] - quat[0]*quat[2]);
|
| }
|
| real Rotation3::matrix32() const {
|
| return 2*(quat[2]*quat[3] + quat[0]*quat[1]);
|
| }
|
| real Rotation3::matrix33() const {
|
| return sqr(quat[0]) - sqr(quat[1]) - sqr(quat[2]) + sqr(quat[3]);
|
| }
|
|
|
| Rotation3::Rotation3()
|
| {
|
| quat[0] = 1;
|
| quat[1] = quat[2] = quat[3] = 0.0;
|
| }
|
|
|
|
|
| Rotation3::Rotation3(const Matrix3& m)
|
| {
|
| quat[0] = fabs(1+m[0][0]+m[1][1]+m[2][2])/4;
|
| quat[1] = fabs(1+m[0][0]-m[1][1]-m[2][2])/4;
|
| quat[2] = fabs(1-m[0][0]+m[1][1]-m[2][2])/4;
|
| quat[3] = fabs(1-m[0][0]-m[1][1]+m[2][2])/4;
|
|
|
|
|
| real sum = quat[0]+quat[1]+quat[2]+quat[3];
|
| quat[0] = sqrt(quat[0]/sum);
|
| quat[1] = sqrt(quat[1]/sum);
|
| quat[2] = sqrt(quat[2]/sum);
|
| quat[3] = sqrt(quat[3]/sum);
|
|
|
| if (m[2][1]-m[1][2] < 0.0) quat[1] = -quat[1];
|
| if (m[0][2]-m[2][0] < 0.0) quat[2] = -quat[2];
|
| if (m[1][0]-m[0][1] < 0.0) quat[3] = -quat[3];
|
| }
|
|
|
| Rotation3::Rotation3(const real &xr, const real &yr, const real &zr)
|
| {
|
| real cx = cos(xr); real cy = cos(yr); real cz = cos(zr);
|
| real sx = sin(xr); real sy = sin(yr); real sz = sin(zr);
|
| real m00 = cz*cy;
|
| real m11 = -sy*sx*sz + cx*cz;
|
| real m22 = cy*cx;
|
|
|
| quat[0] = (real)(sqrt(1+m00+m11+m22)/2.0);
|
| quat[1] = (real)(sqrt(1+m00-m11-m22)/2.0);
|
| quat[2] = (real)(sqrt(1-m00+m11-m22)/2.0);
|
| quat[3] = (real)(sqrt(1-m00-m11+m22)/2.0);
|
|
|
| if (cy*sx + sy*cx*sz + sx*cz < 0.0) quat[1] = -quat[1];
|
| if (sz*sx - sy*cx*cz - sy < 0.0) quat[2] = -quat[2];
|
| if (sz*cy + sy*sx*cz + sz*cx < 0.0) quat[3] = -quat[3];
|
|
|
|
|
|
|
|
|
| }
|
|
|
| Rotation3::Rotation3(const real a, const real b, const real c, const real d)
|
| {
|
| real sum = sqrt(a*a + b*b + c*c + d*d);
|
| if (a<0.0) sum = -sum;
|
| quat[0] = a/sum;
|
| quat[1] = b/sum;
|
| quat[2] = c/sum;
|
| quat[3] = d/sum;
|
| }
|
|
|
| Rotation3::Rotation3(const real a, const real b, const real c, const real d,
|
| int)
|
| {
|
| quat[0] = a;
|
| quat[1] = b;
|
| quat[2] = c;
|
| quat[3] = d;
|
| }
|
|
|
| Rotation3::Rotation3(const real m[3][3])
|
| {
|
| quat[0] = fabs(1+m[0][0]+m[1][1]+m[2][2])/4;
|
| quat[1] = fabs(1+m[0][0]-m[1][1]-m[2][2])/4;
|
| quat[2] = fabs(1-m[0][0]+m[1][1]-m[2][2])/4;
|
| quat[3] = fabs(1-m[0][0]-m[1][1]+m[2][2])/4;
|
|
|
|
|
| real sum = quat[0]+quat[1]+quat[2]+quat[3];
|
| quat[0] = sqrt(quat[0]/sum);
|
| quat[1] = sqrt(quat[1]/sum);
|
| quat[2] = sqrt(quat[2]/sum);
|
| quat[3] = sqrt(quat[3]/sum);
|
|
|
| if (m[2][1]-m[1][2] < 0.0) quat[1] = -quat[1];
|
| if (m[0][2]-m[2][0] < 0.0) quat[2] = -quat[2];
|
| if (m[1][0]-m[0][1] < 0.0) quat[3] = -quat[3];
|
| }
|
|
|
| Rotation3::Rotation3(const double m[3][3])
|
| {
|
| quat[0] = (real)fabs(1+m[0][0]+m[1][1]+m[2][2])/4;
|
| quat[1] = (real)fabs(1+m[0][0]-m[1][1]-m[2][2])/4;
|
| quat[2] = (real)fabs(1-m[0][0]+m[1][1]-m[2][2])/4;
|
| quat[3] = (real)fabs(1-m[0][0]-m[1][1]+m[2][2])/4;
|
|
|
|
|
| real sum = quat[0]+quat[1]+quat[2]+quat[3];
|
| quat[0] = sqrt(quat[0]/sum);
|
| quat[1] = sqrt(quat[1]/sum);
|
| quat[2] = sqrt(quat[2]/sum);
|
| quat[3] = sqrt(quat[3]/sum);
|
|
|
| if (m[2][1]-m[1][2] < 0.0) quat[1] = -quat[1];
|
| if (m[0][2]-m[2][0] < 0.0) quat[2] = -quat[2];
|
| if (m[1][0]-m[0][1] < 0.0) quat[3] = -quat[3];
|
| }
|
|
|
| Rotation3::Rotation3(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
|
| {
|
| quat[0] = fabs(1+v1[0]+v2[1]+v3[2])/4;
|
| quat[1] = fabs(1+v1[0]-v2[1]-v3[2])/4;
|
| quat[2] = fabs(1-v1[0]+v2[1]-v3[2])/4;
|
| quat[3] = fabs(1-v1[0]-v2[1]+v3[2])/4;
|
|
|
|
|
| real sum = quat[0]+quat[1]+quat[2]+quat[3];
|
| quat[0] = sqrt(quat[0]/sum);
|
| quat[1] = sqrt(quat[1]/sum);
|
| quat[2] = sqrt(quat[2]/sum);
|
| quat[3] = sqrt(quat[3]/sum);
|
|
|
| if (v3[1]-v2[2] < 0.0) quat[1] = -quat[1];
|
| if (v1[2]-v3[0] < 0.0) quat[2] = -quat[2];
|
| if (v2[0]-v1[1] < 0.0) quat[3] = -quat[3];
|
| }
|
|
|
| real Rotation3::trace() const
|
| {
|
| return sqr(quat[0]) - sqr(quat[1]) - sqr(quat[2]) - sqr(quat[3]);
|
| }
|
|
|
| const real Rotation3::operator[](const unsigned short r) const
|
| {
|
| return quat[r];
|
| }
|
|
|
| Vector3 Rotation3::row(const unsigned short r) const
|
| {
|
| switch (r) {
|
| case 1: return Vector3(matrix11(), matrix12(), matrix13());
|
| case 2: return Vector3(matrix21(), matrix22(), matrix23());
|
| case 3: return Vector3(matrix31(), matrix32(), matrix33());
|
| }
|
| return Vector3();
|
| }
|
|
|
| Vector3 Rotation3::column(const unsigned short col) const
|
| {
|
| switch (col) {
|
| case 1: return Vector3(matrix11(), matrix21(), matrix31());
|
| case 2: return Vector3(matrix12(), matrix22(), matrix32());
|
| case 3: return Vector3(matrix13(), matrix23(), matrix33());
|
| }
|
| return Vector3();
|
| }
|
|
|
| real Rotation3::element(const unsigned short r,
|
| const unsigned short col) const
|
| {
|
| switch (r) {
|
| case 1:
|
| switch(col) {
|
| case 1: return matrix11();
|
| case 2: return matrix12();
|
| case 3: return matrix13();
|
| }
|
| case 2:
|
| switch(col) {
|
| case 1: return matrix21();
|
| case 2: return matrix22();
|
| case 3: return matrix23();
|
| }
|
| case 3:
|
| switch(col) {
|
| case 1: return matrix31();
|
| case 2: return matrix32();
|
| case 3: return matrix33();
|
| }
|
| }
|
| return 0.0;
|
| }
|
|
|
| real Rotation3::rotX() const
|
| {
|
| return atan2(matrix32(), matrix33());
|
| }
|
|
|
| real Rotation3::rotY() const
|
| {
|
| return atan2(matrix31(), sqrt(sqr(matrix21())+sqr(matrix11())));
|
| }
|
|
|
| real Rotation3::rotZ() const
|
| {
|
| return atan2(matrix21(), matrix11());
|
| }
|
|
|
| Vector3 Rotation3::rotationAngles() const
|
| {
|
| return Vector3(rotX(), rotY(), rotZ());
|
| }
|
|
|
| Rotation3::operator Matrix3() const
|
| {
|
| const real a = quat[0];
|
| const real b = quat[1];
|
| const real c = quat[2];
|
| const real d = quat[3];
|
| return Matrix3(a*a+b*b-c*c-d*d, 2*(b*c-a*d) , 2*(b*d+a*c),
|
| 2*(b*c+a*d) , a*a-b*b+c*c-d*d, 2*(c*d-a*b),
|
| 2*(b*d-a*c) , 2*(c*d+a*b) , a*a-b*b-c*c+d*d);
|
| }
|
|
|
| real Rotation3::dist2(const Rotation3& r) const
|
| {
|
| return (sqr(quat[0]-r.quat[0]) + sqr(quat[1]-r.quat[1]) +
|
| sqr(quat[2]-r.quat[2]) + sqr(quat[3]-r.quat[3]));
|
| }
|
|
|
| real Rotation3::dist(const Rotation3& r) const
|
| {
|
| return sqrt(dist2(r));
|
| }
|
|
|
| std::pair<Vector3, real> Rotation3::getAxisAndAngle() const
|
| {
|
| real a, b, c, d;
|
| a = quat[0];
|
| b = quat[1];
|
| c = quat[2];
|
| d = quat[3];
|
| if (fabs(a) > .9999) return std::make_pair(Vector3(1, 0, 0), 0.0);
|
| double angle = acos(a) * 2;
|
| double s = sin(angle / 2);
|
| Vector3 axis(b / s, c / s, d / s);
|
| return std::make_pair(axis.getUnitVector(), angle);
|
| }
|
|
|
| Rotation3 operator*(const Rotation3 &q, const Rotation3 &p)
|
| {
|
| return Rotation3(q[0]*p[0] - q[1]*p[1] - q[2]*p[2] - q[3]*p[3],
|
| q[0]*p[1] + q[1]*p[0] + q[2]*p[3] - q[3]*p[2],
|
| q[0]*p[2] - q[1]*p[3] + q[2]*p[0] + q[3]*p[1],
|
| q[0]*p[3] + q[1]*p[2] - q[2]*p[1] + q[3]*p[0],
|
| 0);
|
| }
|
|
|
|
|
| Vector3 operator*(const Rotation3 &t, const Vector3 &v)
|
| {
|
| const real a = t.quat[0];
|
| const real b = t.quat[1];
|
| const real c = t.quat[2];
|
| const real d = t.quat[3];
|
| return Vector3((a*a+b*b-c*c-d*d)*v[0] + 2*(b*c-a*d)*v[1] + 2*(b*d+a*c)*v[2],
|
| 2*(b*c+a*d)*v[0] + (a*a-b*b+c*c-d*d)*v[1] + 2*(c*d-a*b)*v[2],
|
| 2*(b*d-a*c)*v[0] + 2*(c*d+a*b)*v[1] + (a*a-b*b-c*c+d*d)*v[2]);
|
| }
|
|
|
| Rotation3& Rotation3::operator*=(const Rotation3 &p)
|
| {
|
| real a = quat[0];
|
| real b = quat[1];
|
| real c = quat[2];
|
| real d = quat[3];
|
|
|
| quat[0] = a*p[0] - b*p[1] - c*p[2] - d*p[3];
|
| quat[1] = a*p[1] + b*p[0] + c*p[3] - d*p[2];
|
| quat[2] = a*p[2] - b*p[3] + c*p[0] + d*p[1];
|
| quat[3] = a*p[3] + b*p[2] - c*p[1] + d*p[0];
|
| return *this;
|
| }
|
|
|
| Rotation3 operator!(const Rotation3 &t)
|
| {
|
| return Rotation3(t.quat[0], -t.quat[1], -t.quat[2], -t.quat[3], 0);
|
| }
|
|
|
| std::ostream& operator<<(std::ostream& s, const Rotation3 &t)
|
| {
|
| return (s << t.quat[0] << ' ' << t.quat[1] << ' ' << t.quat[2] << ' '
|
| << t.quat[3]);
|
| }
|
|
|
