int main(int argc, char** argv) {
printf("libfixmath test tool\n");
hiclock_init();
uintptr_t args = (1 << 8);
uintptr_t iter = (1 << 8);
uintptr_t pass = (1 << 8);
uintptr_t i;
srand(time(NULL));
hiclock_t fix_duration = 0;
hiclock_t flt_duration = 0;
fix16_t fix_error = 0;
uintptr_t k;
for(k = 0; k < pass; k++) {
fix16_t fix_args[args];
for(i = 0; i < args; i++)
fix_args[i] = (rand() ^ (rand() << 16));
fix16_t fix_result[args];
hiclock_t fix_start = hiclock();
for(i = 0; i < iter; i++) {
uintptr_t j;
for(j = 0; j < args; j++)
fix_result[j] = fix_func(fix_args[j]);
}
hiclock_t fix_end = hiclock();
float flt_args[args];
for(i = 0; i < args; i++)
flt_args[i] = fix16_to_float(fix_args[i]);
float flt_result[args];
hiclock_t flt_start = hiclock();
for(i = 0; i < iter; i++) {
uintptr_t j;
for(j = 0; j < args; j++)
flt_result[j] = flt_func(flt_args[j]);
}
hiclock_t flt_end = hiclock();
for(i = 0; i < args; i++)
fix_error += abs(fix16_from_float(flt_result[i]) - fix_result[i]);
flt_duration += (flt_end - flt_start);
fix_duration += (fix_end - fix_start);
}
printf("%16s: %08"PRIuHICLOCK" @ %"PRIu32"Hz\n", flt_func_str, flt_duration, HICLOCKS_PER_SEC);
printf("%16s: %08"PRIuHICLOCK" @ %"PRIu32"Hz\n", fix_func_str, fix_duration, HICLOCKS_PER_SEC);
printf(" Difference: %08"PRIiHICLOCK" (% 3.2f%%)\n", (flt_duration - fix_duration), ((fix_duration * 100.0) / flt_duration));
printf(" Error: %f%%\n", ((fix16_to_dbl(fix_error) * 100.0) / (args * pass)));
return EXIT_SUCCESS;
}
static void
dump_structure(const struct codebook * b, int index)
{
printf(" {\n");
printf(" %d,\n", b->k);
#ifdef MATH_Q16_16
printf(" %i,\n", fix16_from_float((log(b->m) / log(2))));
#else
printf(" %g,\n", log(b->m) / log(2));
#endif
printf(" %d,\n", b->m);
printf(" codes%d\n", index);
printf(" }");
}
static void
dump_array(const struct codebook * b, int index)
{
int limit = b->k * b->m;
int i;
printf("static const scalar codes%d[] = {\n", index);
for ( i = 0; i < limit; i++ ) {
#ifdef MATH_Q16_16
printf(" %i", fix16_from_float(b->cb[i]));
#else
printf(" %g", b->cb[i]);
#endif
if ( i < limit - 1 )
printf(",");
/* organise VQs by rows, looks prettier */
if ( ((i+1) % b->k) == 0 )
printf("\n");
}
printf("};\n");
}
const Fix16 smul(const float other) const { Fix16 ret = fix16_smul(value, fix16_from_float(other)); return ret; }
const Fix16 ssub(const float other) const { Fix16 ret = fix16_sadd(value, -fix16_from_float(other)); return ret; }
Fix16 & operator/=(const float rhs) { value = fix16_div(value, fix16_from_float(rhs)); return *this; }
Fix16 & operator-=(const float rhs) { value -= fix16_from_float(rhs); return *this; }
const int operator> (const float other) const { return (value > fix16_from_float(other)); }
Fix16(const float inValue) { value = fix16_from_float(inValue); }
// ---------------------------------------------------------------------------------------------------------------------
bool cube_tick(const FrameInput& input, FrameOutput& output, FXState& state)
{
EffectData* data = (EffectData*)state.store;
output.fade(3);
Fix16 dial2((int16_t)input.dialChange[2]);
dial2 *= fix16_from_float(1.75f);
data->rotY += dial2;
data->rotX -= dial2 * fix16_pt_five;
data->delta += Fix16((int16_t)input.dialChange[1]) * fix16_from_float(0.05f);
if (data->delta > fix16_one)
data->delta = fix16_one;
if (data->delta < fix16_zero)
data->delta = fix16_zero;
Fxp3D pj[8];
Fix16 fov(9.0f), dist;
dist = dist.cos(data->distPulse);
dist *= 0.5f;
dist += 2.2f;
for (int i=0; i<8; i++)
{
pj[i] = verts[i].eulerProject(data->rotX, data->rotY, data->rotZ, fov, dist);
}
data->rotX += Fix16(0.6f) * data->delta;
data->rotY += Fix16(1.7f) * data->delta;
data->rotZ += Fix16(0.3f) * data->delta;
data->distPulse += Fix16(0.04f) * data->delta;
for (int f=0; f<6; f++)
{
int bi = (f * 4);
draw::WuLine(output.frame,
pj[ faces[bi + 0] ].x,
pj[ faces[bi + 0] ].y,
pj[ faces[bi + 1] ].x,
pj[ faces[bi + 1] ].y,
data->ccCycle);
draw::WuLine(output.frame,
pj[ faces[bi + 1] ].x,
pj[ faces[bi + 1] ].y,
pj[ faces[bi + 2] ].x,
pj[ faces[bi + 2] ].y,
data->ccCycle);
draw::WuLine(output.frame,
pj[ faces[bi + 2] ].x,
pj[ faces[bi + 2] ].y,
pj[ faces[bi + 3] ].x,
pj[ faces[bi + 3] ].y,
data->ccCycle);
draw::WuLine(output.frame,
pj[ faces[bi + 3] ].x,
pj[ faces[bi + 3] ].y,
pj[ faces[bi + 0] ].x,
pj[ faces[bi + 0] ].y,
data->ccCycle);
}
return true;
}
/*!
* \brief Initializes the gravity Kalman filter
*/
static void kalman_gravity_init()
{
/************************************************************************/
/* initialize the filter structures */
/************************************************************************/
#if USE_UNCONTROLLED
kalman_filter_initialize_uc(&kf, KALMAN_NUM_STATES);
#else
kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS);
#endif
kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);
/************************************************************************/
/* set initial state */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *x = kalman_get_state_vector_uc(&kf);
#else
mf16 *x = kalman_get_state_vector(&kf);
#endif
x->data[0][0] = 0; // s_i
x->data[1][0] = 0; // v_i
x->data[2][0] = fix16_from_float(6); // g_i
/************************************************************************/
/* set state transition */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *A = kalman_get_state_transition_uc(&kf);
#else
mf16 *A = kalman_get_state_transition(&kf);
#endif
// set time constant
const fix16_t T = fix16_one;
const fix16_t Tsquare = fix16_sq(T);
// helper
const fix16_t fix16_half = fix16_from_float(0.5);
// transition of x to s
matrix_set(A, 0, 0, fix16_one); // 1
matrix_set(A, 0, 1, T); // T
matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
// transition of x to v
matrix_set(A, 1, 0, 0); // 0
matrix_set(A, 1, 1, fix16_one); // 1
matrix_set(A, 1, 2, T); // T
// transition of x to g
matrix_set(A, 2, 0, 0); // 0
matrix_set(A, 2, 1, 0); // 0
matrix_set(A, 2, 2, fix16_one); // 1
/************************************************************************/
/* set covariance */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *P = kalman_get_system_covariance_uc(&kf);
#else
mf16 *P = kalman_get_system_covariance(&kf);
#endif
matrix_set_symmetric(P, 0, 0, fix16_half); // var(s)
matrix_set_symmetric(P, 0, 1, 0); // cov(s,v)
matrix_set_symmetric(P, 0, 2, 0); // cov(s,g)
matrix_set_symmetric(P, 1, 1, fix16_one); // var(v)
matrix_set_symmetric(P, 1, 2, 0); // cov(v,g)
matrix_set_symmetric(P, 2, 2, fix16_one); // var(g)
/************************************************************************/
/* set system process noise */
/************************************************************************/
#if USE_UNCONTROLLED
mf16 *Q = kalman_get_system_process_noise_uc(&kf);
mf16_fill(Q, F16(0.0001));
#endif
/************************************************************************/
/* set measurement transformation */
/************************************************************************/
mf16 *H = kalman_get_observation_transformation(&kfm);
matrix_set(H, 0, 0, fix16_one); // z = 1*s
matrix_set(H, 0, 1, 0); // + 0*v
matrix_set(H, 0, 2, 0); // + 0*g
/************************************************************************/
/* set process noise */
/************************************************************************/
mf16 *R = kalman_get_observation_process_noise(&kfm);
matrix_set(R, 0, 0, fix16_half); // var(s)
}
// ---------------------------------------------------------------------------------------------------------------------
bool ripple_tick(const FrameInput& input, FrameOutput& output, FXState& state)
{
EffectData* data = (EffectData*)state.store;
output.clear();
Fix16 divFour = fix16_from_float(0.1470588f);
Fix16 edgeMult = fix16_from_float(0.75f);
if(state.rng.genUInt32(0, 100) < data->dropFrequency)
{
data->source[state.rng.genUInt32(1, EffectData::Height-2) * EffectData::Width + state.rng.genUInt32(1, EffectData::Width-2)] = fix16_one;
}
for(int x = 1; x < EffectData::Width - 1; x++)
{
for(int y = 1; y < EffectData::Height - 1; y++)
{
// last last height as proxy for velocity.
Fix16 velocity = -data->destination[y * EffectData::Width + x];
// average local neighborhood.
// todo taking diagonal neigbors x 0.7 makes stuff rounder.
Fix16 smoothed;
smoothed = data->source[y * EffectData::Width + x + 1]; // x+1, y
smoothed += data->source[y * EffectData::Width + x - 1]; // x-1, y
smoothed += data->source[(y + 1) * EffectData::Width + x]; // x, y+1
smoothed += data->source[(y - 1) * EffectData::Width + x]; // x, y-1
smoothed += data->source[(y - 1) * EffectData::Width + (x - 1)] * edgeMult;
smoothed += data->source[(y - 1) * EffectData::Width + (x + 1)] * edgeMult;
smoothed += data->source[(y + 1) * EffectData::Width + (x - 1)] * edgeMult;
smoothed += data->source[(y + 1) * EffectData::Width + (x + 1)] * edgeMult;
smoothed *= divFour;
// combine avg and velocity
Fix16 newHeight = smoothed * fix16_from_float(0.3f) + velocity;
// damp it
newHeight *= data->damping;
// store it...
data->destination[y * EffectData::Width + x] = newHeight;
// adjustments for drawing...
newHeight = (newHeight * fix16_from_float(2.5f));
// are there fix16 clamping functions someplace?
if(newHeight < fix16_zero)
newHeight = fix16_zero;
if(newHeight > fix16_one)
newHeight = fix16_one;
byte r, g;
FullSpectrum(newHeight, r, g);
setLED(output.frame, x-1, y-1, r, g, 0);
}
}
Fix16* temp = data->destination;
data->destination = data->source;
data->source = temp;
return true;
}
// ---------------------------------------------------------------------------------------------------------------------
bool tick(const FrameInput& input, FXState& state, FrameOutput& output)
{
switch (gCurrentInterfaceStage)
{
case InterfaceStage::IS_GUI:
{
output.clear();
if (input.dialChange[1] == 0)
ticksSinceAdjust ++;
else
ticksSinceAdjust = 0;
const int16_t radioLen = sizeof(radioGUI) / sizeof(gui_entry);
Fix16 max_target = fix16_from_int(radioLen);
Fix16 max_value = fix16_from_int(radioLen - 1);
Fix16 dial16 = fix16_from_int( input.dialChange[1] );
Fix16 pt05 = fix16_from_float(-1.0f);
vTargetA += dial16 * fix16_from_float(0.2f);
if (vTargetA < fix16_neg_one)
vTargetA = fix16_neg_one;
if (vTargetA > max_target)
vTargetA = max_target;
vCurA += (vTargetA - vCurA) * fix16_from_float(0.035f);
if (vCurA < fix16_zero)
vCurA = fix16_zero;
if (vCurA > max_value)
vCurA = max_value;
if (ticksSinceAdjust > 30)
{
vTargetA -= fpart(vTargetA);
}
int16_t off = ipart(vCurA).asInt();
if (off < 0)
off = 0;
if (off > radioLen - 1)
off = radioLen - 1;
const gui_entry* guient[3] = {&radioGUI[off], &radioGUI[off], &radioGUI[off]};
if (off - 1 >= 0)
guient[2] = &radioGUI[off - 1];
if (off + 1 <= radioLen - 1)
guient[1] = &radioGUI[off + 1];
Fix16 charASlide = fpart(vCurA);
charASlide *= Fix16(-16.0f);
int16_t slideAInt = charASlide.asInt();
IconGlyph(output.frame, guient[0]->gly, slideAInt, 0, true);
IconGlyph(output.frame, guient[1]->gly, 16 + slideAInt, 0, false);
if (off > 0)
IconGlyph(output.frame, guient[2]->gly, slideAInt - 16, 0, false);
if (fadeTick < 15)
{
fadeTick ++;
output.fade(15 - fadeTick);
}
if (input.dialClick)
{
gCurrentInterfaceStage = InterfaceStage::IS_FADETO;
gNextDisplayMode = guient[0]->mode;
}
}
break;
case InterfaceStage::IS_FADETO:
{
output.fade(1);
fadeTick --;
if (fadeTick <= 0)
{
vTargetA = vCurA;
gCurrentDisplayMode = gNextDisplayMode;
for(int i = 0; i < Constants::MemoryPool; ++i)
state.store[i] = 0xFF;
DisplayMode::doInitFor(gCurrentDisplayMode, state);
fadeTick = 0;
gCurrentInterfaceStage = InterfaceStage::IS_FX;
}
}
break;
case InterfaceStage::IS_FX:
{
DisplayMode::doTickFor(gCurrentDisplayMode, input, output, state);
if (fadeTick < 15)
{
fadeTick ++;
output.fade(15 - fadeTick);
}
if (input.dialClick)
{
gCurrentInterfaceStage = InterfaceStage::IS_FADEFROM;
}
}
break;
case InterfaceStage::IS_FADEFROM:
{
DisplayMode::doTickFor(gCurrentDisplayMode, input, output, state);
output.fade(15 - fadeTick);
fadeTick --;
if (fadeTick <= 0)
{
gCurrentInterfaceStage = InterfaceStage::IS_GUI;
fadeTick = 0;
}
}
break;
}
//
/*
dial --;
if (dial <= 0)
{
// clear the frame
output.clear();
for (int y=0; y<Constants::FrameHeight; y++)
{
for (int x=0; x<Constants::FrameWidth; x++)
{
int32_t RR = state.rng.genInt32(-4, 4);
if (RR<0)
RR =0;
int32_t GG = state.rng.genInt32(-4, 4);
if (GG<0)
GG =0;
setLED(output.frame, x, y, RR, GG);
}
}
dial = 6;
} */
/*
byte red, green;
for (int y=0; y<Constants::FrameHeight; y++)
{
for (int x=0; x<Constants::FrameWidth; x++)
{
pixel &LEDpixel = output.frame[y * Constants::FrameWidth + x];
DecodeByte(LEDpixel, red, green);
if (red > 0)
red --;
if (green > 0)
green --;
LEDpixel = red | (green << 4);
}
}
vTargetA += fix16_from_int( input.dialChange[1] );
vTargetB += fix16_from_int( input.dialChange[2] );
vCurA += (vTargetA - vCurA) * fix16_from_float(0.05f);
vCurB += (vTargetB - vCurB) * fix16_from_float(0.05f);
Fix16 xo = fix16_from_float(8.0f);
Fix16 yo = fix16_from_float(8.0f);
Fix16 ss1 = fix16_sin(vCurA * fix16_from_float(0.1f));
Fix16 cc1 = fix16_cos(vCurA * fix16_from_float(0.1f));
Fix16 ss2 = fix16_sin(vCurB * fix16_from_float(0.1f));
Fix16 cc2 = fix16_cos(vCurB * fix16_from_float(0.1f));
Fix16 rad1(-2.0f), rad2(12.0f);
draw::WuLine(
output.frame,
xo + (ss1 * rad1),
yo + (cc1 * rad1),
xo + (ss1 * rad2),
yo + (cc1 * rad2),
Red);
draw::WuLine(
output.frame,
xo + (ss2 * rad1),
yo + (cc2 * rad1),
xo + (ss2 * rad2),
yo + (cc2 * rad2),
Green);
*/
return true;
}