Menge
Modular Pedestrian Simulation Framework for Research and Development
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Vector3.h
Go to the documentation of this file.
1 /*
2 
3 License
4 
5 Menge
6 Copyright and trademark 2012-14 University of North Carolina at Chapel Hill.
7 All rights reserved.
8 
9 Permission to use, copy, modify, and distribute this software and its documentation
10 for educational, research, and non-profit purposes, without fee, and without a
11 written agreement is hereby granted, provided that the above copyright notice,
12 this paragraph, and the following four paragraphs appear in all copies.
13 
14 This software program and documentation are copyrighted by the University of North
15 Carolina at Chapel Hill. The software program and documentation are supplied "as is,"
16 without any accompanying services from the University of North Carolina at Chapel
17 Hill or the authors. The University of North Carolina at Chapel Hill and the
18 authors do not warrant that the operation of the program will be uninterrupted
19 or error-free. The end-user understands that the program was developed for research
20 purposes and is advised not to rely exclusively on the program for any reason.
21 
22 IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS
23 BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
24 DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
25 DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE
26 AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 
28 THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY
29 DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
30 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY WARRANTY
31 OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND
32 THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE NO OBLIGATIONS
33 TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
34 
35 Any questions or comments should be sent to the authors {menge,geom}@cs.unc.edu
36 
37 */
38 
44 #ifndef __VECTOR3_H__
45 #define __VECTOR3_H__
46 
47 #ifdef _MSC_VER
48  // To export templated classes and functions, requires declaring specializations
49  // extern. This became standard in C++11, but prior to that it is considered a
50  // "microsoft extension". This silences the warnings.
51  #pragma warning( disable : 4231 )
52 #endif
53 
54 #include "CoreConfig.h"
55 #include "Logger.h"
56 #include "Math/consts.h"
57 #include <cmath>
58 #include <ostream>
59 #include <cassert>
60 
61 namespace Menge {
62 
63  namespace Math {
64 
65  // forward declaration
66  template< class Type >
67  class Vector3d;
68 
69  template< class Type >
70  inline MENGE_API Type abs( const Vector3d<Type>& v );
71 
75  template <class Type>
76  class MENGE_API Vector3d {
77 
78  public:
80 
83  inline Vector3d() {}
84 
92  inline Vector3d( Type x, Type y, Type z ): _x(x), _y(y), _z(z) {}
93 
99  inline Vector3d( const Vector3d & v ): _x(v._x), _y(v._y), _z(v._z) {}
100 
102 
108  inline Type x() const { return _x; }
109 
115  inline Type y() const { return _y; }
116 
122  inline Type z() const { return _z; }
123 
131  inline void set( Type x, Type y, Type z ) {
132  _x = x;
133  _y = y;
134  _z = z;
135  }
136 
142  inline void set( const Vector3d & v ) {
143  _x = v._x;
144  _y = v._y;
145  _z = v._z;
146  }
147 
153  inline void setX( Type x ) {
154  _x = x;
155  }
156 
162  inline void setY( Type y ) {
163  _y = y;
164  }
165 
171  inline void setZ( Type z ) {
172  _z = z;
173  }
174 
182  inline Type operator[]( const int i ) const {
183  assert( i >= 0 && i <= 2 && "Invalid index for Vector3" );
184  return i == 0 ? _x : ( i == 1 ? _y : _z );
185  }
186 
194  inline Type & operator[]( const int i ) {
195  assert( i >= 0 && i <= 2 && "Invalid index for Vector3" );
196  return i == 0 ? _x : ( i == 1 ? _y : _z );
197  }
198 
202  inline void zero() {
203  _x = _y = _z = (Type)0;
204  }
205 
207 
214  inline Vector3d<Type> operator-() const {
215  return Vector3d<Type>( -_x, -_y, -_z );
216  }
217 
226  inline float operator*(const Vector3d& v) const {
227  return _x * v._x + _y * v._y + _z * v._z;
228  }
229 
238  inline Vector3d<Type> operator*( float s ) const {
239  return Vector3d<Type>( _x * s, _y * s, _z * s );
240  }
241 
250  inline Vector3d<Type> operator/( float s ) const {
251  const float invS = 1.f / s;
252  return Vector3d<Type>( _x * invS, _y * invS, _z * invS );
253  }
254 
263  inline Vector3d operator+( const Vector3d & v ) const {
264  return Vector3d<Type>( _x + v._x, _y + v._y, _z + v._z );
265  }
266 
275  inline Vector3d operator-( const Vector3d & v ) const {
276  return Vector3d<Type>( _x - v._x, _y - v._y, _z - v._z );
277  }
278 
287  inline bool operator==( const Vector3d & v ) const {
288  return _x == v._x && _y == v._y && _z == v._z;
289  }
290 
299  inline bool operator!=( const Vector3d & v ) const {
300  return _x != v._x || _y != v._y || _z != v._z;
301  }
302 
304 
311  inline Vector3d & operator*=( float s ) {
312  _x *= s;
313  _y *= s;
314  _z *= s;
315  return *this;
316  }
317 
324  inline Vector3d & operator/=( float s ) {
325  const float invS = 1.f / s;
326  _x *= invS;
327  _y *= invS;
328  _z *= invS;
329  return *this;
330  }
331 
338  inline Vector3d & operator+=( const Vector3d & v ) {
339  _x += v._x;
340  _y += v._y;
341  _z += v._z;
342  return *this;
343  }
344 
351  inline Vector3d & operator-=( const Vector3d & v ) {
352  _x -= v._x;
353  _y -= v._y;
354  _z -= v._z;
355  return *this;
356  }
357 
361  inline void negate() {
362  _x = -_x;
363  _y = -_y;
364  _z = -_z;
365  }
366 
370  inline void normalize() {
371  float len = sqrtf( _x * _x + _y * _y + _z * _z );
372  if ( len > EPS ) {
373  _x /= len;
374  _y /= len;
375  _z /= len;
376  } else {
377  _x = _y = _z = 0.f;
378  }
379  }
380 
388  inline void SumScale( Type s, const Vector3d & v ) {
389  _x += v._x * s;
390  _y += v._y * s;
391  _z += v._z * s;
392  }
393 
395 
402  Vector3d rotateX( float angle ) const {
403  float newY = _y, newZ = _z;
404  rotatePair( angle, &newY, &newZ );
405  return Vector3d( _x, newY, newZ );
406  }
407 
414  Vector3d rotateY( float angle ) const {
415  float newX = _x, newZ = _z;
416  rotatePair( angle, &newX, &newZ );
417  return Vector3d( newX, _y, newZ );
418  }
419 
426  Vector3d rotateZ( float angle ) const {
427  float newX = _x, newY = _y;
428  rotatePair( -angle, &newX, &newY );
429  return Vector3d( newX, newY, _z );
430  }
431 
439  Vector3d rotateV( Type angle, const Vector3d & v ) const {
440  assert ( abs( v ) > 0.999 && abs( v ) < 1.001 );
441  Type c = cos( angle );
442  Type s = sin( angle );
443  Type omc = 1 - c;
444  // This is a hack -- it should be done with a matrix.....
445  Type vx = _x * (v._x * v._x * omc + c) + _y * (v._y * v._x * omc + v._z * s) + _z * (v._z * v._x * omc - v._y * s);
446  Type vy = _x * (v._x * v._y * omc - v._z * s) + _y * (v._y * v._y * omc + c) + _z * (v._z * v._y * omc + v._x * s);
447  Type vz = _x * (v._x * v._z * omc + v._y * s) + _y * (v._y * v._z * omc - v._x * s) + _z * (v._z * v._z * omc + c);
448  return Vector3d( vx, vy, vz );
449  }
450 
457  void rotateV_ip( Type angle, const Vector3d & v ) {
458  assert ( abs( v ) > 0.999 && abs( v ) < 1.001 );
459  Type c = cos( angle );
460  Type s = sin(angle);
461  Type omc = 1 - c;
462  // This is a hack -- it should be done with a matrix.....
463  Type vx = _x * (v._x * v._x * omc + c) + _y * (v._y * v._x * omc + v._z * s) + _z * (v._z * v._x * omc - v._y * s);
464  Type vy = _x * (v._x * v._y * omc - v._z * s) + _y * (v._y * v._y * omc + c) + _z * (v._z * v._y * omc + v._x * s);
465  Type vz = _x * (v._x * v._z * omc + v._y * s) + _y * (v._y * v._z * omc - v._x * s) + _z * (v._z * v._z * omc + c);
466  _x = vx;
467  _y = vy;
468  _z = vz;
469  }
470 
472 
478  inline Type Length() const {
479  return sqrt( _x * _x + _y * _y + _z * _z );
480  }
481 
489  inline Vector3d cross( const Vector3d<Type> & v ) {
490  return Vector3d<Type>( _y * v._z - v._y * _z,
491  _z * v._x - _x * v._z,
492  _x * v._y - _y * v._x);
493  }
494 
502  inline float distance( const Vector3d & p ) const {
503  float dx = _x - p._x;
504  float dy = _y - p._y;
505  float dz = _z - p._z;
506  return sqrtf( dx * dx + dy * dy + dz * dz );
507  }
508 
517  inline float distance( float x, float y, float z ) const {
518  float dx = _x - x;
519  float dy = _y - y;
520  float dz = _z - z;
521  return sqrtf( dx * dx + dy * dy + dz * dz );
522  }
523 
531  inline float distanceSq( const Vector3d & p ) const {
532  float dx = _x - p._x;
533  float dy = _y - p._y;
534  float dz = _z - p._z;
535  return dx * dx + dy * dy + dz * dz ;
536  }
537 
546  inline float distanceSq( float x, float y, float z ) const {
547  float dx = _x - x;
548  float dy = _y - y;
549  float dz = _z - z;
550  return dx * dx + dy * dy + dz * dz;
551  }
552 
556  Type _x;
557 
561  Type _y;
562 
566  Type _z;
567 
568  private:
576  void rotatePair( float angle, float * x, float * y ) const {
577  float c = cos( angle );
578  float s = sin( angle );
579  float newX = c * (*x) + s * (*y);
580  float newY = c * (*y) - s * (*x);
581  *x = newX;
582  *y = newY;
583  }
584  };
585 
590  MATHEXTERN template class MENGE_API Vector3d<float>;
591 
595  typedef Vector3d<float> Vector3;
596 
598 
599 
610  template< class Type >
611  inline MENGE_API Vector3d<Type> operator*( Type s, const Vector3d<Type>& v) {
612  return Vector3d<Type>( s * v._x, s * v._y, s * v._z );
613  }
614 
619  MATHEXTERN template MENGE_API Vector3d<float> operator*( float s, const Vector3d<float>& v);
620 
630  template< class Type >
631  inline MENGE_API Logger& operator<<(Logger& logger, const Vector3d<Type>& v ) {
632  logger << "(" << v.x() << "," << v.y() << ", " << v.z() << ")";
633  return logger;
634  }
635 
646  template< class Type >
647  inline MENGE_API std::ostream& operator<<(std::ostream& os, const Vector3d<Type>& v ) {
648  os << "(" << v.x() << "," << v.y() << ", " << v.z() << ")";
649  return os;
650  }
651 
656  MATHEXTERN template MENGE_API Logger& operator<<(Logger& os, const Vector3d<float>& v );
657 
665  template< class Type >
666  inline MENGE_API Type abs( const Vector3d<Type>& v ) {
667  return std::sqrt( v * v );
668  }
669 
674  MATHEXTERN template MENGE_API float abs( const Vector3d<float>& v );
675 
684  template< class Type >
685  inline MENGE_API Type absSq( const Vector3d<Type>& v ) {
686  return v * v;
687  }
688 
693  MATHEXTERN template MENGE_API float absSq( const Vector3d<float>& v );
694 
703  template< class Type >
704  inline MENGE_API Vector3d<Type> norm( const Vector3d<Type>& vector ) {
705  Type mag = abs( vector );
706  if ( mag < EPS ) {
707  // This may not be the "right" behavior. I do this because the "normalized" vector has unit
708  // length. This guarantees that the result is always unit length. Although it introduces other
709  // issues.
710  return Vector3d<Type>( 1.f, 0.f, 0.f );
711  } else {
712  return vector / mag;
713  }
714  }
715 
720  MATHEXTERN template MENGE_API Vector3d<float> norm( const Vector3d<float>& vector );
721 
732  template< class Type >
733  inline MENGE_API bool equivalent( const Vector3d<Type> & v1, const Vector3d<Type> & v2, float threshSqd=0.000001f ) {
734  return absSq( v1 - v2 ) < threshSqd;
735  }
736 
741  MATHEXTERN template MENGE_API bool equivalent( const Vector3d<float> & v1, const Vector3d<float> & v2, float threshSqd );
742 
743  // Possible operations
744  // Normal to a trio of points (i.e. normal of triangle)
745  // Optimized math operations
746  // add in scaled vector
747  // scale this add in scaled vector
748  // Getter/setter from Type pointer
749  // Linear interpolation
750  // reflection of one vector around another
751  // Compute plane from three points
752 
753  } // namespace Math
754 } // namespace Menge
755 #ifdef _MSC_VER
756  // To export templated classes and functions, requires declaring specializations
757  // extern. This became standard in C++11, but prior to that it is considered a
758  // "microsoft extension". This silences the warnings.
759  #pragma warning( default : 4231 )
760 #endif
761 
762 #endif
Vector3d & operator/=(float s)
Perform in-place scalar division on this vector.
Definition: Vector3.h:324
Vector3d rotateY(float angle) const
Rotate the vector around the y axis.
Definition: Vector3.h:414
Type _x
x-component of the vector
Definition: Vector3.h:556
The core namespace. All elements of Menge are contained in this namespace.
Definition: AgentGenerator.cpp:43
Vector3d(const Vector3d &v)
Copy constructor.
Definition: Vector3.h:99
float distance(float x, float y, float z) const
Compute the distance from this vector to another point.
Definition: Vector3.h:517
Vector3d & operator+=(const Vector3d &v)
Perform in-place vector addition on this vector.
Definition: Vector3.h:338
float distanceSq(const Vector3d &p) const
Compute the squared-distance from this vector to another point.
Definition: Vector3.h:531
Sets up the proper compiler directives for platform and dll export/import.
Vector3d operator-(const Vector3d &v) const
Computes the vector difference of this vector with the given vector.
Definition: Vector3.h:275
Vector3d & operator-=(const Vector3d &v)
Perform in-place vector subtraction on this vector.
Definition: Vector3.h:351
Vector3d< Type > operator*(float s) const
Computes the scalar multiplication of this vector with the given scalar value.
Definition: Vector3.h:238
Type y() const
Get the y-value.
Definition: Vector3.h:115
Type _y
y-component of the vector
Definition: Vector3.h:561
float distanceSq(float x, float y, float z) const
Compute the squared-distance from this vector to another point.
Definition: Vector3.h:546
void rotateV_ip(Type angle, const Vector3d &v)
Rotate the vector around an arbitrary vector - change the vector in place.
Definition: Vector3.h:457
A compilation of file system operations.
Type _z
z-component of the vector
Definition: Vector3.h:566
void normalize()
Normalize the vector in place.
Definition: Vector3.h:370
MENGE_API const float EPS
Suitably small number for testing for functional zero values.
Definition: geomQuery.cpp:52
bool operator!=(const Vector3d &v) const
Reports if this vector is the different from the given vector.
Definition: Vector3.h:299
float operator*(const Vector3d &v) const
Computes the dot product of this vector with the given vector.
Definition: Vector3.h:226
void set(const Vector3d &v)
Set the x-, y- and z-values from a vector.
Definition: Vector3.h:142
void set(Type x, Type y, Type z)
Set the x- and y-values from scalar values.
Definition: Vector3.h:131
Type & operator[](const int i)
Index-style access to vector components as a reference.
Definition: Vector3.h:194
Type operator[](const int i) const
Index-style access to vector components.
Definition: Vector3.h:182
Type Length() const
Compute the magnitude (aka length) of the vector.
Definition: Vector3.h:478
The specificaiton of a message logger for menge, such that all messages to the system get properly re...
Some common mathematical constants.
Vector3d(Type x, Type y, Type z)
Constructor with arguments.
Definition: Vector3.h:92
Vector3d rotateX(float angle) const
Rotate the vector around the x axis.
Definition: Vector3.h:402
The namespace for math primitives for simulation and visualization.
float distance(const Vector3d &p) const
Compute the distance from this vector to another point.
Definition: Vector3.h:502
Vector3d rotateV(Type angle, const Vector3d &v) const
Rotate the vector around an arbitrary vector.
Definition: Vector3.h:439
Vector3d cross(const Vector3d< Type > &v)
Cross product of this vector with the given vector this x v.
Definition: Vector3.h:489
Vector3d()
Default constructor. It does NOT initialize the fields.
Definition: Vector3.h:83
Vector3d< Type > operator-() const
Vector negation. Creates a new vector which is the negation of this vector.
Definition: Vector3.h:214
void setZ(Type z)
Set the z-value.
Definition: Vector3.h:171
bool operator==(const Vector3d &v) const
Reports if this vector is the same as the given vector.
Definition: Vector3.h:287
Logger logger
Globally available Logger.
Definition: Logger.cpp:49
Type x() const
Get the x-value.
Definition: Vector3.h:108
Type z() const
Get the z-value.
Definition: Vector3.h:122
void setX(Type x)
Set the x-value.
Definition: Vector3.h:153
void negate()
Negate the vector in place.
Definition: Vector3.h:361
void zero()
Set the vector to zero.
Definition: Vector3.h:202
void setY(Type y)
Set the y-value.
Definition: Vector3.h:162
void SumScale(Type s, const Vector3d &v)
Adds in a scaled version of another vector this += s * v.
Definition: Vector3.h:388
Vector3d & operator*=(float s)
Perform in-place scalar multiplication on this vector.
Definition: Vector3.h:311
Vector3d operator+(const Vector3d &v) const
Computes the vector sum of this vector with the given vector.
Definition: Vector3.h:263
Templated vector in R3.
Definition: Vector3.h:67
Vector3d< Type > operator/(float s) const
Computes the scalar division of this vector with the given scalar value.
Definition: Vector3.h:250
Vector3d rotateZ(float angle) const
Rotate the vector around the z axis.
Definition: Vector3.h:426