File indexing completed on 2024-04-14 04:02:11
0001 /* 0002 SPDX-FileCopyrightText: 2008 Ian Wadham <iandw.au@gmail.com> 0003 0004 SPDX-License-Identifier: GPL-2.0-or-later 0005 */ 0006 0007 /* 0008 A little library to do quaternion arithmetic. Adapted from C to C++. 0009 Acknowledgements and thanks to John Darrington and the Gnubik package. 0010 */ 0011 0012 #include "quaternion.h" 0013 0014 #include <math.h> 0015 #include <stdlib.h> 0016 #include <stdio.h> 0017 0018 Quaternion::Quaternion () 0019 { 0020 quaternionSetIdentity(); 0021 } 0022 0023 0024 Quaternion::Quaternion (double rPart, double iPart, double jPart, double kPart) 0025 { 0026 w = rPart; 0027 x = iPart; 0028 y = jPart; 0029 z = kPart; 0030 } 0031 0032 0033 void Quaternion::quaternionSetIdentity () 0034 { 0035 w = 1.0; 0036 x = 0.0; 0037 y = 0.0; 0038 z = 0.0; 0039 } 0040 0041 0042 void Quaternion::quaternionAddRotation 0043 (const double axis[], const double degrees) 0044 { 0045 // If the axis-vector and quaternion are both normalised, 0046 // then the new quaternion will also be normalised. 0047 0048 Quaternion q; 0049 double radians = degrees * M_PI / 180.0; 0050 double s = sin (radians / 2.0); 0051 0052 q.w = cos (radians / 2.0); 0053 q.x = axis [0] * s; 0054 q.y = axis [1] * s; 0055 q.z = axis [2] * s; 0056 0057 quaternionPreMultiply (this, &q); 0058 } 0059 0060 0061 void Quaternion::quaternionPreMultiply 0062 (Quaternion * q1, const Quaternion * q2) const 0063 { 0064 double s1 = q1->w; 0065 double x1 = q1->x; 0066 double y1 = q1->y; 0067 double z1 = q1->z; 0068 0069 double s2 = q2->w; 0070 double x2 = q2->x; 0071 double y2 = q2->y; 0072 double z2 = q2->z; 0073 0074 double dot_product; 0075 double cross_product [3]; 0076 0077 dot_product = x1*x2 + y1*y2 + z1*z2; 0078 0079 cross_product [0] = y1*z2 - z1*y2; 0080 cross_product [1] = z1*x2 - x1*z2; 0081 cross_product [2] = x1*y2 - y1*x2; 0082 0083 q1->w = s1*s2 - dot_product; 0084 q1->x = s1*x2 + s2*x1 + cross_product[0]; 0085 q1->y = s1*y2 + s2*y1 + cross_product[1]; 0086 q1->z = s1*z2 + s2*z1 + cross_product[2]; 0087 } 0088 0089 0090 void Quaternion::quaternionToMatrix (Matrix M) const 0091 { 0092 double ww = w * w; 0093 double xx = x * x; 0094 double yy = y * y; 0095 double zz = z * z; 0096 0097 double wx = w * x; 0098 double wy = w * y; 0099 double wz = w * z; 0100 0101 double xy = x * y; 0102 double yz = y * z; 0103 double zx = z * x; 0104 0105 int dim = DIMENSIONS; 0106 0107 /* Diagonal */ 0108 M [0 + dim * 0] = ww + xx - yy - zz; // If q is normalised, 1 - 2*y2 - 2*z2. 0109 M [1 + dim * 1] = ww - xx + yy - zz; // etc. 0110 M [2 + dim * 2] = ww - xx - yy + zz; // etc. 0111 M [3 + dim * 3] = ww + xx + yy + zz; // If q is normalised, this is 1.0. 0112 0113 /* Last row */ 0114 M [0 + dim * 3] = 0.0; 0115 M [1 + dim * 3] = 0.0; 0116 M [2 + dim * 3] = 0.0; 0117 0118 /* Last Column */ 0119 M [3 + dim * 0] = 0.0; 0120 M [3 + dim * 1] = 0.0; 0121 M [3 + dim * 2] = 0.0; 0122 0123 /* Others */ 0124 M [0 + dim * 1] = 2.0 * (xy + wz); 0125 M [0 + dim * 2] = 2.0 * (zx - wy); 0126 M [1 + dim * 2] = 2.0 * (yz + wx); 0127 0128 M [1 + dim * 0] = 2.0 * (xy - wz); 0129 M [2 + dim * 0] = 2.0 * (zx + wy); 0130 M [2 + dim * 1] = 2.0 * (yz - wx); 0131 } 0132 0133 0134 void Quaternion::quaternionInvert() 0135 { 0136 x = -x; 0137 y = -y; 0138 z = -z; 0139 } 0140 0141 0142 void Quaternion::quaternionRotateVector (double axis[3]) const 0143 { 0144 // Make a copy of the quaternion ("this") that will rotate the vector. 0145 Quaternion q (w, x, y, z); 0146 0147 // Convert the vector to a quaternion. 0148 Quaternion v (0.0, axis[0], axis[1], axis[2]); 0149 0150 // Get the inverse of "this" quaternion. 0151 Quaternion q1 (w, -x, -y, -z); 0152 0153 // Multiply the three quaternions together: q*v*q1. 0154 quaternionPreMultiply (&q1, &v); 0155 quaternionPreMultiply (&q1, &q); 0156 0157 // Return the rotated vector. 0158 axis[0] = q1.x; 0159 axis[1] = q1.y; 0160 axis[2] = q1.z; 0161 } 0162 0163 0164 void Quaternion::quaternionPrint() const 0165 { 0166 printf ("Quaternion (%6.3f, %6.3f, %6.3f, %6.3f)\n", w, x, y, z); 0167 Quaternion p (w, x, y, z); 0168 double mod = sqrt (p.w * p.w + p.x * p.x + p.y * p.y + p.z * p.z); 0169 p.w = p.w / mod; 0170 p.x = p.x / mod; 0171 p.y = p.y / mod; 0172 p.z = p.z / mod; 0173 double rad = acos (p.w); 0174 if (fabs (rad) >= 0.0001) { 0175 double s = sin (rad); 0176 printf ("Angle %8.3f, axis (%6.3f, %6.3f, %6.3f)\n", 0177 2.0 * rad * 180.0 / M_PI, p.x/s, p.y/s, p.z/s); 0178 } 0179 else { 0180 printf ("Angle %8.3f, axis (%6.3f, %6.3f, %6.3f)\n", 0181 0.0, 0.0, 0.0, 0.0); 0182 } 0183 }