tags/2.3.1/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatIdeal2Observ.java

   1 package jp.nyatla.nyartoolkit.sandbox.x2;\r
   2 \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
   6 \r
   7 public class NyARFixedFloatIdeal2Observ\r
   8 {\r
   9         private double[] _factor=new double[4];\r
  10         public NyARFixedFloatIdeal2Observ(NyARCameraDistortionFactor i_distfactor)\r
  11         {\r
  12                 i_distfactor.getValue(this._factor);\r
  13                 return;\r
  14         }       \r
  15         public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size)\r
  16         {\r
  17                 double x, y;\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
  28                         } else {\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
  32                         }\r
  33                 }\r
  34                 return;\r
  35         }\r
  36         public void ideal2Observ(final NyARFixedFloat16Point2d i_in, NyARFixedFloat16Point2d o_out)\r
  37         {\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
  45                 } else {\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
  49                 }\r
  50                 return;\r
  51         }       \r
  52 }\r