mirror of
https://github.com/gbdev/rgbds.git
synced 2025-11-20 10:12:06 +00:00
3d155d5695
* Use clang-tidy `misc-include-cleaner` for IWYU `#include` cleanup * Use `std::optional<size_t>` instead of `ssize_t` * Rename some functions in linkdefs.hpp * Fix header order
103 lines
2.4 KiB
C++
103 lines
2.4 KiB
C++
// SPDX-License-Identifier: MIT
|
|
|
|
// Fixed-point math routines
|
|
|
|
#include "asm/fixpoint.hpp"
|
|
|
|
#include <math.h>
|
|
#include <numbers>
|
|
#include <stdint.h>
|
|
|
|
static constexpr double tau = std::numbers::pi * 2;
|
|
|
|
static double fix2double(int32_t i, int32_t q) {
|
|
return i / pow(2.0, q);
|
|
}
|
|
|
|
static int32_t double2fix(double d, int32_t q) {
|
|
if (isnan(d)) {
|
|
return 0;
|
|
}
|
|
if (isinf(d)) {
|
|
return d < 0 ? INT32_MIN : INT32_MAX;
|
|
}
|
|
return static_cast<int32_t>(round(d * pow(2.0, q)));
|
|
}
|
|
|
|
static double turn2rad(double t) {
|
|
return t * tau;
|
|
}
|
|
|
|
static double rad2turn(double r) {
|
|
return r / tau;
|
|
}
|
|
|
|
int32_t fix_Sin(int32_t i, int32_t q) {
|
|
return double2fix(sin(turn2rad(fix2double(i, q))), q);
|
|
}
|
|
|
|
int32_t fix_Cos(int32_t i, int32_t q) {
|
|
return double2fix(cos(turn2rad(fix2double(i, q))), q);
|
|
}
|
|
|
|
int32_t fix_Tan(int32_t i, int32_t q) {
|
|
return double2fix(tan(turn2rad(fix2double(i, q))), q);
|
|
}
|
|
|
|
int32_t fix_ASin(int32_t i, int32_t q) {
|
|
return double2fix(rad2turn(asin(fix2double(i, q))), q);
|
|
}
|
|
|
|
int32_t fix_ACos(int32_t i, int32_t q) {
|
|
return double2fix(rad2turn(acos(fix2double(i, q))), q);
|
|
}
|
|
|
|
int32_t fix_ATan(int32_t i, int32_t q) {
|
|
return double2fix(rad2turn(atan(fix2double(i, q))), q);
|
|
}
|
|
|
|
int32_t fix_ATan2(int32_t i, int32_t j, int32_t q) {
|
|
return double2fix(rad2turn(atan2(fix2double(i, q), fix2double(j, q))), q);
|
|
}
|
|
|
|
int32_t fix_Mul(int32_t i, int32_t j, int32_t q) {
|
|
return double2fix(fix2double(i, q) * fix2double(j, q), q);
|
|
}
|
|
|
|
int32_t fix_Div(int32_t i, int32_t j, int32_t q) {
|
|
double dividend = fix2double(i, q);
|
|
double divisor = fix2double(j, q);
|
|
if (fpclassify(divisor) == FP_ZERO) {
|
|
return dividend < 0 ? INT32_MIN : dividend > 0 ? INT32_MAX : 0;
|
|
}
|
|
return double2fix(dividend / divisor, q);
|
|
}
|
|
|
|
int32_t fix_Mod(int32_t i, int32_t j, int32_t q) {
|
|
return double2fix(fmod(fix2double(i, q), fix2double(j, q)), q);
|
|
}
|
|
|
|
int32_t fix_Pow(int32_t i, int32_t j, int32_t q) {
|
|
return double2fix(pow(fix2double(i, q), fix2double(j, q)), q);
|
|
}
|
|
|
|
int32_t fix_Log(int32_t i, int32_t j, int32_t q) {
|
|
double divisor = log(fix2double(j, q));
|
|
if (fpclassify(divisor) == FP_ZERO) {
|
|
return INT32_MAX;
|
|
}
|
|
return double2fix(log(fix2double(i, q)) / divisor, q);
|
|
}
|
|
|
|
int32_t fix_Round(int32_t i, int32_t q) {
|
|
return double2fix(round(fix2double(i, q)), q);
|
|
}
|
|
|
|
int32_t fix_Ceil(int32_t i, int32_t q) {
|
|
return double2fix(ceil(fix2double(i, q)), q);
|
|
}
|
|
|
|
int32_t fix_Floor(int32_t i, int32_t q) {
|
|
return double2fix(floor(fix2double(i, q)), q);
|
|
}
|