Geodetic3D getDefaultCameraPosition(const Sector& shownSector) const{
const Vector3D asw = toCartesian(shownSector.getSW());
const Vector3D ane = toCartesian(shownSector.getNE());
const double height = asw.sub(ane).length() * 1.9;
return Geodetic3D(shownSector._center,
height);
}
Geodetic2D SphericalPlanet::getMidPoint (const Geodetic2D& P0, const Geodetic2D& P1) const
{
const Vector3D v0 = toCartesian(P0);
const Vector3D v1 = toCartesian(P1);
const Vector3D normal = v0.cross(v1).normalized();
const Angle theta = v0.angleBetween(v1);
const Vector3D midPoint = scaleToGeocentricSurface(v0.rotateAroundAxis(normal, theta.times(0.5)));
return toGeodetic2D(midPoint);
}
MutableMatrix44D SphericalPlanet::drag(const Geodetic3D& origin, const Geodetic3D& destination) const
{
const Vector3D P0 = toCartesian(origin);
const Vector3D P1 = toCartesian(destination);
const Vector3D axis = P0.cross(P1);
if (axis.length()<1e-3) return MutableMatrix44D::invalid();
const Angle angle = P0.angleBetween(P1);
const MutableMatrix44D rotation = MutableMatrix44D::createRotationMatrix(angle, axis);
const Vector3D rotatedP0 = P0.transformedBy(rotation, 1);
const MutableMatrix44D traslation = MutableMatrix44D::createTranslationMatrix(P1.sub(rotatedP0));
return traslation.multiply(rotation);
}
/* This function calculates any maneuvers that are necessary for the
current plane to avoid looping. Returns a waypoint based on calculations.
If no maneuvers are necessary, then the function returns the current
destination. */
sim::waypoint sim::takeDubinsPath(PlaneObject &plane1) {
/* Initialize variables */
sim::coordinate circleCenter;
sim::waypoint wp = plane1.getDestination();
double minTurningRadius = 0.75*MINIMUM_TURNING_RADIUS;
bool destOnRight;
/* Calculate cartesian angle from plane to waypoint */
double wpBearing = findAngle(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude);
/* Calculate cartesian current bearing of plane (currentBearing is stored as Cardinal) */
double currentBearingCardinal = plane1.getCurrentBearing();
double currentBearingCartesian = toCartesian(currentBearingCardinal);
if (fabs(currentBearingCardinal) < 90.0)
/* Figure out which side of the plane the waypoint is on */
destOnRight = ((wpBearing < currentBearingCartesian) &&
(wpBearing > manipulateAngle(currentBearingCartesian - 180.0)));
else
destOnRight = !((wpBearing > currentBearingCartesian) &&
(wpBearing < manipulateAngle(currentBearingCartesian - 180.0)));
/* Calculate the center of the circle of minimum turning radius on the side that the waypoint is on */
circleCenter = calculateLoopingCircleCenter(plane1, minTurningRadius, destOnRight);
/* If destination is inside circle, must fly opposite direction before we can reach destination */
if (findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude) <
minTurningRadius)
return calculateWaypoint(plane1, minTurningRadius, !destOnRight);
else
return wp;
}
AU_UAV_ROS::coordinate AU_UAV_ROS::calculateLoopingCircleCenter(PlaneObject &plane, double turnRadius, bool turnRight) {
AU_UAV_ROS::coordinate circleCenter;
circleCenter.altitude = plane.getCurrentLoc().altitude;
double angle;
if (turnRight) {
angle = (toCartesian(plane.getCurrentBearing()) - 90 - 22.5) * PI/180.0;
}
else {
angle = (toCartesian(plane.getCurrentBearing()) + 90 + 22.5) * PI/180.0;
}
double xdiff = turnRadius*cos(angle);
double ydiff = turnRadius*sin(angle);
circleCenter.longitude = plane.getCurrentLoc().longitude + xdiff/DELTA_LON_TO_METERS;
circleCenter.latitude = plane.getCurrentLoc().latitude + ydiff/DELTA_LAT_TO_METERS;
return circleCenter;
}
void Ball::bounce(float offset, sf::Vector2f polarCollisionPoint) {
// move ball outside paddle, to where it would be if
// bounce happened immediately
sf::Vector2f polarPos = toPolar(getPosition() - Settings::getScreenCenter());
const float ballRad = ((sf::CircleShape*)shape)->getRadius();
const float paddleRad = polarCollisionPoint.x;
sf::Vector2f bouncedPos( 2*paddleRad - polarPos.x - 2 * ballRad,
polarPos.y);
shape->setPosition(toCartesian(bouncedPos) + Settings::getScreenCenter());
// invert direction, then modify according to skew
// factor and offset
sf::Vector2f polarVel = toPolar(velocity);
float angleOfIncidence = polarCollisionPoint.y - polarVel.y;
if (angleOfIncidence > 90) {
angleOfIncidence = angleOfIncidence - 180;
} else if (angleOfIncidence < -90) {
angleOfIncidence = angleOfIncidence + 180;
}
float reflectionAngle = 180 - 2*angleOfIncidence;
float skewReflectionAngle = reflectionAngle + skewFactor * offset;
if ((reflectionAngle <=180 &&skewReflectionAngle < reflectionAngle/2) || (reflectionAngle > 180 && (180 -skewReflectionAngle) > (180 -reflectionAngle/2)) ) {
skewReflectionAngle = reflectionAngle/2;
}
// reflect and skew
polarVel.y -= skewReflectionAngle;
mutex.lock();
velocity = toCartesian(polarVel);
mutex.unlock();
}
/* Returns true if the original plane (plane1) should turn right to avoid plane2,
false if otherwise. Takes original plane and its greatest threat as parameters. */
bool sim::shouldTurnRight(PlaneObject &plane1, PlaneObject &plane2) {
/* For checking whether the plane should turn right or left */
double theta, theta_dot, R;
double cartBearing1 = toCartesian(plane1.getCurrentBearing());
double cartBearing2 = toCartesian(plane2.getCurrentBearing());
double V = MPS_SPEED;
/* Calculate theta, theta1, and theta2. Theta is the cartesian angle
from 0 degrees (due East) to plane2 (using plane1 as the origin). This
may be referred to as the LOS angle. */
theta = findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
R = findDistance(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
theta_dot = (V*sin((cartBearing2 - theta)*PI/180) - V*sin((cartBearing1 - theta)*PI/180)) / R;
return (theta_dot >= 0);
}
void VectorDrawable::setVectors(PointListListPtr p, bool radians)
{
_xmin = 0;
_ymin = 0;
_xmax = 0;
_ymax = 0;
if(p==NULL)
return;
if(p->size()==0)
return;
clearVectors();
_pickPointsVector = p;
PointListList::iterator it;
for(it=p->begin(); it!=p->end(); it++)
{
PointListPtr pp = &(*it);
unsigned index = 0;
FloatList ppp;
ppp.p = new float[ pp->size() * 3 ];
iiMath::vec3 p1 = toCartesian( *pp->begin(), radians);
ppp.offset = p1;
PointList::iterator it2;
for(it2=pp->begin(); it2!=pp->end(); it2++)
{
iiMath::vec3 p1 = toCartesian( *it2, radians);
ppp.p[index++] = p1.x() - ppp.offset.x();
ppp.p[index++] = p1.y() - ppp.offset.y();
ppp.p[index++] = p1.z() - ppp.offset.z();
}
ppp.size = pp->size();
_pshiftVector.push_back( ppp );
}
}
/*
* findFieldAngle
* Preconditions: assumes valid planes
* params: me: plane that is potentially in enemy's field
* enemy: plane that is producing field
* returns: field angle - angle between enemy's bearing and my location.
* 0 < x < 180 = to enemy's right
* -180< x < 0= to enemy's left
*
* note: Different from AU_UAV_ROS::PlaneObject::findAngle(). FindAngle finds
* the angle between the relative position vector from one plane to another and the
* x axis in a global, absolute x/y coordinate system based on latitude and
* longitude.
*/
double fsquared::findFieldAngle(AU_UAV_ROS::PlaneObject& me, AU_UAV_ROS::PlaneObject &enemy) {
//Make two vectors - one representing bearing of enemy plane
// the other representing relative position from
// enemy to me
AU_UAV_ROS::mathVector enemyBearing(1, toCartesian(enemy.getCurrentBearing()));
AU_UAV_ROS::mathVector positionToMe(1, enemy.findAngle(me));
//Find angle between two vectors
return enemyBearing.findAngleBetween(positionToMe);
}
AU_UAV_ROS::mathVector fsquared::calculateAttractiveForce(AU_UAV_ROS::PlaneObject &me, AU_UAV_ROS::waypoint goal_wp){
double aAngle, aMagnitude, destLat, destLon, currentLat, currentLon;
//obtain current location by accessing PlaneObject's current coordinate
currentLat = me.getCurrentLoc().latitude;
currentLon = me.getCurrentLoc().longitude;
destLat = goal_wp.latitude;
destLon = goal_wp.longitude;
aAngle = toCartesian(findAngle(currentLat, currentLon, destLat, destLon));
aMagnitude = ATTRACTIVE_FORCE;
//construct the attractrive force vector and return it
AU_UAV_ROS::mathVector attractiveForceVector(aMagnitude, aAngle);
return attractiveForceVector;
}
void ArcShape::refreshTrapesiums() {
// erase old
trapesiums.erase(trapesiums.begin(), trapesiums.end());
// use instance varibles to set trapesium properties
float offset = 0;
float interval = angularLength / blockCount;
for (int i = 0; i < blockCount; i++, offset+=interval) {
sf::ConvexShape trap;
trap.setPointCount(4);
trap.setPoint(0, toCartesian(sf::Vector2f(outerRadius,
offset)));
trap.setPoint(1, toCartesian(sf::Vector2f(outerRadius,
offset+interval)));
trap.setPoint(2, toCartesian(sf::Vector2f(outerRadius - width,
offset + interval)));
trap.setPoint(3, toCartesian(sf::Vector2f(outerRadius - width,
offset)));
trapesiums.push_back(trap);
}
}
/* Find the new collision avoidance waypoint for the plane to go to */
sim::waypoint sim::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
sim::waypoint wp;
double V = MPS_SPEED;
double delta_T = TIME_STEP;
double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
double psi = (cartBearing + delta_psi);
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
wp.altitude = plane1.getCurrentLoc().altitude;
return wp;
}
/* Find the new collision avoidance waypoint for the plane to go to */
AU_UAV_ROS::waypoint AU_UAV_ROS::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
AU_UAV_ROS::waypoint wp;
double V = MPS_SPEED;
double delta_T = TIME_STEP;
double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
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();
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM */
AU_UAV_ROS::threatContainer AU_UAV_ROS::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes){
/* Set reference for origin (Northwest corner of the course)*/
AU_UAV_ROS::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous
ZEM as negative*/
int planeToAvoid = -1;
double mostDangerousZEM = -1;
/* Set the preliminary time-to-go to infinity*/
double minimumTimeToGo = std::numeric_limits<double>::infinity();
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane*/
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop*/
AU_UAV_ROS::mathVector pDiff;
AU_UAV_ROS::mathVector dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest;
std::map<int,AU_UAV_ROS::PlaneObject>::iterator it;
for ( it=planes.begin() ; it!= planes.end(); it++ ){
/* Unpacking plane to check*/
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane*/
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
else if (distanceBetween < MPS_SPEED) {
planeToAvoid = ID;
mostDangerousZEM = 0;
minimumTimeToGo = 0.1;
break;
}
/* Making a position vector representation of plane2*/
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute Time To Go*/
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss*/
zeroEffortMiss = sqrt(pDiff.dotProduct(pDiff) +
2*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2)*dDiff.dotProduct(dDiff));
/* If the Zero Effort Miss is less than the minimum required
separation, and the time to go is the least so far, then avoid this plane*/
if(zeroEffortMiss <= DANGER_ZEM && timeToGo < minimumTimeToGo && timeToGo > 0){
// If the plane is behind you, don't avoid it
if ( fabs(plane2.findAngle(plane1)*180/PI - toCartesian(plane1.getCurrentBearing())) > 35.0) {
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
}
}
AU_UAV_ROS::threatContainer greatestThreat;
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
return greatestThreat;
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM. */
sim::threatContainer sim::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes) {
/* Set reference for origin (Northwest corner of the course)*/
sim::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous ZEM as negative */
int planeToAvoid = -1;
int iPlaneToAvoid = -1;
double mostDangerousZEM = -1.0;
double iMostDangerousZEM = -1.0;
/* Set the preliminary time-to-go high */
double minimumTimeToGo = MINIMUM_TIME_TO_GO;
double iMinimumTimeToGo = 3.5;
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane */
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
sim::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane */
sim::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop */
sim::mathVector pDiff, dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest, bearingDiff;
std::map<int,sim::PlaneObject>::iterator it;
for (it=planes.begin() ; it!= planes.end(); it++) {
/* Unpacking plane to check */
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane */
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
/* Making a position vector representation of plane2 */
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
sim::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the other plane */
sim::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute time-to-go */
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1.0*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss */
zeroEffortMiss = sqrt(fabs(pDiff.dotProduct(pDiff) +
2.0*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2.0)*dDiff.dotProduct(dDiff)));
if( zeroEffortMiss > DANGER_ZEM || (timeToGo > minimumTimeToGo && timeToGo > iMinimumTimeToGo) || timeToGo < 0 ) continue;
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
/* If you're likely to zigzag, don't avoid the other plane */
bearingDiff = fabs(plane1.getCurrentBearing() - planes[ID].getCurrentBearing());
if ( plane1.findDistance(planes[ID]) > 3.5*MPS_SPEED && bearingDiff < CHATTERING_ANGLE) continue;
/* Second Threshold, to prevent planes from flying into others when trying to avoid less imminent collisions */
if ( zeroEffortMiss <= SECOND_THRESHOLD && timeToGo <= iMinimumTimeToGo ) {
iPlaneToAvoid = ID;
iMostDangerousZEM = zeroEffortMiss;
iMinimumTimeToGo = timeToGo;
continue;
}
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
sim::threatContainer greatestThreat;
if (iPlaneToAvoid > -1) {
greatestThreat.planeID = iPlaneToAvoid;
greatestThreat.ZEM = iMostDangerousZEM;
greatestThreat.timeToGo = iMinimumTimeToGo;
}
else {
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
}
return greatestThreat;
}
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;
}
MutableMatrix44D SphericalPlanet::createGeodeticTransformMatrix(const Geodetic3D& position) const {
const MutableMatrix44D translation = MutableMatrix44D::createTranslationMatrix( toCartesian(position) );
const MutableMatrix44D rotation = MutableMatrix44D::createGeodeticRotationMatrix( position );
return translation.multiply(rotation);
}
Vector3D toCartesian(const Geodetic2D& geodetic,
const double height) const {
return toCartesian(geodetic._latitude,
geodetic._longitude,
height);
}
Vector3D toCartesian(const Geodetic2D& geodetic) const {
return toCartesian(geodetic._latitude,
geodetic._longitude,
0.0);
}
