Answer the question
In order to leave comments, you need to log in
Is it possible to get the difference of 2 quaternions by doing the Krockner product?
There are two quaternions: an absolute orientation quaternion and a second quaternion with configurable orientation parameters. Can I get the orientation difference of these quaternions by applying the Krockner product?
Answer the question
In order to leave comments, you need to log in
The difference of quaternions can be obtained by multiplying quaternion 1 by the inverted quaternion 2.
An example of a library with the implementation of the functions of multiplication, inversion, division (multiplication + inversion).
class Quaternion
{
public:
Quaternion(): _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
Quaternion(double w, double x, double y, double z):
_w(w), _x(x), _y(y), _z(z) {}
Quaternion(double w, Vector<3> vec):
_w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
double& w()
{
return _w;
}
double& x()
{
return _x;
}
double& y()
{
return _y;
}
double& z()
{
return _z;
}
double w() const
{
return _w;
}
double x() const
{
return _x;
}
double y() const
{
return _y;
}
double z() const
{
return _z;
}
void Set(double w, double x, double y, double z)
{
_w = w; _x = x; _y = y; _z = z;
}
double magnitude() const
{
return sqrt(_w*_w + _x*_x + _y*_y + _z*_z);
}
Quaternion conjugate() const
{
return Quaternion(_w, -_x, -_y, -_z);
}
void fromAxisAngle(const Vector<3>& axis, double theta)
{
_w = cos(theta/2);
//only need to calculate sine of half theta once
double sht = sin(theta/2);
_x = axis.x() * sht;
_y = axis.y() * sht;
_z = axis.z() * sht;
}
void fromMatrix(const Matrix<3>& m)
{
double tr = m.trace();
double S;
if (tr > 0)
{
S = sqrt(tr+1.0) * 2;
_w = 0.25 * S;
_x = (m(2, 1) - m(1, 2)) / S;
_y = (m(0, 2) - m(2, 0)) / S;
_z = (m(1, 0) - m(0, 1)) / S;
}
else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
{
S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
_w = (m(2, 1) - m(1, 2)) / S;
_x = 0.25 * S;
_y = (m(0, 1) + m(1, 0)) / S;
_z = (m(0, 2) + m(2, 0)) / S;
}
else if (m(1, 1) > m(2, 2))
{
S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
_w = (m(0, 2) - m(2, 0)) / S;
_x = (m(0, 1) + m(1, 0)) / S;
_y = 0.25 * S;
_z = (m(1, 2) + m(2, 1)) / S;
}
else
{
S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
_w = (m(1, 0) - m(0, 1)) / S;
_x = (m(0, 2) + m(2, 0)) / S;
_y = (m(1, 2) + m(2, 1)) / S;
_z = 0.25 * S;
}
}
void toAxisAngle(Vector<3>& axis, double& angle) const
{
double sqw = sqrt(1-_w*_w);
if (sqw == 0) //it's a singularity and divide by zero, avoid
return;
angle = 2 * acos(_w);
axis.x() = _x / sqw;
axis.y() = _y / sqw;
axis.z() = _z / sqw;
}
Matrix<3> toMatrix() const
{
Matrix<3> ret;
ret.cell(0, 0) = 1 - 2*_y*_y - 2*_z*_z;
ret.cell(0, 1) = 2*_x*_y - 2*_w*_z;
ret.cell(0, 2) = 2*_x*_z + 2*_w*_y;
ret.cell(1, 0) = 2*_x*_y + 2*_w*_z;
ret.cell(1, 1) = 1 - 2*_x*_x - 2*_z*_z;
ret.cell(1, 2) = 2*_y*_z - 2*_w*_x;
ret.cell(2, 0) = 2*_x*_z - 2*_w*_y;
ret.cell(2, 1) = 2*_y*_z + 2*_w*_x;
ret.cell(2, 2) = 1 - 2*_x*_x - 2*_y*_y;
return ret;
}
Vector<3> ToEulerAngles (Quaternion q) {
Vector<3> angles;
// yaw (z-axis rotation)
double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
angles.z() = std::atan2(sinr_cosp, cosr_cosp) * rad_to_deg;
// pitch (y-axis rotation)
double sinp = 2 * (q.w() * q.y() - q.z() * q.x());
if (std::abs(sinp) >= 1)
angles.y() = std::copysign(M_PI / 2, sinp) * rad_to_deg; // use 90 degrees if out of range
else
angles.y() = std::asin(sinp) * rad_to_deg;
// roll (x-axis rotation)
double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
angles.x() = std::atan2(siny_cosp, cosy_cosp) * rad_to_deg;
return angles;
}
Vector<3> ToEuler() const
{
Vector<3> ret;
double sqw = _w*_w;
double sqx = _x*_x;
double sqy = _y*_y;
double sqz = _z*_z;
ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw)) * rad_to_deg;
ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw)) * rad_to_deg;
ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw)) * rad_to_deg;
return ret;
}
void EulerToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
pitch *= deg_to_rad;
roll *= deg_to_rad;
yaw *= deg_to_rad;
// Abbreviations for the various angular functions
double c1 = cos(yaw * 0.5);
double s1 = sin(yaw * 0.5);
double c2 = cos(pitch * 0.5);
double s2 = sin(pitch * 0.5);
double c3 = cos(roll * 0.5);
double s3 = sin(roll * 0.5);
_w = c1 * c2 * c3 - s1 * s2 * s3;
_x = s1 * s2 * c3 + c1 * c2 * s3;
_y = s1 * c2 * c3 + c1 * s2 * s3;
_z = c1 * s2 * c3 - s1 * c2 * s3;
}
Vector<3> toAngularVelocity(double dt) const
{
Vector<3> ret;
Quaternion one(1.0, 0.0, 0.0, 0.0);
Quaternion delta = one - *this;
Quaternion r = (delta/dt);
r = r * 2;
r = r * one;
ret.x() = r.x();
ret.y() = r.y();
ret.z() = r.z();
return ret;
}
Vector<3> AngularVelocity_to_Quaternion (Quaternion q, double dt) const
{
Vector<3> ret;
Quaternion delta = q - *this;
Quaternion r = (delta/dt);
r = r * 2;
r = r * q;
ret.x() = r.x();
ret.y() = r.y();
ret.z() = r.z();
return ret;
}
Vector<3> rotateVector(const Vector<2>& v) const
{
return rotateVector(Vector<3>(v.x(), v.y()));
}
Vector<3> rotateVector(const Vector<3>& v) const
{
Vector<3> qv(_x, _y, _z);
Vector<3> t = qv.cross(v) * 2.0;
return v + t*_w + qv.cross(t);
}
Quaternion operator*(const Quaternion& q) const
{
return Quaternion(
_w*q._w - _x*q._x - _y*q._y - _z*q._z,
_w*q._x + _x*q._w + _y*q._z - _z*q._y,
_w*q._y - _x*q._z + _y*q._w + _z*q._x,
_w*q._z + _x*q._y - _y*q._x + _z*q._w
);
}
Quaternion operator *= (const Quaternion q)
{
_w = _w*q._w - _x*q._x - _y*q._y - _z*q._z;
_x = _w*q._x + _x*q._w + _y*q._z - _z*q._y;
_y = _w*q._y + _y*q._w + _z*q._x - _x*q._z;
_z = _w*q._z + _z*q._w + _x*q._y - _y*q._x;
return (*this);
}
Quaternion operator+(const Quaternion& q) const
{
return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
}
Quaternion operator-(const Quaternion& q) const
{
return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
}
Quaternion operator/(double scalar) const
{
return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
}
Quaternion operator += (const Quaternion q)
{
_w += q._w;
_x += q._x;
_y += q._y;
_z += q._z;
return (*this);
}
Quaternion operator -= (const Quaternion q)
{
_w -= q._w;
_x -= q._x;
_y -= q._y;
_z -= q._z;
return (*this);
}
Quaternion operator*(double scalar) const
{
return scale(scalar);
}
Quaternion scale(double scalar) const
{
return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
}
bool operator== (Quaternion q) const
{
return ((_w == q._w) && (_x == q._x) &&
(_y == q._y) && (_z == q._z));
}
bool operator!= (Quaternion q) const
{
return ((_w != q._w) || (_x != q._x) ||
(_y != q._y) || (_z != q._z));
}
Quaternion operator= (Quaternion q)
{
_w = q._w;
_x = q._x;
_y = q._y;
_z = q._z;
return *this;
}
Quaternion operator= (double w)
{
_w = w;
_x = 0;
_y = 0;
_z = 0;
return *this;
}
double Norm2()
{
return (_w *_w + _x *_x + _y *_y + _z *_z);
}
// q.Normalize() scales q such that it is unit size
Quaternion Normalize_1()
{
double invNorm;
invNorm = (double)1.0 / (double)sqrt(Norm2());
_w *= invNorm;
_x *= invNorm;
_y *= invNorm;
_z *= invNorm;
return *this;
}
void Normalize_2()
{
double mag = magnitude();
*this = this->scale(1/mag);
}
Quaternion operator/ (Quaternion q2) const
{
// compute invQ2 = q2^{-1}
Quaternion invQ2;
double invNorm2 = 1.0 / q2.Norm2();
invQ2._w = q2._w * invNorm2;
invQ2._x = -q2._x * invNorm2;
invQ2._y = -q2._y * invNorm2;
invQ2._z = -q2._z * invNorm2;
// result = *this * invQ2
return (*this * invQ2);
}
Quaternion operator /= (Quaternion q)
{
(*this) = (*this)*q.Inverse();
return (*this);
}
Quaternion Inverse()
{
return conjugate().scale(1/Norm2());
}
void QuatRotation(Vector<3> v)
{
Quaternion qv(0, v[0], v[1], v[2]);
Quaternion qm = (*this) * qv * (*this).Inverse();
v[0] = qm._x;
v[1] = qm._y;
v[2] = qm._z;
}
private:
double _w, _x, _y, _z;
double deg_to_rad = 0.0174533;
double rad_to_deg = 57.2958;
};
Didn't find what you were looking for?
Ask your questionAsk a Question
731 491 924 answers to any question