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)