src/base/vec.h

   1 /************************************************************************
   2     This file is part of NE.
   3
   4     NE is free software: you can redistribute it and/or modify
   5     it under the terms of the GNU General Public License as published by
   6     the Free Software Foundation, either version 3 of the License, or
   7     (at your option) any later version.
   8
   9     NE is distributed in the hope that it will be useful,
  10     but WITHOUT ANY WARRANTY; without even the implied warranty of
  11     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12     GNU General Public License for more details.
  13
  14     You should have received a copy of the GNU General Public License
  15     along with NE.  If not, see <http://www.gnu.org/licenses/>.
  16 ************************************************************************/
  17
  18 #ifndef VEC_H_
  19 #define VEC_H_
  20
  21 #include <GL/gl.h>
  22 #include <ode/ode.h>
  23
  24 //namespace Base {
  25
  26 template<typename T>
  27 class Vec
  28 {
  29 public:
  30         Vec(T v=0) : m_x(v), m_y(v), m_z(v) {}
  31         Vec(T x, T y, T z) : m_x(x), m_y(y), m_z(z) {}
  32
  33         ////////////////
  34         // Conversion //
  35         ////////////////
  36
  37         inline static const Vec<T> FromODE(const dReal *v)
  38         {
  39                 return Vec<T>(v[0], v[1], v[2]);
  40         }
  41
  42         ///////////////////////
  43         // Set/Get functions //
  44         ///////////////////////
  45
  46         inline const Vec<T> set(T v)
  47         {
  48                 m_x = m_y = m_z = v;
  49                 return *this;
  50         }
  51         inline const Vec<T> set(T x, T y, T z)
  52         {
  53                 m_x = x; m_y = y; m_z = z;
  54                 return *this;
  55         }
  56         inline void get(T v[3]) const
  57         {
  58                 v[0] = m_x;
  59                 v[1] = m_y;
  60                 v[2] = m_z;
  61         }
  62
  63         inline T x() const { return m_x; }
  64         inline T y() const { return m_y; }
  65         inline T z() const { return m_z; }
  66
  67         inline const Vec<T> operator - () const
  68         {
  69                 return Vec<T>(-m_x, -m_y, -m_z);
  70         }
  71
  72         ///////////////////////
  73         // Equality overload //
  74         ///////////////////////
  75
  76         inline const Vec<T> operator = (const Vec<T> &q)
  77         {
  78                 return set(q.m_x,q.m_y,q.m_z);
  79         }
  80         inline bool operator == (const Vec<T> &q)
  81         {
  82                 return m_x==q.m_x && m_y==q.m_y && m_z==q.m_z;
  83         }
  84
  85         /////////////////////
  86         // Vector overload //
  87         /////////////////////
  88
  89         inline const Vec<T> operator + (const Vec<T> &v) const
  90         {
  91                 return Vec<T>(m_x+v.m_x, m_y+v.m_y, m_z+v.m_z);
  92         }
  93         inline const Vec<T> operator += (const Vec<T> &v)
  94         {
  95                 return set(m_x+v.m_x, m_y+v.m_y, m_z+v.m_z);
  96         }
  97
  98         inline const Vec<T> operator - (const Vec<T> &v) const
  99         {
 100                 return Vec<T>(m_x-v.m_x, m_y-v.m_y, m_z-v.m_z);
 101         }
 102         inline const Vec<T> operator -= (const Vec<T> &v)
 103         {
 104                 return set(m_x-v.m_x, m_y-v.m_y, m_z-v.m_z);
 105         }
 106
 107         inline const Vec<T> operator * (const Vec<T> &v) const
 108         {
 109                 return Vec<T>(m_x*v.m_x, m_y*v.m_y, m_z*v.m_z);
 110         }
 111         inline const Vec<T> operator *= (const Vec<T> &v)
 112         {
 113                 return set(m_x*v.m_x, m_y*v.m_y, m_z*v.m_z);
 114         }
 115
 116         inline const Vec<T> operator / (const Vec<T> &v) const
 117         {
 118                 return Vec<T>(m_x/v.m_x, m_y/v.m_y, m_z/v.m_z);
 119         }
 120         inline const Vec<T> operator /= (const Vec<T> &v)
 121         {
 122                 return set(m_x/v.m_x, m_y/v.m_y, m_z/v.m_z);
 123         }
 124
 125         /////////////////////
 126         // Scalar overload //
 127         /////////////////////
 128
 129         inline const Vec<T> operator + (T v) const
 130         {
 131                 return *this + Vec<T>(v);
 132         }
 133         inline const Vec<T> operator += (T v)
 134         {
 135                 return *this += Vec<T>(v);
 136         }
 137
 138         inline const Vec<T> operator - (T v) const
 139         {
 140                 return *this - Vec<T>(v);
 141         }
 142         inline const Vec<T> &operator -= (T v)
 143         {
 144                 return *this -= Vec<T>(v);
 145         }
 146
 147         inline const Vec<T> operator * (T v) const
 148         {
 149                 return *this * Vec<T>(v);
 150         }
 151         inline const Vec<T> operator *= (T v)
 152         {
 153                 return *this *= Vec<T>(v);
 154         }
 155
 156         inline const Vec<T> operator / (T v) const
 157         {
 158                 return *this / Vec<T>(v);
 159         }
 160         inline const Vec<T> operator /= (T v)
 161         {
 162                 return *this /= Vec<T>(v);
 163         }
 164
 165         /////////////
 166         // 3D Math //
 167         /////////////
 168
 169         inline T length() const
 170         {
 171                 return sqrt(dot(*this));
 172         }
 173
 174         inline const Vec<T> unit() const
 175         {
 176                 return *this / (T)length();
 177         }
 178
 179         inline T dot(const Vec<T> &v) const
 180         {
 181                 return m_x*v.m_x + m_y*v.m_y + m_z*v.m_z;
 182         }
 183
 184         inline const Vec<T> cross(const Vec<T> &v) const
 185         {
 186                 return Vec<T>(m_y * v.m_z - m_z * v.m_y,
 187                                           m_z * v.m_x - m_x * v.m_z,
 188                                           m_x * v.m_y - m_y * v.m_x);
 189         }
 190
 191         // returns a rotation matrix (for OpenGL) that
 192         // specifies a rotation of angle around vector v
 193         inline static void RotateGL(T angle, const Vec<T> &v, GLfloat *r)
 194         {
 195                 GLfloat c = cos(angle);
 196                 GLfloat s = sin(angle);
 197                 GLfloat t = 1.0 - c;
 198
 199                 r[ 0] = t * v.x() * v.x() + c;
 200                 r[ 1] = t * v.x() * v.y() + s * v.z();
 201                 r[ 2] = t * v.x() * v.z() + s * v.y();
 202                 r[ 3] = 0.0;
 203
 204                 r[ 4] = t * v.x() * v.y() - s * v.z();
 205                 r[ 5] = t * v.y() * v.y() + c;
 206                 r[ 6] = t * v.y() * v.z() + s * v.x();
 207                 r[ 7] = 0.0;
 208
 209                 r[ 8] = t * v.x() * v.z() + s * v.y();
 210                 r[ 9] = t * v.y() * v.z() + s * v.x();
 211                 r[10] = t * v.z() * v.z() + c;
 212                 r[11] = 0.0;
 213
 214                 r[12] = 0.0;
 215                 r[13] = 0.0;
 216                 r[14] = 0.0;
 217                 r[15] = 1.0;
 218         }
 219
 220         inline const Vec<T> rotate(T angle, const Vec<T> &v)
 221         {
 222                 GLfloat r[16];
 223                 RotateGL(angle, v, r);
 224                 return rotate(r);
 225         }
 226
 227         // rotate via an OpenGL matrix
 228         inline const Vec<T> rotate(const T *r)
 229         {
 230                 return Vec<T>(m_x*r[0] + m_y*r[4] + m_z*r[8],
 231                                           m_x*r[1] + m_y*r[5] + m_z*r[9],
 232                                           m_x*r[2] + m_y*r[6] + m_z*r[10]);
 233         }
 234
 235         ///////////////
 236         // Debugging //
 237         ///////////////
 238
 239         void draw(const Vec<T> &pos, GLfloat scale = 1.0) const
 240         {
 241                 glPushMatrix();
 242                 glTranslatef(pos.x(), pos.y(), pos.z());
 243                 glScalef(scale, scale, scale);
 244                 draw();
 245                 glPopMatrix();
 246         }
 247
 248         void draw() const
 249         {
 250                 glPushAttrib(GL_ALL_ATTRIB_BITS);
 251                 //glDisable(GL_DEPTH_TEST);
 252                 glDisable(GL_LIGHTING);
 253                 glBegin(GL_LINES);
 254                 glVertex3f(x(), y(), z());
 255                 glVertex3f(0, 0, 0);
 256                 glEnd();
 257                 glPopAttrib();
 258         }
 259
 260 protected:
 261         T m_x, m_y, m_z;
 262 };
 263
 264 typedef Vec<float> Vec3f;
 265 typedef Vec<double> Vec3d;
 266
 267 //}
 268
 269 #endif /*VEC_H_*/
 270