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