/**
* Sets the TrackBearing to val
*
* not in use
* @param val New TrackBearing
*/
void
DeviceBlackboard::SetTrack(Angle val)
{
ScopeLock protect(mutex);
simulator_data.track = val.as_bearing();
ScheduleMerge();
}
void
Units::LatitudeToDMS(Angle Latitude, int *dd, int *mm, int *ss, bool *north)
{
// if (Latitude is negative) -> Latitude is South otherwise North
*north = (Latitude.sign() < 0 ? false : true);
unsigned value = (unsigned)(Latitude.magnitude_degrees() * 3600 +
fixed_half);
*ss = value % 60;
value /= 60;
*mm = value % 60;
value /= 60;
*dd = value;
}
static void
OnTimerNotify(gcc_unused WndForm &Sender)
{
if (direction_filter == 0 && !CommonInterface::Calculated().circling) {
Angle a = last_heading - CommonInterface::Calculated().heading;
if (a.AsDelta().AbsoluteDegrees() >= fixed(10)) {
last_heading = CommonInterface::Calculated().heading;
UpdateList();
DataFieldEnum &df = *(DataFieldEnum *)wpDirection->GetDataField();
TCHAR buffer[64];
df.replaceEnumText(0, GetHeadingString(buffer));
wpDirection->RefreshDisplay();
}
}
}
void
Units::LongitudeToDMS(Angle Longitude, int *dd, int *mm, int *ss, bool *east)
{
// if (Longitude is negative) -> Longitude is West otherwise East
*east = (Longitude.sign() < 0 ? false : true);
unsigned value = (unsigned)(Longitude.magnitude_degrees() * 3600 +
fixed_half);
*ss = value % 60;
value /= 60;
*mm = value % 60;
value /= 60;
*dd = value;
}
/**
* Create and initialize a Latitude value in the planetocentric domain within
* the given angle.
*
* @param latitude The latitude value this instance will represent
* @param errors Error checking conditions
*/
Latitude::Latitude(Angle latitude, ErrorChecking errors) : Angle() {
m_equatorialRadius = NULL;
m_polarRadius = NULL;
m_errors = errors;
setPlanetocentric(latitude.radians(), Radians);
}
void OutTextRaw::writeAngle(const Angle& d, PhysicalOutStream& stream)
{
if(d == SensorData::off)
sprintf(buf, "%g", static_cast<float>(d));
else
sprintf(buf, "%gdeg", d.toDegrees());
stream.writeToStream(buf, strlen(buf));
}
gcc_const
static unsigned
AngleToDonutVertex(Angle angle)
{
return GLDonutVertices::ImportAngle(NATIVE_TO_INT(angle.Native())
+ ARRAY_SIZE(ISINETABLE) * 3u / 4u,
ARRAY_SIZE(ISINETABLE));
}
gcc_const
static char
CalculateZoneLetter(const Angle latitude)
{
static constexpr char letters[] = "CDEFGHJKLMNPQRSTUVWXX";
unsigned index = (unsigned)((latitude.Degrees() + fixed(80)) / 8);
return (index < ARRAY_SIZE(letters)) ? letters[index] : '\0';
}
void
FlarmTrafficControl::UpdateTaskDirection(bool show_task_direction, Angle bearing)
{
if (!show_task_direction)
task_direction = Angle::Degrees(-1);
else
task_direction = bearing.AsBearing();
}
bool
Estimate(fixed &V_westb, Angle &theta_westb, fixed &error)
{
int i;
bool scanned[NUM_V_POINTS];
for (i = 0; i < NUM_V_POINTS; i++)
scanned[i] = false;
// scan for 6 points around current best estimate.
// if a better estimate is found, keep scanning around
// that point, and don't repeat scans
bool improved = false;
bool continue_search = true;
bool full_search = false;
while (continue_search) {
continue_search = false;
int ib = VtoI(V_west_best);
int il, ih;
if (full_search) {
il = 0;
ih = NUM_V_POINTS - 1;
} else {
il = min(NUM_V_POINTS - 1, max(0, ib - 3));
ih = min(NUM_V_POINTS - 1, max(0, ib + 3));
}
for (i = il; i <= ih; i++) {
if (scanned[i]) {
continue;
} else {
scanned[i] = true;
// see if we can find a better estimate
fixed V_west = ItoV(i);
if (UpdateSearch(V_west)) {
improved = true;
continue_search = true; // earnt more search
}
}
}
if (!continue_search && !full_search && (error_best > 100)) {
full_search = true;
continue_search = true;
// if no improvement and still large error,
// try searching all speeds that haven't been checked yet.
}
}
// return true if estimate was improved
V_westb = V_west_best;
theta_westb = theta_west_best.as_bearing();
error = fixed(error_best) / 10;
return improved;
}
AxisAngle<T>
RotationMatrix3<T>::GetAxisAngle( ) const
{
Angle angle = ArcCos( (Trace() - 1.) / 2. );
Vector3<T> axis;
if ( angle.Radians() <= 0. )
axis = Vector3<T>::UnitX; //arbitrary: there is no rotation.
else if ( angle.Radians() < M_PI )
{
axis.Set( m_matrix( 2, 1 ) - m_matrix( 1, 2 ),
m_matrix( 0, 2 ) - m_matrix( 2, 0 ),
m_matrix( 1, 0 ) - m_matrix( 0, 1 ) );
axis.Normalize( );
}
else
{
int i0 = 0;
int i1 = 1;
int i2 = 2;
T maxD = m_matrix( 0, 0 );
if ( m_matrix( 1, 1 ) > maxD )
{
i0 = 1;
i1 = 0;
maxD = m_matrix( 1, 1 );
}
if ( m_matrix( 2, 2 ) > maxD )
{
i0 = 2;
i1 = 0;
i2 = 1;
}
T axisComps[3];
axisComps[i0] = static_cast<T>( 0.5 ) * std::sqrt( m_matrix( i0, i0 )
- m_matrix( i1, i1 )
- m_matrix( i2, i2 )
+ static_cast<T>( 1. ) );
axisComps[i1] = m_matrix( i1, i0 )
/ (static_cast<T>( 2. ) * axisComps[i0]);
axisComps[i2] = m_matrix( i1, i0 )
/ (static_cast<T>( 2. ) * axisComps[i0]);
axis.Set( axisComps[0], axisComps[1], axisComps[2] );
}
return AxisAngle<T>( axis, angle );
}
void HipparcosTest::testWindow() {
debug(LOG_DEBUG, DEBUG_LOG, 0, "testWindow() begin");
RaDec center(0, 0);
center.ra().hours(6.75247702777777777777);
center.dec().degrees(-16.71611583333333333333);
Angle width; width.hours(1);
Angle height; height.degrees(15);
SkyWindow window(center, width, height);
Catalog::starsetptr stars = catalog->find(window,
MagnitudeRange(-30, 4.5));
debug(LOG_DEBUG, DEBUG_LOG, 0, "%d stars", stars->size());
CPPUNIT_ASSERT(stars->size() == 10);
Catalog::starset::const_iterator s;
for (s = stars->begin(); s != stars->end(); s++) {
debug(LOG_DEBUG, DEBUG_LOG, 0, "%s", s->toString().c_str());
}
debug(LOG_DEBUG, DEBUG_LOG, 0, "testWindow() end");
}
gcc_const
static unsigned
AngleToIndex(Angle a)
{
fixed i = ROUTEPOLAR_POINTS * (fixed(1.25)
- a.AsBearing().Radians() / fixed_two_pi);
assert(positive(i));
return uround(i) % ROUTEPOLAR_POINTS;
}
/**
* Sets the TrackBearing to val
*
* not in use
* @param val New TrackBearing
*/
void
DeviceBlackboard::SetTrack(Angle val)
{
std::lock_guard<Mutex> lock(mutex);
simulator.Touch(simulator_data);
simulator_data.track = val.AsBearing();
ScheduleMerge();
}
/**
* Adds another latitude to this one. Handles planetographic latitudes.
*
* @param angleToAdd the latitude being added to this one
* @param equatorialRadius
* @param polarRadius
* @param latType
* @return The result
*/
Latitude Latitude::add(Angle angleToAdd, Distance equatorialRadius, Distance polarRadius,
CoordinateType latType) {
Latitude result;
switch (latType) {
case Planetocentric:
result = Latitude(planetocentric() + angleToAdd.radians(), equatorialRadius, polarRadius,
latType, Radians, m_errors);
break;
case Planetographic:
result = Latitude(planetographic() + angleToAdd.radians(), equatorialRadius, polarRadius,
latType, Radians, m_errors);
break;
}
return result;
}
void MapPanel::DrawPointer(const System *system, Angle &angle, const Color &color) const
{
static const Color black(0., 1.);
angle += Angle(30.);
Point pos = Zoom() * (system->Position() + center);
PointerShader::Draw(pos, angle.Unit(), 14., 19., -4., black);
PointerShader::Draw(pos, angle.Unit(), 8., 15., -6., color);
}
bool Turret::canFollowTarget(DrawableObject & target, bool TargetInvisible)
{
float distance = (float)sqrt(pow(target.getPosition().x - _position.x,2) +
pow(target.getPosition().y - _position.y, 2));
Angle direction = (float)atan2(_position.y - target.getPosition().y,
_position.x - target.getPosition().x) / _pi * 180 ;
if(distance < _lockDistance && (direction.getAngle() - DrawableObject::_direction.getAngle() < 5
&& direction.getAngle() - DrawableObject::_direction.getAngle() > -5)
&& (direction > _range.x && direction < _range.y)
&& !TargetInvisible)
{
DrawableObject::_direction = direction;
return true;
}
else
return false;
}
void
DigitEntry::SetLatitude(Angle value, CoordinateFormat format)
{
// Latitude in floating point degrees
value = value.AsDelta();
const fixed degrees = fabs(value.Degrees());
// Check the first three columns
assert(columns[0].type == Column::Type::NORTH_SOUTH);
assert(columns[1].type == Column::Type::DIGIT);
assert(columns[2].type == Column::Type::DIGIT);
columns[0].value = negative(value.Native());
// Set up and check the remaining digits
SetDigits(degrees, format, true);
Invalidate();
}
inline
void
Polar::Set( Angle bearing, double distance )
{
m_bearing = bearing;
m_bearing.Normalize( );
Assert( distance >= 0. );
m_distance = distance;
}
bool
FlatEllipse::intersect_extended(const FlatPoint &pe, FlatPoint &i1,
FlatPoint &i2) const
{
const FlatLine l_f1p(f1, pe);
const FlatLine l_pf2(pe, f2);
const Angle ang = l_f1p.angle();
const fixed d = l_pf2.d() + max(a, b); // max line length
fixed can, san;
ang.sin_cos(san, can);
FlatLine e_l(pe, FlatPoint(pe.x + d * can, pe.y + d * san));
// e_l is the line extended from p in direction of f1-p
return intersect(e_l, i1, i2);
}
static void
OnGPSUpdate(const MoreData &basic)
{
if (dialog_state.direction == 0 && !CommonInterface::Calculated().circling) {
const Angle heading = basic.attitude.heading;
Angle a = last_heading - heading;
if (a.AsDelta().AbsoluteDegrees() >= fixed(10)) {
last_heading = heading;
UpdateList();
DataFieldEnum &df = *(DataFieldEnum *)direction_control->GetDataField();
TCHAR buffer[64];
df.replaceEnumText(0, GetHeadingString(buffer));
direction_control->RefreshDisplay();
}
}
}
inline Vector3 & Vector3::rotate( const Angle & angle )
{
if ( !angle.isNull() )
{
Angle trueAngle = -angle;
qreal sin,
cos;
trueAngle.sincos( &sin, &cos );
qreal nx = x() * cos - y() * sin;
qreal ny = x() * sin + y() * cos;
setX(nx);
setY(ny);
}
return *this;
}
bool
SectorZone::IsAngleInSector(const Angle b) const
{
// Quit early if we have a full circle
if ((end_radial - start_radial).AsBearing() <= Angle::FullCircle() / 512)
return true;
return b.Between(start_radial, end_radial);
}
bool
FlatEllipse::IntersectExtended(const FlatPoint &pe, FlatPoint &i1,
FlatPoint &i2) const
{
const FlatLine l_f1p(f1, pe);
const FlatLine l_pf2(pe, f2);
const Angle ang = l_f1p.angle();
const fixed d = l_pf2.d() + std::max(a, b); // max line length
const auto sc = ang.SinCos();
fixed san = sc.first, can = sc.second;
FlatLine e_l(pe, FlatPoint(pe.x + d * can, pe.y + d * san));
// e_l is the line extended from p in direction of f1-p
return Intersect(e_l, i1, i2);
}
void
DigitEntry::SetLongitude(Angle value, CoordinateFormat format)
{
// Longitude in floating point degrees
value = value.AsDelta();
const auto degrees = fabs(value.Degrees());
// Check the first three columns here
assert(columns[0].type == Column::Type::EAST_WEST);
assert(columns[1].type == Column::Type::DIGIT19);
assert(columns[2].type == Column::Type::DIGIT);
columns[0].value = value.IsNegative();
// Set up and check the remaining digits
SetDigits(degrees, format, false);
Invalidate();
}
Angle Angle::operator/(const Angle &right) const
{
double val = right.as(m_unit);
if(Math::isZero(val))
{
throw arkhe::base::Exception("division by zero");
}
return Angle(m_angle / val,m_unit);
}
void
FlatPoint::Rotate(const Angle angle)
{
const fixed _x = x;
const fixed _y = y;
const auto sc = angle.SinCos();
const fixed sa = sc.first, ca = sc.second;
x = _x * ca - _y * sa;
y = _x * sa + _y * ca;
}
void
InfoBoxWindow::SetComment(Angle value, const TCHAR *suffix)
{
assert(suffix != NULL);
TCHAR tmp[32];
_stprintf(tmp, _T("%d")_T(DEG)_T("%s"),
iround(value.value_degrees()), suffix);
SetComment(tmp);
}
void
Matrix3<T>::Set( int axis, const Angle & angle )
{
if ( (axis < 0) || (axis >= 3) )
throw std::out_of_range( "Matrix3: out_of_range error" );
static const int indices[3][3]
= { { 0, 1, 2 }, { 1, 2, 0 }, { 2, 0, 1 } };
const int i0 = indices[ axis ][ 0 ];
const int i1 = indices[ axis ][ 1 ];
const int i2 = indices[ axis ][ 2 ];
const T cos = static_cast< T >( angle.Cos( ) );
const T sin = static_cast< T >( angle.Sin( ) );
m_elements[ i0 ][ i0 ] = 1.;
m_elements[ i0 ][ i1 ] = m_elements[ i0 ][ i2 ] = m_elements[ i1 ][ i0 ]
= m_elements[ i2 ][ i0 ] = 0.;
m_elements[ i1 ][ i1 ] = m_elements[ i2 ][ i2 ] = cos;
m_elements[ i2 ][ i1 ] = - sin;
m_elements[ i1 ][ i2 ] = sin;
}
bool
UpdateSearch_Inner(fixed V_west, Angle theta_best)
{
// given wind speed and direction, find TAS error
int V_west_x = iround(100 * V_west * theta_best.cos());
int V_west_y = iround(100 * V_west * theta_best.sin());
int verr = VError(V_west_x, V_west_y);
// search for minimum error
// (this is not monotonous)
if (verr >= error_best)
return false;
error_best = verr;
V_west_best = V_west;
theta_west_best = theta_best;
return true;
}
