Simply function updatewp_density(), reduced to O(N) time complexity

Author: codezs09

test_wp_post.csv

@@ -1,19 +1,17 @@
-215.33378922713493,201.81188825091405,10.60898494720459,-3.1388674628234354
-207.41565325823706,201.7903097664661,10.619364808783029,-3.1408510799080163
-191.4214210078194,201.77844886259473,10.605507721489147,-3.1415164639101048
-175.4266373709534,201.7772302251504,10.611396949219438,-3.140580743635333
-159.4319026861715,201.76104498837955,10.613230713506017,3.141196304457918
-143.43676275858144,201.76738464853605,10.613291479935194,-3.137302315917327
-135.43964216735327,201.73307409027387,10.604526101807872,-3.1372291493871987
-133.4396594802003,201.72434710202552,10.609897548129618,3.1356802584669548
-131.43817566422254,201.7361808030658,10.60818733376841,3.102272447519027
-130.4365059989096,201.77558697115873,10.6096427346935,3.053164876672713
-129.43716305555228,201.8641877034682,10.623217663352115,2.9817592584352175
-128.45475765877168,202.02255982503527,10.606557686226711,2.888153280291491
-127.50008717750973,202.26982785660655,10.611634930667403,2.783093122586031
-126.57554687673233,202.6162445675219,10.61474755621642,2.6815322679243874
-125.68418110839512,203.05793768057114,10.631189557302914,2.5863100498659444
-124.83994420999264,203.5816999073285,10.633261431131434,2.5042530358408204
-123.232211364341,204.7720923706875,10.627961250844871,2.4606781954921946
-110.80154596882166,214.8431060543582,10.6269350207704,2.4790894163582062
-103.97751274215905,220.16669653527612,10.646126730230025,2.4790894163582062
+215.33378942682145,201.81189013707106,10.60898494720459,-3.1388698357672853
+207.407571243807,201.79030843560355,10.619364808783029,-3.140852973086565
+191.41333977648827,201.77847781226544,10.605507721489147,-3.1415227379885993
+175.41855515356585,201.77735952728074,10.611396949219438,-3.1405684653829766
+159.42382039004926,201.76097790283688,10.613230713506017,-3.1415570276597546
+143.42871972390898,201.76040806249935,10.613291479935194,-3.136511891867225
+133.5486034312751,201.7102091138769,10.604526101807872,3.1323144477993283
+131.5988311930191,201.72830002107017,10.609897548129618,3.095923759610543
+130.20032565289256,201.79221266157862,10.60818733376841,3.036305703369278
+129.12984266800655,201.9053388738151,10.6096427346935,2.952479747452028
+128.15521971272076,202.09188179468225,10.623217663352115,2.847905147141233
+127.2138315624822,202.3765887391256,10.606557686226711,2.733024733632629
+126.23927845089067,202.79850168289744,10.611634930667403,2.618942838115936
+125.0283611179826,203.49609381985314,10.61474755621642,2.515986575255768
+123.12379047588523,204.87196786092954,10.631189557302914,2.4587765905022847
+113.58786068698177,212.6278218069211,10.633261431131434,2.4766743948736263
+103.97548917105081,220.16407382029885,10.627961250844871,2.4766743948736263

wpProcess.py

@@ -137,53 +137,43 @@ def updatewp_density(self):
 		idx_org = 0
 		station_.append(self.station[0])
 		curvature_.append(self.curvature[0])
-		while station_[-1] < self.station[-1]:
-			# obtain s, s.t. curvature vlaue is within limits
-			ds = min( 16.0, self.station[-1]-station_[-1] )
-			flg_runonce = False
-			while not flg_runonce or ds>=1.0:
-				flg_runonce = True
-				s = station_[-1]+ds
-				flg_excd = False	# flag indicator whether it exceeds curvature limit
-				coef = 0.1
-
-				# check the curvature in the range [station_[-1], station_[-1]+ds] exceeding limit
-				i = idx_org
-				while i<len(self.station) and self.station[i]<=s:					
-					if ds > 1.0 and abs(self.curvature[i]) > max(0.01, coef/ds):
-						flg_excd = True
-						break
-					i += 1
-
-				if flg_excd:
-					ds /= 2
-				else:
-					break
-			print ds
-			idx_org = i
-			# interp for new x, y, curvature at s
-			fx = interp1d(self.station, self.x)
-			fy = interp1d(self.station, self.y)
-			fcurv = interp1d(self.station, self.curvature)
-			x_.append( float( fx(s) ) )
-			y_.append( float(fy(s)) )
-			station_.append(s)
-			curvature_.append( float(fcurv(s)) )
-
-		# update the class members x, y, th, station, curvature
-		# plt.plot(self.x, self.y, 'b.')
-		# plt.plot(x_, y_, 'r.')
-		# plt.xlabel('x [m]')
-		# plt.ylabel('y [m]')
-		# plt.legend(['original','adjusted'], loc='best')
-		# plt.show()
-
-		# plt.plot(self.station, self.curvature, 'b.')		
-		# plt.plot(station_, curvature_, 'r.')
-		# plt.xlabel('station [m]')
-		# plt.ylabel('curvature [m]')
-		# plt.legend(['original','adjusted'], loc='best')
-		# plt.show()
+		fx = interp1d(self.station, self.x)
+		fy = interp1d(self.station, self.y)
+		fcurv = interp1d(self.station, self.curvature)
+
+		ds = 16.0
+		for i in range(len(self.x) ):
+			if ds < self.station[i] - station_[-1] or i == (len(self.x) - 1):
+				if i == (len(self.x) - 1):
+					ds = self.station[-1] - station_[-1]
+				s = station_[-1] + ds
+				print(ds)
+
+				# interp for new x, y, curvature at s				
+				x_.append( float( fx(s) ) )
+				y_.append( float(fy(s)) )
+				station_.append(s)
+				curvature_.append( float(fcurv(s)) )
+				# reset 
+				ds = 16
+
+			ds = min(ds,  0.1/abs(self.curvature[i]) )			
+			ds = max(ds, 1.0)	# saturation of ds
+
+		#update the class members x, y, th, station, curvature
+		plt.plot(self.x, self.y, 'b.')
+		plt.plot(x_, y_, 'r.')
+		plt.xlabel('x [m]')
+		plt.ylabel('y [m]')
+		plt.legend(['original','adjusted'], loc='best')
+		plt.show()
+
+		plt.plot(self.station, self.curvature, 'b.')		
+		plt.plot(station_, curvature_, 'r.')
+		plt.xlabel('station [m]')
+		plt.ylabel('curvature [m]')
+		plt.legend(['original','adjusted'], loc='best')
+		plt.show()
 
 		self.x = x_
 		self.y = y_