def step(self, action, objective):
self.prev_position = self.position.copy()
# Rate of Turn
# https://dspace.lib.cranfield.ac.uk/bitstream/handle/1826/2912/CHAPTER_8_july27_2.pdf?sequence=5&isAllowed=y
ROT = np.float64(action) * 9.81 * np.sqrt(self.maxLatAx * self.maxLatAx - 1) / Util.rss(self.velocity)
ROD = 0
pol_vel = Util.cart2sph(self.velocity)
pol_vel[0] = pol_vel[0] + ROT
self.velocity = Util.sph2cart(pol_vel)
# Apply accel to vel
self.position += self.velocity * self.Ts
self.objective_in_fov = Util.getAngle(self.velocity + self.position, self.position, objective.position) < self.fov / 2
def CLOS(self, t_pos):
Pm = self.position
Pt = t_pos
Vm = self.velocity
theta_t = Util.cart2sph(Pt - Pm)
theta_m = Util.cart2sph(Vm)
angle = Util.getAngle(Vm + Pm, Pm, Pt)
# Max Rate of Turn
# https://dspace.lib.cranfield.ac.uk/bitstream/handle/1826/2912/CHAPTER_8_july27_2.pdf?sequence=5&isAllowed=y
ROT_max_rad = 9.81 * np.sqrt(self.max_lat_ax**2 - 1) / Util.rss(
self.velocity) * self.Ts
theta_m[0] += np.sign(theta_t[0] - theta_m[0]) * Util.absMin(
ROT_max_rad[0], angle)
self.velocity = Util.sph2cart(theta_m.copy())
self.position += self.velocity * self.Ts
def PN(self, t_pos, t_vel):
# TODO - FIX PN
# To find omega:
# omega = (R*Vr)/dot(R,R)
# Vr = Vt - Vm
# Rr = Rt - Rm
Vm = self.velocity
Rm = self.position
Rr = t_pos - Rm
Vr = t_vel - Vm
eq1 = np.cross(Rr, Vr)
eq2 = np.dot(Rr, Rr)
omega = np.divide(eq1, eq2)
# To find accel:
# a = -N*|Vr|*(Vm/|Vm|)*omega
# eq1 = (Vm/|Vm|)
# eq2 = N*|Vr|
# eq3 = cross(eq1*eq2,omega)
n = self.N
eq1 = np.divide(Vm, Util.rss(Vm))
a = n * np.cross(eq1, omega)
a_sph_rad = Util.cart2sph(a)
v_sph_rad = a_sph_rad * self.Ts
tmp_velocity_sph = Util.cart2sph(self.velocity.copy())
# Max Rate of Turn
# https://dspace.lib.cranfield.ac.uk/bitstream/handle/1826/2912/CHAPTER_8_july27_2.pdf?sequence=5&isAllowed=y
ROT_max_rad = 9.81 * np.sqrt(self.max_lat_ax**2 - 1) / Util.rss(
self.velocity) * self.Ts
ROT_max_deg = np.rad2deg(ROT_max_rad)
# if a_sph_deg < ROT_max_deg:
# min(v_sph_rad[0], ROT_max_rad[0])
tmp_velocity_sph[0] += v_sph_rad[0]
tmp_velocity = Util.sph2cart(tmp_velocity_sph)
# else:
# tmp_velocity += ROT_max_deg * self.Ts
self.velocity = tmp_velocity.copy()
self.position += np.sign(a) * self.velocity * self.Ts