mirror of
https://github.com/CloverHackyColor/CloverBootloader.git
synced 2024-12-11 14:28:08 +01:00
400 lines
7.9 KiB
C++
400 lines
7.9 KiB
C++
//
|
|
// FloatLib.c
|
|
//
|
|
//
|
|
// Created by Slice on 20.06.2018.
|
|
//
|
|
|
|
#include "FloatLib.h"
|
|
#include "IO.h"
|
|
|
|
|
|
#define memcpy(dest,source,count) CopyMem(dest,(void*)source,(UINTN)(count))
|
|
//#define fabsf(x) ((x >= 0.0f)?x:(-x))
|
|
#define fabsf(x) FabsF(x)
|
|
|
|
//we will assume sqrt(abs(x))
|
|
float SqrtF(float X)
|
|
{
|
|
/*
|
|
struct FloatInt {
|
|
union {
|
|
INT32 i;
|
|
float f;
|
|
} fi;
|
|
};
|
|
*/
|
|
if (X == 0.0f) {
|
|
return 0.0f;
|
|
} else if (X < 0.0f) {
|
|
X = -X;
|
|
}
|
|
// struct FloatInt Y;
|
|
float Yf;
|
|
Yf = X * 0.3f;
|
|
// Y.i = Y.i >> 1; // dirty hack - first iteration
|
|
//do six iterations
|
|
Yf = Yf * 0.5f + X / (Yf * 2.0f);
|
|
Yf = Yf * 0.5f + X / (Yf * 2.0f);
|
|
Yf = Yf * 0.5f + X / (Yf * 2.0f);
|
|
Yf = Yf * 0.5f + X / (Yf * 2.0f);
|
|
Yf = Yf * 0.5f + X / (Yf * 2.0f);
|
|
Yf = Yf * 0.5f + X / (Yf * 2.0f);
|
|
return Yf;
|
|
}
|
|
|
|
float CosF(float X);
|
|
|
|
//we know sin is odd
|
|
float SinF(float X)
|
|
{
|
|
INTN Period;
|
|
float X2;
|
|
float Sign = 1.0f;
|
|
|
|
if (X < 0.0f) {
|
|
X = -X;
|
|
Sign = -1.0f;
|
|
}
|
|
Period = (INTN)(X / PI2);
|
|
X = X - Period * PI2;
|
|
if (X > PI) {
|
|
X = X - PI;
|
|
Sign *= -1.0f;
|
|
}
|
|
if (X > PI5) {
|
|
X = PI - X;
|
|
}
|
|
if (X > PI * 0.25f) {
|
|
return (Sign*CosF(PI5 - X));
|
|
}
|
|
X2 = X * X;
|
|
return (Sign*(X - X2 * X / 6.0f + X2 * X2 * X / 120.0f));
|
|
}
|
|
|
|
//we know cos is even
|
|
float CosF(float X)
|
|
{
|
|
INTN Period;
|
|
float Sign = 1.0f;
|
|
float X2;
|
|
|
|
if (X < 0.0f) {
|
|
X = -X;
|
|
}
|
|
Period = (INTN)(X / PI2);
|
|
X = X - Period * PI2;
|
|
if (X > PI) {
|
|
X = PI - X;
|
|
Sign = -1.0f;
|
|
}
|
|
if (X > PI5) {
|
|
X = PI - X;
|
|
Sign *= -1.0f;
|
|
}
|
|
if (X > PI * 0.25f) {
|
|
return (Sign*SinF(PI5 - X));
|
|
}
|
|
X2 = X * X;
|
|
return (Sign * (1.0f - X2 * 0.5f + X2 * X2 / 24.0f));
|
|
}
|
|
|
|
float TanF(float X)
|
|
{
|
|
float Y = CosF(X);
|
|
if (Y == 0.0f) {
|
|
Y = 1.0e-37f;
|
|
}
|
|
return SinF(X)/Y;
|
|
}
|
|
|
|
float PowF(float x, INTN n)
|
|
{
|
|
float Data = x;
|
|
if (n > 0) {
|
|
while (n > 0) {
|
|
Data *= 10.0f;
|
|
n--;
|
|
}
|
|
} else {
|
|
while (n < 0) {
|
|
Data *= 0.1f;
|
|
n++;
|
|
}
|
|
}
|
|
return Data;
|
|
}
|
|
|
|
float CeilF(float X)
|
|
{
|
|
INT32 I = (INT32)X;
|
|
return (float)(++I);
|
|
}
|
|
|
|
float FloorF(float X)
|
|
{
|
|
INT32 I = (INT32)X;
|
|
return (float)I;
|
|
}
|
|
|
|
float ModF(float X, float Y)
|
|
{
|
|
INT32 I = (INT32)(X / Y);
|
|
return (X - (float)I * Y);
|
|
}
|
|
|
|
float AcosF(float X)
|
|
{
|
|
float X2 = X * X;
|
|
float res = 0.f, Y = 0.f;
|
|
INTN Sign = 0;
|
|
|
|
if (X2 < 0.3f) {
|
|
Y = X * (1.0f + X2 / 6.0f + X2 * X2 * (3.0f / 40.0f));
|
|
return (PI5 - Y);
|
|
} else if (X2 >= 1.0f) {
|
|
return 0.0f;
|
|
} else {
|
|
if (X < 0) {
|
|
X = -X;
|
|
Sign = 1;
|
|
}
|
|
Y = 1.0f - X; //for X ~ 1
|
|
X2 = Y * (2.0f + Y * (1.0f / 3.0f + Y * (4.0f / 45.0f + Y / 35.0f))); //Dwight, form.508
|
|
res = SqrtF(X2);
|
|
if (Sign) {
|
|
res = PI - res;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
float AtanF(float X) //assume 0.0 < X < 1.0
|
|
{
|
|
float Eps = 1.0e-8f;
|
|
int i = 1;
|
|
float X2 = X * X;
|
|
float D = X;
|
|
float Y = 0.f;
|
|
float sign = 1.0f;
|
|
|
|
if (X > 0.5f) {
|
|
//make here arctg(1-x)
|
|
X = 1.0f - D;
|
|
X2 = X * X;
|
|
Y = PI4 - X * 0.5f - X2 * 0.25f - X * X2 * 0.25f * ( 1.f / 3.f - X2 * (0.1f + X / 12.f + X2 / 28.f));
|
|
} else {
|
|
// Y = X * (1 - X2 * ( 1.0f / 3.0f - X2 * (1.0f / 5.0f - X2 * ( 1.0f / 7.0f))));
|
|
for (i = 1; i < 50; i += 2) {
|
|
Y += (D * sign / i);
|
|
D *= X2;
|
|
if (D < Eps) {
|
|
break;
|
|
}
|
|
sign = - sign;
|
|
}
|
|
}
|
|
return Y;
|
|
}
|
|
|
|
float Atan2F(float Y, float X) //result -pi..+pi
|
|
{
|
|
float sign = (((X >= 0.0f) && (Y < 0.0f)) ||
|
|
((X < 0.0f) && (Y >= 0.0f)))?-1.0f:1.0f;
|
|
float PP = 0.f;
|
|
float res = 0.f;
|
|
//1,1 = pi4 1,-1=pi34 -1,-1=-pi34 -1,1=-pi4
|
|
if (X < 0.f) {
|
|
PP = PI;
|
|
}
|
|
X = (X >= 0.0f)?X:(-X);
|
|
Y = (Y >= 0.0f)?Y:(-Y);
|
|
if (Y < X) {
|
|
res = AtanF(Y / X);
|
|
} else if (X == 0.0f) {
|
|
res = PI5;
|
|
} else {
|
|
res = (PI5 - AtanF(X / Y));
|
|
}
|
|
return sign * (res - PP);
|
|
}
|
|
|
|
/*
|
|
RETURN_STATUS
|
|
EFIAPI
|
|
AsciiStrDecimalToUintnS (
|
|
IN CONST CHAR8 *String,
|
|
OUT CHAR8 **EndPointer, OPTIONAL
|
|
OUT UINTN *Data
|
|
);
|
|
*/
|
|
RETURN_STATUS
|
|
AsciiStrToFloat(IN CONST CHAR8 *String,
|
|
OUT CHAR8 **EndPointer, OPTIONAL
|
|
OUT float *Data)
|
|
{
|
|
UINTN Temp = 0;
|
|
INTN Sign = 1;
|
|
float Mantissa, Ftemp;
|
|
CHAR8* TmpStr = NULL;
|
|
RETURN_STATUS Status = RETURN_SUCCESS;
|
|
if (EndPointer != NULL) {
|
|
*EndPointer = (CHAR8 *) String;
|
|
}
|
|
//
|
|
// Ignore the pad spaces (space or tab)
|
|
//
|
|
while ((*String == ' ') || (*String == '\t')) {
|
|
String++;
|
|
}
|
|
if (*String == '-') {
|
|
Sign = -1;
|
|
String++;
|
|
} else if (*String == '+') {
|
|
String++;
|
|
}
|
|
|
|
Status = AsciiStrDecimalToUintnS(String, &TmpStr, &Temp);
|
|
Mantissa = (float)Temp;
|
|
String = TmpStr;
|
|
if (*String == '.') {
|
|
String++;
|
|
Temp = 0;
|
|
Status = AsciiStrDecimalToUintnS(String, &TmpStr, &Temp);
|
|
Ftemp = (float)Temp;
|
|
while (String != TmpStr) {
|
|
if (*String == '\0') {
|
|
break;
|
|
}
|
|
Ftemp *= 0.1f;
|
|
String++;
|
|
}
|
|
Mantissa += Ftemp;
|
|
}
|
|
|
|
if ((*String == 'E') || (*String == 'e')){
|
|
INTN ExpSign = 1;
|
|
String++;
|
|
if (*String == '-') {
|
|
ExpSign = -1;
|
|
String++;
|
|
} else if (*String == '+') {
|
|
String++;
|
|
}
|
|
Temp = 0;
|
|
Status = AsciiStrDecimalToUintnS(String, &TmpStr, &Temp);
|
|
if (Status == RETURN_SUCCESS) {
|
|
Ftemp = PowF(10.0f, ExpSign * Temp);
|
|
Mantissa *= Ftemp;
|
|
}
|
|
}
|
|
*Data = (Sign > 0)?Mantissa:-Mantissa;
|
|
if (EndPointer != NULL) {
|
|
*EndPointer = (CHAR8 *) TmpStr;
|
|
}
|
|
return RETURN_SUCCESS;
|
|
}
|
|
|
|
/*
|
|
//Slice - this is my replacement for standard
|
|
qsort(void* Array, int Num, size_t Size,
|
|
int (*compare)(void* a, void* b))
|
|
usage qsort(Array, Num, sizeof(*Array), compare);
|
|
where for example
|
|
int compare(void *a, void* b)
|
|
{
|
|
if (*(float*)a > *(float*)b) return 1;
|
|
if (*(float*)a < *(float*)b) return -1;
|
|
return -0;
|
|
}
|
|
*/
|
|
#if 0
|
|
VOID QuickSort(VOID* Array, INTN Low, INTN High, INTN Size, INTN (*compare)(CONST VOID* a, CONST VOID* b)) {
|
|
INTN i = Low, j = High;
|
|
VOID *Med, *Temp;
|
|
Med = Array + ((Low + High) / 2) * Size; // Central element, just pointer
|
|
Temp = (__typeof__(Temp))AllocatePool(Size);
|
|
// Sort around center
|
|
while (i <= j)
|
|
{
|
|
while (compare((const void*)(Array+i*Size), (const void*)Med) == -1) i++;
|
|
while (compare((const void*)(Array+j*Size), (const void*)Med) == 1) j--;
|
|
// Change
|
|
if (i <= j) {
|
|
memcpy(Temp, Array+i*Size, Size);
|
|
memcpy(Array+i*Size, Array+j*Size, Size);
|
|
memcpy(Array+j*Size, Temp, Size);
|
|
i++;
|
|
j--;
|
|
}
|
|
}
|
|
FreePool(Temp);
|
|
// Recursion
|
|
if (j > Low) QuickSort(Array, Low, j, Size, compare);
|
|
if (High > i) QuickSort(Array, i, High, Size, compare);
|
|
}
|
|
|
|
//S must be allocated before use
|
|
VOID AsciiSPrintFloat(CHAR8* S, INTN N, CHAR8* F, float X)
|
|
{
|
|
INTN I, Fract;
|
|
float D;
|
|
if (!S) {
|
|
return;
|
|
}
|
|
|
|
I = (INTN)X;
|
|
D = (float)I;
|
|
Fract = fabsf((X - D) * 1000000.0f);
|
|
AsciiSPrint(S, N, "%D.%06D", I, (INTN)Fract);
|
|
}
|
|
#endif
|
|
|
|
CHAR16* PoolPrintFloat(float X)
|
|
{
|
|
INTN I, Fract;
|
|
CHAR8 S = ' ';
|
|
float D;
|
|
I = (INTN)X;
|
|
D = (float)I;
|
|
if (I == 0 && X < 0) {
|
|
S = '-';
|
|
}
|
|
Fract = (INTN)fabsf((X - D) * 1000000.0f);
|
|
return PoolPrint(L"%c%d.%06d", S, I, Fract);
|
|
}
|
|
|
|
static UINT32 seed = 12345;
|
|
float rndf() //expected 0..1
|
|
{
|
|
// UINT16 Rand = 0;
|
|
// AsmRdRand16(&Rand); //it's a pity panic
|
|
// return (float)Rand / 65536.f;
|
|
seed = seed * 214013 + 2531011;
|
|
float x = (float)seed / 4294967296.0f;
|
|
return x;
|
|
}
|
|
|
|
int dither(float x, int level)
|
|
{
|
|
if (!level) {
|
|
return (int)x;
|
|
}
|
|
int i = (int)(x) * level; //5.1 * 4 = 20.4, 5.8 * 4 = 23.2|i=20
|
|
float dx = x * level - (float)(i); //0.4, 3.2
|
|
i /= level;
|
|
if (dx > rndf() * level) {
|
|
i += (int)((0.9999f+rndf())*level); //because rndf has mean value 0.5, but (int)rnd=0
|
|
}
|
|
return i;
|
|
}
|
|
//there is
|
|
#if 0
|
|
BOOLEAN
|
|
EFIAPI
|
|
AsmRdRand16 (
|
|
OUT UINT16 *Rand
|
|
);
|
|
#endif
|