1%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch>
2%%
3%%    This program is free software: you can redistribute it and/or modify
4%%    it under the terms of the GNU General Public License as published by
5%%    the Free Software Foundation, either version 3 of the License, or
6%%    any later version.
7%%
8%%    This program is distributed in the hope that it will be useful,
9%%    but WITHOUT ANY WARRANTY; without even the implied warranty of
10%%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11%%    GNU General Public License for more details.
12%%
13%%    You should have received a copy of the GNU General Public License
14%%    along with this program. If not, see <http://www.gnu.org/licenses/>.
15
16%% -*- texinfo -*-
17%% @deftypefn {Function File} {@var{q} = } mat2quat (@var{R})
18%% This function is implemented in paclage quaternions and will be deprecated.
19%% @end deftypefn
20function q = mat2quat(R)
21
22  if all(R == eye(3))
23    q = [1 0 0 0];
24    return
25  end
26
27  %% Angle
28  phi = acos((trace(R)-1)/2);
29
30  %% Axis
31  x = [R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)];
32  x = x/sqrt(sumsq(x));
33
34  q = [ cos(phi/2) sin(phi/2)*x];
35
36endfunction
37