00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef _XNV_3DVECTOR_H_
00010 #define _XNV_3DVECTOR_H_
00011
00012
00013
00014
00015 #include <math.h>
00016 #include "XnVNiteDefs.h"
00017 #include "XnVMathCommon.h"
00018
00019
00020
00021
00022
00023 class XNV_NITE_API XnV3DVector : public XnPoint3D
00024 {
00025 public:
00026
00027 inline XnV3DVector();
00028 inline XnV3DVector(XnFloat fX, XnFloat fY, XnFloat fZ);
00029 inline XnV3DVector(const XnPoint3D& ptOther);
00030 inline XnV3DVector(const XnV3DVector& v3Other);
00031 inline XnV3DVector(XnFloat f);
00032 inline XnV3DVector(XnFloat* pf);
00033
00034
00035 inline XnV3DVector& operator=(const XnV3DVector& v3Rhs);
00036 inline XnV3DVector& operator=(const XnPoint3D& ptRhs);
00037
00038
00039 inline XnFloat operator[](XnUInt32 index) const;
00040 inline XnFloat& operator[](XnUInt32 index);
00041
00042
00043 inline XnV3DVector& Set(const XnV3DVector& v3Other);
00044 inline XnV3DVector& Set(const XnPoint3D& ptOther);
00045 inline XnV3DVector& Set(XnFloat fX, XnFloat fY, XnFloat fZ);
00046 inline XnV3DVector& Set(XnFloat f);
00047
00048
00049 inline XnV3DVector& SetZero();
00050 inline XnBool IsZero() const;
00051
00052
00053 inline XnBool operator==(const XnV3DVector& v3Rhs) const;
00054 inline XnBool operator!=(const XnV3DVector& v3Rhs) const;
00055
00056
00057 inline XnV3DVector operator-() const;
00058 inline XnV3DVector& Negate(const XnV3DVector& v3Other);
00059 inline XnV3DVector& Negate();
00060
00061
00062 inline XnV3DVector operator*(const XnV3DVector& v3Rhs) const;
00063 inline XnV3DVector& operator*=(const XnV3DVector& v3Rhs);
00064 inline XnV3DVector& Multiply(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs);
00065
00066 friend inline XnV3DVector operator*(XnFloat f, const XnV3DVector& v3Rhs);
00067 inline XnV3DVector& Multiply(XnFloat f, const XnV3DVector& v3Rhs);
00068
00069 inline XnV3DVector operator*(XnFloat f) const;
00070 inline XnV3DVector& operator*=(XnFloat f);
00071 inline XnV3DVector& Multiply(const XnV3DVector& v3Lhs, XnFloat f);
00072
00073
00074 inline XnV3DVector operator/(XnFloat f) const;
00075 inline XnV3DVector& operator/=(XnFloat f);
00076 inline XnV3DVector& Divide(const XnV3DVector& v3Lhs, XnFloat f);
00077
00078
00079 inline XnV3DVector operator+(const XnV3DVector& v3Rhs) const;
00080 inline XnV3DVector& operator+=(const XnV3DVector& v3Rhs);
00081 inline XnV3DVector& Add(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs);
00082
00083 inline XnV3DVector operator+(XnFloat f) const;
00084 inline XnV3DVector& operator+=(XnFloat f);
00085 inline XnV3DVector& Add(const XnV3DVector& v3Lhs, XnFloat f);
00086
00087
00088 inline XnV3DVector operator-(const XnV3DVector& v3Rhs) const;
00089 inline XnV3DVector& operator-=(const XnV3DVector& v3Rhs);
00090 inline XnV3DVector& Subtract(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs);
00091
00092 inline XnV3DVector operator-(XnFloat f) const;
00093 inline XnV3DVector& operator-=(XnFloat f);
00094 inline XnV3DVector& Subtract(const XnV3DVector& v3Lhs, XnFloat f);
00095
00096 inline XnV3DVector& Sqrt();
00097 inline XnV3DVector& Sqrt(const XnV3DVector& v3Other);
00098
00099
00100 inline XnFloat Magnitude() const;
00101 inline XnFloat MagnitudeSquared() const;
00102
00103
00104 inline XnFloat Distance(const XnV3DVector& v3Other) const;
00105 inline XnFloat DistanceSquared(const XnV3DVector& v3Other) const;
00106
00107
00108 inline XnFloat Normalize();
00109
00110
00111 inline XnV3DVector& OrthogonalVector(const XnV3DVector& v3Other);
00112 inline XnV3DVector& UnitOrthogonalVector(const XnV3DVector& v3Other);
00113
00114
00115 inline XnV3DVector operator^(const XnV3DVector& v3Rhs) const;
00116 inline XnV3DVector& CrossProduct(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs);
00117
00118
00119 inline XnFloat operator|(const XnV3DVector& v3Rhs) const;
00120 friend inline XnFloat DotProduct(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs);
00121
00122
00123 inline XnV3DVector& Interpolate(const XnV3DVector& v3Vec1, const XnV3DVector& v3Vec2, XnFloat fAlpha);
00124
00125 inline XnBool IsSameDirection(const XnV3DVector& v3Other) const;
00126
00127 inline XnFloat GetTolerance() const;
00128 inline void SetTolerance(XnFloat fTolerance);
00129
00130 protected:
00131 XnFloat m_fTolerance;
00132 };
00133
00134 XnFloat XnV3DVector::GetTolerance() const
00135 {
00136 return m_fTolerance;
00137 }
00138
00139 void XnV3DVector::SetTolerance(XnFloat fTolerance)
00140 {
00141 m_fTolerance = fTolerance;
00142 }
00143
00144 XnV3DVector::XnV3DVector() :
00145 m_fTolerance(1e-5f)
00146 {
00147 Set(0);
00148 }
00149 XnV3DVector::XnV3DVector(XnFloat fX, XnFloat fY, XnFloat fZ) :
00150 m_fTolerance(1e-5f)
00151 {
00152 Set(fX, fY, fZ);
00153 }
00154 XnV3DVector::XnV3DVector(const XnV3DVector& v3Other)
00155 {
00156 Set(v3Other);
00157 }
00158 XnV3DVector::XnV3DVector(const XnPoint3D& ptOther)
00159 {
00160 Set(ptOther);
00161 }
00162 XnV3DVector::XnV3DVector(XnFloat f) :
00163 m_fTolerance(1e-5f)
00164 {
00165 Set(f);
00166 }
00167 XnV3DVector::XnV3DVector(XnFloat* pf) :
00168 m_fTolerance(1e-5f)
00169 {
00170 Set(pf[0], pf[1], pf[2]);
00171 }
00172
00173
00174 XnV3DVector& XnV3DVector::operator=(const XnV3DVector& v3Rhs)
00175 {
00176 m_fTolerance = v3Rhs.m_fTolerance;
00177 return Set(v3Rhs.X, v3Rhs.Y, v3Rhs.Z);
00178 }
00179 XnV3DVector& XnV3DVector::operator=(const XnPoint3D& ptRhs)
00180 {
00181 m_fTolerance = 1e-5f;
00182 return Set(ptRhs.X, ptRhs.Y, ptRhs.Z);
00183 }
00184
00185
00186 XnFloat XnV3DVector::operator[](XnUInt32 nIndex) const
00187 {
00188 switch (nIndex)
00189 {
00190 case 0:
00191 return X;
00192 case 1:
00193 return Y;
00194 case 2:
00195 return Z;
00196 }
00197
00198 return Z;
00199 }
00200 XnFloat& XnV3DVector::operator[](XnUInt32 nIndex)
00201 {
00202 switch (nIndex)
00203 {
00204 case 0:
00205 return X;
00206 case 1:
00207 return Y;
00208 case 2:
00209 return Z;
00210 }
00211
00212 return Z;
00213 }
00214
00215
00216 XnV3DVector& XnV3DVector::Set(const XnV3DVector& v3Other)
00217 {
00218 return *this = v3Other;
00219 }
00220 XnV3DVector& XnV3DVector::Set(const XnPoint3D& ptOther)
00221 {
00222 return *this = ptOther;
00223
00224 }
00225 XnV3DVector& XnV3DVector::Set(XnFloat x, XnFloat y, XnFloat z)
00226 {
00227 X = x;
00228 Y = y;
00229 Z = z;
00230
00231 return *this;
00232 }
00233 XnV3DVector& XnV3DVector::Set(XnFloat f)
00234 {
00235 return Set(f, f, f);
00236 }
00237
00238
00239 XnV3DVector& XnV3DVector::SetZero()
00240 {
00241 return Set(0);
00242 }
00243 XnBool XnV3DVector::IsZero() const
00244 {
00245 return (fabs(X) < m_fTolerance &&
00246 fabs(Y) < m_fTolerance &&
00247 fabs(Z) < m_fTolerance);
00248 }
00249
00250
00251 XnBool XnV3DVector::operator==(const XnV3DVector& v3Rhs) const
00252 {
00253 return (fabs(X - v3Rhs.X) < m_fTolerance &&
00254 fabs(Y - v3Rhs.Y) < m_fTolerance &&
00255 fabs(Z - v3Rhs.Z) < m_fTolerance);
00256 }
00257 XnBool XnV3DVector::operator!=(const XnV3DVector& v3Rhs) const
00258 {
00259 return !this->operator==(v3Rhs);
00260 }
00261
00262
00263 XnV3DVector XnV3DVector::operator-() const
00264 {
00265 return XnV3DVector(-X, -Y, -Z);
00266 }
00267 XnV3DVector& XnV3DVector::Negate(const XnV3DVector& v3Other)
00268 {
00269 return Set(-v3Other.X, -v3Other.Y, -v3Other.Z);
00270 }
00271 XnV3DVector& XnV3DVector::Negate()
00272 {
00273 return Set(-X, -Y, -Z);
00274 }
00275
00276
00277 XnV3DVector XnV3DVector::operator*(const XnV3DVector& v3Rhs) const
00278 {
00279 return XnV3DVector(X * v3Rhs.X, Y * v3Rhs.Y, Z*v3Rhs.Z);
00280 }
00281 XnV3DVector& XnV3DVector::operator*=(const XnV3DVector& v3Rhs)
00282 {
00283 return Set(X * v3Rhs.X, Y * v3Rhs.Y, Z * v3Rhs.Z);
00284 }
00285 XnV3DVector& XnV3DVector::Multiply(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs)
00286 {
00287 return Set(v3Lhs.X * v3Rhs.X, v3Lhs.Y * v3Rhs.Y, v3Lhs.Z * v3Rhs.Z);
00288 }
00289
00290 XnV3DVector operator*(XnFloat f, const XnV3DVector& v3Rhs)
00291 {
00292 return XnV3DVector(f * v3Rhs.X, f * v3Rhs.Y, f * v3Rhs.Z);
00293 }
00294 XnV3DVector& XnV3DVector::Multiply(XnFloat f, const XnV3DVector& v3Rhs)
00295 {
00296 return Set(f * v3Rhs.X, f * v3Rhs.Y, f * v3Rhs.Z);
00297 }
00298
00299 XnV3DVector XnV3DVector::operator*(XnFloat f) const
00300 {
00301 return XnV3DVector(X * f, Y * f, Z * f);
00302 }
00303 XnV3DVector& XnV3DVector::operator*=(XnFloat f)
00304 {
00305 return Set(X * f, Y * f, Z * f);
00306 }
00307 XnV3DVector& XnV3DVector::Multiply(const XnV3DVector& v3Lhs, XnFloat f)
00308 {
00309 return Set(v3Lhs.X * f, v3Lhs.Y * f, v3Lhs.Z * f);
00310 }
00311
00312
00313 XnV3DVector XnV3DVector::operator/(XnFloat f) const
00314 {
00315 return XnV3DVector(X / f, Y / f, Z / f);
00316 }
00317 XnV3DVector& XnV3DVector::operator/=(XnFloat f)
00318 {
00319 return Set(X / f, Y / f, Z / f);
00320 }
00321 XnV3DVector& XnV3DVector::Divide(const XnV3DVector& v3Lhs, XnFloat f)
00322 {
00323 return Set(v3Lhs.X / f, v3Lhs.Y / f, v3Lhs.Z / f);
00324 }
00325
00326
00327 XnV3DVector XnV3DVector::operator+(const XnV3DVector& v3Rhs) const
00328 {
00329 return XnV3DVector(X + v3Rhs.X, Y + v3Rhs.Y, Z + v3Rhs.Z);
00330 }
00331 XnV3DVector& XnV3DVector::operator+=(const XnV3DVector& v3Rhs)
00332 {
00333 return Set(X + v3Rhs.X, Y + v3Rhs.Y, Z + v3Rhs.Z);
00334 }
00335 XnV3DVector& XnV3DVector::Add(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs)
00336 {
00337 return Set(v3Lhs.X + v3Rhs.X, v3Lhs.Y + v3Rhs.Y, v3Lhs.Z + v3Rhs.Z);
00338 }
00339
00340 XnV3DVector XnV3DVector::operator+(XnFloat f) const
00341 {
00342 return XnV3DVector(X + f, Y + f, Z + f);
00343 }
00344 XnV3DVector& XnV3DVector::operator+=(XnFloat f)
00345 {
00346 return Set(X + f, Y + f, Z + f);
00347 }
00348 XnV3DVector& XnV3DVector::Add(const XnV3DVector& v3Lhs, XnFloat f)
00349 {
00350 return Set(v3Lhs.X + f, v3Lhs.Y + f, v3Lhs.Z + f);
00351 }
00352
00353
00354 XnV3DVector XnV3DVector::operator-(const XnV3DVector& v3Rhs) const
00355 {
00356 return XnV3DVector(X - v3Rhs.X, Y - v3Rhs.Y, Z - v3Rhs.Z);
00357 }
00358 XnV3DVector& XnV3DVector::operator-=(const XnV3DVector& v3Rhs)
00359 {
00360 return Set(X - v3Rhs.X, Y - v3Rhs.Y, Z - v3Rhs.Z);
00361 }
00362 XnV3DVector& XnV3DVector::Subtract(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs)
00363 {
00364 return Set(v3Lhs.X - v3Rhs.X, v3Lhs.Y - v3Rhs.Y, v3Lhs.Z - v3Rhs.Z);
00365 }
00366
00367 XnV3DVector XnV3DVector::operator-(XnFloat f) const
00368 {
00369 return XnV3DVector(X - f, Y - f, Z - f);
00370 }
00371 XnV3DVector& XnV3DVector::operator-=(XnFloat f)
00372 {
00373 return Set(X - f, Y - f, Z - f);
00374 }
00375 XnV3DVector& XnV3DVector::Subtract(const XnV3DVector& v3Lhs, XnFloat f)
00376 {
00377 return Set(v3Lhs.X - f, v3Lhs.Y - f, v3Lhs.Z - f);
00378 }
00379
00380 XnV3DVector& XnV3DVector::Sqrt()
00381 {
00382 return Set(sqrt(X), sqrt(Y), sqrt(Z));
00383 }
00384
00385 XnV3DVector& XnV3DVector::Sqrt(const XnV3DVector& v3Other)
00386 {
00387 Set(v3Other);
00388 return Sqrt();
00389 }
00390
00391
00392 XnFloat XnV3DVector::Magnitude() const
00393 {
00394 return sqrt(MagnitudeSquared());
00395 }
00396 XnFloat XnV3DVector::MagnitudeSquared() const
00397 {
00398 return X * X +
00399 Y * Y +
00400 Z * Z;
00401 }
00402
00403
00404 XnFloat XnV3DVector::Distance(const XnV3DVector& v3Other) const
00405 {
00406 return sqrt(DistanceSquared(v3Other));
00407 }
00408 XnFloat XnV3DVector::DistanceSquared(const XnV3DVector& v3Other) const
00409 {
00410 return (*this-v3Other).MagnitudeSquared();
00411 }
00412
00413
00414 XnFloat XnV3DVector::Normalize()
00415 {
00416 XnFloat fLen = Magnitude();
00417 if (fLen > m_fTolerance)
00418 *this /= fLen;
00419 else
00420 Set(1, 0, 0);
00421
00422 return fLen;
00423 }
00424
00425
00426 XnV3DVector& XnV3DVector::OrthogonalVector(const XnV3DVector& v3Other)
00427 {
00428 XnFloat abs_x = fabs(v3Other.X), abs_y = fabs(v3Other.Y), abs_z = fabs(v3Other.Z);
00429
00430 if (abs_x < abs_y)
00431 if (abs_x < abs_z)
00432 Set(0, v3Other.Z, -v3Other.Y);
00433 else
00434 Set(v3Other.Y, -v3Other.X, 0);
00435 else
00436 if (abs_y < abs_z)
00437 Set(-v3Other.Z, 0, v3Other.X);
00438 else
00439 Set(v3Other.Y, -v3Other.X, 0);
00440
00441 return *this;
00442 }
00443 XnV3DVector& XnV3DVector::UnitOrthogonalVector(const XnV3DVector& v3Other)
00444 {
00445 OrthogonalVector(v3Other);
00446 Normalize();
00447
00448 return *this;
00449 }
00450
00451
00452 XnV3DVector XnV3DVector::operator^(const XnV3DVector& v3Rhs) const
00453 {
00454 XnV3DVector v3Result;
00455 v3Result.CrossProduct(*this, v3Rhs);
00456
00457 return v3Result;
00458 }
00459 XnV3DVector& XnV3DVector::CrossProduct(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs)
00460 {
00461 return Set(v3Lhs.Y * v3Rhs.Z - v3Lhs.Z * v3Rhs.Y,
00462 v3Lhs.Z * v3Rhs.X - v3Lhs.X * v3Rhs.Z,
00463 v3Lhs.X * v3Rhs.Y - v3Lhs.Y * v3Rhs.X);
00464 }
00465
00466
00467 XnFloat XnV3DVector::operator|(const XnV3DVector& v3Rhs) const
00468 {
00469 return DotProduct(*this, v3Rhs);
00470 }
00471 XnFloat DotProduct(const XnV3DVector& v3Lhs, const XnV3DVector& v3Rhs)
00472 {
00473 return v3Lhs.X * v3Rhs.X +
00474 v3Lhs.Y * v3Rhs.Y +
00475 v3Lhs.Z * v3Rhs.Z;
00476 }
00477
00478
00479 XnV3DVector& XnV3DVector::Interpolate(const XnV3DVector& v3Vec1, const XnV3DVector& v3Vec2, XnFloat fAlpha)
00480 {
00481 return Set(v3Vec1.X + fAlpha * (v3Vec2.X - v3Vec1.X),
00482 v3Vec1.Y + fAlpha * (v3Vec2.Y - v3Vec1.Y),
00483 v3Vec1.Z + fAlpha * (v3Vec2.Z - v3Vec1.Z));
00484 }
00485 XnBool XnV3DVector::IsSameDirection(const XnV3DVector& v3Other) const
00486 {
00487 if (IsZero() || v3Other.IsZero())
00488 {
00489 return true;
00490 }
00491
00492 XnFloat fRatio = 0;
00493
00494 if (!XnVMathCommon::IsZero(X, m_fTolerance) && !XnVMathCommon::IsZero(v3Other.X, m_fTolerance))
00495 fRatio = v3Other.X/X;
00496 else if (!XnVMathCommon::IsZero(Y, m_fTolerance) && !XnVMathCommon::IsZero(v3Other.Y, m_fTolerance))
00497 fRatio = v3Other.Y/Y;
00498 else if (!XnVMathCommon::IsZero(Z, m_fTolerance) && !XnVMathCommon::IsZero(v3Other.Z, m_fTolerance))
00499 fRatio = v3Other.Z/Z;
00500 else
00501 {
00502
00503 return false;
00504 }
00505
00506 if (v3Other/fRatio == (*this))
00507 return true;
00508
00509 return false;
00510 }
00511
00512
00513 #endif