riley@mipsdal.mips.com (Riley Rainey) (03/02/91)
Submitted-by: riley@mipsdal.mips.com (Riley Rainey) Posting-number: Volume 12, Issue 13 Archive-name: acm/part08 #! /bin/sh # This is a shell archive. Remove anything before this line, then unpack # it by saving it into a file and typing "sh file". To overwrite existing # files, type "sh file -c". You can also feed this as standard input via # unshar, or by typing "sh <file", e.g.. If this archive is complete, you # will see the following message at the end: # "End of archive 8 (of 9)." # Contents: acm/fsim/pm.c # Wrapped by riley@mipsdal on Thu Feb 14 10:09:21 1991 PATH=/bin:/usr/bin:/usr/ucb ; export PATH if test -f 'acm/fsim/pm.c' -a "${1}" != "-c" ; then echo shar: Will not clobber existing file \"'acm/fsim/pm.c'\" else echo shar: Extracting \"'acm/fsim/pm.c'\" \(16393 characters\) sed "s/^X//" >'acm/fsim/pm.c' <<'END_OF_FILE' X/* X * xflight : an aerial combat simulator for X X * X * Written by Riley Rainey, riley@mips.com X * X * Permission to use, copy, modify and distribute (without charge) this X * software, documentation, images, etc. is granted, provided that this X * comment and the author's name is retained. X * X */ X X#include <stdio.h> X#include <math.h> X#include "pm.h" X Xint debug = 0; X X/* X * A Flight Simulator X * X * Riley Rainey X */ X X/* X * We keep a table of atmospheric constants for different altitudes. X * These values are important to flight calculations. X */ X Xstruct { X double alt; /* altitude in feet */ X double rho; /* rho value (air density) */ X double mach1; /* speed of sound in feet per second */ X } *rhop, rhoTable[] = { X 0.0, 23.77, 1116.9, X 2.0, 22.41, 1109.2, X 4.0, 21.11, 1101.4, X 6.0, 19.87, 1093.6, X 8.0, 18.68, 1085.7, X 10.0, 17.55, 1077.8, X 15.0, 14.96, 1057.7, X 20.0, 12.66, 1037.3, X 25.0, 10.65, 1016.4, X 30.0, 8.89, 995.1, X 35.0, 7.365, 973.3, X 40.0, 5.851, 968.5, X 50.0, 3.618, 968.5, X 60.0, 2.238, 968.5, X 80.0, 0.9065, 980.0, X 100.0, 0.3371, 1015.0, X 120.0, 0.1340, 1053.0, X 160.0, 0.02622, 1083.0, X 100000.0, 0.02622, 1083.0}; /* a large value for alt at the end */ X Xdouble deltaT; /* Update interval in seconds */ Xdouble halfDeltaTSquared; /* 0.5 * deltaT * deltaT */ Xdouble CM, CN, CY; X X/* X * calcRho : Calculate air density and the speed of sound by interpolation. X */ X Xdouble calcRho (alt, mach) Xdouble alt; Xdouble *mach; { X X double deltaAlt, b; X X alt = alt / 1000.0; X X for (rhop=rhoTable; alt>(rhop+1)->alt; ++rhop) ; X deltaAlt = (rhop+1)->alt - rhop->alt; X b = ((rhop+1)->mach1 - rhop->mach1) / deltaAlt; X *mach = rhop->mach1 + b * (alt - rhop->alt); X b = ((rhop+1)->rho - rhop->rho) / deltaAlt; X return (rhop->rho + b * (alt - rhop->alt)) / 10000.0; X X} X X/* X * twoorder: X */ X Xvoid twoOrder (k, d, y, v, newy, newv) Xdouble k, d, y, v; Xdouble *newy, *newv; { X X double s, t, ac, x, c1; X X ac = d * d + 4.0 * k; X if (ac < 0.0) { X s = d / 2.0; X t = sqrt (-ac) / 2.0; X } X else { X s = - d - sqrt(ac) / 2.0; X t = 0.0; X } X X if (t == 0.0 || y == 0.0) X x = 0.0; X else X x = atan2 ( y * s - v, t * y ) / t; X X if (x == 0.0) X c1 = y; X else if (cos (t * x) != 0.0) X c1 = y / ( exp (s * x) * cos (t * x) ); X else { X *newy = 0.0; X *newv = v; X } X X/* X * Now we can compute the values of y and v at the end of this X * time interval; X */ X X#ifdef notdef X printf ("s = %g, t = %g, x = %g, y = %g, c1 = %g\n", s, t, x, X y, c1); X X if (fabs (y - (exp(s*x) * c1 * cos(t * x))) > 0.001) X printf ("*** possible error ***\n"); X X *newv = exp(s*x) * c1 * (s * cos(t*x) - t*sin(t*x)); X if (fabs (v - *newv) > 0.001) X printf ("*** possible v error *** %g %g\n", v, *newv); X#endif X X x += deltaT; X X *newy = exp (s * x) * c1 * cos (t * x); X *newv = exp (s * x) * c1 * (s * cos (t * x) - t * sin (t * x)); X X#ifdef notdef X printf ("ny = %g, nv = %g\n", *newy, *newv); X#endif X X} X X X/* X * calcCoefficients : Calculate CLift and friends X */ X Xvoid calcCoefficients (c, CLift, CDrag) Xcraft *c; Xdouble *CLift, *CDrag; { X X double CDAlpha, CDBeta; X X/* X * We used to interpolate these values, but now use several characteristic X * equations to compute these values for a given alpha value. The basic X * formulas are: X * X * X * C = C + (alpha * (C + sin(curFlap) * cFlap )) X * L LOrigin LSlope X * X * X * C = C + (C * Cos (alpha + C )) X * D DOrigin DFactor DPhase X * X * There are independent equations defining drag resulting from alpha X * and beta values. The hypoteneuse of those two values becomes the X * resultant CDrag value. X */ X X if (c->alpha < c->cinfo->CLNegStall || c->alpha > c->cinfo->CLPosStall) { X X *CLift = 0.0; X CM = c->cinfo->cmFactor + c->damageCM; X } X else { X *CLift = c->cinfo->CLOrigin + X (c->cinfo->CLSlope + sin (c->curFlap) * c->cinfo->cFlap) X * c->alpha; X CM = c->cinfo->cmSlope + c->damageCM; X } X X/* CDAlpha = c->cinfo->CDOrigin + c->cinfo->CDFactor * X cos (c->alpha + c->cinfo->CDPhase); X*/ X X CDAlpha = (*c->cinfo->CDi)(c) + X/* CDAlpha = c->cinfo->CDOrigin + */ X *CLift * *CLift / (pi * c->cinfo->aspectRatio); X CDAlpha += sin (c->curSpeedBrake) * c->cinfo->cSpeedBrake; X CDAlpha += sin (c->curFlap) * c->cinfo->cFlapDrag; X X if (fabs(c->beta) > c->cinfo->betaStall) X CN = c->cinfo->cnFactor * sin (c->beta); X else X CN = c->cinfo->cnSlope; X X CDBeta = c->cinfo->CDBOrigin + c->cinfo->CDBFactor * X cos (c->beta + c->cinfo->CDBPhase); X X *CDrag = sqrt (CDAlpha * CDAlpha + CDBeta * CDBeta); X X CY = c->cinfo->CYbeta * c->beta * fabs (cos (c->beta)); X X} X Xdouble heading (x) XVPoint *x; { X X double m; X X if (x->x == 0.0 && x->y == 0.0) X return 0.0; X X if ((m = atan2 (x->y, x->x)) < 0.0) X return (pi * 2.0 + m); X else X return m; X} X Xvoid euler (c) Xcraft *c; { X X register double i, j, k, m; X X/* X * Compute the heading ... X */ X X i = c->trihedral.m[0][0]; X j = c->trihedral.m[1][0]; X k = c->trihedral.m[2][0]; X X if (i == 0.0 && j == 0.0) X c->curHeading = 0.0; X else if ((m = atan2 (j, i)) < 0.0) X c->curHeading = pi * 2.0 + m; X else X c->curHeading = m; X X X/* X * and Pitch ... X */ X X c->curPitch = - asin (k); X X/* X * and Roll ... X */ X X c->curRoll = atan2 (c->trihedral.m[2][1], c->trihedral.m[2][2]); X X} X Xvoid craftToGround (c, p, g) Xcraft *c; XVPoint *p; XVPoint *g; { X X VTransform (p, &(c->trihedral), g); X X} X Xvoid calcGForces (c, f, w) Xcraft *c; XVPoint *f; Xdouble w; { X X VPoint t; X X t = *f; X t.z -= w; X X VTransform (&t, &(c->Itrihedral), &(c->G)); X c->G.x = - c->G.x / w; X c->G.y = - c->G.y / w; X c->G.z = - c->G.z / w; X} X Xvoid calcAlphaBeta (c, alpha, beta) Xcraft *c; Xdouble *alpha, *beta; { X X VPoint C; X double h; X X if (mag(c->Cg) > 0.0) { X VTransform (&(c->Cg), &(c->Itrihedral), &C); X *alpha = atan2 (C.z, C.x); X h = sqrt (C.z * C.z + C.x * C.x); X *beta = atan2 (C.y, h); X } X else { X *alpha = 0.0; X *beta = 0.0; X } X X} X X/* X * buildEulerMatrix : Build a transformation matrix based on the supplied X * euler angles. X */ X Xvoid buildEulerMatrix (roll, pitch, heading, m) Xdouble roll, pitch, heading; XVMatrix *m; { X X register double sinPhi, cosPhi, sinTheta, cosTheta, sinPsi, cosPsi; X X sinPhi = sin (roll); X cosPhi = cos (roll); X sinTheta = sin (pitch); X cosTheta = cos (pitch); X sinPsi = sin (heading); X cosPsi = cos (heading); X X m->m[0][0] = cosTheta * cosPsi; X m->m[0][1] = sinPhi * sinTheta * cosPsi - cosPhi * sinPsi; X m->m[0][2] = cosPhi * sinTheta * cosPsi + sinPhi * sinPsi; X m->m[1][0] = cosTheta * sinPsi; X m->m[1][1] = sinPhi * sinTheta * sinPsi + cosPhi * cosPsi; X m->m[1][2] = cosPhi * sinTheta * sinPsi - sinPhi * cosPsi; X m->m[2][0] = - sinTheta; X m->m[2][1] = sinPhi * cosTheta; X m->m[2][2] = cosPhi * cosTheta; X m->m[0][3] = m->m[1][3] = m->m[2][3] = 0.0; X m->m[3][0] = m->m[3][1] = m->m[3][2] = 0.0; X m->m[3][3] = 1.0; X X#ifdef notdef X VIdentMatrix (m); X if (roll != 0.0) X VRotate (m, XRotation, roll); X if (pitch != 0.0) X VRotate (m, YRotation, -pitch); X if (heading != 0.0) X VRotate (m, ZRotation, heading); X#endif X X} X Xint flightCalculations (c) Xcraft *c; { X X double q, CLift, CDrag; X double FLift, FDrag, FWeight, FSideForce; X double Vmag, D, angle; X double ar, ap, aq, cosR, sinR; X double deltaRoll, deltaPitch, deltaYaw; X double muStatic, muKinetic; X double ke, be; X double y, newy; X VPoint F, Fg, V, tmpPt, r; X VMatrix turn, mtx, mtx1; X int positionUpdated = 0; X X c->prevSg = c->Sg; X X c->rho = calcRho (-(c->Sg.z), &c->mach1); X calcAlphaBeta(c, &(c->alpha), &(c->beta)); X X/* X * A note about thrust: Normal thrust diminishes in proportion to the decrease in X * air density. Afterburners are not affectected in this way. The following formula X * approximates this effect. X */ X X if (c->fuel <= 0.0 || isFunctioning (c, SYS_ENGINE1) == 0) X c->curThrust = 0.0; X else X c->curThrust = calcThrust(c); X X Vmag = mag(c->Cg); X c->mach = Vmag / c->mach1; X calcCoefficients (c, &CLift, &CDrag); X X if (debug) X printf ("alpha = %g, beta = %g\nCL = %g, CD = %g\n", RADtoDEG(c->alpha), X RADtoDEG(c->beta), CLift, CDrag); X X X/* X * Compute the resultant force vector on the aircraft. X */ X X q = c->rho * c->cinfo->wingS * Vmag * Vmag * 0.5; X FLift = CLift * q; X FDrag = CDrag * q; X FSideForce = CY * q; X X if (debug) { X printf ("rho = %g, FLift = %g, FDrag = %g\n", c->rho, FLift, FDrag); X printf ("FThrust = %g\n", c->curThrust); X } X X F.x = c->curThrust - sin(c->alpha) * cos(c->beta) * FLift - X cos(c->alpha) * cos(c->beta) * FDrag; X F.y = sin(c->alpha) * sin(c->beta) * FLift - sin(c->alpha) * sin(c->beta) * X FDrag + FSideForce; X F.z = -cos(c->alpha) * cos(c->beta) * FLift - sin(c->alpha) * cos(c->beta) * X FDrag; X X/* X * Get ground friction coefficients X */ X X if (c->groundContact) X if (c->flags & FL_BRAKES) { X muStatic = c->cinfo->muBStatic; X muKinetic = c->cinfo->muBKinetic; X } X else { X muStatic = c->cinfo->muStatic; X muKinetic = c->cinfo->muKinetic; X } X X/* X * Now calculate changes in position (Sg) and velocity (Cg). X */ X X if (Vmag > c->cinfo->maxNWS || c->groundContact == 0) X c->flags &= ~FL_NWS; X else X c->flags |= FL_NWS; X X if (c->flags & FL_NWS) { X X c->curNWDef = c->Sa * c->cinfo->maxNWDef; X X if (c->curNWDef != 0.0) { X X r.x = c->cinfo->gearD2; X r.y = c->cinfo->gearD1 / tan(c->curNWDef); X r.z = 0.0; X angle = Vmag / r.y * deltaT; X X/* X * Nose wheel steering mode. X * Relocate the aircraft and its trihedral (this code assumes that the X * plane is rolling on a flat surface (i.e. z is constant). X */ X X tmpPt = r; X VTransform(&tmpPt, &(c->trihedral), &r); X X VIdentMatrix (&turn); X turn.m[0][3] = - c->Sg.x - r.x; X turn.m[1][3] = - c->Sg.y - r.y; X turn.m[2][3] = - c->Sg.z; X VRotate (&turn, ZRotation, angle); X turn.m[0][3] = turn.m[0][3] + c->Sg.x + r.x; X turn.m[1][3] = turn.m[1][3] + c->Sg.y + r.y; X turn.m[2][3] = turn.m[2][3] + c->Sg.z; X VTransform (&(c->Sg), &turn, &tmpPt); X c->Sg = tmpPt; X X VIdentMatrix (&turn); X VRotate (&turn, ZRotation, angle); X mtx = c->trihedral; X VMatrixMult (&mtx, &turn, &(c->trihedral)); X VTransform (&(c->Cg), &turn, &tmpPt); X c->Cg = tmpPt; X X transpose (&c->trihedral, &c->Itrihedral); X X euler (c); X positionUpdated = 1; X } X X craftToGround (c, &F, &Fg); X FWeight = c->cinfo->emptyWeight + c->fuel; X X if ((c->fuel -= fuelUsed(c) + c->leakRate * deltaT) <= 0.0) { X c->fuel = 0.0; X c->curThrust = 0.0; X c->throttle = 0; X } X X Fg.z += FWeight; X X/* X * Factor in ground friction for both static and moving planes. X */ X X if (c->Cg.x + c->Cg.y == 0.0 && sqrt (Fg.x*Fg.x + Fg.y*Fg.y) <= X muStatic * Fg.z) { X Fg.x = 0.0; X Fg.y = 0.0; X } X else { X X/* X * Okay, the plane is moving. Quantify the current kinetic energy of the X * moving craft and add the energy added this period by all forces EXCEPT X * ground friction (we'll name this "ke"). If the energy removed by the friction X * force is greater than ke, then the craft will stop sometime during this X * period -- we won't bother to calculate exactly where we stop; just zero out X * the x and y force and velocity components. X */ X X ke = 0.5 * FWeight / a * sqrt (c->Cg.x * c->Cg.x + X c->Cg.y * c->Cg.y); X ke += pow(sqrt(Fg.x * Fg.x + Fg.y * Fg.y), 2.0) * X halfDeltaTSquared * a / FWeight; X be = pow(Fg.z * muKinetic, 2.0) * halfDeltaTSquared * a / FWeight; X if (be >= ke) { X Fg.x = 0.0; X Fg.y = 0.0; X c->Cg.x = 0.0; X c->Cg.y = 0.0; X } X else { X X/* X * Getting to this point means that we're rolling along the ground (and not stopping) X * -- make sure our roll is zeroed, then cancel the local Y-component of our X * velocity vector (tires don't roll sideways) and then calculate the drag X * contributed by the rolling wheels. X */ X X c->curRoll = 0.0; X VTransform (&(c->Cg), &(c->Itrihedral), &V); X V.y = 0.0; X VTransform (&V, &(c->trihedral), &(c->Cg)); X D = Fg.z * muKinetic; X Vmag = mag (c->Cg); X if (Vmag > 0.0) { X Fg.x -= D * c->Cg.x / Vmag; X Fg.y -= D * c->Cg.y / Vmag; X Fg.z -= D * c->Cg.z / Vmag; X } X } X } X X X/* Nose wheel steering is only active when we cannot lift off -- cancel z */ X X Fg.z = 0.0; X calcGForces (c, &Fg, FWeight); X X } X else { X X/* X * Resolve moments X */ X X ap = fsign (c->p); X aq = fsign (c->q); X ar = fsign (c->r); X if (c->groundContact == 0) { X ap = (c->Sa * c->cinfo->effAileron * q - c->cinfo->LDamp * X ap * c->p * c->p * 0.5 ); X ap += (c->beta - c->Sr * c->cinfo->effRudder) * c->cinfo->CLbeta * q; X ap += c->damageCL * q; X ap /= c->cinfo->I.m[0][0]; X } X else X ap = 0.0; X#ifdef notdef X aq = (CM * q - c->cinfo->MDamp * aq * c->q * c->q * 0.5) / X c->cinfo->I.m[1][1]; X ar = (CN * q - c->cinfo->NDamp * ar * c->r * c->r * 0.5) / X c->cinfo->I.m[2][2]; X X deltaRoll = c->p * deltaT + ap * halfDeltaTSquared; X deltaPitch = c->q * deltaT + aq * halfDeltaTSquared; X deltaYaw = c->r * deltaT + ar * halfDeltaTSquared; X c->p = c->p + ap * deltaT; X c->q = c->q + aq * deltaT; X c->r = c->r + ar * deltaT; X#endif X X deltaRoll = c->p * deltaT + ap * halfDeltaTSquared; X c->p = c->p + ap * deltaT; X X y = c->alpha - c->Se * c->cinfo->effElevator; X twoOrder (CM * q / c->cinfo->I.m[1][1], X c->cinfo->MDamp, y, c->q, &newy, &(c->q)); X deltaPitch = newy - y; X X y = c->beta - c->Sr * c->cinfo->effRudder; X twoOrder (CN * q / c->cinfo->I.m[2][2], X c->cinfo->NDamp, y, c->r, &newy, &(c->r)); X deltaYaw = y - newy; X X if (debug) { X printf ("p = %g, q = %g, r = %g\n", c->p, c->q, c->r); X } X X/* X * Compute new transformation matrices X */ X X buildEulerMatrix (deltaRoll, deltaPitch, deltaYaw, &mtx); X VMatrixMult (&mtx, &c->trihedral, &mtx1); X c->trihedral = mtx1; X X transpose (&c->trihedral, &c->Itrihedral); X euler (c); X X craftToGround (c, &F, &Fg); X FWeight = c->cinfo->emptyWeight + c->fuel; X X if ((c->fuel -= fuelUsed(c) + c->leakRate * deltaT) <= 0.0) { X c->fuel = 0.0; X c->curThrust = 0.0; X } X X Fg.z += FWeight; X X/* X * If we are on the ground, level the wings and compute wheel drag forces. Wheel X * drag always acts in the opposite direction of the velocity vector. X */ X X if (c->groundContact) { X c->curRoll = 0.0; X buildEulerMatrix (c->curRoll, c->curPitch, X c->curHeading, &(c->trihedral)); X transpose (&c->trihedral, &c->Itrihedral); X X VTransform (&(c->Cg), &(c->Itrihedral), &V); X V.y = 0.0; X VTransform (&V, &(c->trihedral), &(c->Cg)); X D = Fg.z * muKinetic; X Vmag = mag (c->Cg); X Fg.x -= D * c->Cg.x / Vmag; X Fg.y -= D * c->Cg.y / Vmag; X Fg.z -= D * c->Cg.z / Vmag; X } X X if (debug) { X printf ("v = %g, Fg = { %g, %g, %g }\n", FPStoKTS(Vmag), X Fg.x, Fg.y, Fg.z); X printf ("F = { %g, %g, %g }\n", X F.x, F.y, F.z); X } X X X/* X * Are we on the ground without the prospect of gaining altitude? X * If so, cancel the vertical force component. X */ X X if (c->groundContact && Fg.z > 0.0) X Fg.z = 0.0; X X calcGForces (c, &Fg, FWeight); X X X } X X/* X * Update our position (in flight mode). X */ X X if (positionUpdated == 0) { X X c->Sg.x += c->Cg.x * deltaT + Fg.x / FWeight X * a * halfDeltaTSquared; X c->Sg.y += c->Cg.y * deltaT + Fg.y / FWeight X * a * halfDeltaTSquared; X c->Sg.z += c->Cg.z * deltaT + Fg.z / FWeight X * a * halfDeltaTSquared; X X } X X c->Cg.x += Fg.x / FWeight * a * deltaT; X c->Cg.y += Fg.y / FWeight * a * deltaT; X c->Cg.z += Fg.z / FWeight * a * deltaT; X X if (debug) { X printf ("Altitude = %g\n", -c->Sg.z); X printf ("Euler angles { %g, %g, %g }\n", RADtoDEG(c->curRoll), X RADtoDEG(c->curPitch), RADtoDEG(c->curHeading)); X printf ("Cg = { %g, %g, %g }\n", c->Cg.x, c->Cg.y, c->Cg.z); X printf ("Sg = { %g, %g, %g }\n", c->Sg.x, c->Sg.y, c->Sg.z); X X printf ("X = { %g, %g, %g }\n", c->trihedral.m[0][0], X c->trihedral.m[1][0], c->trihedral.m[2][0]); X printf ("Z = { %g, %g, %g }\n\n", c->trihedral.m[0][2], X c->trihedral.m[1][2], c->trihedral.m[2][2]); X } X X X/* X * Normalize the vertical position. If our altitude is now below our X * contact threshold, mark us as having "groundContact" and adjust the X * altitude. X */ X X if (c->Sg.z >= - c->cinfo->groundingPoint.z) { X c->groundContact = 1; X c->Sg.z = - c->cinfo->groundingPoint.z; X X/* X * If the vertical velocity component is too great, the plane has crashed ... X */ X X if (c->Cg.z > c->cinfo->crashC) X return 1; X else X c->Cg.z = 0.0; X X } X else { X c->groundContact = 0; X c->flags &= ~FL_NWS; X } X X X return 0; X} END_OF_FILE if test 16393 -ne `wc -c <'acm/fsim/pm.c'`; then echo shar: \"'acm/fsim/pm.c'\" unpacked with wrong size! fi # end of 'acm/fsim/pm.c' fi echo shar: End of archive 8 \(of 9\). cp /dev/null ark8isdone MISSING="" for I in 1 2 3 4 5 6 7 8 9 ; do if test ! -f ark${I}isdone ; then MISSING="${MISSING} ${I}" fi done if test "${MISSING}" = "" ; then echo You have unpacked all 9 archives. rm -f ark[1-9]isdone ark[1-9][0-9]isdone else echo You still need to unpack the following archives: echo " " ${MISSING} fi ## End of shell archive. exit 0 -- Riley Rainey Internet: riley@mips.com MIPS Computer Systems Phone: +1 214 770-7979 Dallas, Texas