| 74 | |
| 75 | |
| 76 | Xyz georeferenceWgs84(double range, double scanAngle, |
| 77 | const RotationMatrix& boresightMatrix, |
| 78 | const RotationMatrix& imuMatrix, const Xyz& gpsPoint) |
| 79 | { |
| 80 | Xyz pSocs = Xyz(range * std::sin(scanAngle), 0, -range * std::cos(scanAngle)); |
| 81 | |
| 82 | Xyz pSocsAligned = rotate(pSocs, boresightMatrix); |
| 83 | Xyz pLocalLevel = rotate(pSocsAligned, imuMatrix); |
| 84 | Xyz pCurvilinear = cartesianToCurvilinear(pLocalLevel, gpsPoint.Y); |
| 85 | |
| 86 | return Xyz(gpsPoint.X + pCurvilinear.X, gpsPoint.Y + pCurvilinear.Y, |
| 87 | gpsPoint.Z + pCurvilinear.Z); |
| 88 | } |
| 89 | } |
| 90 | } |