src/motion/splines/plotspline.m

   1 function vdiff = plotspline(a1, a2, distance, m)
   2 if (m<0),
   3     vdiff = inf;
   4     return
   5 end
   6 % a1=60;
   7 % a2=0;
   8 % m=1.30;
   9 p = [
  10     cos(a1/180.*pi) sin(a1/180.*pi);
  11     0 0;
  12     cos(a2/180.*pi) sin(a2/180.*pi);
  13     ]*distance;
  14
  15
  16 x0 = p(1,1);
  17 y0 = p(1,2);
  18 x1 = p(3,1);
  19 y1 = p(3,2);
  20 xx0 = m.*(p(2,1) - p(1,1)) / distance;
  21 yy0 = m.*(p(2,2) - p(1,2)) / distance;
  22 xx1 = m.*(p(3,1) - p(2,1)) / distance;
  23 yy1 = m.*(p(3,2) - p(2,2)) / distance;
  24
  25 px = [
  26    -3.*xx1-3.*xx0-6.*x0+6.*x1
  27   7.*xx1+8.*xx0+15.*x0-15.*x1
  28  -4.*xx1-6.*xx0-10.*x0+10.*x1
  29                         0
  30                       xx0
  31                        x0];
  32
  33
  34
  35 py = [
  36    -3.*yy1-3.*yy0-6.*y0+6.*y1
  37   7.*yy1+8.*yy0+15.*y0-15.*y1
  38  -4.*yy1-6.*yy0-10.*y0+10.*y1
  39                         0
  40                       yy0
  41                        y0];
  42
  43 figure(1);
  44 subplot(2,2,1);
  45 plot(p(:,1), p(:,2), 'r');
  46 hold on
  47 title(sprintf('Angle = %d^o', abs(a1-a2)));
  48 t=0:0.01:1;
  49 plot(polyval(px,t), polyval(py,t), 'b');
  50 axis equal
  51 hold off
  52 grid on
  53
  54 subplot(2,2,2);
  55 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);
  56 plot(t, kappa);
  57 title('Curvature');
  58 xlabel('t');
  59 grid on
  60
  61 subplot(2,2,4);
  62 cla
  63 vmin=inf;
  64 vmax=-inf;
  65 v=sqrt(polyval(polyder(px),t).^2+...
  66            polyval(polyder(py),t).^2);
  67 vmin=min(v);
  68 vmax=max(v);
  69 plot(t, v);
  70 title('Speed');
  71 xlabel('t');
  72 grid on
  73
  74 subplot(2,2,3);
  75 plot(v,kappa, '+');
  76 xlabel('Speed');
  77 ylabel('Curvature');
  78 grid on
  79
  80 vdiff = vmax-vmin;