コード例 #1
0
void booz_sensors_model_accel_init(double time) {

  bsm.accel = v_get(AXIS_NB);
  bsm.accel->ve[AXIS_X] = 0.;
  bsm.accel->ve[AXIS_Y] = 0.;
  bsm.accel->ve[AXIS_Z] = 0.;
  bsm.accel_resolution = BSM_ACCEL_RESOLUTION;

  bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
  m_zero(bsm.accel_sensitivity);
  bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
  bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
  bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;

  bsm.accel_neutral = v_get(AXIS_NB);
  bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
  bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
  bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;

  bsm.accel_noise_std_dev = v_get(AXIS_NB);
  bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
  bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
  bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;

  bsm.accel_bias = v_get(AXIS_NB);
  bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
  bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
  bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;

  bsm.accel_next_update = time;
  bsm.accel_available = FALSE;

}
コード例 #2
0
// kernelRegression
// x:data
// dimention: x's dimention
// y: label of x 
// num: number of data
// result: coeffience
double *kernelRegression(double **x,int dimention,double *y,int num,double gamma,double regilarization){
	VEC *label;
	MAT *gram,*ident,*inv;
	int i,j;
	double *result;

	ident = m_get(num,num);
	label = v_get(num);
	memcpy(label->ve,y,sizeof(double)*num);
	gram = m_get(num,num);
	ident = m_get(num,num);
	for(i=0;i<num;i++){
		for(j=0;j<num;j++){
			gram->me[i][j]=kernel(x[i],x[j],dimention,gamma);
		}
	}
	inv=m_add(gram,sm_mlt(regilarization,m_ident(ident),NULL),NULL);
	inv=m_inverse(inv,NULL); //memory leak.
	VEC *coefficient=v_get(num);
	mv_mlt(inv,label,coefficient);
	result=malloc(sizeof(double)*num);
	memcpy(result,coefficient->ve,sizeof(double)*num);

	m_free(ident);
	m_free(gram);
	m_free(inv);
	v_free(label);
	v_free(coefficient);
	return result;
}
コード例 #3
0
void booz_sensors_model_mag_init( double time ) {

  bsm.mag = v_get(AXIS_NB);
  bsm.mag->ve[AXIS_X] = 0.;
  bsm.mag->ve[AXIS_Y] = 0.;
  bsm.mag->ve[AXIS_Z] = 0.;
  //  bsm.mag_resolution = BSM_MAG_RESOLUTION;

  bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB);
  VEC* tmp_eulers = v_get(AXIS_NB);
  tmp_eulers->ve[EULER_PHI]   = BSM_MAG_IMU_TO_SENSOR_PHI;
  tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA;
  tmp_eulers->ve[EULER_PSI]   = BSM_MAG_IMU_TO_SENSOR_PSI;
  dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor );

  bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
  m_zero(bsm.mag_sensitivity);
  bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = BSM_MAG_SENSITIVITY_XX;
  bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_MAG_SENSITIVITY_YY;
  bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_MAG_SENSITIVITY_ZZ;

  bsm.mag_neutral = v_get(AXIS_NB);
  bsm.mag_neutral->ve[AXIS_X] = BSM_MAG_NEUTRAL_X;
  bsm.mag_neutral->ve[AXIS_Y] = BSM_MAG_NEUTRAL_Y;
  bsm.mag_neutral->ve[AXIS_Z] = BSM_MAG_NEUTRAL_Z;

  bsm.mag_noise_std_dev = v_get(AXIS_NB);
  bsm.mag_noise_std_dev->ve[AXIS_X] = BSM_MAG_NOISE_STD_DEV_X;
  bsm.mag_noise_std_dev->ve[AXIS_Y] = BSM_MAG_NOISE_STD_DEV_Y;
  bsm.mag_noise_std_dev->ve[AXIS_Z] = BSM_MAG_NOISE_STD_DEV_Z;

  bsm.mag_next_update = time;
  bsm.mag_available = FALSE;

}
コード例 #4
0
ファイル: memory.c プロジェクト: openalea-incubator/caribu
/* v_resize -- returns the vector x with dim new_dim
   -- x is set to the zero vector */
extern  VEC	*v_resize(VEC *x, int new_dim)
{
   
   if (new_dim < 0)
     error(E_NEG,"v_resize");

   if ( ! x )
     return v_get(new_dim);

   /* nothing is changed */
   if (new_dim == x->dim)
     return x;

   if ( x->max_dim == 0 )	/* assume that it's from sub_vec */
     return v_get(new_dim);
   
   if ( new_dim > x->max_dim )
   {
      if (mem_info_is_on()) { 
	 mem_bytes(TYPE_VEC,x->max_dim*sizeof(Real),
			 new_dim*sizeof(Real));
      }

      x->ve = RENEW(x->ve,new_dim,Real);
      if ( ! x->ve )
	error(E_MEM,"v_resize");
      x->max_dim = new_dim;
   }
   
   if ( new_dim > x->dim )
     __zero__(&(x->ve[x->dim]),new_dim - x->dim);
   x->dim = new_dim;
   
   return x;
}
コード例 #5
0
ファイル: rot2d.c プロジェクト: jalishah/airborne
rot2d_context_t *rot2d_create(void)
{
   rot2d_context_t *context = (rot2d_context_t *)malloc(sizeof(rot2d_context_t));
   /* allocate memory for matrix/vector data structures: */
   context->matrix = m_get(2, 2);
   context->in = v_get(2);
   context->out = v_get(2);
   return context;
}
コード例 #6
0
ファイル: lufactor.c プロジェクト: trehomudia/skotina
MAT	*m_inverse(const MAT *A, MAT *out)
{
  unsigned int	i;
  char MatrixTempBuffer[ 4000 ];
  VEC	*tmp = VNULL, *tmp2 = VNULL;
  MAT	*A_cp = MNULL;
  PERM	*pivot = PNULL;

  if ( ! A )
    error(E_NULL,"m_inverse");
  if ( A->m != A->n )
    error(E_SQUARE,"m_inverse");
  if ( ! out || out->m < A->m || out->n < A->n )
    out = m_resize(out,A->m,A->n);

  if( SET_MAT_SIZE( A->m, A->n ) < 1000 )
    mat_get( &A_cp, (void *)( MatrixTempBuffer + 2000 ), A->m, A->n );
  else
    A_cp = matrix_get( A->m, A->n  );

  A_cp = m_copy( A, A_cp );
  if( SET_VEC_SIZE( A->m ) < 1000 ) {
    vec_get( &tmp, (void *)MatrixTempBuffer, A->m );
    vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), A->m );
  } else {
    tmp   = v_get( A->m );
    tmp2  = v_get( A->m );
  }

  if( SET_PERM_SIZE( A->m ) < 1000 ) {
    perm_get( &pivot, (void *)( MatrixTempBuffer + 3000 ), A->m );
  } else {
    pivot   = px_get( A->m );
  }

  LUfactor(A_cp,pivot);
  //tracecatch_matrix(LUfactor(A_cp,pivot),"m_inverse");
  for ( i = 0; i < A->n; i++ ){
    v_zero(tmp);
    tmp->ve[i] = 1.0;
    LUsolve(A_cp,pivot,tmp,tmp2);
    //tracecatch_matrix(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
    set_col(out,i,tmp2);
  }
  if( tmp != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить
    V_FREE(tmp);
  if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить
    V_FREE(tmp2);
  if( A_cp != (MAT *)(MatrixTempBuffer + 2000) ) // память выделялась, надо освободить
    M_FREE(A_cp);
  if( pivot != (PERM *)(MatrixTempBuffer + 3000) ) // память выделялась, надо освободить
    PX_FREE( pivot );
  return out;
}
コード例 #7
0
ファイル: id3read.c プロジェクト: xtforever/xtcw
void id3db_print(int db)
{
    struct id3ent *d;
    int p;
    m_foreach(db, p, d )
	{
            char *album = v_get(d->vs, "album", 1 );
            char *title = v_get(d->vs, "title", 1 );

            printf( "A: %s\n", album );
            printf( "T: %s\n", title );
            printf( "F: %s\n", d->path );

	}
コード例 #8
0
ファイル: Omu_IntRKsuite.C プロジェクト: m-sonntag/hqp
//--------------------------------------------------------------------------
Omu_IntRKsuite::Omu_IntRKsuite()
{
  theOmu_IntRKsuite = this;

  _y_head.max_dim = 0;
  _yp_head.max_dim = 0;
  _yp = v_get(1);
  _u = v_get(1);
  _work = v_get(1);
  _thres = v_get(1);
  _hnext = 0.0;
  _tlast = 0.0;
  _method = 2;

  _ifList.append(new If_Int("prg_int_method", &_method));
}
コード例 #9
0
ファイル: kalman.c プロジェクト: jalishah/airborne
static void kalman_init(kalman_t *kf, float q, float r, float pos, float speed)
{
   kf->t0 = v_get(2);
   kf->t1 = v_get(2);
   kf->T0 = m_get(2, 2);
   kf->T1 = m_get(2, 2);
   
   kf->I = m_get(2, 2);
   m_ident(kf->I);

   /* set initial state: */
   kf->x = v_get(2);
   v_set_val(kf->x, 0, pos);
   v_set_val(kf->x, 1, speed);

   /* no measurement or control yet: */
   kf->z = v_get(2);
   kf->u = v_get(1);

   kf->P = m_get(2, 2);
   m_ident(kf->P);
   
   /* set up noise: */
   kf->Q = m_get(2, 2);
   sm_mlt(q, kf->I, kf->Q);
   kf->R = m_get(2, 2);
   sm_mlt(r, kf->I, kf->R);
   
   kf->K = m_get(2, 1);
   kf->H = m_get(2, 2);
   m_set_val(kf->H, 0, 0, 1.0);
   //m_ident(kf->H);
    
   /* A = | 1.0   dt  |
          | 0.0   1.0 |
      dt values is set in kalman_run */
   kf->A = m_get(2, 2);
   m_set_val(kf->A, 0, 0, 1.0);
   m_set_val(kf->A, 1, 0, 0.0);
   m_set_val(kf->A, 1, 1, 1.0);

   /* B = | 0.5 * dt ^ 2 |
          |     dt       |
      dt values are set in kalman_run */
   kf->B = m_get(2, 1);
}
コード例 #10
0
ファイル: Omu_IntODE.C プロジェクト: m-sonntag/hqp
//--------------------------------------------------------------------------
Omu_IntODE::Omu_IntODE()
{
  _sys = NULL;
  _xt_ptr = NULL;
  _Ft_ptr = NULL;

  _y = v_get(1);
  _u = v_get(1);

  _x = v_get(1);
  _v = v_get(1);
  _X2 = m_get(1, 1);
  _Y2 = m_get(1, 1);

  _Yx = m_get(1, 1);
  _Yu = m_get(1, 1);
}
コード例 #11
0
ファイル: dsp_sub.c プロジェクト: CEPBEP/onion-phone
int16_t peakiness(int16_t input[], int16_t npts)
{
	register int16_t i;
	int16_t peak_fact, scale = 4;
	int32_t sum_abs, L_temp;
	int16_t temp1, temp2, *temp_buf;

	temp_buf = v_get(npts);
	v_equ_shr(temp_buf, input, scale, npts);
	L_temp = L_v_magsq(temp_buf, npts, 0, 1);

	if (L_temp) {
		temp1 = melpe_norm_l(L_temp);
		scale = melpe_sub(scale, melpe_shr(temp1, 1));
		if (scale < 0)
			scale = 0;
	} else
		scale = 0;

	sum_abs = 0;
	for (i = 0; i < npts; i++) {
		L_temp = melpe_L_deposit_l(melpe_abs_s(input[i]));
		sum_abs = melpe_L_add(sum_abs, L_temp);
	}

	/* Right shift input signal and put in temp buffer.                       */
	if (scale)
		v_equ_shr(temp_buf, input, scale, npts);

	if (sum_abs > 0) {
		/*      peak_fact = sqrt(npts * v_magsq(input, npts))/sum_abs             */
		/*                        = sqrt(npts) * (sqrt(v_magsq(input, npts))/sum_abs)     */
		if (scale)
			L_temp = L_v_magsq(temp_buf, npts, 0, 0);
		else
			L_temp = L_v_magsq(input, npts, 0, 0);
		L_temp = melpe_L_deposit_l(L_sqrt_fxp(L_temp, 0));	/* L_temp in Q0 */
		peak_fact = L_divider2(L_temp, sum_abs, 0, 0);

		if (peak_fact > LIMIT_PEAKI) {
			peak_fact = SW_MAX;
		} else {	/* shl 7 is mult , other shift is Q7->Q12 */
			temp1 = melpe_add(scale, 5);
			temp2 = melpe_shl(npts, 7);
			temp2 = sqrt_fxp(temp2, 7);
			L_temp = melpe_L_mult(peak_fact, temp2);
			L_temp = melpe_L_shl(L_temp, temp1);
			peak_fact = melpe_extract_h(L_temp);
		}
	} else
		peak_fact = 0;

	v_free(temp_buf);
	return (peak_fact);
}
コード例 #12
0
double kernel(double *x1,double *x2,int num,double gamma){
	VEC *xVector,*yVector,*diffVector;
	int i;

	xVector=v_get(num);
	memcpy(xVector->ve,x1,sizeof(double)*num);
	yVector=v_get(num);
	memcpy(yVector->ve,x2,sizeof(double)*num);

	diffVector=v_sub(xVector,yVector,NULL);

	double sum=0.0;
	for(i=0;i<num;i++){
		sum+=diffVector->ve[i]*diffVector->ve[i];
	}

	v_free(diffVector);
	v_free(xVector);
	v_free(yVector);
	
	return exp(-gamma*sum);
}
コード例 #13
0
ファイル: iftest.C プロジェクト: m-sonntag/hqp
int
Tcl_AppInit(Tcl_Interp *interp)
{
  if (Tcl_InitStubs(interp, "8.1", 0) == NULL) {
    return TCL_ERROR;
  }

  if (Tcl_Init(interp) == TCL_ERROR) {
    return TCL_ERROR;
  }

  theInterp = interp;

  new If_Int("if_int", &if_int);
  new If_Int("if_intCb",
	     new If_GetCb<int, A>(&A::i, &a));

  new If_Bool("if_bool", &if_bool);
  new If_Real("if_real", &if_real);

  if_realVec = v_get(5);
  new If_RealVec("if_realVec", &if_realVec);

  if_realMat = m_get(2,3);
  new If_RealMat("if_realMat", &if_realMat);

  if_intVec = iv_get(3);
  new If_IntVec("if_intVec", &if_intVec);

  new If_IntVec("if_intVecp",
                new If_GetCb<const IVECP, A>(&A::intVecp, &a),
                new If_SetCb<const IVECP, A>(&A::set_intVecp, &a));

  new If_String("if_string", &if_string);
		
  new If_String("if_stringCb",
		new If_GetCb<const char *, A>(&A::string, &a),
		new If_SetCb<const char *, A>(&A::set_string, &a));

  new If_StdString("if_stdString",
		   new If_GetCb<const std::string&, A>(&A::stdString, &a),
		   new If_SetCb<const std::string&, A>(&A::set_stdString, &a));

  new If_Procedure("if_procedure", &if_procedure);

  new If_Method<A>("if_method", &A::if_method, &a);

  Tcl_SetVar(interp, "tcl_rcFileName", "~/.tclshrc", TCL_GLOBAL_ONLY);
  return TCL_OK;
}
コード例 #14
0
ファイル: quat.c プロジェクト: jnemanja/Aalto2_FreeRTOS
void quat_rot_vec(VEC *q1, VEC *v1){
	VEC *quat_of_vec, *qmulv, *qinv, *fin_rot_vec; //all are quaternions

    quat_of_vec = v_get(4);
    v_zero(quat_of_vec);
    qmulv = v_get(4);
    v_zero(qmulv);
    qinv = v_get(4);
    v_zero(qinv);
    fin_rot_vec = v_get(4);
    v_zero(fin_rot_vec);
	vec2quat(v1, quat_of_vec); 
	quat_inv(q1, qinv);
	quat_mul(qinv, quat_of_vec, qmulv);
	quat_mul(qmulv, q1, fin_rot_vec);
	quat2vec(fin_rot_vec, v1);
    
    v_free(quat_of_vec);
    v_free(qmulv);
    v_free(qinv);
    v_free(fin_rot_vec);
    
}
コード例 #15
0
ファイル: UKF.c プロジェクト: jnemanja/Aalto2_FreeRTOS
double rk4(double t, VEC *x, double h, VEC *Torq)
{
    VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL;
    VEC *temp=VNULL;
    double step_size=0.5*h;

    if ( x == VNULL )
        error(E_NULL,"rk4");

    v1   = v_get(x->dim);
    v2   = v_get(x->dim);
    v3   = v_get(x->dim);
    v4   = v_get(x->dim);
    temp = v_get(x->dim);
   
    Sy_m(x, Torq, v1); 
    v_mltadd(x,v1,step_size,temp);    
    Sy_m(temp, Torq, v2); 
    v_mltadd(x,v2,step_size,temp);   
    Sy_m(temp, Torq, v3);
    step_size = h;
    v_mltadd(x, v3, step_size, temp);        
    Sy_m(temp, Torq, v4);
    temp = v_copy(v1,temp);           
    v_mltadd(temp,v2,2.0,temp); 
    v_mltadd(temp,v3,2.0,temp); 
    v_add(temp,v4,temp);
    v_mltadd(x,temp,(h/6.0),x);   
     return t+h;   
     
    V_FREE(v1);
    V_FREE(v2);
    V_FREE(v3);
    V_FREE(v4);
    V_FREE(temp);
    
}
コード例 #16
0
ファイル: Prg_ASCEND.C プロジェクト: m-sonntag/hqp
//-------------------------------------------------------------------------
Prg_ASCEND::Prg_ASCEND()
{
  _nvars = 0;
  _nrels = 0;
  _vars = NULL;
  _rels = NULL;
  _obj = NULL;
  _safe_calc = TRUE;
  _var_lb = v_resize(v_get(1), 0);
  _var_ub = v_resize(v_get(1), 0);
  _var_asc2hqp = iv_resize(iv_get(1), 0);
  _derivatives = v_resize(v_get(1), 0);
  _var_master_idxs = iv_resize(iv_get(1), 0);
  _var_solver_idxs = iv_resize(iv_get(1), 0);
  _Inf = 1e38;	// todo: should be initialized from ASCEND data
  _me_bounds = 0;
  _m_bounds = 0;
  memset(&_slv_status, 0, sizeof(slv_status_t));

  // create interface elements
  _ifList.append(new If_Int("prg_safe_calc", &_safe_calc));

  slv_register_client(Prg_ASCEND::slv_register, NULL, NULL);
}
コード例 #17
0
ファイル: memory.c プロジェクト: openalea-incubator/caribu
extern  int v_get_vars(int dim,...) 
{
   va_list ap;
   int i=0;
   VEC **par;
   
   va_start(ap, dim);
   while (par = va_arg(ap,VEC **)) {   /* NULL ends the list*/
      *par = v_get(dim);
      i++;
   } 

   va_end(ap);
   return i;
}
コード例 #18
0
void booz_sensors_model_init(double time) {
 
  VEC* tmp_eulers = v_get(AXIS_NB);
  tmp_eulers->ve[EULER_PHI]   = BSM_BODY_TO_IMU_PHI;
  tmp_eulers->ve[EULER_THETA] = BSM_BODY_TO_IMU_THETA;
  tmp_eulers->ve[EULER_PSI]   = BSM_BODY_TO_IMU_PSI;
  bsm.body_to_imu = m_get(AXIS_NB, AXIS_NB);
  dcm_of_eulers (tmp_eulers, bsm.body_to_imu );

  booz_sensors_model_accel_init(time);
  booz_sensors_model_gyro_init(time);
  booz_sensors_model_mag_init(time);
  booz_sensors_model_rangemeter_init(time);
  booz_sensors_model_baro_init(time);
  booz_sensors_model_gps_init(time);

} 
コード例 #19
0
ファイル: quat.c プロジェクト: jnemanja/Aalto2_FreeRTOS
void quat_mul(VEC *q1, VEC *q2, VEC *res)
{
    int i;
    VEC *temp;
    temp = v_get(4);
    
    if(q1->dim != 4 || q2->dim != 4)
    {
       // cliPrintf("\nquaternion dimensions not correct in quat_mul");
    }
    
	temp->ve[3] = (q1->ve[3] * q2->ve[3]) - (q1->ve[0] * q2->ve[0]) - (q1->ve[1] * q2->ve[1]) - (q1->ve[2] * q2->ve[2]);
	temp->ve[0] = (q1->ve[3] * q2->ve[0]) + (q1->ve[0] * q2->ve[3]) + (q1->ve[1] * q2->ve[2]) - (q1->ve[2] * q2->ve[1]);
	temp->ve[1] = (q1->ve[3] * q2->ve[1]) + (q1->ve[1] * q2->ve[3]) + (q1->ve[2] * q2->ve[0]) - (q1->ve[0] * q2->ve[2]);
	temp->ve[2] = (q1->ve[3] * q2->ve[2]) + (q1->ve[2] * q2->ve[3]) + (q1->ve[0] * q2->ve[1]) - (q1->ve[1] * q2->ve[0]);
    
    for (i=0; i<4;i++){
         res->ve[i] = temp->ve[i];
    }
    
    v_free(temp);
        
}
コード例 #20
0
ファイル: iternsym.c プロジェクト: Rainwin2015/C
VEC	*iter_cgs(ITER *ip, VEC *r0)
#endif
{
    STATIC VEC  *p = VNULL, *q = VNULL, *r = VNULL, *u = VNULL;
    STATIC VEC  *v = VNULL, *z = VNULL;
    VEC  *tmp;
    Real	alpha, beta, nres, rho, old_rho, sigma, inner;

    if (ip == INULL)
        error(E_NULL,"iter_cgs");
    if (!ip->Ax || !ip->b || !r0)
        error(E_NULL,"iter_cgs");
    if ( ip->x == ip->b )
        error(E_INSITU,"iter_cgs");
    if (!ip->stop_crit)
        error(E_NULL,"iter_cgs");
    if ( r0->dim != ip->b->dim )
        error(E_SIZES,"iter_cgs");

    if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;

    p = v_resize(p,ip->b->dim);
    q = v_resize(q,ip->b->dim);
    r = v_resize(r,ip->b->dim);
    u = v_resize(u,ip->b->dim);
    v = v_resize(v,ip->b->dim);

    MEM_STAT_REG(p,TYPE_VEC);
    MEM_STAT_REG(q,TYPE_VEC);
    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(v,TYPE_VEC);

    if (ip->Bx) {
        z = v_resize(z,ip->b->dim);
        MEM_STAT_REG(z,TYPE_VEC);
    }

    if (ip->x != VNULL) {
        if (ip->x->dim != ip->b->dim)
            error(E_SIZES,"iter_cgs");
        ip->Ax(ip->A_par,ip->x,v);    		/* v = A*x */
        if (ip->Bx) {
            v_sub(ip->b,v,v);			/* v = b - A*x */
            (ip->Bx)(ip->B_par,v,r);		/* r = B*(b-A*x) */
        }
        else v_sub(ip->b,v,r);			/* r = b-A*x */
    }
    else {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);		/* x == 0 */
        ip->shared_x = FALSE;
        if (ip->Bx) (ip->Bx)(ip->B_par,ip->b,r);    /* r = B*b */
        else v_copy(ip->b,r);                       /* r = b */
    }

    v_zero(p);
    v_zero(q);
    old_rho = 1.0;

    for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {

        inner = in_prod(r,r);
        nres = sqrt(fabs(inner));
        if (ip->steps == 0) ip->init_res = nres;

        if (ip->info) ip->info(ip,nres,r,VNULL);
        if ( ip->stop_crit(ip,nres,r,VNULL) ) break;

        rho = in_prod(r0,r);
        if ( old_rho == 0.0 )
            error(E_BREAKDOWN,"iter_cgs");
        beta = rho/old_rho;
        v_mltadd(r,q,beta,u);
        v_mltadd(q,p,beta,v);
        v_mltadd(u,v,beta,p);

        (ip->Ax)(ip->A_par,p,q);
        if (ip->Bx) {
            (ip->Bx)(ip->B_par,q,z);
            tmp = z;
        }
        else tmp = q;

        sigma = in_prod(r0,tmp);
        if ( sigma == 0.0 )
            error(E_BREAKDOWN,"iter_cgs");
        alpha = rho/sigma;
        v_mltadd(u,tmp,-alpha,q);
        v_add(u,q,v);

        (ip->Ax)(ip->A_par,v,u);
        if (ip->Bx) {
            (ip->Bx)(ip->B_par,u,z);
            tmp = z;
        }
        else tmp = u;

        v_mltadd(r,tmp,-alpha,r);
        v_mltadd(ip->x,v,alpha,ip->x);

        old_rho = rho;
    }

#ifdef THREADSAFE
    V_FREE(p);
    V_FREE(q);
    V_FREE(r);
    V_FREE(u);
    V_FREE(v);
    V_FREE(z);
#endif

    return ip->x;
}
コード例 #21
0
ファイル: iternsym.c プロジェクト: Rainwin2015/C
VEC	*iter_lsqr(ITER *ip)
#endif
{
    STATIC VEC	*u = VNULL, *v = VNULL, *w = VNULL, *tmp = VNULL;
    Real	alpha, beta, phi, phi_bar;
    Real rho, rho_bar, rho_max, theta, nres;
    Real	s, c;	/* for Givens' rotations */
    int  m, n;

    if ( ! ip || ! ip->b || !ip->Ax || !ip->ATx )
        error(E_NULL,"iter_lsqr");
    if ( ip->x == ip->b )
        error(E_INSITU,"iter_lsqr");
    if (!ip->stop_crit || !ip->x)
        error(E_NULL,"iter_lsqr");

    if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;

    m = ip->b->dim;
    n = ip->x->dim;

    u = v_resize(u,(unsigned int)m);
    v = v_resize(v,(unsigned int)n);
    w = v_resize(w,(unsigned int)n);
    tmp = v_resize(tmp,(unsigned int)n);

    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(v,TYPE_VEC);
    MEM_STAT_REG(w,TYPE_VEC);
    MEM_STAT_REG(tmp,TYPE_VEC);

    if (ip->x != VNULL) {
        ip->Ax(ip->A_par,ip->x,u);    		/* u = A*x */
        v_sub(ip->b,u,u);				/* u = b-A*x */
    }
    else {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
        v_copy(ip->b,u);                       /* u = b */
    }

    beta = v_norm2(u);
    if ( beta == 0.0 ) return ip->x;

    sv_mlt(1.0/beta,u,u);
    (ip->ATx)(ip->AT_par,u,v);
    alpha = v_norm2(v);
    if ( alpha == 0.0 ) return ip->x;

    sv_mlt(1.0/alpha,v,v);
    v_copy(v,w);
    phi_bar = beta;
    rho_bar = alpha;

    rho_max = 1.0;
    for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) {

        tmp = v_resize(tmp,m);
        (ip->Ax)(ip->A_par,v,tmp);

        v_mltadd(tmp,u,-alpha,u);
        beta = v_norm2(u);
        sv_mlt(1.0/beta,u,u);

        tmp = v_resize(tmp,n);
        (ip->ATx)(ip->AT_par,u,tmp);
        v_mltadd(tmp,v,-beta,v);
        alpha = v_norm2(v);
        sv_mlt(1.0/alpha,v,v);

        rho = sqrt(rho_bar*rho_bar+beta*beta);
        if ( rho > rho_max )
            rho_max = rho;
        c   = rho_bar/rho;
        s   = beta/rho;
        theta   =  s*alpha;
        rho_bar = -c*alpha;
        phi     =  c*phi_bar;
        phi_bar =  s*phi_bar;

        /* update ip->x & w */
        if ( rho == 0.0 )
            error(E_BREAKDOWN,"iter_lsqr");
        v_mltadd(ip->x,w,phi/rho,ip->x);
        v_mltadd(v,w,-theta/rho,w);

        nres = fabs(phi_bar*alpha*c)*rho_max;

        if (ip->info) ip->info(ip,nres,w,VNULL);
        if (ip->steps == 0) ip->init_res = nres;
        if ( ip->stop_crit(ip,nres,w,VNULL) ) break;
    }

#ifdef THREADSAFE
    V_FREE(u);
    V_FREE(v);
    V_FREE(w);
    V_FREE(tmp);
#endif

    return ip->x;
}
コード例 #22
0
ファイル: iternsym.c プロジェクト: Rainwin2015/C
VEC  *iter_cgne(ITER *ip)
#endif
{
    STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
    Real	alpha, beta, inner, old_inner, nres;
    VEC *rr1;   /* pointer only */

    if (ip == INULL)
        error(E_NULL,"iter_cgne");
    if (!ip->Ax || ! ip->ATx || !ip->b)
        error(E_NULL,"iter_cgne");
    if ( ip->x == ip->b )
        error(E_INSITU,"iter_cgne");
    if (!ip->stop_crit)
        error(E_NULL,"iter_cgne");

    if ( ip->eps <= 0.0 ) ip->eps = MACHEPS;

    r = v_resize(r,ip->b->dim);
    p = v_resize(p,ip->b->dim);
    q = v_resize(q,ip->b->dim);

    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(p,TYPE_VEC);
    MEM_STAT_REG(q,TYPE_VEC);

    z = v_resize(z,ip->b->dim);
    MEM_STAT_REG(z,TYPE_VEC);

    if (ip->x) {
        if (ip->x->dim != ip->b->dim)
            error(E_SIZES,"iter_cgne");
        ip->Ax(ip->A_par,ip->x,p);    		/* p = A*x */
        v_sub(ip->b,p,z);		 		/* z = b - A*x */
    }
    else {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
        v_copy(ip->b,z);
    }
    rr1 = z;
    if (ip->Bx) {
        (ip->Bx)(ip->B_par,rr1,p);
        rr1 = p;
    }
    (ip->ATx)(ip->AT_par,rr1,r);		/* r = A^T*B*(b-A*x)  */


    old_inner = 0.0;
    for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
    {
        rr1 = r;
        if ( ip->Bx ) {
            (ip->Bx)(ip->B_par,r,z);		/* rr = B*r */
            rr1 = z;
        }

        inner = in_prod(r,rr1);
        nres = sqrt(fabs(inner));
        if (ip->info) ip->info(ip,nres,r,rr1);
        if (ip->steps == 0) ip->init_res = nres;
        if ( ip->stop_crit(ip,nres,r,rr1) ) break;

        if ( ip->steps )	/* if ( ip->steps > 0 ) ... */
        {
            beta = inner/old_inner;
            p = v_mltadd(rr1,p,beta,p);
        }
        else		/* if ( ip->steps == 0 ) ... */
        {
            beta = 0.0;
            p = v_copy(rr1,p);
            old_inner = 0.0;
        }
        (ip->Ax)(ip->A_par,p,q);     /* q = A*p */
        if (ip->Bx) {
            (ip->Bx)(ip->B_par,q,z);
            (ip->ATx)(ip->AT_par,z,q);
            rr1 = q;			/* q = A^T*B*A*p */
        }
        else {
            (ip->ATx)(ip->AT_par,q,z);	/* z = A^T*A*p */
            rr1 = z;
        }

        alpha = inner/in_prod(rr1,p);
        v_mltadd(ip->x,p,alpha,ip->x);
        v_mltadd(r,rr1,-alpha,r);
        old_inner = inner;
    }

#ifdef THREADSAFE
    V_FREE(r);
    V_FREE(p);
    V_FREE(q);
    V_FREE(z);
#endif

    return ip->x;
}
コード例 #23
0
ファイル: detect_main.c プロジェクト: Goursat/rodinia
// Main
int main(int argc, char ** argv) {

	// Choose the best GPU in case there are multiple available
	choose_GPU();

	// Keep track of the start time of the program
	long long program_start_time = get_time();
	
	if (argc !=3){
	fprintf(stderr, "usage: %s <input file> <number of frames to process>", argv[0]);
	exit(1);
	}
	
	// Let the user specify the number of frames to process
	int num_frames = atoi(argv[2]);
	
	// Open video file
	char *video_file_name = argv[1];
	
	avi_t *cell_file = AVI_open_input_file(video_file_name, 1);
	if (cell_file == NULL)	{
		AVI_print_error("Error with AVI_open_input_file");
		return -1;
	}
	
	int i, j, *crow, *ccol, pair_counter = 0, x_result_len = 0, Iter = 20, ns = 4, k_count = 0, n;
	MAT *cellx, *celly, *A;
	double *GICOV_spots, *t, *G, *x_result, *y_result, *V, *QAX_CENTERS, *QAY_CENTERS;
	double threshold = 1.8, radius = 10.0, delta = 3.0, dt = 0.01, b = 5.0;
	
	// Extract a cropped version of the first frame from the video file
	MAT *image_chopped = get_frame(cell_file, 0, 1, 0);
	printf("Detecting cells in frame 0\n");
	
	// Get gradient matrices in x and y directions
	MAT *grad_x = gradient_x(image_chopped);
	MAT *grad_y = gradient_y(image_chopped);
	
	// Allocate for gicov_mem and strel
	gicov_mem = (float*) malloc(sizeof(float) * grad_x->m * grad_y->n);
	strel = (float*) malloc(sizeof(float) * strel_m * strel_n);

	m_free(image_chopped);

	int grad_m = grad_x->m;
	int grad_n = grad_y->n;
	
#pragma acc data create(sin_angle,cos_angle,theta,tX,tY) \
	create(gicov_mem[0:grad_x->m*grad_y->n])
{
	// Precomputed constants on GPU
	compute_constants();

	// Get GICOV matrices corresponding to image gradients
	long long GICOV_start_time = get_time();
	MAT *gicov = GICOV(grad_x, grad_y);
	long long GICOV_end_time = get_time();

	// Dilate the GICOV matrices
	long long dilate_start_time = get_time();
	MAT *img_dilated = dilate(gicov);
	long long dilate_end_time = get_time();
} /* end acc data */
	
	// Find possible matches for cell centers based on GICOV and record the rows/columns in which they are found
	pair_counter = 0;
	crow = (int *) malloc(gicov->m * gicov->n * sizeof(int));
	ccol = (int *) malloc(gicov->m * gicov->n * sizeof(int));
	for(i = 0; i < gicov->m; i++) {
		for(j = 0; j < gicov->n; j++) {
			if(!double_eq(m_get_val(gicov,i,j), 0.0) && double_eq(m_get_val(img_dilated,i,j), m_get_val(gicov,i,j)))
			{
				crow[pair_counter]=i;
				ccol[pair_counter]=j;
				pair_counter++;
			}
		}
	}

	GICOV_spots = (double *) malloc(sizeof(double) * pair_counter);
	for(i = 0; i < pair_counter; i++)
		GICOV_spots[i] = sqrt(m_get_val(gicov, crow[i], ccol[i]));
	
	G = (double *) calloc(pair_counter, sizeof(double));
	x_result = (double *) calloc(pair_counter, sizeof(double));
	y_result = (double *) calloc(pair_counter, sizeof(double));
	
	x_result_len = 0;
	for (i = 0; i < pair_counter; i++) {
		if ((crow[i] > 29) && (crow[i] < BOTTOM - TOP + 39)) {
			x_result[x_result_len] = ccol[i];
			y_result[x_result_len] = crow[i] - 40;
			G[x_result_len] = GICOV_spots[i];
			x_result_len++;
		}
	}
	
	// Make an array t which holds each "time step" for the possible cells
	t = (double *) malloc(sizeof(double) * 36);
	for (i = 0; i < 36; i++) {
		t[i] = (double)i * 2.0 * PI / 36.0;
	}
	
	// Store cell boundaries (as simple circles) for all cells
	cellx = m_get(x_result_len, 36);
	celly = m_get(x_result_len, 36);
	for(i = 0; i < x_result_len; i++) {
		for(j = 0; j < 36; j++) {
			m_set_val(cellx, i, j, x_result[i] + radius * cos(t[j]));
			m_set_val(celly, i, j, y_result[i] + radius * sin(t[j]));
		}
	}
	
	A = TMatrix(9,4);
	V = (double *) malloc(sizeof(double) * pair_counter);
	QAX_CENTERS = (double * )malloc(sizeof(double) * pair_counter);
	QAY_CENTERS = (double *) malloc(sizeof(double) * pair_counter);
	memset(V, 0, sizeof(double) * pair_counter);
	memset(QAX_CENTERS, 0, sizeof(double) * pair_counter);
	memset(QAY_CENTERS, 0, sizeof(double) * pair_counter);

	// For all possible results, find the ones that are feasibly leukocytes and store their centers
	k_count = 0;
	for (n = 0; n < x_result_len; n++) {
		if ((G[n] < -1 * threshold) || G[n] > threshold) {
			MAT * x, *y;
			VEC * x_row, * y_row;
			x = m_get(1, 36);
			y = m_get(1, 36);

			x_row = v_get(36);
			y_row = v_get(36);

			// Get current values of possible cells from cellx/celly matrices
			x_row = get_row(cellx, n, x_row);
			y_row = get_row(celly, n, y_row);
			uniformseg(x_row, y_row, x, y);

			// Make sure that the possible leukocytes are not too close to the edge of the frame
			if ((m_min(x) > b) && (m_min(y) > b) && (m_max(x) < cell_file->width - b) && (m_max(y) < cell_file->height - b)) {
				MAT * Cx, * Cy, *Cy_temp, * Ix1, * Iy1;
				VEC  *Xs, *Ys, *W, *Nx, *Ny, *X, *Y;
				Cx = m_get(1, 36);
				Cy = m_get(1, 36);
				Cx = mmtr_mlt(A, x, Cx);
				Cy = mmtr_mlt(A, y, Cy);
				
				Cy_temp = m_get(Cy->m, Cy->n);
				
				for (i = 0; i < 9; i++)
					m_set_val(Cy, i, 0, m_get_val(Cy, i, 0) + 40.0);
					
				// Iteratively refine the snake/spline
				for (i = 0; i < Iter; i++) {
					int typeofcell;
					
					if(G[n] > 0.0) typeofcell = 0;
					else typeofcell = 1;
					
					splineenergyform01(Cx, Cy, grad_x, grad_y, ns, delta, 2.0 * dt, typeofcell);
				}
				
				X = getsampling(Cx, ns);
				for (i = 0; i < Cy->m; i++)
					m_set_val(Cy_temp, i, 0, m_get_val(Cy, i, 0) - 40.0);
				Y = getsampling(Cy_temp, ns);
				
				Ix1 = linear_interp2(grad_x, X, Y);
				Iy1 = linear_interp2(grad_x, X, Y);
				Xs = getfdriv(Cx, ns);
				Ys = getfdriv(Cy, ns);
				
				Nx = v_get(Ys->dim);
				for (i = 0; i < Ys->dim; i++)
					v_set_val(Nx, i, v_get_val(Ys, i) / sqrt(v_get_val(Xs, i)*v_get_val(Xs, i) + v_get_val(Ys, i)*v_get_val(Ys, i)));
					
				Ny = v_get(Xs->dim);
				for (i = 0; i < Xs->dim; i++)
					v_set_val(Ny, i, -1.0 * v_get_val(Xs, i) / sqrt(v_get_val(Xs, i)*v_get_val(Xs, i) + v_get_val(Ys, i)*v_get_val(Ys, i)));
					
				W = v_get(Nx->dim);
				for (i = 0; i < Nx->dim; i++)
					v_set_val(W, i, m_get_val(Ix1, 0, i) * v_get_val(Nx, i) + m_get_val(Iy1, 0, i) * v_get_val(Ny, i));
					
				V[n] = mean(W) / std_dev(W);
				
				// Find the cell centers by computing the means of X and Y values for all snaxels of the spline contour
				QAX_CENTERS[k_count] = mean(X);
				QAY_CENTERS[k_count] = mean(Y) + TOP;
				
				k_count++;
				
				// Free memory
				v_free(W);
				v_free(Ny);
				v_free(Nx);
				v_free(Ys);
				v_free(Xs);
				m_free(Iy1);
				m_free(Ix1);
				v_free(Y);
				v_free(X);
				m_free(Cy_temp);
				m_free(Cy);
				m_free(Cx);				
			}

			// Free memory
			v_free(y_row);
			v_free(x_row);
			m_free(y);
			m_free(x);
		}
	}
	
	// Free memory
	free(gicov_mem);
	free(strel);
	free(V);
	free(ccol);
	free(crow);
	free(GICOV_spots);
	free(t);
	free(G);
	free(x_result);
	free(y_result);
	m_free(A);
	m_free(celly);
	m_free(cellx);
	m_free(img_dilated);
	m_free(gicov);
	m_free(grad_y);
	m_free(grad_x);
	
	// Report the total number of cells detected
	printf("Cells detected: %d\n\n", k_count);
	
	// Report the breakdown of the detection runtime
	printf("Detection runtime\n");
	printf("-----------------\n");
	printf("GICOV computation: %.5f seconds\n", ((float) (GICOV_end_time - GICOV_start_time)) / (1000*1000));
	printf("   GICOV dilation: %.5f seconds\n", ((float) (dilate_end_time - dilate_start_time)) / (1000*1000));
	printf("            Total: %.5f seconds\n", ((float) (get_time() - program_start_time)) / (1000*1000));
	
	// Now that the cells have been detected in the first frame,
	//  track the ellipses through subsequent frames
	if (num_frames > 1) printf("\nTracking cells across %d frames\n", num_frames);
	else                printf("\nTracking cells across 1 frame\n");
	long long tracking_start_time = get_time();
	int num_snaxels = 20;
	ellipsetrack(cell_file, QAX_CENTERS, QAY_CENTERS, k_count, radius, num_snaxels, num_frames);
	printf("           Total: %.5f seconds\n", ((float) (get_time() - tracking_start_time)) / (float) (1000*1000*num_frames));	
	
	// Report total program execution time
    printf("\nTotal application run time: %.5f seconds\n", ((float) (get_time() - program_start_time)) / (1000*1000));

	return 0;
}
コード例 #24
0
ファイル: algo.c プロジェクト: huyna/sftc
//FIXME: add LOCALFUNC? (OR GLOBAL?)
BYTE* RECURSIVE_SUBROUTINE(BYTE t, list *cur_guess, list *guesses, list_node *first_guess)
{
	if ( t == key_length)
	{
		unsigned char* key=NULL;
		// generate key from the cur_guess (push guesses into a matrix and use some matrix solving library?)
		MAT* A;
		VEC *x,*b;
		PERM* pivot;
		A=m_get(key_length,key_length);
		b=v_get(key_length);
		x=v_get(key_length);
		int c=0,r=0;
		for(list_node* iter=list_head(cur_guess); iter != NULL && r<key_length; iter=list_node_next(iter)){
			for(c=list_node_data_ptr(iter,byte_sum_guess_t)->i1;
				c <= list_node_data_ptr(iter,byte_sum_guess_t)->i2;
				++c){
				A->me[r][c]=1;
			}
			b->ve[r]=list_node_data_ptr(iter,byte_sum_guess_t)->value;
			++r;
		}
		//Calculate matrix determinant
		SQRMATRIX A_det;
		SQRMATRIX_CreateMatrix(&A_det,key_length);
		for(r=0;r<key_length;++r){
			for(c=0;c<key_length;++c){
				A_det.array[r][c]=A->me[r][c];
			}
		}
		int det;
		det=SQRMATRIX_CalcDeterminant(&A_det);
		//TODO: return this later
		SQRMATRIX_DestroyMatrix(&A_det);
		if(det==0){//If determinant is 0 continue to next guess
			++count_bad_matrix;
#ifdef __DEBUG
			//SQRMATRIX_DisplayMatrix(&A_det);
			v_output(b);
#endif
			DEBUG_PRINT("Matrix determinant is 0\n");
		}else{
			++count_guesses;
			pivot = px_get(A->m);
			LUfactor(A,pivot);

			x=LUsolve(A,pivot,b,VNULL);
			PX_FREE(pivot);
			//test key (use our RC4 impl)
			key=(unsigned char*)malloc(sizeof(unsigned char)*key_length);
			for(int i=0;i<key_length;++i){
				key[i]=x->ve[i];
			}

			int res=rc4_test_key(key);
			if(res){
				printf("MAZAL TOV! we got the right key.\n");
				print_key(key);
				printf("\n");
			}else{
				printf("Tried key: ");
				print_key(key);
				printf("\n");
				free(key);key=NULL;
			}
		}
		//release matrix vars
		M_FREE(A);
		V_FREE(x);
		V_FREE(b);
		return key;
	}

	byte_sum_guess_t cur;
	//list *new_list_head=guesses;


	//TODO: (later) add a for loop running along the "lambeda_t" values here, for the initial impl we'll try the best guess
	//for ()
	//{
	for(int i=0; i<LAMBDA_T; ++i){
		cur = *(list_node_data_ptr(first_guess, byte_sum_guess_t));
		list_node *biatch = list_add_head(cur_guess, &cur);
		BYTE* res=RECURSIVE_SUBROUTINE(t+1, cur_guess, guesses, list_node_next(first_guess));
		if(res!=NULL){
			return res;
		}
		list_del(cur_guess,biatch);
		first_guess = list_node_next(first_guess);
	}
	return NULL;

	//TODO: do something to find the next guess and link it to the current guess
	//when I say something I mean find best guess (i.e best weight) of all the guesses that give us new information
	//(i.e not linearily dependent in our byte values and sums matrix that can be deduced from the cur_guess)
	//see also the note above (intuition)

	//IMPORTANT!
	//explaination how cur_guess is a matrix: each entry in cur_guess contains a list of bytes that are part of the sum and a
	//guess as to the value of the sum. if this matrix is solvable then solving it should give us a value for each byte of the
	//key thus the entire key
	//note: we probably should change cur_guess to a smarter database (for example a (L)x(L+1) matrix as an array?) but we
	//need to consider that we need to keep the ability to backtrack without making it too expensive



	//TODO: if weight of the guess is too small -> return FAIL (section 4.6)


	//These are based on section 4.4
	//correct suggestions (this can be done later, basic alg should work without it)
	//adjust weights (this can be done later, basic alg should work without it)

	//merge counters (section 4.2), also skip for initial impl? need to check


	//go to next iteration in the recurtion
    //RECURSIVE_SUBROUTINE(t+1, cur_guess, );

	//} end of for

}
コード例 #25
0
ファイル: mesch.c プロジェクト: Bhattiasif/gdis
gpointer mesch_vec_new(gint i)
{
return(v_get(i));
}
コード例 #26
0
ファイル: iternsym.c プロジェクト: Rainwin2015/C
VEC	*iter_gmres(ITER *ip)
#endif
{
    STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL;
    STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL;
    STATIC MAT *Q = MNULL, *R = MNULL;
    VEC *rr, v, v1;   /* additional pointers (not real vectors) */
    int	i,j, done;
    Real	nres;
    /*   Real last_h;  */

    if (ip == INULL)
        error(E_NULL,"iter_gmres");
    if ( ! ip->Ax || ! ip->b )
        error(E_NULL,"iter_gmres");
    if ( ! ip->stop_crit )
        error(E_NULL,"iter_gmres");
    if ( ip->k <= 0 )
        error(E_BOUNDS,"iter_gmres");
    if (ip->x != VNULL && ip->x->dim != ip->b->dim)
        error(E_SIZES,"iter_gmres");
    if (ip->eps <= 0.0) ip->eps = MACHEPS;

    r = v_resize(r,ip->k+1);
    u = v_resize(u,ip->b->dim);
    rhs = v_resize(rhs,ip->k+1);
    givs = v_resize(givs,ip->k);  /* Givens rotations */
    givc = v_resize(givc,ip->k);

    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(rhs,TYPE_VEC);
    MEM_STAT_REG(givs,TYPE_VEC);
    MEM_STAT_REG(givc,TYPE_VEC);

    R = m_resize(R,ip->k+1,ip->k);
    Q = m_resize(Q,ip->k,ip->b->dim);
    MEM_STAT_REG(R,TYPE_MAT);
    MEM_STAT_REG(Q,TYPE_MAT);

    if (ip->x == VNULL) {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
    }

    v.dim = v.max_dim = ip->b->dim;      /* v and v1 are pointers to rows */
    v1.dim = v1.max_dim = ip->b->dim;  	/* of matrix Q */

    if (ip->Bx != (Fun_Ax)NULL) {    /* if precondition is defined */
        z = v_resize(z,ip->b->dim);
        MEM_STAT_REG(z,TYPE_VEC);
    }

    done = FALSE;
    for (ip->steps = 0; ip->steps < ip->limit; ) {

        /* restart */

        ip->Ax(ip->A_par,ip->x,u);    		/* u = A*x */
        v_sub(ip->b,u,u);		 		/* u = b - A*x */
        rr = u;				/* rr is a pointer only */

        if (ip->Bx) {
            (ip->Bx)(ip->B_par,u,z);            /* tmp = B*(b-A*x)  */
            rr = z;
        }

        nres = v_norm2(rr);
        if (ip->steps == 0) {
            if (ip->info) ip->info(ip,nres,VNULL,VNULL);
            ip->init_res = nres;
        }

        if ( nres == 0.0 ) {
            done = TRUE;
            break;
        }

        v.ve = Q->me[0];
        sv_mlt(1.0/nres,rr,&v);

        v_zero(r);
        v_zero(rhs);
        rhs->ve[0] = nres;

        for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) {
            ip->steps++;
            v.ve = Q->me[i];
            (ip->Ax)(ip->A_par,&v,u);
            rr = u;
            if (ip->Bx) {
                (ip->Bx)(ip->B_par,u,z);
                rr = z;
            }

            if (i < ip->k - 1) {
                v1.ve = Q->me[i+1];
                v_copy(rr,&v1);
                for (j = 0; j <= i; j++) {
                    v.ve = Q->me[j];
                    /* r->ve[j] = in_prod(&v,rr); */
                    /* modified Gram-Schmidt algorithm */
                    r->ve[j] = in_prod(&v,&v1);
                    v_mltadd(&v1,&v,-r->ve[j],&v1);
                }

                r->ve[i+1] = nres = v_norm2(&v1);
                if (nres <= MACHEPS*ip->init_res) {
                    for (j = 0; j < i; j++)
                        rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
                    set_col(R,i,r);
                    done = TRUE;
                    break;
                }
                sv_mlt(1.0/nres,&v1,&v1);
            }
            else {  /* i == ip->k - 1 */
                /* Q->me[ip->k] need not be computed */

                for (j = 0; j <= i; j++) {
                    v.ve = Q->me[j];
                    r->ve[j] = in_prod(&v,rr);
                }

                nres = in_prod(rr,rr) - in_prod(r,r);
                if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
                    for (j = 0; j < i; j++)
                        rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
                    set_col(R,i,r);
                    done = TRUE;
                    break;
                }
                if (nres < 0.0) { /* do restart */
                    i--;
                    ip->steps--;
                    break;
                }
                r->ve[i+1] = sqrt(nres);
            }

            /* QR update */

            /* last_h = r->ve[i+1]; */ /* for test only */
            for (j = 0; j < i; j++)
                rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
            givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]);
            rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r);
            rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs);

            set_col(R,i,r);

            nres = fabs((double) rhs->ve[i+1]);
            if (ip->info) ip->info(ip,nres,VNULL,VNULL);
            if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) {
                done = TRUE;
                break;
            }
        }

        /* use ixi submatrix of R */

        if (i >= ip->k) i = ip->k - 1;

        R = m_resize(R,i+1,i+1);
        rhs = v_resize(rhs,i+1);

        /* test only */
        /* test_gmres(ip,i,Q,R,givc,givs,last_h);  */

        Usolve(R,rhs,rhs,0.0); 	 /* solve a system: R*x = rhs */

        /* new approximation */

        for (j = 0; j <= i; j++) {
            v.ve = Q->me[j];
            v_mltadd(ip->x,&v,rhs->ve[j],ip->x);
        }

        if (done) break;

        /* back to old dimensions */

        rhs = v_resize(rhs,ip->k+1);
        R = m_resize(R,ip->k+1,ip->k);

    }

#ifdef THREADSAFE
    V_FREE(u);
    V_FREE(r);
    V_FREE(rhs);
    V_FREE(givs);
    V_FREE(givc);
    V_FREE(z);
    M_FREE(Q);
    M_FREE(R);
#endif

    return ip->x;
}
コード例 #27
0
ファイル: lufactor.c プロジェクト: trehomudia/skotina
/* LUfactor -- gaussian elimination with scaled partial pivoting
-- Note: returns LU matrix which is A */
MAT	*LUfactor(MAT *A, PERM *pivot)
{
  unsigned int	i, j, m, n;
  unsigned int	k, k_max;
  int i_max;
  char MatrixTempBuffer[ 1000 ];
  MatrixReal	**A_v, *A_piv, *A_row;
  MatrixReal	max1, temp, tiny;
  VEC	*scale = VNULL;

  if ( A==(MAT *)NULL || pivot==(PERM *)NULL ){
    error(E_NULL,"LUfactor");
  }
  if ( pivot->size != A->m )
    error(E_SIZES,"LUfactor");
  m = A->m;	n = A->n;

  if( SET_VEC_SIZE( A->m ) <1000 )
    vec_get( &scale, (void *)MatrixTempBuffer, A->m );
  else
    scale = v_get( A->m );

  //MEM_STAT_REG(scale,TYPE_VEC);
  A_v = A->me;

  tiny = (MatrixReal)(10.0/HUGE_VAL);

  /* initialise pivot with identity permutation */
  for ( i=0; i<m; i++ )
    pivot->pe[i] = i;

  /* set scale parameters */
  for ( i=0; i<m; i++ )
  {
    max1 = 0.0;
    for ( j=0; j<n; j++ )
    {
      temp = (MatrixReal)fabs(A_v[i][j]);
      max1 = mat_max(max1,temp);
    }
    scale->ve[i] = max1;
  }

  /* main loop */
  k_max = mat_min(m,n)-1;
  for ( k=0; k<k_max; k++ )
  {
    /* find best pivot row */
    max1 = 0.0;	i_max = -1;
    for ( i=k; i<m; i++ )
      if ( fabs(scale->ve[i]) >= tiny*fabs(A_v[i][k]) )
      {
        temp = (MatrixReal)fabs(A_v[i][k])/scale->ve[i];
        if ( temp > max1 )
        { max1 = temp;	i_max = i;	}
      }

      /* if no pivot then ignore column k... */
      if ( i_max == -1 )
      {
        /* set pivot entry A[k][k] exactly to zero,
        rather than just "small" */
        A_v[k][k] = 0.0;
        continue;
      }

      /* do we pivot ? */
      if ( i_max != (int)k )	/* yes we do... */
      {
        px_transp(pivot,i_max,k);
        for ( j=0; j<n; j++ )
        {
          temp = A_v[i_max][j];
          A_v[i_max][j] = A_v[k][j];
          A_v[k][j] = temp;
        }
      }

      /* row operations */
      for ( i=k+1; i<m; i++ )	/* for each row do... */
      {	/* Note: divide by zero should never happen */
        temp = A_v[i][k] = A_v[i][k]/A_v[k][k];
        A_piv = &(A_v[k][k+1]);
        A_row = &(A_v[i][k+1]);
        if ( k+1 < n )
          __mltadd__(A_row,A_piv,-temp,(int)(n-(k+1)));
      }

  }

  if( scale != (VEC *)MatrixTempBuffer ) // память выделялась, надо освободить
    V_FREE(scale);

  return A;
}
コード例 #28
0
void ellipseevolve(MAT *f, double *xc0, double *yc0, double *r0, double *t, int Np, double Er, double Ey) {
    /*
    % ELLIPSEEVOLVE evolves a parametric snake according
    %  to some energy constraints.
    %
    % INPUTS:
    %   f............potential surface
    %   xc0,yc0......initial center position
    %   r0,t.........initial radii & angle vectors (with Np elements each)
    %   Np...........number of snaxel points per snake
    %   Er...........expected radius
    %   Ey...........expected y position
    %
    % OUTPUTS
    %   xc0,yc0.......final center position
    %   r0...........final radii
    %
    % Matlab code written by: DREW GILLIAM (based on work by GANG DONG /
    %                                                        NILANJAN RAY)
    % Ported to C by: MICHAEL BOYER
    */


    // Constants
    double deltax = 0.2;
    double deltay = 0.2;
    double deltar = 0.2;
    double converge = 0.1;
    double lambdaedge = 1;
    double lambdasize = 0.2;
    double lambdapath = 0.05;
    int iterations = 1000;      // maximum number of iterations

    int i, j;

    // Initialize variables
    double xc = *xc0;
    double yc = *yc0;
    double *r = (double *) malloc(sizeof(double) * Np);
    for (i = 0; i < Np; i++) r[i] = r0[i];

    // Compute the x- and y-gradients of the MGVF matrix
    MAT *fx = gradient_x(f);
    MAT *fy = gradient_y(f);

    // Normalize the gradients
    int fh = f->m, fw = f->n;
    for (i = 0; i < fh; i++) {
        for (j = 0; j < fw; j++) {
            double temp_x = m_get_val(fx, i, j);
            double temp_y = m_get_val(fy, i, j);
            double fmag = sqrt((temp_x * temp_x) + (temp_y * temp_y));
            m_set_val(fx, i, j, temp_x / fmag);
            m_set_val(fy, i, j, temp_y / fmag);
        }
    }

    double *r_old = (double *) malloc(sizeof(double) * Np);
    VEC *x = v_get(Np);
    VEC *y = v_get(Np);


    // Evolve the snake
    int iter = 0;
    double snakediff = 1.0;
    while (iter < iterations && snakediff > converge) {

        // Save the values from the previous iteration
        double xc_old = xc, yc_old = yc;
        for (i = 0; i < Np; i++) {
            r_old[i] = r[i];
        }

        // Compute the locations of the snaxels
        for (i = 0; i < Np; i++) {
            v_set_val(x, i, xc + r[i] * cos(t[i]));
            v_set_val(y, i, yc + r[i] * sin(t[i]));
        }

        // See if any of the points in the snake are off the edge of the image
        double min_x = v_get_val(x, 0), max_x = v_get_val(x, 0);
        double min_y = v_get_val(y, 0), max_y = v_get_val(y, 0);
        for (i = 1; i < Np; i++) {
            double x_i = v_get_val(x, i);
            if (x_i < min_x) min_x = x_i;
            else if (x_i > max_x) max_x = x_i;
            double y_i = v_get_val(y, i);
            if (y_i < min_y) min_y = y_i;
            else if (y_i > max_y) max_y = y_i;
        }
        if (min_x < 0.0 || max_x > (double) fw - 1.0 || min_y < 0 || max_y > (double) fh - 1.0) break;


        // Compute the length of the snake
        double L = 0.0;
        for (i = 0; i < Np - 1; i++) {
            double diff_x = v_get_val(x, i + 1) - v_get_val(x, i);
            double diff_y = v_get_val(y, i + 1) - v_get_val(y, i);
            L += sqrt((diff_x * diff_x) + (diff_y * diff_y));
        }
        double diff_x = v_get_val(x, 0) - v_get_val(x, Np - 1);
        double diff_y = v_get_val(y, 0) - v_get_val(y, Np - 1);
        L += sqrt((diff_x * diff_x) + (diff_y * diff_y));

        // Compute the potential surface at each snaxel
        MAT *vf  = linear_interp2(f,  x, y);
        MAT *vfx = linear_interp2(fx, x, y);
        MAT *vfy = linear_interp2(fy, x, y);

        // Compute the average potential surface around the snake
        double vfmean  = sum_m(vf ) / L;
        double vfxmean = sum_m(vfx) / L;
        double vfymean = sum_m(vfy) / L;

        // Compute the radial potential surface
        int m = vf->m, n = vf->n;
        MAT *vfr = m_get(m, n);
        for (i = 0; i < n; i++) {
            double vf_val  = m_get_val(vf,  0, i);
            double vfx_val = m_get_val(vfx, 0, i);
            double vfy_val = m_get_val(vfy, 0, i);
            double x_val = v_get_val(x, i);
            double y_val = v_get_val(y, i);
            double new_val = (vf_val + vfx_val * (x_val - xc) + vfy_val * (y_val - yc) - vfmean) / L;
            m_set_val(vfr, 0, i, new_val);
        }

        // Update the snake center and snaxels
        xc =  xc + (deltax * lambdaedge * vfxmean);
        yc = (yc + (deltay * lambdaedge * vfymean) + (deltay * lambdapath * Ey)) / (1.0 + deltay * lambdapath);
        double r_diff = 0.0;
        for (i = 0; i < Np; i++) {
            r[i] = (r[i] + (deltar * lambdaedge * m_get_val(vfr, 0, i)) + (deltar * lambdasize * Er)) /
                   (1.0 + deltar * lambdasize);
            r_diff += fabs(r[i] - r_old[i]);
        }

        // Test for convergence
        snakediff = fabs(xc - xc_old) + fabs(yc - yc_old) + r_diff;

        // Free temporary matrices
        m_free(vf);
        m_free(vfx);
        m_free(vfy);
        m_free(vfr);

        iter++;
    }

    // Set the return values
    *xc0 = xc;
    *yc0 = yc;
    for (i = 0; i < Np; i++)
        r0[i] = r[i];

    // Free memory
    free(r);
    free(r_old);
    v_free( x);
    v_free( y);
    m_free(fx);
    m_free(fy);
}
コード例 #29
0
ファイル: iternsym.c プロジェクト: Rainwin2015/C
VEC *iter_mgcr(ITER *ip)
#endif
{
    STATIC VEC *As=VNULL, *beta=VNULL, *alpha=VNULL, *z=VNULL;
    STATIC MAT *N=MNULL, *H=MNULL;

    VEC *rr, v, s;  /* additional pointer and structures */
    Real nres;      /* norm of a residual */
    Real dd;        /* coefficient d_i */
    int i,j;
    int done;      /* if TRUE then stop the iterative process */
    int dim;       /* dimension of the problem */

    /* ip cannot be NULL */
    if (ip == INULL) error(E_NULL,"mgcr");
    /* Ax, b and stopping criterion must be given */
    if (! ip->Ax || ! ip->b || ! ip->stop_crit)
        error(E_NULL,"mgcr");
    /* at least one direction vector must exist */
    if ( ip->k <= 0) error(E_BOUNDS,"mgcr");
    /* if the vector x is given then b and x must have the same dimension */
    if ( ip->x && ip->x->dim != ip->b->dim)
        error(E_SIZES,"mgcr");
    if (ip->eps <= 0.0) ip->eps = MACHEPS;

    dim = ip->b->dim;
    As = v_resize(As,dim);
    alpha = v_resize(alpha,ip->k);
    beta = v_resize(beta,ip->k);

    MEM_STAT_REG(As,TYPE_VEC);
    MEM_STAT_REG(alpha,TYPE_VEC);
    MEM_STAT_REG(beta,TYPE_VEC);

    H = m_resize(H,ip->k,ip->k);
    N = m_resize(N,ip->k,dim);

    MEM_STAT_REG(H,TYPE_MAT);
    MEM_STAT_REG(N,TYPE_MAT);

    /* if a preconditioner is defined */
    if (ip->Bx) {
        z = v_resize(z,dim);
        MEM_STAT_REG(z,TYPE_VEC);
    }

    /* if x is NULL then it is assumed that x has
       entries with value zero */
    if ( ! ip->x ) {
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
    }

    /* v and s are additional pointers to rows of N */
    /* they must have the same dimension as rows of N */
    v.dim = v.max_dim = s.dim = s.max_dim = dim;


    done = FALSE;
    for (ip->steps = 0; ip->steps < ip->limit; ) {
        (*ip->Ax)(ip->A_par,ip->x,As);         /* As = A*x */
        v_sub(ip->b,As,As);                    /* As = b - A*x */
        rr = As;                               /* rr is an additional pointer */

        /* if a preconditioner is defined */
        if (ip->Bx) {
            (*ip->Bx)(ip->B_par,As,z);               /* z = B*(b-A*x)  */
            rr = z;
        }

        /* norm of the residual */
        nres = v_norm2(rr);
        dd = nres;                            /* dd = ||r_i||  */

        /* check if the norm of the residual is zero */
        if (ip->steps == 0) {
            /* information for a user */
            if (ip->info) (*ip->info)(ip,nres,As,rr);
            ip->init_res = fabs(nres);
        }

        if (nres == 0.0) {
            /* iterative process is finished */
            done = TRUE;
            break;
        }

        /* save this residual in the first row of N */
        v.ve = N->me[0];
        v_copy(rr,&v);

        for (i = 0; i < ip->k && ip->steps < ip->limit; i++) {
            ip->steps++;
            v.ve = N->me[i];                /* pointer to a row of N (=s_i) */
            /* note that we must use here &v, not v */
            (*ip->Ax)(ip->A_par,&v,As);
            rr = As;                        /* As = A*s_i */
            if (ip->Bx) {
                (*ip->Bx)(ip->B_par,As,z);    /* z = B*A*s_i  */
                rr = z;
            }

            if (i < ip->k - 1) {
                s.ve = N->me[i+1];         /* pointer to a row of N (=s_{i+1}) */
                v_copy(rr,&s);                   /* s_{i+1} = B*A*s_i */
                for (j = 0; j <= i-1; j++) {
                    v.ve = N->me[j+1];      /* pointer to a row of N (=s_{j+1}) */
                    /* beta->ve[j] = in_prod(&v,rr); */      /* beta_{j,i} */
                    /* modified Gram-Schmidt algorithm */
                    beta->ve[j] = in_prod(&v,&s);  	         /* beta_{j,i} */
                    /* s_{i+1} -= beta_{j,i}*s_{j+1} */
                    v_mltadd(&s,&v,- beta->ve[j],&s);
                }

                /* beta_{i,i} = ||s_{i+1}||_2 */
                beta->ve[i] = nres = v_norm2(&s);
                if ( nres <= MACHEPS*ip->init_res) {
                    /* s_{i+1} == 0 */
                    i--;
                    done = TRUE;
                    break;
                }
                sv_mlt(1.0/nres,&s,&s);           /* normalize s_{i+1} */

                v.ve = N->me[0];
                alpha->ve[i] = in_prod(&v,&s);     /* alpha_i = (s_0 , s_{i+1}) */

            }
            else {
                for (j = 0; j <= i-1; j++) {
                    v.ve = N->me[j+1];      /* pointer to a row of N (=s_{j+1}) */
                    beta->ve[j] = in_prod(&v,rr);       /* beta_{j,i} */
                }

                nres = in_prod(rr,rr);                 /* rr = B*A*s_{k-1} */
                for (j = 0; j <= i-1; j++)
                    nres -= beta->ve[j]*beta->ve[j];

                if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res)  {
                    /* s_k is zero */
                    i--;
                    done = TRUE;
                    break;
                }
                if (nres < 0.0) { /* do restart */
                    i--;
                    ip->steps--;
                    break;
                }
                beta->ve[i] = sqrt(nres);         /* beta_{k-1,k-1} */

                v.ve = N->me[0];
                alpha->ve[i] = in_prod(&v,rr);
                for (j = 0; j <= i-1; j++)
                    alpha->ve[i] -= beta->ve[j]*alpha->ve[j];
                alpha->ve[i] /= beta->ve[i];                /* alpha_{k-1} */

            }

            set_col(H,i,beta);

            /* other method of computing dd */
            /* if (fabs((double)alpha->ve[i]) > dd)  {
                nres = - dd*dd + alpha->ve[i]*alpha->ve[i];
                nres = sqrt((double) nres);
                if (ip->info) (*ip->info)(ip,-nres,VNULL,VNULL);
                break;
             }  */
            /* to avoid overflow/underflow in computing dd */
            /* dd *= cos(asin((double)(alpha->ve[i]/dd))); */

            nres = alpha->ve[i]/dd;
            if (fabs(nres-1.0) <= MACHEPS*ip->init_res)
                dd = 0.0;
            else {
                nres = 1.0 - nres*nres;
                if (nres < 0.0) {
                    nres = sqrt((double) -nres);
                    if (ip->info) (*ip->info)(ip,-dd*nres,VNULL,VNULL);
                    break;
                }
                dd *= sqrt((double) nres);
            }

            if (ip->info) (*ip->info)(ip,dd,VNULL,VNULL);
            if ( ip->stop_crit(ip,dd,VNULL,VNULL) ) {
                /* stopping criterion is satisfied */
                done = TRUE;
                break;
            }

        } /* end of for */

        if (i >= ip->k) i = ip->k - 1;

        /* use (i+1) by (i+1) submatrix of H */
        H = m_resize(H,i+1,i+1);
        alpha = v_resize(alpha,i+1);
        Usolve(H,alpha,alpha,0.0);       /* c_i is saved in alpha */

        for (j = 0; j <= i; j++) {
            v.ve = N->me[j];
            v_mltadd(ip->x,&v,alpha->ve[j],ip->x);
        }


        if (done) break;              /* stop the iterative process */
        alpha = v_resize(alpha,ip->k);
        H = m_resize(H,ip->k,ip->k);

    }  /* end of while */

#ifdef THREADSAFE
    V_FREE(As);
    V_FREE(beta);
    V_FREE(alpha);
    V_FREE(z);
    M_FREE(N);
    M_FREE(H);
#endif

    return ip->x;                    /* return the solution */
}
コード例 #30
0
ファイル: qnt12.c プロジェクト: ZF0085/onionphone
/********************************************************************
**
** Function: lspVQ ()
**
** Description:
**		Vector quantizes a set of int term filter coefficients
**		using a multi-stage M-L tree search algorithm.
**
** Arguments:
**
**	int16_t	target[]	: the target coefficients to be quantized (Q15/Q17)
**	int16_t	weight[]	: weights for mse calculation (Q11)
**	int16_t	qout[]		: the output array (Q15/Q17)
**	int16_t	codebook[]	: codebooks,   cb[0..numStages-1] (Q15/Q17)
**	int16_t	tos 		: the number of stages
**  int16_t	cb_size[]	: codebook size (multistages)
**	int16_t	cb_index[]	: codebook indeces; cb_index[0..numStages-1]
**                            (output)
**  int16_t	dim
**  BOOLEAN		flag
**
** Return value:	None
**
***********************************************************************/
static void lspVQ(int16_t target[], int16_t weight[], int16_t qout[],
                  const int16_t codebook[], int16_t tos,
                  const int16_t cb_size[], int16_t cb_index[],
                  int16_t dim, BOOLEAN flag)
{
    register int16_t i, entry;
    register int16_t c1, s1;
    const int16_t *cdbk_ptr, *cdbk_ptr2, *ptr1;
    int16_t index[LSP_VQ_CAND][LSP_VQ_STAGES];
    int16_t nextIndex[LSP_VQ_CAND][LSP_VQ_STAGES];
    int16_t ncPrev;
    int16_t cand[LSP_VQ_CAND][2 * LPC_ORD];
    int16_t max_dMin, dMin[LSP_VQ_CAND], distortion;
    int16_t *cand_target;
    int32_t L_temp;
    int16_t ptr_offset = 0;
    int16_t temp1, temp2;

    /*==================================================================*
    *	Initialize the data before starting the tree search.			*
    *	  - the number of candidates from the "previous" stage is set	*
    *		to 1 since there is no previous stage!						*
    *	  - the candidate vector from the previous stage is set to zero *
    *	  - the list of indeces for each candidate is set to 1			*
    *==================================================================*/
    for (i = 0; i < LSP_VQ_CAND; i++) {
        v_zap(cand[i], dim);
        v_zap(index[i], LSP_VQ_STAGES);
        v_zap(nextIndex[i], LSP_VQ_STAGES);
    }
    cand_target = v_get(dim);
    ncPrev = 1;

    /*==================================================================*
    *	Now we start the search:										*
    *		For each stage												*
    *			For each candidate from the previous stage				*
    *				For each entry in the current stage codebook		*
    *					* add codebook vector to current candidate		*
    *					* compute the distortion with the target		*
    *					* retain candidate if one of the best so far	*
    *==================================================================*/
    cdbk_ptr = codebook;

    /* An observation for lspVQ() shows that if "flag" is FALSE, then we only */
    /* need to keep track of the best one (instead of the best LSP_VQ_CAND,   */
    /* 8) cand[][] and index[][].  This has significant influence on          */
    /* execution speed.                                                       */

    for (s1 = 0; s1 < tos; s1++) {
        /* set the distortions to huge values */
        fill(dMin, SW_MAX, LSP_VQ_CAND);
        max_dMin = SW_MAX;

        /* Loop for each previous candidate selected, and try each entry */
        for (c1 = 0; c1 < ncPrev; c1++) {
            ptr_offset = 0;

            /* cand_target[] is the target vector with cand[c1] removed.      */
            /* This moves some operations from the for-entry loop here.       */
            /* save_saturation(); */
            v_equ(cand_target, target, dim);
            v_sub(cand_target, cand[c1], dim);
            /* restore_saturation(); */

            for (entry = 0; entry < cb_size[s1]; entry++) {
                ptr1 = cdbk_ptr + ptr_offset;	/* Pointer arithmetics. */

                /* compute the distortion */
                distortion =
                    WeightedMSE(dim, weight, ptr1, cand_target,
                                max_dMin);

                /*======================================================*
                * If the error for this entry is less than the worst	*
                * retained candidate so far, keep it. Note that the 	*
                * error list is maintained in order of best to worst.	*
                *=======================================================*/
                if (distortion < max_dMin) {
                    max_dMin =
                        InsertCand(c1, s1, dMin, distortion,
                                   entry, nextIndex[0],
                                   index[0]);
                }
                ptr_offset = melpe_add(ptr_offset, dim);
            }
        }

        /* At this point ptr_offset is (cb_size[s1]*dim).                     */

        /*==================================================================*
        *	Compute the number of candidate vectors which we kept for the	*
        *	next stage. Note that if the size of the stages is less than	*
        *	the number of candidates, we build them up using all entries	*
        *	until we have kept numCand candidates.  On the other hand, if   *
        *   flag is FALSE and (s1 == tos - 1), then we only need to use     *
        *   ncPrev = 1 because we only copy the best candidate before       *
        *   exiting lspVQ().                                                *
        *==================================================================*/

        if (!flag && s1 == tos - 1)
            ncPrev = 1;
        else {
            /* ncPrev = Min(ncPrev*cb_size[s1], LSP_VQ_CAND) for regular      */
            /* loops, and ncPrev = Min(ncPrev*cb_size[s1], LSP_INP_CAND) for  */
            /* the last lap.  Explanations are available near the end of this *//* function.                                                      */

            L_temp = melpe_L_mult(ncPrev, cb_size[s1]);
            L_temp = melpe_L_shr(L_temp, 1);
            temp1 = melpe_extract_l(L_temp);	/* temp1 = ncPrev * cb_size[s1] */
            if (s1 == tos - 1)
                temp2 = LSP_INP_CAND;
            else
                temp2 = LSP_VQ_CAND;
            if (temp1 < temp2)
                ncPrev = temp1;
            else
                ncPrev = temp2;
        }

        /*==================================================================*
        *	We now have the  best indices for the stage just completed, so	*
        *	compute the new candidate vectors for the next stage... 		*
        *==================================================================*/
        for (c1 = 0; c1 < ncPrev; c1++) {
            v_zap(cand[c1], dim);
            cdbk_ptr2 = codebook;
            temp1 = melpe_add(s1, 1);
            v_equ(index[c1], nextIndex[c1], temp1);
            for (i = 0; i < temp1; i++) {
                /*      v_add(cand[c1], cdbk_ptr2 + index[c1][i]*dim, dim); */
                L_temp = melpe_L_mult(index[c1][i], dim);
                L_temp = melpe_L_shr(L_temp, 1);
                temp2 = melpe_extract_l(L_temp);
                ptr1 = cdbk_ptr2 + temp2;
                v_add(cand[c1], ptr1, dim);
                /*      cdbk_ptr2 += cb_size[i]*dim; */
                L_temp = melpe_L_mult(cb_size[i], dim);
                L_temp = melpe_L_shr(L_temp, 1);
                temp2 = melpe_extract_l(L_temp);
                cdbk_ptr2 += temp2;
            }
        }

        /*      cdbk_ptr += cb_size[s1] * dim; */
        cdbk_ptr += ptr_offset;
    }

    /* Copy best candidate and indices into output.  Here we use temp1 and    */
    /* temp2 to compute (c1*tos) and (c1*dim).                                */

    /* Originally this function copies LSP_VQ_CAND (== 8) vectors before      */
    /* exiting if flag is TRUE.  However, in the calling environment of       */
    /* lspVQ() when flag is passed in as TRUE, we only used LSP_INP_CAND      */
    /* (== 5).                                                                */

    temp1 = 0;
    temp2 = 0;
    for (i = 0; i < ncPrev; i++) {
        v_equ(&(cb_index[temp1]), index[i], tos);
        v_equ(&qout[temp2], cand[i], dim);
        temp1 = melpe_add(temp1, tos);
        temp2 = melpe_add(temp2, dim);
    }

    v_free(cand_target);
}