326 lines
9.4 KiB
C
326 lines
9.4 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 "flight.h"
|
|
|
|
|
|
airplaneT _plane;
|
|
|
|
|
|
#define SEGMENT_MATH
|
|
|
|
|
|
#define PI 3.1415926
|
|
|
|
#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){
|
|
return atan(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));
|
|
}
|
|
|
|
|
|
float sinD(float d) {
|
|
return sin(Rads(d));
|
|
}
|
|
|
|
|
|
#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;
|
|
// Modify speed by pitch.
|
|
_plane.iSpeed += (_plane.pitch * 1.5);
|
|
// Horizontal acceleration - thrust.
|
|
_plane.hAccel = ((_plane.rpm * (_plane.iSpeed - _plane.hSpeed)) / 10000);
|
|
_plane.hAccel /= 1000;
|
|
_plane.hAccel *= _plane.loopTime;
|
|
if (_plane.brake && !_plane.airborne) {
|
|
// Handle brakes.
|
|
if (_plane.hSpeed > 0) {
|
|
_plane.hSpeed -= 1;
|
|
} else {
|
|
_plane.hSpeed = 0;
|
|
}
|
|
} else {
|
|
// Accelerate normally.
|
|
_plane.hSpeed += _plane.hAccel;
|
|
}
|
|
// Force speed to range -1..1.
|
|
_plane.lSpeed = (_plane.hSpeed / 65) - 1;
|
|
if (_plane.lSpeed > 1) _plane.lSpeed = 1;
|
|
// Lift curve.
|
|
_plane.lVeloc = Degs(ourAtan(_plane.lSpeed));
|
|
// Force lift to range 0..90.
|
|
_plane.lVeloc += 45;
|
|
// Shift range to 0..-17.
|
|
_plane.lVeloc /= 5.29;
|
|
// Multiply by pitch modifier.
|
|
_plane.lVeloc *= (-(_plane.pitch * 0.157) + 1);
|
|
// Time slice.
|
|
_plane.lVeloc /= 1000;
|
|
_plane.lVeloc *= _plane.loopTime;
|
|
// Gravity.
|
|
_plane.gVeloc = _plane.loopTime * (-16.0 / 1000); // -16.0 is ft/sec for gravity.
|
|
// Sum vertical velocity.
|
|
_plane.vSpeed = _plane.gVeloc + _plane.lVeloc;
|
|
// No vertical speed if we're on the ground.
|
|
if ((!_plane.airborne) && (_plane.vSpeed < 0)) _plane.vSpeed = 0;
|
|
// Save climb rate in ft/min.
|
|
_plane.climbRate = _plane.vSpeed / _plane.loopTime;
|
|
_plane.climbRate *= 60000L;
|
|
// Expand to ft/hr.
|
|
_plane.deltaZ = _plane.hSpeed * 5280;
|
|
// Get ft/ms.
|
|
_plane.deltaZ /= 3600000L;
|
|
_plane.deltaZ *= _plane.loopTime;
|
|
// Find effective angle of flight.
|
|
if (_plane.deltaZ) {
|
|
_plane.efAOF = -(ourAtan(_plane.vSpeed / _plane.deltaZ));
|
|
} else {
|
|
_plane.efAOF = -(ourAtan(_plane.vSpeed));
|
|
}
|
|
// Convert to degrees.
|
|
_plane.AOA = Degs(_plane.efAOF);
|
|
// Handle stalling.
|
|
if (((_plane.pitch < _plane.AOA) && (_plane.AOA < 0)) && (_plane.hSpeed < 40)) {
|
|
if ((_plane.pitch - _plane.AOA) < -20) _plane.stall = true;
|
|
}
|
|
if (_plane.stall) {
|
|
if (_plane.pitch > 30) {
|
|
_plane.stall = false;
|
|
} else {
|
|
_plane.pitch++;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
#define SEGMENT_FLIGHT_MODEL_2
|
|
|
|
|
|
void lightPlane2(void) {
|
|
// Flight model. Inertial Damping.
|
|
if (_plane.dPitch) {
|
|
_plane.dPitch -= _plane.dPitch / 10;
|
|
if (((_plane.dPitch > 0) && (_plane.dPitch < 0.01)) || ((_plane.dPitch < 0) && (_plane.dPitch > -0.01))) _plane.dPitch = 0;
|
|
}
|
|
if (_plane.dYaw) {
|
|
_plane.dYaw -= _plane.dYaw / 10;
|
|
if (((_plane.dYaw > 0) && (_plane.dYaw < 0.01)) || ((_plane.dYaw < 0) && (_plane.dYaw > -0.01))) _plane.dYaw = 0;
|
|
}
|
|
if (_plane.dRoll) {
|
|
_plane.dRoll -= _plane.dRoll / 10;
|
|
if (((_plane.dRoll > 0) && (_plane.dRoll < 0.01)) || ((_plane.dRoll < 0) && (_plane.dRoll > -0.01))) _plane.dRoll = 0;
|
|
}
|
|
|
|
// Flight model. Rate of Change.
|
|
if (_plane.airborne) {
|
|
if (_plane.aileron != 0) {
|
|
_plane.torque = ((_plane.hSpeed * _plane.aileron) / 10000);
|
|
if (_plane.dRoll != (_plane.torque * _plane.loopTime)) _plane.dRoll += _plane.torque * 6;
|
|
}
|
|
}
|
|
if (_plane.elevator != 0) {
|
|
_plane.torque = ((_plane.hSpeed * _plane.elevator) / 10000);
|
|
if ((!_plane.airborne) && (_plane.torque > 0)) _plane.torque = 0; //***FIX*** This is dumb.
|
|
if (_plane.dPitch != (_plane.torque * _plane.loopTime)) _plane.dPitch += _plane.torque * 1.5;
|
|
}
|
|
if (_plane.hSpeed) {
|
|
_plane.torque = 0.0;
|
|
if (_plane.rudder != 0) _plane.torque = -((_plane.hSpeed * _plane.rudder) / 10000);
|
|
_plane.torque2 = 0.0;
|
|
if ((_plane.roll > 0) && (_plane.roll <= 90)) { //***FIX*** This is dumb.
|
|
_plane.torque2 = _plane.roll * 0.00050;
|
|
} else {
|
|
if ((_plane.roll < 0) && (_plane.roll >= -90)) {
|
|
_plane.torque2 = _plane.roll * 0.00050;
|
|
}
|
|
}
|
|
_plane.torque += _plane.torque2;
|
|
if (_plane.dYaw != (_plane.torque * _plane.loopTime)) _plane.dYaw += _plane.torque * 1.5;
|
|
}
|
|
|
|
// Flight model. Apply Rotations.
|
|
// Transform pitch into components of yaw and pitch based on roll.
|
|
_plane.roll += _plane.dRoll;
|
|
_plane.yaw += _plane.dYaw;
|
|
_plane.pitch += (_plane.dPitch * cosD(_plane.roll));
|
|
_plane.yaw += -(_plane.dPitch * sinD(_plane.roll));
|
|
if (_plane.roll > 180) {
|
|
_plane.roll = -180 + (_plane.roll - 180);
|
|
} else {
|
|
if (_plane.roll < -180) _plane.roll = 180 + (_plane.roll + 180);
|
|
}
|
|
if (_plane.yaw > 180) {
|
|
_plane.yaw = -180 + (_plane.yaw - 180);
|
|
} else {
|
|
if (_plane.yaw < -180) _plane.yaw = 180 + (_plane.yaw + 180);
|
|
}
|
|
// Handle special case when aircraft pitch passes vertical.
|
|
if ((_plane.pitch > 90) || (_plane.pitch < -90)) {
|
|
if (_plane.roll >= 0) {
|
|
_plane.roll -= 180;
|
|
} else {
|
|
if (_plane.roll < 0) _plane.roll += 180;
|
|
}
|
|
if (_plane.yaw >= 0) {
|
|
_plane.yaw -= 180;
|
|
} else {
|
|
if (_plane.yaw < 0) _plane.yaw += 180;
|
|
}
|
|
if (_plane.pitch > 0) {
|
|
_plane.pitch = (180 - _plane.pitch);
|
|
} else {
|
|
if (_plane.pitch < 0) _plane.pitch = (-180 - _plane.pitch);
|
|
}
|
|
}
|
|
// Dampen everything out to 0 if they get close enough.
|
|
if ((_plane.pitch > -0.5) && (_plane.pitch < 0.5)) _plane.pitch = 0;
|
|
if ((_plane.roll > -0.5) && (_plane.roll < 0.5)) _plane.roll = 0;
|
|
if ((_plane.yaw > -0.5) && (_plane.yaw < 0.5)) _plane.yaw = 0;
|
|
}
|
|
|
|
|
|
#define SEGMENT_FLIGHT_MODEL_3
|
|
|
|
|
|
void moveAircraft(void) {
|
|
// Calculate new aircraft position. Each coordinate is 1 foot in 3D space.
|
|
_plane.tmpX = 0;
|
|
_plane.tmpY = 0;
|
|
_plane.tmpZ = _plane.deltaZ;
|
|
|
|
// Order of these points is significant.
|
|
// Rotate in X.
|
|
_plane.newY = -(_plane.tmpZ * sinD(_plane.AOA));
|
|
_plane.newZ = (_plane.tmpZ * cosD(_plane.AOA));
|
|
_plane.tmpY = _plane.newY;
|
|
_plane.tmpZ = _plane.newZ;
|
|
// Rotate in Y.
|
|
_plane.newX = (_plane.tmpZ * sinD(_plane.yaw));
|
|
_plane.newZ = (_plane.tmpZ * cosD(_plane.yaw));
|
|
|
|
// Translate rotated point back to where it should be
|
|
// relative to it's last position.
|
|
_plane.collectX += _plane.newX;
|
|
if ((_plane.collectX > 1) || (_plane.collectX < -1)) {
|
|
_plane.x -= _plane.collectX;
|
|
_plane.collectX = 0;
|
|
}
|
|
_plane.collectY += _plane.newY;
|
|
if ((_plane.collectY > 1) || (_plane.collectY < -1)) {
|
|
_plane.y -= _plane.collectY;
|
|
_plane.collectY = 0;
|
|
}
|
|
_plane.collectZ += _plane.newZ;
|
|
if ((_plane.collectY > 1) || (_plane.collectY < -1)) {
|
|
_plane.z += _plane.collectZ;
|
|
_plane.collectZ = 0;
|
|
}
|
|
|
|
// Are we flying?
|
|
if ((!_plane.airborne) && (_plane.y > PLANE_HEIGHT)) _plane.airborne = true;
|
|
}
|
|
|
|
|
|
void resetAircraft(void) {
|
|
// Initialize airplane.
|
|
memset(&_plane, 0, sizeof(airplaneT));
|
|
|
|
_plane.brake = true;
|
|
/*
|
|
_plane.rpm = 0;
|
|
_plane.hSpeed = 0;
|
|
_plane.vSpeed = 0;
|
|
_plane.deltaZ = 0;
|
|
_plane.ignition = false;
|
|
_plane.engine = false;
|
|
_plane.efAOF = 0;
|
|
_plane.x = 0;
|
|
_plane.y = 0;
|
|
_plane.z = 0;
|
|
_plane.pitch = 0;
|
|
_plane.roll = 0;
|
|
_plane.yaw = 0;
|
|
_plane.airborne = false;
|
|
*/
|
|
|
|
_plane.loopTime = 40;
|
|
}
|
|
|
|
|
|
#define SEGMENT_MAIN
|
|
|
|
|
|
void updateAircraft(void) {
|
|
// Do the actual flying!
|
|
lightPlane();
|
|
lightPlane2();
|
|
moveAircraft();
|
|
}
|