int ar2ScreenCoord2MarkerCoord( const ARParamLT *cparamLT, const float trans[3][4], const float sx, const float sy, float *mx, float *my )
{
float ix, iy;
float wtrans[3][4];
float c11, c12, c21, c22, b1, b2;
float m;
if( cparamLT == NULL ) {
c11 = trans[2][0] * sx - trans[0][0];
c12 = trans[2][1] * sx - trans[0][1];
c21 = trans[2][0] * sy - trans[1][0];
c22 = trans[2][1] * sy - trans[1][1];
b1 = trans[0][3] - trans[2][3] * sx;
b2 = trans[1][3] - trans[2][3] * sy;
}
else {
if( arParamObserv2IdealLTf( &cparamLT->paramLTf, sx, sy, &ix, &iy) < 0 ) return -1;
arUtilMatMuldff( cparamLT->param.mat, trans, wtrans );
c11 = wtrans[2][0] * ix - wtrans[0][0];
c12 = wtrans[2][1] * ix - wtrans[0][1];
c21 = wtrans[2][0] * iy - wtrans[1][0];
c22 = wtrans[2][1] * iy - wtrans[1][1];
b1 = wtrans[0][3] - wtrans[2][3] * ix;
b2 = wtrans[1][3] - wtrans[2][3] * iy;
}
m = c11 * c22 - c12 * c21;
if( m == 0.0F ) return -1;
*mx = (c22 * b1 - c12 * b2) / m;
*my = (c11 * b2 - c21 * b1) / m;
return 0;
}
int ar2MarkerCoord2ScreenCoord2(const ARParamLT *cparamLT, const float trans[3][4], const float mx, const float my, float *sx, float *sy)
{
float wtrans[3][4];
float hx, hy, h;
float ix, iy;
float ix1, iy1;
if (cparamLT != NULL)
{
arUtilMatMuldff(cparamLT->param.mat, trans, wtrans);
hx = wtrans[0][0] * mx
+ wtrans[0][1] * my
+ wtrans[0][3];
hy = wtrans[1][0] * mx
+ wtrans[1][1] * my
+ wtrans[1][3];
h = wtrans[2][0] * mx
+ wtrans[2][1] * my
+ wtrans[2][3];
ix = hx / h;
iy = hy / h;
}
else
{
hx = trans[0][0] * mx
+ trans[0][1] * my
+ trans[0][3];
hy = trans[1][0] * mx
+ trans[1][1] * my
+ trans[1][3];
h = trans[2][0] * mx
+ trans[2][1] * my
+ trans[2][3];
*sx = hx / h;
*sy = hy / h;
return 0;
}
if (arParamIdeal2ObservLTf(&cparamLT->paramLTf, ix, iy, sx, sy) < 0)
return -1;
if (arParamObserv2IdealLTf(&cparamLT->paramLTf, *sx, *sy, &ix1, &iy1) < 0)
return -1;
if ((ix - ix1) * (ix - ix1) + (iy - iy1) * (iy - iy1) > 1.0F)
return -1;
return 0;
}
int ar2GetResolution2( ARParam *cparam, float trans[3][4], float pos[2], float dpi[2] )
{
float mat[3][4];
float mx, my, hx, hy, h;
float x0, y0, x1, y1, x2, y2, d1, d2;
if( cparam != NULL ) {
arUtilMatMuldff( cparam->mat, trans, mat );
mx = pos[0];
my = pos[1];
hx = mat[0][0] * mx + mat[0][1] * my + mat[0][3];
hy = mat[1][0] * mx + mat[1][1] * my + mat[1][3];
h = mat[2][0] * mx + mat[2][1] * my + mat[2][3];
x0 = hx / h;
y0 = hy / h;
mx = pos[0] + 10.0F;
my = pos[1];
hx = mat[0][0] * mx + mat[0][1] * my + mat[0][3];
hy = mat[1][0] * mx + mat[1][1] * my + mat[1][3];
h = mat[2][0] * mx + mat[2][1] * my + mat[2][3];
x1 = hx / h;
y1 = hy / h;
mx = pos[0];
my = pos[1] + 10.0F;
hx = mat[0][0] * mx + mat[0][1] * my + mat[0][3];
hy = mat[1][0] * mx + mat[1][1] * my + mat[1][3];
h = mat[2][0] * mx + mat[2][1] * my + mat[2][3];
x2 = hx / h;
y2 = hy / h;
}
else {
mx = pos[0];
my = pos[1];
hx = trans[0][0] * mx + trans[0][1] * my + trans[0][3];
hy = trans[1][0] * mx + trans[1][1] * my + trans[1][3];
h = trans[2][0] * mx + trans[2][1] * my + trans[2][3];
x0 = hx / h;
y0 = hy / h;
mx = pos[0] + 10.0F;
my = pos[1];
hx = trans[0][0] * mx + trans[0][1] * my + trans[0][3];
hy = trans[1][0] * mx + trans[1][1] * my + trans[1][3];
h = trans[2][0] * mx + trans[2][1] * my + trans[2][3];
x1 = hx / h;
y1 = hy / h;
mx = pos[0];
my = pos[1] + 10.0F;
hx = trans[0][0] * mx + trans[0][1] * my + trans[0][3];
hy = trans[1][0] * mx + trans[1][1] * my + trans[1][3];
h = trans[2][0] * mx + trans[2][1] * my + trans[2][3];
x2 = hx / h;
y2 = hy / h;
}
d1 = (x1-x0)*(x1-x0) + (y1-y0)*(y1-y0);
d2 = (x2-x0)*(x2-x0) + (y2-y0)*(y2-y0);
if( d1 < d2 ) {
dpi[0] = sqrtf(d2) * 2.54F;
dpi[1] = sqrtf(d1) * 2.54F;
}
else {
dpi[0] = sqrtf(d1) * 2.54F;
dpi[1] = sqrtf(d2) * 2.54F;
}
return 0;
}
