@Userpc0101
Радиолюбитель.

Можно ли получить разность 2-ух кватернионов выполнив произведение Крокнера?

Есть два кватерниона: кватернион абсолютной ориентации и второй кватернион с задаваемыми параметрами для ориентации. Смогу я получить разность ориентации этих кватернионов, применив произведение Крокнера?
  • Вопрос задан
  • 240 просмотров
Решения вопроса 1
@Userpc0101 Автор вопроса
Радиолюбитель.
Разность кватернионов можно получить умножив кватерион 1 на инвертированный кватернион 2.

Пример библиотеки с реализацией функций умножения, инвертирования, деления (умножения + инвертирования).

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;
};
Ответ написан
Пригласить эксперта
Ответы на вопрос 1
@Taus
Если под "произведением Кронекера" понимается стандартное определение, то независимо от представления кватернионов их произведение Кронекера не будет кватернионом.
Ответ написан
Ваш ответ на вопрос

Войдите, чтобы написать ответ

Войти через центр авторизации
Похожие вопросы