variablestepdoubleintegration.cpp

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

#include "stdafx.h"
#include "VariableStepDoubleIntegration.h"

CVariableStepDoubleIntegration::CVariableStepDoubleIntegration(void)
{
	m_iMaxIntpNums = 9;

	m_dAbsErr = 1e-14;
	m_dRelErr = 1e-6;

	double dGammaStars[] = {-0.5, -0.08333333333333333, 
							-0.04166666666666667, -0.02638888888888891, 
							-0.01874999999999999, -0.01426917989417986,
							-0.01136739417989419, -0.009356536596119958, 
							-0.007892554012345676};
	double dLamdaStars[] = {-1.0, 0.08333333333333333,
							 0.0, -0.004166666666666666,
							 -0.004166666666666666, -0.003654100529100521,
							 -0.0031415343911534404, -0.002708608906525564,
							 -0.002355324074074072};

	m_pGammaStars = dGammaStars;
	m_pLamdaStars = dLamdaStars;

	errno_t err;

	// Open for write 
	if((err = fopen_s(&m_pStream, "data2.txt", "w+")) != 0)
	{
		printf("The file 'data2.txt' was not opened\n");
	}
	else
	{
		printf("The file 'data2.txt' was opened\n");
	}
}

CVariableStepDoubleIntegration::~CVariableStepDoubleIntegration(void)
{
	// Close stream if it is not NULL 
	if(m_pStream)
	{
		if(fclose(m_pStream))
		{
			printf("The file 'data2.txt' was not closed\n");
		}
	}
}

void CVariableStepDoubleIntegration::_2ndODEs(int iEqNums, double dTime, double *pPos, double *pVel, double *pAcc)
{

	// the following is for y''=-y
	for(int i=0; i<iEqNums; i++)
	{
		pAcc[i] = -pPos[i];
	}

/*
	// the following is for y''=u(1-y^2)y'-y
	double dMu = 1;
	for(int i=0; i<iEqNums; i++)
	{
		pAcc[i] = dMu*(1-pPos[i]*pPos[i])*pVel[i] - pPos[i];
	}
*/
}

bool CVariableStepDoubleIntegration::Init(int iEqNums, double *pTime, double dEndTime, double *pInitPos, double *pInitVel)
{
	fprintf_s(m_pStream, "time\tvalue\terr\n");
	fprintf_s(m_pStream, "%17.16f\t%17.16f\t%17.16f\n", *pTime, pInitPos[0], sin(*pTime)-pInitPos[0]);

	m_dEndTime = dEndTime;

	m_iIntpNums = 1;
	m_iLastIntpNums = 0;

	// set EPS, relative epsilon, absolute epsilon
	m_dEps = __max(m_dRelErr, m_dAbsErr); // epsilon
	m_dRelEps = m_dRelErr / m_dEps; // relative epsilon
	m_dAbsEps = m_dAbsErr / m_dEps; // absolute epsilon

	// get machine epsilon
	double dMacEps = numeric_limits<double>::epsilon();

	// get initial value of acceleration
	// y'' = f(t, y, y')
	_2ndODEs(iEqNums, *pTime, pInitPos, pInitVel, m_pAcc);

	// get initial step estimate for single and double integration
	m_dSTempErr = m_dDTempErr = 0.0;
	for(int i=0; i<iEqNums; i++)
	{
		m_pLastVel[i] = pInitVel[i];
		m_pLastPos[i] = pInitPos[i];

		m_pPhiN[i*(m_iMaxIntpNums+1)+0] = m_pAcc[i];

		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_pAcc[i]/m_pSWeight[i]) * (m_pAcc[i]/m_pSWeight[i]);
		m_dDTempErr += (pInitVel[i]/m_pDWeight[i]) * (pInitVel[i]/m_pDWeight[i]);
	}

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

	// get initial step estimate for single and double integration
	double dSStepSize = 0.25 * sqrt(m_dEps/m_dSTempErr);
	double dDStepSize = 0.25 * sqrt(m_dEps/m_dDTempErr);

	m_dIntStep = __max(__min(__min(dSStepSize, dDStepSize), fabs(m_dEndTime-*pTime)), 4.0*dMacEps*fabs(*pTime));

	// set step size in the right direction
	m_iIntDir = (m_dEndTime>=*pTime) ? 1 : -1;
	if(-1 == m_iIntDir)
	{	
		m_dIntStep = -m_dIntStep;
	}

	m_iFail = 0;
	bool bRepeat = true;

	while(bRepeat && m_iFail < 10)
	{
		bRepeat = false;

		m_dIntStepSq = m_dIntStep*m_dIntStep;
		m_pIntStepArray[m_iIntpNums-1] = m_dIntStep;

		// do first step with general formulation
		// predict
		for(int i=0; i<iEqNums; i++)
		{
			pInitVel[i] = m_pLastVel[i] + m_dIntStep*m_pPhiN[i*(m_iMaxIntpNums+1)+0];
			pInitPos[i] = m_pLastPos[i] + m_dIntStep*m_pLastVel[i] + m_dIntStepSq*m_pPhiN[i*(m_iMaxIntpNums+1)+0]/2.0;
		}

		// advance time
		*pTime += m_dIntStep; 

		// evaluate		
		_2ndODEs(iEqNums, *pTime, pInitPos, pInitVel, m_pAcc);

		// get differences and error estimate		
		m_dSTempErr = m_dDTempErr = 0.0;
		for(int i=0; i<iEqNums; i++)
		{
			// get 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)+1])/m_pSWeight[i]) * ((m_pPhi[i*(m_iMaxIntpNums+1)+1])/m_pSWeight[i]);
			m_dDTempErr += ((m_pPhi[i*(m_iMaxIntpNums+1)+1])/m_pDWeight[i]) * ((m_pPhi[i*(m_iMaxIntpNums+1)+1])/m_pDWeight[i]);
		}
		
		m_dSTempErr = sqrt(m_dSTempErr);
		m_dDTempErr = sqrt(m_dDTempErr);

		m_dSErr = fabs(m_dIntStep)*m_dSTempErr/2.0;
		m_dDErr = m_dIntStepSq*m_dDTempErr/3.0;

		if(m_dSErr>m_dEps || m_dDErr>m_dEps)
		{
			// set everything back and try again with half step
			bRepeat = true;
			m_iFail++;
			*pTime -= m_dIntStep;
			m_dIntStep /= 2.0;
		}
		else if(0 == m_iFail)
		{
			// step succeeded, try again with a larger step, 
			// to find maximum initial step
			bRepeat = true;
			*pTime -= m_dIntStep;
			m_dIntStep *= 2.0;
		}
	}
	// end of initial step loop

	if(bRepeat)
	{
		printf("Unable to take first step after 10 tries, giving up!");

		return false;
	}

	// reset fail count
	m_iFail = 0;

	// correct
	for(int i=0; i<iEqNums; i++)
	{
		pInitVel[i] += m_dIntStep*m_pPhi[i*(m_iMaxIntpNums+1)+1]/2.0;
		pInitPos[i] += m_dIntStepSq*m_pPhi[i*(m_iMaxIntpNums+1)+1]/6.0;
	}	

	// re-evaluate and get differences
	_2ndODEs(iEqNums, *pTime, pInitPos, pInitVel, m_pAcc);

	// get differences
	for(int i=0; i<iEqNums; i++)
	{
		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];
		}
	}

	// increment order, double step size on next step
	m_iLastIntpNums = m_iIntpNums++;
	m_dRho = 2.0;

	// 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++)
	{
		// copy latest differences from m_pPhi to m_pPhiN
		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] = pInitVel[i];
		m_pIntPos[i] = pInitPos[i];
	}

	// set integration time
	m_dIntTime = *pTime;

	m_pPsi[0] = m_dIntStep;

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

	printf("The step size is :%f\n", m_dIntStep);

	return true;
}

bool CVariableStepDoubleIntegration::StartIntegration(int iEqNums, double *pTime, double dEndTime, double *pPos, double *pVel)
{	
	// the following new function is for Init(...)
	m_pAcc = new double[iEqNums];

	m_pLastVel = new double[iEqNums];
	m_pLastPos = new double[iEqNums];

	m_pPhiN = new double[iEqNums*(m_iMaxIntpNums+1)];

	m_pSWeight = new double[iEqNums];
	m_pDWeight = new double[iEqNums];

	m_pIntStepArray = new double[m_iMaxIntpNums];
	m_pPhi = new double[iEqNums*(m_iMaxIntpNums+1)];

	m_pIntVel = new double[iEqNums];
	m_pIntPos = new double[iEqNums];

	m_pPsi = new double[m_iMaxIntpNums];

	// init
	if(!Init(iEqNums, pTime, dEndTime, pPos, pVel))
	{
		return false;
	}

	// the following new function is for integration or while loop
	m_pPos2Back = new double[iEqNums];

	m_pPsiNm1 = new double[m_iMaxIntpNums-1];
	m_pPsiN = new double[m_iMaxIntpNums-1];

	m_pAlpha = new double[m_iMaxIntpNums];

	m_pGCoeff = new double[(m_iMaxIntpNums+2)*(m_iMaxIntpNums+1)];
	m_pGpCoeff = new double[(m_iMaxIntpNums+2)*(m_iMaxIntpNums+1)];

	m_pPhiNSave = new double[iEqNums*m_iMaxIntpNums];
	m_pBeta = new double[m_iMaxIntpNums];

	// integrate to, or past, the request time
	while(((m_dEndTime>m_dIntTime)&&(1==m_iIntDir)) || ((m_dEndTime<m_dIntTime)&&(-1==m_iIntDir)))
	{		
		if(0 == m_iFail)
		{
			for(int i=0; i<iEqNums; i++)
			{
				m_pPos2Back[i] = m_pLastPos[i];
				m_pLastPos[i] = m_pIntPos[i];
				m_pLastVel[i] = m_pIntVel[i];