/**
* 中心位置と辺長から、オフセット情報を作成して設定する。
* @param i_width
* FF16で渡すこと！
* @param i_center
*/
public void setSquare(long i_width, NyARFixedFloat16Point2d i_center)
{
long w_2 = i_width >> 1;
NyARFixedFloat16Point3d vertex3d_ptr;
vertex3d_ptr = this.vertex[0];
vertex3d_ptr.x = -w_2;
vertex3d_ptr.y = w_2;
vertex3d_ptr.z = 0;
vertex3d_ptr = this.vertex[1];
vertex3d_ptr.x = w_2;
vertex3d_ptr.y = w_2;
vertex3d_ptr.z = 0;
vertex3d_ptr = this.vertex[2];
vertex3d_ptr.x = w_2;
vertex3d_ptr.y = -w_2;
vertex3d_ptr.z = 0;
vertex3d_ptr = this.vertex[3];
vertex3d_ptr.x = -w_2;
vertex3d_ptr.y = -w_2;
vertex3d_ptr.z = 0;
this.point.x = -i_center.x;
this.point.y = -i_center.y;
this.point.z = 0;
return;
}
public new static NyARFixedFloat16Point2d[] createArray(int i_number)
{
NyARFixedFloat16Point2d[] ret = new NyARFixedFloat16Point2d[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARFixedFloat16Point2d();
}
return ret;
}
public new static NyARFixedFloat16Point2d[] createArray(int i_number)
{
NyARFixedFloat16Point2d[] ret = new NyARFixedFloat16Point2d[i_number];
for (int i = 0; i < i_number; i++)
{
ret[i] = new NyARFixedFloat16Point2d();
}
return(ret);
}
public void ideal2Observ(NyARFixedFloat16Point2d i_in, NyARFixedFloat16Point2d o_out)
{
double f0 = this._factor[0];
double f1 = this._factor[1];
double x = (((double)i_in.x / NyMath.FIXEDFLOAT16_1) - f0) * this._factor[3];
double y = (((double)i_in.y / NyMath.FIXEDFLOAT16_1) - f1) * this._factor[3];
if (x == 0.0 && y == 0.0)
{
o_out.x = (long)(f0 * NyMath.FIXEDFLOAT16_1);
o_out.y = (long)(f1 * NyMath.FIXEDFLOAT16_1);
}
else
{
double d = 1.0 - this._factor[2] / 100000000.0 * (x * x + y * y);
o_out.x = (long)((x * d + f0) * NyMath.FIXEDFLOAT16_1);
o_out.y = (long)((y * d + f1) * NyMath.FIXEDFLOAT16_1);
}
return;
}
public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size)
{
double x, y;
double d0 = this._factor[0];
double d1 = this._factor[1];
double d3 = this._factor[3];
double d2_w = this._factor[2] / 100000000.0;
for (int i = 0; i < i_size; i++) {
x = (i_in[i].x - d0) * d3;
y = (i_in[i].y - d1) * d3;
if (x == 0.0 && y == 0.0) {
o_out[i].x = (long)(d0*NyMath.FIXEDFLOAT16_1);
o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1);
} else {
double d = 1.0 - d2_w * (x * x + y * y);
o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1);
o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1);
}
}
return;
}