-// Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de
+// Copyright (C) 2006-2009 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
#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
+/// Quaternion Class
template<typename T>
-class SGQuat : protected SGQuatStorage<T> {
+class SGQuat {
public:
typedef T value_type;
/// 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]; }
- 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)
static SGQuat fromLonLat(const SGGeod& geod)
{ return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
- /// Return a quaternion rotation from the earth centered to the
- /// OpenGL/viewer horizontal local frame from given longitude and latitude.
- /// This frame matches the usual OpenGL axis directions. That is the target
- /// frame has an x-axis pointing eastwards, y-axis pointing up and y z-axis
- /// pointing south.
- static SGQuat viewHLRad(T lon, T lat)
- {
- // That bails down to a 3-2-1 euler sequence lon+pi/2, 0, -lat-pi
- // what is here is again the hand optimized version ...
- SGQuat q;
- T xd2 = -T(0.5)*lat - T(0.5)*SGMisc<T>::pi();
- T zd2 = T(0.5)*lon + T(0.25)*SGMisc<T>::pi();
- T Szd2 = sin(zd2);
- T Sxd2 = sin(xd2);
- T Czd2 = cos(zd2);
- T Cxd2 = cos(xd2);
- q.w() = Cxd2*Czd2;
- q.x() = Sxd2*Czd2;
- q.y() = Sxd2*Szd2;
- q.z() = Cxd2*Szd2;
- return q;
- }
- /// Like the above provided for convenience
- static SGQuat viewHLDeg(T lon, T lat)
- { return viewHLRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
- /// Like the above provided for convenience
- static SGQuat viewHL(const SGGeod& geod)
- { return viewHLRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
-
- /// Convert a quaternion rotation from the simulation frame
- /// to the view (OpenGL) frame. That is it just swaps the axis part of
- /// this current quaternion.
- /// That proves useful when you want to use the euler 3-2-1 sequence
- /// for the usual heading/pitch/roll sequence within the context of
- /// OpenGL/viewer frames.
- static SGQuat simToView(const SGQuat& q)
- { return SGQuat(q.y(), -q.z(), -q.x(), q.w()); }
/// Create a quaternion from the angle axis representation
static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
{
- T angle2 = 0.5*angle;
+ T angle2 = T(0.5)*angle;
return fromRealImag(cos(angle2), T(sin(angle2))*axis);
}
{
T nAxis = norm(axis);
if (nAxis <= SGLimits<T>::min())
- return SGQuat(1, 0, 0, 0);
- T angle2 = 0.5*nAxis;
+ return SGQuat::unit();
+ T angle2 = T(0.5)*nAxis;
return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis);
}
+ /// Create a normalized quaternion just from the imaginary part.
+ /// The imaginary part should point into that axis direction that results in
+ /// a quaternion with a positive real part.
+ /// This is the smallest numerically stable representation of an orientation
+ /// in space. See getPositiveRealImag()
+ static SGQuat fromPositiveRealImag(const SGVec3<T>& imag)
+ {
+ T r = sqrt(SGMisc<T>::max(T(0), T(1) - dot(imag, imag)));
+ return fromRealImag(r, imag);
+ }
+
+ /// Return a quaternion that rotates the from vector onto the to vector.
static SGQuat fromRotateTo(const SGVec3<T>& from, const SGVec3<T>& to)
{
T nfrom = norm(from);
T nto = norm(to);
- if (nfrom < SGLimits<T>::min() || nto < SGLimits<T>::min())
+ if (nfrom <= SGLimits<T>::min() || nto <= SGLimits<T>::min())
return SGQuat::unit();
return SGQuat::fromRotateToNorm((1/nfrom)*from, (1/nto)*to);
}
- // FIXME more finegrained error behavour.
+ /// Return a quaternion that rotates v1 onto the i1-th unit vector
+ /// and v2 into a plane that is spanned by the i2-th and i1-th unit vector.
static SGQuat fromRotateTo(const SGVec3<T>& v1, unsigned i1,
const SGVec3<T>& v2, unsigned i2)
{
T nrmv1 = norm(v1);
T nrmv2 = norm(v2);
- if (nrmv1 < SGLimits<T>::min() || nrmv2 < SGLimits<T>::min())
+ if (nrmv1 <= SGLimits<T>::min() || nrmv2 <= SGLimits<T>::min())
return SGQuat::unit();
SGVec3<T> nv1 = (1/nrmv1)*v1;
SGVec3<T> nv2 = (1/nrmv2)*v2;
T dv1v2 = dot(nv1, nv2);
- if (fabs(fabs(dv1v2)-1) < SGLimits<T>::epsilon())
+ if (fabs(fabs(dv1v2)-1) <= SGLimits<T>::epsilon())
return SGQuat::unit();
// The target vector for the first rotation
SGVec3<T> tnv2 = q.transform(nv2);
T cosang = dot(nto2, tnv2);
- T cos05ang = T(0.5+0.5*cosang);
+ T cos05ang = T(0.5)+T(0.5)*cosang;
if (cos05ang <= 0)
- cosang = T(0);
+ cosang = 0;
cos05ang = sqrt(cos05ang);
T sig = dot(nto1, cross(nto2, tnv2));
- T sin05ang = T(0.5-0.5*cosang);
+ T sin05ang = T(0.5)-T(0.5)*cosang;
if (sin05ang <= 0)
sin05ang = 0;
sin05ang = copysign(sqrt(sin05ang), sig);
T absv1 = fabs(v(0));
T absv2 = fabs(v(1));
T absv3 = fabs(v(2));
-
+
SGVec3<T> axis;
if (absv2 < absv1 && absv3 < absv1) {
T quot = v(1)/v(0);
T num = 2*(y()*z() + w()*x());
T den = sqrQW - sqrQX - sqrQY + sqrQZ;
- if (fabs(den) < SGLimits<T>::min() &&
- fabs(num) < SGLimits<T>::min())
+ if (fabs(den) <= SGLimits<T>::min() &&
+ fabs(num) <= SGLimits<T>::min())
xRad = 0;
else
xRad = atan2(num, den);
-
+
T tmp = 2*(x()*z() - w()*y());
- if (tmp < -1)
- yRad = 0.5*SGMisc<T>::pi();
- else if (1 < tmp)
- yRad = -0.5*SGMisc<T>::pi();
+ if (tmp <= -1)
+ yRad = T(0.5)*SGMisc<T>::pi();
+ else if (1 <= tmp)
+ yRad = -T(0.5)*SGMisc<T>::pi();
else
yRad = -asin(tmp);
-
- num = 2*(x()*y() + w()*z());
+
+ num = 2*(x()*y() + w()*z());
den = sqrQW + sqrQX - sqrQY - sqrQZ;
- if (fabs(den) < SGLimits<T>::min() &&
- fabs(num) < SGLimits<T>::min())
+ if (fabs(den) <= SGLimits<T>::min() &&
+ fabs(num) <= SGLimits<T>::min())
zRad = 0;
else {
T psi = atan2(num, den);
void getAngleAxis(T& angle, SGVec3<T>& axis) const
{
T nrm = norm(*this);
- if (nrm < SGLimits<T>::min()) {
+ if (nrm <= SGLimits<T>::min()) {
angle = 0;
axis = SGVec3<T>(0, 0, 0);
} else {
T rNrm = 1/nrm;
angle = acos(SGMisc<T>::max(-1, SGMisc<T>::min(1, rNrm*w())));
T sAng = sin(angle);
- if (fabs(sAng) < SGLimits<T>::min())
+ if (fabs(sAng) <= SGLimits<T>::min())
axis = SGVec3<T>(1, 0, 0);
- else
+ else
axis = (rNrm/sAng)*imag(*this);
angle *= 2;
}
axis *= angle;
}
+ /// Get the imaginary part of the quaternion.
+ /// The imaginary part should point into that axis direction that results in
+ /// a quaternion with a positive real part.
+ /// This is the smallest numerically stable representation of an orientation
+ /// in space. See fromPositiveRealImag()
+ SGVec3<T> getPositiveRealImag() const
+ {
+ if (real(*this) < T(0))
+ return (T(-1)/norm(*this))*imag(*this);
+ else
+ return (T(1)/norm(*this))*imag(*this);
+ }
+
/// Access by index, the index is unchecked
const T& operator()(unsigned i) const
{ return data()[i]; }
{ return data()[3]; }
/// Get the data pointer
- using SGQuatStorage<T>::data;
-
- /// Readonly interface function to ssg's sgQuat/sgdQuat
- const T (&sg(void) const)[4]
- { return data(); }
- /// Interface function to ssg's sgQuat/sgdQuat
- T (&sg(void))[4]
- { return data(); }
-
- /// Interface function to osg's Quat*
- using SGQuatStorage<T>::osg;
+ const T (&data(void) const)[4]
+ { return _data; }
+ /// Get the data pointer
+ T (&data(void))[4]
+ { return _data; }
/// Inplace addition
SGQuat& operator+=(const SGQuat& v)
{
SGQuat deriv;
- deriv.w() = 0.5*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2));
- deriv.x() = 0.5*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2));
- deriv.y() = 0.5*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2));
- deriv.z() = 0.5*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2));
-
+ deriv.w() = T(0.5)*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2));
+ deriv.x() = T(0.5)*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2));
+ deriv.y() = T(0.5)*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2));
+ deriv.z() = T(0.5)*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2));
+
return deriv;
}
+ /// Return the angular velocity w that makes q0 translate to q1 using
+ /// an explicit euler step with stepsize h.
+ /// That is, look for an w where
+ /// q1 = normalize(q0 + h*q0.derivative(w))
+ static SGVec3<T>
+ forwardDifferenceVelocity(const SGQuat& q0, const SGQuat& q1, const T& h)
+ {
+ // Let D_q0*w = q0.derivative(w), D_q0 the above 4x3 matrix.
+ // Then D_q0^t*D_q0 = 0.25*Id and D_q0*q0 = 0.
+ // Let lambda be a nonzero normailzation factor, then
+ // q1 = normalize(q0 + h*q0.derivative(w))
+ // can be rewritten
+ // lambda*q1 = q0 + h*D_q0*w.
+ // Multiply left by the transpose D_q0^t and reorder gives
+ // 4*lambda/h*D_q0^t*q1 = w.
+ // Now compute lambda by substitution of w into the original
+ // equation
+ // lambda*q1 = q0 + 4*lambda*D_q0*D_q0^t*q1,
+ // multiply by q1^t from the left
+ // lambda*<q1,q1> = <q0,q1> + 4*lambda*<D_q0^t*q1,D_q0^t*q1>
+ // and solving for lambda gives
+ // lambda = <q0,q1>/(1 - 4*<D_q0^t*q1,D_q0^t*q1>).
+
+ // The transpose of the derivative matrix
+ // the 0.5 factor is handled below
+ // also note that the initializer uses x, y, z, w instead of w, x, y, z
+ SGQuat d0(q0.w(), q0.z(), -q0.y(), -q0.x());
+ SGQuat d1(-q0.z(), q0.w(), q0.x(), -q0.y());
+ SGQuat d2(q0.y(), -q0.x(), q0.w(), -q0.z());
+ // 2*D_q0^t*q1
+ SGVec3<T> Dq(dot(d0, q1), dot(d1, q1), dot(d2, q1));
+ // Like above, but take into account that Dq = 2*D_q0^t*q1
+ T lambda = dot(q0, q1)/(T(1) - dot(Dq, Dq));
+ return (2*lambda/h)*Dq;
+ }
+
private:
// Private because it assumes normalized inputs.
// in the interval [-pi,pi]. That means that 0.5*angle is in the interval
// [-pi/2,pi/2]. But in that range the cosine is allways >= 0.
// So we do not need to care for egative roots in the following equation:
- T cos05ang = sqrt(0.5+0.5*cosang);
+ T cos05ang = sqrt(T(0.5)+T(0.5)*cosang);
// Now our assumption of angles <= 90 deg comes in play.
// For that reason, we know that cos05ang is not zero.
- // It is even more, we can see from the above formula that
+ // It is even more, we can see from the above formula that
// sqrt(0.5) < cos05ang.
SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to);
return q1*q2;
}
+
+ T _data[4];
};
/// Unary +, do nothing ...
// need the scales now, if the angle is very small, do linear interpolation
// to avoid instabilities
T scale0, scale1;
- if (fabs(o) < SGLimits<T>::epsilon()) {
+ if (fabs(o) <= SGLimits<T>::epsilon()) {
scale0 = 1 - t;
scale1 = t;
} else {
projects / simgear.git / blobdiff