00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "object/path.hpp"
00020
00021 #include <sstream>
00022 #include <stdexcept>
00023
00024 #include "lisp/list_iterator.hpp"
00025 #include "util/log.hpp"
00026
00027 Path::Path() :
00028 nodes(),
00029 mode()
00030 {
00031 }
00032
00033 Path::~Path()
00034 {
00035 }
00036
00037 void
00038 Path::read(const Reader& reader)
00039 {
00040 lisp::ListIterator iter(&reader);
00041
00042 mode = CIRCULAR;
00043 while(iter.next()) {
00044 if(iter.item() == "mode") {
00045 std::string mode_string;
00046 if(!iter.value()->get(mode_string))
00047 throw std::runtime_error("Pathmode not a string");
00048
00049 if(mode_string == "oneshot")
00050 mode = ONE_SHOT;
00051 else if(mode_string == "pingpong")
00052 mode = PING_PONG;
00053 else if(mode_string == "circular")
00054 mode = CIRCULAR;
00055 else {
00056 std::ostringstream msg;
00057 msg << "Unknown pathmode '" << mode_string << "' found";
00058 throw std::runtime_error(msg.str());
00059 }
00060 continue;
00061 }
00062
00063 if(iter.item() != "node") {
00064 log_warning << "unknown token '" << iter.item() << "' in Path nodes list. Ignored." << std::endl;
00065 continue;
00066 }
00067 const lisp::Lisp* node_lisp = iter.lisp();
00068
00069
00070 Node node;
00071 node.time = 1;
00072 if( (!node_lisp->get("x", node.position.x) ||
00073 !node_lisp->get("y", node.position.y)))
00074 throw std::runtime_error("Path node without x and y coordinate specified");
00075 node_lisp->get("time", node.time);
00076
00077 if(node.time <= 0)
00078 throw std::runtime_error("Path node with non-positive time");
00079
00080 nodes.push_back(node);
00081 }
00082
00083 if (nodes.empty())
00084 throw std::runtime_error("Path with zero nodes");
00085 }
00086
00087 Vector
00088 Path::get_base() const
00089 {
00090 if(nodes.empty())
00091 return Vector(0, 0);
00092
00093 return nodes[0].position;
00094 }
00095
00096 int
00097 Path::get_nearest_node_no(Vector reference_point) const
00098 {
00099 int nearest_node_id = -1;
00100 float nearest_node_dist = 0;
00101 int id = 0;
00102 for (std::vector<Node>::const_iterator i = nodes.begin(); i != nodes.end(); i++, id++) {
00103 float dist = (i->position - reference_point).norm();
00104 if ((nearest_node_id == -1) || (dist < nearest_node_dist)) {
00105 nearest_node_id = id;
00106 nearest_node_dist = dist;
00107 }
00108 }
00109 return nearest_node_id;
00110 }
00111
00112 int
00113 Path::get_farthest_node_no(Vector reference_point) const
00114 {
00115 int farthest_node_id = -1;
00116 float farthest_node_dist = 0;
00117 int id = 0;
00118 for (std::vector<Node>::const_iterator i = nodes.begin(); i != nodes.end(); i++, id++) {
00119 float dist = (i->position - reference_point).norm();
00120 if ((farthest_node_id == -1) || (dist > farthest_node_dist)) {
00121 farthest_node_id = id;
00122 farthest_node_dist = dist;
00123 }
00124 }
00125 return farthest_node_id;
00126 }
00127
00128