h4570
296 lines
9.4 KiB
C++
296 lines
9.4 KiB
C++
/*
|
|
# _____ ____ ___
|
|
# | \/ ____| |___|
|
|
# | | | \ | |
|
|
#-----------------------------------------------------------------------
|
|
# Copyright 2022, tyra - https://github.com/h4570/tyra
|
|
# Licensed under Apache License 2.0
|
|
# Sandro Sobczyński <sandro.sobczynski@gmail.com>
|
|
*/
|
|
|
|
#ifdef __INTELLISENSE__
|
|
#pragma diag_suppress 1118
|
|
#endif
|
|
|
|
#include "math/math.hpp"
|
|
#include <stdlib.h>
|
|
|
|
extern volatile const u32 TYRA_MATH_ATAN_TABLE[9] alignas(sizeof(float)) = {
|
|
0x3f7ffff5, 0xbeaaa61c, 0x3e4c40a6, 0xbe0e6c63, 0x3dc577df,
|
|
0xbd6501c4, 0x3cb31652, 0xbb84d7e7, 0x3f490fdb,
|
|
};
|
|
|
|
extern volatile const float TYRA_MATH_ATAN_TABLE2[8] = {
|
|
0.0f,
|
|
(Tyra::Math::PI / 2.0f),
|
|
(Tyra::Math::PI / 2.0f),
|
|
(Tyra::Math::PI),
|
|
(-Tyra::Math::PI),
|
|
(-Tyra::Math::PI / 2.0f),
|
|
(-Tyra::Math::PI / 2.0f),
|
|
0.0f,
|
|
};
|
|
|
|
namespace Tyra {
|
|
|
|
float Math::cos(float x) {
|
|
float r;
|
|
asm volatile(
|
|
"lui $9, 0x3f00 \n\t"
|
|
".set noreorder \n\t"
|
|
".align 3 \n\t"
|
|
"abs.s %0, %1 \n\t"
|
|
"lui $8, 0xbe22 \n\t"
|
|
"mtc1 $9, $f1 \n\t"
|
|
"ori $8, $8, 0xf983 \n\t"
|
|
"mtc1 $8, $f8 \n\t"
|
|
"lui $9, 0x4b00 \n\t"
|
|
"mtc1 $9, $f3 \n\t"
|
|
"lui $8, 0x3f80 \n\t"
|
|
"mtc1 $8, $f2 \n\t"
|
|
"mula.s %0, $f8 \n\t"
|
|
"msuba.s $f3, $f2 \n\t"
|
|
"madda.s $f3, $f2 \n\t"
|
|
"lui $8, 0x40c9 \n\t"
|
|
"msuba.s %0, $f8 \n\t"
|
|
"ori $8, 0x0fdb \n\t"
|
|
"msub.s %0, $f1, $f2 \n\t"
|
|
"lui $9, 0xc225 \n\t"
|
|
"abs.s %0, %0 \n\t"
|
|
"lui $10, 0x3e80 \n\t"
|
|
"mtc1 $10, $f7 \n\t"
|
|
"ori $9, 0x5de1 \n\t"
|
|
"sub.s %0, %0, $f7 \n\t"
|
|
"lui $10, 0x42a3 \n\t"
|
|
"mtc1 $8, $f3 \n\t"
|
|
"ori $10, 0x3458 \n\t"
|
|
"mtc1 $9, $f4 \n\t"
|
|
"lui $8, 0xc299 \n\t"
|
|
"mtc1 $10, $f5 \n\t"
|
|
"ori $8, 0x2663 \n\t"
|
|
"mul.s $f8, %0, %0 \n\t"
|
|
"lui $9, 0x421e \n\t"
|
|
"mtc1 $8, $f6 \n\t"
|
|
"ori $9, 0xd7bb \n\t"
|
|
"mtc1 $9, $f7 \n\t"
|
|
"nop \n\t"
|
|
"mul.s $f1, %0, $f8 \n\t"
|
|
"mul.s $f9, $f8, $f8 \n\t"
|
|
"mula.s $f3, %0 \n\t"
|
|
"mul.s $f2, $f1, $f8 \n\t"
|
|
"madda.s $f4, $f1 \n\t"
|
|
"mul.s $f1, $f1, $f9 \n\t"
|
|
"mul.s %0, $f2, $f9 \n\t"
|
|
"madda.s $f5, $f2 \n\t"
|
|
"madda.s $f6, $f1 \n\t"
|
|
"madd.s %0, $f7, %0 \n\t"
|
|
".set reorder \n\t"
|
|
: "=&f"(r)
|
|
: "f"(x)
|
|
: "$f1", "$f2", "$f3", "$f4", "$f5", "$f6", "$f7", "$f8", "$f9", "$8",
|
|
"$9", "$10");
|
|
return r;
|
|
}
|
|
|
|
float Math::invSqrt(const float& x) { return 1.0F / sqrt(x); }
|
|
|
|
float Math::atan2(float y, float x) {
|
|
float r;
|
|
asm volatile(
|
|
"mtc1 $0, %0 \n\t"
|
|
"abs.s $f1, %1 \n\t"
|
|
"abs.s $f2, %2 \n\t"
|
|
"c.lt.s %2, %0 \n\t"
|
|
"move $9, $0 \n\t"
|
|
"bc1f _atan_00 \n\t"
|
|
"addiu $9, $9, 4 \n\t"
|
|
"_atan_00: \n\t"
|
|
"c.eq.s %2, %0 \n\t"
|
|
"bc1f _atan_00_1 \n\t"
|
|
"c.eq.s %1, %2 \n\t"
|
|
"bc1t _atan_06 \n\t"
|
|
"c.lt.s %1, %0 \n\t"
|
|
"bc1f _atan_00_1 \n\t"
|
|
"addiu $9, $0, 3 \n\t"
|
|
"_atan_00_1: \n\t"
|
|
"mul.s %1, %1, %2 \n\t"
|
|
"c.lt.s %1, %0 \n\t"
|
|
"bc1f _atan_01 \n\t"
|
|
"addiu $9, $9, 2 \n\t"
|
|
"c.lt.s $f2, $f1 \n\t"
|
|
"bc1f _atan_02 \n\t"
|
|
"addiu $9, $9, 1 \n\t"
|
|
"b _atan_02 \n\t"
|
|
"_atan_01: \n\t"
|
|
"c.lt.s $f1, $f2 \n\t"
|
|
"bc1f _atan_02 \n\t"
|
|
"addiu $9, $9, 1 \n\t"
|
|
"_atan_02: \n\t"
|
|
"c.lt.s $f1, $f2 \n\t"
|
|
"bc1f _atan_03 \n\t"
|
|
"mov.s %1, $f2 \n\t"
|
|
"mov.s %2, $f1 \n\t"
|
|
"b _atan_04 \n\t"
|
|
"_atan_03: \n\t"
|
|
"mov.s %1, $f1 \n\t"
|
|
"mov.s %2, $f2 \n\t"
|
|
"_atan_04: \n\t"
|
|
"mfc1 $6, %1 \n\t"
|
|
"mfc1 $7, %2 \n\t"
|
|
"la $8, TYRA_MATH_ATAN_TABLE \n\t"
|
|
"lqc2 $vf4, 0x0($8) \n\t"
|
|
"lqc2 $vf5, 0x10($8) \n\t"
|
|
"lqc2 $vf6, 0x20($8) \n\t"
|
|
"qmtc2 $6, $vf21 \n\t"
|
|
"qmtc2 $7, $vf22 \n\t"
|
|
"vadd.x $vf23, $vf21, $vf22 \n\t"
|
|
"vsub.x $vf22, $vf22, $vf21 \n\t"
|
|
"vdiv $Q, $vf22x, $vf23x \n\t"
|
|
"vwaitq \n\t"
|
|
"vaddq.x $vf21, $vf0, $Q \n\t"
|
|
"vmul.x $vf22, $vf21, $vf21 \n\t"
|
|
"vmulax.x $ACC, $vf21, $vf4 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmadday.x $ACC, $vf21, $vf4 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmaddaz.x $ACC, $vf21, $vf4 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmaddaw.x $ACC, $vf21, $vf4 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmaddax.x $ACC, $vf21, $vf5 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmadday.x $ACC, $vf21, $vf5 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmaddaz.x $ACC, $vf21, $vf5 \n\t"
|
|
"vmul.x $vf21, $vf21, $vf22 \n\t"
|
|
"vmaddaw.x $ACC, $vf21, $vf5 \n\t"
|
|
"vmaddw.x $vf21, $vf6, $vf0 \n\t"
|
|
"qmfc2 $6, $vf21 \n\t"
|
|
"mtc1 $6, %0 \n\t"
|
|
"andi $8, $9, 1 \n\t"
|
|
"sll $9, $9, 2 \n\t"
|
|
"la $7, TYRA_MATH_ATAN_TABLE2 \n\t"
|
|
"add $9, $9, $7 \n\t"
|
|
"lw $7, 0x0($9) \n\t"
|
|
"mtc1 $7, %1 \n\t"
|
|
"beq $8, $0, _atan_05 \n\t"
|
|
"sub.s %0, %1, %0 \n\t"
|
|
"b _atan_06 \n\t"
|
|
"_atan_05: \n\t"
|
|
"add.s %0, %1, %0 \n\t"
|
|
"_atan_06: \n\t"
|
|
: "=&f"(r)
|
|
: "f"(x), "f"(y)
|
|
: "$6", "$7", "$8", "$9", "$f0", "$f1", "$f2");
|
|
|
|
return r;
|
|
}
|
|
|
|
float Math::randomf(const float& min, const float& max) {
|
|
float random = ((float)rand()) / (float)RAND_MAX;
|
|
float diff = max - min;
|
|
float r = random * diff;
|
|
return min + r;
|
|
}
|
|
|
|
int Math::randomi(const int& min, const int& max) {
|
|
return rand() % (max - min + 1) + min;
|
|
}
|
|
|
|
float Math::asin(float x) {
|
|
float r;
|
|
asm volatile(
|
|
"lui $9, 0x3f00 \n\t"
|
|
".set noreorder \n\t"
|
|
".align 3 \n\t"
|
|
"abs.s %0, %1 \n\t"
|
|
"lui $8, 0xbe22 \n\t"
|
|
"mtc1 $9, $f1 \n\t"
|
|
"ori $8, $8, 0xf983 \n\t"
|
|
"mtc1 $8, $f8 \n\t"
|
|
"lui $9, 0x4b00 \n\t"
|
|
"mtc1 $9, $f3 \n\t"
|
|
"lui $8, 0x3f80 \n\t"
|
|
"mtc1 $8, $f2 \n\t"
|
|
"mula.s %0, $f8 \n\t"
|
|
"msuba.s $f3, $f2 \n\t"
|
|
"madda.s $f3, $f2 \n\t"
|
|
"lui $8, 0x40c9 \n\t"
|
|
"msuba.s %0, $f8 \n\t"
|
|
"ori $8, 0x0fdb \n\t"
|
|
"msub.s %0, $f1, $f2 \n\t"
|
|
"lui $9, 0xc225 \n\t"
|
|
"abs.s %0, %0 \n\t"
|
|
"lui $10, 0x3e80 \n\t"
|
|
"mtc1 $10, $f7 \n\t"
|
|
"ori $9, 0x5de1 \n\t"
|
|
"sub.s %0, %0, $f7 \n\t"
|
|
"lui $10, 0x42a3 \n\t"
|
|
"mtc1 $8, $f3 \n\t"
|
|
"ori $10, 0x3458 \n\t"
|
|
"mtc1 $9, $f4 \n\t"
|
|
"lui $8, 0xc299 \n\t"
|
|
"mtc1 $10, $f5 \n\t"
|
|
"ori $8, 0x2663 \n\t"
|
|
"mul.s $f8, %0, %0 \n\t"
|
|
"lui $9, 0x421e \n\t"
|
|
"mtc1 $8, $f6 \n\t"
|
|
"ori $9, 0xd7bb \n\t"
|
|
"mtc1 $9, $f7 \n\t"
|
|
"nop \n\t"
|
|
"mul.s $f1, %0, $f8 \n\t"
|
|
"mul.s $f9, $f8, $f8 \n\t"
|
|
"mula.s $f3, %0 \n\t"
|
|
"mul.s $f2, $f1, $f8 \n\t"
|
|
"madda.s $f4, $f1 \n\t"
|
|
"mul.s $f1, $f1, $f9 \n\t"
|
|
"mul.s %0, $f2, $f9 \n\t"
|
|
"madda.s $f5, $f2 \n\t"
|
|
"madda.s $f6, $f1 \n\t"
|
|
"madd.s %0, $f7, %0 \n\t"
|
|
".set reorder \n\t"
|
|
: "=&f"(r)
|
|
: "f"(x)
|
|
: "$f1", "$f2", "$f3", "$f4", "$f5", "$f6", "$f7", "$f8", "$f9", "$8",
|
|
"$9", "$10");
|
|
return r;
|
|
}
|
|
|
|
bool Math::equalf(const float& a, const float& b, const float& epsilon) {
|
|
return fabs(a - b) < epsilon;
|
|
}
|
|
|
|
float Math::mod(const float& x, const float& y) {
|
|
/*
|
|
* Portable fmod(x,y) implementation for systems
|
|
* that don't have it. Adapted from code found here:
|
|
* http://www.opensource.apple.com/source/python/python-3/python/Python/fmod.c
|
|
*/
|
|
float i, f;
|
|
|
|
if (fabs(y) < 0.00001F) {
|
|
return 0.0F;
|
|
}
|
|
|
|
i = floorf(x / y);
|
|
f = x - i * y;
|
|
|
|
if ((x < 0.0f) != (y < 0.0f)) {
|
|
f = f - y;
|
|
}
|
|
|
|
return f;
|
|
}
|
|
|
|
float Math::acos(const float& x) {
|
|
float y = sqrt(1.0f - x * x);
|
|
float t = atan2(y, x);
|
|
return t;
|
|
}
|
|
|
|
float Math::sin(const float& x) { return cos(x - HALF_PI); }
|
|
|
|
float Math::tan(const float& x) { return sin(x) / cos(x); }
|
|
|
|
} // namespace Tyra
|