matlab几何计算程序集

一、求矢量长度（模）

 

function x=leng(a)
x=sqrt(sum(a.^2));

 

二、求矢量的夹角

 

function ang=ang(a,b)
ang=acos((a'*b)/leng(a)*leng(b));

 

三、求点到线上的垂足

 

%求点到线上的垂足；P0线外点，P1线上点，n线的方向；
function P=point_po2l(P0,P1,n)
D=n(1)*P0(1)+n(2)*P0(2)+n(3)*P0(3);
if(n(1)~=0&&n(2)~=0&&n(3)~=0)
    D1=-n(2)*P1(1)+n(1)*P1(2);
    D2=-n(3)*P1(1)+n(1)*P1(3);
    x=(n(1)*D-n(2)*D1-n(3)*D2)/(n(1)^2+n(2)^2+n(3)^2);
    y=(n(2)*x+D1)/n(1);
    z=(n(3)*x+D2)/n(1);
end
if(n(1)==0&&n(2)==0&&n(3)~=0)
    x=P1(1);
    y=P1(2);
    z=D/n(3);
end
if(n(1)==0&&n(2)~=0&&n(3)==0)
    x=P1(1);
    y=D/n(2);
    z=P1(3);
end
if(n(1)~=0&&n(2)==0&&n(3)==0)
    x=D/n1(1);
    y=P1(2);
    z=P1(3);
end
if(n(1)==0&&n(2)~=0&&n(3)~=0)
    x=P1(1);
    y=(n(2)*D+n(3)*(n(3)*P1(2)-n(2)*P1(3)))/(n(2)^2+n(3)^2);
    z=(D-n(2)*y)/n(3);
end
if(n(1)~=0&&n(2)==0&&n(3)~=0)
    y=P1(2);
    x=(n(1)*D+n(3)*(n(3)*P1(1)-n(1)*P1(3)))/(n(1)^2+n(3)^2);
    z=(D-n(1)*y)/n(3);
end
if(n(1)~=0&&n(2)~=0&&n(3)==0)
    z=P1(3);
    y=(n(2)*D+n(1)*(n(1)*P1(2)-n(2)*P1(1)))/(n(2)^2+n(1)^2);
    z=(D-n(2)*y)/n(1);
end
P=[x;y;z];

 

四、求矢量旋转矩阵

 

function Ralfa_ua=Ralfa_ua(ua,alfa)
%u_x=0 u_y=1 u_z=0 //a0a1转向单位矢量
if(leng(ua)~=1)
    ua=ua/leng(ua)
end
Pua=[0,-ua(3),ua(2);ua(3),0,-ua(1);-ua(2),ua(1),0];
Qua=[ua(1)^2,ua(1)*ua(2),ua(1)*ua(3);ua(1)*ua(2),ua(2)^2,ua(2)*ua(3);ua(1)*ua(3),ua(2)*ua(3),ua(3)^2];
%u_x=-cos(7*pai)*sin(1.5*pai) u_y=-sin(7*pai) u_z=cos(7*pai)*cos(1.5*pai) //b0b1转向单位矢
 Ralfa_ua=-Pua*Pua.*cos(alfa)+Pua.*sin(alfa)+Qua;

 

五、求点绕线轴转 

function [a1]=Rot_p(a0,P0,UP0,alfa)  
a1=Ralfa_ua(UP0,alfa)*(a0-P0)+P0;

 

六、求线绕线轴转
 

function [a1,ua1]=Rot_l(a0,ua0,P0,UP0,alfa)  
a1=Ralfa_ua(UP0,alfa)*(a0-P0)+P0;
ua1=Ralfa_ua(UP0,alfa)*ua0;

 

七、坐标系的欧拉角变坐标变换阵（方向余弦或者姿态）

 

%坐标变换矩阵，欧拉角到余弦阵由Er到Eb  Arb 按313变换
%Arb的每列为Eb的基在Er下的投影（方向余弦）
%([0,0,0]',[0,0,]'-->[0,0,0]',[alfa,beta,gama]'
function [Arb]=Arb(OULb)  
alfa=OULb(1);
beta=OULb(2);
gama=OULb(3);
Arbz1=[cos(alfa),-sin(alfa),0;
    sin(alfa),cos(alfa),0;
    0,0,1];
Arbx2=[1,0,0;
    0,cos(beta),-sin(beta);
    0,sin(beta),cos(beta)];
Arbz3=[cos(gama),-sin(gama),0;
    sin(gama),cos(gama),0;
    0,0,1];
Arb=Arbz1*Arbx2*Arbz3;

 

八、坐标系的坐标变换阵（方向余弦或者姿态）变 欧拉角

%坐标变换矩阵，余弦阵到欧拉角:由Er到Eb  Arb 按313变换
%Arb的每列为Eb的基在Er下的投影（方向余弦）
%([0,0,0]',[0,0,0]'-->[0,0,0]',[alfa,beta,gama]'
function [OULb]=OULb(Arb)  
%Arb=[cos(alfa)cos(gama)-sin(alfa)cos(beta)sin(gama),-cos(alfa)sin(gama)-sin(alfa)cos(beta)cos(gama),sin(alfa)sin(beta)
%sin(alfa)cos(gama)+cos(alfa)cos(beta)sin(gama),-sin(alfa)sin(gama)+cos(alfa)cos(beta)cos(gama),-cos(alfa)sin(beta)
%sin(beta)sin(gama),sin(beta)cos(gama),cos(beta)];
%设0<beta<pi，beta不能为0否则alfa和gama无法区分，beta为0出现奇点！！！
if Arb(3,3)==1  
    beta=0;    
    gama=0;
    Calfa=Arb(1,1);
    Salfa=Arb(2,1);
    if(Salfa>0)
        alfa=acos(Calfa);
    else
        alfa=-acos(Calfa);
    end
    OULb=[alfa,beta,gama]';
    return
end  %%%%实际上坐标系只绕z轴转动了alfa！！！或者说alfa+gama，只是两者无法区分！！
if Arb(3,3)==-1
    beta=pi;
    gama=0;
    Calfa=Arb(1,1);
    Salfa=Arb(2,1);
    if(Salfa>0)
        alfa=acos(Calfa);
    else
        alfa=-acos(Calfa);
    end
    OULb=[alfa,beta,gama]';
    return
end  %%%%实际上坐标系绕x轴转动了pi！！！alfa-gama为定值！！
beta=acos(Arb(3,3));
Sbeta=sqrt(1-Arb(3,3)^2);
Salfa=Arb(1,3)/Sbeta;
Calfa=-Arb(2,3)/Sbeta;
if(Salfa>0)
    alfa=acos(Calfa);
else
    alfa=-acos(Calfa);
end
Sgama=Arb(3,1)/Sbeta;
Cgama=Arb(3,2)/Sbeta;
if(Sgama>0)
    gama=acos(Cgama);
else
    gama=-acos(Cgama);
end
OULb=[alfa,beta,gama]';

 

九、求点的坐标变换

 

%求点的坐标变换，欧拉角到余弦阵由Er到Eb  Arb 按313变换
%Pb为Eb下的点坐标，Eb相对Er的坐标变换阵为Arb;或者欧拉角为OULb.Eb原点在Er下的坐标为Ob_r
%求P点在Er下的坐标Pr
%Arb的每列为Eb的基在Er下的投影（方向余弦）
%([0,0,0]',[0,0,]'-->[0,0,0]',[alfa,beta,gama]'
function [Pr]=Pointb2r(Pb,Ob_r,A_rb)  
if(length(A_rb)==3)
    Arb=Arb(A_rb);
else
    Arb=A_rb;
end
Pr=Arb*Pb+Ob_r;

 

十、求线的坐标变换

 

%求线的坐标变换，欧拉角到余弦阵由Er到Eb  Arb 按313变换
%(Pb,UPb)为Eb下的线坐标，Eb相对Er的坐标变换阵为Arb;或者欧拉角为OULb.Eb原点在Er下的坐标为Ob_r
%求线L在Er下的坐标（Pr，UPr)
%Arb的每列为Eb的基在Er下的投影（方向余弦）
%([0,0,0]',[0,0,]'-->[0,0,0]',[alfa,beta,gama]'
function [Pr,UPr]=Lineb2r(Pb,UPb,Ob_r,A_rb)  
if(length(A_rb)==3)
    Arb=Arb(A_rb);
else
    Arb=A_rb;
end
Pr=Arb*Pb+Ob_r;
UPr=Arb*UPb;

 

十、坐标系绕定轴旋转

 

%求坐标系绕定轴旋转，欧拉角到余弦阵由Er到Eb  Arb 按313变换
%(Po,UPo)为基础坐标系Eo下的旋转轴坐标，Er相对Eo的坐标变换阵为Aor;或者欧拉角为OULor.Er原点在Eo下的坐标为Or_o
%求Er绕轴(Po,UPo)旋转alfa角后到达的新的坐标系Eb,其原点在基础坐标系Eo下为Ob_o,
%Eo到Eb的坐标阵为Aob,欧拉角为OULob
%Arb的每列为Eb的基在Er下的投影（方向余弦）
%([0,0,0]',[0,0,]'-->[0,0,0]',[alfa,beta,gama]'
function [Ob_o,OULob,Aob]=Rot_E(Or_o,A_or,Po,UPo,alfa)  
if(length(A_or)==3)
    Aor=Arb(A_or);
else
    Aor=A_or;
end
Ux_ro=Aor(:,1); %Er的x轴单位矢量（方向余弦）
Uy_ro=Aor(:,2); %Er的y轴单位矢量（方向余弦）
Uz_ro=Aor(:,3); %Er的z轴单位矢量（方向余弦）
[Ob_o,Ux_bo]=Rot_l(Or_o,Ux_ro,Po,UPo,alfa);
[Ob_o,Uy_bo]=Rot_l(Or_o,Uy_ro,Po,UPo,alfa);
[Ob_o,Uz_bo]=Rot_l(Or_o,Uz_ro,Po,UPo,alfa);
Aob=[Ux_bo,Uy_bo,Uz_bo];
OULob=OULb(Aob);