int move_up(t_env *e)
{
e->py = e->py - e->speed * sin(get_rad(e->angle));
e->px = e->px + e->speed * cos(get_rad(e->angle));
if (is_wall(e, e->px, e->py) == 1)
{
e->py = e->py + e->speed * sin(get_rad(e->angle));
e->px = e->px - 1 * cos(get_rad(e->angle));
if (is_wall(e, e->px, e->py) == 1)
e->px = e->px + e->speed * cos(get_rad(e->angle));
}
return (0);
}
LatLonHead Spherical::get() const {
LatLonHead llh = get_rad();
llh.Latitude = radiansToDegrees(llh.Latitude);
llh.Longatude = radiansToDegrees(llh.Longatude);
llh.Heading = radiansToDegrees(llh.Heading);
return llh;
}
double Spherical::getBearing_rad(const Spherical &target) const {
double dlon, dlat, a, d;
double bear;
LatLonHead llh = get_rad();
LatLon tl = target.getLocation_rad();
dlon = tl.Longatude - llh.Longatude;
dlat = tl.Latitude - llh.Latitude;
a = sqr(sin(dlat / 2)) + cos(llh.Latitude) * cos(tl.Latitude) * sqr(sin(dlon / 2));
d = 2 * atan2(sqrt(a), sqrt(1 - a));
bear = acos((sin(tl.Latitude) - sin(llh.Latitude) * cos(d)) / (sin(d) * cos(llh.Latitude)));
// If target is to the west.
if(sin(tl.Longatude - llh.Longatude) < 0) {
bear = (2 * pi) - bear;
}
bear -= llh.Heading;
// if bearing is less than -180 degrees reverse the direction.
if(bear < -pi) {
// note: this condition may not be possible? Should be tested for possible optimization.
bear = (2 * pi) - bear;
}
// If bearing is greater than 180 degrees reverse the direction.
if(bear > pi) {
bear = -((2 * pi) - bear);
}
// If bearing is 360 degrees return it as zero.
if(bear == 2 * pi) {
bear = 0;
}
return bear;
}
