Updated divide-by-zero vulnerability for MRPs; added tests for quater…

…nion interpolation

mex/QuaternionUnitTestsMex.m

@@ -107,14 +107,29 @@ function test_qinterp(test)
     end
 
     % Form the truth.
-    ti   = 0:0.5:10;
+    ti   = [-1 0 0.5 1 7 7.5 8 10 10.1];
     qi_0 = zeros(4, length(ti));
     for k = 1:length(ti)
-        qi_0(:,k) = aa2q_mex(w*ti(k), r);
+        qi_0(:,k) = aa2q_mex(w * max(min(ti(k), t(end)), 0), r);
     end
 
-    qi = qinterp(t, q, ti);
+    % Results.
+    qi = qinterp(t, q, ti, 'Ordered', true);    
+    test.verifyEqual(qi, qi_0, 'AbsTol', 10*eps);
+
+    % Now with out-of-order points.
+    ti   = [-1 7 7.5 8 0 0.5 1 10 10.1];
+    qi_0 = zeros(4, length(ti));
+    for k = 1:length(ti)
+        qi_0(:,k) = aa2q_mex(w * max(min(ti(k), t(end)), 0), r);
+    end
 
+    % Results.
+    qi = qinterp(t, q, ti, 'ordered', false);
+    test.verifyEqual(qi, qi_0, 'AbsTol', 10*eps);
+
+    % Results (assumes unordered by default).
+    qi = qinterp(t, q, ti);
     test.verifyEqual(qi, qi_0, 'AbsTol', 10*eps);
 
 end % qinterp
@@ -125,8 +140,8 @@ function test_qinterp(test)
 
 function test_qdot(test)
 
-    r = [1; 0; 0];%randunit(3);
-    w = 1;%randn();
+    r = randunit(3);
+    w = randn();
     t = 0:0.1:10;
     n = length(t);
 
@@ -146,7 +161,7 @@ function test_qdot(test)
         q(:,k) = q(:,k-1) + qd * dt;
     end
 
-    test.verifyEqual(q, q_0, 'AbsTol', 1e-6);
+    test.verifyEqual(q, q_0, 'AbsTol', 1e-5);
 
 end % qdot
 

mrp2q.m

@@ -21,8 +21,7 @@
 
 %#codegen
 
-    % Set defaults so that for small angles, the scaled MRPs approach the
-    % rotation vector.
+    % Set defaults for the standard MRPs.
     if nargin < 2 || isempty(f), f = 1; end;
 
     % Check dimensions.

mrpcomp.m

@@ -40,17 +40,18 @@
     % If in MATLAB, vectorize.
     if isempty(coder.target)
 
-        f2 = f * f;
-        p1m2 = pa(1,:).*pa(1,:) + pa(2,:).*pa(2,:) + pa(3,:).*pa(3,:);
-        p2m2 = pb(1,:).*pb(1,:) + pb(2,:).*pb(2,:) + pb(3,:).*pb(3,:);
-        d    = pa(1,:).*pb(1,:) + pa(2,:).*pb(2,:) + pa(3,:).*pb(3,:);
-        c0   = (f2 - p1m2);
-        c1   = (f2 - p2m2);
-        c2   = 1./(f2 + (1/f2) .* p1m2 .* p2m2 - 2 * d);
-        p    = (-2*f) * cross3(pb, pa);
-        p(1,:) = (p(1,:) + c0 .* pb(1,:) + c1 .* pa(1,:)) .* c2;
-        p(2,:) = (p(2,:) + c0 .* pb(2,:) + c1 .* pa(2,:)) .* c2;
-        p(3,:) = (p(3,:) + c0 .* pb(3,:) + c1 .* pa(3,:)) .* c2;
+        f2        = f * f;
+        p1m2      = pa(1,:).*pa(1,:) + pa(2,:).*pa(2,:) + pa(3,:).*pa(3,:);
+        p2m2      = pb(1,:).*pb(1,:) + pb(2,:).*pb(2,:) + pb(3,:).*pb(3,:);
+        d         = pa(1,:).*pb(1,:) + pa(2,:).*pb(2,:) + pa(3,:).*pb(3,:);
+        c0        = (f2 - p1m2);
+        c1        = (f2 - p2m2);
+        c2        = (f2 + (1/f2) .* p1m2 .* p2m2 - 2 * d);
+        c2(c2~=0) = 1./c2(c2~=0);
+        p         = (-2*f) * cross3(pb, pa);
+        p(1,:)    = (p(1,:) + c0 .* pb(1,:) + c1 .* pa(1,:)) .* c2;
+        p(2,:)    = (p(2,:) + c0 .* pb(2,:) + c1 .* pa(2,:)) .* c2;
+        p(3,:)    = (p(3,:) + c0 .* pb(3,:) + c1 .* pa(3,:)) .* c2;
 
     % Otherwise, make good C.
     else
@@ -65,7 +66,12 @@
             p(:,k) =   (f2 - p1m2) * pb(:,k) ...
                      + (f2 - p2m2) * pa(:,k) ...
                      - 2*f * cross3(pb(:,k), pa(:,k));
-            p(:,k) = (1/(f2 + p1m2 * p2m2 / f2 - 2 * d)) * p(:,k);
+            div = f2 + p1m2 * p2m2 / f2 - 2 * d;
+            if div ~= 0
+                p(:,k) = (1/div) * p(:,k);
+            else
+                p(:,k) = 0;
+            end
         end
 
     end

readme.md

@@ -7,11 +7,6 @@ All rotation operations correspond to frame rotations. That is, they rotate the
 
 It supports MATLAB R2011a and newer.
 
-TODO
-----
-Examples.
-Put on aul? Convert to doc_?
-
 
 Quick Example
 -------------

tests/test_vectors_and_rotations.m

@@ -1,3 +1,6 @@
+% Add the 'tests' directory to the path and then run this script from the
+% top-level directory (<here>/..).
+
 clc;
 
 rebuild = true;

version.txt

@@ -1 +1 @@
-0.1
+0.2