// // FILE: Angle.cpp // AUTHOR: Rob Tillaart // VERSION: 0.1.11 // PURPOSE: library for Angle math for Arduino // URL: https://github.com/RobTillaart/Angle // http://forum.arduino.cc/index.php?topic=339402 // // HISTORY // 0.1.00 initial version // 0.1.01 cleanup a bit // 0.1.02 added toRadians() + fix compare() // 0.1.03 added URL, fromRadians [TEST] // 0.1.04 changed thousands in tenthousands, string constructor // 0.1.05 added AngleFormat proxy added 03/03/15 by Christoper Andrews. // 0.1.06 fixed bug negative values. // 0.1.7 2020-03-26 refactor #pragma once // 0.1.8 2020-05-27 update library.json // 0.1.9 2020-12-10 Arduino ci // 0.1.10 2021-01-16 readme.md + minor refactor // 0.1.11 2021-10-17 update build-ci, #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) // todo optimize { neg = false; d = dd; m = mm; s = ss; t = tt; // TODO // normalize(); // assume only one (largest) parameter is negative at most... if (d < 0) { d = -d; neg = true; } if (m < 0) { m = -m; neg = true; } if (s < 0) { s = -s; neg = true; } if (t < 0) { t = -t; neg = true; } while (t >= 10000) { s++; t -= 10000; } while (s >= 60) { m++; s -= 60; } while (m >= 60) { d++; m -= 60; } if (d == 0 && m == 0 && s == 0 && t == 0) neg = false; } Angle::Angle(const double alpha) { double a = alpha; neg = (alpha < 0); if (neg) a = -a; d = int(a); a = a - d; // unsigned long p = a * 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 a = a * 256; unsigned long p = round(a * 140625.0); t = p % 10000UL; p = p / 10000UL; s = p % 60UL; m = p / 60UL; } Angle::Angle(char * str) { uint32_t yy = 0; uint8_t d_cnt = 0; neg = false; // parse whole degrees char *p = str; d = 0; // skip crap while (!isdigit(*p) && (*p != '-')) p++; // process sign if (*p == '-') { neg = true; p++; } if (*p == '+') p++; // parse whole part into degrees; while (isdigit(*p)) { d *= 10; d += (*p - '0'); p++; } // parse decimal part into an uint32_t if (*p != '\0') { p++; // skip . while (isdigit(*p) && d_cnt < 9) { d_cnt++; yy *= 10; yy += (*p - '0'); p++; } } // make sure we have 9 decimal places. while (d_cnt < 9) { d_cnt++; 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 m, s, tt t = yy % 10000UL; yy = yy / 10000UL; s = yy % 60; m = yy / 60; } // 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 (neg) n += p.print('-'); n += p.print(d); n += p.print(separator[0]); if( --c ) { if (m < 10) n += p.print('0'); n += p.print(m); n += p.print(separator[1]); if( --c ) { if (s < 10) n += p.print('0'); n += p.print(s); n += p.print(separator[2]); if( --c ) { if (t < 1000) n += p.print('0'); if (t < 100) n += p.print('0'); if (t < 10) n += p.print('0'); n += p.print(t); } } } return n; }; double Angle::toDouble(void) { long v = t + s * 10000UL + m * 600000UL; double val = ((1.0 / 140625.0) / 256) * v + d; if (neg) val = -val; return val; } // NEGATE Angle Angle::operator - () { Angle temp = *this; if (temp.d == 0 && temp.m == 0 && temp.s == 0 && temp.t == 0) { temp.neg = false; } else { temp.neg = !neg; } 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.neg == a.neg) { temp.d += a.d; temp.m += a.m; temp.s += a.s; temp.t += a.t; } else { temp.d -= a.d; temp.m -= a.m; temp.s -= a.s; temp.t -= a.t; } 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.neg == a.neg) { temp.d -= a.d; temp.m -= a.m; temp.s -= a.s; temp.t -= a.t; } else { temp.d += a.d; temp.m += a.m; temp.s += a.s; temp.t += a.t; } 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.neg && b.neg) return 1; if (a.neg && !b.neg) return -1; // check abs value int rv = 0; if (a.d > b.d) rv = 1; else if (a.d < b.d) rv = -1; else if (a.m > b.m) rv = 1; else if (a.m < b.m) rv = -1; else if (a.s > b.s) rv = 1; else if (a.s < b.s) rv = -1; else if (a.t > b.t) rv = 1; else if (a.t < b.t) rv = -1; if (rv != 0 && a.neg) rv = -rv; return rv; } void Angle::normalize() // TOCHECK { while (t < 0) { s--; t += 10000; } while (t >= 10000) { s++; t -= 10000; } while (s < 0) { m--; s += 60; } while (s >= 60) { m++; s -= 60; } while (m < 0) { d--; m += 60; } while (m >= 60) { d++; m -= 60; } if (d < 0) { if (t != 0) { t = 10000 - t; s++; } if (s != 0) { s = (60 - s) % 60; m++; } if (m != 0) { m = (60 - m) % 60; d++; } d = -d; neg = !neg; } if (d == 0 && m == 0 && s == 0 && t == 0) neg = false; } // --- END OF FILE ---