Added a constructor to generate a ParamPoly3 geometry from the start and end points.

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);

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);

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)