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; }