67 double sroll = sin(roll);
68 double spitch = sin(pitch);
69 double syaw = sin(yaw);
70 double croll = cos(roll);
71 double cpitch = cos(pitch);
72 double cyaw = cos(yaw);
75 {cyaw*cpitch, cyaw*spitch*sroll - syaw*croll, cyaw*spitch*croll + syaw*sroll},
76 {syaw*cpitch, syaw*spitch*sroll + cyaw*croll, syaw*spitch*croll - cyaw*sroll},
77 { -spitch, cpitch*sroll, cpitch*croll}
80 float _u = (float) (sqrt(std::max(0., 1 + m[0][0] + m[1][1] + m[2][2]))/2.0);
81 float _x = (float) (sqrt(std::max(0., 1 + m[0][0] - m[1][1] - m[2][2]))/2.0);
82 float _y = (float) (sqrt(std::max(0., 1 - m[0][0] + m[1][1] - m[2][2]))/2.0);
83 float _z = (float) (sqrt(std::max(0., 1 - m[0][0] - m[1][1] + m[2][2]))/2.0);
85 x() = (m[2][1] - m[1][2])>=0?fabs(_x):-fabs(_x);
86 y() = (m[0][2] - m[2][0])>=0?fabs(_y):-fabs(_y);
87 z() = (m[1][0] - m[0][1])>=0?fabs(_z):-fabs(_z);
91 double sa = sin(angle/2);
92 double ca = cos(angle/2);
93 x() = (float) (axis.
x()*sa);
94 y() = (float) (axis.
y()*sa);
95 z() = (float) (axis.
z()*sa);
101 for (
unsigned int i=0; i<4; i++) {
104 return (
float) sqrt(n);
109 for (
unsigned int i=0; i<4; ++i)
115 for (
unsigned int i=0; i<4; i++)
117 if (
operator()(i) != other(i))
127 double s = n > 0?2./(n*n):0.;
147 m[0][0] = 1.0 - (yy + zz);
148 m[1][1] = 1.0 - (xx + zz);
149 m[2][2] = 1.0 - (xx + yy);
159 float roll = (float) atan2(m[2][1], m[2][2]);
160 float pitch = (float) atan2(-m[2][0], sqrt(m[2][1]*m[2][1] + m[2][2]*m[2][2]));
161 float yaw = (float) atan2(m[1][0], m[0][0]);
163 return Vector3(roll, pitch, yaw);
171 double s = n > 0?2./(n*n):0.;
190 m[0][0] = 1.0 - (yy + zz);
191 m[1][1] = 1.0 - (xx + zz);
192 m[2][2] = 1.0 - (xx + yy);
202 rot_matrix_3_3.clear();
203 rot_matrix_3_3.resize(9,0.);
204 for (
unsigned int i=0; i<3; i++) {
205 rot_matrix_3_3[i*3] = m[i][0];
206 rot_matrix_3_3[i*3+1] = m[i][1];
207 rot_matrix_3_3[i*3+2] = m[i][2];
221 y()*other.
z() - other.
y()*
z() +
u()*other.
x() + other.
u()*
x(),
222 z()*other.
x() - other.
z()*
x() +
u()*other.
y() + other.
u()*
y(),
223 x()*other.
y() - other.
x()*
y() +
u()*other.
z() + other.
u()*
z());
227 return *
this *
Quaternion(0, v(0), v(1), v(2));
235 double len =
norm ();
237 *
this /= (float) len;
264 for (
unsigned int i=0; i<4; i++)
272 for (
unsigned int i=0; i<4; i++)
273 s <<
" " <<
operator()(i);
281 s.read((
char*)&temp,
sizeof(temp));
283 for (
unsigned int i=0; i<4; i++) {
284 s.read((
char*)&val,
sizeof(val));
293 s.write((
char*)&temp,
sizeof(temp));
295 for (
unsigned int i=0; i<4; i++) {
297 s.write((
char*)&val,
sizeof(val));
305 s <<
"(" << q.
u() <<
" " << q.
x() <<
" " << q.
y() <<
" " << q.
z() <<
")";
Quaternion & operator=(const Quaternion &other)
bool operator==(const Quaternion &other) const
std::ostream & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
std::istream & read(std::istream &s)
std::ostream & writeBinary(std::ostream &s) const
Quaternion operator*(const Quaternion &other) const
Quaternion multiplication.
void toRotMatrix(std::vector< double > &rot_matrix_3_3) const
Vector3 toEuler() const
Conversion to Euler angles.
Vector3 rotate(const Vector3 &v) const
Rotate a vector.
Quaternion normalized() const
This class represents a three-dimensional vector.
Quaternion inv() const
Inversion.
Quaternion & inv_IP()
Inversion.
Quaternion()
Default constructor.
std::ostream & write(std::ostream &s) const
This class represents a Quaternion.
const float & operator()(unsigned int i) const
std::istream & readBinary(std::istream &s)