在 OpenSceneGraph 中实现平面上对象的搜索行为？

Question

我创建了一个开放的平面区域，上面有像冰球一样的薄圆柱体，它们在该区域周围弹跳，并对也放置在平面上的一些较大圆柱体进行碰撞检测。我正在尝试使用转向方法让它们朝着飞机上的设定点前进。

转向的工作原理是通过计算与障碍物的距离，然后计算行进方向与障碍物方向之间的角度，利用与障碍物的距离的计算来避开障碍物，当冰球太近时，它会根据计算出的距离向左或向右转向角度。相反的相同技术无法用于转向某个点，我尝试使用 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);

}

任何帮助都会很棒！

Answer 1

这里的问题是，当冰球朝向障碍物时，总是会出现“有效”避开障碍物的技术。这种特殊的条件使得冰球的方向和障碍物的方向都位于相邻象限内。

然而，当尝试使冰球转向障碍物时，该技术会失败，因为冰球很可能会远离障碍物，不再具有目标向量和方向向量位于相邻象限的条件。

确定转向方向的正确方法是将目标矢量旋转一定角度，使方向矢量在象限 (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 提示符相比，这是一个巨大的痛苦。它与我可以编写的任何伪代码一样可读，并且无论使用何种语言，这些概念都将起作用。