Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <octomap/math/Vector3.h>
00035 #include <octomap/math/Quaternion.h>
00036 #include <octomap/math/Utils.h>
00037
00038 #include <cassert>
00039 #include <math.h>
00040
00041
00042
00043
00044 namespace octomath {
00045
00046
00047 Quaternion::Quaternion(const Quaternion &other) {
00048 data[0] = other(0);
00049 data[1] = other(1);
00050 data[2] = other(2);
00051 data[3] = other(3);
00052 }
00053
00054 Quaternion::Quaternion(float uu, float xx, float yy, float zz) {
00055 u() = uu;
00056 x() = xx;
00057 y() = yy;
00058 z() = zz;
00059 }
00060
00061 Quaternion::Quaternion(const Vector3 &other) {
00062 operator=(Quaternion(other.roll(), other.pitch(), other.yaw()));
00063 }
00064
00065 Quaternion::Quaternion(double roll, double pitch, double yaw) {
00066 double sroll = sin(roll);
00067 double spitch = sin(pitch);
00068 double syaw = sin(yaw);
00069 double croll = cos(roll);
00070 double cpitch = cos(pitch);
00071 double cyaw = cos(yaw);
00072
00073 double m[3][3] = {
00074 {cyaw*cpitch, cyaw*spitch*sroll - syaw*croll, cyaw*spitch*croll + syaw*sroll},
00075 {syaw*cpitch, syaw*spitch*sroll + cyaw*croll, syaw*spitch*croll - cyaw*sroll},
00076 { -spitch, cpitch*sroll, cpitch*croll}
00077 };
00078
00079 float _u = (float) (sqrt(std::max(0., 1 + m[0][0] + m[1][1] + m[2][2]))/2.0);
00080 float _x = (float) (sqrt(std::max(0., 1 + m[0][0] - m[1][1] - m[2][2]))/2.0);
00081 float _y = (float) (sqrt(std::max(0., 1 - m[0][0] + m[1][1] - m[2][2]))/2.0);
00082 float _z = (float) (sqrt(std::max(0., 1 - m[0][0] - m[1][1] + m[2][2]))/2.0);
00083 u() = _u;
00084 x() = (m[2][1] - m[1][2])>=0?fabs(_x):-fabs(_x);
00085 y() = (m[0][2] - m[2][0])>=0?fabs(_y):-fabs(_y);
00086 z() = (m[1][0] - m[0][1])>=0?fabs(_z):-fabs(_z);
00087 }
00088
00089 Quaternion::Quaternion(const Vector3& axis, double angle) {
00090 double sa = sin(angle/2);
00091 double ca = cos(angle/2);
00092 x() = (float) (axis.x()*sa);
00093 y() = (float) (axis.y()*sa);
00094 z() = (float) (axis.z()*sa);
00095 u() = (float) ca;
00096 }
00097
00098 float Quaternion::norm () const {
00099 double n = 0;
00100 for (unsigned int i=0; i<4; i++) {
00101 n += operator()(i) * operator()(i);
00102 }
00103 return (float) sqrt(n);
00104 }
00105
00106 void Quaternion::operator/= (float x)
00107 {
00108 for (unsigned int i=0; i<4; ++i)
00109 operator()(i) /= x;
00110 }
00111
00112 bool Quaternion::operator== (const Quaternion& other) const
00113 {
00114 for (unsigned int i=0; i<4; i++)
00115 {
00116 if (operator()(i) != other(i))
00117 return false;
00118 }
00119 return true;
00120 }
00121
00122
00123 Vector3 Quaternion::toEuler() const {
00124
00125 double n = norm ();
00126 double s = n > 0?2./(n*n):0.;
00127
00128 double xs = x()*s;
00129 double ys = y()*s;
00130 double zs = z()*s;
00131
00132 double ux = u()*xs;
00133 double uy = u()*ys;
00134 double uz = u()*zs;
00135
00136 double xx = x()*xs;
00137 double xy = x()*ys;
00138 double xz = x()*zs;
00139
00140 double yy = y()*ys;
00141 double yz = y()*zs;
00142 double zz = z()*zs;
00143
00144 double m[3][3];
00145
00146 m[0][0] = 1.0 - (yy + zz);
00147 m[1][1] = 1.0 - (xx + zz);
00148 m[2][2] = 1.0 - (xx + yy);
00149
00150 m[1][0] = xy + uz;
00151 m[0][1] = xy - uz;
00152
00153 m[2][0] = xz - uy;
00154 m[0][2] = xz + uy;
00155 m[2][1] = yz + ux;
00156 m[1][2] = yz - ux;
00157
00158 float roll = (float) atan2(m[2][1], m[2][2]);
00159 float pitch = (float) atan2(-m[2][0], sqrt(m[2][1]*m[2][1] + m[2][2]*m[2][2]));
00160 float yaw = (float) atan2(m[1][0], m[0][0]);
00161
00162 return Vector3(roll, pitch, yaw);
00163 }
00164
00165
00166 void Quaternion::toRotMatrix(std::vector <double>& rot_matrix_3_3) const {
00167
00168
00169 double n = norm ();
00170 double s = n > 0?2./(n*n):0.;
00171
00172 double xs = x()*s;
00173 double ys = y()*s;
00174 double zs = z()*s;
00175
00176 double ux = u()*xs;
00177 double uy = u()*ys;
00178 double uz = u()*zs;
00179
00180 double xx = x()*xs;
00181 double xy = x()*ys;
00182 double xz = x()*zs;
00183
00184 double yy = y()*ys;
00185 double yz = y()*zs;
00186 double zz = z()*zs;
00187
00188 double m[3][3];
00189 m[0][0] = 1.0 - (yy + zz);
00190 m[1][1] = 1.0 - (xx + zz);
00191 m[2][2] = 1.0 - (xx + yy);
00192
00193 m[1][0] = xy + uz;
00194 m[0][1] = xy - uz;
00195
00196 m[2][0] = xz - uy;
00197 m[0][2] = xz + uy;
00198 m[2][1] = yz + ux;
00199 m[1][2] = yz - ux;
00200
00201 rot_matrix_3_3.clear();
00202 rot_matrix_3_3.resize(9,0.);
00203 for (unsigned int i=0; i<3; i++) {
00204 rot_matrix_3_3[i*3] = m[i][0];
00205 rot_matrix_3_3[i*3+1] = m[i][1];
00206 rot_matrix_3_3[i*3+2] = m[i][2];
00207 }
00208 }
00209
00210 Quaternion& Quaternion::operator= (const Quaternion& other) {
00211 u() = other.u();
00212 x() = other.x();
00213 y() = other.y();
00214 z() = other.z();
00215 return *this;
00216 }
00217
00218 Quaternion Quaternion::operator* (const Quaternion& other) const {
00219 return Quaternion(u()*other.u() - x()*other.x() - y()*other.y() - z()*other.z(),
00220 y()*other.z() - other.y()*z() + u()*other.x() + other.u()*x(),
00221 z()*other.x() - other.z()*x() + u()*other.y() + other.u()*y(),
00222 x()*other.y() - other.x()*y() + u()*other.z() + other.u()*z());
00223 }
00224
00225 Quaternion Quaternion::operator* (const Vector3& v) const {
00226 return *this * Quaternion(0, v(0), v(1), v(2));
00227 }
00228
00229 Quaternion operator* (const Vector3& v, const Quaternion& q) {
00230 return Quaternion(0, v(0), v(1), v(2)) * q;
00231 }
00232
00233 Quaternion& Quaternion::normalize (){
00234 double len = norm ();
00235 if (len > 0)
00236 *this /= (float) len;
00237 return *this;
00238 }
00239
00240 Quaternion Quaternion::normalized () const {
00241 Quaternion result(*this);
00242 result.normalize ();
00243 return result;
00244 }
00245
00246
00247 Quaternion& Quaternion::inv_IP() {
00248 x() = -x();
00249 y() = -y();
00250 z() = -z();
00251 return *this;
00252 }
00253
00254 Vector3 Quaternion::rotate(const Vector3& v) const {
00255 Quaternion q = *this * v * this->inv();
00256 return Vector3(q.x(), q.y(), q.z());
00257 }
00258
00259
00260 std::istream& Quaternion::read(std::istream &s) {
00261 int temp;
00262 s >> temp;
00263 for (unsigned int i=0; i<4; i++)
00264 s >> operator()(i);
00265 return s;
00266 }
00267
00268
00269 std::ostream& Quaternion::write(std::ostream &s) const {
00270 s << 4;
00271 for (unsigned int i=0; i<4; i++)
00272 s << " " << operator()(i);
00273 return s;
00274 }
00275
00276
00277
00278 std::istream& Quaternion::readBinary(std::istream &s) {
00279 int temp;
00280 s.read((char*)&temp, sizeof(temp));
00281 double val = 0;
00282 for (unsigned int i=0; i<4; i++) {
00283 s.read((char*)&val, sizeof(val));
00284 operator()(i) = (float) val;
00285 }
00286 return s;
00287 }
00288
00289
00290 std::ostream& Quaternion::writeBinary(std::ostream &s) const {
00291 int temp = 4;
00292 s.write((char*)&temp, sizeof(temp));
00293 double val = 0;
00294 for (unsigned int i=0; i<4; i++) {
00295 val = operator()(i);
00296 s.write((char*)&val, sizeof(val));
00297 }
00298 return s;
00299 }
00300
00301
00302
00303 std::ostream& operator<<(std::ostream& s, const Quaternion& q) {
00304 s << "(" << q.u() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
00305 return s;
00306 }
00307
00308 }