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;
 
                        cgv::math::fvec<T, 3>& x = (cgv::math::fvec<T, 3>&)O(0, 0);
                        cgv::math::fvec<T, 3>& y = (cgv::math::fvec<T, 3>&)O(0, 1);
                        cgv::math::fvec<T, 3>& z = (cgv::math::fvec<T, 3>&)O(0, 2);
 
                        x = v0;
                        x.normalize();
                        z = cross(v0, v1);
                        z.normalize();
                        y = cross(z, x);
 
                        return O;
                }
 
        }
}