Implementing seek behaviour of objects on a plane in OpenSceneGraph?

I have created an open plane area with thin cylinders on it like pucks, they bounce around the area and have collision detection for some larger cylinders also placed on the plane. I am trying to get them to now head towards a set point on the plane using a steering method.

The steering works for works for avoiding the obstacles by calculating distance from obstacle then calculating angle between direction travelling and the direction of the obstacle, using the calculation of the distance from obstacle when the puck is too close it steers left or right based on the calculated angle. The same technique reversed fails to work for steering towards a point, I have tried using both acos and atan2 to calculate the angle between direction travelling and target direction and from outputs believe this bit is right but when I try to use that calculation to determine when to steer towards the target I get unexpected results. Sometimes random turning?

#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);

}

any help would be amazing!

The problem here is that the technique that "works" for avoiding obstacles will always occur when the puck is heading towards the obstacle. This special condition makes both the direction of the puck and the direction of the obstacle in adjacent quadrants.

When attempting to make the pucks steer towards the obstacle however, the technique breaks down because the puck most likely will be heading away from the obstacle, no longer having the condition that the target and direction vectors are in adjacent quadrants.

The correct way to determine the steering direction is to rotate the target vector by an angle that would make the the direction vector point straight up in the quadrants (0, 1). Now that the target vector is relative to the direction vector (0, 1) looking at the x component of the target vector will determine the steering direction. If the x component of the target vector is negative, the puck must turn left to steer towards the target (increase the angle). If the x component of the target vector is positive, the puck must turn right to steer towards the target (decrease the angle).

Consider the following snippet written in python to verify this, it should still be easy to read for you to grasp the concept:

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

I suppose I could've written this in C++, but compared to running a python prompt it's a royal pain. It is as readable as any pseudo code I could've written and the concepts will work regardless of language.