fixedmath.h

#pragma once
#include <stdint.h>
#include <psxgte.h>
#include <stdio.h>
#include <stdlib.h>

#define DP	10 //22.8 fixed point



//9.7 fixed point
const int16_t MAX_GRAY_8 = 255 << 7; 
const int16_t MAX_GRAY_6 = 63 << 7;
const int16_t MAX_GRAY_5 = 31 << 7;

const int DP_DIFF_12 = 12 - DP;

const int MAX_INT = 0xffffffff;

const int QUARTER_FP = 1 << (DP - 2);
const int HALF_FP = 1 << (DP - 1);
const int ONE_FP = 1 << DP;
const int TWO_FP = 2 << DP;
const int THREE_FP = 3 << DP;
const int FOUR_FP = 4 << DP;
const int FIVE_FP = 5 << DP;
const int TEN_FP = 10 << DP;

inline float FixedToFloat(int input, uint8_t dp)
{
	return ((float)input / (float)(1 << dp));
}

inline int FloatToFixed32(float num, uint8_t dp)
{
	return num * (float)(1 << dp) + (num >= 0 ? 0.5 : - 0.5);
}

inline int16_t FloatToFixed16(float num, uint8_t dp)
{
	return num * (float)(1 << dp) + (num >= 0 ? 0.5 : - 0.5);
}

inline float Q_rsqrt( float number )
{
	long i;
	float x2, y;
	const float threehalfs = 1.5F;

	x2 = number * 0.5F;
	y  = number;
	i  = * ( long * ) &y;                       // evil floating point bit level hacking
	i  = 0x5f3759df - ( i >> 1 );               // what the fuck?
	y  = * ( float * ) &i;
	y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
//	y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed

	return y;
}

inline int InvSquareRoot(int n, uint8_t dp)
{
	float f = FixedToFloat(n, dp);
	float inv = Q_rsqrt(f);

	return FloatToFixed32(inv, dp);
}

inline int SquareRoot(int n, uint8_t dp)
{
	float f = FixedToFloat(n, dp);
	float radf = 1.0f / Q_rsqrt(f);

	return FloatToFixed32(radf, dp);
}

//copy
inline void Vec32ToVec16(VECTOR* v1, SVECTOR* v2)
{
    v2->vx = v1->vx;
    v2->vy = v1->vy;
    v2->vz = v1->vz;
}

//copy
inline void Vec16ToVec32(SVECTOR* v1, VECTOR* v2)
{
    v2->vx = v1->vx;
    v2->vy = v1->vy;
    v2->vz = v1->vz;
}

inline int16_t MulFixed16_16(int16_t a, int16_t b, uint8_t dp)
{
	return ((a) * (b)) >> dp;
}

inline int MulFixed16_32(int16_t a, int16_t b, uint8_t dp)
{
	return (int)(((int)(a) * (int)(b)) >> dp);
}

inline int MulFixed32_32(int a, int b, uint8_t dp)
{
	return (int)(((int)(a) * (int)(b)) >> dp);
}

inline int64_t MulFixed32_64(int a, int b, uint8_t dp)
{
	return (int64_t)(((int)(a) * (int)(b)) >> dp);
}

inline int16_t DivFixed16_16(int16_t a, int16_t b, uint8_t dp)
{
	return ((a) << dp) / (b);
}

inline int DivFixed16_32(int16_t a, int16_t b, uint8_t dp)
{
	return ((int)(a) << dp) / (int)(b);
}

#define divfixed3232( a, b, c ) __asm__ volatile ( \
	"sll $t0, %1, 10;" \
	"div $t0, %2;" \
	"mflo %0;" \
	: "=r" (c) \
    : "r" (a), "r" (b) \
	: "t0", "hi", "lo", "memory")

inline int DivFixed32_32(int a, int b, uint8_t dp)
{
	int c;
	divfixed3232(a, b, c);

	return c;//(a << dp) / b;
}

inline int64_t DivFixed32_64(int a, int b, uint8_t dp)
{
	return ((int64_t)(a) << dp) / (int64_t)(b);
}

int Complement2(int a)
{
	// int b = ~a;
	// b++;

	//return (a ^ a) + 1;

	int b;

	__asm__ volatile 
	(
		"xor %0, %1, %2\n\t"
		"addi %0, %0, 1"
		: "=r" (b)
		: "r" (a), "r" (MAX_INT)
	);

	return b;
}

inline void VecInvertFixed16(SVECTOR* v, SVECTOR* r)
{
	r->vx = Complement2(v->vx);
	r->vy = Complement2(v->vy);
	r->vz = Complement2(v->vz);
}

inline void VecInvertFixed32(VECTOR* v, VECTOR* r)
{
	r->vx = Complement2(v->vx);
	r->vy = Complement2(v->vy);
	r->vz = Complement2(v->vz);
}

inline void VecScaleFixed16(SVECTOR* v, int16_t scale, SVECTOR* r)
{
	r->vx = MulFixed16_16(v->vx, scale, DP);
	r->vy = MulFixed16_16(v->vy, scale, DP);
	r->vz = MulFixed16_16(v->vz, scale, DP);
}

inline void VecScaleFixed32(VECTOR* v, int scale, VECTOR* r)
{
	r->vx = MulFixed32_32(v->vx, scale, DP);
	r->vy = MulFixed32_32(v->vy, scale, DP);
	r->vz = MulFixed32_32(v->vz, scale, DP);
}

inline void VecAddFixed16(SVECTOR* v1, SVECTOR* v2, SVECTOR* r)
{
	r->vx = v1->vx + v2->vx;
	r->vy = v1->vy + v2->vy;
	r->vz = v1->vz + v2->vz;
}

inline void VecAddFixed32(VECTOR* v1, VECTOR* v2, VECTOR* r)
{
	r->vx = v1->vx + v2->vx;
	r->vy = v1->vy + v2->vy;
	r->vz = v1->vz + v2->vz;
}

inline void VecSubFixed16(SVECTOR* v1, SVECTOR* v2, SVECTOR* r)
{
	r->vx = v1->vx - v2->vx;
	r->vy = v1->vy - v2->vy;
	r->vz = v1->vz - v2->vz;
}

inline void VecSubFixed32(VECTOR* v1, VECTOR* v2, VECTOR* r)
{
	r->vx = v1->vx - v2->vx;
	r->vy = v1->vy - v2->vy;
	r->vz = v1->vz - v2->vz;
}

inline int DotProductFixed16(SVECTOR* v1, SVECTOR* v2)
{
	return MulFixed16_32(v1->vx, v2->vx, DP) + MulFixed16_32(v1->vy, v2->vy, DP) + MulFixed16_32(v1->vz, v2->vz, DP);
}

inline int64_t DotProductFixed32(VECTOR* v1, VECTOR* v2)
{
	return MulFixed32_64(v1->vx, v2->vx, DP) + MulFixed32_64(v1->vy, v2->vy, DP) + MulFixed32_64(v1->vz, v2->vz, DP);
}

inline int SQRTNewtonRaphson(int n)
{
	int guess = n >> 1;

	//3 iterazioni
	guess = (guess + DivFixed32_32(n, guess, DP)) >> 1;
	guess = (guess + DivFixed32_32(n, guess, DP)) >> 1;
	//guess = (guess + DivFixed32_32(n, guess, DP)) >> 1;

	return guess;
}

inline void NormFixed16(SVECTOR* v, SVECTOR* n)
{
	int16_t den = SquareRoot12(DotProductFixed16(v, v) << DP_DIFF_12) >> DP_DIFF_12;
	int16_t invDen = DivFixed16_32(ONE_FP, den, DP);

	n->vx = MulFixed16_32(invDen, v->vx, DP);
	n->vy = MulFixed16_32(invDen, v->vy, DP);
	n->vz = MulFixed16_32(invDen, v->vz, DP);

	// n->vx = DivFixed16_32(v->vx, den, DP);
	// n->vy = DivFixed16_32(v->vy, den, DP);
	// n->vz = DivFixed16_32(v->vz, den, DP);

	// int16_t inv = InvSquareRoot(DotProductFixed16(v, v), DP);

	// n->vx = MulFixed16_32(v->vx, inv, DP);
	// n->vy = MulFixed16_32(v->vy, inv, DP);
	// n->vz = MulFixed16_32(v->vz, inv, DP);
}

inline void NormFixed32(VECTOR* v, SVECTOR* n)
{
	//int den = SquareRoot12(DotProductFixed32(v, v) << DP_DIFF_12) >> DP_DIFF_12;
	int den = SQRTNewtonRaphson(DotProductFixed32(v, v));
	int invDen = DivFixed32_32(ONE_FP, den, DP);

	n->vx = MulFixed32_64(invDen, v->vx, DP);
	n->vy = MulFixed32_64(invDen, v->vy, DP);
	n->vz = MulFixed32_64(invDen, v->vz, DP);

	// n->vx = DivFixed32_64(v->vx, den, DP);
	// n->vy = DivFixed32_64(v->vy, den, DP);
	// n->vz = DivFixed32_64(v->vz, den, DP);

	// int inv = InvSquareRoot(DotProductFixed32(v, v), DP);

	// n->vx = MulFixed32_64(v->vx, inv, DP);
	// n->vy = MulFixed32_64(v->vy, inv, DP);
	// n->vz = MulFixed32_64(v->vz, inv, DP);
}

//a ray reflects from the normal of the surface
inline void VecReflect16(SVECTOR* v, SVECTOR* n, SVECTOR* r) 
{
    SVECTOR nscaled;
	int scaling = DotProductFixed16(v, n) << 1;//MulFixed32_32(TWO, DotProductFixed16(v, n), DP);
	VecScaleFixed16(n, scaling, &nscaled);
    VecSubFixed16(v, &nscaled, r);
}

inline void VecReflect32(VECTOR* v, SVECTOR* n, VECTOR* r) 
{
    VECTOR nscaled;
    VECTOR n32;

    Vec16ToVec32(n, &n32);

	int scaling = DotProductFixed32(v, &n32) << 2; //MulFixed32_32(TWO, DotProductFixed32(v, &n32), DP);
	VecScaleFixed32(&n32, scaling, &nscaled);
    VecSubFixed32(v, &nscaled, r);
}



inline int max(int a, int b)
{ 
    return (a > b) ? a : b;
}

inline int min(int a, int b)
{
    return (a < b) ? a : b;
}

inline int pow(int a, int b)
{
	int i;
	int c = a;
	
	for(i = 0; i < b; i++)
	{
		//c = MulFixed32(c, a, DP);
	}

	return c;
}