SUMO - Simulation of Urban MObility
|
00001 /****************************************************************************/ 00010 // A position in the 2D- or 3D-world 00011 /****************************************************************************/ 00012 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/ 00013 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors 00014 /****************************************************************************/ 00015 // 00016 // This file is part of SUMO. 00017 // SUMO is free software: you can redistribute it and/or modify 00018 // it under the terms of the GNU General Public License as published by 00019 // the Free Software Foundation, either version 3 of the License, or 00020 // (at your option) any later version. 00021 // 00022 /****************************************************************************/ 00023 #ifndef Position_h 00024 #define Position_h 00025 00026 00027 // =========================================================================== 00028 // included modules 00029 // =========================================================================== 00030 #include <iostream> 00031 #include <cmath> 00032 00033 #ifdef _MSC_VER 00034 #include <windows_config.h> 00035 #else 00036 #include <config.h> 00037 #endif 00038 00039 // =========================================================================== 00040 // class definitions 00041 // =========================================================================== 00046 class Position { 00047 public: 00049 Position() : myX(0.0), myY(0.0), myZ(0.0) { } 00050 00052 Position(SUMOReal x, SUMOReal y) 00053 : myX(x), myY(y), myZ(0) { } 00054 00056 Position(SUMOReal x, SUMOReal y, SUMOReal z) 00057 : myX(x), myY(y), myZ(z) { } 00058 00060 ~Position() { } 00061 00063 SUMOReal x() const { 00064 return myX; 00065 } 00066 00068 SUMOReal y() const { 00069 return myY; 00070 } 00071 00073 SUMOReal z() const { 00074 return myZ; 00075 } 00076 00078 void set(SUMOReal x, SUMOReal y) { 00079 myX = x; 00080 myY = y; 00081 } 00082 00084 void set(SUMOReal x, SUMOReal y, SUMOReal z) { 00085 myX = x; 00086 myY = y; 00087 myZ = z; 00088 } 00089 00091 void set(const Position& pos) { 00092 myX = pos.myX; 00093 myY = pos.myY; 00094 myZ = pos.myZ; 00095 } 00096 00097 00099 void mul(SUMOReal val) { 00100 myX *= val; 00101 myY *= val; 00102 myZ *= val; 00103 } 00104 00106 void mul(SUMOReal mx, SUMOReal my) { 00107 myX *= mx; 00108 myY *= my; 00109 } 00110 00112 void mul(SUMOReal mx, SUMOReal my, SUMOReal mz) { 00113 myX *= mx; 00114 myY *= my; 00115 myZ *= mz; 00116 } 00117 00119 void add(const Position& pos) { 00120 myX += pos.myX; 00121 myY += pos.myY; 00122 myZ += pos.myZ; 00123 } 00124 00126 void add(SUMOReal dx, SUMOReal dy) { 00127 myX += dx; 00128 myY += dy; 00129 } 00130 00132 void add(SUMOReal dx, SUMOReal dy, SUMOReal dz) { 00133 myX += dx; 00134 myY += dy; 00135 myZ += dz; 00136 } 00137 00139 void sub(SUMOReal dx, SUMOReal dy) { 00140 myX -= dx; 00141 myY -= dy; 00142 } 00143 00145 void sub(SUMOReal dx, SUMOReal dy, SUMOReal dz) { 00146 myX -= dx; 00147 myY -= dy; 00148 myZ -= dz; 00149 } 00150 00152 void sub(const Position& pos) { 00153 myX -= pos.myX; 00154 myY -= pos.myY; 00155 myZ -= pos.myZ; 00156 } 00157 00158 void norm2d() { 00159 SUMOReal val = sqrt(myX * myX + myY * myY); 00160 myX = myX / val; 00161 myY = myY / val; 00162 } 00163 00164 void reshiftRotate(SUMOReal xoff, SUMOReal yoff, SUMOReal rot) { 00165 SUMOReal x = myX * cos(rot) - myY * sin(rot) + xoff; 00166 SUMOReal y = myX * sin(rot) + yoff + myY * cos(rot); 00167 myX = x; 00168 myY = y; 00169 } 00170 00171 00173 friend std::ostream& operator<<(std::ostream& os, const Position& p) { 00174 os << p.x() << "," << p.y(); 00175 if (p.z() != SUMOReal(0.0)) { 00176 os << "," << p.z(); 00177 } 00178 return os; 00179 } 00180 00181 Position operator+(const Position& p2) const { 00182 return Position(myX + p2.myX, myY + p2.myY, myZ + p2.myZ); 00183 } 00184 00185 Position operator-(const Position& p2) const { 00186 return Position(myX - p2.myX, myY - p2.myY, myZ - p2.myZ); 00187 } 00188 00189 Position operator*(SUMOReal scalar) const { 00190 return Position(myX * scalar, myY * scalar, myZ * scalar); 00191 } 00192 00193 bool operator==(const Position& p2) const { 00194 return myX == p2.myX && myY == p2.myY && myZ == p2.myZ; 00195 } 00196 00197 bool operator!=(const Position& p2) const { 00198 return myX != p2.myX || myY != p2.myY || myZ != p2.myZ; 00199 } 00200 00201 00202 bool almostSame(const Position& p2, SUMOReal maxDiv = POSITION_EPS) const { 00203 return fabs(myX - p2.myX) < maxDiv && fabs(myY - p2.myY) < maxDiv && fabs(myZ - p2.myZ) < maxDiv; 00204 } 00205 00206 00208 inline SUMOReal distanceTo(const Position& p2) const { 00209 return sqrt(distanceSquaredTo(p2)); 00210 } 00211 00212 00213 inline SUMOReal distanceSquaredTo(const Position& p2) const { 00214 return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY) + (myZ - p2.myZ) * (myZ - p2.myZ); 00215 } 00216 00217 00219 inline SUMOReal distanceTo2D(const Position& p2) const { 00220 return sqrt(distanceSquaredTo2D(p2)); 00221 } 00222 00223 00224 inline SUMOReal distanceSquaredTo2D(const Position& p2) const { 00225 return (myX - p2.myX) * (myX - p2.myX) + (myY - p2.myY) * (myY - p2.myY); 00226 } 00227 00229 Position crossProduct(const Position& pos) { 00230 return Position( 00231 myY * pos.myZ - myZ * pos.myY, 00232 myZ * pos.myX - myX * pos.myZ, 00233 myX * pos.myY - myY * pos.myX); 00234 } 00235 00237 inline SUMOReal dotProduct(const Position& pos) { 00238 return myX * pos.myX + myY * pos.myY + myZ * pos.myZ; 00239 } 00240 00241 00242 private: 00244 SUMOReal myX; 00245 00247 SUMOReal myY; 00248 00250 SUMOReal myZ; 00251 00252 }; 00253 00254 00255 #endif 00256 00257 /****************************************************************************/ 00258