/* Ċ Ġ* ĠCopyright Ġ 2 0 1 9 - 2 0 2 2 ĠCh air Ġof ĠGe oin form atics , ĠTechnical ĠUniversity Ġof ĠM un ich Ċ Ġ* Ċ Ġ* ĠLicensed Ġunder Ġthe ĠApache ĠLicense , ĠVersion Ġ 2 . 0 Ġ( the Ġ" License "); Ċ Ġ* Ġyou Ġmay Ġnot Ġuse Ġthis Ġfile Ġexcept Ġin Ġcompliance Ġwith Ġthe ĠLicense . Ċ Ġ* ĠYou Ġmay Ġobtain Ġa Ġcopy Ġof Ġthe ĠLicense Ġat Ċ Ġ* Ċ Ġ* ĠĠĠĠ Ġhttp :// www . apache . org / licenses / LICENSE - 2 . 0 Ċ Ġ* Ċ Ġ* ĠUnless Ġrequired Ġby Ġapplicable Ġlaw Ġor Ġagreed Ġto Ġin Ġwriting , Ġsoftware Ċ Ġ* Ġdistributed Ġunder Ġthe ĠLicense Ġis Ġdistributed Ġon Ġan Ġ" AS ĠIS " ĠBASIS , Ċ Ġ* ĠWITHOUT ĠWARRANTIES ĠOR ĠCONDITIONS ĠOF ĠANY ĠKIND , Ġeither Ġexpress Ġor Ġimplied . Ċ Ġ* ĠSee Ġthe ĠLicense Ġfor Ġthe Ġspecific Ġlanguage Ġgoverning Ġpermissions Ġand Ċ Ġ* Ġlimitations Ġunder Ġthe ĠLicense . Ċ Ġ*/ Ċ Ċ package Ġio . r tron . transformer . opend rive 2 road spaces . geometry Ċ Ċ import Ġcom . github . kit tin un f . result . Result Ċ import Ġio . r tron . io . logging . Logger Ċ import Ġio . r tron . math . analysis . function . un ivariate . combin ation . Stack ed Function Ċ import Ġio . r tron . math . geometry . e uclidean . th reed . curve . Curve 3 D Ċ import Ġio . r tron . math . geometry . e uclidean . th reed . point . Vector 3 D Ċ import Ġio . r tron . math . geometry . e uclidean . th reed . surface . Circle 3 D Ċ import Ġio . r tron . math . geometry . e uclidean . th reed . surface . Linear Ring 3 D Ċ import Ġio . r tron . math . geometry . e uclidean . th reed . surface . Parame tric Bounded Surface 3 D Ċ import Ġio . r tron . math . geometry . e uclidean . th reed . surface . Rectangle 3 D Ċ import Ġio . r tron . math . processing . Linear Ring 3 D Factory Ċ import Ġio . r tron . math . transform . Affine 3 D Ċ import Ġio . r tron . math . transform . Affine Sequence 3 D Ċ import Ġio . r tron . model . opend rive . road . objects . Road Objects Object Ċ import Ġio . r tron . model . opend rive . road . objects . Road Objects Object Out lines Outline Ċ import Ġio . r tron . model . opend rive . road . objects . Road Objects Object Out lines Outline Corner Road Ċ import Ġio . r tron . model . opend rive . road . objects . Road Objects Object Repeat Ċ import Ġio . r tron . model . road spaces . road space . objects . Road space Object Identifier Ċ import Ġio . r tron . std . Context Message Ċ import Ġio . r tron . std . handle And Remove Failure Ċ import Ġio . r tron . std . handle Failure Ċ import Ġio . r tron . std . handle Message Ċ import Ġio . r tron . transformer . opend rive 2 road spaces . analysis . Function Builder Ċ import Ġio . r tron . transformer . opend rive 2 road spaces . configuration . O pend rive 2 Road spaces Configuration Ċ Ċ /** Ċ Ġ* ĠBuilder Ġfor Ġsurface Ġge ometries Ġin Ġ 3 D Ġfrom Ġthe ĠOpen DRIVE Ġdata Ġmodel . Ċ Ġ*/ Ċ class ĠSurface 3 D Builder ( ĊĠĠĠ Ġprivate Ġval Ġreport Logger : ĠLogger , ĊĠĠĠ Ġprivate Ġval Ġconfiguration : ĠO pend rive 2 Road spaces Configuration Ċ ) Ġ{ ĊĊĠĠĠ Ġ// ĠProperties Ġand ĠInitial izers ĊĠĠĠ Ġprivate Ġval Ġ_ function Builder Ġ= ĠFunction Builder ( report Logger , Ġconfiguration ) ĊĠĠĠ Ġprivate Ġval Ġ_ curve 2 D Builder Ġ= ĠCurve 2 D Builder ( report Logger , Ġconfiguration ) ĊĊĠĠĠ Ġ// ĠMethods ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġlist Ġof Ġrect angles Ġfrom Ġthe ĠOpen DRIVE Ġroad Ġobject Ġclass Ġ([ Road Objects Object ]) Ġdirectly Ġor Ġfrom Ġthe ĊĠĠĠĠ Ġ* Ġrepeated Ġentries Ġdefined Ġin Ġ[ Road Objects Object Repeat ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġbuild Rect angles ( road Object : ĠRoad Objects Object , Ġcurve Affine : ĠAff ine 3 D ): ĠList < Rectangle 3 D > Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġrectangle List Ġ= Ġmutable ListOf < Rectangle 3 D >() ĊĊĠĠĠĠĠĠĠ Ġif Ġ( road Object . is Rectangle ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġobject Affine Ġ= ĠAff ine 3 D . of ( road Object . reference Line Point Relative Pose ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġaffine Sequence Ġ= ĠAff ine Sequence 3 D . of ( curve Affine , Ġobject Affine ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġrectangle List Ġ+= ĠRectangle 3 D ( road Object . length , Ġroad Object . width , Ġconfiguration . tolerance , Ġaffine Sequence ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġif Ġ( road Object . repeat . is Repeated Cub oid ()) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġthis . report Logger . info Once (" Geometry ĠRe peated Rectangle Ġnot Ġimplemented Ġyet .") ĊĊĠĠĠĠĠĠĠ Ġreturn Ġrectangle List ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġlist Ġof Ġcircles Ġfrom Ġthe ĠOpen DRIVE Ġroad Ġobject Ġclass Ġ([ Road Objects Object ]) Ġdirectly Ġor Ġfrom Ġthe ĊĠĠĠĠ Ġ* Ġrepeated Ġentries Ġdefined Ġin Ġ[ Road Objects Object Repeat ]. ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġbuild Circ les ( road Object : ĠRoad Objects Object , Ġcurve Affine : ĠAff ine 3 D ): ĠList < Circle 3 D > Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġcircle List Ġ= Ġmutable ListOf < Circle 3 D >() ĊĊĠĠĠĠĠĠĠ Ġif Ġ( road Object . is Circle ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġobject Affine Ġ= ĠAff ine 3 D . of ( road Object . reference Line Point Relative Pose ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġaffine Sequence Ġ= ĠAff ine Sequence 3 D . of ( curve Affine , Ġobject Affine ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġcircle List Ġ+= ĠCircle 3 D ( road Object . radius , Ġconfiguration . tolerance , Ġaffine Sequence ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġif Ġ( road Object . repeat . is Repeat Cy linder ()) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġthis . report Logger . info Once (" Geometry ĠRe peated Circle Ġnot Ġimplemented Ġyet .") ĊĊĠĠĠĠĠĠĠ Ġreturn Ġcircle List ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġlist Ġof Ġlinear Ġr ings Ġfrom Ġan ĠOpen DRIVE Ġroad Ġobject Ġdefined Ġby Ġroad Ġcorner Ġout lines . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġbuild Linear R ings By Road C orners ( ĊĠĠĠĠĠĠĠ Ġid : ĠRoad space Object Identifier , ĊĠĠĠĠĠĠĠ Ġroad Object : ĠRoad Objects Object , ĊĠĠĠĠĠĠĠ Ġreference Line : ĠCurve 3 D ĊĠĠĠ Ġ): ĠList < Linear Ring 3 D > Ġ{ ĊĊĠĠĠĠĠĠĠ Ġreturn Ġroad Object . get Linear R ings Defined By Road C orners () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. map Ġ{ Ġbuild Linear Ring By Road C orners ( it , Ġreference Line ) Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle And Remove Failure Ġ{ Ġreport Logger . log ( it , Ġid . toString ()) Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle Message Ġ{ Ġreport Logger . log ( it , Ġid . toString ()) Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġsingle Ġlinear Ġring Ġfrom Ġan ĠOpen DRIVE Ġroad Ġobject Ġdefined Ġby Ġroad Ġcorner Ġout lines . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġbuild Linear Ring By Road C orners ( outline : ĠRoad Objects Object Out lines Outline , Ġreference Line : ĠCurve 3 D ): ĊĠĠĠĠĠĠĠ ĠResult < Context Message < Linear Ring 3 D >, ĠIllegalArgumentException > Ġ{ ĊĊĠĠĠĠĠĠĠ Ġval Ġvertices Ġ= Ġoutline . corner Road ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. map Ġ{ Ġbuild Vertices ( it , Ġreference Line ) Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle And Remove Failure Ġ{ Ġreport Logger . log ( it ) Ġ} ĊĊĠĠĠĠĠĠĠ Ġreturn ĠLinear Ring 3 D Factory . build From Vertices ( vertices , Ġconfiguration . tolerance ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġvertex Ġfrom Ġthe ĠOpen DRIVE Ġroad Ġcorner Ġelement . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġbuild Vertices ( corner Road : ĠRoad Objects Object Out lines Outline Corner Road , Ġreference Line : ĠCurve 3 D ): ĊĠĠĠĠĠĠĠ ĠResult < Vector 3 D , ĠException > Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġaffine Ġ= Ġreference Line . calculate Affine ( corner Road . curve Relative Position ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle Failure Ġ{ Ġreturn Ġit Ġ} ĊĠĠĠĠĠĠĠ Ġval Ġbase Point Ġ= Ġcorner Road . getBase Point () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle Failure Ġ{ Ġreturn Ġit Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. let Ġ{ Ġaffine . transform ( it . get Cartesian Curve Offset ()) Ġ} ĊĠĠĠĠĠĠĠ Ġreturn ĠResult . success ( base Point ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġlist Ġof Ġlinear Ġr ings Ġfrom Ġan ĠOpen DRIVE Ġroad Ġobject Ġdefined Ġby Ġlocal Ġcorner Ġout lines . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġbuild Linear R ings By Local C orners ( ĊĠĠĠĠĠĠĠ Ġid : ĠRoad space Object Identifier , ĊĠĠĠĠĠĠĠ Ġroad Object : ĠRoad Objects Object , ĊĠĠĠĠĠĠĠ Ġcurve Affine : ĠAff ine 3 D ĊĠĠĠ Ġ): ĠList < Linear Ring 3 D > Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġobject Affine Ġ= ĠAff ine 3 D . of ( road Object . reference Line Point Relative Pose ) ĊĠĠĠĠĠĠĠ Ġval Ġaffine Sequence Ġ= ĠAff ine Sequence 3 D . of ( curve Affine , Ġobject Affine ) ĊĊĠĠĠĠĠĠĠ Ġreturn Ġroad Object . get Linear R ings Defined By Local C orners () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. map Ġ{ Ġbuild Linear Ring By Local C orners ( id , Ġit ) Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle And Remove Failure Ġ{ Ġreport Logger . log ( it , Ġid . toString ()) Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle Message Ġ{ Ġreport Logger . log ( it , Ġid . toString ()) Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. map Ġ{ Ġit . copy ( affine Sequence Ġ= Ġaffine Sequence ) Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġsingle Ġlinear Ġring Ġfrom Ġan ĠOpen DRIVE Ġroad Ġobject Ġdefined Ġby Ġlocal Ġcorner Ġout lines . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġbuild Linear Ring By Local C orners ( id : ĠRoad space Object Identifier , Ġoutline : ĠRoad Objects Object Out lines Outline ): ĊĠĠĠĠĠĠĠ ĠResult < Context Message < Linear Ring 3 D >, ĠIllegalArgumentException > Ġ{ ĊĊĠĠĠĠĠĠĠ Ġval Ġvertices Ġ= Ġoutline . corner Local ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. map Ġ{ Ġit . getBase Point () Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle And Remove Failure Ġ{ Ġreport Logger . log ( it , Ġid . toString (), Ġ" Removing Ġoutline Ġpoint .") Ġ} ĊĊĠĠĠĠĠĠĠ Ġreturn ĠLinear Ring 3 D Factory . build From Vertices ( vertices , Ġconfiguration . tolerance ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġparame tric Ġbounded Ġsurface Ġfrom ĠOpen DRIVE Ġroad Ġobjects Ġdefined Ġby Ġrepeat Ġentries Ġrepresenting Ġa Ġhorizontal Ġsurface . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġbuild Parame tric Bounded Sur faces By Horizontal Repeat ( ĊĠĠĠĠĠĠĠ Ġid : ĠRoad space Object Identifier , ĊĠĠĠĠĠĠĠ Ġroad Object Repeat : ĠRoad Objects Object Repeat , ĊĠĠĠĠĠĠĠ Ġroad Reference Line : ĠCurve 3 D ĊĠĠĠ Ġ): ĠList < Parame tric Bounded Surface 3 D > Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ(! road Object Repeat . is Horizontal Parame tric Bounded Surface ()) Ġreturn Ġempty List () ĊĊĠĠĠĠĠĠĠ Ġ// Ġcurve Ġover Ġwhich Ġthe Ġobject Ġis Ġmoved ĊĠĠĠĠĠĠĠ Ġval Ġobject Reference Curve 2 D Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ_ curve 2 D Builder . build L ateral Translated Curve ( road Object Repeat , Ġroad Reference Line ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle Failure Ġ{ Ġreport Logger . log ( it , Ġid . toString (), Ġ" Removing Ġobject ."); Ġreturn Ġempty List () Ġ} ĊĠĠĠĠĠĠĠ Ġval Ġobject Reference Height Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ_ function Builder . build Stack ed Height Function From Repeat ( road Object Repeat , Ġroad Reference Line ) ĊĊĠĠĠĠĠĠĠ Ġ// Ġdimension Ġof Ġthe Ġobject ĊĠĠĠĠĠĠĠ Ġval Ġwidth Function Ġ= Ġroad Object Repeat . getObject Width Function () ĊĊĠĠĠĠĠĠĠ Ġ// Ġabsolute Ġboundary Ġcurves ĊĠĠĠĠĠĠĠ Ġval Ġleft Boundary Curve 2 D Ġ= Ġobject Reference Curve 2 D . add L ateral Translation ( width Function , Ġ- 0 . 5 ) ĊĠĠĠĠĠĠĠ Ġval Ġleft Boundary Ġ= ĠCurve 3 D ( left Boundary Curve 2 D , Ġobject Reference Height ) ĊĠĠĠĠĠĠĠ Ġval Ġright Boundary Curve 2 D Ġ= Ġobject Reference Curve 2 D . add L ateral Translation ( width Function , Ġ+ 0 . 5 ) ĊĠĠĠĠĠĠĠ Ġval Ġright Boundary Ġ= ĠCurve 3 D ( right Boundary Curve 2 D , Ġobject Reference Height ) ĊĊĠĠĠĠĠĠĠ Ġval Ġparame tric Bounded Surface Ġ= ĠPar ame tric Bounded Surface 3 D ( left Boundary , Ġright Boundary , Ġconfiguration . tolerance , ĠPar ame tric Bounded Surface 3 D . DEFAULT _ STEP _ SIZE ) ĊĠĠĠĠĠĠĠ Ġreturn ĠlistOf ( parame tric Bounded Surface ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠBuilds Ġa Ġparame tric Ġbounded Ġsurface Ġfrom ĠOpen DRIVE Ġroad Ġobjects Ġdefined Ġby Ġrepeat Ġentries Ġrepresenting Ġa Ġvertical Ġsurface . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġbuild Parame tric Bounded Sur faces By Vertical Repeat ( ĊĠĠĠĠĠĠĠ Ġid : ĠRoad space Object Identifier , ĊĠĠĠĠĠĠĠ Ġroad Object Repeat : ĠRoad Objects Object Repeat , ĊĠĠĠĠĠĠĠ Ġroad Reference Line : ĠCurve 3 D ĊĠĠĠ Ġ): ĠList < Parame tric Bounded Surface 3 D > Ġ{ ĊĠĠĠĠĠĠĠ Ġif Ġ(! road Object Repeat . is Vertical Parame tric Bounded Surface ()) Ġreturn Ġempty List () ĊĊĠĠĠĠĠĠĠ Ġ// Ġcurve Ġover Ġwhich Ġthe Ġobject Ġis Ġmoved ĊĠĠĠĠĠĠĠ Ġval Ġobject Reference Curve 2 D Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ_ curve 2 D Builder . build L ateral Translated Curve ( road Object Repeat , Ġroad Reference Line ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ. handle Failure Ġ{ Ġreport Logger . log ( it , Ġid . toString (), Ġ" Removing Ġobject ."); Ġreturn Ġempty List () Ġ} ĊĠĠĠĠĠĠĠ Ġval Ġobject Reference Height Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ_ function Builder . build Stack ed Height Function From Repeat ( road Object Repeat , Ġroad Reference Line ) ĊĊĠĠĠĠĠĠĠ Ġ// Ġdimension Ġof Ġthe Ġobject ĊĠĠĠĠĠĠĠ Ġval Ġheight Function Ġ= Ġroad Object Repeat . getObject Height Function () ĊĊĠĠĠĠĠĠĠ Ġ// Ġabsolute Ġboundary Ġcurves ĊĠĠĠĠĠĠĠ Ġval Ġlower Boundary Ġ= ĠCurve 3 D ( object Reference Curve 2 D , Ġobject Reference Height ) ĊĠĠĠĠĠĠĠ Ġval Ġupper Boundary Height Ġ= ĠStack ed Function . of Sum ( object Reference Height , Ġheight Function , ĠdefaultValue Ġ= Ġ 0 . 0 ) ĊĠĠĠĠĠĠĠ Ġval Ġupper Boundary Ġ= ĠCurve 3 D ( object Reference Curve 2 D , Ġupper Boundary Height ) ĊĊĠĠĠĠĠĠĠ Ġval Ġparame tric Bounded Surface Ġ= ĠPar ame tric Bounded Surface 3 D ( lower Boundary , Ġupper Boundary , Ġconfiguration . tolerance , ĠPar ame tric Bounded Surface 3 D . DEFAULT _ STEP _ SIZE ) ĊĠĠĠĠĠĠĠ Ġreturn ĠlistOf ( parame tric Bounded Surface ) ĊĠĠĠ Ġ} Ċ } Ċ