39 auto rxyz = dlib::range(1,3);
40 auto d = dlib::range(4,4);
41 static const double min_length = 1e-3;
44 adoreMatrix<double,3,1> La,Lb,Lc,Lac;
45 adoreMatrix<double,3,1> Ra,Rb,Rc,Rac;
46 adoreMatrix<double,3,1> Ca,Cc;
50 La = dlib::subm(
left,rxyz,dlib::range(0,0));
51 Ra = dlib::subm(
right,rxyz,dlib::range(0,0));
53 D = adore::mad::norm2<double,3>(La-Ra);
57 dlib::set_subm(center,rxyz,dlib::range(0,0)) = Ca;
58 dlib::set_subm(center,d,dlib::range(0,0)) = D;
67 double length_Lac=0,length_Rac=0,length_Cac;
69 double progress_RonL,progress_LonR;
72 while(i_left<N || i_right<M)
75 while(length_Lac<min_length && i_left<N)
77 Lc = dlib::subm(
left,rxyz,dlib::range(i_left,i_left));
79 length_Lac = adore::mad::norm2<double,3>(Lac);
80 Lac = Lac / length_Lac;
83 while(length_Rac<min_length && i_right<M)
85 Rc = dlib::subm(
right,rxyz,dlib::range(i_right,i_right));
87 length_Rac = adore::mad::norm2<double,3>(Rac);
88 Rac = Rac / length_Rac;
91 if(length_Lac>min_length)
93 progress_RonL = dlib::trans(Lac)*(Rc-La) / length_Lac;
94 progress_RonL = (
std::max)(0.0,progress_RonL);
100 if(length_Rac>min_length)
102 progress_LonR = dlib::trans(Rac)*(Lc-Ra) / length_Rac;
103 progress_LonR = (
std::max)(0.0,progress_LonR);
110 if( std::abs(progress_RonL-progress_LonR)<min_length
111 || (progress_LonR>=1.0 && progress_RonL>=1.0)
122 if(progress_RonL<progress_LonR)
124 La = La + Lac * progress_RonL * length_Lac;
125 length_Lac = (1.0-progress_RonL) * length_Lac;
131 Ra = Ra + Rac * progress_LonR * length_Rac;
132 length_Rac = (1.0-progress_LonR) * length_Rac;
136 if(i_left==N || i_right==M)
140 if(i_left<N||i_right<M) D = adore::mad::norm2<double,3>(La-Ra);
141 length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
142 if( length_Cac > min_length )
144 dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
145 dlib::set_subm(center,d,dlib::range(i_center,i_center)) = D;
146 center(0,i_center) = center(0,i_center-1) + length_Cac;
164 if(i_left<N||i_right<M) D = adore::mad::norm2<double,3>(La-Ra);
165 length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
166 if( length_Cac > min_length )
168 dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
169 dlib::set_subm(center,d,dlib::range(i_center,i_center)) = D;
170 center(0,i_center) = center(0,i_center-1) + length_Cac;
176 La = dlib::subm(
left,rxyz,dlib::range(N-1,N-1));
177 Ra = dlib::subm(
right,rxyz,dlib::range(M-1,M-1));
179 length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
180 if( length_Cac > min_length )
182 dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
183 dlib::set_subm(center,d,dlib::range(i_center,i_center)) = D;
184 center(0,i_center) = center(0,i_center-1) + length_Cac;
192 La = dlib::subm(
left,rxyz,dlib::range(0,0));
193 Lb = dlib::subm(
left,rxyz,dlib::range(N-1,N-1));
194 Ra = dlib::subm(
right,rxyz,dlib::range(0,0));
195 Rb = dlib::subm(
right,rxyz,dlib::range(M-1,M-1));
196 dlib::set_subm(center,rxyz,dlib::range(0,0)) = (La+Ra)*0.5;
197 dlib::set_subm(center,rxyz,dlib::range(1,1)) = (Lb+Rb)*0.5;
198 dlib::set_subm(center,d,dlib::range(0,0)) = adore::mad::norm2<double,3>(La-Ra);
199 dlib::set_subm(center,d,dlib::range(1,1)) = adore::mad::norm2<double,3>(Lb-Rb);
207 adoreMatrix<double, 0, 0> center_data;
211 center.
setData(dlib::colm(center_data, dlib::range(0, K - 1)));
void setData(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:580
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
int computeLaneWidthAndCenter(const adoreMatrix< double, 0, 0 > &left, const adoreMatrix< double, 0, 0 > &right, adoreMatrix< double, 0, 0 > ¢er)
Definition: centerandlanewidth.h:33
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20