矩阵形式的龙格库塔程序C++源代码
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 |
// |A3A4|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.SetElement (i,0,Y.GetElement (i,0));
CopydX.SetElement (i,0,Y.GetElement (i+m_nNumRows,0));
RValue .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 .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;
}
矩阵类的构造及运算符重载太长，在此不提供 {:{39}:} 很好恨强大。。学习之······ 收藏一下 学习了{:{23}:}
页:
[1]