add angdist methods

Author: petercorke

SO3.m

@@ -534,6 +534,34 @@
             [varargout{1:nargout}] = tr2angvec(obj.R, varargin{:});
         end
 
+        
+        function d = angdist(R1, R2, method)
+            %SO3.angdist Distance metric
+            %
+            % R1.angdist(R2) is a distance metric between two SO(3) rotation
+            % matrices.
+            %
+            % R1.angdist(R2, M) as above but uses the method specified by M
+            %
+            % M = 1 (default): || I - R1 R2' ||  in [0,2]
+            % M = 2:           || log R1 R2' ||  in [0,pi]
+            %
+            %
+            % See also: trlog, UnitQuaternion.angdist
+            if ~isa(R2, 'SO3')
+                error('SMTB:SO3:badarg', 'angdist: incorrect operand');
+            end
+            if nargin == 2
+                method = 1
+            end
+            
+            switch method
+                case 1
+                    d = norm(eye(3) - R1.R * R2.R')
+                case 2
+                    d = norm(trlog(R1.R * R2.R'))
+            end
+        end
 
 
         function s = SE3(obj)

UnitQuaternion.m

@@ -950,6 +950,37 @@ function animate(Q, varargin)
                 error('SMTB:UnitQuaternion:eq: badargs');
             end
         end
+        
+        
+        function d = angdist(q1, q2, method)
+            %UnitQuaternion.angdist Distance metric
+            %
+            % Q1.angdist(Q2) is a distance metric between two unit quaternions.
+            %
+            % Q1.angdist(Q2, M) as above but uses the method specified by M
+            %
+            % M = 1 (default): 1 - | < q1, q2> |  in [0,pi/2]
+            % M = 2:           acos | <q1, q2> |  in [0,1]
+            %
+            %  
+            % Note::
+            %   - angdist(q, -q) is equal to zero due to double mapping
+            %
+            % See also: SO3.angdist
+            if ~isa(q2, 'UnitQuaternion')
+                error('SMTB:UnitQuaternion:badarg', 'angdist: incorrect operand');
+            end
+            if nargin == 2
+                method = 1
+            end
+            
+            switch method
+                case 1
+                    d = 1 - abs(q1.inner(q2))
+                case 2
+                    d = acos(abs(q1.inner(q2)))
+            end
+        end
 
     end % methods