% kinodynamic_rrtstar_for_guidance_3d.m文献Heuristic Guidance Techniques
% for the Exploration of Small Celestial Bodies小天体着陆部分的仿真主程序。
% 使用外部函数：ball_is_state_free.m, is_input_free.m, 
% ball_sample_free_states.m, ball_plot_field.m
% 作    者：Dustin J. Webb and Jur van den Berg（参考文献的作者）
% 创建日期：未 知
% 修 改 人：鲁 鹏
% 修改日期：2018年6月3日

% 参考文献：Kinodynamic RRT*: Optimal Motion Planning for Systems with
% Linear Differential Constraints

start = [-20000, 0, 10000, 0, 0, 0];
goal  = [-7014, 0, 5189, 0, 0, 0];

radius = 100; % 机器人和障碍物最近距离小于radius时，视为发生碰撞

state_limits = ...
    [-21000,-7014;	% x limit
      -1000, 1000;  % y limit
       5189,11000;	% z limit
       -100,  100;	% vx limit
       -100,  100;  % vy limit
       -100,  100];	% vz limit

input_limits = ...
    [-500, 500;
     -500, 500;
     -500, 500];

state_dims = size(start,2); % 系统的维数
input_dims = 3;             % 控制量的维数
omega = 3.31182e-4; % 433Eros绕z轴的旋转角速度

A=zeros(state_dims);
A(1,4) = 1; A(2,5) = 1; A(3,6) = 1;
A(4,1) = omega^2; A(4,5) = 2*omega; A(5,2) = omega^2; A(5,4) = -2*omega;

B=zeros(state_dims, input_dims);
B(4,1) = 1; B(5,2) = 1; B(6,3) = 1;

c = zeros(state_dims,1);
% c(4,1) = -0.0020;
% c(5,1) = -0.0020;
% c(6,1) = -0.0020; %433Eros表面引力在10^(-3)这个量级

R = diag([1/2,1/2,1/2]);  % 
h = 0.1; % 积分时间步长

obstacles = [-21000,-1000,11000;    %Free Space的边界点坐标（每行为一个点的坐标）
             -21000,-1000, 5189;
             -7014, -1000, 5189;
             -7014, -1000,11000;
             -21000, 1000,11000;
             -21000, 1000, 5189;
             -7014,  1000, 5189;
             -7014,  1000,11000];

% disp('calculating closed form solution.');
rrt = rrtstar(A,B,c,R,h);
% disp('starting algorithm.');

% @用于创建匿名函数的函数句柄 fhandle = @(x,y) x.^2 + y.^2;
state_free = @(state, time_range)(is_state_free(state,state_limits,...
    obstacles, radius, time_range));
input_free = @(input, time_range)(is_input_free(input, input_limits,...
    time_range));
sample_state = @()(sample_free_states(state_limits, obstacles,...
    radius));
display = @(scratch, obj, tree, parents, goal, goal_cost, goal_parent)...
    (plot_field(scratch, obj, tree, parents, goal, goal_cost,...
    goal_parent));

[T, parents] = rrt.run(sample_state, state_free, input_free, start',...
    goal', display);