1  #ifndef _AFFINE_TRANSFORM_H_
  2  #define _AFFINE_TRANSFORM_H_
  3  
  4  // Stolen from RISCBoy
  5  
  6  #include <stdint.h>
  7  #include "pico/platform.h"
  8  
  9  // Store unpacked affine transforms as signed 16.16 fixed point in the following order:
 10  // a00, a01, b0,   a10, a11, b1
 11  // i.e. the top two rows of the matrix
 12  // [ a00 a01 b0 ]
 13  // [ a01 a11 b1 ]
 14  // [ 0   0   1  ]
 15  // Then pack integers appropriately 
 16  
 17  typedef int32_t affine_transform_t[6];
 18  static const int32_t AF_ONE = 1 << 16;
 19  
 20  static inline __attribute__((always_inline)) int32_t mul_fp1616(int32_t x, int32_t y)
 21  {
 22  	int64_t result = (int64_t)x * y;
 23  	return result >> 16;
 24  }
 25  
 26  // result can not be == left or right
 27  static inline void affine_mul(affine_transform_t result, const affine_transform_t left, const affine_transform_t right)
 28  {
 29  	result[0] = mul_fp1616(left[0], right[0]) + mul_fp1616(left[1], right[3]);
 30  	result[1] = mul_fp1616(left[0], right[1]) + mul_fp1616(left[1], right[4]);
 31  	result[2] = mul_fp1616(left[0], right[2]) + mul_fp1616(left[1], right[5]) + left[2];
 32  	result[3] = mul_fp1616(left[3], right[0]) + mul_fp1616(left[4], right[3]);
 33  	result[4] = mul_fp1616(left[3], right[1]) + mul_fp1616(left[4], right[4]);
 34  	result[5] = mul_fp1616(left[3], right[2]) + mul_fp1616(left[4], right[5]) + left[5];
 35  }
 36  
 37  static inline void affine_copy(affine_transform_t dst, const affine_transform_t src)
 38  {
 39  	for (int i = 0; i < 6; ++i)
 40  		dst[i] = src[i];
 41  }
 42  
 43  // User is describing a sequence of transformations from texture space to
 44  // screen space, which are applied by premultiplying a column vector. However,
 45  // hardware transforms *from* screenspace *to* texture space, so we want the
 46  // inverse of the transform the user is building. Therefore our functions each
 47  // produce the inverse of the requested transform, and we apply transforms by
 48  // *post*-multiplication.
 49  static inline void affine_identity(affine_transform_t current_trans)
 50  {
 51  	int32_t tmp[6] = {
 52  		AF_ONE, 0,      0,
 53  		0,      AF_ONE, 0
 54  	};
 55  	affine_copy(current_trans, tmp);
 56  }
 57  
 58  static inline void affine_translate(affine_transform_t current_trans, int32_t x, int32_t y)
 59  {
 60  	int32_t tmp[6];
 61  	int32_t transform[6] = {
 62  		AF_ONE, 0,      -AF_ONE * x,
 63  		0,      AF_ONE, -AF_ONE * y
 64  	};
 65  	affine_mul(tmp, current_trans, transform);
 66  	affine_copy(current_trans, tmp);
 67  }
 68  
 69  // TODO this is shit
 70  static const int32_t __not_in_flash("atrans") sin_lookup_fp1616[256] = {
 71  	0x0, 0x648, 0xc8f, 0x12d5, 0x1917, 0x1f56, 0x2590, 0x2bc4, 0x31f1, 0x3817,
 72  	0x3e33, 0x4447, 0x4a50, 0x504d, 0x563e, 0x5c22, 0x61f7, 0x67bd, 0x6d74,
 73  	0x7319, 0x78ad, 0x7e2e, 0x839c, 0x88f5, 0x8e39, 0x9368, 0x987f, 0x9d7f,
 74  	0xa267, 0xa736, 0xabeb, 0xb085, 0xb504, 0xb968, 0xbdae, 0xc1d8, 0xc5e4,
 75  	0xc9d1, 0xcd9f, 0xd14d, 0xd4db, 0xd848, 0xdb94, 0xdebe, 0xe1c5, 0xe4aa,
 76  	0xe76b, 0xea09, 0xec83, 0xeed8, 0xf109, 0xf314, 0xf4fa, 0xf6ba, 0xf853,
 77  	0xf9c7, 0xfb14, 0xfc3b, 0xfd3a, 0xfe13, 0xfec4, 0xff4e, 0xffb1, 0xffec,
 78  	0x10000, 0xffec, 0xffb1, 0xff4e, 0xfec4, 0xfe13, 0xfd3a, 0xfc3b, 0xfb14,
 79  	0xf9c7, 0xf853, 0xf6ba, 0xf4fa, 0xf314, 0xf109, 0xeed8, 0xec83, 0xea09,
 80  	0xe76b, 0xe4aa, 0xe1c5, 0xdebe, 0xdb94, 0xd848, 0xd4db, 0xd14d, 0xcd9f,
 81  	0xc9d1, 0xc5e4, 0xc1d8, 0xbdae, 0xb968, 0xb504, 0xb085, 0xabeb, 0xa736,
 82  	0xa267, 0x9d7f, 0x987f, 0x9368, 0x8e39, 0x88f5, 0x839c, 0x7e2e, 0x78ad,
 83  	0x7319, 0x6d74, 0x67bd, 0x61f7, 0x5c22, 0x563e, 0x504d, 0x4a50, 0x4447,
 84  	0x3e33, 0x3817, 0x31f1, 0x2bc4, 0x2590, 0x1f56, 0x1917, 0x12d5, 0xc8f, 0x648,
 85  	0x0, 0xfffff9b8, 0xfffff371, 0xffffed2b, 0xffffe6e9, 0xffffe0aa, 0xffffda70,
 86  	0xffffd43c, 0xffffce0f, 0xffffc7e9, 0xffffc1cd, 0xffffbbb9, 0xffffb5b0,
 87  	0xffffafb3, 0xffffa9c2, 0xffffa3de, 0xffff9e09, 0xffff9843, 0xffff928c,
 88  	0xffff8ce7, 0xffff8753, 0xffff81d2, 0xffff7c64, 0xffff770b, 0xffff71c7,
 89  	0xffff6c98, 0xffff6781, 0xffff6281, 0xffff5d99, 0xffff58ca, 0xffff5415,
 90  	0xffff4f7b, 0xffff4afc, 0xffff4698, 0xffff4252, 0xffff3e28, 0xffff3a1c,
 91  	0xffff362f, 0xffff3261, 0xffff2eb3, 0xffff2b25, 0xffff27b8, 0xffff246c,
 92  	0xffff2142, 0xffff1e3b, 0xffff1b56, 0xffff1895, 0xffff15f7, 0xffff137d,
 93  	0xffff1128, 0xffff0ef7, 0xffff0cec, 0xffff0b06, 0xffff0946, 0xffff07ad,
 94  	0xffff0639, 0xffff04ec, 0xffff03c5, 0xffff02c6, 0xffff01ed, 0xffff013c,
 95  	0xffff00b2, 0xffff004f, 0xffff0014, 0xffff0000, 0xffff0014, 0xffff004f,
 96  	0xffff00b2, 0xffff013c, 0xffff01ed, 0xffff02c6, 0xffff03c5, 0xffff04ec,
 97  	0xffff0639, 0xffff07ad, 0xffff0946, 0xffff0b06, 0xffff0cec, 0xffff0ef7,
 98  	0xffff1128, 0xffff137d, 0xffff15f7, 0xffff1895, 0xffff1b56, 0xffff1e3b,
 99  	0xffff2142, 0xffff246c, 0xffff27b8, 0xffff2b25, 0xffff2eb3, 0xffff3261,
100  	0xffff362f, 0xffff3a1c, 0xffff3e28, 0xffff4252, 0xffff4698, 0xffff4afc,
101  	0xffff4f7b, 0xffff5415, 0xffff58ca, 0xffff5d99, 0xffff6281, 0xffff6781,
102  	0xffff6c98, 0xffff71c7, 0xffff770b, 0xffff7c64, 0xffff81d2, 0xffff8753,
103  	0xffff8ce7, 0xffff928c, 0xffff9843, 0xffff9e09, 0xffffa3de, 0xffffa9c2,
104  	0xffffafb3, 0xffffb5b0, 0xffffbbb9, 0xffffc1cd, 0xffffc7e9, 0xffffce0f,
105  	0xffffd43c, 0xffffda70, 0xffffe0aa, 0xffffe6e9, 0xffffed2b, 0xfffff371,
106  	0xfffff9b8
107  };
108  
109  static inline int32_t sin_fp1616(uint8_t theta)
110  {
111  	return sin_lookup_fp1616[theta];
112  }
113  
114  static inline int32_t cos_fp1616(uint8_t theta)
115  {
116  	return sin_lookup_fp1616[(theta + 64) & 0xff];
117  }
118  
119  // Appears as a counterclockwise rotation (when viewed from texture space to screen space)
120  // Units of angle are 256 = one turn
121  static inline void affine_rotate(affine_transform_t current_trans, uint8_t theta)
122  {
123  	int32_t tmp[6];
124  	int32_t transform[6] = {
125  		cos_fp1616(theta), -sin_fp1616(theta), 0,
126  		sin_fp1616(theta),  cos_fp1616(theta), 0
127  	};
128  	affine_mul(tmp, current_trans, transform);
129  	affine_copy(current_trans, tmp);
130  }
131  
132  static inline void affine_scale(affine_transform_t current_trans, int32_t sx, int32_t sy) {
133  	int32_t sx_inv = ((int64_t)AF_ONE * AF_ONE) / sx;
134  	int32_t sy_inv = ((int64_t)AF_ONE * AF_ONE) / sy;
135  	int32_t tmp[6];
136  	int32_t transform[6] = {
137  		sx_inv, 0,      0,
138  		0,      sy_inv, 0
139  	};
140  	affine_mul(tmp, current_trans, transform);
141  	affine_copy(current_trans, tmp);
142  }
143  
144  #endif // _AFFINE_TRANSFORM_H_