417 lines
14 KiB
Python
417 lines
14 KiB
Python
import math
|
|
|
|
from Angle import *
|
|
|
|
epsilon = 1E-05
|
|
|
|
class Vector2:
|
|
def __init__(self, right: float = 0, up: float = 0):
|
|
"""! A new 2-dimensional vector
|
|
@param right The distance in the right direction in meters
|
|
@param up The distance in the upward direction in meters
|
|
"""
|
|
## The right axis of the vector
|
|
self.right: float = right
|
|
## The upward axis of the vector
|
|
self.up: float = 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.right == other.right and
|
|
self.up == other.up
|
|
)
|
|
|
|
def SqrMagnitude(self) -> float:
|
|
"""! The squared vector length
|
|
@return The squared vector length
|
|
@remark The squared length is computationally simpler than the real
|
|
length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
|
|
calculation of the squared root of C.
|
|
"""
|
|
return self.right ** 2 + self.up ** 2
|
|
|
|
def Magnitude(self) -> float:
|
|
"""! The vector length
|
|
@return The vector length
|
|
"""
|
|
return math.sqrt(self.SqrMagnitude())
|
|
|
|
def Normalized(self):
|
|
"""! Convert the vector to a length of 1
|
|
@return The vector normalized to a length of 1
|
|
"""
|
|
length: float = self.Magnitude();
|
|
result = Vector2.zero
|
|
if length > epsilon:
|
|
result = self / length;
|
|
return result
|
|
|
|
def __neg__(self):
|
|
"""! Negate te vector such that it points in the opposite direction
|
|
@return The negated vector
|
|
"""
|
|
return Vector2(-self.right, -self.up)
|
|
|
|
def __sub__(self, other):
|
|
"""! Subtract a vector from this vector
|
|
@param other The vector to subtract from this vector
|
|
@return The result of this subtraction
|
|
"""
|
|
return Vector2(
|
|
self.right - other.right,
|
|
self.up - other.up
|
|
)
|
|
|
|
def __add__(self, other):
|
|
"""! Add a vector to this vector
|
|
@param other The vector to add to this vector
|
|
@return The result of the addition
|
|
"""
|
|
return Vector2(
|
|
self.right + other.right,
|
|
self.up + other.up
|
|
)
|
|
|
|
def Scale(self, scaling):
|
|
"""! Scale the vector using another vector
|
|
@param scaling A vector with the scaling factors
|
|
@return The scaled vector
|
|
@remark Each component of the vector will be multiplied with the
|
|
matching component from the scaling vector.
|
|
"""
|
|
return Vector2(
|
|
self.right * scaling.right,
|
|
self.up * scaling.up
|
|
)
|
|
|
|
def __mul__(self, factor):
|
|
"""! Scale the vector uniformly up
|
|
@param factor The scaling factor
|
|
@return The scaled vector
|
|
@remark Each component of the vector will be multiplied by the same factor.
|
|
"""
|
|
return Vector2(
|
|
self.right * factor,
|
|
self.up * factor
|
|
)
|
|
|
|
def __truediv__(self, factor):
|
|
"""! Scale the vector uniformly down
|
|
@param f The scaling factor
|
|
@return The scaled vector
|
|
@remark Each component of the vector will be divided by the same factor.
|
|
"""
|
|
return Vector2(
|
|
self.right / factor,
|
|
self.up / factor
|
|
)
|
|
|
|
@staticmethod
|
|
def Distance(v1, v2) -> float:
|
|
"""! The distance between two vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The distance between the two vectors
|
|
"""
|
|
return (v1 - v2).Magnitude()
|
|
|
|
@staticmethod
|
|
def Dot(v1, v2) -> float:
|
|
"""! The dot product of two vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The dot product of the two vectors
|
|
"""
|
|
return v1.right * v2.right + v1.up * v2.up
|
|
|
|
@staticmethod
|
|
def Angle(v1, v2) -> Angle:
|
|
"""! The angle between two vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The angle between the two vectors
|
|
@remark This reterns an unsigned angle which is the shortest distance
|
|
between the two vectors. Use Vector3::SignedAngle if a signed angle is
|
|
needed.
|
|
"""
|
|
denominator: float = math.sqrt(v1.SqrMagnitude() * v2.SqrMagnitude())
|
|
if denominator < epsilon:
|
|
return Angle.zero
|
|
|
|
dot: float = Vector2.Dot(v1, v2)
|
|
fraction: float = dot / denominator
|
|
# if math.nan(fraction):
|
|
# return Angle.Degrees(fraction) # short cut to returning NaN universally
|
|
|
|
cdot: float = Float.Clamp(fraction, -1.0, 1.0)
|
|
r: float = math.acos(cdot)
|
|
return Angle.Radians(r);
|
|
|
|
@staticmethod
|
|
def SignedAngle(v1, v2) -> Angle:
|
|
"""! The signed angle between two vectors
|
|
@param v1 The starting vector
|
|
@param v2 The ending vector
|
|
@param axis The axis to rotate around
|
|
@return The signed angle between the two vectors
|
|
"""
|
|
sqr_mag_from: float = v1.SqrMagnitude()
|
|
sqr_mag_to: float = v2.SqrMagnitude()
|
|
|
|
if sqr_mag_from == 0 or sqr_mag_to == 0:
|
|
return Angle.zero
|
|
# if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
|
|
# return nanf("");
|
|
|
|
angle_from = math.atan2(v1.up, v1.right)
|
|
angle_to = math.atan2(v2.up, v2.right)
|
|
return Angle.Radians(-(angle_to - angle_from))
|
|
|
|
@staticmethod
|
|
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
|
|
|
|
|
|
## A vector with zero for all axis
|
|
Vector2.zero = Vector2(0, 0)
|
|
## A vector with one for all axis
|
|
Vector2.one = Vector2(1, 1)
|
|
## A normalized right-oriented vector
|
|
Vector2.right = Vector2(1, 0)
|
|
## A normalized left-oriented vector
|
|
Vector2.left = Vector2(-1, 0)
|
|
## A normalized up-oriented vector
|
|
Vector2.up = Vector2(0, 1)
|
|
## A normalized down-oriented vector
|
|
Vector2.down = Vector2(0, -1)
|
|
|
|
class Vector3(Vector2):
|
|
def __init__(self, right: float = 0, up: float = 0, forward: float = 0):
|
|
"""! A new 3-dimensional vector
|
|
@param right The distance in the right direction in meters
|
|
@param up The distance in the upward direction in meters
|
|
@param forward The distance in the forward direction in meters
|
|
"""
|
|
## The right axis of the vector
|
|
self.right: float = right
|
|
## The upward axis of the vector
|
|
self.up: float = up
|
|
## The forward axis of the vector
|
|
self.forward: float = 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.right == other.right and
|
|
self.up == other.up and
|
|
self.forward == other.forward
|
|
)
|
|
|
|
def SqrMagnitude(self) -> float:
|
|
"""! The squared vector length
|
|
@return The squared vector length
|
|
@remark The squared length is computationally simpler than the real
|
|
length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
|
|
calculation of the squared root of C.
|
|
"""
|
|
return self.right ** 2 + self.up ** 2 + self.forward ** 2
|
|
|
|
def Normalized(self):
|
|
"""! Convert the vector to a length of 1
|
|
@return The vector normalized to a length of 1
|
|
"""
|
|
length: float = self.Magnitude();
|
|
result = Vector3()
|
|
if length > epsilon:
|
|
result = self / length;
|
|
return result
|
|
|
|
def __neg__(self):
|
|
"""! Negate te vector such that it points in the opposite direction
|
|
@return The negated vector
|
|
"""
|
|
return Vector3(-self.right, -self.up, -self.forward)
|
|
|
|
def __sub__(self, other):
|
|
"""! Subtract a vector from this vector
|
|
@param other The vector to subtract from this vector
|
|
@return The result of this subtraction
|
|
"""
|
|
return Vector3(
|
|
self.right - other.right,
|
|
self.up - other.up,
|
|
self.forward - other.forward
|
|
)
|
|
|
|
def __add__(self, other):
|
|
"""! Add a vector to this vector
|
|
@param other The vector to add to this vector
|
|
@return The result of the addition
|
|
"""
|
|
return Vector3(
|
|
self.right + other.right,
|
|
self.up + other.up,
|
|
self.forward + other.forward
|
|
)
|
|
|
|
def Scale(self, scaling):
|
|
"""! Scale the vector using another vector
|
|
@param scaling A vector with the scaling factors
|
|
@return The scaled vector
|
|
@remark Each component of the vector will be multiplied with the
|
|
matching component from the scaling vector.
|
|
"""
|
|
return Vector3(
|
|
self.right * scaling.right,
|
|
self.up * scaling.up,
|
|
self.forward * scaling.forward
|
|
)
|
|
|
|
def __mul__(self, factor):
|
|
"""! Scale the vector uniformly up
|
|
@param factor The scaling factor
|
|
@return The scaled vector
|
|
@remark Each component of the vector will be multiplied by the same factor.
|
|
"""
|
|
return Vector3(
|
|
self.right * factor,
|
|
self.up * factor,
|
|
self.forward * factor
|
|
)
|
|
|
|
def __truediv__(self, factor):
|
|
"""! Scale the vector uniformly down
|
|
@param f The scaling factor
|
|
@return The scaled vector
|
|
@remark Each component of the vector will be divided by the same factor.
|
|
"""
|
|
return Vector3(
|
|
self.right / factor,
|
|
self.up / factor,
|
|
self.forward / factor
|
|
)
|
|
|
|
@staticmethod
|
|
def Dot(v1, v2) -> float:
|
|
"""! The dot product of two vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The dot product of the two vectors
|
|
"""
|
|
return v1.right * v2.right + v1.up * v2.up + v1.forward * v2.forward
|
|
|
|
@staticmethod
|
|
def Cross(v1, v2):
|
|
"""! The cross product of two vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The cross product of the two vectors
|
|
"""
|
|
return Vector3(
|
|
v1.up * v2.forward - v1.forward * v2.up,
|
|
v1.forward * v2.right - v1.right * v2.forward,
|
|
v1.right * v2.up - v1.up * v2.right
|
|
)
|
|
|
|
def Project(self, other):
|
|
"""! Project the vector on another vector
|
|
@param other The normal vecto to project on
|
|
@return The projected vector
|
|
"""
|
|
sqrMagnitude = other.SqrMagnitude()
|
|
if sqrMagnitude < epsilon:
|
|
return Vector3.zero
|
|
else:
|
|
dot = Vector3.Dot(self, other)
|
|
return other * dot / sqrMagnitude;
|
|
|
|
def ProjectOnPlane(self, normal):
|
|
"""! Project the vector on a plane defined by a normal orthogonal to the
|
|
plane.
|
|
@param normal The normal of the plane to project on
|
|
@return Teh projected vector
|
|
"""
|
|
return self - self.Project(normal)
|
|
|
|
@staticmethod
|
|
def Angle(v1, v2) -> Angle:
|
|
"""! The angle between two vectors
|
|
@param v1 The first vector
|
|
@param v2 The second vector
|
|
@return The angle between the two vectors
|
|
@remark This reterns an unsigned angle which is the shortest distance
|
|
between the two vectors. Use Vector3::SignedAngle if a signed angle is
|
|
needed.
|
|
"""
|
|
denominator: float = math.sqrt(v1.SqrMagnitude() * v2.SqrMagnitude())
|
|
if denominator < epsilon:
|
|
return Angle.zero
|
|
|
|
dot: float = Vector3.Dot(v1, v2)
|
|
fraction: float = dot / denominator
|
|
if math.isnan(fraction):
|
|
return Angle.Degrees(fraction) # short cut to returning NaN universally
|
|
|
|
cdot: float = Float.Clamp(fraction, -1.0, 1.0)
|
|
r: float = math.acos(cdot)
|
|
return Angle.Radians(r);
|
|
|
|
@staticmethod
|
|
def SignedAngle(v1, v2, axis) -> Angle:
|
|
"""! The signed angle between two vectors
|
|
@param v1 The starting vector
|
|
@param v2 The ending vector
|
|
@param axis The axis to rotate around
|
|
@return The signed angle between the two vectors
|
|
"""
|
|
# angle in [0,180]
|
|
angle: Angle = Vector3.Angle(v1, v2)
|
|
|
|
cross: Vector3 = Vector3.Cross(v1, v2)
|
|
b: float = Vector3.Dot(axis, cross)
|
|
sign:int = 0
|
|
if b < 0:
|
|
sign = -1
|
|
elif b > 0:
|
|
sign = 1
|
|
|
|
# angle in [-179,180]
|
|
return angle * sign
|
|
|
|
## A vector with zero for all axis
|
|
Vector3.zero = Vector3(0, 0, 0)
|
|
## A vector with one for all axis
|
|
Vector3.one = Vector3(1, 1, 1)
|
|
## A normalized forward-oriented vector
|
|
Vector3.forward = Vector3(0, 0, 1)
|
|
## A normalized back-oriented vector
|
|
Vector3.back = Vector3(0, 0, -1)
|
|
## A normalized right-oriented vector
|
|
Vector3.right = Vector3(1, 0, 0)
|
|
## A normalized left-oriented vector
|
|
Vector3.left = Vector3(-1, 0, 0)
|
|
## A normalized up-oriented vector
|
|
Vector3.up = Vector3(0, 1, 0)
|
|
## A normalized down-oriented vector
|
|
Vector3.down = Vector3(0, -1, 0)
|