mirror of
https://github.com/CTurt/dsgmLib.git
synced 2025-06-18 22:55:33 -04:00
113 lines
3.4 KiB
C
113 lines
3.4 KiB
C
//
|
|
// trig.cpp : Basic trigonometry routines.
|
|
//
|
|
//! \file trig.cpp
|
|
//! \author J Vijn
|
|
//! \date 20080130 - 20080210
|
|
//
|
|
/* === NOTES ===
|
|
Multiple atan2's present. Select the one you want.
|
|
Best performance tends to be with atan2Lerp or possibly atan2Tonc.
|
|
If your division really sucks, use atan2Cordic.
|
|
|
|
This module still needs a header for some system-specific basics
|
|
before you can use it. Specifically, ALIGN and QDIV will needs some
|
|
extra effort.
|
|
*/
|
|
|
|
/*
|
|
dsgmLib adaptations:
|
|
|
|
- Only atan2Lerp was needed, all other trigonometry functions are
|
|
already present in libnds and were removed from this library
|
|
|
|
- Upgraded the code from C++ to C
|
|
*/
|
|
|
|
#include "DSGM.h"
|
|
|
|
// --------------------------------------------------------------------
|
|
// MACROS / INLINES
|
|
// --------------------------------------------------------------------
|
|
|
|
// Get the octant a coordinate pair is in.
|
|
#define OCTANTIFY(_x, _y, _o) do { \
|
|
int _t; _o= 0; \
|
|
if(_y< 0) { _x= -_x; _y= -_y; _o += 4; } \
|
|
if(_x<= 0) { _t= _x; _x= _y; _y= -_t; _o += 2; } \
|
|
if(_x<=_y) { _t= _y-_x; _x= _x+_y; _y= _t; _o += 1; } \
|
|
} while(0);
|
|
|
|
// Special-case division because I need a little more control
|
|
// than divf32 offers
|
|
static inline int QDIV(int num, int den, const int bits) {
|
|
while(REG_DIVCNT & DIV_BUSY);
|
|
REG_DIVCNT = DIV_64_32;
|
|
|
|
REG_DIV_NUMER = ((int64)num)<<bits;
|
|
REG_DIV_DENOM_L = den;
|
|
|
|
while(REG_DIVCNT & DIV_BUSY);
|
|
|
|
return (REG_DIV_RESULT_L);
|
|
}
|
|
|
|
// --------------------------------------------------------------------
|
|
// CONSTANTS
|
|
// --------------------------------------------------------------------
|
|
|
|
static const uint ATAN_ONE = 0x1000, ATAN_FP = 12, ATAN_PI = BRAD_PI;
|
|
#define ATANLUT_STRIDE (ATAN_ONE / 0x80)
|
|
static const uint ATANLUT_STRIDE_SHIFT = 5;
|
|
|
|
// --------------------------------------------------------------------
|
|
// LUTS
|
|
// --------------------------------------------------------------------
|
|
|
|
//{{ATANLUT
|
|
extern const unsigned short atanLUT[130];
|
|
|
|
// Arctangens LUT. Interval: [0, 1] (one=128); PI=0x20000
|
|
const unsigned short atanLUT[130] ALIGN(4)=
|
|
{
|
|
0x0000,0x0146,0x028C,0x03D2,0x0517,0x065D,0x07A2,0x08E7,
|
|
0x0A2C,0x0B71,0x0CB5,0x0DF9,0x0F3C,0x107F,0x11C1,0x1303,
|
|
0x1444,0x1585,0x16C5,0x1804,0x1943,0x1A80,0x1BBD,0x1CFA,
|
|
0x1E35,0x1F6F,0x20A9,0x21E1,0x2319,0x2450,0x2585,0x26BA,
|
|
0x27ED,0x291F,0x2A50,0x2B80,0x2CAF,0x2DDC,0x2F08,0x3033,
|
|
0x315D,0x3285,0x33AC,0x34D2,0x35F6,0x3719,0x383A,0x395A,
|
|
0x3A78,0x3B95,0x3CB1,0x3DCB,0x3EE4,0x3FFB,0x4110,0x4224,
|
|
0x4336,0x4447,0x4556,0x4664,0x4770,0x487A,0x4983,0x4A8B,
|
|
// 64
|
|
0x4B90,0x4C94,0x4D96,0x4E97,0x4F96,0x5093,0x518F,0x5289,
|
|
0x5382,0x5478,0x556E,0x5661,0x5753,0x5843,0x5932,0x5A1E,
|
|
0x5B0A,0x5BF3,0x5CDB,0x5DC1,0x5EA6,0x5F89,0x606A,0x614A,
|
|
0x6228,0x6305,0x63E0,0x64B9,0x6591,0x6667,0x673B,0x680E,
|
|
0x68E0,0x69B0,0x6A7E,0x6B4B,0x6C16,0x6CDF,0x6DA8,0x6E6E,
|
|
0x6F33,0x6FF7,0x70B9,0x717A,0x7239,0x72F6,0x73B3,0x746D,
|
|
0x7527,0x75DF,0x7695,0x774A,0x77FE,0x78B0,0x7961,0x7A10,
|
|
0x7ABF,0x7B6B,0x7C17,0x7CC1,0x7D6A,0x7E11,0x7EB7,0x7F5C,
|
|
// 128
|
|
0x8000,0x80A2
|
|
};
|
|
//}}ATANLUT
|
|
|
|
// Basic lookup+linear interpolation for atan2.
|
|
// Returns [0,2pi), where pi ~ 0x4000.
|
|
uint atan2Lerp(int x, int y) {
|
|
if(y == 0) return (x >= 0 ? 0 : BRAD_PI);
|
|
|
|
int phi, fa, fb, h;
|
|
uint t;
|
|
|
|
OCTANTIFY(x, y, phi);
|
|
phi *= BRAD_PI/4;
|
|
|
|
t = QDIV(y, x, ATAN_FP);
|
|
h = t % ATANLUT_STRIDE;
|
|
fa = atanLUT[t/ATANLUT_STRIDE ];
|
|
fb = atanLUT[t/ATANLUT_STRIDE+1];
|
|
|
|
return phi + ((fa + ((fb-fa)*h >> ATANLUT_STRIDE_SHIFT))>>3);
|
|
}
|