444 lines
16 KiB
Python
444 lines
16 KiB
Python
import math
|
|
|
|
from .Direction import *
|
|
from .Vector import *
|
|
|
|
class Polar:
|
|
"""! A polar 2D vector
|
|
"""
|
|
|
|
def __init__(self, distance: float, angle: Angle):
|
|
"""! Create a new polar vector
|
|
@param distance The length of the vector
|
|
@param angle The direction of the angle
|
|
"""
|
|
self.distance: float = distance
|
|
## The length of the vector
|
|
self.direction: Angle = angle
|
|
## The direction of the vector
|
|
|
|
## Normalizing such that distance >= 0
|
|
if self.distance < 0:
|
|
self.distance = -self.distance
|
|
self.direction = self.direction.Inverse()
|
|
elif self.distance == 0:
|
|
self.direction = Angle.zero
|
|
|
|
def Degrees(distance: float, degrees: float):
|
|
"""! Create a polar vector without using the Angle type. All given
|
|
angles are in degrees
|
|
@param distance The distance in meters
|
|
@param horizontal The angle in degrees
|
|
@return The polar vector
|
|
"""
|
|
angle: Angle = Angle.Degrees(degrees)
|
|
r: Polar = Polar(distance, angle)
|
|
return r
|
|
|
|
def Radians(distance: float, radians: float):
|
|
"""! Create polar vector without using the Angle type. All given
|
|
angles are in radians
|
|
@param distance The distance in meters
|
|
@param horizontal The horizontal angle in radians
|
|
@param vertical The vertical angle in radians
|
|
@return The polar vector
|
|
"""
|
|
angle: Angle = Angle.Radians(radians)
|
|
r: Polar = Polar(distance, angle)
|
|
return r
|
|
|
|
@staticmethod
|
|
def FromVector2(v: Vector2):
|
|
"""! Create a polar coordinate from a Vector2 coordinate
|
|
@param v The vector coordinate
|
|
@return The polar coordinate
|
|
"""
|
|
distance = v.Magnitude()
|
|
if distance == 0:
|
|
return Polar(0, Angle.zero)
|
|
|
|
angle = Angle.Radians(math.atan2(v.right, v.up))
|
|
return Polar(distance, angle)
|
|
|
|
def ToVector2(self) -> Vector2:
|
|
"""! Convert the polar coordinate to a Vector2 coordinate
|
|
@return The vector coordinate
|
|
"""
|
|
horizontalRad = self.direction.InRadians()
|
|
|
|
cosHorizontal = math.cos(horizontalRad)
|
|
sinHorizontal = math.sin(horizontalRad)
|
|
|
|
right = self.distance * sinHorizontal
|
|
up = self.distance * cosHorizontal
|
|
return Vector2(right, up)
|
|
|
|
def __eq__(self, other) -> bool:
|
|
"""! Check if this vector is equal to the given vector
|
|
@param v The vector to check against
|
|
@return true if it is identical to the given vector
|
|
@note This uses float comparison to check equality which may have strange
|
|
effects. Equality on floats should be avoided.
|
|
"""
|
|
return (
|
|
self.distance == other.distance and
|
|
self.direction == other.direction
|
|
)
|
|
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
|
|
return (
|
|
math.isclose(self.distance, other.distance, rel_tol=rel_tol, abs_tol=abs_tol) and
|
|
self.direction.isclose(other.direction, rel_tol, abs_tol)
|
|
)
|
|
|
|
|
|
def Magnitude(self) -> float:
|
|
return math.fabs(self.distance)
|
|
|
|
def Normalized(self):
|
|
if self.distance == 0:
|
|
return Polar(0, self.direction)
|
|
|
|
return Polar(1, self.direction)
|
|
|
|
def __neg__(self):
|
|
"""! Negate the vector
|
|
@return The negated vector
|
|
This will negate the direction. Distance will stay the same.
|
|
"""
|
|
return Polar(self.distance, self.direction.Inverse())
|
|
|
|
def __sub__(self, other):
|
|
"""! Subtract a polar vector from this vector
|
|
@param other The vector to subtract
|
|
@return The result of the subtraction
|
|
"""
|
|
# v1 = self.ToVector2()
|
|
# v2 = other.ToVector2()
|
|
# r = v1 - v2
|
|
# return Polar.FromVector2(r)
|
|
r = self + (-other)
|
|
return r
|
|
|
|
def __add__(self, other):
|
|
"""! Add a polar vector to this vector
|
|
@param other The vector to add
|
|
@return The result of the addition
|
|
"""
|
|
# v1 = self.ToVector2()
|
|
# v2 = other.ToVector2()
|
|
# r = v1 - v2
|
|
# return Polar.FromVector2(r)
|
|
|
|
if other.distance == 0:
|
|
return Polar(self.distance, self.direction)
|
|
if self.distance == 0:
|
|
return other
|
|
|
|
deltaAngle: float = (other.direction - self.direction).InDegrees();
|
|
if deltaAngle < 0:
|
|
rotation = 180 + deltaAngle
|
|
else:
|
|
rotation = 180 - deltaAngle
|
|
|
|
if rotation == 180 and other.distance > 0:
|
|
# angle is too small, take this angle and add the distances
|
|
return Polar(self.distance + other.distance, self.direction)
|
|
|
|
newDistance: float = Angle.CosineRuleSide(other.distance, self.distance, Angle.Degrees(rotation))
|
|
|
|
angle: float = Angle.CosineRuleAngle(newDistance, self.distance, other.distance).InDegrees()
|
|
if deltaAngle < 0:
|
|
new_angle: float = self.direction.InDegrees() - angle
|
|
else:
|
|
new_angle: float = self.direction.InDegrees() + angle
|
|
new_angle_a: Angle = Angle.Degrees(new_angle)
|
|
vector = Polar(newDistance, new_angle_a)
|
|
return vector
|
|
|
|
def __mul__(self, factor):
|
|
"""! Scale the vector uniformly up
|
|
@param factor The scaling factor
|
|
@return The scaled vector
|
|
@remark This operation will scale the distance of the vector. The angle
|
|
will be unaffected.
|
|
"""
|
|
return Polar(
|
|
self.distance * factor,
|
|
self.direction
|
|
)
|
|
|
|
def __truediv__(self, factor):
|
|
"""! Scale the vector uniformly down
|
|
@param factor The scaling factor
|
|
@return The scaled vector
|
|
@remark This operation will scale the distance of the vector. The angle
|
|
will be unaffected.
|
|
"""
|
|
return Polar(
|
|
self.distance / factor,
|
|
self.direction
|
|
)
|
|
|
|
@staticmethod
|
|
def Distance(v1, v2) -> float:
|
|
"""! Calculate the distance between two spherical coordinates
|
|
@param v1 The first coordinate
|
|
@param v2 The second coordinate
|
|
@return The distance between the coordinates in meters
|
|
"""
|
|
v1 = v1.ToVector2()
|
|
v2 = v2.ToVector2()
|
|
distance: float = Vector2.Distance(v1, v2)
|
|
return distance
|
|
|
|
@staticmethod
|
|
def Angle(v1, v2) -> Angle:
|
|
"""! Calculate the unsigned angle between two spherical vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The unsigned angle between the vectors [0..179.9999999..., -180]
|
|
@remark the strange range is caused by the 2s complement signed values
|
|
which has range [minvalue..maxvalue). This is a hardware limitation,
|
|
not something we can change.
|
|
"""
|
|
angle: Angle = Angle.Abs(v1.direction - v2.direction)
|
|
return angle
|
|
|
|
@staticmethod
|
|
def SignedAngle(v1, v2) -> Angle:
|
|
"""! Calculate the unsigned angle between two spherical vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The unsigned angle between the vectors [0..179.9999999..., -180]
|
|
@remark the strange range is caused by the 2s complement signed values
|
|
which has range [minvalue..maxvalue). This is a hardware limitation,
|
|
not something we can change.
|
|
"""
|
|
angle: Angle = v2.direction - v1.direction
|
|
return angle
|
|
|
|
def Lerp(v1, v2, f: float):
|
|
"""! Lerp (linear interpolation) between two vectors
|
|
@param v1 The starting vector
|
|
@param v2 The ending vector
|
|
@param f The interpolation distance
|
|
@return The lerped vector
|
|
@remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
|
|
1 matches vector *v2*. Value -1 is vector *v1* minus the difference
|
|
between *v1* and *v2* etc.
|
|
"""
|
|
return v1 + (v2 - v1) * f
|
|
# return v1 * (1 - f) + v2 * f
|
|
|
|
Polar.zero = Polar(0, Angle.zero)
|
|
|
|
class Spherical(Polar):
|
|
"""! A spherical 3D vector
|
|
"""
|
|
|
|
def __init__(self, distance: float, direction: Direction):
|
|
"""! Create a new spherical vector
|
|
@param distance The length of the vector
|
|
@param direction The direction of the vector
|
|
"""
|
|
self.distance: float = distance
|
|
## The length of the vector
|
|
self.direction: Direction = direction
|
|
## The direction of the vector
|
|
|
|
## Normalizing such that distance >= 0
|
|
if self.distance < 0:
|
|
self.distance = -self.distance
|
|
self.direction = -self.direction
|
|
elif self.distance == 0:
|
|
self.direction = Direction.zero
|
|
|
|
def Degrees(distance: float, horizontal: float, vertical: float):
|
|
"""! Create sperical vector without using the Direction type. All given
|
|
angles are in degrees
|
|
@param distance The distance in meters
|
|
@param horizontal The horizontal angle in degrees
|
|
@param vertical The vertical angle in degrees
|
|
@return The spherical vector
|
|
"""
|
|
direction: Direction = Direction.Degrees(horizontal, vertical)
|
|
r: Spherical = Spherical(distance, direction)
|
|
return r
|
|
|
|
def Radians(distance: float, horizontal: float, vertical: float):
|
|
"""! Create sperical vector without using the Direction type. All given
|
|
angles are in radians
|
|
@param distance The distance in meters
|
|
@param horizontal The horizontal angle in radians
|
|
@param vertical The vertical angle in radians
|
|
@return The spherical vector
|
|
"""
|
|
direction: Direction = Direction.Radians(horizontal, vertical)
|
|
r: Spherical = Spherical(distance, direction)
|
|
return r
|
|
|
|
@staticmethod
|
|
def FromVector3(v: Vector3):
|
|
"""! Create a Spherical coordinate from a Vector3 coordinate
|
|
@param v The vector coordinate
|
|
@return The spherical coordinate
|
|
"""
|
|
distance = v.Magnitude()
|
|
if distance == 0:
|
|
return Spherical(0, Angle(), Angle())
|
|
|
|
verticalAngle = Angle.Radians((math.pi / 2 - math.acos(v.up / distance)))
|
|
horizontalAngle = Angle.Radians(math.atan2(v.right, v.forward))
|
|
return Spherical(distance, Direction(horizontalAngle, verticalAngle))
|
|
|
|
def ToVector3(self) -> Vector3:
|
|
"""! Convert the spherical coordinate to a Vector3 coordinate
|
|
@return The vector coordinate
|
|
"""
|
|
verticalRad = (math.pi / 2) - self.direction.vertical.InRadians()
|
|
horizontalRad = self.direction.horizontal.InRadians()
|
|
|
|
cosVertical = math.cos(verticalRad)
|
|
sinVertical = math.sin(verticalRad)
|
|
cosHorizontal = math.cos(horizontalRad)
|
|
sinHorizontal = math.sin(horizontalRad)
|
|
|
|
right = self.distance * sinVertical * sinHorizontal
|
|
up = self.distance * cosVertical
|
|
forward = self.distance * sinVertical * cosHorizontal
|
|
return Vector3(right, up, forward)
|
|
|
|
def __eq__(self, other) -> bool:
|
|
"""! Check if this vector is equal to the given vector
|
|
@param v The vector to check against
|
|
@return true if it is identical to the given vector
|
|
@note This uses float comparison to check equality which may have strange
|
|
effects. Equality on floats should be avoided.
|
|
"""
|
|
return (
|
|
self.distance == other.distance and
|
|
self.direction == other.direction
|
|
)
|
|
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
|
|
return (
|
|
math.isclose(self.distance, other.distance, rel_tol=rel_tol, abs_tol=abs_tol) and
|
|
self.direction.isclose(other.direction, rel_tol, abs_tol)
|
|
)
|
|
|
|
def Normalized(self) -> float:
|
|
if self.distance == 0:
|
|
return Spherical(0, self.direction)
|
|
|
|
return Spherical(1, self.direction)
|
|
|
|
def __neg__(self):
|
|
"""! Negate the vector
|
|
@return The negated vector
|
|
This will negate the direction. Distance will stay the same.
|
|
"""
|
|
return Spherical(self.distance, self.direction.Inverse())
|
|
|
|
def __sub__(self, other):
|
|
"""! Subtract a spherical vector from this vector
|
|
@param other The vector to subtract
|
|
@return The result of the subtraction
|
|
"""
|
|
v1 = self.ToVector3()
|
|
v2 = other.ToVector3()
|
|
r = v1 - v2
|
|
return Spherical.FromVector3(r)
|
|
|
|
def __add__(self, other):
|
|
"""! Add a spherical vector to this vector
|
|
@param other The vector to add
|
|
@return The result of the addition
|
|
"""
|
|
v1 = self.ToVector3()
|
|
v2 = other.ToVector3()
|
|
r = v1 + v2
|
|
return Spherical.FromVector3(r)
|
|
|
|
def __mul__(self, factor):
|
|
"""! Scale the vector uniformly up
|
|
@param factor The scaling factor
|
|
@return The scaled vector
|
|
@remark This operation will scale the distance of the vector. The angle
|
|
will be unaffected.
|
|
"""
|
|
return Spherical(
|
|
self.distance * factor,
|
|
self.direction
|
|
)
|
|
|
|
def __truediv__(self, factor):
|
|
"""! Scale the vector uniformly down
|
|
@param factor The scaling factor
|
|
@return The scaled vector
|
|
@remark This operation will scale the distance of the vector. The angle
|
|
will be unaffected.
|
|
"""
|
|
return Spherical(
|
|
self.distance / factor,
|
|
self.direction
|
|
)
|
|
|
|
@staticmethod
|
|
def Distance(v1, v2) -> float:
|
|
"""! Calculate the distance between two spherical coordinates
|
|
@param s1 The first coordinate
|
|
@param s2 The second coordinate
|
|
@return The distance between the coordinates in meters
|
|
"""
|
|
v1: Vector3 = v1.ToVector3()
|
|
v2: Vector3 = v2.ToVector3()
|
|
distance: float = Vector3.Distance(v1, v2)
|
|
return distance
|
|
|
|
@staticmethod
|
|
def Angle(s1, s2) -> Angle:
|
|
"""! Calculate the unsigned angle between two spherical vectors
|
|
@param s1 The first vector
|
|
@param s2 The second vector
|
|
@return The unsigned angle between the vectors [0..179.9999999..., -180]
|
|
@remark the strange range is caused by the 2s complement signed values
|
|
which has range [minvalue..maxvalue). This is a hardware limitation,
|
|
not something we can change.
|
|
"""
|
|
v1: Vector3 = s1.ToVector3()
|
|
v2: Vector3 = s2.ToVector3()
|
|
angle: Angle = Vector3.Angle(v1, v2)
|
|
return angle
|
|
|
|
def SignedAngle(s1, s2, axis) -> Angle:
|
|
"""! Calculate the signed angle between two spherical vectors
|
|
@param s1 The first vector
|
|
@param s2 The second vector
|
|
@param axis The axis around which the angle is calculated
|
|
@return The signed angle between the vectors
|
|
"""
|
|
v1 = s1.ToVector3()
|
|
v2 = s2.ToVector3()
|
|
v_axis = axis.ToVector3()
|
|
angle = Vector3.SignedAngle(v1, v2, v_axis)
|
|
return angle
|
|
|
|
@staticmethod
|
|
def Rotate(s, horizontal: Angle, vertical: Angle):
|
|
"""! Rotate a spherical vector
|
|
@param s The vector to rotate
|
|
@param horizontal The horizontal rotation angle in local space
|
|
@param vertical The vertical rotation angle in local space
|
|
@return The rotated vector
|
|
"""
|
|
direction: Direction = Direction(
|
|
s.direction.horizontal + horizontal,
|
|
s.direction.vertical + vertical
|
|
)
|
|
r = Spherical(s.distance, direction)
|
|
return r
|
|
|
|
def __repr__(self):
|
|
return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})"
|
|
|
|
Spherical.zero = Spherical(0, Direction.zero)
|