diff --git a/include/opendrive_arc.h b/include/opendrive_arc.h index 94ee22b58c6474bf8f3e43cb61e3101375ab06e6..644278e6e8a7dad30fa3fec3e28636f8d2ed97fd 100644 --- a/include/opendrive_arc.h +++ b/include/opendrive_arc.h @@ -5,19 +5,19 @@ class COpendriveArc : public COpendriveGeometry { + friend class COpendriveRoadNode; private: double curvature; protected: + COpendriveArc(); + COpendriveArc(const COpendriveArc &object); virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const; virtual void print(std::ostream &out); virtual void load_params(const planView::geometry_type &geometry_info); virtual std::string get_name(void); public: - COpendriveArc(); - COpendriveArc(double min_s, double max_s, double x, double y, double heading, double curvature); - COpendriveArc(const COpendriveArc &object); virtual COpendriveGeometry *clone(void); - double get_curvature(void); + virtual void get_curvature(double &start,double &end); void operator=(const COpendriveArc &object); ~COpendriveArc(); }; diff --git a/src/opendrive_arc.cpp b/src/opendrive_arc.cpp index c08a1e6297d1237facd36127c05741b79481ae12..2610e3483c6a689dec4119d6131cb00fb1c45896 100644 --- a/src/opendrive_arc.cpp +++ b/src/opendrive_arc.cpp @@ -6,11 +6,6 @@ COpendriveArc::COpendriveArc() this->curvature=0.0; } -COpendriveArc::COpendriveArc(double min_s, double max_s, double x, double y, double heading, double curvature) : COpendriveGeometry(min_s,max_s,x,y,heading) -{ - this->curvature=curvature; -} - COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(object) { this->curvature=object.curvature; @@ -19,21 +14,21 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o bool COpendriveArc::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const { double alpha; - bool pos_arc; - - alpha = std::fabs((track.s-this->min_s/this->scale_factor)*this->curvature*this->scale_factor); - pos_arc = (this->curvature < 0.0 ? false : true); - local.u = std::sin(alpha)/(this->curvature*this->scale_factor) - track.t*std::sin(alpha)*(pos_arc ? 1 : -1); - local.v = (1 - std::cos(alpha))*(pos_arc ? 1 : -1)/(this->curvature*this->scale_factor) + track.t*std::cos(alpha); - local.heading = normalize_angle(track.heading + alpha*(pos_arc ? 1 : -1)); + alpha = track.s*this->curvature*this->scale_factor; + local.u = std::sin(alpha)/(this->curvature*this->scale_factor) - track.t*std::sin(alpha); + local.v = (1 - std::cos(alpha))/(this->curvature*this->scale_factor) + track.t*std::cos(alpha); + local.heading = normalize_angle(track.heading + alpha); return true; } void COpendriveArc::print(std::ostream &out) { + double start_curvature,end_curvature; + COpendriveGeometry::print(out); - out << " curvature = " << this->get_curvature() << std::endl; + this->get_curvature(start_curvature,end_curvature); + out << " curvature = " << start_curvature << std::endl; } std::string COpendriveArc::get_name(void) @@ -53,9 +48,10 @@ COpendriveGeometry *COpendriveArc::clone(void) return new_arc; } -double COpendriveArc::get_curvature(void) +void COpendriveArc::get_curvature(double &start,double &end) { - return this->curvature*this->scale_factor; + start=this->curvature*this->scale_factor; + end=this->curvature*this->scale_factor; } void COpendriveArc::operator=(const COpendriveArc &object)