コード例 #1
0
ファイル: Quaternion.cpp プロジェクト: jjuiddong/Dx3D-Study
//--------------------------------
//
//--------------------------------
void Quaternion::Normalize()
{
    float norm = sqrtf( SQR(x) + SQR(y) + SQR(z) + SQR(w) );

    if( FLOAT_EQ( 0.0F, norm ) )
    {
        return ;
    } //if

    norm = 1.0F / norm;

    x = x * norm;
    y = y * norm;
    z = z * norm;
    w = w * norm;

    norm = sqrtf( SQR(x) + SQR(y) + SQR(z) + SQR(w) );

    if( !FLOAT_EQ( 1.0F, norm ) )
    {
        return ;
    } //if

    LIMIT_RANGE(-1.0f, w, 1.0f);

    LIMIT_RANGE(-1.0f, x, 1.0f);
    LIMIT_RANGE(-1.0f, y, 1.0f);
    LIMIT_RANGE(-1.0f, z, 1.0f);
} //Quaternion::Normalize
コード例 #2
0
ファイル: quadTree.cpp プロジェクト: spuzz/RTS-Demo
bool quadTree::pointToRectCollision(point2d* point, rectangle rect) {
	if((point->getX() >= rect.getStartPoint()->getX() || FLOAT_EQ(point->getX(),rect.getStartPoint()->getX())) && 
		(point->getX() <= rect.getEndPoint()->getX()|| FLOAT_EQ(point->getX(),rect.getEndPoint()->getX())) && 
		(point->getY() >= rect.getStartPoint()->getY() || FLOAT_EQ(point->getY(),rect.getStartPoint()->getY())) && 
		(point->getY() <= rect.getEndPoint()->getY() || FLOAT_EQ(point->getY(),rect.getEndPoint()->getY())))
	{
		return true;
	}
	return false;
}
コード例 #3
0
ファイル: Convert.cpp プロジェクト: BlackMael/DirectEncode
bool CConvert::IsFrameRateSupported(float fps)
{
	if (FLOAT_EQ(fps, 24) || FLOAT_EQ(fps, 25) || 
		FLOAT_EQ(fps, 30) || FLOAT_EQ(fps, 50) || 
		FLOAT_EQ(fps, 60) || FLOAT_EQ(fps, 23.976) ||
		FLOAT_EQ(fps, 29.97) || FLOAT_EQ(fps, 59.94))
	{
		return true;
	}

	return false;
}
コード例 #4
0
ファイル: sce40.c プロジェクト: tangxl0591/camera
/** sce_find_intersection:
 *    @line1: first line
 *    @line2: second line
 *    @t: parameter for intersection point in first line
 *
 *  This function runs in ISP HW thread context.
 *
 *  This function finds a intersection between two lines and return its position
 *  in first line by parameter.
 *
 *  Return:  TRUE  - lines are intersecting
 *           FALSE - lines are parallel
 *
 **/
static boolean sce_find_intersection(ISP_Skin_enhan_line *line1,
  ISP_Skin_enhan_line *line2, double* t)
{
  double t2;
  /* check if one of the lines is either vertical or horizontal */
  /* Assumptions according SCE documentation:
     - lines are not matching:
        One of the line is crossing center of pentagon and other is one
        of its sides
     - none of the points belong to neither axis (cb and cr does not equal 0)
     - no line can have both shift_cr and shift_cb equal to 0
   */
  if ((FLOAT_EQ((line2->shift_cb * line1->shift_cr -
      line1->shift_cb * line2->shift_cr), 0)) ||
      ((FLOAT_EQ(line1->shift_cr, 0) && FLOAT_EQ(line2->shift_cr, 0)))){
    /* Lines are parallel */

    ISP_DBG(ISP_MOD_SCE, "%s: parallel lines", __func__);
    return FALSE;
  }

  if(FLOAT_EQ(line1->shift_cr, 0)) {
    *t =  ((line1->point0.cr - line2->point0.cr) * line2->shift_cb +
          (line2->point0.cb - line1->point0.cb) * line2->shift_cr) /
          (line1->shift_cb * line2->shift_cr - line2->shift_cb * line1->shift_cr);
    t2  = (line1->point0.cr - line2->point0.cr + line1->shift_cr * *t) /
          line2->shift_cr;
  } else {
    t2 =  ((line2->point0.cr - line1->point0.cr) * line1->shift_cb +
          (line1->point0.cb - line2->point0.cb) * line1->shift_cr) /
          (line2->shift_cb * line1->shift_cr - line1->shift_cb * line2->shift_cr);
    *t =  (line2->point0.cr - line1->point0.cr + line2->shift_cr * t2) /
          line1->shift_cr;
  }

  if ((t2 < 0) || (t2 > 1)){
    /* Lines intersect outside poligon */
    ISP_DBG(ISP_MOD_SCE, "%s: outside intersection", __func__);
    return FALSE;
  }

  return TRUE;
}
コード例 #5
0
/*
 * The function Convert_Geodetic_To_Sinusoidal converts geodetic (latitude and
 * longitude) coordinates to Sinusoidal projection (easting and northing)
 * coordinates, according to the current ellipsoid and Sinusoidal projection
 * parameters.  If any errors occur, the error code(s) are returned by the
 * function, otherwise SINU_NO_ERROR is returned.
 *
 *    Latitude          : Latitude (phi) in radians           (input)
 *    Longitude         : Longitude (lambda) in radians       (input)
 *    Easting           : Easting (X) in meters               (output)
 *    Northing          : Northing (Y) in meters              (output)
 */
  double slat = sin(Latitude);
  double sin2lat, sin4lat, sin6lat;
  double dlam;                      /* Longitude - Central Meridan */
  double mm;
  double MM;
  long Error_Code = SINU_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
    dlam = Longitude - Sinu_Origin_Long;
    mm = sqrt(1.0 - es2 * slat * slat);
    sin2lat = SINU_COEFF_TIMES_SIN(c1, 2.0, Latitude);
    sin4lat = SINU_COEFF_TIMES_SIN(c2, 4.0, Latitude);
    sin6lat = SINU_COEFF_TIMES_SIN(c3, 6.0, Latitude);
    MM = Sinu_a * (c0 * Latitude - sin2lat + sin4lat - sin6lat);
    *Easting = Sinu_a * dlam * cos(Latitude) / mm + Sinu_False_Easting;
    *Northing = MM + Sinu_False_Northing;
  }
  return (Error_Code);
} /* END OF Convert_Geodetic_To_Sinusoidal */
long rspfSinusoidalProjection::Convert_Sinusoidal_To_Geodetic(double Easting,
                                                               double Northing,
                                                               double *Latitude,
                                                               double *Longitude)const
{ /* BEGIN Convert_Sinusoidal_To_Geodetic */
/*
 * The function Convert_Sinusoidal_To_Geodetic converts Sinusoidal projection
 * (easting and northing) coordinates to geodetic (latitude and longitude)
 * coordinates, according to the current ellipsoid and Sinusoidal projection
 * coordinates.  If any errors occur, the error code(s) are returned by the
 * function, otherwise SINU_NO_ERROR is returned.
 *
 *    Easting           : Easting (X) in meters                  (input)
 *    Northing          : Northing (Y) in meters                 (input)
 *    Latitude          : Latitude (phi) in radians              (output)
 *    Longitude         : Longitude (lambda) in radians          (output)
 */
  double dx;     /* Delta easting - Difference in easting (easting-FE)      */
  double dy;     /* Delta northing - Difference in northing (northing-FN)   */
  double mu;
  double sin2mu, sin4mu, sin6mu, sin8mu;
  double sin_lat;
  long Error_Code = SINU_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
    dy = Northing - Sinu_False_Northing;
    dx = Easting - Sinu_False_Easting;
    mu = dy / (Sinu_a * c0);
    sin2mu = SINU_COEFF_TIMES_SIN(a0, 2.0, mu);
    sin4mu = SINU_COEFF_TIMES_SIN(a1, 4.0, mu);
    sin6mu = SINU_COEFF_TIMES_SIN(a2, 6.0, mu);
    sin8mu = SINU_COEFF_TIMES_SIN(a3, 8.0, mu);
    *Latitude = mu + sin2mu + sin4mu + sin6mu + sin8mu;
    if (*Latitude > PI_OVER_2)  /* force distorted values to 90, -90 degrees */
      *Latitude = PI_OVER_2;
    else if (*Latitude < -PI_OVER_2)
      *Latitude = -PI_OVER_2;
    if (FLOAT_EQ(fabs(*Latitude),PI_OVER_2,1.0e-8))
      *Longitude = Sinu_Origin_Long;
    else
    {
      sin_lat = sin(*Latitude);
      *Longitude = Sinu_Origin_Long + dx * sqrt(1.0 - es2 *
                                                sin_lat * sin_lat) / (Sinu_a * cos(*Latitude));
    }
  }
  return (Error_Code);
} /* END OF Convert_Sinusoidal_To_Geodetic */
コード例 #6
0
ファイル: lda.c プロジェクト: gmrandazzo/libscientific
/* last column must be an integer column which define the classes */
void LDA(matrix *mx, uivector *y, LDAMODEL *lda)
{
  size_t i, j, l, k, cc, imin, imax;
  array *classes;
  array *S;
  matrix *X, *X_T, *Sb, *Sw, *InvSw_Sb;
  dvector *mutot;
  dvector *classmu;
  dvector *evect_, *ldfeature;
  matrix *covmx;
  
  
  lda->nclass = 0;
  
  
  imin = imax = y->data[0];
  
  for(i = 1; i < y->size; i++){
    if(y->data[i] > imax){
      imax = y->data[i];
    }
    
    if(y->data[i] < imin){
      imin = y->data[i];
    }
  }
  
  /* get the number of classes */
  if(imin == 0){
    lda->class_start = 0;
    lda->nclass = imax + 1;
  }
  else{
    lda->class_start = 1;
    lda->nclass = imax;
  }
  
  /*printf("nclass %d\n", (int)lda->nclass);*/
  
  /* Copy data */
  NewMatrix(&X, mx->row, mx->col);
  MatrixCopy(mx, &X);
  MatrixCheck(X);
  /*
  for(j = 0; j < mx->col-1; j++){
    for(i = 0; i < mx->row; i++){
      X->data[i][j] = mx->data[i][j];
    }
  }
  */
  
  /*create classes of objects */
  NewArray(&classes, lda->nclass);
  UIVectorResize(&lda->classid, mx->row);
  j = 0;
  if(imin == 0){
    for(k = 0; k < lda->nclass; k++){
      cc = 0;
      for(i = 0; i < X->row; i++){
        if(y->data[i] == k)
          cc++;
        else
          continue;
      }
      NewArrayMatrix(&classes, k, cc, X->col);
      
      cc = 0;
      for(i = 0; i < X->row; i++){
        if(y->data[i] == k){
          for(l = 0; l < X->col; l++){
            classes->m[k]->data[cc][l] = X->data[i][l];
          }
          lda->classid->data[j] = i;
          j++;
          cc++;
        }
        else{
          continue;
        }
      }
    }
  }
  else{
    for(k = 0; k < lda->nclass; k++){
      cc = 0;
      for(i = 0; i < X->row; i++){
        if(y->data[i] == k+1)
          cc++;
        else
          continue;
      }
      NewArrayMatrix(&classes, k, cc, X->col);
      
      cc = 0;
      for(i = 0; i < X->row; i++){
        if(y->data[i] == k+1){
          for(l = 0; l < X->col; l++){
            classes->m[k]->data[cc][l] = X->data[i][l];
          }
          lda->classid->data[j] = i;
          j++;
          cc++;
        }
        else{
          continue;
        }
      }
    }
  }
  
  /*puts("Classes"); PrintArray(classes);*/
  
  /* Compute the prior probability */
  for(k = 0; k < classes->order; k++){
    DVectorAppend(&lda->pprob, (classes->m[k]->row/(double)X->row));
  }
  
  /*
  puts("Prior Probability"); 
  PrintDVector(lda->pprob);
  */
  
  /*Compute the mean of each class*/
  for(k = 0; k < classes->order; k++){
    initDVector(&classmu);
    MatrixColAverage(classes->m[k], &classmu);

    MatrixAppendRow(&lda->mu, classmu);

    DelDVector(&classmu);
  }
  
  /*puts("Class Mu");
  FindNan(lda->mu);
  PrintMatrix(lda->mu);*/
  
  /*Calculate the total mean of samples..*/
  initDVector(&mutot);
  MatrixColAverage(X, &mutot);
  
  /*puts("Mu tot");
  PrintDVector(mutot);*/
  
  /*NewDVector(&mutot, mu->col);
  
  for(k = 0; k < mu->row; k++){
    for(i = 0; i < mu->col; i++){
      mutot->data[i] += mu->data[k][i];
    }
  }
  
  for(i = 0; i < mutot->size; i++){
    mutot->data[i] /= nclasses;
  }*/
  
  
  /*Centering data before computing the scatter matrix*/
  for(k = 0; k < classes->order; k++){
    for(i = 0; i < classes->m[k]->row; i++){
      for(j = 0; j < classes->m[k]->col; j++){
        classes->m[k]->data[i][j] -= mutot->data[j];
      }
    }
  }
  
  /*
  puts("Classes");
  for(i = 0; i < classes->order; i++){
    FindNan(classes->m[i]);
  }
   PrintArray(classes);
  */
  /*Compute the scatter matrix
   * S = nobj - 1 * covmx
   */
  initArray(&S);
  NewMatrix(&covmx, X->col, X->col);
  for(k = 0; k < classes->order; k++){
    matrix *m_T;
    NewMatrix(&m_T, classes->m[k]->col, classes->m[k]->row);
    MatrixTranspose(classes->m[k], m_T);
    
    MatrixDotProduct(m_T, classes->m[k], covmx);
    
    for(i = 0; i < covmx->row; i++){
      for(j = 0; j < covmx->col; j++){
        covmx->data[i][j] /= classes->m[k]->row;
      }
    }
    ArrayAppendMatrix(&S, covmx);
    MatrixSet(covmx, 0.f);
    DelMatrix(&m_T);
  }
  /*
  puts("Scatter Matrix");
  for(i = 0; i < S->order; i++)
    FindNan(S->m[i]);

  PrintArray(S);*/
  
  /* Compute the class scatter which represent the covariance matrix */
  NewMatrix(&Sw, X->col, X->col);
  
  for(k = 0; k < S->order; k++){
    for(i = 0; i  < S->m[k]->row; i++){
      for(j = 0; j  < S->m[k]->col; j++){
        Sw->data[i][j] += (double)(classes->m[k]->row/(double)X->row) *  S->m[k]->data[i][j];
      }
    }
  }
  /*
  puts("Class scatter matrix Sw");
  FindNan(Sw);
  PrintMatrix(Sw);
  */
  /*Compute the between class scatter matrix Sb*/
  NewMatrix(&Sb, X->col, X->col);
  for(k = 0; k < lda->mu->row; k++){ /*for each class of object*/
    cc = classes->m[k]->row;
    for(i = 0; i < Sb->row; i++){
      for(j = 0; j < Sb->col; j++){
        Sb->data[i][j] += cc * (lda->mu->data[k][i] - mutot->data[i]) * (lda->mu->data[k][j] - mutot->data[j]);
      }
    }
  }

  /*
  puts("Between class scatter matrix Sb");
  FindNan(Sb);
  PrintMatrix(Sb); */
  
  /* Computing the LDA projection */
  /*puts("Compute Matrix Inversion");*/
  MatrixInversion(Sw, &lda->inv_cov);

  double ss = 0.f;
  for(i = 0; i < lda->inv_cov->row; i++){
    for(j = 0; j < lda->inv_cov->col; j++){
      ss += square(lda->inv_cov->data[i][j]);
    }
    if(_isnan_(ss))
      break;
  }
  
  if(FLOAT_EQ(ss, 0.f, EPSILON) || _isnan_(ss)){
    /*Do SVD as pseudoinversion accordin to Liu et al. because matrix is nonsingular
     *
     * JUN LIU et al, Int. J. Patt. Recogn. Artif. Intell. 21, 1265 (2007). DOI: 10.1142/S0218001407005946
     * EFFICIENT PSEUDOINVERSE LINEAR DISCRIMINANT ANALYSIS AND ITS NONLINEAR FORM FOR FACE RECOGNITION
     *
     *
     * Sw`^-1 = Q * G^-1 * Q_T
     * Q G AND Q_T come from SVD
     */
    MatrixPseudoinversion(Sw, &lda->inv_cov);

    /*
    NewMatrix(&A_T, Sw->col, Sw->row);
    MatrixInversion(Sw, &A_T);
    NewMatrix(&A_T_Sw, A_T->row, Sw->col);
    MatrixDotProduct(A_T, Sw, A_T_Sw);
    
    initMatrix(&A_T_Sw_inv);
    MatrixInversion(A_T_Sw, &A_T_Sw_inv);
    
    MatrixDotProduct(A_T_Sw_inv, A_T, lda->inv_cov);
    DelMatrix(&A_T);
    DelMatrix(&A_T_Sw);
    DelMatrix(&A_T_Sw_inv);
    */
  }

  /*puts("Inverted Covariance Matrix from Sw");
   FindNan(lda->inv_cov);
   PrintMatrix(lda->inv_cov);
  */
  /*puts("Compute Matrix Dot Product");*/
  NewMatrix(&InvSw_Sb, lda->inv_cov->row, Sb->col);
  MatrixDotProduct(lda->inv_cov, Sb, InvSw_Sb);
  
  /*puts("InvSw_Sb"); PrintMatrix(InvSw_Sb);*/
  /*puts("Compute Eigenvectors");*/
  EVectEval(InvSw_Sb, &lda->eval, &lda->evect);
  /*EvectEval3(InvSw_Sb, InvSw_Sb->row, &lda->eval, &lda->evect);*/
  /*EVectEval(InvSw_Sb, &lda->eval, &lda->evect); */
  
  /* Calculate the new projection in the feature space 
   * 
   * and the multivariate normal distribution
   */
/*       Remove centering data   */
  for(k = 0; k < classes->order; k++){
    for(i = 0; i < classes->m[k]->row; i++){
      for(j = 0; j < classes->m[k]->col; j++){
        classes->m[k]->data[i][j] += mutot->data[j];
      }
    }
  }
    
  initMatrix(&X_T);
  
  for(k = 0; k < classes->order; k++){
    /*printf("row %d  col %d\n", (int)classes->m[k]->row, (int)classes->m[k]->col);*/
    AddArrayMatrix(&lda->features, classes->m[k]->row, classes->m[k]->col);
    AddArrayMatrix(&lda->mnpdf, classes->m[k]->row, classes->m[k]->col);
  }
  
  NewDVector(&evect_, lda->evect->row);
  initDVector(&ldfeature);
  
  ResizeMatrix(&lda->fmean, classes->order, lda->evect->col);
  ResizeMatrix(&lda->fsdev, classes->order, lda->evect->col);
  
  for(l = 0; l < lda->evect->col; l++){
    
    for(i = 0; i < lda->evect->row; i++){
      evect_->data[i] = lda->evect->data[i][l];
    }
    
    for(k = 0; k < classes->order; k++){
      
      ResizeMatrix(&X_T, classes->m[k]->col, classes->m[k]->row);
      MatrixTranspose(classes->m[k], X_T);
      DVectorResize(&ldfeature, classes->m[k]->row);
      
      DVectorMatrixDotProduct(X_T, evect_, ldfeature);
      
      for(i = 0; i < ldfeature->size; i++){
        lda->features->m[k]->data[i][l] = ldfeature->data[i];
      }
      
/*        Calculate the multivariate normal distribution  */
      double mean = 0.f, sdev = 0.f;
      DVectorMean(ldfeature, &mean);
      DVectorSDEV(ldfeature, &sdev);
      
      lda->fmean->data[k][l] = mean;
      lda->fsdev->data[k][l] = sdev;
      for(i = 0; i < ldfeature->size; i++){
        lda->mnpdf->m[k]->data[i][l] = 1./sqrt(2 * pi* sdev) * exp(-square((ldfeature->data[i] - mean)/sdev)/2.f);
      }
    }
  }
  
  DelDVector(&evect_);
  DelMatrix(&covmx);
  DelDVector(&ldfeature);
  DelDVector(&mutot);
  DelMatrix(&Sb);
  DelMatrix(&InvSw_Sb);
  DelArray(&classes);
  DelArray(&S);
  DelMatrix(&Sw);
  DelMatrix(&X_T);
  DelMatrix(&X);
}
コード例 #7
0
ファイル: lda.c プロジェクト: gmrandazzo/libscientific
void LDAError(matrix *mx, uivector *my, LDAMODEL *lda, dvector **sens, dvector **spec, dvector **ppv, dvector **npv, dvector **acc)
{
  size_t  i, k, pos;
  matrix *pfeatures;
  matrix *probability;
  matrix *mnpdf;
  uivector *classprediction;
  uivector* tp, *fp, *fn, *tn;
  double d, p, n;
  
  initMatrix(&pfeatures);
  initMatrix(&probability);
  initMatrix(&mnpdf);
  initUIVector(&classprediction);
  LDAPrediction(mx, lda, &pfeatures, &probability, &mnpdf, &classprediction);
  
  NewUIVector(&tp, lda->nclass);
  NewUIVector(&fp, lda->nclass);
  NewUIVector(&tn, lda->nclass);
  NewUIVector(&fn, lda->nclass);
  
  if(lda->class_start == 1){
    pos = -1;
  }
  else{
    pos = 0;
  }
  
  for(k = 0; k < lda->nclass; k++){ /*for each class*/
    for(i = 0; i < my->size; i++){
      /*printf("class %d  object class %d  predicted class %d\n", (int)k, (int)(my->data[i] - pos), (int)(classprediction->data[i] - pos));*/
      if(k == (my->data[i] - pos)){ /* oggetto della stessa classe */
        if(my->data[i] == classprediction->data[i]){
          tp->data[k]++;
          /*puts("TP");*/
        }
        else{
          fn->data[k]++;
          /*puts("FN");*/
        }
      }
      else{ /*oggetto di diversa classe dal quella cercata*/
        if(classprediction->data[i] - pos == k){ /*false positive!*/
          fp->data[k]++;
          /*puts("FP");*/
        }
        else{
          tn->data[k]++;
          /*puts("TN");*/
        }
      }
    }
  }
  
  if((*sens)->size != lda->nclass)
    DVectorResize(sens, lda->nclass);
  
  if((*spec)->size != lda->nclass)
    DVectorResize(spec, lda->nclass);
  
  if((*ppv)->size != lda->nclass)
    DVectorResize(ppv, lda->nclass);
  
  if((*npv)->size != lda->nclass)
    DVectorResize(npv, lda->nclass);
  
  if((*acc)->size != lda->nclass)
    DVectorResize(acc, lda->nclass);
  
  for(i = 0; i < tp->size; i++){ /* for each class*/
    /* sensitivity calculation */
    d = tp->data[i] + fn->data[i];
    if(FLOAT_EQ(d, 0, 1e-4)){
      (*sens)->data[i] = 0;
    }
    else{
      (*sens)->data[i] = tp->data[i] / d;
    }
    
    /* specificity calculation */
    d = tn->data[i] + fp->data[i];
    if(FLOAT_EQ(d, 0, 1e-4)){
      (*spec)->data[i] = 0;
    }
    else{
      (*spec)->data[i] = tn->data[i] / d;
    }
    
    /* ppv calculation */
    d = tp->data[i] + fp->data[i];
    if(FLOAT_EQ(d, 0, 1e-4)){
      (*ppv)->data[i] = 0;
    }
    else{
      (*ppv)->data[i] = tp->data[i] / d;
    }
    
    /* npv calculation */
    d = fn->data[i] + tn->data[i];
    if(FLOAT_EQ(d, 0, 1e-4)){
      (*npv)->data[i] = 0;
    }
    else{
      (*npv)->data[i] = tn->data[i] / d;
    }
    
    /* acc calculation */
    p = tp->data[i] + fn->data[i];
    n = tn->data[i] + fp->data[i];
    (*acc)->data[i] = (tp->data[i] + tn->data[i]) / (double)(p+n);
  }
  
  DelUIVector(&tp);
  DelUIVector(&fp);
  DelUIVector(&fn);
  DelUIVector(&tn);
  DelMatrix(&mnpdf);
  DelMatrix(&pfeatures);
  DelMatrix(&probability);
  DelUIVector(&classprediction);
}
コード例 #8
0
/*
 * The function Convert_Geodetic_To_Bonne converts geodetic (latitude and
 * longitude) coordinates to Bonne projection (easting and northing)
 * coordinates, according to the current ellipsoid and Bonne projection
 * parameters.  If any errors occur, the error code(s) are returned by the
 * function, otherwise BONN_NO_ERROR is returned.
 *
 *    Latitude          : Latitude (phi) in radians           (input)
 *    Longitude         : Longitude (lambda) in radians       (input)
 *    Easting           : Easting (X) in meters               (output)
 *    Northing          : Northing (Y) in meters              (output)
 */
  double dlam; /* Longitude - Central Meridan */
  double mm;
  double MM;
  double rho;
  double EE;
  double clat = cos(Latitude);
  double slat = sin(Latitude);
  double lat, sin2lat, sin4lat, sin6lat;
  long Error_Code = BONN_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
     if (Bonn_Origin_Lat == 0.0)
        Convert_Geodetic_To_Sinusoidal(Latitude, Longitude, Easting, Northing);
     else
     {
        dlam = Longitude - Bonn_Origin_Long;
        if (dlam > M_PI)
        {
           dlam -= TWO_PI;
        }
        if (dlam < -M_PI)
        {
           dlam += TWO_PI;
        }
        if ((Latitude - Bonn_Origin_Lat) == 0.0 && FLOAT_EQ(fabs(Latitude),PI_OVER_2,.00001))
        {
           *Easting = 0.0;
           *Northing = 0.0;
        }
        else
        {
           mm = BONN_m(clat, slat);
           
           lat = c0 * Latitude;
           sin2lat = COEFF_TIMES_BONN_SIN(c1, 2.0, Latitude);
           sin4lat = COEFF_TIMES_BONN_SIN(c2, 4.0, Latitude);
           sin6lat = COEFF_TIMES_BONN_SIN(c3, 6.0, Latitude);
           MM = BONN_M(lat, sin2lat, sin4lat, sin6lat);         
           
           rho = Bonn_am1sin + M1 - MM;
           if (rho == 0)
              EE = 0;
           else
              EE = Bonn_a * mm * dlam / rho;
           *Easting = rho * sin(EE) + Bonn_False_Easting;
           *Northing = Bonn_am1sin - rho * cos(EE) + Bonn_False_Northing;
        }
     }
  }
  return (Error_Code);
} /* End Convert_Geodetic_To_Bonne */
long rspfBonneProjection::Convert_Bonne_To_Geodetic(double Easting,
                                                     double Northing,
                                                     double *Latitude,
                                                     double *Longitude)const
{ /* Begin Convert_Bonne_To_Geodetic */
/*
 * The function Convert_Bonne_To_Geodetic converts Bonne projection
 * (easting and northing) coordinates to geodetic (latitude and longitude)
 * coordinates, according to the current ellipsoid and Bonne projection
 * coordinates.  If any errors occur, the error code(s) are returned by the
 * function, otherwise BONN_NO_ERROR is returned.
 *
 *    Easting           : Easting (X) in meters                  (input)
 *    Northing          : Northing (Y) in meters                 (input)
 *    Latitude          : Latitude (phi) in radians              (output)
 *    Longitude         : Longitude (lambda) in radians          (output)
 */
  double dx;     /* Delta easting - Difference in easting (easting-FE)      */
  double dy;     /* Delta northing - Difference in northing (northing-FN)   */
  double mu;
  double MM;
  double mm;
  double am1sin_dy;
  double rho;
  double sin2mu, sin4mu, sin6mu, sin8mu;
  double clat, slat;
  long Error_Code = BONN_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
     if (Bonn_Origin_Lat == 0.0)
        Convert_Sinusoidal_To_Geodetic(Easting, Northing, Latitude, Longitude);
     else
     {
        dy = Northing - Bonn_False_Northing;
        dx = Easting - Bonn_False_Easting;
        am1sin_dy = Bonn_am1sin - dy;
        rho = sqrt(dx * dx + am1sin_dy * am1sin_dy);
        if (Bonn_Origin_Lat < 0.0)
           rho = -rho;
        MM = Bonn_am1sin + M1 - rho;
        
        mu = MM / (Bonn_a * c0); 
        sin2mu = COEFF_TIMES_BONN_SIN(a0, 2.0, mu);
        sin4mu = COEFF_TIMES_BONN_SIN(a1, 4.0, mu);
        sin6mu = COEFF_TIMES_BONN_SIN(a2, 6.0, mu);
        sin8mu = COEFF_TIMES_BONN_SIN(a3, 8.0, mu);
        *Latitude = mu + sin2mu + sin4mu + sin6mu + sin8mu;
        
        if (FLOAT_EQ(fabs(*Latitude),PI_OVER_2,.00001))
        {
           *Longitude = Bonn_Origin_Long;
        }
        else
        {
           clat = cos(*Latitude);
           slat = sin(*Latitude);
           mm = BONN_m(clat, slat);
           
           if (Bonn_Origin_Lat < 0.0)
           {
              dx = -dx;
              am1sin_dy = -am1sin_dy;
           }
           *Longitude = Bonn_Origin_Long + rho * (atan2(dx, am1sin_dy)) /
                        (Bonn_a * mm);
        }
        
        if (*Latitude > PI_OVER_2)  /* force distorted values to 90, -90 degrees */
           *Latitude = PI_OVER_2;
        else if (*Latitude < -PI_OVER_2)
           *Latitude = -PI_OVER_2;
        
        
        if (*Longitude > M_PI)/* force distorted values to 180, -180 degrees */
           *Longitude = M_PI;
        else if (*Longitude < -M_PI)
           *Longitude = -M_PI;
     }
  }
  return (Error_Code);
} /* End Convert_Bonne_To_Geodetic */
コード例 #9
0
/*
 * The function Get_Bonne_Parameters returns the current ellipsoid
 * parameters and Bonne projection parameters.
 *
 *    a                 : Semi-major axis of ellipsoid, in meters   (output)
 *    f                 : Flattening of ellipsoid                   (output)
 *    Origin_Latitude   : Latitude in radians at which the          (output)
 *                          point scale factor is 1.0
 *    Central_Meridian  : Longitude in radians at the center of     (output)
 *                          the projection
 *    False_Easting     : A coordinate value in meters assigned to the
 *                          central meridian of the projection.     (output)
 *    False_Northing    : A coordinate value in meters assigned to the
 *                          origin latitude of the projection       (output)
 */
  *a = Bonn_a;
  *f = Bonn_f;
  *Origin_Latitude = Bonn_Origin_Lat;
  *Central_Meridian = Bonn_Origin_Long;
  *False_Easting = Bonn_False_Easting;
  *False_Northing = Bonn_False_Northing;
  return;
} /* End Get_Bonne_Parameters */
long rspfBonneProjection::Convert_Geodetic_To_Bonne (double Latitude,
                                                      double Longitude,
                                                      double *Easting,
                                                      double *Northing)const
{ /* Begin Convert_Geodetic_To_Bonne */
/*
 * The function Convert_Geodetic_To_Bonne converts geodetic (latitude and
 * longitude) coordinates to Bonne projection (easting and northing)
 * coordinates, according to the current ellipsoid and Bonne projection
 * parameters.  If any errors occur, the error code(s) are returned by the
 * function, otherwise BONN_NO_ERROR is returned.
 *
 *    Latitude          : Latitude (phi) in radians           (input)
 *    Longitude         : Longitude (lambda) in radians       (input)
 *    Easting           : Easting (X) in meters               (output)
 *    Northing          : Northing (Y) in meters              (output)
 */
  double dlam; /* Longitude - Central Meridan */
  double mm;
  double MM;
  double rho;
  double EE;
  double clat = cos(Latitude);
  double slat = sin(Latitude);
  double lat, sin2lat, sin4lat, sin6lat;
  long Error_Code = BONN_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
     if (Bonn_Origin_Lat == 0.0)
        Convert_Geodetic_To_Sinusoidal(Latitude, Longitude, Easting, Northing);
     else
     {
        dlam = Longitude - Bonn_Origin_Long;
        if (dlam > M_PI)
        {
           dlam -= TWO_PI;
        }
        if (dlam < -M_PI)
        {
           dlam += TWO_PI;
        }
        if ((Latitude - Bonn_Origin_Lat) == 0.0 && FLOAT_EQ(fabs(Latitude),PI_OVER_2,.00001))
        {
           *Easting = 0.0;
           *Northing = 0.0;
        }
        else
        {
           mm = BONN_m(clat, slat);
           
           lat = c0 * Latitude;
           sin2lat = COEFF_TIMES_BONN_SIN(c1, 2.0, Latitude);
           sin4lat = COEFF_TIMES_BONN_SIN(c2, 4.0, Latitude);
           sin6lat = COEFF_TIMES_BONN_SIN(c3, 6.0, Latitude);
           MM = BONN_M(lat, sin2lat, sin4lat, sin6lat);         
           
           rho = Bonn_am1sin + M1 - MM;
           if (rho == 0)
              EE = 0;
           else
              EE = Bonn_a * mm * dlam / rho;
           *Easting = rho * sin(EE) + Bonn_False_Easting;
           *Northing = Bonn_am1sin - rho * cos(EE) + Bonn_False_Northing;
        }
     }
  }
  return (Error_Code);
} /* End Convert_Geodetic_To_Bonne */
コード例 #10
0
/*
 * The function Convert_Geodetic_To_Polyconic converts geodetic (latitude and
 * longitude) coordinates to Polyconic projection (easting and northing)
 * coordinates, according to the current ellipsoid and Polyconic projection
 * parameters.  If any errors occur, the error code(s) are returned by the
 * function, otherwise POLY_NO_ERROR is returned.
 *
 *    Latitude          : Latitude (phi) in radians           (input)
 *    Longitude         : Longitude (lambda) in radians       (input)
 *    Easting           : Easting (X) in meters               (output)
 *    Northing          : Northing (Y) in meters              (output)
 */
  double slat = sin(Latitude);
  double lat, sin2lat, sin4lat, sin6lat;
  double dlam;                      /* Longitude - Central Meridan */
  double NN;
  double NN_OVER_tlat;
  double MM;
  double EE;
  long Error_Code = POLY_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
    dlam = Longitude - Poly_Origin_Long;
    if (fabs(dlam) > (M_PI / 2.0))
    { /* Distortion will result if Longitude is more than 90 degrees from the Central Meridian */
      Error_Code |= POLY_LON_WARNING;
    }
    if (Latitude == 0.0)
    {
      *Easting = Poly_a * dlam + Poly_False_Easting;
      *Northing = -M0 + Poly_False_Northing;
    }
    else
    {
      NN = Poly_a / sqrt(1.0 - es2 * (slat * slat));
      NN_OVER_tlat = NN  / tan(Latitude);
      lat = c0 * Latitude;
      sin2lat = POLY_COEFF_TIMES_SIN(c1, 2.0, Latitude);
      sin4lat = POLY_COEFF_TIMES_SIN(c2, 4.0, Latitude);
      sin6lat = POLY_COEFF_TIMES_SIN(c3, 6.0, Latitude);
      MM = POLY_M(lat, sin2lat, sin4lat, sin6lat);
      EE = dlam * slat;
      *Easting = NN_OVER_tlat * sin(EE) + Poly_False_Easting;
      *Northing = MM - M0 + NN_OVER_tlat * (1.0 - cos(EE)) +
                  Poly_False_Northing;
    }
  }
  return (Error_Code);
} /* END OF Convert_Geodetic_To_Polyconic */
long rspfPolyconicProjection::Convert_Polyconic_To_Geodetic(double Easting,
                                                             double Northing,
                                                             double *Latitude,
                                                             double *Longitude)const
{ /* BEGIN Convert_Polyconic_To_Geodetic */
/*
 * The function Convert_Polyconic_To_Geodetic converts Polyconic projection
 * (easting and northing) coordinates to geodetic (latitude and longitude)
 * coordinates, according to the current ellipsoid and Polyconic projection
 * coordinates.  If any errors occur, the error code(s) are returned by the
 * function, otherwise POLY_NO_ERROR is returned.
 *
 *    Easting           : Easting (X) in meters                  (input)
 *    Northing          : Northing (Y) in meters                 (input)
 *    Latitude          : Latitude (phi) in radians              (output)
 *    Longitude         : Longitude (lambda) in radians          (output)
 */
  double dx;     /* Delta easting - Difference in easting (easting-FE)      */
  double dy;     /* Delta northing - Difference in northing (northing-FN)   */
  double dx_OVER_Poly_a;
  double AA;
  double BB;
  double CC = 0.0;
  double PHIn, Delta_PHI = 1.0;
  double sin_PHIn;
  double PHI, sin2PHI,sin4PHI, sin6PHI;
  double Mn, Mn_prime, Ma;
  double AA_Ma;
  double Ma2_PLUS_BB;
  double AA_MINUS_Ma;
  double tolerance = 1.0e-12;        /* approximately 1/1000th of
                               an arc second or 1/10th meter */
  long Error_Code = POLY_NO_ERROR;
  if (!Error_Code)
  { /* no errors */
    dy = Northing - Poly_False_Northing;
    dx = Easting - Poly_False_Easting;
    dx_OVER_Poly_a = dx / Poly_a;
    if (FLOAT_EQ(dy,-M0,1))
    {
      *Latitude = 0.0;
      *Longitude = dx_OVER_Poly_a + Poly_Origin_Long;
    }
    else
    {
      AA = (M0 + dy) / Poly_a;
      BB = dx_OVER_Poly_a * dx_OVER_Poly_a + (AA * AA);
      PHIn = AA;
      while (fabs(Delta_PHI) > tolerance)
      {
        sin_PHIn = sin(PHIn);
        CC = sqrt(1.0 - es2 * sin_PHIn * sin_PHIn) * tan(PHIn);
        PHI = c0 * PHIn;
        sin2PHI = POLY_COEFF_TIMES_SIN(c1, 2.0, PHIn);
        sin4PHI = POLY_COEFF_TIMES_SIN(c2, 4.0, PHIn);
        sin6PHI = POLY_COEFF_TIMES_SIN(c3, 6.0, PHIn);
        Mn = POLY_M(PHI, sin2PHI, sin4PHI, sin6PHI);
        Mn_prime = c0 - 2.0 * c1 * cos(2.0 * PHIn) + 4.0 * c2 * cos(4.0 * PHIn) - 
                   6.0 * c3 * cos(6.0 * PHIn);  
        Ma = Mn / Poly_a;
        AA_Ma = AA * Ma;
        Ma2_PLUS_BB = Ma * Ma + BB;
        AA_MINUS_Ma = AA - Ma;
        Delta_PHI = (AA_Ma * CC + AA_MINUS_Ma - 0.5 * (Ma2_PLUS_BB) * CC) /
                    (es2 * sin2PHI * (Ma2_PLUS_BB - 2.0 * AA_Ma) /
                     4.0 * CC + (AA_MINUS_Ma) * (CC * Mn_prime - 2.0 / sin2PHI) - Mn_prime);
        PHIn -= Delta_PHI;
      }
      *Latitude = PHIn;
      if (FLOAT_EQ(fabs(*Latitude),PI_OVER_2,.00001) || (*Latitude == 0))
        *Longitude = Poly_Origin_Long;
      else
      {
        *Longitude = (asin(dx_OVER_Poly_a * CC)) / sin(*Latitude) +
                     Poly_Origin_Long;
      }
    }
  }
  return (Error_Code);
} /* END OF Convert_Polyconic_To_Geodetic */
コード例 #11
0
ファイル: algebra.c プロジェクト: gmrandazzo/libscientific
/* Solve Linear System of Equation with the Gauss-Jordan
 */
void SolveLSE(matrix *mx, dvector **solution)
{
  size_t i, j, k;
  long int l;
  double tmp;
  matrix *X;
  initMatrix(&X);
  MatrixCopy(mx, &X);
  /*
   Problem: Solve the following system:
      x + y + z  = 4
      x - 2y - z = 1
      2x - y - 2z = -1

    Start out by multiplying the
    first X by -1 and add
    it to the second X to 
    eliminate x from the second
    (*X).

    -x  - y - z = -4
      x - 2y - z = 1
    ----------------
        -3y - 2z = -3

    Now eliminate x from the third
    X by multiplying the first
    X by -2 and add it to
    the third (*X).

    -2x - 2y - 2z = -8
      2x -  y - 2z = -1
    ------------------
          -3y - 4z = -9

    Next, eliminate y from the third
    X by multiplying the second
    X by -1 and adding it to
    the third (*X).

      3y +  2z = 3 
    -3y -  4z = -9
    --------------
          -2z = -6

    Solve the third X for z.
          
    -2z = -6
      z = 3

    Substitute 3 for z in the
    second X and solve for y.

    -3y - 2z = -3
    -3y - 2(3) = -3
    -3y - 6 = -3
    -3y = 3
    y = -1

    Lastly, substitute -1 for y and
    3 for z in the first X
    and solve for x.

    x + (-1) + 3 = 4
    x + 2 = 4
    x = 2

    The answer is (2, -1, 3).

  */
  
  /* Reorganize the matrix in order to find the pivot != 0 in the first k row k column */
  for(k = 0; k < X->row; k++){
    if(FLOAT_EQ(X->data[k][k], 0, 1e-4)){
      for(i = 0; i < X->row; i++){
        if(FLOAT_EQ(X->data[i][k], 0, 1e-4) == 0){
          /* move the row i to the null value */   
          for(j = 0; j < X->col; j++){
            tmp = X->data[i][j];
            X->data[i][j] = X->data[k][j];
            X->data[k][j] = tmp;
          }
          break;
        }
        else{
          continue;
        }
      }
    }
  }
    
  /* (*X).row is the number of X, so is equal to the number of unknowns variables */
  for(k = 0; k < X->row; k++){
    for(i = k+1; i < X->row; i++){
      /*if(getMatrixValue(X, i, k) != 0){  if the value is not 0 */
      if(FLOAT_EQ(X->data[i][k], 0, 1e-4) == 0){ /* if the value is not 0 */
        /*tmp = getMatrixValue(X, i, k) / getMatrixValue(X, k, k);*/
        if(FLOAT_EQ(X->data[k][k], 0, 1e-4) == 1){
          tmp = 0.f;
        }
        else{
          tmp = X->data[i][k]/X->data[k][k];
        }

        for(j = 0; j < X->col; j++){
          X->data[i][j] = (X->data[k][j] * (-tmp)) + X->data[i][j];
        }
      }
      else
        continue;
    }
  }

  /*Reset solution vector*/
  if((*solution)->size != X->row){
   DVectorResize(solution, X->row); 
  }

  l = (*X).row-1;
  
  while(l > -1){
    double b = 0.f;
    for(i = 0; i < (*X).col-1; i++){ 
      if(i != l){
        b += X->data[l][i] * (*solution)->data[i];
      }
      else
        continue;
    }
    
    if(FLOAT_EQ(X->data[l][l], 0, 1e-4) == 1)
      (*solution)->data[l] = 0.f;
    else
      (*solution)->data[l] = (X->data[l][X->col-1] -b) / X->data[l][l];
    l--;
  }
  DelMatrix(&X);
}
コード例 #12
0
long Convert_Geodetic_To_Van_der_Grinten (double Latitude,
                                          double Longitude,
                                          double *Easting,
                                          double *Northing)

{ /* BEGIN Convert_Geodetic_To_Van_der_Grinten */
/*
 * The function Convert_Geodetic_To_Van_der_Grinten converts geodetic (latitude and
 * longitude) coordinates to Van Der Grinten projection (easting and northing)
 * coordinates, according to the current ellipsoid and Van Der Grinten projection
 * parameters.  If any errors occur, the error code(s) are returned by the
 * function, otherwise GRIN_NO_ERROR is returned.
 *
 *    Latitude          : Latitude (phi) in radians           (input)
 *    Longitude         : Longitude (lambda) in radians       (input)
 *    Easting           : Easting (X) in meters               (output)
 *    Northing          : Northing (Y) in meters              (output)
 */

  double dlam;                      /* Longitude - Central Meridan */
  double aa, aasqr;
  double gg;
  double pp, ppsqr;
  double gg_MINUS_ppsqr, ppsqr_PLUS_aasqr;
  double in_theta;
  double theta;
  double sin_theta, cos_theta;
  double qq;
  long   Error_Code = GRIN_NO_ERROR;

  if ((Latitude < -PI_OVER_2) || (Latitude > PI_OVER_2))
  {  /* Latitude out of range */
    Error_Code |= GRIN_LAT_ERROR;
  }
  if ((Longitude < -PI) || (Longitude > TWO_PI))
  {  /* Longitude out of range */
    Error_Code|= GRIN_LON_ERROR;
  }

  if (!Error_Code)
  { /* no errors */

    dlam = Longitude - Grin_Origin_Long;
    if (dlam > PI)
    {
      dlam -= TWO_PI;
    }
    if (dlam < -PI)
    {
      dlam += TWO_PI;
    }

    if (Latitude == 0.0)
    {
      *Easting = Ra * dlam + Grin_False_Easting;
      *Northing = 0.0;
    }
    else if (dlam == 0.0 || FLOAT_EQ(Latitude,MAX_LAT,.00001)  || FLOAT_EQ(Latitude,-MAX_LAT,.00001))
    {
      in_theta = fabs(TWO_OVER_PI * Latitude);

      if (in_theta > 1.0)
        in_theta = 1.0;
      else if (in_theta < -1.0)
        in_theta = -1.0;

      theta = asin(in_theta);
      *Easting = 0.0;
      *Northing = PI_Ra * tan(theta / 2) + Grin_False_Northing;
      if (Latitude < 0.0)
        *Northing *= -1.0;
    }
    else
    {
      aa = 0.5 * fabs(PI / dlam - dlam / PI);
      in_theta = fabs(TWO_OVER_PI * Latitude);

      if (in_theta > 1.0)
        in_theta = 1.0;
      else if (in_theta < -1.0)
        in_theta = -1.0;

      theta = asin(in_theta);
      sin_theta = sin(theta);
      cos_theta = cos(theta);
      gg = cos_theta / (sin_theta + cos_theta - 1);
      pp = gg * (2 / sin_theta - 1);
      aasqr = aa * aa;
      ppsqr = pp * pp;
      gg_MINUS_ppsqr = gg - ppsqr;
      ppsqr_PLUS_aasqr = ppsqr + aasqr;
      qq = aasqr + gg;
      *Easting = PI_Ra * (aa * (gg_MINUS_ppsqr) +
                          sqrt(aasqr * (gg_MINUS_ppsqr) * (gg_MINUS_ppsqr) -
                               (ppsqr_PLUS_aasqr) * (gg * gg - ppsqr))) /
                 (ppsqr_PLUS_aasqr) + Grin_False_Easting;
      if (dlam < 0.0)
        *Easting *= -1.0;
      *Northing = PI_Ra * (pp * qq - aa * sqrt ((aasqr + 1) * (ppsqr_PLUS_aasqr) - qq * qq)) /
                  (ppsqr_PLUS_aasqr) + Grin_False_Northing;
      if (Latitude < 0.0)
        *Northing *= -1.0;
    }
  }
  return (Error_Code);

} /* END OF Convert_Geodetic_To_Van_der_Grinten */
コード例 #13
0
long ossimCassiniProjection::Convert_Cassini_To_Geodetic(double Easting,
                                                         double Northing,
                                                         double *Latitude,
                                                         double *Longitude)const
{ /* Begin Convert_Cassini_To_Geodetic */
/*
 * The function Convert_Cassini_To_Geodetic converts Cassini projection
 * (easting and northing) coordinates to geodetic (latitude and longitude)
 * coordinates, according to the current ellipsoid and Cassini projection
 * coordinates.  If any errors occur, the error code(s) are returned by the
 * function, otherwise CASS_NO_ERROR is returned.
 *
 *    Easting           : Easting (X) in meters                  (input)
 *    Northing          : Northing (Y) in meters                 (input)
 *    Latitude          : Latitude (phi) in radians              (output)
 *    Longitude         : Longitude (lambda) in radians          (output)
 */

  double dx;     /* Delta easting - Difference in easting (easting-FE)      */
  double dy;     /* Delta northing - Difference in northing (northing-FN)   */
  double mu1;
  double sin2mu, sin4mu, sin6mu, sin8mu;
  double M1;
  double phi1;
  double tanphi1, sinphi1, cosphi1;
  double T1, T;
  double N1;
  double RD, R1;
  double DD, D2, D3, D4, D5;
//  const double epsilon = 1.0e-1;
  long Error_Code = CASS_NO_ERROR;

//   if ((Easting < (Cass_False_Easting + Cass_Min_Easting))
//       || (Easting > (Cass_False_Easting + Cass_Max_Easting)))
//   { /* Easting out of range */
//     Error_Code |= CASS_EASTING_ERROR;
//   }
//   if ((Northing < (Cass_False_Northing + Cass_Min_Northing - epsilon))
//       || (Northing > (Cass_False_Northing + Cass_Max_Northing + epsilon)))
//   { /* Northing out of range */
//     Error_Code |= CASS_NORTHING_ERROR;
//   }
  if (!Error_Code)
  { /* no errors */
    dy = Northing - Cass_False_Northing;
    dx = Easting - Cass_False_Easting;
    M1 = M0 + dy;
    mu1 = M1 / (Cass_a * c0); 

    sin2mu = CASS_COEFF_TIMES_SIN(a0, 2.0, mu1);
    sin4mu = CASS_COEFF_TIMES_SIN(a1, 4.0, mu1);
    sin6mu = CASS_COEFF_TIMES_SIN(a2, 6.0, mu1);
    sin8mu = CASS_COEFF_TIMES_SIN(a3, 8.0, mu1);
    phi1 = mu1 + sin2mu + sin4mu + sin6mu + sin8mu;

    if (FLOAT_EQ(phi1,PI_OVER_2,.00001))
    {
      *Latitude =  PI_OVER_2;
      *Longitude = Cass_Origin_Long;
    }
    else if (FLOAT_EQ(phi1,-PI_OVER_2,.00001))
    {
      *Latitude = -PI_OVER_2;
      *Longitude = Cass_Origin_Long;
    }
    else
    {
      tanphi1 = tan(phi1);
      sinphi1 = sin(phi1);
      cosphi1 = cos(phi1);
      T1 = tanphi1 * tanphi1;
      RD = CASS_RD(sinphi1);
      N1 = Cass_a / RD;
      R1 = N1 * One_Minus_es2 / (RD * RD);
      DD = dx / N1;
      D2 = DD * DD;
      D3 = D2 * DD;
      D4 = D3 * DD;
      D5 = D4 * DD;
      T = (1.0 + 3.0 * T1);
      *Latitude = phi1 - (N1 * tanphi1 / R1) * (D2 / 2.0 - T * D4 / 24.0);

      *Longitude = Cass_Origin_Long + (DD - T1 * D3 / 3.0 + T * T1 * D5 / 15.0) / cosphi1;

      if (*Latitude > PI_OVER_2)  /* force distorted values to 90, -90 degrees */
        *Latitude = PI_OVER_2;
      else if (*Latitude < -PI_OVER_2)
        *Latitude = -PI_OVER_2;

//       if (*Longitude > PI)
//         *Longitude -= TWO_PI;
//       if (*Longitude < -PI)
//         *Longitude += TWO_PI;

      if (*Longitude > M_PI)  /* force distorted values to 180, -180 degrees */
        *Longitude = M_PI;
      else if (*Longitude < -M_PI)
        *Longitude = -M_PI;
    }
    if (fabs(*Longitude - Cass_Origin_Long) > (4.0 * M_PI / 180.0))
    { /* Distortion will result if Longitude is more than 4 degrees from the Central Meridian */
      Error_Code |= CASS_LON_WARNING;
    }
  }
  return (Error_Code);
} /* End Convert_Cassini_To_Geodetic */
コード例 #14
0
ファイル: test_fft_0.c プロジェクト: vikaschouhan/ppcsimbooke
int main()
{
  int i;                    /* generic index */
  int N;                    /* number of points in FFT */
  double (*x)[2];           /* pointer to time-domain samples */
  double (*X)[2];           /* pointer to frequency-domain samples */
  double (*Xexp)[2];        /* pointer expected frequency-domain samples */
  double epsilon;           /* tolerance factor */

  /* Initialize parameters */
  N=2;

  /* Check that N = 2^n for some integer n >= 1. */
  if(N >= 2)
    {
      i = N;
      while(i==2*(i/2)) i = i/2;  /* While i is even, factor out a 2. */
    }  /* For N >=2, we now have N = 2^n iff i = 1. */
  if(N < 2 || i != 1)
    {
      printf(", which does not equal 2^n for an integer n >= 1.");
      exit(EXIT_FAILURE);
    }

  /* Allocate time- and frequency-domain memory. */
  x    = malloc(2 * N * sizeof(double));
  X    = malloc(2 * N * sizeof(double));
  Xexp = malloc(2 * N * sizeof(double));

  /* Initialize time domain samples */
  x[0][0] = 1.2 ; x[0][1] = 3.4;
  x[1][0] = 5.6 ; x[1][1] = 0.4;

  /* Initialize freq domain expected samples */
  Xexp[0][0] = 6.8  ; Xexp[0][1] = 3.8 ;
  Xexp[1][0] = -4.4 ; Xexp[1][1] = 3.0 ;

  epsilon = 0.1;

  /* Calculate FFT. */
  fft(N, x, X);

  /* check results */
  for(i=0; i<N; i++){
      if(!(FLOAT_EQ(X[i][0], Xexp[i][0], epsilon)  && FLOAT_EQ(X[i][1], Xexp[i][1], epsilon))){
          LOG_FAIL();
      }
  }

  /* Print time-domain samples and resulting frequency-domain samples. */
#if 0
  printf("\nx(n):");
  for(i=0; i<N; i++) printf("\n   n=%d: %12f %12f", i, x[i][0], x[i][1]);
  printf("\nX(k):");
  for(i=0; i<N; i++) printf("\n   k=%d: %12f %12f", i, X[i][0], X[i][1]);

  printf("\n");
#endif

  /* Free memory. */
  free(x);
  free(X);

  LOG_PASS();

}
コード例 #15
0
ファイル: sce40.c プロジェクト: tangxl0591/camera
/** sce_found_boundaries:
 *    @triangles: set of SCE triangles
 *    @line: line for SCE interpolation
 *    @t_pos boundary in positive direction of shift vector
 *    @t_neg boundary in negative direction of shift vector
 *
 *  This function runs in ISP HW thread context.
 *
 *  This function finds boundaries in both directions from origin point between
 *  which point position can be interpolated according system team documentation
 *
 *  Return:   0 - Success
 *           -1 - Numbers from chromatix are inconsistent: triangles
 *                don't form proper poligon.
 **/
static int sce_found_boundaries(sce_cr_cb_triangle_set *triangles,
  ISP_Skin_enhan_line *line, double *t_pos, double*t_neg)
{
  cr_cb_triangle *array[5];
  double ints_t[2], tmp;
  int idx_t = 0;
  ISP_Skin_enhan_line temp_line;
  int i, j;
  boolean is_vertex;

  array[0] = &triangles->triangle1;
  array[1] = &triangles->triangle2;
  array[2] = &triangles->triangle3;
  array[3] = &triangles->triangle4;
  array[4] = &triangles->triangle5;

  for(i = 0; i < 5; i++) {
    sce_find_line_by_two_points(&temp_line, &array[i]->point2, &array[i]->point3);
    if(sce_find_intersection(line, &temp_line, &tmp)){
      is_vertex = FALSE;
      for(j = 0; j < idx_t; j++)
        if(j<2 && FLOAT_EQ(ints_t[j], tmp)) {
          /* intersection point is vertex */
          is_vertex = TRUE;
          break;
        }
      if(is_vertex)
        continue;
      if(idx_t > 1){
        /* line intersects pentagon in more than two points  - this should not
           happen unless chromatix is incorrect */
        CDBG_ERROR("%s: Error: too many intersections", __func__);
        return -1;
      }
      ints_t[idx_t] = tmp;
      idx_t++;
    }
  }

  if(idx_t < 2){
    /* line intersects pentagon in less than two points  - this should not
        happen unless chromatix is incorrect */
    CDBG_ERROR("%s: Error: less than two intersections %d", __func__, idx_t);
    return -1;
  }

  if(ints_t[0] > 0){
    *t_pos = ints_t[0];
    *t_neg = ints_t[1];
  } else {
    *t_pos = ints_t[1];
    *t_neg = ints_t[0];
  }

  if((*t_pos < 0) || (*t_neg > 0)) {
    /* intersections are on the same side of central point  - this should not
       happen unless chromatix is incorrect */
    CDBG_ERROR("%s: Error: intersections are on the same side of central point",
               __func__);
    return -1;
  }

  /* According documentation boundaries should be at two thirds of distance
     between central and intersection points. */
  *t_pos *= 2.0/3.0;
  *t_neg *= 2.0/3.0;

  ISP_DBG(ISP_MOD_SCE, "%s: Boundaries %lf,%lf and %lf,%lf",__func__,
    line->point0.cr + line->shift_cr * *t_pos,
    line->point0.cb + line->shift_cb * *t_pos,
    line->point0.cr + line->shift_cr * *t_neg,
    line->point0.cb + line->shift_cb * *t_neg);

  return 0;
}