diff --git a/include/osg/Quat b/include/osg/Quat index e9ef7cfe6..5ca3bfdff 100644 --- a/include/osg/Quat +++ b/include/osg/Quat @@ -84,6 +84,8 @@ class SG_EXPORT Quat makeRotate(angle1,axis1,angle2,axis2,angle3,axis3); } + inline Quat& operator = (const Quat& v) { _v[0]=v._v[0]; _v[1]=v._v[1]; _v[2]=v._v[2]; _v[3]=v._v[3]; return *this; } + inline bool operator == (const Quat& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1] && _v[2]==v._v[2] && _v[3]==v._v[3]; } inline bool operator != (const Quat& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1] || _v[2]!=v._v[2] || _v[3]!=v._v[3]; } diff --git a/include/osgGA/NodeTrackerManipulator b/include/osgGA/NodeTrackerManipulator index e53c575b6..59d9063a3 100644 --- a/include/osgGA/NodeTrackerManipulator +++ b/include/osgGA/NodeTrackerManipulator @@ -35,6 +35,8 @@ class OSGGA_EXPORT NodeTrackerManipulator : public MatrixManipulator enum TrackerMode { NODE_CENTER, + NODE_CENTER_AND_AZMIM_ROTATION, + NODE_CENTER_AND_AZMIM_ELEVATION_ROTATION, NODE_CENTER_AND_ROTATION, }; diff --git a/src/osg/Matrix_implementation.cpp b/src/osg/Matrix_implementation.cpp index 0bb5708a2..6f4546beb 100644 --- a/src/osg/Matrix_implementation.cpp +++ b/src/osg/Matrix_implementation.cpp @@ -62,8 +62,16 @@ void Matrix_implementation::set( value_type a00, value_type a01, value_type a02, #define QZ q._v[2] #define QW q._v[3] -void Matrix_implementation::set(const Quat& q) +void Matrix_implementation::set(const Quat& q_in) { + Quat q(q_in); + double length2 = q.length2(); + if (length2!=1.0) + { + // normalize quat if required. + q /= sqrt(length2); + } + // Source: Gamasutra, Rotating Objects Using Quaternions // //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm diff --git a/src/osgGA/NodeTrackerManipulator.cpp b/src/osgGA/NodeTrackerManipulator.cpp index 2fec5cb39..9e7c49b90 100644 --- a/src/osgGA/NodeTrackerManipulator.cpp +++ b/src/osgGA/NodeTrackerManipulator.cpp @@ -31,6 +31,9 @@ public: NodeTrackerManipulator::NodeTrackerManipulator() { _trackerMode = NODE_CENTER_AND_ROTATION; + _trackerMode = NODE_CENTER_AND_AZMIM_ROTATION; + _trackerMode = NODE_CENTER_AND_AZMIM_ELEVATION_ROTATION; + _rotationMode = ELEVATION_AZIM; _distance = 1.0; @@ -361,7 +364,67 @@ void NodeTrackerManipulator::computeNodeCenterAndRotation(osg::Vec3d& nodeCenter 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*osg::Matrixd::scale(sx,sy,sz); - localToWorld.get(nodeRotation); + +/* + Euler Code not in correct coordinate frame, just an experiment here. + + // Assuming the angles are in radians. + double heading, attitude, bank = 0; + if (localToWorld(0,1) > 0.998) + { // singularity at north pole + heading = atan2(localToWorld(2,0),localToWorld(2,2)); + attitude = osg::PI_2; + bank = 0; + } + else if (localToWorld(0,1) < -0.998) + { // singularity at south pole + heading = atan2(localToWorld(2,0),localToWorld(2,2)); + attitude = -osg::PI_2; + bank = 0; + } + else + { + heading = atan2(-localToWorld(0,2),localToWorld(0,0)); + bank = atan2(-localToWorld(2,1),localToWorld(1,1)); + attitude = asin(localToWorld(0,1)); + } + + osg::notify(osg::NOTICE)<<"heading = "<