在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提示符是一個皇家的痛苦。它和我可以編寫的任何僞代碼一樣可讀，並且這些概念無論語言如何都可以工作。