# HG changeset patch
# User Willem Atsma <willem.atsma@tanglebridge.com>
# Date 1421454302 28800
#      Fri Jan 16 16:25:02 2015 -0800
# Node ID 4923b520e9a5c029df94843925e8f52763cc8490
# Parent  c7eb69973a97bde5f77eac3eb869c75413fee6c3
New quaternion functions mean, numel, rotm2q

diff -r c7eb69973a97 -r 4923b520e9a5 inst/@quaternion/mean.m
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inst/@quaternion/mean.m	Fri Jan 16 16:25:02 2015 -0800
@@ -0,0 +1,31 @@
+## Copyright (C) 2013 Willem Atsma
+## 
+## This program is free software; you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation; either version 3 of the License, or
+## (at your option) any later version.
+## 
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU General Public License for more details.
+## 
+## You should have received a copy of the GNU General Public License
+## along with Octave; see the file COPYING.  If not, see
+## <http://www.gnu.org/licenses/>.
+
+## @quaternion/mean - return mean of quaternion matrix
+
+## Author: Willem Atsma <willem.atsma@tanglebridge.com>
+## Created: 2013-06-06
+
+function q = mean (q,varargin)
+
+  w = mean (q.w,varargin{:});
+  x = mean (q.x,varargin{:});
+  y = mean (q.y,varargin{:});
+  z = mean (q.z,varargin{:});
+
+  q = quaternion (w, x, y, z);
+   
+endfunction
diff -r c7eb69973a97 -r 4923b520e9a5 inst/@quaternion/numel.m
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inst/@quaternion/numel.m	Fri Jan 16 16:25:02 2015 -0800
@@ -0,0 +1,26 @@
+## Copyright (C) 2013 Willem Atsma
+## 
+## This program is free software; you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation; either version 3 of the License, or
+## (at your option) any later version.
+## 
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU General Public License for more details.
+## 
+## You should have received a copy of the GNU General Public License
+## along with Octave; see the file COPYING.  If not, see
+## <http://www.gnu.org/licenses/>.
+
+## @quaternion/numel - return numel of quaternion object
+##
+
+## Author: Willem Atsma <willem.atsma@tanglebridge.com>
+## Created: 2013-06-04
+## Version: 0.1
+
+function n = numel (q)
+   n = numel(q.w);
+endfunction
diff -r c7eb69973a97 -r 4923b520e9a5 inst/rotm2q.m
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inst/rotm2q.m	Fri Jan 16 16:25:02 2015 -0800
@@ -0,0 +1,68 @@
+## Copyright (C) 2013 Willem Atsma
+## 
+## This program is free software; you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation; either version 3 of the License, or
+## (at your option) any later version.
+## 
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU General Public License for more details.
+## 
+## You should have received a copy of the GNU General Public License
+## along with Octave; see the file COPYING.  If not, see
+## <http://www.gnu.org/licenses/>.
+
+## rotm2q - convert a 3x3 rotation matrix to a unit quaternion
+
+## Author: Willem Atsma willem.atsma@tanglebridge.com
+## Created: 2013-05-14
+## Code based on C code at the wonderful resource:
+##    http://www.euclideanspace.com/
+
+function q = rotm2q (R)
+
+  T = trace(R);
+  if(T>0)
+    s = 0.5/sqrt(T+1);
+    w = 0.25/s;
+    x = ( R(3,2) - R(2,3) ) * s;
+    y = ( R(1,3) - R(3,1) ) * s;
+    z = ( R(2,1) - R(1,2) ) * s;
+  else
+    if ( R(1,1)>R(2,2) && R(1,1)>R(3,3) )
+      s = 2*sqrt(1 + R(1,1) - R(2,2) - R(3,3));
+      w = (R(3,2) - R(2,3) ) / s;
+      x = 0.25*s;
+      y = (R(1,2) + R(2,1) ) / s;
+      z = (R(1,3) + R(3,1) ) / s;
+    elseif (R(2,2) > R(3,3))
+      s = 2*sqrt( 1 + R(2,2) - R(1,1) - R(3,3));
+      w = (R(1,3) - R(3,1) ) / s;
+      x = (R(1,2) + R(2,1) ) / s;
+      y = 0.25*s;
+      z = (R(2,3) + R(3,2) ) / s;
+    else
+      s = 2*sqrt( 1 + R(3,3) - R(1,1) - R(2,2) );
+      w = (R(2,1) - R(1,2) ) / s;
+      x = (R(1,3) + R(3,1) ) / s;
+      y = (R(2,3) + R(3,2) ) / s;
+      z = 0.25 * s;
+    endif
+endif
+
+q = quaternion(w,x,y,z);
+
+endfunction
+
+%!test
+%! R = eye(3);
+%! q = rotm2q(R);
+%! assert( q.w==1 && q.x==0 && q.y==0 && q.z==0 );
+%!test
+%! R = [[1;0;0] [0;0;1] [0;-1;0]];
+%! q = rotm2q (R);
+%! [ax,an] = q2rot(q);
+%! assert ( all(ax==[1;0;0]) && an==pi/2 );
+
