/* ------------------------------------------------------------------------ */ /* File: MGSolveNonlinearEqns.c created Jul 03 2026 by MotionGenesis 6.6. */ /* Portions copyright (c) 2009-2026 Motion Genesis LLC. are licensed under */ /* the 3-clause BSD license. https://opensource.org/licenses/BSD-3-Clause. */ /* This copyright notice must appear in all copies and distributions. */ /* MotionGenesis Professional Licensee: Motion Genesis LLC. */ /* ------------------------------------------------------------------------ */ #include #include #include #include #include #include #define myNumberOfODES 2 static char* ReadInputFile( double* absError ); static char* ReadFormattedString( FILE* Fptr, double* numberToReturnToCallingFunction ); static char* ReadFormattedNumbers( FILE* Fptr, double* next, ... ); static char* OpenOutputFilesWriteHeaders( int isPrintToScreen, int isPrintToFile ); static void OutputToScreenOrFile( double integTau, double Output[], int isPrintToScreen, int isPrintToFile ); static void CloseOutputFiles( char* errorMessage, int isPrintToScreen, int isPrintToFile ); static char* MGIntegrateForwardOrBackward( int numVariables, double varArrayToIntegrate[], double OutputToFill[], double tInitial, double tFinal, double tStep, double absError, double relError, int printIntScreen, int printIntFile ); static void SetArrayFromVariables( double VAR[] ); double A, R; double x, y; double Pi = 3.141592653589793, _COEF[2][2], *COEF[2], RHS[2], varArrayToIntegrate[myNumberOfODES], Output[6]; /* ------------------------------------------------------------------------ */ int main( void ) { int printIntScreen = 100, printIntFile = 100; double absError; char* errorMessage = ReadInputFile( &absError ); if( !errorMessage ) errorMessage = OpenOutputFilesWriteHeaders( printIntScreen, printIntFile ); if( errorMessage ) return 1; /* Initialize COEF pointers to beginning of each row */ { int iloop; for( iloop = 0; iloop < 2; iloop++ ) COEF[iloop] = &(_COEF[iloop][0]); } /* Begin FOR-loop(s) */ for( R = 1; R <= 3 && !errorMessage; R += 0.5 ) { /* Numerically integrate. */ SetArrayFromVariables( varArrayToIntegrate ); errorMessage = MGIntegrateForwardOrBackward( myNumberOfODES, varArrayToIntegrate, Output, 0, 1, 0.01, absError, 1.0E-08, printIntScreen, printIntFile ); } /* End FOR-loop */ /* Close output files. Inform user of input and output filename(s). */ CloseOutputFiles( errorMessage, printIntScreen, printIntFile ); return errorMessage ? 1 : 0; } /* ------------------------------------------------------------------------ */ static char* ReadInputFile( double* absError ) { char* inputFileName = "MGSolveNonlinearEqns.in"; FILE* inputFilePtr = fopen( inputFileName, "r" ); char* errorMessage = !inputFilePtr ? "Error: unable to open input file." : NULL; /* Ensure input file can be opened and read comment lines. */ int iloop; for( iloop = 0; iloop++ < 4 && !errorMessage; ) errorMessage = ReadFormattedString( inputFilePtr, NULL ); /* Read values of constants from input file */ if( !errorMessage ) errorMessage = ReadFormattedNumbers( inputFilePtr, &A, NULL ); /* Read the initial value of each variable from input file */ if( !errorMessage ) errorMessage = ReadFormattedNumbers( inputFilePtr, &x, &y, NULL ); /* Read integration parameters from input file */ if( !errorMessage ) errorMessage = ReadFormattedNumbers( inputFilePtr, absError, NULL ); /* Close the input file. */ if( inputFilePtr ) fclose( inputFilePtr ); /* If there is an error message, print it. */ if( errorMessage ) printf("\n%s\nFile: %s\n", errorMessage, inputFileName); return errorMessage; } /* ------------------------------------------------------------------------ */ static void CalculateCoefficientMatrix( double *coef[] ) { coef[0][0] = 2*x; coef[0][1] = 2*y; coef[1][0] = -A*cos(x); coef[1][1] = 1; } /* ------------------------------------------------------------------------ */ static void CalculateRhsMatrix( double rhs[] ) { rhs[0] = pow(R,2) - pow(x,2) - pow(y,2); rhs[1] = A*sin(x) - y; } /* ------------------------------------------------------------------------ */ static void SetArrayFromVariables( double VAR[] ) { VAR[0] = x; VAR[1] = y; } /* ------------------------------------------------------------------------ */ static void SetVariablesFromArray( double VAR[] ) { x = VAR[0]; y = VAR[1]; } /* ------------------------------------------------------------------------ */ static char* MGeqns( double integTau, double VAR[], double VARp[], char isIntegratorBoundary ) { /* Update named variables from variables passed to function */ SetVariablesFromArray( VAR ); /* Calculate the coefficient and rhs matrices and solve linear algebraic equations. */ CalculateCoefficientMatrix( COEF ); if( isIntegratorBoundary && integTau==0.0 ) CalculateRhsMatrix( RHS ); VARp[0] = COEF[0][0]*COEF[1][1] - COEF[0][1]*COEF[1][0]; VARp[1] =(COEF[0][0]*RHS[1]-COEF[1][0]*RHS[0])/VARp[0]; VARp[0] =(COEF[1][1]*RHS[0]-COEF[0][1]*RHS[1])/VARp[0]; return NULL; } /* ------------------------------------------------------------------------ */ static void CalculateOutput( double Output[] ) { /* Output[0] stores the sum of the absolute value of each equation (which when converged is approximately 0). */ CalculateRhsMatrix( Output ); Output[0] = fabs(Output[0]) + fabs(Output[1]); Output[1] = x; Output[2] = y; Output[3] = R; Output[4] = x; Output[5] = y; } /* ------------------------------------------------------------------------ */ static void OutputFormattedNumbers( FILE* Fptr, double* Output, ... ) { va_list varArgList; va_start( varArgList, Output ); { int numBlankSpaces; while( (numBlankSpaces = va_arg(varArgList, int)) != 0 ) fprintf(Fptr, "%*s%- 14.6E", numBlankSpaces, " ", *Output++); } va_end( varArgList ); fprintf( Fptr, "\n" ); } static FILE* myFptr[1] = { NULL }; /* ------------------------------------------------------------------------ */ static char* OpenOutputFilesWriteHeaders( int isPrintToScreen, int isPrintToFile ) { char* errorMessage = NULL; if( isPrintToScreen || isPrintToFile ) { const char* headerFile[1]; headerFile[0] = "% FILE: MGSolveNonlinearEqns.1\n%\n" "% ResidualSum x y R x y\n" "% (UNITS) (meters) (meters) (meters) (meters) (meters)\n\n"; if( isPrintToScreen ) fputs( headerFile[0], stdout ); if( isPrintToFile && !myFptr[0] ) { char* outputFileName = "MGSolveNonlinearEqns.1"; myFptr[0] = fopen( outputFileName, "w" ); if( myFptr[0] ) fputs( headerFile[0], myFptr[0] ); else { errorMessage = "Error: Unable to open/write to file"; printf("%s %s\n", errorMessage, outputFileName); } } } return errorMessage; } /* ------------------------------------------------------------------------ */ static void CloseOutputFiles( char* errorMessage, int isPrintToScreen, int isPrintToFile ) { if( errorMessage ) { int iMin = (isPrintToScreen ? -1 : 0), iMax = (isPrintToFile && myFptr[0]) ? 1 : 0, i; for( i = iMin; i < iMax; i++ ) fprintf( i==-1 ? stdout : myFptr[i], "\n%% %s\n", errorMessage ); } if( myFptr[0] ) fclose( myFptr[0] ); myFptr[0] = NULL; if( isPrintToScreen ) { fputs( "\n Input is in the file MGSolveNonlinearEqns.in", stdout ); fputs( "\n Output is in the file MGSolveNonlinearEqns.1\n", stdout ); } } /* ------------------------------------------------------------------------ */ static void OutputToScreenOrFile( double integTau, double Output[], int isPrintToScreen, int isPrintToFile ) { if( isPrintToScreen || isPrintToFile ) { CalculateOutput( Output ); if( isPrintToScreen ) OutputFormattedNumbers( stdout, Output, 1, 1, 1, 1, 1, 1, 0 ); if( isPrintToFile ) OutputFormattedNumbers( myFptr[0], Output, 1, 1, 1, 1, 1, 1, 0 ); if( isPrintToScreen && integTau ) fprintf(stdout, "\n"); if( isPrintToFile && integTau ) fprintf(myFptr[0], "\n"); } } /* ------------------------------------------------------------------------ */ static double StringToDouble( char* s ) { char* endNumber; double x = strtod(s, &endNumber); while( isspace(*endNumber) ) endNumber++; if(*endNumber || (x == HUGE_VAL || x == -HUGE_VAL)) { x = HUGE_VAL; printf("\nError: Unable to convert to valid double precision number from string %s", s); } return x; } /* ------------------------------------------------------------------------ */ static char* ReadFormattedString( FILE* Fptr, double* returnNumber ) { static char line[256], *errorMessage = NULL; static long lineNumber = 0; lineNumber++; if( !fgets(line, 256, Fptr) ) sprintf( errorMessage = line, "Error: End of input file found while reading line %ld", lineNumber ); else if( returnNumber && (strlen(line) < 60 || (*returnNumber = StringToDouble(line+59)) == HUGE_VAL) ) { printf("\n\n%s\n", line ); sprintf( errorMessage = line, "Error in input file: Expecting number starting at 60th character of line %ld", lineNumber); } return errorMessage; } /* ------------------------------------------------------------------------ */ static char* ReadFormattedNumbers( FILE* Fptr, double* next, ... ) { char* errorMessage = NULL; va_list args; /* Variable argument list */ for( va_start(args, next); next && !errorMessage; next = va_arg(args, double*) ) errorMessage = ReadFormattedString(Fptr, next); va_end( args ); /* Help function make normal return */ /* If no error message, also read to end of line (read newline char). */ return errorMessage ? errorMessage : ReadFormattedString(Fptr, NULL); } /* -------------------------------------------------------------------------- ** ** PURPOSE: Solve a set of first order ordinary differential equations of ** ** the form dy(i)/dt = f( t, y(1), ..., y(n) ) (i = 1 ... n) ** ** with a Runga-Kutta-Merson algorithm that numerically integrates ** ** the differential equations from tStart to tStart + hEntry. ** ** (L. Fox, Numerical Solutions of Ordinary and Partial Differ- ** ** ential Equations, Palo Alto: Addison-Wesley, 1962, pp. 24-25) ** ** ** ** INPUT: ** ** eqns Function that evaluates dy(i)/dt (i = 1 ... n), the first ** ** derivatives of y(1) ... y(n) with respect to t. ** ** ** ** numY The number n of differential equations to be solved. ** ** y One-dimensional array whose elements are y(1) ... y(n). ** ** ** ** tStart Value of independent variable t on entry to this function. ** ** ** ** hEntry Suggested numerical integration stepsize for this function. ** ** If *hEntry == 0, the function eqns is called and dy(i)/dt ** ** (i = 1 ... n) are evaluated, but no integration is performed. ** ** On return, *hEntry is the actual stepsize the integrator used. ** ** ** ** hNext Returns estimated stepsize for the next call to this function. ** ** ** ** absError Allowable absolute error in y(i) (i = 1 ... n). ** ** relError Allowable relative error in y(i) (i = 1 ... n). ** ** ** ** OUTPUT: Returns an error message if integration failed, otherwise NULL. ** ** y The values of y(i) (i=1 ... n) at t + *hEntry are returned in y. ** ** hEntry The actual stepsize used by integrator is returned in hEntry. ** ** hNext The integrator's estimate (based on error analysis of current ** ** step) is returned in hNext, or 0 if integration failed. ** ** ** ** Copyright (c) 2009-2026 by Motion Genesis LLC. Use permitted under the ** ** 3-clause BSD license: https://opensource.org/licenses/BSD-3-Clause. ** ** This copyright notice must appear in all copies & distributions. ** ** This copyright notice applies to the functions MGIntegrator, ** ** MGIntegrateOneStep, MGIntegrateForwardOrBackward. ** ** -------------------------------------------------------------------------- */ static char* MGIntegrator( char*(*eqns)(double, double*, double*, char), int numY, double y[], double tStart, double* hEntry, double* hNext, double smallestAllowableStepsize, double absError, double relError ) { double f0[myNumberOfODES], f1[myNumberOfODES], f2[myNumberOfODES]; double y1[myNumberOfODES], y2[myNumberOfODES]; double h = hEntry ? *hEntry : 0; /* Always calculate derivatives at tStart (integration boundary here). */ /* If h == 0, just call eqns at tStart and return. */ char* errorMessage = (*eqns)( tStart, y, f0, 1 ); if( !errorMessage && h == 0 ) return NULL; /* Avoid endless loop due to adding tiny h to tStart (precision problem) */ while( !errorMessage && tStart + 0.1*h != tStart ) { int i, indexFailedVariable = -1; /* Variable that failed. */ double errorRatioMax = 0; /* Testing error criterion */ double h2 = h * 0.5; /* Half of h */ double h3 = h / 3.0; /* Third of h */ double h6 = h / 6.0; /* Sixth of h */ double h8 = h * 0.125; /* Eighth of h */ for( i=0; i relTest ? absError : relTest; double errorRatio = errorInEstimate / largerOfAbsErrOrRelTest; if( errorRatio > errorRatioMax ) { errorRatioMax = errorRatio; indexFailedVariable = i; } } /* If integration succeeded, update values of y and t before return. */ /* Return actual stepsize and estimate for next integration stepsize. */ if( errorRatioMax < 1 ) { for( i = 0; i < numY; i++ ) y[i] = y2[i]; *hEntry = h; *hNext = (errorRatioMax < 1.0/64.0) ? 2*h : h; return NULL; } /* Otherwise, errorRatioMax >= 1, so absError or relTest failed. */ /* In other words, errorInEstimate >= largerOfAbsErrOrRelTest. */ /* Try to halve stepsize and restart integration. */ if( fabs(h=h2) <= fabs(smallestAllowableStepsize) ) { static char stepsizeCutMessage[128]; sprintf(errorMessage = stepsizeCutMessage, "Error: Numerical integration stepsize cut too many times at t = %17.9E (variable %d).", tStart, indexFailedVariable); } } /* Check if loop terminated due to numerical round-off. */ if( !errorMessage ) errorMessage = "Error: Numerical round off makes stepsize h too small relative to tStart, so tStart+h = tStart." "\nIntegration stepsize may have been cut too many times."; /* Print error message that numerical integrator failed. */ /* If h != 0, call eqns to fill for error display. */ printf( "\n Error: Numerical integration failed at t = %17.9E.\n\n", tStart ); if( h != 0 ) (*eqns)( tStart, y, f0, 1 ); return errorMessage; } /* -------------------------------------------------------------------------- ** ** PURPOSE: Controls the independent variable, e.g., t while solving a set ** ** of first order ordinary differential equations of the form ** ** dy(i)/dt = f( t, y(1), ..., y(n) ) (i = 1, ..., n). ** ** Discontinuities in functions and t should be handled here. ** ** ** ** INPUT: eqns, numY, y, absError, relError are described above. ** ** If numY = 0, the static value of myPreviousStepsize is reset to ** ** tStep. This is useful when integrating several sets of ODEs. ** ** ** ** t The current value of the independent variable. ** ** tStepMax Maximum integration stepsize for this integrator step. On entry ** ** if tStepMax = 0, eqns is called and dy(i)/dt (i=1 ... n) are ** ** evaluated, but no integration performed. ** ** ** ** OUTPUT: Returns an error message if integration failed, otherwise NULL. ** ** t The value of t + tStep is returned in t. ** ** y The values of y(i) (i=1 ... n) at t + tStep are returned in y. ** ** -------------------------------------------------------------------------- */ static char* MGIntegrateOneStep( char*(*eqns)(double, double*, double*, char), int numY, double y[], double* t, double tStepMax, double* stepsizeSuggested, double smallestAllowableStepsize, double absError, double relError ) { double hAccumulated = 0; /* How far to tStepMax. */ double h = *stepsizeSuggested; /* Current stepsize. */ int isStepFinished = (tStepMax == 0); /* Make as many little integration steps as necessary to get to end. */ /* Each integration step starts at *t and ends at *t + h. */ while( !isStepFinished ) { /* Numerically integrate y[i] and maybe get a smaller value of h. */ /* Set hNext to integrator's estimate of next integration step-size. */ double hBeforeCall = h, hNext; /* Suggested stepsize. */ char* errorMessage = MGIntegrator( eqns, numY, y, *t, &h, &hNext, smallestAllowableStepsize, absError, relError ); if( errorMessage ) return errorMessage; /* Integration failed */ /* Any time or function discontinuities should be handled here. */ /* Increment value of t using stepsize returned by the integrator. */ *t += h; /* Prepare for next step. */ /* Change the stepsize if not taking the last step. */ /* Reduce the stepsize if the integrator reduced the stepsize. */ /* Increase the stepsize if the integrator suggests a larger one. */ /* Update stepsizeSuggested for next call to integrator. */ /* Ensure stepsizeSuggested is not larger than tStepMax. */ isStepFinished = fabs( (hAccumulated+=h) + smallestAllowableStepsize) > fabs(tStepMax); if( !isStepFinished || fabs(h) < fabs(hBeforeCall) || fabs(hNext) > fabs(*stepsizeSuggested) ) { if( fabs(hNext) > fabs(tStepMax) ) hNext = tStepMax; *stepsizeSuggested = h = hNext; } /* Integrator may suggest a stepsize that is bigger than allowed. */ /* If next jump is past or very close to end of tStepMax, adjust h. */ if( fabs(hAccumulated + 1.1*h) > fabs(tStepMax) ) h = tStepMax - hAccumulated; } /* Integration finished. Calculate derivatives of y at final value of t */ return MGIntegrator( eqns, numY, y, *t, NULL, NULL, 0, 0, 0 ); } /* ------------------------------------------------------------------------ */ static char* MGIntegrateForwardOrBackward( int numVariables, double varArrayToIntegrate[], double outputToFill[], double tInitial, double tFinal, double tStepMax, double absError, double relError, int printIntScreen, int printIntFile ) { double stepsizeSuggested = tStepMax, smallestAllowableStepsize = 1.0E-7 * tStepMax; double t = tInitial, tFinalMinusEpsilon = tFinal - smallestAllowableStepsize; int isFirstCall = 1, isIntegrateForward = (tFinal >= tInitial), integrateDirection = isIntegrateForward ? 1 : -1; int isPrintToScreen, isPrintToFile, printCounterScreen = 0, printCounterFile = 0; /* Ensure valid parameters and sign(tStepMax) moves integration in proper direction. */ char* errorMessage = (numVariables <= 0 || !varArrayToIntegrate || absError <= 0 || relError < 0 || integrateDirection * tStepMax <= 0) ? "Error: Invalid argument to MGIntegrateForwardOrBackward" : NULL; /* Initialize integrator with call at t = tInitial, thereafter integrate */ int isIntegrationFinished = 0; while( !isIntegrationFinished && !errorMessage ) { /* Near the end of numerical integration, perhaps take a partial step (decrease tStepMax). */ if( (isIntegrateForward && t+tStepMax > tFinal) || (!isIntegrateForward && t+tStepMax < tFinal) ) { tStepMax = tFinal - t; if( integrateDirection * stepsizeSuggested > fabs(tStepMax) ) stepsizeSuggested = tStepMax; } /* Due to round-off or other, maybe set stepsizeSuggested = tStepMax. */ else if( fabs(stepsizeSuggested) >= 0.99 * fabs(tStepMax) ) stepsizeSuggested = tStepMax; errorMessage = MGIntegrateOneStep( MGeqns, numVariables, varArrayToIntegrate, &t, (isFirstCall ? 0 : tStepMax), &stepsizeSuggested, smallestAllowableStepsize, absError, relError ); isIntegrationFinished = errorMessage || (isIntegrateForward && t > tFinalMinusEpsilon) || (!isIntegrateForward && t < tFinalMinusEpsilon); if( (isPrintToScreen = printIntScreen && (isIntegrationFinished || --printCounterScreen <= 0)) != 0 ) printCounterScreen = printIntScreen; if( (isPrintToFile = printIntFile && (isIntegrationFinished || --printCounterFile <= 0)) != 0 ) printCounterFile = printIntFile; OutputToScreenOrFile( t, outputToFill, isPrintToScreen, isPrintToFile ); isFirstCall = 0; } return errorMessage; }