Implementing seek behaviour of objects on a plane in OpenSceneGraph? - c++

Question

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!

Answer 1

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.