void MyServerThread::sendMatrix(Matrixd transNode)
{
float matrix1[16];
Vec3 objTrans = transNode.getTrans();
Quat objQuad = transNode.getRotate();
//cout << " Orientation " << objQuad.x() << "," << objQuad.y() << "," << objQuad.z() << "," << objQuad.w() << endl; // orientation
//cout << " Postion " << objTrans.x() << "," << objTrans.y() << "," << objTrans.z() << endl; // position
/*for (int i=0; i<4; ++i){
for (int j=0; j<4; ++j)
{
cout << transNode(i,j) << " " ; // position
}
cout << endl;
} */
cout << endl;
for (int i=0; i<4; ++i)
for (int j=0; j<4; ++j)
matrix[i*4+j] = transNode(i,j);
for(int i=0; i<16; i++)
{
cout <<"matrix:" << i << " " << matrix1[i] << endl;
}
//memcpy(buf,matrix1,sizeof(float)*16);
//sendMSG();
}
/** Set the position of the manipulator using a 4x4 matrix.*/
void FirstPersonManipulator::setByMatrix( const Matrixd& matrix )
{
// set variables
_eye = matrix.getTrans();
_rotation = matrix.getRotate();
// fix current rotation
if( getVerticalAxisFixed() )
fixVerticalAxis( _eye, _rotation, true );
}
void SceneGraphManipulator::computeNodeCenterAndRotation(Vec3d& nodeCenter, Quat& nodeRotation) const
{
Matrixd localToWorld;
computeNodeLocalToWorld(localToWorld);
if (validateNodePath())
nodeCenter = Vec3d(_trackNodePath.back()->getBound().center())*localToWorld;
else
nodeCenter = Vec3d(0.0f,0.0f,0.0f)*localToWorld;
// scale the matrix to get rid of any scales before we extract the rotation.
double sx = 1.0/sqrt(localToWorld(0,0)*localToWorld(0,0) + localToWorld(1,0)*localToWorld(1,0) + localToWorld(2,0)*localToWorld(2,0));
double sy = 1.0/sqrt(localToWorld(0,1)*localToWorld(0,1) + localToWorld(1,1)*localToWorld(1,1) + localToWorld(2,1)*localToWorld(2,1));
double sz = 1.0/sqrt(localToWorld(0,2)*localToWorld(0,2) + localToWorld(1,2)*localToWorld(1,2) + localToWorld(2,2)*localToWorld(2,2));
localToWorld = localToWorld*Matrixd::scale(sx,sy,sz);
nodeRotation = localToWorld.getRotate();
}
void Quat::set(const Matrixd& matrix)
{
*this = matrix.getRotate();
}
void TerrainManipulator::setByMatrix(const Matrixd& matrix)
{
Vec3d lookVector(- matrix(2,0),-matrix(2,1),-matrix(2,2));
Vec3d eye(matrix(3,0),matrix(3,1),matrix(3,2));
OSG_INFO<<"eye point "<<eye<<std::endl;
OSG_INFO<<"lookVector "<<lookVector<<std::endl;
if (!_node)
{
_center = eye+ lookVector;
_distance = lookVector.length();
_rotation = matrix.getRotate();
return;
}
// need to reintersect with the terrain
const BoundingSphere& bs = _node->getBound();
float distance = (eye-bs.center()).length() + _node->getBound().radius();
Vec3d start_segment = eye;
Vec3d end_segment = eye + lookVector*distance;
Vec3d ip;
bool hitFound = false;
if (intersect(start_segment, end_segment, ip))
{
OSG_INFO << "Hit terrain ok A"<< std::endl;
_center = ip;
_distance = (eye-ip).length();
Matrixd rotation_matrix = Matrixd::translate(0.0,0.0,-_distance)*
matrix*
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();
}
