Modified Files:
Makefile.am SGMath.hxx SGMathFwd.hxx SGMatrix.hxx SGQuat.hxx Added Files: SGVec2.hxx Improove the matrix functions. Improove the quaterion functions. Add the 2d vector.
This commit is contained in:
frohlich
@@ -30,7 +30,8 @@ include_HEADERS = \
|
||||
SGMisc.hxx \
|
||||
SGQuat.hxx \
|
||||
SGVec4.hxx \
|
||||
SGVec3.hxx
|
||||
SGVec3.hxx \
|
||||
SGVec2.hxx
|
||||
|
||||
|
||||
libsgmath_a_SOURCES = \
|
||||
|
||||
@@ -28,6 +28,7 @@
|
||||
#include "SGLimits.hxx"
|
||||
#include "SGMisc.hxx"
|
||||
#include "SGGeodesy.hxx"
|
||||
#include "SGVec2.hxx"
|
||||
#include "SGVec3.hxx"
|
||||
#include "SGVec4.hxx"
|
||||
#include "SGGeoc.hxx"
|
||||
|
||||
@@ -32,6 +32,8 @@ class SGMisc;
|
||||
template<typename T>
|
||||
class SGQuat;
|
||||
template<typename T>
|
||||
class SGVec2;
|
||||
template<typename T>
|
||||
class SGVec3;
|
||||
template<typename T>
|
||||
class SGVec4;
|
||||
@@ -44,6 +46,8 @@ typedef SGMisc<float> SGMiscf;
|
||||
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;
|
||||
|
||||
@@ -63,14 +63,13 @@ public:
|
||||
}
|
||||
|
||||
/// 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
|
||||
@@ -78,39 +77,22 @@ public:
|
||||
{ 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;
|
||||
@@ -199,6 +181,45 @@ public:
|
||||
/// 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;
|
||||
|
||||
@@ -180,14 +180,100 @@ public:
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -198,36 +284,36 @@ public:
|
||||
/// 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();
|
||||
T sqrQW = w()*w();
|
||||
T sqrQX = x()*x();
|
||||
T sqrQY = y()*y();
|
||||
T 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 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;
|
||||
}
|
||||
}
|
||||
@@ -341,18 +427,18 @@ public:
|
||||
/// 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);
|
||||
}
|
||||
|
||||
@@ -376,6 +462,67 @@ public:
|
||||
|
||||
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 ...
|
||||
|
||||
322
simgear/math/SGVec2.hxx
Normal file
322
simgear/math/SGVec2.hxx
Normal file
@@ -0,0 +1,322 @@
|
||||
// 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
|
||||
Reference in New Issue
Block a user