Public_Skunkworks/sierrahotel
Public_Skunkworks/sierrahotel
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fixed point macros complete. Bug somewhere.

Browse source
This commit is contained in:
Scott Duensing 2024-05-31 19:33:09 -05:00
parent e33a37b3b6
commit 7e3b33a2e5
5 changed files with 108 additions and 84 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
pc/include/flight.h
View file

@ -88,7 +88,6 @@ typedef struct airplaneS {
extern airplaneT _plane;
void initAircraft(void);
void resetAircraft(void);
void updateAircraft(void);

8
pc/include/fmath.h
View file

@ -40,11 +40,11 @@ typedef double fixT;
#define fixAdd(x, y) ((x) + (y))
#define fixAtan(x) atan(x)
#define fixCos(x) cos(x)
#define fixCosD(x) cos((x) * M_PI / 180)
#define fixDegs(x) ((x) * 180 / M_PI)
#define fixDiv(x, y) ((x) / (y))
#define fixMul(x, y) ((x) * (y))
#define fixSin(x) sin(x)
#define fixSinD(x) sin((x) * M_PI / 180)
#define fixSub(x, y) ((x) - (y))
#define fixToF(x) (x)
#define fixToI(x) ((int)(x))
@ -68,11 +68,11 @@ typedef int32_t fixT;
fixT fixAdd(fixT x, fixT y);
fixT fixAtan(fixT x);
fixT fixCos(fixT x);
fixT fixCosD(fixT x);
//fixT fixDegs(fixT x);
fixT fixDiv(fixT x, fixT y);
fixT fixMul(fixT x, fixT y);
fixT fixSin(fixT x);
fixT fixSinD(fixT x);
fixT fixSub(fixT x, fixT y);
double fixToF(fixT x);
int16_t fixToI(fixT x);

8
pc/src/a23d2.c
View file

@ -72,7 +72,13 @@ void a23d2Draw(void) {
y1 = 255 - ((int8_t)_RAM[pointer++] + 128);
x2 = (int8_t)_RAM[pointer++] + 128;
y2 = 255 - ((int8_t)_RAM[pointer++] + 128);
bitmapLine(x1 + _scaleX[x1], _scaleY[y1], x2 + _scaleX[x2], _scaleY[y2]);
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);
fflush(0);
} else {
bitmapLine(x1 + _scaleX[x1], _scaleY[y1], x2 + _scaleX[x2], _scaleY[y2]);
}
continue;
// Point.

146
pc/src/flight.c
View file

@ -92,18 +92,18 @@ void lightPlane(void) {
_plane.FlVeloc = fixDegs(fixAtan(_plane.FlSpeed));
// Force lift to range 0..90.
//_plane.lVeloc += 45;
_plane.FlVeloc = fixAdd(_plane.FlVeloc, 45);
_plane.FlVeloc = fixAdd(_plane.FlVeloc, iToFix(45));
// Shift range to 0..-17.
//_plane.lVeloc /= 5.29;
_plane.FlVeloc = fixDiv(_plane.FlVeloc, 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, 1000);
_plane.FlVeloc = fixAdd(_plane.FlVeloc, _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.
@ -181,68 +181,84 @@ void lightPlane2(void) {
// 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;
//_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);
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;
//_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.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;
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.roll < 0) && (_plane.roll >= -90)) {
_plane.torque2 = _plane.roll * 0.00050;
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;
if (_plane.dYaw != (_plane.torque * _plane.loopTime)) _plane.dYaw += _plane.torque * 1.5;
//_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.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);
_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.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.yaw > 180) {
_plane.yaw = -180 + (_plane.yaw - 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.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.pitch > 90) || (_plane.pitch < -90)) {
if (_plane.roll >= 0) {
_plane.roll -= 180;
if ((_plane.Fpitch > iToFix(90)) || (_plane.Fpitch < iToFix(-90))) {
if (_plane.Froll >= iToFix(0)) {
_plane.Froll = fixSub(_plane.Froll, iToFix(180));
} else {
if (_plane.roll < 0) _plane.roll += 180;
if (_plane.Froll < iToFix(0)) _plane.Froll = fixAdd(_plane.Froll, iToFix(180));
}
if (_plane.yaw >= 0) {
_plane.yaw -= 180;
if (_plane.Fyaw >= iToFix(0)) {
_plane.Fyaw = fixSub(_plane.Fyaw, iToFix(180));
} else {
if (_plane.yaw < 0) _plane.yaw += 180;
if (_plane.Fyaw < iToFix(0)) _plane.Fyaw = fixAdd(_plane.Fyaw, iToFix(180));
}
if (_plane.pitch > 0) {
_plane.pitch = (180 - _plane.pitch);
if (_plane.Fpitch > iToFix(0)) {
_plane.Fpitch = fixSub(iToFix(180), _plane.Fpitch);
} else {
if (_plane.pitch < 0) _plane.pitch = (-180 - _plane.pitch);
if (_plane.Fpitch < iToFix(0)) _plane.Fpitch = fixSub(iToFix(-180), _plane.Fpitch);
}
}
// 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;
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);
}
@ -251,36 +267,40 @@ void lightPlane2(void) {
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;
_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.newZ = (_plane.tmpZ * cosD(_plane.AOA));
_plane.tmpY = _plane.newY;
_plane.tmpZ = _plane.newZ;
//_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.newZ = (_plane.tmpZ * cosD(_plane.yaw));
//_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.collectX += _plane.newX;
if ((_plane.collectX > 1) || (_plane.collectX < -1)) {
_plane.x -= _plane.collectX;
_plane.collectX = 0;
_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.collectY += _plane.newY;
if ((_plane.collectY > 1) || (_plane.collectY < -1)) {
_plane.y -= _plane.collectY;
_plane.collectY = 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.collectZ += _plane.newZ;
if ((_plane.collectZ > 1) || (_plane.collectZ < -1)) {
_plane.z += _plane.collectZ;
_plane.collectZ = 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?

29
pc/src/main.c
View file

@ -106,14 +106,13 @@ int appMain(app_t *app, void *userData) {
a23d2Init();
// Build a plane!
initAircraft();
resetAircraft();
_plane.x = _camera->x;
_plane.y = _camera->y - PLANE_HEIGHT;
_plane.z = _camera->z;
_plane.pitch = -_camera->p * CAM2DEG;
_plane.roll = -_camera->b * CAM2DEG;
_plane.yaw = _camera->h * CAM2DEG;
_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);
// No input.
_gamepad = 0;
@ -241,9 +240,9 @@ int appMain(app_t *app, void *userData) {
}
printAt(0, 25, "X:%d Y:%d Z:%d", _plane.x, _plane.y, _plane.z);
printAt(0, 26, "P:%f R:%f Y:%f", _plane.pitch, _plane.roll, _plane.yaw);
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, _plane.hSpeed, _plane.vSpeed);
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));
// Update timers.
_plane.loopTime = (app_time_count(app) - lastTime) / timeFreqMs;
@ -259,12 +258,12 @@ int appMain(app_t *app, void *userData) {
updateAircraft();
// Move camera.
_camera->x = _plane.x;
_camera->y = _plane.y + PLANE_HEIGHT;
_camera->z = _plane.z;
_camera->p = -_plane.pitch * DEG2CAM;
_camera->b = -_plane.roll * DEG2CAM;
_camera->h = _plane.yaw * DEG2CAM;
_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;
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);