3dcam-headers/math.c

272 lines
5.3 KiB
C

#include "math.h"
// Stolen from grumpycoder
// this is from here : https://github.com/grumpycoders/Balau/blob/master/tests/test-Handles.cc#L20-L102
// precalc costable
static int m_cosTable[512];
static const unsigned int DC_2PI = 2048;
static const unsigned int DC_PI = 1024;
static const unsigned int DC_PI2 = 512;
// f(n) = cos(n * 2pi / 2048) <- 2048 is == DC_2PI value
// f(n) = 2 * f(1) * f(n - 1) - f(n - 2)
void generateTable(void){
m_cosTable[0] = 16777216; // 2^24 * cos(0 * 2pi / 2048) => 2^24 * 1 = 2^24 : here, 2^24 defines the precision we want after the decimal point
static const long long C = 16777137; // 2^24 * cos(1 * 2pi / 2048) = C = f(1);
m_cosTable[1] = C;
for (int i = 2; i < 512; i++){
m_cosTable[i] = ((C * m_cosTable[i - 1]) >> 23) - m_cosTable[i - 2];
m_cosTable[511] = 0;
}
};
// A few notes on the following code :
int ncos(unsigned int t) {
t %= DC_2PI;
int r;
if (t < DC_PI2) {
r = m_cosTable[t];
} else if (t < DC_PI) {
r = -m_cosTable[DC_PI - 1 - t];
} else if (t < (DC_PI + DC_PI2)) {
r = -m_cosTable[t - DC_PI];
} else {
r = m_cosTable[DC_2PI - 1 - t];
};
return r >> 12;
};
// sin(x) = cos(x - pi / 2)
int nsin(unsigned int t) {
t %= DC_2PI;
if (t < DC_PI2){
return ncos(t + DC_2PI - DC_PI2);
};
return ncos(t - DC_PI2);
};
// https://github.com/Arsunt/TR2Main/blob/411cacb35914c616cb7960c0e677e00c71c7ee88/3dsystem/phd_math.cpp#L432
long long patan(long x, long y){
long long result;
int swapBuf;
int flags = 0;
// if either x or y are 0, return 0
if( x == 0 && y == 0){
return 0;
}
if( x < 0 ) {
flags |= 4;
x = -x;
}
if ( y < 0 ) {
flags |= 2;
y = -y;
}
if ( y > x ) {
flags |= 1;
SWAP(x, y ,swapBuf);
}
result = AtanBaseTable[flags] + AtanAngleTable[0x800 * y / x];
if ( result < 0 ){
result = -result;
return result;
}
};
u_int psqrt(u_int n){
u_int result = 0;
u_int base = 0x40000000;
u_int basedResult;
for( ; base != 0; base >>= 2 ) {
for( ; base != 0; base >>= 2 ) {
basedResult = base + result;
result >>= 1;
if( basedResult > n ) {
break;
}
n -= basedResult;
result |= base;
}
}
return result;
};
// From : https://github.com/grumpycoders/pcsx-redux/blob/7438e9995833db5bc1e14da735bbf9dc78300f0b/src/mips/shell/math.h
int32_t dMul(int32_t a, int32_t b) {
long long r = a;
r *= b;
return r >> 24;
};
// standard lerp function
// s = source, an arbitrary number up to 2^24
// d = destination, an arbitrary number up to 2^24
// p = position, a number between 0 and 256, inclusive
// p = 0 means output = s
// p = 256 means output = d
uint32_t lerpU(uint32_t start, uint32_t dest, unsigned pos) {
return (start * (256 - pos) + dest * pos) >> 8;
};
int32_t lerpS(int32_t start, int32_t dest, unsigned pos) {
return (start * (256 - pos) + dest * pos) >> 8;
};
// start, dest and pos have to be << x, then the result has to be >> x where x defines precision:
// precision = 2^24 - 2^x
// << x : 0 < pos < precision
// https://discord.com/channels/642647820683444236/646765703143227394/811318550978494505
// my angles are between 0 and 2048 (full circle), so 2^11 for the range of angles; with numbers on a 8.24 representation, a 1.0 angle (or 2pi) means it's 2^24, so to "convert" my angles from 8.24 to my internal discrete cos, I only have to shift by 13
int32_t lerpD(int32_t start, int32_t dest, int32_t pos) {
return dMul(start, 16777216 - pos) + dMul(dest, pos);
};
long long lerpL(long long start, long long dest, long long pos) {
return dMul( (start << 12), 16777216 - (pos << 12) ) + dMul((dest << 12), (pos << 12) ) >> 12;
};
int lerp(int start, int end, int factor){
// lerp interpolated cam movement
// InBetween = Value 1 + ( ( Value2 - Value1 ) * lerpValue ) ;
// lerpValue should be a float between 0 and 1.
return ( start ) + (( end - start ) * factor ) >> 12;
};
long long easeIn(long long i){
return ((i << 7) * (i << 7) * (i << 7) / 32 ) >> 19;
};
int easeOut(int i){
return (4096 >> 7) - ((4096 - (i << 7)) * (4096 - (i << 7))) >> 12;
};
//~ int easeInOut(int i, int div){
//~ return lerp(easeIn(i, div), easeOut(i) , i);
//~ };
SVECTOR SVlerp(SVECTOR start, SVECTOR end, int factor){
SVECTOR output = {0,0,0,0};
output.vx = lerp(start.vx, end.vx, factor);
output.vy = lerp(start.vy, end.vy, factor);
output.vz = lerp(start.vz, end.vz, factor);
return output;
};
VECTOR getVectorTo( VECTOR actor, VECTOR target ) {
VECTOR direction = { subVector(target, actor) };
VECTOR Ndirection = {0,0,0,0};
u_int distSq = (direction.vx * direction.vx) + (direction.vz * direction.vz);
direction.pad = psqrt(distSq);
VectorNormal(&direction, &Ndirection);
return Ndirection ;
};