原創文檔

BOOL CMatrix::RungeKutta(const CMatrix &M, const CMatrix &C,const CMatrix &K,const CMatrix &F,
       const CMatrix &X,  const CMatrix &dX,double &h, int NumDeta,CMatrix *RValue)
{
 int i,j;
 m_nNumColumns =M.GetColumnNum();
 m_nNumRows=M.GetRowNum ();
 CMatrix CopyM,InvM;
 CopyM.Init (m_nNumRows,m_nNumColumns);
 InvM.Init (m_nNumRows,m_nNumColumns);
 CopyM=M;
 InvM=M;
 CMatrix zero;//取負值用。-沒有完全重載。
 zero.Init (m_nNumRows,m_nNumColumns );
 CMatrix A,A3,A4;
 A.Init(2*m_nNumRows,2*m_nNumColumns);
 A3.Init(m_nNumRows,m_nNumColumns);
 A4.Init(m_nNumRows,m_nNumColumns);
// A=|0   I | 
//   |A3  A4|14*14
 CMatrix B,B2;
 B.Init(2*m_nNumRows,1);
 B2.Init(m_nNumRows,1);
// B=|B1|
//   |B2|14*7
 //構造系數矩陣
 A.Init (2*m_nNumRows,2*m_nNumColumns );
 B.Init (2*m_nNumRows,m_nNumColumns );
 //賦值A2;
 for (i=0;i<m_nNumRows;i++)
 {
  A.SetElement(i,i+m_nNumColumns,1);
 }
 //計算A3
 InvM.InvertGaussJordan ();
 A3=InvM*K;
 A3=zero-A3;
 //賦值A3;
 for (i=0;i<m_nNumRows;i++)
 {
  for (j=0;j<m_nNumColumns ;j++)
  {
   A.SetElement (i+m_nNumRows,j,A3.GetElement (i,j));
  }
 }
 //計算A4
 A4=InvM*C;
 A4=zero-A4;
 for (i=0;i<m_nNumRows;i++)
 {
  for (j=0;j<m_nNumColumns ;j++)
  {
   A.SetElement (i+m_nNumRows,j+m_nNumColumns ,A4.GetElement (i,j));
  }
 }
 //計算B2
 B2=InvM;
 for (i=0;i<m_nNumRows;i++)
 {
  for (j=0;j<m_nNumColumns ;j++)
  {
   B.SetElement (i+m_nNumRows,j,B2.GetElement (i,j));
  }
 }
 //A,B系數矩陣賦值結束
 //四階龍格庫塔公式的系數K1,K2,K3,K4;
 CMatrix K1,K2,K3,K4;
 //賦初值為0
 K1.Init (2*m_nNumRows,1);
 K2.Init (2*m_nNumRows,1);
 K3.Init (2*m_nNumRows,1);
 K4.Init (2*m_nNumRows,1);
 //計算K1,K2,K3,K4
 //構造Y陣
 CMatrix Y;
 Y.Init (2*m_nNumRows,1);
 for (i=0;i<m_nNumRows;i++)
 {
  Y.SetElement (i,0,X .GetElement (i,0));
  Y.SetElement (i+m_nNumRows,0,dX .GetElement (i,0));
 }
 //一次只計算一個步長。
 //臨時的列I陣
// CMatrix tempI;
// tempI.Init (2*m_nNumRows,1);
// for (i=0;i<m_nNumRows;i++)
// {
//  tempI.SetElement (i,0,1);
//  tempI.SetElement (i+m_nNumRows,0,1);
// }
 CMatrix CopyF;
 CopyF=F;
 CMatrix Result;
 zero.Init (m_nNumRows,1);
 for (j=0;j<NumDeta;j++)
 {
  Result=EleFunction (A,B,Y,CopyF);
  K1=Result*h;
  //F為常量，不增加步長。
  Result=EleFunction (A,B,Y+K1*0.5,CopyF);
  K2=Result*h;
  Result=EleFunction (A,B,Y+K2*0.5,CopyF);
  K3=Result*h;
  Result=EleFunction (A,B,Y+K3,CopyF);
  K4=Result*h;
  Y =Y+(K1+K2*2+K3*2+K4)/6;
  //為了計算加速度，采用以下方法：
  //   ddX=-invM*C*dX-invM*K*X+invM*F
  //構造ddX;又由于X，dX是const，定義copyX,copydX
  CMatrix ddX,CopyX,CopydX;
  ddX.Init (m_nNumRows,1);
  CopyX.Init (m_nNumRows,1);
  CopydX.Init (m_nNumRows,1);
  //構造臨時變量
  CMatrix t;
  for (i=0;i<m_nNumRows;i++)
  {
   CopyX.SetElement (i,0,Y.GetElement (i,0));
   RValue[j].SetElement (i,0,Y.GetElement (i,0));
   CopydX.SetElement (i,0,Y.GetElement (i+m_nNumRows,0));
   RValue[j] .SetElement (i+m_nNumRows,0,Y.GetElement (i+m_nNumRows,0));
  }
  t=InvM *C;
  t=t*CopydX;
  ddX=zero-t;
  t=InvM*K;
  t=t*CopyX;
  ddX=ddX-t;
  t=InvM *F;
  ddX=ddX+t;
  //賦值給返回矩陣RValue,RValue是21*1的矩陣。包含速度，位移加速度值。
  for (i=0;i<m_nNumRows;i++)
  {
   RValue[j] .SetElement (i+2*m_nNumRows,0,ddX.GetElement (i,0));
  }
 }
 return true;
}
//A 14*14, B 14*7 Y 14*1 F 7*1;
CMatrix CMatrix::EleFunction(CMatrix &A,CMatrix &B,CMatrix &Y,CMatrix &F)
{
 int m_nRow;
 m_nRow=A.GetRowNum ();
 CMatrix dY;
 dY.Init (m_nRow ,1);
 dY=A*Y+B*F;
 return dY;
}