geom.h

#pragma once
 
#include "fmat.h"
 
namespace cgv {
    namespace math {
 
        template <typename T>
        cgv::math::fvec<T, 3> rotate(const cgv::math::fvec<T, 3>& v, const cgv::math::fvec<T, 3>& n, T a)
        {
            cgv::math::fvec<T, 3> vn = dot(n, v)*n;
            return vn + cos(a)*(v - vn) + sin(a)*cross(n, v);
        }
 
        template <typename T>
        void compute_rotation_axis_and_angle_from_vector_pair(const cgv::math::fvec<T, 3>& v0, const cgv::math::fvec<T, 3>& v1, cgv::math::fvec<T, 3>& axis, T& angle)
        {
            axis = cross(v0, v1);
            T len = axis.length();
            if (len > 2 * std::numeric_limits<T>::epsilon())
                axis *= T(1) / len;
            else
                axis[1] = T(1);
            angle = atan2(len, dot(v0, v1));
        }
 
        template <typename T>
        int decompose_rotation_to_axis_and_angle(const cgv::math::fmat<T, 3, 3>& R, cgv::math::fvec<T, 3>& axis, T& angle)
        {
            axis(0) = R(2, 1) - R(1, 2);
            axis(1) = R(0, 2) - R(2, 0);
            axis(2) = R(1, 0) - R(0, 1);
            T len = axis.length();
            T tra = R.trace();
            if (len < 2 * std::numeric_limits<T>::epsilon()) {
                if (tra < 0) {
                    for (unsigned c = 0; c<3; ++c)
                        if (R(c, c) > 0) {
                            axis(c) = T(1);
                            angle = M_PI;
                            break;
                        }
                    return 1;
                }
                else {
                    axis(1) = T(1);
                    angle = 0;
                    return 2;
                }
            }
            else {
                axis *= T(1) / len;
                angle = atan2(len, tra - T(1));
                return 0;
            }
        }
 
        template <typename T>
        cgv::math::fmat<T, 3, 3> build_orthogonal_frame(const cgv::math::fvec<T, 3>& v0, const cgv::math::fvec<T, 3>& v1)
        {
            cgv::math::fmat<T, 3, 3> O;
 
            auto &x = /*(cgv::math::fvec<T, 3>&)O(0, 0)*/O.col(0);
            auto &y = /*(cgv::math::fvec<T, 3>&)O(0, 1)*/O.col(1);
            auto &z = /*(cgv::math::fvec<T, 3>&)O(0, 2)*/O.col(2);
 
            x = v0;
            x.normalize();
            z = cross(v0, v1);
            z.normalize();
            y = cross(z, x);
 
            return O;
        }
 
    }
}