/**
* この関数は、指定サイズのオブジェクト配列を作ります。
* @param i_length
* 作成する配列の長さ
* @return
* 新しい配列。
*/
public static NyARVecLinear2d[] createArray(int i_length)
{
NyARVecLinear2d[] r = new NyARVecLinear2d[i_length];
for (int i = 0; i < i_length; i++)
{
r[i] = new NyARVecLinear2d();
}
return r;
}
/**
* この関数は、直線との交点を求めます。
* @param i_vector1
* 交点を求める直線
* @param o_point
* 交点座標を得るオブジェクト。
* @return
* 交点が求まると、trueを返します。
*/
public bool crossPos(NyARVecLinear2d i_vector1, NyARDoublePoint2d o_point)
{
double a1 = i_vector1.dy;
double b1 = -i_vector1.dx;
double c1 = (i_vector1.dx * i_vector1.y - i_vector1.dy * i_vector1.x);
double a2 = this.dy;
double b2 = -this.dx;
double c2 = (this.dx * this.y - this.dy * this.x);
double w1 = a1 * b2 - a2 * b1;
if (w1 == 0.0)
{
return false;
}
o_point.x = (b1 * c2 - b2 * c1) / w1;
o_point.y = (a2 * c1 - a1 * c2) / w1;
return true;
}
/**
* この関数は、オブジェクトの値をインスタンスにセットします。
* @param i_value
* コピー元のオブジェクト
*/
public void setValue(NyARVecLinear2d i_value)
{
this.dx = i_value.dx;
this.dy = i_value.dy;
this.x = i_value.x;
this.y = i_value.y;
}
/**
* この関数は、この直線と引数の直線とが作るCos値を返します。
* @param i_v1
* 直線を格納したオブジェクト
* @return
* ２直線のCOS値（radian）
*/
public double getVecCos(NyARVecLinear2d i_v1)
{
double x1 = i_v1.dx;
double y1 = i_v1.dy;
double x2 = this.dx;
double y2 = this.dy;
double d = (x1 * x2 + y1 * y2) / Math.Sqrt((x1 * x1 + y1 * y1) * (x2 * x2 + y2 * y2));
return d;
}
/**
* この関数は、法線を計算します。
* 通過点は変更しません。
* @param i_src
* 元のインスタンスを指定します。この値には、thisを指定できます。
*/
public void normalVec(NyARVecLinear2d i_src)
{
double w = this.dx;
this.dx = i_src.dy;
this.dy = -w;
}
/**
* この関数は、座標を観察座標系から理想座標系へ変換します。
* @param ix
* 変換元の座標
* @param iy
* 変換元の座標
* @param o_veclinear
* 変換後の座標を受け取るオブジェクト。{@link NyARVecLinear2d#x}と{@link NyARVecLinear2d#y}のみに値をセットします。
*/
public void observ2Ideal(double ix, double iy, NyARVecLinear2d o_veclinear)
{
double z02, z0, p, q, z, px, py, opttmp_1;
double d0 = this._f0;
double d1 = this._f1;
px = ix - d0;
py = iy - d1;
p = this._f2 / 100000000.0;
z02 = px * px + py * py;
q = z0 = Math.Sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py);
for (int i = 1; ; i++)
{
if (z0 != 0.0)
{
// Optimize opttmp_1
opttmp_1 = p * z02;
z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1);
px = px * z / z0;
py = py * z / z0;
}
else
{
px = 0.0;
py = 0.0;
break;
}
if (i == PD_LOOP)
{
break;
}
z02 = px * px + py * py;
z0 = Math.Sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);
}
o_veclinear.x = px / this._f3 + d0;
o_veclinear.y = py / this._f3 + d1;
return;
}
/**
* この関数は、この直線と引数の直線とが作るCos値の絶対値を返します。
* @param i_v1
* 直線を格納したオブジェクト
* @return
* ２直線のCOS値の絶対値（radian）
*/
public double getAbsVecCos(NyARVecLinear2d i_v1)
{
double d = getVecCos(i_v1);
return d >= 0 ? d : -d;
}
/**
* RECT範囲内の画素ベクトルの合計値と、ベクトルのエッジ中心を取得します。 320*240の場合、
* RECTの範囲は(x>=0 && x<319 x+w>=0 && x+w<319),(y>=0 && y<239 x+w>=0 && x+w<319)となります。
* @param ix
* ピクセル取得を行う位置を設定します。
* @param iy
* ピクセル取得を行う位置を設定します。
* @param iw
* ピクセル取得を行う範囲を設定します。
* @param ih
* ピクセル取得を行う範囲を設定します。
* @param o_posvec
* エッジ中心とベクトルを返します。
* @return
* ベクトルの強度を返します。強度値は、差分値の二乗の合計です。
*/
public override int getAreaVector33(int ix, int iy, int iw, int ih, NyARVecLinear2d o_posvec)
{
Debug.Assert (ih >= 3 && iw >= 3);
Debug.Assert ((ix >= 0) && (iy >= 0) && (ix + iw) <= this._ref_base_raster.getWidth() && (iy + ih) <= this._ref_base_raster.getHeight());
int[] buf =(int[])this._ref_base_raster.getBuffer();
int stride =this._ref_base_raster.getWidth();
// x=(Σ|Vx|*Xn)/n,y=(Σ|Vy|*Yn)/n
// x=(ΣVx)^2/(ΣVx+ΣVy)^2,y=(ΣVy)^2/(ΣVx+ΣVy)^2
int sum_x, sum_y, sum_wx, sum_wy, sum_vx, sum_vy;
sum_x = sum_y = sum_wx = sum_wy = sum_vx = sum_vy = 0;
int lw=iw - 3;
int vx, vy;
for (int i = ih - 3; i >= 0; i--) {
int idx_0 = stride * (i + 1 + iy) + (iw - 3 + 1 + ix);
for (int i2 = lw; i2 >= 0; i2--){
// 1ビット分のベクトルを計算
int idx_p1 = idx_0 + stride;
int idx_m1 = idx_0 - stride;
int b = buf[idx_m1 - 1];
int d = buf[idx_m1 + 1];
int h = buf[idx_p1 - 1];
int f = buf[idx_p1 + 1];
vx = ((buf[idx_0 + 1] - buf[idx_0 - 1]) >> 1)+ ((d - b + f - h) >> 2);
vy = ((buf[idx_p1] - buf[idx_m1]) >> 1)+ ((f - d + h - b) >> 2);
idx_0--;
// 加重はvectorの絶対値
int wx = vx * vx;
int wy = vy * vy;
sum_wx += wx; //加重値
sum_wy += wy; //加重値
sum_vx += wx * vx; //加重*ベクトルの積
sum_vy += wy * vy; //加重*ベクトルの積
sum_x += wx * (i2 + 1);//位置
sum_y += wy * (i + 1); //
}
}
//x,dx,y,dyの計算
double xx,yy;
if (sum_wx == 0) {
xx = ix + (iw >> 1);
o_posvec.dx = 0;
} else {
xx = ix+(double) sum_x / sum_wx;
o_posvec.dx = (double) sum_vx / sum_wx;
}
if (sum_wy == 0) {
yy = iy + (ih >> 1);
o_posvec.dy = 0;
} else {
yy = iy+(double) sum_y / sum_wy;
o_posvec.dy = (double) sum_vy / sum_wy;
}
//必要なら歪みを解除
if(this._factor!=null){
this._factor.observ2Ideal(xx, yy, this.__tmp);
o_posvec.x = this.__tmp.x;
o_posvec.y = this.__tmp.y;
}else{
o_posvec.x=xx;
o_posvec.y=yy;
}
//加重平均の分母を返却
return sum_wx+sum_wy;
}
public override int getAreaVector22(int ix, int iy, int iw, int ih,NyARVecLinear2d o_posvec)
{
Debug.Assert (ih >= 3 && iw >= 3);
Debug.Assert ((ix >= 0) && (iy >= 0) && (ix + iw) <= this._ref_base_raster.getWidth() && (iy + ih) <= this._ref_base_raster.getHeight());
int[] buf =(int[])this._ref_base_raster.getBuffer();
int stride =this._ref_base_raster.getWidth();
int sum_x, sum_y, sum_wx, sum_wy, sum_vx, sum_vy;
sum_x = sum_y = sum_wx = sum_wy = sum_vx = sum_vy = 0;
int vx, vy;
int ll=iw-1;
for (int i = 0; i<ih-1; i++) {
int idx_0 = stride * (i+iy) + ix+1;
int a=buf[idx_0-1];
int b=buf[idx_0];
int c=buf[idx_0+stride-1];
int d=buf[idx_0+stride];
for (int i2 = 0; i2<ll; i2++){
// 1ビット分のベクトルを計算
vx=(b-a+d-c)>>2;
vy=(c-a+d-b)>>2;
idx_0++;
a=b;
c=d;
b=buf[idx_0];
d=buf[idx_0+stride];
// 加重はvectorの絶対値
int wx = vx * vx;
sum_wx += wx; //加重値
sum_vx += wx * vx; //加重*ベクトルの積
sum_x += wx * i2;//位置
int wy = vy * vy;
sum_wy += wy; //加重値
sum_vy += wy * vy; //加重*ベクトルの積
sum_y += wy * i; //
}
}
//x,dx,y,dyの計算
double xx,yy;
if (sum_wx == 0) {
xx = ix + (iw >> 1);
o_posvec.dx = 0;
} else {
xx = ix+(double) sum_x / sum_wx;
o_posvec.dx = (double) sum_vx / sum_wx;
}
if (sum_wy == 0) {
yy = iy + (ih >> 1);
o_posvec.dy = 0;
} else {
yy = iy+(double) sum_y / sum_wy;
o_posvec.dy = (double) sum_vy / sum_wy;
}
//必要なら歪みを解除
if(this._factor!=null){
this._factor.observ2Ideal(xx, yy, this.__tmp);
o_posvec.x = this.__tmp.x;
o_posvec.y = this.__tmp.y;
}
else
{
o_posvec.x=xx;
o_posvec.y=yy;
}
//加重平均の分母を返却
return sum_wx+sum_wy;
}
/**
* この関数は、{@link NyARVecLinear2d}を正規化された直線式に変換して、インスタンスへセットします。
* @param i_vector
* セットするオブジェクト
*/
public bool setVectorWithNormalize(NyARVecLinear2d i_vector)
{
double dx = i_vector.dx;
double dy = i_vector.dy;
double sq = Math.Sqrt(dx * dx + dy * dy);
if (sq == 0)
{
return false;
}
sq = 1 / sq;
this.a = dy * sq;
this.b = -dx * sq;
this.c = -(this.a * i_vector.x + this.b * i_vector.y);
return true;
}
/**
* この関数は、{@link NyARVecLinear2d}を直線式に変換して、インスタンスへセットします。
* @param i_vector
* セットするオブジェクト
*/
public void setVector(NyARVecLinear2d i_vector)
{
this.a = i_vector.dy;
this.b = -i_vector.dx;
this.c = (i_vector.dx * i_vector.y - i_vector.dy * i_vector.x);
return;
}
public abstract int getAreaVector33(int ix, int iy, int iw, int ih, NyARVecLinear2d o_posvec);