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());
}
void KeyPadEventHandler::setByMatrix(const osg::Matrixd& matrix)
{
_pos = matrix.getTrans();
_ori = getHPRfromQuat(matrix.getRotate());
}