Spline: plotspline is now much faster

diff --git a/src/motion/splines/plotspline.m b/src/motion/splines/plotspline.m
index a8f31247c1d55b2bc50a4843682434273662f878..3e5f699c91b6999a53765f9099983d05f0ea8a7b 100644 (file)
--- a/src/motion/splines/plotspline.m
+++ b/src/motion/splines/plotspline.m
@@ -10,17 +10,17 @@ p = [
     cos(a1/180.*pi) sin(a1/180.*pi);
     0 0;
     cos(a2/180.*pi) sin(a2/180.*pi);
-    ];
+    ]*distance;
 
 
-x0 = p(1,1) .* distance;
-y0 = p(1,2) .* distance;
-x1 = p(3,1) .* distance;
-y1 = p(3,2) .* distance;
-xx0 = m.*(p(2,1) - p(1,1));
-yy0 = m.*(p(2,2) - p(1,2));
-xx1 = m.*(p(3,1) - p(2,1));
-yy1 = m.*(p(3,2) - p(2,2));
+x0 = p(1,1);
+y0 = p(1,2);
+x1 = p(3,1);
+y1 = p(3,2);
+xx0 = m.*(p(2,1) - p(1,1)) / distance;
+yy0 = m.*(p(2,2) - p(1,2)) / distance;
+xx1 = m.*(p(3,1) - p(2,1)) / distance;
+yy1 = m.*(p(3,2) - p(2,2)) / distance;
 
 px = [ 
    -3.*xx1-3.*xx0-6.*x0+6.*x1
@@ -41,32 +41,25 @@ py = [
                        y0];
                        
 figure(1);
-subplot(3,1,1);
-cla
-axis equal
+subplot(2,2,1);
+plot(p(:,1), p(:,2), 'r');
 hold on
-plot(p(:,1), p(:,2));
 title(sprintf('Angle = %d^o', abs(a1-a2)));
-for t=0:0.01:1,
-    plot(polyval(px,t), polyval(py,t), '.');
-end
+t=0:0.01:1;
+plot(polyval(px,t), polyval(py,t), 'b');
+axis equal
+hold off
+grid on
 
-subplot(3,1,2);
-cla
+subplot(2,2,2);
+kappa = ((5.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^4+4.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^3+3.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t.^2+xx0).*(20.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^3+12.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^2+6.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t)-(5.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^4+4.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^3+3.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t.^2+yy0).*(20.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^3+12.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^2+6.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t))./((5.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^4+4.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^3+3.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t.^2+xx0).^2+(5.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^4+4.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^3+3.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t.^2+yy0).^2).^(3/2);
+plot(t, kappa);
 title('Curvature');
 xlabel('t');
 grid on
-hold on
-t=0:0.01:1;
-kappa = ((5.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^4+4.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^3+3.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t.^2+xx0).*(20.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^3+12.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^2+6.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t)-(5.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^4+4.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^3+3.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t.^2+yy0).*(20.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^3+12.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^2+6.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t))./((5.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^4+4.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^3+3.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t.^2+xx0).^2+(5.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^4+4.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^3+3.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t.^2+yy0).^2).^(3/2);
-plot(t, kappa);
 
-subplot(3,1,3);
+subplot(2,2,4);
 cla
-title('Speed');
-xlabel('t');
-grid on
-hold on
 vmin=inf;
 vmax=-inf;
 v=sqrt(polyval(polyder(px),t).^2+...
@@ -74,10 +67,14 @@ v=sqrt(polyval(polyder(px),t).^2+...
 vmin=min(v);
 vmax=max(v);
 plot(t, v);
-hold off
+title('Speed');
+xlabel('t');
+grid on
 
-figure(3);
+subplot(2,2,3);
 plot(v,kappa, '+');
+xlabel('Speed');
+ylabel('Curvature');
 grid on
 
 vdiff = vmax-vmin;
\ No newline at end of file