|
ポテンシャル法がどういうものか全く分かりませんが、プログラムから察するところ
ロボットが多角形の複数の障害物からできるだけ遠ざかるように移動するものだと
解釈して適当に書いてみました。本当に、全く的をはずしているかもしれません。
そのときはご容赦ください。
#include <stdio.h>
#include <math.h>
#define N 3 /* 障害物(object)の個数 */
#define MAXVTX 10 /* 障害物の頂点(vertex)の最大数 */
typedef struct Object {
int vtx; /* 頂点の数 + 1 */
double x[MAXVTX]; /* 頂点の x座標 */
double y[MAXVTX]; /* 頂点の y座標 */
} Object;
Object obj[N] = {
{ 3+1, { 0., 0., -1., 0. }, { 1., 2., 1., 1. } }, /* 三角形 */
{ 3+1, { -.5, 0., .5, 0. }, { -1., -2., -1., -1.} }, /* 三角形 */
{ 4+1, { 1., 2., 2., 1., 1. }, { 0., 0., 1., 1., 0. } }, /* 四角形 */
};
double calc_segment_length(double x1, double y1, double x2, double y2)
{
double x = x2 - x1, y = y2 - y1;
return sqrt(x * x + y * y);
}
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, mx, my, len;
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 + vec_x * alpha;
my = sy + vec_y * alpha;
if (alpha < 0.0) {
len = calc_segment_length(sx, sy, orgx, orgy);
*ptx = sx;
*pty = sy;
}
else if (alpha > 1.0) {
len = calc_segment_length(ex, ey, orgx, orgy);
*ptx = ex;
*pty = ey;
}
else {
len = calc_segment_length(mx, my, orgx, orgy);
*ptx = mx;
*pty = my;
}
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].vtx; 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 vector_normalize(double vx, double vy, double *nx, double *ny)
{
double tmp;
tmp = sqrt(vx * vx + vy * vy);
*nx = vx / tmp;
*ny = vy / tmp;
}
int main()
{
double tx, ty; /* 障害物までの最短点の座標 */
double len; /* 障害物までの最短点までの距離 */
double vx, vy; /* ロボットの進むべき方向 */
double nx, ny; /* 速度 1 の場合の進むべき方向 */
double rx, ry; /* ロボットの座標 */
int t, i;
rx = ry = 0;
for (t = 0; t < 10; t++) {
vx = vy = 0;
for (i = 0; i < N; i++) {
len = distance_to_object(i, rx, ry, &tx, &ty);
vx += (rx - tx) / len;
vy += (ry - ty) / len;
}
vector_normalize(vx, vy, &nx, &ny);
rx += nx;
ry += ny;
printf("t=%d: rx = %g, ry = %g\n", t, rx, ry);
}
return 0;
}
|
