mirror of
https://github.com/RobTillaart/Arduino.git
synced 2024-09-19 16:46:11 -04:00
365 lines
8.3 KiB
C++
365 lines
8.3 KiB
C++
//
|
|
// FILE: Angle.cpp
|
|
// AUTHOR: Rob Tillaart
|
|
// VERSION: 0.2.0
|
|
// PURPOSE: library for Angle math for Arduino
|
|
// URL: https://github.com/RobTillaart/Angle
|
|
// http://forum.arduino.cc/index.php?topic=339402
|
|
|
|
|
|
#include "Angle.h"
|
|
|
|
|
|
AngleFormat::AngleFormat( const Angle &ref, AngleFormatMode format )
|
|
: angle(ref), mode(format)
|
|
{
|
|
}
|
|
|
|
|
|
size_t AngleFormat::printTo(Print& p) const
|
|
{
|
|
return angle.printTo( p, mode );
|
|
}
|
|
|
|
|
|
Angle::Angle(int dd, int mm, int ss, int tt)
|
|
{
|
|
_negative = false;
|
|
_degrees = dd;
|
|
_minutes = mm;
|
|
_seconds = ss;
|
|
_tenths = tt;
|
|
// normalize();
|
|
// assume only one (largest) parameter is negative at most...
|
|
if (_degrees < 0) { _degrees = -_degrees; _negative = true; }
|
|
if (_minutes < 0) { _minutes = -_minutes; _negative = true; }
|
|
if (_seconds < 0) { _seconds = -_seconds; _negative = true; }
|
|
if (_tenths < 0) { _tenths = -_tenths ; _negative = true; }
|
|
// modulo might be faster
|
|
while (_tenths >= 10000) { _seconds++; _tenths -= 10000; }
|
|
while (_seconds >= 60) { _minutes++; _seconds -= 60; }
|
|
while (_minutes >= 60) { _degrees++; _minutes -= 60; }
|
|
// check special case 0
|
|
if (_degrees == 0 && _minutes == 0 && _seconds == 0 && _tenths == 0)
|
|
{
|
|
_negative = false;
|
|
}
|
|
}
|
|
|
|
|
|
Angle::Angle(const double alpha)
|
|
{
|
|
double angle = alpha;
|
|
_negative = (alpha < 0);
|
|
if (_negative) angle = -angle;
|
|
|
|
_degrees = int(angle);
|
|
angle = angle - _degrees;
|
|
// unsigned long p = angle * 3600000L;
|
|
// 3600 000 = 2^7 • 3^2 • 5^5 = 128 * 28125
|
|
// 2^7 = 128 will only affect exponent - no loss precision
|
|
// 28125 is less digits so less loss of significant digits.
|
|
// upgraded to 4 decimal seconds
|
|
// 36 000 000L = 256 * 140625
|
|
angle = angle * 256;
|
|
unsigned long p = round(angle * 140625.0);
|
|
_tenths = p % 10000UL;
|
|
p = p / 10000UL;
|
|
_seconds = p % 60UL;
|
|
_minutes = p / 60UL;
|
|
// check special case 0
|
|
if (_degrees == 0 && _minutes == 0 && _seconds == 0 && _tenths == 0)
|
|
{
|
|
_negative = false;
|
|
}
|
|
}
|
|
|
|
|
|
Angle::Angle(const char * str)
|
|
{
|
|
uint32_t yy = 0;
|
|
uint8_t decimals = 0;
|
|
_negative = false;
|
|
// parse whole degrees
|
|
char *p = (char *) str;
|
|
_degrees = 0;
|
|
// skip crap
|
|
while (!isdigit(*p) && (*p != '-')) p++;
|
|
// process sign
|
|
if (*p == '-')
|
|
{
|
|
_negative = true;
|
|
p++;
|
|
}
|
|
if (*p == '+') p++;
|
|
// parse whole part into degrees
|
|
while (isdigit(*p))
|
|
{
|
|
_degrees *= 10;
|
|
_degrees += (*p - '0');
|
|
p++;
|
|
}
|
|
// parse decimal part into an uint32_t
|
|
if (*p != '\0')
|
|
{
|
|
p++; // skip decimal point .
|
|
while (isdigit(*p) && decimals < 9)
|
|
{
|
|
decimals++;
|
|
yy *= 10;
|
|
yy += (*p - '0');
|
|
p++;
|
|
}
|
|
}
|
|
// make sure we have 9 decimal places.
|
|
while (decimals < 9)
|
|
{
|
|
decimals++;
|
|
yy *= 10;
|
|
}
|
|
// convert float to degrees. 1000000000 ~> 36000000 -> /250 * 9
|
|
// yy = yy * 4 / 125 + yy / 250; // just keeps the maths within 32 bits
|
|
yy = yy * 4 / 125;
|
|
yy = yy + (yy + 4)/ 8; // just keeps the maths within 32 bits
|
|
// split yy in minutes, seconds, tenThousands
|
|
_tenths = yy % 10000UL;
|
|
yy = yy / 10000UL;
|
|
_seconds = yy % 60;
|
|
_minutes = yy / 60;
|
|
// check special case 0
|
|
if (_degrees == 0 && _minutes == 0 && _seconds == 0 && _tenths == 0)
|
|
{
|
|
_negative = false;
|
|
}
|
|
}
|
|
|
|
|
|
// PRINTING
|
|
size_t Angle::printTo(Print& p, AngleFormatMode mode) const
|
|
{
|
|
unsigned char c = mode;
|
|
char separator[4] = ".\'\""; // "..."; // ALT-0176 = ° 179.59.59.9999
|
|
|
|
size_t n = 0;
|
|
if (_negative) n += p.print('-');
|
|
n += p.print(_degrees);
|
|
n += p.print(separator[0]);
|
|
if( --c )
|
|
{
|
|
if (_minutes < 10) n += p.print('0');
|
|
n += p.print(_minutes);
|
|
n += p.print(separator[1]);
|
|
if( --c )
|
|
{
|
|
if (_seconds < 10) n += p.print('0');
|
|
n += p.print(_seconds);
|
|
n += p.print(separator[2]);
|
|
if( --c )
|
|
{
|
|
if (_tenths < 1000) n += p.print('0');
|
|
if (_tenths < 100) n += p.print('0');
|
|
if (_tenths < 10) n += p.print('0');
|
|
n += p.print(_tenths);
|
|
}
|
|
}
|
|
}
|
|
return n;
|
|
};
|
|
|
|
|
|
double Angle::toDouble(void)
|
|
{
|
|
long v = _tenths + _seconds * 10000UL + _minutes * 600000UL;
|
|
double value = ((1.0 / 140625.0) / 256) * v + _degrees;
|
|
if (_negative) value = -value;
|
|
return value;
|
|
}
|
|
|
|
|
|
// NEGATE
|
|
Angle Angle::operator - ()
|
|
{
|
|
Angle temp = *this;
|
|
if (temp._degrees == 0 && temp._minutes == 0 && temp._seconds == 0 && temp._tenths == 0)
|
|
{
|
|
temp._negative = false;
|
|
}
|
|
else
|
|
{
|
|
temp._negative = !_negative;
|
|
}
|
|
return temp;
|
|
};
|
|
|
|
|
|
// BASIC MATH
|
|
Angle Angle::operator + (const Angle &a) // TOCHECK
|
|
{
|
|
return addHelper(a);
|
|
}
|
|
|
|
|
|
Angle& Angle::operator += (const Angle &a) // TOCHECK
|
|
{
|
|
*this = addHelper(a);
|
|
return *this;
|
|
}
|
|
|
|
|
|
Angle Angle::addHelper(const Angle &a) // TOCHECK
|
|
{
|
|
Angle temp = *this;
|
|
if (temp._negative == a._negative)
|
|
{
|
|
temp._degrees += a._degrees;
|
|
temp._minutes += a._minutes;
|
|
temp._seconds += a._seconds;
|
|
temp._tenths += a._tenths ;
|
|
}
|
|
else
|
|
{
|
|
temp._degrees -= a._degrees;
|
|
temp._minutes -= a._minutes;
|
|
temp._seconds -= a._seconds;
|
|
temp._tenths -= a._tenths ;
|
|
}
|
|
temp.normalize();
|
|
|
|
return temp;
|
|
}
|
|
|
|
|
|
Angle Angle::operator - (const Angle &a) // TOCHECK
|
|
{
|
|
return subHelper(a);
|
|
}
|
|
|
|
|
|
Angle& Angle::operator -= (const Angle &a) // TOCHECK
|
|
{
|
|
*this = subHelper(a);
|
|
return *this;
|
|
}
|
|
|
|
|
|
Angle Angle::subHelper(const Angle &a) // TOCHECK
|
|
{
|
|
Angle temp = *this;
|
|
if (temp._negative == a._negative)
|
|
{
|
|
temp._degrees -= a._degrees;
|
|
temp._minutes -= a._minutes;
|
|
temp._seconds -= a._seconds;
|
|
temp._tenths -= a._tenths ;
|
|
}
|
|
else
|
|
{
|
|
temp._degrees += a._degrees;
|
|
temp._minutes += a._minutes;
|
|
temp._seconds += a._seconds;
|
|
temp._tenths += a._tenths ;
|
|
}
|
|
temp.normalize();
|
|
return temp;
|
|
}
|
|
|
|
|
|
Angle Angle::operator * (const double dd)
|
|
{
|
|
return Angle(this->toDouble() * dd);
|
|
}
|
|
|
|
|
|
Angle Angle::operator / (const double dd)
|
|
{
|
|
return Angle(this->toDouble() / dd);
|
|
}
|
|
|
|
|
|
Angle& Angle::operator *= (const double dd)
|
|
{
|
|
*this = this->toDouble() * dd;
|
|
return *this;
|
|
}
|
|
|
|
|
|
Angle& Angle::operator /= (const double dd)
|
|
{
|
|
*this = this->toDouble() / dd;
|
|
return *this;
|
|
}
|
|
|
|
|
|
// !! can divide by zero
|
|
double Angle::operator / (Angle& a)
|
|
{
|
|
double f = this->toDouble();
|
|
double g = a.toDouble();
|
|
return f / g;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////
|
|
//
|
|
// PRIVATE
|
|
//
|
|
int Angle::compare(const Angle &a, const Angle &b)
|
|
{
|
|
// check sign first
|
|
if (!a._negative && b._negative) return 1;
|
|
if (a._negative && !b._negative) return -1;
|
|
// check abs value
|
|
int rv = 0;
|
|
if (a._degrees > b._degrees) rv = 1;
|
|
else if (a._degrees < b._degrees) rv = -1;
|
|
else if (a._minutes > b._minutes) rv = 1;
|
|
else if (a._minutes < b._minutes) rv = -1;
|
|
else if (a._minutes > b._minutes) rv = 1;
|
|
else if (a._minutes < b._minutes) rv = -1;
|
|
else if (a._tenths > b._tenths) rv = 1;
|
|
else if (a._tenths < b._tenths) rv = -1;
|
|
|
|
if (rv != 0 && a._negative) rv = -rv;
|
|
return rv;
|
|
}
|
|
|
|
|
|
void Angle::normalize()
|
|
{
|
|
while (_tenths < 0) { _seconds--; _tenths += 10000; }
|
|
while (_tenths >= 10000) { _seconds++; _tenths -= 10000; }
|
|
while (_seconds < 0) { _minutes--; _seconds += 60; }
|
|
while (_seconds >= 60) { _minutes++; _seconds -= 60; }
|
|
while (_minutes < 0) { _degrees--; _minutes += 60; }
|
|
while (_minutes >= 60) { _degrees++; _minutes -= 60; }
|
|
|
|
if (_degrees < 0)
|
|
{
|
|
if (_tenths != 0)
|
|
{
|
|
_tenths = 10000 - _tenths;
|
|
_seconds++;
|
|
}
|
|
if (_seconds != 0)
|
|
{
|
|
_seconds = (60 - _seconds) % 60;
|
|
_minutes++;
|
|
}
|
|
if (_minutes != 0)
|
|
{
|
|
_minutes = (60 - _minutes) % 60;
|
|
_degrees++;
|
|
}
|
|
_degrees = -_degrees;
|
|
_negative = !_negative;
|
|
}
|
|
|
|
if (_degrees == 0 && _minutes == 0 && _seconds == 0 && _tenths == 0)
|
|
{
|
|
_negative = false;
|
|
}
|
|
}
|
|
|
|
|
|
// --- END OF FILE ---
|