319 lines
9.6 KiB
C
319 lines
9.6 KiB
C
/*
|
|
* 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();
|
|
}
|