SGMisc.hxx \
SGQuat.hxx \
SGVec4.hxx \
- SGVec3.hxx
+ SGVec3.hxx \
+ SGVec2.hxx
libsgmath_a_SOURCES = \
#include "SGLimits.hxx"
#include "SGMisc.hxx"
#include "SGGeodesy.hxx"
+#include "SGVec2.hxx"
#include "SGVec3.hxx"
#include "SGVec4.hxx"
#include "SGGeoc.hxx"
template<typename T>
class SGQuat;
template<typename T>
+class SGVec2;
+template<typename T>
class SGVec3;
template<typename T>
class SGVec4;
typedef SGMisc<double> SGMiscd;
typedef SGQuat<float> SGQuatf;
typedef SGQuat<double> SGQuatd;
+typedef SGVec2<float> SGVec2f;
+typedef SGVec2<double> SGVec2d;
typedef SGVec3<float> SGVec3f;
typedef SGVec3<double> SGVec3d;
typedef SGVec4<float> SGVec4f;
}
/// Constructor, build up a SGMatrix from a translation
- SGMatrix(const SGVec3<T>& trans)
+ template<typename S>
+ SGMatrix(const SGVec3<S>& trans)
{ set(trans); }
/// Constructor, build up a SGMatrix from a rotation and a translation
- SGMatrix(const SGQuat<T>& quat, const SGVec3<T>& trans)
- { set(quat, trans); }
- /// Constructor, build up a SGMatrix from a rotation and a translation
- SGMatrix(const SGQuat<T>& quat)
+ template<typename S>
+ SGMatrix(const SGQuat<S>& quat)
{ set(quat); }
/// Copy constructor for a transposed negated matrix
{ set(tm); }
/// Set from a tranlation
- void set(const SGVec3<T>& trans)
+ template<typename S>
+ void set(const SGVec3<S>& trans)
{
_data.flat[0] = 1; _data.flat[4] = 0;
- _data.flat[8] = 0; _data.flat[12] = -trans(0);
+ _data.flat[8] = 0; _data.flat[12] = T(trans(0));
_data.flat[1] = 0; _data.flat[5] = 1;
- _data.flat[9] = 0; _data.flat[13] = -trans(1);
+ _data.flat[9] = 0; _data.flat[13] = T(trans(1));
_data.flat[2] = 0; _data.flat[6] = 0;
- _data.flat[10] = 1; _data.flat[14] = -trans(2);
+ _data.flat[10] = 1; _data.flat[14] = T(trans(2));
_data.flat[3] = 0; _data.flat[7] = 0;
_data.flat[11] = 0; _data.flat[15] = 1;
}
/// Set from a scale/rotation and tranlation
- void set(const SGQuat<T>& quat, const SGVec3<T>& trans)
- {
- T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
- T xx = x*x; T yy = y*y; T zz = z*z;
- T wx = w*x; T wy = w*y; T wz = w*z;
- T xy = x*y; T xz = x*z; T yz = y*z;
- _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
- _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
- _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
- _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
- _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
- _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
- // Well, this one is ugly here, as that xform method on the current
- // object needs the above data to be already set ...
- SGVec3<T> t = xformVec(trans);
- _data.flat[12] = -t(0); _data.flat[13] = -t(1);
- _data.flat[14] = -t(2); _data.flat[15] = 1;
- }
- /// Set from a scale/rotation and tranlation
- void set(const SGQuat<T>& quat)
+ template<typename S>
+ void set(const SGQuat<S>& quat)
{
T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
T xx = x*x; T yy = y*y; T zz = z*z;
/// Inplace matrix multiplication, post multiply
SGMatrix& operator*=(const SGMatrix<T>& m2);
+ template<typename S>
+ SGMatrix& preMultTranslate(const SGVec3<S>& t)
+ {
+ for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i)
+ (*this)(i,3) += T(t(i));
+ return *this;
+ }
+ template<typename S>
+ SGMatrix& postMultTranslate(const SGVec3<S>& t)
+ {
+ SGVec4<T> col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
+ for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
+ SGVec4<T> tmp((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
+ col3 += T(t(i))*tmp;
+ }
+ (*this)(0,3) = col3(0); (*this)(1,3) = col3(1);
+ (*this)(2,3) = col3(2); (*this)(3,3) = col3(3);
+ return *this;
+ }
+
+ SGMatrix& preMultRotate(const SGQuat<T>& r)
+ {
+ for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
+ SGVec3<T> col((*this)(0,i), (*this)(1,i), (*this)(2,i));
+ col = r.transform(col);
+ (*this)(0,i) = col(0); (*this)(1,i) = col(1); (*this)(2,i) = col(2);
+ }
+ return *this;
+ }
+ SGMatrix& postMultRotate(const SGQuat<T>& r)
+ {
+ for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
+ SGVec3<T> col((*this)(i,0), (*this)(i,1), (*this)(i,2));
+ col = r.backTransform(col);
+ (*this)(i,0) = col(0); (*this)(i,1) = col(1); (*this)(i,2) = col(2);
+ }
+ return *this;
+ }
+
SGVec3<T> xformPt(const SGVec3<T>& pt) const
{
SGVec3<T> tpt;
return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis);
}
+ 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())
+ return SGQuat::unit();
+
+ return SGQuat::fromRotateToNorm((1/nfrom)*from, (1/nto)*to);
+ }
+
+ // FIXME more finegrained error behavour.
+ 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())
+ 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())
+ return SGQuat::unit();
+
+ // The target vector for the first rotation
+ SGVec3<T> nto1 = SGVec3<T>::zeros();
+ SGVec3<T> nto2 = SGVec3<T>::zeros();
+ nto1[i1] = 1;
+ nto2[i2] = 1;
+
+ // The first rotation can be done with the usual routine.
+ SGQuat q = SGQuat::fromRotateToNorm(nv1, nto1);
+
+ // The rotation axis for the second rotation is the
+ // target for the first one, so the rotation axis is nto1
+ // We need to get the angle.
+
+ // Make nv2 exactly orthogonal to nv1.
+ nv2 = normalize(nv2 - dv1v2*nv1);
+
+ SGVec3<T> tnv2 = q.transform(nv2);
+ T cosang = dot(nto2, tnv2);
+ T cos05ang = T(0.5+0.5*cosang);
+ if (cos05ang <= 0)
+ cosang = T(0);
+ cos05ang = sqrt(cos05ang);
+ T sig = dot(nto1, cross(nto2, tnv2));
+ T sin05ang = T(0.5-0.5*cosang);
+ if (sin05ang <= 0)
+ sin05ang = 0;
+ sin05ang = copysign(sqrt(sin05ang), sig);
+ q *= SGQuat::fromRealImag(cos05ang, sin05ang*nto1);
+
+ return q;
+ }
+
+
+ // Return a quaternion which rotates the vector given by v
+ // to the vector -v. Other directions are *not* preserved.
+ static SGQuat fromChangeSign(const SGVec3<T>& v)
+ {
+ // The vector from points to the oposite direction than to.
+ // Find a vector perpandicular to the vector to.
+ 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);
+ axis = (1/sqrt(1+quot*quot))*SGVec3<T>(quot, -1, 0);
+ } else if (absv1 < absv2 && absv3 < absv2) {
+ T quot = v(2)/v(1);
+ axis = (1/sqrt(1+quot*quot))*SGVec3<T>(0, quot, -1);
+ } else if (absv1 < absv3 && absv2 < absv3) {
+ T quot = v(0)/v(2);
+ axis = (1/sqrt(1+quot*quot))*SGVec3<T>(-1, 0, quot);
+ } else {
+ // The all zero case.
+ return SGQuat::unit();
+ }
+
+ return SGQuat::fromRealImag(0, axis);
+ }
+
/// Return a quaternion from real and imaginary part
static SGQuat fromRealImag(T r, const SGVec3<T>& i)
{
SGQuat q;
q.w() = r;
- q.x() = i(0);
- q.y() = i(1);
- q.z() = i(2);
+ q.x() = i.x();
+ q.y() = i.y();
+ q.z() = i.z();
return q;
}
/// write the euler angles into the references
void getEulerRad(T& zRad, T& yRad, T& xRad) const
{
- value_type sqrQW = w()*w();
- value_type sqrQX = x()*x();
- value_type sqrQY = y()*y();
- value_type sqrQZ = z()*z();
-
- value_type num = 2*(y()*z() + w()*x());
- value_type den = sqrQW - sqrQX - sqrQY + sqrQZ;
- if (fabs(den) < SGLimits<value_type>::min() &&
- fabs(num) < SGLimits<value_type>::min())
+ T sqrQW = w()*w();
+ T sqrQX = x()*x();
+ T sqrQY = y()*y();
+ T sqrQZ = z()*z();
+
+ T num = 2*(y()*z() + w()*x());
+ T den = sqrQW - sqrQX - sqrQY + sqrQZ;
+ if (fabs(den) < SGLimits<T>::min() &&
+ fabs(num) < SGLimits<T>::min())
xRad = 0;
else
xRad = atan2(num, den);
- value_type tmp = 2*(x()*z() - w()*y());
+ T tmp = 2*(x()*z() - w()*y());
if (tmp < -1)
- yRad = 0.5*SGMisc<value_type>::pi();
+ yRad = 0.5*SGMisc<T>::pi();
else if (1 < tmp)
- yRad = -0.5*SGMisc<value_type>::pi();
+ yRad = -0.5*SGMisc<T>::pi();
else
yRad = -asin(tmp);
num = 2*(x()*y() + w()*z());
den = sqrQW + sqrQX - sqrQY - sqrQZ;
- if (fabs(den) < SGLimits<value_type>::min() &&
- fabs(num) < SGLimits<value_type>::min())
+ if (fabs(den) < SGLimits<T>::min() &&
+ fabs(num) < SGLimits<T>::min())
zRad = 0;
else {
- value_type psi = atan2(num, den);
+ T psi = atan2(num, den);
if (psi < 0)
- psi += 2*SGMisc<value_type>::pi();
+ psi += 2*SGMisc<T>::pi();
zRad = psi;
}
}
/// frame rotated with the quaternion
SGVec3<T> transform(const SGVec3<T>& v) const
{
- value_type r = 2/dot(*this, *this);
+ T r = 2/dot(*this, *this);
SGVec3<T> qimag = imag(*this);
- value_type qr = real(*this);
+ T qr = real(*this);
return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag - (r*qr)*cross(qimag, v);
}
/// Transform a vector from the coordinate frame rotated with the quaternion
/// to the current coordinate frame
SGVec3<T> backTransform(const SGVec3<T>& v) const
{
- value_type r = 2/dot(*this, *this);
+ T r = 2/dot(*this, *this);
SGVec3<T> qimag = imag(*this);
- value_type qr = real(*this);
+ T qr = real(*this);
return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag + (r*qr)*cross(qimag, v);
}
return deriv;
}
+
+private:
+
+ // Private because it assumes normalized inputs.
+ static SGQuat
+ fromRotateToSmaller90Deg(T cosang,
+ const SGVec3<T>& from, const SGVec3<T>& to)
+ {
+ // In this function we assume that the angle required to rotate from
+ // the vector from to the vector to is <= 90 deg.
+ // That is done so because of possible instabilities when we rotate more
+ // then 90deg.
+
+ // Note that the next comment does actually cover a *more* *general* case
+ // than we need in this function. That shows that this formula is even
+ // valid for rotations up to 180deg.
+
+ // Because of the signs in the axis, it is sufficient to care for angles
+ // 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);
+
+
+ // 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
+ // sqrt(0.5) < cos05ang.
+
+
+ // Compute the rotation axis, that is
+ // sin(angle)*normalized rotation axis
+ SGVec3<T> axis = cross(to, from);
+
+ // We need sin(0.5*angle)*normalized rotation axis.
+ // So rescale with sin(0.5*x)/sin(x).
+ // To do that we use the equation:
+ // sin(x) = 2*sin(0.5*x)*cos(0.5*x)
+ return SGQuat::fromRealImag( cos05ang, (1/(2*cos05ang))*axis);
+ }
+
+ // Private because it assumes normalized inputs.
+ static SGQuat
+ fromRotateToNorm(const SGVec3<T>& from, const SGVec3<T>& to)
+ {
+ // To avoid instabilities with roundoff, we distinguish between rotations
+ // with more then 90deg and rotations with less than 90deg.
+
+ // Compute the cosine of the angle.
+ T cosang = dot(from, to);
+
+ // For the small ones do direct computation
+ if (T(-0.5) < cosang)
+ return SGQuat::fromRotateToSmaller90Deg(cosang, from, to);
+
+ // For larger rotations. first rotate from to -from.
+ // Past that we will have a smaller angle again.
+ SGQuat q1 = SGQuat::fromChangeSign(from);
+ SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to);
+ return q1*q2;
+ }
};
/// Unary +, do nothing ...
--- /dev/null
+// 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 SGVec2_H
+#define SGVec2_H
+
+#include <osg/Vec2f>
+#include <osg/Vec2d>
+
+template<typename T>
+struct SGVec2Storage {
+ /// Readonly raw storage interface
+ const T (&data(void) const)[2]
+ { return _data; }
+ /// Readonly raw storage interface
+ T (&data(void))[2]
+ { return _data; }
+
+ void osg() const
+ { }
+
+private:
+ T _data[2];
+};
+
+template<>
+struct SGVec2Storage<float> : public osg::Vec2f {
+ /// Access raw data by index, the index is unchecked
+ const float (&data(void) const)[2]
+ { return osg::Vec2f::_v; }
+ /// Access raw data by index, the index is unchecked
+ float (&data(void))[2]
+ { return osg::Vec2f::_v; }
+
+ const osg::Vec2f& osg() const
+ { return *this; }
+ osg::Vec2f& osg()
+ { return *this; }
+};
+
+template<>
+struct SGVec2Storage<double> : public osg::Vec2d {
+ /// Access raw data by index, the index is unchecked
+ const double (&data(void) const)[2]
+ { return osg::Vec2d::_v; }
+ /// Access raw data by index, the index is unchecked
+ double (&data(void))[2]
+ { return osg::Vec2d::_v; }
+
+ const osg::Vec2d& osg() const
+ { return *this; }
+ osg::Vec2d& osg()
+ { return *this; }
+};
+
+/// 2D Vector Class
+template<typename T>
+class SGVec2 : protected SGVec2Storage<T> {
+public:
+ typedef T value_type;
+
+ /// Default constructor. Does not initialize at all.
+ /// If you need them zero initialized, use SGVec2::zeros()
+ SGVec2(void)
+ {
+ /// Initialize with nans in the debug build, that will guarantee to have
+ /// a fast uninitialized default constructor in the release but shows up
+ /// uninitialized values in the debug build very fast ...
+#ifndef NDEBUG
+ for (unsigned i = 0; i < 2; ++i)
+ data()[i] = SGLimits<T>::quiet_NaN();
+#endif
+ }
+ /// Constructor. Initialize by the given values
+ SGVec2(T x, T y)
+ { data()[0] = x; data()[1] = y; }
+ /// Constructor. Initialize by the content of a plain array,
+ /// make sure it has at least 2 elements
+ explicit SGVec2(const T* d)
+ { data()[0] = d[0]; data()[1] = d[1]; }
+ explicit SGVec2(const osg::Vec2f& d)
+ { data()[0] = d[0]; data()[1] = d[1]; }
+ explicit SGVec2(const osg::Vec2d& d)
+ { data()[0] = d[0]; data()[1] = d[1]; }
+
+ /// Access by index, the index is unchecked
+ const T& operator()(unsigned i) const
+ { return data()[i]; }
+ /// Access by index, the index is unchecked
+ T& operator()(unsigned 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]; }
+ /// Access the x component
+ T& x(void)
+ { return data()[0]; }
+ /// Access the y component
+ const T& y(void) const
+ { return data()[1]; }
+ /// Access the y component
+ T& y(void)
+ { return data()[1]; }
+
+ /// Get the data pointer
+ using SGVec2Storage<T>::data;
+
+ /// Readonly interface function to ssg's sgVec2/sgdVec2
+ const T (&sg(void) const)[2]
+ { return data(); }
+ /// Interface function to ssg's sgVec2/sgdVec2
+ T (&sg(void))[2]
+ { return data(); }
+
+ /// Interface function to osg's Vec2*
+ using SGVec2Storage<T>::osg;
+
+ /// Inplace addition
+ SGVec2& operator+=(const SGVec2& v)
+ { data()[0] += v(0); data()[1] += v(1); return *this; }
+ /// Inplace subtraction
+ SGVec2& operator-=(const SGVec2& v)
+ { data()[0] -= v(0); data()[1] -= v(1); return *this; }
+ /// Inplace scalar multiplication
+ template<typename S>
+ SGVec2& operator*=(S s)
+ { data()[0] *= s; data()[1] *= s; return *this; }
+ /// Inplace scalar multiplication by 1/s
+ template<typename S>
+ SGVec2& operator/=(S s)
+ { return operator*=(1/T(s)); }
+
+ /// Return an all zero vector
+ static SGVec2 zeros(void)
+ { return SGVec2(0, 0); }
+ /// Return unit vectors
+ static SGVec2 e1(void)
+ { return SGVec2(1, 0); }
+ static SGVec2 e2(void)
+ { return SGVec2(0, 1); }
+};
+
+/// Unary +, do nothing ...
+template<typename T>
+inline
+const SGVec2<T>&
+operator+(const SGVec2<T>& v)
+{ return v; }
+
+/// Unary -, do nearly nothing
+template<typename T>
+inline
+SGVec2<T>
+operator-(const SGVec2<T>& v)
+{ return SGVec2<T>(-v(0), -v(1)); }
+
+/// Binary +
+template<typename T>
+inline
+SGVec2<T>
+operator+(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ return SGVec2<T>(v1(0)+v2(0), v1(1)+v2(1)); }
+
+/// Binary -
+template<typename T>
+inline
+SGVec2<T>
+operator-(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ return SGVec2<T>(v1(0)-v2(0), v1(1)-v2(1)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGVec2<T>
+operator*(S s, const SGVec2<T>& v)
+{ return SGVec2<T>(s*v(0), s*v(1)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGVec2<T>
+operator*(const SGVec2<T>& v, S s)
+{ return SGVec2<T>(s*v(0), s*v(1)); }
+
+/// Scalar dot product
+template<typename T>
+inline
+T
+dot(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ return v1(0)*v2(0) + v1(1)*v2(1); }
+
+/// The euclidean norm of the vector, that is what most people call length
+template<typename T>
+inline
+T
+norm(const SGVec2<T>& v)
+{ return sqrt(dot(v, v)); }
+
+/// The euclidean norm of the vector, that is what most people call length
+template<typename T>
+inline
+T
+length(const SGVec2<T>& v)
+{ return sqrt(dot(v, v)); }
+
+/// The 1-norm of the vector, this one is the fastest length function we
+/// can implement on modern cpu's
+template<typename T>
+inline
+T
+norm1(const SGVec2<T>& v)
+{ return fabs(v(0)) + fabs(v(1)); }
+
+/// The euclidean norm of the vector, that is what most people call length
+template<typename T>
+inline
+SGVec2<T>
+normalize(const SGVec2<T>& v)
+{ return (1/norm(v))*v; }
+
+/// Return true if exactly the same
+template<typename T>
+inline
+bool
+operator==(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ return v1(0) == v2(0) && v1(1) == v2(1); }
+
+/// Return true if not exactly the same
+template<typename T>
+inline
+bool
+operator!=(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ return ! (v1 == v2); }
+
+/// Return true if equal to the relative tolerance tol
+template<typename T>
+inline
+bool
+equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2, T rtol, T atol)
+{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; }
+
+/// Return true if equal to the relative tolerance tol
+template<typename T>
+inline
+bool
+equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2, T rtol)
+{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
+
+/// Return true if about equal to roundoff of the underlying type
+template<typename T>
+inline
+bool
+equivalent(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{
+ T tol = 100*SGLimits<T>::epsilon();
+ return equivalent(v1, v2, tol, tol);
+}
+
+/// The euclidean distance of the two vectors
+template<typename T>
+inline
+T
+dist(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ return norm(v1 - v2); }
+
+/// The squared euclidean distance of the two vectors
+template<typename T>
+inline
+T
+distSqr(const SGVec2<T>& v1, const SGVec2<T>& v2)
+{ SGVec2<T> tmp = v1 - v2; return dot(tmp, tmp); }
+
+#ifndef NDEBUG
+template<typename T>
+inline
+bool
+isNaN(const SGVec2<T>& v)
+{
+ return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1));
+}
+#endif
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec2<T>& v)
+{ return s << "[ " << v(0) << ", " << v(1) << " ]"; }
+
+inline
+SGVec2f
+toVec2f(const SGVec2d& v)
+{ return SGVec2f((float)v(0), (float)v(1)); }
+
+inline
+SGVec2d
+toVec2d(const SGVec2f& v)
+{ return SGVec2d(v(0), v(1)); }
+
+#endif
projects / simgear.git / commitdiff