So I currently am trying to create some method which when taking in a simulation vehicles position, direction, and an objects position, Will determine whether or not the object lies on the right and side or left hand side of that vehicles direction. An image will be shown here,Simple Diagram of Problem Situation
So far I have tried to use the cross product and some other methods to solve the problem i will include relevant code blocks here:
void Class::sortCones()
{
// Clearing both _lhsCones and _rhsCones vectors
_rhsCones.clear();
_lhsCones.clear();
for (int i =0; i < _cones.size(); i++)
{
if (indicateSide(_x, _y, _cones[i].x(), _cones[i].y(), _yaw) > 0)
{
_lhsCones.push_back(_cones[i]);
}
if (indicateSide(_x, _y, _cones[i].x(), _cones[i].y(), _yaw) == 0)
{
return;
}
else
{
_rhsCones.push_back(_cones[i]);
}
}
return;
}
double Class::indicateSide(double xCar, double yCar, double xCone, double yCone, double yawCar)
{
// Compute the i and j compoents of the yaw measurment as a unit vector i.e Vector Mag = 1
double iOne = cos(yawCar);
double jOne = sin(yawCar);
// Create the Car to Cone Vector
double iTwo = xCone - xCar;
double jTwo = yCone - yCar;
//ensure to normalise the vCar to Cone Vector
double magTwo = std::sqrt(std::pow(iTwo, 2) + std::pow(jTwo, 2));
iTwo = iTwo / magTwo;
jTwo = jTwo / magTwo;
// - old method
// Using the transformation Matrix with Theta = yaw (angle in radians) transform the axis to the augmented 2D space
// Take the Cross Product of < Ex, 0 > x < x', y' > where x', y' have the same location in the simulation space.
// double Ex = cos(yawCar)*iOne - sin(yawCar)*jOne;
// double Ey = sin(yawCar)*iOne + cos(yawCar)*jOne;
double result = iOne*jTwo - jOne*iTwo;
return result;
}
The car currently just seems to run off in a straight line and one of the funny elements is the sorting method of left and right any direction is GREATLY appreciated.
Related
so I currently am trying to create some method which when taking in a simulation vehicles position, direction, and an objects position, Will determine whether or not the object lies on the right and side or left hand side of that vehicles direction. This is what i have implemented so far (Note I am in a 2D co-ord system):
This is the code block that uses the method
void Class::leftOrRight()
{
// Clearing both _lhsCones and _rhsCones vectors
_rhsCones.clear();
_lhsCones.clear();
for (int i =0; i < _cones.size(); i++)
{
if (dotAngleFromYaw(_x, _y, _cones[i].x(), _cones[i].y(), _yaw) > 0)
{
_lhsCones.push_back(_cones[i]);
}
else
{
_rhsCones.push_back(_cones[i]);
}
}
return;
}
This is the code block which computes the angle
double Class::dotAngleFromYaw(double xCar, double yCar, double xCone, double yCone, double yawCar)
{
double iOne = cos(yawCar);
double jOne = sin(yawCar);
double iTwo = xCone - xCar;
double jTwo = yCone - yCar;
//ensure to normalise the vector two
double magTwo = std::sqrt(std::pow(iTwo, 2) + std::pow(jTwo, 2));
iTwo = iTwo / magTwo;
jTwo = jTwo / magTwo;
double theta = acos((iOne * iTwo) + (jOne * jTwo)); // in radians
return theta;
}
My issue with this is that dotAngleFromYaw(0,0,0,1,0) = +pi/2 and dotAngleFromYaw(0,0,0,-1,0) = +pi/2 hence the if statements fail to sort the cones.
Any help would be great
*Adjustments made from comment suggestions
I have change the sort method as follows
double Class::indicateSide(double xCar, double yCar, double xCone, double yCone, double yawCar)
{
// Compute the i and j compoents of the yaw measurment as a unit vector i.e Vector Mag = 1
double iOne = cos(yawCar);
double jOne = sin(yawCar);
// Create the Car to Cone Vector
double iTwo = xCone - xCar;
double jTwo = yCone - yCar;
//ensure to normalise the vCar to Cone Vector
double magTwo = std::sqrt(std::pow(iTwo, 2) + std::pow(jTwo, 2));
iTwo = iTwo / magTwo;
jTwo = jTwo / magTwo;
// // Using the transformation Matrix with Theta = yaw (angle in radians) transform the axis to the augmented 2D space
// double Ex = cos(yawCar)*iOne - sin(yawCar)*jOne;
// double Ey = sin(yawCar)*iOne + cos(yawCar)*jOne;
// Take the Cross Product of < Ex, 0 > x < x', y' > where x', y' have the same location in the simulation space.
double result = iOne*jTwo - jOne*iTwo;
return result;
}
However I still am having issues defining the left and right, note that I have also become aware that objects behind the vehicle are still passed to every instance of the array of objects to be evaluated and hence I have implemented a dot product check elsewhere that seems to work fine for now, which is why I have not included it here I can make another adjustment to the post to include said code. I did try to implement the co-ordinate system transformation however i did not see improvements compared to when the added lines are not commented out and implemented.
Any further feedback is greatly appreciated
If the angle does not matter and you only want to know whether "left or right" I'd go for another approach.
Set up a plane that has xCar and yCar on its surface. When setting it up it's up to you how to define the plane's normal i.e. the side its facing to.
After that you can apply the dot-product to determine the 'sign' indicating which side it's on.
Note that dot product does not provide information about left/right position.
Sign of dot product says whether position is ahead or backward.
To get left/right side, you need to check sign of cross product
cross = iOne * jTwo - jOne * iTwo
(note subtraction and i/j alternation)
To see the difference between dot and cross product info:
Quick test. Mathematical coordinate system (CCW) is used (left/right depends on CW/CCW)
BTW, in kinematics simulations it is worth to store components of direction vector rather than angle.
#define _USE_MATH_DEFINES // для C++
#include <cmath>
#include <iostream>
void check_target(float carx, float cary, float dirx, float diry, float tx, float ty) {
float cross = (tx - carx) * diry - (ty - cary) * dirx;
float dot = (tx - carx) * dirx + (ty - cary) * diry;
if (cross >= 0) {
if (dot >= 0)
std::cout << "ahead right\n";
else
std::cout << "behind right\n";
}
else {
if (dot >= 0)
std::cout << "ahead left\n";
else
std::cout << "behind left\n";
}
}
int main()
{
float carx, cary, car_dir_angle, dirx, diry;
float tx, ty;
carx = 1;
cary = 1;
car_dir_angle = M_PI / 4;
dirx = cos(car_dir_angle);
diry = sin(car_dir_angle);
check_target(carx, cary, dirx, diry, 2, 3);
check_target(carx, cary, dirx, diry, 2, 1);
check_target(carx, cary, dirx, diry, 1, 0);
check_target(carx, cary, dirx, diry, 0, 1);
}
I'm creating a program which simulates a robot moving around a map. I have an environment class which holds the robot and obstacles the robot could run into. At the moment I have class objects for my robot, and obstacles and I have a function which tells me if they collide (returns true/false). I am just not sure how to put it into the movement function for the robot.
The robot is a square and has a center point (x,y) a width, length and some orientation in degrees (fyi, the environment class is a friend of the robot class). The obstacles are circles with a center point (x,y) and a radius.
class Environment{
Robot robot;
vector<Obstacle> obstacles;
//random obstacle generation function
bool collision_circle(Obstacle obstacle) {
//Check if the circle intersects any of the corners of the robot
std::vector<Point> points;
points.push_back(robot.top_right);
points.push_back(robot.top_left);
points.push_back(robot.bottom_right);
points.push_back(robot.bottom_left);
Point obst_center(obstacle.return_x(), obstacle.return_y());
for (int i = 0; i < points.size(); i++) {
points[i].set_distance(obst_center);
if (points[i].distance <= obstacle.return_radius()) { return true; }
}
//Sort the points by distance away from the obstacle
std::sort(points.begin(), points.end(), less_than());
//Use the two closest to the obstacle to create a line
double m = (points[0].x - points[1].x) / (points[0].y -
points[1].y);
double b = points[0].y - (m * points[0].x);
//Determine a line perpendicular which intersects the obstacle's
center
double m_perp = 1 / m;
double b_perp = obst_center.y - (m * obst_center.x);
Point on Robot closest to obstacle
double new_x = (b - b_perp) / (m_perp - m);
double new_y = m_perp * new_x + b_perp;
distance between points
double diff_x = obst_center.x - new_x;
double diff_y = obst_center.y - new_y;
double distance = sqrt(pow(diff_x, 2) + pow(diff_y, 2));
if (distance <= obstacle.return_radius()) { return true; }
else { return false; }
}
Environment(Robot& t_robot): robot(t_robot) {}
void forward(double num_inches){
robot.y += num_inches * sin(robot.orientation * convert_deg);
//Convert_deg is a global variable = PI/180
robot.x += num_inches * cos(robot.orientation * convert_deg);
}
//void backward, left, right, etc.
}
I tried having the forward function check for intersection with each obstacle (up to 15 on the map) after a certain increment of distance, but that froze up my program or would have required thousands of calculations for each inch covered. Am I even on the right track in how to execute this? I am also using SFML for graphics, but, from what I know, it only supports bounding box collision detection. Also, I want the graphics to be something secondary to the program. I am writing this so that I can create and test an program for the robot's movement and would eventually like to just run the sample and be told if it worked and watch the replay if I want.
I've got a 3D terrain environment like so:
I'm trying to get the character (camera) to look up when climbing hills, and look down when descending, like climbing in real life.
This is what it's currently doing:
Right now the camera moves up and down the hills just fine, but I can't get the camera angle to work correctly. The only way I can think of aiming up or down depending on the terrain is getting the z-index of the cell my character is currently facing, and set that as the focus, but I really have no idea how to do that.
This is admittedly for an assignment, and we're intentionally not using objects so things are organized a little strangely.
Here's how I'm currently doing things:
const int M = 100; // width
const int N = 100; // height
double zHeights[M+1][N+1]; // 2D array containing the z-indexes of terrain cells
double gRX = 1.5; // x position of character
double gRY = 2.5; // y position of character
double gDirection = 45; // direction of character
double gRSpeed = 0.05; // move speed of character
double getZ(double x, double y) // returns the height of the current cell
{
double z = .5*sin(x*.25) + .4*sin(y*.15-.43);
z += sin(x*.45-.7) * cos(y*.315-.31)+.5;
z += sin(x*.15-.97) * sin(y*.35-8.31);
double amplitute = 5;
z *= amplitute;
return z;
}
void generateTerrain()
{
glBegin(GL_QUADS);
for (int i = 0; i <= M; i++)
{
for (int j = 0; j <= N; j++)
{
zHeights[i][j] = getZ(i,j);
}
}
}
void drawTerrain()
{
for (int i = 0; i < M; i++)
{
for (int j = 0; j < N; j++)
{
glColor3ub( (i*34525+j*5245)%256, (i*3456345+j*6757)%256, (i*98776+j*6554544)%256);
glVertex3d(i, j, getZ(i,j));
glVertex3d(i, j+1, getZ(i,j+1));
glVertex3d(i+1, j+1, getZ(i+1,j+1));
glVertex3d(i+1, j, getZ(i+1,j));
}
}
}
void display() // callback to glutDisplayFunc
{
glEnable(GL_DEPTH_TEST);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
double radians = gDirection /180.*3.141592654; // converts direction to radians
double z = getZ((int)gRX, (int)gRY); // casts as int to find z-index in zHeights[][]
double dx = cos(radians)*gRSpeed;
double dy = sin(radians)*gRSpeed;
double at_x = gRX + dx;
double at_y = gRY + dy;
double at_z = z; // source of problem, no idea what to do
gluLookAt(gRX, gRY, z + 2, // eye position
at_x, at_y, at_z + 2, // point to look at, also wrong
0, 0, 1); // up vector
drawTerrain();
glEnd();
}
void init()
{
generateTerrain();
}
Firstly, I don't see any reason to cast to int here:
double z = getZ((int)gRX, (int)gRY);
Just use the double values to get a smooth behavior.
Your basic approach is already pretty good. You take the current position (gRX, gRY), walk a bit in the viewing direction (dx, dy) and use that as the point to look at. There are just two small things that need adaptation:
double dx = cos(radians)*gRSpeed;
double dy = sin(radians)*gRSpeed;
Although multiplying by gRSpeed might be a good idea, in my opinion, this factor should not be related to the character's kinematics. Instead, this represents the smoothness of your view direction. Small values make the direction stick very closely to the terrain geometry, larger values smooth it out.
And finally, you need to evaluate the height at your look-at point:
double at_z = getZ(at_x, at_y);
I've been working on detecting collision between to object in my game. Right now everything tavels vertically, but would like to keep the option for other movement open. It's classic 2d vertical space shooter.
Right now I loop through every object, checking for collisions:
for(std::list<Object*>::iterator iter = mObjectList.begin(); iter != mObjectList.end();) {
Object *m = (*iter);
for(std::list<Object*>::iterator innerIter = ++iter; innerIter != mObjectList.end(); innerIter++ ) {
Object *s = (*innerIter);
if(m->getType() == s->getType()) {
break;
}
if(m->checkCollision(s)) {
m->onCollision(s);
s->onCollision(m);
}
}
}
Here is how I check for a collision:
bool checkCollision(Object *other) {
float radius = mDiameter / 2.f;
float theirRadius = other->getDiameter() / 2.f;
Vector<float> ourMidPoint = getAbsoluteMidPoint();
Vector<float> theirMidPoint = other->getAbsoluteMidPoint();
// If the other object is in between our path on the y axis
if(std::min(getAbsoluteMidPoint().y - radius, getPreviousAbsoluteMidPoint().y - radius) <= theirMidPoint.y &&
theirMidPoint.y <= std::max(getAbsoluteMidPoint().y + radius, getPreviousAbsoluteMidPoint().y + radius)) {
// Get the distance between the midpoints on the x axis
float xd = abs(ourMidPoint.x - theirMidPoint.x);
// If the distance between the two midpoints
// is greater than both of their radii together
// then they are too far away to collide
if(xd > radius+theirRadius) {
return false;
} else {
return true;
}
}
return false;
}
The problem is it will randomly detect collisions correctly, but other times does not detect it at all. It's not the if statement breaking away from the object loop because the objects do have different types. The closer the object is to the top of the screen, the better chance it has of collision getting detected correctly. Closer to the bottom of the screen, the less chance it has of getting detected correctly or even at all. However, these situations don't always occur. The diameter for the objects are massive (10 and 20) to see if that was the problem, but it doesn't help much at all.
EDIT - Updated Code
bool checkCollision(Object *other) {
float radius = mDiameter / 2.f;
float theirRadius = other->getDiameter() / 2.f;
Vector<float> ourMidPoint = getAbsoluteMidPoint();
Vector<float> theirMidPoint = other->getAbsoluteMidPoint();
// Find the distance between the two points from the center of the object
float a = theirMidPoint.x - ourMidPoint.x;
float b = theirMidPoint.y - ourMidPoint.y;
// Find the hypotenues
double c = (a*a)+(b*b);
double radii = pow(radius+theirRadius, 2.f);
// If the distance between the points is less than or equal to the radius
// then the circles intersect
if(c <= radii*radii) {
return true;
} else {
return false;
}
}
Two circular objects collide when the distance between their centers is small enough. You can use the following code to check this:
double distanceSquared =
pow(ourMidPoint.x - theirMidPoint.x, 2.0) +
pow(ourMidPoint.x - theirMidPoint.x, 2.0);
bool haveCollided = (distanceSquared <= pow(radius + theirRadius, 2.0));
In order to check whether there was a collision between two points in time, you can check for collision at the start of the time interval and at the end of it; however, if the objects move very fast, the collision detection can fail (i guess you have encountered this problem for falling objects that have the fastest speed at the bottom of the screen).
The following might make the collision detection more reliable (though still not perfect). Suppose the objects move with constant speed; then, their position is a linear function of time:
our_x(t) = our_x0 + our_vx * t;
our_y(t) = our_y0 + our_vy * t;
their_x(t) = their_x0 + their_vx * t;
their_y(t) = their_y0 + their_vy * t;
Now you can define the (squared) distance between them as a quadratic function of time. Find at which time it assumes its minimum value (i.e. its derivative is 0); if this time belongs to current time interval, calculate the minimum value and check it for collision.
This must be enough to detect collisions almost perfectly; if your application works heavily with free-falling objects, you might want to refine the movement functions to be quadratic:
our_x(t) = our_x0 + our_v0x * t;
our_y(t) = our_y0 + our_v0y * t + g/2 * t^2;
This logic is wrong:
if(std::min(getAbsoluteMidPoint().y - radius, getPreviousAbsoluteMidPoint().y - radius) <= theirMidPoint.y &&
theirMidPoint.y <= std::max(getAbsoluteMidPoint().y + radius, getPreviousAbsoluteMidPoint().y + radius))
{
// then a collision is possible, check x
}
(The logic inside the braces is wrong too, but that should produce false positives, not false negatives.) Checking whether a collision has occurred during a time interval can be tricky; I'd suggest checking for a collision at the present time, and getting that to work first. When you check for a collision (now) you can't check x and y independently, you must look at the distance between the object centers.
EDIT:
The edited code is still not quite right.
// Find the hypotenues
double c = (a*a)+(b*b); // actual hypotenuse squared
double radii = pow(radius+theirRadius, 2.f); // critical hypotenuse squared
if(c <= radii*radii) { // now you compare a distance^2 to a distance^4
return true; // collision
}
It should be either this:
double c2 = (a*a)+(b*b); // actual hypotenuse squared
double r2 = pow(radius+theirRadius, 2.f); // critical hypotenuse squared
if(c2 <= r2) {
return true; // collision
}
or this:
double c2 = (a*a)+(b*b); // actual hypotenuse squared
double c = pow(c2, 0.5); // actual hypotenuse
double r = radius + theirRadius; // critical hypotenuse
if(c <= r) {
return true; // collision
}
Your inner loop needs to start at mObjectList.begin() instead of iter.
The inner loop needs to iterate over the entire list otherwise you miss collision candidates the further you progress in the outer loop.
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.