bool
MercatorLocator::convertModelToLocal(const osg::Vec3d& world, osg::Vec3d& local) const
{
bool result = false;
// required becasue of an OSG bug
if ( !_inverseCalculated )
{
const_cast<MercatorLocator*>(this)->_inverse.invert( _transform );
const_cast<MercatorLocator*>(this)->_inverseCalculated = true;
}
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;
double lon_deg = osg::RadiansToDegrees(longitude);
double lat_deg = osg::RadiansToDegrees(latitude);
double xr, yr;
getUV( _geoDataExtent, lon_deg, lat_deg, xr, yr );
local.x() = xr;
local.y() = 1.0-yr;
result = true;
}
break;
case(GEOGRAPHIC):
{
local = world * _inverse;
osg::Vec3d w = world;
double lon_deg = w.x();
double lat_deg = w.y();
double xr, yr;
getUV( _geoDataExtent, lon_deg, lat_deg, xr, yr );
local.x() = xr;
local.y() = 1.0-yr;
result = true;
}
break;
case(PROJECTED):
{
local = world * _inverse;
result = true;
}
break;
}
return result;
}
void ArucoThread::printVec( const osg::Vec3d v, const QString name )const
{
qDebug() << name << " " << v.x() << " " << v.y() << " " << v.z();
}
float OsgDrawableCullCallback::GetDistance (osg::Vec3d punto)
{
return sqrt ( (eye.x()-punto.x()) * (eye.x()-punto.x())+
(eye.y()-punto.y()) * (eye.y()-punto.y())+
(eye.z()-punto.z()) * (eye.z()-punto.z()));
}
LargeScaleCoord::LargeScaleCoord(const osg::Vec3d& realCoord)
{
m_smallScale.set(0, 0, 0);
m_largeScale.set(realCoord.x() / sizeFactor, realCoord.y() / sizeFactor, realCoord.z() / sizeFactor);
}
bool Locator::computeLocalBounds(Locator& source, osg::Vec3d& bottomLeft, osg::Vec3d& topRight) const
{
typedef std::list<osg::Vec3d> Corners;
Corners corners;
osg::Vec3d cornerNDC;
if (convertLocalToModel(osg::Vec3d(0.0,0.0,0.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(1.0,0.0,0.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(0.0,1.0,0.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(1.0,1.0,0.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(0.0,0.0,1.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(1.0,0.0,1.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(0.0,1.0,1.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (convertLocalToModel(osg::Vec3d(1.0,1.0,1.0), cornerNDC))
{
corners.push_back(cornerNDC);
}
if (corners.empty()) return false;
for(Corners::iterator itr = corners.begin();
itr != corners.end();
++itr)
{
bottomLeft.x() = osg::minimum( bottomLeft.x(), itr->x());
bottomLeft.y() = osg::minimum( bottomLeft.y(), itr->y());
bottomLeft.z() = osg::minimum( bottomLeft.z(), itr->z());
topRight.x() = osg::maximum( topRight.x(), itr->x());
topRight.y() = osg::maximum( topRight.y(), itr->y());
topRight.z() = osg::maximum( topRight.z(), itr->z());
}
return true;
}
bool LineSegmentIntersector::intersectAndClip(osg::Vec3d& s, osg::Vec3d& e,const osg::BoundingBox& bbInput)
{
osg::Vec3d bb_min(bbInput._min);
osg::Vec3d bb_max(bbInput._max);
#if 1
double epsilon = 1e-4;
bb_min.x() -= epsilon;
bb_min.y() -= epsilon;
bb_min.z() -= epsilon;
bb_max.x() += epsilon;
bb_max.y() += epsilon;
bb_max.z() += epsilon;
#endif
// compate s and e against the xMin to xMax range of bb.
if (s.x()<=e.x())
{
// trivial reject of segment wholely outside.
if (e.x()<bb_min.x()) return false;
if (s.x()>bb_max.x()) return false;
if (s.x()<bb_min.x())
{
// clip s to xMin.
s = s+(e-s)*(bb_min.x()-s.x())/(e.x()-s.x());
}
if (e.x()>bb_max.x())
{
// clip e to xMax.
e = s+(e-s)*(bb_max.x()-s.x())/(e.x()-s.x());
}
}
else
{
if (s.x()<bb_min.x()) return false;
if (e.x()>bb_max.x()) return false;
if (e.x()<bb_min.x())
{
// clip s to xMin.
e = s+(e-s)*(bb_min.x()-s.x())/(e.x()-s.x());
}
if (s.x()>bb_max.x())
{
// clip e to xMax.
s = s+(e-s)*(bb_max.x()-s.x())/(e.x()-s.x());
}
}
// compate s and e against the yMin to yMax range of bb.
if (s.y()<=e.y())
{
// trivial reject of segment wholely outside.
if (e.y()<bb_min.y()) return false;
if (s.y()>bb_max.y()) return false;
if (s.y()<bb_min.y())
{
// clip s to yMin.
s = s+(e-s)*(bb_min.y()-s.y())/(e.y()-s.y());
}
if (e.y()>bb_max.y())
{
// clip e to yMax.
e = s+(e-s)*(bb_max.y()-s.y())/(e.y()-s.y());
}
}
else
{
if (s.y()<bb_min.y()) return false;
if (e.y()>bb_max.y()) return false;
if (e.y()<bb_min.y())
{
// clip s to yMin.
e = s+(e-s)*(bb_min.y()-s.y())/(e.y()-s.y());
}
if (s.y()>bb_max.y())
{
// clip e to yMax.
s = s+(e-s)*(bb_max.y()-s.y())/(e.y()-s.y());
}
}
// compate s and e against the zMin to zMax range of bb.
if (s.z()<=e.z())
{
// trivial reject of segment wholely outside.
if (e.z()<bb_min.z()) return false;
if (s.z()>bb_max.z()) return false;
if (s.z()<bb_min.z())
{
// clip s to zMin.
s = s+(e-s)*(bb_min.z()-s.z())/(e.z()-s.z());
}
if (e.z()>bb_max.z())
{
// clip e to zMax.
e = s+(e-s)*(bb_max.z()-s.z())/(e.z()-s.z());
}
}
else
{
if (s.z()<bb_min.z()) return false;
if (e.z()>bb_max.z()) return false;
if (e.z()<bb_min.z())
{
// clip s to zMin.
e = s+(e-s)*(bb_min.z()-s.z())/(e.z()-s.z());
}
if (s.z()>bb_max.z())
{
// clip e to zMax.
s = s+(e-s)*(bb_max.z()-s.z())/(e.z()-s.z());
}
}
// osg::notify(osg::NOTICE)<<"clampped segment "<<s<<" "<<e<<std::endl;
// if (s==e) return false;
return true;
}