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

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

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

}

#包括“Puck.h”
#包括
#包括
使用名称空间std；
#包括
of Stream fout（“danna.txt”）；
#伊夫德夫·穆皮
#定义M_PI 3.1415
#恩迪夫
类translateECB:public osg:：NodeCallback
{
公众：
translateECB（）：
translateECB（冰球**pp，障碍**ob，整数计数，双r，双x，双y）：\u dx（0.），\u dy（0.），
_dirx（2.0*rand（）/rand_MAX-1）、diry（2.0*rand（）/rand_MAX-1）、inc（0.3）、θ（0）
{ 
障碍=ob；
ob_count=计数；
_半径=r；
_x=x；
_y=y；
puckH=pp；
}
虚拟void操作符（）（osg:：Node*Node，osg:：NodeVisitor*nv）
{
osg:：矩阵变换*mt=
动态广播（节点）；
osg：：矩阵mR，mT；
mT.makeTranslate（_-dx，_-dy，0.）；
mt->setMatrix（mt）；
双ob_-dirx；
双盲法；
双ob_区；
双中心X=0，中心Y=0；
_θ=0；
双最小=4；
//我正试图让冰球飞向的位置
centerX=1；
centerY=5；
双tDirx=（\uX+\uDX）-centerX；
双tDiry=（_y+_dy）-centerY；
双tDist=sqrt（tDirx*tDirx+tDiry*tDiry）；//到目标位置的距离
//标准化我的目标方向
tDirx=tDirx/tDist；
tDiry=tDiry/tDist；
双hDist=sqrt（_dirx*_dirx+_diry*_diry）；//到下一个航向的距离
_dirx=_dirx/hDist；
_diry=_diry/hDist；
double cAngle=acos（_dirx*tDirx+_diry*tDiry）；//使用cos的倒数来计算方向之间的角度
double tAngle=atan2（centerY-（_y+_dy），centerX-（_x+_dx））；//使用tan的倒数来计算方向之间的角度
双tMin=tDist*sin（cAngle）；
//用于定义何时应用转向方向的if语句
如果（tMin>3）
{
如果（tDist<1）{uθ=0；}//冰球在目标位置内，则保持直线移动
如果（cAngle>mpi/2）{uθ=-0.1；}//向左拐
否则{θ=0.1；}//右转
}
else{uθ=0；}
//////障碍物的碰撞检测与我上面使用的原理相同
对于（int i=0；ix；
ob_diry=（_y+_dy）-障碍[i]>y；
ob_dist=sqrt（ob_dirx*ob_dirx+ob_diry*ob_diry）；
如果（观察距离<3）{
//归一化方向
双ob_范数=sqrt（ob_dirx*ob_dirx+ob_diry*ob_diry）；
ob_dirx=（ob_dirx）/ob_范数；
ob_diry=（ob_diry）/ob_norm；
双范数=sqrt（_dirx*_dirx+_diry*_diry）；
_dirx=（_dirx）/norm；
_diry=（_diry）/norm；
//计算行驶方向与障碍物方向之间的角度
双角度=acos（_-dirx*ob-dirx+_-diry*ob-diry）；
//如果在同一路径上继续，计算冰球和障碍物之间的最近距离
双最小距离=观察距离*正弦（角度）；
如果（最小距离<最小半径+障碍物[i]->半径和最小距离+障碍物[i]->半径）
{
最小值=观察距离；
如果（观察距离<_半径+障碍物[i]->半径）{uθ=0；}
如果（角度20&&U dirx>0）| |（x+dx<-20&&U dirx<0））
{
_dirx=-\u dirx；
_diry+=（0.2*rand（）/rand_MAX-0.1）；//为反弹添加随机性
}
_dy+=\u inc.*\u diry；
如果（（_y+_dy>20&&u diry>0）|（_y+_dy<-20&&u diry<0））
{
_diry=-\u diry；
_dirx+=（0.2*rand（）/rand_MAX-0.1）；//为反弹添加随机性
}
导线测量（节点，nv）；
}
私人：
双dx，双dy；
双_dirx，_diry；
双欧公司；
双θ；
双半径；
双倍x，y；
障碍**障碍；
冰球**冰球；
国际观察计数；
};
冰球
{
}
无效冰球：：创建冰球（冰球**冰球，组*根，障碍**障碍，整数计数，双xx，双yy）
{
//几何学
半径=0.2；
x=xx；
y=yy；
ob_count=计数；
圆柱体*形状=新圆柱体（Vec3（x，y，0），半径，0.1）；
ShapeDrawable*draw=新的ShapeDrawable（形状）；
绘制->设置颜色（Vec4（1,0,0,1））；
大地坐标*大地坐标=新的大地坐标（）；
测地线->可添加绘图（绘图）；
//转化
MatrixTransform*T=新MatrixTransform（）；
TranslateECB*tcb=新的TranslateECB（冰球、障碍物、ob_计数、半径、x、y）；
T->setUpdateCallback（tcb）；
T->addChild（geode）；
root->addChild（T）；
}

---

任何帮助都将是惊人的！

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

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

确定转向的正确方法

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()