コード例 #1
0
// Calculates the distance between two GPS lat/lon coordinates
// returns units: KM
// From: http://www.movable-type.co.uk/scripts/latlong.html
sll DistanceBetweenSLL(sll lat1, sll lon1, sll lat2, sll lon2) {  
  sll R = int2sll(6371); // kilometres
  sll lat1_rad = slldeg2rad(lat1);
  sll lon1_rad = slldeg2rad(lon1);
  sll lat2_rad = slldeg2rad(lat2);
  sll lon2_rad = slldeg2rad(lon2);
  sll dist_lat = sllsub(lat2_rad, lat1_rad);
  sll dist_lon = sllsub(lon2_rad, lon1_rad);

  dist_lat = slldiv2(dist_lat);
  dist_lon = slldiv2(dist_lon);

  dist_lat = sllsin(dist_lat);
  dist_lon = sllsin(dist_lon);

  sll a = sllmul(dist_lat, dist_lat);
  sll b = sllmul(dist_lon, dist_lon);
  sll c = sllmul(sllcos(lat1_rad), sllcos(lat2_rad));
  sll d = slladd(a, sllmul(b,c));

  sll d_sqrt = sllsqrt(d);
  sll d_sqrt_1 = sllsqrt(sllsub(CONST_1,d));

  sll result = sllmul2(sllatan2(d_sqrt, d_sqrt_1));

  return sllmul(R,result);
}
コード例 #2
0
ファイル: zmath.c プロジェクト: joshdekock/jim-psp
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;
}
コード例 #3
0
// 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;
}
コード例 #4
0
ファイル: matrix.c プロジェクト: joshdekock/jim-psp
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);
}
コード例 #5
0
ファイル: zmath.c プロジェクト: joshdekock/jim-psp
/* inversion of an orthogonal matrix of type Y=M.X+P */ 
void gl_M4_InvOrtho(M4 *a,M4 b)
{
	int i,j;
	GLfloat s;
	for(i=0;i<3;i++)
	for(j=0;j<3;j++) a->m[i][j]=b.m[j][i];
	a->m[3][0]=int2sll(0); a->m[3][1]=int2sll(0); a->m[3][2]=int2sll(0); a->m[3][3]=int2sll(1);
	for(i=0;i<3;i++) {
		s=int2sll(0);
		for(j=0;j<3;j++) s=sllsub(s, sllmul(b.m[j][i],b.m[j][3]));
		a->m[i][3]=s;
	}
}
コード例 #6
0
ファイル: fireworks.c プロジェクト: pazos/pebble_fireworks
void Firework_Explode(FireworkStruct* firework) {
  for (int loop = 0; loop < FIREWORK_PARTICLES; loop++) {
    // Dampen the horizontal speed by 1% per frame
    firework->xSpeed[loop] = sllmul(firework->xSpeed[loop], Firework_xDampen);

    // Move the particle
    firework->x[loop] = slladd(firework->x[loop], firework->xSpeed[loop]);
    firework->y[loop] = slladd(firework->y[loop], firework->ySpeed[loop]);

    // Apply gravity to the particle's speed
    firework->ySpeed[loop] = slladd(firework->ySpeed[loop], Firework_GRAVITY);
  }

  // Fade out the particles (alpha is stored per firework, not per particle)
  if (firework->alpha > CONST_0) {
    firework->alpha = sllsub(firework->alpha, Firework_aDampen);
    if (firework->alpha < CONST_0) {
      firework->alpha = CONST_0;
    }
  } else { // Once the alpha hits zero reset the firework
    Firework_Start(firework);
  }
}
コード例 #7
0
ファイル: fireworks.c プロジェクト: pazos/pebble_fireworks
void Firework_Start(FireworkStruct* firework) {
  // Pick an initial x location and  random x/y speeds
  sll xLoc = int2sll(10 + (rand() % (screen_width - 20)));
  sll yLoc = int2sll(screen_height); // start at the bottom of the screen
  sll xSpeedVal = slladd(int2sll(-2), rand_sll(CONST_4));
  sll ySpeedVal = sllsub(Firework_baselineYSpeed, rand_sll(Firework_maxYSpeed));

  // Set initial x/y location and speeds
  for (int loop = 0; loop < FIREWORK_PARTICLES; loop++) {
    firework->x[loop] = xLoc;
    firework->y[loop] = yLoc;
    firework->xSpeed[loop] = xSpeedVal;
    firework->ySpeed[loop] = ySpeedVal;
  }

  // Max out at 170 for more vibrant colors
  firework->red   = ((rand() % 4) * 85);
  firework->green = ((rand() % 4) * 85);
  firework->blue  = ((rand() % 4) * 85);
  
  // Instead of white, black or gray, do GColorFolly
  if (firework->red == firework->green  && firework->green && firework->blue) {
    firework->red = 255;
    firework->blue = 85;
  } else if (firework->red == 0 && firework->green == 0 && firework->blue == 85) {
    // Oxford blue too dark, do Cyan
    firework->green = 255;
    firework->blue =  255;
  }

  firework->alpha = CONST_1;

  firework->framesUntilLaunch = (rand() % 100);

  firework->hasExploded = false;
}
コード例 #8
0
ファイル: process.c プロジェクト: dmitrysmagin/vorton
void phase2_process(void)
{
    sll inc=dbl2sll(0.0022);

    phase2_ship_x=128; phase2_ship_y=147;
    phase2_shoot.x=0; phase2_shoot.y=0; phase2_shoot.state=100;
    phase2_bad.x=0; phase2_bad.y=300; phase2_bad.state=105;
    phase2_global_x=0; phase2_global_y=0;

    music_play_fade(MUSIC_LEVEL);
    init_frame_time();

    theend=0;
    while (!theend)
    {
	getKey();
	if (hit1_pressed)
		if (phase2_shoot.state>=25)
		{

			sll angle=sllsub(mode7_angle,sllmul(inc,int2sll((128-phase2_ship_x)<<1)));
			sll multi=int2sll((256-phase2_ship_y)/24);
			phase2_shoot.dx=sllmul(sllrotl(sllcos(angle),3),SLL_CONST_PI);
			phase2_shoot.dy=sllmul(sllrotl(sllsin(angle),3),SLL_CONST_PI);
			phase2_shoot.x=slladd(mode7_x,sllmul(phase2_shoot.dx,multi));
			phase2_shoot.y=slladd(mode7_y,sllmul(phase2_shoot.dy,multi));
			angle=sllsub(mode7_angle,sllmul(inc,int2sll((128-phase2_ship_x)<<2)));
			phase2_shoot.dx=sllmul(sllrotl(sllcos(angle),3),SLL_CONST_PI);
			phase2_shoot.dy=sllmul(sllrotl(sllsin(angle),3),SLL_CONST_PI);
			phase2_shoot.alt=(phase2_ship_y-128)>>2;
			phase2_shoot.state=24;
			play_sound(SND_SHIP_LASER);
		}

	if (left_pressed)
		phase2_ship_x-=8;
	else if (right_pressed)
		phase2_ship_x+=8;
	else if (phase2_ship_x<128)
		phase2_ship_x+=4;
	else if (phase2_ship_x>128)
		phase2_ship_x-=4;

	if (phase2_ship_x>=192)
		phase2_ship_x-=8;
	else if (phase2_ship_x<=64)
		phase2_ship_x+=8;

	if (up_pressed)
	{
		if (mode7_z<512)
		{
			mode7_z += 32;
			phase2_ship_y-=8;
		}
	}
	else if (down_pressed)
	{
		if (mode7_z>256)
		{
			mode7_z -= 32;
			phase2_ship_y+=8;
		}

	}
	else
	if (mode7_z>400)
	{
		mode7_z-=16;
		phase2_ship_y+=4;
	}
	else
	if (mode7_z<368)
	{
		mode7_z+=16;
		phase2_ship_y-=4;
	}

	mode7_process(sllsub(mode7_angle,sllmul(inc,int2sll((128-phase2_ship_x)>>1))));
	phase2_global_x= sll2int(mode7_x);
	phase2_global_y= sll2int(mode7_y);

	phase2_draw();
	delay_frame_time();
    }
コード例 #9
0
ファイル: zmath.c プロジェクト: joshdekock/jim-psp
/* 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);
}
コード例 #10
0
ファイル: matrix.c プロジェクト: joshdekock/jim-psp
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);
}