矩阵形式的龙格库塔程序C++源代码

assist

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;
}

矩阵类的构造及运算符重载太长，在此不提供

lnicholas

   很好恨强大。。学习之······

huannet

收藏一下 学习了