text: from quaternion import *
  import timeit
  import numpy as np
  import cv2
  import random
  
  
  class Node(object):
  	def __init__(self, _id, _layer, _x, _y):
  		self.id = _id
  		self.layer = _layer
  		self.x = _x
  		self.y = _y
  
  		return
  
  
  class Link(object):
  	def __init__(self, _id, _type, _node0, _node1):
  		self.id = _id
  		self.type = _type
  		self.layer = _node0.layer
  		self.node0 = _node0
  		self.node1 = _node1
  
  		return
  
  
  class Arrow(object):
  	def __init__(self, _id, _type, _node0, _node1):
  		self.id = _id
  		# 0: down, 1: up, 2: both
  		self.type = _type
  		self.layer = _node0.layer
  		self.node0 = _node0
  		self.node1 = _node1
  
  		return
  
  
  class Step(object):
  	def __init__(self, _dx, _dy, _pressure, _current_floor, _scale=1.0):
  		self.dx = _dx * _scale
  		self.dy = _dy * _scale
  		self.pressure = _pressure
  		self.current_floor = _current_floor
  
  		self.current_position = None
  
  		return
  
  
  class Particle(object):
  	def __init__(self, _x, _y, _pressure, _current_floor, _scale, _rotate_angle):
  		self.x = _x
  		self.y = _y
  		self.pressure = _pressure
  		self.current_floor = _current_floor
  
  		self.scale = _scale
  		self.rotate_angle = _rotate_angle
  		self.rotation = Quaternion()
  		self.rotation.RotateZ(_rotate_angle)
  
  		return
  
  
  class MapMatching(object):
  	def __init__(self):
  		map_path = 'map\\'
  		map_filename = 'core.txt'
  
  		self.floor_number = 1
  		self.map_images = []
  		self.floor_height = 5.0
  		self.start_floor = 0
  		self.start_x = 0.0
  		self.start_y = 0.0
  		self.dir_angle = 0.0
  		self.meter_to_pixel = 1.0
  		self.node_filename = None
  		self.link_filename = None
  		self.arrow_filename = None
  		self.polygon_filename = None
  
  		in_file = open(map_path + map_filename, 'r')
  		in_file.readline()
  		for tLine in in_file.readlines():
  			tSeq = tLine.split(',')
  			if len(tSeq) < 1:
  				continue
  			if tSeq[0] == 'floor':
  				self.floor_number = int(tSeq[1])
  				for i in range(self.floor_number):
  					self.map_images.append(
  						cv2.imread(
  							map_path + tSeq[i + 2].split('\n')[0]
  						)
  					)
  			elif tSeq[0] == 'height':
  				self.floor_height = float(tSeq[1])
  			elif tSeq[0] == 'start':
  				self.start_floor = int(tSeq[1])
  			elif tSeq[0] == 'start_point':
  				self.start_x = float(tSeq[1])
  				self.start_y = float(tSeq[2])
  			elif tSeq[0] == 'dir':
  				self.dir_angle = float(tSeq[1])
  			elif tSeq[0] == 'scale':
  				self.meter_to_pixel = 1.0 / float(tSeq[1])
  			elif tSeq[0] == 'node':
  				self.node_filename = tSeq[1].split('\n')[0]
  			elif tSeq[0] == 'link':
  				self.link_filename = tSeq[1].split('\n')[0]
  			elif tSeq[0] == 'arrow':
  				self.arrow_filename = tSeq[1].split('\n')[0]
  			elif tSeq[0] == 'polygon':
  				self.polygon_filename = tSeq[1].split('\n')[0]
  		in_file.close()
  
  		self.current_floor = self.start_floor
  		self.current_position = Vector3()
  		self.current_position.x = self.start_x
  		self.current_position.y = self.start_y
  		self.current_position.z = self.dir_angle
  		self.delta_pressure_to_delta_height = -9.0
  		self.start_pressure = -1.0
  		self.pressure_gain = 0.0
  
  		self.map_width = self.map_images[0].shape[1]
  		self.map_height = self.map_images[0].shape[0]
  
  		self.resize_width = self.map_width
  		self.resize_height = self.map_height
  
  		# self.video_writer = cv2.VideoWriter('video.avi', cv2.cv.CV_FOURCC('X', 'V', 'I', 'D'), 20, (self.resize_width, self.resize_height), True)
  
  		# video recorder
  
  
  		self.floor_changed = False
  		self.last_floor = self.current_floor
  
  		self.initial_rotation = Quaternion()
  		self.initial_rotation.RotateZ(self.dir_angle)
  
  		self.nodes = {}
  		self.links = []
  		self.arrows = []
  		self.polygon_zone = []
  		for i in range(self.floor_number):
  			self.links.append([])
  			self.arrows.append([])
  			self.polygon_zone.append(
  				np.ones(
  					(self.map_height, self.map_width, 1),
  					dtype=np.uint8
  				) * 255
  			)
  
  		if self.node_filename is None:
  			print('Error: node file is None...')
  			return
  
  		in_file = open(map_path + self.node_filename, 'r')
  		in_file.readline()
  		for tLine in in_file.readlines():
  			tSeq = tLine.split(',')
  			if len(tSeq) < 1:
  				continue
  			tNode = Node(
  				int(tSeq[0]),
  				int(tSeq[1]),
  				float(tSeq[2]),
  				float(tSeq[3])
  			)
  			self.nodes[tNode.id] = tNode
  		in_file.close()
  
  		if self.link_filename is None:
  			print('Error: link file is None...')
  
  		in_file = open(map_path + self.link_filename, 'r')
  		in_file.readline()
  		for tLine in in_file.readlines():
  			tSeq = tLine.split(',')
  			if len(tSeq) < 1:
  				continue
  			tLink = Link(
  				int(tSeq[0]),
  				int(tSeq[1]),
  				self.nodes[int(tSeq[2])],
  				self.nodes[int(tSeq[3])]
  			)
  			self.links[tLink.layer].append(tLink)
  		in_file.close()
  
  		if self.arrow_filename is None:
  			print('Error: arrow file is None...')
  
  		in_file = open(map_path + self.arrow_filename, 'r')
  		in_file.readline()
  		for tLine in in_file.readlines():
  			tSeq = tLine.split(',')
  			tArrow = Arrow(
  				int(tSeq[0]),
  				int(tSeq[1]),
  				self.nodes[int(tSeq[2])],
  				self.nodes[int(tSeq[3])]
  			)
  			self.arrows[tArrow.layer].append(tArrow)
  		in_file.close()
  
  		if self.polygon_filename is None:
  			print('Error: polygon file is None...')
  
  		in_file = open(map_path + self.polygon_filename, 'r')
  		in_file.readline()
  		for tLine in in_file.readlines():
  			tSeq = tLine.split(',')
  			temp_type = int(tSeq[1])
  			if temp_type != 1:
  				continue
  			temp_node_number = int(tSeq[2])
  			temp_points = []
  			temp_layer = None
  			for i in range(temp_node_number):
  				tNode = self.nodes[int(tSeq[i + 3])]
  				temp_points.append((int(tNode.x), int(tNode.y)))
  				temp_layer = tNode.layer
  			temp_points_array = np.array([temp_points], dtype=np.int32)
  			cv2.fillPoly(self.polygon_zone[temp_layer], temp_points_array, temp_type)
  		in_file.close()
  
  		in_file = open(map_path + self.polygon_filename, 'r')
  		in_file.readline()
  		for tLine in in_file.readlines():
  			tSeq = tLine.split(',')
  			temp_type = int(tSeq[1])
  			if temp_type == 1:
  				continue
  			temp_node_number = int(tSeq[2])
  			temp_points = []
  			temp_layer = None
  			for i in range(temp_node_number):
  				tNode = self.nodes[int(tSeq[i + 3])]
  				temp_points.append((int(tNode.x), int(tNode.y)))
  				temp_layer = tNode.layer
  			temp_points_array = np.array([temp_points], dtype=np.int32)
  			cv2.fillPoly(self.polygon_zone[temp_layer], temp_points_array, temp_type)
  		in_file.close()
  
  		self.steps = []
  
  		self.particles = []
  		for i in range(9):
  			for j in range(5):
  				self.particles.append(
  					Particle(
  						self.start_x,
  						self.start_y,
  						self.start_pressure + self.pressure_gain,
  						self.start_floor,
  						1.0 + (i - 4) * 0.025,
  						self.dir_angle + (j - 2) * 0.05
  					)
  				)
  
  		self.particle_number_max = 512
  		particle_number = self.particle_number_max - 45
  		for i in range(particle_number):
  			self.particles.append(
  				Particle(
  					self.start_x,
  					self.start_y,
  					self.start_pressure + self.pressure_gain,
  					self.start_floor,
  					random.uniform(0.9, 1.1),           # initialising scale uniformly. generate a uniform random number distribution in the range [a, b) or [a, b] depending on rounding
  					self.dir_angle + random.uniform(-0.1, 0.1)
  				)
  			)
  
  		self.view_particle = True
  
  		self.window_name = 'Current Position'
  
  		self.position_filename = 'log\\position.csv'
  		log_file = open(self.position_filename, 'w')
  		log_file.write('x,y,z')
  		log_file.close()
  
  		self.time_start = 1515688724000
  		self.log_list = []
  
  		self.IPIN_filename = 'log\\amit.csv'
  		log_file = open(self.IPIN_filename, 'w')
  		temp_longitude, temp_latitude, temp_floor = self.MapToWorld(self.current_position.x, self.current_position.y, self.current_floor)
  		log_file.write(
  			'%i,%.9f,%.9f,%i,%i' %
  			(
  				self.time_start + int(timeit.default_timer() * 1000),
  				temp_longitude,
  				temp_latitude,
  				temp_floor,
  				0
  			)
  		)
  		log_file.close()
  
  		return
  
  	def Release(self):
  		cv2.destroyWindow(self.window_name)
  
  		# self.video_writer.release()
  
  		return
  
  	def MapToWorld(self, _x, _y, _floor):                   # how to get the values
  		homography = [
  			[-2.10388667722e-05, -0.000262518473727, -1.63503055188],
  			[0.000604240370159, 0.00774479195666, 47.2205108312],
  			[1.27234617272e-05, 0.000163991977084, 1.0]
  		]
  
  		rx = homography[0][0] * _x + homography[0][1] * _y + homography[0][2]
  		ry = homography[1][0] * _x + homography[1][1] * _y + homography[1][2]
  		rw = homography[2][0] * _x + homography[2][1] * _y + homography[2][2]
  		rx /= rw
  		ry /= rw
  
  		return rx, ry, _floor - 1
  
  	def DistanceToLine(self, _line, _pt):
  		line_length = math.sqrt((_line[0][0] - _line[1][0]) ** 2 + (_line[0][1] - _line[1][1]) ** 2)
  		dist_0 = math.sqrt((_pt[0] - _line[0][0]) ** 2 + (_pt[1] - _line[0][1]) ** 2)
  		dot_0 = (_pt[0] - _line[0][0]) * (_line[1][0] - _line[0][0]) + (_pt[1] - _line[0][1]) * (_line[1][1] - _line[0][1])
  		line_projection = dot_0 / line_length
  
  		if line_projection < 0.0:
  			return math.sqrt((_pt[0] - _line[0][0]) ** 2 + (_pt[1] - _line[0][1]) ** 2)
  		if line_projection > line_length:
  			return math.sqrt((_pt[0] - _line[1][0]) ** 2 + (_pt[1] - _line[1][1]) ** 2)
  
  		return math.sqrt(max(0.01, dist_0 ** 2 - line_projection ** 2))
  
  	def IntersectBase(self, _x0, _y0, _x1, _y1, _x2, _y2, _x3, _y3):
  		threshold = 0.1
  
  		mid_x_0 = (_x0 + _x1) * 0.5
  		mid_y_0 = (_y0 + _y1) * 0.5
  
  		mid_x_1 = (_x2 + _x3) * 0.5
  		mid_y_1 = (_y2 + _y3) * 0.5
  
  		if (abs(mid_x_1 - mid_x_0) > abs(_x1 - mid_x_0) + abs(_x3 - mid_x_1) + threshold) or (abs(mid_y_1 - mid_y_0) > abs(_y1 - mid_y_0) + abs(_y3 - mid_y_1) + threshold):
  			return False
  
  		return True
  
  	def Intersect(self, _x0, _y0, _x1, _y1, _x2, _y2, _x3, _y3):
  		if not self.IntersectBase(_x0, _y0, _x1, _y1, _x2, _y2, _x3, _y3):
  			return False, -1.0, -1.0
  
  		s_x_0 = _x1 - _x0
  		s_y_0 = _y1 - _y0
  		s_x_1 = _x3 - _x2
  		s_y_1 = _y3 - _y2
  
  		determinant = s_x_0 * s_y_1 - s_x_1 * s_y_0
  		threshold = 0.0001
  		if abs(determinant) < threshold:
  			return False, -1.0, -1.0
  
  		s = (s_x_0 * (_y0 - _y2) - s_y_0 * (_x0 - _x2)) / determinant
  		if s < 0.0 or s > 1.0:
  			return False, -1.0, -1.0
  
  		t = (s_x_1 * (_y0 - _y2) - s_y_1 * (_x0 - _x2)) / determinant
  		if t < 0.0 or t > 1.0:
  			return False, -1.0, -1.0
  
  		return True, t, s
  
  	def GetCurrentFloor(self, _current_pressure):
  		current_floor = self.start_floor
  		delta_height = (_current_pressure - (self.start_pressure + self.pressure_gain)) * self.delta_pressure_to_delta_height
  		while delta_height > self.floor_height * 0.5:
  			current_floor += 1
  			delta_height -= self.floor_height
  		while delta_height < -self.floor_height * 0.5:
  			current_floor -= 1
  			delta_height += self.floor_height
  
  		if current_floor >= self.floor_number:
  			current_floor = self.floor_number - 1
  			print('Pressure error, start pressure: %.3f, current pressure: %.3f' % (self.start_pressure + self.pressure_gain, _current_pressure))
  		if current_floor < 0:
  			current_floor = 0
  			print('Pressure error, start pressure: %.3f, current pressure: %.3f' % (self.start_pressure + self.pressure_gain, _current_pressure))
  
  		return current_floor
  
  	def UpdateCurrentFloor(self, _current_pressure):
  		if not self.floor_changed:
  			self.last_floor = self.current_floor
  		self.current_floor = self.start_floor
  		delta_height = (_current_pressure - (self.start_pressure + self.pressure_gain)) * self.delta_pressure_to_delta_height
  		while delta_height > self.floor_height * 0.5:
  			self.current_floor += 1
  			delta_height -= self.floor_height
  		while delta_height < -self.floor_height * 0.5:
  			self.current_floor -= 1
  			delta_height += self.floor_height
  
  		if self.current_floor >= self.floor_number:
  			self.current_floor = self.floor_number - 1
  			print('Pressure error, start pressure: %.3f, current pressure: %.3f' % (self.start_pressure + self.pressure_gain, _current_pressure))
  		if self.current_floor < 0:
  			self.current_floor = 0
  			print('Pressure error, start pressure: %.3f, current pressure: %.3f' % (self.start_pressure + self.pressure_gain, _current_pressure))
  
  		if self.current_floor != self.last_floor:
  			self.floor_changed = True
  
  			temp_particles = self.particles
  			self.particles = []
  			for temp_particle in temp_particles:
  				temp_x = int(temp_particle.x)
  				temp_y = int(temp_particle.y)
  				if (temp_x > -1) and (temp_x < self.map_width) and (temp_y > -1) and (temp_y < self.map_height) and (self.polygon_zone[self.current_floor][temp_y][temp_x] != 0):
  					self.particles.append(temp_particle)
  
  		return
  
  	def UpdateCurrentAngle(self):
  		particle_number = len(self.particles)
  		if particle_number > 0:
  			if particle_number > 1:
  				base_angle = self.particles[0].rotate_angle
  				self.current_position.z = 0.0
  				for i in range(1, particle_number):
  					temp_angle = self.particles[i].rotate_angle - base_angle
  					while temp_angle > math.pi:
  						temp_angle -= math.pi * 2.0
  					while temp_angle < -math.pi:
  						temp_angle += math.pi * 2.0
  					self.current_position.z += temp_angle
  				self.current_position.z /= particle_number
  				self.current_position.z += base_angle
  			else:
  				self.current_position.z = self.particles[0].rotate_angle
  
  		while self.current_position.z > math.pi:
  			self.current_position.z -= math.pi * 2.0
  		while self.current_position.z < -math.pi:
  			self.current_position.z += math.pi * 2.0
  
  		return
  
  	def CheckNewParticle(self, _start, _end, _floor):
  		for tLink in self.links[_floor]:
  			tNode0 = tLink.node0
  			tNode1 = tLink.node1
  			temp_hit, temp_t, temp_s = self.Intersect(_start[0], _start[1], _end[0], _end[1], tNode0.x, tNode0.y, tNode1.x, tNode1.y)
  			if temp_hit:
  				return False
  
  		return True
  
  	def BackTrackingParticle(self, _particle, _step, _position_check_number=0, _position_error_max=2.0):
  		last_x = _particle.x
  		last_y = _particle.y
  		scale = _particle.scale
  		rotation = _particle.rotation
  
  		temp_position_check_number = _position_check_number
  
  		for tStep in _step:
  			displacement = rotation.RotateVector(tStep.dx, tStep.dy, 0.0)
  			current_x = last_x - displacement.x * scale
  			current_y = last_y - displacement.y * scale
  			for tLink in self.links[tStep.current_floor]:
  				tNode0 = tLink.node0
  				tNode1 = tLink.node1
  				temp_hit, temp_t, temp_s = self.Intersect(last_x, last_y, current_x, current_y, tNode0.x, tNode0.y, tNode1.x, tNode1.y)
  				if temp_hit:
  					return False
  			if temp_position_check_number > 0:
  				temp_distance = math.sqrt((current_x - tStep.current_position.x) ** 2 + (current_y - tStep.current_position.y) ** 2) #distance of a line formula when 2 points are given
  				if temp_distance > _position_error_max:
  					return False
  				temp_position_check_number -= 1
  			last_x = current_x
  			last_y = current_y
  
  		return True
  
  	def CheckSteps(self, _position_start, _steps, _scale, _rotation):
  		last_x = _position_start.x
  		last_y = _position_start.y
  		for tStep in _steps:
  			displacement = _rotation.RotateVector(tStep.dx, tStep.dy, 0.0)
  			current_x = last_x + displacement.x * _scale
  			current_y = last_y + displacement.y * _scale
  			for tLink in self.links[tStep.current_floor]:
  				tNode0 = tLink.node0
  				tNode1 = tLink.node1
  				temp_hit, temp_t, temp_s = self.Intersect(last_x, last_y, current_x, current_y, tNode0.x, tNode0.y, tNode1.x, tNode1.y)
  				if temp_hit:
  					return False
  			last_x = current_x
  			last_y = current_y
  
  		return True
  
  	def CheckParticleSteps(self, _particle, _steps):
  		for tStep in _steps:
  			displacement = _particle.rotation.RotateVector(tStep.dx, tStep.dy, 0.0)
  			current_x = _particle.x + displacement.x * _particle.scale
  			current_y = _particle.y + displacement.y * _particle.scale
  			for tLink in self.links[tStep.current_floor]:
  				tNode0 = tLink.node0
  				tNode1 = tLink.node1
  				temp_hit, temp_t, temp_s = self.Intersect(_particle.x, _particle.y, current_x, current_y, tNode0.x, tNode0.y, tNode1.x, tNode1.y)
  				if temp_hit:
  					return False
  			_particle.x = current_x
  			_particle.y = current_y
  
  		return True
  
  	def WritePosition(self):
  		log_file = open(self.position_filename, 'a')
  		log_file.write(
  			'\n%.3f,%.3f,%d' %
  			(
  				self.current_position.x,
  				self.current_position.y,
  				self.current_floor
  			)
  		)
  		log_file.close()
  
  		log_file = open(self.IPIN_filename, 'a')
  		temp_longitude, temp_latitude, temp_floor = self.MapToWorld(self.current_position.x, self.current_position.y, self.current_floor)
  		temp_index = 0
  		if len(self.log_list) > 0:
  			temp_index = self.log_list[0]
  			self.log_list = self.log_list[1:]
  			print('Logged index %i' % temp_index)
  		log_file.write(
  			'\n%i,%.9f,%.9f,%i,%i' %
  			(
  				self.time_start + int(timeit.default_timer() * 1000),
  				temp_longitude,
  				temp_latitude,
  				temp_floor,
  				temp_index
  			)
  		)
  		log_file.close()
  
  		return
  
  	def ShowMap(self):
  		image_show = self.map_images[self.current_floor].copy()
  		if self.view_particle:
  			for tParticle in self.particles:
  				cv2.circle(
  					image_show,
  					(int(tParticle.x), int(tParticle.y)),
  					1,
  					(255, 0, 0),
  					thickness=-1
  				)
  		cv2.circle(
  			image_show,
  			(int(self.current_position.x), int(self.current_position.y)),
  			3,
  			(0, 0, 255),
  			thickness=-1
  		)
  
  		# resized_image = cv2.resize(image_show, (self.resize_width, self.resize_height))
  		resized_image = cv2.resize(image_show, (960, 540))
  		cv2.imshow(self.window_name, resized_image)
  		cv2.waitKey(5)
  		# self.video_writer.write(resized_image)
  		####added####3
  		print('current_position x', self.current_position.x)
  		print('current_position y', self.current_position.y)
  		print('current heading i.e. angle', self.current_position.z)
  
  		return
  
  	def TryRevive(self, _radius, _angle_range, _max_try_time, _back_tracking_step_number, _pressure, _position_check_number=0, _position_error_max=2.0):
  		back_tracking_steps = []
  		for i in range(1, _back_tracking_step_number + 1):
  			if self.steps[-i].current_floor != self.current_floor:
  				break
  			back_tracking_steps.append(self.steps[-i])
  
  		for i in range(_max_try_time):
  			temp_angle = random.uniform(-math.pi, math.pi)
  			temp_radius = random.uniform(0.0, _radius)
  			new_x = self.current_position.x + math.cos(temp_angle) * temp_radius
  			new_y = self.current_position.y + math.sin(temp_angle) * temp_radius
  			temp_x = int(new_x)
  			temp_y = int(new_y)
  			if (temp_x > -1) and (temp_x < self.map_width) and (temp_y > -1) and (temp_y < self.map_height) and (self.polygon_zone[self.current_floor][temp_y][temp_x] != 0):
  				new_particle = Particle(
  					new_x,
  					new_y,
  					_pressure,
  					self.current_floor,
  					random.uniform(0.9, 1.1),
  					self.current_position.z + random.uniform(-_angle_range, _angle_range)
  				)
  				if self.BackTrackingParticle(new_particle, back_tracking_steps, _position_check_number=_position_check_number, _position_error_max=_position_error_max):
  					self.particles.append(new_particle)
  
  		return
  
  	def UpdatePressureGain(self, _dx, _dy, _z, _threshold):
  		temp_rotation = Quaternion()
  		temp_rotation.RotateZ(self.current_position.z)
  		temp_displacement = temp_rotation.RotateVector(_dx, -_dy, 0.0)
  		temp_x = int(self.current_position.x + temp_displacement.x * self.meter_to_pixel)
  		temp_y = int(self.current_position.y + temp_displacement.y * self.meter_to_pixel)
  
  		if (temp_x > -1) and (temp_x < self.map_width) and (temp_y > -1) and (temp_y < self.map_height) and (self.polygon_zone[self.current_floor][temp_y][temp_x] != 1):
  			self.pressure_gain = _z - self.start_pressure - (self.current_floor - self.start_floor) * self.floor_height / self.delta_pressure_to_delta_height
  
  		return
  
  #### main part
  	def Update(self, _dx, _dy, _z, _check_floor=True):
  		step_length = math.sqrt(_dx ** 2 + _dy ** 2)
  		if (step_length < 0.1) or (step_length > 1.5):
  			print('Dropped step, length: %.3fm' % step_length)
  			return
  
  		self.UpdatePressureGain(_dx, _dy, _z, 5.0 * self.meter_to_pixel)
  
  		self.UpdateCurrentFloor(_z)
  
  		temp_scale = self.meter_to_pixel
  		temp_step = Step(_dx, -_dy, _z, self.current_floor, _scale=temp_scale)
  		self.steps.append(temp_step)
  
  		step_number = len(self.steps)
  
  		temp_particles = self.particles
  		self.particles = []
  		x_sum = 0.0
  		y_sum = 0.0
  		for tParticle in temp_particles:
  			if self.CheckParticleSteps(tParticle, [temp_step]):
  				x_sum += tParticle.x
  				y_sum += tParticle.y
  				self.particles.append(tParticle)
  
  		particle_number = len(self.particles)
  		try_time_max = 8
  		sample_number_max = 1
  		back_tracking_step_number = min(32, step_number) + 1
  
  		if particle_number > 0:
  			self.current_position.x = x_sum / particle_number  # final position P, equation 4
  			self.current_position.y = y_sum / particle_number
  			self.UpdateCurrentAngle()
  		else:
  			temp_rotation = Quaternion()
  			temp_rotation.RotateZ(self.current_position.z)
  			temp_displacement = temp_rotation.RotateVector(temp_step.dx, temp_step.dy, 0.0)
  			self.current_position.x += temp_displacement.x
  			self.current_position.y += temp_displacement.y
  		self.WritePosition()
  		self.steps[-1].current_position = Vector3(_v=self.current_position)
  
  		if particle_number > 0:
  			sample_number = min(sample_number_max, particle_number // 2 + 1)
  			generate_radius_min = 0.2 * self.meter_to_pixel
  			generate_radius_max = 2.0 * self.meter_to_pixel
  			temp_particles = []
  			new_particle_number = self.particle_number_max - particle_number
  			while new_particle_number > 0:
  				slice_particles = random.sample(self.particles, sample_number)   # algorithm 1
  				x_sum = 0.0
  				y_sum = 0.0
  				angle_sum = 0.0
  				for tParticle in slice_particles:
  					x_sum += tParticle.x
  					y_sum += tParticle.y
  					angle_sum += tParticle.rotate_angle
  				x_sum /= sample_number
  				y_sum /= sample_number
  				angle_sum /= sample_number
  				angle_sum += random.uniform(-0.1, 0.1)
  				try_time = try_time_max
  				while try_time > 0:
  					try_time -= 1
  					temp_angle = random.uniform(-math.pi, math.pi)
  					temp_radius = random.uniform(generate_radius_min, generate_radius_max)
  					new_x = x_sum + math.cos(temp_angle) * temp_radius
  					new_y = y_sum + math.sin(temp_angle) * temp_radius
  					new_particle = Particle(
  						new_x,
  						new_y,
  						_z,
  						self.current_floor,
  						random.uniform(0.9, 1.1),
  						angle_sum
  					)
  					if self.CheckNewParticle((x_sum, y_sum), (new_x, new_y), self.current_floor):
  						back_tracking_steps = []
  						for i in range(1, back_tracking_step_number):
  							if self.steps[-i].current_floor != self.current_floor:
  								break
  							back_tracking_steps.append(self.steps[-i])
  						if self.BackTrackingParticle(new_particle, back_tracking_steps, _position_check_number=0, _position_error_max=3.0):
  							temp_particles.append(new_particle)
  							break
  				new_particle_number -= 1
  			self.particles.extend(temp_particles)
  		else:
  			print('Try reviving...')
  			self.TryRevive(10.0 * self.meter_to_pixel, 0.3, 256, min(16, step_number), _z, _position_check_number=0, _position_error_max=3.0)
  
  		if _check_floor and self.floor_changed:
  			step_number_min = 6
  			step_number_test = 3
  			back_step_number_max = 40
  			# step_number_min >= step_number_test * 2
  			delta_height_threshold = 0.3
  			height_distance_threshold = 0.3 * self.floor_height
  			height_distance_threshold_min = 0.25
  			distance_threshold = 20.0 * self.meter_to_pixel
  			if step_number > step_number_min:
  				temp_floor_check = True
  				for i in range(1, step_number_test + 1):
  					if self.steps[-i].current_floor != self.current_floor:
  						temp_floor_check = False
  						break
  				temp_delta_height = (self.steps[-1].pressure - self.steps[-1 - step_number_test].pressure) * self.delta_pressure_to_delta_height
  				temp_height_distance = (self.steps[-1].pressure - (self.start_pressure + self.pressure_gain)) * self.delta_pressure_to_delta_height - (self.current_floor - self.start_floor) * self.floor_height
  				if (((abs(temp_height_distance) < height_distance_threshold) and (abs(temp_delta_height) < delta_height_threshold)) or (abs(temp_height_distance) < height_distance_threshold_min)) and temp_floor_check:
  					back_step_index = min(step_number - step_number_test, back_step_number_max + step_number_test) + 1
  					temp_floor_flag = True
  					for i in range(step_number_test + 1, back_step_index):
  						if temp_floor_flag:
  							if self.steps[-i].current_floor != self.current_floor:
  								temp_floor_flag = False
  						if not temp_floor_flag:
  							if self.steps[-i].current_floor != self.last_floor:
  								break
  						temp_delta_height = (self.steps[-i].pressure - self.steps[-i - step_number_test].pressure) * self.delta_pressure_to_delta_height
  						temp_height_distance = (self.steps[-i].pressure - (self.start_pressure + self.pressure_gain)) * self.delta_pressure_to_delta_height - (self.last_floor - self.start_floor) * self.floor_height
  						if ((abs(temp_height_distance) < height_distance_threshold) and (abs(temp_delta_height) < delta_height_threshold)) or (abs(temp_height_distance) < height_distance_threshold_min):
  							entrance_arrow = None
  							entrance_distance = 0.0
  							temp_type = 1
  							if self.current_floor > self.last_floor:
  								temp_type = 0
  							for tArrow in self.arrows[self.current_floor]:
  								if (tArrow.type == temp_type) or (tArrow.type == 2):
  									temp_entrance_distance = math.sqrt((self.steps[-i].current_position.x - tArrow.node0.x) ** 2 + (self.steps[-i].current_position.y - tArrow.node0.y) ** 2)
  									if (entrance_arrow is None) or (temp_entrance_distance < entrance_distance):
  										entrance_arrow = tArrow
  										entrance_distance = temp_entrance_distance
  							if (entrance_arrow is not None) and (entrance_distance < distance_threshold):
  								self.particles = []
  								self.current_position.x = entrance_arrow.node1.x
  								self.current_position.y = entrance_arrow.node1.y
  								for j in range(5):
  									for k in range(9):
  										self.particles.append(
  											Particle(
  												entrance_arrow.node1.x,
  												entrance_arrow.node1.y,
  												self.steps[-1 - step_number_test].pressure,
  												self.current_floor,
  												1.0 + (k - 4) * 0.025,
  												self.current_position.z + (j - 2) * 0.05
  											)
  										)
  								temp_steps = self.steps[-step_number_test:]
  								self.steps = self.steps[:-step_number_test]
  								for tStep in temp_steps:
  									self.Update(tStep.dx / self.meter_to_pixel, -tStep.dy / self.meter_to_pixel, tStep.pressure, _check_floor=False)
  							break
  					self.floor_changed = False
  			else:
  				self.floor_changed = False
  
  		return
  