Changeset 2171 for code/trunk/src/util/Math.cc
- Timestamp:
- Nov 10, 2008, 12:05:03 AM (16 years ago)
- Location:
- code/trunk
- Files:
-
- 3 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk
- Property svn:mergeinfo changed
/code/branches/objecthierarchy merged: 2111-2115,2123,2132-2134,2143-2144,2153-2158,2160-2169
- Property svn:mergeinfo changed
-
code/trunk/src/util
- Property svn:mergeinfo changed
/code/branches/objecthierarchy/src/util merged: 2111,2114,2123,2155,2158,2161-2162
- Property svn:mergeinfo changed
-
code/trunk/src/util/Math.cc
r2087 r2171 38 38 #include "SubString.h" 39 39 40 / **41 @brief Function for writing a Radian to a stream. 42 */ 43 std::ostream& operator<<(std::ostream& out, const orxonox::Radian& radian) 40 // Do not remove this include to avoid linker errors. 41 #include "mbool.h" 42 43 namespace orxonox 44 44 { 45 out << radian.valueRadians(); 46 return out; 45 /** 46 @brief Function for writing a Radian to a stream. 47 */ 48 std::ostream& operator<<(std::ostream& out, const orxonox::Radian& radian) 49 { 50 out << radian.valueRadians(); 51 return out; 52 } 53 54 /** 55 @brief Function for reading a Radian from a stream. 56 */ 57 std::istream& operator>>(std::istream& in, orxonox::Radian& radian) 58 { 59 float temp; 60 in >> temp; 61 radian = temp; 62 return in; 63 } 64 65 /** 66 @brief Function for writing a Degree to a stream. 67 */ 68 std::ostream& operator<<(std::ostream& out, const orxonox::Degree& degree) 69 { 70 out << degree.valueDegrees(); 71 return out; 72 } 73 74 /** 75 @brief Function for reading a Degree from a stream. 76 */ 77 std::istream& operator>>(std::istream& in, orxonox::Degree& degree) 78 { 79 float temp; 80 in >> temp; 81 degree = temp; 82 return in; 83 } 84 85 86 /** 87 @brief Gets the angle between my viewing direction and the direction to the position of the other object. 88 @param myposition My position 89 @param mydirection My viewing direction 90 @param otherposition The position of the other object 91 @return The angle 92 93 @example 94 If the other object is exactly in front of me, the function returns 0. 95 If the other object is exactly behind me, the function returns pi. 96 If the other object is exactly right/left to me (or above/below), the function returns pi/2. 97 */ 98 float getAngle(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& otherposition) 99 { 100 orxonox::Vector3 distance = otherposition - myposition; 101 float distancelength = distance.length(); 102 if (distancelength == 0) 103 return 0; 104 else 105 return acos(clamp<float>(mydirection.dotProduct(distance) / distancelength, -1, 1)); 106 } 107 108 /** 109 @brief Gets the 2D viewing direction (up/down, left/right) to the position of the other object. 110 @param myposition My position 111 @param mydirection My viewing direction 112 @param myorthonormal My orthonormalvector (pointing upwards through my head) 113 @param otherposition The position of the other object 114 @return The viewing direction 115 116 @example 117 If the other object is exactly in front of me, the function returns Vector2(0, 0). 118 If the other object is exactly at my left, the function returns Vector2(-1, 0). 119 If the other object is exactly at my right, the function returns Vector2(1, 0). 120 If the other object is only a bit at my right, the function still returns Vector2(1, 0). 121 If the other object is exactly above me, the function returns Vector2(0, 1). 122 */ 123 orxonox::Vector2 get2DViewdirection(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition) 124 { 125 orxonox::Vector3 distance = otherposition - myposition; 126 127 // project difference vector on our plane 128 orxonox::Vector3 projection = Ogre::Plane(mydirection, myposition).projectVector(distance); 129 130 float projectionlength = projection.length(); 131 if (projectionlength == 0) return orxonox::Vector2(0, 0); 132 float angle = acos(clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1)); 133 134 if ((mydirection.crossProduct(myorthonormal)).dotProduct(distance) > 0) 135 return orxonox::Vector2(sin(angle), cos(angle)); 136 else 137 return orxonox::Vector2(-sin(angle), cos(angle)); 138 } 139 140 /** 141 @brief Gets the 2D viewing direction (up/down, left/right) to the position of the other object, multiplied with the viewing distance to the object (0° = 0, 180° = 1). 142 @param myposition My position 143 @param mydirection My viewing direction 144 @param myorthonormal My orthonormalvector (pointing upwards through my head) 145 @param otherposition The position of the other object 146 @return The viewing direction 147 148 @example 149 If the other object is exactly in front of me, the function returns Vector2(0, 0). 150 If the other object is exactly at my left, the function returns Vector2(-0.5, 0). 151 If the other object is exactly at my right, the function returns Vector2(0.5, 0). 152 If the other object is only a bit at my right, the function still returns Vector2(0.01, 0). 153 If the other object is exactly above me, the function returns Vector2(0, 0.5). 154 */ 155 orxonox::Vector2 get2DViewcoordinates(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition) 156 { 157 orxonox::Vector3 distance = otherposition - myposition; 158 159 // project difference vector on our plane 160 orxonox::Vector3 projection = Ogre::Plane(mydirection, myposition).projectVector(distance); 161 162 float projectionlength = projection.length(); 163 if (projectionlength == 0) return orxonox::Vector2(0, 0); 164 float angle = acos(clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1)); 165 166 float distancelength = distance.length(); 167 if (distancelength == 0) return orxonox::Vector2(0, 0); 168 float radius = acos(clamp<float>(mydirection.dotProduct(distance) / distancelength, -1, 1)) / Ogre::Math::PI; 169 170 if ((mydirection.crossProduct(myorthonormal)).dotProduct(distance) > 0) 171 return orxonox::Vector2(sin(angle) * radius, cos(angle) * radius); 172 else 173 return orxonox::Vector2(-sin(angle) * radius, cos(angle) * radius); 174 } 175 176 /** 177 @brief Returns the predicted position I have to aim at, if I want to hit a moving target with a moving projectile. 178 @param myposition My position 179 @param projectilespeed The speed of my projectile 180 @param targetposition The position of my target 181 @param targetvelocity The velocity of my target 182 @return The predicted position 183 184 The function predicts the position based on a linear velocity of the target. If the target changes speed or direction, the projectile will miss. 185 */ 186 orxonox::Vector3 getPredictedPosition(const orxonox::Vector3& myposition, float projectilespeed, const orxonox::Vector3& targetposition, const orxonox::Vector3& targetvelocity) 187 { 188 float squaredProjectilespeed = projectilespeed * projectilespeed; 189 orxonox::Vector3 distance = targetposition - myposition; 190 float a = distance.squaredLength(); 191 float b = 2 * (distance.x + distance.y + distance.z) * (targetvelocity.x + targetvelocity.y + targetvelocity.z); 192 float c = targetvelocity.squaredLength(); 193 194 float temp = 4*squaredProjectilespeed*c + a*a - 4*b*c; 195 if (temp < 0) 196 return orxonox::Vector3::ZERO; 197 198 temp = sqrt(temp); 199 float time = (temp + a) / (2 * (squaredProjectilespeed - b)); 200 return (targetposition + targetvelocity * time); 201 } 202 203 unsigned long getUniqueNumber() 204 { 205 static unsigned long aNumber = 135; 206 return aNumber++; 207 } 208 209 210 ////////////////////////// 211 // Conversion functions // 212 ////////////////////////// 213 214 // std::string to Vector2 215 bool ConverterFallback<std::string, orxonox::Vector2>::convert(orxonox::Vector2* output, const std::string& input) 216 { 217 size_t opening_parenthesis, closing_parenthesis = input.find(')'); 218 if ((opening_parenthesis = input.find('(')) == std::string::npos) 219 opening_parenthesis = 0; 220 else 221 opening_parenthesis++; 222 223 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), 224 ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0'); 225 if (tokens.size() >= 2) 226 { 227 if (!ConvertValue(&(output->x), tokens[0])) 228 return false; 229 if (!ConvertValue(&(output->y), tokens[1])) 230 return false; 231 232 return true; 233 } 234 return false; 235 } 236 237 // std::string to Vector3 238 bool ConverterFallback<std::string, orxonox::Vector3>::convert(orxonox::Vector3* output, const std::string& input) 239 { 240 size_t opening_parenthesis, closing_parenthesis = input.find(')'); 241 if ((opening_parenthesis = input.find('(')) == std::string::npos) 242 opening_parenthesis = 0; 243 else 244 opening_parenthesis++; 245 246 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), 247 ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0'); 248 if (tokens.size() >= 3) 249 { 250 if (!ConvertValue(&(output->x), tokens[0])) 251 return false; 252 if (!ConvertValue(&(output->y), tokens[1])) 253 return false; 254 if (!ConvertValue(&(output->z), tokens[2])) 255 return false; 256 257 return true; 258 } 259 return false; 260 } 261 262 // std::string to Vector4 263 bool ConverterFallback<std::string, orxonox::Vector4>::convert(orxonox::Vector4* output, const std::string& input) 264 { 265 size_t opening_parenthesis, closing_parenthesis = input.find(')'); 266 if ((opening_parenthesis = input.find('(')) == std::string::npos) 267 opening_parenthesis = 0; 268 else 269 opening_parenthesis++; 270 271 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), 272 ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0'); 273 if (tokens.size() >= 4) 274 { 275 if (!ConvertValue(&(output->x), tokens[0])) 276 return false; 277 if (!ConvertValue(&(output->y), tokens[1])) 278 return false; 279 if (!ConvertValue(&(output->z), tokens[2])) 280 return false; 281 if (!ConvertValue(&(output->w), tokens[3])) 282 return false; 283 284 return true; 285 } 286 return false; 287 } 288 289 // std::string to Quaternion 290 bool ConverterFallback<std::string, orxonox::Quaternion>::convert(orxonox::Quaternion* output, const std::string& input) 291 { 292 size_t opening_parenthesis, closing_parenthesis = input.find(')'); 293 if ((opening_parenthesis = input.find('(')) == std::string::npos) { opening_parenthesis = 0; } else { opening_parenthesis++; } 294 295 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0'); 296 if (tokens.size() >= 4) 297 { 298 if (!ConvertValue(&(output->w), tokens[0])) 299 return false; 300 if (!ConvertValue(&(output->x), tokens[1])) 301 return false; 302 if (!ConvertValue(&(output->y), tokens[2])) 303 return false; 304 if (!ConvertValue(&(output->z), tokens[3])) 305 return false; 306 307 return true; 308 } 309 return false; 310 } 311 312 // std::string to ColourValue 313 bool ConverterFallback<std::string, orxonox::ColourValue>::convert(orxonox::ColourValue* output, const std::string& input) 314 { 315 size_t opening_parenthesis, closing_parenthesis = input.find(')'); 316 if ((opening_parenthesis = input.find('(')) == std::string::npos) { opening_parenthesis = 0; } else { opening_parenthesis++; } 317 318 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0'); 319 if (tokens.size() >= 3) 320 { 321 if (!ConvertValue(&(output->r), tokens[0])) 322 return false; 323 if (!ConvertValue(&(output->g), tokens[1])) 324 return false; 325 if (!ConvertValue(&(output->b), tokens[2])) 326 return false; 327 if (tokens.size() >= 4) 328 { 329 if (!ConvertValue(&(output->a), tokens[3])) 330 return false; 331 } 332 else 333 output->a = 1.0; 334 335 return true; 336 } 337 return false; 338 } 47 339 } 48 49 /**50 @brief Function for reading a Radian from a stream.51 */52 std::istream& operator>>(std::istream& in, orxonox::Radian& radian)53 {54 float temp;55 in >> temp;56 radian = temp;57 return in;58 }59 60 /**61 @brief Function for writing a Degree to a stream.62 */63 std::ostream& operator<<(std::ostream& out, const orxonox::Degree& degree)64 {65 out << degree.valueDegrees();66 return out;67 }68 69 /**70 @brief Function for reading a Degree from a stream.71 */72 std::istream& operator>>(std::istream& in, orxonox::Degree& degree)73 {74 float temp;75 in >> temp;76 degree = temp;77 return in;78 }79 80 81 /**82 @brief Gets the angle between my viewing direction and the direction to the position of the other object.83 @param myposition My position84 @param mydirection My viewing direction85 @param otherposition The position of the other object86 @return The angle87 88 @example89 If the other object is exactly in front of me, the function returns 0.90 If the other object is exactly behind me, the function returns pi.91 If the other object is exactly right/left to me (or above/below), the function returns pi/2.92 */93 float getAngle(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& otherposition)94 {95 orxonox::Vector3 distance = otherposition - myposition;96 float distancelength = distance.length();97 if (distancelength == 0)98 return 0;99 else100 return acos(clamp<float>(mydirection.dotProduct(distance) / distancelength, -1, 1));101 }102 103 /**104 @brief Gets the 2D viewing direction (up/down, left/right) to the position of the other object.105 @param myposition My position106 @param mydirection My viewing direction107 @param myorthonormal My orthonormalvector (pointing upwards through my head)108 @param otherposition The position of the other object109 @return The viewing direction110 111 @example112 If the other object is exactly in front of me, the function returns Vector2(0, 0).113 If the other object is exactly at my left, the function returns Vector2(-1, 0).114 If the other object is exactly at my right, the function returns Vector2(1, 0).115 If the other object is only a bit at my right, the function still returns Vector2(1, 0).116 If the other object is exactly above me, the function returns Vector2(0, 1).117 */118 orxonox::Vector2 get2DViewdirection(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition)119 {120 orxonox::Vector3 distance = otherposition - myposition;121 122 // project difference vector on our plane123 orxonox::Vector3 projection = Ogre::Plane(mydirection, myposition).projectVector(distance);124 125 float projectionlength = projection.length();126 if (projectionlength == 0) return orxonox::Vector2(0, 0);127 float angle = acos(clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1));128 129 if ((mydirection.crossProduct(myorthonormal)).dotProduct(distance) > 0)130 return orxonox::Vector2(sin(angle), cos(angle));131 else132 return orxonox::Vector2(-sin(angle), cos(angle));133 }134 135 /**136 @brief Gets the 2D viewing direction (up/down, left/right) to the position of the other object, multiplied with the viewing distance to the object (0° = 0, 180° = 1).137 @param myposition My position138 @param mydirection My viewing direction139 @param myorthonormal My orthonormalvector (pointing upwards through my head)140 @param otherposition The position of the other object141 @return The viewing direction142 143 @example144 If the other object is exactly in front of me, the function returns Vector2(0, 0).145 If the other object is exactly at my left, the function returns Vector2(-0.5, 0).146 If the other object is exactly at my right, the function returns Vector2(0.5, 0).147 If the other object is only a bit at my right, the function still returns Vector2(0.01, 0).148 If the other object is exactly above me, the function returns Vector2(0, 0.5).149 */150 orxonox::Vector2 get2DViewcoordinates(const orxonox::Vector3& myposition, const orxonox::Vector3& mydirection, const orxonox::Vector3& myorthonormal, const orxonox::Vector3& otherposition)151 {152 orxonox::Vector3 distance = otherposition - myposition;153 154 // project difference vector on our plane155 orxonox::Vector3 projection = Ogre::Plane(mydirection, myposition).projectVector(distance);156 157 float projectionlength = projection.length();158 if (projectionlength == 0) return orxonox::Vector2(0, 0);159 float angle = acos(clamp<float>(myorthonormal.dotProduct(projection) / projectionlength, -1, 1));160 161 float distancelength = distance.length();162 if (distancelength == 0) return orxonox::Vector2(0, 0);163 float radius = acos(clamp<float>(mydirection.dotProduct(distance) / distancelength, -1, 1)) / Ogre::Math::PI;164 165 if ((mydirection.crossProduct(myorthonormal)).dotProduct(distance) > 0)166 return orxonox::Vector2(sin(angle) * radius, cos(angle) * radius);167 else168 return orxonox::Vector2(-sin(angle) * radius, cos(angle) * radius);169 }170 171 /**172 @brief Returns the predicted position I have to aim at, if I want to hit a moving target with a moving projectile.173 @param myposition My position174 @param projectilespeed The speed of my projectile175 @param targetposition The position of my target176 @param targetvelocity The velocity of my target177 @return The predicted position178 179 The function predicts the position based on a linear velocity of the target. If the target changes speed or direction, the projectile will miss.180 */181 orxonox::Vector3 getPredictedPosition(const orxonox::Vector3& myposition, float projectilespeed, const orxonox::Vector3& targetposition, const orxonox::Vector3& targetvelocity)182 {183 float squaredProjectilespeed = projectilespeed * projectilespeed;184 orxonox::Vector3 distance = targetposition - myposition;185 float a = distance.squaredLength();186 float b = 2 * (distance.x + distance.y + distance.z) * (targetvelocity.x + targetvelocity.y + targetvelocity.z);187 float c = targetvelocity.squaredLength();188 189 float temp = 4*squaredProjectilespeed*c + a*a - 4*b*c;190 if (temp < 0)191 return orxonox::Vector3::ZERO;192 193 temp = sqrt(temp);194 float time = (temp + a) / (2 * (squaredProjectilespeed - b));195 return (targetposition + targetvelocity * time);196 }197 198 unsigned long getUniqueNumber()199 {200 static unsigned long aNumber = 135;201 return aNumber++;202 }203 204 205 //////////////////////////206 // Conversion functions //207 //////////////////////////208 209 // std::string to Vector2210 bool ConverterFallback<std::string, orxonox::Vector2>::convert(orxonox::Vector2* output, const std::string& input)211 {212 size_t opening_parenthesis, closing_parenthesis = input.find(')');213 if ((opening_parenthesis = input.find('(')) == std::string::npos)214 opening_parenthesis = 0;215 else216 opening_parenthesis++;217 218 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),219 ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');220 if (tokens.size() >= 2)221 {222 if (!ConvertValue(&(output->x), tokens[0]))223 return false;224 if (!ConvertValue(&(output->y), tokens[1]))225 return false;226 227 return true;228 }229 return false;230 }231 232 // std::string to Vector3233 bool ConverterFallback<std::string, orxonox::Vector3>::convert(orxonox::Vector3* output, const std::string& input)234 {235 size_t opening_parenthesis, closing_parenthesis = input.find(')');236 if ((opening_parenthesis = input.find('(')) == std::string::npos)237 opening_parenthesis = 0;238 else239 opening_parenthesis++;240 241 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),242 ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');243 if (tokens.size() >= 3)244 {245 if (!ConvertValue(&(output->x), tokens[0]))246 return false;247 if (!ConvertValue(&(output->y), tokens[1]))248 return false;249 if (!ConvertValue(&(output->z), tokens[2]))250 return false;251 252 return true;253 }254 return false;255 }256 257 // std::string to Vector4258 bool ConverterFallback<std::string, orxonox::Vector4>::convert(orxonox::Vector4* output, const std::string& input)259 {260 size_t opening_parenthesis, closing_parenthesis = input.find(')');261 if ((opening_parenthesis = input.find('(')) == std::string::npos)262 opening_parenthesis = 0;263 else264 opening_parenthesis++;265 266 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis),267 ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');268 if (tokens.size() >= 4)269 {270 if (!ConvertValue(&(output->x), tokens[0]))271 return false;272 if (!ConvertValue(&(output->y), tokens[1]))273 return false;274 if (!ConvertValue(&(output->z), tokens[2]))275 return false;276 if (!ConvertValue(&(output->w), tokens[3]))277 return false;278 279 return true;280 }281 return false;282 }283 284 // std::string to Quaternion285 bool ConverterFallback<std::string, orxonox::Quaternion>::convert(orxonox::Quaternion* output, const std::string& input)286 {287 size_t opening_parenthesis, closing_parenthesis = input.find(')');288 if ((opening_parenthesis = input.find('(')) == std::string::npos) { opening_parenthesis = 0; } else { opening_parenthesis++; }289 290 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');291 if (tokens.size() >= 4)292 {293 if (!ConvertValue(&(output->w), tokens[0]))294 return false;295 if (!ConvertValue(&(output->x), tokens[1]))296 return false;297 if (!ConvertValue(&(output->y), tokens[2]))298 return false;299 if (!ConvertValue(&(output->z), tokens[3]))300 return false;301 302 return true;303 }304 return false;305 }306 307 // std::string to ColourValue308 bool ConverterFallback<std::string, orxonox::ColourValue>::convert(orxonox::ColourValue* output, const std::string& input)309 {310 size_t opening_parenthesis, closing_parenthesis = input.find(')');311 if ((opening_parenthesis = input.find('(')) == std::string::npos) { opening_parenthesis = 0; } else { opening_parenthesis++; }312 313 SubString tokens(input.substr(opening_parenthesis, closing_parenthesis - opening_parenthesis), ",", SubString::WhiteSpaces, false, '\\', true, '"', true, '\0', '\0', true, '\0');314 if (tokens.size() >= 3)315 {316 if (!ConvertValue(&(output->r), tokens[0]))317 return false;318 if (!ConvertValue(&(output->g), tokens[1]))319 return false;320 if (!ConvertValue(&(output->b), tokens[2]))321 return false;322 if (tokens.size() >= 4)323 {324 if (!ConvertValue(&(output->a), tokens[3]))325 return false;326 }327 else328 output->a = 1.0;329 330 return true;331 }332 return false;333 }
Note: See TracChangeset
for help on using the changeset viewer.