axerolingl.pas

求解三维轴及坐标旋转

unit Unit21;

interface

type

 T3DVector=array[1..3]of Double;

 BaseMeshError=class(Exception);

 function MatrixMultiply(Matrix1,Matrix2:TFloatArray;M1Row,M1Col,M2Row,M2Col:integer):TFloatArray;
 //矩阵Matrix1与Matrix2相乘,M1Row:Matrix1的行数，M1Col:Matrix1的列数，M2Row:Matrix2的行数，M2Col:Matrix2的列数
 procedure AxesRoll(AxesVec:T3DVector;var APoint:T3DPoint;Mode:byte);
 //轴旋转；将AxesVec轴旋转至Mode所要求的方向，同时APoint也随之转至新位置。Mode=1:转至X轴正向处，Mode=2:至Y轴，Mode=3:至Z轴
 procedure AxesRolling(AxesVec:T3DVector;var APoint:T3DPoint;Mode:byte);
 //轴旋转；以Z轴为轴，将AxesVec轴旋转至Mode所要求的方向，同时APoint也随之转至新位置。Mode=1:转至X轴正向处，Mode=2:至Y轴

implementation

function  BasicVector(SVector:T3DVector):T3DVector;
var
  AModel:Extended;
begin
 AModel:=Model(SVector);
 if abs(AModel)>=Ep then
    begin
     Result[1]:=SVector[1]/AModel;
     Result[2]:=SVector[2]/AModel;
     Result[3]:=SVector[3]/AModel;
    end;
end;

function MatrixMultiply(Matrix1,Matrix2:TFloatArray;M1Row,M1Col,M2Row,M2Col:integer):TFloatArray;
var
  I,J,K:integer;
begin
 if M1Col<>M2Row then
   raise BaseMeshError.Create('矩阵维数对应错误！');
 if High(Matrix1)<>(M1Row*M1Col-1) then
   raise BaseMeshError.Create('第一矩阵个数错误！');
 if High(Matrix2)<>(M2Row*M2Col-1) then
   raise BaseMeshError.Create('第二矩阵个数错误！');
 SetLength(Result,M1Row*M2Col);
 for I:=1 to M1Row do
   for J:=1 to M2Col do
    begin
     Result[(I-1)*M2Col+J-1]:=0.0;
     for K:=1 to M1Col do
       Result[(I-1)*M2Col+J-1]:=Result[(I-1)*M2Col+J-1]+Matrix1[(I-1)*M1Col+K-1]*Matrix2[(K-1)*M2Col+J-1];
    end;
end;

procedure AxesRoll(AxesVec:T3DVector;var APoint:T3DPoint;Mode:byte);
var
  I:integer;
  CosAlpha,SinAlpha,CosBeita,SinBeita:extended;//弧度单位
  R,Ra,Rb:TFloatArray;
begin
 AxesVec:=BasicVector(AxesVec);
 SetLength(R,3);
 SetLength(Ra,9);
 SetLength(Rb,9);
 for I:=0 to 2 do
   R[I]:=APoint[I+1];
 for I:=0 to 8 do
  begin
   Ra[I]:=0.0;
   Rb[I]:=0.0;
  end;
 case Mode of
   1:begin
      if abs(AxesVec[1]+1.0)<=EP then
       begin
        Rb[0]:=cos(Pi);
        Rb[2]:=-sin(Pi);
        Rb[4]:=1.0;
        Rb[6]:=sin(Pi);
        Rb[8]:=cos(Pi);
        R:=MatrixMultiply(R,Rb,1,3,3,3);
       end
      else if abs(AxesVec[1]-1.0)>EP then
       begin
        CosAlpha:=AxesVec[3]/sqrt(1.0-sqr(AxesVec[1]));
        SinAlpha:=sqrt(1.0-sqr(CosAlpha));
        CosBeita:=AxesVec[1];
        SinBeita:=sqrt(1.0-sqr(CosBeita));
        Ra[0]:=1.0;
        Ra[4]:=CosAlpha;
        Ra[5]:=SinAlpha;
        Ra[7]:=-SinAlpha;
        Ra[8]:=CosAlpha;
        Rb[0]:=CosBeita;
        Rb[2]:=-SinBeita;
        Rb[4]:=1.0;
        Rb[6]:=SinBeita;
        Rb[8]:=CosBeita;
        R:=MatrixMultiply(MatrixMultiply(R,Ra,1,3,3,3),Rb,1,3,3,3);
       end;
     end;
   2:begin
      if abs(AxesVec[2]+1.0)<=EP then
       begin
        Rb[0]:=1.0;
        Rb[4]:=cos(Pi);
        Rb[5]:=sin(Pi);
        Rb[7]:=-sin(Pi);
        Rb[8]:=cos(Pi);
        R:=MatrixMultiply(R,Rb,1,3,3,3);
       end
      else if abs(AxesVec[2]-1.0)>EP then
       begin
        CosAlpha:=AxesVec[1]/sqrt(1.0-sqr(AxesVec[2]));
        SinAlpha:=sqrt(1.0-sqr(CosAlpha));
        CosBeita:=AxesVec[2];
        SinBeita:=sqrt(1.0-sqr(CosBeita));
        Ra[0]:=CosAlpha;
        Ra[2]:=-SinAlpha;
        Ra[4]:=1.0;
        Ra[6]:=SinAlpha;
        Ra[8]:=CosAlpha;
        Rb[0]:=CosBeita;
        Rb[1]:=SinBeita;
        Rb[3]:=-SinBeita;
        Rb[4]:=CosBeita;
        Rb[8]:=1.0;
        R:=MatrixMultiply(MatrixMultiply(R,Ra,1,3,3,3),Rb,1,3,3,3);
       end;
     end;
   3:begin
      if abs(AxesVec[3]+1.0)<=EP then
       begin
        Rb[0]:=1.0;
        Rb[4]:=cos(Pi);
        Rb[5]:=sin(Pi);
        Rb[7]:=-sin(Pi);
        Rb[8]:=cos(Pi);
        R:=MatrixMultiply(R,Rb,1,3,3,3);
       end
      else if abs(AxesVec[3]-1.0)>EP then
       begin
        CosAlpha:=AxesVec[2]/sqrt(1.0-sqr(AxesVec[3]));
        SinAlpha:=sqrt(1.0-sqr(CosAlpha));
        CosBeita:=AxesVec[3];
        SinBeita:=sqrt(1.0-sqr(CosBeita));
        Ra[0]:=CosAlpha;
        Ra[1]:=SinAlpha;
        Ra[3]:=-SinAlpha;
        Ra[4]:=CosAlpha;
        Ra[8]:=1.0;
        Rb[0]:=1.0;
        Rb[4]:=CosBeita;
        Rb[5]:=SinBeita;
        Rb[7]:=-SinBeita;
        Rb[8]:=CosBeita;
        R:=MatrixMultiply(MatrixMultiply(R,Ra,1,3,3,3),Rb,1,3,3,3);
       end;
     end;
   else
     raise BaseMeshError.Create('参数错误！');
 end;
 for I:=0 to 2 do
   APoint[I+1]:=R[I];
end;

procedure AxesRolling(AxesVec:T3DVector;var APoint:T3DPoint;Mode:byte);
var
  I:integer;
  CosAlpha,SinAlpha:extended;//弧度单位
  R,Ra:TFloatArray;
begin
 AxesVec:=BasicVector(AxesVec);
 if abs(AxesVec[3])>Ep then
   raise BaseMeshError.Create('AxesVec轴必须平行于XOY面！');
 SetLength(R,3);
 SetLength(Ra,9);
 for I:=0 to 2 do
   R[I]:=APoint[I+1];
 case Mode of
   1:begin
      CosAlpha:=AxesVec[1];
      SinAlpha:=sqrt(1.0-sqr(CosAlpha));
      if AxesVec[2]>0.0 then SinAlpha:=-SinAlpha;
      Ra[0]:=CosAlpha;
      Ra[1]:=SinAlpha;
      Ra[3]:=-SinAlpha;
      Ra[4]:=CosAlpha;
      Ra[8]:=1.0;
      R:=MatrixMultiply(R,Ra,1,3,3,3);
     end;
   2:begin
      CosAlpha:=AxesVec[2];
      SinAlpha:=sqrt(1.0-sqr(CosAlpha));
      if AxesVec[1]<0.0 then SinAlpha:=-SinAlpha;
      Ra[0]:=CosAlpha;
      Ra[1]:=SinAlpha;
      Ra[3]:=-SinAlpha;
      Ra[4]:=CosAlpha;
      Ra[8]:=1.0;
      R:=MatrixMultiply(R,Ra,1,3,3,3);
     end;
   else
     raise BaseMeshError.Create('参数错误！');
 end;
 for I:=0 to 2 do
   APoint[I+1]:=R[I];
end;

end.