diff --git a/pc/CMakeLists.txt b/pc/CMakeLists.txt index 79fbcdb..6c34c8a 100644 --- a/pc/CMakeLists.txt +++ b/pc/CMakeLists.txt @@ -9,7 +9,8 @@ find_package(SDL2 REQUIRED) set(HEADERS app.h sysfont.h - flight.h + iflight.h + fflight.h vrEmu6502.h a23d2bin.h a23d2.h @@ -20,7 +21,8 @@ list(TRANSFORM HEADERS PREPEND "${CMAKE_CURRENT_SOURCE_DIR}/include/") set(SOURCE main.c - flight.c + iflight.c + fflight.c vrEmu6502.c util.c ) diff --git a/pc/include/fflight.h b/pc/include/fflight.h new file mode 100644 index 0000000..eb0ddc1 --- /dev/null +++ b/pc/include/fflight.h @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2024 Scott Duensing, scott@kangaroopunch.com + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + +#ifndef FFLIGHT_H +#define FFLIGHT_H + + +#include +#include +#include +#include + + +#ifndef PLANE_HEIGHT +#define PLANE_HEIGHT 20 +#endif + +#ifndef byte +typedef unsigned char byte; +#endif + + +typedef struct fAirplaneS { + int8_t aileron; // -15 to 15 + int8_t elevator; // -15 to 15 + int8_t rudder; // -15 to 15 + int8_t throttle; // -15 to 15 + bool ignition; + bool engine; + int16_t rpm; + float hSpeed; + float vSpeed; + float deltaZ; + float efAOF; + float climbRate; + bool airborne; + bool stall; + bool brake; + int16_t x; + int16_t y; + int16_t z; + double pitch; // 0 to 255 + double yaw; // 0 to 255 + double roll; // 0 to 255 + // Flight model stuff below here. + float tmpX; + float tmpY; + float tmpZ; + float newX; + float newY; + float newZ; + float iSpeed; + float lSpeed; + float hAccel; + float lVeloc; + float gVeloc; + float AOA; + float torque; + float torque2; + uint16_t loopTime; // Milliseconds + // Used to be static. Need preserved. + double dPitch; + double dYaw; + double dRoll; + float collectX; + float collectY; + float collectZ; +} fAirplaneT; + + +extern fAirplaneT _fPlane; + + +void fResetAircraft(void); +void fUpdateAircraft(void); + + +#endif // FFLIGHT_H diff --git a/pc/include/flight.h b/pc/include/iflight.h similarity index 91% rename from pc/include/flight.h rename to pc/include/iflight.h index 55329e9..890d10c 100644 --- a/pc/include/flight.h +++ b/pc/include/iflight.h @@ -21,8 +21,8 @@ */ -#ifndef FLIGHT_H -#define FLIGHT_H +#ifndef IFLIGHT_H +#define IFLIGHT_H #include @@ -31,13 +31,17 @@ #include "fmath.h" +#ifndef PLANE_HEIGHT #define PLANE_HEIGHT 20 +#endif +#ifndef byte typedef unsigned char byte; +#endif -typedef struct airplaneS { +typedef struct iAirplaneS { int8_t aileron; // -15 to 15 int8_t elevator; // -15 to 15 int8_t rudder; // -15 to 15 @@ -82,14 +86,14 @@ typedef struct airplaneS { fixT FcollectX; fixT FcollectY; fixT FcollectZ; -} airplaneT; +} iAirplaneT; -extern airplaneT _plane; +extern iAirplaneT _iPlane; -void resetAircraft(void); -void updateAircraft(void); +void iResetAircraft(void); +void iUpdateAircraft(void); -#endif // FLIGHT_H +#endif // IFLIGHT_H diff --git a/pc/src/a23d2.c b/pc/src/a23d2.c index 4bc91fc..818404a 100644 --- a/pc/src/a23d2.c +++ b/pc/src/a23d2.c @@ -26,7 +26,8 @@ #include "vrEmu6502.h" #include "a23d2bin.h" #include "bitmap.h" -#include "flight.h" +#include "iflight.h" +#include "fflight.h" #include "mem.h" #include "a23d2.h" @@ -40,10 +41,11 @@ VrEmu6502 *_v6502 = NULL; uint16_t _drawlistInDatabase; -#define SCALE 1.25 // Scale factor for 256->320 - -#define SCREEN3DX 255 // Resolution of A2-3D2 renderer. -#define SCREEN3DY 233 // These get scaled up to full-screen. +#define SCALE 1.25 // Scale factor for 256->320 +#define SCREEN3DX 255 // Resolution of A2-3D2 renderer. +#define SCREEN3DY 233 // These get scaled up to full-screen. +#define SCREEN3DXH 128 // Half of SCREEN3DX. +#define SCREEN3DYH 117 // Half of SCREEN3DY. byte _scaleX[SCREEN3DX + 1]; @@ -68,10 +70,10 @@ void a23d2Draw(void) { // Line. case LIN2D: - x1 = (int8_t)_RAM[pointer++] + 128; - y1 = 255 - ((int8_t)_RAM[pointer++] + 128); - x2 = (int8_t)_RAM[pointer++] + 128; - y2 = 255 - ((int8_t)_RAM[pointer++] + 128); + x1 = (int8_t)_RAM[pointer++] + SCREEN3DXH; + y1 = SCREEN3DYH - (int8_t)_RAM[pointer++]; + x2 = (int8_t)_RAM[pointer++] + SCREEN3DXH; + y2 = SCREEN3DYH - (int8_t)_RAM[pointer++]; if ((x1 < 0) || (y1 < 0) || (x2 < 0) || (y2 < 0) || (x1 > SCREEN3DX) || (y1 > SCREEN3DY) || (x2 > SCREEN3DX) || (y2 > SCREEN3DY)) { printf("Point out of bounds! %d %d %d %d\n", x1, y1, x2, y2); @@ -83,8 +85,8 @@ void a23d2Draw(void) { // Point. case PNT2D: - x1 = (int8_t)_RAM[pointer++] + 128; - y1 = 255 - ((int8_t)_RAM[pointer++] + 128); + x1 = (int8_t)_RAM[pointer++] + SCREEN3DXH; + y1 = SCREEN3DYH - (int8_t)_RAM[pointer++]; bitmapPutPixelIOSet(x1 + _scaleX[x1], _scaleY[y1]); continue; @@ -124,7 +126,7 @@ void a23d2Init(void) { // Initialize A2-3D2 so we can use the "fast entry point" (NXTPT) when rendering. bytes = A23D2_TEST_DATABASE; - _RAM[bytes++] = SCRSZ; // Screen size. 256x240. Center is 0,0. We're going to scale this up. + _RAM[bytes++] = SCRSZ; // Screen size. 255x233. Center is 0,0. We're going to scale this up. _RAM[bytes++] = SCREEN3DX; // Scales up to 320. _RAM[bytes++] = SCREEN3DY; // Scales up to 193 (leaves 46 pixels for a control panel). _RAM[bytes++] = 0; diff --git a/pc/src/fflight.c b/pc/src/fflight.c new file mode 100644 index 0000000..a573391 --- /dev/null +++ b/pc/src/fflight.c @@ -0,0 +1,319 @@ +/* + * Copyright (c) 2024 Scott Duensing, scott@kangaroopunch.com + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + +#include "fflight.h" +#include "a23d2.h" + + +fAirplaneT _fPlane; + + +#define SEGMENT_MATH + + +#define PI 3.1415 + +#define Rads(d) (((d) < 0 ? (d) + 360 : (d)) * (PI / 180)) +#define Degs(r) ((r) * (180 / PI)) + +// https://www.reddit.com/r/programming/comments/3e7ghi/discrete_arctan_in_6502 +#define ATAN_SPLINE_C0 (double)(-0.14380550980765507115) // 3pi/4 - 5/2 +#define ATAN_SPLINE_C1 (double)(-0.07079632679489661923) // 3/2 - pi/2 +double ourAtan(double x){ + if (x >= 0) { + if ( x<= 1) { + return x + (ATAN_SPLINE_C0 + ATAN_SPLINE_C1 * x) * x * x; + } else { + x = 1 / x; + return PI / 2 - (x + (ATAN_SPLINE_C0 + ATAN_SPLINE_C1 * x) * x * x); + } + } else { + if (x >= -1) { + return x - (ATAN_SPLINE_C0 - ATAN_SPLINE_C1 * x) * x * x; + } else { + x = -1 / x; + return (x + (ATAN_SPLINE_C0 + ATAN_SPLINE_C1 * x) * x * x) - PI / 2; + } + } +} + + +float cosD(float d) { + return cos(Rads(d)); + /* + // Map 0-359 into 0-255. + _tdata = (int)d % 359; + a23d2Cos(); + return _trig; + */ +} + + +float sinD(float d) { + return sin(Rads(d)); + /* + // Map 0-359 into 0-255. + _tdata = (int)d % 359; + a23d2Sin(); + return _trig; + */ +} + + +#define SEGMENT_FLIGHT_MODEL_1 + + +// This flight model is basically the one from "Build Your Own Flight Sim +// in C++" by Michael Radtke and Chris Lampton. +void fLightPlane(void) { + // Flight model. Power Dynamics. + if (_fPlane.ignition) { + // Start engine. + if (!_fPlane.engine) _fPlane.engine = true; + // Adjust RPM. + if (_fPlane.rpm < (375 + (_fPlane.throttle * 117))) _fPlane.rpm += _fPlane.loopTime * 0.5; + if (_fPlane.rpm > (375 + (_fPlane.throttle * 117))) _fPlane.rpm -= _fPlane.loopTime * 0.5; + } else { + // Stop engine. + if (_fPlane.engine) _fPlane.engine = false; + // Run down engine. + if (_fPlane.rpm > 0) _fPlane.rpm -= (int16_t)(_fPlane.loopTime / 2); + if (_fPlane.rpm < 0) _fPlane.rpm = 0; + } + + // Flight model. Flight Dynamics. + // Calculate speed from RPM. + _fPlane.iSpeed = _fPlane.rpm / 17.5; + // Modify speed by pitch. + _fPlane.iSpeed += (_fPlane.pitch * 1.5); + // Horizontal acceleration - thrust. + _fPlane.hAccel = ((_fPlane.rpm * (_fPlane.iSpeed - _fPlane.hSpeed)) / 10000); + _fPlane.hAccel /= 1000; + _fPlane.hAccel *= _fPlane.loopTime; + if (_fPlane.brake && !_fPlane.airborne) { + // Handle brakes. + if (_fPlane.hSpeed > 0) { + _fPlane.hSpeed -= 1; + } else { + _fPlane.hSpeed = 0; + } + } else { + // Accelerate normally. + _fPlane.hSpeed += _fPlane.hAccel; + } + // Force speed to range -1..1. + _fPlane.lSpeed = (_fPlane.hSpeed / 65) - 1; + if (_fPlane.lSpeed > 1) _fPlane.lSpeed = 1; + // Lift curve. + _fPlane.lVeloc = Degs(ourAtan(_fPlane.lSpeed)); + // Force lift to range 0..90. + _fPlane.lVeloc += 45; + // Shift range to 0..-17. + _fPlane.lVeloc /= 5.29; + // Multiply by pitch modifier. + _fPlane.lVeloc *= (-(_fPlane.pitch * 0.157) + 1); + // Time slice. + _fPlane.lVeloc /= 1000; + _fPlane.lVeloc *= _fPlane.loopTime; + // Gravity. + _fPlane.gVeloc = _fPlane.loopTime * (-16.0 / 1000); // -16.0 is ft/sec for gravity. + // Sum vertical velocity. + _fPlane.vSpeed = _fPlane.gVeloc + _fPlane.lVeloc; + // No vertical speed if we're on the ground. + if ((!_fPlane.airborne) && (_fPlane.vSpeed < 0)) _fPlane.vSpeed = 0; + // Save climb rate in ft/min. + _fPlane.climbRate = _fPlane.vSpeed / _fPlane.loopTime; + _fPlane.climbRate *= 60000L; + // Expand to ft/hr. + _fPlane.deltaZ = _fPlane.hSpeed * 5280; + // Get ft/ms. + _fPlane.deltaZ /= 3600000L; + _fPlane.deltaZ *= _fPlane.loopTime; + // Find effective angle of flight. + if (_fPlane.deltaZ) { + _fPlane.efAOF = -(ourAtan(_fPlane.vSpeed / _fPlane.deltaZ)); + } else { + _fPlane.efAOF = -(ourAtan(_fPlane.vSpeed)); + } + // Convert to degrees. + _fPlane.AOA = Degs(_fPlane.efAOF); + // Handle stalling. + if (((_fPlane.pitch < _fPlane.AOA) && (_fPlane.AOA < 0)) && (_fPlane.hSpeed < 40)) { + if ((_fPlane.pitch - _fPlane.AOA) < -20) _fPlane.stall = true; + } + if (_fPlane.stall) { + if (_fPlane.pitch > 30) { + _fPlane.stall = false; + } else { + _fPlane.pitch++; + } + } +} + + +#define SEGMENT_FLIGHT_MODEL_2 + + +void fLightPlane2(void) { + // Flight model. Inertial Damping. + if (_fPlane.dPitch) { + _fPlane.dPitch -= _fPlane.dPitch / 10; + if (((_fPlane.dPitch > 0) && (_fPlane.dPitch < 0.01)) || ((_fPlane.dPitch < 0) && (_fPlane.dPitch > -0.01))) _fPlane.dPitch = 0; + } + if (_fPlane.dYaw) { + _fPlane.dYaw -= _fPlane.dYaw / 10; + if (((_fPlane.dYaw > 0) && (_fPlane.dYaw < 0.01)) || ((_fPlane.dYaw < 0) && (_fPlane.dYaw > -0.01))) _fPlane.dYaw = 0; + } + if (_fPlane.dRoll) { + _fPlane.dRoll -= _fPlane.dRoll / 10; + if (((_fPlane.dRoll > 0) && (_fPlane.dRoll < 0.01)) || ((_fPlane.dRoll < 0) && (_fPlane.dRoll > -0.01))) _fPlane.dRoll = 0; + } + + // Flight model. Rate of Change. + if (_fPlane.airborne) { + if (_fPlane.aileron != 0) { + _fPlane.torque = ((_fPlane.hSpeed * _fPlane.aileron) / 10000); + if (_fPlane.dRoll != (_fPlane.torque * _fPlane.loopTime)) _fPlane.dRoll += _fPlane.torque * 6; + } + } + if (_fPlane.elevator != 0) { + _fPlane.torque = ((_fPlane.hSpeed * _fPlane.elevator) / 10000); + if ((!_fPlane.airborne) && (_fPlane.torque > 0)) _fPlane.torque = 0; //***FIX*** This is dumb. + if (_fPlane.dPitch != (_fPlane.torque * _fPlane.loopTime)) _fPlane.dPitch += _fPlane.torque * 1.5; + } + if (_fPlane.hSpeed) { + _fPlane.torque = 0.0; + if (_fPlane.rudder != 0) _fPlane.torque = -((_fPlane.hSpeed * _fPlane.rudder) / 10000); + _fPlane.torque2 = 0.0; + if ((_fPlane.roll > 0) && (_fPlane.roll <= 90)) { //***FIX*** This is dumb. + _fPlane.torque2 = _fPlane.roll * 0.00050; + } else { + if ((_fPlane.roll < 0) && (_fPlane.roll >= -90)) { + _fPlane.torque2 = _fPlane.roll * 0.00050; + } + } + _fPlane.torque += _fPlane.torque2; + if (_fPlane.dYaw != (_fPlane.torque * _fPlane.loopTime)) _fPlane.dYaw += _fPlane.torque * 1.5; + } + + // Flight model. Apply Rotations. + // Transform pitch into components of yaw and pitch based on roll. + _fPlane.roll += _fPlane.dRoll; + _fPlane.yaw += _fPlane.dYaw; + _fPlane.pitch += (_fPlane.dPitch * cosD(_fPlane.roll)); + _fPlane.yaw += -(_fPlane.dPitch * sinD(_fPlane.roll)); + if (_fPlane.roll > 180) { + _fPlane.roll = -180 + (_fPlane.roll - 180); + } else { + if (_fPlane.roll < -180) _fPlane.roll = 180 + (_fPlane.roll + 180); + } + if (_fPlane.yaw > 180) { + _fPlane.yaw = -180 + (_fPlane.yaw - 180); + } else { + if (_fPlane.yaw < -180) _fPlane.yaw = 180 + (_fPlane.yaw + 180); + } + // Handle special case when aircraft pitch passes vertical. + if ((_fPlane.pitch > 90) || (_fPlane.pitch < -90)) { + if (_fPlane.roll >= 0) { + _fPlane.roll -= 180; + } else { + if (_fPlane.roll < 0) _fPlane.roll += 180; + } + if (_fPlane.yaw >= 0) { + _fPlane.yaw -= 180; + } else { + if (_fPlane.yaw < 0) _fPlane.yaw += 180; + } + if (_fPlane.pitch > 0) { + _fPlane.pitch = (180 - _fPlane.pitch); + } else { + if (_fPlane.pitch < 0) _fPlane.pitch = (-180 - _fPlane.pitch); + } + } + // Dampen everything out to 0 if they get close enough. + if ((_fPlane.pitch > -0.5) && (_fPlane.pitch < 0.5)) _fPlane.pitch = 0; + if ((_fPlane.roll > -0.5) && (_fPlane.roll < 0.5)) _fPlane.roll = 0; + if ((_fPlane.yaw > -0.5) && (_fPlane.yaw < 0.5)) _fPlane.yaw = 0; +} + + +#define SEGMENT_FLIGHT_MODEL_3 + + +void fMoveAircraft(void) { + // Calculate new aircraft position. Each coordinate is 1 foot in 3D space. + _fPlane.tmpX = 0; + _fPlane.tmpY = 0; + _fPlane.tmpZ = _fPlane.deltaZ; + + // Order of these points is significant. + // Rotate in X. + _fPlane.newY = -(_fPlane.tmpZ * sinD(_fPlane.AOA)); + _fPlane.newZ = (_fPlane.tmpZ * cosD(_fPlane.AOA)); + _fPlane.tmpY = _fPlane.newY; + _fPlane.tmpZ = _fPlane.newZ; + // Rotate in Y. + _fPlane.newX = (_fPlane.tmpZ * sinD(_fPlane.yaw)); + _fPlane.newZ = (_fPlane.tmpZ * cosD(_fPlane.yaw)); + + // Translate rotated point back to where it should be + // relative to it's last position. + _fPlane.collectX += _fPlane.newX; + if ((_fPlane.collectX > 1) || (_fPlane.collectX < -1)) { + _fPlane.x -= _fPlane.collectX; + _fPlane.collectX = 0; + } + _fPlane.collectY += _fPlane.newY; + if ((_fPlane.collectY > 1) || (_fPlane.collectY < -1)) { + _fPlane.y -= _fPlane.collectY; + _fPlane.collectY = 0; + } + _fPlane.collectZ += _fPlane.newZ; + if ((_fPlane.collectZ > 1) || (_fPlane.collectZ < -1)) { + _fPlane.z += _fPlane.collectZ; + _fPlane.collectZ = 0; + } + + // Are we flying? + if ((!_fPlane.airborne) && (_fPlane.y != 0)) _fPlane.airborne = true; +} + + +void fResetAircraft(void) { + // Initialize airplane. + memset(&_fPlane, 0, sizeof(fAirplaneT)); + + _fPlane.brake = true; + _fPlane.loopTime = 40; +} + + +#define SEGMENT_MAIN + + +void fUpdateAircraft(void) { + // Do the actual flying! + fLightPlane(); + fLightPlane2(); + fMoveAircraft(); +} diff --git a/pc/src/flight.c b/pc/src/flight.c deleted file mode 100644 index 2878afd..0000000 --- a/pc/src/flight.c +++ /dev/null @@ -1,331 +0,0 @@ -/* - * Copyright (c) 2024 Scott Duensing, scott@kangaroopunch.com - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - - -#include "a23d2.h" -#include "mem.h" - -#include "flight.h" - - -// Extern. -airplaneT _plane; - - -#define SEGMENT_FLIGHT_MODEL_1 - - -// This flight model is basically the one from "Build Your Own Flight Sim -// in C++" by Michael Radtke and Chris Lampton. -void lightPlane(void) { - // Flight model. Power Dynamics. - if (_plane.ignition) { - // Start engine. - if (!_plane.engine) _plane.engine = true; - // Adjust RPM. - if (_plane.rpm < (375 + (_plane.throttle * 117))) _plane.rpm += _plane.loopTime * 0.5; - if (_plane.rpm > (375 + (_plane.throttle * 117))) _plane.rpm -= _plane.loopTime * 0.5; - } else { - // Stop engine. - if (_plane.engine) _plane.engine = false; - // Run down engine. - if (_plane.rpm > 0) _plane.rpm -= (int16_t)(_plane.loopTime / 2); - if (_plane.rpm < 0) _plane.rpm = 0; - } - - // Flight model. Flight Dynamics. - // Calculate speed from RPM. - //_plane.iSpeed = _plane.rpm / 17.5; - _plane.FiSpeed = fixDiv(iToFix(_plane.rpm), fToFix(17.5)); - // Modify speed by pitch. - //_plane.iSpeed += (_plane.pitch * 1.5); - _plane.FiSpeed = fixAdd(_plane.FiSpeed, fixMul(_plane.Fpitch, fToFix(1.5))); - // Horizontal acceleration - thrust. - //_plane.hAccel = ((_plane.rpm * (_plane.iSpeed - _plane.hSpeed)) / 10000); - //_plane.hAccel /= 1000; - //_plane.hAccel *= _plane.loopTime; - _plane.FtmpX = fixSub(_plane.FiSpeed, _plane.FhSpeed); - _plane.FtmpX = fixMul(iToFix(_plane.rpm), _plane.FtmpX); - _plane.FhAccel = fixDiv(_plane.FtmpX, iToFix(10000)); - _plane.FhAccel = fixDiv(_plane.FhAccel, iToFix(1000)); - _plane.FhAccel = fixMul(_plane.FhAccel, iToFix(_plane.loopTime)); - if (_plane.brake && !_plane.airborne) { - // Handle brakes. - if (_plane.FhSpeed > 0) { - //_plane.hSpeed -= 1; - _plane.FhSpeed = fixSub(_plane.FhSpeed, iToFix(1)); - } else { - //_plane.hSpeed = 0; - _plane.FhSpeed = iToFix(0); - } - } else { - // Accelerate normally. - //_plane.hSpeed += _plane.hAccel; - _plane.FhSpeed = fixAdd(_plane.FhSpeed, _plane.FhAccel); - } - // Force speed to range -1..1. - //_plane.lSpeed = (_plane.hSpeed / 65) - 1; - _plane.FlSpeed = fixSub(fixDiv(_plane.FhSpeed, iToFix(65)), iToFix(1)); - //if (_plane.lSpeed > 1) _plane.lSpeed = 1; - if (_plane.FlSpeed > iToFix(1)) _plane.FlSpeed = iToFix(1); - // Lift curve. - //_plane.lVeloc = Degs(ourAtan(_plane.lSpeed)); - _plane.FlVeloc = fixDegs(fixAtan(_plane.FlSpeed)); - // Force lift to range 0..90. - //_plane.lVeloc += 45; - _plane.FlVeloc = fixAdd(_plane.FlVeloc, iToFix(45)); - // Shift range to 0..-17. - //_plane.lVeloc /= 5.29; - _plane.FlVeloc = fixDiv(_plane.FlVeloc, fToFix(5.29)); - // Multiply by pitch modifier. - //_plane.lVeloc *= (-(_plane.pitch * 0.157) + 1); - _plane.FlVeloc = fixAdd(_plane.FlVeloc, fixAdd(-(fixMul(_plane.Fpitch, fToFix(0.157))), iToFix(1))); - // Time slice. - //_plane.lVeloc /= 1000; - //_plane.lVeloc *= _plane.loopTime; - _plane.FlVeloc = fixDiv(_plane.FlVeloc, iToFix(1000)); - _plane.FlVeloc = fixMul(_plane.FlVeloc, iToFix(_plane.loopTime)); - // Gravity. - //_plane.gVeloc = _plane.loopTime * (-16.0 / 1000); // -16.0 is ft/sec for gravity. - _plane.FgVeloc = fixMul(iToFix(_plane.loopTime), fToFix(-0.016)); // -16.0 is ft/sec for gravity. - // Sum vertical velocity. - //_plane.vSpeed = _plane.gVeloc + _plane.lVeloc; - _plane.FvSpeed = fixAdd(_plane.FgVeloc, _plane.FlVeloc); - // No vertical speed if we're on the ground. - if ((!_plane.airborne) && (_plane.FvSpeed < 0)) _plane.FvSpeed = 0; - // Save climb rate in ft/min. - //_plane.climbRate = _plane.vSpeed / _plane.loopTime; - //_plane.climbRate *= 60000L; - _plane.FclimbRate = fixDiv(_plane.FvSpeed, iToFix(_plane.loopTime)); - _plane.FclimbRate = fixMul(_plane.FclimbRate, iToFix(60000L)); //***TODO*** This long could be an issue. - // Expand to ft/hr. - //_plane.deltaZ = _plane.hSpeed * 5280; - _plane.FdeltaZ = fixMul(_plane.FhSpeed, iToFix(5280)); - // Get ft/ms. - //_plane.deltaZ /= 3600000L; - //_plane.deltaZ *= _plane.loopTime; - _plane.FdeltaZ = fixDiv(_plane.FdeltaZ, iToFix(3600000L)); //***TODO*** This long could be an issue. - _plane.FdeltaZ = fixMul(_plane.FdeltaZ, iToFix(_plane.loopTime)); - // Find effective angle of flight. - if (_plane.FdeltaZ) { - //_plane.efAOF = -(ourAtan(_plane.vSpeed / _plane.deltaZ)); - _plane.FefAOF = -(fixAtan(fixDiv(_plane.FvSpeed, _plane.FdeltaZ))); - } else { - //_plane.efAOF = -(ourAtan(_plane.vSpeed)); - _plane.FefAOF = -(fixAtan(_plane.FvSpeed)); - } - // Convert to degrees. - //_plane.AOA = Degs(_plane.efAOF); - _plane.FAOA = fixDegs(_plane.FefAOF); - // Handle stalling. - //if (((_plane.pitch < _plane.AOA) && (_plane.AOA < 0)) && (_plane.hSpeed < 40)) { - if (((_plane.Fpitch < _plane.FAOA) && (_plane.FAOA < iToFix(0))) && (_plane.FhSpeed < iToFix(40))) { - //if ((_plane.pitch - _plane.AOA) < -20) _plane.stall = true; - if (fixSub(_plane.Fpitch, _plane.FAOA) < iToFix(-20)) _plane.stall = true; - } - if (_plane.stall) { - //if (_plane.pitch > 30) { - if (_plane.Fpitch > iToFix(30)) { - _plane.stall = false; - } else { - //_plane.pitch++; - _plane.Fpitch = fixAdd(_plane.Fpitch, iToFix(1)); - } - } -} - - -#define SEGMENT_FLIGHT_MODEL_2 - - -void lightPlane2(void) { - // Flight model. Inertial Damping. - if (_plane.FdPitch != iToFix(0)) { - //_plane.dPitch -= _plane.dPitch / 10; - _plane.FdPitch = fixSub(_plane.FdPitch, fixDiv(_plane.FdPitch, iToFix(10))); - //if (((_plane.dPitch > 0) && (_plane.dPitch < 0.01)) || ((_plane.dPitch < 0) && (_plane.dPitch > -0.01))) _plane.dPitch = 0; - if (((_plane.FdPitch > iToFix(0)) && (_plane.FdPitch < fToFix(0.01))) || ((_plane.FdPitch < iToFix(0)) && (_plane.FdPitch > fToFix(-0.01)))) _plane.FdPitch = iToFix(0); - } - if (_plane.FdYaw != iToFix(0)) { - //_plane.dYaw -= _plane.dYaw / 10; - _plane.FdYaw = fixSub(_plane.FdYaw, fixDiv(_plane.FdYaw, iToFix(10))); - //if (((_plane.dYaw > 0) && (_plane.dYaw < 0.01)) || ((_plane.dYaw < 0) && (_plane.dYaw > -0.01))) _plane.dYaw = 0; - if (((_plane.FdYaw > iToFix(0)) && (_plane.FdYaw < fToFix(0.01))) || ((_plane.FdYaw < iToFix(0)) && (_plane.FdYaw > fToFix(-0.01)))) _plane.FdYaw = iToFix(0); - } - if (_plane.FdRoll != iToFix(0)) { - //_plane.dRoll -= _plane.dRoll / 10; - _plane.FdRoll = fixSub(_plane.FdRoll, fixDiv(_plane.FdRoll, iToFix(10))); - //if (((_plane.dRoll > 0) && (_plane.dRoll < 0.01)) || ((_plane.dRoll < 0) && (_plane.dRoll > -0.01))) _plane.dRoll = 0; - if (((_plane.FdRoll > iToFix(0)) && (_plane.FdRoll < fToFix(0.01))) || ((_plane.FdRoll < iToFix(0)) && (_plane.FdRoll > fToFix(-0.01)))) _plane.FdRoll = iToFix(0); - } - - // Flight model. Rate of Change. - if (_plane.airborne) { - if (_plane.aileron != 0) { - //_plane.torque = ((_plane.hSpeed * _plane.aileron) / 10000); - _plane.Ftorque = fixDiv(fixAdd(_plane.FhSpeed, iToFix(_plane.aileron)), iToFix(10000)); - //if (_plane.dRoll != (_plane.torque * _plane.loopTime)) _plane.dRoll += _plane.torque * 6; - if (_plane.FdRoll != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdRoll = fixAdd(_plane.FdRoll, fixMul(_plane.Ftorque, iToFix(6))); - } - } - if (_plane.elevator != 0) { - //_plane.torque = ((_plane.hSpeed * _plane.elevator) / 10000); - _plane.Ftorque = fixDiv(fixAdd(_plane.FhSpeed, iToFix(_plane.elevator)), iToFix(10000)); - //if ((!_plane.airborne) && (_plane.torque > 0)) _plane.torque = 0; //***FIX*** This is dumb. - if ((!_plane.airborne) && (_plane.Ftorque > iToFix(0))) _plane.Ftorque = iToFix(0); //***FIX*** This is dumb. - //if (_plane.dPitch != (_plane.torque * _plane.loopTime)) _plane.dPitch += _plane.torque * 1.5; - if (_plane.FdPitch != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdPitch = fixAdd(_plane.FdRoll, fixMul(_plane.Ftorque, fToFix(1.5))); - } - if (_plane.FhSpeed != iToFix(0)) { - _plane.Ftorque = iToFix(0); - //if (_plane.rudder != 0) _plane.torque = -((_plane.hSpeed * _plane.rudder) / 10000); - if (_plane.rudder != 0) _plane.Ftorque = -(fixDiv(fixMul(_plane.FhSpeed, iToFix(_plane.rudder)), iToFix(10000))); - _plane.Ftorque2 = iToFix(0); - if ((_plane.Froll > iToFix(0)) && (_plane.Froll <= iToFix(90))) { //***FIX*** This is dumb. - //_plane.torque2 = _plane.roll * 0.00050; - _plane.Ftorque2 = fixMul(_plane.Froll, fToFix(0.00050)); - } else { - if ((_plane.Froll < iToFix(0)) && (_plane.Froll >= iToFix(-90))) { - //_plane.torque2 = _plane.roll * 0.00050; - _plane.Ftorque2 = fixMul(_plane.Froll, fToFix(0.00050)); - } - } - //_plane.torque += _plane.torque2; - _plane.Ftorque = fixAdd(_plane.Ftorque, _plane.Ftorque2); - //if (_plane.dYaw != (_plane.torque * _plane.loopTime)) _plane.dYaw += _plane.torque * 1.5; - if (_plane.FdYaw != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdYaw = fixAdd(_plane.FdYaw, fixMul(_plane.Ftorque, fToFix(1.5))); - } - - // Flight model. Apply Rotations. - // Transform pitch into components of yaw and pitch based on roll. - _plane.Froll = fixAdd(_plane.Froll, _plane.FdRoll); - _plane.Fyaw = fixAdd(_plane.Fyaw, _plane.FdYaw); - //_plane.pitch += (_plane.dPitch * cosD(_plane.roll)); - _plane.Fpitch = fixAdd(_plane.Fpitch, fixMul(_plane.FdPitch, fixCosD(_plane.Froll))); - //_plane.yaw += -(_plane.dPitch * sinD(_plane.roll)); - _plane.Fyaw = fixAdd(_plane.Fyaw, -fixMul(_plane.FdPitch, fixSinD(_plane.Froll))); - if (_plane.Froll > iToFix(180)) { - //_plane.roll = -180 + (_plane.roll - 180); - _plane.Froll = fixAdd(iToFix(-180), fixSub(_plane.Froll, iToFix(180))); - } else { - //if (_plane.roll < -180) _plane.roll = 180 + (_plane.roll + 180); - if (_plane.Froll < iToFix(-180)) _plane.Froll = fixAdd(iToFix(180), fixAdd(_plane.Froll, iToFix(180))); - } - if (_plane.Fyaw > iToFix(180)) { - //_plane.yaw = -180 + (_plane.yaw - 180); - _plane.Fyaw = fixAdd(iToFix(-180), fixSub(_plane.Fyaw, iToFix(180))); - } else { - //if (_plane.yaw < -180) _plane.yaw = 180 + (_plane.yaw + 180); - if (_plane.Fyaw < iToFix(-180)) _plane.Fyaw = fixAdd(iToFix(180), fixAdd(_plane.Fyaw, iToFix(180))); - } - // Handle special case when aircraft pitch passes vertical. - if ((_plane.Fpitch > iToFix(90)) || (_plane.Fpitch < iToFix(-90))) { - if (_plane.Froll >= iToFix(0)) { - _plane.Froll = fixSub(_plane.Froll, iToFix(180)); - } else { - if (_plane.Froll < iToFix(0)) _plane.Froll = fixAdd(_plane.Froll, iToFix(180)); - } - if (_plane.Fyaw >= iToFix(0)) { - _plane.Fyaw = fixSub(_plane.Fyaw, iToFix(180)); - } else { - if (_plane.Fyaw < iToFix(0)) _plane.Fyaw = fixAdd(_plane.Fyaw, iToFix(180)); - } - if (_plane.Fpitch > iToFix(0)) { - _plane.Fpitch = fixSub(iToFix(180), _plane.Fpitch); - } else { - if (_plane.Fpitch < iToFix(0)) _plane.Fpitch = fixSub(iToFix(-180), _plane.Fpitch); - } - } - // Dampen everything out to 0 if they get close enough. - if ((_plane.Fpitch > fToFix(-0.5)) && (_plane.Fpitch < fToFix(0.5))) _plane.Fpitch = iToFix(0); - if ((_plane.Froll > fToFix(-0.5)) && (_plane.Froll < fToFix(0.5))) _plane.Froll = iToFix(0); - if ((_plane.Fyaw > fToFix(-0.5)) && (_plane.Fyaw < fToFix(0.5))) _plane.Fyaw = iToFix(0); -} - - -#define SEGMENT_FLIGHT_MODEL_3 - - -void moveAircraft(void) { - // Calculate new aircraft position. Each coordinate is 1 foot in 3D space. - _plane.FtmpX = iToFix(0); - _plane.FtmpY = iToFix(0); - _plane.FtmpZ = _plane.FdeltaZ; - - // Order of these points is significant. - // Rotate in X. - //_plane.newY = -(_plane.tmpZ * sinD(_plane.AOA)); - _plane.FnewY = -fixMul(_plane.FtmpZ, fixSinD(_plane.FAOA)); - //_plane.newZ = (_plane.tmpZ * cosD(_plane.AOA)); - _plane.FnewZ = fixMul(_plane.FtmpZ, fixCosD(_plane.FAOA)); - _plane.FtmpY = _plane.FnewY; - _plane.FtmpZ = _plane.FnewZ; - // Rotate in Y. - //_plane.newX = (_plane.tmpZ * sinD(_plane.yaw)); - _plane.FnewX = fixMul(_plane.FtmpZ, fixSinD(_plane.Fyaw)); - //_plane.newZ = (_plane.tmpZ * cosD(_plane.yaw)); - _plane.FnewZ = fixMul(_plane.FtmpZ, fixCosD(_plane.Fyaw)); - - // Translate rotated point back to where it should be - // relative to it's last position. - _plane.FcollectX = fixAdd(_plane.FcollectX, _plane.FnewX); - if ((_plane.FcollectX > iToFix(1)) || (_plane.FcollectX < iToFix(-1))) { - _plane.x -= fixToI(_plane.FcollectX); - _plane.FcollectX = iToFix(0); - } - _plane.FcollectY = fixAdd(_plane.FcollectY, _plane.FnewY); - if ((_plane.FcollectY > iToFix(1)) || (_plane.FcollectY < iToFix(-1))) { - _plane.y -= fixToI(_plane.FcollectY); - _plane.FcollectY = iToFix(0); - } - _plane.FcollectZ = fixAdd(_plane.FcollectZ, _plane.FnewZ); - if ((_plane.FcollectZ > iToFix(1)) || (_plane.FcollectZ < iToFix(-1))) { - _plane.z += fixToI(_plane.FcollectZ); - _plane.FcollectZ = iToFix(0); - } - - // Are we flying? - if ((!_plane.airborne) && (_plane.y != 0)) _plane.airborne = true; -} - - -#define SEGMENT_SETUP - - -void resetAircraft(void) { - // Initialize airplane. - scdMemSet((byte *)&_plane, 0, sizeof(airplaneT)); - - _plane.brake = true; - _plane.loopTime = 40; -} - - -#define SEGMENT_MAIN - - -void updateAircraft(void) { - // Do the actual flying! - lightPlane(); - lightPlane2(); - moveAircraft(); -} diff --git a/pc/src/iflight.c b/pc/src/iflight.c new file mode 100644 index 0000000..71f608a --- /dev/null +++ b/pc/src/iflight.c @@ -0,0 +1,331 @@ +/* + * Copyright (c) 2024 Scott Duensing, scott@kangaroopunch.com + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + +#include "a23d2.h" +#include "mem.h" + +#include "iflight.h" + + +// Extern. +iAirplaneT _iPlane; + + +#define SEGMENT_FLIGHT_MODEL_1 + + +// This flight model is basically the one from "Build Your Own Flight Sim +// in C++" by Michael Radtke and Chris Lampton. +void iLightPlane(void) { + // Flight model. Power Dynamics. + if (_iPlane.ignition) { + // Start engine. + if (!_iPlane.engine) _iPlane.engine = true; + // Adjust RPM. + if (_iPlane.rpm < (375 + (_iPlane.throttle * 117))) _iPlane.rpm += _iPlane.loopTime * 0.5; + if (_iPlane.rpm > (375 + (_iPlane.throttle * 117))) _iPlane.rpm -= _iPlane.loopTime * 0.5; + } else { + // Stop engine. + if (_iPlane.engine) _iPlane.engine = false; + // Run down engine. + if (_iPlane.rpm > 0) _iPlane.rpm -= (int16_t)(_iPlane.loopTime / 2); + if (_iPlane.rpm < 0) _iPlane.rpm = 0; + } + + // Flight model. Flight Dynamics. + // Calculate speed from RPM. + //_iPlane.iSpeed = _iPlane.rpm / 17.5; + _iPlane.FiSpeed = fixDiv(iToFix(_iPlane.rpm), fToFix(17.5)); + // Modify speed by pitch. + //_iPlane.iSpeed += (_iPlane.pitch * 1.5); + _iPlane.FiSpeed = fixAdd(_iPlane.FiSpeed, fixMul(_iPlane.Fpitch, fToFix(1.5))); + // Horizontal acceleration - thrust. + //_iPlane.hAccel = ((_iPlane.rpm * (_iPlane.iSpeed - _iPlane.hSpeed)) / 10000); + //_iPlane.hAccel /= 1000; + //_iPlane.hAccel *= _iPlane.loopTime; + _iPlane.FtmpX = fixSub(_iPlane.FiSpeed, _iPlane.FhSpeed); + _iPlane.FtmpX = fixMul(iToFix(_iPlane.rpm), _iPlane.FtmpX); + _iPlane.FhAccel = fixDiv(_iPlane.FtmpX, iToFix(10000)); + _iPlane.FhAccel = fixDiv(_iPlane.FhAccel, iToFix(1000)); + _iPlane.FhAccel = fixMul(_iPlane.FhAccel, iToFix(_iPlane.loopTime)); + if (_iPlane.brake && !_iPlane.airborne) { + // Handle brakes. + if (_iPlane.FhSpeed > 0) { + //_iPlane.hSpeed -= 1; + _iPlane.FhSpeed = fixSub(_iPlane.FhSpeed, iToFix(1)); + } else { + //_iPlane.hSpeed = 0; + _iPlane.FhSpeed = iToFix(0); + } + } else { + // Accelerate normally. + //_iPlane.hSpeed += _iPlane.hAccel; + _iPlane.FhSpeed = fixAdd(_iPlane.FhSpeed, _iPlane.FhAccel); + } + // Force speed to range -1..1. + //_iPlane.lSpeed = (_iPlane.hSpeed / 65) - 1; + _iPlane.FlSpeed = fixSub(fixDiv(_iPlane.FhSpeed, iToFix(65)), iToFix(1)); + //if (_iPlane.lSpeed > 1) _iPlane.lSpeed = 1; + if (_iPlane.FlSpeed > iToFix(1)) _iPlane.FlSpeed = iToFix(1); + // Lift curve. + //_iPlane.lVeloc = Degs(ourAtan(_iPlane.lSpeed)); + _iPlane.FlVeloc = fixDegs(fixAtan(_iPlane.FlSpeed)); + // Force lift to range 0..90. + //_iPlane.lVeloc += 45; + _iPlane.FlVeloc = fixAdd(_iPlane.FlVeloc, iToFix(45)); + // Shift range to 0..-17. + //_iPlane.lVeloc /= 5.29; + _iPlane.FlVeloc = fixDiv(_iPlane.FlVeloc, fToFix(5.29)); + // Multiply by pitch modifier. + //_iPlane.lVeloc *= (-(_iPlane.pitch * 0.157) + 1); + _iPlane.FlVeloc = fixMul(_iPlane.FlVeloc, fixAdd(-(fixMul(_iPlane.Fpitch, fToFix(0.157))), iToFix(1))); + // Time slice. + //_iPlane.lVeloc /= 1000; + //_iPlane.lVeloc *= _iPlane.loopTime; + _iPlane.FlVeloc = fixDiv(_iPlane.FlVeloc, iToFix(1000)); + _iPlane.FlVeloc = fixMul(_iPlane.FlVeloc, iToFix(_iPlane.loopTime)); + // Gravity. + //_iPlane.gVeloc = _iPlane.loopTime * (-16.0 / 1000); // -16.0 is ft/sec for gravity. + _iPlane.FgVeloc = fixMul(iToFix(_iPlane.loopTime), fToFix(-0.016)); // -16.0 is ft/sec for gravity. + // Sum vertical velocity. + //_iPlane.vSpeed = _iPlane.gVeloc + _iPlane.lVeloc; + _iPlane.FvSpeed = fixAdd(_iPlane.FgVeloc, _iPlane.FlVeloc); + // No vertical speed if we're on the ground. + if ((!_iPlane.airborne) && (_iPlane.FvSpeed < iToFix(0))) _iPlane.FvSpeed = iToFix(0); + // Save climb rate in ft/min. + //_iPlane.climbRate = _iPlane.vSpeed / _iPlane.loopTime; + //_iPlane.climbRate *= 60000L; + _iPlane.FclimbRate = fixDiv(_iPlane.FvSpeed, iToFix(_iPlane.loopTime)); + _iPlane.FclimbRate = fixMul(_iPlane.FclimbRate, iToFix(60000L)); //***TODO*** This long could be an issue. + // Expand to ft/hr. + //_iPlane.deltaZ = _iPlane.hSpeed * 5280; + _iPlane.FdeltaZ = fixMul(_iPlane.FhSpeed, iToFix(5280)); + // Get ft/ms. + //_iPlane.deltaZ /= 3600000L; + //_iPlane.deltaZ *= _iPlane.loopTime; + _iPlane.FdeltaZ = fixDiv(_iPlane.FdeltaZ, iToFix(3600000L)); //***TODO*** This long could be an issue. + _iPlane.FdeltaZ = fixMul(_iPlane.FdeltaZ, iToFix(_iPlane.loopTime)); + // Find effective angle of flight. + if (_iPlane.FdeltaZ) { + //_iPlane.efAOF = -(ourAtan(_iPlane.vSpeed / _iPlane.deltaZ)); + _iPlane.FefAOF = -(fixAtan(fixDiv(_iPlane.FvSpeed, _iPlane.FdeltaZ))); + } else { + //_iPlane.efAOF = -(ourAtan(_iPlane.vSpeed)); + _iPlane.FefAOF = -(fixAtan(_iPlane.FvSpeed)); + } + // Convert to degrees. + //_iPlane.AOA = Degs(_iPlane.efAOF); + _iPlane.FAOA = fixDegs(_iPlane.FefAOF); + // Handle stalling. + //if (((_iPlane.pitch < _iPlane.AOA) && (_iPlane.AOA < 0)) && (_iPlane.hSpeed < 40)) { + if (((_iPlane.Fpitch < _iPlane.FAOA) && (_iPlane.FAOA < iToFix(0))) && (_iPlane.FhSpeed < iToFix(40))) { + //if ((_iPlane.pitch - _iPlane.AOA) < -20) _iPlane.stall = true; + if (fixSub(_iPlane.Fpitch, _iPlane.FAOA) < iToFix(-20)) _iPlane.stall = true; + } + if (_iPlane.stall) { + //if (_iPlane.pitch > 30) { + if (_iPlane.Fpitch > iToFix(30)) { + _iPlane.stall = false; + } else { + //_iPlane.pitch++; + _iPlane.Fpitch = fixAdd(_iPlane.Fpitch, iToFix(1)); + } + } +} + + +#define SEGMENT_FLIGHT_MODEL_2 + + +void iLightPlane2(void) { + // Flight model. Inertial Damping. + if (_iPlane.FdPitch != iToFix(0)) { + //_iPlane.dPitch -= _iPlane.dPitch / 10; + _iPlane.FdPitch = fixSub(_iPlane.FdPitch, fixDiv(_iPlane.FdPitch, iToFix(10))); + //if (((_iPlane.dPitch > 0) && (_iPlane.dPitch < 0.01)) || ((_iPlane.dPitch < 0) && (_iPlane.dPitch > -0.01))) _iPlane.dPitch = 0; + if (((_iPlane.FdPitch > iToFix(0)) && (_iPlane.FdPitch < fToFix(0.01))) || ((_iPlane.FdPitch < iToFix(0)) && (_iPlane.FdPitch > fToFix(-0.01)))) _iPlane.FdPitch = iToFix(0); + } + if (_iPlane.FdYaw != iToFix(0)) { + //_iPlane.dYaw -= _iPlane.dYaw / 10; + _iPlane.FdYaw = fixSub(_iPlane.FdYaw, fixDiv(_iPlane.FdYaw, iToFix(10))); + //if (((_iPlane.dYaw > 0) && (_iPlane.dYaw < 0.01)) || ((_iPlane.dYaw < 0) && (_iPlane.dYaw > -0.01))) _iPlane.dYaw = 0; + if (((_iPlane.FdYaw > iToFix(0)) && (_iPlane.FdYaw < fToFix(0.01))) || ((_iPlane.FdYaw < iToFix(0)) && (_iPlane.FdYaw > fToFix(-0.01)))) _iPlane.FdYaw = iToFix(0); + } + if (_iPlane.FdRoll != iToFix(0)) { + //_iPlane.dRoll -= _iPlane.dRoll / 10; + _iPlane.FdRoll = fixSub(_iPlane.FdRoll, fixDiv(_iPlane.FdRoll, iToFix(10))); + //if (((_iPlane.dRoll > 0) && (_iPlane.dRoll < 0.01)) || ((_iPlane.dRoll < 0) && (_iPlane.dRoll > -0.01))) _iPlane.dRoll = 0; + if (((_iPlane.FdRoll > iToFix(0)) && (_iPlane.FdRoll < fToFix(0.01))) || ((_iPlane.FdRoll < iToFix(0)) && (_iPlane.FdRoll > fToFix(-0.01)))) _iPlane.FdRoll = iToFix(0); + } + + // Flight model. Rate of Change. + if (_iPlane.airborne) { + if (_iPlane.aileron != 0) { + //_iPlane.torque = ((_iPlane.hSpeed * _iPlane.aileron) / 10000); + _iPlane.Ftorque = fixDiv(fixMul(_iPlane.FhSpeed, iToFix(_iPlane.aileron)), iToFix(10000)); + //if (_iPlane.dRoll != (_iPlane.torque * _iPlane.loopTime)) _iPlane.dRoll += _iPlane.torque * 6; + if (_iPlane.FdRoll != fixMul(_iPlane.Ftorque, iToFix(_iPlane.loopTime))) _iPlane.FdRoll = fixAdd(_iPlane.FdRoll, fixMul(_iPlane.Ftorque, iToFix(6))); + } + } + if (_iPlane.elevator != 0) { + //_iPlane.torque = ((_iPlane.hSpeed * _iPlane.elevator) / 10000); + _iPlane.Ftorque = fixDiv(fixMul(_iPlane.FhSpeed, iToFix(_iPlane.elevator)), iToFix(10000)); + //if ((!_iPlane.airborne) && (_iPlane.torque > 0)) _iPlane.torque = 0; //***FIX*** This is dumb. + if ((!_iPlane.airborne) && (_iPlane.Ftorque > iToFix(0))) _iPlane.Ftorque = iToFix(0); //***FIX*** This is dumb. + //if (_iPlane.dPitch != (_iPlane.torque * _iPlane.loopTime)) _iPlane.dPitch += _iPlane.torque * 1.5; + if (_iPlane.FdPitch != fixMul(_iPlane.Ftorque, iToFix(_iPlane.loopTime))) _iPlane.FdPitch = fixAdd(_iPlane.FdPitch, fixMul(_iPlane.Ftorque, fToFix(1.5))); + } + if (_iPlane.FhSpeed != iToFix(0)) { + _iPlane.Ftorque = iToFix(0); + //if (_iPlane.rudder != 0) _iPlane.torque = -((_iPlane.hSpeed * _iPlane.rudder) / 10000); + if (_iPlane.rudder != 0) _iPlane.Ftorque = -(fixDiv(fixMul(_iPlane.FhSpeed, iToFix(_iPlane.rudder)), iToFix(10000))); + _iPlane.Ftorque2 = iToFix(0); + if ((_iPlane.Froll > iToFix(0)) && (_iPlane.Froll <= iToFix(90))) { //***FIX*** This is dumb. + //_iPlane.torque2 = _iPlane.roll * 0.00050; + _iPlane.Ftorque2 = fixMul(_iPlane.Froll, fToFix(0.00050)); + } else { + if ((_iPlane.Froll < iToFix(0)) && (_iPlane.Froll >= iToFix(-90))) { + //_iPlane.torque2 = _iPlane.roll * 0.00050; + _iPlane.Ftorque2 = fixMul(_iPlane.Froll, fToFix(0.00050)); + } + } + //_iPlane.torque += _iPlane.torque2; + _iPlane.Ftorque = fixAdd(_iPlane.Ftorque, _iPlane.Ftorque2); + //if (_iPlane.dYaw != (_iPlane.torque * _iPlane.loopTime)) _iPlane.dYaw += _iPlane.torque * 1.5; + if (_iPlane.FdYaw != fixMul(_iPlane.Ftorque, iToFix(_iPlane.loopTime))) _iPlane.FdYaw = fixAdd(_iPlane.FdYaw, fixMul(_iPlane.Ftorque, fToFix(1.5))); + } + + // Flight model. Apply Rotations. + // Transform pitch into components of yaw and pitch based on roll. + _iPlane.Froll = fixAdd(_iPlane.Froll, _iPlane.FdRoll); + _iPlane.Fyaw = fixAdd(_iPlane.Fyaw, _iPlane.FdYaw); + //_iPlane.pitch += (_iPlane.dPitch * cosD(_iPlane.roll)); + _iPlane.Fpitch = fixAdd(_iPlane.Fpitch, fixMul(_iPlane.FdPitch, fixCosD(_iPlane.Froll))); + //_iPlane.yaw += -(_iPlane.dPitch * sinD(_iPlane.roll)); + _iPlane.Fyaw = fixAdd(_iPlane.Fyaw, -fixMul(_iPlane.FdPitch, fixSinD(_iPlane.Froll))); + if (_iPlane.Froll > iToFix(180)) { + //_iPlane.roll = -180 + (_iPlane.roll - 180); + _iPlane.Froll = fixAdd(iToFix(-180), fixSub(_iPlane.Froll, iToFix(180))); + } else { + //if (_iPlane.roll < -180) _iPlane.roll = 180 + (_iPlane.roll + 180); + if (_iPlane.Froll < iToFix(-180)) _iPlane.Froll = fixAdd(iToFix(180), fixAdd(_iPlane.Froll, iToFix(180))); + } + if (_iPlane.Fyaw > iToFix(180)) { + //_iPlane.yaw = -180 + (_iPlane.yaw - 180); + _iPlane.Fyaw = fixAdd(iToFix(-180), fixSub(_iPlane.Fyaw, iToFix(180))); + } else { + //if (_iPlane.yaw < -180) _iPlane.yaw = 180 + (_iPlane.yaw + 180); + if (_iPlane.Fyaw < iToFix(-180)) _iPlane.Fyaw = fixAdd(iToFix(180), fixAdd(_iPlane.Fyaw, iToFix(180))); + } + // Handle special case when aircraft pitch passes vertical. + if ((_iPlane.Fpitch > iToFix(90)) || (_iPlane.Fpitch < iToFix(-90))) { + if (_iPlane.Froll >= iToFix(0)) { + _iPlane.Froll = fixSub(_iPlane.Froll, iToFix(180)); + } else { + if (_iPlane.Froll < iToFix(0)) _iPlane.Froll = fixAdd(_iPlane.Froll, iToFix(180)); + } + if (_iPlane.Fyaw >= iToFix(0)) { + _iPlane.Fyaw = fixSub(_iPlane.Fyaw, iToFix(180)); + } else { + if (_iPlane.Fyaw < iToFix(0)) _iPlane.Fyaw = fixAdd(_iPlane.Fyaw, iToFix(180)); + } + if (_iPlane.Fpitch > iToFix(0)) { + _iPlane.Fpitch = fixSub(iToFix(180), _iPlane.Fpitch); + } else { + if (_iPlane.Fpitch < iToFix(0)) _iPlane.Fpitch = fixSub(iToFix(-180), _iPlane.Fpitch); + } + } + // Dampen everything out to 0 if they get close enough. + if ((_iPlane.Fpitch > fToFix(-0.5)) && (_iPlane.Fpitch < fToFix(0.5))) _iPlane.Fpitch = iToFix(0); + if ((_iPlane.Froll > fToFix(-0.5)) && (_iPlane.Froll < fToFix(0.5))) _iPlane.Froll = iToFix(0); + if ((_iPlane.Fyaw > fToFix(-0.5)) && (_iPlane.Fyaw < fToFix(0.5))) _iPlane.Fyaw = iToFix(0); +} + + +#define SEGMENT_FLIGHT_MODEL_3 + + +void iMoveAircraft(void) { + // Calculate new aircraft position. Each coordinate is 1 foot in 3D space. + _iPlane.FtmpX = iToFix(0); + _iPlane.FtmpY = iToFix(0); + _iPlane.FtmpZ = _iPlane.FdeltaZ; + + // Order of these points is significant. + // Rotate in X. + //_iPlane.newY = -(_iPlane.tmpZ * sinD(_iPlane.AOA)); + _iPlane.FnewY = -fixMul(_iPlane.FtmpZ, fixSinD(_iPlane.FAOA)); + //_iPlane.newZ = (_iPlane.tmpZ * cosD(_iPlane.AOA)); + _iPlane.FnewZ = fixMul(_iPlane.FtmpZ, fixCosD(_iPlane.FAOA)); + _iPlane.FtmpY = _iPlane.FnewY; + _iPlane.FtmpZ = _iPlane.FnewZ; + // Rotate in Y. + //_iPlane.newX = (_iPlane.tmpZ * sinD(_iPlane.yaw)); + _iPlane.FnewX = fixMul(_iPlane.FtmpZ, fixSinD(_iPlane.Fyaw)); + //_iPlane.newZ = (_iPlane.tmpZ * cosD(_iPlane.yaw)); + _iPlane.FnewZ = fixMul(_iPlane.FtmpZ, fixCosD(_iPlane.Fyaw)); + + // Translate rotated point back to where it should be + // relative to it's last position. + _iPlane.FcollectX = fixAdd(_iPlane.FcollectX, _iPlane.FnewX); + if ((_iPlane.FcollectX > iToFix(1)) || (_iPlane.FcollectX < iToFix(-1))) { + _iPlane.x -= fixToI(_iPlane.FcollectX); + _iPlane.FcollectX = iToFix(0); + } + _iPlane.FcollectY = fixAdd(_iPlane.FcollectY, _iPlane.FnewY); + if ((_iPlane.FcollectY > iToFix(1)) || (_iPlane.FcollectY < iToFix(-1))) { + _iPlane.y -= fixToI(_iPlane.FcollectY); + _iPlane.FcollectY = iToFix(0); + } + _iPlane.FcollectZ = fixAdd(_iPlane.FcollectZ, _iPlane.FnewZ); + if ((_iPlane.FcollectZ > iToFix(1)) || (_iPlane.FcollectZ < iToFix(-1))) { + _iPlane.z += fixToI(_iPlane.FcollectZ); + _iPlane.FcollectZ = iToFix(0); + } + + // Are we flying? + if ((!_iPlane.airborne) && (_iPlane.y != 0)) _iPlane.airborne = true; +} + + +#define SEGMENT_SETUP + + +void iResetAircraft(void) { + // Initialize airplane. + scdMemSet((byte *)&_iPlane, 0, sizeof(iAirplaneT)); + + _iPlane.brake = true; + _iPlane.loopTime = 40; +} + + +#define SEGMENT_MAIN + + +void iUpdateAircraft(void) { + // Do the actual flying! + iLightPlane(); + iLightPlane2(); + iMoveAircraft(); +} diff --git a/pc/src/main.c b/pc/src/main.c index 50c45cf..2dbbca1 100644 --- a/pc/src/main.c +++ b/pc/src/main.c @@ -22,7 +22,8 @@ #include "a23d2.h" -#include "flight.h" +#include "fflight.h" +#include "iflight.h" #include "util.h" #include "bitmap.h" @@ -106,13 +107,23 @@ int appMain(app_t *app, void *userData) { a23d2Init(); // Build a plane! - resetAircraft(); - _plane.x = _camera->x; - _plane.y = _camera->y - PLANE_HEIGHT; - _plane.z = _camera->z; - _plane.Fpitch = fToFix(-_camera->p * CAM2DEG); //***TODO*** Always use fixed point? - _plane.Froll = fToFix(-_camera->b * CAM2DEG); - _plane.Fyaw = fToFix(_camera->h * CAM2DEG); + fResetAircraft(); + _fPlane.x = _camera->x; + _fPlane.y = _camera->y - PLANE_HEIGHT; + _fPlane.z = _camera->z; + _fPlane.pitch = -_camera->p * CAM2DEG; + _fPlane.roll = -_camera->b * CAM2DEG; + _fPlane.yaw = _camera->h * CAM2DEG; + + /* + iResetAircraft(); + _iPlane.x = _camera->x; + _iPlane.y = _camera->y - PLANE_HEIGHT; + _iPlane.z = _camera->z; + _iPlane.Fpitch = fToFix(-_camera->p * CAM2DEG); // ***TODO*** Always use fixed point? + _iPlane.Froll = fToFix(-_camera->b * CAM2DEG); + _iPlane.Fyaw = fToFix(_camera->h * CAM2DEG); + */ // No input. _gamepad = 0; @@ -217,53 +228,65 @@ int appMain(app_t *app, void *userData) { // Update aircraft control state. if ((_gamepad & JOY_BUTTON_1) || (_gamepad & JOY_BUTTON_2) || (_gamepad & JOY_BUTTON_3)) { // Modified input with button down. - if ((_gamepad & JOY_UP) && (_plane.throttle < 15)) _plane.throttle++; - if ((_gamepad & JOY_DOWN) && (_plane.throttle > 0)) _plane.throttle--; - if (_gamepad & JOY_RIGHT) _plane.brake = !_plane.brake; + if ((_gamepad & JOY_UP) && (_fPlane.throttle < 20)) _fPlane.throttle++; + if ((_gamepad & JOY_DOWN) && (_fPlane.throttle > 0)) _fPlane.throttle--; + if (_gamepad & JOY_RIGHT) _fPlane.brake = !_fPlane.brake; if (_gamepad & JOY_LEFT) { - _plane.elevator = 0; - _plane.aileron = 0; + _fPlane.elevator = 0; + _fPlane.aileron = 0; } } else { // No button pressed. - if ((_gamepad & JOY_UP) && (_plane.elevator > -15)) _plane.elevator--; - if ((_gamepad & JOY_DOWN) && (_plane.elevator < 15)) _plane.elevator++; - if ((_gamepad & JOY_LEFT) && (_plane.aileron > -15)) _plane.aileron--; - if ((_gamepad & JOY_RIGHT) && (_plane.aileron < 15)) _plane.aileron++; + if ((_gamepad & JOY_UP) && (_fPlane.elevator > -7)) _fPlane.elevator--; + if ((_gamepad & JOY_DOWN) && (_fPlane.elevator < 7)) _fPlane.elevator++; + if ((_gamepad & JOY_LEFT) && (_fPlane.aileron > -7)) _fPlane.aileron--; + if ((_gamepad & JOY_RIGHT) && (_fPlane.aileron < 7)) _fPlane.aileron++; } // We have no rudder input. "Coordinated" flight. - _plane.rudder = _plane.aileron; + _fPlane.rudder = _fPlane.aileron; // We have no ignition input. - if (_plane.throttle) _plane.ignition = 1; + if (_fPlane.throttle) _fPlane.ignition = 1; } - printAt(0, 25, "X:%d Y:%d Z:%d", _plane.x, _plane.y, _plane.z); - printAt(0, 26, "P:%f R:%f Y:%f", fixToF(_plane.Fpitch), fixToF(_plane.Froll), fixToF(_plane.Fyaw)); - printAt(0, 27, "T:%d E:%d A:%d B:%d", _plane.throttle, _plane.elevator, _plane.aileron, _plane.brake); - printAt(0, 28, "I:%d A:%d R:%d H:%f V:%f", _plane.ignition, _plane.airborne, _plane.rpm, fixToF(_plane.FhSpeed), fixToF(_plane.FvSpeed)); + printAt(0, 25, "X:%d Y:%d Z:%d", _fPlane.x, _fPlane.y, _fPlane.z); + //printAt(0, 26, "P:%f R:%f Y:%f", fixToF(_fPlane.Fpitch), fixToF(_fPlane.Froll), fixToF(_fPlane.Fyaw)); + printAt(0, 26, "P:%f R:%f Y:%f", _fPlane.pitch, _fPlane.roll, _fPlane.yaw); + printAt(0, 27, "T:%d E:%d A:%d B:%d", _fPlane.throttle, _fPlane.elevator, _fPlane.aileron, _fPlane.brake); + //printAt(0, 28, "I:%d A:%d R:%d H:%f V:%f", _fPlane.ignition, _fPlane.airborne, _fPlane.rpm, fixToF(_fPlane.FhSpeed), fixToF(_fPlane.FvSpeed)); + printAt(0, 28, "I:%d A:%d R:%d H:%f V:%f", _fPlane.ignition, _fPlane.airborne, _fPlane.rpm, _fPlane.hSpeed, _fPlane.vSpeed); // Update timers. - _plane.loopTime = (app_time_count(app) - lastTime) / timeFreqMs; - if (_plane.loopTime == 0) _plane.loopTime = 1; + _fPlane.loopTime = (app_time_count(app) - lastTime) / timeFreqMs; + if (_fPlane.loopTime == 0) _fPlane.loopTime = 1; lastTime = app_time_count(app); - milliseconds += _plane.loopTime; + milliseconds += _fPlane.loopTime; if (milliseconds > 1000) { milliseconds -= 1000; fps = 0; } // FLY! - updateAircraft(); + fUpdateAircraft(); + //iUpdateAircraft(); // Move camera. - _camera->x = _plane.x; - _camera->y = _plane.y + PLANE_HEIGHT; - _camera->z = _plane.z; - _camera->p = -fixToF(_plane.Fpitch) * DEG2CAM; //***TODO*** Always use fixed point? - _camera->b = -fixToF(_plane.Froll) * DEG2CAM; - _camera->h = fixToF(_plane.Fyaw) * DEG2CAM; + _camera->x = _fPlane.x; + _camera->y = _fPlane.y + PLANE_HEIGHT; + _camera->z = _fPlane.z; + _camera->p = -_fPlane.pitch * DEG2CAM; + _camera->b = -_fPlane.roll * DEG2CAM; + _camera->h = _fPlane.yaw * DEG2CAM; + + /* + _camera->x = _iPlane.x; + _camera->y = _iPlane.y + PLANE_HEIGHT; + _camera->z = _iPlane.z; + _camera->p = -fixToF(_iPlane.Fpitch) * DEG2CAM; // ***TODO*** Always use fixed point? + _camera->b = -fixToF(_iPlane.Froll) * DEG2CAM; + _camera->h = fixToF(_iPlane.Fyaw) * DEG2CAM; + */ printAt(0, 0, "X:%d Y:%d Z:%d", _camera->x, _camera->y, _camera->z); printAt(0, 1, "P:%d B:%d H:%d", _camera->p, _camera->b, _camera->h);