void
fix16_vector3_add(const fix16_vector3_t *v0, const fix16_vector3_t *v1,
fix16_vector3_t *result)
{
result->x = fix16_add(v0->x, v1->x);
result->y = fix16_add(v0->y, v1->y);
result->z = fix16_add(v0->z, v1->z);
}
fix16_t
fix16_vector3_dot(const fix16_vector3_t *v0, const fix16_vector3_t *v1)
{
fix16_t term[3];
term[0] = fix16_mul(v0->x, v1->x);
term[1] = fix16_mul(v0->y, v1->y);
term[2] = fix16_mul(v0->z, v1->z);
return fix16_add(term[0], fix16_add(term[1], term[2]));
}
static void mf16_addsub(mf16 *dest, const mf16 *a, const mf16 *b, uint8_t add)
{
int row, column;
dest->errors = a->errors | b->errors;
if (a->columns != b->columns || a->rows != b->rows)
dest->errors |= FIXMATRIX_DIMERR;
dest->rows = a->rows;
dest->columns = a->columns;
for (row = 0; row < dest->rows; row++)
{
for (column = 0; column < dest->columns; column++)
{
fix16_t sum;
if (add)
sum = fix16_add(a->data[row][column], b->data[row][column]);
else
sum = fix16_sub(a->data[row][column], b->data[row][column]);
if (sum == fix16_overflow)
dest->errors |= FIXMATRIX_OVERFLOW;
dest->data[row][column] = sum;
}
}
}
void kalman_gravity_demo_lambda()
{
// initialize the filter
kalman_gravity_init();
mf16 *x = kalman_get_state_vector(&kf);
mf16 *z = kalman_get_observation_vector(&kfm);
// forcibly increase uncertainty in every prediction step by ~20% (1/lambda^2)
const fix16_t lambda = F16(0.9);
// filter!
uint_fast16_t i;
for (i = 0; i < MEAS_COUNT; ++i)
{
// prediction.
kalman_predict_tuned(&kf, lambda);
// measure ...
fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
matrix_set(z, 0, 0, measurement);
// update
kalman_correct(&kf, &kfm);
}
// fetch estimated g
const fix16_t g_estimated = x->data[2][0];
const float value = fix16_to_float(g_estimated);
assert(value > 9.7 && value < 10);
}
void kalman_gravity_demo()
{
// initialize the filter
kalman_gravity_init();
mf16 *x = kalman_get_state_vector_uc(&kf);
mf16 *z = kalman_get_observation_vector(&kfm);
// filter!
uint_fast16_t i;
for (i = 0; i < MEAS_COUNT; ++i)
{
// prediction.
kalman_predict_uc(&kf);
// measure ...
fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
matrix_set(z, 0, 0, measurement);
// update
kalman_correct_uc(&kf, &kfm);
}
// fetch estimated g
const fix16_t g_estimated = x->data[2][0];
const float value = fix16_to_float(g_estimated);
assert(value > 9.7 && value < 10);
}
// calculate phase
static inline void osc_calc_pm(osc* osc) {
osc->idxMod = fix16_add( osc->idx,
fix16_mul( FRACT_FIX16( mult_fr1x32x32( osc->pmIn,
osc->pmAmt ) ),
WAVE_TAB_MAX16
) );
// wrap negative
while (BIT_SIGN_32(osc->idxMod)) {
osc->idxMod = fix16_add(osc->idxMod, WAVE_TAB_MAX16);
}
// wrap positive
while(osc->idxMod > WAVE_TAB_MAX16) {
osc->idxMod = fix16_sub(osc->idxMod, WAVE_TAB_MAX16);
}
}
static inline void
_set_fixed_point_scroll(fix16_t *scroll, fix16_t amount, uint16_t *in,
uint16_t *dn)
{
int32_t integral;
integral = fix16_to_int(amount);
fix16_t fractional;
fractional = fix16_fractional(amount);
*in = integral;
*dn = fractional & 0xFF00;
*scroll = fix16_add(fix16_from_int(*in), fractional);
}
void
fix16_vector3_matrix3_multiply(const fix16_matrix3_t *m0,
const fix16_vector3_t *v0, fix16_vector3_t *result)
{
result->comp[0] = fix16_add(
fix16_add(fix16_mul(v0->x, m0->row[0].x), fix16_mul(v0->y, m0->row[0].y)),
fix16_mul(v0->z, m0->row[0].z));
result->comp[1] = fix16_add(
fix16_add(fix16_mul(v0->x, m0->row[1].x), fix16_mul(v0->y, m0->row[1].y)),
fix16_mul(v0->z, m0->row[1].z));
result->comp[2] = fix16_add(
fix16_add(fix16_mul(v0->x, m0->row[2].x), fix16_mul(v0->y, m0->row[2].y)),
fix16_mul(v0->z, m0->row[2].z));
}
int main()
{
int i;
interface_init();
start_timing();
print_value("Timestamp bias", end_timing());
for (i = 0; i < TESTCASES1_COUNT; i++)
{
fix16_t input = testcases1[i].a;
fix16_t result;
fix16_t expected = testcases1[i].sqrt;
MEASURE(sqrt_cycles, result = fix16_sqrt(input));
if (input > 0 && delta(result, expected) > max_delta)
{
print_value("Failed SQRT, i", i);
print_value("Failed SQRT, input", input);
print_value("Failed SQRT, output", result);
print_value("Failed SQRT, expected", expected);
}
expected = testcases1[i].exp;
MEASURE(exp_cycles, result = fix16_exp(input));
if (delta(result, expected) > 400)
{
print_value("Failed EXP, i", i);
print_value("Failed EXP, input", input);
print_value("Failed EXP, output", result);
print_value("Failed EXP, expected", expected);
}
}
PRINT(sqrt_cycles, "fix16_sqrt");
PRINT(exp_cycles, "fix16_exp");
for (i = 0; i < TESTCASES2_COUNT; i++)
{
fix16_t a = testcases2[i].a;
fix16_t b = testcases2[i].b;
volatile fix16_t result;
fix16_t expected = testcases2[i].add;
MEASURE(add_cycles, result = fix16_add(a, b));
if (delta(result, expected) > max_delta)
{
print_value("Failed ADD, i", i);
print_value("Failed ADD, a", a);
print_value("Failed ADD, b", b);
print_value("Failed ADD, output", result);
print_value("Failed ADD, expected", expected);
}
expected = testcases2[i].sub;
MEASURE(sub_cycles, result = fix16_sub(a, b));
if (delta(result, expected) > max_delta)
{
print_value("Failed SUB, i", i);
print_value("Failed SUB, a", a);
print_value("Failed SUB, b", b);
print_value("Failed SUB, output", result);
print_value("Failed SUB, expected", expected);
}
expected = testcases2[i].mul;
MEASURE(mul_cycles, result = fix16_mul(a, b));
if (delta(result, expected) > max_delta)
{
print_value("Failed MUL, i", i);
print_value("Failed MUL, a", a);
print_value("Failed MUL, b", b);
print_value("Failed MUL, output", result);
print_value("Failed MUL, expected", expected);
}
if (b != 0)
{
expected = testcases2[i].div;
MEASURE(div_cycles, result = fix16_div(a, b));
if (delta(result, expected) > max_delta)
{
print_value("Failed DIV, i", i);
print_value("Failed DIV, a", a);
print_value("Failed DIV, b", b);
print_value("Failed DIV, output", result);
print_value("Failed DIV, expected", expected);
}
}
}
PRINT(add_cycles, "fix16_add");
PRINT(sub_cycles, "fix16_sub");
PRINT(mul_cycles, "fix16_mul");
PRINT(div_cycles, "fix16_div");
/* Compare with floating point performance */
#ifndef NO_FLOAT
for (i = 0; i < TESTCASES1_COUNT; i++)
{
float input = fix16_to_float(testcases1[i].a);
volatile float result;
MEASURE(float_sqrtf_cycles, result = sqrtf(input));
}
PRINT(float_sqrtf_cycles, "float sqrtf");
for (i = 0; i < TESTCASES2_COUNT; i++)
{
float a = fix16_to_float(testcases2[i].a);
float b = fix16_to_float(testcases2[i].b);
volatile float result;
MEASURE(float_add_cycles, result = a + b);
MEASURE(float_sub_cycles, result = a - b);
MEASURE(float_mul_cycles, result = a * b);
if (b != 0)
{
MEASURE(float_div_cycles, result = a / b);
}
}
PRINT(float_add_cycles, "float add");
PRINT(float_sub_cycles, "float sub");
PRINT(float_mul_cycles, "float mul");
PRINT(float_div_cycles, "float div");
#endif
return 0;
}
void
main(void)
{
hardware_init();
uint32_t color_idx;
for (color_idx = 0; color_idx < TEAPOT_POLYGON_CNT; color_idx++) {
uint32_t idx;
idx = color_idx;
uint32_t polygon_idx;
fix16_vector4_t *polygon[4];
for (polygon_idx = 0; polygon_idx < 4; polygon_idx++) {
uint32_t vertex_idx;
vertex_idx = teapot_indices[(4 * idx) + polygon_idx];
fix16_vector4_t *vertex;
vertex = &teapot_vertices[vertex_idx];
polygon[polygon_idx] = vertex;
}
fix16_t avg;
avg = fix16_add(fix16_mul(fix16_add(
fix16_add(polygon[0]->z, polygon[1]->z),
fix16_add(polygon[2]->z, polygon[3]->z)),
F16(1.0f / 4.0f)), F16(0.1f));
uint16_t color;
color = fix16_to_int(fix16_add(
fix16_mul(fix16_abs(avg), F16(255.0f)),
F16(16.0f)));
colors[color_idx] = RGB888_TO_RGB555(color, color, color);
}
ot_init();
matrix_stack_init();
matrix_stack_mode(MATRIX_STACK_MODE_PROJECTION);
fix16_t ratio;
ratio = F16((float)SCREEN_WIDTH / (float)SCREEN_HEIGHT);
matrix_stack_orthographic_project(-fix16_mul(F16(1.0f), ratio),
fix16_mul(F16(1.0f), ratio),
F16(1.0f), F16(-1.0f),
F16(1.0f), F16(1.0f));
matrix_stack_mode(MATRIX_STACK_MODE_MODEL_VIEW);
matrix_stack_translate(F16(0.0f), F16(0.0f), F16(-10.0f));
vdp1_cmdt_list_begin(0); {
struct vdp1_cmdt_local_coord local_coord;
local_coord.lc_coord.x = SCREEN_WIDTH / 2;
local_coord.lc_coord.y = SCREEN_HEIGHT / 2;
vdp1_cmdt_local_coord_set(&local_coord);
vdp1_cmdt_end();
} vdp1_cmdt_list_end(0);
struct vdp1_cmdt_polygon polygon;
memset(&polygon, 0x00, sizeof(polygon));
vdp2_tvmd_vblank_out_wait();
vdp2_tvmd_vblank_in_wait();
fix16_t angle = F16(0.0f);
while (true) {
vdp2_tvmd_vblank_out_wait();
// Update
matrix_stack_mode(MATRIX_STACK_MODE_MODEL_VIEW);
matrix_stack_push(); {
matrix_stack_rotate(angle, 0);
matrix_stack_rotate(angle, 1);
matrix_stack_rotate(angle, 2);
angle = fix16_add(angle, F16(-1.0f));
model_polygon_project(teapot_vertices, teapot_indices,
teapot_normals, TEAPOT_POLYGON_CNT);
} matrix_stack_pop();
vdp1_cmdt_list_begin(1); {
int32_t idx;
for (idx = OT_PRIMITIVE_BUCKETS - 1; idx >= 0; idx--) {
/* Skip empty buckets */
if (ot_bucket_empty(idx)) {
continue;
}
#if POLYGON_SORT == 1
ot_bucket_primitive_sort(idx & (OT_PRIMITIVE_BUCKETS - 1),
OT_PRIMITIVE_BUCKET_SORT_INSERTION);
#endif
#if RENDER == 1
struct ot_primitive *otp;
TAILQ_FOREACH (otp, ot_bucket(idx), otp_entries) {
polygon.cp_color = otp->otp_color;
polygon.cp_mode.transparent_pixel = true;
polygon.cp_mode.end_code = true;
polygon.cp_vertex.a.x = otp->otp_coords[0].x;
polygon.cp_vertex.a.y = otp->otp_coords[0].y;
polygon.cp_vertex.b.x = otp->otp_coords[1].x;
polygon.cp_vertex.b.y = otp->otp_coords[1].y;
polygon.cp_vertex.c.x = otp->otp_coords[2].x;
polygon.cp_vertex.c.y = otp->otp_coords[2].y;
polygon.cp_vertex.d.x = otp->otp_coords[3].x;
polygon.cp_vertex.d.y = otp->otp_coords[3].y;
vdp1_cmdt_polygon_draw(&polygon);
}
#endif
/* Clear OT bucket */
ot_bucket_init(idx);
}
vdp1_cmdt_end();
} vdp1_cmdt_list_end(1);
vdp2_tvmd_vblank_in_wait();
// Draw
vdp1_cmdt_list_commit();
}
fix16_t
fix16_vector3_length(const fix16_vector3_t *v0)
{
return fix16_sqrt(fix16_add(fix16_add(fix16_mul(v0->x, v0->x),
fix16_mul(v0->y, v0->y)), fix16_mul(v0->z, v0->z)));
}
