diff --git a/.gitignore b/.gitignore index 330d167..eb64bfe 100644 --- a/.gitignore +++ b/.gitignore @@ -35,18 +35,15 @@ timeline.xctimeline playground.xcworkspace # Swift Package Manager -# -# Add this line if you want to avoid checking in source code from Swift Package Manager dependencies. -# Packages/ -# Package.pins -# Package.resolved -# *.xcodeproj -# -# Xcode automatically generates this directory with a .xcworkspacedata file and xcuserdata -# hence it is not needed unless you have added a package configuration file to your project -# .swiftpm - -.build/ +.DS_Store +/.build +/Packages +/*.xcodeproj +xcuserdata/ +DerivedData/ +.swiftpm/config/registries.json +.swiftpm/xcode/package.xcworkspace/contents.xcworkspacedata +.netrc # CocoaPods # diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..7026b53 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,16 @@ + + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..f68639e --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/swift-sgp.iml b/.idea/swift-sgp.iml new file mode 100644 index 0000000..c4588c0 --- /dev/null +++ b/.idea/swift-sgp.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Package.swift b/Package.swift new file mode 100644 index 0000000..1533c3c --- /dev/null +++ b/Package.swift @@ -0,0 +1,32 @@ +// swift-tools-version: 5.6 + +import PackageDescription + +let package = Package( + name: "SGPKit", + platforms: [.iOS(.v13)], + products: [ + .library( + name: "SGPKit", + targets: ["SGPKit"]), + ], + dependencies: [ + + ], + targets: [ + .target( + name: "SGPKit", + dependencies: ["SGPKitOBJC"]), + .target( + name: "SGPKitCPP", + path: "Sources/sgp4-f5cb54b" + ), + .target( + name: "SGPKitOBJC", + dependencies: ["SGPKitCPP"], + path: "Sources/OBJC"), + .testTarget( + name: "SGPKitTests", + dependencies: ["SGPKit"]), + ] +) diff --git a/README.md b/README.md index 9aee35a..74a6239 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,41 @@ # swift-spg -A Swift package to compute satellite positions from two-line elements (TLE). +A Swift package to compute satellite positions from two-line elements (TLE), wrapping the [sgp4lib](https://www.danrw.com/sgp4/) library (by Daniel Warner) + + +## Usage + +```swift +import SGPKit + +let firstLine = "1 25544U 98067A 13165.59097222 .00004759 00000-0 88814-4 0 47" +let secondLine = "2 25544 51.6478 121.2152 0011003 68.5125 263.9959 15.50783143834295" + +// Instantiate a new TLE descriptor +let tle = TLE(firstLine: firstLine, secondLine: secondLine) + +// Instantiate the interpreter +let interpreter = TLEInterpreter() + +// Obtain the data +let data: SatelliteData = interpreter.satelliteData(from: tle, date: .now) + +print(data.latitude) +print(data.longitude) +print(data.altitude) +print(data.speed) +``` + +## Installation + +### Swift Package Manager + +If you want to use SGPKit in any other project that uses [SwiftPM](https://swift.org/package-manager/), add the package as a dependency in `Package.swift`: + +```swift +dependencies: [ + .package( + url: "https://github.com/csanfilippo/swift-sgp", + from: "1.0.0" + ), +] +``` diff --git a/Sources/OBJC/SGP4Wrapper.mm b/Sources/OBJC/SGP4Wrapper.mm new file mode 100644 index 0000000..1859885 --- /dev/null +++ b/Sources/OBJC/SGP4Wrapper.mm @@ -0,0 +1,84 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#import "SGP4Wrapper.h" +#import +#import +#import +#import +#import +#import +#import +using namespace std; + +@implementation SGP4Wrapper + +- (SatelliteData* _Nonnull) getSatelliteDataFrom:(TLEWrapper*) tleWrapper date:(NSDate*) date { + SGP4 (^getSGP4)(void) = ^{ + + string first("ISS (ZARYA)"); + string second(tleWrapper.firstLine.UTF8String); + string third(tleWrapper.secondLine.UTF8String); + + Tle tle(first, second, third); + SGP4 sgp4(tle); + return sgp4; + }; + + SGP4 sgp4 = getSGP4(); + + DateTime currentTime = [self dateTimeFrom: date]; + + SatelliteData *data = [self issDataFromSgp4:sgp4 Date:currentTime]; + + return data; +} + +- (DateTime) dateTimeFrom:(NSDate*) date { + + NSCalendar *calendar = [NSCalendar currentCalendar]; + calendar.timeZone = [NSTimeZone timeZoneForSecondsFromGMT:0]; + + int day = static_cast([calendar component:NSCalendarUnitDay fromDate:date]); + int month = static_cast([calendar component:NSCalendarUnitMonth fromDate:date]); + int year = static_cast([calendar component:NSCalendarUnitYear fromDate:date]); + int hour = static_cast([calendar component:NSCalendarUnitHour fromDate:date]); + int minute = static_cast([calendar component:NSCalendarUnitMinute fromDate:date]); + int second = static_cast([calendar component:NSCalendarUnitSecond fromDate:date]); + int micro = static_cast([calendar component:NSCalendarUnitNanosecond fromDate:date] / 1000); + + DateTime time = DateTime(year, month, day, hour, minute, second); + time.Initialise(year, month, day, hour, minute, second, micro); + return time; +} + +- (SatelliteData *) issDataFromSgp4:(SGP4) sgp4 Date:(DateTime) date { + Eci eci = sgp4.FindPosition(date); + CoordGeodetic geo = eci.ToGeodetic(); + + double velocity = eci.Velocity().Magnitude() * 3600.0; + return [[SatelliteData alloc] initWithLatitude:geo.latitude * 180.0 / M_PI longitude:geo.longitude * 180.0 / M_PI speed:velocity altitude:geo.altitude]; +} + +@end diff --git a/Sources/OBJC/SatelliteData.m b/Sources/OBJC/SatelliteData.m new file mode 100644 index 0000000..88d0d59 --- /dev/null +++ b/Sources/OBJC/SatelliteData.m @@ -0,0 +1,46 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#import "SatelliteData.h" + +@interface SatelliteData () +@property(readwrite) double latitude; +@property(readwrite) double longitude; +@property(readwrite) double speed; +@property(readwrite) double altitude; +@end + +@implementation SatelliteData +- (instancetype) initWithLatitude:(double) latitude longitude:(double) longitude speed:(double) speed altitude:(double) altitude { + self = [super init]; + if (self) { + self.latitude = latitude; + self.longitude = longitude; + self.speed = speed; + self.altitude = altitude; + } + return self; +} + +@end diff --git a/Sources/OBJC/TLEWrapper.m b/Sources/OBJC/TLEWrapper.m new file mode 100644 index 0000000..c9b688b --- /dev/null +++ b/Sources/OBJC/TLEWrapper.m @@ -0,0 +1,40 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#import "TLEWrapper.h" + +@implementation TLEWrapper + +- (instancetype)initWithFirstLine:(NSString *)firstLine secondLine:(NSString *)secondLine { + self = [super init]; + + if (self) { + self.firstLine = firstLine; + self.secondLine = secondLine; + } + + return self; +} + +@end diff --git a/Sources/OBJC/include/SGP4Wrapper.h b/Sources/OBJC/include/SGP4Wrapper.h new file mode 100644 index 0000000..c510826 --- /dev/null +++ b/Sources/OBJC/include/SGP4Wrapper.h @@ -0,0 +1,37 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#import +#import "SatelliteData.h" +#import "TLEWrapper.h" + +NS_ASSUME_NONNULL_BEGIN + +@interface SGP4Wrapper : NSObject + +- (SatelliteData* _Nonnull) getSatelliteDataFrom:(TLEWrapper* _Nonnull) tle date:(NSDate* _Nonnull) date; + +@end + +NS_ASSUME_NONNULL_END diff --git a/Sources/OBJC/include/SatelliteData.h b/Sources/OBJC/include/SatelliteData.h new file mode 100644 index 0000000..5132365 --- /dev/null +++ b/Sources/OBJC/include/SatelliteData.h @@ -0,0 +1,35 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#import + +@interface SatelliteData : NSObject +@property(readonly) double latitude; +@property(readonly) double longitude; +@property(readonly) double speed; +@property(readonly) double altitude; + +- (instancetype) initWithLatitude:(double) latitude longitude:(double) longitude speed:(double) speed altitude:(double) altitude; + +@end diff --git a/Sources/OBJC/include/TLEWrapper.h b/Sources/OBJC/include/TLEWrapper.h new file mode 100644 index 0000000..7c46741 --- /dev/null +++ b/Sources/OBJC/include/TLEWrapper.h @@ -0,0 +1,38 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#import + +NS_ASSUME_NONNULL_BEGIN + +@interface TLEWrapper : NSObject + +@property(nonnull, nonatomic) NSString *firstLine; +@property(nonnull, nonatomic) NSString *secondLine; + +- (nonnull instancetype) initWithFirstLine:(NSString* _Nonnull) firstLine secondLine:(NSString* _Nonnull) secondLine; + +@end + +NS_ASSUME_NONNULL_END diff --git a/Sources/SGPKit/Interpreter.swift b/Sources/SGPKit/Interpreter.swift new file mode 100644 index 0000000..150ba67 --- /dev/null +++ b/Sources/SGPKit/Interpreter.swift @@ -0,0 +1,46 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +import SGPKitOBJC + +private extension TLE { + var asTLEWrapper: TLEWrapper { + TLEWrapper(firstLine: firstLine, secondLine: secondLine) + } +} + +/// A class that calculates the satellite position, speed and altitude using a TLE set +public final class TLEInterpreter { + + /// Returns a SatelliteData instance calculated from a TLE set + /// + /// - parameter tle: The TLE set + /// - parameter date: Date for which we want to obtain information about the satellite + /// - returns: A `SatelliteData` describing the satellite + public func satelliteData(from tle: TLE, date: Date) -> SatelliteData { + let wrapper = SGP4Wrapper() + let result: SGPKitOBJC.SatelliteData = wrapper.getSatelliteData(from: tle.asTLEWrapper, date: date) + return SatelliteData(latitude: result.latitude, longitude: result.longitude, speed: result.speed, altitude: result.altitude) + } +} diff --git a/Sources/SGPKit/Model.swift b/Sources/SGPKit/Model.swift new file mode 100644 index 0000000..d69b42f --- /dev/null +++ b/Sources/SGPKit/Model.swift @@ -0,0 +1,53 @@ +/* + MIT License + + Copyright (c) 2022 Calogero Sanfilippo + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +import Foundation +/// An object describing a TLE set +public struct TLE { + /// The first line of the TLE set + public let firstLine: String + + /// The first line of the TLE set + public let secondLine: String + + public init(firstLine: String, secondLine: String) { + self.firstLine = firstLine + self.secondLine = secondLine + } +} + +public struct SatelliteData { + + /// The geodetic latitude of the satellite + public let latitude: Double + + /// The geodetic longitude of the satellite + public let longitude: Double + + /// Satellite's speed expressed in km/h + public let speed: Double + + /// The altitude + public let altitude: Double +} diff --git a/Sources/sgp4-f5cb54b/CoordGeodetic.cc b/Sources/sgp4-f5cb54b/CoordGeodetic.cc new file mode 100644 index 0000000..0a92103 --- /dev/null +++ b/Sources/sgp4-f5cb54b/CoordGeodetic.cc @@ -0,0 +1,18 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "CoordGeodetic.h" diff --git a/Sources/sgp4-f5cb54b/CoordTopocentric.cc b/Sources/sgp4-f5cb54b/CoordTopocentric.cc new file mode 100644 index 0000000..a418a90 --- /dev/null +++ b/Sources/sgp4-f5cb54b/CoordTopocentric.cc @@ -0,0 +1,18 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "CoordTopocentric.h" diff --git a/Sources/sgp4-f5cb54b/DateTime.cc b/Sources/sgp4-f5cb54b/DateTime.cc new file mode 100644 index 0000000..707b2de --- /dev/null +++ b/Sources/sgp4-f5cb54b/DateTime.cc @@ -0,0 +1,149 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "DateTime.h" + +#if 0 + +bool jd_dmy(int JD, int c_year, int c_month, int c_day) +{ + // For the Gregorian calendar: + int a = JD + 32044; + int b = (4 * a + 3) / 146097; + int c = a - (b * 146097) / 4; + + // Then, for both calendars: + int d = (4 * c + 3) / 1461; + int e = c - (1461 * d) / 4; + int m = (5 * e + 2) / 153; + + int day = e - (153 * m + 2) / 5 + 1; + int month = m + 3 - 12 * (m / 10); + int year = b * 100 + d - 4800 + m / 10; + + if (c_year != year || c_month != month || c_day != day) + { + std::cout << year << " " << month << " " << day << std::endl; + return false; + } + else + { + return true; + } +} + + +int main() +{ + for (int year = 1; year <= 9999; year++) + { + for (int month = 1; month <= 12; month++) + { + for (int day = 1; day <= DateTime::DaysInMonth(year, month); day++) + { + int hour = 23; + int minute = 59; + int second = 59; + int microsecond = 999999; + + DateTime dt(year, month, day, hour, minute, second, microsecond); + + if (dt.Year() != year || + dt.Month() != month || + dt.Day() != day || + dt.Hour() != hour || + dt.Minute() != minute || + dt.Second() != second || + dt.Microsecond() != microsecond) + { + std::cout << "failed" << std::endl; + std::cout << "Y " << dt.Year() << " " << year << std::endl; + std::cout << "M " << dt.Month() << " " << month << std::endl; + std::cout << "D " << dt.Day() << " " << day << std::endl; + std::cout << "H " << dt.Hour() << " " << hour << std::endl; + std::cout << "M " << dt.Minute() << " " << minute << std::endl; + std::cout << "S " << dt.Second() << " " << second << std::endl; + std::cout << "F " << dt.Microsecond() << " " << microsecond << std::endl; + return 0; + } + + if (!jd_dmy(dt.Julian() + 0.5, year, month, day)) + { + std::cout << "julian" << std::endl; + return 0; + } + } + } + } + + for (int hour = 1; hour < 24; hour++) + { + std::cout << hour << std::endl; + for (int minute = 0; minute < 60; minute++) + { + for (int second = 0; second < 60; second++) + { + for (int microsecond = 0; microsecond < 1000000; microsecond += 10000) + { + int year = 1000; + int month = 10; + int day = 23; + + DateTime dt(year, month, day, hour, minute, second, microsecond); + + if (dt.Year() != year || + dt.Month() != month || + dt.Day() != day || + dt.Hour() != hour || + dt.Minute() != minute || + dt.Second() != second || + dt.Microsecond() != microsecond) + { + std::cout << "failed" << std::endl; + std::cout << "Y " << dt.Year() << " " << year << std::endl; + std::cout << "M " << dt.Month() << " " << month << std::endl; + std::cout << "D " << dt.Day() << " " << day << std::endl; + std::cout << "H " << dt.Hour() << " " << hour << std::endl; + std::cout << "M " << dt.Minute() << " " << minute << std::endl; + std::cout << "S " << dt.Second() << " " << second << std::endl; + std::cout << "F " << dt.Microsecond() << " " << microsecond << std::endl; + return 0; + } + } + } + } + } + + jd_dmy(1721425.5, 0, 0, 0); + + DateTime d1(1000, 1, 1); + DateTime d2(2000, 1, 1); + DateTime d3(4000, 1, 1); + DateTime d4(6000, 1, 1); + DateTime d5(8000, 1, 1); + + std::cout << std::setprecision(20); + std::cout << d1.Julian() << std::endl; + std::cout << d2.Julian() << std::endl; + std::cout << d3.Julian() << std::endl; + std::cout << d4.Julian() << std::endl; + std::cout << d5.Julian() << std::endl; + + return 0; +} + +#endif diff --git a/Sources/sgp4-f5cb54b/DecayedException.cc b/Sources/sgp4-f5cb54b/DecayedException.cc new file mode 100644 index 0000000..2b4c522 --- /dev/null +++ b/Sources/sgp4-f5cb54b/DecayedException.cc @@ -0,0 +1 @@ +#include "DecayedException.h" diff --git a/Sources/sgp4-f5cb54b/Eci.cc b/Sources/sgp4-f5cb54b/Eci.cc new file mode 100644 index 0000000..eee586b --- /dev/null +++ b/Sources/sgp4-f5cb54b/Eci.cc @@ -0,0 +1,106 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "Eci.h" + +#include "Globals.h" +#include "Util.h" + +/** + * Converts a DateTime and Geodetic position to Eci coordinates + * @param[in] dt the date + * @param[in] geo the geodetic position + */ +void Eci::ToEci(const DateTime& dt, const CoordGeodetic &geo) +{ + /* + * set date + */ + m_dt = dt; + + static const double mfactor = kTWOPI * (kOMEGA_E / kSECONDS_PER_DAY); + + /* + * Calculate Local Mean Sidereal Time for observers longitude + */ + const double theta = m_dt.ToLocalMeanSiderealTime(geo.longitude); + + /* + * take into account earth flattening + */ + const double c = 1.0 + / sqrt(1.0 + kF * (kF - 2.0) * pow(sin(geo.latitude), 2.0)); + const double s = pow(1.0 - kF, 2.0) * c; + const double achcp = (kXKMPER * c + geo.altitude) * cos(geo.latitude); + + /* + * X position in km + * Y position in km + * Z position in km + * W magnitude in km + */ + m_position.x = achcp * cos(theta); + m_position.y = achcp * sin(theta); + m_position.z = (kXKMPER * s + geo.altitude) * sin(geo.latitude); + m_position.w = m_position.Magnitude(); + + /* + * X velocity in km/s + * Y velocity in km/s + * Z velocity in km/s + * W magnitude in km/s + */ + m_velocity.x = -mfactor * m_position.y; + m_velocity.y = mfactor * m_position.x; + m_velocity.z = 0.0; + m_velocity.w = m_velocity.Magnitude(); +} + +/** + * @returns the position in geodetic form + */ +CoordGeodetic Eci::ToGeodetic() const +{ + const double theta = Util::AcTan(m_position.y, m_position.x); + + const double lon = Util::WrapNegPosPI(theta + - m_dt.ToGreenwichSiderealTime()); + + const double r = sqrt((m_position.x * m_position.x) + + (m_position.y * m_position.y)); + + static const double e2 = kF * (2.0 - kF); + + double lat = Util::AcTan(m_position.z, r); + double phi = 0.0; + double c = 0.0; + int cnt = 0; + + do + { + phi = lat; + const double sinphi = sin(phi); + c = 1.0 / sqrt(1.0 - e2 * sinphi * sinphi); + lat = Util::AcTan(m_position.z + kXKMPER * c * e2 * sinphi, r); + cnt++; + } + while (fabs(lat - phi) >= 1e-10 && cnt < 10); + + const double alt = r / cos(lat) - kXKMPER * c; + + return CoordGeodetic(lat, lon, alt, true); +} diff --git a/Sources/sgp4-f5cb54b/Globals.cc b/Sources/sgp4-f5cb54b/Globals.cc new file mode 100644 index 0000000..dad80d5 --- /dev/null +++ b/Sources/sgp4-f5cb54b/Globals.cc @@ -0,0 +1,18 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "Globals.h" diff --git a/Sources/sgp4-f5cb54b/Observer.cc b/Sources/sgp4-f5cb54b/Observer.cc new file mode 100644 index 0000000..7461d92 --- /dev/null +++ b/Sources/sgp4-f5cb54b/Observer.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "Observer.h" + +#include "CoordTopocentric.h" + +/* + * calculate lookangle between the observer and the passed in Eci object + */ +CoordTopocentric Observer::GetLookAngle(const Eci &eci) +{ + /* + * update the observers Eci to match the time of the Eci passed in + * if necessary + */ + Update(eci.GetDateTime()); + + /* + * calculate differences + */ + Vector range_rate = eci.Velocity() - m_eci.Velocity(); + Vector range = eci.Position() - m_eci.Position(); + + range.w = range.Magnitude(); + + /* + * Calculate Local Mean Sidereal Time for observers longitude + */ + double theta = eci.GetDateTime().ToLocalMeanSiderealTime(m_geo.longitude); + + double sin_lat = sin(m_geo.latitude); + double cos_lat = cos(m_geo.latitude); + double sin_theta = sin(theta); + double cos_theta = cos(theta); + + double top_s = sin_lat * cos_theta * range.x + + sin_lat * sin_theta * range.y - cos_lat * range.z; + double top_e = -sin_theta * range.x + + cos_theta * range.y; + double top_z = cos_lat * cos_theta * range.x + + cos_lat * sin_theta * range.y + sin_lat * range.z; + double az = atan(-top_e / top_s); + + if (top_s > 0.0) + { + az += kPI; + } + + if (az < 0.0) + { + az += 2.0 * kPI; + } + + double el = asin(top_z / range.w); + double rate = range.Dot(range_rate) / range.w; + + /* + * azimuth in radians + * elevation in radians + * range in km + * range rate in km/s + */ + return CoordTopocentric(az, + el, + range.w, + rate); +} diff --git a/Sources/sgp4-f5cb54b/OrbitalElements.cc b/Sources/sgp4-f5cb54b/OrbitalElements.cc new file mode 100644 index 0000000..0254e2d --- /dev/null +++ b/Sources/sgp4-f5cb54b/OrbitalElements.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "OrbitalElements.h" + +#include "Tle.h" + +OrbitalElements::OrbitalElements(const Tle& tle) +{ + /* + * extract and format tle data + */ + mean_anomoly_ = tle.MeanAnomaly(false); + ascending_node_ = tle.RightAscendingNode(false); + argument_perigee_ = tle.ArgumentPerigee(false); + eccentricity_ = tle.Eccentricity(); + inclination_ = tle.Inclination(false); + mean_motion_ = tle.MeanMotion() * kTWOPI / kMINUTES_PER_DAY; + bstar_ = tle.BStar(); + epoch_ = tle.Epoch(); + + /* + * recover original mean motion (xnodp) and semimajor axis (aodp) + * from input elements + */ + const double a1 = pow(kXKE / MeanMotion(), kTWOTHIRD); + const double cosio = cos(Inclination()); + const double theta2 = cosio * cosio; + const double x3thm1 = 3.0 * theta2 - 1.0; + const double eosq = Eccentricity() * Eccentricity(); + const double betao2 = 1.0 - eosq; + const double betao = sqrt(betao2); + const double temp = (1.5 * kCK2) * x3thm1 / (betao * betao2); + const double del1 = temp / (a1 * a1); + const double a0 = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + del1 * 134.0 / 81.0))); + const double del0 = temp / (a0 * a0); + + recovered_mean_motion_ = MeanMotion() / (1.0 + del0); + /* + * alternative way to calculate + * doesnt affect final results + * recovered_semi_major_axis_ = pow(XKE / RecoveredMeanMotion(), TWOTHIRD); + */ + recovered_semi_major_axis_ = a0 / (1.0 - del0); + + /* + * find perigee and period + */ + perigee_ = (RecoveredSemiMajorAxis() * (1.0 - Eccentricity()) - kAE) * kXKMPER; + period_ = kTWOPI / RecoveredMeanMotion(); +} + diff --git a/Sources/sgp4-f5cb54b/SGP4.cc b/Sources/sgp4-f5cb54b/SGP4.cc new file mode 100644 index 0000000..705fc7e --- /dev/null +++ b/Sources/sgp4-f5cb54b/SGP4.cc @@ -0,0 +1,1348 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "SGP4.h" + +#include "Util.h" +#include "Vector.h" +#include "SatelliteException.h" +#include "DecayedException.h" + +#include +#include +#include + +void SGP4::SetTle(const Tle& tle) +{ + /* + * extract and format tle data + */ + elements_ = OrbitalElements(tle); + + Initialise(); +} + +void SGP4::Initialise() +{ + /* + * reset all constants etc + */ + Reset(); + + /* + * error checks + */ + if (elements_.Eccentricity() < 0.0 || elements_.Eccentricity() > 0.999) + { + throw SatelliteException("Eccentricity out of range"); + } + + if (elements_.Inclination() < 0.0 || elements_.Inclination() > kPI) + { + throw SatelliteException("Inclination out of range"); + } + + RecomputeConstants(elements_.Inclination(), + common_consts_.sinio, + common_consts_.cosio, + common_consts_.x3thm1, + common_consts_.x1mth2, + common_consts_.x7thm1, + common_consts_.xlcof, + common_consts_.aycof); + + const double theta2 = common_consts_.cosio * common_consts_.cosio; + const double eosq = elements_.Eccentricity() * elements_.Eccentricity(); + const double betao2 = 1.0 - eosq; + const double betao = sqrt(betao2); + + if (elements_.Period() >= 225.0) + { + use_deep_space_ = true; + } + else + { + use_deep_space_ = false; + use_simple_model_ = false; + /* + * for perigee less than 220 kilometers, the simple_model flag is set + * and the equations are truncated to linear variation in sqrt a and + * quadratic variation in mean anomly. also, the c3 term, the + * delta omega term and the delta m term are dropped + */ + if (elements_.Perigee() < 220.0) + { + use_simple_model_ = true; + } + } + + /* + * for perigee below 156km, the values of + * s4 and qoms2t are altered + */ + double s4 = kS; + double qoms24 = kQOMS2T; + if (elements_.Perigee() < 156.0) + { + s4 = elements_.Perigee() - 78.0; + if (elements_.Perigee() < 98.0) + { + s4 = 20.0; + } + qoms24 = pow((120.0 - s4) * kAE / kXKMPER, 4.0); + s4 = s4 / kXKMPER + kAE; + } + + /* + * generate constants + */ + const double pinvsq = 1.0 + / (elements_.RecoveredSemiMajorAxis() + * elements_.RecoveredSemiMajorAxis() + * betao2 * betao2); + const double tsi = 1.0 / (elements_.RecoveredSemiMajorAxis() - s4); + common_consts_.eta = elements_.RecoveredSemiMajorAxis() + * elements_.Eccentricity() * tsi; + const double etasq = common_consts_.eta * common_consts_.eta; + const double eeta = elements_.Eccentricity() * common_consts_.eta; + const double psisq = fabs(1.0 - etasq); + const double coef = qoms24 * pow(tsi, 4.0); + const double coef1 = coef / pow(psisq, 3.5); + const double c2 = coef1 * elements_.RecoveredMeanMotion() + * (elements_.RecoveredSemiMajorAxis() + * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) + + 0.75 * kCK2 * tsi / psisq * common_consts_.x3thm1 + * (8.0 + 3.0 * etasq * (8.0 + etasq))); + common_consts_.c1 = elements_.BStar() * c2; + common_consts_.c4 = 2.0 * elements_.RecoveredMeanMotion() + * coef1 * elements_.RecoveredSemiMajorAxis() * betao2 + * (common_consts_.eta * (2.0 + 0.5 * etasq) + elements_.Eccentricity() + * (0.5 + 2.0 * etasq) + - 2.0 * kCK2 * tsi / (elements_.RecoveredSemiMajorAxis() * psisq) + * (-3.0 * common_consts_.x3thm1 * (1.0 - 2.0 * eeta + etasq + * (1.5 - 0.5 * eeta)) + + 0.75 * common_consts_.x1mth2 * (2.0 * etasq - eeta * + (1.0 + etasq)) * cos(2.0 * elements_.ArgumentPerigee()))); + const double theta4 = theta2 * theta2; + const double temp1 = 3.0 * kCK2 * pinvsq * elements_.RecoveredMeanMotion(); + const double temp2 = temp1 * kCK2 * pinvsq; + const double temp3 = 1.25 * kCK4 * pinvsq * pinvsq * elements_.RecoveredMeanMotion(); + common_consts_.xmdot = elements_.RecoveredMeanMotion() + 0.5 * temp1 * betao * + common_consts_.x3thm1 + 0.0625 * temp2 * betao * + (13.0 - 78.0 * theta2 + 137.0 * theta4); + const double x1m5th = 1.0 - 5.0 * theta2; + common_consts_.omgdot = -0.5 * temp1 * x1m5th + + 0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) + + temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4); + const double xhdot1 = -temp1 * common_consts_.cosio; + common_consts_.xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 * + (3.0 - 7.0 * theta2)) * common_consts_.cosio; + common_consts_.xnodcf = 3.5 * betao2 * xhdot1 * common_consts_.c1; + common_consts_.t2cof = 1.5 * common_consts_.c1; + + if (use_deep_space_) + { + deepspace_consts_.gsto = elements_.Epoch().ToGreenwichSiderealTime(); + + DeepSpaceInitialise(eosq, + common_consts_.sinio, + common_consts_.cosio, + betao, + theta2, + betao2, + common_consts_.xmdot, + common_consts_.omgdot, + common_consts_.xnodot); + } + else + { + double c3 = 0.0; + if (elements_.Eccentricity() > 1.0e-4) + { + c3 = coef * tsi * kA3OVK2 * elements_.RecoveredMeanMotion() * kAE * + common_consts_.sinio / elements_.Eccentricity(); + } + + nearspace_consts_.c5 = 2.0 * coef1 * elements_.RecoveredSemiMajorAxis() * betao2 * (1.0 + 2.75 * + (etasq + eeta) + eeta * etasq); + nearspace_consts_.omgcof = elements_.BStar() * c3 * cos(elements_.ArgumentPerigee()); + + nearspace_consts_.xmcof = 0.0; + if (elements_.Eccentricity() > 1.0e-4) + { + nearspace_consts_.xmcof = -kTWOTHIRD * coef * elements_.BStar() * kAE / eeta; + } + + nearspace_consts_.delmo = pow(1.0 + common_consts_.eta * (cos(elements_.MeanAnomoly())), 3.0); + nearspace_consts_.sinmo = sin(elements_.MeanAnomoly()); + + if (!use_simple_model_) + { + const double c1sq = common_consts_.c1 * common_consts_.c1; + nearspace_consts_.d2 = 4.0 * elements_.RecoveredSemiMajorAxis() * tsi * c1sq; + const double temp = nearspace_consts_.d2 * tsi * common_consts_.c1 / 3.0; + nearspace_consts_.d3 = (17.0 * elements_.RecoveredSemiMajorAxis() + s4) * temp; + nearspace_consts_.d4 = 0.5 * temp * elements_.RecoveredSemiMajorAxis() * + tsi * (221.0 * elements_.RecoveredSemiMajorAxis() + 31.0 * s4) * common_consts_.c1; + nearspace_consts_.t3cof = nearspace_consts_.d2 + 2.0 * c1sq; + nearspace_consts_.t4cof = 0.25 * (3.0 * nearspace_consts_.d3 + common_consts_.c1 * + (12.0 * nearspace_consts_.d2 + 10.0 * c1sq)); + nearspace_consts_.t5cof = 0.2 * (3.0 * nearspace_consts_.d4 + 12.0 * common_consts_.c1 * + nearspace_consts_.d3 + 6.0 * nearspace_consts_.d2 * nearspace_consts_.d2 + 15.0 * + c1sq * (2.0 * nearspace_consts_.d2 + c1sq)); + } + } +} + +Eci SGP4::FindPosition(const DateTime& dt) const +{ + return FindPosition((dt - elements_.Epoch()).TotalMinutes()); +} + +Eci SGP4::FindPosition(double tsince) const +{ + if (use_deep_space_) + { + return FindPositionSDP4(tsince); + } + else + { + return FindPositionSGP4(tsince); + } +} + +Eci SGP4::FindPositionSDP4(double tsince) const +{ + /* + * the final values + */ + double e; + double a; + double omega; + double xl; + double xnode; + double xinc; + + /* + * update for secular gravity and atmospheric drag + */ + double xmdf = elements_.MeanAnomoly() + + common_consts_.xmdot * tsince; + double omgadf = elements_.ArgumentPerigee() + + common_consts_.omgdot * tsince; + const double xnoddf = elements_.AscendingNode() + + common_consts_.xnodot * tsince; + + const double tsq = tsince * tsince; + xnode = xnoddf + common_consts_.xnodcf * tsq; + double tempa = 1.0 - common_consts_.c1 * tsince; + double tempe = elements_.BStar() * common_consts_.c4 * tsince; + double templ = common_consts_.t2cof * tsq; + + double xn = elements_.RecoveredMeanMotion(); + double em = elements_.Eccentricity(); + xinc = elements_.Inclination(); + + DeepSpaceSecular(tsince, + elements_, + common_consts_, + deepspace_consts_, + integrator_params_, + xmdf, + omgadf, + xnode, + em, + xinc, + xn); + + if (xn <= 0.0) + { + throw SatelliteException("Error: (xn <= 0.0)"); + } + + a = pow(kXKE / xn, kTWOTHIRD) * tempa * tempa; + e = em - tempe; + double xmam = xmdf + elements_.RecoveredMeanMotion() * templ; + + DeepSpacePeriodics(tsince, + deepspace_consts_, + e, + xinc, + omgadf, + xnode, + xmam); + + /* + * keeping xinc positive important unless you need to display xinc + * and dislike negative inclinations + */ + if (xinc < 0.0) + { + xinc = -xinc; + xnode += kPI; + omgadf -= kPI; + } + + xl = xmam + omgadf + xnode; + omega = omgadf; + + /* + * fix tolerance for error recognition + */ + if (e <= -0.001) + { + throw SatelliteException("Error: (e <= -0.001)"); + } + else if (e < 1.0e-6) + { + e = 1.0e-6; + } + else if (e > (1.0 - 1.0e-6)) + { + e = 1.0 - 1.0e-6; + } + + /* + * re-compute the perturbed values + */ + double perturbed_sinio; + double perturbed_cosio; + double perturbed_x3thm1; + double perturbed_x1mth2; + double perturbed_x7thm1; + double perturbed_xlcof; + double perturbed_aycof; + RecomputeConstants(xinc, + perturbed_sinio, + perturbed_cosio, + perturbed_x3thm1, + perturbed_x1mth2, + perturbed_x7thm1, + perturbed_xlcof, + perturbed_aycof); + + /* + * using calculated values, find position and velocity + */ + return CalculateFinalPositionVelocity(elements_.Epoch().AddMinutes(tsince), + e, + a, + omega, + xl, + xnode, + xinc, + perturbed_xlcof, + perturbed_aycof, + perturbed_x3thm1, + perturbed_x1mth2, + perturbed_x7thm1, + perturbed_cosio, + perturbed_sinio); +} + +void SGP4::RecomputeConstants(const double xinc, + double& sinio, + double& cosio, + double& x3thm1, + double& x1mth2, + double& x7thm1, + double& xlcof, + double& aycof) +{ + sinio = sin(xinc); + cosio = cos(xinc); + + const double theta2 = cosio * cosio; + + x3thm1 = 3.0 * theta2 - 1.0; + x1mth2 = 1.0 - theta2; + x7thm1 = 7.0 * theta2 - 1.0; + + if (fabs(cosio + 1.0) > 1.5e-12) + { + xlcof = 0.125 * kA3OVK2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); + } + else + { + xlcof = 0.125 * kA3OVK2 * sinio * (3.0 + 5.0 * cosio) / 1.5e-12; + } + + aycof = 0.25 * kA3OVK2 * sinio; +} + +Eci SGP4::FindPositionSGP4(double tsince) const +{ + /* + * the final values + */ + double e; + double a; + double omega; + double xl; + double xnode; + const double xinc = elements_.Inclination(); + + /* + * update for secular gravity and atmospheric drag + */ + const double xmdf = elements_.MeanAnomoly() + + common_consts_.xmdot * tsince; + const double omgadf = elements_.ArgumentPerigee() + + common_consts_.omgdot * tsince; + const double xnoddf = elements_.AscendingNode() + + common_consts_.xnodot * tsince; + + omega = omgadf; + double xmp = xmdf; + + const double tsq = tsince * tsince; + xnode = xnoddf + common_consts_.xnodcf * tsq; + double tempa = 1.0 - common_consts_.c1 * tsince; + double tempe = elements_.BStar() * common_consts_.c4 * tsince; + double templ = common_consts_.t2cof * tsq; + + if (!use_simple_model_) + { + const double delomg = nearspace_consts_.omgcof * tsince; + const double delm = nearspace_consts_.xmcof + * (pow(1.0 + common_consts_.eta * cos(xmdf), 3.0) + - nearspace_consts_.delmo); + const double temp = delomg + delm; + + xmp += temp; + omega -= temp; + + const double tcube = tsq * tsince; + const double tfour = tsince * tcube; + + tempa = tempa - nearspace_consts_.d2 * tsq - nearspace_consts_.d3 + * tcube - nearspace_consts_.d4 * tfour; + tempe += elements_.BStar() * nearspace_consts_.c5 + * (sin(xmp) - nearspace_consts_.sinmo); + templ += nearspace_consts_.t3cof * tcube + tfour + * (nearspace_consts_.t4cof + tsince * nearspace_consts_.t5cof); + } + + a = elements_.RecoveredSemiMajorAxis() * tempa * tempa; + e = elements_.Eccentricity() - tempe; + xl = xmp + omega + xnode + elements_.RecoveredMeanMotion() * templ; + + /* + * fix tolerance for error recognition + */ + if (e <= -0.001) + { + throw SatelliteException("Error: (e <= -0.001)"); + } + else if (e < 1.0e-6) + { + e = 1.0e-6; + } + else if (e > (1.0 - 1.0e-6)) + { + e = 1.0 - 1.0e-6; + } + + /* + * using calculated values, find position and velocity + * we can pass in constants from Initialise() as these dont change + */ + return CalculateFinalPositionVelocity(elements_.Epoch().AddMinutes(tsince), + e, + a, + omega, + xl, + xnode, + xinc, + common_consts_.xlcof, + common_consts_.aycof, + common_consts_.x3thm1, + common_consts_.x1mth2, + common_consts_.x7thm1, + common_consts_.cosio, + common_consts_.sinio); +} + +Eci SGP4::CalculateFinalPositionVelocity( + const DateTime& dt, + const double e, + const double a, + const double omega, + const double xl, + const double xnode, + const double xinc, + const double xlcof, + const double aycof, + const double x3thm1, + const double x1mth2, + const double x7thm1, + const double cosio, + const double sinio) +{ + const double beta2 = 1.0 - e * e; + const double xn = kXKE / pow(a, 1.5); + /* + * long period periodics + */ + const double axn = e * cos(omega); + const double temp11 = 1.0 / (a * beta2); + const double xll = temp11 * xlcof * axn; + const double aynl = temp11 * aycof; + const double xlt = xl + xll; + const double ayn = e * sin(omega) + aynl; + const double elsq = axn * axn + ayn * ayn; + + if (elsq >= 1.0) + { + throw SatelliteException("Error: (elsq >= 1.0)"); + } + + /* + * solve keplers equation + * - solve using Newton-Raphson root solving + * - here capu is almost the mean anomoly + * - initialise the eccentric anomaly term epw + * - The fmod saves reduction of angle to +/-2pi in sin/cos() and prevents + * convergence problems. + */ + const double capu = fmod(xlt - xnode, kTWOPI); + double epw = capu; + + double sinepw = 0.0; + double cosepw = 0.0; + double ecose = 0.0; + double esine = 0.0; + + /* + * sensibility check for N-R correction + */ + const double max_newton_naphson = 1.25 * fabs(sqrt(elsq)); + + bool kepler_running = true; + + for (int i = 0; i < 10 && kepler_running; i++) + { + sinepw = sin(epw); + cosepw = cos(epw); + ecose = axn * cosepw + ayn * sinepw; + esine = axn * sinepw - ayn * cosepw; + + double f = capu - epw + esine; + + if (fabs(f) < 1.0e-12) + { + kepler_running = false; + } + else + { + /* + * 1st order Newton-Raphson correction + */ + const double fdot = 1.0 - ecose; + double delta_epw = f / fdot; + + /* + * 2nd order Newton-Raphson correction. + * f / (fdot - 0.5 * d2f * f/fdot) + */ + if (i == 0) + { + if (delta_epw > max_newton_naphson) + { + delta_epw = max_newton_naphson; + } + else if (delta_epw < -max_newton_naphson) + { + delta_epw = -max_newton_naphson; + } + } + else + { + delta_epw = f / (fdot + 0.5 * esine * delta_epw); + } + + /* + * Newton-Raphson correction of -F/DF + */ + epw += delta_epw; + } + } + /* + * short period preliminary quantities + */ + const double temp21 = 1.0 - elsq; + const double pl = a * temp21; + + if (pl < 0.0) + { + throw SatelliteException("Error: (pl < 0.0)"); + } + + const double r = a * (1.0 - ecose); + const double temp31 = 1.0 / r; + const double rdot = kXKE * sqrt(a) * esine * temp31; + const double rfdot = kXKE * sqrt(pl) * temp31; + const double temp32 = a * temp31; + const double betal = sqrt(temp21); + const double temp33 = 1.0 / (1.0 + betal); + const double cosu = temp32 * (cosepw - axn + ayn * esine * temp33); + const double sinu = temp32 * (sinepw - ayn - axn * esine * temp33); + const double u = atan2(sinu, cosu); + const double sin2u = 2.0 * sinu * cosu; + const double cos2u = 2.0 * cosu * cosu - 1.0; + + /* + * update for short periodics + */ + const double temp41 = 1.0 / pl; + const double temp42 = kCK2 * temp41; + const double temp43 = temp42 * temp41; + + const double rk = r * (1.0 - 1.5 * temp43 * betal * x3thm1) + + 0.5 * temp42 * x1mth2 * cos2u; + const double uk = u - 0.25 * temp43 * x7thm1 * sin2u; + const double xnodek = xnode + 1.5 * temp43 * cosio * sin2u; + const double xinck = xinc + 1.5 * temp43 * cosio * sinio * cos2u; + const double rdotk = rdot - xn * temp42 * x1mth2 * sin2u; + const double rfdotk = rfdot + xn * temp42 * (x1mth2 * cos2u + 1.5 * x3thm1); + + /* + * orientation vectors + */ + const double sinuk = sin(uk); + const double cosuk = cos(uk); + const double sinik = sin(xinck); + const double cosik = cos(xinck); + const double sinnok = sin(xnodek); + const double cosnok = cos(xnodek); + const double xmx = -sinnok * cosik; + const double xmy = cosnok * cosik; + const double ux = xmx * sinuk + cosnok * cosuk; + const double uy = xmy * sinuk + sinnok * cosuk; + const double uz = sinik * sinuk; + const double vx = xmx * cosuk - cosnok * sinuk; + const double vy = xmy * cosuk - sinnok * sinuk; + const double vz = sinik * cosuk; + /* + * position and velocity + */ + const double x = rk * ux * kXKMPER; + const double y = rk * uy * kXKMPER; + const double z = rk * uz * kXKMPER; + Vector position(x, y, z); + const double xdot = (rdotk * ux + rfdotk * vx) * kXKMPER / 60.0; + const double ydot = (rdotk * uy + rfdotk * vy) * kXKMPER / 60.0; + const double zdot = (rdotk * uz + rfdotk * vz) * kXKMPER / 60.0; + Vector velocity(xdot, ydot, zdot); + + if (rk < 1.0) + { + throw DecayedException( + dt, + position, + velocity); + } + + return Eci(dt, position, velocity); +} + +static inline double EvaluateCubicPolynomial( + const double x, + const double constant, + const double linear, + const double squared, + const double cubed) +{ + return constant + x * linear + x * x * squared + x * x * x * cubed; +} + +void SGP4::DeepSpaceInitialise( + const double eosq, + const double sinio, + const double cosio, + const double betao, + const double theta2, + const double betao2, + const double xmdot, + const double omgdot, + const double xnodot) +{ + double se = 0.0; + double si = 0.0; + double sl = 0.0; + double sgh = 0.0; + double shdq = 0.0; + + double bfact = 0.0; + + static const double ZNS = 1.19459E-5; + static const double C1SS = 2.9864797E-6; + static const double ZES = 0.01675; + static const double ZNL = 1.5835218E-4; + static const double C1L = 4.7968065E-7; + static const double ZEL = 0.05490; + static const double ZCOSIS = 0.91744867; + static const double ZSINI = 0.39785416; + static const double ZSINGS = -0.98088458; + static const double ZCOSGS = 0.1945905; + static const double Q22 = 1.7891679E-6; + static const double Q31 = 2.1460748E-6; + static const double Q33 = 2.2123015E-7; + static const double ROOT22 = 1.7891679E-6; + static const double ROOT32 = 3.7393792E-7; + static const double ROOT44 = 7.3636953E-9; + static const double ROOT52 = 1.1428639E-7; + static const double ROOT54 = 2.1765803E-9; + + const double aqnv = 1.0 / elements_.RecoveredSemiMajorAxis(); + const double xpidot = omgdot + xnodot; + const double sinq = sin(elements_.AscendingNode()); + const double cosq = cos(elements_.AscendingNode()); + const double sing = sin(elements_.ArgumentPerigee()); + const double cosg = cos(elements_.ArgumentPerigee()); + + /* + * initialize lunar / solar terms + */ + const double jday = elements_.Epoch().ToJ2000(); + + const double xnodce = Util::WrapTwoPI(4.5236020 - 9.2422029e-4 * jday); + const double stem = sin(xnodce); + const double ctem = cos(xnodce); + const double zcosil = 0.91375164 - 0.03568096 * ctem; + const double zsinil = sqrt(1.0 - zcosil * zcosil); + const double zsinhl = 0.089683511 * stem / zsinil; + const double zcoshl = sqrt(1.0 - zsinhl * zsinhl); + const double c = 4.7199672 + 0.22997150 * jday; + const double gam = 5.8351514 + 0.0019443680 * jday; + deepspace_consts_.zmol = Util::WrapTwoPI(c - gam); + double zx = 0.39785416 * stem / zsinil; + double zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; + zx = atan2(zx, zy); + zx = gam + zx - xnodce; + + const double zcosgl = cos(zx); + const double zsingl = sin(zx); + deepspace_consts_.zmos = Util::WrapTwoPI(6.2565837 + 0.017201977 * jday); + + /* + * do solar terms + */ + double zcosg = ZCOSGS; + double zsing = ZSINGS; + double zcosi = ZCOSIS; + double zsini = ZSINI; + double zcosh = cosq; + double zsinh = sinq; + double cc = C1SS; + double zn = ZNS; + double ze = ZES; + const double xnoi = 1.0 / elements_.RecoveredMeanMotion(); + + for (int cnt = 0; cnt < 2; cnt++) + { + /* + * solar terms are done a second time after lunar terms are done + */ + const double a1 = zcosg * zcosh + zsing * zcosi * zsinh; + const double a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + const double a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + const double a8 = zsing * zsini; + const double a9 = zsing * zsinh + zcosg * zcosi*zcosh; + const double a10 = zcosg * zsini; + const double a2 = cosio * a7 + sinio * a8; + const double a4 = cosio * a9 + sinio * a10; + const double a5 = -sinio * a7 + cosio * a8; + const double a6 = -sinio * a9 + cosio * a10; + const double x1 = a1 * cosg + a2 * sing; + const double x2 = a3 * cosg + a4 * sing; + const double x3 = -a1 * sing + a2 * cosg; + const double x4 = -a3 * sing + a4 * cosg; + const double x5 = a5 * sing; + const double x6 = a6 * sing; + const double x7 = a5 * cosg; + const double x8 = a6 * cosg; + const double z31 = 12.0 * x1 * x1 - 3. * x3 * x3; + const double z32 = 24.0 * x1 * x2 - 6. * x3 * x4; + const double z33 = 12.0 * x2 * x2 - 3. * x4 * x4; + double z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * eosq; + double z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * eosq; + double z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * eosq; + + const double z11 = -6.0 * a1 * a5 + + eosq * (-24. * x1 * x7 - 6. * x3 * x5); + const double z12 = -6.0 * (a1 * a6 + a3 * a5) + + eosq * (-24. * (x2 * x7 + x1 * x8) - 6. * (x3 * x6 + x4 * x5)); + const double z13 = -6.0 * a3 * a6 + + eosq * (-24. * x2 * x8 - 6. * x4 * x6); + const double z21 = 6.0 * a2 * a5 + + eosq * (24. * x1 * x5 - 6. * x3 * x7); + const double z22 = 6.0 * (a4 * a5 + a2 * a6) + + eosq * (24. * (x2 * x5 + x1 * x6) - 6. * (x4 * x7 + x3 * x8)); + const double z23 = 6.0 * a4 * a6 + + eosq * (24. * x2 * x6 - 6. * x4 * x8); + + z1 = z1 + z1 + betao2 * z31; + z2 = z2 + z2 + betao2 * z32; + z3 = z3 + z3 + betao2 * z33; + + const double s3 = cc * xnoi; + const double s2 = -0.5 * s3 / betao; + const double s4 = s3 * betao; + const double s1 = -15.0 * elements_.Eccentricity() * s4; + const double s5 = x1 * x3 + x2 * x4; + const double s6 = x2 * x3 + x1 * x4; + const double s7 = x2 * x4 - x1 * x3; + + se = s1 * zn * s5; + si = s2 * zn * (z11 + z13); + sl = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eosq); + sgh = s4 * zn * (z31 + z33 - 6.0); + + /* + * replaced + * sh = -zn * s2 * (z21 + z23 + * with + * shdq = (-zn * s2 * (z21 + z23)) / sinio + */ + if (elements_.Inclination() < 5.2359877e-2 + || elements_.Inclination() > kPI - 5.2359877e-2) + { + shdq = 0.0; + } + else + { + shdq = (-zn * s2 * (z21 + z23)) / sinio; + } + + deepspace_consts_.ee2 = 2.0 * s1 * s6; + deepspace_consts_.e3 = 2.0 * s1 * s7; + deepspace_consts_.xi2 = 2.0 * s2 * z12; + deepspace_consts_.xi3 = 2.0 * s2 * (z13 - z11); + deepspace_consts_.xl2 = -2.0 * s3 * z2; + deepspace_consts_.xl3 = -2.0 * s3 * (z3 - z1); + deepspace_consts_.xl4 = -2.0 * s3 * (-21.0 - 9.0 * eosq) * ze; + deepspace_consts_.xgh2 = 2.0 * s4 * z32; + deepspace_consts_.xgh3 = 2.0 * s4 * (z33 - z31); + deepspace_consts_.xgh4 = -18.0 * s4 * ze; + deepspace_consts_.xh2 = -2.0 * s2 * z22; + deepspace_consts_.xh3 = -2.0 * s2 * (z23 - z21); + + if (cnt == 1) + { + break; + } + /* + * do lunar terms + */ + deepspace_consts_.sse = se; + deepspace_consts_.ssi = si; + deepspace_consts_.ssl = sl; + deepspace_consts_.ssh = shdq; + deepspace_consts_.ssg = sgh - cosio * deepspace_consts_.ssh; + deepspace_consts_.se2 = deepspace_consts_.ee2; + deepspace_consts_.si2 = deepspace_consts_.xi2; + deepspace_consts_.sl2 = deepspace_consts_.xl2; + deepspace_consts_.sgh2 = deepspace_consts_.xgh2; + deepspace_consts_.sh2 = deepspace_consts_.xh2; + deepspace_consts_.se3 = deepspace_consts_.e3; + deepspace_consts_.si3 = deepspace_consts_.xi3; + deepspace_consts_.sl3 = deepspace_consts_.xl3; + deepspace_consts_.sgh3 = deepspace_consts_.xgh3; + deepspace_consts_.sh3 = deepspace_consts_.xh3; + deepspace_consts_.sl4 = deepspace_consts_.xl4; + deepspace_consts_.sgh4 = deepspace_consts_.xgh4; + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cosq + zsinhl * sinq; + zsinh = sinq * zcoshl - cosq * zsinhl; + zn = ZNL; + cc = C1L; + ze = ZEL; + } + + deepspace_consts_.sse += se; + deepspace_consts_.ssi += si; + deepspace_consts_.ssl += sl; + deepspace_consts_.ssg += sgh - cosio * shdq; + deepspace_consts_.ssh += shdq; + + deepspace_consts_.shape = DeepSpaceConstants::NONE; + + if (elements_.RecoveredMeanMotion() < 0.0052359877 + && elements_.RecoveredMeanMotion() > 0.0034906585) + { + /* + * 24h synchronous resonance terms initialisation + */ + deepspace_consts_.shape = DeepSpaceConstants::SYNCHRONOUS; + + const double g200 = 1.0 + eosq * (-2.5 + 0.8125 * eosq); + const double g310 = 1.0 + 2.0 * eosq; + const double g300 = 1.0 + eosq * (-6.0 + 6.60937 * eosq); + const double f220 = 0.75 * (1.0 + cosio) * (1.0 + cosio); + const double f311 = 0.9375 * sinio * sinio * (1.0 + 3.0 * cosio) + - 0.75 * (1.0 + cosio); + double f330 = 1.0 + cosio; + f330 = 1.875 * f330 * f330 * f330; + deepspace_consts_.del1 = 3.0 * elements_.RecoveredMeanMotion() + * elements_.RecoveredMeanMotion() + * aqnv * aqnv; + deepspace_consts_.del2 = 2.0 * deepspace_consts_.del1 + * f220 * g200 * Q22; + deepspace_consts_.del3 = 3.0 * deepspace_consts_.del1 + * f330 * g300 * Q33 * aqnv; + deepspace_consts_.del1 = deepspace_consts_.del1 + * f311 * g310 * Q31 * aqnv; + + deepspace_consts_.xlamo = Util::WrapTwoPI(elements_.MeanAnomoly() + + elements_.AscendingNode() + + elements_.ArgumentPerigee() + - deepspace_consts_.gsto); + bfact = xmdot + xpidot - kTHDT + + deepspace_consts_.ssl + + deepspace_consts_.ssg + + deepspace_consts_.ssh; + } + else if (elements_.RecoveredMeanMotion() < 8.26e-3 + || elements_.RecoveredMeanMotion() > 9.24e-3 + || elements_.Eccentricity() < 0.5) + { + // do nothing + } + else + { + /* + * geopotential resonance initialisation for 12 hour orbits + */ + deepspace_consts_.shape = DeepSpaceConstants::RESONANCE; + + double g211; + double g310; + double g322; + double g410; + double g422; + double g520; + + double g201 = -0.306 - (elements_.Eccentricity() - 0.64) * 0.440; + + if (elements_.Eccentricity() <= 0.65) + { + g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), + 3.616, -13.247, 16.290, 0.0); + g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -19.302, 117.390, -228.419, 156.591); + g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -18.9068, 109.7927, -214.6334, 146.5816); + g410 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -41.122, 242.694, -471.094, 313.953); + g422 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -146.407, 841.880, -1629.014, 1083.435); + g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -532.114, 3017.977, -5740.032, 3708.276); + } + else + { + g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -72.099, 331.819, -508.738, 266.724); + g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -346.844, 1582.851, -2415.925, 1246.113); + g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -342.585, 1554.908, -2366.899, 1215.972); + g410 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -1052.797, 4758.686, -7193.992, 3651.957); + g422 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -3581.69, 16178.11, -24462.77, 12422.52); + + if (elements_.Eccentricity() <= 0.715) + { + g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), + 1464.74, -4664.75, 3763.64, 0.0); + } + else + { + g520 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -5149.66, 29936.92, -54087.36, 31324.56); + } + } + + double g533; + double g521; + double g532; + + if (elements_.Eccentricity() < 0.7) + { + g533 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -919.2277, 4988.61, -9064.77, 5542.21); + g521 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -822.71072, 4568.6173, -8491.4146, 5337.524); + g532 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -853.666, 4690.25, -8624.77, 5341.4); + } + else + { + g533 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -37995.78, 161616.52, -229838.2, 109377.94); + g521 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -51752.104, 218913.95, -309468.16, 146349.42); + g532 = EvaluateCubicPolynomial(elements_.Eccentricity(), + -40023.88, 170470.89, -242699.48, 115605.82); + } + + const double sini2 = sinio * sinio; + const double f220 = 0.75 * (1.0 + 2.0 * cosio + theta2); + const double f221 = 1.5 * sini2; + const double f321 = 1.875 * sinio * (1.0 - 2.0 * cosio - 3.0 * theta2); + const double f322 = -1.875 * sinio * (1.0 + 2.0 * cosio - 3.0 * theta2); + const double f441 = 35.0 * sini2 * f220; + const double f442 = 39.3750 * sini2 * sini2; + const double f522 = 9.84375 * sinio + * (sini2 * (1.0 - 2.0 * cosio - 5.0 * theta2) + + 0.33333333 * (-2.0 + 4.0 * cosio + 6.0 * theta2)); + const double f523 = sinio + * (4.92187512 * sini2 * (-2.0 - 4.0 * cosio + 10.0 * theta2) + + 6.56250012 * (1.0 + 2.0 * cosio - 3.0 * theta2)); + const double f542 = 29.53125 * sinio * (2.0 - 8.0 * cosio + theta2 * + (-12.0 + 8.0 * cosio + 10.0 * theta2)); + const double f543 = 29.53125 * sinio * (-2.0 - 8.0 * cosio + theta2 * + (12.0 + 8.0 * cosio - 10.0 * theta2)); + + const double xno2 = elements_.RecoveredMeanMotion() + * elements_.RecoveredMeanMotion(); + const double ainv2 = aqnv * aqnv; + + double temp1 = 3.0 * xno2 * ainv2; + double temp = temp1 * ROOT22; + deepspace_consts_.d2201 = temp * f220 * g201; + deepspace_consts_.d2211 = temp * f221 * g211; + + temp1 *= aqnv; + temp = temp1 * ROOT32; + deepspace_consts_.d3210 = temp * f321 * g310; + deepspace_consts_.d3222 = temp * f322 * g322; + + temp1 *= aqnv; + temp = 2.0 * temp1 * ROOT44; + deepspace_consts_.d4410 = temp * f441 * g410; + deepspace_consts_.d4422 = temp * f442 * g422; + + temp1 *= aqnv; + temp = temp1 * ROOT52; + deepspace_consts_.d5220 = temp * f522 * g520; + deepspace_consts_.d5232 = temp * f523 * g532; + + temp = 2.0 * temp1 * ROOT54; + deepspace_consts_.d5421 = temp * f542 * g521; + deepspace_consts_.d5433 = temp * f543 * g533; + + deepspace_consts_.xlamo = Util::WrapTwoPI( + elements_.MeanAnomoly() + + elements_.AscendingNode() + + elements_.AscendingNode() + - deepspace_consts_.gsto + - deepspace_consts_.gsto); + bfact = xmdot + + xnodot + xnodot + - kTHDT - kTHDT + + deepspace_consts_.ssl + + deepspace_consts_.ssh + + deepspace_consts_.ssh; + } + + if (deepspace_consts_.shape != DeepSpaceConstants::NONE) + { + /* + * initialise integrator + */ + deepspace_consts_.xfact = bfact - elements_.RecoveredMeanMotion(); + integrator_params_.atime = 0.0; + integrator_params_.xni = elements_.RecoveredMeanMotion(); + integrator_params_.xli = deepspace_consts_.xlamo; + } +} + +/** + * From DeepSpaceConstants, this uses: + * zmos, se2, se3, si2, si3, sl2, sl3, sl4, sgh2, sgh3, sgh4, sh2, sh3 + * zmol, ee2, e3, xi2, xi3, xl2, xl3, xl4, xgh2, xgh3, xgh4, xh2, xh3 + */ +void SGP4::DeepSpacePeriodics( + const double tsince, + const DeepSpaceConstants& ds_constants, + double& em, + double& xinc, + double& omgasm, + double& xnodes, + double& xll) +{ + static const double ZES = 0.01675; + static const double ZNS = 1.19459E-5; + static const double ZNL = 1.5835218E-4; + static const double ZEL = 0.05490; + + // calculate solar terms for time tsince + double zm = ds_constants.zmos + ZNS * tsince; + double zf = zm + 2.0 * ZES * sin(zm); + double sinzf = sin(zf); + double f2 = 0.5 * sinzf * sinzf - 0.25; + double f3 = -0.5 * sinzf * cos(zf); + + const double ses = ds_constants.se2 * f2 + + ds_constants.se3 * f3; + const double sis = ds_constants.si2 * f2 + + ds_constants.si3 * f3; + const double sls = ds_constants.sl2 * f2 + + ds_constants.sl3 * f3 + + ds_constants.sl4 * sinzf; + const double sghs = ds_constants.sgh2 * f2 + + ds_constants.sgh3 * f3 + + ds_constants.sgh4 * sinzf; + const double shs = ds_constants.sh2 * f2 + + ds_constants.sh3 * f3; + + // calculate lunar terms for time tsince + zm = ds_constants.zmol + ZNL * tsince; + zf = zm + 2.0 * ZEL * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + + const double sel = ds_constants.ee2 * f2 + + ds_constants.e3 * f3; + const double sil = ds_constants.xi2 * f2 + + ds_constants.xi3 * f3; + const double sll = ds_constants.xl2 * f2 + + ds_constants.xl3 * f3 + + ds_constants.xl4 * sinzf; + const double sghl = ds_constants.xgh2 * f2 + + ds_constants.xgh3 * f3 + + ds_constants.xgh4 * sinzf; + const double shl = ds_constants.xh2 * f2 + + ds_constants.xh3 * f3; + + // merge calculated values + const double pe = ses + sel; + const double pinc = sis + sil; + const double pl = sls + sll; + const double pgh = sghs + sghl; + const double ph = shs + shl; + + xinc += pinc; + em += pe; + + /* Spacetrack report #3 has sin/cos from before perturbations + * added to xinc (oldxinc), but apparently report # 6 has then + * from after they are added. + * use for strn3 + * if (elements_.Inclination() >= 0.2) + * use for gsfc + * if (xinc >= 0.2) + * (moved from start of function) + */ + const double sinis = sin(xinc); + const double cosis = cos(xinc); + + if (xinc >= 0.2) + { + // apply periodics directly + omgasm += pgh - cosis * ph / sinis; + xnodes += ph / sinis; + xll += pl; + } + else + { + // apply periodics with lyddane modification + const double sinok = sin(xnodes); + const double cosok = cos(xnodes); + double alfdp = sinis * sinok; + double betdp = sinis * cosok; + const double dalf = ph * cosok + pinc * cosis * sinok; + const double dbet = -ph * sinok + pinc * cosis * cosok; + alfdp += dalf; + betdp += dbet; + xnodes = Util::WrapTwoPI(xnodes); + double xls = xll + omgasm + cosis * xnodes; + double dls = pl + pgh - pinc * xnodes * sinis; + xls += dls; + const double oldxnodes = xnodes; + xnodes = atan2(alfdp, betdp); + /** + * Get perturbed xnodes in to same quadrant as original. + * RAAN is in the range of 0 to 360 degrees + * atan2 is in the range of -180 to 180 degrees + */ + if (fabs(oldxnodes - xnodes) > kPI) + { + if (xnodes < oldxnodes) + { + xnodes += kTWOPI; + } + else + { + xnodes -= kTWOPI; + } + } + + xll += pl; + omgasm = xls - xll - cosis * xnodes; + } +} + +void SGP4::DeepSpaceSecular( + const double tsince, + const OrbitalElements& elements, + const CommonConstants& c_constants, + const DeepSpaceConstants& ds_constants, + IntegratorParams& integ_params, + double& xll, + double& omgasm, + double& xnodes, + double& em, + double& xinc, + double& xn) +{ + static const double G22 = 5.7686396; + static const double G32 = 0.95240898; + static const double G44 = 1.8014998; + static const double G52 = 1.0508330; + static const double G54 = 4.4108898; + static const double FASX2 = 0.13130908; + static const double FASX4 = 2.8843198; + static const double FASX6 = 0.37448087; + + static const double STEP = 720.0; + static const double STEP2 = 259200.0; + + xll += ds_constants.ssl * tsince; + omgasm += ds_constants.ssg * tsince; + xnodes += ds_constants.ssh * tsince; + em += ds_constants.sse * tsince; + xinc += ds_constants.ssi * tsince; + + if (ds_constants.shape != DeepSpaceConstants::NONE) + { + double xndot = 0.0; + double xnddt = 0.0; + double xldot = 0.0; + /* + * 1st condition (if tsince is less than one time step from epoch) + * 2nd condition (if atime and + * tsince are of opposite signs, so zero crossing required) + * 3rd condition (if tsince is closer to zero than + * atime, only integrate away from zero) + */ + if (fabs(tsince) < STEP || + tsince * integ_params.atime <= 0.0 || + fabs(tsince) < fabs(integ_params.atime)) + { + // restart back at the epoch + integ_params.atime = 0.0; + // TODO: check + integ_params.xni = elements.RecoveredMeanMotion(); + // TODO: check + integ_params.xli = ds_constants.xlamo; + } + + bool running = true; + while (running) + { + // always calculate dot terms ready for integration beginning + // from the start of the range which is 'atime' + if (ds_constants.shape == DeepSpaceConstants::SYNCHRONOUS) + { + xndot = ds_constants.del1 * sin(integ_params.xli - FASX2) + + ds_constants.del2 * sin(2.0 * (integ_params.xli - FASX4)) + + ds_constants.del3 * sin(3.0 * (integ_params.xli - FASX6)); + xnddt = ds_constants.del1 * cos(integ_params.xli - FASX2) + + 2.0 * ds_constants.del2 * cos(2.0 * (integ_params.xli - FASX4)) + + 3.0 * ds_constants.del3 * cos(3.0 * (integ_params.xli - FASX6)); + } + else + { + // TODO: check + const double xomi = elements.ArgumentPerigee() + c_constants.omgdot * integ_params.atime; + const double x2omi = xomi + xomi; + const double x2li = integ_params.xli + integ_params.xli; + xndot = ds_constants.d2201 * sin(x2omi + integ_params.xli - G22) + + ds_constants.d2211 * sin(integ_params.xli - G22) + + ds_constants.d3210 * sin(xomi + integ_params.xli - G32) + + ds_constants.d3222 * sin(-xomi + integ_params.xli - G32) + + ds_constants.d4410 * sin(x2omi + x2li - G44) + + ds_constants.d4422 * sin(x2li - G44) + + ds_constants.d5220 * sin(xomi + integ_params.xli - G52) + + ds_constants.d5232 * sin(-xomi + integ_params.xli - G52) + + ds_constants.d5421 * sin(xomi + x2li - G54) + + ds_constants.d5433 * sin(-xomi + x2li - G54); + xnddt = ds_constants.d2201 * cos(x2omi + integ_params.xli - G22) + + ds_constants.d2211 * cos(integ_params.xli - G22) + + ds_constants.d3210 * cos(xomi + integ_params.xli - G32) + + ds_constants.d3222 * cos(-xomi + integ_params.xli - G32) + + ds_constants.d5220 * cos(xomi + integ_params.xli - G52) + + ds_constants.d5232 * cos(-xomi + integ_params.xli - G52) + + 2.0 * (ds_constants.d4410 * cos(x2omi + x2li - G44) + + ds_constants.d4422 * cos(x2li - G44) + + ds_constants.d5421 * cos(xomi + x2li - G54) + + ds_constants.d5433 * cos(-xomi + x2li - G54)); + } + xldot = integ_params.xni + ds_constants.xfact; + xnddt *= xldot; + + double ft = tsince - integ_params.atime; + if (fabs(ft) >= STEP) + { + const double delt = (ft >= 0.0 ? STEP : -STEP); + // integrate by a full step ('delt'), updating the cached + // values for the new 'atime' + integ_params.xli = integ_params.xli + xldot * delt + xndot * STEP2; + integ_params.xni = integ_params.xni + xndot * delt + xnddt * STEP2; + integ_params.atime += delt; + } + else + { + // integrate by the difference 'ft' remaining + xn = integ_params.xni + xndot * ft + + xnddt * ft * ft * 0.5; + const double xl_temp = integ_params.xli + xldot * ft + + xndot * ft * ft * 0.5; + + const double theta = Util::WrapTwoPI(ds_constants.gsto + tsince * kTHDT); + if (ds_constants.shape == DeepSpaceConstants::SYNCHRONOUS) + { + xll = xl_temp + theta - xnodes - omgasm; + } + else + { + xll = xl_temp + 2.0 * (theta - xnodes); + } + running = false; + } + } + } +} + +void SGP4::Reset() +{ + use_simple_model_ = false; + use_deep_space_ = false; + + std::memset(&common_consts_, 0, sizeof(common_consts_)); + std::memset(&nearspace_consts_, 0, sizeof(nearspace_consts_)); + std::memset(&deepspace_consts_, 0, sizeof(deepspace_consts_)); + std::memset(&integrator_params_, 0, sizeof(integrator_params_)); +} diff --git a/Sources/sgp4-f5cb54b/SatelliteException.cc b/Sources/sgp4-f5cb54b/SatelliteException.cc new file mode 100644 index 0000000..5fe1212 --- /dev/null +++ b/Sources/sgp4-f5cb54b/SatelliteException.cc @@ -0,0 +1 @@ +#include "SatelliteException.h" diff --git a/Sources/sgp4-f5cb54b/SolarPosition.cc b/Sources/sgp4-f5cb54b/SolarPosition.cc new file mode 100644 index 0000000..1ddad9b --- /dev/null +++ b/Sources/sgp4-f5cb54b/SolarPosition.cc @@ -0,0 +1,63 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "SolarPosition.h" + +#include "Globals.h" +#include "Util.h" + +#include + +Eci SolarPosition::FindPosition(const DateTime& dt) +{ + const double mjd = dt.ToJ2000(); + const double year = 1900 + mjd / 365.25; + const double T = (mjd + Delta_ET(year) / kSECONDS_PER_DAY) / 36525.0; + const double M = Util::DegreesToRadians(Util::Wrap360(358.47583 + + Util::Wrap360(35999.04975 * T) + - (0.000150 + 0.0000033 * T) * T * T)); + const double L = Util::DegreesToRadians(Util::Wrap360(279.69668 + + Util::Wrap360(36000.76892 * T) + + 0.0003025 * T*T)); + const double e = 0.01675104 - (0.0000418 + 0.000000126 * T) * T; + const double C = Util::DegreesToRadians((1.919460 + - (0.004789 + 0.000014 * T) * T) * sin(M) + + (0.020094 - 0.000100 * T) * sin(2 * M) + + 0.000293 * sin(3 * M)); + const double O = Util::DegreesToRadians( + Util::Wrap360(259.18 - 1934.142 * T)); + const double Lsa = Util::WrapTwoPI(L + C + - Util::DegreesToRadians(0.00569 - 0.00479 * sin(O))); + const double nu = Util::WrapTwoPI(M + C); + double R = 1.0000002 * (1 - e * e) / (1 + e * cos(nu)); + const double eps = Util::DegreesToRadians(23.452294 - (0.0130125 + + (0.00000164 - 0.000000503 * T) * T) * T + 0.00256 * cos(O)); + R = R * kAU; + + Vector solar_position(R * cos(Lsa), + R * sin(Lsa) * cos(eps), + R * sin(Lsa) * sin(eps), + R); + + return Eci(dt, solar_position); +} + +double SolarPosition::Delta_ET(double year) const +{ + return 26.465 + 0.747622 * (year - 1950) + 1.886913 + * sin(kTWOPI * (year - 1975) / 33); +} diff --git a/Sources/sgp4-f5cb54b/TimeSpan.cc b/Sources/sgp4-f5cb54b/TimeSpan.cc new file mode 100644 index 0000000..c426258 --- /dev/null +++ b/Sources/sgp4-f5cb54b/TimeSpan.cc @@ -0,0 +1,18 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "TimeSpan.h" diff --git a/Sources/sgp4-f5cb54b/Tle.cc b/Sources/sgp4-f5cb54b/Tle.cc new file mode 100644 index 0000000..07f62b6 --- /dev/null +++ b/Sources/sgp4-f5cb54b/Tle.cc @@ -0,0 +1,368 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "Tle.h" + +#include + +namespace +{ + static const unsigned int TLE1_COL_NORADNUM = 2; + static const unsigned int TLE1_LEN_NORADNUM = 5; + static const unsigned int TLE1_COL_INTLDESC_A = 9; + static const unsigned int TLE1_LEN_INTLDESC_A = 2; +// static const unsigned int TLE1_COL_INTLDESC_B = 11; + static const unsigned int TLE1_LEN_INTLDESC_B = 3; +// static const unsigned int TLE1_COL_INTLDESC_C = 14; + static const unsigned int TLE1_LEN_INTLDESC_C = 3; + static const unsigned int TLE1_COL_EPOCH_A = 18; + static const unsigned int TLE1_LEN_EPOCH_A = 2; + static const unsigned int TLE1_COL_EPOCH_B = 20; + static const unsigned int TLE1_LEN_EPOCH_B = 12; + static const unsigned int TLE1_COL_MEANMOTIONDT2 = 33; + static const unsigned int TLE1_LEN_MEANMOTIONDT2 = 10; + static const unsigned int TLE1_COL_MEANMOTIONDDT6 = 44; + static const unsigned int TLE1_LEN_MEANMOTIONDDT6 = 8; + static const unsigned int TLE1_COL_BSTAR = 53; + static const unsigned int TLE1_LEN_BSTAR = 8; +// static const unsigned int TLE1_COL_EPHEMTYPE = 62; +// static const unsigned int TLE1_LEN_EPHEMTYPE = 1; +// static const unsigned int TLE1_COL_ELNUM = 64; +// static const unsigned int TLE1_LEN_ELNUM = 4; + + static const unsigned int TLE2_COL_NORADNUM = 2; + static const unsigned int TLE2_LEN_NORADNUM = 5; + static const unsigned int TLE2_COL_INCLINATION = 8; + static const unsigned int TLE2_LEN_INCLINATION = 8; + static const unsigned int TLE2_COL_RAASCENDNODE = 17; + static const unsigned int TLE2_LEN_RAASCENDNODE = 8; + static const unsigned int TLE2_COL_ECCENTRICITY = 26; + static const unsigned int TLE2_LEN_ECCENTRICITY = 7; + static const unsigned int TLE2_COL_ARGPERIGEE = 34; + static const unsigned int TLE2_LEN_ARGPERIGEE = 8; + static const unsigned int TLE2_COL_MEANANOMALY = 43; + static const unsigned int TLE2_LEN_MEANANOMALY = 8; + static const unsigned int TLE2_COL_MEANMOTION = 52; + static const unsigned int TLE2_LEN_MEANMOTION = 11; + static const unsigned int TLE2_COL_REVATEPOCH = 63; + static const unsigned int TLE2_LEN_REVATEPOCH = 5; +} + +/** + * Initialise the tle object. + * @exception TleException + */ +void Tle::Initialize() +{ + if (!IsValidLineLength(line_one_)) + { + throw TleException("Invalid length for line one"); + } + + if (!IsValidLineLength(line_two_)) + { + throw TleException("Invalid length for line two"); + } + + if (line_one_[0] != '1') + { + throw TleException("Invalid line beginning for line one"); + } + + if (line_two_[0] != '2') + { + throw TleException("Invalid line beginning for line two"); + } + + unsigned int sat_number_1; + unsigned int sat_number_2; + + ExtractInteger(line_one_.substr(TLE1_COL_NORADNUM, + TLE1_LEN_NORADNUM), sat_number_1); + ExtractInteger(line_two_.substr(TLE2_COL_NORADNUM, + TLE2_LEN_NORADNUM), sat_number_2); + + if (sat_number_1 != sat_number_2) + { + throw TleException("Satellite numbers do not match"); + } + + norad_number_ = sat_number_1; + + if (name_.empty()) + { + name_ = line_one_.substr(TLE1_COL_NORADNUM, TLE1_LEN_NORADNUM); + } + + int_designator_ = line_one_.substr(TLE1_COL_INTLDESC_A, + TLE1_LEN_INTLDESC_A + TLE1_LEN_INTLDESC_B + TLE1_LEN_INTLDESC_C); + + unsigned int year = 0; + double day = 0.0; + + ExtractInteger(line_one_.substr(TLE1_COL_EPOCH_A, + TLE1_LEN_EPOCH_A), year); + ExtractDouble(line_one_.substr(TLE1_COL_EPOCH_B, + TLE1_LEN_EPOCH_B), 4, day); + ExtractDouble(line_one_.substr(TLE1_COL_MEANMOTIONDT2, + TLE1_LEN_MEANMOTIONDT2), 2, mean_motion_dt2_); + ExtractExponential(line_one_.substr(TLE1_COL_MEANMOTIONDDT6, + TLE1_LEN_MEANMOTIONDDT6), mean_motion_ddt6_); + ExtractExponential(line_one_.substr(TLE1_COL_BSTAR, + TLE1_LEN_BSTAR), bstar_); + + /* + * line 2 + */ + ExtractDouble(line_two_.substr(TLE2_COL_INCLINATION, + TLE2_LEN_INCLINATION), 4, inclination_); + ExtractDouble(line_two_.substr(TLE2_COL_RAASCENDNODE, + TLE2_LEN_RAASCENDNODE), 4, right_ascending_node_); + ExtractDouble(line_two_.substr(TLE2_COL_ECCENTRICITY, + TLE2_LEN_ECCENTRICITY), -1, eccentricity_); + ExtractDouble(line_two_.substr(TLE2_COL_ARGPERIGEE, + TLE2_LEN_ARGPERIGEE), 4, argument_perigee_); + ExtractDouble(line_two_.substr(TLE2_COL_MEANANOMALY, + TLE2_LEN_MEANANOMALY), 4, mean_anomaly_); + ExtractDouble(line_two_.substr(TLE2_COL_MEANMOTION, + TLE2_LEN_MEANMOTION), 3, mean_motion_); + ExtractInteger(line_two_.substr(TLE2_COL_REVATEPOCH, + TLE2_LEN_REVATEPOCH), orbit_number_); + + if (year < 57) + year += 2000; + else + year += 1900; + + epoch_ = DateTime(year, day); +} + +/** + * Check + * @param str The string to check + * @returns Whether true of the string has a valid length + */ +bool Tle::IsValidLineLength(const std::string& str) +{ + return str.length() == LineLength() ? true : false; +} + +/** + * Convert a string containing an integer + * @param[in] str The string to convert + * @param[out] val The result + * @exception TleException on conversion error + */ +void Tle::ExtractInteger(const std::string& str, unsigned int& val) +{ + bool found_digit = false; + unsigned int temp = 0; + + for (std::string::const_iterator i = str.begin(); i != str.end(); ++i) + { + if (isdigit(*i)) + { + found_digit = true; + temp = (temp * 10) + static_cast(*i - '0'); + } + else if (found_digit) + { + throw TleException("Unexpected non digit"); + } + else if (*i != ' ') + { + throw TleException("Invalid character"); + } + } + + if (!found_digit) + { + val = 0; + } + else + { + val = temp; + } +} + +/** + * Convert a string containing an double + * @param[in] str The string to convert + * @param[in] point_pos The position of the decimal point. (-1 if none) + * @param[out] val The result + * @exception TleException on conversion error + */ +void Tle::ExtractDouble(const std::string& str, int point_pos, double& val) +{ + std::string temp; + bool found_digit = false; + + for (std::string::const_iterator i = str.begin(); i != str.end(); ++i) + { + /* + * integer part + */ + if (point_pos >= 0 && i < str.begin() + point_pos - 1) + { + bool done = false; + + if (i == str.begin()) + { + if(*i == '-' || *i == '+') + { + /* + * first character could be signed + */ + temp += *i; + done = true; + } + } + + if (!done) + { + if (isdigit(*i)) + { + found_digit = true; + temp += *i; + } + else if (found_digit) + { + throw TleException("Unexpected non digit"); + } + else if (*i != ' ') + { + throw TleException("Invalid character"); + } + } + } + /* + * decimal point + */ + else if (point_pos >= 0 && i == str.begin() + point_pos - 1) + { + if (temp.length() == 0) + { + /* + * integer part is blank, so add a '0' + */ + temp += '0'; + } + + if (*i == '.') + { + /* + * decimal point found + */ + temp += *i; + } + else + { + throw TleException("Failed to find decimal point"); + } + } + /* + * fraction part + */ + else + { + if (i == str.begin() && point_pos == -1) + { + /* + * no decimal point expected, add 0. beginning + */ + temp += '0'; + temp += '.'; + } + + /* + * should be a digit + */ + if (isdigit(*i)) + { + temp += *i; + } + else + { + throw TleException("Invalid digit"); + } + } + } + + if (!Util::FromString(temp, val)) + { + throw TleException("Failed to convert value to double"); + } +} + +/** + * Convert a string containing an exponential + * @param[in] str The string to convert + * @param[out] val The result + * @exception TleException on conversion error + */ +void Tle::ExtractExponential(const std::string& str, double& val) +{ + std::string temp; + + for (std::string::const_iterator i = str.begin(); i != str.end(); ++i) + { + if (i == str.begin()) + { + if (*i == '-' || *i == '+' || *i == ' ') + { + if (*i == '-') + { + temp += *i; + } + temp += '0'; + temp += '.'; + } + else + { + throw TleException("Invalid sign"); + } + } + else if (i == str.end() - 2) + { + if (*i == '-' || *i == '+') + { + temp += 'e'; + temp += *i; + } + else + { + throw TleException("Invalid exponential sign"); + } + } + else + { + if (isdigit(*i)) + { + temp += *i; + } + else + { + throw TleException("Invalid digit"); + } + } + } + + if (!Util::FromString(temp, val)) + { + throw TleException("Failed to convert value to double"); + } +} diff --git a/Sources/sgp4-f5cb54b/TleException.cc b/Sources/sgp4-f5cb54b/TleException.cc new file mode 100644 index 0000000..6a23d01 --- /dev/null +++ b/Sources/sgp4-f5cb54b/TleException.cc @@ -0,0 +1 @@ +#include "TleException.h" diff --git a/Sources/sgp4-f5cb54b/Util.cc b/Sources/sgp4-f5cb54b/Util.cc new file mode 100644 index 0000000..2e9ee3d --- /dev/null +++ b/Sources/sgp4-f5cb54b/Util.cc @@ -0,0 +1,54 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "Util.h" + +#include +#include +#include + +namespace Util +{ + namespace + { + struct IsDigit: std::unary_function + { + bool operator()(char c) const + { + return std::isdigit(c, std::locale::classic()) == 0; + } + }; + } + + void TrimLeft(std::string& s) + { + s.erase(s.begin(), + std::find_if(s.begin(), s.end(), std::not1(IsDigit()))); + } + + void TrimRight(std::string& s) + { + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(IsDigit())).base(), + s.end()); + } + + void Trim(std::string& s) + { + TrimLeft(s); + TrimRight(s); + } +} diff --git a/Sources/sgp4-f5cb54b/Vector.cc b/Sources/sgp4-f5cb54b/Vector.cc new file mode 100644 index 0000000..1b12c95 --- /dev/null +++ b/Sources/sgp4-f5cb54b/Vector.cc @@ -0,0 +1,18 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "Vector.h" diff --git a/Sources/sgp4-f5cb54b/include/CoordGeodetic.h b/Sources/sgp4-f5cb54b/include/CoordGeodetic.h new file mode 100644 index 0000000..8ab7d8c --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/CoordGeodetic.h @@ -0,0 +1,129 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef COORDGEODETIC_H_ +#define COORDGEODETIC_H_ + +#include "Util.h" + +#include +#include +#include + +/** + * @brief Stores a geodetic location (latitude, longitude, altitude). + * + * Internally the values are stored in radians and kilometres. + */ +struct CoordGeodetic +{ +public: + /** + * Default constructor + */ + CoordGeodetic() + : latitude(0.0), + longitude(0.0), + altitude(0.0) + { + } + + /** + * Constructor + * @param[in] lat the latitude (degrees by default) + * @param[in] lon the longitude (degrees by default) + * @param[in] alt the altitude in kilometers + * @param[in] is_radians whether the latitude/longitude is in radians + */ + CoordGeodetic( + double lat, + double lon, + double alt, + bool is_radians = false) + { + if (is_radians) + { + latitude = lat; + longitude = lon; + } + else + { + latitude = Util::DegreesToRadians(lat); + longitude = Util::DegreesToRadians(lon); + } + altitude = alt; + } + + /** + * Copy constructor + * @param[in] geo object to copy from + */ + CoordGeodetic(const CoordGeodetic& geo) + { + latitude = geo.latitude; + longitude = geo.longitude; + altitude = geo.altitude; + } + + /** + * Assignment operator + * @param[in] geo object to copy from + */ + CoordGeodetic& operator=(const CoordGeodetic& geo) + { + if (this != &geo) + { + latitude = geo.latitude; + longitude = geo.longitude; + altitude = geo.altitude; + } + return *this; + } + + /** + * Dump this object to a string + * @returns string + */ + std::string ToString() const + { + std::stringstream ss; + ss << std::right << std::fixed << std::setprecision(3); + ss << "Lat: " << std::setw(8) << Util::RadiansToDegrees(latitude); + ss << ", Lon: " << std::setw(8) << Util::RadiansToDegrees(longitude); + ss << ", Alt: " << std::setw(10) << altitude; + return ss.str(); + } + + /** latitude in radians (-PI >= latitude < PI) */ + double latitude; + /** latitude in radians (-PI/2 >= latitude <= PI/2) */ + double longitude; + /** altitude in kilometers */ + double altitude; +}; + +/** + * Dump a Coordgeodetic to a stream + * @param[in,out] strm stream to output to + * @param[in] g the CoordGeodetic to print + */ +inline std::ostream& operator<<(std::ostream& strm, const CoordGeodetic& g) +{ + return strm << g.ToString(); +} + +#endif diff --git a/Sources/sgp4-f5cb54b/include/CoordTopocentric.h b/Sources/sgp4-f5cb54b/include/CoordTopocentric.h new file mode 100644 index 0000000..f9fb798 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/CoordTopocentric.h @@ -0,0 +1,126 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef COORDTOPOCENTRIC_H_ +#define COORDTOPOCENTRIC_H_ + +#include "Util.h" + +#include +#include +#include + +/** + * @brief Stores a topocentric location (azimuth, elevation, range and range + * rate). + * + * Azimuth and elevation are stored in radians. Range in kilometres. Range + * rate in kilometres/second. + */ +struct CoordTopocentric +{ +public: + /** + * Default constructor + */ + CoordTopocentric() + : azimuth(0.0) + , elevation(0.0) + , range(0.0) + , range_rate(0.0) + { + } + + /** + * Constructor + * @param[in] az azimuth in radians + * @param[in] el elevation in radians + * @param[in] rnge range in kilometers + * @param[in] rnge_rate range rate in kilometers per second + */ + CoordTopocentric( + double az, + double el, + double rnge, + double rnge_rate) + : azimuth(az) + , elevation(el) + , range(rnge) + , range_rate(rnge_rate) + { + } + + /** + * Copy constructor + * @param[in] topo object to copy from + */ + CoordTopocentric(const CoordTopocentric& topo) + { + azimuth = topo.azimuth; + elevation = topo.elevation; + range = topo.range; + range_rate = topo.range_rate; + } + + /** + * Assignment operator + * @param[in] topo object to copy from + */ + CoordTopocentric& operator=(const CoordTopocentric& topo) + { + if (this != &topo) + { + azimuth = topo.azimuth; + elevation = topo.elevation; + range = topo.range; + range_rate = topo.range_rate; + } + return *this; + } + + /** + * Dump this object to a string + * @returns string + */ + std::string ToString() const + { + std::stringstream ss; + ss << std::right << std::fixed << std::setprecision(3); + ss << "Az: " << std::setw(8) << Util::RadiansToDegrees(azimuth); + ss << ", El: " << std::setw(8) << Util::RadiansToDegrees(elevation); + ss << ", Rng: " << std::setw(10) << range; + ss << ", Rng Rt: " << std::setw(7) << range_rate; + return ss.str(); + } + + /** azimuth in radians */ + double azimuth; + /** elevations in radians */ + double elevation; + /** range in kilometers */ + double range; + /** range rate in kilometers per second */ + double range_rate; +}; + + +inline std::ostream& operator<<(std::ostream& strm, const CoordTopocentric& t) +{ + return strm << t.ToString(); +} + +#endif diff --git a/Sources/sgp4-f5cb54b/include/DateTime.h b/Sources/sgp4-f5cb54b/include/DateTime.h new file mode 100644 index 0000000..9c98301 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/DateTime.h @@ -0,0 +1,705 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef DATETIME_H_ +#define DATETIME_H_ + +#include +#include +#include +#include +#include +#include +#include "TimeSpan.h" +#include "Util.h" + +namespace +{ + static int daysInMonth[2][13] = { + // 1 2 3 4 5 6 7 8 9 10 11 12 + {0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}, + {0, 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31} + }; + static int cumulDaysInMonth[2][13] = { + // 1 2 3 4 5 6 7 8 9 10 11 12 + {0, 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}, + {0, 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335} + }; +} + +/** + * @brief Represents an instance in time. + */ +class DateTime +{ +public: + /** + * Default contructor + * Initialise to 0001/01/01 00:00:00.000000 + */ + DateTime() + { + Initialise(1, 1, 1, 0, 0, 0, 0); + } + + /** + * Constructor + * @param[in] ticks raw tick value + */ + DateTime(int64_t ticks) + : m_encoded(ticks) + { + } + + /** + * Constructor + * @param[in] year the year + * @param[in] doy the day of the year + */ + DateTime(unsigned int year, double doy) + { + m_encoded = TimeSpan( + static_cast(AbsoluteDays(year, doy) * TicksPerDay)).Ticks(); + } + + /** + * Constructor + * @param[in] year the year + * @param[in] month the month + * @param[in] day the day + */ + DateTime(int year, int month, int day) + { + Initialise(year, month, day, 0, 0, 0, 0); + } + + /** + * Constructor + * @param[in] year the year + * @param[in] month the month + * @param[in] day the day + * @param[in] hour the hour + * @param[in] minute the minute + * @param[in] second the second + */ + DateTime(int year, int month, int day, int hour, int minute, int second) + { + Initialise(year, month, day, hour, minute, second, 0); + } + + /** + * Constructor + * @param[in] year the year + * @param[in] month the month + * @param[in] day the day + * @param[in] hour the hour + * @param[in] minute the minute + * @param[in] second the second + * @param[in] microsecond the microsecond + */ + void Initialise(int year, + int month, + int day, + int hour, + int minute, + int second, + int microsecond) + { + if (!IsValidYearMonthDay(year, month, day) || + hour < 0 || hour > 23 || + minute < 0 || minute > 59 || + second < 0 || second > 59 || + microsecond < 0 || microsecond > 999999) + { + assert(false && "Invalid date"); + } + m_encoded = TimeSpan( + AbsoluteDays(year, month, day), + hour, + minute, + second, + microsecond).Ticks(); + } + + /** + * Return the current time + * @param[in] microseconds whether to set the microsecond component + * @returns a DateTime object set to the current date and time + */ + static DateTime Now(bool useMicroseconds = false) + { + using namespace std::chrono; + if (useMicroseconds) + { + return DateTime(UnixEpoch + + duration_cast(system_clock::now() + .time_since_epoch()).count() * TicksPerMicrosecond); + } + else + { + return DateTime(UnixEpoch + + duration_cast(system_clock::now() + .time_since_epoch()).count() * TicksPerSecond); + } + } + + /** + * Find whether a year is a leap year + * @param[in] year the year to check + * @returns whether the year is a leap year + */ + static bool IsLeapYear(int year) + { + if (!IsValidYear(year)) + { + assert(false && "Invalid year"); + } + + return (((year % 4) == 0 && (year % 100) != 0) || (year % 400) == 0); + } + + /** + * Checks whether the given year is valid + * @param[in] year the year to check + * @returns whether the year is valid + */ + static bool IsValidYear(int year) + { + bool valid = true; + if (year < 1 || year > 9999) + { + valid = false; + } + return valid; + } + + /** + * Check whether the year/month is valid + * @param[in] year the year to check + * @param[in] month the month to check + * @returns whether the year/month is valid + */ + static bool IsValidYearMonth(int year, int month) + { + bool valid = true; + if (IsValidYear(year)) + { + if (month < 1 || month > 12) + { + valid = false; + } + } + else + { + valid = false; + } + return valid; + } + + /** + * Check whether the year/month/day is valid + * @param[in] year the year to check + * @param[in] month the month to check + * @param[in] day the day to check + * @returns whether the year/month/day is valid + */ + static bool IsValidYearMonthDay(int year, int month, int day) + { + bool valid = true; + if (IsValidYearMonth(year, month)) + { + if (day < 1 || day > DaysInMonth(year, month)) + { + valid = false; + } + } + else + { + valid = false; + } + return valid; + } + + /** + * Find the number of days in a month given the year/month + * @param[in] year the year + * @param[in] month the month + * @returns the days in the given month + */ + static int DaysInMonth(int year, int month) + { + if (!IsValidYearMonth(year, month)) + { + assert(false && "Invalid year and month"); + } + + const int* daysInMonthPtr; + + if (IsLeapYear(year)) + { + daysInMonthPtr = daysInMonth[1]; + } + else + { + daysInMonthPtr = daysInMonth[0]; + } + + return daysInMonthPtr[month]; + } + + /** + * Find the day of the year given the year/month/day + * @param[in] year the year + * @param[in] month the month + * @param[in] day the day + * @returns the day of the year + */ + int DayOfYear(int year, int month, int day) const + { + if (!IsValidYearMonthDay(year, month, day)) + { + assert(false && "Invalid year, month and day"); + } + + int daysThisYear = day; + + if (IsLeapYear(year)) + { + daysThisYear += cumulDaysInMonth[1][month]; + } + else + { + daysThisYear += cumulDaysInMonth[0][month]; + } + + return daysThisYear; + } + + /** + * + */ + double AbsoluteDays(unsigned int year, double doy) const + { + int64_t previousYear = year - 1; + + /* + * + days in previous years ignoring leap days + * + Julian leap days before this year + * - minus prior century years + * + plus prior years divisible by 400 days + */ + int64_t daysSoFar = 365 * previousYear + + previousYear / 4LL + - previousYear / 100LL + + previousYear / 400LL; + + return static_cast(daysSoFar) + doy - 1.0; + } + + int AbsoluteDays(int year, int month, int day) const + { + int previousYear = year - 1; + + /* + * days this year (0 - ...) + * + days in previous years ignoring leap days + * + Julian leap days before this year + * - minus prior century years + * + plus prior years divisible by 400 days + */ + int result = DayOfYear(year, month, day) - 1 + + 365 * previousYear + + previousYear / 4 + - previousYear / 100 + + previousYear / 400; + + return result; + } + + TimeSpan TimeOfDay() const + { + return TimeSpan(Ticks() % TicksPerDay); + } + + int DayOfWeek() const + { + /* + * The fixed day 1 (January 1, 1 Gregorian) is Monday. + * 0 Sunday + * 1 Monday + * 2 Tuesday + * 3 Wednesday + * 4 Thursday + * 5 Friday + * 6 Saturday + */ + return static_cast(((m_encoded / TicksPerDay) + 1LL) % 7LL); + } + + bool Equals(const DateTime& dt) const + { + return (m_encoded == dt.m_encoded); + } + + int Compare(const DateTime& dt) const + { + int ret = 0; + + if (m_encoded < dt.m_encoded) + { + return -1; + } + else if (m_encoded > dt.m_encoded) + { + return 1; + } + + return ret; + } + + DateTime AddYears(const int years) const + { + return AddMonths(years * 12); + } + + DateTime AddMonths(const int months) const + { + int year; + int month; + int day; + FromTicks(year, month, day); + month += months % 12; + year += months / 12; + + if (month < 1) + { + month += 12; + --year; + } + else if (month > 12) + { + month -= 12; + ++year; + } + + int maxday = DaysInMonth(year, month); + day = std::min(day, maxday); + + return DateTime(year, month, day).Add(TimeOfDay()); + } + + /** + * Add a TimeSpan to this DateTime + * @param[in] t the TimeSpan to add + * @returns a DateTime which has the given TimeSpan added + */ + DateTime Add(const TimeSpan& t) const + { + return AddTicks(t.Ticks()); + } + + DateTime AddDays(const double days) const + { + return AddMicroseconds(days * 86400000000.0); + } + + DateTime AddHours(const double hours) const + { + return AddMicroseconds(hours * 3600000000.0); + } + + DateTime AddMinutes(const double minutes) const + { + return AddMicroseconds(minutes * 60000000.0); + } + + DateTime AddSeconds(const double seconds) const + { + return AddMicroseconds(seconds * 1000000.0); + } + + DateTime AddMicroseconds(const double microseconds) const + { + int64_t ticks = static_cast(microseconds * TicksPerMicrosecond); + return AddTicks(ticks); + } + + DateTime AddTicks(int64_t ticks) const + { + return DateTime(m_encoded + ticks); + } + + /** + * Get the number of ticks + * @returns the number of ticks + */ + int64_t Ticks() const + { + return m_encoded; + } + + void FromTicks(int& year, int& month, int& day) const + { + int totalDays = static_cast(m_encoded / TicksPerDay); + + /* + * number of 400 year cycles + */ + int num400 = totalDays / 146097; + totalDays -= num400 * 146097; + /* + * number of 100 year cycles + */ + int num100 = totalDays / 36524; + if (num100 == 4) + { + /* + * last day of the last leap century + */ + num100 = 3; + } + totalDays -= num100 * 36524; + /* + * number of 4 year cycles + */ + int num4 = totalDays / 1461; + totalDays -= num4 * 1461; + /* + * number of years + */ + int num1 = totalDays / 365; + if (num1 == 4) + { + /* + * last day of the last leap olympiad + */ + num1 = 3; + } + totalDays -= num1 * 365; + + /* + * find year + */ + year = (num400 * 400) + (num100 * 100) + (num4 * 4) + num1 + 1; + + /* + * convert day of year to month/day + */ + const int* daysInMonthPtr; + if (IsLeapYear(year)) + { + daysInMonthPtr = daysInMonth[1]; + } + else + { + daysInMonthPtr = daysInMonth[0]; + } + + month = 1; + while (totalDays >= daysInMonthPtr[month] && month <= 12) + { + totalDays -= daysInMonthPtr[month++]; + } + + day = totalDays + 1; + } + + int Year() const + { + int year; + int month; + int day; + FromTicks(year, month, day); + return year; + } + + int Month() const + { + int year; + int month; + int day; + FromTicks(year, month, day); + return month; + } + + int Day() const + { + int year; + int month; + int day; + FromTicks(year, month, day); + return day; + } + + /** + * Hour component + * @returns the hour component + */ + int Hour() const + { + return static_cast(m_encoded % TicksPerDay / TicksPerHour); + } + + /** + * Minute component + * @returns the minute component + */ + int Minute() const + { + return static_cast(m_encoded % TicksPerHour / TicksPerMinute); + } + + /** + * Second component + * @returns the Second component + */ + int Second() const + { + return static_cast(m_encoded % TicksPerMinute / TicksPerSecond); + } + + /** + * Microsecond component + * @returns the microsecond component + */ + int Microsecond() const + { + return static_cast(m_encoded % TicksPerSecond / TicksPerMicrosecond); + } + + /** + * Convert to a julian date + * @returns the julian date + */ + double ToJulian() const + { + TimeSpan ts = TimeSpan(Ticks()); + return ts.TotalDays() + 1721425.5; + } + + /** + * Convert to greenwich sidereal time + * @returns the greenwich sidereal time + */ + double ToGreenwichSiderealTime() const + { + // julian date of previous midnight + double jd0 = floor(ToJulian() + 0.5) - 0.5; + // julian centuries since epoch + double t = (jd0 - 2451545.0) / 36525.0; + double jdf = ToJulian() - jd0; + + double gt = 24110.54841 + t * (8640184.812866 + t * (0.093104 - t * 6.2E-6)); + gt += jdf * 1.00273790935 * 86400.0; + + // 360.0 / 86400.0 = 1.0 / 240.0 + return Util::WrapTwoPI(Util::DegreesToRadians(gt / 240.0)); + } + + /** + * Return the modified julian date since the j2000 epoch + * January 1, 2000, at 12:00 TT + * @returns the modified julian date + */ + double ToJ2000() const + { + return ToJulian() - 2415020.0; + } + + /** + * Convert to local mean sidereal time (GMST plus the observer's longitude) + * @param[in] lon observers longitude + * @returns the local mean sidereal time + */ + double ToLocalMeanSiderealTime(const double lon) const + { + return Util::WrapTwoPI(ToGreenwichSiderealTime() + lon); + } + + std::string ToString() const + { + std::stringstream ss; + int year; + int month; + int day; + FromTicks(year, month, day); + ss << std::right << std::setfill('0'); + ss << std::setw(4) << year << "-"; + ss << std::setw(2) << month << "-"; + ss << std::setw(2) << day << " "; + ss << std::setw(2) << Hour() << ":"; + ss << std::setw(2) << Minute() << ":"; + ss << std::setw(2) << Second() << "."; + ss << std::setw(6) << Microsecond() << " UTC"; + return ss.str(); + } + +private: + int64_t m_encoded; +}; + +inline std::ostream& operator<<(std::ostream& strm, const DateTime& dt) +{ + return strm << dt.ToString(); +} + +inline DateTime operator+(const DateTime& dt, TimeSpan ts) +{ + return DateTime(dt.Ticks() + ts.Ticks()); +} + +inline DateTime operator-(const DateTime& dt, const TimeSpan& ts) +{ + return DateTime(dt.Ticks() - ts.Ticks()); +} + +inline TimeSpan operator-(const DateTime& dt1, const DateTime& dt2) +{ + return TimeSpan(dt1.Ticks() - dt2.Ticks()); +} + +inline bool operator==(const DateTime& dt1, const DateTime& dt2) +{ + return dt1.Equals(dt2); +} + +inline bool operator>(const DateTime& dt1, const DateTime& dt2) +{ + return (dt1.Compare(dt2) > 0); +} + +inline bool operator>=(const DateTime& dt1, const DateTime& dt2) +{ + return (dt1.Compare(dt2) >= 0); +} + +inline bool operator!=(const DateTime& dt1, const DateTime& dt2) +{ + return !dt1.Equals(dt2); +} + +inline bool operator<(const DateTime& dt1, const DateTime& dt2) +{ + return (dt1.Compare(dt2) < 0); +} + +inline bool operator<=(const DateTime& dt1, const DateTime& dt2) +{ + return (dt1.Compare(dt2) <= 0); +} + +#endif diff --git a/Sources/sgp4-f5cb54b/include/DecayedException.h b/Sources/sgp4-f5cb54b/include/DecayedException.h new file mode 100644 index 0000000..b7c769f --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/DecayedException.h @@ -0,0 +1,77 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef DECAYEDEXCEPTION_H_ +#define DECAYEDEXCEPTION_H_ + +#include "DateTime.h" +#include "Vector.h" + +#include +#include + +/** + * @brief The exception that the SGP4 class throws when a satellite decays. + */ +class DecayedException : public std::runtime_error +{ +public: + /** + * Constructor + * @param[in] dt time of the event + * @param[in] pos position of the satellite at dt + * @param[in] vel velocity of the satellite at dt + */ + DecayedException(const DateTime& dt, const Vector& pos, const Vector& vel) + : runtime_error("Satellite decayed") + , _dt(dt) + , _pos(pos) + , _vel(vel) + { + } + + /** + * @returns the date + */ + DateTime Decayed() const + { + return _dt; + } + + /** + * @returns the position + */ + Vector Position() const + { + return _pos; + } + + /** + * @returns the velocity + */ + Vector Velocity() const + { + return _vel; + } + +private: + DateTime _dt; + Vector _pos; + Vector _vel; +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/Eci.h b/Sources/sgp4-f5cb54b/include/Eci.h new file mode 100644 index 0000000..1a6bd71 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/Eci.h @@ -0,0 +1,144 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ECI_H_ +#define ECI_H_ + +#include "CoordGeodetic.h" +#include "Vector.h" +#include "DateTime.h" + +/** + * @brief Stores an Earth-centered inertial position for a particular time. + */ +class Eci +{ +public: + + /** + * @param[in] dt the date to be used for this position + * @param[in] latitude the latitude in degrees + * @param[in] longitude the longitude in degrees + * @param[in] altitude the altitude in kilometers + */ + Eci(const DateTime& dt, + const double latitude, + const double longitude, + const double altitude) + { + ToEci(dt, CoordGeodetic(latitude, longitude, altitude)); + } + + /** + * @param[in] dt the date to be used for this position + * @param[in] geo the position + */ + Eci(const DateTime& dt, const CoordGeodetic& geo) + { + ToEci(dt, geo); + } + + /** + * @param[in] dt the date to be used for this position + * @param[in] position the position + */ + Eci(const DateTime &dt, const Vector &position) + : m_dt(dt) + , m_position(position) + { + } + + /** + * @param[in] dt the date to be used for this position + * @param[in] position the position + * @param[in] velocity the velocity + */ + Eci(const DateTime &dt, const Vector &position, const Vector &velocity) + : m_dt(dt) + , m_position(position) + , m_velocity(velocity) + { + } + + /** + * Equality operator + * @param dt the date to compare + * @returns true if the object matches + */ + bool operator==(const DateTime& dt) const + { + return m_dt == dt; + } + + /** + * Inequality operator + * @param dt the date to compare + * @returns true if the object doesn't match + */ + bool operator!=(const DateTime& dt) const + { + return m_dt != dt; + } + + /** + * Update this object with a new date and geodetic position + * @param dt new date + * @param geo new geodetic position + */ + void Update(const DateTime& dt, const CoordGeodetic& geo) + { + ToEci(dt, geo); + } + + /** + * @returns the position + */ + Vector Position() const + { + return m_position; + } + + /** + * @returns the velocity + */ + Vector Velocity() const + { + return m_velocity; + } + + /** + * @returns the date + */ + DateTime GetDateTime() const + { + return m_dt; + } + + /** + * @returns the position in geodetic form + */ + CoordGeodetic ToGeodetic() const; + +private: + void ToEci(const DateTime& dt, const CoordGeodetic& geo); + + DateTime m_dt; + Vector m_position; + Vector m_velocity; +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/Globals.h b/Sources/sgp4-f5cb54b/include/Globals.h new file mode 100644 index 0000000..c31fd80 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/Globals.h @@ -0,0 +1,76 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef GLOBALS_H_ +#define GLOBALS_H_ + +#include + +const double kAE = 1.0; +const double kQ0 = 120.0; +const double kS0 = 78.0; +const double kMU = 398600.8; +const double kXKMPER = 6378.135; +const double kXJ2 = 1.082616e-3; +const double kXJ3 = -2.53881e-6; +const double kXJ4 = -1.65597e-6; + +/* + * alternative XKE + * affects final results + * aiaa-2006-6573 + * const double kXKE = 60.0 / sqrt(kXKMPER * kXKMPER * kXKMPER / kMU); + * dundee + * const double kXKE = 7.43669161331734132e-2; + */ +const double kXKE = 60.0 / sqrt(kXKMPER * kXKMPER * kXKMPER / kMU); +const double kCK2 = 0.5 * kXJ2 * kAE * kAE; +const double kCK4 = -0.375 * kXJ4 * kAE * kAE * kAE * kAE; + +/* + * alternative QOMS2T + * affects final results + * aiaa-2006-6573 + * #define QOMS2T (pow(((Q0 - S0) / XKMPER), 4.0)) + * dundee + * #define QOMS2T (1.880279159015270643865e-9) + */ +const double kQOMS2T = pow(((kQ0 - kS0) / kXKMPER), 4.0); + +const double kS = kAE * (1.0 + kS0 / kXKMPER); +const double kPI = 3.14159265358979323846264338327950288419716939937510582; +const double kTWOPI = 2.0 * kPI; +const double kTWOTHIRD = 2.0 / 3.0; +const double kTHDT = 4.37526908801129966e-3; +/* + * earth flattening + */ +const double kF = 1.0 / 298.26; +/* + * earth rotation per sideral day + */ +const double kOMEGA_E = 1.00273790934; +const double kAU = 1.49597870691e8; + +const double kSECONDS_PER_DAY = 86400.0; +const double kMINUTES_PER_DAY = 1440.0; +const double kHOURS_PER_DAY = 24.0; + +const double kA3OVK2 = -kXJ3 / kCK2 * kAE * kAE * kAE; + +#endif + diff --git a/Sources/sgp4-f5cb54b/include/Observer.h b/Sources/sgp4-f5cb54b/include/Observer.h new file mode 100644 index 0000000..16238f4 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/Observer.h @@ -0,0 +1,102 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef OBSERVER_H_ +#define OBSERVER_H_ + +#include "CoordGeodetic.h" +#include "Eci.h" + +class DateTime; +struct CoordTopocentric; + +/** + * @brief Stores an observers location in Eci coordinates. + */ +class Observer +{ +public: + /** + * Constructor + * @param[in] latitude observers latitude in degrees + * @param[in] longitude observers longitude in degrees + * @param[in] altitude observers altitude in kilometers + */ + Observer(const double latitude, + const double longitude, + const double altitude) + : m_geo(latitude, longitude, altitude) + , m_eci(DateTime(), m_geo) + { + } + + /** + * Constructor + * @param[in] geo the observers position + */ + Observer(const CoordGeodetic &geo) + : m_geo(geo) + , m_eci(DateTime(), geo) + { + } + + /** + * Set the observers location + * @param[in] geo the observers position + */ + void SetLocation(const CoordGeodetic& geo) + { + m_geo = geo; + m_eci.Update(m_eci.GetDateTime(), m_geo); + } + + /** + * Get the observers location + * @returns the observers position + */ + CoordGeodetic GetLocation() const + { + return m_geo; + } + + /** + * Get the look angle for the observers position to the object + * @param[in] eci the object to find the look angle to + * @returns the lookup angle + */ + CoordTopocentric GetLookAngle(const Eci &eci); + +private: + /** + * @param[in] dt the date to update the observers position for + */ + void Update(const DateTime &dt) + { + if (m_eci != dt) + { + m_eci.Update(dt, m_geo); + } + } + + /** the observers position */ + CoordGeodetic m_geo; + /** the observers Eci for a particular time */ + Eci m_eci; +}; + +#endif + diff --git a/Sources/sgp4-f5cb54b/include/OrbitalElements.h b/Sources/sgp4-f5cb54b/include/OrbitalElements.h new file mode 100644 index 0000000..e11e2c6 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/OrbitalElements.h @@ -0,0 +1,145 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ORBITALELEMENTS_H_ +#define ORBITALELEMENTS_H_ + +#include "Util.h" +#include "DateTime.h" + +class Tle; + +/** + * @brief The extracted orbital elements used by the SGP4 propagator. + */ +class OrbitalElements +{ +public: + OrbitalElements(const Tle& tle); + + /* + * XMO + */ + double MeanAnomoly() const + { + return mean_anomoly_; + } + + /* + * XNODEO + */ + double AscendingNode() const + { + return ascending_node_; + } + + /* + * OMEGAO + */ + double ArgumentPerigee() const + { + return argument_perigee_; + } + + /* + * EO + */ + double Eccentricity() const + { + return eccentricity_; + } + + /* + * XINCL + */ + double Inclination() const + { + return inclination_; + } + + /* + * XNO + */ + double MeanMotion() const + { + return mean_motion_; + } + + /* + * BSTAR + */ + double BStar() const + { + return bstar_; + } + + /* + * AODP + */ + double RecoveredSemiMajorAxis() const + { + return recovered_semi_major_axis_; + } + + /* + * XNODP + */ + double RecoveredMeanMotion() const + { + return recovered_mean_motion_; + } + + /* + * PERIGE + */ + double Perigee() const + { + return perigee_; + } + + /* + * Period in minutes + */ + double Period() const + { + return period_; + } + + /* + * EPOCH + */ + DateTime Epoch() const + { + return epoch_; + } + +private: + double mean_anomoly_; + double ascending_node_; + double argument_perigee_; + double eccentricity_; + double inclination_; + double mean_motion_; + double bstar_; + double recovered_semi_major_axis_; + double recovered_mean_motion_; + double perigee_; + double period_; + DateTime epoch_; +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/SGP4.h b/Sources/sgp4-f5cb54b/include/SGP4.h new file mode 100644 index 0000000..4cb68ea --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/SGP4.h @@ -0,0 +1,258 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef SGP4_H_ +#define SGP4_H_ + +#include "Tle.h" +#include "OrbitalElements.h" +#include "Eci.h" +#include "SatelliteException.h" +#include "DecayedException.h" + +/** + * @mainpage + * + * This documents the SGP4 tracking library. + */ + +/** + * @brief The simplified perturbations model 4 propagater. + */ +class SGP4 +{ +public: + SGP4(const Tle& tle) + : elements_(tle) + { + Initialise(); + } + + void SetTle(const Tle& tle); + Eci FindPosition(double tsince) const; + Eci FindPosition(const DateTime& date) const; + +private: + struct CommonConstants + { + double cosio; + double sinio; + double eta; + double t2cof; + double x1mth2; + double x3thm1; + double x7thm1; + double aycof; + double xlcof; + double xnodcf; + double c1; + double c4; + double omgdot; // secular rate of omega (radians/sec) + double xnodot; // secular rate of xnode (radians/sec) + double xmdot; // secular rate of xmo (radians/sec) + }; + + struct NearSpaceConstants + { + double c5; + double omgcof; + double xmcof; + double delmo; + double sinmo; + double d2; + double d3; + double d4; + double t3cof; + double t4cof; + double t5cof; + }; + + struct DeepSpaceConstants + { + double gsto; + double zmol; + double zmos; + + /* + * lunar / solar constants for epoch + * applied during DeepSpaceSecular() + */ + double sse; + double ssi; + double ssl; + double ssg; + double ssh; + /* + * lunar / solar constants + * used during DeepSpaceCalculateLunarSolarTerms() + */ + double se2; + double si2; + double sl2; + double sgh2; + double sh2; + double se3; + double si3; + double sl3; + double sgh3; + double sh3; + double sl4; + double sgh4; + double ee2; + double e3; + double xi2; + double xi3; + double xl2; + double xl3; + double xl4; + double xgh2; + double xgh3; + double xgh4; + double xh2; + double xh3; + /* + * used during DeepSpaceCalcDotTerms() + */ + double d2201; + double d2211; + double d3210; + double d3222; + double d4410; + double d4422; + double d5220; + double d5232; + double d5421; + double d5433; + double del1; + double del2; + double del3; + /* + * integrator constants + */ + double xfact; + double xlamo; + + enum TOrbitShape + { + NONE, + RESONANCE, + SYNCHRONOUS + } shape; + }; + + struct IntegratorParams + { + /* + * integrator values + */ + double xli; + double xni; + double atime; + }; + + void Initialise(); + static void RecomputeConstants(const double xinc, + double& sinio, + double& cosio, + double& x3thm1, + double& x1mth2, + double& x7thm1, + double& xlcof, + double& aycof); + Eci FindPositionSDP4(const double tsince) const; + Eci FindPositionSGP4(double tsince) const; + static Eci CalculateFinalPositionVelocity( + const DateTime& date, + const double e, + const double a, + const double omega, + const double xl, + const double xnode, + const double xinc, + const double xlcof, + const double aycof, + const double x3thm1, + const double x1mth2, + const double x7thm1, + const double cosio, + const double sinio); + /** + * Deep space initialisation + */ + void DeepSpaceInitialise( + const double eosq, + const double sinio, + const double cosio, + const double betao, + const double theta2, + const double betao2, + const double xmdot, + const double omgdot, + const double xnodot); + /** + * Calculate lunar / solar periodics and apply + */ + static void DeepSpacePeriodics( + const double tsince, + const DeepSpaceConstants& ds_constants, + double& em, + double& xinc, + double& omgasm, + double& xnodes, + double& xll); + /** + * Deep space secular effects + */ + static void DeepSpaceSecular( + const double tsince, + const OrbitalElements& elements, + const CommonConstants& c_constants, + const DeepSpaceConstants& ds_constants, + IntegratorParams& integ_params, + double& xll, + double& omgasm, + double& xnodes, + double& em, + double& xinc, + double& xn); + + /** + * Reset + */ + void Reset(); + + /* + * the constants used + */ + struct CommonConstants common_consts_; + struct NearSpaceConstants nearspace_consts_; + struct DeepSpaceConstants deepspace_consts_; + mutable struct IntegratorParams integrator_params_; + + /* + * the orbit data + */ + OrbitalElements elements_; + + /* + * flags + */ + bool use_simple_model_; + bool use_deep_space_; +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/SatelliteException.h b/Sources/sgp4-f5cb54b/include/SatelliteException.h new file mode 100644 index 0000000..a771a75 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/SatelliteException.h @@ -0,0 +1,36 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef SATELLITEEXCEPTION_H_ +#define SATELLITEEXCEPTION_H_ + +#include +#include + +/** + * @brief The exception that the SGP4 class throws upon an error. + */ +class SatelliteException : public std::runtime_error +{ +public: + SatelliteException(const char* message) + : runtime_error(message) + { + } +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/SolarPosition.h b/Sources/sgp4-f5cb54b/include/SolarPosition.h new file mode 100644 index 0000000..e74c548 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/SolarPosition.h @@ -0,0 +1,40 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef SOLARPOSITION_H_ +#define SOLARPOSITION_H_ + +#include "DateTime.h" +#include "Eci.h" + +/** + * @brief Find the position of the sun + */ +class SolarPosition +{ +public: + SolarPosition() + { + } + + Eci FindPosition(const DateTime& dt); + +private: + double Delta_ET(double year) const; +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/TimeSpan.h b/Sources/sgp4-f5cb54b/include/TimeSpan.h new file mode 100644 index 0000000..21edeff --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/TimeSpan.h @@ -0,0 +1,257 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef TIMESPAN_H_ +#define TIMESPAN_H_ + +#include +#include +#include +#include +#include + +namespace +{ + static const int64_t TicksPerDay = 86400000000LL; + static const int64_t TicksPerHour = 3600000000LL; + static const int64_t TicksPerMinute = 60000000LL; + static const int64_t TicksPerSecond = 1000000LL; + static const int64_t TicksPerMillisecond = 1000LL; + static const int64_t TicksPerMicrosecond = 1LL; + + static const int64_t UnixEpoch = 62135596800000000LL; + + static const int64_t MaxValueTicks = 315537897599999999LL; + + // 1582-Oct-15 + static const int64_t GregorianStart = 49916304000000000LL; +} + +/** + * @brief Represents a time interval. + * + * Represents a time interval (duration/elapsed) that is measured as a positive + * or negative number of days, hours, minutes, seconds, and fractions + * of a second. + */ +class TimeSpan +{ +public: + TimeSpan(int64_t ticks) + : m_ticks(ticks) + { + } + + TimeSpan(int hours, int minutes, int seconds) + { + CalculateTicks(0, hours, minutes, seconds, 0); + } + + TimeSpan(int days, int hours, int minutes, int seconds) + { + CalculateTicks(days, hours, minutes, seconds, 0); + } + + TimeSpan(int days, int hours, int minutes, int seconds, int microseconds) + { + CalculateTicks(days, hours, minutes, seconds, microseconds); + } + + TimeSpan Add(const TimeSpan& ts) const + { + return TimeSpan(m_ticks + ts.m_ticks); + } + + TimeSpan Subtract(const TimeSpan& ts) const + { + return TimeSpan(m_ticks - ts.m_ticks); + } + + int Compare(const TimeSpan& ts) const + { + int ret = 0; + + if (m_ticks < ts.m_ticks) + { + ret = -1; + } + if (m_ticks > ts.m_ticks) + { + ret = 1; + } + return ret; + } + + bool Equals(const TimeSpan& ts) const + { + return m_ticks == ts.m_ticks; + } + + int Days() const + { + return static_cast(m_ticks / TicksPerDay); + } + + int Hours() const + { + return static_cast(m_ticks % TicksPerDay / TicksPerHour); + } + + int Minutes() const + { + return static_cast(m_ticks % TicksPerHour / TicksPerMinute); + } + + int Seconds() const + { + return static_cast(m_ticks % TicksPerMinute / TicksPerSecond); + } + + int Milliseconds() const + { + return static_cast(m_ticks % TicksPerSecond / TicksPerMillisecond); + } + + int Microseconds() const + { + return static_cast(m_ticks % TicksPerSecond / TicksPerMicrosecond); + } + + int64_t Ticks() const + { + return m_ticks; + } + + double TotalDays() const + { + return static_cast(m_ticks) / TicksPerDay; + } + + double TotalHours() const + { + return static_cast(m_ticks) / TicksPerHour; + } + + double TotalMinutes() const + { + return static_cast(m_ticks) / TicksPerMinute; + } + + double TotalSeconds() const + { + return static_cast(m_ticks) / TicksPerSecond; + } + + double TotalMilliseconds() const + { + return static_cast(m_ticks) / TicksPerMillisecond; + } + + double TotalMicroseconds() const + { + return static_cast(m_ticks) / TicksPerMicrosecond; + } + + std::string ToString() const + { + std::stringstream ss; + + ss << std::right << std::setfill('0'); + + if (m_ticks < 0) + { + ss << '-'; + } + + if (Days() != 0) + { + ss << std::setw(2) << std::abs(Days()) << '.'; + } + + ss << std::setw(2) << std::abs(Hours()) << ':'; + ss << std::setw(2) << std::abs(Minutes()) << ':'; + ss << std::setw(2) << std::abs(Seconds()); + + if (Microseconds() != 0) + { + ss << '.' << std::setw(6) << std::abs(Microseconds()); + } + + return ss.str(); + } + +private: + int64_t m_ticks; + + void CalculateTicks(int days, + int hours, + int minutes, + int seconds, + int microseconds) + { + m_ticks = days * TicksPerDay + + (hours * 3600LL + minutes * 60LL + seconds) * TicksPerSecond + + microseconds * TicksPerMicrosecond; + } +}; + +inline std::ostream& operator<<(std::ostream& strm, const TimeSpan& t) +{ + return strm << t.ToString(); +} + +inline TimeSpan operator+(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return ts1.Add(ts2); +} + +inline TimeSpan operator-(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return ts1.Subtract(ts2); +} + +inline bool operator==(const TimeSpan& ts1, TimeSpan& ts2) +{ + return ts1.Equals(ts2); +} + +inline bool operator>(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return (ts1.Compare(ts2) > 0); +} + +inline bool operator>=(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return (ts1.Compare(ts2) >= 0); +} + +inline bool operator!=(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return !ts1.Equals(ts2); +} + +inline bool operator<(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return (ts1.Compare(ts2) < 0); +} + +inline bool operator<=(const TimeSpan& ts1, const TimeSpan& ts2) +{ + return (ts1.Compare(ts2) <= 0); +} + +#endif diff --git a/Sources/sgp4-f5cb54b/include/Tle.h b/Sources/sgp4-f5cb54b/include/Tle.h new file mode 100644 index 0000000..60122bc --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/Tle.h @@ -0,0 +1,342 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef TLE_H_ +#define TLE_H_ + +#include "Util.h" +#include "DateTime.h" +#include "TleException.h" + +/** + * @brief Processes a two-line element set used to convey OrbitalElements. + * + * Used to extract the various raw fields from a two-line element set. + */ +class Tle +{ +public: + /** + * @details Initialise given the two lines of a tle + * @param[in] line_one Tle line one + * @param[in] line_two Tle line two + */ + Tle(const std::string& line_one, + const std::string& line_two) + : line_one_(line_one) + , line_two_(line_two) + { + Initialize(); + } + + /** + * @details Initialise given the satellite name and the two lines of a tle + * @param[in] name Satellite name + * @param[in] line_one Tle line one + * @param[in] line_two Tle line two + */ + Tle(const std::string& name, + const std::string& line_one, + const std::string& line_two) + : name_(name) + , line_one_(line_one) + , line_two_(line_two) + { + Initialize(); + } + + /** + * Copy constructor + * @param[in] tle Tle object to copy from + */ + Tle(const Tle& tle) + { + name_ = tle.name_; + line_one_ = tle.line_one_; + line_two_ = tle.line_two_; + + norad_number_ = tle.norad_number_; + int_designator_ = tle.int_designator_; + epoch_ = tle.epoch_; + mean_motion_dt2_ = tle.mean_motion_dt2_; + mean_motion_ddt6_ = tle.mean_motion_ddt6_; + bstar_ = tle.bstar_; + inclination_ = tle.inclination_; + right_ascending_node_ = tle.right_ascending_node_; + eccentricity_ = tle.eccentricity_; + argument_perigee_ = tle.argument_perigee_; + mean_anomaly_ = tle.mean_anomaly_; + mean_motion_ = tle.mean_motion_; + orbit_number_ = tle.orbit_number_; + } + + /** + * Get the satellite name + * @returns the satellite name + */ + std::string Name() const + { + return name_; + } + + /** + * Get the first line of the tle + * @returns the first line of the tle + */ + std::string Line1() const + { + return line_one_; + } + + /** + * Get the second line of the tle + * @returns the second line of the tle + */ + std::string Line2() const + { + return line_two_; + } + + /** + * Get the norad number + * @returns the norad number + */ + unsigned int NoradNumber() const + { + return norad_number_; + } + + /** + * Get the international designator + * @returns the international designator + */ + std::string IntDesignator() const + { + return int_designator_; + } + + /** + * Get the tle epoch + * @returns the tle epoch + */ + DateTime Epoch() const + { + return epoch_; + } + + /** + * Get the first time derivative of the mean motion divided by two + * @returns the first time derivative of the mean motion divided by two + */ + double MeanMotionDt2() const + { + return mean_motion_dt2_; + } + + /** + * Get the second time derivative of mean motion divided by six + * @returns the second time derivative of mean motion divided by six + */ + double MeanMotionDdt6() const + { + return mean_motion_ddt6_; + } + + /** + * Get the BSTAR drag term + * @returns the BSTAR drag term + */ + double BStar() const + { + return bstar_; + } + + /** + * Get the inclination + * @param in_degrees Whether to return the value in degrees or radians + * @returns the inclination + */ + double Inclination(bool in_degrees) const + { + if (in_degrees) + { + return inclination_; + } + else + { + return Util::DegreesToRadians(inclination_); + } + } + + /** + * Get the right ascension of the ascending node + * @param in_degrees Whether to return the value in degrees or radians + * @returns the right ascension of the ascending node + */ + double RightAscendingNode(const bool in_degrees) const + { + if (in_degrees) + { + return right_ascending_node_; + } + else + { + return Util::DegreesToRadians(right_ascending_node_); + } + } + + /** + * Get the eccentricity + * @returns the eccentricity + */ + double Eccentricity() const + { + return eccentricity_; + } + + /** + * Get the argument of perigee + * @param in_degrees Whether to return the value in degrees or radians + * @returns the argument of perigee + */ + double ArgumentPerigee(const bool in_degrees) const + { + if (in_degrees) + { + return argument_perigee_; + } + else + { + return Util::DegreesToRadians(argument_perigee_); + } + } + + /** + * Get the mean anomaly + * @param in_degrees Whether to return the value in degrees or radians + * @returns the mean anomaly + */ + double MeanAnomaly(const bool in_degrees) const + { + if (in_degrees) + { + return mean_anomaly_; + } + else + { + return Util::DegreesToRadians(mean_anomaly_); + } + } + + /** + * Get the mean motion + * @returns the mean motion (revolutions per day) + */ + double MeanMotion() const + { + return mean_motion_; + } + + /** + * Get the orbit number + * @returns the orbit number + */ + unsigned int OrbitNumber() const + { + return orbit_number_; + } + + /** + * Get the expected tle line length + * @returns the tle line length + */ + static unsigned int LineLength() + { + return TLE_LEN_LINE_DATA; + } + + /** + * Dump this object to a string + * @returns string + */ + std::string ToString() const + { + std::stringstream ss; + ss << std::right << std::fixed; + ss << "Norad Number: " << NoradNumber() << std::endl; + ss << "Int. Designator: " << IntDesignator() << std::endl; + ss << "Epoch: " << Epoch() << std::endl; + ss << "Orbit Number: " << OrbitNumber() << std::endl; + ss << std::setprecision(8); + ss << "Mean Motion Dt2: "; + ss << std::setw(12) << MeanMotionDt2() << std::endl; + ss << "Mean Motion Ddt6: "; + ss << std::setw(12) << MeanMotionDdt6() << std::endl; + ss << "Eccentricity: "; + ss << std::setw(12) << Eccentricity() << std::endl; + ss << "BStar: "; + ss << std::setw(12) << BStar() << std::endl; + ss << "Inclination: "; + ss << std::setw(12) << Inclination(true) << std::endl; + ss << "Right Ascending Node: "; + ss << std::setw(12) << RightAscendingNode(true) << std::endl; + ss << "Argument Perigee: "; + ss << std::setw(12) << ArgumentPerigee(true) << std::endl; + ss << "Mean Anomaly: "; + ss << std::setw(12) << MeanAnomaly(true) << std::endl; + ss << "Mean Motion: "; + ss << std::setw(12) << MeanMotion() << std::endl; + return ss.str(); + } + +private: + void Initialize(); + static bool IsValidLineLength(const std::string& str); + void ExtractInteger(const std::string& str, unsigned int& val); + void ExtractDouble(const std::string& str, int point_pos, double& val); + void ExtractExponential(const std::string& str, double& val); + +private: + std::string name_; + std::string line_one_; + std::string line_two_; + + std::string int_designator_; + DateTime epoch_; + double mean_motion_dt2_; + double mean_motion_ddt6_; + double bstar_; + double inclination_; + double right_ascending_node_; + double eccentricity_; + double argument_perigee_; + double mean_anomaly_; + double mean_motion_; + unsigned int norad_number_; + unsigned int orbit_number_; + + static const unsigned int TLE_LEN_LINE_DATA = 69; + static const unsigned int TLE_LEN_LINE_NAME = 22; +}; + + +inline std::ostream& operator<<(std::ostream& strm, const Tle& t) +{ + return strm << t.ToString(); +} + +#endif diff --git a/Sources/sgp4-f5cb54b/include/TleException.h b/Sources/sgp4-f5cb54b/include/TleException.h new file mode 100644 index 0000000..5393792 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/TleException.h @@ -0,0 +1,42 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef TLEEXCEPTION_H_ +#define TLEEXCEPTION_H_ + +#include +#include + +/** + * @brief The exception that the Tle class throws on an error. + * + * The exception that the Tle decoder will throw on an error. + */ +class TleException : public std::runtime_error +{ +public: + /** + * Constructor + * @param message Exception message + */ + TleException(const char* message) + : runtime_error(message) + { + } +}; + +#endif diff --git a/Sources/sgp4-f5cb54b/include/Util.h b/Sources/sgp4-f5cb54b/include/Util.h new file mode 100644 index 0000000..fd79bc6 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/Util.h @@ -0,0 +1,110 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef UTIL_H_ +#define UTIL_H_ + +#include "Globals.h" + +#include + +namespace Util +{ + template + + bool FromString(const std::string& str, T& val) + { + std::stringstream ss(str); + return !(ss >> val).fail(); + } + + /* + * always positive result + * Mod(-3,4)= 1 fmod(-3,4)= -3 + */ + inline double Mod(const double x, const double y) + { + if (y == 0.0) + { + return x; + } + + return x - y * floor(x / y); + } + + inline double WrapNegPosPI(const double a) + { + return Mod(a + kPI, kTWOPI) - kPI; + } + + inline double WrapTwoPI(const double a) + { + return Mod(a, kTWOPI); + } + + inline double WrapNegPos180(const double a) + { + return Mod(a + 180.0, 360.0) - 180.0; + } + + inline double Wrap360(const double a) + { + return Mod(a, 360.0); + } + + inline double DegreesToRadians(const double degrees) + { + return degrees * kPI / 180.0; + } + + inline double RadiansToDegrees(const double radians) + { + return radians * 180.0 / kPI; + } + + inline double AcTan(const double sinx, const double cosx) + { + if (cosx == 0.0) + { + if (sinx > 0.0) + { + return kPI / 2.0; + } + else + { + return 3.0 * kPI / 2.0; + } + } + else + { + if (cosx > 0.0) + { + return atan(sinx / cosx); + } + else + { + return kPI + atan(sinx / cosx); + } + } + } + + void TrimLeft(std::string& s); + void TrimRight(std::string& s); + void Trim(std::string& s); +} + +#endif diff --git a/Sources/sgp4-f5cb54b/include/Vector.h b/Sources/sgp4-f5cb54b/include/Vector.h new file mode 100644 index 0000000..111c5a7 --- /dev/null +++ b/Sources/sgp4-f5cb54b/include/Vector.h @@ -0,0 +1,161 @@ +/* + * Copyright 2013 Daniel Warner + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef VECTOR_H_ +#define VECTOR_H_ + +#include +#include +#include +#include + +/** + * @brief Generic vector + * + * Stores x, y, z, w + */ +struct Vector +{ +public: + + /** + * Default constructor + */ + Vector() + : x(0.0), y(0.0), z(0.0), w(0.0) + { + } + + /** + * Constructor + * @param arg_x x value + * @param arg_y y value + * @param arg_z z value + */ + Vector(const double arg_x, + const double arg_y, + const double arg_z) + : x(arg_x), y(arg_y), z(arg_z), w(0.0) + { + } + + /** + * Constructor + * @param arg_x x value + * @param arg_y y value + * @param arg_z z value + * @param arg_w w value + */ + Vector(const double arg_x, + const double arg_y, + const double arg_z, + const double arg_w) + : x(arg_x), y(arg_y), z(arg_z), w(arg_w) + { + } + + /** + * Copy constructor + * @param v value to copy from + */ + Vector(const Vector& v) + { + x = v.x; + y = v.y; + z = v.z; + w = v.w; + } + + /** + * Assignment operator + * @param v value to copy from + */ + Vector& operator=(const Vector& v) + { + if (this != &v) + { + x = v.x; + y = v.y; + z = v.z; + w = v.w; + } + return *this; + } + + /** + * Subtract operator + * @param v value to suctract from + */ + Vector operator-(const Vector& v) + { + return Vector(x - v.x, + y - v.y, + z - v.z, + 0.0); + } + + /** + * Calculates the magnitude of the vector + * @returns magnitude of the vector + */ + double Magnitude() const + { + return sqrt(x * x + y * y + z * z); + } + + /** + * Calculates the dot product + * @returns dot product + */ + double Dot(const Vector& vec) const + { + return (x * vec.x) + + (y * vec.y) + + (z * vec.z); + } + + /** + * Converts this vector to a string + * @returns this vector as a string + */ + std::string ToString() const + { + std::stringstream ss; + ss << std::right << std::fixed << std::setprecision(3); + ss << "X: " << std::setw(9) << x; + ss << ", Y: " << std::setw(9) << y; + ss << ", Z: " << std::setw(9) << z; + ss << ", W: " << std::setw(9) << w; + return ss.str(); + } + + /** x value */ + double x; + /** y value */ + double y; + /** z value */ + double z; + /** w value */ + double w; +}; + +inline std::ostream& operator<<(std::ostream& strm, const Vector& v) +{ + return strm << v.ToString(); +} + +#endif diff --git a/Tests/SGPKitTests/SPGKitTests.swift b/Tests/SGPKitTests/SPGKitTests.swift new file mode 100644 index 0000000..c23ab36 --- /dev/null +++ b/Tests/SGPKitTests/SPGKitTests.swift @@ -0,0 +1,56 @@ +import XCTest +@testable import SGPKit + +private enum Error: Swift.Error { + case invalidDate + case invalidTimezone +} + +final class SGPKitTests: XCTestCase { + func testGeoPosition() throws { + let firstLine = "1 25544U 98067A 13165.59097222 .00004759 00000-0 88814-4 0 47" + let secondLine = "2 25544 51.6478 121.2152 0011003 68.5125 263.9959 15.50783143834295" + let tle = TLE(firstLine: firstLine, secondLine: secondLine) + let interpreter = TLEInterpreter() + let data = interpreter.satelliteData(from: tle, date: try generateTestDate()) + + let expectedLatitude = 45.2893067 + let expectedLongitude = -136.62764 + let expectedAltitude = 411.5672031 + + let latitudeDiff = fabs(data.latitude - expectedLatitude) + let longitudeDiff = fabs(data.longitude - expectedLongitude) + let altitudeDiff = fabs(data.altitude - expectedAltitude) + + XCTAssertTrue(latitudeDiff <= 0.000001) + XCTAssertTrue(longitudeDiff <= 0.000001) + XCTAssertTrue(altitudeDiff <= 0.000001) + } + + private func generateTestDate() throws -> Date { + var calendar = Calendar.current + + guard let utcTimezone = TimeZone(secondsFromGMT: 0) else { + throw Error.invalidTimezone + } + + calendar.timeZone = utcTimezone + + var components = DateComponents() + + components.year = 2013 + components.month = 6 + components.day = 15 + components.hour = 2 + components.minute = 57 + components.second = 7 + components.nanosecond = 200000 * 1000 + + guard let date = calendar.date(from: components) else { + throw Error.invalidDate + } + + return date + } +} +