void ComputeOrientation(const osg::Vec3& lv,const osg::Vec3& up, osg::Quat &rotationDest)
{
osg::Vec3 f(lv);
f.normalize();
osg::Vec3 s(f^up);
s.normalize();
osg::Vec3 u(s^f);
u.normalize();
{
__asm NOP
}
osg::Matrix rotation_matrix(s[0], u[0], -f[0], 0.0f,
s[1], u[1], -f[1], 0.0f,
s[2], u[2], -f[2], 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
rotationDest.set(rotation_matrix);
//debug_matrix = rotation_matrix;
//debug_quat = rotationDest;
rotationDest = rotationDest.inverse();
} // ComputeOrientation
unsigned int PhysicsEngine::addUserVehicle(const osg::Vec3f& pos, const osg::Vec3f& sizes, const osg::Quat& orient, float mass)
{
assert(OpenThreads::Thread::CurrentThread() == m_physicsThread);
if (! m_vehicleShape)
m_vehicleShape = new btBoxShape(btVector3(sizes.x() / 2, sizes.y() / 2, sizes.z() / 2));
//m_collisionShapes.push_back(m_vehicleShape);
btTransform startTransform;
startTransform.setIdentity();
startTransform.setRotation(btQuaternion(orient.x(), orient.y(), orient.z(), orient.w()));
startTransform.setOrigin(btVector3(pos.x(), pos.y(), pos.z()));
VehicleMotionState* motionState = new VehicleMotionState(this, m_nextUsedId, startTransform);
btVector3 inertia;
m_vehicleShape->calculateLocalInertia(mass, inertia);
btRigidBody* body = new btRigidBody(mass, motionState, m_vehicleShape, inertia);
m_vehicles[m_nextUsedId] = body;
++m_nextUsedId;
body->setDamping(0.5f, 0.7f);
m_physicsWorld->addRigidBody(body, COLLISION_SHIPGROUP, COLLISION_SHIPGROUP | COLLISION_ASTEROIDGROUP);
return m_nextUsedId - 1;
}
// ================================================
// updateMotionTracker
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
void PluginMotionTrackerRecvr::updateMotionTracker( int trackerID, int viewID,
osg::Quat orientation )
{
if( trackerParamsMap == NULL )
return;
TrackerParams *trackerParams = (*trackerParamsMap)[trackerID];
if( trackerParams == NULL )
{
// The tracker ID specified doesn't yet have an entry in
// trackerParamsMap. It will be created and added to the map.
// The default values set by TrackerParams constructor should be good
// enough.
(*trackerParamsMap)[trackerID] = trackerParams = new TrackerParams;
}
// this tracker plugin generates quaternions
trackerParams->useQuaternion = true;
trackerParams->trackerRotateQuat[0] = orientation.x();
trackerParams->trackerRotateQuat[1] = orientation.y();
trackerParams->trackerRotateQuat[2] = orientation.z();
trackerParams->trackerRotateQuat[3] = orientation.w();
trackerParams->trackerAbsoluteYaw = 0.0;
trackerParams->trackerOffset[0] = 0.0;
trackerParams->trackerOffset[1] = 0.0;
trackerParams->trackerOffset[2] = 0.0;
}
osg::Vec3d Math::fromQuat(const osg::Quat& quat)
{
// From: http://guardian.curtin.edu.au/cga/faq/angles.html
// Except OSG exchanges pitch & roll from what is listed on that page
double qx = quat.x();
double qy = quat.y();
double qz = quat.z();
double qw = quat.w();
double sqx = qx * qx;
double sqy = qy * qy;
double sqz = qz * qz;
double sqw = qw * qw;
double term1 = 2 * (qx*qy + qw*qz);
double term2 = sqw + sqx - sqy - sqz;
double term3 = -2 * (qx*qz - qw*qy);
double term4 = 2 * (qw*qx + qy*qz);
double term5 = sqw - sqx - sqy + sqz;
double heading = atan2(term1, term2);
double pitch = atan2(term4, term5);
double roll = asin(term3);
return osg::Vec3d(heading, pitch, roll);
}
void DataOutputStream::writeQuat(const osg::Quat& q){
writeFloat(q.x());
writeFloat(q.y());
writeFloat(q.z());
writeFloat(q.w());
if (_verboseOutput) std::cout<<"read/writeQuat() ["<<q<<"]"<<std::endl;
}
void daeWriter::writeUpdateTransformElements(const osg::Vec3 &pos, const osg::Quat &q, const osg::Vec3 &s)
{
// Make a scale place element
domScale *scale = daeSafeCast< domScale >( currentNode->add( COLLADA_ELEMENT_SCALE ) );
scale->setSid("scale");
scale->getValue().append3( s.x(), s.y(), s.z() );
// Make a three rotate place elements for the euler angles
// TODO decompose quaternion into three euler angles
osg::Quat::value_type angle;
osg::Vec3 axis;
q.getRotate( angle, axis );
domRotate *rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
rot->setSid("rotateZ");
rot->getValue().append4( 0, 0, 1, osg::RadiansToDegrees(angle) );
rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
rot->setSid("rotateY");
rot->getValue().append4( 0, 1, 0, osg::RadiansToDegrees(angle) );
rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
rot->setSid("rotateX");
rot->getValue().append4( 1, 0, 0, osg::RadiansToDegrees(angle) );
// Make a translate place element
domTranslate *trans = daeSafeCast< domTranslate >( currentNode->add( COLLADA_ELEMENT_TRANSLATE ) );
trans->setSid("translate");
trans->getValue().append3( pos.x(), pos.y(), pos.z() );
}
bool FrameManager::getTransform(const std::string& frame, ros::Time time, osg::Vec3d& position, osg::Quat& orientation)
{
boost::mutex::scoped_lock lock(cache_mutex_);
position = osg::Vec3d(9999999, 9999999, 9999999);
orientation.set(0,0,0,1);
if (fixed_frame_.empty())
{
return false;
}
M_Cache::iterator it = cache_.find(CacheKey(frame, time));
if (it != cache_.end())
{
position = it->second.position;
orientation = it->second.orientation;
return true;
}
geometry_msgs::Pose pose;
pose.orientation.w = 1.0f;
if (!transform(frame, time, pose, position, orientation))
{
return false;
}
cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation)));
return true;
}
/* Public functions */
void OculusHealthAndSafetyWarning::updatePosition(osg::Matrix cameraMatrix, osg::Vec3 position, osg::Quat orientation) {
if (m_transform.valid()) {
osg::Matrix matrix;
matrix.setTrans(osg::Vec3(0.0, 0.0, -m_distance));
matrix.postMultRotate(orientation.inverse());
matrix.postMultTranslate(-position);
m_transform->setMatrix(matrix*cameraMatrix);
}
}
static void invertOSGTransform(osg::Vec3d& trans, osg::Quat& quat,
osg::PositionAttitudeTransform*& source, osg::PositionAttitudeTransform*& target,
std::string& source_frame, std::string& target_frame)
{
quat = quat.inverse();
trans = -(quat * trans);
std::swap(source, target);
std::swap(source_frame, target_frame);
}
void vehiclePoseCallback(const nav_msgs::Odometry& odom) {
if (!firstpass) {
initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
wMv_initial.setTrans(initialT);
wMv_initial.setRotate(initialQ);
firstpass=true;
}
}
osg::Quat lerp_Quat(osg::Quat v0, osg::Quat v1, double t)
{
osg::Quat vec;
vec.x() = lerp_lf(v0.x(), v1.x(), t);
vec.y() = lerp_lf(v0.y(), v1.y(), t);
vec.z() = lerp_lf(v0.z(), v1.z(), t);
vec.w() = lerp_lf(v0.w(), v1.w(), t);
return vec;
}
inline void copyOsgQuatToLib3dsQuat(float lib3ds_vector[4], const osg::Quat& osg_quat)
{
//lib3ds_vector[0] = osg_quat[3]; // Not sure
//lib3ds_vector[1] = osg_quat[0];
//lib3ds_vector[2] = osg_quat[1];
//lib3ds_vector[3] = osg_quat[2];
// 3DS seems to store (angle in radians, axis_x, axis_y, axis_z), but it works with (axis_x, axis_y, axis_z, -angle in radians)!
osg::Quat::value_type angle, x, y, z;
osg_quat.getRotate(angle, x, y, z);
lib3ds_vector[0] = static_cast<float>(x);
lib3ds_vector[1] = static_cast<float>(y);
lib3ds_vector[2] = static_cast<float>(z);
lib3ds_vector[3] = static_cast<float>(-angle);
}
void Node::quat_to_euler(osg::Quat& q, osg::Vec3& euler) {
//To calculate back the euler angles from a quaternion we use this formulas
//but since we want the postmultiplication order we use the conjugate of the
//quaternion, also there has to be a change in the sign of the angles
//http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
osg::Quat q_c = q.conj();
euler[0] = -std::atan2(2.0 * (q_c.y() * q_c.z() + q_c.w() * q_c.x()),
q_c.w() * q_c.w() - q_c.x() * q_c.x() - q_c.y() * q_c.y()
+ q_c.z() * q_c.z());
euler[1] = -std::asin(-2.0 * (q_c.x() * q_c.z() - q_c.w() * q_c.y()));
euler[2] = -std::atan2(2.0 * (q_c.x() * q_c.y() + q_c.w() * q_c.z()),
q_c.w() * q_c.w() + q_c.x() * q_c.x() - q_c.y() * q_c.y()
- q_c.z() * q_c.z());
}
osg::PositionAttitudeTransform* creation_CHARRR(float posx, float posy, osg::Node* terrain){
osg::Node* LECHARRR = osgDB::readNodeFile("t72-tank_des.flt");
osg::PositionAttitudeTransform* pos_tank = new osg::PositionAttitudeTransform;
osg::Vec3 pos, normal;
intersection_terrain(posx, posy, terrain, pos, normal);
pos_tank->setPosition(pos);
osg::Quat rotation;
rotation.makeRotate(osg::Vec3f(0, 0, 1), normal);
pos_tank->setAttitude(rotation);
pos_tank->addChild(LECHARRR);
pos_tank->setUpdateCallback(new Deplacement);
return pos_tank;
}
bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, osg::Vec3d& position, osg::Quat& orientation)
{
position.set(0,0,0);
orientation.set(0,0,0,1);
// put all pose data into a tf stamped pose
btQuaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
btVector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);
if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
{
bt_orientation.setW(1.0);
}
tf::Stamped<tf::Pose> pose_in(btTransform(bt_orientation,bt_position), time, frame);
tf::Stamped<tf::Pose> pose_out;
// convert pose into new frame
try
{
tf_->transformPose( fixed_frame_, pose_in, pose_out );
}
catch(tf::TransformException& e)
{
ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
return false;
}
#if ROS_VERSION_MINIMUM(1, 8, 0)
//ROS Fuerte version
bt_position = pose_out.asBt().getOrigin();
bt_orientation = pose_out.asBt().getRotation();
#else
bt_position = pose_out.getOrigin();
bt_orientation = pose_out.getRotation();
#endif
position = osg::Vec3d(bt_position.x(), bt_position.y(), bt_position.z());
orientation = osg::Quat( bt_orientation.x(), bt_orientation.y(), bt_orientation.z(), bt_orientation.w() );
return true;
}
void FlexDrawable::ConvexManager::add( const osg::Vec3& pos, const osg::Quat& q,
const std::vector<osg::Plane>& faces,
const osg::BoundingBox& bb, bool isStatic )
{
unsigned int startIndex = starts.size();
for ( unsigned int i=0; i<faces.size(); ++i )
{
const osg::Plane& plane = faces[i];
planes.push_back( osg::Vec4f(plane[0], plane[1], plane[2], plane[3]) );
}
starts.push_back( startIndex );
lengths.push_back( faces.size() );
flags.push_back( isStatic ? 0 : 1 );
positions.push_back( osg::Vec4(pos, 0.0f) );
rotations.push_back( q.asVec4() );
prevPositions.push_back( positions.back() );
prevRotations.push_back( rotations.back() );
aabbMin.push_back( osg::Vec4(bb._min, 0.0f) );
aabbMax.push_back( osg::Vec4(bb._max, 0.0f) );
}
int InterpretCameraForUse(NVAnimObject *Cam, double Moment, osg::Vec3 &EyePoint, osg::Quat &Orientation, float &HFOV)
{
int NumValid;
float HFOV_S, HFOV_N, HFOV_E;
NumValid = Cam->GetBracketingKeyFrames(Moment, PrevKey, NextKey, false); // don't use caching yet, we're not set up for it
if(NumValid)
{
if(NumValid == 2)
{ // interpolate
double KeyTimeDif, MomentTimeDif;
//determine interpolation fraction
KeyTimeDif = NextKey->GetTimeValue() - PrevKey->GetTimeValue();
MomentTimeDif = Moment - PrevKey->GetTimeValue();
if(KeyTimeDif > 0.0 && MomentTimeDif >= 0.0) // do rationality checking before a risky divide
{
double MomentFrac;
osg::Vec3 EyePoint_S, EyePoint_E;
osg::Quat Orientation_S, Orientation_E;
MomentFrac = MomentTimeDif / KeyTimeDif; // divide by zero prevented by rationality checking above
MiscReadout = MomentFrac;
if(MomentFrac > 1.0)
{
Cam->GetBracketingKeyFrames(Moment, PrevKey, NextKey, false); // trace only
} // if
// HFOV
HFOV_S = PrevKey->ChannelValues[NVAnimObject::CANONHANGLE];
HFOV_E = NextKey->ChannelValues[NVAnimObject::CANONHANGLE];
HFOV_N = flerp(MomentFrac, HFOV_S, HFOV_E);
HFOV = HFOV_N;
// create start and end eye/orient pairs
InterpretCameraConfiguration(PrevKey, EyePoint_S, Orientation_S);
InterpretCameraConfiguration(NextKey, EyePoint_E, Orientation_E);
// Eye position
EyePoint = EyePoint_S + ((EyePoint_E - EyePoint_S) * MomentFrac);
// Orientation quat via slerp
Orientation.slerp(MomentFrac, Orientation_S, Orientation_E);
return(1);
} // if
else
{ // drop back ten and punt by going with NumValid == 1 code below
NumValid = 1;
} // else
} // if
if(NumValid == 1) // not an else-if, this is a fail-safe case if the above NumValid=2 fails somehow
{ // only have one, only use PrevKey, NextKey is identical and tweening would be a waste of time
InterpretCameraConfiguration(PrevKey, EyePoint, Orientation);
HFOV = PrevKey->ChannelValues[NVAnimObject::CANONHANGLE];
} // else
} // if
return(0);
} // InterpretCameraForUse
static boost::python::tuple getRotate_7cd433cce0eabe855a9b774688d2845e( ::osg::Quat const & inst ){
double angle2;
osg::Vec3f vec2;
inst.getRotate(angle2, vec2);
return bp::make_tuple( angle2, vec2 );
}
bool GestionEvenements::handle( const osgGA::GUIEventAdapter& ea,
osgGA::GUIActionAdapter& aa)
{
switch(ea.getEventType()){
case osgGA::GUIEventAdapter::KEYDOWN :
switch(ea.getKey()){
case 'q':
noeudTourelle = rechercheTourelle.getNode();
if (noeudTourelle != NULL){
osgSim::DOFTransform* tourelleDOF = dynamic_cast<osgSim::DOFTransform*>(noeudTourelle);
tourelleDOF->setCurrentHPR(osg::Vec3(tourelleDOF->getCurrentHPR().x() + osg::DegreesToRadians(20.0), 0.0, 0.0));
}
break;
case 'd':
noeudTourelle = rechercheTourelle.getNode();
if (noeudTourelle != NULL){
osgSim::DOFTransform* tourelleDOF = dynamic_cast<osgSim::DOFTransform*>(noeudTourelle);
tourelleDOF->setCurrentHPR(osg::Vec3(tourelleDOF->getCurrentHPR().x() - osg::DegreesToRadians(20.0), 0.0, 0.0));
}
break;
case '8':
intersection_terrain(LECHARRR->getPosition().x(), LECHARRR->getPosition().y() + 3, terrain, posTank, normalTank);
LECHARRR->setPosition(posTank);
rotation.makeRotate(osg::Vec3f(0, 0, 1), normalTank);
LECHARRR->setAttitude(rotation);
fumeeTank->setPosition(LECHARRR->getPosition());
break;
case '2':
intersection_terrain(LECHARRR->getPosition().x(), LECHARRR->getPosition().y() - 3, terrain, posTank, normalTank);
LECHARRR->setPosition(posTank);
rotation.makeRotate(osg::Vec3f(0, 0, 1), normalTank);
LECHARRR->setAttitude(rotation);
fumeeTank->setPosition(LECHARRR->getPosition());
break;
case '4':
intersection_terrain(LECHARRR->getPosition().x() - 3, LECHARRR->getPosition().y(), terrain, posTank, normalTank);
LECHARRR->setPosition(posTank);
rotation.makeRotate(osg::Vec3f(0, 0, 1), normalTank);
LECHARRR->setAttitude(rotation);
fumeeTank->setPosition(LECHARRR->getPosition());
break;
case '6':
intersection_terrain(LECHARRR->getPosition().x() + 3, LECHARRR->getPosition().y(), terrain, posTank, normalTank);
LECHARRR->setPosition(posTank);
rotation.makeRotate(osg::Vec3f(0, 0, 1), normalTank);
LECHARRR->setAttitude(rotation);
fumeeTank->setPosition(LECHARRR->getPosition());
break;
case 'p':
LECHARRR->setScale(LECHARRR->getScale() + osg::Vec3(1.0, 1.0, 1.0));
LECHARRR->getOrCreateStateSet()->setMode(GL_NORMALIZE, osg::StateAttribute::ON);
fumeeTank->setScale(fumeeTank->getScale() + 5);
break;
case 'm':
LECHARRR->setScale(LECHARRR->getScale() - osg::Vec3(1.0, 1.0, 1.0));
LECHARRR->getOrCreateStateSet()->setMode(GL_NORMALIZE, osg::StateAttribute::ON);
fumeeTank->setScale(fumeeTank->getScale() - 5);
break;
case 'f':
scene->addChild(new osgParticle::ExplosionEffect(osg::Vec3(posCanonX, posCanonY, posCanonZ), 1.0f, 1.0f));
break;
}
break;
case osgGA::GUIEventAdapter::PUSH :{
int x = ea.getX();
int y = ea.getY();
if( ea.getButton() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
//std::cout << "bouton gauche" << std::endl;
if (ea.getButton() == osgGA::GUIEventAdapter::MIDDLE_MOUSE_BUTTON)
//std::cout << "bouton milieu" << std::endl;
if (ea.getButton() == osgGA::GUIEventAdapter::RIGHT_MOUSE_BUTTON)
//std::cout << "bouton droit" << std::endl;
break;
}
case osgGA::GUIEventAdapter::DOUBLECLICK :
break;
}
return false; // pour que l'événement soit traité par d'autres gestionnaires
}
void CamInfo::setCamRot(osg::Quat rot)
{
CamRot.set(rot.x(),rot.y(),rot.z(),rot.w());
}
void osg::av_pushMsg(av::Msg& netMsg, const ::osg::Quat& buf)
{
::osg::Vec4d vec = buf.asVec4();
av_pushMsg(netMsg, vec);
}
bool CameraFlight::buttonEvent(int type/*, const osg::Matrix & mat*/)
{
// osg::Matrix curMatrix = SceneManager::instance()->getObjectTransform()->getMatrix();
// double curScale = SceneManager::instance()->getObjectScale();
// osg::Matrix w2o = SceneManager::instance()->getWorldToObjectTransform();
// osg::Matrix o2w = SceneManager::instance()->getObjectToWorldTransform();
if(type == 'p') {
std::cerr<<"curMatrix"<<endl;
printMat(curMatrix, curScale);
cout<<"x = "<<latLonHeight.x()<<", y = "<<latLonHeight.y()<<", z = "<<latLonHeight.z()<<endl;
// std::cerr<<"WorldToObject"<<endl;
// printMat(w2o, curScale);
// std::cerr<<"ObjectToWorld"<<endl;
// printMat(o2w, curScale);
}
else if(type == 'd') {
curMatrix.decompose(trans1, rot1, scale1, so1);
std::cerr<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
cout<<"Trans = ";
printVec(trans1);
cout<<"Scale = ";
printVec(scale1);
cout<<"Rotate = ";
printQuat(rot1);
cout<<"Scale Orient =";
printQuat(so1);
std::cerr<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
//osg::Matrix _trans = osg::Matrix::rotate(oldPos,currentPos) * osg::Matrix::translate(trans);
//osg::Matrix rotMat = osg::Matrix::rotate(oldPos,currentPos);
//osg::Matrix wr = o2w * rotMat* w2o;
// osg::Matrix _tmp = w2o * osg::Matrix::rotate(oldPos,currentPos) * o2w;
//osg::Matrix _scale = osg::Matrix::scale(scale);
//osg::Matrix _temp = osg::Matrix::translate(-trans) * _scale * _trans;
}
else if(type == 't') {
curMatrix.setTrans(osg::Vec3(0.0,1e+09/*6.41844e+09*/,0));
SceneManager::instance()->setObjectMatrix(curMatrix);
}
else if(type == 's') {
_origMatrix = SceneManager::instance()->getObjectTransform()->getMatrix();
_origScale = SceneManager::instance()->getObjectScale();
}
else if(type == 'z') {
tstart = time(0);
// zIn = 1e+10;
// zOut = 1e+10;
cout<<"zpressed"<<endl;
if (flagZoom == false)
flagZoom = true;
}
else if(type == 'r') {
if (flagRot == false) {
flagRot = true;
}
else {
flagRot = false;
}
//cout<<"Old Matrix"<<endl;
//curMatrix = SceneManager::instance()->getObjectTransform()->getMatrix();
//curScale = SceneManager::instance()->getObjectScale();
// printMat(curMatrix, curScale);
/*osg::Matrix mat2 = */
osg::Matrix rotM;
rotM.makeRotate(DegreesToRadians(1.0),osg::Vec3(0,1,0));
// printMat(rotM, curScale);
curMatrix= o2w * rotM * w2o;
// printMat(curMatrix, curScale);
//curMatrix.setTrans(trans);
//cout<<"New Matrix"<<endl;
//printMat(curMatrix, curScale);
cout<<"x = "<<origPlanetPoint[0]<<", y = "<<origPlanetPoint[1]<<", z = "<<origPlanetPoint[2]<<endl;
osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();
objMat.decompose(trans1,rot1,scale1,so1);
_destMat1.decompose(trans2,rot2,scale2,so2);
// cout<<"rotate from"<<endl;
// printQuat(rot1);
// cout<<"rotate to"<<endl;
// printQuat(rot2);
osg::Vec3 vect1, vect2;
double ang1, ang2;
rot1.getRotate(ang1, vect1);
rot2.getRotate(ang2, vect2);
osg::Vec3 vec1 = rot1.asVec3();
osg::Vec3 vec2 = rot2.asVec3();
printVec(vect1);
printVec(vect2);
osg::Quat rotQuat;
rotQuat.makeRotate(vect1, vect2);
// printQuat(rotQuat);
rotM.makeRotate(rotQuat);
osg::Matrix mat = objMat* rotM;
mat.setTrans(trans1);
SceneManager::instance()->setObjectMatrix(mat);
}
else if(type == 'q') {
cout<<"Distance = "<<distanceToSurface<<endl;
}
else if(type == 'g') {
flagOut = false;
flagIn = false;
flagRot = false;
flagZoom = true;
osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();
/* osg::Matrix rotM8;
rotM8.makeRotate(DegreesToRadians(10.0),osg::Vec3(0,1,0));
osg::Matrix mat9 = rotM8 * objMat;
SceneManager::instance()->setObjectMatrix(mat9);
*/
objMat.decompose(trans2, rot2, scale2, so2);
double LFD, PD, FS, PS;
a = (maxHeight - trans2[1])/25.0;
t = 0.0;
total = 0.0;
}
else if(type == 'a') {
SceneManager::instance()->setObjectMatrix(_origMatrix);
}
else if(type == 'm') {
if(flagRot) {
flagRot = false;
}
else {
tstart = time(0);
zIn = 1e+10;
zOut = 1e+10;
osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();
osg::Vec3d tolatLon(0.573827, -2.04617,0);
osg::Vec3d tolatLon1(0.622566, 2.43884, 0);
osg::Vec3d toVec1, toVec2;
map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
tolatLon.x(),tolatLon.y(),tolatLon.z(),
toVec1.x(),toVec1.y(),toVec1.z());
// map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
// tolatLon1.x(),tolatLon1.y(),tolatLon1.z(),
// toVec2.x(),toVec2.y(),toVec2.z());
fromVec = origPlanetPoint;
toVec1.normalize();
// toVec2.normalize();
fromVec.normalize();
/*
osg::Vec3 offset(0.0,1.0,0.0);
offset = offset - fromVec;
fromVec = fromVec + offset;
toVec1 = toVec1 + offset;
toVec2 = toVec2 + offset;
printVec(fromVec);
printVec(toVec1);
printVec(toVec2);
*/
cout<<endl;
toVec = toVec1;
double dot = fromVec * toVec;
angle = acos(dot/((fromVec.length() * toVec.length())));
angle = RadiansToDegrees(angle);
rotAngle = angle/100.0;
cout<<angle<<endl;
flagRot = true;
}
// osg::Vec3 crsVec = toVec1^toVec2;
// crsVec.normalize();
// cout<<"Where you at"<<endl;
// printVec(fromVec);
// cout<<"UCSD"<<endl;
// printVec(toVec1);
// cout<<"Tokyo"<<endl;
// printVec(toVec2);
//osg::Vec3 rotVec = (fromVec ^ toVec);
//rotVec.normalize();
//origPlanetPoint.normalize();
//latLonHeight.normalize();
// printVec(crsVec);
// osg::Matrix rotM;
// rotM.makeRotate(DegreesToRadians(1.0),crsVec);
//printVec(origPlanetPoint);
// printVec(origPlanetPoint);
// printVec(latLonHeight);
// printMat(rotM, curScale);
//objMat.decompose(trans1,rot1,scale1,so1);
//osg::Vec3 vect1, vect2;
//double ang1, ang2;
//rot1.getRotate(ang1, vect1);
//printVec(vect1);
// osg::Matrix mat = objMat* rotM;
// mat.setTrans(trans1);
// SceneManager::instance()->setObjectMatrix(mat);
}
else if(type == 'x') {
cout<<"DRAWlING"<<endl;
osgEarth::MapNode* outMapNode = MapNode::findMapNode(SceneManager::instance()->getObjectsRoot());
outputMap = outMapNode->getMap();
double lat = 0.0;
double lon = -1.5708;
double hght = 0.0;
osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();
osg::Vec3d tolatLon(0.573827, -2.04617,0);
osg::Vec3d tolatLon1(0.622566, 2.43884, 0);
osg::Vec3d toVec1, toVec2;
map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
lat,lon,hght,
toVec1.x(),toVec1.y(),toVec1.z());
osg::Matrixd output, output2;
map->getProfile()->getSRS()->getEllipsoid()->computeLocalToWorldTransformFromXYZ(
toVec1.x(), toVec1.y(), toVec1.z(), output);
map->getProfile()->getSRS()->getEllipsoid()->computeLocalToWorldTransformFromXYZ(
lat, lon, hght, output2);
printMat(output,10.0);
printMat(output2,10.0);
osg::Geode * geode = new osg::Geode();
osg::Vec3 centerVec(0.0,0.0,-20000.0);
osg::ShapeDrawable* shape = new osg::ShapeDrawable(
new osg::Cylinder(centerVec,10000.0, 2000000.0));
geode->addDrawable(shape);
osg::MatrixTransform * mat1 = new osg::MatrixTransform();
mat1->setMatrix(output);
mat1->addChild(geode);
SceneManager::instance()->getObjectsRoot()->addChild(mat1);
map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
tolatLon.x(),tolatLon.y(),tolatLon.z(),
toVec2.x(),toVec2.y(),toVec2.z());
osg::Matrixd output1;
// lat = 0.573827;
// lon = -2.04617;
map->getProfile()->getSRS()->getEllipsoid()->computeLocalToWorldTransformFromXYZ(
toVec2.x(), toVec2.y(), toVec2.z(), output1);
osg::Geode * geode1 = new osg::Geode();
osg::Vec3 centerVec1(0.0,0.0,-20000.0);
osg::ShapeDrawable* shape1 = new osg::ShapeDrawable(
new osg::Cylinder(centerVec1,10000.0, 2000000.0));
geode1->addDrawable(shape1);
osg::MatrixTransform * mat2 = new osg::MatrixTransform();
mat2->setMatrix(output1);
mat2->addChild(geode1);
SceneManager::instance()->getObjectsRoot()->addChild(mat2);
//geode->addDrawable(shape1);
}
return false;
}
bool Node::equivalent(const osg::Quat& q0, const osg::Quat& q1) {
return osg::equivalent(q0.x(), q1.x(), 1e-4)
&& osg::equivalent(q0.y(), q1.y(), 1e-4)
&& osg::equivalent(q0.z(), q1.z(), 1e-4)
&& osg::equivalent(q0.w(), q1.w(), 1e-4);
}
void osg::av_popMsg(av::Msg& netMsg, ::osg::Quat& buf)
{
::osg::Vec4d vec;
av_popMsg(netMsg, vec);
buf.set(vec);
}
