00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef WFMATH_VECTOR_FUNCS_H
00031 #define WFMATH_VECTOR_FUNCS_H
00032
00033 #include <wfmath/vector.h>
00034 #include <wfmath/rotmatrix.h>
00035 #include <wfmath/const.h>
00036
00037 namespace WFMath {
00038
00039 template<const int dim>
00040 Vector<dim>::Vector(const Vector<dim>& v) : m_valid(v.m_valid)
00041 {
00042 for(int i = 0; i < dim; ++i)
00043 m_elem[i] = v.m_elem[i];
00044 }
00045
00046 template<const int dim>
00047 Vector<dim>& Vector<dim>::operator=(const Vector<dim>& v)
00048 {
00049 m_valid = v.m_valid;
00050
00051 for(int i = 0; i < dim; ++i)
00052 m_elem[i] = v.m_elem[i];
00053
00054 return *this;
00055 }
00056
00057 template<const int dim>
00058 bool Vector<dim>::isEqualTo(const Vector<dim>& v, double epsilon) const
00059 {
00060 double delta = _ScaleEpsilon(m_elem, v.m_elem, dim, epsilon);
00061
00062 for(int i = 0; i < dim; ++i)
00063 if(fabs(m_elem[i] - v.m_elem[i]) > delta)
00064 return false;
00065
00066 return true;
00067 }
00068
00069 template<const int dim>
00070 bool Vector<dim>::operator< (const Vector<dim>& v) const
00071 {
00072 if(operator==(v))
00073 return false;
00074
00075 for(int i = 0; i < dim; ++i)
00076 if(m_elem[i] != v.m_elem[i])
00077 return m_elem[i] < v.m_elem[i];
00078
00079 assert(false);
00080 return false;
00081 }
00082
00083 template <const int dim>
00084 Vector<dim> operator+(const Vector<dim>& v1, const Vector<dim>& v2)
00085 {
00086 Vector<dim> ans;
00087
00088 ans.m_valid = v1.m_valid && v2.m_valid;
00089
00090 for(int i = 0; i < dim; ++i)
00091 ans.m_elem[i] = v1.m_elem[i] + v2.m_elem[i];
00092
00093 return ans;
00094 }
00095
00096 template <const int dim>
00097 Vector<dim> operator-(const Vector<dim>& v1, const Vector<dim>& v2)
00098 {
00099 Vector<dim> ans;
00100
00101 ans.m_valid = v1.m_valid && v2.m_valid;
00102
00103 for(int i = 0; i < dim; ++i)
00104 ans.m_elem[i] = v1.m_elem[i] - v2.m_elem[i];
00105
00106 return ans;
00107 }
00108
00109 template <const int dim>
00110 Vector<dim> operator*(const Vector<dim>& v, CoordType d)
00111 {
00112 Vector<dim> ans;
00113
00114 ans.m_valid = v.m_valid;
00115
00116 for(int i = 0; i < dim; ++i)
00117 ans.m_elem[i] = v.m_elem[i] * d;
00118
00119 return ans;
00120 }
00121
00122 template<const int dim>
00123 Vector<dim> operator*(CoordType d, const Vector<dim>& v)
00124 {
00125 Vector<dim> ans;
00126
00127 ans.m_valid = v.m_valid;
00128
00129 for(int i = 0; i < dim; ++i)
00130 ans.m_elem[i] = v.m_elem[i] * d;
00131
00132 return ans;
00133 }
00134
00135 template <const int dim>
00136 Vector<dim> operator/(const Vector<dim>& v, CoordType d)
00137 {
00138 Vector<dim> ans;
00139
00140 ans.m_valid = v.m_valid;
00141
00142 for(int i = 0; i < dim; ++i)
00143 ans.m_elem[i] = v.m_elem[i] / d;
00144
00145 return ans;
00146 }
00147
00148 template <const int dim>
00149 Vector<dim> operator-(const Vector<dim>& v)
00150 {
00151 Vector<dim> ans;
00152
00153 ans.m_valid = v.m_valid;
00154
00155 for(int i = 0; i < dim; ++i)
00156 ans.m_elem[i] = -v.m_elem[i];
00157
00158 return ans;
00159 }
00160
00161 template <const int dim>
00162 Vector<dim>& operator+=(Vector<dim>& v1, const Vector<dim>& v2)
00163 {
00164 v1.m_valid = v1.m_valid && v2.m_valid;
00165
00166 for(int i = 0; i < dim; ++i)
00167 v1.m_elem[i] += v2.m_elem[i];
00168
00169 return v1;
00170 }
00171
00172 template <const int dim>
00173 Vector<dim>& operator-=(Vector<dim>& v1, const Vector<dim>& v2)
00174 {
00175 v1.m_valid = v1.m_valid && v2.m_valid;
00176
00177 for(int i = 0; i < dim; ++i)
00178 v1.m_elem[i] -= v2.m_elem[i];
00179
00180 return v1;
00181 }
00182
00183 template <const int dim>
00184 Vector<dim>& operator*=(Vector<dim>& v, CoordType d)
00185 {
00186 for(int i = 0; i < dim; ++i)
00187 v.m_elem[i] *= d;
00188
00189 return v;
00190 }
00191
00192 template <const int dim>
00193 Vector<dim>& operator/=(Vector<dim>& v, CoordType d)
00194 {
00195 for(int i = 0; i < dim; ++i)
00196 v.m_elem[i] /= d;
00197
00198 return v;
00199 }
00200
00201 template<const int dim>
00202 Vector<dim>& Vector<dim>::sloppyNorm(CoordType norm)
00203 {
00204 CoordType mag = sloppyMag();
00205
00206 assert("need nonzero length vector" && mag > norm / WFMATH_MAX);
00207
00208 return (*this *= norm / mag);
00209 }
00210
00211 template<const int dim>
00212 Vector<dim>& Vector<dim>::zero()
00213 {
00214 m_valid = true;
00215
00216 for(int i = 0; i < dim; ++i)
00217 m_elem[i] = 0;
00218
00219 return *this;
00220 }
00221
00222 template<const int dim>
00223 CoordType Angle(const Vector<dim>& v, const Vector<dim>& u)
00224 {
00225
00226
00227
00228 CoordType dp = FloatClamp(Dot(u, v) / sqrt(u.sqrMag() * v.sqrMag()),
00229 -1.0, 1.0);
00230
00231 CoordType angle = acos(dp);
00232
00233 return angle;
00234 }
00235
00236 template<const int dim>
00237 Vector<dim>& Vector<dim>::rotate(int axis1, int axis2, CoordType theta)
00238 {
00239 assert(axis1 >= 0 && axis2 >= 0 && axis1 < dim && axis2 < dim && axis1 != axis2);
00240
00241 CoordType tmp1 = m_elem[axis1], tmp2 = m_elem[axis2];
00242 CoordType stheta = (CoordType) sin(theta), ctheta = (CoordType) cos(theta);
00243
00244 m_elem[axis1] = tmp1 * ctheta - tmp2 * stheta;
00245 m_elem[axis2] = tmp2 * ctheta + tmp1 * stheta;
00246
00247 return *this;
00248 }
00249
00250 template<const int dim>
00251 Vector<dim>& Vector<dim>::rotate(const Vector<dim>& v1, const Vector<dim>& v2,
00252 CoordType theta)
00253 {
00254 RotMatrix<dim> m;
00255 return operator=(Prod(*this, m.rotation(v1, v2, theta)));
00256 }
00257
00258 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00259 template<> Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta);
00260 template<> Vector<3>& Vector<3>::rotate(const Quaternion& q);
00261 #else
00262 Vector<3>& _NCFS_Vector3_rotate(Vector<3>& v, const Vector<3>& axis, CoordType theta);
00263 Vector<3>& _NCFS_Vector3_rotate(Vector<3>& v, const Quaternion& q);
00264
00265 template<>
00266 inline Vector<3>& Vector<3>::rotate(const Vector<3>& axis, CoordType theta)
00267 {
00268 return _NCFS_Vector3_rotate(*this, axis, theta);
00269 }
00270
00271 template<>
00272 inline Vector<3>& Vector<3>::rotate(const Quaternion& q)
00273 {
00274 return _NCFS_Vector3_rotate(*this, q);
00275 }
00276 #endif
00277
00278 template<const int dim>
00279 CoordType Dot(const Vector<dim>& v1, const Vector<dim>& v2)
00280 {
00281 double delta = _ScaleEpsilon(v1.m_elem, v2.m_elem, dim);
00282
00283 CoordType ans = 0;
00284
00285 for(int i = 0; i < dim; ++i)
00286 ans += v1.m_elem[i] * v2.m_elem[i];
00287
00288 return (fabs(ans) >= delta) ? ans : 0;
00289 }
00290
00291 template<const int dim>
00292 CoordType Vector<dim>::sqrMag() const
00293 {
00294 CoordType ans = 0;
00295
00296 for(int i = 0; i < dim; ++i)
00297
00298 ans += m_elem[i] * m_elem[i];
00299
00300 return ans;
00301 }
00302
00303 template<const int dim>
00304 bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2, bool& same_dir)
00305 {
00306 CoordType dot = Dot(v1, v2);
00307
00308 same_dir = (dot > 0);
00309
00310 return Equal(dot * dot, v1.sqrMag() * v2.sqrMag());
00311 }
00312
00313 template<const int dim>
00314 bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2)
00315 {
00316 bool same_dir;
00317
00318 return Parallel(v1, v2, same_dir);
00319 }
00320
00321 template<const int dim>
00322 bool Perpendicular(const Vector<dim>& v1, const Vector<dim>& v2)
00323 {
00324 double max1 = 0, max2 = 0;
00325
00326 for(int i = 0; i < dim; ++i) {
00327 double val1 = fabs(v1[i]), val2 = fabs(v2[i]);
00328 if(val1 > max1)
00329 max1 = val1;
00330 if(val2 > max2)
00331 max2 = val2;
00332 }
00333
00334
00335 int exp1, exp2;
00336 (void) frexp(max1, &exp1);
00337 (void) frexp(max2, &exp2);
00338
00339 return fabs(Dot(v1, v2)) < ldexp(WFMATH_EPSILON, exp1 + exp2);
00340 }
00341
00342 template<>
00343 inline const CoordType Vector<1>::sloppyMagMax()
00344 {
00345 return (CoordType) 1;
00346 }
00347
00348 template<>
00349 inline const CoordType Vector<2>::sloppyMagMax()
00350 {
00351 return (CoordType) 1.082392200292393968799446410733;
00352 }
00353
00354 template<>
00355 inline const CoordType Vector<3>::sloppyMagMax()
00356 {
00357 return (CoordType) 1.145934719303161490541433900265;
00358 }
00359
00360 template<>
00361 inline const CoordType Vector<1>::sloppyMagMaxSqrt()
00362 {
00363 return (CoordType) 1;
00364 }
00365
00366 template<>
00367 inline const CoordType Vector<2>::sloppyMagMaxSqrt()
00368 {
00369 return (CoordType) 1.040380795811030899095785063701;
00370 }
00371
00372 template<>
00373 inline const CoordType Vector<3>::sloppyMagMaxSqrt()
00374 {
00375 return (CoordType) 1.070483404496847625250328653179;
00376 }
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00392 template<> Vector<2>& Vector<2>::polar(CoordType r, CoordType theta);
00393 template<> void Vector<2>::asPolar(CoordType& r, CoordType& theta) const;
00394
00395 template<> Vector<3>& Vector<3>::polar(CoordType r, CoordType theta,
00396 CoordType z);
00397 template<> void Vector<3>::asPolar(CoordType& r, CoordType& theta,
00398 CoordType& z) const;
00399 template<> Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta,
00400 CoordType phi);
00401 template<> void Vector<3>::asSpherical(CoordType& r, CoordType& theta,
00402 CoordType& phi) const;
00403
00404 template<> CoordType Vector<2>::sloppyMag() const;
00405 template<> CoordType Vector<3>::sloppyMag() const;
00406 #else
00407 void _NCFS_Vector2_polar(CoordType *m_elem, CoordType r, CoordType theta);
00408 void _NCFS_Vector2_asPolar(CoordType *m_elem, CoordType& r, CoordType& theta);
00409
00410 void _NCFS_Vector3_polar(CoordType *m_elem, CoordType r, CoordType theta,
00411 CoordType z);
00412 void _NCFS_Vector3_asPolar(CoordType *m_elem, CoordType& r, CoordType& theta,
00413 CoordType& z);
00414 void _NCFS_Vector3_spherical(CoordType *m_elem, CoordType r, CoordType theta,
00415 CoordType phi);
00416 void _NCFS_Vector3_asSpherical(CoordType *m_elem, CoordType& r, CoordType& theta,
00417 CoordType& phi);
00418
00419 CoordType _NCFS_Vector2_sloppyMag(CoordType *m_elem);
00420 CoordType _NCFS_Vector3_sloppyMag(CoordType *m_elem);
00421
00422 template<>
00423 inline Vector<2>& Vector<2>::polar(CoordType r, CoordType theta)
00424 {
00425 _NCFS_Vector2_polar((CoordType*) m_elem, r, theta);
00426 m_valid = true;
00427 return *this;
00428 }
00429
00430 template<>
00431 inline void Vector<2>::asPolar(CoordType& r, CoordType& theta) const
00432 {
00433 _NCFS_Vector2_asPolar((CoordType*) m_elem, r, theta);
00434 }
00435
00436 template<>
00437 inline Vector<3>& Vector<3>::polar(CoordType r, CoordType theta, CoordType z)
00438 {
00439 _NCFS_Vector3_polar((CoordType*) m_elem, r, theta, z);
00440 m_valid = true;
00441 return *this;
00442 }
00443
00444 template<>
00445 inline void Vector<3>::asPolar(CoordType& r, CoordType& theta, CoordType& z) const
00446 {
00447 _NCFS_Vector3_asPolar((CoordType*) m_elem, r, theta, z);
00448 }
00449
00450 template<>
00451 inline Vector<3>& Vector<3>::spherical(CoordType r, CoordType theta, CoordType phi)
00452 {
00453 _NCFS_Vector3_spherical((CoordType*) m_elem, r, theta, phi);
00454 m_valid = true;
00455 return *this;
00456 }
00457
00458 template<>
00459 inline void Vector<3>::asSpherical(CoordType& r, CoordType& theta, CoordType& phi) const
00460 {
00461 _NCFS_Vector3_asSpherical((CoordType*) m_elem, r, theta, phi);
00462 }
00463
00464 template<>
00465 inline CoordType Vector<2>::sloppyMag() const
00466 {
00467 return _NCFS_Vector2_sloppyMag((CoordType*) m_elem);
00468 }
00469
00470 template<>
00471 inline CoordType Vector<3>::sloppyMag() const
00472 {
00473 return _NCFS_Vector3_sloppyMag((CoordType*) m_elem);
00474 }
00475 #endif
00476
00477 template<> inline CoordType Vector<1>::sloppyMag() const
00478 {return (CoordType) fabs(m_elem[0]);}
00479
00480 template<> inline Vector<2>::Vector(CoordType x, CoordType y) : m_valid(true)
00481 {m_elem[0] = x; m_elem[1] = y;}
00482 template<> inline Vector<3>::Vector(CoordType x, CoordType y, CoordType z) : m_valid(true)
00483 {m_elem[0] = x; m_elem[1] = y; m_elem[2] = z;}
00484
00485
00486
00487 template<> inline Vector<2>& Vector<2>::rotate(CoordType theta)
00488 {return rotate(0, 1, theta);}
00489
00490 template<> inline Vector<3>& Vector<3>::rotateX(CoordType theta)
00491 {return rotate(1, 2, theta);}
00492 template<> inline Vector<3>& Vector<3>::rotateY(CoordType theta)
00493 {return rotate(2, 0, theta);}
00494 template<> inline Vector<3>& Vector<3>::rotateZ(CoordType theta)
00495 {return rotate(0, 1, theta);}
00496
00497
00498 }
00499
00500 #endif // WFMATH_VECTOR_FUNCS_H