//ベクトルの類似度判定式
private static bool checkVecCos(VecLinearCoordinates.VecLinearCoordinatePoint i_current_vec, VecLinearCoordinates.VecLinearCoordinatePoint i_prev_vec, double i_ave_dx, double i_ave_dy)
{
double x1 = i_current_vec.dx;
double y1 = i_current_vec.dy;
double n = (x1 * x1 + y1 * y1);
//平均ベクトルとこのベクトルがCOS_DEG_20未満であることを確認(pos_ptr.getAbsVecCos(i_ave_dx,i_ave_dy)<NyARMath.COS_DEG_20 と同じ)
double d;
d = (x1 * i_ave_dx + y1 * i_ave_dy) / NyARMath.COS_DEG_20;
if (d * d < (n * (i_ave_dx * i_ave_dx + i_ave_dy * i_ave_dy)))
{
//隣接ベクトルとこのベクトルが5度未満であることを確認(pos_ptr.getAbsVecCos(i_prev_vec)<NyARMath.COS_DEG_5と同じ)
d = (x1 * i_prev_vec.dx + y1 * i_prev_vec.dy) / NyARMath.COS_DEG_5;
if (d * d < n * (i_prev_vec.dx * i_prev_vec.dx + i_prev_vec.dy * i_prev_vec.dy))
{
return(true);
}
}
return(false);
}
/**
* ノイズらしいベクトルを無視しながら最小二乗法でベクトルを統合する関数
* @param i_points
* @param i_number_of_data
* @param o_dest
* @param i_scale_th
* @return
*/
private bool leastSquaresWithNormalize(VecLinearCoordinates.VecLinearCoordinatePoint[] i_points, int i_number_of_data, VecLinearCoordinates.VecLinearCoordinatePoint o_dest, double i_scale_th)
{
int i;
int num = 0;
double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;
for (i = i_number_of_data - 1; i >= 0; i--)
{
VecLinearCoordinates.VecLinearCoordinatePoint ptr = i_points[i];
//規定より小さいスケールは除外なう
if (ptr.scalar < i_scale_th)
{
continue;
}
double xw = ptr.x;
sum_xy += xw * ptr.y;
sum_x += xw;
sum_y += ptr.y;
sum_x2 += xw * xw;
num++;
}
if (num < 3)
{
return(false);
}
double la = -(num * sum_x2 - sum_x * sum_x);
double lb = -(num * sum_xy - sum_x * sum_y);
double cc = (sum_x2 * sum_y - sum_xy * sum_x);
double lc = -(la * sum_x + lb * sum_y) / num;
//交点を計算
double w1 = -lb * lb - la * la;
if (w1 == 0.0)
{
return(false);
}
o_dest.x = ((la * lc - lb * cc) / w1);
o_dest.y = ((la * cc + lb * lc) / w1);
o_dest.dy = -lb;
o_dest.dx = -la;
o_dest.scalar = num;
return(true);
}
/**
* 配列の前方に、似たベクトルを集めます。似たベクトルの判定基準は、2線の定義点における直線の法線上での距離の二乗和です。
* ベクトルの統合と位置情報の計算には、加重平均を用います。
* @param i_vector
* 編集するオブジェクトを指定します。
*/
public void margeResembleCoords(VecLinearCoordinates i_vector)
{
VecLinearCoordinates.VecLinearCoordinatePoint[] items = i_vector.items;
NyARLinear l1 = this._l1;
NyARLinear l2 = this._l2;
NyARDoublePoint2d p = this._p;
for (int i = i_vector.length - 1; i >= 0; i--)
{
VecLinearCoordinates.VecLinearCoordinatePoint target1 = items[i];
if (target1.scalar == 0)
{
continue;
}
double rdx = target1.dx;
double rdy = target1.dy;
double rx = target1.x;
double ry = target1.y;
l1.setVector(target1);
double s_tmp = target1.scalar;
target1.dx *= s_tmp;
target1.dy *= s_tmp;
target1.x *= s_tmp;
target1.y *= s_tmp;
for (int i2 = i - 1; i2 >= 0; i2--)
{
VecLinearCoordinates.VecLinearCoordinatePoint target2 = items[i2];
if (target2.scalar == 0)
{
continue;
}
if (target2.getVecCos(rdx, rdy) >= _SQ_ANG_TH)
{
// それぞれの代表点から法線を引いて、相手の直線との交点を計算する。
l2.setVector(target2);
l1.normalLineCrossPos(rx, ry, l2, p);
double wx, wy;
double l = 0;
// 交点間の距離の合計を計算。lに2*dist^2を得る。
wx = (p.x - rx);
wy = (p.y - ry);
l += wx * wx + wy * wy;
l2.normalLineCrossPos(target2.x, target2.y, l2, p);
wx = (p.x - target2.x);
wy = (p.y - target2.y);
l += wx * wx + wy * wy;
// 距離が一定値以下なら、マージ
if (l > _SQ_DIFF_DOT_TH)
{
continue;
}
// 似たようなベクトル発見したら、後方のアイテムに値を統合。
s_tmp = target2.scalar;
target1.x += target2.x * s_tmp;
target1.y += target2.y * s_tmp;
target1.dx += target2.dx * s_tmp;
target1.dy += target2.dy * s_tmp;
target1.scalar += s_tmp;
//要らない子を無効化しておく。
target2.scalar = 0;
}
}
}
//前方詰め
i_vector.removeZeroDistItem();
//加重平均解除なう(x,y位置のみ)
for (int i = 0; i < i_vector.length; i++)
{
VecLinearCoordinates.VecLinearCoordinatePoint ptr = items[i];
double d = 1 / ptr.scalar;
ptr.x *= d;
ptr.y *= d;
ptr.dx *= d;
ptr.dy *= d;
}
}
