int gl_V3_Norm(V3 *a)
{
GLfloat n;
n=sllsqrt(slladd(slladd(sllmul(a->X,a->X), sllmul(a->Y,a->Y)), sllmul(a->Z,a->Z)));
if (sllvalue(n)==sllvalue(int2sll(0))) return 1;
a->X=slldiv(a->X, n);
a->Y=slldiv(a->Y, n);
a->Z=slldiv(a->Z, n);
return 0;
}
/* inverse of a 3x3 matrix */
void gl_M3_Inv(M3 *a,M3 *m)
{
GLfloat det;
det =
sllsub(
slladd(
slladd(
sllsub(
sllsub(sllmul(sllmul(m->m[0][0], m->m[1][1]), m->m[2][2]),
sllmul(sllmul(m->m[0][0], m->m[1][2]), m->m[2][1])),
sllmul(sllmul(m->m[1][0], m->m[0][1]), m->m[2][2])),
sllmul(sllmul(m->m[1][0], m->m[0][2]), m->m[2][1])),
sllmul(sllmul(m->m[2][0], m->m[0][1]), m->m[1][2])),
sllmul(sllmul(m->m[2][0], m->m[0][2]), m->m[1][1]));
a->m[0][0] = slldiv(sllsub(sllmul(m->m[1][1],m->m[2][2]), sllmul(m->m[1][2],m->m[2][1])), det);
a->m[0][1] = sllneg(slldiv(sllsub(sllmul(m->m[0][1],m->m[2][2]), sllmul(m->m[0][2],m->m[2][1])), det));
a->m[0][2] = sllneg(slldiv(slladd(sllmul(sllneg(m->m[0][1]),m->m[1][2]), sllmul(m->m[0][2], m->m[1][1])), det));
a->m[1][0] = sllneg(slldiv(sllsub(sllmul(m->m[1][0],m->m[2][2]), sllmul(m->m[1][2],m->m[2][0])), det));
a->m[1][1] = slldiv(sllsub(sllmul(m->m[0][0],m->m[2][2]), sllmul(m->m[0][2],m->m[2][0])), det);
a->m[1][2] = sllneg(slldiv(sllsub(sllmul(m->m[0][0],m->m[1][2]), sllmul(m->m[0][2],m->m[1][0])), det));
a->m[2][0] = slldiv(sllsub(sllmul(m->m[1][0],m->m[2][1]), sllmul(m->m[1][1],m->m[2][0])), det);
a->m[2][1] = sllneg(slldiv(sllsub(sllmul(m->m[0][0],m->m[2][1]), sllmul(m->m[0][1],m->m[2][0])), det));
a->m[2][2] = slldiv(sllsub(sllmul(m->m[0][0],m->m[1][1]), sllmul(m->m[0][1],m->m[1][0])), det);
}
void Firework_Initialize(int width, int height) {
Firework_GRAVITY = slldiv(CONST_8, int2sll(100));
Firework_baselineYSpeed = slldiv(int2sll(-25), CONST_10);
Firework_maxYSpeed = CONST_3; // This gets negated later
Firework_xDampen = slldiv(int2sll(99), int2sll(100));
Firework_aDampen = slldiv(CONST_1, int2sll(100));
screen_width = width;
screen_height = height;
for (int i = 0; i < FIREWORKS; i++) {
Firework_Start(&prv_fireworks[i]);
}
}
// http://math.stackexchange.com/questions/1098487/atan2-faster-approximation
// atan2(y,x)
// a := min (|x|, |y|) / max (|x|, |y|)
// s := a * a
// r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
// if |y| > |x| then r := 1.57079637 - r
// if x < 0 then r := 3.14159274 - r
// if y < 0 then r := -r
sll sllatan2(sll y, sll x) {
sll abs_x = sllabs(x);
sll abs_y = sllabs(y);
sll maxyx = MAX(abs_x, abs_y);
sll minyx = MIN(abs_x, abs_y);
sll a = slldiv(minyx, maxyx);
sll s = sllmul(a, a);
sll r_1 = sllmul(dbl2sll((-0.0464964749)), s);
sll r_2 = slladd(r_1, dbl2sll(0.15931422));
sll r_3 = sllsub(sllmul(r_2, s), dbl2sll(0.327622764));
sll r = slladd(sllmul(sllmul(r_3, s), a), a);
if(sllabs(y) > sllabs(x)) {
r = sllsub(CONST_PI_2, r);
}
if(x < CONST_0) {
r = sllsub(CONST_PI, r);
}
if(y < CONST_0) {
r = sllneg(r);
}
return r;
}
int Matrix_Inv(GLfloat *r,GLfloat *m,int n)
{
int i,j,k,l;
GLfloat max,tmp,t;
/* identitée dans r */
for(i=0;i<n*n;i++) r[i]=int2sll(0);
for(i=0;i<n;i++) r[i*n+i]=int2sll(1);
for(j=0;j<n;j++) {
/* recherche du nombre de plus grand module sur la colonne j */
max=m[j*n+j];
k=j;
for(i=j+1;i<n;i++)
if (sllvalue(sll_abs(m[i*n+j]))>sllvalue(sll_abs(max))) {
k=i;
max=m[i*n+j];
}
/* non intersible matrix */
if (sllvalue(max)==sllvalue(int2sll(0))) return 1;
/* permutation des lignes j et k */
if (k!=j) {
for(i=0;i<n;i++) {
tmp=m[j*n+i];
m[j*n+i]=m[k*n+i];
m[k*n+i]=tmp;
tmp=r[j*n+i];
r[j*n+i]=r[k*n+i];
r[k*n+i]=tmp;
}
}
/* multiplication de la ligne j par 1/max */
max=slldiv(int2sll(1), max);
for(i=0;i<n;i++) {
tmp=m[j*n+i];
m[j*n+i]=sllmul(tmp, max);
tmp=r[j*n+i];
r[j*n+i]=sllmul(tmp, max);
}
for(l=0;l<n;l++) if (l!=j) {
t=m[l*n+j];
for(i=0;i<n;i++) {
tmp=m[l*n+i];
m[l*n+i]=sllsub(tmp, sllmul(m[j*n+i],t));
tmp=r[l*n+i];
r[l*n+i]=sllsub(tmp, sllmul(r[j*n+i],t));
}
}
}
return 0;
}
void glopFrustum(GLContext *c,GLParam *p)
{
GLfloat *r;
M4 m;
GLfloat left=p[1].f;
GLfloat right=p[2].f;
GLfloat bottom=p[3].f;
GLfloat top=p[4].f;
GLfloat near=p[5].f;
GLfloat farp=p[6].f;
GLfloat x,y,A,B,C,D,tmp2=int2sll(2);
x = slldiv(sllmul(tmp2,near), sllsub(right,left));
y = slldiv(sllmul(tmp2,near), sllsub(top,bottom));
A = slldiv(slladd(right,left), sllsub(right,left));
B = slldiv(slladd(top,bottom), sllsub(top,bottom));
C = slldiv(sllneg(slladd(farp,near)), sllsub(farp,near));
D = slldiv(sllneg(sllmul(sllmul(tmp2,farp),near)), sllsub(farp,near));
r=&m.m[0][0];
r[0]= x; r[1]=int2sll(0); r[2]=A; r[3]=int2sll(0);
r[4]= int2sll(0); r[5]=y; r[6]=B; r[7]=int2sll(0);
r[8]= int2sll(0); r[9]=int2sll(0); r[10]=C; r[11]=D;
r[12]=int2sll(0); r[13]=int2sll(0); r[14]=int2sll(-1); r[15]=int2sll(0);
gl_M4_MulLeft(c->matrix_stack_ptr[c->matrix_mode],&m);
gl_matrix_update(c);
}
void
eval_outer_loop (struct rt_tmbench_context *ctx)
{
long curr_max_ns = tsc2ns (ctx->curr.max);
long curr_min_ns = tsc2ns (ctx->curr.min);
long curr_avg_ns = tsc2ns (ctx->curr.avg);
if (!ctx->warmup)
{
if (ctx->histogram_size)
{
add_histogram (ctx, ctx->histogram_max, curr_max_ns);
add_histogram (ctx, ctx->histogram_min, curr_min_ns);
}
ctx->result.last.min = curr_min_ns;
if (curr_min_ns < ctx->result.overall.min)
ctx->result.overall.min = curr_min_ns;
ctx->result.last.max = curr_max_ns;
if (curr_max_ns > ctx->result.overall.max)
ctx->result.overall.max = curr_max_ns;
ctx->result.last.avg = slldiv (curr_avg_ns, ctx->samples_per_sec);
ctx->result.overall.avg += ctx->result.last.avg;
ctx->result.overall.overruns += ctx->curr.overruns;
wake_up_interruptible (ctx->result_event);
}
if (ctx->warmup && (ctx->result.overall.test_loops == ctx->warmup_loops))
{
ctx->result.overall.test_loops = 0;
ctx->warmup = 0;
}
ctx->curr.min = 10000000;
ctx->curr.max = -10000000;
ctx->curr.avg = 0;
ctx->curr.overruns = 0;
ctx->result.overall.test_loops++;
ctx->done = 1;
}
static void eval_outer_loop(struct rt_tmbench_context *ctx)
{
if (!ctx->warmup) {
if (ctx->histogram_size) {
add_histogram(ctx, ctx->histogram_max, ctx->curr.max);
add_histogram(ctx, ctx->histogram_min, ctx->curr.min);
}
ctx->result.last.min = ctx->curr.min;
if (ctx->curr.min < ctx->result.overall.min)
ctx->result.overall.min = ctx->curr.min;
ctx->result.last.max = ctx->curr.max;
if (ctx->curr.max > ctx->result.overall.max)
ctx->result.overall.max = ctx->curr.max;
ctx->result.last.avg =
slldiv(ctx->curr.avg, ctx->samples_per_sec);
ctx->result.overall.avg += ctx->result.last.avg;
ctx->result.overall.overruns += ctx->curr.overruns;
rtdm_event_pulse(&ctx->result_event);
}
if (ctx->warmup &&
(ctx->result.overall.test_loops == ctx->warmup_loops)) {
ctx->result.overall.test_loops = 0;
ctx->warmup = 0;
}
ctx->curr.min = 10000000;
ctx->curr.max = -10000000;
ctx->curr.avg = 0;
ctx->curr.overruns = 0;
ctx->result.overall.test_loops++;
}
int
tb_ioctl (struct inode *inode, struct file *filp, uint cmd, unsigned long arg)
{
struct rt_tmbench_context *ctx;
int ret = 0;
volatile unsigned long long tsc = 0;
ctx = (struct rt_tmbench_context *) filp->private_data;
switch (cmd)
{
case RTTST_RTIOC_TMBENCH_START:
{
struct rttst_tmbench_config config_buf;
struct rttst_tmbench_config *config;
copy_from_user (&config_buf, (void *) arg,
sizeof (struct rttst_tmbench_config));
config = &config_buf;
down (&ctx->nrt_mutex);
ctx->period = ns2tsc (config->period);
ctx->warmup_loops = config->warmup_loops;
ctx->samples_per_sec = 1000000000 / (long) config->period;
ctx->histogram_size = config->histogram_size;
ctx->freeze_max = config->freeze_max;
if (ctx->histogram_size > 0)
{
ctx->histogram_min =
kmalloc (3 * ctx->histogram_size * sizeof (long), GFP_KERNEL);
ctx->histogram_max = ctx->histogram_min + config->histogram_size;
ctx->histogram_avg = ctx->histogram_max + config->histogram_size;
if (!ctx->histogram_min)
{
up (&ctx->nrt_mutex);
return -ENOMEM;
}
memset (ctx->histogram_min, 0,
3 * ctx->histogram_size * sizeof (long));
ctx->bucketsize = config->histogram_bucketsize;
}
ctx->result.overall.min = 10000000;
ctx->result.overall.max = -10000000;
ctx->result.overall.avg = 0;
ctx->result.overall.test_loops = 1;
ctx->result.overall.overruns = 0;
ctx->warmup = 1;
ctx->curr.min = 10000000;
ctx->curr.max = -10000000;
ctx->curr.avg = 0;
ctx->curr.overruns = 0;
//ctx->result_event = &tb_wq;
ctx->curr.test_loops = 0;
ctx->mode = RTTST_TMBENCH_HANDLER;
read_tsc (tsc);
ctx->start_time = tsc + 1000000;
ctx->date = ctx->start_time + ctx->period;
tb_timer_init (timer_proc, &tb_ctx);
read_tsc (tsc);
tb_timer_start ((long) (ctx->date - tsc));
up (&ctx->nrt_mutex);
break;
}
case RTTST_RTIOC_TMBENCH_STOP:
{
struct rttst_overall_bench_res *usr_res;
usr_res = (struct rttst_overall_bench_res *) arg;
down (&ctx->nrt_mutex);
if (ctx->mode < 0)
{
up (&ctx->nrt_mutex);
return -EINVAL;
}
tb_timer_stop ();
ctx->mode = -1;
ctx->result.overall.avg =
slldiv (ctx->result.overall.avg,
(((ctx->result.overall.test_loops) > 1 ?
ctx->result.overall.test_loops : 2) - 1));
copy_to_user (&usr_res->result,
&ctx->result.overall, sizeof (struct rttst_bench_res));
if (ctx->histogram_size)
{
int size = ctx->histogram_size * sizeof (long);
copy_to_user (usr_res->histogram_min, ctx->histogram_min, size);
copy_to_user (usr_res->histogram_max, ctx->histogram_max, size);
copy_to_user (usr_res->histogram_avg, ctx->histogram_avg, size);
kfree (ctx->histogram_min);
}
up (&ctx->nrt_mutex);
free_irq (IRQ_WATCH, &tb_ctx);
break;
}
case RTTST_RTIOC_INTERM_BENCH_RES:
{
struct rttst_interm_bench_res *usr_res;
usr_res = (struct rttst_interm_bench_res *) arg;
ret = wait_event_interruptible (*(ctx->result_event), ctx->done != 0);
if (ret < 0)
return ret;
ctx->done = 0;
copy_to_user (usr_res, &ctx->result,
sizeof (struct rttst_interm_bench_res));
break;
}
case RTTST_GETCCLK:
{
copy_to_user ((void *) arg, &tb_cclk, sizeof (tb_cclk));
break;
}
case RTTST_TMR_START:
{
struct timer_info t_info;
copy_from_user (&t_info, (void *) arg, sizeof (t_info));
ctx->period = t_info.period_tsc;
ctx->start_time = t_info.start_tsc;
ctx->date = ctx->start_time + ctx->period;
tb_timer_init (user_timer_proc, &tb_ctx);
read_tsc (tsc);
tb_timer_start ((long) (ctx->date - tsc));
break;
}
case RTTST_TMR_WAIT:
{
ctx->curr.overruns = 0;
ret = wait_event_interruptible (*(ctx->result_event), ctx->done != 0);
if (ret < 0)
return ret;
ctx->date += ctx->period;
read_tsc (ctx->start_time);
//printk("KERNEL: wake up - tsc: %lld, overrun: %ld\n",
//ctx->start_time, ctx->curr.overruns);
if (ctx->date <= ctx->start_time)
{
while (ctx->date <= ctx->start_time)
{
/* set next release point */
ctx->curr.overruns++;
ctx->date += ctx->period;
}
ret = -ETIMEDOUT;
}
ctx->done = 0;
tb_timer_start ((long) (ctx->date - ctx->start_time));
copy_to_user ((void *) arg, &(ctx->curr.overruns),
sizeof (ctx->curr.overruns));
break;
}
case RTTST_TMR_STOP:
{
tb_timer_stop ();
free_irq (IRQ_WATCH, &tb_ctx);
break;
}
default:
printk ("%s: bad ioctl code (0x%x)\n", __FUNCTION__, cmd);
ret = -ENOTTY;
}
return ret;
}
sll rand_sll(sll max_val) {
sll max_scale = int2sll(1000);
int max_val_int = sll2int(sllmul(max_val, max_scale));
return slldiv(int2sll(rand() % (max_val_int + 1)), max_scale);
}
sll slldeg2rad(sll deg) {
return (slldiv(sllmul(deg,CONST_PI), int2sll(180)));
}
void glopRotate(GLContext *c,GLParam *p)
{
#define SLL_M_PI dbl2sll(M_PI)
M4 m;
GLfloat u[3];
GLfloat angle;
int dir_code;
angle = slldiv(sllmul(p[1].f, SLL_M_PI), int2sll(180));
u[0]=p[2].f;
u[1]=p[3].f;
u[2]=p[4].f;
/* simple case detection */
dir_code = ((sllvalue(u[0]) != sllvalue(int2sll(0)))<<2) | ((sllvalue(u[1]) != sllvalue(int2sll(0)))<<1) | (sllvalue(u[2]) != sllvalue(int2sll(0)));
switch(dir_code) {
case 0:
gl_M4_Id(&m);
break;
case 4:
if (sllvalue(u[0]) < sllvalue(int2sll(0))) angle=sllneg(angle);
gl_M4_Rotate(&m,angle,0);
break;
case 2:
if (sllvalue(u[1]) < sllvalue(int2sll(0))) angle=sllneg(angle);
gl_M4_Rotate(&m,angle,1);
break;
case 1:
if (sllvalue(u[2]) < sllvalue(int2sll(0))) angle=sllneg(angle);
gl_M4_Rotate(&m,angle,2);
break;
default:
{
GLfloat cost, sint;
/* normalize vector */
GLfloat len = slladd(
slladd(
sllmul(u[0],u[0]),
sllmul(u[1],u[1])
),
sllmul(u[2],u[2]));
if (sllvalue(len) == sllvalue(int2sll(0))) return;
len = slldiv(int2sll(1), sllsqrt(len));
u[0] = sllmul(u[0], len);
u[1] = sllmul(u[1], len);
u[2] = sllmul(u[2], len);
/* store cos and sin values */
cost=sllcos(angle);
sint=sllsin(angle);
/* fill in the values */
m.m[3][0]=m.m[3][1]=m.m[3][2]=
m.m[0][3]=m.m[1][3]=m.m[2][3]=int2sll(0);
m.m[3][3]=int2sll(1);
/* do the math */
m.m[0][0]=slladd(sllmul(u[0],u[0]), sllmul(cost, sllsub(int2sll(1), sllmul(u[0],u[0]))));
m.m[1][0]=sllsub(sllmul(sllmul(u[0],u[1]), sllsub(int2sll(1), cost)), sllmul(u[2],sint));
m.m[2][0]=slladd(sllmul(sllmul(u[2],u[0]), sllsub(int2sll(1), cost)), sllmul(u[1],sint));
m.m[0][1]=slladd(sllmul(sllmul(u[0],u[1]), sllsub(int2sll(1), cost)), sllmul(u[2],sint));
m.m[1][1]=slladd(sllmul(u[1],u[1]), sllmul(cost, sllsub(int2sll(1), sllmul(u[1],u[1]))));
m.m[2][1]=sllsub(sllmul(sllmul(u[1],u[2]), sllsub(int2sll(1), cost)), sllmul(u[0],sint));
m.m[0][2]=sllsub(sllmul(sllmul(u[2],u[0]), sllsub(int2sll(1), cost)), sllmul(u[1],sint));
m.m[1][2]=slladd(sllmul(sllmul(u[1],u[2]), sllsub(int2sll(1), cost)), sllmul(u[0],sint));
m.m[2][2]=slladd(sllmul(u[2],u[2]), sllmul(cost, sllsub(int2sll(1), sllmul(u[2],u[2]))));
}
}
gl_M4_MulLeft(c->matrix_stack_ptr[c->matrix_mode],&m);
gl_matrix_update(c);
}