## PengLu / 使用kinodynamic RRTStar算法规划轨迹 .gitee-modal { width: 500px !important; }

Explore and code with more than 6 million developers，Free private repositories ！：）
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Poly_gravity.m 5.57 KB
% X = [160000,0,0]'
% 参考文献见EXTERIOR GRAVITATION OF A POLYHEDRON DERIVED AND COMPARED WITH
% HARMONIC AND MASCON GRAVITATION REPRESENTATIONS OF ASTEROID 4769 CASTALIA
function dU= Poly_gravity(X,vertex,facet,index)

%  vertex=vertex_216;
%   facet=facet_216;
% for j = 1:3
%     for i = 1:size(vertex(:,1))
%     vertex(i,j)= vertex(i,j)*1000;  %vertex中的数据为千米 换算成米
%     end
% end
% for j = 1:3
%     for i = 1:size(facet(:,1))
%     facet(i,j)= facet(i,j)+1;  %facet中数据是距离第一个点的偏差 全部加1  防止出现0点
%     end
% end
% dU = [0 0 0]';
dU_face = [0 0 0]';
dU_edge = [0 0 0]';
% ddU = zeros(3,3);
ddU_face = zeros(3,3);
ddU_edge = zeros(3,3);
delta_U = 0;
nv = length(vertex(:,1));      %顶点的数量
n_facet = length(facet(:,1));     %面的数量
ne = n_facet + nv - 2;            %多面体边的数量
% r1 = zeros(nf,3);
% r2 = zeros(nf,3);
% r3 = zeros(nf,3);
% rou1 = zeros(nf,3);
% rou2 = zeros(nf,3);
% rou3 = zeros(nf,3);
% omiga = zeros(nf,1);
face_part = 0;

% for i = 1:nf
%     offset = facet(i,:);
%     r1(i,:) = vertex(offset(1),:);   %面上的三个顶点
%     r2(i,:) = vertex(offset(2),:);
%     r3(i,:) = vertex(offset(3),:);
%
%     n(i,:) = cross(r2(i,:)- r1(i,:),r3(i,:) - r2(i,:))/norm(cross(r2(i,:) - r1(i,:),r3(i,:) - r2(i,:)));  %计算平面的法向向量 指向外
%     rou1(i,:) = r1(i,:)' -X             %用于计算omiga
%     rou2(i,:) = r2(i,:)' -X;
%     rou3(i,:) = r3(i,:)' -X;
%     omiga(i) = 2*atan(dot(rou1(i,:) ,cross(rou2(i,:),rou3(i,:)))/...
%     (norm(rou1(i,:))*norm(rou2(i,:))*norm(rou3(i,:)) + norm(rou1(i,:))* dot(rou2(i,:),rou3(i,:))...
%     + norm(rou2(i,:))* dot(rou3(i,:),rou1(i,:)) +norm(rou3(i,:))* dot(rou1(i,:),rou2(i,:))  ) );
%
%     %计算omiga
%     face_part = face_part + rou1(i,:)*(n(i,:)'*n(i,:))*rou1(i,:)'*omiga(i,:);
%     dU_face = dU_face + n(i,:)'*n(i,:)*rou1(i,:)'*omiga(i);
%     ddU_face = ddU_face + n(i,:)'*n(i,:)*omiga(i);
%     delta_U = delta_U + omiga(i);
% end
for i = 1:n_facet
offset = facet(i,:);
r1 = vertex(offset(1),:)  ; %面上的三个顶点
r2 = vertex(offset(2),:);
r3 = vertex(offset(3),:);

%     if (dot(nf,r1) < 0)
%         nf=-nf
%     end
rou1 = r1' - X ;         %用于计算omiga，面的顶点位置 - 当前位置
rou2 = r2' - X;
rou3 = r3' - X;
nf = cross((r2-r1),(r3 - r2))/norm(cross((r2 - r1),(r3 - r2)))  ;%计算平面的法向向量 指向外
%     nf2 = cross((rou2-rou1),(rou3 - rou2))/norm(cross((rou2 - rou1),(rou3 - rou2))) %计算平面的法向向量 指向外
omiga = 2*atan(dot(rou1 ,cross(rou2,rou3))/...
(norm(rou1)*norm(rou2)*norm(rou3) + norm(rou1)* dot(rou2,rou3)...
+ norm(rou2)* dot(rou3,rou1) +norm(rou3)* dot(rou1,rou2)  ) );

%计算omiga
face_part = face_part + rou1'*(nf'*nf)*rou1*omiga;
dU_face = dU_face + nf'*nf*rou1*omiga;
ddU_face = ddU_face + nf'*nf*omiga;
delta_U = delta_U + omiga;
end
%%%%%求出与一个面相互共边的索引 索引的是facet中的列
% h = 1;
% for i = 1:nf
%     for j = i:nf
%         a = facet(i,:);
%         [c, ia, ib] = intersect(a ,facet(j,:));
%         if length(c)== 2
%             index(h,:) = [i,j,c];
%             h = h+1;
%         end
%
%     end
% end
%   计算好之后 供给仿真的时候调用生成的index
tic

for i =1:ne
face_A = facet(index(i,1),:)    ;     %共边的两个面 其一
face_B = facet(index(i,2),:)    ;    %共边的两个面 其二
edge_vertex1 = vertex(index(i,3),:);  %共边的两个顶点
edge_vertex2 = vertex(index(i,4),:);
re1 = norm(edge_vertex1'-X)  ; %见199页 B.25-27定义
re2 = norm(edge_vertex2'-X);
e12 = norm(edge_vertex1' - edge_vertex2' );
r_A1 = vertex(face_A(1),:)  ;   %A B  两面的顶点
r_A2 = vertex(face_A(2),:);
r_A3 = vertex(face_A(3),:);
r_B1 = vertex(face_B(1),:);
r_B2 = vertex(face_B(2),:);
r_B3 = vertex(face_B(3),:)   ;
nA = cross((r_A2 - r_A1),(r_A3 - r_A2))/norm(cross((r_A2 - r_A1),(r_A3 - r_A2))) ; %计算A/B平面的法向向量  指向外侧
nB = cross((r_B2 - r_B1),(r_B3 - r_B2))/norm(cross((r_B2 - r_B1),(r_B3 - r_B2)));
if (dot(r_A1,nA) < 0)
nA=-nA;
end
if (dot(r_B1,nB) < 0)
nB=-nB;
end
P1P2 = edge_vertex1 - edge_vertex2;
nA12 = cross(nA,P1P2)/norm(cross(nA,P1P2));   %公共边的位于平面内的法向 指向外侧
nB21 = cross(nB,-P1P2)/norm( cross(nB,-P1P2));
%判断nA12是否是指向外侧
%     m1 = dot(edge_vertex2-r_A1,nA12)
%     m2= dot(edge_vertex2-r_A2,nA12)
%     m3= dot(edge_vertex2-r_A3,nA12)
if (dot(edge_vertex2-r_A1,nA12)>=-0.1&&dot(edge_vertex2-r_A2,nA12)>=-0.1&&dot(edge_vertex2-r_A3,nA12)>=-0.1)
nA12 = nA12;
else
nA12 = -nA12;
end
if (dot(edge_vertex1-r_B1,nB21)>=-0.1&&dot(edge_vertex1-r_B2,nB21)>=-0.1&&dot(edge_vertex1-r_B3,nB21)>=-0.1)
nB21 = nB21;
else
nB21 = -nB21;
end
%     m4 = dot(edge_vertex1-r_B1,nB21)
%     m5 = dot(edge_vertex1-r_B2,nB21)
%     m6 = dot(edge_vertex1-r_B3,nB21)
%  dot(nA,nA12)
%  nA/norm(nA)
%  nA12/norm(nA12)
%  dot(nB,nB21)
Eab = nA'*nA12 + nB'*nB21;
Le = log((re1 + re2 + e12)/(re1 + re2 - e12));
dU_edge = dU_edge + Eab*(edge_vertex1'-X)*Le;
ddU_edge = ddU_edge + Eab*Le;
end
%  toc     %%记录时间
G=6.67e-11;  % 引力常数
rou = 2670;   % 433 eros密度 2670 kg/m^3
dU= -G*rou*dU_edge + G*rou*dU_face;
% ddU = G*rou*ddU_edge - G*rou*ddU_face;