osg::Vec3d spherical_to_cartesian (osg::Vec3d & spher, double scale)
{
osg::Vec3d rads(spher.x (),
spher.y () * DEGS_TO_RADS,
spher.z () * DEGS_TO_RADS);
return spherical_to_cartesian_radians(rads, scale);
}
// gets the point of intersection between two lines represented by the line
// segments passed in (note the intersection point may not be on the finite
// segment). If the lines are parallel, returns a point in the middle
static bool
getLineIntersection( Segment& s0, Segment& s1, osg::Vec3d& output )
{
osg::Vec3d& p1 = s0.p0;
osg::Vec3d& p2 = s0.p1;
osg::Vec3d& p3 = s1.p0;
osg::Vec3d& p4 = s1.p1;
double denom = (p4.y()-p3.y())*(p2.x()-p1.x()) - (p4.x()-p3.x())*(p2.y()-p1.y());
if ( ::fabs(denom) >= 0.001 ) //denom != 0.0 )
{
double ua_num = (p4.x()-p3.x())*(p1.y()-p3.y()) - (p4.y()-p3.y())*(p1.x()-p3.x());
double ub_num = (p2.x()-p1.x())*(p1.y()-p3.y()) - (p2.y()-p1.y())*(p1.x()-p3.x());
double ua = ua_num/denom;
double ub = ub_num/denom;
double isect_x = p1.x() + ua*(p2.x()-p1.x());
double isect_y = p1.y() + ua*(p2.y()-p1.y());
output.set( isect_x, isect_y, p2.z() );
return true;
}
else // colinear or parallel
{
output.set( p2 );
return false;
}
//return true;
}
osg::Vec3d ConfigManager::getVec3d(std::string attributeX,
std::string attributeY, std::string attributeZ, std::string path,
osg::Vec3d def, bool * found)
{
bool hasEntry = false;
bool isFound;
osg::Vec3d result;
result.x() = getDouble(attributeX,path,def.x(),&isFound);
if(isFound)
{
hasEntry = true;
}
result.y() = getDouble(attributeY,path,def.y(),&isFound);
if(isFound)
{
hasEntry = true;
}
result.z() = getDouble(attributeZ,path,def.z(),&isFound);
if(isFound)
{
hasEntry = true;
}
if(found)
{
*found = hasEntry;
}
return result;
}
void
GeoLocator::cropLocal( osg::Vec3d& local ) const
{
// crop if necessary:
local.x() = osg::clampBetween( _x0 + local.x()*(_x1 - _x0), 0.0, 1.0 );
local.y() = osg::clampBetween( _y0 + local.y()*(_y1 - _y0), 0.0, 1.0 );
}
bool
SpatialReference::createLocalToWorld(const osg::Vec3d& xyz, osg::Matrixd& out_local2world ) const
{
if ( (isProjected() || _is_plate_carre) && !isCube() )
{
osg::Vec3d world;
if ( !transformToWorld( xyz, world ) )
return false;
out_local2world = osg::Matrix::translate(world);
}
else if ( isECEF() )
{
//out_local2world = ECEF::createLocalToWorld(xyz);
_ellipsoid->computeLocalToWorldTransformFromXYZ(xyz.x(), xyz.y(), xyz.z(), out_local2world);
}
else
{
// convert MSL to HAE if necessary:
osg::Vec3d geodetic;
if ( !transform(xyz, getGeodeticSRS(), geodetic) )
return false;
// then to ECEF:
osg::Vec3d ecef;
if ( !transform(geodetic, getGeodeticSRS()->getECEF(), ecef) )
return false;
//out_local2world = ECEF::createLocalToWorld(ecef);
_ellipsoid->computeLocalToWorldTransformFromXYZ(ecef.x(), ecef.y(), ecef.z(), out_local2world);
}
return true;
}
bool Locator::convertModelToLocal(const osg::Vec3d& world, osg::Vec3d& local) const
{
switch(_coordinateSystemType)
{
case(GEOCENTRIC):
{
double longitude, latitude, height;
_ellipsoidModel->convertXYZToLatLongHeight(world.x(), world.y(), world.z(),
latitude, longitude, height );
local = osg::Vec3d(longitude, latitude, height) * _inverse;
return true;
}
case(GEOGRAPHIC):
{
local = world * _inverse;
return true;
}
case(PROJECTED):
{
local = world * _inverse;
return true;
}
}
return false;
}
osg::Matrixd ViewingCore::computeProjection() const
{
if( !( _scene.valid() ) ) {
osg::notify( osg::WARN ) << "ViewingCore::computeProjection: _scene == NULL." << std::endl;
return( osg::Matrixd::identity() );
}
// TBD do we really want eyeToCenter to be a vector
// to the *bound* center, or to the *view* center?
const osg::BoundingSphere& bs = _scene->getBound();
const osg::Vec3d eyeToCenter( bs._center - getEyePosition() );
if( _ortho ) {
double zNear = eyeToCenter.length() - bs._radius;
double zFar = eyeToCenter.length() + bs._radius;
const double xRange = _aspect * ( _orthoTop - _orthoBottom );
const double right = xRange * .5;
return( osg::Matrixd::ortho( -right, right, _orthoBottom, _orthoTop, zNear, zFar ) );
} else {
double zNear = eyeToCenter.length() - bs._radius;
double zFar = zNear + ( bs._radius * 2. );
if( zNear < 0. ) {
zNear = zFar / 2000.; // Default z ratio.
}
return( osg::Matrixd::perspective( _fovy, _aspect, zNear, zFar ) );
}
}
void DataOutputStream::writeVec3d(const osg::Vec3d& v){
writeDouble(v.x());
writeDouble(v.y());
writeDouble(v.z());
if (_verboseOutput) std::cout<<"read/writeVec3d() ["<<v<<"]"<<std::endl;
}
std::string toString(const osg::Vec3d &value)
{
std::stringstream str;
str << value.x() << " " << value.y() << " " << value.z();
return str.str();
}
void ScreenBase::computeDefaultViewProj(osg::Vec3d eyePos, osg::Matrix & view,
osg::Matrix & proj)
{
//translate screen to origin
osg::Matrix screenTrans;
screenTrans.makeTranslate(-_myInfo->xyz);
//rotate screen to xz
osg::Matrix screenRot;
screenRot.makeRotate(-_myInfo->h * M_PI / 180.0,osg::Vec3(0,0,1),
-_myInfo->p * M_PI / 180.0,osg::Vec3(1,0,0),
-_myInfo->r * M_PI / 180.0,osg::Vec3(0,1,0));
eyePos = eyePos * screenTrans * screenRot;
//make frustum
float top, bottom, left, right;
float screenDist = -eyePos.y();
top = _near * (_myInfo->height / 2.0 - eyePos.z()) / screenDist;
bottom = _near * (-_myInfo->height / 2.0 - eyePos.z()) / screenDist;
right = _near * (_myInfo->width / 2.0 - eyePos.x()) / screenDist;
left = _near * (-_myInfo->width / 2.0 - eyePos.x()) / screenDist;
proj.makeFrustum(left,right,bottom,top,_near,_far);
// move camera to origin
osg::Matrix cameraTrans;
cameraTrans.makeTranslate(-eyePos);
//make view
view = screenTrans * screenRot * cameraTrans
* osg::Matrix::lookAt(osg::Vec3(0,0,0),osg::Vec3(0,1,0),
osg::Vec3(0,0,1));
}
bool
CubeFaceLocator::convertLocalToModel( const osg::Vec3d& local, osg::Vec3d& world ) const
{
#if ((OPENSCENEGRAPH_MAJOR_VERSION <= 2) && (OPENSCENEGRAPH_MINOR_VERSION < 8))
// OSG 2.7 bug workaround: bug fix in Locator submitted by GW
const_cast<CubeFaceLocator*>(this)->_inverse.invert( _transform );
#endif
if ( _coordinateSystemType == GEOCENTRIC )
{
//Convert the NDC coordinate into face space
osg::Vec3d faceCoord = local * _transform;
double lat_deg, lon_deg;
if ( !CubeUtils::faceCoordsToLatLon( faceCoord.x(), faceCoord.y(), _face, lat_deg, lon_deg ))
return false;
//OE_NOTICE << "LatLon=" << latLon << std::endl;
// convert to geocentric:
_ellipsoidModel->convertLatLongHeightToXYZ(
osg::DegreesToRadians( lat_deg ),
osg::DegreesToRadians( lon_deg ),
local.z(),
world.x(), world.y(), world.z() );
return true;
}
return true;
}
bool
SpatialReference::transformFromWorld(const osg::Vec3d& world,
osg::Vec3d& output,
double* out_haeZ ) const
{
if ( (isGeographic() && !isPlateCarre()) || isCube() ) //isGeographic() && !_is_plate_carre )
{
return transformFromECEF(world, output, out_haeZ);
}
else // isProjected || _is_plate_carre
{
output = world;
if (out_haeZ)
*out_haeZ = world.z();
if ( _vdatum.valid() )
{
// get the geographic coords by converting x/y/hae -> lat/long/msl:
osg::Vec3d lla;
if (!transform(world, getGeographicSRS(), lla) )
return false;
output.z() = lla.z();
}
return true;
}
}
// Returns angle between 2 vectors in degrees
double ViroManipulator::VecAngle(osg::Vec3d a, osg::Vec3d b){
double ang;
a.normalize();
b.normalize();
ang = RadiansToDegrees( acos(a * b) );
return fabs( ang );
}
void LOSCreationDialog::getLOSPoint(LOSPoint point, osg::Vec3d& out_point, bool relative)
{
double alt = 0.0;
switch(point)
{
case P2P_START:
alt = _ui.p1AltBox->value();
if (!relative && _ui.p2pRelativeCheckBox->checkState() == Qt::Checked)
alt += _p1BaseAlt;
out_point.set(_ui.p1LonBox->value(), _ui.p1LatBox->value(), alt);
break;
case P2P_END:
alt = _ui.p2AltBox->value();
if (!relative && _ui.p2pRelativeCheckBox->checkState() == Qt::Checked)
alt += _p2BaseAlt;
out_point.set(_ui.p2LonBox->value(), _ui.p2LatBox->value(), alt);
break;
case RADIAL_CENTER:
alt = _ui.radAltBox->value();
if (!relative && _ui.radRelativeCheckBox->checkState() == Qt::Checked)
alt += _radBaseAlt;
out_point.set(_ui.radLonBox->value(), _ui.radLatBox->value(), alt);
break;
}
}
inline double turbulence(module::Perlin& noise, const osg::Vec3d& v, double f )
{
double t = -0.5;
for( ; f<getPixelsPerTile()/2; f *= 2 )
t += abs(noise.GetValue(v.x(), v.y(), v.z())/f);
return t;
}
static bool getRelativeWorld(double x, double y, double relativeHeight, MapNode* mapNode, osg::Vec3d& world )
{
GeoPoint mapPoint(mapNode->getMapSRS(), x, y);
osg::Vec3d pos;
mapNode->getMap()->toWorldPoint(mapPoint, pos);
osg::Vec3d up(0,0,1);
const osg::EllipsoidModel* em = mapNode->getMap()->getProfile()->getSRS()->getEllipsoid();
if (em)
{
up = em->computeLocalUpVector( world.x(), world.y(), world.z());
}
up.normalize();
double segOffset = 50000;
osg::Vec3d start = pos + (up * segOffset);
osg::Vec3d end = pos - (up * segOffset);
osgUtil::LineSegmentIntersector* i = new osgUtil::LineSegmentIntersector( start, end );
osgUtil::IntersectionVisitor iv;
iv.setIntersector( i );
mapNode->getTerrainEngine()->accept( iv );
osgUtil::LineSegmentIntersector::Intersections& results = i->getIntersections();
if ( !results.empty() )
{
const osgUtil::LineSegmentIntersector::Intersection& result = *results.begin();
world = result.getWorldIntersectPoint();
world += up * relativeHeight;
return true;
}
return false;
}
bool Locator::convertLocalToModel(const osg::Vec3d& local, osg::Vec3d& world) const
{
switch(_coordinateSystemType)
{
case(GEOCENTRIC):
{
osg::Vec3d geographic = local * _transform;
_ellipsoidModel->convertLatLongHeightToXYZ(geographic.y(), geographic.x(), geographic.z(),
world.x(), world.y(), world.z());
return true;
}
case(GEOGRAPHIC):
{
world = local * _transform;
return true;
}
case(PROJECTED):
{
world = local * _transform;
return true;
}
}
return false;
}
HUDHoverScaler::HUDHoverScaler(osgviz::Object* obj, const osg::Vec3d &size, const osg::Vec3d &scale, Type type, osg::Vec3d anchor_offset, HUD* hud):obj(obj),anchor_offset(anchor_offset),scale(scale),size(size),type(type),hud(hud){
scaled = false;
initial_scale = obj->getScale();
if (scale.x() == 0 || scale.y() == 0 || scale.z() == 0){
printf("%s scale cannot be 0\n",__PRETTY_FUNCTION__);
}
}
void GlobePlugin::setSelectedCoordinates( osg::Vec3d coords )
{
mSelectedLon = coords.x();
mSelectedLat = coords.y();
mSelectedElevation = coords.z();
QgsPoint coord = QgsPoint( mSelectedLon, mSelectedLat );
emit newCoordinatesSelected( coord );
}
void LazyCameraManipulator::calculateNextCameraPosition()
{
// retrieve player position and direction of movement
const Player *player = dynamic_cast<const Player *>(getTrackNode());
const osg::Vec3d nodePosition = player->getPosition();
const PlayerState *playerState = player->getPlayerState();
const int newDirectionX = playerState->getDirectionX();
// if this is the first run, follow node directly
if(_firstRun)
{
_newCameraPosition.x() = nodePosition.x();
_oldCameraPosition.x() = nodePosition.x();
_oldNodePosition = nodePosition;
_durationOfMovementX = 0;
_firstRun = false;
return;
}
// +++ step 1 +++
// check if direction of movement has changed
bool directionChanged = false;
if(newDirectionX != _directionOfMovementX)
{
if(newDirectionX != 0)
{
_durationOfMovementX = 0;
directionChanged = true;
}
_directionOfMovementX = newDirectionX;
}
float stepper = (_numSimulationSubSteps > 0) ? ((nodePosition.x() - _oldNodePosition.x()) / _numSimulationSubSteps) : 0.0f;
for(int i=0; i<_numSimulationSubSteps; i++, _oldNodePosition.x() += stepper, _durationOfMovementX++)
{
if(_fadeOut)
_distance += 10.0f * _numSimulationSubSteps;
if(!directionChanged && fabs(_oldCameraPosition.x() - _oldNodePosition.x()) < 0.001 && _durationOfMovementX > 20)
{
// +++ step 2 +++
// if direction has not changed, check if we were already following the node
_newCameraPosition.x() = nodePosition.x();
}
else
{
// +++ step 3 +++
// in any other case continue (or begin) approaching the node
_newCameraPosition.x() = _oldCameraPosition.x() + (_oldNodePosition.x() - _oldCameraPosition.x()) * (_durationOfMovementX / MAX_FRAME_DELAY);
}
_oldCameraPosition = _newCameraPosition;
}
_oldNodePosition = nodePosition;
}
bool CameraMath::isInRect( osg::Vec3d point, double width, double height, double margin )
{
if ( point.x() < margin || point.x() > width - margin || point.y() < margin || point.y() > height - margin ) {
return false;
}
else {
return true;
}
}
bool
TritonContext::intersect(const osg::Vec3d& start, const osg::Vec3d& dir, float& out_height, osg::Vec3f& out_normal) const
{
::Triton::Vector3 p(start.ptr());
::Triton::Vector3 d(dir.ptr());
::Triton::Vector3 normal;
bool ok = _ocean->GetHeight(p, d, out_height, normal);
out_normal.set(normal.x, normal.y, normal.z);
return ok;
}
osg::Vec3d LineAnalysis::getWorldCoord(osg::Vec3d pos)
{
osg::Vec3d world;
m_pMap3D->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
osg::DegreesToRadians(pos.y()),
osg::DegreesToRadians(pos.x()),
pos.z(), world.x(), world.y(), world.z());
return world;
}
bool CalcPolysIntersectSAT(std::vector<osg::Vec3d> const &polyA_list_vx,
std::vector<osg::Vec3d> const &polyA_list_face_norms,
std::vector<osg::Vec3d> const &polyA_list_edges,
std::vector<osg::Vec3d> const &polyB_list_vx,
std::vector<osg::Vec3d> const &polyB_list_face_norms,
std::vector<osg::Vec3d> const &polyB_list_edges)
{
// Test the faces of polyA
for(auto const &polyA_face_norm : polyA_list_face_norms)
{
if(!ProjectionIntervalsOverlap(polyA_face_norm,
polyA_list_vx,
polyB_list_vx)) {
return false;
}
}
// Test the faces of polyB
for(auto const &polyB_face_norm : polyB_list_face_norms)
{
if(!ProjectionIntervalsOverlap(polyB_face_norm,
polyA_list_vx,
polyB_list_vx)) {
return false;
}
}
// Test the cross product of the edges of polyA and polyB
for(auto const &polyA_edge : polyA_list_edges) {
for(auto const &polyB_edge : polyB_list_edges)
{
osg::Vec3d const axis = polyA_edge^polyB_edge;
// Edge may be invalid -- need to handle
// this case better
if(axis.length2() < 1E-5) {
continue;
}
if(!ProjectionIntervalsOverlap(axis,
polyA_list_vx,
polyB_list_vx)) {
return false;
}
}
}
// for(auto const &interval : list_polyA_intervals) {
// std::cout << "###: min: " << interval.first
// << "max: " << interval.second << std::endl;
// }
// Intersection
return true;
}
virtual bool clicked(const int &buttonMask, const osg::Vec2d &cursor,
const osg::Vec3d &world, const osg::Vec3d &local,
Clickable* object, const int modifierMask,
osgviz::WindowInterface* window = NULL){
osgviz::Object* osgvizObject = dynamic_cast<osgviz::Object*> (object);
if (osgvizObject){
printf("clicked %s (%.2f %.2f %.2f)\n",osgvizObject->getName().c_str(),world.x(),world.y(),world.z());
}else{
printf("clicked (%.2f %.2f %.2f)\n",world.x(),world.y(),world.z());
}
}
bool
CubeFaceLocator::convertModelToLocal(const osg::Vec3d& world, osg::Vec3d& local) const
{
#if ((OPENSCENEGRAPH_MAJOR_VERSION <= 2) && (OPENSCENEGRAPH_MINOR_VERSION < 8))
// OSG 2.7 bug workaround: bug fix in Locator submitted by GW
const_cast<CubeFaceLocator*>(this)->_inverse.invert( _transform );
#endif
switch(_coordinateSystemType)
{
case(GEOCENTRIC):
{
double longitude, latitude, height;
_ellipsoidModel->convertXYZToLatLongHeight(world.x(), world.y(), world.z(), latitude, longitude, height );
int face=-1;
double x, y;
double lat_deg = osg::RadiansToDegrees(latitude);
double lon_deg = osg::RadiansToDegrees(longitude);
bool success = CubeUtils::latLonToFaceCoords( lat_deg, lon_deg, x, y, face, _face );
if (!success)
{
OE_WARN << LC << "Couldn't convert to face coords " << std::endl;
return false;
}
if (face != _face)
{
OE_WARN << LC
<< "Face should be " << _face << " but is " << face
<< ", lat = " << lat_deg
<< ", lon = " << lon_deg
<< std::endl;
}
local = osg::Vec3d( x, y, height ) * _inverse;
return true;
}
case(GEOGRAPHIC):
case(PROJECTED):
// Neither of these is supported for this locator..
{
local = world * _inverse;
return true;
}
}
return false;
}
void convertXYZToLatLongHeight(osg::EllipsoidModel* em, osg::Vec3d& v)
{
em->convertXYZToLatLongHeight(v.x(), v.y(), v.z(),
v.y(), v.x(), v.z());
v.x() = osg::RadiansToDegrees(v.x());
v.y() = osg::RadiansToDegrees(v.y());
}
osg::Vec3d RadarMap::getLonLat(const osg::Vec3d& worldPos)
{
osg::Vec3d vecPos = osg::Vec3d();
if (m_pOSGViewer)
{
m_pOSGViewer->getSRS()->getEllipsoid()->convertXYZToLatLongHeight(
worldPos.x(), worldPos.y(), worldPos.z(), vecPos.y(), vecPos.x(), vecPos.z());
vecPos.x() = osg::RadiansToDegrees(vecPos.x());
vecPos.y() = osg::RadiansToDegrees(vecPos.y());
}
return vecPos;
}
void
SpatialReference::createLocal2World(const osg::Vec3d& xyz, osg::Matrixd& out_local2world ) const
{
if ( isProjected() )
{
out_local2world = osg::Matrix::translate(xyz);
}
else
{
getEllipsoid()->computeLocalToWorldTransformFromLatLongHeight(
osg::DegreesToRadians(xyz.y()), osg::DegreesToRadians(xyz.x()), xyz.z(),
out_local2world);
}
}
// z0 is the point that lies on the plane A parallel to the near plane through e
// and on the near plane of the C frustum (the plane z = bZmax) and on the line x = e.x
osg::Vec3d LispSM::getZ0_ls
(const osg::Matrix &lightSpace, const osg::Vec3d &e, const double &b_lsZmax, const osg::Vec3d &eyeDir) const
{
// to calculate the parallel plane to the near plane through e we
// calculate the plane A with the three points
osg::Plane A(eyeDir, e);
// to transform plane A into lightSpace
A.transform(lightSpace);
// transform to light space
const osg::Vec3d e_ls = e * lightSpace;
// z_0 has the x coordinate of e, the z coord of B_lsZmax
// and the y coord of the plane A and plane (z==B_lsZmax) intersection
#if 1
osg::Vec3d v = osg::Vec3d(e_ls.x(), 0, b_lsZmax);
// x & z are given. We compute y from equations:
// A.distance( x,y,z ) == 0
// A.distance( x,y,z ) == A.distance( x,0,z ) + A.normal.y * y
// hence A.distance( x,0,z ) == -A.normal.y * y
v.y() = -A.distance(v) / A.getNormal().y();
#else
// get the parameters of A from the plane equation n dot d = 0
const double d = A.asVec4()[3];
const osg::Vec3d n = A.getNormal();
osg::Vec3d v(e_ls.x(), (-d - n.z() * b_lsZmax - n.x() * e_ls.x()) / n.y(), b_lsZmax);
#endif
return v;
}
