-
Notifications
You must be signed in to change notification settings - Fork 0
/
catmull_twoForLoops.m
34 lines (26 loc) · 1.39 KB
/
catmull_twoForLoops.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
function [pointsCatmull,fakePoints] = catmull_twoForLoops(path,pointsPerSection)
%CARMULL_TWOFORLOOPS By using the Catmull-rom spline, compute the curve
% Detailed explanation goes here
Catmull = @(t, P_0, P_1, P_2, P_3) [1 t t^2 t^3] * (1/2.*[0 2 0 0; -1 0 1 0; 2 -5 4 -1; -1 3 -3 1]) * [P_0; P_1; P_2; P_3];
firstFakePoint = -(path(:,2) - path(:,1)) + path(:,1);
lastFakePoint = -(path(:,end-1) - path(:,end)) + path(:,end);
fakePoints = [firstFakePoint lastFakePoint];
t = linspace(0,1,pointsPerSection);
pathAndFake = [firstFakePoint path lastFakePoint];
pointsCatmull = zeros(2, (size(path,2)-1)*size(t,2));
sectionSize = size(t,2);
%One problem right now is that the last point of a section is also the
%first point in the next section.
%This could effect the car in bad ways.
%One fix would be to skip on (j==1 || i>1), mostly temporary
%A better fix would be to do a linspace from 0, to #sections, this will
%ensure that all the points are diffrent.
%However, this method will not guarantee that the curve has a point exactly
%at a path-point.
for i = 1:size(pathAndFake,2)-3
for j = 1:sectionSize
pointsCatmull(1,j+(sectionSize)*(i-1)) = Catmull(t(j), pathAndFake(1,i), pathAndFake(1,i+1), pathAndFake(1,i+2), pathAndFake(1,i+3));
pointsCatmull(2,j+(sectionSize)*(i-1)) = Catmull(t(j), pathAndFake(2,i), pathAndFake(2,i+1), pathAndFake(2,i+2), pathAndFake(2,i+3));
end
end
end