//--------------------------------
//
//--------------------------------
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
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;
}
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;
}
/** 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;
}
/*
* 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 */
/* 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);
}
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);
}
/*
* 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 */
/*
* 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 */
/*
* 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 */
/* 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);
}
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 */
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 */
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();
}
/** 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;
}