diff --git a/include/opendrive/opendrive_geometry.h b/include/opendrive/opendrive_geometry.h
index 6989dfc708da38fd7e3705405e498702cc4f3321..50baf13b881b196650c291b0368da3ea77752a6c 100644
--- a/include/opendrive/opendrive_geometry.h
+++ b/include/opendrive/opendrive_geometry.h
@@ -27,7 +27,7 @@ class COpendriveGeometry
virtual void save_params(planView::geometry_type &geometry_info) = 0;
virtual std::string get_name(void)=0;
void set_start_pose(TOpendriveWorldPose &pose);
- virtual void set_max_s(double s);
+ void set_max_s(double s);
void set_min_s(double s);
public:
COpendriveGeometry(TOpendriveWorldPose &start_pose,double length);
diff --git a/include/opendrive/opendrive_param_poly3.h b/include/opendrive/opendrive_param_poly3.h
index 462dbb0aef9009cb3edd659e56b0cb7eb8a244b7..53fa3aa0525de9bf5ebaadc81d620991c360c703 100644
--- a/include/opendrive/opendrive_param_poly3.h
+++ b/include/opendrive/opendrive_param_poly3.h
@@ -27,10 +27,9 @@ class COpendriveParamPoly3 : public COpendriveGeometry
virtual void load_params(const planView::geometry_type &geometry_info);
virtual void save_params(planView::geometry_type &geometry_info);
virtual std::string get_name(void);
- void set_start_pose(TOpendriveWorldPose &pose,double s);
- virtual void set_max_s(double s);
public:
COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendrivePoly3Params &u_params,TOpendrivePoly3Params &v_params);
+ COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose);
virtual COpendriveGeometry *clone(void);
virtual void get_curvature(double &start,double &end);
virtual void get_curvature_at(double length,double &curvature);
diff --git a/src/opendrive/opendrive_param_poly3.cpp b/src/opendrive/opendrive_param_poly3.cpp
index 9f570bce2c5f7f7f625fc6bd01dc759f51dc167b..bbb3354bb1a7423782396212724aaaf09ec430a7 100644
--- a/src/opendrive/opendrive_param_poly3.cpp
+++ b/src/opendrive/opendrive_param_poly3.cpp
@@ -30,51 +30,19 @@ COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) :
this->normalized=object.normalized;
}
-bool COpendriveParamPoly3::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
-{
- double p = (this->normalized ? track.s/(this->max_s - this->min_s):track.s);
- double p2 = p*p;
- double p3 = p2*p;
- double du = this->u.b + 2*this->u.c*p + 3*this->u.d*p2;
- double dv = this->v.b + 2*this->v.c*p + 3*this->v.d*p2;
- double alpha;
- if(p==0.0)
- alpha = 0.0;
- else
- alpha = std::atan2(dv,du);
- local.u = this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3 - track.t*std::sin(alpha);
- local.v = this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3 + track.t*std::cos(alpha);
- local.heading = normalize_angle(track.heading + alpha);
-
- return true;
-}
-
-void COpendriveParamPoly3::print(std::ostream &out)
-{
- COpendriveGeometry::print(out);
- std::cout << " U params: a = " << this->u.a << ", b = " << this->u.b << ", c = " << this->u.c << ", d = " << this->u.d << std::endl;
- std::cout << " V params: a = " << this->v.a << ", b = " << this->v.b << ", c = " << this->v.c << ", d = " << this->v.d << std::endl;
- if(this->normalized)
- std::cout << " Normalized" << std::endl;
- else
- std::cout << " Not normalized" << std::endl;
-}
-
-std::string COpendriveParamPoly3::get_name(void)
-{
- return std::string("Parametric polynomial 3th degree");
-}
-
-void COpendriveParamPoly3::set_start_pose(TOpendriveWorldPose &pose,double s)
+COpendriveParamPoly3::COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose)
{
+ TOpendriveWorldPose end_pose_zero,end_pose_rot;
Eigen::MatrixXd A=Eigen::MatrixXd::Zero(6,8),pinv;
- Eigen::VectorXd b=Eigen::VectorXd::Zero(6,1),sol;
- TOpendriveWorldPose end_pose,new_end_pose;
+ Eigen::VectorXd b=Eigen::VectorXd::Zero(6,1),sol;
+ double length=0.0,x,prev_x,y,prev_y,p,p2,p3;
- end_pose=this->get_end_pose();
- new_end_pose.heading=end_pose.heading-pose.heading;
- new_end_pose.x=(end_pose.x-pose.x)*std::cos(pose.heading)+(end_pose.y-pose.y)*std::sin(pose.heading);
- new_end_pose.y=-(end_pose.x-pose.x)*std::sin(pose.heading)+(end_pose.y-pose.y)*std::cos(pose.heading);
+ end_pose_zero.x=end_pose.x-start_pose.x;
+ end_pose_zero.y=end_pose.y-start_pose.y;
+ end_pose_zero.heading=end_pose.heading;
+ end_pose_rot.x=end_pose_zero.x*std::cos(start_pose.heading)+end_pose_zero.y*std::sin(start_pose.heading);
+ end_pose_rot.y=-end_pose_zero.x*std::sin(start_pose.heading)+end_pose_zero.y*std::cos(start_pose.heading);
+ end_pose_rot.heading=end_pose.heading-start_pose.heading;
A(0,0)=1.0;
A(1,4)=1.0;
A(2,0)=1.0;
@@ -87,19 +55,18 @@ void COpendriveParamPoly3::set_start_pose(TOpendriveWorldPose &pose,double s)
A(3,7)=1.0;
A(4,1)=-tan(0.0);
A(4,5)=1.0;
- A(5,1)=-tan(new_end_pose.heading);
- A(5,2)=-2.0*tan(new_end_pose.heading);
- A(5,3)=-3.0*tan(new_end_pose.heading);
+ A(5,1)=-tan(end_pose_rot.heading);
+ A(5,2)=-2.0*tan(end_pose_rot.heading);
+ A(5,3)=-3.0*tan(end_pose_rot.heading);
A(5,5)=1.0;
A(5,6)=2.0;
A(5,7)=3.0;
pinv=A.completeOrthogonalDecomposition().pseudoInverse();
b(0)=0.0;
b(1)=0.0;
- b(2)=new_end_pose.x;
- b(3)=new_end_pose.y;
+ b(2)=end_pose_rot.x;
+ b(3)=end_pose_rot.y;
sol=pinv*b;
-
this->u.a=sol(0);
this->u.b=sol(1);
this->u.c=sol(2);
@@ -109,57 +76,59 @@ void COpendriveParamPoly3::set_start_pose(TOpendriveWorldPose &pose,double s)
this->v.c=sol(6);
this->v.d=sol(7);
- COpendriveGeometry::set_start_pose(pose);
- COpendriveGeometry::set_max_s(s);
+ this->normalized=true;
+ // compute start pose
+ COpendriveGeometry::set_start_pose(start_pose);
+ // compute length
+ prev_x=this->u.a;
+ prev_y=this->v.a;
+ for(unsigned int i=1;i<=100;i++)
+ {
+ p=i*0.01;
+ p2=p*p;
+ p3=p2*p;
+ x=this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3;
+ y=this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3;
+ length+=sqrt(pow(x-prev_x,2.0)+pow(y-prev_y,2.0));
+ prev_x=x;
+ prev_y=y;
+ }
+ this->set_max_s(length);
}
-void COpendriveParamPoly3::set_max_s(double s)
+bool COpendriveParamPoly3::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
- Eigen::MatrixXd A=Eigen::MatrixXd::Zero(6,8),pinv;
- Eigen::VectorXd b=Eigen::VectorXd::Zero(6,1),sol;
- TOpendriveWorldPose end_pose,new_end_pose;
+ double p = (this->normalized ? track.s/(this->max_s - this->min_s):track.s);
+ double p2 = p*p;
+ double p3 = p2*p;
+ double du = this->u.b + 2*this->u.c*p + 3*this->u.d*p2;
+ double dv = this->v.b + 2*this->v.c*p + 3*this->v.d*p2;
+ double alpha;
+ if(p==0.0)
+ alpha = 0.0;
+ else
+ alpha = std::atan2(dv,du);
+ local.u = this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3 - track.t*std::sin(alpha);
+ local.v = this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3 + track.t*std::cos(alpha);
+ local.heading = normalize_angle(track.heading + alpha);
- if(this->get_pose_at(s,end_pose))
- {
- new_end_pose.heading=end_pose.heading-this->pose.heading;
- new_end_pose.x=(end_pose.x-this->pose.x)*std::cos(this->pose.heading)+(end_pose.y-this->pose.y)*std::sin(this->pose.heading);
- new_end_pose.y=-(end_pose.x-this->pose.x)*std::sin(this->pose.heading)+(end_pose.y-this->pose.y)*std::cos(this->pose.heading);
- A(0,0)=1.0;
- A(1,4)=1.0;
- A(2,0)=1.0;
- A(2,1)=1.0;
- A(2,2)=1.0;
- A(2,3)=1.0;
- A(3,4)=1.0;
- A(3,5)=1.0;
- A(3,6)=1.0;
- A(3,7)=1.0;
- A(4,1)=-tan(0.0);
- A(4,5)=1.0;
- A(5,1)=-tan(new_end_pose.heading);
- A(5,2)=-2.0*tan(new_end_pose.heading);
- A(5,3)=-3.0*tan(new_end_pose.heading);
- A(5,5)=1.0;
- A(5,6)=2.0;
- A(5,7)=3.0;
- pinv=A.completeOrthogonalDecomposition().pseudoInverse();
- b(0)=0.0;
- b(1)=0.0;
- b(2)=new_end_pose.x;
- b(3)=new_end_pose.y;
- sol=pinv*b;
+ return true;
+}
- this->u.a=sol(0);
- this->u.b=sol(1);
- this->u.c=sol(2);
- this->u.d=sol(3);
- this->v.a=sol(4);
- this->v.b=sol(5);
- this->v.c=sol(6);
- this->v.d=sol(7);
- }
+void COpendriveParamPoly3::print(std::ostream &out)
+{
+ COpendriveGeometry::print(out);
+ std::cout << " U params: a = " << this->u.a << ", b = " << this->u.b << ", c = " << this->u.c << ", d = " << this->u.d << std::endl;
+ std::cout << " V params: a = " << this->v.a << ", b = " << this->v.b << ", c = " << this->v.c << ", d = " << this->v.d << std::endl;
+ if(this->normalized)
+ std::cout << " Normalized" << std::endl;
+ else
+ std::cout << " Not normalized" << std::endl;
+}
- COpendriveGeometry::set_max_s(s);
+std::string COpendriveParamPoly3::get_name(void)
+{
+ return std::string("Parametric polynomial 3th degree");
}
void COpendriveParamPoly3::load_params(const planView::geometry_type &geometry_info)