/**
* 予想位置を基準に四角形をトレースして、一定の基準をクリアするかを評価します。
* @param i_reader
* @param i_edge_size
* @param i_prevsq
* @return
* @throws NyARException
*/
private bool traceSquareLine(INyARVectorReader i_reader, int i_edge_size, NyARRectTargetStatus i_prevsq, NyARLinear[] o_line)
{
NyARDoublePoint2d p1, p2;
VecLinearCoordinates vecpos = this._ref_my_pool._vecpos;
//NyARIntRect i_rect
p1 = i_prevsq.estimate_vertex[0];
int dist_limit = i_edge_size * i_edge_size;
//強度敷居値(セルサイズ-1)
// int min_th=i_edge_size*2+1;
// min_th=(min_th*min_th);
for (int i = 0; i < 4; i++)
{
p2 = i_prevsq.estimate_vertex[(i + 1) % 4];
//クリップ付きで予想位置周辺の直線のトレース
i_reader.traceLineWithClip(p1, p2, i_edge_size, vecpos);
//クラスタリングして、傾きの近いベクトルを探す。(限界は10度)
this._ref_my_pool._vecpos_op.margeResembleCoords(vecpos);
//基本的には1番でかいベクトルだよね。だって、直線状に取るんだもの。
int vid = vecpos.getMaxCoordIndex();
//データ品質規制(強度が多少強くないと。)
// if(vecpos.items[vid].sq_dist<(min_th)){
// return false;
// }
//@todo:パラメタ調整
//角度規制(元の線分との角度を確認)
if (vecpos.items[vid].getAbsVecCos(i_prevsq.vertex[i], i_prevsq.vertex[(i + 1) % 4]) < NyARMath.COS_DEG_5)
{
//System.out.println("CODE1");
return(false);
}
//@todo:パラメタ調整
//予想点からさほど外れていない点であるか。(検出点の移動距離を計算する。)
double dist;
dist = vecpos.items[vid].sqDistBySegmentLineEdge(i_prevsq.vertex[i], i_prevsq.vertex[i]);
if (dist < dist_limit)
{
o_line[i].setVectorWithNormalize(vecpos.items[vid]);
}
else
{
//System.out.println("CODE2:"+dist+","+dist_limit);
return(false);
}
//頂点ポインタの移動
p1 = p2;
}
return(true);
}
public bool traceConture(int i_th,
NyARIntPoint2d i_entry, VecLinearCoordinates o_coord)
{
NyARIntCoordinates coord = this._coord_buf;
// Robertsラスタから輪郭抽出
if (!this._cpickup.getContour(this._ref_rob_raster, i_th, i_entry.x, i_entry.y,
coord)) {
// 輪郭線MAXならなにもできないね。
return false;
}
// 輪郭線のベクトル化
return traceConture(coord, this._rob_resolution,
this._rob_resolution * 2, o_coord);
}
public bool traceConture(int i_th,
NyARIntPoint2d i_entry, VecLinearCoordinates o_coord)
{
NyARIntCoordinates coord = this._coord_buf;
// Robertsラスタから輪郭抽出
if (!this._cpickup.getContour(this._ref_rob_raster, i_th, i_entry.x, i_entry.y,
coord))
{
// 輪郭線MAXならなにもできないね。
return(false);
}
// 輪郭線のベクトル化
return(traceConture(coord, this._rob_resolution,
this._rob_resolution * 2, o_coord));
}
/**
* 値をセットします。この関数は、処理の成功失敗に関わらず、内容変更を行います。
* @param i_sampler_in
* @param i_source
* @param i_prev_status
* @return
* @throws NyARException
*/
public bool setValueWithDeilyCheck(INyARVectorReader i_vec_reader, LowResolutionLabelingSamplerOut.Item i_source, NyARRectTargetStatus i_prev_status)
{
VecLinearCoordinates vecpos = this._ref_my_pool._vecpos;
//輪郭線を取る
if (!i_vec_reader.traceConture(i_source.lebeling_th, i_source.entry_pos, vecpos))
{
return(false);
}
//3,4象限方向のベクトルは1,2象限のベクトルに変換する。
vecpos.limitQuadrantTo12();
//ベクトルのマージ
this._ref_my_pool._vecpos_op.margeResembleCoords(vecpos);
if (vecpos.length < 4)
{
return(false);
}
//キーベクトルを取得
vecpos.getKeyCoord(this._ref_my_pool._indexbuf);
//点に変換
NyARDoublePoint2d[] this_vx = this.vertex;
if (!this._ref_my_pool._line_detect.line2SquareVertex(this._ref_my_pool._indexbuf, this_vx))
{
return(false);
}
//頂点並び順の調整
rotateVertexL(this.vertex, checkVertexShiftValue(i_prev_status.vertex, this.vertex));
//パラメタチェック
if (!checkDeilyRectCondition(i_prev_status))
{
return(false);
}
//次回の予測
setEstimateParam(i_prev_status);
return(true);
}
/**
* クリッピング付きのライントレーサです。
*
* @param i_pos1
* @param i_pos2
* @param i_edge
* @param o_coord
* @return
* @throws NyARException
*/
public bool traceLineWithClip(NyARDoublePoint2d i_pos1,
NyARDoublePoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
{
NyARIntSize s=this._ref_base_raster.getSize();
bool is_p1_inside_area, is_p2_inside_area;
NyARIntPoint2d[] pt = this.__pt;
// 線分が範囲内にあるかを確認
is_p1_inside_area = s.isInnerPoint(i_pos1);
is_p2_inside_area = s.isInnerPoint(i_pos2);
// 個数で分岐
if (is_p1_inside_area && is_p2_inside_area) {
// 2ならクリッピング必要なし。
if (!this.traceLine(i_pos1, i_pos2, i_edge, o_coord)) {
return false;
}
return true;
}
// 1,0個の場合は、線分を再定義
if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2)) {
return false;
}
if (!this.__temp_l.makeSegmentLine(s.w,s.h,pt)) {
return false;
}
if (is_p1_inside_area != is_p2_inside_area) {
// 1ならクリッピング後に、外に出ていた点に近い輪郭交点を得る。
if (is_p1_inside_area) {
// p2が範囲外
pt[(i_pos2.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos1);
} else {
// p1が範囲外
pt[(i_pos1.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos2);
}
} else {
// 0ならクリッピングして得られた２点を使う。
if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2)) {
return false;
}
if (!this.__temp_l.makeSegmentLine(s.w,s.h, pt)) {
return false;
}
}
if (!this.traceLine(pt[0], pt[1], i_edge, o_coord)) {
return false;
}
return true;
}
/**
* ノイズらしいベクトルを無視しながら最小二乗法でベクトルを統合する関数
* @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;
}
/**
* 輪郭線を取得します。
* 取得アルゴリズムは、以下の通りです。
* 1.輪郭座標(n)の画素周辺の画素ベクトルを取得。
* 2.輪郭座標(n+1)周辺の画素ベクトルと比較。
* 3.差分が一定以下なら、座標と強度を保存
* 4.3点以上の集合になったら、最小二乗法で直線を計算。
* 5.直線の加重値を個々の画素ベクトルの和として返却。
*/
public bool traceConture(NyARIntCoordinates i_coord, int i_pos_mag, int i_cell_size, VecLinearCoordinates o_coord)
{
VecLinearCoordinates.VecLinearCoordinatePoint[] pos = this._tmp_coord_pos;
// ベクトル化
int MAX_COORD = o_coord.items.Length;
int i_coordlen = i_coord.length;
NyARIntPoint2d[] coord = i_coord.items;
VecLinearCoordinates.VecLinearCoordinatePoint pos_ptr;
//0個目のライン探索
int number_of_data = 0;
int sq;
long sq_sum=0;
//0番目のピクセル
pos[0].scalar=sq=this.getAreaVector33(coord[0].x * i_pos_mag, coord[0].y * i_pos_mag,i_cell_size, i_cell_size,pos[0]);
sq_sum+=(int)sq;
//[2]に0を保管
//1点目だけは前方と後方、両方に探索をかける。
//前方探索の終点
int coord_last_edge=i_coordlen;
//後方探索
int sum=1;
double ave_dx=pos[0].dx;
double ave_dy=pos[0].dy;
for (int i = i_coordlen-1; i >0; i--)
{
// ベクトル取得
pos_ptr=pos[sum];
pos_ptr.scalar=sq=this.getAreaVector33(coord[i].x * i_pos_mag,coord[i].y * i_pos_mag, i_cell_size, i_cell_size,pos_ptr);
sq_sum += (int)sq;
// 類似度判定
if(checkVecCos(pos[sum],pos[sum-1],ave_dx,ave_dy))
{
//相関なし->前方探索へ。
ave_dx=pos_ptr.dx;
ave_dy=pos_ptr.dy;
coord_last_edge=i;
break;
} else {
//相関あり- 点の蓄積
ave_dx+=pos_ptr.dx;
ave_dy+=pos_ptr.dy;
sum++;
}
}
//前方探索
for (int i = 1; i<coord_last_edge; i++)
{
// ベクトル取得
pos_ptr=pos[sum];
pos_ptr.scalar=sq=this.getAreaVector33(coord[i].x * i_pos_mag,coord[i].y * i_pos_mag, i_cell_size, i_cell_size,pos_ptr);
sq_sum += (int)sq;
if(sq==0){
continue;
}
//if (pos_ptr.getAbsVecCos(pos[sum-1]) < NyARMath.COS_DEG_5 && pos_ptr.getAbsVecCos(ave_dx,ave_dy)<NyARMath.COS_DEG_20) {
if (checkVecCos(pos[sum],pos[sum-1],ave_dx,ave_dy)) {
//相関なし->新しい要素を作る。
if(this.leastSquaresWithNormalize(pos,sum,o_coord.items[number_of_data],sq_sum/(sum*5))){
number_of_data++;
}
ave_dx=pos_ptr.dx;
ave_dy=pos_ptr.dy;
//獲得した値を0へ移動
pos[0].setValue(pos[sum]);
sq_sum=0;
sum=1;
} else {
//相関あり- 点の蓄積
ave_dx+=pos_ptr.dx;
ave_dy+=pos_ptr.dy;
sum++;
}
// 輪郭中心を出すための計算
if (number_of_data == MAX_COORD) {
// 輪郭ベクトルバッファの最大を超えたら失敗
return false;
}
}
if(this.leastSquaresWithNormalize(pos,sum,o_coord.items[number_of_data],sq_sum/(sum*5))){
number_of_data++;
}
// ベクトル化2:最後尾と先頭の要素が似ていれば連結する。
// sq_distの合計を計算
o_coord.length = number_of_data;
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;
}
public bool traceLine(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
{
NyARIntCoordinates coord = this._coord_buf;
NyARIntSize base_s=this._ref_base_raster.getSize();
// (i_area*2)の矩形が範囲内に収まるように線を引く
// 移動量
// 点間距離を計算
int dist = (int) Math.Sqrt(i_pos1.sqDist(i_pos2));
// 最低AREA*2以上の大きさが無いなら、ラインのトレースは不可能。
if (dist < 4) {
return false;
}
// dist最大数の決定
if (dist > 12) {
dist = 12;
}
// サンプリングサイズを決定(移動速度とサイズから)
int s = i_edge * 2 + 1;
int dx = (int) (i_pos2.x - i_pos1.x);
int dy = (int) (i_pos2.y - i_pos1.y);
int r = base_s.w - s;
int b = base_s.h - s;
// 最大24点を定義して、そのうち両端の2個を除いた点を使用する。
for (int i = 1; i < dist - 1; i++) {
int x = (int) (i * dx / dist + i_pos1.x - i_edge);
int y = (int) (i * dy / dist + i_pos1.y - i_edge);
// limit
coord.items[i - 1].x = x < 0 ? 0 : (x >= r ? r : x);
coord.items[i - 1].y = y < 0 ? 0 : (y >= b ? b : y);
}
coord.length = dist - 2;
// 点数は10点程度を得る。
return traceConture(coord, 1, s, o_coord);
}
/**
* クリッピング付きのライントレーサです。
*
* @param i_pos1
* @param i_pos2
* @param i_edge
* @param o_coord
* @return
* @throws NyARException
*/
public bool traceLineWithClip(NyARDoublePoint2d i_pos1,
NyARDoublePoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
{
NyARIntSize s = this._ref_base_raster.getSize();
bool is_p1_inside_area, is_p2_inside_area;
NyARIntPoint2d[] pt = this.__pt;
// 線分が範囲内にあるかを確認
is_p1_inside_area = s.isInnerPoint(i_pos1);
is_p2_inside_area = s.isInnerPoint(i_pos2);
// 個数で分岐
if (is_p1_inside_area && is_p2_inside_area)
{
// 2ならクリッピング必要なし。
if (!this.traceLine(i_pos1, i_pos2, i_edge, o_coord))
{
return(false);
}
return(true);
}
// 1,0個の場合は、線分を再定義
if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2))
{
return(false);
}
if (!this.__temp_l.makeSegmentLine(s.w, s.h, pt))
{
return(false);
}
if (is_p1_inside_area != is_p2_inside_area)
{
// 1ならクリッピング後に、外に出ていた点に近い輪郭交点を得る。
if (is_p1_inside_area)
{
// p2が範囲外
pt[(i_pos2.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos1);
}
else
{
// p1が範囲外
pt[(i_pos1.sqDist(pt[0]) < i_pos2.sqDist(pt[1])) ? 1 : 0].setValue(i_pos2);
}
}
else
{
// 0ならクリッピングして得られた２点を使う。
if (!this.__temp_l.makeLinearWithNormalize(i_pos1, i_pos2))
{
return(false);
}
if (!this.__temp_l.makeSegmentLine(s.w, s.h, pt))
{
return(false);
}
}
if (!this.traceLine(pt[0], pt[1], i_edge, o_coord))
{
return(false);
}
return(true);
}
/**
* 輪郭線を取得します。
* 取得アルゴリズムは、以下の通りです。
* 1.輪郭座標(n)の画素周辺の画素ベクトルを取得。
* 2.輪郭座標(n+1)周辺の画素ベクトルと比較。
* 3.差分が一定以下なら、座標と強度を保存
* 4.3点以上の集合になったら、最小二乗法で直線を計算。
* 5.直線の加重値を個々の画素ベクトルの和として返却。
*/
public bool traceConture(NyARIntCoordinates i_coord, int i_pos_mag, int i_cell_size, VecLinearCoordinates o_coord)
{
VecLinearCoordinates.VecLinearCoordinatePoint[] pos = this._tmp_coord_pos;
// ベクトル化
int MAX_COORD = o_coord.items.Length;
int i_coordlen = i_coord.length;
NyARIntPoint2d[] coord = i_coord.items;
VecLinearCoordinates.VecLinearCoordinatePoint pos_ptr;
//0個目のライン探索
int number_of_data = 0;
int sq;
long sq_sum = 0;
//0番目のピクセル
pos[0].scalar = sq = this.getAreaVector33(coord[0].x * i_pos_mag, coord[0].y * i_pos_mag, i_cell_size, i_cell_size, pos[0]);
sq_sum += (int)sq;
//[2]に0を保管
//1点目だけは前方と後方、両方に探索をかける。
//前方探索の終点
int coord_last_edge = i_coordlen;
//後方探索
int sum = 1;
double ave_dx = pos[0].dx;
double ave_dy = pos[0].dy;
for (int i = i_coordlen - 1; i > 0; i--)
{
// ベクトル取得
pos_ptr = pos[sum];
pos_ptr.scalar = sq = this.getAreaVector33(coord[i].x * i_pos_mag, coord[i].y * i_pos_mag, i_cell_size, i_cell_size, pos_ptr);
sq_sum += (int)sq;
// 類似度判定
if (checkVecCos(pos[sum], pos[sum - 1], ave_dx, ave_dy))
{
//相関なし->前方探索へ。
ave_dx = pos_ptr.dx;
ave_dy = pos_ptr.dy;
coord_last_edge = i;
break;
}
else
{
//相関あり- 点の蓄積
ave_dx += pos_ptr.dx;
ave_dy += pos_ptr.dy;
sum++;
}
}
//前方探索
for (int i = 1; i < coord_last_edge; i++)
{
// ベクトル取得
pos_ptr = pos[sum];
pos_ptr.scalar = sq = this.getAreaVector33(coord[i].x * i_pos_mag, coord[i].y * i_pos_mag, i_cell_size, i_cell_size, pos_ptr);
sq_sum += (int)sq;
if (sq == 0)
{
continue;
}
//if (pos_ptr.getAbsVecCos(pos[sum-1]) < NyARMath.COS_DEG_5 && pos_ptr.getAbsVecCos(ave_dx,ave_dy)<NyARMath.COS_DEG_20) {
if (checkVecCos(pos[sum], pos[sum - 1], ave_dx, ave_dy))
{
//相関なし->新しい要素を作る。
if (this.leastSquaresWithNormalize(pos, sum, o_coord.items[number_of_data], sq_sum / (sum * 5)))
{
number_of_data++;
}
ave_dx = pos_ptr.dx;
ave_dy = pos_ptr.dy;
//獲得した値を0へ移動
pos[0].setValue(pos[sum]);
sq_sum = 0;
sum = 1;
}
else
{
//相関あり- 点の蓄積
ave_dx += pos_ptr.dx;
ave_dy += pos_ptr.dy;
sum++;
}
// 輪郭中心を出すための計算
if (number_of_data == MAX_COORD)
{
// 輪郭ベクトルバッファの最大を超えたら失敗
return(false);
}
}
if (this.leastSquaresWithNormalize(pos, sum, o_coord.items[number_of_data], sq_sum / (sum * 5)))
{
number_of_data++;
}
// ベクトル化2:最後尾と先頭の要素が似ていれば連結する。
// sq_distの合計を計算
o_coord.length = number_of_data;
return(true);
}
public bool traceLine(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2, int i_edge, VecLinearCoordinates o_coord)
{
NyARIntCoordinates coord = this._coord_buf;
NyARIntSize base_s = this._ref_base_raster.getSize();
// (i_area*2)の矩形が範囲内に収まるように線を引く
// 移動量
// 点間距離を計算
int dist = (int)Math.Sqrt(i_pos1.sqDist(i_pos2));
// 最低AREA*2以上の大きさが無いなら、ラインのトレースは不可能。
if (dist < 4)
{
return(false);
}
// dist最大数の決定
if (dist > 12)
{
dist = 12;
}
// サンプリングサイズを決定(移動速度とサイズから)
int s = i_edge * 2 + 1;
int dx = (int)(i_pos2.x - i_pos1.x);
int dy = (int)(i_pos2.y - i_pos1.y);
int r = base_s.w - s;
int b = base_s.h - s;
// 最大24点を定義して、そのうち両端の2個を除いた点を使用する。
for (int i = 1; i < dist - 1; i++)
{
int x = (int)(i * dx / dist + i_pos1.x - i_edge);
int y = (int)(i * dy / dist + i_pos1.y - i_edge);
// limit
coord.items[i - 1].x = x < 0 ? 0 : (x >= r ? r : x);
coord.items[i - 1].y = y < 0 ? 0 : (y >= b ? b : y);
}
coord.length = dist - 2;
// 点数は10点程度を得る。
return(traceConture(coord, 1, s, o_coord));
}
/**
* 配列の前方に、似たベクトルを集めます。似たベクトルの判定基準は、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;
}
}
/**
* 適当に与えられた4線分から、四角形の頂点を計算する。
* @param i_line
* 4線分を格納した配列
* @param o_point
* 検出した4頂点
* @return
* 四角形を検出したらtrue
* @throws NyARException
*/
public bool line2SquareVertex(VecLinearCoordinates.VecLinearCoordinatePoint[] i_line, NyARDoublePoint2d[] o_point)
{
NyARDoublePoint2d[] v=this.__wk_v;
int number_of_vertex=0;
int non_vertexid=0;
int ptr=0;
for(int i=0;i<3;i++){
for(int i2=i+1;i2<4;i2++){
if(i_line[i].crossPos(i_line[i2],v[ptr])){
number_of_vertex++;
}else{
non_vertexid=ptr;
}
ptr++;
}
}
int num_of_plus=-1;
int[] target_order;
switch(number_of_vertex){
case 4:
case 5:
//正の外積の数を得る。0,4ならば、目的の図形
num_of_plus=countPlusExteriorProduct(v,_45vertextable[non_vertexid]);
target_order=_45vertextable[non_vertexid];
break;
case 6:
//(0-5),(1-4),(2-3)の頂点ペアの組合せを試す。頂点の検索順は、(0,1,5,4),(0,2,5,3),(1,2,4,3)
//3パターンについて、正の外積の数を得る。0,4のものがあればOK
int order_id=-1;
num_of_plus=-1;
for(int i=0;i<3;i++){
num_of_plus=countPlusExteriorProduct(v,_order_table[i]);
if(num_of_plus%4==0){
order_id=i;
break;
}
}
if(order_id==-1){
return false;
}
target_order=_order_table[order_id];
break;
default:
//他の頂点数の時はNG
return false;
}
//回転方向の正規化(ここパラメータ化しようよ)
switch(num_of_plus){
case 0:
//逆回転で検出した場合
for(int i=0;i<4;i++){
o_point[i].setValue(v[target_order[3-i]]);
}
break;
case 4:
//正回転で検出した場合
for(int i=0;i<4;i++){
o_point[i].setValue(v[target_order[i]]);
}
break;
default:
return false;
}
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;
}
}