variablestepdoubleintegration.cpp

用于求解二阶常微分方程的变步长多步积分法

			}
			
			// get psi(n-1) and psi(n)
			for(int i=m_iLastIntpNums-1; i>-1; i--)
			{
				m_pPsiNm1[i] = (0==i) ? 0.0 : m_pPsiN[i-1];

				m_pPsiN[i] = m_pPsi[i];
			}

			if(m_iMaxIntpNums == m_iLastIntpNums)
			{			
				// no longer in variable-order mode
				// calculate step for next time
				double dSigma = 1.0;
				for(int i=1; i<m_iLastIntpNums+1; i++)
				{
					dSigma *= i*m_pAlpha[i-1];
				}

				double dSErk = fabs(m_dIntStep*m_pGammaStars[m_iLastIntpNums-1]*dSigma*m_pPhi[m_iLastIntpNums]);
				double dDErk = m_dIntStepSq*fabs(m_pLamdaStars[m_iLastIntpNums-1]*dSigma*m_pPhi[m_iLastIntpNums]);

				// calculate r for single and double integration
				double dSRho = pow(m_dEps/2.0/dSErk, 1.0/(m_iLastIntpNums+1));
				double dDRho = pow(m_dEps/2.0/dDErk, 1.0/(m_iLastIntpNums+2));

				// use smallest r
				m_dRho = __min(dSRho, dDRho);

				// bound r
				if(m_dRho > 2.0)
				{
					m_dRho = 2.0;
				}
				else if(m_dRho < 0.5)
				{
					m_dRho = 0.5;
				}						

				// already using maximum value of k, cycle this value into steps array.
				for(int i=0; i<m_iLastIntpNums-1; i++)
				{
					m_pIntStepArray[i] = m_pIntStepArray[i+1];
				}

				m_iIntpNums = m_iLastIntpNums;
			}
		}

		// change the step size
		m_dIntStep *= m_dRho;
		m_dIntStepSq = m_dIntStep*m_dIntStep;
		
		printf("The step size is :%f\n", m_dIntStep);

		m_pIntStepArray[m_iIntpNums-1] = m_dIntStep;

		// calculate alpha(n+1) and psi(n+1)		
		for(int i=0; i<m_iIntpNums; i++)
		{
			m_pPsi[i] = (0==i) ? m_dIntStep : m_pPsi[i-1]+m_pIntStepArray[m_iIntpNums-1-i];

			m_pAlpha[i] = (0==i) ? 1.0 : m_dIntStep/m_pPsi[i];
		}

		// compute ratio of current step size to last
		double dIntRatio = m_dIntStep / m_pIntStepArray[m_iIntpNums-2];
		double dInvIntRat = 1.0 / dIntRatio;

		// calculate integration coefficients
		for(int i=0; i<m_iIntpNums+1; i++)
		{
			for(int q=0; q<m_iIntpNums+2-i; q++)
			{
				if(0 == i)
				{
					m_pGCoeff[q*(m_iMaxIntpNums+1)+i] = 1.0/(q+1);
					m_pGpCoeff[q*(m_iMaxIntpNums+1)+i] = m_pGCoeff[q*(m_iMaxIntpNums+1)+i] * pow(-dInvIntRat, q+1);
				}
				else if(1 == i)
				{
					m_pGCoeff[q*(m_iMaxIntpNums+1)+i] = 1.0/(q+1)/(q+2);
					m_pGpCoeff[q*(m_iMaxIntpNums+1)+i] = m_pGCoeff[q*(m_iMaxIntpNums+1)+i] * pow(-dInvIntRat, q+2);
				}
				else
				{
					m_pGCoeff[q*(m_iMaxIntpNums+1)+i] = m_pGCoeff[q*(m_iMaxIntpNums+1)+i-1] - m_pAlpha[i-1]*m_pGCoeff[(q+1)*(m_iMaxIntpNums+1)+i-1];
					m_pGpCoeff[q*(m_iMaxIntpNums+1)+i] = m_pPsiNm1[i-2]/m_pPsi[i-1]*m_pGpCoeff[q*(m_iMaxIntpNums+1)+i-1] - m_pAlpha[i-1]*m_pGpCoeff[(q+1)*(m_iMaxIntpNums+1)+i-1];
				}
			}
		}

		// now integrate
		double dSum1, dSum2;
		for(int i=0; i<iEqNums; i++)
		{
			dSum1 = dSum2 = 0.0;
			for(int m=0; m<m_iIntpNums; m++)
			{
				// save differences before multiplying by beta
				m_pPhiNSave[i*m_iMaxIntpNums+m] = m_pPhiN[i*(m_iMaxIntpNums+1)+m];

				// calculate beta(n+1) and only once
				if(0 == i)
				{
					m_pBeta[m] = (0==m) ? 1.0 : m_pBeta[m-1]*m_pPsi[m-1]/m_pPsiN[m-1];
				}

				// calculate phi* = phi*beta
				m_pPhiN[i*(m_iMaxIntpNums+1)+m] *= m_pBeta[m];

				// calculate sums for integration
				dSum1 += m_pGCoeff[0*(m_iMaxIntpNums+1)+m]*m_pPhiN[i*(m_iMaxIntpNums+1)+m];
				dSum2 += (m_pGCoeff[1*(m_iMaxIntpNums+1)+m]+dIntRatio*m_pGpCoeff[1*(m_iMaxIntpNums+1)+m])*m_pPhiN[i*(m_iMaxIntpNums+1)+m];
			}

			// predict velocity and position
			pVel[i] = m_pLastVel[i] + m_dIntStep*dSum1;
			pPos[i] = (1.0+dIntRatio)*m_pLastPos[i] - dIntRatio*m_pPos2Back[i] + m_dIntStepSq*dSum2;
		}

		// update the time
		*pTime = m_dIntTime + m_dIntStep;

		// evaluate second order ODE equation
		_2ndODEs(iEqNums, *pTime, pPos, pVel, m_pAcc);

		// get new differences and error estimate
		m_dSTempErr = m_dDTempErr = 0.0;
		for(int i=0; i<iEqNums; i++)
		{
			// get new differences
			for(int m=0; m<m_iIntpNums+1; m++)
			{
				m_pPhi[i*(m_iMaxIntpNums+1)+m] = (0==m) ? m_pAcc[i] : m_pPhi[i*(m_iMaxIntpNums+1)+m-1]-m_pPhiN[i*(m_iMaxIntpNums+1)+m-1];
			}

			// get error estimate
			m_pSWeight[i] = fabs(m_pLastVel[i])*m_dRelEps + m_dAbsEps;
			m_pDWeight[i] = fabs(m_pLastPos[i])*m_dRelEps + m_dAbsEps;
			m_dSTempErr += (m_pPhi[i*(m_iMaxIntpNums+1)+m_iIntpNums]/m_pSWeight[i])*(m_pPhi[i*(m_iMaxIntpNums+1)+m_iIntpNums]/m_pSWeight[i]);
			m_dDTempErr += (m_pPhi[i*(m_iMaxIntpNums+1)+m_iIntpNums]/m_pDWeight[i])*(m_pPhi[i*(m_iMaxIntpNums+1)+m_iIntpNums]/m_pDWeight[i]);
		}

		m_dSTempErr = sqrt(m_dSTempErr);
		m_dDTempErr = sqrt(m_dDTempErr);

		m_dSErr = fabs(m_dIntStep*(m_pGCoeff[0*(m_iMaxIntpNums+1)+m_iIntpNums]-m_pGCoeff[0*(m_iMaxIntpNums+1)+m_iIntpNums-1])) * m_dSTempErr;
		m_dDErr = m_dIntStepSq*fabs(m_pGCoeff[1*(m_iMaxIntpNums+1)+m_iIntpNums]-m_pGCoeff[1*(m_iMaxIntpNums+1)+m_iIntpNums-1]+dIntRatio*(m_pGpCoeff[1*(m_iMaxIntpNums+1)+m_iIntpNums]-m_pGpCoeff[1*(m_iMaxIntpNums+1)+m_iIntpNums-1])) * m_dDTempErr;
		
		if(m_dSErr>m_dEps || m_dDErr>m_dEps)
		{
			// step failed
			// reset everything and use half this step next time
			m_iFail++;
			m_dRho = 0.5;

			for(int i=0; i<iEqNums; i++)
			{
				for(int m=0; m<m_iIntpNums; m++)
				{
					m_pPhiN[i*(m_iMaxIntpNums+1)+m] = m_pPhiNSave[i*(m_iMaxIntpNums)+m];
				}
			}

			// if 3rd failure, start over
			if(m_iFail >= 3)
			{
				//Init(dInitTime, dReqTime, dInitPos, dInitVel);
				printf("You are wrong!\n");

				return false;
			}

			continue;		
		}
		else
		{
			// step succeeded, reset fail count
			m_iFail = 0;

			// set last used value of k, in case an interpolation is needed now
			m_iLastIntpNums = m_iIntpNums++;
			m_dRho = 2.0;	
		}

		// correct velocity and position
		for(int i=0; i<iEqNums; i++)
		{
			pVel[i] += m_dIntStep*m_pGCoeff[0*(m_iMaxIntpNums+1)+m_iLastIntpNums]*m_pPhi[i*(m_iMaxIntpNums+1)+m_iLastIntpNums];
			pPos[i] += m_dIntStepSq*(m_pGCoeff[1*(m_iMaxIntpNums+1)+m_iLastIntpNums]+dIntRatio*m_pGpCoeff[1*(m_iMaxIntpNums+1)+m_iLastIntpNums])*m_pPhi[i*(m_iMaxIntpNums+1)+m_iLastIntpNums];
		}
		
		// the following "if" test is for stability, because algorithm is on the start-up mode
		if(m_iMaxIntpNums != m_iLastIntpNums)
		{
			// evaluate second order ODE equation
			_2ndODEs(iEqNums, *pTime, pPos, pVel, m_pAcc);

			// get new differences
			for(int i=0; i<iEqNums; i++)
			{
				for(int m=0; m<m_iLastIntpNums+1; m++) 
				{
					m_pPhi[i*(m_iMaxIntpNums+1)+m] = (0==m) ? m_pAcc[i] : m_pPhi[i*(m_iMaxIntpNums+1)+m-1]-m_pPhiN[i*(m_iMaxIntpNums+1)+m-1];
				}
			}
		}

		// get ready for next step, copy latest differences from m_pPhi to m_pPhiN,
		// and set integration state and time to current values.
		for(int i=0; i<iEqNums; i++)
		{
			for(int m=0; m<m_iLastIntpNums+1; m++)
			{
				m_pPhiN[i*(m_iMaxIntpNums+1)+m] = m_pPhi[i*(m_iMaxIntpNums+1)+m];
			}
			
			// set integration state to current values
			m_pIntVel[i] = pVel[i];
			m_pIntPos[i] = pPos[i];
		}		

		// set integration time		
		m_dIntTime = *pTime;

		fprintf_s(m_pStream, "%17.16f\t%17.16f\t%17.16f\n", m_dIntTime, m_pIntPos[0], sin(m_dIntTime)-m_pIntPos[0]);
	}
	// end of integration loop, check to see if interpolation is needed
	
	printf("The time is %f\n", m_dIntTime);

	// the following new function is for interpolation
	m_pGamma1 = new double[m_iMaxIntpNums];
	m_pGammaIntp = new double[m_iMaxIntpNums];

	m_pGIntpCoeff = new double[(m_iMaxIntpNums+2)*(m_iMaxIntpNums+1)];
	m_pGpIntpCoeff = new double[(m_iMaxIntpNums+2)*(m_iMaxIntpNums+1)];

	if(((m_dIntTime>m_dEndTime)&&(1==m_iIntDir)) || ((m_dIntTime<m_dEndTime)&&(-1==m_iIntDir)))
	{
		// integrated too far, interpolate to request time

		// set interpolation step
		m_dIntpStep = m_dEndTime - m_dIntTime;
		m_dIntpStepSq = m_dIntpStep*m_dIntpStep;

		double dIntpRatio = m_dIntpStep / m_pIntStepArray[m_iLastIntpNums-1]; // or dIntpRatio = m_dIntpStep / m_dIntStep
		double dInvIntpRat = 1.0 / dIntpRatio;

		// compute gamma values
		for(int i=0; i<m_iLastIntpNums; i++)
		{
			m_pGamma1[i] = (0==i) ? m_dIntpStep/m_pPsi[i] : (m_dIntpStep+m_pPsi[i-1])/m_pPsi[i];

			m_pGammaIntp[i] = (0==i) ? -1.0 : ((1==i) ? 0.0 : m_pPsiN[i-2]/m_pPsi[i]);
		}

		// calculate interpolation coefficients
		for(int i=0; i<m_iLastIntpNums+1; i++)
		{
			for(int q=0; q<m_iLastIntpNums+2-i; q++)
			{
				if(0 == i)
				{
					m_pGIntpCoeff[q*(m_iMaxIntpNums+1)+i] = 1.0/(q+1);
					m_pGpIntpCoeff[q*(m_iMaxIntpNums+1)+i] = m_pGIntpCoeff[q*(m_iMaxIntpNums+1)+i] * pow(-dInvIntpRat, q+1);
				}
				else
				{
					m_pGIntpCoeff[q*(m_iMaxIntpNums+1)+i] = m_pGamma1[i-1]*m_pGIntpCoeff[q*(m_iMaxIntpNums+1)+i-1] - m_dIntpStep/m_pPsi[i-1]*m_pGIntpCoeff[(q+1)*(m_iMaxIntpNums+1)+i-1];
					m_pGpIntpCoeff[q*(m_iMaxIntpNums+1)+i] = m_pGammaIntp[i-1]*m_pGpIntpCoeff[q*(m_iMaxIntpNums+1)+i-1] - m_dIntpStep/m_pPsi[i-1]*m_pGpIntpCoeff[(q+1)*(m_iMaxIntpNums+1)+i-1];
				}
			}
		}

		// compute interpolation values
		double dSum1, dSum2;
		for(int i=0; i<iEqNums; i++)
		{
			dSum1 = dSum2 = 0.0;
			for(int m=0; m<m_iLastIntpNums+1; m++)
			{
				dSum1 += m_pGIntpCoeff[0*(m_iMaxIntpNums+1)+m]*m_pPhi[i*(m_iMaxIntpNums+1)+m];
				dSum2 += (m_pGIntpCoeff[1*(m_iMaxIntpNums+1)+m]+dIntpRatio*m_pGpIntpCoeff[1*(m_iMaxIntpNums+1)+m])*m_pPhi[i*(m_iMaxIntpNums+1)+m];
			}

			pVel[i] = m_pIntVel[i] + m_dIntpStep*dSum1;
			pPos[i] = (1.0+dIntpRatio)*m_pIntPos[i] - dIntpRatio*m_pLastPos[i] + m_dIntpStepSq*dSum2;
		}
	} 
	// end of interpolation

	fprintf_s(m_pStream, "%17.16f\t%17.16f\t%17.16f\n", m_dEndTime, pPos[0], sin(m_dEndTime)-pPos[0]);

	return true;
}