52 lines
1.3 KiB
Python
52 lines
1.3 KiB
Python
import math
|
|
from typing import NamedTuple
|
|
|
|
class Vector(NamedTuple):
|
|
x: float
|
|
y: float
|
|
|
|
current_pos = Vector(3,2)
|
|
target_pos = Vector(7,5)
|
|
zero_vector = Vector(0,0)
|
|
|
|
def main() -> None:
|
|
print("Hello Teodora!")
|
|
|
|
distance = displacementVector(current_pos, target_pos)
|
|
print(distance)
|
|
|
|
lengthOfDistanceVector = length(distance)
|
|
print(lengthOfDistanceVector)
|
|
|
|
direction = safe_normalize(distance, 0.001)
|
|
print(direction)
|
|
|
|
## calculate the displacement from current position to target position
|
|
|
|
|
|
def displacementVector(current_pos, target_pos):
|
|
displacementVector = Vector(target_pos.x - current_pos.x, target_pos.y - current_pos.y)
|
|
return displacementVector
|
|
|
|
## distance between two positions
|
|
# Euclidian norm
|
|
|
|
def length(vector: Vector) -> float:
|
|
squared_x = vector.x*vector.x
|
|
squared_y = vector.y*vector.y
|
|
squared_x + squared_y
|
|
return math.sqrt(squared_x + squared_y)
|
|
|
|
# safe normalisation
|
|
# The minimum-distance check also prevents division by zero.
|
|
|
|
def safe_normalize(vector: Vector, minimum_distance: float) -> Vector:
|
|
vector_length = length(vector)
|
|
|
|
if vector_length < minimum_distance:
|
|
return zero_vector
|
|
|
|
einheitsVector = Vector(vector.x / vector_length, vector.y / vector_length)
|
|
return einheitsVector
|
|
|