void QuatToEuler2(float x,float y,float z,float w,float &pitch,float& yaw,float& roll){
float sint=(2.0*w*y)-(2.0*x*z);
float cost_temp=1.0-(sint*sint);
float cost;
if (abs(cost_temp)>QuatToEulerAccuracy){
cost=sqrt(cost_temp);
}else{
cost=0.0;
}
float sinv,cosv,sinf,cosf;
if (abs(cost)>QuatToEulerAccuracy){
sinv=((2.0*y*z)+(2.0*w*x))/cost;
cosv=(1.0-(2.0*x*x)-(2.0*y*y))/cost;
sinf=((2.0*x*y)+(2.0*w*z))/cost;
cosf=(1.0-(2.0*y*y)-(2.0*z*z))/cost;
}else{
sinv=(2.0*w*x)-(2.0*y*z);
cosv=1.0-(2.0*x*x)-(2.0*z*z);
sinf=0.0;
cosf=1.0;
}
pitch=atan2deg(sint,cost);
yaw=atan2deg(sinf,cosf);
roll=-atan2deg(sinv,cosv);
}
double LCMSEXPORT cmsCIE2000DeltaE(LPcmsCIELab Lab1, LPcmsCIELab Lab2,
double Kl, double Kc, double Kh)
{
double L1 = Lab1->L;
double a1 = Lab1->a;
double b1 = Lab1->b;
double C = sqrt( Sqr(a1) + Sqr(b1) );
double Ls = Lab2 ->L;
double as = Lab2 ->a;
double bs = Lab2 ->b;
double Cs = sqrt( Sqr(as) + Sqr(bs) );
double G = 0.5 * ( 1 - sqrt(pow((C + Cs) / 2 , 7.0) / (pow((C + Cs) / 2, 7.0) + pow(25.0, 7.0) ) ));
double a_p = (1 + G ) * a1;
double b_p = b1;
double C_p = sqrt( Sqr(a_p) + Sqr(b_p));
double h_p = atan2deg(a_p, b_p);
double a_ps = (1 + G) * as;
double b_ps = bs;
double C_ps = sqrt(Sqr(a_ps) + Sqr(b_ps));
double h_ps = atan2deg(a_ps, b_ps);
double meanC_p =(C_p + C_ps) / 2;
double meanh_p = fabs(h_ps-h_p) <= 180 ? (h_ps + h_p)/2 : (h_ps+h_p-360)/2;
double delta_h = fabs(h_p - h_ps) <= 180 ? fabs(h_p - h_ps) : 360 - fabs(h_p - h_ps);
double delta_L = fabs(L1 - Ls);
double delta_C = fabs(C_p - C_ps);
double delta_H =2 * sqrt(C_ps*C_p) * sin(RADIANES(delta_h) / 2);
double T = 1 - 0.17 * cos(RADIANES(meanh_p-30))
+ 0.24 * cos(RADIANES(2*meanh_p))
+ 0.32 * cos(RADIANES(3*meanh_p + 6))
- 0.2 * cos(RADIANES(4*meanh_p - 63));
double Sl = 1 + (0.015 * Sqr((Ls + L1) /2- 50) )/ sqrt(20 + Sqr( (Ls+L1)/2 - 50) );
double Sc = 1 + 0.045 * (C_p + C_ps)/2;
double Sh = 1 + 0.015 * ((C_ps + C_p)/2) * T;
double delta_ro = 30 * exp( -Sqr(((meanh_p - 275 ) / 25)));
double Rc = 2 * sqrt(( pow(meanC_p, 7.0) )/( pow(meanC_p, 7.0) + pow(25.0, 7.0)));
double Rt = -sin(2 * RADIANES(delta_ro)) * Rc;
double deltaE00 = sqrt( Sqr(delta_L /(Sl * Kl)) +
Sqr(delta_C/(Sc * Kc)) +
Sqr(delta_H/(Sh * Kh)) +
Rt*(delta_C/(Sc * Kc)) * (delta_H / (Sh * Kh)));
return deltaE00;
}
