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