so3.hpp

浏览该文件的文档.

#pragma once
#include "TensorType.hpp"
#include "VectorType.hpp"
#include <iostream>
#include <cmath>
 
namespace z
{
    namespace math
    {
        template<typename Scalar>
        z::math::Vector<Scalar, 3> cross(const z::math::Vector<Scalar, 3>& a, const z::math::Vector<Scalar, 3>& b)
        {
            z::math::Vector<Scalar, 3> result;
            result[0] = a[1] * b[2] - a[2] * b[1];
            result[1] = a[2] * b[0] - a[0] * b[2];
            result[2] = a[0] * b[1] - a[1] * b[0];
            return result;
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 4> quat_conjugate(const z::math::Vector<Scalar, 4>& quat)
        {
            return z::math::Vector<Scalar, 4>{ -quat[0], -quat[1], -quat[2], quat[3] };
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 4> quat_unit(const z::math::Vector<Scalar, 4>& quat)
        {
            Scalar norm = std::sqrt(quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]);
            if (norm == 0) {
                throw std::runtime_error("Quaternion norm is zero, cannot normalize.");
            }
            return z::math::Vector<Scalar, 4>{ quat[0] / norm, quat[1] / norm, quat[2] / norm, quat[3] / norm };
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 4> quat_mul(const z::math::Vector<Scalar, 4>& a, const z::math::Vector<Scalar, 4>& b)
        {
            Scalar x1 = a[0], y1 = a[1], z1 = a[2], w1 = a[3];
            Scalar x2 = b[0], y2 = b[1], z2 = b[2], w2 = b[3];
 
            Scalar ww = (z1 + x1) * (x2 + y2);
            Scalar yy = (w1 - y1) * (w2 + z2);
            Scalar zz = (w1 + y1) * (w2 - z2);
            Scalar xx = ww + yy + zz;
            Scalar qq = 0.5 * (xx + (z1 - x1) * (x2 - y2));
            Scalar w = qq - ww + (z1 - y1) * (y2 - z2);
            Scalar x = qq - xx + (x1 + w1) * (x2 + w2);
            Scalar y = qq - yy + (w1 - x1) * (y2 + z2);
            Scalar z = qq - zz + (z1 + y1) * (w2 - x2);
 
            return z::math::Vector<Scalar, 4>{ x, y, z, w };
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 3> quat_rotate(const z::math::Vector<Scalar, 4>& q, const z::math::Vector<Scalar, 3>& v)
        {
            Scalar q_w = q[3];
            z::math::Vector<Scalar, 3> q_vec = { q[0], q[1], q[2] };
            z::math::Vector<Scalar, 3> a = v * (2 * q_w * q_w - 1.0);
            z::math::Vector<Scalar, 3> b = cross(q_vec, v) * (2 * q_w);
            z::math::Vector<Scalar, 3> c = q_vec * (q_vec.dot(v)) * static_cast<Scalar>(2.0);
            return a + b + c;
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 3> quat_rotate_inverse(const z::math::Vector<Scalar, 4>& q, const z::math::Vector<Scalar, 3>& v)
        {
            Scalar q_w = q[3];
            z::math::Vector<Scalar, 3> q_vec = { q[0], q[1], q[2] };
            z::math::Vector<Scalar, 3> a = v * (2 * q_w * q_w - 1.0);
            z::math::Vector<Scalar, 3> b = cross(q_vec, v) * (2 * q_w);
            z::math::Vector<Scalar, 3> c = q_vec * (q_vec.dot(v)) * static_cast<Scalar>(2.0);
            return a - b + c;
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 4> quat_from_euler_xyz(const z::math::Vector<Scalar, 3>& euler)
        {
            Scalar cy = std::cos(euler[2] * 0.5);
            Scalar sy = std::sin(euler[2] * 0.5);
            Scalar cp = std::cos(euler[1] * 0.5);
            Scalar sp = std::sin(euler[1] * 0.5);
            Scalar cr = std::cos(euler[0] * 0.5);
            Scalar sr = std::sin(euler[0] * 0.5);
 
            z::math::Vector<Scalar, 4> quat;
            Scalar qw, qx, qy, qz;
            qw = cy * cr * cp + sy * sr * sp;
            qx = cy * sr * cp - sy * cr * sp;
            qy = cy * cr * sp + sy * sr * cp;
            qz = sy * cr * cp - cy * sr * sp;
            quat = { qx, qy, qz, qw };
 
            return quat;
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 3> get_euler_xyz(const z::math::Vector<Scalar, 4>& quat)
        {
            Scalar qw = quat[3];
            Scalar qx = quat[0];
            Scalar qy = quat[1];
            Scalar qz = quat[2];
 
            Scalar roll = std::atan2(2.0 * (qy * qz + qw * qx), qw * qw - qx * qx - qy * qy + qz * qz);
            Scalar pitch = std::asin(-2.0 * (qx * qz - qw * qy));
            Scalar yaw = std::atan2(2.0 * (qx * qy + qw * qz), qw * qw + qx * qx - qy * qy - qz * qz);
 
            return z::math::Vector<Scalar, 3>{ roll, pitch, yaw };
        }
 

        template<typename Scalar>
        z::math::Vector<Scalar, 3> so3_from_quat(const z::math::Vector<Scalar, 4>& quat)
        {
            auto quat = quat_unit(quat);
            z::math::Vector<Scalar, 3> v = { quat[0], quat[1], quat[2] };
            Scalar w = quat[3];
            Scalar theta = 2 * std::acos(w);
            Scalar sin_theta = std::sin(theta / 2.0);
            if (std::abs(sin_theta) < 1e-6) {
                return z::math::Vector<Scalar, 3>{0, 0, 0}; // Return zero vector if sin(theta/2) is too small
            }
            return v * (theta / sin_theta);
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 4> so3_to_quat(const z::math::Vector<Scalar, 3>& so3)
        {
            Scalar theta = so3.length();
            if (theta < 1e-6) {
                return z::math::Vector<Scalar, 4>{0, 0, 0, 1}; // Return identity quaternion if norm is too small
            }
            Scalar half_theta = theta / 2.0;
            Scalar sin_half_theta = std::sin(half_theta);
            return z::math::Vector<Scalar, 4>{ so3[0] * sin_half_theta / theta,
                so3[1] * sin_half_theta / theta,
                so3[2] * sin_half_theta / theta,
                std::cos(half_theta) };
        }
 

        template<typename Scalar>
        z::math::Vector<Scalar, 4> quat_slerp(const z::math::Vector<Scalar, 4>& a, const z::math::Vector<Scalar, 4>& b, Scalar t)
        {
            // Ensure both quaternions are unit quaternions
            auto a_unit = quat_unit(a);
            auto b_unit = quat_unit(b);
 
            // Compute the dot product
            Scalar dot = a_unit.dot(b_unit);
 
            // If the dot product is negative, negate one quaternion to take the shortest path
            if (dot < 0.0) {
                b_unit = -b_unit;
                dot = -dot;
            }
 
            // Perform spherical linear interpolation
            Scalar theta = std::acos(dot);
            Scalar sin_theta = std::sin(theta);
            if (sin_theta < 1e-6) {
                // If sin(theta) is too small, return a linear interpolation
                return a_unit * (1 - t) + b_unit * t;
            }
            Scalar a_scale = std::sin((1 - t) * theta) / sin_theta;
            Scalar b_scale = std::sin(t * theta) / sin_theta;
            return quat_unit(a_unit * a_scale + b_unit * b_scale);
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 4> quat_diff(const z::math::Vector<Scalar, 4>& a, const z::math::Vector<Scalar, 4>& b)
        {
            auto quat_a = quat_unit(a);
            auto quat_b = quat_unit(b);
            Scalar dot = quat_a.dot(quat_b);
            if (dot < 0.0) {
                quat_b = -quat_b; // Ensure the shortest path
            }
            return quat_unit(quat_mul(quat_conjugate(a), b));
        }

        template<typename Scalar>
        z::math::Vector<Scalar, 3> so3_diff(const z::math::Vector<Scalar, 3>& a, const z::math::Vector<Scalar, 3>& b)
        {
            Scalar dot = a.dot(b);
            if (dot < 0.0) {
                return cross(b, a);
            }
            return cross(a, b);
        }
    };
}