C++ Matrixd::getRotate方法代码示例

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();
}

/** 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 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 Vwr::CameraManipulator::setByMatrix(const osg::Matrixd& matrix)
{
    _center = osg::Vec3(0.0f,0.0f,-_distance)*matrix;
    _rotation = matrix.getRotate();
}

void CSulManipulatorCamera::setByMatrix( const osg::Matrixd& matrix )          
{
    m_c = osg::Vec3(0.0f,0.0f,-m_d)*matrix;
    m_q = 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();
}

void DriveManipulator::setByMatrix(const osg::Matrixd& matrix)
{
    _eye = matrix.getTrans();
    _rotation = matrix.getRotate();
}

void GliderManipulator::setByMatrix(const osg::Matrixd& matrix)
{
    _eye = matrix.getTrans();
    _rotation = matrix.getRotate();
    _distance = 1.0f;
}

void KeyPadCameraManipulator::setByMatrix(const osg::Matrixd& matrix)
{
    _pos = matrix.getTrans();
    _ori = getHPRfromQuat(matrix.getRotate());

}