1.1 --- a/src/ds3.c	Sun Jan 28 20:05:26 2018 +0200
     1.2 +++ b/src/ds3.c	Mon Jan 29 03:48:05 2018 +0200
     1.3 @@ -1,4 +1,5 @@
     1.4  #include <stdint.h>
     1.5 +#include <math.h>
     1.6  #include "dsregs.h"
     1.7  #include "ds3.h"
     1.8  
     1.9 @@ -52,6 +53,42 @@
    1.10  	}
    1.11  }
    1.12  
    1.13 +void ds3_load_matrix4x3(int32_t *m)
    1.14 +{
    1.15 +	int i;
    1.16 +	for(i=0; i<12; i++) {
    1.17 +		int16_t val = (int16_t)(*m++ >> 4);
    1.18 +		REG_MTX_LOAD_4X3 = val;
    1.19 +	}
    1.20 +}
    1.21 +
    1.22 +void ds3_mult_matrix(int32_t *m)
    1.23 +{
    1.24 +	int i;
    1.25 +	for(i=0; i<16; i++) {
    1.26 +		int16_t val = (int16_t)(*m++ >> 4);
    1.27 +		REG_MTX_MULT_4X4 = val;
    1.28 +	}
    1.29 +}
    1.30 +
    1.31 +void ds3_mult_matrix4x3(int32_t *m)
    1.32 +{
    1.33 +	int i;
    1.34 +	for(i=0; i<12; i++) {
    1.35 +		int16_t val = (int16_t)(*m++ >> 4);
    1.36 +		REG_MTX_MULT_4X3 = val;
    1.37 +	}
    1.38 +}
    1.39 +
    1.40 +void ds3_mult_matrix3x3(int32_t *m)
    1.41 +{
    1.42 +	int i;
    1.43 +	for(i=0; i<9; i++) {
    1.44 +		int16_t val = (int16_t)(*m++ >> 4);
    1.45 +		REG_MTX_MULT_3X3 = val;
    1.46 +	}
    1.47 +}
    1.48 +
    1.49  void ds3_push_matrix(void)
    1.50  {
    1.51  	REG_MTX_PUSH = 0;
    1.52 @@ -97,6 +134,21 @@
    1.53  	REG_VTX_16 = (z >> 4) & 0xffff;
    1.54  }
    1.55  
    1.56 +void ds3_vertex3f(float x, float y, float z)
    1.57 +{
    1.58 +	ds3_vertex3((int32_t)(x * 65536.0f), (int32_t)(y * 65536.0f), (int32_t)(z * 65536.0f));
    1.59 +}
    1.60 +
    1.61 +void ds3_vertex2(int32_t x, int32_t y)
    1.62 +{
    1.63 +	REG_VTX_XY = ((x >> 4) & 0xffff) | ((y << 12) & 0xffff0000);
    1.64 +}
    1.65 +
    1.66 +void ds3_vertex2f(float x, float y)
    1.67 +{
    1.68 +	ds3_vertex2((int32_t)(x * 65536.0f), (int32_t)(y * 65536.0f));
    1.69 +}
    1.70 +
    1.71  void ds3_color(uint16_t color)
    1.72  {
    1.73  	REG_COLOR = color;
    1.74 @@ -106,3 +158,130 @@
    1.75  {
    1.76  	REG_COLOR = RGB15(r >> 3, g >> 3, b >> 3);
    1.77  }
    1.78 +
    1.79 +void ds3_color3f(float r, float g, float b)
    1.80 +{
    1.81 +	uint16_t ir = r * 31.0f;
    1.82 +	uint16_t ig = g * 31.0f;
    1.83 +	uint16_t ib = b * 31.0f;
    1.84 +	REG_COLOR = RGB15(ir, ig, ib);
    1.85 +}
    1.86 +
    1.87 +void ds3_normal(int32_t x, int32_t y, int32_t z)
    1.88 +{
    1.89 +	REG_NORMAL = ((x >> 7) & 0x3ff) | ((y << 17) & 0xffc00) | ((z << 27) & 0x3ff00000);
    1.90 +}
    1.91 +
    1.92 +void ds3_normal3f(float x, float y, float z)
    1.93 +{
    1.94 +	ds3_normal((int32_t)(x * 65536.0f), (int32_t)(y * 65536.0f), (int32_t)(z * 65536.0f));
    1.95 +}
    1.96 +
    1.97 +void ds3_texcoord2(int32_t s, int32_t t)
    1.98 +{
    1.99 +	REG_TEXCOORD = (((s) >> 12) & 0xffff) | (((t) << 4) & 0xffff0000);
   1.100 +}
   1.101 +
   1.102 +void ds3_texcoord2f(float s, float t)
   1.103 +{
   1.104 +	ds3_texcoord2((int32_t)(s * 65536.0f), (int32_t)(t * 65536.0f));
   1.105 +}
   1.106 +
   1.107 +void ds3_ortho(int32_t left, int32_t right, int32_t top, int32_t bottom, int32_t znear, int32_t zfar)
   1.108 +{
   1.109 +	int32_t m[16] = {0};
   1.110 +	int32_t dx = right - left;
   1.111 +	int32_t dy = top - bottom;
   1.112 +	int32_t dz = zfar - znear;
   1.113 +
   1.114 +	m[0] = 0x2000000 / (dx << 8);
   1.115 +	m[5] = 0x2000000 / (dy << 8);
   1.116 +	m[10] = -0x2000000 / (dz << 8);
   1.117 +	m[12] = -((right + left) << 8) / (dx << 8);
   1.118 +	m[13] = -((top + bottom) << 8) / (dy << 8);
   1.119 +	m[14] = -((zfar + znear) << 8) / (dz << 8);
   1.120 +
   1.121 +	ds3_mult_matrix(m);
   1.122 +}
   1.123 +
   1.124 +void ds3_orthof(float left, float right, float top, float bottom, float znear, float zfar)
   1.125 +{
   1.126 +	int32_t m[16] = {0};
   1.127 +	float dx = right - left;
   1.128 +	float dy = top - bottom;
   1.129 +	float dz = zfar - znear;
   1.130 +
   1.131 +	m[0] = (int32_t)(2.0f / dx * 65536.0f);
   1.132 +	m[5] = (int32_t)(2.0f / dy * 65536.0f);
   1.133 +	m[10] = (int32_t)(-2.0f / dz * 65536.0f);
   1.134 +	m[12] = (int32_t)(-(right + left) / dx);
   1.135 +	m[13] = (int32_t)(-(top + bottom) / dy);
   1.136 +	m[14] = (int32_t)(-(zfar + znear) / dz);
   1.137 +
   1.138 +	ds3_mult_matrix(m);
   1.139 +}
   1.140 +
   1.141 +void ds3_frustum(int32_t left, int32_t right, int32_t top, int32_t bottom, int32_t znear, int32_t zfar)
   1.142 +{
   1.143 +	int32_t m[16] = {0};
   1.144 +
   1.145 +	int32_t dx = right - left;
   1.146 +	int32_t dy = top - bottom;
   1.147 +	int32_t dz = zfar - znear;
   1.148 +
   1.149 +	int32_t a = ((right + left) << 8) / (dx << 8);
   1.150 +	int32_t b = ((top + bottom) << 8) / (dy << 8);
   1.151 +	int32_t c = (-(zfar + znear) << 8) / (dz << 8);
   1.152 +	int32_t d = -(((zfar >> 8) * znear) << 1) / (dz << 8);
   1.153 +
   1.154 +	m[0] = (znear << 9) / (dx << 8);
   1.155 +	m[5] = (znear << 9) / (dy << 8);
   1.156 +	m[8] = a;
   1.157 +	m[9] = b;
   1.158 +	m[10] = c;
   1.159 +	m[11] = -65536;
   1.160 +	m[14] = d;
   1.161 +
   1.162 +	ds3_mult_matrix(m);
   1.163 +}
   1.164 +
   1.165 +void g3d_frustum(float left, float right, float bottom, float top, float nr, float fr)
   1.166 +{
   1.167 +	int32_t m[16] = {0};
   1.168 +
   1.169 +	float dx = right - left;
   1.170 +	float dy = top - bottom;
   1.171 +	float dz = fr - nr;
   1.172 +
   1.173 +	float a = (right + left) / dx;
   1.174 +	float b = (top + bottom) / dy;
   1.175 +	float c = -(fr + nr) / dz;
   1.176 +	float d = -2.0 * fr * nr / dz;
   1.177 +
   1.178 +	m[0] = (int32_t)(2.0 * nr / dx * 65536.0f);
   1.179 +	m[5] = (int32_t)(2.0 * nr / dy * 65536.0f);
   1.180 +	m[8] = (int32_t)(a * 65536.0f);
   1.181 +	m[9] = (int32_t)(b * 65536.0f);
   1.182 +	m[10] = (int32_t)(c * 65536.0f);
   1.183 +	m[11] = -65536;
   1.184 +	m[14] = (int32_t)(d * 65536.0f);
   1.185 +
   1.186 +	ds3_mult_matrix(m);
   1.187 +}
   1.188 +
   1.189 +void ds3_perspective(float vfov_deg, float aspect, float znear, float zfar)
   1.190 +{
   1.191 +	int32_t m[16] = {0};
   1.192 +
   1.193 +	float vfov = M_PI * vfov_deg / 180.0f;
   1.194 +	float s = 1.0f / tan(vfov * 0.5f);
   1.195 +	float range = znear - zfar;
   1.196 +
   1.197 +	m[0] = (int32_t)(s / aspect * 65536.0f);
   1.198 +	m[5] = (int32_t)(s * 65536.0f);
   1.199 +	m[10] = (int32_t)((znear + zfar) / range * 65536.0f);
   1.200 +	m[11] = -65536;
   1.201 +	m[14] = (int32_t)(2.0f * znear * zfar / range * 65536.0f);
   1.202 +
   1.203 +	ds3_mult_matrix(m);
   1.204 +}