double Ellipsoid::greatCircleDistance(const Geodetic2& p1, const Geodetic2& p2) const{
// https://en.wikipedia.org/wiki/Meridian_arc
// https://en.wikipedia.org/wiki/Great-circle_distance#Vector_version
glm::dvec3 n1 = geodeticSurfaceNormal(p1);
glm::dvec3 n2 = geodeticSurfaceNormal(p2);
double centralAngle = glm::atan(glm::length(glm::cross(n1, n2)) / glm::dot(n1, n2));
Geodetic2 pMid = (p1 + p2) / 2;
glm::dvec3 centralNormal = cartesianSurfacePosition(pMid);
return centralAngle * glm::length(centralNormal);
}
Geodetic2D SphericalPlanet::toGeodetic2D(const Vector3D& position) const {
const Vector3D n = geodeticSurfaceNormal(position);
const IMathUtils* mu = IMathUtils::instance();
return Geodetic2D(Angle::fromRadians(mu->asin(n._z)),
Angle::fromRadians(mu->atan2(n._y, n._x)));
}
glm::dvec3 Ellipsoid::cartesianPosition(const Geodetic3& geodetic3) const
{
glm::dvec3 normal = geodeticSurfaceNormal(geodetic3.geodetic2);
glm::dvec3 k = _cached._radiiSquared * normal;
double gamma = sqrt(dot(k, normal));
glm::dvec3 rSurface = k / gamma;
return rSurface + geodetic3.height * normal;
}
Vector3D SphericalPlanet::toCartesian(const Angle& latitude,
const Angle& longitude,
const double height) const {
return geodeticSurfaceNormal(latitude, longitude).times( _sphere._radius + height);
}
MutableMatrix44D SphericalPlanet::doubleDrag(const Vector3D& finalRay0,
const Vector3D& finalRay1) const
{
// test if initialPoints are valid
if (_initialPoint0.isNan() || _initialPoint1.isNan())
return MutableMatrix44D::invalid();
// init params
const IMathUtils* mu = IMathUtils::instance();
MutableVector3D positionCamera = _origin;
const double finalRaysAngle = finalRay0.angleBetween(finalRay1)._degrees;
const double factor = finalRaysAngle / _angleBetweenInitialRays;
double dAccum=0, angle0, angle1;
double distance = _origin.sub(_centerPoint).length();
// compute estimated camera translation: step 0
double d = distance*(factor-1)/factor;
MutableMatrix44D translation = MutableMatrix44D::createTranslationMatrix(_centerRay.asVector3D().normalized().times(d));
positionCamera = positionCamera.transformedBy(translation, 1.0);
dAccum += d;
{
const Vector3D point0 = closestIntersection(positionCamera.asVector3D(), finalRay0);
const Vector3D point1 = closestIntersection(positionCamera.asVector3D(), finalRay1);
angle0 = point0.angleBetween(point1)._degrees;
if (ISNAN(angle0)) return MutableMatrix44D::invalid();
}
// compute estimated camera translation: step 1
d = mu->abs((distance-d)*0.3);
if (angle0 < _angleBetweenInitialPoints) d*=-1;
translation = MutableMatrix44D::createTranslationMatrix(_centerRay.asVector3D().normalized().times(d));
positionCamera = positionCamera.transformedBy(translation, 1.0);
dAccum += d;
{
const Vector3D point0 = closestIntersection(positionCamera.asVector3D(), finalRay0);
const Vector3D point1 = closestIntersection(positionCamera.asVector3D(), finalRay1);
angle1 = point0.angleBetween(point1)._degrees;
if (ISNAN(angle1)) return MutableMatrix44D::invalid();
}
// compute estimated camera translation: steps 2..n until convergence
//int iter=0;
double precision = mu->pow(10, mu->log10(distance)-8.0);
double angle_n1=angle0, angle_n=angle1;
while (mu->abs(angle_n-_angleBetweenInitialPoints) > precision) {
// iter++;
if ((angle_n1-angle_n)/(angle_n-_angleBetweenInitialPoints) < 0) d*=-0.5;
translation = MutableMatrix44D::createTranslationMatrix(_centerRay.asVector3D().normalized().times(d));
positionCamera = positionCamera.transformedBy(translation, 1.0);
dAccum += d;
angle_n1 = angle_n;
{
const Vector3D point0 = closestIntersection(positionCamera.asVector3D(), finalRay0);
const Vector3D point1 = closestIntersection(positionCamera.asVector3D(), finalRay1);
angle_n = point0.angleBetween(point1)._degrees;
if (ISNAN(angle_n)) return MutableMatrix44D::invalid();
}
}
//if (iter>2) printf("----------- iteraciones=%d precision=%f angulo final=%.4f distancia final=%.1f\n", iter, precision, angle_n, dAccum);
// start to compound matrix
MutableMatrix44D matrix = MutableMatrix44D::identity();
positionCamera = _origin;
MutableVector3D viewDirection = _centerRay;
MutableVector3D ray0 = finalRay0.asMutableVector3D();
MutableVector3D ray1 = finalRay1.asMutableVector3D();
// drag from initialPoint to centerPoint
{
Vector3D initialPoint = _initialPoint.asVector3D();
const Vector3D rotationAxis = initialPoint.cross(_centerPoint.asVector3D());
const Angle rotationDelta = Angle::fromRadians( - mu->acos(_initialPoint.normalized().dot(_centerPoint.normalized())) );
if (rotationDelta.isNan()) return MutableMatrix44D::invalid();
MutableMatrix44D rotation = MutableMatrix44D::createRotationMatrix(rotationDelta, rotationAxis);
positionCamera = positionCamera.transformedBy(rotation, 1.0);
viewDirection = viewDirection.transformedBy(rotation, 0.0);
ray0 = ray0.transformedBy(rotation, 0.0);
ray1 = ray1.transformedBy(rotation, 0.0);
matrix = rotation.multiply(matrix);
}
// move the camera forward
{
MutableMatrix44D translation2 = MutableMatrix44D::createTranslationMatrix(viewDirection.asVector3D().normalized().times(dAccum));
positionCamera = positionCamera.transformedBy(translation2, 1.0);
matrix = translation2.multiply(matrix);
}
// compute 3D point of view center
Vector3D centerPoint2 = closestIntersection(positionCamera.asVector3D(), viewDirection.asVector3D());
// compute middle point in 3D
Vector3D P0 = closestIntersection(positionCamera.asVector3D(), ray0.asVector3D());
Vector3D P1 = closestIntersection(positionCamera.asVector3D(), ray1.asVector3D());
Geodetic2D g = getMidPoint(toGeodetic2D(P0), toGeodetic2D(P1));
Vector3D finalPoint = toCartesian(g);
// drag globe from centerPoint to finalPoint
{
const Vector3D rotationAxis = centerPoint2.cross(finalPoint);
const Angle rotationDelta = Angle::fromRadians( - mu->acos(centerPoint2.normalized().dot(finalPoint.normalized())) );
if (rotationDelta.isNan()) return MutableMatrix44D::invalid();
MutableMatrix44D rotation = MutableMatrix44D::createRotationMatrix(rotationDelta, rotationAxis);
positionCamera = positionCamera.transformedBy(rotation, 1.0);
viewDirection = viewDirection.transformedBy(rotation, 0.0);
ray0 = ray0.transformedBy(rotation, 0.0);
ray1 = ray1.transformedBy(rotation, 0.0);
matrix = rotation.multiply(matrix);
}
// camera rotation
{
Vector3D normal = geodeticSurfaceNormal(centerPoint2);
Vector3D v0 = _initialPoint0.asVector3D().sub(centerPoint2).projectionInPlane(normal);
Vector3D p0 = closestIntersection(positionCamera.asVector3D(), ray0.asVector3D());
Vector3D v1 = p0.sub(centerPoint2).projectionInPlane(normal);
double angle = v0.angleBetween(v1)._degrees;
double sign = v1.cross(v0).dot(normal);
if (sign<0) angle = -angle;
MutableMatrix44D rotation = MutableMatrix44D::createGeneralRotationMatrix(Angle::fromDegrees(angle), normal, centerPoint2);
matrix = rotation.multiply(matrix);
}
return matrix;
}
Vector3D SphericalPlanet::scaleToGeodeticSurface(const Vector3D& position) const {
return geodeticSurfaceNormal(position).times( _sphere._radius );
}
