C:/Active/libraries/cxutils/2.0/src/cxutils/math/point3d.cpp
Go to the documentation of this file.
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 00031 00032 00033 00034 00035 00036 00037 00038 00039 00040 #include "cxutils/math/point3d.h" 00041 #include "cxutils/math/cxmath.h" 00042 #include <iostream> 00043 00044 using namespace CxUtils; 00045 00046 00054 Point3D::Point3D(const Point3D& p) 00055 { 00056 mX = p.mX; 00057 mY = p.mY; 00058 mZ = p.mZ; 00059 } 00060 00061 00071 Point3D::Point3D(const double x, const double y, const double z) : mX(x), mY(y), mZ(z) 00072 { 00073 00074 } 00075 00076 00088 int Point3D::Set(const double x, const double y, const double z) 00089 { 00090 mX = x; mY = y; mZ = z; 00091 return 1; 00092 } 00093 00094 00106 int Point3D::Get(double& x, double& y, double &z) const 00107 { 00108 x = mX; y = mY; z = mZ; 00109 return 1; 00110 } 00111 00112 00118 void Point3D::Clear() 00119 { 00120 mX = mY = mZ = 0; 00121 } 00122 00123 00132 void Point3D::Floor(const double thresh) 00133 { 00134 if( fabs(mX) <= thresh ) 00135 { 00136 mX = 0.0; 00137 } 00138 if( fabs(mY) <= thresh ) 00139 { 00140 mY = 0.0; 00141 } 00142 if( fabs(mZ) <= thresh ) 00143 { 00144 mZ = 0.0; 00145 } 00146 } 00147 00148 00154 void Point3D::Print() const 00155 { 00156 double x, y, z; 00157 x = fabs(mX) < .0000001 ? 0.0 : mX; 00158 y = fabs(mY) < .0000001 ? 0.0 : mY; 00159 z = fabs(mZ) < .0000001 ? 0.0 : mZ; 00160 std::cout << "<" << x << ", " << y << ", " << z << ">\n"; 00161 } 00162 00163 00174 bool Point3D::IsParallel(const Point3D& p) const 00175 { 00176 double dx1, dx2; 00177 double dy1, dy2; 00178 double dz1, dz2; 00179 00180 dx1 = -mX; 00181 dx2 = -p.mX; 00182 00183 dy1 = -mY; 00184 dy2 = -p.mY; 00185 00186 dz1 = -mZ; 00187 dz2 = -p.mZ; 00188 00189 if(fabs(dy1/(dx1 + CX_EPSILON) - dy2/(dx2 + CX_EPSILON)) > .000000001) 00190 return false; 00191 if(fabs(dz1/(dy1 + CX_EPSILON) - dz2/(dy2 + CX_EPSILON)) > .000000001) 00192 return false; 00193 if(fabs(dx1/(dz1 + CX_EPSILON) - dx2/(dz2 + CX_EPSILON)) > .000000001) 00194 return false; 00195 // If they are all equal, return true. 00196 return true; 00197 } 00198 00199 00211 bool Point3D::IsCoplanar(const Point3D& p1, const Point3D& p2, const Point3D& p3) const 00212 { 00213 Vector3D ab(p1.mX - mX, p1.mY - mY, p1.mZ - mZ); 00214 Vector3D ac(p2.mX - mX, p2.mY - mY, p2.mZ - mZ); 00215 Vector3D ad(p3.mX - mX, p3.mY - mY, p3.mZ - mZ); 00216 00217 if(fabs(ab.Dot(ac*ad)) < CX_EPSILON) 00218 return true; 00219 00220 return 0; 00221 } 00222 00223 00229 bool Point3D::IsCollinear(const Point3D &p1, 00230 const Point3D &p2) const 00231 { 00232 double dx1, dx2; 00233 double dy1, dy2; 00234 double dz1, dz2; 00235 double cx, cy, cz; 00236 dx1 = p1.mX - mX; 00237 dy1 = p1.mY - mY; 00238 dz1 = p1.mZ - mZ; 00239 dx2 = p2.mX - mX; 00240 dy2 = p2.mY - mY; 00241 dz2 = p2.mZ - mZ; 00242 00243 // Perform 3D cross product 00244 cx = dy1*dz1 - dy2*dz1; 00245 cy = dx2*dz1 - dx1*dz2; 00246 cz = dx1*dy2 - dx2*dy1; 00247 00248 // Check for 0 degree angle. 00249 if(fabs(cx*cx + cy*cy + cz*cz) < CX_EPSILON) 00250 return true; 00251 return false; 00252 } 00253 00265 bool Point3D::IsInside(const Point3D &p1, const Point3D &p2, const Point3D &p3) const 00266 { 00267 Point3D u, v, w; 00268 u = p2 - p1; 00269 v = p3 - p1; 00270 double s, t; 00271 double uu, uv, vv, wu, wv, d; 00272 uu = Dot(u, u); 00273 uv = Dot(u, v); 00274 vv = Dot(v, v); 00275 w = *this - p1; 00276 wu = Dot(w, u); 00277 wv = Dot(w, v); 00278 d = uv*uv - uu*vv; 00279 s = (uv*wv - vv*wu)/(d + CX_EPSILON); 00280 if(s < 0.0 || s > 1.0) 00281 return false; 00282 t = (uv*wu - uu*wv)/(d + CX_EPSILON); 00283 if(t < 0.0 || (s + t) > 1.0) 00284 return false; 00285 return true; 00286 } 00287 00288 00296 double Point3D::Distance() const 00297 { 00298 return sqrt(mX*mX + mY*mY + mZ*mZ); 00299 } 00300 00308 double Point3D::Magnitude() const 00309 { 00310 return sqrt(mX*mX + mY*mY + mZ*mZ); 00311 } 00312 00313 00321 double Point3D::SumOfSquares() const 00322 { 00323 return mX*mX + mY*mY + mZ*mZ; 00324 } 00325 00326 00336 double Point3D::Distance(const Point3D& p) const 00337 { 00338 return sqrt((mX - p.mX)*(mX - p.mX) + (mY - p.mY)*(mY - p.mY) + (mZ - p.mZ)*(mZ - p.mZ)); 00339 } 00340 00341 00352 double Point3D::Dot(const Point3D& p) const 00353 { 00354 return mX*p.mX + mY*p.mY + mZ*p.mZ; 00355 } 00356 00357 00365 Point3D& Point3D::Normalize() 00366 { 00367 double mag = sqrt(mX*mX + mY*mY + mZ*mZ); 00368 // Check for divide by zero. 00369 if(mag < CX_EPSILON) 00370 mag = CX_EPSILON; 00371 mX /= mag; 00372 mY /= mag; 00373 mZ /= mag; 00374 return *this; 00375 } 00376 00377 00386 Point3D Point3D::GetUnitVector() const 00387 { 00388 double mag = sqrt(mX*mX + mY*mY + mZ*mZ); 00389 // Check for divide by zero. 00390 if(mag < CX_EPSILON) 00391 mag = CX_EPSILON; 00392 return Point3D(mX/mag, mY/mag, mZ/mag); 00393 } 00394 00395 00407 Point3D Point3D::Rotate(const double angle, 00408 const unsigned int axis, 00409 const bool angleInDegrees) const 00410 { 00411 Quaternion rotation; 00412 00413 switch(axis) 00414 { 00415 case X: 00416 rotation.SetRotationX(angle, angleInDegrees); 00417 break; 00418 case Y: 00419 rotation.SetRotationY(angle, angleInDegrees); 00420 break; 00421 default: // Z 00422 rotation.SetRotationZ(angle, angleInDegrees); 00423 break; 00424 } 00425 00426 return rotation.Rotate(*this); 00427 } 00428 00429 00441 Point3D Point3D::Rotate(const Point3D& pivot, 00442 const double angle, 00443 const unsigned int axis, 00444 const bool angleInDegrees) const 00445 { 00446 Point3D point(mX - pivot.mX, 00447 mY - pivot.mY, 00448 mZ - pivot.mZ); 00449 00450 Quaternion rotation; 00451 00452 switch(axis) 00453 { 00454 case X: 00455 rotation.SetRotationX(angle, angleInDegrees); 00456 break; 00457 case Y: 00458 rotation.SetRotationY(angle, angleInDegrees); 00459 break; 00460 default: // Z 00461 rotation.SetRotationZ(angle, angleInDegrees); 00462 break; 00463 } 00464 00465 return *this + rotation.Rotate(point); 00466 } 00467 00468 00478 Point3D Point3D::Midpoint(const Point3D& p) const 00479 { 00480 return Point3D( (p.mX + mX)/2, (p.mY + mY)/2, (p.mZ + mZ)/2); 00481 } 00482 00493 Point3D Point3D::Midpoint(const Point3D& p1, const Point3D& p2) 00494 { 00495 return Point3D( (p1.mX + p2.mX)/2, (p1.mY + p2.mY)/2, (p1.mZ + p2.mZ)/2); 00496 } 00497 00498 00511 bool Point3D::IsCoplanar(const Point3D& p1, 00512 const Point3D& p2, 00513 const Point3D& p3, 00514 const Point3D& p4) 00515 { 00516 return p1.IsCoplanar(p2, p3, p4); 00517 } 00518 00519 00525 Point3D& Point3D::operator()(const double x, const double y, const double z) 00526 { 00527 mX = x; 00528 mY = y; 00529 mZ = z; 00530 return *this; 00531 } 00532 00538 Point3D& Point3D::operator=(const Point3D& p) 00539 { 00540 mX = p.mX; 00541 mY = p.mY; 00542 mZ = p.mZ; 00543 return *this; 00544 } 00545 00546 00556 Point3D& Point3D::operator+=(const Point3D& p) 00557 { 00558 mX += p.mX; 00559 mY += p.mY; 00560 mZ += p.mZ; 00561 return *this; 00562 } 00563 00564 00574 Point3D& Point3D::operator-=(const Point3D& p) 00575 { 00576 mX -= p.mX; 00577 mY -= p.mY; 00578 mZ -= p.mZ; 00579 return *this; 00580 } 00581 00582 00592 Point3D& Point3D::operator*=(const double val) 00593 { 00594 mX *= val; 00595 mY *= val; 00596 mZ *= val; 00597 return *this; 00598 } 00599 00600 00610 Point3D& Point3D::operator/=(const double val) 00611 { 00612 mX /= val; 00613 mY /= val; 00614 mZ /= val; 00615 return *this; 00616 } 00617 00618 00628 Point3D Point3D::operator+(const Point3D& p) const 00629 { 00630 return Point3D(mX + p.mX, mY + p.mY, mZ + p.mZ); 00631 } 00632 00633 00643 Point3D Point3D::operator-(const Point3D& p) const 00644 { 00645 return Point3D(mX - p.mX, mY - p.mY, mZ - p.mZ); 00646 } 00647 00648 00658 Point3D Point3D::operator*(const Point3D& p) const 00659 { 00660 Point3D r; 00661 00662 r.mX = mY*p.mZ - mZ*p.mY; 00663 r.mY = -1.0*(mX*p.mZ - mZ*p.mX); 00664 r.mZ = mX*p.mY - mY*p.mX; 00665 00666 return r; 00667 } 00668 00669 00679 Point3D Point3D::operator*(const double scalar) const 00680 { 00681 return Point3D(mX*scalar, mY*scalar, mZ*scalar); 00682 } 00683 00684 00694 Point3D Point3D::operator/(const double scalar) const 00695 { 00696 return Point3D(mX/scalar, mY/scalar, mZ/scalar); 00697 } 00698 00699 00710 double Point3D::Distance(const Point3D& p1, const Point3D& p2) 00711 { 00712 return sqrt( (p1.mX - p2.mX)*(p1.mX - p2.mX) + (p1.mY - p2.mY)*(p1.mY - p2.mY) + (p1.mZ - p2.mZ)*(p1.mZ - p2.mZ)); 00713 } 00714 00725 double Point3D::LinearRegressionSlope(const Point3D::List& points) 00726 { 00727 double slopeXY = 0; 00728 double numerator = 0, denominator = 0, sumXY = 0; 00729 CxUtils::Point3D sumPoints, sumPoints2; 00730 if(points.size() < 2) return 0; 00731 00732 for(unsigned int i = 0; i < points.size(); i++) 00733 { 00734 sumPoints += points[i]; 00735 sumPoints2.mX += points[i].mX * points[i].mX; 00736 sumPoints2.mY += points[i].mY * points[i].mY; 00737 sumXY += points[i].mX * points[i].mY; 00738 } 00739 numerator = ((double)points.size()) * sumXY - sumPoints.mX * sumPoints.mY; 00740 denominator = ((double)points.size()) * sumPoints2.mX - sumPoints.mX * sumPoints.mX; 00741 00742 if(denominator < CxUtils::CX_EPSILON) 00743 { 00744 slopeXY = 1.0/CxUtils::CX_EPSILON; 00745 } 00746 else 00747 { 00748 slopeXY = numerator/denominator; 00749 } 00750 00751 return slopeXY; 00752 } 00753 00764 double Point3D::GetLinearRegressionAngle(const Point3D::List& points) 00765 { 00766 double slopeXY = 0; 00767 double numerator = 0, denominator = 0, sumXY = 0; 00768 CxUtils::Point3D sumPoints, sumPoints2; 00769 if(points.size() < 2) return 0; 00770 00771 for(unsigned int i = 0; i < points.size(); i++) 00772 { 00773 sumPoints += points[i]; 00774 sumPoints2.mX += points[i].mX * points[i].mX; 00775 sumPoints2.mY += points[i].mY * points[i].mY; 00776 sumXY += points[i].mX * points[i].mY; 00777 } 00778 numerator = ((double)points.size()) * sumXY - sumPoints.mX * sumPoints.mY; 00779 denominator = ((double)points.size()) * sumPoints2.mX - sumPoints.mX * sumPoints.mX; 00780 00781 //if(denominator < CxUtils::CX_EPSILON) 00782 //{ 00783 // slopeXY = 1.0/CxUtils::CX_EPSILON; 00784 //} 00785 //else 00786 //{ 00787 // slopeXY = numerator/denominator; 00788 //} 00789 return atan2(numerator,denominator); 00790 } 00791 /* End of File */
