1 /* 2 * Copyright (C) 2010-2011 Dmitry Marakasov 3 * 4 * This file is part of glosm. 5 * 6 * glosm is free software: you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation, either version 3 of the License, or 9 * (at your option) any later version. 10 * 11 * glosm is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 * GNU General Public License for more details. 15 * 16 * You should have received a copy of the GNU General Public License 17 * along with glosm. If not, see <http://www.gnu.org/licenses/>. 18 */ 19 20 #ifndef MATH_HH 21 #define MATH_HH 22 23 #include <glosm/osmtypes.h> 24 25 #include <cmath> 26 #include <limits> 27 28 /** 29 * Long type trait 30 * 31 * For type T this defines type that can hold result of T*T (e.g. 32 * results of LengthSquare, DotProduct, area etc.) 33 */ 34 template <typename T> 35 struct LongType { 36 typedef T type; 37 }; 38 39 /** 40 * Long type for osmint_t 41 */ 42 template<> 43 struct LongType<osmint_t> { 44 typedef osmlong_t type; 45 }; 46 47 template <typename T> 48 struct Vector3; 49 50 /** 51 * Template 2D vector class 52 */ 53 template <typename T> 54 struct Vector2 { 55 typedef typename LongType<T>::type LT; 56 57 /* ctors */ Vector2Vector258 Vector2(): x(0), y(0) {} Vector2Vector259 Vector2(T x_, T y_): x(x_), y(y_) {} Vector2Vector260 Vector2(const Vector2<T>& v): x(v.x), y(v.y) {} Vector2Vector261 Vector2(const Vector3<T>& v): x(v.x), y(v.y) {} 62 63 /* static ctoroids */ 64 template <typename TT> FromYawVector265 static Vector2<T> FromYaw(const TT yaw) { return Vector2(std::sin(yaw), std::cos(yaw)); } 66 67 template <typename TT> Vector2Vector268 Vector2(const Vector2<TT>& v): x(v.x), y(v.y) {} 69 70 /* */ LongVectorVector271 Vector2<LT> LongVector() const { 72 return Vector2<LT>(*this); 73 } 74 75 /* operators */ operator -Vector276 Vector2<T> operator- () const { return Vector2<T>(-x, -y); } 77 operator +Vector278 Vector2<T> operator+ (const Vector2<T>& other) const { return Vector2<T>(x + other.x, y + other.y); } operator -Vector279 Vector2<T> operator- (const Vector2<T>& other) const { return Vector2<T>(x - other.x, y - other.y); } operator *Vector280 Vector2<T> operator* (const Vector2<T>& other) const { return Vector2<T>(x * other.x, y * other.y); } operator /Vector281 Vector2<T> operator/ (const Vector2<T>& other) const { return Vector2<T>(x / other.x, y / other.y); } 82 operator +Vector283 Vector2<T> operator+ (const T v) const { return Vector2<T>(x + v, y + v); } operator -Vector284 Vector2<T> operator- (const T v) const { return Vector2<T>(x - v, y - v); } operator *Vector285 Vector2<T> operator* (const T v) const { return Vector2<T>(x * v, y * v); } operator /Vector286 Vector2<T> operator/ (const T v) const { return Vector2<T>(x / v, y / v); } 87 88 /* mutating operators */ operator =Vector289 Vector2<T>& operator= (const Vector2<T>& other) { x = other.x; y = other.y; return *this; } 90 operator +=Vector291 Vector2<T>& operator+= (const Vector2<T>& other) { x += other.x; y += other.y; return *this; } operator -=Vector292 Vector2<T>& operator-= (const Vector2<T>& other) { x -= other.x; y -= other.y; return *this; } operator *=Vector293 Vector2<T>& operator*= (const Vector2<T>& other) { x *= other.x; y *= other.y; return *this; } operator /=Vector294 Vector2<T>& operator/= (const Vector2<T>& other) { x /= other.x; y /= other.y; return *this; } 95 operator +=Vector296 Vector2<T>& operator+= (const T& v) { x += v; y += v; return *this; } operator -=Vector297 Vector2<T>& operator-= (const T& v) { x -= v; y -= v; return *this; } operator *=Vector298 Vector2<T>& operator*= (const T& v) { x *= v; y *= v; return *this; } operator /=Vector299 Vector2<T>& operator/= (const T& v) { x /= v; y /= v; return *this; } 100 101 /* comparison */ operator ==Vector2102 bool operator== (const Vector2<T>& other) const { return x == other.x && y == other.y; } operator !=Vector2103 bool operator!= (const Vector2<T>& other) const { return x != other.x || y != other.y; } 104 105 /* validity check */ IsValidVector2106 bool IsValid() const { 107 return std::isnormal(x) && std::isnormal(y); 108 } 109 110 /* functions */ LengthVector2111 T Length() { return std::sqrt((LT)x*(LT)x + (LT)y*(LT)y); } LengthSquareVector2112 LT LengthSquare() { return (LT)x*(LT)x + (LT)y*(LT)y; } 113 DotProductVector2114 LT DotProduct(const Vector2<T>& other) const { return (LT)x*(LT)other.x + (LT)y*(LT)other.y; } CrossProductVector2115 LT CrossProduct(const Vector2<T>& other) const { return (LT)x*(LT)other.y - (LT)y*(LT)other.x; } 116 NormalizeVector2117 void Normalize() { 118 T len = Length(); 119 if (len == 0) 120 return; 121 122 x /= len; 123 y /= len; 124 } 125 NormalizedVector2126 Vector2<T> Normalized() { 127 T len = Length(); 128 if (len == 0) 129 return Vector2<T>(); /* Zero vector */ 130 131 return Vector2<T>(x / len, y / len); 132 } 133 IsInTriangleVector2134 bool IsInTriangle(const Vector2<T>& v1, const Vector2<T>& v2, const Vector2<T>& v3) { 135 LT a = (v2-v1).CrossProduct(*this - v1); 136 LT b = (v3-v2).CrossProduct(*this - v2); 137 LT c = (v1-v3).CrossProduct(*this - v3); 138 139 return (a < 0 && b < 0 && c < 0) || (a > 0 && b > 0 && c > 0); 140 } 141 142 /* data */ 143 T x; 144 T y; 145 }; 146 147 /** 148 * Template 3D vector class 149 */ 150 template <typename T> 151 struct Vector3 { 152 typedef typename LongType<T>::type LT; 153 154 /* ctors */ Vector3Vector3155 Vector3(): x(0), y(0), z(0) {} Vector3Vector3156 Vector3(T x_, T y_): x(x_), y(y_), z(0) {} Vector3Vector3157 Vector3(T x_, T y_, T z_): x(x_), y(y_), z(z_) {} 158 Vector3Vector3159 Vector3(const Vector2<T>& v): x(v.x), y(v.y), z(0) {} Vector3Vector3160 Vector3(const Vector2<T>& v, T z_): x(v.x), y(v.y), z(z_) {} Vector3Vector3161 Vector3(const Vector3<T>& v): x(v.x), y(v.y), z(v.z) {} 162 163 template <typename TT> Vector3Vector3164 Vector3(const Vector3<TT>& v): x(v.x), y(v.y), z(v.z) {} 165 166 /* static ctoroids */ 167 template <typename TT> FromYawPitchVector3168 static Vector3<T> FromYawPitch(const TT yaw, const TT pitch) { return Vector3(sin(yaw)*cos(pitch), cos(yaw)*cos(pitch), sin(pitch)); } 169 170 /* */ LongVectorVector3171 Vector3<LT> LongVector() const { 172 return Vector3<LT>(*this); 173 } 174 175 /* operators */ operator -Vector3176 Vector3<T> operator- () const { return Vector3<T>(-x, -y, -z); } 177 operator +Vector3178 Vector3<T> operator+ (const Vector3<T>& other) const { return Vector3<T>(x + other.x, y + other.y, z + other.z); } operator -Vector3179 Vector3<T> operator- (const Vector3<T>& other) const { return Vector3<T>(x - other.x, y - other.y, z - other.z); } operator *Vector3180 Vector3<T> operator* (const Vector3<T>& other) const { return Vector3<T>(x * other.x, y * other.y, z * other.z); } operator /Vector3181 Vector3<T> operator/ (const Vector3<T>& other) const { return Vector3<T>(x / other.x, y / other.y, z / other.z); } 182 operator *Vector3183 Vector3<T> operator* (const T v) const { return Vector3<T>(x * v, y * v, z * v); } operator /Vector3184 Vector3<T> operator/ (const T v) const { return Vector3<T>(x / v, y / v, z / v); } 185 186 /* mutating operators */ operator =Vector3187 Vector3<T>& operator= (const Vector3<T>& other) { x = other.x; y = other.y; z = other.z; return *this; } 188 operator +=Vector3189 Vector3<T>& operator+= (const Vector3<T>& other) { x += other.x; y += other.y; z += other.z; return *this; } operator -=Vector3190 Vector3<T>& operator-= (const Vector3<T>& other) { x -= other.x; y -= other.y; z -= other.z; return *this; } operator *=Vector3191 Vector3<T>& operator*= (const Vector3<T>& other) { x *= other.x; y *= other.y; z *= other.z; return *this; } operator /=Vector3192 Vector3<T>& operator/= (const Vector3<T>& other) { x /= other.x; y /= other.y; z /= other.z; return *this; } 193 operator *=Vector3194 Vector3<T>& operator*= (const T& v) { x *= v; y *= v; z *= v; return *this; } operator /=Vector3195 Vector3<T>& operator/= (const T& v) { x /= v; y /= v; z /= v; return *this; } 196 197 /* comparison */ operator ==Vector3198 bool operator== (const Vector3<T>& other) const { return x == other.x && y == other.y && z == other.z; } operator !=Vector3199 bool operator!= (const Vector3<T>& other) const { return x != other.x || y != other.y || z != other.z; } 200 201 /* validity check */ IsValidVector3202 bool IsValid() const { 203 return std::isnormal(x) && std::isnormal(y) && std::isnormal(z); 204 } 205 206 /* functions */ LengthVector3207 T Length() const { return std::sqrt((LT)x*(LT)x + (LT)y*(LT)y + (LT)z*(LT)z); } LengthSquareVector3208 LT LengthSquare() const { return (LT)x*(LT)x + (LT)y*(LT)y + (LT)z*(LT)z; } 209 DotProductVector3210 LT DotProduct(const Vector3<T>& other) const { return (LT)x*(LT)other.x + (LT)y*(LT)other.y + (LT)z*(LT)other.z; } CrossProductVector3211 Vector3<LT> CrossProduct(const Vector3<T>& other) const { return Vector3<LT>((LT)y*(LT)other.z - (LT)z*(LT)other.y, (LT)z*(LT)other.x - (LT)x*(LT)other.z, (LT)x*(LT)other.y - (LT)y*(LT)other.x); } 212 NormalizeVector3213 void Normalize() { 214 T len = Length(); 215 if (len == 0) 216 return; 217 218 x /= len; 219 y /= len; 220 z /= len; 221 } 222 NormalizedVector3223 Vector3<T> Normalized() const { 224 T len = Length(); 225 if (len == 0) 226 return Vector3<T>(); /* Zero vector */ 227 228 return Vector3<T>(x / len, y / len, z / len); 229 } 230 FlattenedVector3231 Vector3<T> Flattened() const { 232 return Vector3<T>(x, y, 0); 233 } 234 235 /* data */ 236 T x; 237 T y; 238 T z; 239 }; 240 241 typedef Vector2<osmint_t> Vector2i; 242 typedef Vector2<osmlong_t> Vector2l; 243 typedef Vector2<float> Vector2f; 244 typedef Vector2<double> Vector2d; 245 246 typedef Vector3<osmint_t> Vector3i; 247 typedef Vector3<osmlong_t> Vector3l; 248 typedef Vector3<float> Vector3f; 249 typedef Vector3<double> Vector3d; 250 251 #endif 252