合計 昨日 今日

ロドットの最短移動経路に関するプログラム

フォーラムルール
フォーラムルールはこちら  ※コードを貼り付ける場合は [code][/code] で囲って下さい。詳しくはこちら
Name: こんたい
[URL]
Date: 2017年12月02日(土) 00:21
No: 1
(OFFLINE)

 ロドットの最短移動経路に関するプログラム

ロボットが障害物を避けながら指定したスタートからゴールに向かうプログラムについての質問です。
プログラムですが以下に載せておきます。
プログラムの大半は与えられており、プログラムの中頃のdouble distance_to_segment{}を考えるというものです。
プログラムの障害物の点(sx、sy)(ex,ey)ロボットの座標(orgx,orgy)ロボットから障害物の辺の最近接点(*ptx、*pty)です。
最近接点、最短距離を自分で考えましたがこれで合っているのでしょうか。最短距離は一応alpha > 1.0の場合だけ考えています。
宜しくお願いします。

大体のプログラムは与えられており
//#include <windows.h>
#include <math.h>
#include <stdio.h>
#include "eglib22.h"
#define OBJ_MAX 10
#define EG_RED eg_rgb(255,0, 0 )
#define EG_GLAY eg_rgb(128,128,128)
#define EG_GREEN eg_rgb(0, 255,0 )

struct _obj_st {
int x[30], y[30];
int vertex;
} obj[OBJ_MAX];

int obj_num;
double sx, sy, gx, gy,gain=20.0;

void calc_normal_vector(double x, double y,
double *nx, double *ny)
{
double len;
len = sqrt(x*x + y*y);
*nx = x / len;
*ny = y / len;
}

double calc_segment_length(double sx, double sy,
double ex, double ey)
{
return sqrt((sx-ex)*(sx-ex) + (sy-ey)*(sy-ey));
}

double calc_alpha(double sx, double sy,
double ex, double ey,
double orgx, double orgy)
{
double vec_x, vec_y, alpha;
vec_x = ex-sx;
vec_y = ey-sy;
alpha = (vec_x * orgx + vec_y * orgy -
vec_x*sx - vec_y*sy) /
(vec_x * vec_x + vec_y * vec_y);
return alpha;
}

double distance_to_segment(double sx, double sy,
double ex, double ey,
double orgx, double orgy,
double *ptx, double *pty)
{
double vec_x, vec_y, alpha, len, mx, my;
double a, b, c;
double root;
double dist; /* 求める距離 */
vec_x = ex - sx;
vec_y = ey - sy;
alpha = (vec_x * orgx + vec_y * orgy -
vec_x*sx - vec_y*sy) /
(vec_x * vec_x + vec_y * vec_y);
mx = sx + (ex - sx) * alpha;
my = sy + (ey - sy) * alpha;

if (alpha < 0.0) {
len = calc_segment_length(sx, sy, orgx, orgy);
*ptx = sx;
*pty = sy;
dist = 0;
}
else if (alpha > 1.0) {
len = calc_segment_length(ex, ey, orgx, orgy);
*ptx = ex;
*pty = ey;
dist = 0;
}
else {
len = calc_segment_length(mx, my, orgx, orgy);
*ptx = mx;
*pty = my;

a = -(vec_y);
b = vec_x;
c = (-b * sy) + (-a * sx);
root = sqrt(a*a + b*b);

if (root == 0.0) {
return -1.0; /* 求められない場合は負の値を返す */
}

dist = ((a * orgx) + (b * orgy) + c) / root;

if (dist < 0.0) {
dist = -dist;
}

printf("最近接点%d,%d\n最短距離%d\n\n", *ptx, *pty, dist);
}
return len;


}

double distance_to_object(int idx,
double rx, double ry,
double *ptx, double *pty)
{
double tmp_len, tmp_x, tmp_y, len, x, y;
int i;
len = 999.0;
for (i=1; i < obj[idx].vertex; i++) {
tmp_len = distance_to_segment
(obj[idx].x[i-1], obj[idx].y[i-1],
obj[idx].x[i ], obj[idx].y[i ],
rx, ry, &tmp_x, &tmp_y);
if (len > tmp_len) {
x = tmp_x;
y = tmp_y;
len = tmp_len;
}
}
*ptx = x;
*pty = y;
return len;
}

void open_vec_file(void){
FILE *fp;
int i, j;
fp = fopen("vec_env2.dat", "r");
fscanf(fp, "%d", &obj_num);
for (i=0; i<obj_num; i++) {
fscanf(fp, "%d", &obj[i].vertex);
for (j=0; j<obj[i].vertex; j++) {
fscanf(fp, "%d %d",
&obj[i].x[j], &obj[i].y[j]);
}
}
fclose(fp);
}

void eg_init(void)
{
open_vec_file();
//srand(time());

//printf("start x,y > "); scanf("%lf%lf",&sx,&sy);
//printf("goal x,y > "); scanf("%lf%lf",&gx,&gy);
printf("gain > "); scanf("%lf",&gain);

eg_open_window(640, 480);

}

void eg_main(void)
{
int i, j;
double nx, ny, vx, vy, px, py, dist;
int x,y;

for(;;){

getchar();

eg_cls();

for (i=0; i<obj_num; i++) {
eg_rectf(obj[i].vertex, obj[i].x, obj[i].y ,EG_GLAY);
}

eg_update();

while(!eg_mouse_clickL(&x,&y));
sx = (double)x; sy = (double)y;
eg_circlef((int)sx, (int)sy, 3, EG_RED);
eg_update();

while(!eg_mouse_clickR(&x,&y));
gx = (double)x; gy = (double)y;
eg_circlef((int)gx, (int)gy, 5, EG_GREEN);
eg_update();

for (;;) {
eg_circlef((int)sx, (int)sy, 3, EG_RED);
vx = 0.0; vy = 0.0;
for (j=0; j<obj_num; j++){
dist = distance_to_object(j, sx, sy, &px, &py);
calc_normal_vector(px-sx, py-sy, &nx, &ny);
vx -= nx / dist * gain;
vy -= ny / dist * gain;
}

dist = calc_segment_length(sx, sy, gx, gy);
if (dist < 1.0) {
printf("Goal!\n");
break;
}

vx += (gx-sx) / dist; // Unit vector
vy += (gy-sy) / dist; // Unit vector

sx += vx;
sy += vy;

}
eg_update();

}
}

Name: こんたい
[URL]
Date: 2017年12月02日(土) 00:29
No: 2
(OFFLINE)

 Re: ロドットの最短移動経路に関するプログラム

実行するとスタートからゴールまでの
最近接点〇、〇
最短距離〇
と表示され続けますが
最近接点-57145994,1081648397
最短距離-2021292611
など表示され
最近接点座標の数字が8桁、最短距離も10桁で絶対値をとっているはずだが-となったりと、うまくいきません。
どのような結果になれば合っているのか分からないため途方に暮れています。
スタートとゴールは別ウィンドウが開き(障害物が表示されている)、クリックするとスタート点、ゴール点を決められます。
よろしくお願いします。

Name: みけCAT
[URL]
伝説なるハッカー(685,415 ポイント)
Date: 2017年12月02日(土) 04:56
No: 3
(ONLINE)

 Re: ロドットの最短移動経路に関するプログラム

ソースコードを提示する際は、BBCodeが有効な(無効にしない)状態で、
BBCodeのcodeタグの開始タグと終了タグの組(開始タグが先)で囲んでいただけると、
見やすくてありがたいです。

こんたい さんが書きました:最近接点、最短距離を自分で考えましたがこれで合っているのでしょうか。

アルゴリズムはまだ読んでいませんが、int型のデータを要求するprintfの書式%dにdouble型のデータを渡している時点で未定義動作となり、間違っています。
複雑な問題?マシンの性能を上げてOpenMPで殴ればいい!(死亡フラグ)


Return to C言語何でも質問掲示板

オンラインデータ

このフォーラムを閲覧中のユーザー: なし & ゲスト[16人]