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;
}
