1 package jp.nyatla.nyartoolkit.sandbox.x2;
\r
3 import jp.nyatla.nyartoolkit.core.param.*;
\r
4 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;
\r
5 import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;
\r
7 public class NyARFixedFloatIdeal2Observ
\r
9 private double[] _factor=new double[4];
\r
10 public NyARFixedFloatIdeal2Observ(NyARCameraDistortionFactor i_distfactor)
\r
12 i_distfactor.getValue(this._factor);
\r
15 public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size)
\r
18 final double d0 = this._factor[0];
\r
19 final double d1 = this._factor[1];
\r
20 final double d3 = this._factor[3];
\r
21 final double d2_w = this._factor[2] / 100000000.0;
\r
22 for (int i = 0; i < i_size; i++) {
\r
23 x = (i_in[i].x - d0) * d3;
\r
24 y = (i_in[i].y - d1) * d3;
\r
25 if (x == 0.0 && y == 0.0) {
\r
26 o_out[i].x = (long)(d0*NyMath.FIXEDFLOAT16_1);
\r
27 o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1);
\r
29 final double d = 1.0 - d2_w * (x * x + y * y);
\r
30 o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1);
\r
31 o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1);
\r
36 public void ideal2Observ(final NyARFixedFloat16Point2d i_in, NyARFixedFloat16Point2d o_out)
\r
38 final double f0=this._factor[0];
\r
39 final double f1=this._factor[1];
\r
40 final double x = (((double)i_in.x/NyMath.FIXEDFLOAT16_1) - f0) * this._factor[3];
\r
41 final double y = (((double)i_in.y/NyMath.FIXEDFLOAT16_1) - f1) * this._factor[3];
\r
42 if (x == 0.0 && y == 0.0) {
\r
43 o_out.x = (long)(f0*NyMath.FIXEDFLOAT16_1);
\r
44 o_out.y = (long)(f1*NyMath.FIXEDFLOAT16_1);
\r
46 final double d = 1.0 - this._factor[2] / 100000000.0 * (x * x + y * y);
\r
47 o_out.x = (long)((x * d + f0)*NyMath.FIXEDFLOAT16_1);
\r
48 o_out.y = (long)((y * d + f1)*NyMath.FIXEDFLOAT16_1);
\r