void p_spark_colorful_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_colorfulspark);
	if(you) {
		vec_add(you.x,vector(60.425,91.578,0.000));
		vec_set(you.pan,vector(0.000,0.000,0.000));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
Example #2
0
static void update_system( m_elem *z, m_elem *x_pre,
             m_elem **K, m_elem *x_post )
{
#ifdef PRINT_DEBUG
  printf( "ekf: updating system\n" );
#endif

  apply_measurement( x_pre, z_estimate );
  vec_sub( z, z_estimate, z_estimate, measurement_size );
  mat_mult_vector( K, z_estimate, x_post, state_size, measurement_size );
  vec_add( x_post, x_pre, x_post, state_size );
}
void pFountain(PARTICLE *p) {
	VECTOR vecTemp;
	vec_randomize(vecTemp, 2);
	vec_add(p.vel_x, vecTemp);
	vec_set(p.blue, vector(random(255), random(255), 255));
	set(p, MOVE | BRIGHT | TRANSLUCENT);
	p.alpha = 100;
	p.size = 2;
	p.gravity = 0.2;
	p.skill_a = 3;
	p.event = pAlphaFade;
}
void p_composition_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_composition);
	if(you) {
		vec_add(you.x,vector(-141.194,16.143,0.000));
		vec_set(you.pan,vector(0.000,0.000,0.000));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
void p_double_helix_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_doublehelix);
	if(you) {
		vec_add(you.x,vector(-149.688,31.689,0.000));
		vec_set(you.pan,vector(0.000,0.000,0.000));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
void p_fire_1_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_fire1);
	if(you) {
		vec_add(you.x,vector(-27.054,186.542,14.000));
		vec_set(you.pan,vector(32.498,-25.617,-3.110));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
void pDenseSmoke(PARTICLE *p) {
	VECTOR vecTemp;
	vec_randomize(vecTemp, 2);
	vec_add(p.vel_x, vector(-1+random(2), -1+random(2), 1));
	vec_set(p.blue, vector(140, 140, 140));
	set(p, MOVE | TRANSLUCENT);
	p.alpha = 60 + random(20);
	p.size = 20;
	p.gravity = 0;
	p.skill_a = 1;
	p.event = pAlphaFade;
}
void p_space_hole_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_spacehole);
	if(you) {
		vec_add(you.x,vector(414.936,660.420,69.408));
		vec_set(you.pan,vector(0.000,0.000,0.000));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
Example #9
0
vprop_t vprop_animate(vprop_t vp, float dt){
	vec_t s;
	if (!(vp.speed == 0.0 || vec_equal(vp.val,vp.target))){
		s = vec_scale(vec_normalize(vec_diff(vp.val,vp.target)),vp.speed);
		if(vec_dist(vp.val,vp.target) < vec_len(s)){
			vp.val = vp.target;
		}else{
			vp.val = vec_add(vp.val,s);
		}
	}
	return vp;
}
Example #10
0
static void ahrs_update_imu(ahrs_t *ahrs, real_t gx, real_t gy, real_t gz,
                            real_t ax, real_t ay, real_t az, real_t dt)
{
   /* get rate of change of quaternion from gyroscope: */
   vec4_t qdot;
   vec4_init(&qdot);
   qdot.ve[0] = REAL(0.5) * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz);
   qdot.ve[1] = REAL(0.5) * ( ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy);
   qdot.ve[2] = REAL(0.5) * ( ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx);
   qdot.ve[3] = REAL(0.5) * ( ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx);

   if (!((ax == REAL(0.0)) && (ay == REAL(0.0)) && (az == REAL(0.0))))
   {
      /* normalize accelerometer measurements: */
      ahrs_normalize_3(&ax, &ay, &az);

      /* auxiliary variables to avoid repeated arithmetic: */
      real_t _2q0 = REAL(2.0) * ahrs->quat.q0;
      real_t _2q1 = REAL(2.0) * ahrs->quat.q1;
      real_t _2q2 = REAL(2.0) * ahrs->quat.q2;
      real_t _2q3 = REAL(2.0) * ahrs->quat.q3;
      real_t _4q0 = REAL(4.0) * ahrs->quat.q0;
      real_t _4q1 = REAL(4.0) * ahrs->quat.q1;
      real_t _4q2 = REAL(4.0) * ahrs->quat.q2;
      real_t _8q1 = REAL(8.0) * ahrs->quat.q1;
      real_t _8q2 = REAL(8.0) * ahrs->quat.q2;
      real_t q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
      real_t q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
      real_t q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
      real_t q3q3 = ahrs->quat.q3 * ahrs->quat.q3;

      /* gradient decent algorithm corrective step: */
      vec4_t s;
      vec4_init(&s);
      s.ve[0] = _4q0 * q2q2 - _2q2 * ax + _4q0 * q1q1 + _2q1 * ay;
      s.ve[1] = _4q1 * q3q3 + _2q3 * ax + REAL(4.0) * q0q0 * ahrs->quat.q1 + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 - _4q1 * az;
      s.ve[2] = REAL(4.0) * q0q0 * ahrs->quat.q2 - _2q0 * ax + _4q2 * q3q3 + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 - _4q2 * az;
      s.ve[3] = REAL(4.0) * q1q1 * ahrs->quat.q3 + _2q1 * ax + REAL(4.0) * q2q2 * ahrs->quat.q3 + _2q2 * ay;
      
      vec_normalize(&s);
      
      /* apply feedback step: */
      vec_scalar_mul(&s, &s, ahrs->beta);
      vec_sub(&qdot, &qdot, &s);
   }

   /* integrate rate of change to yield quaternion: */
   vec_scalar_mul(&qdot, &qdot, dt);
   vec_add(&ahrs->quat, &ahrs->quat, &qdot);
   
   /* normalise quaternion: */
   quat_normalize(&ahrs->quat);
}
Example #11
0
void item_particle (PARTICLE *p) 
{
	VECTOR vecTemp;
	vec_randomize(&vecTemp, 10);
	vec_normalize(&vecTemp, 1);
	vec_add (&p->vel_x, &vecTemp);
	vec_set(&p->blue, vector(0, 150, 150));
	set(p, MOVE | TRANSLUCENT /*| BRIGHT*/);
	p->lifespan = 80;
	p->size  = 15 + random(5);
	p->event = item_particleFader;
}
void p_fountain_1_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_fountain1);
	if(you) {
		vec_add(you.x,vector(-200.174,290.466,0.000));
		vec_set(you.pan,vector(0.000,0.000,0.000));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
void p_spiral_create(VECTOR *position) {
	if(!position)position = nullvector;
	wait(3);
	you = ent_create(NULL,position,emit_spiral);
	if(you) {
		vec_add(you.x,vector(371.551,349.278,0.000));
		vec_set(you.pan,vector(0.000,0.000,0.000));
		vec_set(you.scale_x,vector(1.000,1.000,1.000));
		set(you,PASSABLE);
		set(you,INVISIBLE);
	}
}
Example #14
0
static void PmlTableGen(int bw,
			int m,
			double *storeplm,
			double *workspace)
{
  double *prev, *prevprev, *temp1, *temp2, *temp3, *temp4, *x_i, *eval_args;
  int i;
  /* double *storeplm_ptr; */
  
  prevprev = workspace;
  prev = prevprev + (2*bw);
  temp1 = prev + (2*bw);
  temp2 = temp1 + (2*bw);
  temp3 = temp2 + (2*bw);
  temp4 = temp3 + (2*bw);
  x_i = temp4 + (2*bw);
  eval_args = x_i + (2*bw);
  
  /* storeplm_ptr = storeplm; */

  /* get the evaluation nodes */
  EvalPts(2*bw,x_i);
  ArcCosEvalPts(2*bw,eval_args);
  
  /* set initial values of first two Pmls */
  for (i=0; i<2*bw; i++) 
    prevprev[i] = 0.0;
  if (m == 0)
    for (i=0; i<2*bw; i++) {
      /* prev[i] = 0.5; */
      prev[i] = 0.707106781186547 ;  /* 1/sqrt(2) */
    }
  else 
    Pmm_L2(m, eval_args, 2*bw, prev);

  memcpy(storeplm, prev, (size_t) sizeof(double) * 2 * bw);

  for(i = 0; i < bw - m - 1; i++)
    {
      vec_mul(L2_cn(m,m+i),prevprev,temp1,2*bw);
      vec_pt_mul(prev, x_i, temp2, 2*bw);
      vec_mul(L2_an(m,m+i), temp2, temp3, 2*bw);
      vec_add(temp3, temp1, temp4, 2*bw); /* temp4 now contains P(m,m+i+1) */
      
      storeplm += (2 * bw);
      memcpy(storeplm, temp4, (size_t) sizeof(double) * 2 * bw);
      memcpy(prevprev, prev, (size_t) sizeof(double) * 2 * bw);
      memcpy(prev, temp4, (size_t) sizeof(double) * 2 * bw);
    }

  /*  storeplm = storeplm_ptr; */
}
Example #15
0
/* Computes metallic reflection */
static void ga_shade_reflect(vec_t *color,const ga_scene_t *s, const ga_material_t *mat,const vec_t *pos, const vec_t *dir, const vec_t *norm, float importance, int max_rec){
	vec_t r,d1,d2,dr;
	vec_t rcolor;
	int sample = mat->soft_ref_sample;
	int i;
	r = vec_sub(*dir,vec_scale(2.0f*vec_dot(*norm,*dir),*norm));
	vec_fperp(&r,&d1,&d2);
	if(sample <= 0){
		sample = 1;
	}
	i = sample;
	while(i--){
		dr = r;
		vec_ffadd(&dr,mat->soft_ref_angle*(2.0f*random()*RAND_NORM) -1.0f,&d1);
		vec_ffadd(&dr,mat->soft_ref_angle*(2.0f*random()*RAND_NORM) -1.0f,&d2);
		vec_fnorm(&dr);
		rcolor = vec_add(rcolor,
				vec_scale(mat->ref_factor,vec_mult(mat->ref_color,
				ga_ray_trace(s,*pos,dr,importance,max_rec -1, NULL))));
	}
	*color = vec_add(*color,vec_scale(1.0f/sample,rcolor));
}
static void vorbis_inverse_coupling_altivec(float *mag, float *ang,
                                            intptr_t blocksize)
{
    int i;
    vector float m, a;
    vector bool int t0, t1;
    const vector unsigned int v_31 = //XXX
        vec_add(vec_add(vec_splat_u32(15),vec_splat_u32(15)),vec_splat_u32(1));
    for (i = 0; i < blocksize; i += 4) {
        m = vec_ld(0, mag+i);
        a = vec_ld(0, ang+i);
        t0 = vec_cmple(m, (vector float)vec_splat_u32(0));
        t1 = vec_cmple(a, (vector float)vec_splat_u32(0));
        a = vec_xor(a, (vector float) vec_sl((vector unsigned int)t0, v_31));
        t0 = (vector bool int)vec_and(a, t1);
        t1 = (vector bool int)vec_andc(a, t1);
        a = vec_sub(m, (vector float)t1);
        m = vec_add(m, (vector float)t0);
        vec_stl(a, 0, ang+i);
        vec_stl(m, 0, mag+i);
    }
}
Example #17
0
void rgbaint_t::blend(const rgbaint_t& other, UINT8 factor)
{
	const VECU32 shift = vec_splat_u32(-16);
	const VECS32 scale1 = { factor, factor, factor, factor };
	const VECS32 scale2 = { 0x100 - factor, 0x100 - factor, 0x100 - factor, 0x100 - factor, };

	VECU32 temp = vec_msum((VECU16)m_value, (VECU16)vec_rl(scale1, shift), vec_splat_u32(0));
	temp = vec_msum((VECU16)other.m_value, (VECU16)vec_rl(scale2, shift), temp);

	m_value = vec_msum((VECU16)m_value, (VECU16)scale1, vec_mulo((VECU16)other.m_value, (VECU16)scale2));
	m_value = vec_add(vec_sl(temp, shift), (VECU32)m_value);
	sra(8);
}
Example #18
0
static void kalman_predict(kalman_t *kf, float a)
{
   /* x = A * x + B * u */
   kf->u.ve[0] = a;
   mat_vec_mul(&kf->t0, &kf->A, &kf->x); /* t0 = A * x */
   mat_vec_mul(&kf->t1, &kf->B, &kf->u); /* t1 = B * u */
   vec_add(&kf->x, &kf->t0, &kf->t1);    /* x = t0 + t1 */

   /* P = A * P * AT + Q */
   mat_mul(&kf->T0, &kf->A, &kf->P);   /* T0 = A * P */
   mmtr_mul(&kf->T1, &kf->T0, &kf->A); /* T1 = T0 * AT */
   mat_add(&kf->P, &kf->T1, &kf->Q);   /* P = T1 * Q */
}
Example #19
0
int shadow(vec pt, light l, sphere_list *ss)
{
  vec nudge = vec_scale(0.0001, l.direction);
  vec lifted = vec_add(pt, nudge);
  ray test_ray = {lifted, l.direction};
  while (ss!=NULL) {
    hit_test h = intersect(test_ray, ss->s);
    if (!h.miss)
      return 1;
    ss = ss->next;
  }
  return 0;
}
Example #20
0
File: gram.c Project: PDelak/intent
static Elem *
new_term_string(Grammar *g, char *s, char *e, Rule *r) { 
  Term *t = new_term();
  Elem *elem;

  t->string = reinterpret_cast<char*>(MALLOC(e - s + 1));
  memcpy(t->string, s, e - s);
  t->string[e - s] = 0;
  t->string_len = e - s;
  vec_add(&g->terminals, t);
  elem = new_elem_term(t, r);
  return elem;
}
Example #21
0
/* Computes the phong hilight */
static void ga_shade_phong(vec_t *color, const ga_material_t *mat,const vec_t *lcolor, const vec_t *ldir, const vec_t *dir, const vec_t *norm, float factor){
	vec_t r;
	float fact;
	float power;
	r = vec_sub(
		vec_scale(2.0f,vec_scale(vec_dot(*norm,*ldir),*norm)),
		*ldir);	
	if((fact = vec_dot(r,vec_neg(*dir))) > 0.0f){
		power = powf(fact,mat->spec_power)*mat->spec_factor;
		*color = vec_add(*color, vec_scale( power*factor,
				vec_mult(mat->spec_color,*lcolor)));
	}
}
Example #22
0
static void add_int16_altivec(int16_t * v1, int16_t * v2, int order)
{
    int i;
    register vec_s16_t vec, *pv;

    for(i = 0; i < order; i += 8){
        pv = (vec_s16_t*)v2;
        vec = vec_perm(pv[0], pv[1], vec_lvsl(0, v2));
        vec_st(vec_add(vec_ld(0, v1), vec), 0, v1);
        v1 += 8;
        v2 += 8;
    }
}
void pStars(PARTICLE *p) {
	VECTOR vecTemp;
	vec_randomize(vecTemp, -1);
	vec_add(p.vel_x, vecTemp);
	vec_set(p.blue, vector(random(255), random(255), 255));
	set(p, MOVE | BRIGHT | TRANSLUCENT);
	p.alpha = 100;
	p.size = 3;
	p.gravity = 1;
	p.skill_a = 3;
	p.bmap = bmapStar;
	p.event = pAlphaFade;
}
Example #24
0
void update_tick_avatar(){

  // boost when player wants to boost, has boost and not currently boosting
  Bool can_boost = !tick_boost_ttl and tick_energie;
  if(input_key_down(INPUT_KEY_UP) and can_boost){ 
    synth_play_channel4(SYNTH_NOTE_CIS, SYNTH_OCTAVE_C0);
    tick_boost_vec = vec_make_float( 0.00, - BOOST_ACCEL);
    tick_boost_ttl = BOOST_TIME;
    tick_energie--;
  } else if(input_key_down(INPUT_KEY_DOWN) and can_boost){ 
    synth_play_channel4(SYNTH_NOTE_CIS, SYNTH_OCTAVE_C0);
    tick_boost_vec = vec_make_float( 0.00, BOOST_ACCEL);
    tick_boost_ttl = BOOST_TIME;
    tick_energie--;
  } else if(input_key_down(INPUT_KEY_A) and can_boost){ 
    synth_play_channel4(SYNTH_NOTE_AIS, SYNTH_OCTAVE_C0);
    if (tick_energie > 1){
      tick_boost_vec = vec_make_float(BOOST_ACCEL + BOOST_ACCEL, 0.00);
      tick_boost_ttl = BOOST_TIME;
      tick_energie -= 2;
    } else {
      tick_boost_vec = vec_make_float(BOOST_ACCEL, 0.00);
      tick_boost_ttl = BOOST_TIME;
      tick_energie--;
    }
  }

  // move tick_avatar
  if (tick_boost_ttl){
    tick_avatar.vec = vec_add(tick_avatar.vec, tick_boost_vec);
    tick_boost_ttl--;
  } 
  tick_avatar.pos = vec_add(tick_avatar.pos, tick_avatar.vec);

  if (tick_avatar.hit_ttl){
    tick_avatar.hit_ttl--;
  }
}
Example #25
0
int			init_env(t_env *e, t_obj *obj)
{
	if ((e->mlx = mlx_init()) == NULL)
		return (m_error("mlx_init(): fail"));
	if ((e->win = mlx_new_window(e->mlx, WIN_X, WIN_Y, "rtv1")) == NULL)
		return (m_error("mlx_new_window(): fail"));
	if ((e->img = mlx_new_image(e->mlx, WIN_X, WIN_Y)) == NULL)
		return (m_error("mlx_new_image(): fail"));
	if ((e->addr = mlx_get_data_addr(e->img, &(e->bpp), &(e->size_line),
		&(e->endian))) == NULL)
		return (m_error("mlx_get_data_addr(): fail"));
	if ((e->rgb_tab = (t_rgb *)malloc(sizeof(t_rgb) * (WIN_X * WIN_Y))) == NULL)
		return (m_error("rgb_tab_init(): fail"));
	e->eye_pos = vec_new(0, 0, 0);
	e->eye_dir = vec_new(0, 0, 1);
	e->right_vec = vec_new(1, 0, 0);
	e->up_vec = vec_new(0, -1, 0);
	e->view_plane_ori = vec_add(vec_add(e->eye_pos, vec_numb(e->eye_dir,
		VIEW_PLANE_DIST)), vec_sub(vec_numb(e->up_vec, VIEW_PLANE_HEIGHT / 2.0)
		, vec_numb(e->right_vec, VIEW_PLANE_WIDTH / 2.0)));
	init_obj(obj);
	return (0);
}
Example #26
0
File: gram.c Project: PDelak/intent
Production *
new_production(Grammar *g, char *name) {
  Production *p; 
  if ((p = lookup_production(g, name, strlen(name)))) {
    FREE(name);
    return p;
  }
  p = reinterpret_cast<Production*>(MALLOC(sizeof(Production)));
  memset(p, 0, sizeof(Production));
  vec_add(&g->productions, p);
  p->name = name;
  p->name_len = strlen(name);
  return p;
}
void BaseStateEstimation::transformImuStateToBase()
{
	  MY_MATRIX(S_angrate, 1,3,1,3);
	  vectorToSkewMatrix(O_angrate_I_, S_angrate);
	  MY_MATRIX(S_angacc, 1,3,1,3);
	  vectorToSkewMatrix(O_angacc_I_, S_angacc);
	  MY_MATRIX(S_helper, 1,3,1,3);
	  mat_mult(S_angrate, S_angrate, S_helper);
	  mat_add(S_angacc, S_helper, S_helper);

	  mat_mult(O_rot_I_, I_rot_B_, O_rot_B_);

	  mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_);
	  vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_);

	  mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_);
	  mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_);
	  vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_);

	  mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_);
	  mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_);
	  vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_);
}
Example #28
0
File: optcode.c Project: 1c0n/xbmc
/* this is an alternate version of set_dry_signal() in reverb.c */
void v_set_dry_signal(void* destp, const int32* buf, int32 n)
{
    int i = 0;
    if( n>=8 ) {
        vint32* dest = (vint32*) destp;
	for( ; i < n / 4; i++) {
            dest[i] = vec_add(dest[i],((vint32*) buf)[i]);
	}
    }
    /* write remaining few bytes. */
    for( i *= 4; i < n; i++) {
        ((int32*) destp)[i] += buf[i];
    }
}
Example #29
0
/**
   main navigation routine. This is called periodically evaluates the current
   Position and stage and navigates accordingly.
   Returns True until the survey is finished
**/
bool_t poly_survey_adv(void)
{
  //entry circle around entry-center until the desired altitude is reached
  if (psa_stage == ENTRY) {
	nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
	if (NavCourseCloseTo(segment_angle)
		&& nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
		&& fabs(estimator_z - psa_altitude) <= 20) {
	  psa_stage = SEG;
	  NavVerticalAutoThrottleMode(0.0);
	  nav_init_stage();
	  //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
	}
  }
  //fly the segment until seg_end is reached
  if (psa_stage == SEG) {
	nav_points(seg_start, seg_end);
	//calculate all needed points for the next flyover
	if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
	  //dc_stop();
	  VEC_CALC(seg_center1, seg_end, rad_vec, -);
	  ret_start.x = seg_end.x - 2*rad_vec.x;
	  ret_start.y = seg_end.y - 2*rad_vec.y;

	  //if we get no intersection the survey is finished
	  if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
		return FALSE;

	  ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
	  ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;

	  seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
	  seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);

	  psa_stage = TURN1;
	  nav_init_stage();
	}
        void ConditionalSquareSum(const uint8_t * src, size_t srcStride, size_t width, size_t height,
            const uint8_t * mask, size_t maskStride, uint8_t value, uint64_t * sum)
        {
            assert(width >= A);
            if (align)
                assert(Aligned(src) && Aligned(srcStride) && Aligned(mask) && Aligned(maskStride));

            size_t alignedWidth = AlignLo(width, QA);
            size_t bodyWidth = AlignLo(width, A);
            v128_u8 tailMask = ShiftLeft(K8_FF, A - width + alignedWidth);
            v128_u8 _value = SetU8(value);
            *sum = 0;
            for (size_t row = 0; row < height; ++row)
            {
                size_t col = 0;
                v128_u32 sums[4] = { K32_00000000, K32_00000000, K32_00000000, K32_00000000 };
                for (; col < alignedWidth; col += QA)
                {
                    ConditionalSquareSum<align, compareType>(src, mask, col, _value, sums[0]);
                    ConditionalSquareSum<align, compareType>(src, mask, col + A, _value, sums[1]);
                    ConditionalSquareSum<align, compareType>(src, mask, col + 2 * A, _value, sums[2]);
                    ConditionalSquareSum<align, compareType>(src, mask, col + 3 * A, _value, sums[3]);
                }
                sums[0] = vec_add(vec_add(sums[0], sums[1]), vec_add(sums[2], sums[3]));
                for (; col < bodyWidth; col += A)
                    ConditionalSquareSum<align, compareType>(src, mask, col, _value, sums[0]);
                if (alignedWidth != width)
                {
                    const v128_u8 _mask = Compare8u<compareType>(Load<false>(mask + width - A), _value);
                    const v128_u8 _src = vec_and(vec_and(Load<false>(src + width - A), _mask), tailMask);
                    sums[0] = vec_msum(_src, _src, sums[0]);
                }
                *sum += ExtractSum(sums[0]);
                src += srcStride;
                mask += maskStride;
            }
        }