pageldev / libOpenDRIVE

Small, lightweight C++ library for handling OpenDRIVE files
Apache License 2.0
401 stars 141 forks source link

ParamPoly3 :need help #7

Closed qqkkwan closed 3 years ago

qqkkwan commented 3 years ago

First ,It's a great project. I am doing something like converting some data to opendrive files,but i met some problems by converting to ParamPoly3 geometry . I think maybe I will be inspired by this project,but i really don't understand how to build ParamPoly3 after i read the code. Could you please give me some explanations ? Thanks a lot~

pageldev commented 3 years ago

Thanks! I appreciate it. So the ParamPoly3 is actually a Cubic Bézier curve, which is a more common representation. For the Cubic Bézier curve there are four control points determining how the curve looks like. There are examples like this online where you can play around with the control points.

In ParamPoly3.cpp I'm actually getting the control points from the ParamPoly3 parameters (aU, bU,...). To get the control points for a ParamPoly3 the equations are (implemented in CubicBezier.hpp in a more generic way):

p0 = (aU | aV)
p1 = (bu/3 + p0x | bV/3 + p0y)
p2 = (cU/3 + 2*p1x - p0x  | cV/3 + 2*p1y - p0y)
p3 = (dU + 3*p2x - 3*p1x + p0x | dV + 3*p2y - 3*p1y + p0y)

The inverse of that (from control points to ParamPoly3 coefficients) is implemented here.

So you could fit a Cubic Bézier curve through your data, get the coefficients of it and write that to the .xodr file.

qqkkwan commented 3 years ago

good ! thanks a lot!

YChen-1998 commented 3 years ago

Thanks! I appreciate it.I want to know how to calculate the equation of finding ParamPoly3 parameters at known control points,Thanks a lot~

pageldev commented 3 years ago

For converting the control points to ParamPoly3 parameters I used this equation. It's implemented here

image

Resolving that equation for a cubic Bezier should result in:

C0 = P0
C1 = 3*P1 - 3P0
C2 = 3*P2 - 6*P1 + 3P0
C3 = P3 - 3*P2 + 3*P1 - P=