10 using namespace boost::numeric::ublas;
19 SurfacePoint::SurfacePoint() {
30 if(other.p_majorAxis) {
31 p_majorAxis =
new Distance(*other.p_majorAxis);
37 if(other.p_minorAxis) {
38 p_minorAxis =
new Distance(*other.p_minorAxis);
44 if(other.p_polarAxis) {
45 p_polarAxis =
new Distance(*other.p_polarAxis);
73 p_rectCovar =
new symmetric_matrix<double, upper>(*other.
p_rectCovar);
80 p_sphereCovar =
new symmetric_matrix<double, upper>(*other.
p_sphereCovar);
100 SetSphericalPoint(lat, lon, radius);
125 SetSpherical(lat, lon, radius, latSigma, lonSigma, radiusSigma);
135 const Distance &radius,
const symmetric_matrix<double, upper> &covar) {
139 SetSpherical(lat, lon, radius, covar);
155 SetRectangular(x, y, z);
179 SetRectangular(x, y, z, xSigma, ySigma, zSigma);
193 const Displacement &z,
const symmetric_matrix<double, upper> &covar) {
197 SetRectangular(x, y, z, covar);
205 SurfacePoint::~SurfacePoint() {
206 FreeAllocatedMemory();
214 void SurfacePoint::InitCovariance() {
216 p_sphereCovar = NULL;
224 void SurfacePoint::InitPoint() {
234 void SurfacePoint::InitRadii() {
257 IString msg =
"x, y, and z must be set to valid displacements. One or "
258 "more coordinates have been set to an invalid displacement.";
302 SetRectangularPoint(x, y, z);
305 SetRectangularSigmas(xSigma, ySigma, zSigma);
321 const symmetric_matrix<double,upper>& covar) {
322 SetRectangularPoint(x, y, z);
323 SetRectangularMatrix(covar);
335 void SurfacePoint::SetRectangularSigmas(
const Distance &xSigma,
340 IString msg =
"x sigma, y sigma , and z sigma must be set to valid "
341 "distances. One or more sigmas have been set to an invalid distance.";
345 symmetric_matrix<double,upper> covar(3);
350 SetRectangularMatrix(covar);
361 void SurfacePoint::SetRectangularMatrix(
362 const symmetric_matrix<double, upper> &covar) {
365 IString msg =
"A point must be set before a variance/covariance matrix "
371 *p_rectCovar = covar;
374 p_rectCovar =
new symmetric_matrix<double, upper>(covar);
377 SpiceDouble rectMat[3][3];
380 double x2 = p_x->meters() * p_x->meters();
381 double y2 = p_y->meters() * p_y->meters();
382 double z = p_z->meters();
383 double radius = sqrt(x2 + y2 + z*z);
386 rectMat[0][0] = covar(0,0);
387 rectMat[0][1] = rectMat[1][0] = covar(0,1);
388 rectMat[0][2] = rectMat[2][0] = covar(0,2);
389 rectMat[1][1] = covar(1,1);
390 rectMat[1][2] = rectMat[2][1] = covar(1,2);
391 rectMat[2][2] = covar(2,2);
395 double zOverR = p_z->meters() / radius;
396 double r2 = radius*radius;
397 double denom = r2*radius*sqrt(1.0 - (zOverR*zOverR));
398 J[0][0] = -p_x->meters() * p_z->meters() / denom;
399 J[0][1] = -p_y->meters() * p_z->meters() / denom;
400 J[0][2] = (r2 - p_z->meters() * p_z->meters()) / denom;
401 J[1][0] = -p_y->meters() / (x2 + y2);
402 J[1][1] = p_x->meters() / (x2 + y2);
404 J[2][0] = p_x->meters() / radius;
405 J[2][1] = p_y->meters() / radius;
406 J[2][2] = p_z->meters() / radius;
409 p_sphereCovar =
new symmetric_matrix<double, upper>(3);
411 SpiceDouble mat[3][3];
412 mxm_c (J, rectMat, mat);
413 mxmt_c (mat, J, mat);
414 (*p_sphereCovar)(0,0) = mat[0][0];
415 (*p_sphereCovar)(0,1) = mat[0][1];
416 (*p_sphereCovar)(0,2) = mat[0][2];
417 (*p_sphereCovar)(1,1) = mat[1][1];
418 (*p_sphereCovar)(1,2) = mat[1][2];
419 (*p_sphereCovar)(2,2) = mat[2][2];
434 void SurfacePoint::SetSphericalPoint(
const Latitude &lat,
439 IString msg =
"Latitude, longitude, or radius is an invalid value.";
443 SpiceDouble dlat = (double) lat.
radians();
444 SpiceDouble dlon = (double) lon.
radians();
448 latrec_c ( dradius, dlon, dlat, rect);
450 SetRectangularPoint(
Displacement(rect[0], Displacement::Kilometers),
472 SetSphericalPoint(lat, lon, radius);
475 SetSphericalSigmas(latSigma, lonSigma, radiusSigma);
489 const Distance &radius,
const symmetric_matrix<double, upper> &covar) {
490 SetSphericalPoint(lat, lon, radius);
491 SetSphericalMatrix(covar);
503 void SurfacePoint::SetSphericalCoordinates(
const Latitude &lat,
506 SetSphericalPoint(lat, lon, radius);
517 void SurfacePoint::SetSphericalSigmas(
const Angle &latSigma,
518 const Angle &lonSigma,
521 symmetric_matrix<double,upper> covar(3);
524 double sphericalCoordinate;
525 sphericalCoordinate = (double) latSigma.
radians();
526 covar(0,0) = sphericalCoordinate*sphericalCoordinate;
527 sphericalCoordinate = (double) lonSigma.
radians();
528 covar(1,1) = sphericalCoordinate*sphericalCoordinate;
529 sphericalCoordinate = (double) radiusSigma.
meters();
530 covar(2,2) = sphericalCoordinate*sphericalCoordinate;
532 SetSphericalMatrix(covar);
535 delete p_sphereCovar;
536 p_sphereCovar = NULL;
555 void SurfacePoint::SetSphericalSigmasDistance(
const Distance &latSigma,
558 if (!p_majorAxis || !p_minorAxis || !p_polarAxis || !p_majorAxis->isValid() ||
559 !p_minorAxis->isValid() || !p_polarAxis->isValid()) {
560 IString msg =
"In order to use sigmas in meter units, the equitorial "
561 "radius must be set with a call to SetRadii or an appropriate "
567 IString msg =
"Cannot set spherical sigmas on an invalid surface point";
571 double scaledLatSig = latSigma / *p_majorAxis;
572 double scaledLonSig = lonSigma * cos((
double)GetLatitude().radians())
574 SetSphericalSigmas(
Angle(scaledLatSig, Angle::Radians),
575 Angle(scaledLonSig, Angle::Radians), radiusSigma);
586 void SurfacePoint::SetSphericalMatrix(
587 const symmetric_matrix<double, upper> & covar) {
591 IString msg =
"A point must be set before a variance/covariance matrix "
597 *p_sphereCovar = covar;
600 p_sphereCovar =
new symmetric_matrix<double, upper>(covar);
603 SpiceDouble sphereMat[3][3];
605 sphereMat[0][0] = covar(0,0);
606 sphereMat[0][1] = sphereMat[1][0] = covar(0,1);
607 sphereMat[0][2] = sphereMat[2][0] = covar(0,2);
608 sphereMat[1][1] = covar(1,1);
609 sphereMat[1][2] = sphereMat[2][1] = covar(1,2);
610 sphereMat[2][2] = covar(2,2);
617 double lat = (double) GetLatitude().radians();
618 double lon = (double) GetLongitude().radians();
619 double radius = (double) GetLocalRadius().meters();
623 double cosPhi = cos(lat);
624 double sinPhi = sin(lat);
625 double cosLamda = cos(lon);
626 double sinLamda = sin(lon);
627 double rcosPhi = radius*cosPhi;
628 double rsinPhi = radius*sinPhi;
629 J[0][0] = -rsinPhi * cosLamda;
630 J[0][1] = -rcosPhi * sinLamda;
631 J[0][2] = cosPhi * cosLamda;
632 J[1][0] = -rsinPhi * sinLamda;
633 J[1][1] = rcosPhi * cosLamda;
634 J[1][2] = cosPhi * sinLamda;
640 p_rectCovar =
new symmetric_matrix<double, upper>(3);
642 SpiceDouble mat[3][3];
643 mxm_c (J, sphereMat, mat);
644 mxmt_c (mat, J, mat);
646 (*p_rectCovar)(0,0) = mat[0][0];
647 (*p_rectCovar)(0,1) = mat[0][1];
648 (*p_rectCovar)(0,2) = mat[0][2];
649 (*p_rectCovar)(1,1) = mat[1][1];
650 (*p_rectCovar)(1,2) = mat[1][2];
651 (*p_rectCovar)(2,2) = mat[2][2];
668 void SurfacePoint::ToNaifArray(
double naifOutput[3])
const {
670 naifOutput[0] = p_x->kilometers();
671 naifOutput[1] = p_y->kilometers();
672 naifOutput[2] = p_z->kilometers();
675 IString msg =
"Cannot convert an invalid surface point to a naif array";
689 void SurfacePoint::FromNaifArray(
const double naifValues[3]) {
690 if(p_x && p_y && p_z) {
691 p_x->setKilometers(naifValues[0]);
692 p_y->setKilometers(naifValues[1]);
693 p_z->setKilometers(naifValues[2]);
696 p_x =
new Displacement(naifValues[0], Displacement::Kilometers);
697 p_y =
new Displacement(naifValues[1], Displacement::Kilometers);
698 p_z =
new Displacement(naifValues[2], Displacement::Kilometers);
710 void SurfacePoint::SetRadii(
const Distance &majorRadius,
717 IString msg =
"Radii must be set to valid distances. One or more radii "
718 "have been set to an invalid distance.";
723 *p_majorAxis = majorRadius;
726 p_majorAxis =
new Distance(majorRadius);
730 *p_minorAxis = minorRadius;
733 p_minorAxis =
new Distance(minorRadius);
737 *p_polarAxis = polarRadius;
740 p_polarAxis =
new Distance(polarRadius);
751 void SurfacePoint::ResetLocalRadius(
const Distance &radius) {
754 IString msg =
"Radius value must be a valid Distance.";
758 if (!p_x || !p_y || !p_z || !p_x->isValid() || !p_y->isValid() ||
760 IString msg =
"In order to reset the local radius, a Surface Point must "
765 SpiceDouble lat = (double) GetLatitude().radians();
766 SpiceDouble lon = (double) GetLongitude().radians();
768 latrec_c ((SpiceDouble) radius.
kilometers(), lon, lat, rect);
769 p_x->setKilometers(rect[0]);
770 p_y->setKilometers(rect[1]);
771 p_z->setKilometers(rect[2]);
779 bool SurfacePoint::Valid()
const {
780 static const Displacement zero(0, Displacement::Meters);
781 return p_x && p_y && p_z && p_x->isValid() && p_y->isValid() && p_z->isValid() &&
782 (*p_x != zero || *p_y != zero || *p_z != zero);
786 Displacement SurfacePoint::GetX()
const {
787 if(!p_x)
return Displacement();
793 Displacement SurfacePoint::GetY()
const {
794 if(!p_y)
return Displacement();
800 Displacement SurfacePoint::GetZ()
const {
801 if(!p_z)
return Displacement();
807 Distance SurfacePoint::GetXSigma()
const {
808 if(!p_rectCovar)
return Distance();
810 return Distance(sqrt((*p_rectCovar)(0, 0)), Distance::Meters);
814 Distance SurfacePoint::GetYSigma()
const {
815 if(!p_rectCovar)
return Distance();
817 return Distance(sqrt((*p_rectCovar)(1, 1)), Distance::Meters);
821 Distance SurfacePoint::GetZSigma()
const {
822 if(!p_rectCovar)
return Distance();
824 return Distance(sqrt((*p_rectCovar)(2, 2)), Distance::Meters);
828 symmetric_matrix<double, upper> SurfacePoint::GetRectangularMatrix()
831 symmetric_matrix<double, upper> tmp(3);
840 Angle SurfacePoint::GetLatSigma()
const {
844 return Angle(sqrt((*p_sphereCovar)(0, 0)), Angle::Radians);
848 Angle SurfacePoint::GetLonSigma()
const {
852 return Angle(sqrt((*p_sphereCovar)(1, 1)), Angle::Radians);
865 double x = p_x->meters();
866 double y = p_y->meters();
867 double z = p_z->meters();
869 if (x != 0. || y != 0. || z != 0.)
870 return Latitude(atan2(z, sqrt(x*x + y*y) ), Angle::Radians);
884 double x = p_x->meters();
885 double y = p_y->meters();
887 if(x == 0.0 && y == 0.0) {
891 double lon = atan2(y, x);
908 double x = p_x->meters();
909 double y = p_y->meters();
910 double z = p_z->meters();
912 return Distance(sqrt(x*x + y*y + z*z), Distance::Meters);
920 Distance SurfacePoint::GetLatSigmaDistance()
const {
924 Angle latSigma = GetLatSigma();
927 if (!p_majorAxis || !p_majorAxis->isValid()) {
928 IString msg =
"In order to calculate sigmas in meter units, the "
929 "equitorial radius must be set with a call to SetRadii.";
934 latSigmaDistance = latSigma.
radians() * *p_majorAxis;
938 return latSigmaDistance;
946 Distance SurfacePoint::GetLonSigmaDistance()
const {
950 Angle lonSigma = GetLonSigma();
953 if (!p_majorAxis || !p_majorAxis->isValid()) {
954 IString msg =
"In order to calculate sigmas in meter units, the "
955 "equitorial radius must be set with a call to SetRadii.";
960 double scaler = cos(lat.
radians());
964 lonSigmaDistance = lonSigma.
radians() * *p_majorAxis / scaler;
968 return lonSigmaDistance;
972 Distance SurfacePoint::GetLocalRadiusSigma()
const {
976 return Distance(sqrt((*p_sphereCovar)(2, 2)), Distance::Meters);
980 symmetric_matrix<double, upper> SurfacePoint::GetSphericalMatrix()
const {
982 symmetric_matrix<double, upper> tmp(3);
987 return *p_sphereCovar;
996 double SurfacePoint::GetLatWeight()
const {
997 double dlatSigma = GetLatSigma().radians();
999 if( dlatSigma <= 0.0 ) {
1000 IString msg =
"SurfacePoint::GetLatWeight(): Sigma <= 0.0";
1004 return 1.0/(dlatSigma*dlatSigma);
1012 double SurfacePoint::GetLonWeight()
const {
1013 double dlonSigma = GetLonSigma().radians();
1015 if( dlonSigma <= 0.0 ) {
1016 IString msg =
"SurfacePoint::GetLonWeight(): Sigma <= 0.0";
1020 return 1.0/(dlonSigma*dlonSigma);
1028 double SurfacePoint::GetLocalRadiusWeight()
const {
1030 double dlocalRadiusSigma = GetLocalRadiusSigma().kilometers();
1032 if (dlocalRadiusSigma <= 0.0 ) {
1033 IString msg =
"SurfacePoint::GetRadWeight(): Sigma <= 0.0";
1037 return 1.0/(dlocalRadiusSigma*dlocalRadiusSigma);
1047 if(p_majorAxis || p_minorAxis || p_polarAxis ||
1048 other.p_majorAxis || other.p_minorAxis || other.p_polarAxis) {
1049 IString msg =
"SurfacePoint::GetDistanceToPoint not yet implemented for "
1054 if(!Valid() || !other.Valid())
1057 return GetDistanceToPoint(other,
1070 const Distance &sphereRadius)
const {
1071 if(!Valid() || !other.Valid())
1075 const Angle &latitude = GetLatitude();
1076 const Angle &longitude = GetLongitude();
1082 Angle deltaLat = latitude - otherLatitude;
1083 Angle deltaLon = longitude - otherLongitude;
1085 double haversinLat = sin(deltaLat.
radians() / 2.0);
1086 haversinLat *= haversinLat;
1088 double haversinLon = sin(deltaLon.
radians() / 2.0);
1089 haversinLon *= haversinLon;
1091 double a = haversinLat + cos(latitude.
radians()) *
1092 cos(otherLatitude.radians()) *
1095 double c = 2 * atan(sqrt(a) / sqrt(1 - a));
1097 return sphereRadius * c;
1101 bool SurfacePoint::operator==(
const SurfacePoint &other)
const {
1104 if(equal && p_x && p_y && p_z && other.p_x && other.p_y && other.p_z) {
1105 equal = equal && *p_x == *other.p_x;
1106 equal = equal && *p_y == *other.p_y;
1107 equal = equal && *p_z == *other.p_z;
1110 equal = equal && p_x == NULL && other.p_x == NULL;
1111 equal = equal && p_y == NULL && other.p_y == NULL;
1112 equal = equal && p_z == NULL && other.p_z == NULL;
1115 if(equal && p_majorAxis && p_minorAxis && p_polarAxis) {
1116 equal = equal && *p_majorAxis == *other.p_majorAxis;
1117 equal = equal && *p_minorAxis == *other.p_minorAxis;
1118 equal = equal && *p_polarAxis == *other.p_polarAxis;
1121 equal = equal && p_majorAxis == NULL && other.p_majorAxis == NULL;
1122 equal = equal && p_minorAxis == NULL && other.p_minorAxis == NULL;
1123 equal = equal && p_polarAxis == NULL && other.p_polarAxis == NULL;
1126 if(equal && p_rectCovar) {
1127 equal = equal && (*p_rectCovar)(0, 0) == (*other.
p_rectCovar)(0, 0);
1128 equal = equal && (*p_rectCovar)(0, 1) == (*other.
p_rectCovar)(0, 1);
1129 equal = equal && (*p_rectCovar)(0, 2) == (*other.
p_rectCovar)(0, 2);
1130 equal = equal && (*p_rectCovar)(1, 1) == (*other.
p_rectCovar)(1, 1);
1131 equal = equal && (*p_rectCovar)(1, 2) == (*other.
p_rectCovar)(1, 2);
1132 equal = equal && (*p_rectCovar)(2, 2) == (*other.
p_rectCovar)(2, 2);
1135 equal = equal && p_rectCovar == NULL && other.
p_rectCovar == NULL;
1138 if(equal && p_sphereCovar) {
1139 equal = equal && (*p_sphereCovar)(0, 0) == (*other.
p_sphereCovar)(0, 0);
1140 equal = equal && (*p_sphereCovar)(0, 1) == (*other.
p_sphereCovar)(0, 1);
1141 equal = equal && (*p_sphereCovar)(0, 2) == (*other.
p_sphereCovar)(0, 2);
1142 equal = equal && (*p_sphereCovar)(1, 1) == (*other.
p_sphereCovar)(1, 1);
1143 equal = equal && (*p_sphereCovar)(1, 2) == (*other.
p_sphereCovar)(1, 2);
1144 equal = equal && (*p_sphereCovar)(2, 2) == (*other.
p_sphereCovar)(2, 2);
1147 equal = equal && p_sphereCovar == NULL && other.
p_sphereCovar == NULL;
1153 SurfacePoint &SurfacePoint::operator=(
const SurfacePoint &other) {
1156 if(p_x && other.p_x &&
1159 !p_majorAxis && !other.p_majorAxis &&
1160 !p_minorAxis && !other.p_minorAxis &&
1161 !p_polarAxis && !other.p_polarAxis &&
1162 !p_rectCovar && !other.p_rectCovar &&
1163 !p_sphereCovar && !other.p_sphereCovar) {
1169 FreeAllocatedMemory();
1170 if(other.p_majorAxis) {
1171 p_majorAxis =
new Distance(*other.p_majorAxis);
1174 if(other.p_minorAxis) {
1175 p_minorAxis =
new Distance(*other.p_minorAxis);
1178 if(other.p_polarAxis) {
1179 p_polarAxis =
new Distance(*other.p_polarAxis);
1183 p_x =
new Displacement(*other.p_x);
1187 p_y =
new Displacement(*other.p_y);
1191 p_z =
new Displacement(*other.p_z);
1194 if(other.p_rectCovar) {
1195 p_rectCovar =
new symmetric_matrix<double, upper>(*other.p_rectCovar);
1198 if(other.p_sphereCovar) {
1199 p_sphereCovar =
new symmetric_matrix<double, upper>(*other.p_sphereCovar);
1206 void SurfacePoint::FreeAllocatedMemory() {
1243 delete p_sphereCovar;
1244 p_sphereCovar = NULL;