+// Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de
+//
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Library General Public
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
#ifndef SGQuat_H
#define SGQuat_H
+#ifdef min
+#undef min
+#endif
+
+#ifdef max
+#undef max
+#endif
+
+#include <osg/Quat>
+
+template<typename T>
+struct SGQuatStorage {
+ /// Readonly raw storage interface
+ const T (&data(void) const)[4]
+ { return _data; }
+ /// Readonly raw storage interface
+ T (&data(void))[4]
+ { return _data; }
+
+ void osg() const
+ { }
+
+private:
+ T _data[4];
+};
+
+template<>
+struct SGQuatStorage<double> : public osg::Quat {
+ /// Access raw data by index, the index is unchecked
+ const double (&data(void) const)[4]
+ { return osg::Quat::_v; }
+ /// Access raw data by index, the index is unchecked
+ double (&data(void))[4]
+ { return osg::Quat::_v; }
+
+ const osg::Quat& osg() const
+ { return *this; }
+ osg::Quat& osg()
+ { return *this; }
+};
+
/// 3D Vector Class
template<typename T>
-class SGQuat {
+class SGQuat : protected SGQuatStorage<T> {
public:
typedef T value_type;
/// uninitialized values in the debug build very fast ...
#ifndef NDEBUG
for (unsigned i = 0; i < 4; ++i)
- _data[i] = SGLimits<T>::quiet_NaN();
+ data()[i] = SGLimits<T>::quiet_NaN();
#endif
}
/// Constructor. Initialize by the given values
/// Constructor. Initialize by the content of a plain array,
/// make sure it has at least 4 elements
explicit SGQuat(const T* d)
- { _data[0] = d[0]; _data[1] = d[1]; _data[2] = d[2]; _data[3] = d[3]; }
+ { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
+ explicit SGQuat(const osg::Quat& d)
+ { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
/// Return a unit quaternion
static SGQuat unit(void)
{ return fromRealImag(1, SGVec3<T>(0)); }
/// Return a quaternion from euler angles
- static SGQuat fromEuler(T z, T y, T x)
+ static SGQuat fromEulerRad(T z, T y, T x)
{
SGQuat q;
T zd2 = T(0.5)*z; T yd2 = T(0.5)*y; T xd2 = T(0.5)*x;
return q;
}
+ /// Return a quaternion from euler angles in degrees
+ static SGQuat fromEulerDeg(T z, T y, T x)
+ {
+ return fromEulerRad(SGMisc<T>::deg2rad(z), SGMisc<T>::deg2rad(y),
+ SGMisc<T>::deg2rad(x));
+ }
+
/// Return a quaternion from euler angles
static SGQuat fromYawPitchRoll(T y, T p, T r)
- { return fromEuler(y, p, r); }
+ { return fromEulerRad(y, p, r); }
+
+ /// Return a quaternion from euler angles
+ static SGQuat fromYawPitchRollDeg(T y, T p, T r)
+ { return fromEulerDeg(y, p, r); }
/// Return a quaternion from euler angles
static SGQuat fromHeadAttBank(T h, T a, T b)
- { return fromEuler(h, a, b); }
+ { return fromEulerRad(h, a, b); }
+
+ /// Return a quaternion from euler angles
+ static SGQuat fromHeadAttBankDeg(T h, T a, T b)
+ { return fromEulerDeg(h, a, b); }
/// Return a quaternion rotation the the horizontal local frame from given
/// longitude and latitude
- static SGQuat fromLonLat(T lon, T lat)
+ static SGQuat fromLonLatRad(T lon, T lat)
{
SGQuat q;
T zd2 = T(0.5)*lon;
return q;
}
+ /// Return a quaternion rotation the the horizontal local frame from given
+ /// longitude and latitude
+ static SGQuat fromLonLatDeg(T lon, T lat)
+ { return fromLonLatRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
+
+ /// Return a quaternion rotation the the horizontal local frame from given
+ /// longitude and latitude
+ static SGQuat fromLonLat(const SGGeod& geod)
+ { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
+
/// Create a quaternion from the angle axis representation
static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
{
return fromRealImag(cos(angle2), T(sin(angle2))*axis);
}
+ /// Create a quaternion from the angle axis representation
+ static SGQuat fromAngleAxisDeg(T angle, const SGVec3<T>& axis)
+ { return fromAngleAxis(SGMisc<T>::deg2rad(angle), axis); }
+
/// Create a quaternion from the angle axis representation where the angle
/// is stored in the axis' length
static SGQuat fromAngleAxis(const SGVec3<T>& axis)
void getEulerDeg(T& zDeg, T& yDeg, T& xDeg) const
{
getEulerRad(zDeg, yDeg, xDeg);
- zDeg *= 180/SGMisc<value_type>::pi();
- yDeg *= 180/SGMisc<value_type>::pi();
- xDeg *= 180/SGMisc<value_type>::pi();
+ zDeg = SGMisc<T>::rad2deg(zDeg);
+ yDeg = SGMisc<T>::rad2deg(yDeg);
+ xDeg = SGMisc<T>::rad2deg(xDeg);
}
/// write the angle axis representation into the references
/// Access by index, the index is unchecked
const T& operator()(unsigned i) const
- { return _data[i]; }
+ { return data()[i]; }
/// Access by index, the index is unchecked
T& operator()(unsigned i)
- { return _data[i]; }
+ { return data()[i]; }
+
+ /// Access raw data by index, the index is unchecked
+ const T& operator[](unsigned i) const
+ { return data()[i]; }
+ /// Access raw data by index, the index is unchecked
+ T& operator[](unsigned i)
+ { return data()[i]; }
/// Access the x component
const T& x(void) const
- { return _data[0]; }
+ { return data()[0]; }
/// Access the x component
T& x(void)
- { return _data[0]; }
+ { return data()[0]; }
/// Access the y component
const T& y(void) const
- { return _data[1]; }
+ { return data()[1]; }
/// Access the y component
T& y(void)
- { return _data[1]; }
+ { return data()[1]; }
/// Access the z component
const T& z(void) const
- { return _data[2]; }
+ { return data()[2]; }
/// Access the z component
T& z(void)
- { return _data[2]; }
+ { return data()[2]; }
/// Access the w component
const T& w(void) const
- { return _data[3]; }
+ { return data()[3]; }
/// Access the w component
T& w(void)
- { return _data[3]; }
+ { return data()[3]; }
- /// Get the data pointer, usefull for interfacing with plib's sg*Vec
- const T* data(void) const
- { return _data; }
- /// Get the data pointer, usefull for interfacing with plib's sg*Vec
- T* data(void)
- { return _data; }
+ /// Get the data pointer
+ using SGQuatStorage<T>::data;
/// Readonly interface function to ssg's sgQuat/sgdQuat
const T (&sg(void) const)[4]
- { return _data; }
+ { return data(); }
/// Interface function to ssg's sgQuat/sgdQuat
T (&sg(void))[4]
- { return _data; }
+ { return data(); }
+
+ /// Interface function to osg's Quat*
+ using SGQuatStorage<T>::osg;
/// Inplace addition
SGQuat& operator+=(const SGQuat& v)
- { _data[0]+=v(0);_data[1]+=v(1);_data[2]+=v(2);_data[3]+=v(3);return *this; }
+ { data()[0]+=v(0);data()[1]+=v(1);data()[2]+=v(2);data()[3]+=v(3);return *this; }
/// Inplace subtraction
SGQuat& operator-=(const SGQuat& v)
- { _data[0]-=v(0);_data[1]-=v(1);_data[2]-=v(2);_data[3]-=v(3);return *this; }
+ { data()[0]-=v(0);data()[1]-=v(1);data()[2]-=v(2);data()[3]-=v(3);return *this; }
/// Inplace scalar multiplication
template<typename S>
SGQuat& operator*=(S s)
- { _data[0] *= s; _data[1] *= s; _data[2] *= s; _data[3] *= s; return *this; }
+ { data()[0] *= s; data()[1] *= s; data()[2] *= s; data()[3] *= s; return *this; }
/// Inplace scalar multiplication by 1/s
template<typename S>
SGQuat& operator/=(S s)
return deriv;
}
-
-private:
- /// The actual data
- T _data[4];
};
/// Unary +, do nothing ...
operator<<(std::basic_ostream<char_type, traits_type>& s, const SGQuat<T>& v)
{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
-/// Two classes doing actually the same on different types
-typedef SGQuat<float> SGQuatf;
-typedef SGQuat<double> SGQuatd;
-
inline
SGQuatf
toQuatf(const SGQuatd& v)
-{ return SGQuatf(v(0), v(1), v(2), v(3)); }
+{ return SGQuatf((float)v(0), (float)v(1), (float)v(2), (float)v(3)); }
inline
SGQuatd