1 // This file is part of libigl, a simple c++ geometry processing library.
2 //
3 // Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>, Olga Diamanti <olga.diam@gmail.com>
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public License
6 // v. 2.0. If a copy of the MPL was not distributed with this file, You can
7 // obtain one at http://mozilla.org/MPL/2.0/.
8
9 #include "rotation_matrix_from_directions.h"
10 #include <Eigen/Geometry>
11 #include <iostream>
12
13 template <typename Scalar>
rotation_matrix_from_directions(const Eigen::Matrix<Scalar,3,1> v0,const Eigen::Matrix<Scalar,3,1> v1)14 IGL_INLINE Eigen::Matrix<Scalar, 3, 3> igl::rotation_matrix_from_directions(
15 const Eigen::Matrix<Scalar, 3, 1> v0,
16 const Eigen::Matrix<Scalar, 3, 1> v1)
17 {
18 Eigen::Matrix<Scalar, 3, 3> rotM;
19 const double epsilon=1e-8;
20 Scalar dot=v0.normalized().dot(v1.normalized());
21 ///control if there is no rotation
22 if ((v0-v1).norm()<epsilon)
23 {
24 rotM = Eigen::Matrix<Scalar, 3, 3>::Identity();
25 return rotM;
26 }
27 if ((v0+v1).norm()<epsilon)
28 {
29 rotM = -Eigen::Matrix<Scalar, 3, 3>::Identity();
30 rotM(0,0) = 1.;
31 std::cerr<<"igl::rotation_matrix_from_directions: rotating around x axis by 180o"<<std::endl;
32 return rotM;
33 }
34 ///find the axis of rotation
35 Eigen::Matrix<Scalar, 3, 1> axis;
36 axis=v0.cross(v1);
37 axis.normalize();
38
39 ///construct rotation matrix
40 Scalar u=axis(0);
41 Scalar v=axis(1);
42 Scalar w=axis(2);
43 Scalar phi=acos(dot);
44 Scalar rcos = cos(phi);
45 Scalar rsin = sin(phi);
46
47 rotM(0,0) = rcos + u*u*(1-rcos);
48 rotM(1,0) = w * rsin + v*u*(1-rcos);
49 rotM(2,0) = -v * rsin + w*u*(1-rcos);
50 rotM(0,1) = -w * rsin + u*v*(1-rcos);
51 rotM(1,1) = rcos + v*v*(1-rcos);
52 rotM(2,1) = u * rsin + w*v*(1-rcos);
53 rotM(0,2) = v * rsin + u*w*(1-rcos);
54 rotM(1,2) = -u * rsin + v*w*(1-rcos);
55 rotM(2,2) = rcos + w*w*(1-rcos);
56
57 return rotM;
58 }
59
60 #ifdef IGL_STATIC_LIBRARY
61 // Explicit template instantiation
62 template Eigen::Matrix<double, 3, 3, 0, 3, 3> igl::rotation_matrix_from_directions<double>(const Eigen::Matrix<double, 3, 1, 0, 3, 1>, const Eigen::Matrix<double, 3, 1, 0, 3, 1>);
63 #endif
64