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);
}
void SphericalPlanet::beginDoubleDrag(const Vector3D& origin,
const Vector3D& centerRay,
const Vector3D& initialRay0,
const Vector3D& initialRay1) const
{
_origin = origin.asMutableVector3D();
_centerRay = centerRay.asMutableVector3D();
_initialPoint0 = closestIntersection(origin, initialRay0).asMutableVector3D();
_initialPoint1 = closestIntersection(origin, initialRay1).asMutableVector3D();
_angleBetweenInitialPoints = _initialPoint0.angleBetween(_initialPoint1)._degrees;
_centerPoint = closestIntersection(origin, centerRay).asMutableVector3D();
_angleBetweenInitialRays = initialRay0.angleBetween(initialRay1)._degrees;
// middle point in 3D
Geodetic2D g0 = toGeodetic2D(_initialPoint0.asVector3D());
Geodetic2D g1 = toGeodetic2D(_initialPoint1.asVector3D());
Geodetic2D g = getMidPoint(g0, g1);
_initialPoint = toCartesian(g).asMutableVector3D();
}
Effect* SphericalPlanet::createDoubleTapEffect(const Vector3D& origin,
const Vector3D& centerRay,
const Vector3D& tapRay) const
{
const Vector3D initialPoint = closestIntersection(origin, tapRay);
if (initialPoint.isNan()) return NULL;
// compute central point of view
const Vector3D centerPoint = closestIntersection(origin, centerRay);
// compute drag parameters
const IMathUtils* mu = IMathUtils::instance();
const Vector3D axis = initialPoint.cross(centerPoint);
const Angle angle = Angle::fromRadians(- mu->asin(axis.length()/initialPoint.length()/centerPoint.length()));
// compute zoom factor
const double height = toGeodetic3D(origin)._height;
const double distance = height * 0.6;
// create effect
return new DoubleTapRotationEffect(TimeInterval::fromSeconds(0.75), axis, angle, distance);
}
IntersectionData KDTree::searchNode(KDNode *node, const ray &viewRay, double tmin, double tmax, int curAxis) {
assert(tmin <= tmax);
assert(curAxis >= 0 && curAxis < 3);
if(node->is_leaf) {
return closestIntersection(node->objects, viewRay);
}
int nextAxis = (curAxis + 1) % 3;
double tSplit = (node->split_pos - viewRay.orig(curAxis)) / viewRay.dir(curAxis);
KDNode *nearNode, *farNode;
if(viewRay.orig(curAxis) < node->split_pos) {
nearNode = node->left;
farNode = node->right;
} else {
nearNode = node->right;
farNode = node->left;
}
if(tSplit > tmax) {
return searchNode(nearNode, viewRay, tmin, tmax, nextAxis);
} else if (tSplit < tmin) {
if(tSplit > 0)
return searchNode(farNode, viewRay, tmin, tmax, nextAxis);
else if(tSplit < 0)
return searchNode(nearNode, viewRay, tmin, tmax, nextAxis);
else {
if(viewRay.dir(curAxis) < 0)
return searchNode(node->left, viewRay, tmin, tmax, nextAxis);
else
return searchNode(node->right, viewRay, tmin, tmax, nextAxis);
}
} else {
if(tSplit > 0) {
IntersectionData test = searchNode(nearNode, viewRay, tmin, tSplit, nextAxis);
if(test.obj != NULL && test.time < tSplit + EPSILON)
return test;
else
return searchNode(farNode, viewRay, tSplit, tmax, nextAxis);
} else {
return searchNode(nearNode, viewRay, tSplit, tmax, nextAxis);
}
}
}
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;
}
void SphericalPlanet::beginSingleDrag(const Vector3D& origin, const Vector3D& initialRay) const
{
_origin = origin.asMutableVector3D();
_initialPoint = closestIntersection(origin, initialRay).asMutableVector3D();
_validSingleDrag = false;
}
void Vision::run()
{
_drawPoints.clear();
_light.clear();
if(getSource().x < 0 || getSource().x > _mp->getMapSize().x) return;
if(getSource().y < 0 || getSource().y > _mp->getMapSize().y) return;
float fov = Tools::deg2rad(_fov);
float half_fov = fov / 2;
float min_fov = Tools::normalizeRad(_heading - half_fov);
float max_fov = Tools::normalizeRad(_heading + half_fov);
/* Ray minf; */
/* minf.start = getSource(); */
/* minf.end.x = (int)minf.start.x + _raylineMax * std::cos(min_fov); */
/* minf.end.y = (int)minf.start.y + _raylineMax * std::sin(min_fov); */
/* DrawTools::drawLine(minf.start, minf.end, sf::Color::Blue, _window); */
/* Ray maxf; */
/* maxf.start = getSource(); */
/* maxf.end.x = (int)maxf.start.x + _raylineMax * std::cos(max_fov); */
/* maxf.end.y = (int)maxf.start.y + _raylineMax * std::sin(max_fov); */
/* DrawTools::drawLine(maxf.start, maxf.end, sf::Color::Blue, _window); */
std::vector<float> _angles;
_angles.push_back(min_fov);
_angles.push_back(max_fov);
for(Wall* wall : _mp->getWalls())
{
for(std::size_t i = 0; i < 4; i++)
{
float a = std::atan2(wall->points[i].y - getSource().y, wall->points[i].x - getSource().x);
if(!Tools::radiansBetween(a, min_fov, max_fov)) continue;
_angles.push_back(Tools::normalizeRad(a) - 0.0001);
_angles.push_back(Tools::normalizeRad(a));
_angles.push_back(Tools::normalizeRad(a) + 0.0001);
}
}
std::unique(_angles.begin(), _angles.end());
std::sort(_angles.begin(), _angles.end());
auto it = std::find(_angles.begin(), _angles.end(), min_fov);
std::rotate(_angles.begin(), it, _angles.end());
Ray ray;
ray.start = getSource();
Point intersection;
for(float angle : _angles)
{
ray.end.x = (int)ray.start.x + _raylineMax * std::cos(angle);
ray.end.y = (int)ray.start.y + _raylineMax * std::sin(angle);
Point closestIntersection(-1000,-1000);
for(Wall* wall : _mp->getWalls())
{
for(std::size_t i = 0; i < 4; i++)
{
if(Tools::getIntersection(ray, wall->segments[i], intersection))
{
if(Tools::distance(intersection, ray.start) < Tools::distance(closestIntersection, ray.start))
closestIntersection = intersection;
}
}
}
_drawPoints.push_back(closestIntersection);
}
sf::Vertex tripoint;
tripoint.position = sf::Vector2f(getSource().x, getSource().y);
tripoint.color = _colour;
_light.append(tripoint);
for(Point& p : _drawPoints)
{
tripoint.position = sf::Vector2f(p.x, p.y);
_light.append(tripoint);
}
}
