/**
* ノイズらしいベクトルを無視しながら最小二乗法でベクトルを統合する関数
* @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);
}
//ベクトルの類似度判定式
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);
}
