For a game I'm making, I want 3 ships which will all race around the map following a collection of points. It works perfectly fine, except for one point in the map, where the ships decide to rotate almost 360 degrees counter clockwise even though only 10 degrees clockwise should be enough.
The code for calculating the rotation:
vec2 distance = *desiredPosition - position;
float rot = atan2(distance.y, distance.x);
rot = rot * 180.f / PI + 90.f;
if (rot < angle)
{
angle -= dAngle;
boat->RotateImage(-dAngle);
}
if (rot > angle)
{
angle += dAngle;
boat->RotateImage(dAngle);
}
velocity += vec2(acceleration * cos((angle - 90) * PI / 180.0), acceleration * sin((angle - 90) * PI / 180.0));
How do I ensure it won't rotate in the wrong direction there?
Thanks to Richard Byron (accepted answer below), the problem is fixed. Taking the dot product is better than using degrees.
The final code:
vec2 distance = desiredPosition - position;
normal = vec2(sin((angle - 90) * PI / 180.0), cos((angle - 90) * PI / 180.0) * -1);
float dir = normal.x * distance.x + normal.y * distance.y;
//turn
if (dir > 0)
{
angle -= dAngle;
boat->RotateImage(-dAngle);
}
if (dir < 0)
{
angle += dAngle;
boat->RotateImage(dAngle);
}
velocity += vec2(acceleration * cos((angle - 90) * PI / 180.0), acceleration * sin((angle - 90) * PI / 180.0));
The angle the boat turns should be less than 180 degrees either CW or CCW. If it turns more than 180 degrees in one direction it would have been better to turn the other way.
A more general solution would be calculate the distance vector with respect to the boat's frame of reference.
There are a couple of problems with your updated code. Firstly, it should be rot2 = 360 - rot1; (rot1 + 360 is exactly the same angle as rot1).
The second issue is that you are not taking into account that 1 and 359 degrees are almost the same angle. So if abs(rot1 - angle) > 180, then you really want to use 360 - abs(rot1 - angle) in that case. Your later comparisons with rot and angle are a problem for the same reason, and you need to handle angle incrementing above 360 and decrementing below 0.
I could write out code for this, but there's actually a much simpler and faster way to do this. If you take the dot product of the vector (desiredPosition - position) and a vector at right angles to the ships current heading, then you can turn based on the sign of that result. If it's not clear how to do this, let me know and I can expand on it in the comments.
Related
I am trying to build a game where you control a pirate ship and other computer pirate ships try to attack you. I'm currently working on code that gets the enemies to turn to face the player, and I am running into an issue when atan2() returns a value that jumps from 0 to 2PI (I am using the fmod function to change the range from -pi to pi).
The ships will be following and turning with the player until the player crosses over from the 1st quadrant to the 4th quadrant, and then the ship will turn in the exact opposite direction to face the player. Any ideas on how to fix it and make it a continuous smooth turn? Here is my code: rotation is the current angle that the enemy ship is facing
float angleReq = atan2(distToPlayer.y, distToPlayer.x);
angleReq = fmod(angleReq + (2 * M_PI), 2 * M_PI);
if(rotation - angleReq > 0.01) {
rotation -= 0.01;
rotation = fmod(rotation, 2 * M_PI);
} else if (rotation - angleReq < -0.01) {
rotation += 0.01;
rotation = fmod(rotation, 2 * M_PI);
}
I realized how to fix it. As someone stated I was thinking about the problem wrong, I needed to tell the enemy ship to go the "shorter route" in order to get to the required angle based on its current position. Here is the solution code that solved that problem:
float angleReq = atan2(distToPlayer.y, distToPlayer.x);
angleReq = fmod(angleReq + (2 * M_PI), 2 * M_PI);
/* Moves ship counter clockwise from 0 over to 2pi radians */
if ((2 * M_PI - angleReq) + rotation < angleReq - rotation) {
rotation -= 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
} /* Moves ship clockwise from 2pi over to 0 radians */
else if ((2 * M_PI - rotation) + angleReq < rotation - angleReq) {
rotation += 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
} /* Moves ship clockwise normally */
else if (angleReq - rotation > 0.01) {
rotation += 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
} /* Moves ship counterclockwise normally */
else if (angleReq - rotation < -0.01) {
rotation -= 0.01;
rotation = fmod(rotation + (2 * M_PI), 2 * M_PI);
}
I am trying to develop an alghoritm for controlling the rudder of the boat...
I got lost in geo algoritms...
Function Does not work corectly.
direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
double azimuthHysteresis = 5; //straight if the deviation is less than 5 degrees
double pi = 2 * asin(1.0);
double target = atan2(TargetLatitude - CurrentLatitude, TargetLongitude - CurrentLongitude) * 180 / pi;
double delta = azimuth - target;
if (delta > 180) delta -= 360;
if (delta < -180) delta += 360;
if (delta < -2) {
return right;
}
if (delta > 2) {
return left;
}
return straight; // go straight
}
A few points:
You could use the constant M_PI for pi
I would imagine you want your angles to be measured clockwise from north. atan2 gives an angle counter clockwise from the x axis. This is simple to fix, use
atan2( dLon, dLat)
instead of
atan2( dLat, dLon)
The distance represented by a degree of longitude is, roughly, cos(lat) times the distance represented by a degree of latitude. So you should scale your dlon in the above by cos( M_PI/180.0 * lat). (cos, like all the math functions that deal in angles, takes radians as an argument).
You could simplify computing the difference of azimuth and target by using the math library function remainder, as in
delta = remainder( azimuth-target, 360.0)
this will give a delta between -180 and 180
I don't know if your code will ever be used near 180E. I'd say you should compute the difference in longitudes as if it might, ie use
remainder( TargetLongitude - CurrentLongitude, 360.0)
instead of
TargetLongitude - CurrentLongitude
This might seem OTT, but I've found (the hard way) that its much easier to get into the habit of always computing the difference of longitudes this way than to track down everywhere in the code that such differences are taken when your code is used across 180E.
Final version
direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
double azimuthHysteresis = 2; //straight if the deviation is less than 2 degrees
double target = atan2(remainder(TargetLongitude - CurrentLongitude, 360.0), remainder(TargetLatitude - CurrentLatitude, 360.0)) * 180 / M_PI;
double delta = remainder(azimuth - target, 360.0);
double deltaModule = sqrt(delta * delta);
if (deltaModule <= azimuthHysteresis) //deviation is small go straight
{
return straight;
}
if (delta <= -azimuthHysteresis) {
return right;
}
if (delta >= azimuthHysteresis) {
return left;
}
return straight; // go straight
}
There is already an answer for this question but for a 2D mesh:
Angles of triangles of a 3D mesh using #CGAL
I understand that the answer is different regarding 2D. So: how to compute the angles of triangles of a 2D mesh in CGAL? I can take the vertices and their respective Vector by making pairs, but I'm looking for a straight forward way of calculating the angles, without making a check if it's the outer or the inner angle.
If it makes any difference, this mesh has been produced by a CDT.
It was simple alright, but I thought it might help someone, since a comment by Laurent Rineauin in the initial 3D mesh question, proposed that the solution is different. So here it is:
// faces_iterator iterates through the triangles of l_cdt CDT triangulation
CGAL::Point_2<K> vertex1 = l_cdt.triangle(faces_iterator)[0];
CGAL::Point_2<K> vertex2 = l_cdt.triangle(faces_iterator)[1];
CGAL::Point_2<K> vertex3 = l_cdt.triangle(faces_iterator)[2];
double a = CGAL::sqrt((vertex2.x() - vertex3.x()) * (vertex2.x() - vertex3.x()) + (vertex2.y() - vertex3.y()) * (vertex2.y() - vertex3.y()));
double b = CGAL::sqrt((vertex1.x() - vertex3.x()) * (vertex1.x() - vertex3.x()) + (vertex1.y() - vertex3.y()) * (vertex1.y() - vertex3.y()));
double c = CGAL::sqrt((vertex2.x() - vertex1.x()) * (vertex2.x() - vertex1.x()) + (vertex2.y() - vertex1.y()) * (vertex2.y() - vertex1.y()));
// constants::PI is just π, for conversion to degrees instead of radians
double angle1 = ((std::acos((b*b + c*c - a*a) / (2*b*c))) * 180) / constants::PI;
double angle2 = ((std::acos((a*a + c*c - b*b) / (2*a*c))) * 180) / constants::PI;
double angle3 = ((std::acos((a*a + b*b - c*c) / (2*b*a))) * 180) / constants::PI;
Is there a way to safety and simply deal with angle wrap with the minimum number of case statements.
Angle wrap occurs when using a particular representation for angle (either 0-360 deg or -180 - 180 deg (or equivalent in radians)) and you wrap over the angle. For example say you have an angle of -170, and you subtract 50 deg. You mathematically add up to -220 but should actually be +140 deg.
Obviously you can check for this using:
if (deg < -180) { 180 - abs(deg + 180); }
or similar. But firstly you need multitudes of checks and secondly it doesn't work if you wrap twice.
The second case where this is prevalent is in the interpolation between two angles.
For Example, say I have an angle of -170 deg and 160 deg and I want halfway in between them. A common way to do this is ang1 + 0.5(ang2-ang1) but in the example i have provided it will cause the angle to be -5 deg when it should be 175.
Is there a common way to handle angle wrap in these scenarios?
For completeness I'll include both [0, 360) and [-180, 180) normalizations.
You will need #include <math.h>.
Normalize to [0,360):
double constrainAngle(double x){
x = fmod(x,360);
if (x < 0)
x += 360;
return x;
}
Normalize to [-180,180):
double constrainAngle(double x){
x = fmod(x + 180,360);
if (x < 0)
x += 360;
return x - 180;
}
The pattern should be easy enough to recognize to generalize to radians.
Angle Bisection:
double angleDiff(double a,double b){
double dif = fmod(b - a + 180,360);
if (dif < 0)
dif += 360;
return dif - 180;
}
double bisectAngle(double a,double b){
return constrainAngle(a + angleDiff(a,b) * 0.5);
}
This should bisect an angle on the "smaller" side. (warning: not fully tested)
I find using remainder() from the math library is convenient. Given an angle a, to constrain it to -180, 180 you can just do:
remainder(a, 360.0);
and change the 360.0 to 2.0 * M_PI for radians
Normalise an angle to range [-180, 180)
deg -= 360. * std::floor((deg + 180.) * (1. / 360.));
Normalise an angle to range [0, 360)
deg -= 360. * std::floor(deg * (1. / 360.));
Examples:
deg = -90 -> [0, 360):
deg -= 360. * std::floor(-90 / 360.);
deg -= 360. * -1;
deg = 270
deg = 270 -> [-180, 180):
deg -= 360. * std::floor((deg + 180.) / 360.);
deg -= 360. * std::floor(480. / 360.);
deg -= 360. * 1.;
deg = -90;
See: http://en.cppreference.com/w/cpp/numeric/math/floor
So if figured out a way to effectively do what i want using Mystical's approach to constraining the Angle. Here it is:
This seems to work with any example i can think of.
I know this is an old thread, but try this on:
double angleRef(double thtIn, double thtRef){
tht = fmod(thtIn + (180-thtRef),360) + (thtRef-180);
return tht;
}
So as in your example, if A=-170 and B=160, then the angle halfway between them is
A + 0.5*(angleRef(B,A) - A) = -185
or if you prefer A=160 and B=-170
A + 0.5*(angleRef(B,A) - A) = 175
Please forgive any c++ format errors, it is not my native language.
auto new_angle = atan2(sin(old_angle), cos(old_angle));
Map angle(+PI ~ -PI) to signed int value (or short value):
angle_signed_short = angle_float / PI * 0x7FFFF;
Then you can add or sub value as normal. Then map back:
angle_float = angle_signed_short * PI / 0x7FFFF;
so I've got a math function here that supposed to return a rotated point, and takes an original point, point to rotate around (origin) and radians to rotate it.
However it rotating only at half speed (aka 180 degree movement = 90 degree rotation)
sf::Vector2f RotatePoint(sf::Vector2f origin, sf::Vector2f point, float radian) {
float s = sin(radian);
float c = cos(radian);
// translate point back to origin:
point.x -= origin.x;
point.y -= origin.y;
// rotate point
float xnew = point.x * c - point.y * s;
float ynew = point.x * s + point.y * c;
// translate point back to global coords:
sf::Vector2f TranslatedPoint;
TranslatedPoint.x = xnew + origin.x;
TranslatedPoint.y = ynew + origin.y;
return TranslatedPoint;
}
The function looks ok to me. The rotation, in case you're wondering, is just multiplying a vector by the 2D Euclidean rotation matrix (http://en.wikipedia.org/wiki/Rotation_matrix ). The only errors I can think of are some misunderstanding of the usage of the function. E.g. that 2*PI radians = 360 degrees, or that the rotation is counterclockwise.
Your code seems fine. Do we agree that 180° in degrees is Pi in radians ?
The function is fine. However, you might want to fresh up on your linear algebra a bit: You basically compute
return rotation * (point - origin) + origin;
with rotation as the matrix consisting of cos(radian) on the diagonals and +/- sin(radian) on the off-diagonals. So, the whole function is a one-liner if you let your linear algebra library compute that matrix; and if you factor out the -origin part (remember, linear algebra is linear), it becomes:
return rotation * point + ( - rotation * origin + origin );
where the second part is point-invariant and can be precomputed.