MutableMatrix44D SphericalPlanet::singleDrag(const Vector3D& finalRay) const
{
// test if initialPoint is valid
if (_initialPoint.isNan()) return MutableMatrix44D::invalid();
// compute final point
const Vector3D origin = _origin.asVector3D();
MutableVector3D finalPoint = closestIntersection(origin, finalRay).asMutableVector3D();
if (finalPoint.isNan()) {
//printf ("--invalid final point in drag!!\n");
finalPoint = closestPointToSphere(origin, finalRay).asMutableVector3D();
}
// compute the rotation axis
const Vector3D rotationAxis = _initialPoint.cross(finalPoint).asVector3D();
// compute the angle
double sinus = rotationAxis.length()/_initialPoint.length()/finalPoint.length();
const Angle rotationDelta = Angle::fromRadians(-IMathUtils::instance()->asin(sinus));
if (rotationDelta.isNan()) return MutableMatrix44D::invalid();
// save params for possible inertial animations
_lastDragAxis = rotationAxis.asMutableVector3D();
double radians = rotationDelta._radians;
_lastDragRadiansStep = radians - _lastDragRadians;
_lastDragRadians = radians;
_validSingleDrag = true;
// return rotation matrix
return MutableMatrix44D::createRotationMatrix(rotationDelta, rotationAxis);
}
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;
}
bool isNan() const{
return _latitude.isNan() || _longitude.isNan();
}