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