src/motion/splines/plotspline.m

   1 function vdiff = plotspline(a1, a2, 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     ];
  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));
  21 yy0 = m*(p(2,2) - p(1,2));
  22 xx1 = m*(p(3,1) - p(2,1));
  23 yy1 = m*(p(3,2) - p(2,2));
  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(3,1,1);
  45 cla
  46 axis equal
  47 hold on
  48 plot(p(:,1), p(:,2));
  49 title(sprintf('Angle = %d^o', abs(a1-a2)));
  50 for t=0:0.01:1,
  51     plot(polyval(px,t), polyval(py,t), '.');
  52 end
  53
  54 subplot(3,1,2);
  55 cla
  56 title('Curvature');
  57 xlabel('t');
  58 grid on
  59 hold on
  60 for t=0:0.01:1,
  61     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);
  62     plot(t, kappa, '+');
  63 end
  64
  65 subplot(3,1,3);
  66 cla
  67 title('Speed');
  68 xlabel('t');
  69 grid on
  70 hold on
  71 vmin=inf;
  72 vmax=-inf;
  73 for t=0:0.01:1,
  74     v=sqrt(polyval(polyder(px),t)^2+...
  75            polyval(polyder(py),t)^2);
  76     vmin=min(v,vmin);
  77     vmax=max(v,vmax);
  78     plot(t, v, '+');
  79 end
  80 hold off
  81
  82 vdiff = vmax-vmin;