void MinimalShadowMap::ViewData::clampProjection
( osg::Matrixd & projection, float new_near, float new_far )
{
double r, l, t, b, n, f;
bool perspective = projection.getFrustum( l, r, b, t, n, f );
if( !perspective && !projection.getOrtho( l, r, b, t, n, f ) )
{
// What to do here ?
osg::notify( osg::WARN )
<< "MinimalShadowMap::clampProjectionFarPlane failed - non standard matrix"
<< std::endl;
} else if( n < new_near || new_far < f ) {
if( n < new_near && new_near < f ) {
if( perspective ) {
l *= new_near / n;
r *= new_near / n;
b *= new_near / n;
t *= new_near / n;
}
n = new_near;
}
if( n < new_far && new_far < f ) {
f = new_far;
}
if( perspective )
projection.makeFrustum( l, r, b, t, n, f );
else
projection.makeOrtho( l, r, b, t, n, f );
}
}
void ViroManipulator::setByMatrix(const osg::Matrixd& matrix){
this->Disable(GRAVITY);
_speed = 0.0;
_padEvent = NAVPAD_NONE;
_vEye = matrix.getTrans();
osg::Quat q = matrix.getRotate();
double qx,qy,qz,qw;
qx = q.x();
qy = q.y();
qz = q.z();
qw = q.w();
_yaw = atan2( 2*qy*qw-2*qx*qz , 1 - 2*qy*qy - 2*qz*qz);
_pitch = asin(2*qx*qy + 2*qz*qw);
_roll = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx*qz - 2*qz*qz);
osg::Quat rot = osg::Quat( (_pitch+osg::PI_2), osg::X_AXIS) * osg::Quat(0.0/*(_roll) */, osg::Y_AXIS) * osg::Quat( (_yaw), osg::Z_AXIS);
_qRotation = rot;
//matrix.decompose(_vEye, Q, vScale, q);
//osg::Matrixd M,Ms;
//M = matrix;
//M.makeScale(-vScale);
//_qRotation = M.getRotate();
/*
osg::Vec3d vScale,E,T,U;
osg::Quat Q;
matrix.getLookAt(E, T, U );
matrix.decompose(_vEye, _qRotation, vScale, Q);
_vEye = matrix.getTrans();
//osg::Vec3d vLook = -osg::Z_AXIS * matrix;
//vLook = matrix.getScale() ^ (-vLook);
//_vTarget = vLook + _vEye;
//_vUp = osg::Z_AXIS;
//matrix.getScale();
//matrix.get( _qRotation );
//matrix.getLookAt( _vEye, _vTarget, _vUp );
//computePosition( _vEye, _vTarget, osg::Z_AXIS );
//_qRotation.set( matrix );
//SyncData( matrix.getRotate() );
//SyncData(_qRotation);
computePosition( _vEye, T, U );
SyncData();
//computePosition( matrix.getTrans(), (matrix.getTrans()+matrix.get), osg::Z_AXIS );
*/
SyncData();
}
void Math::fromMatrix(const osg::Matrixd& mx, double& x, double& y, double& z, double& h, double& p, double& r)
{
x = mx.getTrans().x();
y = mx.getTrans().y();
z = mx.getTrans().z();
osg::Vec3 hpr = fromQuat(mx.getRotate());
h = osg::RadiansToDegrees(hpr.x());
p = osg::RadiansToDegrees(hpr.y())-90.0;
r = osg::RadiansToDegrees(hpr.z());
}
void ViewingCore::getZNearZFarProj(double &zNear, double &zFar, osg::Matrixd &projMat)
{
projMat = computeProjection();
if( getOrtho() ) {
double l, r, b, t;
projMat.getOrtho( l, r, b, t, zNear, zFar );
} else {
double fovy, aspect;
projMat.getPerspective( fovy, aspect, zNear, zFar );
}
}
void MxCore::updateFovy( osg::Matrixd& proj ) const
{
if( _ortho )
{
osg::notify( osg::WARN ) << "MxCore::updateFovy: Ortho is not yet implemented. TBD." << std::endl;
}
else
{
double left, right, bottom, top, near, far;
proj.getFrustum( left, right, bottom, top, near, far );
const double fovBottom = atan( bottom / near );
const double fovTop = atan( top / near );
const double fovyRatio = getFovyRadians() /
( osg::absolute< double >( fovBottom ) + osg::absolute< double >( fovTop ) );
const double newBottom = tan( fovBottom * fovyRatio ) * near;
const double newTop = tan( fovTop * fovyRatio ) * near;
const double xScale = newTop / top;
left *= xScale;
right *= xScale;
proj = osg::Matrixd::frustum( left, right, newBottom, newTop, near, far );
}
}
bool Uniform::getElement( unsigned int index, osg::Matrixd& m4 ) const
{
if( index>=getNumElements() || !isCompatibleType(FLOAT_MAT4) ) return false;
unsigned int j = index * getTypeNumComponents(getType());
m4.set( &((*_floatArray)[j]) );
return true;
}
bool Uniform::setElement( unsigned int index, const osg::Matrixd& m4 )
{
if( index>=getNumElements() || !isCompatibleType(FLOAT_MAT4) ) return false;
unsigned int j = index * getTypeNumComponents(getType());
const Matrixd::value_type* p = m4.ptr();
for( int i = 0; i < 16; ++i ) (*_floatArray)[j+i] = static_cast<float>(p[i]);
dirty();
return true;
}
bool
SpatialReference::createWorldToLocal(const osg::Vec3d& xyz, osg::Matrixd& out_world2local ) const
{
osg::Matrixd local2world;
if ( !createLocalToWorld(xyz, local2world) )
return false;
out_world2local.invert(local2world);
return true;
}
/** Set the position of the manipulator using a 4x4 matrix.*/
void OrbitManipulator::setByMatrix( const osg::Matrixd& matrix )
{
_center = osg::Vec3d( 0., 0., -_distance ) * matrix;
_rotation = matrix.getRotate();
// fix current rotation
if( getVerticalAxisFixed() )
fixVerticalAxis( _center, _rotation, true );
}
void ComputeBoundingBoxByRotation(osg::BoundingBox & InOut,osg::Vec3 BBCenter,float BBSize,osg::Matrixd Rotation)
{
osg::BoundingBox bb;
bb.set(BBCenter.x()-BBSize,BBCenter.y()-BBSize,BBCenter.z()-BBSize,BBCenter.x()+BBSize,BBCenter.y()+BBSize,BBCenter.z()+BBSize);
osg::BoundingBox Tbb;
for(unsigned int i=0;i<8;i++)
{
Tbb.expandBy(Rotation.preMult(bb.corner(i)));
}
InOut.set(Tbb.xMin(),Tbb.yMin(),Tbb.zMin(),Tbb.xMax(),Tbb.yMax(),Tbb.zMax());
}
void MinimalShadowMap::ViewData::extendProjection
( osg::Matrixd & projection, osg::Viewport * viewport, const osg::Vec2& margin )
{
double l,r,b,t,n,f;
//osg::Matrix projection = camera.getProjectionMatrix();
bool frustum = projection.getFrustum( l,r,b,t,n,f );
if( !frustum && !projection.getOrtho( l,r,b,t,n,f ) ) {
osg::notify( osg::WARN )
<< " Awkward projection matrix. ComputeExtendedProjection failed"
<< std::endl;
return;
}
osg::Matrix window = viewport->computeWindowMatrix();
osg::Vec3 vMin( viewport->x() - margin.x(),
viewport->y() - margin.y(),
0.0 );
osg::Vec3 vMax( viewport->width() + margin.x() * 2 + vMin.x(),
viewport->height() + margin.y() * 2 + vMin.y(),
0.0 );
osg::Matrix inversePW = osg::Matrix::inverse( projection * window );
vMin = vMin * inversePW;
vMax = vMax * inversePW;
l = vMin.x();
r = vMax.x();
b = vMin.y();
t = vMax.y();
if( frustum )
projection.makeFrustum( l,r,b,t,n,f );
else
projection.makeOrtho( l,r,b,t,n,f );
}
void
FeaturesToNodeFilter::computeLocalizers( const FilterContext& context, const osgEarth::GeoExtent &extent, osg::Matrixd &out_w2l, osg::Matrixd &out_l2w )
{
if ( context.isGeoreferenced() )
{
if ( context.getSession()->getMapInfo().isGeocentric() )
{
const SpatialReference* geogSRS = context.profile()->getSRS()->getGeographicSRS();
GeoExtent geodExtent = extent.transform( geogSRS );
if ( geodExtent.width() < 180.0 )
{
osg::Vec3d centroid, centroidECEF;
geodExtent.getCentroid( centroid.x(), centroid.y() );
geogSRS->transform( centroid, geogSRS->getECEF(), centroidECEF );
geogSRS->getECEF()->createLocalToWorld( centroidECEF, out_l2w );
out_w2l.invert( out_l2w );
}
}
else // projected
{
if ( extent.isValid() )
{
osg::Vec3d centroid;
extent.getCentroid(centroid.x(), centroid.y());
extent.getSRS()->transform(
centroid,
context.getSession()->getMapInfo().getProfile()->getSRS(),
centroid );
out_w2l.makeTranslate( -centroid );
out_l2w.invert( out_w2l );
}
}
}
}
OGR_SpatialReference::OGR_SpatialReference(void* _handle,
bool _delete_handle,
const osg::Matrixd& _ref_frame )
{
handle = _handle;
owns_handle = _delete_handle;
ref_frame = _ref_frame;
inv_ref_frame = _ref_frame.isIdentity()? _ref_frame : osg::Matrixd::inverse( _ref_frame );
getWKT();
// intialize the basis ellipsoid
OGR_SCOPE_LOCK();
int err;
double semi_major_axis = OSRGetSemiMajor( handle, &err );
double semi_minor_axis = OSRGetSemiMinor( handle, &err );
ellipsoid = Ellipsoid( semi_major_axis, semi_minor_axis );
is_geographic = OSRIsGeographic( handle ) != 0;
is_projected = OSRIsProjected( handle ) != 0;
}
void MinimalShadowMap::ViewData::trimProjection
( osg::Matrixd & projectionMatrix, osg::BoundingBox bb, unsigned int trimMask )
{
#if 1
if( !bb.valid() || !( trimMask & (1|2|4|8|16|32) ) ) return;
double l = -1, r = 1, b = -1, t = 1, n = 1, f = -1;
#if 0
// make sure bounding box does not extend beyond unit frustum clip range
for( int i = 0; i < 3; i ++ ) {
if( bb._min[i] < -1 ) bb._min[i] = -1;
if( bb._max[i] > 1 ) bb._max[i] = 1;
}
#endif
if( trimMask & 1 ) l = bb._min[0];
if( trimMask & 2 ) r = bb._max[0];
if( trimMask & 4 ) b = bb._min[1];
if( trimMask & 8 ) t = bb._max[1];
if( trimMask & 16 ) n = -bb._min[2];
if( trimMask & 32 ) f = -bb._max[2];
projectionMatrix.postMult( osg::Matrix::ortho( l,r,b,t,n,f ) );
#else
if( !bb.valid() || !( trimMask & (1|2|4|8|16|32) ) ) return;
double l, r, t, b, n, f;
bool ortho = projectionMatrix.getOrtho( l, r, b, t, n, f );
if( !ortho && !projectionMatrix.getFrustum( l, r, b, t, n, f ) )
return; // rotated or skewed or other crooked projection - give up
// make sure bounding box does not extend beyond unit frustum clip range
for( int i = 0; i < 3; i ++ ) {
if( bb._min[i] < -1 ) bb._min[i] = -1;
if( bb._max[i] > 1 ) bb._max[i] = 1;
}
osg::Matrix projectionToView = osg::Matrix::inverse( projectionMatrix );
osg::Vec3 min =
osg::Vec3( bb._min[0], bb._min[1], bb._min[2] ) * projectionToView;
osg::Vec3 max =
osg::Vec3( bb._max[0], bb._max[1], bb._max[2] ) * projectionToView;
if( trimMask & 16 ) { // trim near
if( !ortho ) { // recalc frustum corners on new near plane
l *= -min[2] / n;
r *= -min[2] / n;
b *= -min[2] / n;
t *= -min[2] / n;
}
n = -min[2];
}
if( trimMask & 32 ) // trim far
f = -max[2];
if( !ortho ) {
min[0] *= -n / min[2];
min[1] *= -n / min[2];
max[0] *= -n / max[2];
max[1] *= -n / max[2];
}
if( l < r ) { // check for inverted X range
if( l < min[0] && ( trimMask & 1 ) ) l = min[0];
if( r > max[0] && ( trimMask & 2 ) ) r = max[0];
} else {
if( l > min[0] && ( trimMask & 1 ) ) l = min[0];
if( r < max[0] && ( trimMask & 2 ) ) r = max[0];
}
if( b < t ) { // check for inverted Y range
if( b < min[1] && ( trimMask & 4 ) ) b = min[1];
if( t > max[1] && ( trimMask & 8 ) ) t = max[1];
} else {
if( b > min[1] && ( trimMask & 4 ) ) b = min[1];
if( t < max[1] && ( trimMask & 8 ) ) t = max[1];
}
if( ortho )
projectionMatrix.makeOrtho( l, r, b, t, n, f );
else
projectionMatrix.makeFrustum( l, r, b, t, n, f );
#endif
}
virtual void apply(const osg::Matrixd& value) { for(unsigned int i=0; i<16; ++i) _stream << (value.ptr())[i]; }
void KeyPadCameraManipulator::setByMatrix(const osg::Matrixd& matrix)
{
_pos = matrix.getTrans();
_ori = getHPRfromQuat(matrix.getRotate());
}
void GliderManipulator::setByMatrix(const osg::Matrixd& matrix)
{
_eye = matrix.getTrans();
_rotation = matrix.getRotate();
_distance = 1.0f;
}
void NodeTrackerManipulator::setByMatrix(const osg::Matrixd& matrix)
{
osg::Vec3d eye,center,up;
matrix.getLookAt(eye,center,up,_distance);
computePosition(eye,center,up);
}
void Vwr::CameraManipulator::setByMatrix(const osg::Matrixd& matrix)
{
_center = osg::Vec3(0.0f,0.0f,-_distance)*matrix;
_rotation = matrix.getRotate();
}
osg::Matrixf::Matrixf( const osg::Matrixd& mat )
{
set(mat.ptr());
}
void CSulManipulatorCamera::setByMatrix( const osg::Matrixd& matrix )
{
m_c = osg::Vec3(0.0f,0.0f,-m_d)*matrix;
m_q = matrix.getRotate();
}
void osg::Matrixf::set(const osg::Matrixd& rhs)
{
set(rhs.ptr());
}
void DriveManipulator::setByMatrix(const osg::Matrixd& matrix)
{
_eye = matrix.getTrans();
_rotation = matrix.getRotate();
}
void TerrainManipulator::setByMatrix(const osg::Matrixd& matrix)
{
osg::Vec3d lookVector(- matrix(2,0),-matrix(2,1),-matrix(2,2));
osg::Vec3d eye(matrix(3,0),matrix(3,1),matrix(3,2));
OSG_NOTIFY(INFO)<<"eye point "<<eye<<std::endl;
OSG_NOTIFY(INFO)<<"lookVector "<<lookVector<<std::endl;
if (!_node)
{
_center = eye+ lookVector;
_distance = lookVector.length();
_rotation = matrix.getRotate();
return;
}
// need to reintersect with the terrain
const osg::BoundingSphere& bs = _node->getBound();
float distance = (eye-bs.center()).length() + _node->getBound().radius();
osg::Vec3d start_segment = eye;
osg::Vec3d end_segment = eye + lookVector*distance;
osg::Vec3d ip;
bool hitFound = false;
if (intersect(start_segment, end_segment, ip))
{
notify(INFO) << "Hit terrain ok A"<< std::endl;
_center = ip;
_distance = (eye-ip).length();
osg::Matrixd rotation_matrix = osg::Matrixd::translate(0.0,0.0,-_distance)*
matrix*
osg::Matrixd::translate(-_center);
_rotation = rotation_matrix.getRotate();
hitFound = true;
}
if (!hitFound)
{
CoordinateFrame eyePointCoordFrame = getCoordinateFrame( eye );
if (intersect(eye+getUpVector(eyePointCoordFrame)*distance,
eye-getUpVector(eyePointCoordFrame)*distance,
ip))
{
_center = ip;
_distance = (eye-ip).length();
_rotation.set(0,0,0,1);
hitFound = true;
}
}
CoordinateFrame coordinateFrame = getCoordinateFrame(_center);
_previousUp = getUpVector(coordinateFrame);
clampOrientation();
}
/** Set the position of the manipulator using a 4x4 matrix.*/
void OrbitCameraManipulator::setByMatrix( const osg::Matrixd& matrix )
{
matrix.getLookAt( m_eye, m_lookat, m_up );
}
void KeyPadEventHandler::setByMatrix(const osg::Matrixd& matrix)
{
_pos = matrix.getTrans();
_ori = getHPRfromQuat(matrix.getRotate());
}
void TopView::setByMatrix(const osg::Matrixd& matrix)
{
eye = matrix.getTrans();
}
void HumanController::setByInverseMatrix(const osg::Matrixd& matrix)
{
_eye = -matrix.getTrans();
}