From e891ce3f0923eae7cfe39114aeed7d8659913586 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Wed, 5 Apr 2023 16:51:35 +0200
Subject: [PATCH] Added length limits checks in evaluate(length) and get_part
functions. Added bool return to check success
---
include/g2_spline.h | 4 ++--
src/g2_spline.cpp | 25 ++++++++++++++++++++-----
src/opendrive_link.cpp | 14 ++++++++------
src/opendrive_road_segment.cpp | 14 ++++++++------
4 files changed, 38 insertions(+), 19 deletions(-)
diff --git a/include/g2_spline.h b/include/g2_spline.h
index 7d9a103..c2faa6c 100644
--- a/include/g2_spline.h
+++ b/include/g2_spline.h
@@ -63,14 +63,14 @@ class CG2Spline
void generate(double resolution,unsigned int iterations=10);
void generate(double resolution,double length,unsigned int iterations=10);
void generate(double &resolution,double n1,double n2, double n3, double n4);
- TPoint evaluate(double length);
+ bool evaluate(double length, TPoint& point);
void evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading);
double get_length(void);
double get_max_curvature(double max_error=0.001,unsigned int max_iter=10);
double get_max_curvature_der(double max_error=0.001,unsigned int max_iter=10);
double find_closest_point(double x,double y,TPoint &point,double max_error=0.001,unsigned int max_iter=10);
double find_closest_point(double x,double y,double max_error=0.001,unsigned int max_iter=10);
- void get_part(CG2Spline *spline,double start_length, double end_length=-1.0);
+ bool get_part(CG2Spline *spline,double start_length, double end_length=-1.0);
CG2Spline& operator=(const CG2Spline &obj);
~CG2Spline();
};
diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index 292db9b..afd0df2 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -863,9 +863,12 @@ void CG2Spline::generate(double &resolution,double n1,double n2, double n3, doub
this->generated=true;
}
-TPoint CG2Spline::evaluate(double length)
+bool CG2Spline::evaluate(double length, TPoint& point)
{
- return this->evaluate_parameter(this->find_parameter(length));
+ if (length < 0.0 || length > this->length[this->num_points-1])
+ return false;
+ point = this->evaluate_parameter(this->find_parameter(length));
+ return true;
}
void CG2Spline::evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading)
@@ -1014,17 +1017,29 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne
}
}
-void CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length)
+bool CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length)
{
+ TPoint p;
+
if(start_length==0.0)
spline->set_start_point(this->start);
else
- spline->set_start_point(this->evaluate(start_length));
+ {
+ if (!this->evaluate(start_length, p))
+ return false;
+ spline->set_start_point(p);
+ }
+
if(end_length==-1.0)
spline->set_end_point(this->end);
else
- spline->set_end_point(this->evaluate(end_length));
+ {
+ if (!this->evaluate(end_length, p))
+ return false;
+ spline->set_end_point(p);
+ }
spline->generate(this->resolution);
+ return true;
}
double CG2Spline::curvature(double u)
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 7678a8b..4c5cdcd 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -286,10 +286,12 @@ TOpendriveWorldPose COpendriveLink::get_pose_at(double length)
if(this->spline!=NULL)
{
- spline_pose=this->spline->evaluate(length);
- world_pose.x=spline_pose.x;
- world_pose.y=spline_pose.y;
- world_pose.heading=spline_pose.heading;
+ if (this->spline->evaluate(length, spline_pose))
+ {
+ world_pose.x=spline_pose.x;
+ world_pose.y=spline_pose.y;
+ world_pose.heading=spline_pose.heading;
+ }
}
return world_pose;
@@ -302,8 +304,8 @@ double COpendriveLink::get_curvature_at(double length)
if(this->spline!=NULL)
{
- spline_pose=this->spline->evaluate(length);
- curvature=spline_pose.curvature;
+ if (this->spline->evaluate(length, spline_pose))
+ curvature=spline_pose.curvature;
}
return curvature;
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index bf8f465..8bd5108 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -1411,10 +1411,12 @@ TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length,int road_si
{
if((int_length-this->geometries[i].opendrive->get_length())<=this->resolution)
{
- spline_pose=this->geometries[i].spline->evaluate(int_length);
- world_pose.x=spline_pose.x;
- world_pose.y=spline_pose.y;
- world_pose.heading=spline_pose.heading;
+ if (this->geometries[i].spline->evaluate(int_length, spline_pose))
+ {
+ world_pose.x=spline_pose.x;
+ world_pose.y=spline_pose.y;
+ world_pose.heading=spline_pose.heading;
+ }
return world_pose;
}
else
@@ -1440,8 +1442,8 @@ double COpendriveRoadSegment::get_curvature_at(double length,int road_side) cons
{
if((int_length-this->geometries[i].opendrive->get_length())<=this->resolution)
{
- spline_pose=this->geometries[i].spline->evaluate(int_length);
- curvature=spline_pose.curvature;
+ if (this->geometries[i].spline->evaluate(int_length, spline_pose))
+ curvature=spline_pose.curvature;
return curvature;
}
else
--
GitLab