CGR Localization
 All Classes Namespaces Files Functions Variables Macros Pages
gvector.h
1 /*========================================================================
2  GVector.h : Simple vector class for 2D and 3D vectors
3  ------------------------------------------------------------------------
4  Copyright (C) 1999-2006 James R. Bruce
5  Modified by Joydeep Biswas, 2009-2011
6  School of Computer Science, Carnegie Mellon University
7  ------------------------------------------------------------------------
8  This software is distributed under the GNU General Public License,
9  version 2. If you do not have a copy of this licence, visit
10  www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
11  Suite 330 Boston, MA 02111-1307 USA. This program is distributed
12  in the hope that it will be useful, but WITHOUT ANY WARRANTY,
13  including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
14  ========================================================================*/
15 
16 #ifndef __GVECTOR_H__
17 #define __GVECTOR_H__
18 
19 #include <math.h>
20 #include "util.h"
21 #include <xmmintrin.h>
22 #include <smmintrin.h>
23 
24 #define V3COMP(p) (p).x,(p).y,(p).z
25 #define V2COMP(p) (p).x,(p).y
26 #define pBool(var) (var)? ("true"):("false")
27 
28 #define GVECTOR_SSE_OPTIMIZATIONS
29 
30 #define VECTOR_TEM \
31 template <class num>
32 
33 namespace GVector {
34 
35 //=====================================================================//
36 // Vector3D Class
37 //=====================================================================//
38 
39 #define EPSILON (1.0E-10)
40 
41 #ifdef GVECTOR_SSE_OPTIMIZATIONS
42 template <typename num>
43 inline num sse_sqrt(num xin)
44 {
45  __m128d x = _mm_set_sd(double(xin));
46  __m128d y = _mm_set_sd(double(xin));
47  x = _mm_sqrt_sd(x,y);
48  double retVal;
49  _mm_store_sd(&retVal,x);
50  return(retVal);
51 }
52 
53 template <> inline float sse_sqrt<float>(float xin)
54 {
55  __m128 x = _mm_set_ss((float) xin);
56  x = _mm_sqrt_ss(x);
57  float retVal;
58  _mm_store_ss(&retVal,x);
59  return(retVal);
60 }
61 
62 template <> inline double sse_sqrt<double>(double xin)
63 {
64  __m128d x = _mm_set_sd(xin);
65  __m128d y = _mm_set_sd(xin);
66  x = _mm_sqrt_sd(x,y);
67  double retVal;
68  _mm_store_sd(&retVal,x);
69  return(retVal);
70 }
71 
72 template <typename num>
73 inline num sse_dot_product(const num* fv1, const num* fv2, const int mask)
74 {
75  __m128 v1 = _mm_set_ps(float(fv1[3]),float(fv1[2]),float(fv1[1]),float(fv1[0]));
76  __m128 v2 = _mm_set_ps(float(fv2[3]),float(fv2[2]),float(fv2[1]),float(fv2[0]));
77  __m128 dp = _mm_dp_ps(v1,v2,mask);
78  float res;
79  _mm_store_ss(&res,dp);
80  return res;
81 }
82 
83 template <>
84 inline float sse_dot_product(const float* fv1, const float* fv2, const int mask)
85 {
86  __m128 v1 = _mm_load_ps(fv1);
87  __m128 v2 = _mm_load_ps(fv2);
88  __m128 dp = _mm_dp_ps(v1,v2,mask);
89  float res;
90  _mm_store_ss(&res,dp);
91  return res;
92 }
93 
94 template <>
95 inline double sse_dot_product(const double* fv1, const double* fv2, const int mask)
96 {
97  __m128d v1 = _mm_load_pd(fv1);
98  __m128d v2 = _mm_load_pd(fv2);
99  __m128d dp1 = _mm_dp_pd(v1,v2,0x31);
100  v1 = _mm_load_pd(&fv1[2]);
101  v2 = _mm_load_pd(&fv2[2]);
102  __m128d dp2 = _mm_dp_pd(v1,v2,mask);
103  __m128d dp = _mm_add_sd(dp1,dp2);
104  double res;
105  _mm_store_sd(&res,dp);
106  return res;
107 }
108 
109 #endif
110 
111 template <class num> inline num mysqrt(num x)
112 {
113 #ifdef GVECTOR_SSE_OPTIMIZATIONS
114  return sse_sqrt<num>(x);
115 #else
116  return sqrt(x);
117 #endif
118 }
119 
120 template <class num> class vector3d;
121 
122 template <class num>
123 class matrix3d{
124 public:
125  union {
126  struct{
127  num m11;
128  num m12;
129  num m13;
130  num m14;
131  num m21;
132  num m22;
133  num m23;
134  num m24;
135  num m31;
136  num m32;
137  num m33;
138  num m34;
139  num m41;
140  num m42;
141  num m43;
142  num m44;
143  };
144  num elements[16];
145  }__attribute__ ((aligned (16)));
146 
147  void setxyzRotations(num rx, num ry, num rz);
148  void xyzRotationAndTransformation(num rx, num ry, num rz, vector3d<num> t);
149  matrix3d<num> operator*(const matrix3d< num > &M2) const;
150  matrix3d<num> transpose() const;
151 };
152 
153 template <class num> void matrix3d<num>::setxyzRotations(num rx, num ry, num rz)
154 {
155  double cx = cos(rx);
156  double sx = sin(rx);
157  double cy = cos(ry);
158  double sy = sin(ry);
159  double cz = cos(rz);
160  double sz = sin(rz);
161 
162  m11 = cz*cy; m12 = -sz; m13 = cz*sy; m14 = 0.0;
163  m21 = cx*sz*cy+sx*sy; m22 = cx*cz; m23 = cx*sz*sy-sx*cy; m24 = 0.0;
164  m31 = sx*sz*cy-cx*sy; m32 = sx*cz; m33 = cx*cy; m34 = 0.0;
165  m41 = 0.0; m42 = 0.0; m43 = 0.0; m44 = 1.0;
166 }
167 
168 template <class num> void matrix3d<num>::xyzRotationAndTransformation(num rx, num ry, num rz, vector3d< num > t)
169 {
170  double cx = cos(rx);
171  double sx = sin(rx);
172  double cy = cos(ry);
173  double sy = sin(ry);
174  double cz = cos(rz);
175  double sz = sin(rz);
176 
177  m11 = cz*cy; m12 = -sz; m13 = cz*sy; m14 = t.x;
178  m21 = cx*sz*cy+sx*sy; m22 = cx*cz; m23 = cx*sz*sy-sx*cy; m24 = t.y;
179  m31 = sx*sz*cy-cx*sy; m32 = sx*cz; m33 = cx*cy; m34 = t.z;
180  m41 = 0.0; m42 = 0.0; m43 = 0.0; m44 = 1.0;
181 }
182 
183 template <class num> matrix3d<num> matrix3d<num>::operator*(const matrix3d< num >& M2) const
184 {
185  #define m1(i,j) m ## i ## j
186  #define m2(i,j) M2.m ## i ## j
187  #define m3(i,j) M3.m ## i ## j
188 
189  matrix3d<num> M3;
190 
191  m3(1,1) = m1(1,1)*m2(1,1) + m1(1,2)*m2(2,1) + m1(1,3)*m2(3,1) + m1(1,4)*m2(4,1);
192  m3(1,2) = m1(1,1)*m2(1,2) + m1(1,2)*m2(2,2) + m1(1,3)*m2(3,2) + m1(1,4)*m2(4,2);
193  m3(1,3) = m1(1,1)*m2(1,3) + m1(1,2)*m2(2,3) + m1(1,3)*m2(3,3) + m1(1,4)*m2(4,3);
194  m3(1,4) = m1(1,1)*m2(1,4) + m1(1,2)*m2(2,4) + m1(1,3)*m2(3,4) + m1(1,4)*m2(4,4);
195 
196  m3(2,1) = m1(2,1)*m2(1,1) + m1(2,2)*m2(2,1) + m1(2,3)*m2(3,1) + m1(2,4)*m2(4,1);
197  m3(2,2) = m1(2,1)*m2(1,2) + m1(2,2)*m2(2,2) + m1(2,3)*m2(3,2) + m1(2,4)*m2(4,2);
198  m3(2,3) = m1(2,1)*m2(1,3) + m1(2,2)*m2(2,3) + m1(2,3)*m2(3,3) + m1(2,4)*m2(4,3);
199  m3(2,4) = m1(2,1)*m2(1,4) + m1(2,2)*m2(2,4) + m1(2,3)*m2(3,4) + m1(2,4)*m2(4,4);
200 
201  m3(3,1) = m1(3,1)*m2(1,1) + m1(3,2)*m2(2,1) + m1(3,3)*m2(3,1) + m1(3,4)*m2(4,1);
202  m3(3,2) = m1(3,1)*m2(1,2) + m1(3,2)*m2(2,2) + m1(3,3)*m2(3,2) + m1(3,4)*m2(4,2);
203  m3(3,3) = m1(3,1)*m2(1,3) + m1(3,2)*m2(2,3) + m1(3,3)*m2(3,3) + m1(3,4)*m2(4,3);
204  m3(3,4) = m1(3,1)*m2(1,4) + m1(3,2)*m2(2,4) + m1(3,3)*m2(3,4) + m1(3,4)*m2(4,4);
205 
206  m3(4,1) = m1(4,1)*m2(1,1) + m1(4,2)*m2(2,1) + m1(4,3)*m2(3,1) + m1(4,4)*m2(4,1);
207  m3(4,2) = m1(4,1)*m2(1,2) + m1(4,2)*m2(2,2) + m1(4,3)*m2(3,2) + m1(4,4)*m2(4,2);
208  m3(4,3) = m1(4,1)*m2(1,3) + m1(4,2)*m2(2,3) + m1(4,3)*m2(3,3) + m1(4,4)*m2(4,3);
209  m3(4,4) = m1(4,1)*m2(1,4) + m1(4,2)*m2(2,4) + m1(4,3)*m2(3,4) + m1(4,4)*m2(4,4);
210 
211  return M3;
212 }
213 
214 template <class num> matrix3d<num> matrix3d<num>::transpose() const
215 {
216  matrix3d<num> M2 = *this;
217  swap(M2.m12,M2.m21);
218  swap(M2.m13,M2.m31);
219  swap(M2.m14,M2.m41);
220  swap(M2.m23,M2.m32);
221  swap(M2.m24,M2.m42);
222  swap(M2.m34,M2.m43);
223 
224  return M2;
225 }
226 
227 template <class num>
228 class vector3d{
229 public:
230  union {
231  struct{
232  num x;
233  num y;
234  num z;
235  num w;
236  };
237  num coefficients[4];
238  }__attribute__ ((aligned (16)));
239  //num x,y,z;
240 
241  vector3d()
242  {x=y=z=0.0;w=1.0;}
243  vector3d(num nx,num ny,num nz)
244  {x=nx; y=ny; z=nz; w=1.0;}
245  vector3d(num nx,num ny,num nz,num nw)
246  {x=nx; y=ny; z=nz; w=nw;}
247  void set(num nx,num ny,num nz)
248  {x=nx; y=ny; z=nz; w=1.0;}
249  void set(num nx,num ny,num nz,num nw)
250  {x=nx; y=ny; z=nz; w=nw;}
251  void setAll(num nx)
252  {x=y=z=nx; w=1.0;}
253  void set(vector3d<num> p)
254  {x=p.x; y=p.y; z=p.z; w = p.w;}
255  void zero()
256  {x=y=z=0; w=1.0;}
257 
258  vector3d<num> &operator=(const vector3d<num> p)
259  {set(p); return(*this);}
260 
262  num &operator[](int idx)
263  {return(coefficients[idx]);}
264  const num &operator[](int idx) const
265  {return(coefficients[idx]);}
266 
267  num length() const MustUseResult;
268  num sqlength() const MustUseResult;
269  vector3d<num> norm() const MustUseResult;
270  vector3d<num> norm(const num len) const MustUseResult;
271  void normalize();
272  bool nonzero() const MustUseResult
273  {return(x!=0 || y!=0 || z!=0);}
274 
275  num dot(const vector3d<num> p) const MustUseResult;
276  vector3d<num> cross(const vector3d<num> p) const MustUseResult;
277 
278  vector3d<num> &operator+=(const vector3d<num> p);
279  vector3d<num> &operator-=(const vector3d<num> p);
280  vector3d<num> &operator*=(const vector3d<num> p);
281  vector3d<num> &operator/=(const vector3d<num> p);
282 
283  vector3d<num> operator+(const vector3d<num> p) const;
284  vector3d<num> operator-(const vector3d<num> p) const;
285  vector3d<num> operator*(const vector3d<num> p) const;
286  vector3d<num> operator/(const vector3d<num> p) const;
287 
288  vector3d<num> operator*(num f) const;
289  vector3d<num> operator/(num f) const;
290  vector3d<num> &operator*=(num f);
291  vector3d<num> &operator/=(num f);
292 
293  vector3d<num> operator-() const;
294 
295  bool operator==(const vector3d<num> p) const;
296  bool operator!=(const vector3d<num> p) const;
297  bool operator< (const vector3d<num> p) const;
298  bool operator> (const vector3d<num> p) const;
299  bool operator<=(const vector3d<num> p) const;
300  bool operator>=(const vector3d<num> p) const;
301 
302  vector3d<num> rotate(const vector3d<num> r,const double a) const;
303  vector3d<num> transform(const matrix3d<num> &M) const;
304  vector3d<num> rotate_x(const double a) const;
305  vector3d<num> rotate_y(const double a) const;
306  vector3d<num> rotate_z(const double a) const;
307 
308  //double shortest_angle(const vector3d<num> a,const vector3d<num> b);
309  //vector3d<num> shortest_axis(const vector3d<num> a,const vector3d<num> b);
310 
311  bool finite() const MustUseResult
312  {return(::finite(x) && ::finite(y) && ::finite(z));}
313 
314  void take_min(const vector3d<num> p);
315  void take_max(const vector3d<num> p);
316 };
317 
318 #ifdef GVECTOR_SSE_OPTIMIZATIONS
319 template <> float vector3d<float>::sqlength() const;
320 
321 template <> float vector3d<float>::dot(const vector3d<float> p) const;
322 
323 template <> vector3d<float> vector3d<float>::transform(const matrix3d<float> &M) const;
324 #endif
325 
326 VECTOR_TEM num vector3d<num>::length() const
327 {
328  return(mysqrt<num>(x*x + y*y + z*z));
329 
330 }
331 
332 VECTOR_TEM num vector3d<num>::sqlength() const
333 {
334 #ifdef GVECTOR_SSE_OPTIMIZATIONS
335  __m128 p = _mm_set_ps((float)x,(float)y,(float)z,0.0f);
336  p = _mm_mul_ps(p,p);
337  __attribute__((aligned (16))) float x2[4];
338  _mm_store_ps(x2,p);
339  return(x2[3]+x2[2]+x2[1]);
340 #else
341  return(x*x + y*y + z*z);
342 #endif
343 }
344 
345 VECTOR_TEM vector3d<num> vector3d<num>::norm() const
346 {
347 #ifdef GVECTOR_SSE_OPTIMIZATIONS
348  vector3d<num> p;
349  num lInv = 1.0/(this->length());
350 
351  p.x = x * lInv;
352  p.y = y * lInv;
353  p.z = z * lInv;
354 
355  return(p);
356 #else
357  vector3d<num> p;
358  num l;
359 
360  l = mysqrt<num>(x*x + y*y + z*z);
361  p.x = x / l;
362  p.y = y / l;
363  p.z = z / l;
364 
365  return(p);
366 #endif
367 }
368 
369 VECTOR_TEM vector3d<num> vector3d<num>::norm(const num len) const
370 {
371 #ifdef GVECTOR_SSE_OPTIMIZATIONS
372  vector3d<num> p;
373 
374  __m128 f = _mm_set1_ps(((float)len) / (this->length()));
375  __m128 pm = _mm_set_ps((float)x,(float)y,(float)z,0.0f);
376  pm = _mm_mul_ps(pm,f);
377  __attribute__((aligned (16))) float x2[4];
378  _mm_store_ps(x2,pm);
379  p.x = x2[3];
380  p.y = x2[2];
381  p.z = x2[1];
382 
383  return(p);
384 #else
385  vector3d<num> p;
386  num f;
387 
388  f = len / mysqrt<num>(x*x + y*y + z*z);
389  p.x = x * f;
390  p.y = y * f;
391  p.z = z * f;
392 
393  return(p);
394 #endif
395 }
396 
397 VECTOR_TEM void vector3d<num>::normalize()
398 {
399 #ifdef GVECTOR_SSE_OPTIMIZATIONS
400  __m128 f = _mm_set1_ps(1.0f / (this->length()));
401  __m128 pm = _mm_set_ps((float)x,(float)y,(float)z,0.0f);
402  pm = _mm_mul_ps(pm,f);
403  __attribute__((aligned (16))) float x2[4];
404  _mm_store_ps(x2,pm);
405  x = x2[3];
406  y = x2[2];
407  z = x2[1];
408  return;
409 #else
410  num l;
411 
412  l = mysqrt<num>(x*x + y*y + z*z);
413  x /= l;
414  y /= l;
415  z /= l;
416 #endif
417 }
418 
419 VECTOR_TEM num vector3d<num>::dot(const vector3d<num> p) const
420 {
421  return(x*p.x + y*p.y + z*p.z);
422 }
423 
424 VECTOR_TEM num dot(const vector3d<num> a,const vector3d<num> b)
425 {
426  return(a.x*b.x + a.y*b.y + a.z*b.z);
427 }
428 
429 VECTOR_TEM num absdot(const vector3d<num> a,const vector3d<num> b)
430 {
431  return(fabs(a.x*b.x) + fabs(a.y*b.y) + fabs(a.z*b.z));
432 }
433 
434 VECTOR_TEM vector3d<num> vector3d<num>::cross(const vector3d<num> p) const
435 {
436  vector3d<num> r;
437 
438  // right handed
439  r.x = y*p.z - z*p.y;
440  r.y = z*p.x - x*p.z;
441  r.z = x*p.y - y*p.x;
442 
443  return(r);
444 }
445 
446 VECTOR_TEM vector3d<num> cross(const vector3d<num> a,const vector3d<num> b)
447 {
448  vector3d<num> r;
449 
450  // right handed
451  r.x = a.y*b.z - a.z*b.y;
452  r.y = a.z*b.x - a.x*b.z;
453  r.z = a.x*b.y - a.y*b.x;
454 
455  return(r);
456 }
457 
458 #define VECTOR3D_EQUAL_BINARY_OPERATOR(opr) \
459  template <class num> \
460  vector3d<num> &vector3d<num>::operator opr (const vector3d<num> p) \
461  { \
462  x opr p.x; \
463  y opr p.y; \
464  z opr p.z; \
465  return(*this); \
466  }
467 
468 VECTOR3D_EQUAL_BINARY_OPERATOR(+=)
469 VECTOR3D_EQUAL_BINARY_OPERATOR(-=)
470 VECTOR3D_EQUAL_BINARY_OPERATOR(*=)
471 VECTOR3D_EQUAL_BINARY_OPERATOR(/=)
472 
473 #define VECTOR3D_BINARY_OPERATOR(opr) \
474  template <class num> \
475  vector3d<num> vector3d<num>::operator opr (const vector3d<num> p) const \
476  { \
477  vector3d<num> r; \
478  r.x = x opr p.x; \
479  r.y = y opr p.y; \
480  r.z = z opr p.z; \
481  return(r); \
482  }
483 
484 VECTOR3D_BINARY_OPERATOR(+)
485 VECTOR3D_BINARY_OPERATOR(-)
486 VECTOR3D_BINARY_OPERATOR(*)
487 VECTOR3D_BINARY_OPERATOR(/)
488 
489 #define VECTOR3D_SCALAR_OPERATOR(opr) \
490  template <class num> \
491  vector3d<num> vector3d<num>::operator opr (const num f) const \
492  { \
493  vector3d<num> r; \
494  r.x = x opr f; \
495  r.y = y opr f; \
496  r.z = z opr f; \
497  return(r); \
498  }
499 
500 VECTOR3D_SCALAR_OPERATOR(*)
501 VECTOR3D_SCALAR_OPERATOR(/)
502 
503 #define VECTOR3D_EQUAL_SCALAR_OPERATOR(opr) \
504  template <class num> \
505  vector3d<num> &vector3d<num>::operator opr (num f) \
506  { \
507  x opr f; \
508  y opr f; \
509  z opr f; \
510  return(*this); \
511  }
512 
513 VECTOR3D_EQUAL_SCALAR_OPERATOR(*=)
514 VECTOR3D_EQUAL_SCALAR_OPERATOR(/=)
515 
516 #define VECTOR3D_LOGIC_OPERATOR(opr,combine) \
517  template <class num> \
518  bool vector3d<num>::operator opr (const vector3d<num> p) const \
519  { \
520  return((x opr p.x) combine \
521  (y opr p.y) combine \
522  (z opr p.z)); \
523  }
524 
525 VECTOR3D_LOGIC_OPERATOR(==,&&)
526 VECTOR3D_LOGIC_OPERATOR(!=,||)
527 
528 VECTOR3D_LOGIC_OPERATOR(< ,&&)
529 VECTOR3D_LOGIC_OPERATOR(> ,&&)
530 VECTOR3D_LOGIC_OPERATOR(<=,&&)
531 VECTOR3D_LOGIC_OPERATOR(>=,&&)
532 
533 VECTOR_TEM vector3d<num> vector3d<num>::operator-() const
534 {
535  vector3d<num> r;
536 
537  r.x = -x;
538  r.y = -y;
539  r.z = -z;
540 
541  return(r);
542 }
543 
544 template<class num1, class num2> vector3d<num2> operator*(num1 f,const vector3d<num2> &a)
545 {
546  vector3d<num2> r;
547 
548  r.x = f * a.x;
549  r.y = f * a.y;
550  r.z = f * a.z;
551 
552  return(r);
553 }
554 
555 VECTOR_TEM vector3d<num> abs(vector3d<num> a)
556 {
557  a.x = ::fabs(a.x);
558  a.y = ::fabs(a.y);
559  a.z = ::fabs(a.z);
560 
561  return(a);
562 }
563 
564 VECTOR_TEM vector3d<num> min(vector3d<num> a,vector3d<num> b)
565 {
566  vector3d<num> v;
567 
568  v.x = ::min(a.x,b.x);
569  v.y = ::min(a.y,b.y);
570  v.z = ::min(a.z,b.z);
571 
572  return(v);
573 }
574 
575 VECTOR_TEM vector3d<num> max(vector3d<num> a,vector3d<num> b)
576 {
577  vector3d<num> v;
578 
579  v.x = ::max(a.x,b.x);
580  v.y = ::max(a.y,b.y);
581  v.z = ::max(a.z,b.z);
582 
583  return(v);
584 }
585 
586 VECTOR_TEM vector3d<num> bound(vector3d<num> v,num low,num high)
587 {
588  v.x = ::bound(v.x,low,high);
589  v.y = ::bound(v.y,low,high);
590  v.z = ::bound(v.z,low,high);
591 
592  return(v);
593 }
594 
595 // returns point rotated around axis <r> by <a> radians (right handed)
596 VECTOR_TEM vector3d<num> vector3d<num>::rotate(const vector3d<num> r,const double a) const
597 {
598  vector3d<num> q;
599  double s,c,t;
600 
601  s = sin(a);
602  c = cos(a);
603  t = 1 - c;
604 
605  q.x = (t * r.x * r.x + c ) * x
606  + (t * r.x * r.y - s * r.z) * y
607  + (t * r.x * r.z + s * r.y) * z;
608 
609  q.y = (t * r.y * r.x + s * r.z) * x
610  + (t * r.y * r.y + c ) * y
611  + (t * r.y * r.z - s * r.x) * z;
612 
613  q.z = (t * r.z * r.x - s * r.y) * x
614  + (t * r.z * r.y + s * r.x) * y
615  + (t * r.z * r.z + c ) * z;
616 
617  return(q);
618 }
619 
620 VECTOR_TEM vector3d<num> vector3d<num>::transform(const matrix3d< num >& M) const
621 {
622  vector3d<num> q;
623  q.x = M.m11*x + M.m12*y + M.m13*z + M.m14*w;
624  q.y = M.m21*x + M.m22*y + M.m23*z + M.m24*w;
625  q.z = M.m31*x + M.m32*y + M.m33*z + M.m34*w;
626 
627  return(q);
628 }
629 
630 // returns point rotated around X axis by <a> radians (right handed)
631 VECTOR_TEM vector3d<num> vector3d<num>::rotate_x(const double a) const
632 {
633  vector3d<num> q;
634  double s,c;
635 
636  s = sin(a);
637  c = cos(a);
638 
639  q.x = x;
640  q.y = c*y + -s*z;
641  q.z = s*y + c*z;
642 
643  return(q);
644 }
645 
646 // returns point rotated around Y axis by <a> radians (right handed)
647 VECTOR_TEM vector3d<num> vector3d<num>::rotate_y(const double a) const
648 {
649  vector3d<num> q;
650  double s,c;
651 
652  s = sin(a);
653  c = cos(a);
654 
655  q.x = c*x + s*z;
656  q.y = y;
657  q.z = -s*x + c*z;
658 
659  return(q);
660 }
661 
662 // returns point rotated around Z axis by <a> radians (right handed)
663 VECTOR_TEM vector3d<num> vector3d<num>::rotate_z(const double a) const
664 {
665  vector3d<num> q;
666  double s,c;
667 
668  s = sin(a);
669  c = cos(a);
670 
671  q.x = c*x + -s*y;
672  q.y = s*x + c*y;
673  q.z = z;
674 
675  return(q);
676 }
677 
678 VECTOR_TEM double shortest_angle(const vector3d<num> a,const vector3d<num> b)
679 {
680  return(acos(std::max(-1.0,std::min(1.0,dot(a,b)/(a.length()*b.length())))));
681 }
682 
683 VECTOR_TEM vector3d<num> shortest_axis(const vector3d<num> a,const vector3d<num> b)
684 {
685  return(cross(a,b).norm());
686 }
687 
688 
689 // set the vector to the minimum of its components and p's components
690 VECTOR_TEM void vector3d<num>::take_min(const vector3d<num> p)
691 {
692  if(p.x < x) x = p.x;
693  if(p.y < y) y = p.y;
694  if(p.z < z) z = p.z;
695 }
696 
697 // set the vector to the maximum of its components and p's components
698 VECTOR_TEM void vector3d<num>::take_max(const vector3d<num> p)
699 {
700  if(p.x > x) x = p.x;
701  if(p.y > y) y = p.y;
702  if(p.z > z) z = p.z;
703 }
704 
705 // returns distance between two points
706 VECTOR_TEM num dist(const vector3d<num> a,const vector3d<num> b)
707 {
708  num dx,dy,dz;
709 
710  dx = a.x - b.x;
711  dy = a.y - b.y;
712  dz = a.z - b.z;
713 
714  return(mysqrt<num>(dx*dx + dy*dy + dz*dz));
715 }
716 
717 VECTOR_TEM num distance(const vector3d<num> a,const vector3d<num> b)
718 {
719  return(dist(a,b));
720 }
721 
722 // returns square of distance between two points
723 VECTOR_TEM num sqdist(const vector3d<num> a,const vector3d<num> b)
724 {
725  num dx,dy,dz;
726 
727  dx = a.x - b.x;
728  dy = a.y - b.y;
729  dz = a.z - b.z;
730 
731  return(dx*dx + dy*dy + dz*dz);
732 }
733 
734 VECTOR_TEM num sqdistance(const vector3d<num> a,const vector3d<num> b)
735 {
736  return(sqdist(a,b));
737 }
738 
739 // returns distance from point p to line x0-x1
740 VECTOR_TEM num distance_to_line(const vector3d<num> x0,const vector3d<num> x1,const vector3d<num> p)
741 {
742  // FIXME: this is probably broken
743  vector3d<num> x;
744  num t;
745 
746  t = ((p.x - x0.x) + (p.y - x0.y) + (p.z - x0.z)) / (x1.x + x1.y + x1.z);
747  x = x0 + (x1 - x0) * t;
748 
749  return(distance(x,p));
750 }
751 
752 
753 //=====================================================================//
754 // Vector2D Class
755 //=====================================================================//
756 
757 template <class num>
758 class vector2d{
759 public:
760  num x,y;
761 
762  vector2d()
763  {}
764  vector2d(num nx,num ny)
765  {x=nx; y=ny;}
766 
768  void set(num nx,num ny)
769  {x=nx; y=ny;}
771  void setAll(num nx)
772  {x=y=nx;}
774  template <class vec> void set(vec p)
775  {x=p.x; y=p.y;}
777  void zero()
778  {x=y=0;}
779 
781  vector2d<num> &operator=(vector2d<num> p)
782  {set(p); return(*this);}
783 
785  template <class vec> vector2d<num> &operator=(vec p)
786  {set(p.x,p.y); return(*this);}
787 
789  num &operator[](int idx)
790  {return(((num*)this)[idx]);}
791  const num &operator[](int idx) const
792  {return(((num*)this)[idx]);}
793 
795  num length() const MustUseResult;
797  num sqlength() const MustUseResult;
799  num angle() const MustUseResult
800  {return(atan2(y,x));}
802  void heading(num angle)
803  {x=cos(angle); y=sin(angle);}
804 
806  vector2d<num> norm() const MustUseResult;
808  vector2d<num> norm(const num len) const MustUseResult;
810  void normalize();
812  vector2d<num> bound(const num max_length) const MustUseResult;
814  bool nonzero() const MustUseResult
815  {return(x!=0 || y!=0);}
816 
818  template <class vec> num dot(const vec p) const MustUseResult;
820  num perpdot(const vector2d<num> p) const MustUseResult;
822  num cross(const vector2d<num> p) const MustUseResult;
823 
825  vector2d<num> perp() const MustUseResult
826  {return(vector2d(-y, x));}
827 
829  vector2d<num> &operator+=(const vector2d<num> p);
831  vector2d<num> &operator-=(const vector2d<num> p);
833  vector2d<num> &operator*=(const vector2d<num> p);
835  vector2d<num> &operator/=(const vector2d<num> p);
836 
838  vector2d<num> operator+(const vector2d<num> p) const;
840  vector2d<num> operator-(const vector2d<num> p) const;
842  vector2d<num> operator*(const vector2d<num> p) const;
844  vector2d<num> operator/(const vector2d<num> p) const;
845 
847  vector2d<num> operator*(const num f) const;
849  vector2d<num> operator/(const num f) const;
851  vector2d<num> &operator*=(num f);
853  vector2d<num> &operator/=(num f);
854 
856  vector2d<num> operator-() const;
857 
858  bool operator==(const vector2d<num> p) const;
859  bool operator!=(const vector2d<num> p) const;
860  bool operator< (const vector2d<num> p) const;
861  bool operator> (const vector2d<num> p) const;
862  bool operator<=(const vector2d<num> p) const;
863  bool operator>=(const vector2d<num> p) const;
864 
866  vector2d<num> rotate(const double a) const MustUseResult;
867 
868  // vector2d<num> project(const vector2d<num> p) const;
869  vector2d<num> project_in(const vector2d<num> p) const MustUseResult;
870  vector2d<num> project_out(const vector2d<num> p) const MustUseResult;
871 
873  bool finite() const MustUseResult
874  {return(::finite(x) && ::finite(y));}
875 
877  void take_min(const vector2d<num> p);
879  void take_max(const vector2d<num> p);
880 };
881 
882 VECTOR_TEM num vector2d<num>::length() const
883 {
884  return(mysqrt<num>(x*x + y*y));
885 }
886 
887 VECTOR_TEM num vector2d<num>::sqlength() const
888 {
889  return(x*x + y*y);
890 }
891 
892 VECTOR_TEM vector2d<num> vector2d<num>::norm() const
893 {
894  vector2d<num> p;
895  num l;
896 
897  l = mysqrt<num>(x*x + y*y);
898  p.x = x / l;
899  p.y = y / l;
900 
901  return(p);
902 }
903 
904 VECTOR_TEM vector2d<num> vector2d<num>::norm(const num len) const
905 {
906  vector2d<num> p;
907  num f;
908 
909  f = len / mysqrt<num>(x*x + y*y);
910  p.x = x * f;
911  p.y = y * f;
912 
913  return(p);
914 }
915 
916 VECTOR_TEM void vector2d<num>::normalize()
917 {
918  num l;
919 
920  l = mysqrt<num>(x*x + y*y);
921  x /= l;
922  y /= l;
923 }
924 
925 VECTOR_TEM vector2d<num> vector2d<num>::bound(const num max_length) const
926 {
927  vector2d<num> p;
928  num lsq,f;
929 
930  lsq = x*x + y*y;
931 
932  if(lsq < sq(max_length)){
933  p.set(x,y);
934  }else{
935  f = max_length / mysqrt<num>(lsq);
936  p.set(f*x,f*y);
937  }
938 
939  return(p);
940 }
941 
942 template <class num> template <class vec> num vector2d<num>::dot(const vec p) const
943 {
944  return(x*p.x + y*p.y);
945 }
946 
947 VECTOR_TEM num vector2d<num>::perpdot(const vector2d<num> p) const
948 // perp product, equivalent to (this->perp()).dot(p)
949 {
950  return(x*p.y - y*p.x);
951 }
952 
953 VECTOR_TEM num dot(const vector2d<num> a,const vector2d<num> b)
954 {
955  return(a.x*b.x + a.y*b.y);
956 }
957 
958 VECTOR_TEM num cosine(const vector2d<num> a,const vector2d<num> b)
959 // equivalent to dot(a.norm(),b.norm())
960 {
961  num l;
962 
963  l = mysqrt<num>(a.x*a.x + a.y*a.y) * mysqrt<num>(b.x*b.x + b.y*b.y);
964 
965  return((a.x*b.x + a.y*b.y) / l);
966 }
967 
968 VECTOR_TEM num vector2d<num>::cross(const vector2d<num> p) const
969 {
970  // right handed
971  return(x*p.y - p.x*y);
972 }
973 
974 // returns point rotated by <a> radians
975 VECTOR_TEM vector2d<num> vector2d<num>::rotate(const double a) const
976 {
977  vector2d<num> q;
978  double s,c;
979 
980  s = sin(a);
981  c = cos(a);
982 
983  q.x = c*x + -s*y;
984  q.y = s*x + c*y;
985 
986  return(q);
987 }
988 
989 /* Depricated: replace "p.project(basis)" with "basis.project_out(p)"
992 template <class num>
993 vector2d<num> vector2d<num>::project(const vector2d<num> p) const
994 {
995  vector2d<num> q;
996 
997  q.x = p.x*x - p.y*y;
998  q.y = p.y*x + p.x*y;
999 
1000  return(q);
1001 }
1002 */
1003 
1006 template <class num>
1007 vector2d<num> vector2d<num>::project_in(const vector2d<num> p) const
1008 {
1009  vector2d<num> q;
1010  q.x = x*p.x + y*p.y; // q.x = this->dot(p);
1011  q.y = x*p.y - y*p.x; // q.y = this->perpdot(p);
1012  return(q);
1013 }
1014 
1017 template <class num>
1018 vector2d<num> vector2d<num>::project_out(const vector2d<num> p) const
1019 {
1020  vector2d<num> q;
1021  q.x = x*p.x - y*p.y;
1022  q.y = y*p.x + x*p.y;
1023  return(q);
1024 }
1025 
1026 #define VECTOR2D_EQUAL_BINARY_OPERATOR(opr) \
1027  template <class num> \
1028  vector2d<num> &vector2d<num>::operator opr (const vector2d<num> p) \
1029  { \
1030  x opr p.x; \
1031  y opr p.y; \
1032  return(*this); \
1033  }
1034 
1035 VECTOR2D_EQUAL_BINARY_OPERATOR(+=)
1036 VECTOR2D_EQUAL_BINARY_OPERATOR(-=)
1037 VECTOR2D_EQUAL_BINARY_OPERATOR(*=)
1038 VECTOR2D_EQUAL_BINARY_OPERATOR(/=)
1039 
1040 #define VECTOR2D_BINARY_OPERATOR(opr) \
1041  template <class num> \
1042  vector2d<num> vector2d<num>::operator opr (const vector2d<num> p) const \
1043  { \
1044  vector2d<num> r; \
1045  r.x = x opr p.x; \
1046  r.y = y opr p.y; \
1047  return(r); \
1048  }
1049 
1050 VECTOR2D_BINARY_OPERATOR(+)
1051 VECTOR2D_BINARY_OPERATOR(-)
1052 VECTOR2D_BINARY_OPERATOR(*)
1053 VECTOR2D_BINARY_OPERATOR(/)
1054 
1055 #define VECTOR2D_SCALAR_OPERATOR(opr) \
1056  template <class num> \
1057  vector2d<num> vector2d<num>::operator opr (const num f) const \
1058  { \
1059  vector2d<num> r; \
1060  r.x = x opr f; \
1061  r.y = y opr f; \
1062  return(r); \
1063  }
1064 
1065 VECTOR2D_SCALAR_OPERATOR(*)
1066 VECTOR2D_SCALAR_OPERATOR(/)
1067 
1068 #define VECTOR2D_EQUAL_SCALAR_OPERATOR(opr) \
1069  template <class num> \
1070  vector2d<num> &vector2d<num>::operator opr (num f) \
1071  { \
1072  x opr f; \
1073  y opr f; \
1074  return(*this); \
1075  }
1076 
1077 VECTOR2D_EQUAL_SCALAR_OPERATOR(*=)
1078 VECTOR2D_EQUAL_SCALAR_OPERATOR(/=)
1079 
1080 #define VECTOR2D_LOGIC_OPERATOR(opr,combine) \
1081  template <class num> \
1082  bool vector2d<num>::operator opr (const vector2d<num> p) const \
1083  { \
1084  return((x opr p.x) combine \
1085  (y opr p.y)); \
1086  }
1087 
1088 VECTOR2D_LOGIC_OPERATOR(==,&&)
1089 VECTOR2D_LOGIC_OPERATOR(!=,||)
1090 
1091 VECTOR2D_LOGIC_OPERATOR(< ,&&)
1092 VECTOR2D_LOGIC_OPERATOR(> ,&&)
1093 VECTOR2D_LOGIC_OPERATOR(<=,&&)
1094 VECTOR2D_LOGIC_OPERATOR(>=,&&)
1095 
1096 
1097 template <class num>
1098 inline vector2d<num> vector2d<num>::operator-() const
1099 {
1100  vector2d<num> r;
1101  r.x = -x;
1102  r.y = -y;
1103  return(r);
1104 }
1105 
1106 template <class num1,class num2>
1107 inline vector2d<num2> operator*(num1 f,const vector2d<num2> &a)
1108 {
1109  vector2d<num2> r;
1110 
1111  r.x = f * a.x;
1112  r.y = f * a.y;
1113 
1114  return(r);
1115 }
1116 
1117 template <class num>
1118 inline void vector2d<num>::take_min(const vector2d<num> p)
1119 {
1120  if(p.x < x) x = p.x;
1121  if(p.y < y) y = p.y;
1122 }
1123 
1124 template <class num>
1125 inline void vector2d<num>::take_max(const vector2d<num> p)
1126 {
1127  if(p.x > x) x = p.x;
1128  if(p.y > y) y = p.y;
1129 }
1130 
1131 VECTOR_TEM vector2d<num> abs(vector2d<num> a)
1132 {
1133  a.x = ::fabs(a.x);
1134  a.y = ::fabs(a.y);
1135 
1136  return(a);
1137 }
1138 
1139 VECTOR_TEM vector2d<num> min(vector2d<num> a,vector2d<num> b)
1140 {
1141  vector2d<num> v;
1142 
1143  v.x = ::min(a.x,b.x);
1144  v.y = ::min(a.y,b.y);
1145 
1146  return(v);
1147 }
1148 
1149 VECTOR_TEM vector2d<num> max(vector2d<num> a,vector2d<num> b)
1150 {
1151  vector2d<num> v;
1152 
1153  v.x = ::max(a.x,b.x);
1154  v.y = ::max(a.y,b.y);
1155 
1156  return(v);
1157 }
1158 
1159 VECTOR_TEM vector2d<num> bound(vector2d<num> v,num low,num high)
1160 {
1161  v.x = ::bound(v.x,low,high);
1162  v.y = ::bound(v.y,low,high);
1163 
1164  return(v);
1165 }
1166 
1167 VECTOR_TEM num dist(const vector2d<num> a,const vector2d<num> b)
1168 {
1169  num dx,dy;
1170 
1171  dx = a.x - b.x;
1172  dy = a.y - b.y;
1173 
1174  return(mysqrt<num>(dx*dx + dy*dy));
1175 }
1176 
1177 VECTOR_TEM num distance(const vector2d<num> a,const vector2d<num> b)
1178 {
1179  return(dist(a,b));
1180 }
1181 
1182 // returns square of distance between two points
1183 VECTOR_TEM num sqdist(const vector2d<num> a,const vector2d<num> b)
1184 {
1185  num dx,dy;
1186 
1187  dx = a.x - b.x;
1188  dy = a.y - b.y;
1189 
1190  return(dx*dx + dy*dy);
1191 }
1192 
1193 VECTOR_TEM num sqdistance(const vector2d<num> a,const vector2d<num> b)
1194 {
1195  return(sqdist(a,b));
1196 }
1197 
1198 }; // namespace vector
1199 
1200 #endif
1201 // __VECTOR_H__
GVector::vector2d::bound
vector2d< num > bound(const num max_length) const MustUseResult
bound vector to a maximum length
Definition: gvector.h:925
GVector::vector2d::finite
bool finite() const MustUseResult
return true if both elements are finite, otherwise return false
Definition: gvector.h:873
GVector::vector2d::set
void set(vec p)
set the components of the vector
Definition: gvector.h:774
GVector::vector2d::operator[]
num & operator[](int idx)
element accessor
Definition: gvector.h:789
GVector::vector2d::set
void set(num nx, num ny)
set the components of the vector
Definition: gvector.h:768
GVector::vector2d::operator-=
vector2d< num > & operator-=(const vector2d< num > p)
subtract a vector from the current element values
GVector::vector2d::operator*
vector2d< num > operator*(const vector2d< num > p) const
return elementwise product of this vector and p
GVector::vector2d::angle
num angle() const MustUseResult
calculate the clockwise angle from <1,0>
Definition: gvector.h:799
GVector::vector2d::setAll
void setAll(num nx)
set the components of the vector to the same value
Definition: gvector.h:771
GVector::vector2d::project_in
vector2d< num > project_in(const vector2d< num > p) const MustUseResult
Definition: gvector.h:1007
GVector::vector2d::nonzero
bool nonzero() const MustUseResult
return if vector has any length at all
Definition: gvector.h:814
GVector::vector2d::take_max
void take_max(const vector2d< num > p)
set the vector to the maximum of its components and p's components
Definition: gvector.h:1125
GVector::vector2d::take_min
void take_min(const vector2d< num > p)
set the vector to the minimum of its components and p's components
Definition: gvector.h:1118
GVector::vector2d::operator+
vector2d< num > operator+(const vector2d< num > p) const
return vector sum of this vector and p
GVector::vector2d::perpdot
num perpdot(const vector2d< num > p) const MustUseResult
return dot product of vector's perp with p, equivalent to (this->perp()).dot(p)
Definition: gvector.h:947
GVector::vector2d::operator/
vector2d< num > operator/(const vector2d< num > p) const
return elementwise division of this vector by p
GVector::vector2d::operator=
vector2d< num > & operator=(vector2d< num > p)
copy constructor
Definition: gvector.h:781
GVector::vector2d::normalize
void normalize()
normalize to unit length in place
Definition: gvector.h:916
GVector::vector2d::sqlength
num sqlength() const MustUseResult
calculate squared Euclidean length (faster than length())
Definition: gvector.h:887
GVector::vector2d::operator-
vector2d< num > operator-() const
negate vector (reflect through origin) <x,y> -> <-x,-y>
Definition: gvector.h:1098
GVector::vector2d::operator/=
vector2d< num > & operator/=(const vector2d< num > p)
divide (elementwise) a vector with the current element values
GVector::vector2d::project_out
vector2d< num > project_out(const vector2d< num > p) const MustUseResult
Definition: gvector.h:1018
GVector::vector3d::operator[]
num & operator[](int idx)
element accessor
Definition: gvector.h:262
GVector::vector2d::cross
num cross(const vector2d< num > p) const MustUseResult
return z component of 3D cross product on 2D vectors. right handed.
Definition: gvector.h:968
GVector::vector2d::norm
vector2d< num > norm() const MustUseResult
return a unit length vector in the same direction
Definition: gvector.h:892
GVector::vector2d::rotate
vector2d< num > rotate(const double a) const MustUseResult
return vector rotated by angle a
Definition: gvector.h:975
GVector::vector2d::operator*=
vector2d< num > & operator*=(const vector2d< num > p)
multiply (elementwise) a vector with the current element values
GVector::vector2d::operator=
vector2d< num > & operator=(vec p)
copy constructor from compatible classes
Definition: gvector.h:785
GVector::vector2d::perp
vector2d< num > perp() const MustUseResult
return the perpendicular of a vector (i.e. rotated 90 deg counterclockwise)
Definition: gvector.h:825
GVector::vector2d::operator+=
vector2d< num > & operator+=(const vector2d< num > p)
add a vector to the current element values
GVector::vector2d::heading
void heading(num angle)
make a unit vector at given angle
Definition: gvector.h:802
GVector::vector2d::length
num length() const MustUseResult
calculate Euclidean length
Definition: gvector.h:882
GVector::vector2d::dot
num dot(const vec p) const MustUseResult
return dot product of vector with p
Definition: gvector.h:942
GVector::vector2d::zero
void zero()
zero all components of the vector
Definition: gvector.h:777