void process_squareDist(Distortion *t, fixedp *x) {
Uint32 n;
if(t->fdb != 0) {
for(n = 0; n < PROCESS_SIZE; n++) {
t->prev = qadd(qmul(x[n], t->gain), qmul(t->prev, t->fdb));
if (t->prev > 0)
t->prev = t->lvl1;
else if (t->prev < 0)
t->prev = -t->lvl2;
x[n] = t->prev;
}
}
else {
for(n = 0; n < PROCESS_SIZE; n++) {
t->prev = qmul(x[n], t->gain);
if (t->prev > 0)
t->prev = t->lvl1;
else if (t->prev < 0)
t->prev = -t->lvl2;
x[n] = t->prev;
}
}
}
void process_thunderFuzz( Distortion *t, fixedp *x ) {
Uint32 n;
// remember to keep sample from previous cycle. :) keep in structure. disttruc
if(t->fdb) {
for(n=0; n < PROCESS_SIZE; n++) {
t->prev = qadd(qmul(x[n],t->gain), qmul(t->prev, t->fdb));
if (t->prev > t->lvl1)
t->prev = t->lvl1;
else if (t->prev < -t->lvl2)
t->prev = -t->lvl2;
x[n] = qabs(t->prev);
}
}
else {
for(n=0; n < PROCESS_SIZE; n++) {
t->prev = qmul(x[n],t->gain);
if (t->prev > t->lvl1)
t->prev = t->lvl1;
else if (t->prev < -t->lvl2)
t->prev = -t->lvl2;
x[n] = qabs(t->prev);
}
}
}
fixedp getExp(fixedp base) {
Int32 pos;
if (base > EXP_POS_MAX)
return posExp[EXP_TABLE_SIZE-1];
else if (base < EXP_NEG_MAX)
return negExp[EXP_TABLE_SIZE-1];
else if (base > 0) {
// måste anpassas till fixt tal så rätt element i tabellen anropas
pos = q2int(qmul(base, EXP_POS_POS_MUL));
if (pos < 0) {
return posExp[0];
}
return posExp[pos];
}
else if (base < 0) {
// samma här, VIKTIGT: LÄGG TILL AVRUNDNING, DERO HERO
pos = q2int(qmul(qabs(base), EXP_NEG_POS_MUL));
if(pos < 0) {
return negExp[0];
}
return negExp[pos];
}
return 1;
}
void qrot_vec(vec dest, quat *q, vec x) {
quat qc, qx, tmpdest;
memcpy(&qc, q, sizeof(quat));
qconj(&qc);
qx.s = 0;
memcpy(qx.v, x, sizeof(vec));
qmul(&tmpdest, q, &qx);
qmul(&qx, &tmpdest, &qc);
memcpy(dest, qx.v, sizeof(vec));
}
inline fixedp dLinTerp(fixedp x1, fixedp x2, fixedp y1, fixedp y2, fixedp x)
{
fixedp dx, result, denom;
denom = qsub(x2, x1);
if(denom == 0)
return y1; // should not ever happen
// calculate decimal position of x
dx = qdiv(qsub(x, x1), denom);
// use weighted sum method of interpolating
result = qadd( qmul( dx, y2 ), qmul( qsub( short2q(1), dx ), y1 ));
return result;
}
/*
** ドラッグ中
** マウスのドラッグ中に実行する
*/
void trackballMotion(int x, int y)
{
if (drag) {
double dx, dy, a;
/* マウスポインタの位置のドラッグ開始位置からの変位 */
dx = (x - cx) * sx;
dy = (y - cy) * sy;
//dy = 0;
/* マウスポインタの位置のドラッグ開始位置からの距離 */
a = sqrt(dx * dx + dy * dy);
if (a != 0.0) {
double ar = a * SCALE * 0.5;
double as = sin(ar) / a;
double dq[4] = { cos(ar), dy * as, dx * as, 0.0 };
/* クォータニオンを掛けて回転を合成 */
qmul(tq, dq, cq);
/* クォータニオンから回転の変換行列を求める */
qrot(rt, tq);
}
}
}
void
multiply_numbers(void)
{
double a, b;
if (isrational(stack[tos - 1]) && isrational(stack[tos - 2])) {
qmul();
return;
}
save();
p2 = pop();
p1 = pop();
if (isdouble(p1))
a = p1->u.d;
else
a = convert_rational_to_double(p1);
if (isdouble(p2))
b = p2->u.d;
else
b = convert_rational_to_double(p2);
push_double(a * b);
restore();
}
void qball(Rectangle r, Mouse *m, Quaternion *result, void (*redraw)(void), Quaternion *ap){
Quaternion q, down;
Point rad;
axis=ap;
ctlcen=divpt(addpt(r.min, r.max), 2);
rad=divpt(subpt(r.max, r.min), 2);
ctlrad=(rad.x<rad.y?rad.x:rad.y)-BORDER;
down=qinv(mouseq(m->xy));
q=*result;
for(;;){
*m=emouse();
if(!m->buttons) break;
*result=qmul(q, qmul(down, mouseq(m->xy)));
(*redraw)();
}
}
void init_3band_state(EQSTATE* es, int lowfreq, int highfreq, int mixfreq)
{
// Clear state
memset(es,0,sizeof(EQSTATE));
// Set Low/Mid/High gains to unity
es->lg = 1.0;
es->mg = 1.0;
es->hg = 1.0;
// Calculate filter cutoff frequencies
es->lf = qmul(Q2, qsin(qmul(QPI, qdiv(int2q(lowfreq), int2q(mixfreq)))));
es->hf = qmul(Q2, qsin(qmul(QPI, qdiv(int2q(highfreq), int2q(mixfreq)))));
}
/*
* Multiply a complex number by a real number.
*/
COMPLEX *
cmulq(COMPLEX *c, NUMBER *q)
{
COMPLEX *r;
if (qiszero(q))
return clink(&_czero_);
if (qisone(q))
return clink(c);
if (qisnegone(q))
return cneg(c);
r = comalloc();
qfree(r->real);
qfree(r->imag);
r->real = qmul(c->real, q);
r->imag = qmul(c->imag, q);
return r;
}
/*
* Multiply two complex numbers.
* This saves one multiplication over the obvious algorithm by
* trading it for several extra additions, as follows. Let
* q1 = (a + b) * (c + d)
* q2 = a * c
* q3 = b * d
* Then (a+bi) * (c+di) = (q2 - q3) + (q1 - q2 - q3)i.
*/
COMPLEX *
cmul(COMPLEX *c1, COMPLEX *c2)
{
COMPLEX *r;
NUMBER *q1, *q2, *q3, *q4;
if (ciszero(c1) || ciszero(c2))
return clink(&_czero_);
if (cisone(c1))
return clink(c2);
if (cisone(c2))
return clink(c1);
if (cisreal(c2))
return cmulq(c1, c2->real);
if (cisreal(c1))
return cmulq(c2, c1->real);
/*
* Need to do the full calculation.
*/
r = comalloc();
q2 = qqadd(c1->real, c1->imag);
q3 = qqadd(c2->real, c2->imag);
q1 = qmul(q2, q3);
qfree(q2);
qfree(q3);
q2 = qmul(c1->real, c2->real);
q3 = qmul(c1->imag, c2->imag);
q4 = qqadd(q2, q3);
qfree(r->real);
r->real = qsub(q2, q3);
qfree(r->imag);
r->imag = qsub(q1, q4);
qfree(q1);
qfree(q2);
qfree(q3);
qfree(q4);
return r;
}
int main( int argc, char **argv ) {
(void) argc;
(void) argv;
// init
srand((unsigned int)time(NULL)); // might break in 2038
aa_test_ulimit();
cross();
qmul();
qrot();
tfmul();
duqumul();
}
void process_isr(void)
{
int i;
// buffer switching
// input -> process
// process -> output
// output -> input
tmp = output;
output = process;
process = input;
input = output;
// DO PROCESSING
pGain = (fixedp)(*(unsigned volatile int *)GAINADRESS);
for(i = 0; i < N; i++) {
result = (short)qmul(process[i],pGain);
process[i] = result;
}
return;
}
void Trackball::motion(int x, int y)
{
if (dragged) {
double dx, dy, a;
dx = (x - cx) * sx;
dy = (y - cy) * sy;
a = sqrt(dx * dx + dy * dy);
if (a != 0.0)
{
double ar = a * SCALE * 0.5;
double as = sin(ar) / a;
double dq[4] = { cos(ar), dy * as, dx * as, 0.0 };
qmul(tq, dq, cq);
qrot(rt, tq);
}
}
}
/*
* Square a complex number.
*/
COMPLEX *
csquare(COMPLEX *c)
{
COMPLEX *r;
NUMBER *q1, *q2;
if (ciszero(c))
return clink(&_czero_);
if (cisrunit(c))
return clink(&_cone_);
if (cisiunit(c))
return clink(&_cnegone_);
r = comalloc();
if (cisreal(c)) {
qfree(r->real);
r->real = qsquare(c->real);
return r;
}
if (cisimag(c)) {
qfree(r->real);
q1 = qsquare(c->imag);
r->real = qneg(q1);
qfree(q1);
return r;
}
q1 = qsquare(c->real);
q2 = qsquare(c->imag);
qfree(r->real);
r->real = qsub(q1, q2);
qfree(q1);
qfree(q2);
qfree(r->imag);
q1 = qmul(c->real, c->imag);
r->imag = qscale(q1, 1L);
qfree(q1);
return r;
}
/*
* Add an opcode to the current function being compiled.
* Note: This can change the curfunc global variable when the
* function needs expanding.
*/
void
addop(long op)
{
register FUNC *fp; /* current function */
NUMBER *q, *q1, *q2;
unsigned long count;
BOOL cut;
int diff;
fp = curfunc;
count = fp->f_opcodecount;
cut = TRUE;
diff = 2;
q = NULL;
if ((count + 5) >= maxopcodes) {
maxopcodes += OPCODEALLOCSIZE;
fp = (FUNC *) malloc(funcsize(maxopcodes));
if (fp == NULL) {
math_error("cannot malloc function");
/*NOTREACHED*/
}
memcpy((char *) fp, (char *) curfunc,
funcsize(curfunc->f_opcodecount));
if (curfunc != functemplate)
free(curfunc);
curfunc = fp;
}
/*
* Check the current opcode against the previous opcode and try to
* slightly optimize the code depending on the various combinations.
*/
switch (op) {
case OP_GETVALUE:
switch (oldop) {
case OP_NUMBER:
case OP_ZERO:
case OP_ONE:
case OP_IMAGINARY:
case OP_GETEPSILON:
case OP_SETEPSILON:
case OP_STRING:
case OP_UNDEF:
case OP_GETCONFIG:
case OP_SETCONFIG:
return;
case OP_DUPLICATE:
diff = 1;
oldop = OP_DUPVALUE;
break;
case OP_FIADDR:
diff = 1;
oldop = OP_FIVALUE;
break;
case OP_GLOBALADDR:
diff = 1 + PTR_SIZE;
oldop = OP_GLOBALVALUE;
break;
case OP_LOCALADDR:
oldop = OP_LOCALVALUE;
break;
case OP_PARAMADDR:
oldop = OP_PARAMVALUE;
break;
case OP_ELEMADDR:
oldop = OP_ELEMVALUE;
break;
default:
cut = FALSE;
}
if (cut) {
fp->f_opcodes[count - diff] = oldop;
return;
}
break;
case OP_POP:
switch (oldop) {
case OP_ASSIGN:
fp->f_opcodes[count-1] = OP_ASSIGNPOP;
oldop = OP_ASSIGNPOP;
return;
case OP_NUMBER:
case OP_IMAGINARY:
q = constvalue(fp->f_opcodes[count-1]);
qfree(q);
break;
case OP_STRING:
sfree(findstring((long)fp->f_opcodes[count-1]));
break;
case OP_LOCALADDR:
case OP_PARAMADDR:
break;
case OP_GLOBALADDR:
diff = 1 + PTR_SIZE;
break;
case OP_UNDEF:
fp->f_opcodecount -= 1;
oldop = OP_NOP;
oldoldop = OP_NOP;
return;
default:
cut = FALSE;
}
if (cut) {
fp->f_opcodecount -= diff;
oldop = OP_NOP;
oldoldop = OP_NOP;
fprintf(stderr,
"Line %ld: unused value ignored\n",
linenumber());
return;
}
break;
case OP_NEGATE:
if (oldop == OP_NUMBER) {
q = constvalue(fp->f_opcodes[count-1]);
fp->f_opcodes[count-1] = addqconstant(qneg(q));
qfree(q);
return;
}
}
if (oldop == OP_NUMBER) {
if (oldoldop == OP_NUMBER) {
q1 = constvalue(fp->f_opcodes[count - 3]);
q2 = constvalue(fp->f_opcodes[count - 1]);
switch (op) {
case OP_DIV:
if (qiszero(q2)) {
cut = FALSE;
break;
}
q = qqdiv(q1,q2);
break;
case OP_MUL:
q = qmul(q1,q2);
break;
case OP_ADD:
q = qqadd(q1,q2);
break;
case OP_SUB:
q = qsub(q1,q2);
break;
case OP_POWER:
if (qisfrac(q2) || qisneg(q2))
cut = FALSE;
else
q = qpowi(q1,q2);
break;
default:
cut = FALSE;
}
if (cut) {
qfree(q1);
qfree(q2);
fp->f_opcodes[count - 3] = addqconstant(q);
fp->f_opcodecount -= 2;
oldoldop = OP_NOP;
return;
}
} else if (op != OP_NUMBER) {
q = constvalue(fp->f_opcodes[count - 1]);
if (op == OP_POWER) {
if (qcmpi(q, 2L) == 0) {
fp->f_opcodecount--;
fp->f_opcodes[count - 2] = OP_SQUARE;
qfree(q);
oldop = OP_SQUARE;
return;
}
if (qcmpi(q, 4L) == 0) {
fp->f_opcodes[count - 2] = OP_SQUARE;
fp->f_opcodes[count - 1] = OP_SQUARE;
qfree(q);
oldop = OP_SQUARE;
return;
}
}
if (qiszero(q)) {
qfree(q);
fp->f_opcodes[count - 2] = OP_ZERO;
fp->f_opcodecount--;
} else if (qisone(q)) {
qfree(q);
fp->f_opcodes[count - 2] = OP_ONE;
fp->f_opcodecount--;
}
}
}
/*
* No optimization possible, so store the opcode.
*/
fp->f_opcodes[fp->f_opcodecount] = op;
fp->f_opcodecount++;
oldoldop = oldop;
oldop = op;
}
// Minimal c++11 implementation of dual quaternion as style agnostic as possible.
// Assumes there is a float4 struct defined the obvious xyzw way and usual support quat functions.
// This implementation just uses 4x2 matrix where the second ([1]) column is the dual part.
// Feel free to convert to your own design/style preferences and struct names.
// The Pose class is just a position,orientation (vec3,quat) pair defined the obvious way.
// The functions dqmake/dqpose convert Pose to/from dual quat representation.
//
float4x2 dqmul(const float4x2 &a, float4x2 &b) { return{ qmul(a[0], b[0]), qmul(a[0], b[1]) + qmul(a[1], b[0]) }; }
Pose dqpose(const float4x2 &d) { return{ qmul(d[1], qconj(d[0])).xyz()*2.0f, d[0] }; }
/*
* Divide two complex numbers.
*/
COMPLEX *
cdiv(COMPLEX *c1, COMPLEX *c2)
{
COMPLEX *r;
NUMBER *q1, *q2, *q3, *den;
if (ciszero(c2)) {
math_error("Division by zero");
/*NOTREACHED*/
}
if ((c1->real == c2->real) && (c1->imag == c2->imag))
return clink(&_cone_);
r = comalloc();
if (cisreal(c1) && cisreal(c2)) {
qfree(r->real);
r->real = qqdiv(c1->real, c2->real);
return r;
}
if (cisimag(c1) && cisimag(c2)) {
qfree(r->real);
r->real = qqdiv(c1->imag, c2->imag);
return r;
}
if (cisimag(c1) && cisreal(c2)) {
qfree(r->imag);
r->imag = qqdiv(c1->imag, c2->real);
return r;
}
if (cisreal(c1) && cisimag(c2)) {
qfree(r->imag);
q1 = qqdiv(c1->real, c2->imag);
r->imag = qneg(q1);
qfree(q1);
return r;
}
if (cisreal(c2)) {
qfree(r->real);
qfree(r->imag);
r->real = qqdiv(c1->real, c2->real);
r->imag = qqdiv(c1->imag, c2->real);
return r;
}
q1 = qsquare(c2->real);
q2 = qsquare(c2->imag);
den = qqadd(q1, q2);
qfree(q1);
qfree(q2);
q1 = qmul(c1->real, c2->real);
q2 = qmul(c1->imag, c2->imag);
q3 = qqadd(q1, q2);
qfree(q1);
qfree(q2);
qfree(r->real);
r->real = qqdiv(q3, den);
qfree(q3);
q1 = qmul(c1->real, c2->imag);
q2 = qmul(c1->imag, c2->real);
q3 = qsub(q2, q1);
qfree(q1);
qfree(q2);
qfree(r->imag);
r->imag = qqdiv(q3, den);
qfree(q3);
qfree(den);
return r;
}
fixedp do_3band(EQSTATE* es, fixedp sample)
{
// Locals
fixedp l,m,h; // Low / Mid / High - Sample Values
// Filter #1 (lowpass)
es->f1p0 = qadd(es->f1p0, qmul(es->lf, qsub(sample, es->f1p0)));// + vsa; might need this but i dont think so. who knows?
es->f1p1 = qadd(es->f1p1, qmul(es->lf, qsub(es->f1p0, es->f1p1)));
es->f1p2 = qadd(es->f1p2, qmul(es->lf, qsub(es->f1p1, es->f1p2)));
es->f1p3 = qadd(es->f1p3, qmul(es->lf, qsub(es->f1p2, es->f1p3)));
l = es->f1p3;
// Filter #2 (highpass)
es->f2p0 = qadd(es->f2p0,qmul(es->hf, qsub(sample , es->f2p0))); // + vsa;
es->f2p1 = qadd(es->f2p1,qmul(es->hf, qsub(es->f2p0 , es->f2p1)));
es->f2p2 = qadd(es->f2p2,qmul(es->hf, qsub(es->f2p1 , es->f2p2)));
es->f2p3 = qadd(es->f2p3,qmul(es->hf, qsub(es->f2p2 , es->f2p3)));
h = qsub(es->sdm3, es->f2p3);
// Calculate midrange (signal - (low + high))
m = qsub(es->sdm3, qadd(h, l));
// Scale, Combine and store
l = qmul(l, es->lg);
m = qmul(m, es->mg);
h = qmul(h, es->hg);
// Shuffle history buffer
es->sdm3 = es->sdm2;
es->sdm2 = es->sdm1;
es->sdm1 = sample;
// Return result
return(qadd(l, qadd(m, h)));
}
int main(int argc, char *argv[]) try
{
std::cout << "TestDQ\n";
Pose camera = { { 0, 0, 8 }, { 0, 0, 0, 1 } };
bool showaxis = true;
float3 focuspoint(0, 0, 0);
float3 mousevec_prev;
float4 model_orientation(0, 0, 0, 1);
Pose p0 = { { -3, 0, 0 }, { 0, 0, 0, 1 } };
Pose p1 = { { 3, 0, 0 }, { 0, 0, sqrtf(0.5f),sqrtf(0.5f) } };
float dt = 0.01f, t = 0;
Pose *selected = NULL;
std::vector<float4> planes = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { -1, 0, 0, 0 }, { 0, -1, 0, 0 }, { 0, 0, -1, 0 } };
for (auto &p : planes)
p.w = -0.25f;
GLWin glwin("Dual Quaternion Pose Interpolation");
glwin.keyboardfunc = [&](int key, int, int)
{
showaxis = key == 'a' != showaxis;
};
while (glwin.WindowUp())
{
t = t + dt; // advance our global time t is in 0..1
if (t > 1.0f)
t = 0.0f;
Pose pt = dqinterp(p0,p1,t); // And here we show our dual quaterion usage
// some extras to help visualize the axis of rotation, not the best math to get the result, but oh well
float4 aq = qmul(dot(p0.orientation, p1.orientation) < 0 ? -p1.orientation : p1.orientation, qconj(p0.orientation));
float3 axis = normalize(aq.xyz()*(aq.w < 0 ? -1.0f : 1.0f)); // direction of the axis of rotation
float3 axisp = cross(axis, p1.position - p0.position) / 2.0f * sqrtf(1/dot(aq.xyz(),aq.xyz())-1); // origin projected onto the axis of rotation
// user interaction:
float3 ray = qrot(camera.orientation, normalize(glwin.MouseVector)); // for mouse selection
float3 v1 = camera.position + ray*100.0f;
if (!glwin.MouseState) // note that we figure out what is being selected only when the mouse is up
{
selected = NULL;
for (Pose *p : { &p0, &p1 })
{
if (auto h = ConvexHitCheck(planes, *p, camera.position, v1))
{
selected = p;
v1 = h.impact;
}
}
}
else // if (glwin.MouseState)
{
if (selected)
selected->orientation = qmul(VirtualTrackBall(camera.position, selected->position, qrot(camera.orientation, mousevec_prev), qrot(camera.orientation, glwin.MouseVector)), selected->orientation);
else
camera.orientation = qmul(camera.orientation, qconj(VirtualTrackBall(float3(0, 0, 1), float3(0, 0, 0), mousevec_prev, glwin.MouseVector))); // equation is non-typical we are orbiting the camera, not rotating the object
}
camera.position = focuspoint + qzdir(camera.orientation)*magnitude(camera.position - focuspoint);
camera.position -= focuspoint;
camera.position *= powf(1.1f, (float)glwin.mousewheel);
camera.position += focuspoint;
mousevec_prev = glwin.MouseVector;
// Render the scene
glPushAttrib(GL_ALL_ATTRIB_BITS);
glViewport(0, 0, glwin.Width, glwin.Height); // Set up the viewport
glClearColor(0.1f, 0.1f, 0.15f, 1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glPushMatrix(); glLoadIdentity();
gluPerspective(glwin.ViewAngle, (double)glwin.Width / glwin.Height, 0.25, 250);
glMatrixMode(GL_MODELVIEW);
glPushMatrix(); glLoadIdentity();
glMultMatrixf(camera.Inverse().Matrix());
glDisable(GL_LIGHTING);
glAxis();
glGridxy(4.0f);
if (showaxis)
{
glPushAttrib(GL_ALL_ATTRIB_BITS);
glLineWidth(3.0f);
glBegin(GL_LINES);
glColor3f(1, 1, 1);
for (auto p : { p0.position, p1.position, pt.position ,axisp})
glVertex3fv(p - axis*0.5f), glVertex3fv(p + axis*0.5f); // note the comma
glEnd();
glPopAttrib();
glColor3f(1, 1, 0);
glBegin(GL_LINES);
glVertex3fv(axisp + axis*dot(axis, p0.position)), glVertex3fv(axisp + axis*dot(axis, p1.position));
glVertex3fv(axisp + axis*dot(axis, p0.position)), glVertex3fv(p0.position);
glVertex3fv(axisp + axis*dot(axis, p1.position)), glVertex3fv(p1.position);
glVertex3fv(axisp + axis*dot(axis, pt.position)), glVertex3fv(pt.position);
glEnd();
}
glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_COLOR_MATERIAL);
for (auto p : { p0, p1, pt })
glcolorbox(0.25f, p);
glPopMatrix(); //should be currently in modelview mode
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopAttrib();// Restore state
glwin.PrintString({ 0, 0 }, "ESC to quit.");
glwin.PrintString({ 0, 1 }, "'a' show axis (%s)", showaxis ? "on" : "off");
glwin.PrintString({ 0, 2 }, "%selected: %s", glwin.MouseState?"S":"s", (selected) ? ((selected==&p0)?"box0":"box1") : "none");
glwin.SwapBuffers();
}
std::cout << "\n";
return 0;
}
catch (std::exception e)
{
std::cerr << e.what() << "\n";
}
void process_overdrive(Distortion *t, fixedp *x) {
Uint32 n;
fixedp denom, tmp, a, b, numerator, denom1, denom2;
// kom ihåg förra processens sampel?
/*if(t->fdb) {
for(n = 0; n < PROCESS_SIZE; n++) {
t->prev = qadd( qmul( x[n],t->gain ), qmul( qmul(t->prev, t->gain), t->fdb ) );
// 6554 = 0.2
numeratorLvl1 = qmul(t->lvl1, qexp(qmul(t->prev, qadd(Q1, qmul(6554, qsub(Q1, t->lvl1))))));
numeratorLvl2 = qmul(t->lvl2, qexp(qmul(-t->prev, qadd(Q1, qmul(6554, qsub(Q1, t->lvl2))))));
denom = qadd(qexp(t->prev), qexp(-t->prev));
x[n] = qdiv(qsub(numeratorLvl1, numeratorLvl2), denom);
}
}
else {*/
for(n = 0; n < PROCESS_SIZE; n++) {
t->prev = qmul(x[n],t->gain);
a = qadd(Q1,qmul(3254,qsub(Q1,t->lvl1)));
b = qadd(Q1,qmul(3254,qsub(Q1,t->lvl2)));
tmp = qmul(-t->prev, qadd(a,b));
if(tmp > short2q(32)) tmp = short2q(32);
if(tmp < short2q(-32)) tmp = short2q(-32);
tmp = qexp(tmp);
numerator = qsub(t->lvl1, qmul(t->lvl2, tmp));
tmp = qmul(t->prev, qsub(Q1, a));
if(tmp > short2q(32)) tmp = short2q(32);
if(tmp < short2q(-32)) tmp = short2q(-32);
denom1 = qexp(tmp);
tmp = qmul(-t->prev, qadd(Q1, a));
if(tmp > short2q(32)) tmp = short2q(32);
if(tmp < short2q(-32)) tmp = short2q(-32);
denom2 = qexp(tmp);
denom = qadd(denom1, denom2);
tmp = qdiv(numerator, denom);
if (tmp > AUDIOMAX) {
// nu är tmp större än 32767
tmp = AUDIOMAX;
} else if (tmp < AUDIOMIN) {
tmp = AUDIOMIN;
}
x[n] = tmp;
}
//}
}
void phaseVocoder_process(PhaseVocoder *t, fixedp *x) {
// Läs från buffern
Int32 n, q;
Int32 rpi, start;
start = segmentIndex * HOP_SIZE;
for(n=start; n < HOP_SIZE; n++) {
INPUT_BUFFER_N[start++] = x[n];
if(start >= 1024) start = 0;
}
if(++segmentIndex >= 4) segmentIndex = 0;
start = segmentIndex * HOP_SIZE;
for(n=0; n < FRAME_SIZE; n++) {
frank[n].r = qmul(INPUT_BUFFER_N[start], HANN[n]);
frank[n].i = 0;
start += 1;
if(start >= 1024) start = 0;
}
// input buffern är full,
process_Segment(t, frank);
start = segmentIndex * HOP_SIZE;
for(n = 0; n < 256; n++) {
x[n] = frank[start+n].r;
if(start >= 1024) start = 0;
}
/*
for(n = 0; n < FRAME_SIZE; n++) {
q = t->hopOutIndex+n;
if( q > BUFFER_SIZE) {
q -= BUFFER_SIZE;
}
if ( n < FRAME_SIZE-(t->hopSizeOut)) {
OUTPUT_BUFFER_N[q] = qadd(OUTPUT_BUFFER_N[q], qmul(outputFrame[n].r, HANN[n]));
}
else {
OUTPUT_BUFFER_N[q] = qmul(outputFrame[n].r, HANN[n]); //, 31, FIXED_FRACBITS, FIXED_FRACBITS);
}
}
t->hopOutIndex = t->hopOutIndex + t->hopSizeOut;
if (t->hopOutIndex >= BUFFER_SIZE) t->hopOutIndex -= BUFFER_SIZE;
rpi = qipart(t->rp);
for(n=0;n<256;n++) {
x[n] = OUTPUT_BUFFER_N[rpi];
t->rp += t->pShift;
rpi = qipart(t->rp);
if(rpi >= 1024) {
t->rp -= int2q(1024);
}
}*/
return;
}
inline this_type XM_CALLCONV operator * (const this_type rhs) const
{
this_type ret;
ret.v = qmul(this->v, rhs.v);
return ret;
}
main()
{
char s[80];
double fabs(), floor();
#if EXPSCALE || EXPSC2
double exp();
#endif
double sqrt(); /* required to compute rms error */
int i, j, k;
long m, n;
dprec(); /* set up floating point coprocessor */
merror = 0;
/*aiconf = -1;*/ /* configure Airy function */
x = 1.0;
z = x * x;
qclear( qmax );
qtoasc( qmax, strmax, 4 );
qclear( qrmsa );
qclear( qave );
#if 1
printf(" Start at random number #:" );
gets( s );
sscanf( s, "%ld", &n );
printf("%ld\n", n );
#else
n = 0;
#endif
for( m=0; m<n; m++ )
drand( &x );
n = 0;
m = 0;
x = floor( x );
loop:
for( i=0; i<100; i++ )
{
n++;
m++;
/* make random number in desired range */
drand( &x );
x = WIDTH * ( x - 1.0 ) + LOW;
#if EXPSCALE
x = exp(x);
drand( &a );
a = 1.0e-13 * x * a;
if( x > 0.0 )
x -= a;
else
x += a;
#endif
#if ONEINT
k = x;
x = k;
#endif
etoq( &x, q1 ); /* double number to q type */
/* do again if second argument required */
#if TWOARG || THREEARG || FOURARG
drand( &a );
a = WIDTHA * ( a - 1.0 ) + LOWA;
/*a /= 50.0;*/
#if EXPSC2
a = exp(a);
drand( &y2 );
y2 = 1.0e-13 * y2 * a;
if( a > 0.0 )
a -= y2;
else
a += y2;
#endif
#if TWOINT || THREEINT
k = a + 0.25;
a = k;
#endif
etoq( &a, qy4 );
#endif
#if THREEARG || FOURARG
drand( &b );
#if PROB
/*
b = b - 1.0;
b = a * b;
*/
b = WIDTHA * ( b - 1.0 ) + LOWA;
/* Half-integer a and b */
/*a = 0.5*floor(2.0*a+1.0);*/
a = 0.5;
b = 0.5*floor(2.0*b+1.0);
etoq( &a, qy4 );
/*x = (a / (a+b));*/
#else
b = WIDTHA * ( b - 1.0 ) + LOWA;
#endif
#if THREEINT
j = b + 0.25;
b = j;
#endif
etoq( &b, qb );
#endif
#if FOURARG
drand( &c );
c = WIDTHA * ( c - 1.0 ) + LOWA;
/* for hyp2f1 to ensure c-a-b > -1 */
/*
z = c-a-b;
if( z < -1.0 )
c -= 1.6 * z;
*/
etoq( &c, qc );
#endif
/*printf("%.16E %.16E\n", a, x);*/
/* compute function under test */
#if ONEARG
#if FOURANS
/*FUNC( x, &z, &y2, &y3, &y4 );*/
FUNC( x, &y4, &y2, &y3, &z );
#else
#if TWOANS
FUNC( x, &z, &y2 );
/*FUNC( x, &y2, &z );*/
#else
#if ONEINT
z = FUNC( k );
#else
z = FUNC( x );
#endif
#endif
#endif
#endif
#if TWOARG
#if TWOINT
/*z = FUNC( k, x );*/
/*z = FUNC( x, k );*/
z = FUNC( a, x );
#else
#if FOURANS
FUNC( a, x, &z, &y2, &y3, &y4 );
#else
z = FUNC( a, x );
#endif
#endif
#endif
#if THREEARG
#if THREEINT
z = FUNC( j, k, x );
#else
z = FUNC( a, b, x );
#endif
#endif
#if FOURARG
z = FUNC( a, b, c, x );
#endif
etoq( &z, q2 );
/* handle detected overflow */
if( (z == MAXNUM) || (z == -MAXNUM) )
{
printf("detected overflow ");
#if FOURARG
printf("%.4E %.4E %.4E %.4E %.4E %6ld \n",
a, b, c, x, y, n);
#else
printf("%.16E %.4E %.4E %6ld \n", x, a, z, n);
#endif
e = 0.0;
m -= 1;
goto endlup;
}
/* Skip high precision if underflow. */
if( merror == UNDERFLOW )
goto underf;
/* compute high precision function */
#if ONEARG
#if FOURANS
/*QFUNC( q1, qz, qy2, qy3, qy4 );*/
QFUNC( q1, qy4, qy2, qy3, qz );
#else
#if TWOANS
QFUNC( q1, qz, qy2 );
/*QFUNC( q1, qy2, qz );*/
#else
/*qclear( qy4 );*/
/*qmov( qone, qy4 );*/
/*QFUNC( qy4, q1, qz );*/
/*QFUNC( 1, q1, qz );*/
QFUNC( q1, qz ); /* normal */
#endif
#endif
#endif
#if TWOARG
#if TWOINT
/*QFUNC( k, q1, qz );*/
/*QFUNC( q1, qy4, qz );*/
QFUNC( qy4, q1, qz );
#else
#if FOURANS
QFUNC( qy4, q1, qz, qy2, qy3, qc );
#else
/*qclear( qy4 );*/
/*qmov( qone, qy4 );*/
QFUNC( qy4, q1, qz );
#endif
#endif
#endif
#if THREEARG
#if THREEINT
QFUNC( j, k, q1, qz );
#else
QFUNC( qy4, qb, q1, qz );
#endif
#endif
#if FOURARG
QFUNC( qy4, qb, qc, q1, qz );
#endif
qtoe( qz, &y ); /* correct answer, in double precision */
/* get absolute error, in extended precision */
qsub( qz, q2, qe );
qtoe( qe, &e ); /* the error in double precision */
/* handle function result equal to zero
or underflowed. */
if( qz[1] < 3 || merror == UNDERFLOW || fabs(z) < underthresh )
{
underf:
merror = 0;
/* Don't bother to print anything. */
#if 0
printf("ans 0 ");
#if ONEARG
printf("%.8E %.8E %.4E %6ld \n", x, y, e, n);
#endif
#if TWOARG
#if TWOINT
printf("%d %.8E %.8E %.4E %6ld \n", k, x, y, e, n);
#else
printf("%.6E %.6E %.6E %.4E %6ld \n", a, x, y, e, n);
#endif
#endif
#if THREEARG
printf("%.6E %.6E %.6E %.6E %.4E %6ld \n", a, b, x, y, e, n);
#endif
#if FOURARG
printf("%.4E %.4E %.4E %.4E %.4E %.4E %6ld \n",
a, b, c, x, y, e, n);
#endif
#endif /* 0 */
qclear( qe );
e = 0.0;
m -= 1;
goto endlup;
}
else
/* relative error */
/* comment out the following two lines if absolute accuracy report */
#if RELERR
qdiv( qz, qe, qe );
#else
{
qmov( qz, q2 );
q2[0] = 0;
if( qcmp( q2, qone ) > 0 )
qdiv( qz, qe, qe );
}
#endif
qadd( qave, qe, qave );
/* absolute value of error */
qe[0] = 0;
/* peak detect the error */
if( qcmp(qe, qmax) > 0 )
{
qmov( qe, qmax );
qtoasc( qmax, strmax, 4 );
#if ONEARG
printf("%.8E %.8E %s %6ld \n", x, y, strmax, n);
#endif
#if TWOARG
#if TWOINT
printf("%d %.8E %.8E %s %6ld \n", k, x, y, strmax, n);
#else
printf("%.6E %.6E %.6E %s %6ld \n", a, x, y, strmax, n);
#endif
#endif
#if THREEARG
printf("%.6E %.6E %.6E %.6E %s %6ld \n", a, b, x, y, strmax, n);
#endif
#if FOURARG
printf("%.4E %.4E %.4E %.4E %.4E %s %6ld \n",
a, b, c, x, y, strmax, n);
#endif
}
/* accumulate rms error */
/* rmsa += e * e; accumulate the square of the error */
qmul( qe, qe, q2 );
qadd( q2, qrmsa, qrmsa );
endlup:
;
}
/* report every 100 trials */
/* rms = sqrt( rmsa/m ); */
ltoq( &m, q1 );
qdiv( q1, qrmsa, q2 );
qsqrt( q2, q2 );
qtoasc( q2, strrms, 4 );
qdiv( q1, qave, q2 );
qtoasc( q2, strave, 4 );
/*
printf("%6ld max = %s rms = %s ave = %s \n", m, strmax, strrms, strave );
*/
printf("%6ld max = %s rms = %s ave = %s \r", m, strmax, strrms, strave );
fflush(stdout);
goto loop;
}
BiQuad* BiQuad_create() {
BiQuad* filter = (BiQuad*)malloc(sizeof(BiQuad));
return filter;
}
void BiQuad_FlushDelays(BiQuad* filter) {
filter->xz1 = short2q(0);
filter->xz2 = short2q(0);
filter->yz1 = short2q(0);
filter->yz2 = short2q(0);
}
// Do the filter: given input xn, calculate output yn and return it
fixedp BiQuad_do(BiQuad* this, fixedp xn) {
// just do the difference equation: y(n) = a0x(n) + a1x(n-1) + a2x(n-2) - b1y(n-1) - b2y(n-2)
fixedp a0 = qmul(this->a0, xn);
fixedp a1 = qmul(this->a1, this->xz1);
fixedp yn = qadd(a0, a1);
a1 = qmul(this->a2, this->xz2);
yn = qadd(yn, a1);
a1 = qmul(this->b1, this->yz1);
yn = qsub(yn, a1);
a1 = qmul(this->b2, this->yz2);
yn = qsub(yn, a1);
//float yn = this->a0*xn + this->a1*this->xz1 + this->a2*this->xz2 - this->b1*this->yz1 - this->b2*this->yz2;
// underflow check
if(yn > 0.0 && yn < FLT_MIN_PLUS) yn = 0;
if(yn < 0.0 && yn > FLT_MIN_MINUS) yn = 0;
