Priklad5.m File Reference

Functions
	while (1)%vypocet koeficientu jednotlivych kubickych polynomu---------A0
vypocet podminek	konec ((3 *Ax+2 *Bx+Cx)*(6 *Ay+2 *By)-(3 *Ay+2 *By+%Cy)*(6 *Ax+2 *Bx))/((3 *Ax+2 *Bx+Cx)^2+(3 *Ay+2 *By+Cy)^2)^(3/2)
	A0 (1)
	A0 (2)
	B0 (1)
	B0 (2)
	C0 (1)
	C0 (2)
	D0 (1)
	D0 (2)
case if	~kolize (U-1) cur1 = ((3*A1(1) + 2*B1(1) + C1(1))*(6*A1(2) + 2*B1(2)) - (3*A1(2) + 2*B1(2) + C1(2))*(6*A1(1) + 2*B1(1))) / ((3*A1(1) + 2*B1(1) + C1(1))^2 + (3*A1(2) + 2*B1(2) + C1(2))^2)^(3/2)
	A1 (1)
	A1 (2)
	B1 (1)
	B1 (2)
	C1 (1)
	C1 (2)
	D1 (1)
	D1 (2)
	A2 (1)
	A2 (2)
	B2 (1)
	B2 (2)
	C2 (1)
	C2 (2)
	D2 (1)
	D2 (2)
	A3 (1)
	A3 (2)
	B3 (1)
	B3 (2)
	C3 (1)
	C3 (2)
	D3 (1)
	D3 (2)
	x2 (1)-x1(1))/(sqrt((x2(1)-x1(1))^2+(x2(2)-x1(2))^2))
	x3 (1)-x2(1))/(sqrt((x3(1)-x2(1))^2+(x3(2)-x2(2))^2))
	x4 (1)-x3(1))/(sqrt((x4(1)-x3(1))^2+(x4(2)-x3(2))^2))]
	x2 (2)-x1(2))/(sqrt((x2(1)-x1(1))^2+(x2(2)-x1(2))^2))
	x3 (2)-x2(2))/(sqrt((x3(1)-x2(1))^2+(x3(2)-x2(2))^2))
	x4 (2)-x3(2))/(sqrt((x4(1)-x3(1))^2+(x4(2)-x3(2))^2))]
id	oboru ()
1	cosAlpha ()
uprava vzorecku pro konkretni segment	if (tM0(1) > 0 &&tM0(1)< 1) d
	devi (1, 1)
end	if (tM0(2) > 0 &&tM0(2)< 1) d
	devi (1, 2)
end	if (tM1(1) > 0 &&tM1(1)< 1) d
	devi (2, 1)
end	if (tM1(2) > 0 &&tM1(2)< 1) d
	devi (2, 2)
end	if (tM2(1) > 0 &&tM2(1)< 1) d
	devi (3, 1)
end	if (tM2(2) > 0 &&tM2(2)< 1) d
	devi (3, 2)
end	if (tM3(1) > 0 &&tM3(1)< 1) d
	devi (4, 1)
end	if (tM3(2) > 0 &&tM3(2)< 1) d
	devi (4, 2)
end devi zjisteni kolizi	kolize (1)
	kolize (2)
	kolize (3)
	kolize (4)
vykresleni jak vysel v Matlabu if	figure (1)
	plot (x0(1), x0(2),'rx')
	plot (x1(1), x1(2),'rx') plot(x2(1)
rx	plot (x3(1), x3(2),'rx') plot(x4(1)
rx rx	axis ([0 10 0 10]) xlabel('x')
	ylabel ('y')
	title ('Kubicky spline z Matlabu')
else	plot (X1(i), Y1(i))
end	if (kolize(2)) plot(X2(i)
end	Y2 (i)
else	plot (X2(i), Y2(i))
end	if (kolize(3)) plot(X3(i)
end	Y3 (i)
else	plot (X3(i), Y3(i))
end	if (kolize(4)) plot(X4(i)
end	Y4 (i)
else	plot (X4(i), Y4(i))
end end end	if (~max(kolize))%bez koizi konci cyklus disp('hotovo')
break end zkraceni tecnych vektoru na krajich koliznich useku	tecny_zkratit (1)
	tecny_zkratit (2)
	tecny_zkratit (3)
	tecny_zkratit (4)
	tecny_zkratit (5)
1	figure ()
end	if (~kolize(2)) plot(X2(i)
end	if (~kolize(3)) plot(X3(i)
end	if (~kolize(4)) plot(X4(i)
Variables
clear	all
	clc
zadani uzlovych bodu a omezeni	x0 = [1,1]'
	x1 = [8,2]'
	bnd1 = 0.3
	x2 = [3,5]'
	bnd2 = 0.3
	x3 = [7,6]'
	bnd3 = 0.3
	x4 = [9,1]'
	bnd4 = 0.3
toto zadani vede k	chybe
vypocet tecnych vektoru v krajnich uzlovych bodech x0 a x4 je potreba pocitat s pocatecnim natocenim robota a smerem prijezdu do cile	v0 = [0,3]'
	v4 = [0,3]'
vypocet tecnych vektoru ve vnitrnich uzlovych bodech x2 a x3	M = [ 4 1 0
	Q = [ [-3*x0 + 3*x2 - v0]'
	v = inv(M) * Q
	v1 = v(1,:)'
	v2 = v(2,:)'
	v3 = v(3,:)'
	kolize = [0 0 0 0]
	B0 = -3*x0 + 3*x1 - 2*v0 - v1
	C0 = v0
	D0 = x0
	A1 = 2*x1 - 2*x2 + v1 + v2
	B1 = -3*x1 + 3*x2 - 2*v1 - v2
	C1 = v1
	D1 = x1
	A2 = 2*x2 - 2*x3 + v2 + v3
	B2 = -3*x2 + 3*x3 - 2*v2 - v3
	C2 = v2
	D2 = x2
	A3 = 2*x3 - 2*x4 + v3 + v4
	B3 = -3*x3 + 3*x4 - 2*v3 - v4
	C3 = v3
	D3 = x3
NOTE zde vlozit se prepocitaji	useky
NOTE zde vlozit se prepocitaji ktere by nenavazovaly krivosti v krajnich bodech for	U
switch U case	cur1 = (2*C0(1)*B0(2) - 2*C0(2)*B0(1)) / (C0(1)^2 + C0(2)^2)^(3/2)
	cur2 = (2*C1(1)*B1(2) - 2*C1(2)*B1(1)) / (C1(1)^2 + C1(2)^2)^(3/2)
	repair = nahrada_useku(x0', x1', v0', v1', abs(cur1), abs(cur2))
end keyboard end pokus o vypocet deviace segmentu trajektorie	devi = zeros (4,2)
	cosAlpha = [ (x1(1)-x0(1)) / (sqrt((x1(1)-x0(1))^2 + (x1(2)-x0(2))^2))
	sinAlpha = [ (x1(2)-x0(2)) / (sqrt((x1(1)-x0(1))^2 + (x1(2)-x0(2))^2))
	max1 = '(-1/27*Ax*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*Bx*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cx*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*sinAlpha+(-1/27*Ay*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*By*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cy*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*cosAlpha'
	max2 = '(-1/27*Ax*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*Bx*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cx*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*sinAlpha+(-1/27*Ay*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*By*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cy*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*cosAlpha'
	tM = [ -1/3*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha)
	tM1 = [-1/3*(B1(1)*sinAlpha(2)+B1(2)*cosAlpha(2)-(B1(1)^2*sinAlpha(2)^2+2*B1(1)*sinAlpha(2)*B1(2)*cosAlpha(2)+B1(2)^2*cosAlpha(2)^2-3*A1(1)*sinAlpha(2)^2*C1(1)-3*A1(1)*sinAlpha(2)*C1(2)*cosAlpha(2)-3*A1(2)*cosAlpha(2)*C1(1)*sinAlpha(2)-3*A1(2)*cosAlpha(2)^2*C1(2))^(1/2))/(A1(1)*sinAlpha(2)+A1(2)*cosAlpha(2))
	tM2 = [-1/3*(B2(1)*sinAlpha(3)+B2(2)*cosAlpha(3)-(B2(1)^2*sinAlpha(3)^2+2*B2(1)*sinAlpha(3)*B2(2)*cosAlpha(3)+B2(2)^2*cosAlpha(3)^2-3*A2(1)*sinAlpha(3)^2*C2(1)-3*A2(1)*sinAlpha(3)*C2(2)*cosAlpha(3)-3*A2(2)*cosAlpha(3)*C2(1)*sinAlpha(3)-3*A2(2)*cosAlpha(3)^2*C2(2))^(1/2))/(A2(1)*sinAlpha(3)+A2(2)*cosAlpha(3))
	tM3 = [-1/3*(B3(1)*sinAlpha(4)+B3(2)*cosAlpha(4)-(B3(1)^2*sinAlpha(4)^2+2*B3(1)*sinAlpha(4)*B3(2)*cosAlpha(4)+B3(2)^2*cosAlpha(4)^2-3*A3(1)*sinAlpha(4)^2*C3(1)-3*A3(1)*sinAlpha(4)*C3(2)*cosAlpha(4)-3*A3(2)*cosAlpha(4)*C3(1)*sinAlpha(4)-3*A3(2)*cosAlpha(4)^2*C3(2))^(1/2))/(A3(1)*sinAlpha(4)+A3(2)*cosAlpha(4))
vykresleni	splinu
hold	on
	t = 0:0.005:1
polynom	X1 = A0(1)*t.^3 + B0(1)*t.^2 + C0(1)*t + D0(1)
	Y1 = A0(2)*t.^3 + B0(2)*t.^2 + C0(2)*t + D0(2)
polynom	X2 = A1(1)*t.^3 + B1(1)*t.^2 + C1(1)*t + D1(1)
	Y2 = A1(2)*t.^3 + B1(2)*t.^2 + C1(2)*t + D1(2)
polynom	X3 = A2(1)*t.^3 + B2(1)*t.^2 + C2(1)*t + D2(1)
	Y3 = A2(2)*t.^3 + B2(2)*t.^2 + C2(2)*t + D2(2)
polynom	X4 = A3(1)*t.^3 + B3(1)*t.^2 + C3(1)*t + D3(1)
	Y4 = A3(2)*t.^3 + B3(2)*t.^2 + C3(2)*t + D3(2)
for	i
end	c
	tecny_zkratit = tecny_zkratit + 1

---

Function Documentation

A0

(

1

)

A0

(

2

)

A1

(

1

)

A1

(

2

)

A2

(

1

)

A2

(

2

)

A3

(

1

)

A3

(

2

)

rx rx axis

(

)

B0

(

1

)

B0

(

2

)

B1

(

1

)

B1

(

2

)

B2

(

1

)

B2

(

2

)

B3

(

1

)

B3

(

2

)

C0

(

2

)

C0

(

1

)

C1

(

1

)

C1

(

2

)

C2

(

1

)

C2

(

2

)

C3

(

1

)

C3

(

2

)

1 cosAlpha

(

)

[virtual]

D0

(

1

)

D0

(

2

)

D1

(

1

)

D1

(

2

)

D2

(

1

)

D2

(

2

)

D3

(

1

)

D3

(

2

)

devi	(	1	,
		1
	)

devi	(	1	,
		2
	)

devi	(	2	,
		1
	)

devi	(	2	,
		2
	)

devi	(	3	,
		1
	)

devi	(	3	,
		2
	)

devi	(	4	,
		1
	)

devi	(	4	,
		2
	)

vykresleni jak vysel v Matlabu if figure

(

1

)

1 figure

(

)

[virtual]

end if

(

kolize(2)

)

end if

(

kolize(3)

)

end if

(

kolize(4)

)

end end end if

(

~

maxkolize

)

end if

(

~

kolize4

)

end if

(

~

kolize2

)

uprava vzorecku pro konkretni segment if

(

tM0(1)

)

end if

(

tM0(2)

)

end if

(

tM1(1)

)

end if

(

tM1(2)

)

end if

(

tM2(1)

)

end if

(

~

kolize3

)

end if

(

tM2(2)

)

end if

(

tM3(1)

)

end if

(

tM3(2)

)

kolize

(

4

)

end devi zjisteni kolizi kolize

(

1

)

kolize

(

2

)

kolize

(

3

)

vypocet podminek konec

(

(3 *Ax+2 *Bx+Cx)*(6 *Ay+2 *By)-(3 *Ay+2 *By+%Cy)*(6 *Ax+2 *Bx)

)

id oboru

(

)

[virtual]

plot	(	x0(1)	,
		x0(2)	,
		'rx'
	)

plot	(	x1(1)	,
		x1(2)	,
		'rx'
	)

else plot	(	X1(i)	,
		Y1(i)
	)

else plot	(	X3(i)	,
		Y3(i)
	)

else plot	(	X2(i)	,
		Y2(i)
	)

rx plot	(	x3(1)	,
		x3(2)	,
		'rx'
	)

else plot	(	X4(i)	,
		Y4(i)
	)

tecny_zkratit

(

2

)

tecny_zkratit

(

4

)

break end zkraceni tecnych vektoru na krajich koliznich useku tecny_zkratit

(

1

)

tecny_zkratit

(

3

)

tecny_zkratit

(

5

)

title

(

'Kubicky spline z Matlabu'

)

while

(

1

)

x2

(

2

)

x2

(

1

)

x3

(

1

)

x3

(

2

)

x4

(

1

)

x4

(

2

)

end Y2

(

i

)

end Y3

(

i

)

end Y4

(

i

)

ylabel

(

'y'

)

case if ~kolize

(

U-

1

)

= ((3*A1(1) + 2*B1(1) + C1(1))*(6*A1(2) + 2*B1(2)) - (3*A1(2) + 2*B1(2) + C1(2))*(6*A1(1) + 2*B1(1))) / ((3*A1(1) + 2*B1(1) + C1(1))^2 + (3*A1(2) + 2*B1(2) + C1(2))^2)^(3/2)

---

Variable Documentation

A1 = 2*x1 - 2*x2 + v1 + v2

A2 = 2*x2 - 2*x3 + v2 + v3

A3 = 2*x3 - 2*x4 + v3 + v4

close all

B0 = -3*x0 + 3*x1 - 2*v0 - v1

B1 = -3*x1 + 3*x2 - 2*v1 - v2

B2 = -3*x2 + 3*x3 - 2*v2 - v3

B3 = -3*x3 + 3*x4 - 2*v3 - v4

bnd1 = 0.3

bnd2 = 0.3

bnd3 = 0.3

bnd4 = 0.3

end c

C0 = v0

C1 = v1

C2 = v2

C3 = v3

toto zadani vede k chybe

clc

cosAlpha = [ (x1(1)-x0(1)) / (sqrt((x1(1)-x0(1))^2 + (x1(2)-x0(2))^2))

else cur1 = (2*C0(1)*B0(2) - 2*C0(2)*B0(1)) / (C0(1)^2 + C0(2)^2)^(3/2)

end cur2 = (2*C1(1)*B1(2) - 2*C1(2)*B1(1)) / (C1(1)^2 + C1(2)^2)^(3/2)

D0 = x0

D1 = x1

D2 = x2

D3 = x3

end keyboard end pokus o vypocet deviace segmentu trajektorie devi = zeros (4,2)

for i

Initial value:

1:size(t,2)-1
       if (kolize(1))
           plot(X1(i),Y1(i), 'c')

kolize = [0 0 0 0]

vypocet tecnych vektoru ve vnitrnich uzlovych bodech x2 a x3 M = [ 4 1 0

max1 = '(-1/27*Ax*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*Bx*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cx*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*sinAlpha+(-1/27*Ay*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*By*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cy*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*cosAlpha'

max2 = '(-1/27*Ax*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*Bx*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cx*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*sinAlpha+(-1/27*Ay*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*By*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cy*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*cosAlpha'

hold on

Q = [ [-3*x0 + 3*x2 - v0]'

repair = nahrada_useku(x0', x1', v0', v1', abs(cur1), abs(cur2))

sinAlpha = [ (x1(2)-x0(2)) / (sqrt((x1(1)-x0(1))^2 + (x1(2)-x0(2))^2))

vykresleni splinu

t = 0:0.005:1

tecny_zkratit = tecny_zkratit + 1

tM = [ -1/3*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha)

tM1 = [-1/3*(B1(1)*sinAlpha(2)+B1(2)*cosAlpha(2)-(B1(1)^2*sinAlpha(2)^2+2*B1(1)*sinAlpha(2)*B1(2)*cosAlpha(2)+B1(2)^2*cosAlpha(2)^2-3*A1(1)*sinAlpha(2)^2*C1(1)-3*A1(1)*sinAlpha(2)*C1(2)*cosAlpha(2)-3*A1(2)*cosAlpha(2)*C1(1)*sinAlpha(2)-3*A1(2)*cosAlpha(2)^2*C1(2))^(1/2))/(A1(1)*sinAlpha(2)+A1(2)*cosAlpha(2))

tM2 = [-1/3*(B2(1)*sinAlpha(3)+B2(2)*cosAlpha(3)-(B2(1)^2*sinAlpha(3)^2+2*B2(1)*sinAlpha(3)*B2(2)*cosAlpha(3)+B2(2)^2*cosAlpha(3)^2-3*A2(1)*sinAlpha(3)^2*C2(1)-3*A2(1)*sinAlpha(3)*C2(2)*cosAlpha(3)-3*A2(2)*cosAlpha(3)*C2(1)*sinAlpha(3)-3*A2(2)*cosAlpha(3)^2*C2(2))^(1/2))/(A2(1)*sinAlpha(3)+A2(2)*cosAlpha(3))

tM3 = [-1/3*(B3(1)*sinAlpha(4)+B3(2)*cosAlpha(4)-(B3(1)^2*sinAlpha(4)^2+2*B3(1)*sinAlpha(4)*B3(2)*cosAlpha(4)+B3(2)^2*cosAlpha(4)^2-3*A3(1)*sinAlpha(4)^2*C3(1)-3*A3(1)*sinAlpha(4)*C3(2)*cosAlpha(4)-3*A3(2)*cosAlpha(4)*C3(1)*sinAlpha(4)-3*A3(2)*cosAlpha(4)^2*C3(2))^(1/2))/(A3(1)*sinAlpha(4)+A3(2)*cosAlpha(4))

NOTE zde vlozit se prepocitaji ktere by nenavazovaly krivosti v krajnich bodech for U

Initial value:

 1:4
        if ~kolize(U)
            continue
        end
        disp(['nahrada ', num2str(U)])

NOTE zde vlozit se prepocitaji useky

v = inv(M) * Q

v0 = [0,3]'

v1 = v(1,:)'

v2 = v(2,:)'

v3 = v(3,:)'

v4 = [0,3]'

x0 = [1,1]'

axis ([0 10 0 10]) t = 0 polynom X1 = A0(1)*t.^3 + B0(1)*t.^2 + C0(1)*t + D0(1)

vypocet tecnych vektoru ve vnitrnich uzlovych bodech x1 = [8,2]'

polynom X2 = A1(1)*t.^3 + B1(1)*t.^2 + C1(1)*t + D1(1)

x2 = [3,5]'

polynom X3 = A2(1)*t.^3 + B2(1)*t.^2 + C2(1)*t + D2(1)

x3 = [7,6]'

x4 = [9,1]'

polynom X4 = A3(1)*t.^3 + B3(1)*t.^2 + C3(1)*t + D3(1)

Y1 = A0(2)*t.^3 + B0(2)*t.^2 + C0(2)*t + D0(2)

Y2 = A1(2)*t.^3 + B1(2)*t.^2 + C1(2)*t + D1(2)

Y3 = A2(2)*t.^3 + B2(2)*t.^2 + C2(2)*t + D2(2)

Y4 = A3(2)*t.^3 + B3(2)*t.^2 + C3(2)*t + D3(2)