Реализация поведения поиска объектов на плоскости в OpenSceneGraph? - PullRequest
1 голос
/ 28 ноября 2010

Я создал открытую плоскую область с тонкими цилиндрами на ней, как шайбы, они подпрыгивают вокруг области и имеют обнаружение столкновений для некоторых более крупных цилиндров, также размещенных на плоскости. Я пытаюсь заставить их сейчас направиться к заданной точке на самолете, используя метод управления.

Рулевое управление предназначено для работ по обходу препятствий путем расчета расстояния от препятствия, а затем вычисления угла между движением в направлении и направлением препятствия, используя расчет расстояния от препятствия, когда шайба находится слишком близко, она поворачивает влево или вправо на основе на рассчитанный угол. Одна и та же техника в обратном порядке не работает для поворота к точке, я пытался использовать и acos, и atan2 для вычисления угла между движением в направлении и направлением цели, и по выводам считаю, что этот бит верен, но когда я пытаюсь использовать этот расчет для определения чтобы двигаться к цели, я получаю неожиданные результаты. Иногда случайный поворот?

#include "Puck.h"
#include <iostream>
#include <fstream>
using namespace std;
#include <math.h>

ofstream fout("danna.txt");

#ifndef M_PI
#define M_PI 3.1415
#endif

class TranslateCB : public osg::NodeCallback
{
  public:
  TranslateCB() : _dx( 0. ), _dy( 0. ), _dirx(1), _diry(0), _inc(0.1), _theta(0) {}

  TranslateCB(Puck** pp, Obstacle** ob, int count, double r, double x, double y) : _dx( 0. ), _dy( 0. ), 
_dirx(2.0*rand()/RAND_MAX-1), _diry(2.0*rand()/RAND_MAX-1), _inc(0.3), _theta(0)
{ 
    obstacles = ob; 
    ob_count = count; 
    _radius = r; 
    _x = x; 
    _y = y;
    puckH = pp;

}

virtual void operator()( osg::Node* node,osg::NodeVisitor* nv )
{
    osg::MatrixTransform* mt =
    dynamic_cast<osg::MatrixTransform*>( node );
    osg::Matrix mR, mT;
    mT.makeTranslate( _dx , _dy, 0. );
    mt->setMatrix( mT );

    double ob_dirx;
    double ob_diry;
    double ob_dist;
    double centerX=0, centerY =0;
    _theta = 0;
    double min = 4;

    // location that I am trying to get the pucks to head towards
    centerX = 1;
    centerY = 5;

    double tDirx = (_x+_dx) - centerX;
    double tDiry = (_y+_dy) - centerY;
    double tDist = sqrt(tDirx*tDirx+tDiry*tDiry); //distance to target location

    // normalizing my target direction
    tDirx = tDirx/tDist;
    tDiry = tDiry/tDist;

    double hDist = sqrt(_dirx*_dirx + _diry*_diry); //distance to next heading 
    _dirx= _dirx/hDist;
    _diry= _diry/hDist;

    double cAngle = acos(_dirx*tDirx+_diry*tDiry); //using inverse of cos to calculate angle between directions 
    double tAngle = atan2(centerY - (_y+_dy),centerX - (_x+_dx)); // using inverse of tan to calculate angle between directions
    double tMin = tDist*sin(cAngle);

    //if statement used to define when to apply steering direction
    if(tMin > 3)
    {
        if(tDist < 1){ _theta = 0; }        //puck is inside target location, so keep travelling straight
        if(cAngle > M_PI/2){ _theta = -0.1; }   //turn left
        else{ _theta = 0.1; }   //turn right
    }
    else{ _theta = 0; }

    ////// The collision detection for the obstacles that works on the same princables that I am using above 
    for(int i = 0; i < ob_count; i++)
    {   
        ob_dirx = (_x+_dx) - obstacles[i]->x;
        ob_diry = (_y+_dy) - obstacles[i]->y;
        ob_dist = sqrt(ob_dirx*ob_dirx+ob_diry*ob_diry);

        if (ob_dist < 3) {

              //normalise directions
              double ob_norm = sqrt(ob_dirx*ob_dirx+ob_diry*ob_diry);
              ob_dirx = (ob_dirx)/ob_norm;
              ob_diry = (ob_diry)/ob_norm;
              double norm = sqrt(_dirx*_dirx+_diry*_diry);
              _dirx = (_dirx)/norm;
              _diry = (_diry)/norm;

            //calculate angle between direction travelling, and direction to obstacle
            double angle = acos(_dirx*ob_dirx + _diry*ob_diry);
            //calculate closest distance between puck and obstacle if continues on same path
            double min_dist = ob_dist*sin(angle);

            if(min_dist < _radius + obstacles[i]->radius  && ob_dist < min+obstacles[i]->radius)
            {
                min = ob_dist;
                if(ob_dist < _radius + obstacles[i]->radius){ _theta = 0; }
                else if(angle <= M_PI/2){ _theta = -0.3; }
                else{ _theta = 0.3; }
            }
        }
    }


    //change direction accordingly
    _dirx = _dirx*cos(_theta) + _diry*sin(_theta);
    _diry = _diry*cos(_theta) - _dirx*sin(_theta);

    _dx += _inc*_dirx;
    if((_x+_dx > 20 && _dirx > 0) || (_x+_dx < -20 && _dirx < 0))
    {
        _dirx = -_dirx;
        _diry += (0.2*rand()/RAND_MAX-0.1); //add randomness to bounce
    }
    _dy += _inc*_diry;
    if((_y+_dy > 20 && _diry > 0) || (_y+_dy < -20 && _diry < 0))
    {
        _diry = -_diry;
        _dirx += (0.2*rand()/RAND_MAX-0.1); //add randomness to bounce
    }

    traverse( node, nv );

}

private: 
double _dx,_dy;
double _dirx,_diry;
double _inc;
double _theta;
double _radius;
double _x,_y;
Obstacle** obstacles;
Puck** puckH;
int ob_count;
};

Puck::Puck()
{

}

void Puck::createBoids (Puck** pucks, Group *root, Obstacle** obstacles, int count, double xx, double yy)
{
  // geometry
  radius = 0.2;
  x = xx;
  y = yy;
  ob_count = count;

  Cylinder *shape=new Cylinder(Vec3(x,y,0),radius,0.1);
  ShapeDrawable *draw=new ShapeDrawable(shape);
  draw->setColor(Vec4(1,0,0,1));
  Geode *geode=new Geode();
  geode->addDrawable(draw);

  // transformation
  MatrixTransform *T=new MatrixTransform();
  TranslateCB *tcb = new TranslateCB(pucks, obstacles,ob_count,radius,x,y);
  T->setUpdateCallback(tcb);
  T->addChild(geode);

  root->addChild(T);

}

любая помощь будет потрясающей!

1 Ответ

0 голосов
/ 07 февраля 2011

Проблема здесь в том, что техника, которая «работает» для избежания препятствий, всегда будет происходить, когда шайба движется к препятствию. Это специальное условие определяет направление шайбы и препятствия в соседних квадрантах.

Однако при попытке направить шайбу к препятствию, техника перестает работать, потому что шайба, скорее всего, будет отклоняться от препятствия, больше не имея условия, что векторы цели и направления находятся в смежных квадрантах.

Правильный способ определения направления рулевого управления заключается в повороте целевого вектора на угол, который позволил бы направить вектор направления в квадрантах (0, 1). Теперь, когда целевой вектор относительно вектора направления (0, 1), глядя на компонент x целевого вектора, определит направление управления. Если компонент x целевого вектора отрицателен, шайба должна повернуть налево, чтобы направиться к цели (увеличить угол). Если компонент x целевого вектора положительный, шайба должна повернуть направо, чтобы повернуть к цели (уменьшить угол).

Рассмотрим следующий фрагмент, написанный на python, чтобы убедиться в этом, но вам все равно будет легко прочитать эту концепцию:

from math import *

dirX = 0.0
dirY = 0.0
targX = 1.0
targY = 0.0


def dir():
    global dirX, dirY, targX, targY
    # get magnitiude of direction
    mag1 = sqrt(dirX*dirX + dirY*dirY)
    if mag1 != 0:
        # normalize direction vector
        normX = dirX / mag1
        normY = dirY / mag1
    # get magnitude of target vector
    mag2 = sqrt(targX*targX + targY*targY)
    if mag2 != 0:
        # normalize target vector
        targX = targX / mag2
        targY = targY / mag2
    # find the angle need to rotate the dir vector to (0, 1)
    rotateAngle = (pi/2.0) - atan2(normY, normX)
    # rotate targ vector by that angle (we only care about the x component)
    relTargX = cos(rotateAngle) * normX + sin(rotateAngle) * normY
    # if the target vector's x is negative
    if relTargX < 0:
        # turn left
        print "Left!"
    # otherwise the target vector is 0 or positive
    else:
        # turn right
        print "Right!"

def out():
    global dirX, dirY, targX, targY
    # function just prints values to the screen
    print "dir(%f, %f) targ(%f, %f)" % (dirX, dirY, targX, targY)

# for values 0 to 360
for i in range(360):
    # pretend this is the pucks direction
    dirX = sin(radians(i))
    dirY = cos(radians(i))
    # print dir and target vectors to screen
    out()
    # print the direction to turn
    dir()

Полагаю, я мог бы написать это на C ++, но по сравнению с запуском Python, это королевская боль. Он так же читабелен, как и любой другой псевдокод, который я мог бы написать, и концепции будут работать независимо от языка.

...