waypoints.m

function C = waypoints(boundary, ps, ts)
    syms t
    degree = 8;
    
    % get the time by assuming t=0 for each trajectory
    ts_ = [0 ts'];
    T = zeros(1, length(ts_));
    for i=2:length(T)
        T(i) = ts_(i) - ts_(i-1);
    end
    
    % get points for the given times
    pts = zeros(1, length(T));
    pts(1) = boundary(1);
    pts(end) = boundary(5);
    for i=2:(length(pts)-1)
        pts(i) = ps(i-1);
    end
    
    % get x for each trajectory segment
    x = sym( zeros(length(ts), degree*length(ts)) );
    vars = [1 t t^2 t^3 t^4 t^5 t^6 t^7];
    for i=1:length(ts)
        x(i, :) = [ zeros(1, degree*(i-1)) vars zeros(1, degree*(length(ts)-i)) ];
    end
    
    % get the 1, 2, 3, 4, 5, 6 derivatives
    D = sym( zeros(length(ts), degree*length(ts)) );
    for i=1:6
        D(:, :, i) = diff(x, t, i);
    end
    
    % write equations
    D_1 = sym( zeros(length(ps)*2 + 2, degree*length(ts)) );
    index = 1;
    for i=1:length(ts)
        D_1(index, :)   = subs(x(i, :), t, T(1));
        D_1(index+1, :) = subs(x(i, :), t, T(i+1));
        index = index+2;
    end
    
    D_2 = [subs(D(1, :, 1), t, T(1));
           subs(D(1, :, 2), t, T(1));
           subs(D(1, :, 3), t, T(1));
           subs(D(end, :, 1), t, T(end));
           subs(D(end, :, 2), t, T(end));
           subs(D(end, :, 3), t, T(end))];
       
    D_3 = sym( zeros(6*length(ps), degree*length(ts)) );
    index = 1;
    for i=1:length(ps)
        D_3(index, :) = subs(D(i, :, 1), t, T(i+1)) - subs(D(i+1, :, 1), t, T(1));
        index = index + 1;
        D_3(index, :) = subs(D(i, :, 2), t, T(i+1)) - subs(D(i+1, :, 2), t, T(1));
        index = index + 1;
        D_3(index, :) = subs(D(i, :, 3), t, T(i+1)) - subs(D(i+1, :, 3), t, T(1));
        index = index + 1;
        D_3(index, :) = subs(D(i, :, 4), t, T(i+1)) - subs(D(i+1, :, 4), t, T(1));
        index = index + 1;
        D_3(index, :) = subs(D(i, :, 5), t, T(i+1)) - subs(D(i+1, :, 5), t, T(1));
        index = index + 1;
        D_3(index, :) = subs(D(i, :, 6), t, T(i+1)) - subs(D(i+1, :, 6), t, T(1));
        index = index + 1;
    end
    
    D_final = [D_1; D_2; D_3];
    
    q1 = zeros(length(ps)*2, 1);
    index = 1;
    for i=1:length(ps)
        q1(index, 1) = ps(i);
        q1(index+1, 1) = ps(i);
        index = index + 2;
    end
    
    q = [pts(1); q1; pts(end);
        boundary(2); boundary(3); boundary(4);
        boundary(6); boundary(7); boundary(8);
        zeros(degree*length(ts)-(length(q1)+8), 1)];
    
    % get the coefficients
    C_ = inv(D_final) * q;
    C_ = double(C_);
    C = zeros(8, length(ts));
    index = 1;
    for i=1:8:(degree*length(ts))
        C(1:8, index) = C_(i:i+7, 1);
        index = index + 1;
    end
end