path: root/third_party/gldc/src/sh4_math.h

// ---- sh4_math.h - SH7091 Math Module ----
//
// Version 1.1.3
//
// This file is part of the DreamHAL project, a hardware abstraction library
// primarily intended for use on the SH7091 found in hardware such as the SEGA
// Dreamcast game console.
//
// This math module is hereby released into the public domain in the hope that it
// may prove useful. Now go hit 60 fps! :)
//
// --Moopthehedgehog
//

// Notes:
// - GCC 4 users have a different return type for the fsca functions due to an
//  internal compiler error regarding complex numbers; no issue under GCC 9.2.0
// - Using -m4 instead of -m4-single-only completely breaks the matrix and
//  vector operations
// - Function inlining must be enabled and not blocked by compiler options such
//  as -ffunction-sections, as blocking inlining will result in significant
//  performance degradation for the vector and matrix functions employing a
//  RETURN_VECTOR_STRUCT return type. I have added compiler hints and attributes
//  "static inline __attribute__((always_inline))" to mitigate this, so in most
//  cases the functions should be inlined regardless. If in doubt, check the
//  compiler asm output!
//

#ifndef __SH4_MATH_H_
#define __SH4_MATH_H_

#define GNUC_FSCA_ERROR_VERSION 4

//
// Fast SH4 hardware math functions
//
//
// High-accuracy users beware, the fsrra functions have an error of +/- 2^-21
// per http://www.shared-ptr.com/sh_insns.html
//

//==============================================================================
// Definitions
//==============================================================================
//
// Structures, useful definitions, and reference comments
//

// Front matrix format:
//
//    FV0 FV4 FV8  FV12
//    --- --- ---  ----
//  [ fr0 fr4 fr8  fr12 ]
//  [ fr1 fr5 fr9  fr13 ]
//  [ fr2 fr6 fr10 fr14 ]
//  [ fr3 fr7 fr11 fr15 ]
//
// Back matrix, XMTRX, is similar, although it has no FVn vector groups:
//
//  [ xf0 xf4 xf8  xf12 ]
//  [ xf1 xf5 xf9  xf13 ]
//  [ xf2 xf6 xf10 xf14 ]
//  [ xf3 xf7 xf11 xf15 ]
//

typedef struct __attribute__((aligned(32))) {
  float fr0;
  float fr1;
  float fr2;
  float fr3;
  float fr4;
  float fr5;
  float fr6;
  float fr7;
  float fr8;
  float fr9;
  float fr10;
  float fr11;
  float fr12;
  float fr13;
  float fr14;
  float fr15;
} ALL_FLOATS_STRUCT;

// Return structs should be defined locally so that GCC optimizes them into
// register usage instead of memory accesses.
typedef struct {
  float z1;
  float z2;
  float z3;
  float z4;
} RETURN_VECTOR_STRUCT;

#if __GNUC__ <= GNUC_FSCA_ERROR_VERSION
typedef struct {
  float sine;
  float cosine;
} RETURN_FSCA_STRUCT;
#endif

// Identity Matrix
//
//    FV0 FV4 FV8 FV12
//    --- --- --- ----
//  [  1   0   0   0  ]
//  [  0   1   0   0  ]
//  [  0   0   1   0  ]
//  [  0   0   0   1  ]
//

static const ALL_FLOATS_STRUCT MATH_identity_matrix = {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};

// Constants
#define MATH_pi 3.14159265358979323846264338327950288419716939937510f
#define MATH_e 2.71828182845904523536028747135266249775724709369995f
#define MATH_phi 1.61803398874989484820458683436563811772030917980576f

//==============================================================================
// Basic math functions
//==============================================================================
//
// The following functions are available.
// Please see their definitions for other usage info, otherwise they may not
// work for you.
//
/*
  // |x|
  float MATH_fabs(float x)

  // sqrt(x)
  float MATH_fsqrt(float x)

  // a*b+c
  float MATH_fmac(float a, float b, float c)

  // a*b-c
  float MATH_fmac_Dec(float a, float b, float c)

  // fminf() - return the min of two floats
  // This doesn't check for NaN
  float MATH_Fast_Fminf(float a, float b)

  // fmaxf() - return the max of two floats
  // This doesn't check for NaN
  float MATH_Fast_Fmaxf(float a, float b)

  // Fast floorf() - return the nearest integer <= x as a float
  // This doesn't check for NaN
  float MATH_Fast_Floorf(float x)

  // Fast ceilf() - return the nearest integer >= x as a float
  // This doesn't check for NaN
  float MATH_Fast_Ceilf(float x)

  // Very fast floorf() - return the nearest integer <= x as a float
  // Inspired by a cool trick I came across here:
  // https://www.codeproject.com/Tips/700780/Fast-floor-ceiling-functions
  // This doesn't check for NaN
  float MATH_Very_Fast_Floorf(float x)

  // Very fast ceilf() - return the nearest integer >= x as a float
  // Inspired by a cool trick I came across here:
  // https://www.codeproject.com/Tips/700780/Fast-floor-ceiling-functions
  // This doesn't check for NaN
  float MATH_Very_Fast_Ceilf(float x)
*/

// |x|
// This one works on ARM and x86, too!
static inline __attribute__((always_inline)) float MATH_fabs(float x)
{
  asm volatile ("fabs %[floatx]\n"
    : [floatx] "+f" (x) // outputs, "+" means r/w
    : // no inputs
    : // no clobbers
  );

  return x;
}

// sqrt(x)
// This one works on ARM and x86, too!
// NOTE: There is a much faster version (MATH_Fast_Sqrt()) in the fsrra section of
// this file. Chances are you probably want that one.
static inline __attribute__((always_inline)) float MATH_fsqrt(float x)
{
  asm volatile ("fsqrt %[floatx]\n"
    : [floatx] "+f" (x) // outputs, "+" means r/w
    : // no inputs
    : // no clobbers
  );

  return x;
}

// a*b+c
static inline __attribute__((always_inline)) float MATH_fmac(float a, float b, float c)
{
  asm volatile ("fmac fr0, %[floatb], %[floatc]\n"
    : [floatc] "+f" (c) // outputs, "+" means r/w
    : "w" (a), [floatb] "f" (b) // inputs
    : // no clobbers
  );

  return c;
}

// a*b-c
static inline __attribute__((always_inline)) float MATH_fmac_Dec(float a, float b, float c)
{
  asm volatile ("fneg %[floatc]\n\t"
    "fmac fr0, %[floatb], %[floatc]\n"
    : [floatc] "+&f" (c) // outputs, "+" means r/w, "&" means it's written to before all inputs are consumed
    : "w" (a), [floatb] "f" (b) // inputs
    : // no clobbers
  );

  return c;
}

// Fast fminf() - return the min of two floats
// This doesn't check for NaN
static inline __attribute__((always_inline)) float MATH_Fast_Fminf(float a, float b)
{
  float output_float;

  asm volatile (
    "fcmp/gt %[floata], %[floatb]\n\t" // b > a (NaN evaluates to !GT; 0 -> T)
    "bt.s 1f\n\t" // yes, a is smaller
    " fmov %[floata], %[float_out]\n\t" // so return a
    "fmov %[floatb], %[float_out]\n" // no, either b is smaller or they're equal and it doesn't matter
  "1:\n"
    : [float_out] "=&f" (output_float) // outputs
    : [floata] "f" (a), [floatb] "f" (b) // inputs
    : "t" // clobbers
  );

  return output_float;
}

// Fast fmaxf() - return the max of two floats
// This doesn't check for NaN
static inline __attribute__((always_inline)) float MATH_Fast_Fmaxf(float a, float b)
{
  float output_float;

  asm volatile (
    "fcmp/gt %[floata], %[floatb]\n\t" // b > a (NaN evaluates to !GT; 0 -> T)
    "bt.s 1f\n\t" // yes, a is smaller
    " fmov %[floatb], %[float_out]\n\t" // so return b
    "fmov %[floata], %[float_out]\n" // no, either a is bigger or they're equal and it doesn't matter
  "1:\n"
    : [float_out] "=&f" (output_float) // outputs
    : [floata] "f" (a), [floatb] "f" (b) // inputs
    : "t" // clobbers
  );

  return output_float;
}

// Fast floorf() - return the nearest integer <= x as a float
// This doesn't check for NaN
static inline __attribute__((always_inline)) float MATH_Fast_Floorf(float x)
{
  float output_float;

  // To hold -1.0f
  float minus_one;

  asm volatile (
    "fldi1 %[minus_1]\n\t"
    "fneg %[minus_1]\n\t"
    "fcmp/gt %[minus_1], %[floatx]\n\t" // x >= 0
    "ftrc %[floatx], fpul\n\t" // convert float to int
    "bt.s 1f\n\t"
    " float fpul, %[float_out]\n\t" // convert int to float
    "fadd %[minus_1], %[float_out]\n" // if input x < 0, subtract 1.0
  "1:\n"
    : [minus_1] "=&f" (minus_one), [float_out] "=f" (output_float)
    : [floatx] "f" (x)
    : "fpul", "t"
  );

  return output_float;
}

// Fast ceilf() - return the nearest integer >= x as a float
// This doesn't check for NaN
static inline __attribute__((always_inline)) float MATH_Fast_Ceilf(float x)
{
  float output_float;

  // To hold 0.0f and 1.0f
  float zero_one;

  asm volatile (
    "fldi0 %[zero_1]\n\t"
    "fcmp/gt %[zero_1], %[floatx]\n\t" // x > 0
    "ftrc %[floatx], fpul\n\t" // convert float to int
    "bf.s 1f\n\t"
    " float fpul, %[float_out]\n\t" // convert int to float
    "fldi1 %[zero_1]\n\t"
    "fadd %[zero_1], %[float_out]\n" // if input x > 0, add 1.0
  "1:\n"
    : [zero_1] "=&f" (zero_one), [float_out] "=f" (output_float)
    : [floatx] "f" (x)
    : "fpul", "t"
  );

  return output_float;
}

// Very fast floorf() - return the nearest integer <= x as a float
// Inspired by a cool trick I came across here:
// https://www.codeproject.com/Tips/700780/Fast-floor-ceiling-functions
// This doesn't check for NaN
static inline __attribute__((always_inline)) float MATH_Very_Fast_Floorf(float x)
{
  float output_float;
  unsigned int scratch_reg;
  unsigned int scratch_reg2;

  // 0x4f000000 == 2^31 in float -- 0x4f << 24 is INT_MAX + 1.0f
  // 0x80000000 == -2^31 == INT_MIN == -(INT_MAX + 1.0f)

  // floor = (float)( (int)(x + (float)2^31) - 2^31)

  asm volatile (
    "mov #0x4f, %[scratch]\n\t" // Build float INT_MAX + 1 as a float using only regs (EX)
    "shll16 %[scratch]\n\t" // (EX)
    "shll8 %[scratch]\n\t" // (EX)
    "lds %[scratch], fpul\n\t" // move float INT_MAX + 1 to float regs (LS)
    "mov #1, %[scratch2]\n\t" // Build INT_MIN from scratch in parallel (EX)
    "fsts fpul, %[float_out]\n\t" // (LS)
    "fadd %[floatx], %[float_out]\n\t" // float-add float INT_MAX + 1 to x (FE)
    "rotr %[scratch2]\n\t" // rotate the 1 in bit 0 from LSB to MSB for INT_MIN, clobber T (EX)
    "ftrc %[float_out], fpul\n\t" // convert float to int (FE) -- ftrc -> sts is special combo
    "sts fpul, %[scratch]\n\t" // move back to int regs (LS)
    "add %[scratch2], %[scratch]\n\t" // Add INT_MIN to int (EX)
    "lds %[scratch], fpul\n\t" // (LS) -- lds -> float is a special combo
    "float fpul, %[float_out]\n" // convert back to float (FE)
    : [scratch] "=&r" (scratch_reg), [scratch2] "=&r" (scratch_reg2), [float_out] "=&f" (output_float)
    : [floatx] "f" (x)
    : "fpul", "t"
  );

  return output_float;
}

// Very fast ceilf() - return the nearest integer >= x as a float
// Inspired by a cool trick I came across here:
// https://www.codeproject.com/Tips/700780/Fast-floor-ceiling-functions
// This doesn't check for NaN
static inline __attribute__((always_inline)) float MATH_Very_Fast_Ceilf(float x)
{
  float output_float;
  unsigned int scratch_reg;
  unsigned int scratch_reg2;

  // 0x4f000000 == 2^31 in float -- 0x4f << 24 is INT_MAX + 1.0f
  // 0x80000000 == -2^31 == INT_MIN == -(INT_MAX + 1.0f)

  // Ceiling is the inverse of floor such that f^-1(x) = -f(-x)
  // To make very fast ceiling have as wide a range as very fast floor,
  // use this property to subtract x from INT_MAX + 1 and get the negative of the
  // ceiling, and then negate the final output. This allows ceiling to use
  // -2^31 and have the same range as very fast floor.

  // Given:
  // floor = (float)( (int)(x + (float)2^31) - 2^31 )
  // We can do:
  // ceiling = -( (float)( (int)((float)2^31 - x) - 2^31 ) )
  // or (slower on SH4 since 'fneg' is faster than 'neg'):
  // ceiling = (float) -( (int)((float)2^31 - x) - 2^31 )
  // Since mathematically these functions are related by f^-1(x) = -f(-x).

  asm volatile (
    "mov #0x4f, %[scratch]\n\t" // Build float INT_MAX + 1 as a float using only regs (EX)
    "shll16 %[scratch]\n\t" // (EX)
    "shll8 %[scratch]\n\t" // (EX)
    "lds %[scratch], fpul\n\t" // move float INT_MAX + 1 to float regs (LS)
    "mov #1, %[scratch2]\n\t" // Build INT_MIN from scratch in parallel (EX)
    "fsts fpul, %[float_out]\n\t" // (LS)
    "fsub %[floatx], %[float_out]\n\t" // float-sub x from float INT_MAX + 1 (FE)
    "rotr %[scratch2]\n\t" // rotate the 1 in bit 0 from LSB to MSB for INT_MIN, clobber T (EX)
    "ftrc %[float_out], fpul\n\t" // convert float to int (FE) -- ftrc -> sts is special combo
    "sts fpul, %[scratch]\n\t" // move back to int regs (LS)
    "add %[scratch2], %[scratch]\n\t" // Add INT_MIN to int (EX)
    "lds %[scratch], fpul\n\t" // (LS) -- lds -> float is a special combo
    "float fpul, %[float_out]\n\t" // convert back to float (FE)
    "fneg %[float_out]\n"
    : [scratch] "=&r" (scratch_reg), [scratch2] "=&r" (scratch_reg2), [float_out] "=&f" (output_float)
    : [floatx] "f" (x)
    : "fpul", "t"
  );

  return output_float;
}

//==============================================================================
// Fun with fsrra, which does 1/sqrt(x) in one cycle
//==============================================================================
//
// Error of 'fsrra' is +/- 2^-21 per http://www.shared-ptr.com/sh_insns.html
//
// The following functions are available.
// Please see their definitions for other usage info, otherwise they may not
// work for you.
//
/*
  // 1/sqrt(x)
  float MATH_fsrra(float x)

  // 1/x
  float MATH_Fast_Invert(float x)

  // A faster divide than the 'fdiv' instruction
  float MATH_Fast_Divide(float numerator, float denominator)

  // A faster square root then the 'fsqrt' instruction
  float MATH_Fast_Sqrt(float x)

  // Standard, accurate, and slow float divide. Use this if MATH_Fast_Divide() gives you issues.
  float MATH_Slow_Divide(float numerator, float denominator)
*/

// 1/sqrt(x)
static inline __attribute__((always_inline)) float MATH_fsrra(float x)
{
  asm volatile ("fsrra %[one_div_sqrt]\n"
  : [one_div_sqrt] "+f" (x) // outputs, "+" means r/w
  : // no inputs
  : // no clobbers
  );

  return x;
}

// 1/x
// 1.0f / sqrt(x^2)
static inline __attribute__((always_inline)) float MATH_Fast_Invert(float x)
{
  int neg = 0;

  if(x < 0.0f)
  {
    neg = 1;
  }

  x = MATH_fsrra(x*x); // 1.0f / sqrt(x^2)

  if(neg)
  {
    return -x;
  }
  else
  {
    return x;
  }
}

// It's faster to do this than to use 'fdiv'.
// Only fdiv can do doubles, however.
// Of course, not having to divide at all is generally the best way to go. :P
static inline __attribute__((always_inline)) float MATH_Fast_Divide(float numerator, float denominator)
{
  denominator = MATH_Fast_Invert(denominator);
  return numerator * denominator;
}

// fast sqrt(x)
// Crazy thing: invert(fsrra(x)) is actually about 3x faster than fsqrt.
static inline __attribute__((always_inline)) float MATH_Fast_Sqrt(float x)
{
  return MATH_Fast_Invert(MATH_fsrra(x));
}

// Standard, accurate, and slow float divide. Use this if MATH_Fast_Divide() gives you issues.
// This DOES work on negatives.
static inline __attribute__((always_inline)) float MATH_Slow_Divide(float numerator, float denominator)
{
  asm volatile ("fdiv %[div_denom], %[div_numer]\n"
  : [div_numer] "+f" (numerator) // outputs, "+" means r/w
  : [div_denom] "f" (denominator) // inputs
  : // clobbers
  );

  return numerator;
}

//==============================================================================
// Fun with fsca, which does simultaneous sine and cosine in 3 cycles
//==============================================================================
//
// NOTE: GCC 4.7 has a bug that prevents it from working with fsca and complex
// numbers in m4-single-only mode, so GCC 4 users will get a RETURN_FSCA_STRUCT
// instead of a _Complex float. This may be much slower in some instances.
//
// VERY IMPORTANT USAGE INFORMATION (sine and cosine functions):
//
// Due to the nature in which the fsca instruction behaves, you MUST do the
// following in your code to get sine and cosine from these functions:
//
//  _Complex float sine_cosine = [Call the fsca function here]
//  float sine_value = __real__ sine_cosine;
//  float cosine_value = __imag__ sine_cosine;
//  Your output is now in sine_value and cosine_value.
//
// This is necessary because fsca outputs both sine and cosine simultaneously
// and uses a double register to do so. The fsca functions do not actually
// return a double--they return two floats--and using a complex float here is
// just a bit of hacking the C language to make GCC do something that's legal in
// assembly according to the SH4 calling convention (i.e. multiple return values
// stored in floating point registers FR0-FR3). This is better than using a
// struct of floats for optimization purposes--this will operate at peak
// performance even at -O0, whereas a struct will not be fast at low
// optimization levels due to memory accesses.
//
// Technically you may be able to use the complex return values as a complex
// number if you wanted to, but that's probably not what you're after and they'd
// be flipped anyways (in mathematical convention, sine is the imaginary part).
//

// Notes:
// - From http://www.shared-ptr.com/sh_insns.html:
//      The input angle is specified as a signed fraction in twos complement.
//      The result of sin and cos is a single-precision floating-point number.
//      0x7FFFFFFF to 0x00000001: 360×2^15−360/2^16 to 360/2^16 degrees
//      0x00000000: 0 degree
//      0xFFFFFFFF to 0x80000000: −360/2^16 to −360×2^15 degrees
// - fsca format is 2^16 is 360 degrees, so a value of 1 is actually
//  1/182.044444444 of a degree or 1/10430.3783505 of a radian
// - fsca does a %360 automatically for values over 360 degrees
//
// Also:
// In order to make the best use of fsca units, a program must expect them from
// the outset and not "make them" by dividing radians or degrees to get them,
// otherwise it's just giving the 'fsca' instruction radians or degrees!
//

// The following functions are available.
// Please see their definitions for other usage info, otherwise they may not
// work for you.
//
/*
  // For integer input in native fsca units (fastest)
  _Complex float MATH_fsca_Int(unsigned int input_int)

  // For integer input in degrees
  _Complex float MATH_fsca_Int_Deg(unsigned int input_int)

  // For integer input in radians
  _Complex float MATH_fsca_Int_Rad(unsigned int input_int)

  // For float input in native fsca units
  _Complex float MATH_fsca_Float(float input_float)

  // For float input in degrees
  _Complex float MATH_fsca_Float_Deg(float input_float)

  // For float input in radians
  _Complex float MATH_fsca_Float_Rad(float input_float)
*/

//------------------------------------------------------------------------------
#if __GNUC__ <= GNUC_FSCA_ERROR_VERSION
//------------------------------------------------------------------------------
//
// This set of fsca functions is specifically for old versions of GCC.
// See later for functions for newer versions of GCC.
//

//
// Integer input (faster)
//

// For int input, input_int is in native fsca units (fastest)
static inline __attribute__((always_inline)) RETURN_FSCA_STRUCT MATH_fsca_Int(unsigned int input_int)
{
  register float __sine __asm__("fr0");
  register float __cosine __asm__("fr1");

  asm volatile ("lds %[input_number], FPUL\n\t" // load int from register (1 cycle)
    "fsca FPUL, DR0\n" // 3 cycle simultaneous sine/cosine
    : "=w" (__sine), "=f" (__cosine) // outputs
    : [input_number] "r" (input_int)  // inputs
    : "fpul" // clobbers
  );

  RETURN_FSCA_STRUCT output = {__sine, __cosine};
  return output;
}

// For int input, input_int is in degrees
static inline __attribute__((always_inline)) RETURN_FSCA_STRUCT MATH_fsca_Int_Deg(unsigned int input_int)
{
  // normalize whole number input degrees to fsca format
  input_int = ((1527099483ULL * input_int) >> 23);

  register float __sine __asm__("fr0");
  register float __cosine __asm__("fr1");

  asm volatile ("lds %[input_number], FPUL\n\t" // load int from register (1 cycle)
    "fsca FPUL, DR0\n" // 3 cycle simultaneous sine/cosine
    : "=w" (__sine), "=f" (__cosine) // outputs
    : [input_number] "r" (input_int)  // inputs
    : "fpul" // clobbers
  );

  RETURN_FSCA_STRUCT output = {__sine, __cosine};
  return output;
}

// For int input, input_int is in radians
static inline __attribute__((always_inline)) RETURN_FSCA_STRUCT MATH_fsca_Int_Rad(unsigned int input_int)
{
  // normalize whole number input rads to fsca format
  input_int = ((2734261102ULL * input_int) >> 18);

  register float __sine __asm__("fr0");
  register float __cosine __asm__("fr1");

  asm volatile ("lds %[input_number], FPUL\n\t" // load int from register (1 cycle)
    "fsca FPUL, DR0\n" // 3 cycle simultaneous sine/cosine
    : "=w" (__sine), "=f" (__cosine) // outputs
    : [input_number] "r" (input_int)  // inputs
    : "fpul" // clobbers
  );

  RETURN_FSCA_STRUCT output = {__sine, __cosine};
  return output;
}

//
// Float input (slower)
//

// For float input, input_float is in native fsca units
static inline __attribute__((always_inline)) RETURN_FSCA_STRUCT MATH_fsca_Float(float input_float)
{
  register float __sine __asm__("fr0");
  register float __cosine __asm__("fr1");

  asm volatile ("ftrc %[input_number], FPUL\n\t" // convert float to int. takes 3 cycles
    "fsca FPUL, DR0\n" // 3 cycle simultaneous sine/cosine
    : "=w" (__sine), "=f" (__cosine) // outputs
    : [input_number] "f" (input_float)  // inputs
    : "fpul" // clobbers
  );

  RETURN_FSCA_STRUCT output = {__sine, __cosine};
  return output;
}

// For float input, input_float is in degrees
static inline __attribute__((always_inline)) RETURN_FSCA_STRUCT MATH_fsca_Float_Deg(float input_float)
{
  input_float *= 182.044444444f;

  register float __sine __asm__("fr0");
  register float __cosine __asm__("fr1");

  asm volatile ("ftrc %[input_number], FPUL\n\t" // convert float to int. takes 3 cycles
    "fsca FPUL, DR0\n" // 3 cycle simultaneous sine/cosine
    : "=w" (__sine), "=f" (__cosine) // outputs
    : [input_number] "f" (input_float)  // inputs
    : "fpul" // clobbers
  );

  RETURN_FSCA_STRUCT output = {__sine, __cosine};
  return output;
}

// For float input, input_float is in radians
static inline __attribute__((always_inline)) RETURN_FSCA_STRUCT MATH_fsca_Float_Rad(float input_float)
{
  input_float *= 10430.3783505f;

  register float __sine __asm__("fr0");
  register float __cosine __asm__("fr1");

  asm volatile ("ftrc %[input_number], FPUL\n\t" // convert float to int. takes 3 cycles
    "fsca FPUL, DR0\n" // 3 cycle simultaneous sine/cosine
    : "=w" (__sine), "=f" (__cosine) // outputs
    : [input_number] "f" (input_float)  // inputs
    : "fpul" // clobbers
  );

  RETURN_FSCA_STRUCT output = {__sine, __cosine};
  return output;
}

//------------------------------------------------------------------------------
#else
//------------------------------------------------------------------------------
//
// This set of fsca functions is specifically for newer versions of GCC. They
// work fine under GCC 9.2.0.
//

//
// Integer input (faster)
//

// For int input, input_int is in native fsca units (fastest)
static inline __attribute__((always_inline)) _Complex float MATH_fsca_Int(unsigned int input_int)
{
  _Complex float output;

  asm volatile ("lds %[input_number], FPUL\n\t" // load int from register (1 cycle)
    "fsca FPUL, %[out]\n" // 3 cycle simultaneous sine/cosine
    : [out] "=d" (output) // outputs
    : [input_number] "r" (input_int)  // inputs
    : "fpul" // clobbers
  );

  return output;
}

// For int input, input_int is in degrees
static inline __attribute__((always_inline)) _Complex float MATH_fsca_Int_Deg(unsigned int input_int)
{
  // normalize whole number input degrees to fsca format
  input_int = ((1527099483ULL * input_int) >> 23);

  _Complex float output;

  asm volatile ("lds %[input_number], FPUL\n\t" // load int from register (1 cycle)
    "fsca FPUL, %[out]\n" // 3 cycle simultaneous sine/cosine
    : [out] "=d" (output) // outputs
    : [input_number] "r" (input_int)  // inputs
    : "fpul" // clobbers
  );

  return output;
}

// For int input, input_int is in radians
static inline __attribute__((always_inline)) _Complex float MATH_fsca_Int_Rad(unsigned int input_int)
{
  // normalize whole number input rads to fsca format
  input_int = ((2734261102ULL * input_int) >> 18);

  _Complex float output;

  asm volatile ("lds %[input_number], FPUL\n\t" // load int from register (1 cycle)
    "fsca FPUL, %[out]\n" // 3 cycle simultaneous sine/cosine
    : [out] "=d" (output) // outputs
    : [input_number] "r" (input_int)  // inputs
    : "fpul" // clobbers
  );

  return output;
}

//
// Float input (slower)
//

// For float input, input_float is in native fsca units
static inline __attribute__((always_inline)) _Complex float MATH_fsca_Float(float input_float)
{
  _Complex float output;

  asm volatile ("ftrc %[input_number], FPUL\n\t" // convert float to int. takes 3 cycles
    "fsca FPUL, %[out]\n" // 3 cycle simultaneous sine/cosine
    : [out] "=d" (output) // outputs
    : [input_number] "f" (input_float)  // inputs
    : "fpul" // clobbers
  );

  return output;
}

// For float input, input_float is in degrees
static inline __attribute__((always_inline)) _Complex float MATH_fsca_Float_Deg(float input_float)
{
  input_float *= 182.044444444f;

  _Complex float output;

  asm volatile ("ftrc %[input_number], FPUL\n\t" // convert float to int. takes 3 cycles
    "fsca FPUL, %[out]\n" // 3 cycle simultaneous sine/cosine
    : [out] "=d" (output) // outputs
    : [input_number] "f" (input_float)  // inputs
    : "fpul" // clobbers
  );

  return output;
}

// For float input, input_float is in radians
static inline __attribute__((always_inline)) _Complex float MATH_fsca_Float_Rad(float input_float)
{
  input_float *= 10430.3783505f;

  _Complex float output;

  asm volatile ("ftrc %[input_number], FPUL\n\t" // convert float to int. takes 3 cycles
    "fsca FPUL, %[out]\n" // 3 cycle simultaneous sine/cosine
    : [out] "=d" (output) // outputs
    : [input_number] "f" (input_float)  // inputs
    : "fpul" // clobbers
  );

  return output;
}

//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------

//==============================================================================
// Hardware vector and matrix operations
//==============================================================================
//
// These functions each have very specific usage instructions. Please be sure to
// read them before use or else they won't seem to work right!
//
// The following functions are available.
// Please see their definitions for important usage info, otherwise they may not
// work for you.
//
/*

  //------------------------------------------------------------------------------
  // Vector and matrix math operations
  //------------------------------------------------------------------------------

  // Inner/dot product (4x1 vec . 4x1 vec = scalar)
  float MATH_fipr(float x1, float x2, float x3, float x4, float y1, float y2, float y3, float y4)

  // Sum of Squares (w^2 + x^2 + y^2 + z^2)
  float MATH_Sum_of_Squares(float w, float x, float y, float z)

  // Cross product with bonus multiply (vec X vec = orthogonal vec, with an extra a*b=c)
  RETURN_VECTOR_STRUCT MATH_Cross_Product_with_Mult(float x1, float x2, float x3, float y1, float y2, float y3, float a, float b)

  // Cross product (vec X vec = orthogonal vec)
  RETURN_VECTOR_STRUCT MATH_Cross_Product(float x1, float x2, float x3, float y1, float y2, float y3)

  // Outer product (vec (X) vec = 4x4 matrix)
  void MATH_Outer_Product(float x1, float x2, float x3, float x4, float y1, float y2, float y3, float y4)

  // Matrix transform (4x4 matrix * 4x1 vec = 4x1 vec)
  RETURN_VECTOR_STRUCT MATH_Matrix_Transform(float x1, float x2, float x3, float x4)

  // 4x4 Matrix transpose (XMTRX^T)
  void MATH_Matrix_Transpose(void)

  // 4x4 Matrix product (XMTRX and one from memory)
  void MATH_Matrix_Product(ALL_FLOATS_STRUCT * front_matrix)

  // 4x4 Matrix product (two from memory)
  void MATH_Load_Matrix_Product(ALL_FLOATS_STRUCT * matrix1, ALL_FLOATS_STRUCT * matrix2)

  //------------------------------------------------------------------------------
  // Matrix load and store operations
  //------------------------------------------------------------------------------

  // Load 4x4 XMTRX from memory
  void MATH_Load_XMTRX(ALL_FLOATS_STRUCT * back_matrix)

  // Store 4x4 XMTRX to memory
  ALL_FLOATS_STRUCT * MATH_Store_XMTRX(ALL_FLOATS_STRUCT * destination)
*/

//------------------------------------------------------------------------------
// Vector and matrix math operations
//------------------------------------------------------------------------------

// Inner/dot product: vec . vec = scalar
//                       _    _
//                      |  y1  |
//  [ x1 x2 x3 x4 ]  .  |  y2  | = scalar
//                      |  y3  |
//                      |_ y4 _|
//
// SH4 calling convention states we get 8 float arguments. Perfect!
static inline __attribute__((always_inline)) float MATH_fipr(float x1, float x2, float x3, float x4, float y1, float y2, float y3, float y4)
{
  // FR4-FR11 are the regs that are passed in, aka vectors FV4 and FV8.
  // Just need to make sure GCC doesn't modify anything, and these register vars do that job.

  // Temporary variables are necessary per GCC to avoid clobbering:
  // https://gcc.gnu.org/onlinedocs/gcc/Local-Register-Variables.html#Local-Register-Variables

  float tx1 = x1;
  float tx2 = x2;
  float tx3 = x3;
  float tx4 = x4;

  float ty1 = y1;
  float ty2 = y2;
  float ty3 = y3;
  float ty4 = y4;

  // vector FV4
  register float __x1 __asm__("fr4") = tx1;
  register float __x2 __asm__("fr5") = tx2;
  register float __x3 __asm__("fr6") = tx3;
  register float __x4 __asm__("fr7") = tx4;

  // vector FV8
  register float __y1 __asm__("fr8") = ty1;
  register float __y2 __asm__("fr9") = ty2;
  register float __y3 __asm__("fr10") = ty3;
  register float __y4 __asm__("fr11") = ty4;

  // take care of all the floats in one fell swoop
  asm volatile ("fipr FV4, FV8\n"
  : "+f" (__y4) // output (gets written to FR11)
  : "f" (__x1), "f" (__x2), "f" (__x3), "f" (__x4), "f" (__y1), "f" (__y2), "f" (__y3) // inputs
  : // clobbers
  );

  return __y4;
}

// Sum of Squares
//                   _   _
//                  |  w  |
//  [ w x y z ]  .  |  x  | = w^2 + x^2 + y^2 + z^2 = scalar
//                  |  y  |
//                  |_ z _|
//
static inline __attribute__((always_inline)) float MATH_Sum_of_Squares(float w, float x, float y, float z)
{
  // FR4-FR7 are the regs that are passed in, aka vector FV4.
  // Just need to make sure GCC doesn't modify anything, and these register vars do that job.

  // Temporary variables are necessary per GCC to avoid clobbering:
  // https://gcc.gnu.org/onlinedocs/gcc/Local-Register-Variables.html#Local-Register-Variables

  float tw = w;
  float tx = x;
  float ty = y;
  float tz = z;

  // vector FV4
  register float __w __asm__("fr4") = tw;
  register float __x __asm__("fr5") = tx;
  register float __y __asm__("fr6") = ty;
  register float __z __asm__("fr7") = tz;

  // take care of all the floats in one fell swoop
  asm volatile ("fipr FV4, FV4\n"
  : "+f" (__z) // output (gets written to FR7)
  : "f" (__w), "f" (__x), "f" (__y) // inputs
  : // clobbers
  );

  return __z;
}

// Cross product: vec X vec = orthogonal vec
//   _    _       _    _       _    _
//  |  x1  |     |  y1  |     |  z1  |
//  |  x2  |  X  |  y2  |  =  |  z2  |
//  |_ x3 _|     |_ y3 _|     |_ z3 _|
//
// With bonus multiply:
//
//      a     *     b      =      c
//
// IMPORTANT USAGE INFORMATION (cross product):
//
// Return vector struct maps as below to the above diagram:
//
//  typedef struct {
//   float z1;
//   float z2;
//   float z3;
//   float z4; // c is stored in z4, and c = a*b if using 'with mult' version (else c = 0)
// } RETURN_VECTOR_STRUCT;
//
//  For people familiar with the unit vector notation, z1 == 'i', z2 == 'j',
//  and z3 == 'k'.
//
// The cross product matrix will also be stored in XMTRX after this, so calling
// MATH_Matrix_Transform() on a vector after using this function will do a cross
// product with the same x1-x3 values and a multiply with the same 'a' value
// as used in this function. In this a situation, 'a' will be multiplied with
// the x4 parameter of MATH_Matrix_Transform(). a = 0 if not using the 'with mult'
// version of the cross product function.
//
// For reference, XMTRX will look like this:
//
//  [  0 -x3 x2 0 ]
//  [  x3 0 -x1 0 ]
//  [ -x2 x1 0  0 ]
//  [  0  0  0  a ] (<-- a = 0 if not using 'with mult')
//
// Similarly to how the sine and cosine functions use fsca and return 2 floats,
// the cross product functions actually return 4 floats. The first 3 are the
// cross product output, and the 4th is a*b. The SH4 only multiplies 4x4
// matrices with 4x1 vectors, which is why the output is like that--but it means
// we also get a bonus float multiplication while we do our cross product!
//

// Please do not call this function directly (notice the weird syntax); call
// MATH_Cross_Product() or MATH_Cross_Product_with_Mult() instead.
static inline __attribute__((always_inline)) RETURN_VECTOR_STRUCT xMATH_do_Cross_Product_with_Mult(float x3, float a, float y3, float b, float x2, float x1, float y1, float y2)
{
  // FR4-FR11 are the regs that are passed in, in that order.
  // Just need to make sure GCC doesn't modify anything, and these register vars do that job.

  // Temporary variables are necessary per GCC to avoid clobbering:
  // https://gcc.gnu.org/onlinedocs/gcc/Local-Register-Variables.html#Local-Register-Variables

  float tx1 = x1;
  float tx2 = x2;
  float tx3 = x3;
  float ta = a;

  float ty1 = y1;
  float ty2 = y2;
  float ty3 = y3;
  float tb = b;

  register float __x1 __asm__("fr9") = tx1; // need to negate (need to move to fr6, then negate fr9)
  register float __x2 __asm__("fr8") = tx2; // in place for matrix (need to move to fr2 then negate fr2)
  register float __x3 __asm__("fr4") = tx3; // need to negate (move to fr1 first, then negate fr4)
  register float __a __asm__("fr5") = ta;

  register float __y1 __asm__("fr10") = ty1;
  register float __y2 __asm__("fr11") = ty2;
  register float __y3 __asm__("fr6") = ty3;
  register float __b __asm__("fr7") = tb;

  register float __z1 __asm__("fr0") = 0.0f; // z1
  register float __z2 __asm__("fr1") = 0.0f; // z2 (not moving x3 here yet since a double 0 is needed)
  register float __z3 __asm__("fr2") = tx2; // z3 (this handles putting x2 in fr2)
  register float __c __asm__("fr3") = 0.0f; // c

  // This actually does a matrix transform to do the cross product.
  // It's this:
  //                   _    _       _            _
  //  [  0 -x3 x2 0 ] |  y1  |     | -x3y2 + x2y3 |
  //  [  x3 0 -x1 0 ] |  y2  |  =  |  x3y1 - x1y3 |
  //  [ -x2 x1 0  0 ] |  y3  |     | -x2y1 + x1y2 |
  //  [  0  0  0  a ] |_ b  _|     |_      c     _|
  //

  asm volatile (
    // set up back bank's FV0
    "fschg\n\t" // switch fmov to paired moves (note: only paired moves can access XDn regs)

    // Save FR12-FR15, which are supposed to be preserved across functions calls.
    // This stops them from getting clobbered and saves 4 stack pushes (memory accesses).
    "fmov DR12, XD12\n\t"
    "fmov DR14, XD14\n\t"

    "fmov DR10, XD0\n\t" // fmov 'y1' and 'y2' from FR10, FR11 into position (XF0, XF1)
    "fmov DR6, XD2\n\t" // fmov 'y3' and 'b' from FR6, FR7 into position (XF2, XF3)

    // pair move zeros for some speed in setting up front bank for matrix
    "fmov DR0, DR10\n\t" // clear FR10, FR11
    "fmov DR0, DR12\n\t" // clear FR12, FR13
    "fschg\n\t" // switch back to single moves
    // prepare front bank for XMTRX
    "fmov FR5, FR15\n\t" // fmov 'a' into position
    "fmov FR0, FR14\n\t" // clear out FR14
    "fmov FR0, FR7\n\t" // clear out FR7
    "fmov FR0, FR5\n\t" // clear out FR5

    "fneg FR2\n\t" // set up 'x2'
    "fmov FR9, FR6\n\t" // set up 'x1'
    "fneg FR9\n\t"
    "fmov FR4, FR1\n\t" // set up 'x3'
    "fneg FR4\n\t"
    // flip banks and matrix multiply
    "frchg\n\t"
    "ftrv XMTRX, FV0\n"
  : "+&w" (__z1), "+&f" (__z2), "+&f" (__z3), "+&f" (__c) // output (using FV0)
  : "f" (__x1), "f" (__x2), "f" (__x3), "f" (__y1), "f" (__y2), "f" (__y3), "f" (__a), "f" (__b) // inputs
  : // clobbers (all of the float regs get clobbered, except for FR12-FR15 which were specially preserved)
  );

  RETURN_VECTOR_STRUCT output = {__z1, __z2, __z3, __c};
  return output;
}

// Please do not call this function directly (notice the weird syntax); call
// MATH_Cross_Product() or MATH_Cross_Product_with_Mult() instead.
static inline __attribute__((always_inline)) RETURN_VECTOR_STRUCT xMATH_do_Cross_Product(float x3, float zero, float x1, float y3, float x2, float x1_2, float y1, float y2)
{
  // FR4-FR11 are the regs that are passed in, in that order.
  // Just need to make sure GCC doesn't modify anything, and these register vars do that job.

  // Temporary variables are necessary per GCC to avoid clobbering:
  // https://gcc.gnu.org/onlinedocs/gcc/Local-Register-Variables.html#Local-Register-Variables

  float tx1 = x1;
  float tx2 = x2;
  float tx3 = x3;
  float tx1_2 = x1_2;

  float ty1 = y1;
  float ty2 = y2;
  float ty3 = y3;
  float tzero = zero;

  register float __x1 __asm__("fr6") = tx1; // in place
  register float __x2 __asm__("fr8") = tx2; // in place (fmov to fr2, negate fr2)
  register float __x3 __asm__("fr4") = tx3; // need to negate (fmov to fr1, negate fr4)

  register float __zero __asm__("fr5") = tzero; // in place
  register float __x1_2 __asm__("fr9") = tx1_2; // need to negate

  register float __y1 __asm__("fr10") = ty1;
  register float __y2 __asm__("fr11") = ty2;
  // no __y3 needed in this function

  register float __z1 __asm__("fr0") = tzero; // z1
  register float __z2 __asm__("fr1") = tzero; // z2
  register float __z3 __asm__("fr2") = ty3; // z3
  register float __c __asm__("fr3") = tzero; // c

  // This actually does a matrix transform to do the cross product.
  // It's this:
  //                   _    _       _            _
  //  [  0 -x3 x2 0 ] |  y1  |     | -x3y2 + x2y3 |
  //  [  x3 0 -x1 0 ] |  y2  |  =  |  x3y1 - x1y3 |
  //  [ -x2 x1 0  0 ] |  y3  |     | -x2y1 + x1y2 |
  //  [  0  0  0  0 ] |_ 0  _|     |_      0     _|
  //

  asm volatile (
    // zero out FR7. For some reason, if this is done in C after __z3 is set:
    // register float __y3 __asm__("fr7") = tzero;
    // then GCC will emit a spurious stack push (pushing FR12). So just zero it here.
    "fmov FR5, FR7\n\t"
    // set up back bank's FV0
    "fschg\n\t" // switch fmov to paired moves (note: only paired moves can access XDn regs)

    // Save FR12-FR15, which are supposed to be preserved across functions calls.
    // This stops them from getting clobbered and saves 4 stack pushes (memory accesses).
    "fmov DR12, XD12\n\t"
    "fmov DR14, XD14\n\t"

    "fmov DR10, XD0\n\t" // fmov 'y1' and 'y2' from FR10, FR11 into position (XF0, XF1)
    "fmov DR2, XD2\n\t" // fmov 'y3' and '0' from FR2, FR3 into position (XF2, XF3)

    // pair move zeros for some speed in setting up front bank for matrix
    "fmov DR0, DR10\n\t" // clear FR10, FR11
    "fmov DR0, DR12\n\t" // clear FR12, FR13
    "fmov DR0, DR14\n\t" // clear FR14, FR15
    "fschg\n\t" // switch back to single moves
    // prepare front bank for XMTRX
    "fneg FR9\n\t" // set up 'x1'
    "fmov FR8, FR2\n\t" // set up 'x2'
    "fneg FR2\n\t"
    "fmov FR4, FR1\n\t" // set up 'x3'
    "fneg FR4\n\t"
    // flip banks and matrix multiply
    "frchg\n\t"
    "ftrv XMTRX, FV0\n"
  : "+&w" (__z1), "+&f" (__z2), "+&f" (__z3), "+&f" (__c) // output (using FV0)
  : "f" (__x1), "f" (__x2), "f" (__x3), "f" (__y1), "f" (__y2), "f" (__zero), "f" (__x1_2) // inputs
  : "fr7" // clobbers (all of the float regs get clobbered, except for FR12-FR15 which were specially preserved)
  );

  RETURN_VECTOR_STRUCT output = {__z1, __z2, __z3, __c};
  return output;
}

//------------------------------------------------------------------------------
// Functions that wrap the xMATH_do_Cross_Product[_with_Mult]() functions to make
// it easier to organize parameters
//------------------------------------------------------------------------------

// Cross product with a bonus float multiply (c = a * b)
static inline __attribute__((always_inline)) RETURN_VECTOR_STRUCT MATH_Cross_Product_with_Mult(float x1, float x2, float x3, float y1, float y2, float y3, float a, float b)
{
  return xMATH_do_Cross_Product_with_Mult(x3, a, y3, b, x2, x1, y1, y2);
}

// Plain cross product; does not use the bonus float multiply (c = 0 and a in the cross product matrix will be 0)
// This is a tiny bit faster than 'with_mult' (about 2 cycles faster)
static inline __attribute__((always_inline)) RETURN_VECTOR_STRUCT MATH_Cross_Product(float x1, float x2, float x3, float y1, float y2, float y3)
{
  return xMATH_do_Cross_Product(x3, 0.0f, x1, y3, x2, x1, y1, y2);
}

// Outer product: vec (X) vec = matrix
//   _    _
//  |  x1  |
//  |  x2  |  (X)  [ y1 y2 y3 y4 ] = 4x4 matrix
//  |  x3  |
//  |_ x4 _|
//
// This returns the floats in the back bank (XF0-15), which are inaccessible
// outside of using frchg or paired-move fmov. It's meant to set up a matrix for
// use with other matrix functions. GCC also does not touch the XFn bank.
// This will also wipe out anything stored in the float registers, as it uses the
// whole FPU register file (all 32 of the float registers).
static inline __attribute__((always_inline)) void MATH_Outer_Product(float x1, float x2, float x3, float x4, float y1, float y2, float y3, float y4)
{
  // FR4-FR11 are the regs that are passed in, in that order.
  // Just need to make sure GCC doesn't modify anything, and these register vars do that job.

  // Temporary variables are necessary per GCC to avoid clobbering:
  // https://gcc.gnu.org/onlinedocs/gcc/Local-Register-Variables.html#Local-Register-Variables

  float tx1 = x1;
  float tx2 = x2;
  float tx3 = x3;
  float tx4 = x4;

  float ty1 = y1;
  float ty2 = y2;
  float ty3 = y3;
  float ty4 = y4;

  // vector FV4
  register float __x1 __asm__("fr4") = tx1;
  register float __x2 __asm__("fr5") = tx2;
  register float __x3 __asm__("fr6") = tx3;
  register float __x4 __asm__("fr7") = tx4;

  // vector FV8
  register float __y1 __asm__("fr8") = ty1;
  register float __y2 __asm__("fr9") = ty2;
  register float __y3 __asm__("fr10") = ty3; // in place already
  register float __y4 __asm__("fr11") = ty4;

  // This actually does a 4x4 matrix multiply to do the outer product.
  // It's this:
  //
  //  [ x1 x1 x1 x1 ] [ y1 0 0 0 ]     [ x1y1 x1y2 x1y3 x1y4 ]
  //  [ x2 x2 x2 x2 ] [ 0 y2 0 0 ]  =  [ x2y1 x2y2 x2y3 x2y4 ]
  //  [ x3 x3 x3 x3 ] [ 0 0 y3 0 ]     [ x3y1 x3y2 x3y3 x3y4 ]
  //  [ x4 x4 x4 x4 ] [ 0 0 0 y4 ]     [ x4y1 x4y2 x4y3 x4y4 ]
  //

  asm volatile (
    // zero out unoccupied front floats to make a double 0 in DR2
    "fldi0 FR2\n\t"
    "fmov FR2, FR3\n\t"
    "fschg\n\t" // switch fmov to paired moves (note: only paired moves can access XDn regs)
    // fmov 'x1' and 'x2' from FR4, FR5 into position (XF0,4,8,12, XF1,5,9,13)
    "fmov DR4, XD0\n\t"
    "fmov DR4, XD4\n\t"
    "fmov DR4, XD8\n\t"
    "fmov DR4, XD12\n\t"
    // fmov 'x3' and 'x4' from FR6, FR7 into position (XF2,6,10,14, XF3,7,11,15)
    "fmov DR6, XD2\n\t"
    "fmov DR6, XD6\n\t"
    "fmov DR6, XD10\n\t"
    "fmov DR6, XD14\n\t"
    // set up front floats (y1-y4)
    "fmov DR8, DR0\n\t"
    "fmov DR8, DR4\n\t"
    "fmov DR10, DR14\n\t"
    // finish zeroing out front floats
    "fmov DR2, DR6\n\t"
    "fmov DR2, DR8\n\t"
    "fmov DR2, DR12\n\t"
    "fschg\n\t" // switch back to single-move mode
    // zero out remaining values and matrix multiply 4x4
    "fmov FR2, FR1\n\t"
    "ftrv XMTRX, FV0\n\t"

    "fmov FR6, FR4\n\t"
    "ftrv XMTRX, FV4\n\t"

    "fmov FR8, FR11\n\t"
    "ftrv XMTRX, FV8\n\t"

    "fmov FR12, FR14\n\t"
    "ftrv XMTRX, FV12\n\t"
    // Save output in XF regs
    "frchg\n"
  : // no outputs
  : "f" (__x1), "f" (__x2), "f" (__x3), "f" (__x4), "f" (__y1), "f" (__y2), "f" (__y3), "f" (__y4) // inputs
  : "fr0", "fr1", "fr2", "fr3", "fr12", "fr13", "fr14", "fr15" // clobbers, can't avoid it
  );
  // GCC will restore FR12-FR15 from the stack after this, so we really can't keep the output in the front bank.
}

// Matrix transform: matrix * vector = vector
//                   _    _       _    _
//  [ ----------- ] |  x1  |     |  z1  |
//  [ ---XMTRX--- ] |  x2  |  =  |  z2  |
//  [ ----------- ] |  x3  |     |  z3  |
//  [ ----------- ] |_ x4 _|     |_ z4 _|
//
// IMPORTANT USAGE INFORMATION (matrix transform):
//
// Return vector struct maps 1:1 to the above diagram:
//
//  typedef struct {
//   float z1;
//   float z2;
//   float z3;
//   float z4;
// } RETURN_VECTOR_STRUCT;
//
// Similarly to how the sine and cosine functions use fsca and return 2 floats,
// the matrix transform function actually returns 4 floats. The SH4 only multiplies
// 4x4 matrices with 4x1 vectors, which is why the output is like that.
//
// Multiply a matrix stored in the back bank (XMTRX) with an input vector
static inline __attribute__((always_inline)) RETURN_VECTOR_STRUCT MATH_Matrix_Transform(float x1, float x2, float x3, float x4)
{
  // The floats comprising FV4 are the regs that are passed in.
  // Just need to make sure GCC doesn't modify anything, and these register vars do that job.

  // Temporary variables are necessary per GCC to avoid clobbering:
  // https://gcc.gnu.org/onlinedocs/gcc/Local-Register-Variables.html#Local-Register-Variables

  float tx1 = x1;
  float tx2 = x2;
  float tx3 = x3;
  float tx4 = x4;

  // output vector FV0
  register float __z1 __asm__("fr0") = tx1;
  register float __z2 __asm__("fr1") = tx2;
  register float __z3 __asm__("fr2") = tx3;
  register float __z4 __asm__("fr3") = tx4;

  asm volatile ("ftrv XMTRX, FV0\n\t"
    // have to do this to obey SH4 calling convention--output returned in FV0
    : "+w" (__z1), "+f" (__z2), "+f" (__z3), "+f" (__z4) // outputs, "+" means r/w
    : // no inputs
    : // no clobbers
  );

  RETURN_VECTOR_STRUCT output = {__z1, __z2, __z3, __z4};
  return output;
}

// Matrix Transpose
//
// This does a matrix transpose on the matrix in XMTRX, which swaps rows with
// columns as follows (math notation is [XMTRX]^T):
//
//  [ a b c d ] T   [ a e i m ]
//  [ e f g h ]  =  [ b f j n ]
//  [ i j k l ]     [ c g k o ]
//  [ m n o p ]     [ d h l p ]
//
// PLEASE NOTE: It is faster to avoid the need for a transpose altogether by
// structuring matrices and vectors accordingly.
static inline __attribute__((always_inline)) void MATH_Matrix_Transpose(void)
{
  asm volatile (
    "frchg\n\t" // fmov for singles only works on front bank
    // FR0, FR5, FR10, and FR15 are already in place
    // swap FR1 and FR4
    "flds FR1, FPUL\n\t"
    "fmov FR4, FR1\n\t"
    "fsts FPUL, FR4\n\t"
    // swap FR2 and FR8
    "flds FR2, FPUL\n\t"
    "fmov FR8, FR2\n\t"
    "fsts FPUL, FR8\n\t"
    // swap FR3 and FR12
    "flds FR3, FPUL\n\t"
    "fmov FR12, FR3\n\t"
    "fsts FPUL, FR12\n\t"
    // swap FR6 and FR9
    "flds FR6, FPUL\n\t"
    "fmov FR9, FR6\n\t"
    "fsts FPUL, FR9\n\t"
    // swap FR7 and FR13
    "flds FR7, FPUL\n\t"
    "fmov FR13, FR7\n\t"
    "fsts FPUL, FR13\n\t"
    // swap FR11 and FR14
    "flds FR11, FPUL\n\t"
    "fmov FR14, FR11\n\t"
    "fsts FPUL, FR14\n\t"
    // restore XMTRX to back bank
    "frchg\n"
    : // no outputs
    : // no inputs
    : "fpul" // clobbers
  );
}

// Matrix product: matrix * matrix = matrix
//
// These use the whole dang floating point unit.
//
//  [ ----------- ] [ ----------- ]     [ ----------- ]
//  [ ---Back---- ] [ ---Front--- ]  =  [ ---XMTRX--- ]
//  [ ---Matrix-- ] [ ---Matrix-- ]     [ ----------- ]
//  [ --(XMTRX)-- ] [ ----------- ]     [ ----------- ]
//
// Multiply a matrix stored in the back bank with a matrix loaded from memory
// Output is stored in the back bank (XMTRX)
static inline __attribute__((always_inline)) void MATH_Matrix_Product(ALL_FLOATS_STRUCT * front_matrix)
{
  /*
    // This prefetching should help a bit if placed suitably far enough in advance (not here)
    // Possibly right before this function call. Change the "front_matrix" variable appropriately.
    // SH4 does not support r/w or temporal prefetch hints, so we only need to pass in an address.
    __builtin_prefetch(front_matrix);
  */

  unsigned int prefetch_scratch;

  asm volatile (
    "mov %[fmtrx], %[pref_scratch]\n\t" // parallel-load address for prefetching (MT)
    "add #32, %[pref_scratch]\n\t" // offset by 32 (EX - flow dependency, but 'add' is actually parallelized since 'mov Rm, Rn' is 0-cycle)
    "fschg\n\t" // switch fmov to paired moves (FE)
    "pref @%[pref_scratch]\n\t" // Get a head start prefetching the second half of the 64-byte data (LS)
    // interleave loads and matrix multiply 4x4
    "fmov.d @%[fmtrx]+, DR0\n\t" // (LS)
    "fmov.d @%[fmtrx]+, DR2\n\t"
    "fmov.d @%[fmtrx]+, DR4\n\t" // (LS) want to issue the next one before 'ftrv' for parallel exec
    "ftrv XMTRX, FV0\n\t" // (FE)

    "fmov.d @%[fmtrx]+, DR6\n\t"
    "fmov.d @%[fmtrx]+, DR8\n\t" // prefetch should work for here
    "ftrv XMTRX, FV4\n\t"

    "fmov.d @%[fmtrx]+, DR10\n\t"
    "fmov.d @%[fmtrx]+, DR12\n\t"
    "ftrv XMTRX, FV8\n\t"

    "fmov.d @%[fmtrx], DR14\n\t" // (LS, but this will stall 'ftrv' for 3 cycles)
    "fschg\n\t" // switch back to single moves (and avoid stalling 'ftrv') (FE)
    "ftrv XMTRX, FV12\n\t" // (FE)
    // Save output in XF regs
    "frchg\n"
    : [fmtrx] "+r" ((unsigned int)front_matrix), [pref_scratch] "=&r" (prefetch_scratch) // outputs, "+" means r/w
    : // no inputs
    : "fr0", "fr1", "fr2", "fr3", "fr4", "fr5", "fr6", "fr7", "fr8", "fr9", "fr10", "fr11", "fr12", "fr13", "fr14", "fr15" // clobbers (GCC doesn't know about back bank, so writing to it isn't clobbered)
  );
}

// Load two 4x4 matrices and multiply them, storing the output into the back bank (XMTRX)
//
// MATH_Load_Matrix_Product() is slightly faster than doing this:
//    MATH_Load_XMTRX(matrix1)
//    MATH_Matrix_Product(matrix2)
// as it saves having to do 2 extraneous 'fschg' instructions.
//
static inline __attribute__((always_inline)) void MATH_Load_Matrix_Product(ALL_FLOATS_STRUCT * matrix1, ALL_FLOATS_STRUCT * matrix2)
{
  /*
    // This prefetching should help a bit if placed suitably far enough in advance (not here)
    // Possibly right before this function call. Change the "matrix1" variable appropriately.
    // SH4 does not support r/w or temporal prefetch hints, so we only need to pass in an address.
    __builtin_prefetch(matrix1);
  */

  unsigned int prefetch_scratch;

  asm volatile (
    "mov %[bmtrx], %[pref_scratch]\n\t" // (MT)
    "add #32, %[pref_scratch]\n\t" // offset by 32 (EX - flow dependency, but 'add' is actually parallelized since 'mov Rm, Rn' is 0-cycle)
    "fschg\n\t" // switch fmov to paired moves (note: only paired moves can access XDn regs) (FE)
    "pref @%[pref_scratch]\n\t" // Get a head start prefetching the second half of the 64-byte data (LS)
    // back matrix
    "fmov.d @%[bmtrx]+, XD0\n\t" // (LS)
    "fmov.d @%[bmtrx]+, XD2\n\t"
    "fmov.d @%[bmtrx]+, XD4\n\t"
    "fmov.d @%[bmtrx]+, XD6\n\t"
    "pref @%[fmtrx]\n\t" // prefetch fmtrx now while we wait (LS)
    "fmov.d @%[bmtrx]+, XD8\n\t" // bmtrx prefetch should work for here
    "fmov.d @%[bmtrx]+, XD10\n\t"
    "fmov.d @%[bmtrx]+, XD12\n\t"
    "mov %[fmtrx], %[pref_scratch]\n\t" // (MT)
    "add #32, %[pref_scratch]\n\t" // store offset by 32 in r0 (EX - flow dependency, but 'add' is actually parallelized since 'mov Rm, Rn' is 0-cycle)
    "fmov.d @%[bmtrx], XD14\n\t"
    "pref @%[pref_scratch]\n\t" // Get a head start prefetching the second half of the 64-byte data (LS)
    // front matrix
    // interleave loads and matrix multiply 4x4
    "fmov.d @%[fmtrx]+, DR0\n\t"
    "fmov.d @%[fmtrx]+, DR2\n\t"
    "fmov.d @%[fmtrx]+, DR4\n\t" // (LS) want to issue the next one before 'ftrv' for parallel exec
    "ftrv XMTRX, FV0\n\t" // (FE)

    "fmov.d @%[fmtrx]+, DR6\n\t"
    "fmov.d @%[fmtrx]+, DR8\n\t"
    "ftrv XMTRX, FV4\n\t"

    "fmov.d @%[fmtrx]+, DR10\n\t"
    "fmov.d @%[fmtrx]+, DR12\n\t"
    "ftrv XMTRX, FV8\n\t"

    "fmov.d @%[fmtrx], DR14\n\t" // (LS, but this will stall 'ftrv' for 3 cycles)
    "fschg\n\t" // switch back to single moves (and avoid stalling 'ftrv') (FE)
    "ftrv XMTRX, FV12\n\t" // (FE)
    // Save output in XF regs
    "frchg\n"
    : [bmtrx] "+&r" ((unsigned int)matrix1), [fmtrx] "+r" ((unsigned int)matrix2), [pref_scratch] "=&r" (prefetch_scratch) // outputs, "+" means r/w, "&" means it's written to before all inputs are consumed
    : // no inputs
    : "fr0", "fr1", "fr2", "fr3", "fr4", "fr5", "fr6", "fr7", "fr8", "fr9", "fr10", "fr11", "fr12", "fr13", "fr14", "fr15" // clobbers (GCC doesn't know about back bank, so writing to it isn't clobbered)
  );
}

//------------------------------------------------------------------------------
// Matrix load and store operations
//------------------------------------------------------------------------------

// Load a matrix from memory into the back bank (XMTRX)
static inline __attribute__((always_inline)) void MATH_Load_XMTRX(ALL_FLOATS_STRUCT * back_matrix)
{
  /*
    // This prefetching should help a bit if placed suitably far enough in advance (not here)
    // Possibly right before this function call. Change the "back_matrix" variable appropriately.
    // SH4 does not support r/w or temporal prefetch hints, so we only need to pass in an address.
    __builtin_prefetch(back_matrix);
  */

  unsigned int prefetch_scratch;

  asm volatile (
    "mov %[bmtrx], %[pref_scratch]\n\t" // (MT)
    "add #32, %[pref_scratch]\n\t" // offset by 32 (EX - flow dependency, but 'add' is actually parallelized since 'mov Rm, Rn' is 0-cycle)
    "fschg\n\t" // switch fmov to paired moves (note: only paired moves can access XDn regs) (FE)
    "pref @%[pref_scratch]\n\t" // Get a head start prefetching the second half of the 64-byte data (LS)
    "fmov.d @%[bmtrx]+, XD0\n\t"
    "fmov.d @%[bmtrx]+, XD2\n\t"
    "fmov.d @%[bmtrx]+, XD4\n\t"
    "fmov.d @%[bmtrx]+, XD6\n\t"
    "fmov.d @%[bmtrx]+, XD8\n\t"
    "fmov.d @%[bmtrx]+, XD10\n\t"
    "fmov.d @%[bmtrx]+, XD12\n\t"
    "fmov.d @%[bmtrx], XD14\n\t"
    "fschg\n" // switch back to single moves
    : [bmtrx] "+r" ((unsigned int)back_matrix), [pref_scratch] "=&r" (prefetch_scratch) // outputs, "+" means r/w
    : // no inputs
    : // clobbers (GCC doesn't know about back bank, so writing to it isn't clobbered)
  );
}

// Store XMTRX to memory
static inline __attribute__((always_inline)) ALL_FLOATS_STRUCT * MATH_Store_XMTRX(ALL_FLOATS_STRUCT * destination)
{
  /*
    // This prefetching should help a bit if placed suitably far enough in advance (not here)
    // Possibly right before this function call. Change the "destination" variable appropriately.
    // SH4 does not support r/w or temporal prefetch hints, so we only need to pass in an address.
    __builtin_prefetch( (ALL_FLOATS_STRUCT*)((unsigned char*)destination + 32) ); // Store works backwards, so note the '+32' here
  */

  char * output = ((char*)destination) + sizeof(ALL_FLOATS_STRUCT) + 8; // ALL_FLOATS_STRUCT should be 64 bytes

  asm volatile (
    "fschg\n\t" // switch fmov to paired moves (note: only paired moves can access XDn regs) (FE)
    "pref @%[dest_base]\n\t" // Get a head start prefetching the second half of the 64-byte data (LS)
    "fmov.d XD0, @-%[out_mtrx]\n\t" // These do *(--output) = XDn (LS)
    "fmov.d XD2, @-%[out_mtrx]\n\t"
    "fmov.d XD4, @-%[out_mtrx]\n\t"
    "fmov.d XD6, @-%[out_mtrx]\n\t"
    "fmov.d XD8, @-%[out_mtrx]\n\t"
    "fmov.d XD10, @-%[out_mtrx]\n\t"
    "fmov.d XD12, @-%[out_mtrx]\n\t"
    "fmov.d XD14, @-%[out_mtrx]\n\t"
    "fschg\n" // switch back to single moves
    : [out_mtrx] "+&r" ((unsigned int)output) // outputs, "+" means r/w, "&" means it's written to before all inputs are consumed
    : [dest_base] "r" ((unsigned int)destination) // inputs
    : "memory" // clobbers
  );

  return destination;
}


// In general, writing the entire required math routine in one asm function is
// the best way to go for performance reasons anyways, and in that situation one
// can just throw calling convention to the wind until returning back to C.

//==============================================================================
// Miscellaneous Functions
//==============================================================================
//
// The following functions are provided as examples of ways in which these math
// functions can be used.
//
// Reminder: 1 fsca unit = 1/182.044444444 of a degree or 1/10430.3783505 of a radian
// In order to make the best use of fsca units, a program must expect them from
// the outset and not "make them" by dividing radians or degrees to get them,
// otherwise it's just giving the 'fsca' instruction radians or degrees!
//
/*

  //------------------------------------------------------------------------------
  // Commonly useful functions
  //------------------------------------------------------------------------------

  // Returns 1 if point 't' is inside triangle with vertices 'v0', 'v1', and 'v2', and 0 if not
  int MATH_Is_Point_In_Triangle(float v0x, float v0y, float v1x, float v1y, float v2x, float v2y, float ptx, float pty)

  //------------------------------------------------------------------------------
  // Interpolation
  //------------------------------------------------------------------------------

  // Linear interpolation
  float MATH_Lerp(float a, float b, float t)

  // Speherical interpolation ('theta' in fsca units)
  float MATH_Slerp(float a, float b, float t, float theta)

  //------------------------------------------------------------------------------
  // Fast Sinc functions (unnormalized, sin(x)/x version)
  //------------------------------------------------------------------------------
  // Just pass in MATH_pi * x for normalized versions :)

  // Sinc function (fsca units)
  float MATH_Fast_Sincf(float x)

  // Sinc function (degrees)
  float MATH_Fast_Sincf_Deg(float x)

  // Sinc function (rads)
  float MATH_Fast_Sincf_Rad(float x)

*/

//------------------------------------------------------------------------------
// Commonly useful functions
//------------------------------------------------------------------------------

// Returns 1 if point 'pt' is inside triangle with vertices 'v0', 'v1', and 'v2', and 0 if not
// Determines triangle center using barycentric coordinate transformation
// Adapted from: https://stackoverflow.com/questions/2049582/how-to-determine-if-a-point-is-in-a-2d-triangle
// Specifically the answer by user 'adreasdr' in addition to the comment by user 'urraka' on the answer from user 'Andreas Brinck'
//
// The notation here assumes v0x is the x-component of v0, v0y is the y-component of v0, etc.
//
static inline __attribute__((always_inline)) int MATH_Is_Point_In_Triangle(float v0x, float v0y, float v1x, float v1y, float v2x, float v2y, float ptx, float pty)
{
  float sdot = MATH_fipr(v0y, -v0x, v2y - v0y, v0x - v2x, v2x, v2y, ptx, pty);
  float tdot = MATH_fipr(v0x, -v0y, v0y - v1y, v1x - v0x, v1y, v1x, ptx, pty);

  float areadot = MATH_fipr(-v1y, v0y, v0x, v1x, v2x, -v1x + v2x, v1y - v2y, v2y);

  // 'areadot' could be negative depending on the winding of the triangle
  if(areadot < 0.0f)
  {
    sdot *= -1.0f;
    tdot *= -1.0f;
    areadot *= -1.0f;
  }

  if( (sdot > 0.0f) && (tdot > 0.0f) && (areadot > (sdot + tdot)) )
  {
    return 1;
  }

  return 0;
}

//------------------------------------------------------------------------------
// Interpolation
//------------------------------------------------------------------------------

// Linear interpolation
static inline __attribute__((always_inline)) float MATH_Lerp(float a, float b, float t)
{
  return MATH_fmac(t, (b-a), a);
}

// Speherical interpolation ('theta' in fsca units)
static inline __attribute__((always_inline)) float MATH_Slerp(float a, float b, float t, float theta)
{
  // a is an element of v0, b is an element of v1
  // v = ( v0 * sin(theta - t * theta) + v1 * sin(t * theta) ) / sin(theta)
  // by using sine/cosine identities and properties, this can be optimized to:
  // v = v0 * cos(-t * theta) + ( v0 * ( cos(theta) * sin(-t * theta) ) - sin(-t * theta) * v1 ) / sin(theta)
  // which only requires two calls to fsca.
  // Specifically, sin(a + b) = sin(a)cos(b) + cos(a)sin(b) & sin(-a) = -sin(a)

  // MATH_fsca_* functions return reverse-ordered complex numbers for speed reasons (i.e. normally sine is the imaginary part)
  // This could be made even faster by using MATH_fsca_Int() with 'theta' and 't' as unsigned ints

#if __GNUC__ <= GNUC_FSCA_ERROR_VERSION

  RETURN_FSCA_STRUCT sine_cosine = MATH_fsca_Float(theta);
  float sine_value_theta = sine_cosine.sine;
  float cosine_value_theta = sine_cosine.cosine;

  RETURN_FSCA_STRUCT sine_cosine2 = MATH_fsca_Float(-t * theta);
  float sine_value_minus_t_theta = sine_cosine2.sine;
  float cosine_value_minus_t_theta = sine_cosine2.cosine;

#else

  _Complex float sine_cosine = MATH_fsca_Float(theta);
  float sine_value_theta = __real__ sine_cosine;
  float cosine_value_theta = __imag__ sine_cosine;

  _Complex float sine_cosine2 = MATH_fsca_Float(-t * theta);
  float sine_value_minus_t_theta = __real__ sine_cosine2;
  float cosine_value_minus_t_theta = __imag__ sine_cosine2;

#endif

  float numer = a * cosine_value_theta * sine_value_minus_t_theta - sine_value_minus_t_theta * b;
  float output_float = a * cosine_value_minus_t_theta + MATH_Fast_Divide(numer, sine_value_theta);

  return output_float;
}

//------------------------------------------------------------------------------
// Fast Sinc (unnormalized, sin(x)/x version)
//------------------------------------------------------------------------------
//
// Just pass in MATH_pi * x for normalized versions :)
//

// Sinc function (fsca units)
static inline __attribute__((always_inline)) float MATH_Fast_Sincf(float x)
{
  if(x == 0.0f)
  {
    return 1.0f;
  }

#if __GNUC__ <= GNUC_FSCA_ERROR_VERSION

  RETURN_FSCA_STRUCT sine_cosine = MATH_fsca_Float(x);
  float sine_value = sine_cosine.sine;

#else

  _Complex float sine_cosine = MATH_fsca_Float(x);
  float sine_value = __real__ sine_cosine;

#endif

  return MATH_Fast_Divide(sine_value, x);
}

// Sinc function (degrees)
static inline __attribute__((always_inline)) float MATH_Fast_Sincf_Deg(float x)
{
  if(x == 0.0f)
  {
    return 1.0f;
  }

#if __GNUC__ <= GNUC_FSCA_ERROR_VERSION

  RETURN_FSCA_STRUCT sine_cosine = MATH_fsca_Float_Deg(x);
  float sine_value = sine_cosine.sine;

#else

  _Complex float sine_cosine = MATH_fsca_Float_Deg(x);
  float sine_value = __real__ sine_cosine;

#endif

  return MATH_Fast_Divide(sine_value, x);
}

// Sinc function (rads)
static inline __attribute__((always_inline)) float MATH_Fast_Sincf_Rad(float x)
{
  if(x == 0.0f)
  {
    return 1.0f;
  }

#if __GNUC__ <= GNUC_FSCA_ERROR_VERSION

  RETURN_FSCA_STRUCT sine_cosine = MATH_fsca_Float_Rad(x);
  float sine_value = sine_cosine.sine;

#else

  _Complex float sine_cosine = MATH_fsca_Float_Rad(x);
  float sine_value = __real__ sine_cosine;

#endif

  return MATH_Fast_Divide(sine_value, x);
}

//==============================================================================
// Miscellaneous Snippets
//==============================================================================
//
// The following snippets are best implemented manually in user code (they can't
// be put into their own functions without incurring performance penalties).
//
// They also serve as examples of how one might use the functions in this header.
//
/*
  Normalize a vector (x, y, z) and get its pre-normalized magnitude (length)
*/

//
// Normalize a vector (x, y, z) and get its pre-normalized magnitude (length)
//
// magnitude = sqrt(x^2 + y^2 + z^2)
// (x, y, z) = 1/magnitude * (x, y, z)
//
// x, y, z, and magnitude are assumed already existing floats
//

/* -- start --

  // Don't need an 'else' with this (if length is 0, x = y = z = 0)
  magnitude = 0;

  if(__builtin_expect(x || y || z, 1))
  {
    temp = MATH_Sum_of_Squares(x, y, z, 0); // temp = x^2 + y^2 + z^2 + 0^2
    float normalizer = MATH_fsrra(temp); // 1/sqrt(temp)
    x = normalizer * x;
    y = normalizer * y;
    z = normalizer * z;
    magnitude = MATH_Fast_Invert(normalizer);
  }

-- end -- */


#endif /* __SH4_MATH_H_ */