void fix16_vector3_sub(const fix16_vector3_t *v0, const fix16_vector3_t *v1, fix16_vector3_t *result) { result->x = fix16_sub(v0->x, v1->x); result->y = fix16_sub(v0->y, v1->y); result->z = fix16_sub(v0->z, v1->z); }
void fix16_vector3_cross(const fix16_vector3_t *u, const fix16_vector3_t *v, fix16_vector3_t *result) { result->x = fix16_sub(fix16_mul(u->y, v->z), fix16_mul(u->z, v->y)); result->y = fix16_sub(fix16_mul(u->z, v->x), fix16_mul(u->x, v->z)); result->z = fix16_sub(fix16_mul(u->x, v->y), fix16_mul(u->y, v->x)); }
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; } } }
// calculate modulated and bandlimited waveshape static inline void osc_calc_wm(osc* osc) { fract32 sm; // mod shape // fract32 sl; // shape limit given current freq // add modulation sm = add_fr1x32(osc->shape, mult_fr1x32x32(osc->wmIn, osc->wmAmt) ); //- hacky magic formula for pseudo-bandlimiting: //- with maximal bandlimiting, want to limit shape to a function of unit freq //- max freq = min shape, min freq = max shape // : // map phase increment to [0,1] fract32 /* sl = (fract32)(((u32)(osc->inc) - incRange) * shapeLimMul); */ /* // invert [0,1] to [1,0] */ /* sl = sub_fr1x32(FR32_MAX, sl); */ /* // limit */ /* if(sl < sm) { */ /* sm = dsp_lerp32(sm, sl, osc->bandLim); */ /* } */ // ok, time for serious bullshit! sm = sub_fr1x32(sm, mult_fr1x32x32( (fract32)(fix16_sub(osc->inc, incMin) * shapeLimMul), osc->bandLim ) ); if(sm < 0) { sm = 0; } osc->shapeMod = sm; }
void mf16_solve(mf16 *dest, const mf16 *q, const mf16 *r, const mf16 *matrix) { int row, column, variable; if (r->columns != r->rows || r->columns != q->columns || r == dest) { dest->errors |= FIXMATRIX_USEERR; return; } // Ax=b <=> QRx=b <=> Q'QRx=Q'b <=> Rx=Q'b // Q'b is calculated directly and x is then solved row-by-row. mf16_mul_at(dest, q, matrix); for (column = 0; column < dest->columns; column++) { for (row = dest->rows - 1; row >= 0; row--) { fix16_t value = dest->data[row][column]; // Subtract any already solved variables for (variable = row + 1; variable < r->columns; variable++) { fix16_t multiplier = r->data[row][variable]; fix16_t known_value = dest->data[variable][column]; fix16_t product = fix16_mul(multiplier, known_value); value = fix16_sub(value, product); if (product == fix16_overflow || value == fix16_overflow) { dest->errors |= FIXMATRIX_OVERFLOW; } } // Now value = R_ij x_i <=> x_i = value / R_ij fix16_t divider = r->data[row][row]; if (divider == 0) { dest->errors |= FIXMATRIX_SINGULAR; dest->data[row][column] = 0; continue; } fix16_t result = fix16_div(value, divider); dest->data[row][column] = result; if (result == fix16_overflow) { dest->errors |= FIXMATRIX_OVERFLOW; } } } }
// 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); } }
// Takes two columns vectors, v and u, of size n. // Performs v = v - dot(u, v) * u, // where dot(u,v) has already been computed // u is assumed to be an unit vector. static void subtract_projection(fix16_t *v, const fix16_t *u, fix16_t dot, int n, uint8_t *errors) { while (n--) { // For unit vector u, u[i] <= 1 // Therefore this multiplication cannot overflow fix16_t product = fix16_mul(dot, *u); // Overflow here is rare, but possible. fix16_t diff = fix16_sub(*v, product); if (diff == fix16_overflow) *errors |= FIXMATRIX_OVERFLOW; *v = diff; v += FIXMATRIX_MAX_SIZE; u += FIXMATRIX_MAX_SIZE; } }
void mf16_cholesky(mf16 *dest, const mf16 *matrix) { // This is the Cholesky–Banachiewicz algorithm. // Refer to http://en.wikipedia.org/wiki/Cholesky_decomposition#The_Cholesky.E2.80.93Banachiewicz_and_Cholesky.E2.80.93Crout_algorithms int row, column, k; dest->errors = matrix->errors; if (matrix->rows != matrix->columns) dest->errors |= FIXMATRIX_DIMERR; dest->rows = dest->columns = matrix->rows; for (row = 0; row < dest->rows; row++) { for (column = 0; column < dest->columns; column++) { if (row == column) { // Value on the diagonal // Ljj = sqrt(Ajj - sum(Ljk^2, k = 1..(j-1)) fix16_t value = matrix->data[row][column]; for (k = 0; k < column; k++) { fix16_t Ljk = dest->data[row][k]; Ljk = fix16_mul(Ljk, Ljk); value = fix16_sub(value, Ljk); if (value == fix16_overflow || Ljk == fix16_overflow) dest->errors |= FIXMATRIX_OVERFLOW; } if (value < 0) { if (value < -65) dest->errors |= FIXMATRIX_NEGATIVE; value = 0; } dest->data[row][column] = fix16_sqrt(value); } else if (row < column) { // Value above diagonal dest->data[row][column] = 0; } else { // Value below diagonal // Lij = 1/Ljj (Aij - sum(Lik Ljk, k = 1..(j-1))) fix16_t value = matrix->data[row][column]; for (k = 0; k < column; k++) { fix16_t Lik = dest->data[row][k]; fix16_t Ljk = dest->data[column][k]; fix16_t product = fix16_mul(Lik, Ljk); value = fix16_sub(value, product); if (value == fix16_overflow || product == fix16_overflow) dest->errors |= FIXMATRIX_OVERFLOW; } fix16_t Ljj = dest->data[column][column]; value = fix16_div(value, Ljj); dest->data[row][column] = value; if (value == fix16_overflow) dest->errors |= FIXMATRIX_OVERFLOW; } } } }
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; }