Mercurial Hosting > traffic-intelligence

annotate trajectorymanagement/src/TrajectoryElement.h @ 1219:8a626226793e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
update where optimization uses either nomad-parameter file depending on optimizing 1 or 2 steps
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 19 Jun 2023 17:09:56 -0400
parents e1e7acef8eab
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
1159
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
1 #ifndef TRAJECTORYELEMENT_H_
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
2 #define TRAJECTORYELEMENT_H_
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
3
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
4 #include <istream>
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
5 #include <ostream>
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
6
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
7 #include "PointOperations.h"
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
8
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
9 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
10 * TrajectoryElement class.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
11 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
12 * The Trajectory class is composed of components called TrajectoryElement.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
13 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
14 template<typename T>
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
15 class TrajectoryElement {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
16 public:
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
17 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
18 * Constructor.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
19 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
20 TrajectoryElement() {}
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
21
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
22 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
23 * Constructor.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
24 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
25 * @param[in] frameNumber number of a frame
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
26 * @param[in] position position
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
27 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
28 TrajectoryElement(unsigned int frameNumber, const T& point) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
29 setFrameNumber(frameNumber);
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
30 setPoint(point);
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
31 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
32
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
33 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
34 * Constructor.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
35 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
36 * @param[in] trajectoryElement element of the trajectory
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
37 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
38 TrajectoryElement(const TrajectoryElement& trajectoryElement) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
39 setFrameNumber(trajectoryElement.getFrameNumber());
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
40 setPoint(trajectoryElement.getPoint());
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
41 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
42
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
43 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
44 * Get number of a frame.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
45 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
46 * @return number of a frame
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
47 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
48 unsigned int getFrameNumber() const {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
49 return frameNumber;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
50 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
51
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
52 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
53 * Set number of a frame.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
54 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
55 * @param[in] frameNumber number of a frame
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
56 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
57 void setFrameNumber(unsigned int iframeNumber) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
58 frameNumber = iframeNumber;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
59 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
60
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
61 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
62 * Get position.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
63 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
64 * @return position
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
65 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
66 const T &getPoint() const {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
67 return point;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
68 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
69
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
70 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
71 * Set position.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
72 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
73 * @param[in] position position
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
74 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
75 void setPoint(const T& ipoint) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
76 point = ipoint;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
77 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
78
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
79 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
80 * Shift position.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
81 *
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
82 * @param[in] t vector by which the position of the element should be shifted.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
83 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
84 void shift(const T& t) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
85 point += t;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
86 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
87
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
88 private:
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
89 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
90 * Frame number.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
91 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
92 unsigned int frameNumber;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
93
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
94 /**
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
95 * Position of the element.
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
96 */
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
97 T point;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
98 };
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
99
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
100 template<typename T>
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
101 static inline bool operator==(const TrajectoryElement<T>& a, const TrajectoryElement<T>& b) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
102 return (a.getFrameNumber() == b.getFrameNumber()) && (a.getPoint() == b.getPoint());
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
103 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
104
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
105 template<typename Ti, typename Tr>
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
106 std::basic_istream<Ti>& operator>>(std::basic_istream<Ti>& in, TrajectoryElement<Tr>& trajectoryElement) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
107 unsigned int frameNumber;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
108 Tr point;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
109 if (in >> frameNumber >> point) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
110 trajectoryElement.setFrameNumber(frameNumber);
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
111 trajectoryElement.setPoint(point);
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
112 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
113 return in;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
114 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
115
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
116 template<typename Tr>
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
117 std::ostream& operator<<(std::ostream& out, const TrajectoryElement<Tr>& trajectoryElement) {
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
118 out << trajectoryElement.getFrameNumber() << " " << trajectoryElement.getPoint();
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
119 return out;
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
120 }
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
121
e1e7acef8eab moved trajectory management library into Traffic Intelligence
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
122 #endif /* TRAJECTORYELEMENT_H_ */