MTK++ Latest version: 0.2.0

vector3d.h
Go to the documentation of this file.
00001 
00051 #ifndef VECTOR3D_H
00052 #define VECTOR3D_H
00053 
00054 #include <math.h>
00055 #include <cmath>
00056 #include <iostream>
00057 
00058 #include "constants.h"
00059 
00060 namespace MTKpp
00061 {
00062 
00063 // ============================================================
00064 // Class : vector3d()
00065 // ------------------------------------------------------------
00074 // ============================================================
00075 class vector3d
00076 {
00077 public:
00078 
00084    inline vector3d() 
00085           : X(0.0), Y(0.0), Z(0.0) {};
00086 
00093    inline vector3d(double x, double y, double z)
00094           : X(x), Y(y), Z(z) {};
00095 
00100    inline vector3d(double xyz)
00101           : X(xyz), Y(xyz), Z(xyz) {};
00102 
00109    inline void set(double x, double y, double z) {
00110           this->X = x;
00111           this->Y = y;
00112           this->Z = z;}
00113 
00118    inline void setX(double x) {
00119           this->X = x;}
00120 
00125    inline void setY(double y) {
00126           this->Y = y;}
00127 
00132    inline void setZ(double z) {
00133           this->Z = z;}
00134 
00139    inline double getX() {
00140           return this->X;}
00141 
00146    inline double getY() {
00147           return this->Y;}
00148 
00153    inline double getZ() {
00154           return this->Z;}
00155 
00161    inline double &operator[](int i) {
00162           return  i == 0 ? X
00163                  :i == 1 ? Y
00164                  :i == 2 ? Z
00165                  : X; }
00166 
00172    inline vector3d& operator=(const double &value) {
00173             X = value;
00174             Y = value;
00175             Z = value;
00176             return *this; }
00177 
00183    inline friend vector3d operator-(const vector3d &lhs) {
00184             return vector3d(-lhs.X, -lhs.Y, -lhs.Z); }
00185 
00190    inline void operator+=(const vector3d &rhs) {
00191             X += rhs.X;
00192             Y += rhs.Y;
00193             Z += rhs.Z; }
00194 
00199    inline void operator-=(const vector3d &rhs) {
00200             X -= rhs.X;
00201             Y -= rhs.Y;
00202             Z -= rhs.Z; }
00203 
00208    inline void operator*=(const double &value) {
00209             X *= value;
00210             Y *= value;
00211             Z *= value; }
00212 
00217    inline void operator/=(const double &value) {
00218             X /= value;
00219             Y /= value;
00220             Z /= value; }
00221 
00227    inline friend bool operator==(const vector3d &lhs, const vector3d &rhs) {
00228             return (lhs.X == rhs.X) && (lhs.Y == rhs.Y) && (lhs.Z == rhs.Z); }
00229 
00235    inline friend bool operator!=(const vector3d &lhs, const vector3d &rhs) {
00236             return (lhs.X != rhs.X) || (lhs.Y != rhs.Y) || (lhs.Z != rhs.Z); }
00237 
00243    inline friend vector3d operator+(const vector3d &lhs, const vector3d &rhs) {
00244             return vector3d( (lhs.X + rhs.X), (lhs.Y + rhs.Y), (lhs.Z + rhs.Z) ); }
00245 
00251    inline friend vector3d operator-(const vector3d &lhs, const vector3d &rhs) {
00252             return vector3d( (lhs.X - rhs.X), (lhs.Y - rhs.Y), (lhs.Z - rhs.Z) ); }
00253 
00260    inline friend double operator*(const vector3d &lhs, const vector3d &rhs) {
00261             return lhs.X*rhs.X + lhs.Y*rhs.Y + lhs.Z*rhs.Z; }
00262 
00269    inline friend vector3d operator+(const vector3d &lhs, const double &scalar) {
00270             return vector3d( lhs.X + scalar, lhs.Y + scalar, lhs.Z + scalar); }
00271 
00278    inline friend vector3d operator+(const double &scalar, const vector3d &rhs) {
00279             return vector3d( rhs.X + scalar, rhs.Y + scalar, rhs.Z + scalar); }
00280 
00287    inline friend vector3d operator-(const vector3d &lhs, const double &scalar) {
00288             return vector3d( lhs.X - scalar, lhs.Y - scalar, lhs.Z - scalar); }
00289 
00296    inline friend vector3d operator-(const double &scalar, const vector3d &rhs) {
00297             return vector3d( scalar - rhs.X, scalar - rhs.Y, scalar - rhs.Z); }
00298 
00305    inline friend vector3d operator*(const vector3d &lhs, const double &scalar) {
00306             return vector3d( scalar*lhs.X, scalar*lhs.Y, scalar*lhs.Z ); }
00307 
00314    inline friend vector3d operator*(const double &scalar, const vector3d &rhs) {
00315             return vector3d( scalar*rhs.X, scalar*rhs.Y, scalar*rhs.Z) ; }
00316 
00323    inline friend vector3d operator/(const vector3d &lhs, const double &scalar) {
00324             return vector3d( lhs.X/scalar, lhs.Y/scalar, lhs.Z/scalar ); }
00325 
00331    inline friend std::ostream& operator<< (std::ostream& os, vector3d& v) {
00332       os << v.X << " " << v.Y << " " << v.Z;
00333       return os; }
00334 
00339    inline double length() {
00340             return sqrt(X*X + Y*Y + Z*Z); }
00341 
00346    inline double length_squared() {
00347             return (X*X + Y*Y + Z*Z); }
00348 
00353    inline vector3d unit() {
00354              return vector3d(X, Y, Z)/length(); }
00355 
00360    inline friend vector3d cross(const vector3d &lhs, const vector3d &rhs) {
00361             return vector3d( (lhs.Y*rhs.Z - rhs.Y*lhs.Z),
00362                              (lhs.Z*rhs.X - rhs.Z*lhs.X),
00363                              (lhs.X*rhs.Y - rhs.X*lhs.Y) ); }
00364 
00369    inline double dist(const vector3d &v2) {
00370             return sqrt( (X-v2.X)*(X-v2.X) + (Y-v2.Y)*(Y-v2.Y) + (Z-v2.Z)*(Z-v2.Z) ); }
00371 
00376    inline double dist2(const vector3d &v2) {
00377             return ( (X-v2.X)*(X-v2.X) + (Y-v2.Y)*(Y-v2.Y) + (Z-v2.Z)*(Z-v2.Z) ); }
00378 
00386    inline friend double angle(const vector3d &a, const vector3d &b,
00387                               const vector3d &c) {
00388             vector3d ab = a - b;
00389             vector3d cb = c - b;
00390 
00391             ab = ab.unit();
00392             cb = cb.unit();
00393 
00394             return acos(ab*cb); }
00395 
00417    inline friend double torsion(const vector3d& a, const vector3d& b,
00418                                 const vector3d& c, const vector3d& d) {
00419             vector3d ab = a - b;
00420             vector3d cb = c - b;
00421             vector3d cd = c - d;
00422 
00423             vector3d p,q;
00424             p = cross(ab, cb);
00425             q = cross(cb, cd);
00426 
00427             p = p.unit();
00428             q = q.unit();
00429 
00430             double pXq = p*q;
00431             if (std::abs(pXq) > 1) {
00432               pXq = 1;
00433             }
00434             else if (pXq < -1) {
00435               pXq = -1;
00436             }
00437 
00438             // acos calculates the arc cosine
00439             // Return value in range [0, PI].
00440             double ang = acos(pXq);
00441             double s   = cb*(cross(p, q));
00442 
00443             if (s < 0) ang = -ang;
00444 
00445             return ang;   }
00446 
00466    inline friend double torsion2(const vector3d& a, const vector3d& b,
00467                                  const vector3d& c, const vector3d& d) {
00468             vector3d b1 = a - b;
00469             vector3d b2 = c - b;
00470             vector3d b3 = c - d;
00471 
00472             double b2Length = b2.length();
00473             vector3d v1 = b2Length * b1;
00474 
00475             vector3d b2Xb3 = cross(b2, b3);
00476             vector3d b1Xb2 = cross(b1, b2);
00477 
00478             double lhs = v1 * b2Xb3;
00479             double rhs = b1Xb2 * b2Xb3;
00480 
00481             // atan2 calculates the arc tangent of lhs/rhs in radians
00482             // To compute the value, the function uses the sign of both 
00483             //   arguments to determine the quadrant.
00484             // Return value in range [-PI, PI].
00485             double ang = atan2(lhs, rhs);
00486 
00487             return ang;   }
00488 
00493    inline void rotateX(const double &ang) {
00494             double cosAng = cos(ang);
00495             double sinAng = sin(ang);
00496             double Yt     = Y;
00497             double Zt     = Z;
00498 
00499             Y = Yt*cosAng - Zt*sinAng;
00500             Z = Yt*sinAng + Zt*cosAng;  }
00501 
00507    inline void rotateX(const double &ang, const vector3d &o) {
00508             double dY = Y - o.Y;
00509             double dZ = Z - o.Z;
00510             double cosAng = cos(ang);
00511             double sinAng = sin(ang);
00512             double Yt     = Y;
00513             double Zt     = Z;           
00514 
00515             Yt = dY*cosAng - dZ*sinAng;
00516             Zt = dY*sinAng + dZ*cosAng;
00517 
00518             Y = Yt + o.Y;
00519             Z = Zt + o.Z;  }
00520 
00525    inline void rotateY(const double &ang) {
00526             double cosAng = cos(ang);
00527             double sinAng = sin(ang);
00528             double Zt     = Z;
00529             double Xt     = X;
00530             
00531             X = Zt*sinAng + Xt*cosAng;
00532             Z = Zt*cosAng - Xt*sinAng;  }
00533 
00539    inline void rotateY(const double &ang, const vector3d &o) {
00540             double dX = X - o.X;
00541             double dZ = Z - o.Z;
00542             double cosAng = cos(ang);
00543             double sinAng = sin(ang);
00544             double Zt     = Z;
00545             double Xt     = X;
00546 
00547             Xt = dZ*sinAng + dX*cosAng;
00548             Zt = dZ*cosAng - dX*sinAng;
00549             X = Xt + o.X;
00550             Z = Zt + o.Z;  }
00551 
00556    inline void rotateZ(const double &ang) {
00557             double cosAng = cos(ang);
00558             double sinAng = sin(ang);
00559             double Xt     = X;
00560             double Yt     = Y;
00561             
00562             X = Xt*cosAng - Yt*sinAng;
00563             Y = Xt*sinAng + Yt*cosAng;  }
00564 
00570    inline void rotateZ(const double &ang, const vector3d &o) {
00571             double dX = X - o.X;
00572             double dY = Y - o.Y;
00573             double cosAng = cos(ang);
00574             double sinAng = sin(ang);
00575             double Xt     = X;
00576             double Yt     = Y;
00577 
00578             Xt = dX*cosAng - dY*sinAng;
00579             Yt = dX*sinAng + dY*cosAng;
00580 
00581             X = Xt + o.X;
00582             Y = Yt + o.Y;  }
00583 
00622    inline friend int formRotMat(const vector3d& a, const vector3d& b,
00623                                 const vector3d& c, const vector3d& d,
00624                                 double angle, double rotMat[9]) {
00625 
00626             // Measure Torsion, t
00627             double torsionAngle = torsion(a,b,c,d);
00628             if (torsionAngle < 0) torsionAngle = torsionAngle + 2*PI;
00629 
00630             // Calculate Rotational Angle
00631             double rotAngle = angle - torsionAngle;
00632 
00633             double cosAng = 0;
00634             double sinAng = 0;
00635             double t = 0;
00636             vector3d bc;
00637 
00638             if (std::abs(rotAngle) > 0.0001) {
00639               cosAng = cos(rotAngle);
00640               sinAng = sin(rotAngle);
00641               t = 1.0 - cosAng;
00642 
00643               // Normalize the vector b - c
00644               bc = b - c;
00645               bc = bc.unit();
00646             }
00647             else {
00648               return 0;
00649             }
00650 
00651             // Form R
00652             rotMat[0] = t*bc.X*bc.X + cosAng;
00653             rotMat[1] = t*bc.X*bc.Y + sinAng*bc.Z;
00654             rotMat[2] = t*bc.X*bc.Z - sinAng*bc.Y;
00655 
00656             rotMat[3] = t*bc.X*bc.Y - sinAng*bc.Z;
00657             rotMat[4] = t*bc.Y*bc.Y + cosAng;
00658             rotMat[5] = t*bc.Y*bc.Z + sinAng*bc.X;
00659 
00660             rotMat[6] = t*bc.X*bc.Z + sinAng*bc.Y;
00661             rotMat[7] = t*bc.Y*bc.Z - sinAng*bc.X;
00662             rotMat[8] = t*bc.Z*bc.Z + cosAng; 
00663             return 1;}
00664 
00670    inline void rotateXYZ(const double R[9], const vector3d& o) {
00671             this->X -= o.X;
00672             this->Y -= o.Y;
00673             this->Z -= o.Z;
00674             double newX = this->X*R[0] + this->Y*R[1] + this->Z*R[2];
00675             double newY = this->X*R[3] + this->Y*R[4] + this->Z*R[5];
00676             double newZ = this->X*R[6] + this->Y*R[7] + this->Z*R[8];
00677             this->X = newX + o.X;
00678             this->Y = newY + o.Y;
00679             this->Z = newZ + o.Z;}
00680 
00696    inline friend void buildCoord(vector3d& a, const vector3d& b,
00697                                  const vector3d& c, const vector3d& d,
00698                                  double dist, double angle, double torsion) {
00699             // Normalize the vector b - c 
00700             vector3d bc = b - c;
00701             bc = bc.unit();
00702 
00703             // Normalize the vector d - c 
00704             vector3d dc = d - c;
00705             dc = dc.unit();
00706 
00707             vector3d r = cross(bc, dc);
00708             r = r.unit();
00709 
00710             vector3d s = cross(r, bc);
00711             s = s.unit();
00712 
00713             a = b - dist * cos(angle) * bc + 
00714                     dist * sin(angle) * sin(torsion) * r +
00715                     dist * sin(angle) * cos(torsion) * s;}
00716 
00717 protected:
00718 
00720    double X;
00721 
00723    double Y;
00724 
00726    double Z;
00727 };
00728 
00729 } // MTKpp namespace
00730 
00731 #endif

Generated on Fri Dec 23 2011 09:28:52 for MTK++ by Doxygen 1.7.5.1