Merge commit 'a11338ac1dcf1d2b1a83c353e0d8651ddc36a795'
This commit is contained in:
commit
8efcb35132
@ -1,13 +1,272 @@
|
||||
import math
|
||||
# This Source Code Form is subject to the terms of the Mozilla Public
|
||||
# License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
# file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
import math
|
||||
from Float import *
|
||||
|
||||
# This is in fact AngleSingle
|
||||
class Angle:
|
||||
# The angle is internally limited to (-180..180] degrees or (-PI...PI]
|
||||
# radians. When an angle exceeds this range, it is normalized to a value
|
||||
# within the range.
|
||||
Rad2Deg = 360 / (math.pi * 2)
|
||||
Deg2Rad = (math.pi * 2) / 360
|
||||
|
||||
def __init__(self, degrees = 0):
|
||||
self.value: float = degrees
|
||||
Angle.Normalize(self)
|
||||
|
||||
@staticmethod
|
||||
def Degrees(degrees):
|
||||
angle = Angle(degrees)
|
||||
return angle
|
||||
|
||||
@staticmethod
|
||||
def Radians(radians):
|
||||
angle = Angle(radians * Angle.Rad2Deg)
|
||||
return angle;
|
||||
|
||||
def InDegrees(self):
|
||||
return self.value;
|
||||
def InRadians(self) -> float:
|
||||
return self.value * Angle.Deg2Rad;
|
||||
|
||||
def __eq__(self, angle):
|
||||
"""! Tests whether this angle is equal to the given angle
|
||||
@param angle The angle to compare to
|
||||
@return True when the angles are equal, False otherwise
|
||||
@note The equality is determine within the limits of precision of the raw
|
||||
type T
|
||||
"""
|
||||
return self.value == angle.value
|
||||
def __gt__(self, angle):
|
||||
"""! Tests if this angle is greater than the given angle
|
||||
@param angle The given angle
|
||||
@return True when this angle is greater than the given angle, False
|
||||
otherwise
|
||||
"""
|
||||
return self.value > angle.value
|
||||
def __gte__(self, angle):
|
||||
"""! Tests if this angle is greater than or equal to the given angle
|
||||
@param angle The given angle
|
||||
@return True when this angle is greater than or equal to the given angle.
|
||||
False otherwise.
|
||||
"""
|
||||
return self.value >= angle.value
|
||||
def __lt__(self, angle):
|
||||
"""! Tests if this angle is less than the given angle
|
||||
@param angle The given angle
|
||||
@return True when this angle is less than the given angle, False
|
||||
otherwise
|
||||
"""
|
||||
return self.value < angle.value
|
||||
def __lte__(self, angle):
|
||||
"""! Tests if this angle is less than or equal to the given angle
|
||||
@param angle The given angle
|
||||
@return True when this angle is less than or equal to the given angle.
|
||||
False otherwise.
|
||||
"""
|
||||
return self.value <= angle.value
|
||||
|
||||
def Sign(self):
|
||||
"""! Returns the sign of the angle
|
||||
@param angle The angle
|
||||
@return -1 when the angle is negative, 1 when it is positive and 0
|
||||
otherwise.
|
||||
"""
|
||||
if self.value < 0:
|
||||
return -1
|
||||
if self.value > 0:
|
||||
return 1
|
||||
return 0
|
||||
def Abs(self):
|
||||
"""! Returns the magnitude of the angle
|
||||
@param angle The angle
|
||||
@return The positive magitude of the angle.
|
||||
Negative values are negated to get a positive result
|
||||
"""
|
||||
if self.value < 0:
|
||||
return -self
|
||||
return self
|
||||
|
||||
def __neg__(self):
|
||||
"""! Negate the angle
|
||||
@return The negated angle
|
||||
"""
|
||||
return Angle(-self.value)
|
||||
def Inverse(self):
|
||||
"""! Invert the angle: rotate by 180 degrees
|
||||
"""
|
||||
return self + Angle.Degrees(180)
|
||||
|
||||
def __sub__(self, other):
|
||||
"""! Substract another angle from this angle
|
||||
@param angle The angle to subtract from this angle
|
||||
@return The result of the subtraction
|
||||
"""
|
||||
return Angle(self.value - other.value)
|
||||
def __add__(self, other):
|
||||
"""! Add another angle from this angle
|
||||
@param angle The angle to add to this angle
|
||||
@return The result of the addition
|
||||
"""
|
||||
return Angle(self.value + other.value)
|
||||
def __mul__(self, factor):
|
||||
"""! Multiplies the angle by a factor
|
||||
@param angle The angle to multiply
|
||||
@param factor The factor by which the angle is multiplied
|
||||
@return The multiplied angle
|
||||
"""
|
||||
return Angle(self.value * factor)
|
||||
def __truediv__(self, factor):
|
||||
"""! Divides the angle by a factor
|
||||
@param angle The angle to devide
|
||||
@param factor The factor by which the angle is devided
|
||||
@return The devided angle
|
||||
"""
|
||||
return Angle(self.value / factor)
|
||||
|
||||
@staticmethod
|
||||
def Normalize(angle):
|
||||
while angle < -180:
|
||||
angle += 360
|
||||
while angle >= 180:
|
||||
angle -= 360
|
||||
return angle
|
||||
"""! Normalizes the angle to (-180..180] or (-PI..PI]
|
||||
@note Should not be needed but available in case it is.
|
||||
"""
|
||||
while angle.value < -180:
|
||||
angle.value += 360
|
||||
while angle.value >= 180:
|
||||
angle.value -= 360
|
||||
return angle
|
||||
|
||||
@staticmethod
|
||||
def Clamp(angle, min, max):
|
||||
"""! Clamps the angle value between the two given angles
|
||||
@param angle The angle to clamp
|
||||
@param min The minimum angle
|
||||
@param max The maximum angle
|
||||
@return The clamped value
|
||||
@remark When the min value is greater than the max value, angle is
|
||||
returned unclamped.
|
||||
"""
|
||||
degrees = Float.Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
|
||||
return Angle.Degrees(degrees)
|
||||
|
||||
@staticmethod
|
||||
def MoveTowards(from_angle, to_angle, max_angle):
|
||||
"""! Rotates an angle towards another angle with a max distance
|
||||
@param from_angle The angle to start from
|
||||
@param to_angle The angle to rotate towards
|
||||
@param max_angle The maximum angle to rotate
|
||||
@return The rotated angle
|
||||
"""
|
||||
max_degrees = max(0, max_angle) # filter out negative distances
|
||||
delta_angle = Angle.Abs(to_angle - from_angle)
|
||||
delta_degrees = delta_angle.InDegrees()
|
||||
delta_degrees = Float.Clamp(delta_degrees, 0, max_degrees)
|
||||
if delta_degrees < 0:
|
||||
delta_degrees = -delta_degrees
|
||||
return from_angle + Angle.Degrees(delta_degrees)
|
||||
|
||||
@staticmethod
|
||||
def Cos(angle):
|
||||
"""! Calculates the cosine of an angle
|
||||
@param angle The given angle
|
||||
@return The cosine of the angle
|
||||
"""
|
||||
return math.cos(angle.InRadians())
|
||||
@staticmethod
|
||||
def Sin(angle):
|
||||
"""! Calculates the sine of an angle
|
||||
@param angle The given angle
|
||||
@return The sine of the angle
|
||||
"""
|
||||
return math.sin(angle.InRadians())
|
||||
@staticmethod
|
||||
def Tan(angle):
|
||||
"""! Calculates the tangent of an angle
|
||||
@param angle The given angle
|
||||
@return The tangent of the angle
|
||||
"""
|
||||
return math.tan(angle.InRadians())
|
||||
|
||||
@staticmethod
|
||||
def Acos(f):
|
||||
"""! Calculates the arc cosine angle
|
||||
@param f The value
|
||||
@return The arc cosine for the given value
|
||||
"""
|
||||
return Angle.Radians(math.acos(f))
|
||||
@staticmethod
|
||||
def Asin(f):
|
||||
"""! Calculates the arc sine angle
|
||||
@param f The value
|
||||
@return The arc sine for the given value
|
||||
"""
|
||||
return Angle.Radians(math.asin(f))
|
||||
@staticmethod
|
||||
def Atan(f):
|
||||
"""! Calculates the arc tangent angle
|
||||
@param f The value
|
||||
@return The arc tangent for the given value
|
||||
"""
|
||||
return Angle.Radians(math.atan(f))
|
||||
def Atan2(y, x):
|
||||
"""! Calculates the tangent for the given values
|
||||
@param y The vertical value
|
||||
@param x The horizontal value
|
||||
@return The tanget for the given values
|
||||
Uses the y and x signs to compute the quadrant
|
||||
"""
|
||||
return Angle.Radians(math.atan2(y, x))
|
||||
|
||||
@staticmethod
|
||||
def CosineRuleSide(a, b, gamma):
|
||||
"""! Computes the length of a side of a triangle using the rule of cosines
|
||||
@param a The length of side A
|
||||
@param b The length of side B
|
||||
@param gamma The angle of the corner opposing side C
|
||||
@return The length of side C
|
||||
"""
|
||||
a2: float = a * a
|
||||
b2: float = b * b
|
||||
d: float = a2 + b2 - 2 * a * b * Angle.Cos(gamma)
|
||||
# Catch edge cases where float inaccuracies lead tot NaNs
|
||||
if d < 0:
|
||||
return 0
|
||||
|
||||
c: float = math.sqrt(d)
|
||||
return c
|
||||
@staticmethod
|
||||
def CosineRuleAngle(a, b, c):
|
||||
"""! Computes the angle of a corner of a triangle using the rule of cosines
|
||||
@param a The length of side A
|
||||
@param b The length of side B
|
||||
@param c The length of side C
|
||||
@return The angle of the corner opposing side C
|
||||
"""
|
||||
a2: float = a * a
|
||||
b2: float = b * b
|
||||
c2: float = c * c
|
||||
d: float = (a2 + b2 - c2) / (2 * a * b);
|
||||
# Catch edge cases where float inaccuracies lead tot NaNs
|
||||
if d >= 1:
|
||||
return Angle()
|
||||
if d <= -1:
|
||||
return Angle.Degrees(180)
|
||||
gamma: Angle = Angle.Acos(d)
|
||||
return gamma;
|
||||
|
||||
@staticmethod
|
||||
def SineRuleAngle(a, beta, c):
|
||||
"""! Computes the angle of a triangle corner using the rule of sines
|
||||
@param a The length of side A
|
||||
@param beta the angle of the corner opposing side B
|
||||
@param c The length of side C
|
||||
@return The angle of the corner opposing side A
|
||||
"""
|
||||
alpha:Angle = Angle.Asin(a * Angle.Sin(beta) / c);
|
||||
return alpha;
|
||||
|
||||
|
||||
Angle.zero = Angle(0)
|
||||
## An zero value angle
|
||||
|
||||
@ -1,52 +1,109 @@
|
||||
import math
|
||||
from LinearAlgebra.Angle import Angle
|
||||
from Angle import Angle
|
||||
from Vector import Vector3
|
||||
|
||||
class Direction:
|
||||
def __init__(self, horizontal=0, vertical=0):
|
||||
self.horizontal: float = Angle.Normalize(horizontal)
|
||||
self.vertical: float = Angle.Normalize(vertical)
|
||||
"""! A direction using angles
|
||||
|
||||
* The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
|
||||
degrees which is a rotation in the horizontal plane
|
||||
* A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
|
||||
which is the rotation in the up/down direction applied after the horizontal
|
||||
rotation has been applied.
|
||||
The angles are automatically normalized to stay within the abovenmentioned
|
||||
ranges.
|
||||
"""
|
||||
def __init__(self, horizontal=Angle(), vertical=Angle()):
|
||||
"""! Create a new direction
|
||||
@param horizontal The horizontal angle
|
||||
@param vertical The vertical angle.
|
||||
"""
|
||||
## horizontal angle, range in degrees = (-180..180]
|
||||
self.horizontal: Angle = horizontal
|
||||
## vertical angle, range in degrees = (-90..90]
|
||||
self.vertical: Angle = vertical
|
||||
|
||||
self.Normalize()
|
||||
|
||||
@staticmethod
|
||||
def Degrees(horizontal: float, vertical: float):
|
||||
direction = Direction (horizontal, vertical)
|
||||
"""! Create a direction using angle values in degrees
|
||||
@param horizontal The horizontal angle in degrees
|
||||
@param vertical The vertical angle in degrees
|
||||
@return The direction
|
||||
"""
|
||||
direction = Direction(Angle.Degrees(horizontal), Angle.Degrees(vertical))
|
||||
return direction
|
||||
@staticmethod
|
||||
def Radians(horizontal: float, vertical: float):
|
||||
"""! Create a direction using angle values in radians
|
||||
@param horizontal The horizontal angle in radians
|
||||
@param vertical The vertical angle in radians
|
||||
@return The direction
|
||||
"""
|
||||
direction = Direction(Angle.Radians(horizontal), Angle.Radians(vertical))
|
||||
return direction
|
||||
|
||||
def __add__(self, other):
|
||||
return Direction(self.horizontal + other.x, self.vertical + other.y)
|
||||
def FromVector3(v: Vector3):
|
||||
d = Direction(
|
||||
horizontal = Angle.Atan2(v.right, v.forward),
|
||||
vertical = Angle.Degrees(-90) - Angle.Acos(v.up)
|
||||
)
|
||||
return d;
|
||||
def ToVector3(self) -> Vector3:
|
||||
"""! Convert the direction to a Vector3 coordinate
|
||||
@return The vector coordinate
|
||||
"""
|
||||
verticalRad = (math.pi / 2) - self.vertical.InRadians()
|
||||
horizontalRad = self.horizontal.InRadians()
|
||||
|
||||
cosVertical = math.cos(verticalRad)
|
||||
sinVertical = math.sin(verticalRad)
|
||||
cosHorizontal = math.cos(horizontalRad)
|
||||
sinHorizontal = math.sin(horizontalRad)
|
||||
|
||||
right = sinVertical * sinHorizontal
|
||||
up = cosVertical
|
||||
forward = sinVertical * cosHorizontal
|
||||
return Vector3(right, up, forward)
|
||||
|
||||
def __eq__(self, direction):
|
||||
"""! Test whether this direction is equal to another direction
|
||||
@param direction The direction to compare to
|
||||
@return True when the direction angles are equal, false otherwise.
|
||||
"""
|
||||
return (self.horizontal == direction.horizontal and
|
||||
self.vertical == direction.vertical)
|
||||
|
||||
def __neg__(self):
|
||||
return Direction(self.horizontal + 180, -self.vertical)
|
||||
|
||||
def __sub__(self, other):
|
||||
return Direction(self.horizontal - other.x, self.vertical - other.y)
|
||||
|
||||
def __mul__(self, scalar):
|
||||
return Direction(self.horizontal * scalar, self.vertical * scalar)
|
||||
|
||||
def __truediv__(self, scalar):
|
||||
if scalar != 0:
|
||||
return Direction(self.horizontal / scalar, self.vertical / scalar)
|
||||
else:
|
||||
raise ValueError("Cannot divide by zero")
|
||||
|
||||
def Magnitude(self):
|
||||
return math.sqrt(self.horizontal**2 + self.vertical**2)
|
||||
"""! Negate/reverse the direction
|
||||
@return The reversed direction.
|
||||
"""
|
||||
h: Angle = self.horizontal + Angle.Degrees(-180)
|
||||
v: Angle = -self.vertical
|
||||
return Direction(h, v)
|
||||
def Inverse(self):
|
||||
"""! This is a synonym for negation
|
||||
"""
|
||||
return -self
|
||||
|
||||
def Normalize(self):
|
||||
if self.vertical > 90 or self.vertical < -90:
|
||||
self.horizontal += 180
|
||||
self.verical = 180 - self.verical
|
||||
|
||||
def Dot(self, other):
|
||||
return self.horizontal * other.x + self.vertical * other.y
|
||||
"""! Normalize this vector to the specified ranges
|
||||
@note Should not be needed but available in case it is.
|
||||
"""
|
||||
v = self.vertical.InDegrees()
|
||||
deg180 = Angle.Degrees(-180)
|
||||
if v > 90 or v < -90:
|
||||
self.horizontal += deg180
|
||||
self.vertical = deg180 - self.vertical
|
||||
|
||||
def __repr__(self):
|
||||
return f"Direction(x={self.horizontal}, y={self.vertical})"
|
||||
|
||||
Direction.forward = Direction(0, 0)
|
||||
Direction.backward = Direction(-180, 0)
|
||||
Direction.up = Direction(0, 90)
|
||||
Direction.down = Direction(0, -90)
|
||||
Direction.left = Direction(-90, 0)
|
||||
Direction.right = Direction(90, 0)
|
||||
|
||||
Direction.zero = Direction.Degrees(0, 0)
|
||||
Direction.forward = Direction.Degrees(0, 0)
|
||||
Direction.backward = Direction.Degrees(-180, 0)
|
||||
Direction.up = Direction.Degrees(0, 90)
|
||||
Direction.down = Direction.Degrees(0, -90)
|
||||
Direction.left = Direction.Degrees(-90, 0)
|
||||
Direction.right = Direction.Degrees(90, 0)
|
||||
13
LinearAlgebra/Float.py
Normal file
13
LinearAlgebra/Float.py
Normal file
@ -0,0 +1,13 @@
|
||||
# This Source Code Form is subject to the terms of the Mozilla Public
|
||||
# License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
# file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
class Float:
|
||||
def Clamp(f, min, max):
|
||||
if max < min:
|
||||
return f
|
||||
if f < min:
|
||||
return min
|
||||
if f > max:
|
||||
return max
|
||||
return f
|
||||
@ -1,30 +1,36 @@
|
||||
import math
|
||||
|
||||
from Vector import Vector3
|
||||
from Angle import Angle
|
||||
from Direction import Direction
|
||||
|
||||
Deg2Rad = (math.pi * 2) / 360
|
||||
|
||||
class Quaternion:
|
||||
|
||||
def __init__(self):
|
||||
self.x = 0
|
||||
self.y = 0
|
||||
self.z = 0
|
||||
self.w = 1
|
||||
|
||||
@staticmethod
|
||||
def Euler(x, y, z):
|
||||
yaw = x * Deg2Rad
|
||||
pitch = y * Deg2Rad
|
||||
roll = z * Deg2Rad
|
||||
def __init__(self, x: float = 0, y: float = 0, z: float = 0, w: float = 1):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
self.w = w
|
||||
|
||||
roll_over_2 = roll * 0.5
|
||||
magnitude: float = self.Magnitude()
|
||||
self.x /= magnitude
|
||||
self.y /= magnitude
|
||||
self.z /= magnitude
|
||||
self.w /= magnitude
|
||||
|
||||
@staticmethod
|
||||
def FromAngles(yaw: Angle, pitch: Angle, roll: Angle):
|
||||
roll_over_2 = roll.InRadians() * 0.5
|
||||
sin_roll_over_2 = math.sin(roll_over_2)
|
||||
cos_roll_over_2 = math.cos(roll_over_2)
|
||||
|
||||
pitch_over_2 = pitch * 0.5
|
||||
pitch_over_2 = pitch.InRadians() * 0.5
|
||||
sin_pitch_over_2 = math.sin(pitch_over_2)
|
||||
cos_pitch_over_2 = math.cos(pitch_over_2)
|
||||
|
||||
yaw_over_2 = yaw * 0.5
|
||||
yaw_over_2 = yaw.InRadians() * 0.5
|
||||
sin_yaw_over_2 = math.sin(yaw_over_2)
|
||||
cos_yaw_over_2 = math.cos(yaw_over_2)
|
||||
|
||||
@ -37,8 +43,154 @@ class Quaternion:
|
||||
sin_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2)
|
||||
result.z = (cos_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2 -
|
||||
sin_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2)
|
||||
|
||||
return result
|
||||
|
||||
def ToAngles(self):
|
||||
test: float = self.x * self.y + self.z * self.w;
|
||||
if test > 0.499: # singularity at north pole
|
||||
return (
|
||||
Angle.zero,
|
||||
Angle.Radians(2 * math.atan2(self.x, self.w)),
|
||||
Angle.Degrees(90)
|
||||
)
|
||||
elif test < -0.499: # singularity at south pole
|
||||
return (
|
||||
Angle.zero,
|
||||
Angle.Radians(-2 * math.atan2(self.x, self.w)),
|
||||
Angle.Degrees(-90)
|
||||
)
|
||||
else:
|
||||
sqx: float = self.x * self.x
|
||||
sqy: float = self.y * self.y
|
||||
sqz: float = self.z * self.z
|
||||
|
||||
yaw = Angle.Radians(math.atan2(2 * self.y * self.w - 2 * self.x * self.z, 1 - 2 * sqy - 2 * sqz))
|
||||
pitch = Angle.Radians(math.atan2(2 * self.x * self.w - 2 * self.y * self.z, 1 - 2 * sqx - 2 * sqz))
|
||||
roll = Angle.Radians(math.asin(2 * test))
|
||||
return (yaw, pitch, roll)
|
||||
|
||||
def Degrees(yaw: float, pitch: float, roll: float):
|
||||
return Quaternion.FromAngles(
|
||||
Angle.Degrees(yaw),
|
||||
Angle.Degrees(pitch),
|
||||
Angle.Degrees(roll)
|
||||
)
|
||||
|
||||
def Radians(yaw: float, pitch: float, roll: float):
|
||||
return Quaternion.FromAngles(
|
||||
Angle.Radians(yaw),
|
||||
Angle.Radians(pitch),
|
||||
Angle.Radians(roll)
|
||||
)
|
||||
|
||||
def FromAngleAxis(angle: Angle, axis: Direction):
|
||||
if axis.SqrMagnitude() == 0:
|
||||
return Quaternion.identity
|
||||
|
||||
result: Quaternion = Quaternion.identity
|
||||
radians = angle.InRadians()
|
||||
radians *= 0.5
|
||||
|
||||
axis2: Vector3 = axis * math.sin(radians);
|
||||
q = Quaternion(
|
||||
axis2.right,
|
||||
axis2.up,
|
||||
axis2.forward,
|
||||
math.cos(radians)
|
||||
)
|
||||
return q
|
||||
|
||||
def ToAngleAxis(self) -> tuple[Angle, Direction]:
|
||||
angle: Angle = Angle.Radians(2 * math.acos(self.w))
|
||||
den: float = math.sqrt(1 - self.w * self.w)
|
||||
if den > 0.0001:
|
||||
axis = Direction.FromVector3(self.Axis() / den)
|
||||
else:
|
||||
# This occurs when the angle is zero.
|
||||
# Not a problem: just set an arbitrary normalized axis.
|
||||
axis = Direction.right
|
||||
return (angle, axis)
|
||||
|
||||
def __eq__(self, other) -> bool:
|
||||
return (
|
||||
self.x == other.x and
|
||||
self.y == other.y and
|
||||
self.z == other.z and
|
||||
self.w == other.w
|
||||
)
|
||||
|
||||
def SqrMagnitude(self) -> float:
|
||||
return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
|
||||
|
||||
def Magnitude(self) -> float:
|
||||
"""! The vector length
|
||||
@return The vector length
|
||||
"""
|
||||
return math.sqrt(self.SqrMagnitude())
|
||||
|
||||
def __mul__(self, other):
|
||||
if isinstance(other, Quaternion):
|
||||
return self.MultQuaternion(other)
|
||||
elif isinstance(other, Vector3):
|
||||
return self.MultVector(other)
|
||||
|
||||
def MultQuaternion(self, q):
|
||||
return Quaternion(
|
||||
self.x * q.w + self.y * q.z - self.z * q.y + self.w * q.x,
|
||||
-self.x * q.z + self.y * q.w + self.z * q.x + self.w * q.y,
|
||||
self.x * q.y - self.y * q.x + self.z * q.w + self.w * q.z,
|
||||
-self.x * q.x - self.y * q.y - self.z * q.z + self.w * q.w
|
||||
)
|
||||
|
||||
def MultVector(self, v: Vector3):
|
||||
num = self.x * 2
|
||||
num2 = self.y * 2
|
||||
num3 = self.z * 2
|
||||
num4 = self.x * num
|
||||
num5 = self.y * num2
|
||||
num6 = self.z * num3
|
||||
num7 = self.x * num2
|
||||
num8 = self.x * num3
|
||||
num9 = self.y * num3
|
||||
num10 = self.w * num
|
||||
num11 = self.w * num2
|
||||
num12 = self.w * num3
|
||||
|
||||
px = v.right
|
||||
py = v.up
|
||||
pz = v.forward
|
||||
rx = (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz
|
||||
ry = (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz
|
||||
rz = (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz
|
||||
result = Vector3(rx, ry, rz)
|
||||
return result
|
||||
|
||||
def Dot(q1, q2) -> float:
|
||||
return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w
|
||||
|
||||
def Angle(q1, q2):
|
||||
f: float = Quaternion.Dot(q1, q2)
|
||||
angle = Angle.Radians(math.acos(min(math.fabs(f), 1)) * 2)
|
||||
return angle
|
||||
# return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
|
||||
|
||||
def AngleAround(self, axis: Direction):
|
||||
secondaryRotation: Quaternion = self.RotationAround(axis);
|
||||
(rotationAngle, rotationAxis) = secondaryRotation.ToAngleAxis();
|
||||
|
||||
# Do the axis point in opposite directions?
|
||||
if Vector3.Dot(axis.ToVector3(), rotationAxis.ToVector3()) < 0:
|
||||
return -rotationAngle;
|
||||
|
||||
return rotationAngle;
|
||||
|
||||
def RotationAround(self, axis: Direction):
|
||||
ra = Vector3(self.x, self.y, self.z) # rotation axis
|
||||
p: Vector3 = Vector3.Project(ra, axis.ToVector3()) # return projection ra on to axis (parallel component)
|
||||
twist: Quaternion = Quaternion(p.right, p.up, p.forward, self.w)
|
||||
return twist;
|
||||
|
||||
def Axis(self) -> Vector3:
|
||||
return Vector3(self.x, self.y, self.z)
|
||||
|
||||
Quaternion.identity = Quaternion()
|
||||
@ -1,41 +1,431 @@
|
||||
import math
|
||||
from LinearAlgebra.Direction import Direction
|
||||
from Direction import *
|
||||
from Vector import *
|
||||
|
||||
class Spherical:
|
||||
def __init__(self, distance, direction):
|
||||
if distance < 0:
|
||||
self.distance = -distance
|
||||
self.direction = -direction
|
||||
else:
|
||||
self.distance: float = distance
|
||||
self.direction: Direction = direction
|
||||
class Polar:
|
||||
"""! A polar 2D vector
|
||||
"""
|
||||
|
||||
# def __init__(self, distance, horizontal, vertical):
|
||||
# self.distance = distance
|
||||
# self.direction = Direction(horizontal, vertical)
|
||||
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
|
||||
|
||||
def to_cartesian(self):
|
||||
x = self.distance * math.sin(self.direction.horizontal) * math.cos(self.direction.vertical)
|
||||
y = self.distance * math.sin(self.direction.horizontal) * math.sin(self.direction.vertical)
|
||||
z = self.distance * math.cos(self.direction.horizontal)
|
||||
return x, y, z
|
||||
## 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 from_cartesian(self, x, y, z):
|
||||
self.distance = math.sqrt(x**2 + y**2 + z**2)
|
||||
self.direction.horizontal = math.acos(z / self.distance)
|
||||
self.direction.vertical = math.atan2(y, x)
|
||||
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 __add__(self, other):
|
||||
x1, y1, z1 = self.to_cartesian()
|
||||
x2, y2, z2 = other.to_cartesian()
|
||||
return Spherical.from_cartesian(x1 + x2, y1 + y2, z1 + z2)
|
||||
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 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):
|
||||
x1, y1, z1 = self.to_cartesian()
|
||||
x2, y2, z2 = other.to_cartesian()
|
||||
return Spherical.from_cartesian(x1 - x2, y1 - y2, z1 - z2)
|
||||
"""! 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 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.forward)
|
||||
Spherical.zero = Spherical(0, Direction.zero)
|
||||
|
||||
@ -1,14 +1,9 @@
|
||||
from LinearAlgebra.Direction import Direction
|
||||
from LinearAlgebra.Quaternion import Quaternion
|
||||
from Direction import *
|
||||
from Quaternion import *
|
||||
|
||||
class SwingTwist:
|
||||
"""A rotation using swing and twist angle components"""
|
||||
def __init__(self, swing: Direction, twist: float):
|
||||
if swing.vertical > 90 or swing.vertical < -90:
|
||||
swing.horizontal += 180
|
||||
swing.vertical = 180 - swing.vertical
|
||||
twist += 180
|
||||
|
||||
def __init__(self, swing: Direction, twist: Angle):
|
||||
## Swing component of the rotation
|
||||
self.swing = swing
|
||||
## The twist component of the rotation
|
||||
@ -16,13 +11,79 @@ class SwingTwist:
|
||||
|
||||
@staticmethod
|
||||
def Degrees(horizontal: float, vertical: float, twist: float):
|
||||
direction = Direction(horizontal, vertical)
|
||||
swing_twist = SwingTwist(direction, twist)
|
||||
horizontal_angle = Angle.Degrees(horizontal)
|
||||
vertical_angle = Angle.Degrees(vertical)
|
||||
twist_angle = Angle.Degrees(twist)
|
||||
|
||||
deg90 = Angle.Degrees(90)
|
||||
deg180 = Angle.Degrees(180)
|
||||
if vertical_angle > deg90 or vertical_angle < -deg90:
|
||||
horizontal_angle += deg180
|
||||
vertical_angle = deg180 - vertical_angle
|
||||
twist_angle += deg180
|
||||
|
||||
direction = Direction(horizontal_angle, vertical_angle)
|
||||
swing_twist = SwingTwist(direction, twist_angle)
|
||||
return swing_twist
|
||||
@staticmethod
|
||||
def Radians(horizontal: float, vertical: float, twist: float):
|
||||
horizontal_angle = Angle.Radians(horizontal)
|
||||
vertical_angle = Angle.Radians(vertical)
|
||||
twist_angle = Angle.Radians(twist)
|
||||
|
||||
deg90 = Angle.Radians(math.pi / 2)
|
||||
deg180 = Angle.Radians(math.pi)
|
||||
if vertical_angle > deg90 or vertical_angle < -deg90:
|
||||
horizontal_angle += deg180
|
||||
vertical_angle = deg180 - vertical_angle
|
||||
twist_angle += deg180
|
||||
|
||||
direction = Direction(horizontal_angle, vertical_angle)
|
||||
swing_twist = SwingTwist(direction, twist_angle)
|
||||
return swing_twist
|
||||
|
||||
def ToQuaternion(self) -> Quaternion:
|
||||
"""Convert the SwingTwist rotation to a Quaternion"""
|
||||
q = Quaternion.Euler(-self.swing.vertical,
|
||||
self.swing.horizontal,
|
||||
self.twist)
|
||||
return q
|
||||
q = Quaternion.FromAngles(
|
||||
-self.swing.vertical,
|
||||
self.swing.horizontal,
|
||||
self.twist
|
||||
)
|
||||
return q
|
||||
@staticmethod
|
||||
def FromQuaternion(q: Quaternion):
|
||||
angles = Quaternion.ToAngles(q)
|
||||
# direction = Direction(angles[0], angles[1])
|
||||
# r: SwingTwist = SwingTwist(direction, angles[2])
|
||||
r = SwingTwist.Degrees(
|
||||
angles[0].InDegrees(),
|
||||
angles[1].InDegrees(),
|
||||
angles[2].InDegrees()
|
||||
)
|
||||
return r
|
||||
|
||||
def FromAngleAxis(angle: Angle, axis: Direction):
|
||||
vectorAxis: Vector3 = axis.ToVector3();
|
||||
q: Quaternion = Quaternion.FromAngleAxis(angle, vectorAxis);
|
||||
return SwingTwist.FromQuaternion(q)
|
||||
|
||||
|
||||
def __eq__(self, other) -> bool:
|
||||
"""! Check if this orientation is equal to the given orientation
|
||||
@param other The orientation to check against
|
||||
@return true if it is identical to the given orientation
|
||||
@note This uses float comparison to check equality which may have strange
|
||||
effects. Equality on floats should be avoided.
|
||||
"""
|
||||
return (
|
||||
self.swing == other.swing and
|
||||
self.twist == other.twist
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def Angle(r1, r2) -> Angle:
|
||||
q1: Quaternion = r1.ToQuaternion()
|
||||
q2: Quaternion = r2.ToQuaternion()
|
||||
angle: float = Quaternion.Angle(q1, q2)
|
||||
return angle
|
||||
|
||||
416
LinearAlgebra/Vector.py
Normal file
416
LinearAlgebra/Vector.py
Normal file
@ -0,0 +1,416 @@
|
||||
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)
|
||||
@ -1,14 +1,144 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
import unittest
|
||||
from Angle import *
|
||||
|
||||
class AngleTest(unittest.TestCase):
|
||||
def test_one(self):
|
||||
pass
|
||||
def test_Construct(self):
|
||||
degrees: float = 0
|
||||
a: Angle = Angle.Degrees(degrees)
|
||||
assert(a.InDegrees() == degrees)
|
||||
|
||||
degrees = -180
|
||||
a = Angle.Degrees(degrees)
|
||||
assert(a.InDegrees() == degrees)
|
||||
|
||||
degrees = 270
|
||||
a = Angle.Degrees(degrees)
|
||||
assert(a.InDegrees() == -90.0)
|
||||
|
||||
def test_Negate(self):
|
||||
angle = 0
|
||||
a:Angle = Angle.Degrees(angle)
|
||||
a = -a
|
||||
assert(a.InDegrees() == angle)
|
||||
|
||||
angle = 90
|
||||
a = Angle.Degrees(angle)
|
||||
a = -a
|
||||
assert(a.InDegrees() == -angle)
|
||||
|
||||
def test_Add(self):
|
||||
a: Angle = Angle.Degrees(-45)
|
||||
b: Angle = Angle.Degrees(45)
|
||||
r: Angle = a + b
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_Subtract(self):
|
||||
a: Angle = Angle.Degrees(0)
|
||||
b: Angle = Angle.Degrees(45)
|
||||
r: Angle = a - b
|
||||
assert(r.InDegrees() == -45)
|
||||
|
||||
def test_Compare(self):
|
||||
a: Angle = Angle.Degrees(45)
|
||||
r: bool = False
|
||||
|
||||
r = a > Angle.Degrees(0)
|
||||
assert(r == True)
|
||||
|
||||
r = a > Angle.Degrees(90)
|
||||
assert(r == False)
|
||||
|
||||
r = a > Angle.Degrees(-90)
|
||||
assert(r == True)
|
||||
|
||||
def test_Normalize(self):
|
||||
r = Angle()
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(90))
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(-90))
|
||||
assert(r.InDegrees() == -90)
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(270))
|
||||
assert(r.InDegrees() == -90)
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(270 + 360))
|
||||
assert(r.InDegrees() == -90)
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(-270));
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(-270 - 360));
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
r = Angle.Normalize(Angle.Degrees(0));
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_Clamp(self):
|
||||
r = Angle()
|
||||
|
||||
r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2))
|
||||
assert(r.InDegrees() == 1)
|
||||
|
||||
r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2))
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2))
|
||||
assert(r.InDegrees() == 2)
|
||||
|
||||
r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0))
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0))
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1))
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_MoveTowards(self):
|
||||
r = Angle();
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30)
|
||||
assert(r.InDegrees() == 30)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
46
LinearAlgebra/test/Direction_test.py
Normal file
46
LinearAlgebra/test/Direction_test.py
Normal file
@ -0,0 +1,46 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from Direction import *
|
||||
|
||||
class DirectionTest(unittest.TestCase):
|
||||
def test_Compare(self):
|
||||
d = Direction.Degrees(45, 135)
|
||||
r = Direction(Angle.Degrees(45), Angle.Degrees(135))
|
||||
assert(d == r)
|
||||
|
||||
r = Direction(Angle.Degrees(45 + 360), Angle.Degrees(135 - 360))
|
||||
assert (d == r)
|
||||
|
||||
def test_Inverse(self):
|
||||
d = Direction.Degrees(45, 135)
|
||||
r = Direction.Degrees(-135, -135)
|
||||
assert(-d == r)
|
||||
|
||||
d = Direction.Degrees(-45, -135)
|
||||
r = Direction.Degrees(135, 135)
|
||||
assert(-d == r)
|
||||
|
||||
d = Direction.Degrees(0, 0)
|
||||
r = Direction.Degrees(180, 0)
|
||||
assert(-d == r)
|
||||
|
||||
d = Direction.Degrees(0, 45)
|
||||
r = Direction.Degrees(180, -45)
|
||||
assert(-d == r)
|
||||
|
||||
def test_Equality(self):
|
||||
d = Direction.Degrees(135, 45)
|
||||
r = Direction.Degrees(135, 45)
|
||||
assert(d == r)
|
||||
r = Direction.Degrees(135 + 360, 45)
|
||||
assert(d == r)
|
||||
r = Direction.Degrees(135 - 360, 45)
|
||||
assert(d == r)
|
||||
|
||||
d = Direction.Degrees(0, 45 + 180);
|
||||
r = Direction.Degrees(180, -45)
|
||||
assert(d == r)
|
||||
27
LinearAlgebra/test/Float_test.py
Normal file
27
LinearAlgebra/test/Float_test.py
Normal file
@ -0,0 +1,27 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from Float import *
|
||||
|
||||
class FloatTest(unittest.TestCase):
|
||||
def test_Clamp(self):
|
||||
r = Float.Clamp(1, 0, 2)
|
||||
assert(r == 1)
|
||||
|
||||
r = Float.Clamp(-1, 0, 2)
|
||||
assert(r == 0)
|
||||
|
||||
r = Float.Clamp(3, 0, 2)
|
||||
assert(r == 2)
|
||||
|
||||
r = Float.Clamp(1, 0, 0)
|
||||
assert(r == 0)
|
||||
|
||||
r = Float.Clamp(0, 0, 0)
|
||||
assert(r == 0)
|
||||
|
||||
r = Float.Clamp(0, 1, -1)
|
||||
assert(r == 0)
|
||||
106
LinearAlgebra/test/Quaternion_test.py
Normal file
106
LinearAlgebra/test/Quaternion_test.py
Normal file
@ -0,0 +1,106 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from Quaternion import *
|
||||
|
||||
class QuaternionTest(unittest.TestCase):
|
||||
def test_Equality(self):
|
||||
q1 = Quaternion.identity
|
||||
q2 = Quaternion(1, 0, 0, 0)
|
||||
|
||||
assert(q1 != q2)
|
||||
|
||||
q2 = Quaternion(0, 0, 0, 1)
|
||||
assert(q1 == q2)
|
||||
|
||||
def test_FromAngles(self):
|
||||
q = Quaternion.FromAngles(Angle.zero, Angle.zero, Angle.zero)
|
||||
assert(q == Quaternion.identity)
|
||||
|
||||
q = Quaternion.FromAngles(Angle.Degrees(90), Angle.Degrees(90), Angle.Degrees(-90))
|
||||
sqrt2_2 = math.sqrt(2) / 2
|
||||
assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
|
||||
|
||||
def test_ToAngles(self):
|
||||
q1 = Quaternion.identity
|
||||
angles = Quaternion.ToAngles(q1)
|
||||
assert(angles == (Angle.zero, Angle.zero, Angle.zero))
|
||||
|
||||
q1 = Quaternion(1, 0, 0, 0)
|
||||
angles = Quaternion.ToAngles(q1)
|
||||
assert(angles == (Angle.zero, Angle.Degrees(180), Angle.zero))
|
||||
|
||||
def test_Degrees(self):
|
||||
q = Quaternion.Degrees(0, 0, 0)
|
||||
assert(q == Quaternion.identity)
|
||||
|
||||
q = Quaternion.Degrees(90, 90, -90)
|
||||
sqrt2_2 = math.sqrt(2) / 2
|
||||
assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
|
||||
|
||||
def test_Radians(self):
|
||||
q = Quaternion.Radians(0, 0, 0)
|
||||
assert(q == Quaternion.identity)
|
||||
|
||||
q = Quaternion.Radians(math.pi / 2, math.pi / 2, -math.pi / 2)
|
||||
sqrt2_2 = math.sqrt(2) / 2
|
||||
assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
|
||||
|
||||
def test_Multiply(self):
|
||||
q1 = Quaternion.identity
|
||||
q2 = Quaternion(1, 0, 0, 0)
|
||||
|
||||
r = q1 * q2;
|
||||
assert(r == Quaternion(1, 0, 0, 0))
|
||||
|
||||
def test_MultiplyVector(self):
|
||||
v1 = Vector3.up
|
||||
|
||||
q1 = Quaternion.identity
|
||||
v = q1 * v1
|
||||
assert(v == Vector3(0, 1, 0))
|
||||
|
||||
q1 = Quaternion(1, 0, 0, 0)
|
||||
v = q1 * v1
|
||||
assert(v == Vector3(0, -1, 0))
|
||||
|
||||
def test_Angle(self):
|
||||
q1 = Quaternion.identity
|
||||
q2 = Quaternion.identity
|
||||
r = Quaternion.Angle(q1, q2)
|
||||
assert(r == Angle.zero)
|
||||
|
||||
def test_AngleAround(self):
|
||||
axis = Direction.up
|
||||
q1 = Quaternion.identity
|
||||
|
||||
angle = q1.AngleAround(axis)
|
||||
assert(angle == Angle.Degrees(0))
|
||||
|
||||
sqrt2_2 = math.sqrt(2) / 2
|
||||
q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0)
|
||||
angle = q1.AngleAround(axis)
|
||||
assert(angle == Angle.Degrees(180))
|
||||
|
||||
axis = Direction.zero
|
||||
angle = q1.AngleAround(axis)
|
||||
assert(math.isnan(angle.InDegrees()))
|
||||
|
||||
def test_RotationAround(self):
|
||||
axis = Direction.up
|
||||
q1 = Quaternion.identity
|
||||
|
||||
q = q1.RotationAround(axis)
|
||||
assert(q == Quaternion.identity)
|
||||
|
||||
sqrt2_2 = math.sqrt(2) / 2
|
||||
q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0)
|
||||
q = q1.RotationAround(axis)
|
||||
assert(q == Quaternion(0, 1, 0, 0))
|
||||
|
||||
axis = Direction.zero
|
||||
q = q1.RotationAround(axis);
|
||||
assert(math.isnan(q.x) and math.isnan(q.y) and math.isnan(q.z) and math.isnan(q.w))
|
||||
442
LinearAlgebra/test/Spherical_test.py
Normal file
442
LinearAlgebra/test/Spherical_test.py
Normal file
@ -0,0 +1,442 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from Spherical import *
|
||||
|
||||
class PolarTest(unittest.TestCase):
|
||||
def test_FromVector2(self):
|
||||
v: Vector2 = Vector2(0, 1)
|
||||
p: Polar = Polar.FromVector2(v)
|
||||
|
||||
assert(p.distance == 1)
|
||||
assert(p.direction.InDegrees() == 0)
|
||||
|
||||
v = Vector2(1, 0)
|
||||
p = Polar.FromVector2(v)
|
||||
|
||||
assert(p.distance, 1)
|
||||
assert(p.direction.InDegrees(), 90)
|
||||
|
||||
v = Vector2(-1, 1)
|
||||
p = Polar.FromVector2(v)
|
||||
|
||||
assert(p.distance == math.sqrt(2))
|
||||
assert(p.direction.InDegrees() == -45)
|
||||
|
||||
def test_Equality(self):
|
||||
v1: Polar = Polar.Degrees(4, 5)
|
||||
|
||||
v2: Polar = Polar.Degrees(1, 2)
|
||||
assert(v1 != v2)
|
||||
|
||||
v2 = Polar.Degrees(4, 5)
|
||||
assert(v1 == v2)
|
||||
|
||||
def test_Magnitude(self):
|
||||
v: Polar = Polar.Degrees(2, 30)
|
||||
r: float = 0
|
||||
|
||||
r = v.Magnitude()
|
||||
assert(r == 2)
|
||||
|
||||
v = Polar.Degrees(-2, -30)
|
||||
r = v.Magnitude()
|
||||
assert(r == 2)
|
||||
|
||||
v = Polar.Degrees(0, 0)
|
||||
r = v.Magnitude()
|
||||
assert(r == 0)
|
||||
|
||||
def test_Normalize(self):
|
||||
v1: Polar = Polar.Degrees(2, 90)
|
||||
r: Polar = Polar.zero
|
||||
|
||||
r = v1.Normalized()
|
||||
assert(r == Polar.Degrees(1, 90))
|
||||
|
||||
v1 = Polar.Degrees(2, -90)
|
||||
r = v1.Normalized()
|
||||
assert(r == Polar.Degrees(1, -90))
|
||||
|
||||
v1 = Polar.Degrees(0, 0)
|
||||
r = v1.Normalized()
|
||||
assert(r == Polar.Degrees(0, 0))
|
||||
|
||||
def test_Negate(self):
|
||||
v1: Polar = Polar.Degrees(2, 45)
|
||||
r: Polar = Polar.zero
|
||||
|
||||
r = -v1
|
||||
assert(r == Polar.Degrees(2, -135))
|
||||
|
||||
v1 = Polar.Degrees(2, -45)
|
||||
r = -v1
|
||||
assert(r == Polar.Degrees(2, 135))
|
||||
|
||||
v1 = Polar.Degrees(2, 0)
|
||||
r = -v1
|
||||
assert(r == Polar.Degrees(2, 180))
|
||||
|
||||
v1 = Polar.Degrees(0, 0)
|
||||
r = -v1
|
||||
assert(r == Polar.Degrees(0, 0))
|
||||
|
||||
def test_Subtract(self):
|
||||
r: Polar = Polar.zero
|
||||
v1: Polar = Polar.Degrees(4, 45)
|
||||
|
||||
v2: Polar = Polar.zero
|
||||
r = v1 - v2
|
||||
assert(r == v1)
|
||||
|
||||
r = v1
|
||||
r -= v2
|
||||
assert(r == v1)
|
||||
|
||||
v2: Polar = Polar.Degrees(1, 45)
|
||||
r = v1 - v2
|
||||
assert(r == Polar.Degrees(3, 45))
|
||||
|
||||
v2: Polar = Polar.Degrees(1, -135)
|
||||
r = v1 - v2
|
||||
assert(r == Polar.Degrees(5, 45))
|
||||
|
||||
def test_Addition(self):
|
||||
r = Polar.zero
|
||||
|
||||
v1 = Polar.Degrees(4, 45)
|
||||
v2 = Polar.zero
|
||||
r = v1 + v2
|
||||
assert(r == v1)
|
||||
|
||||
r = v1
|
||||
r += v2
|
||||
assert(r == v1)
|
||||
|
||||
v2 = Polar.Degrees(1, 45)
|
||||
r = v1 + v2
|
||||
assert(r == Polar.Degrees(5, 45))
|
||||
|
||||
v2 = Polar.Degrees(1, -135)
|
||||
r = v1 + v2
|
||||
assert(r == Polar.Degrees(3, 45))
|
||||
|
||||
def test_Multiply(self):
|
||||
r: Polar = Polar.zero
|
||||
|
||||
v1: Polar = Polar.Degrees(4, 45)
|
||||
r = v1 * 2
|
||||
assert(r == Polar.Degrees(8, 45))
|
||||
|
||||
r = v1 * -2
|
||||
assert(r == Polar.Degrees(8, -135))
|
||||
|
||||
r = v1 * 0
|
||||
assert(r == Polar.Degrees(0, 0))
|
||||
|
||||
def test_Divide(self):
|
||||
r: Polar.zero
|
||||
|
||||
v1 = Polar.Degrees(4, 45)
|
||||
r = v1 / 2
|
||||
assert(r == Polar.Degrees(2, 45))
|
||||
|
||||
r = v1 / -2
|
||||
assert(r == Polar.Degrees(2, -135))
|
||||
|
||||
def test_Distance(self):
|
||||
r: float = 0
|
||||
|
||||
v1 = Polar.Degrees(4, 45)
|
||||
v2 = Polar.Degrees(1, -135)
|
||||
r = Polar.Distance(v1, v2)
|
||||
assert(r == 5)
|
||||
|
||||
v2 = Polar.Degrees(-1, -135)
|
||||
r = Polar.Distance(v1, v2)
|
||||
assert(r == 3)
|
||||
|
||||
v2 = Polar.Degrees(0, 0)
|
||||
r = Polar.Distance(v1, v2)
|
||||
assert(r == 4)
|
||||
|
||||
def test_Angle(self):
|
||||
r = Angle.zero
|
||||
|
||||
v1 = Polar.Degrees(4, 45)
|
||||
v2 = Polar.Degrees(1, -45)
|
||||
|
||||
r = Polar.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
v2 = Polar.Degrees(1, 135)
|
||||
r = Polar.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
v2 = Polar.Degrees(1, 45)
|
||||
r = Polar.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_SignedAngle(self):
|
||||
r = Angle.zero
|
||||
|
||||
v1 = Polar.Degrees(4, 45)
|
||||
v2 = Polar.Degrees(1, -45)
|
||||
|
||||
r = Polar.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees() == -90)
|
||||
|
||||
v2 = Polar.Degrees(1, 135)
|
||||
r = Polar.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
v2 = Polar.Degrees(1, 45)
|
||||
r = Polar.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_Lerp(self):
|
||||
r = Polar.zero
|
||||
|
||||
v1 = Polar.Degrees(5, 45)
|
||||
v2 = Polar.Degrees(1, -45)
|
||||
|
||||
r = Polar.Lerp(v1, v2, 0)
|
||||
assert(r == v1)
|
||||
|
||||
r = Polar.Lerp(v1, v2, 1)
|
||||
assert(r == v2)
|
||||
|
||||
r = Polar.Lerp(v1, v2, 0.5)
|
||||
assert(r == Polar.Degrees(3, 0))
|
||||
|
||||
r = Polar.Lerp(v1, v2, -1)
|
||||
assert(r == Polar.Degrees(9, 135))
|
||||
|
||||
r = Polar.Lerp(v1, v2, 2)
|
||||
assert(r == Polar.Degrees(-3, -135))
|
||||
|
||||
class SphericalTest(unittest.TestCase):
|
||||
def test_FromVector3(self):
|
||||
v: Vector3 = Vector3(0, 0, 1)
|
||||
p: Spherical = Spherical.FromVector3(v)
|
||||
|
||||
assert(p.distance == 1)
|
||||
assert(p.direction.horizontal.InDegrees() == 0)
|
||||
assert(p.direction.vertical.InDegrees() == 0)
|
||||
|
||||
v = Vector3(1, 0, 0)
|
||||
p = Spherical.FromVector3(v)
|
||||
|
||||
assert(p.distance, 1)
|
||||
assert(p.direction.horizontal.InDegrees(), 90)
|
||||
assert(p.direction.vertical.InDegrees(), 0)
|
||||
|
||||
v = Vector3(0, 1, 0)
|
||||
p = Spherical.FromVector3(v)
|
||||
|
||||
assert(p.distance, 1)
|
||||
assert(p.direction.horizontal.InDegrees(), 0)
|
||||
assert(p.direction.vertical.InDegrees(), 90)
|
||||
|
||||
v = Vector3(-1, 0, 1)
|
||||
p = Spherical.FromVector3(v)
|
||||
|
||||
assert(p.distance == math.sqrt(2))
|
||||
assert(p.direction.horizontal.InDegrees() == -45)
|
||||
assert(p.direction.vertical.InDegrees() == 0)
|
||||
|
||||
def test_Equality(self):
|
||||
v1: Spherical = Spherical.Degrees(4, 5, 6)
|
||||
|
||||
v2: Spherical = Spherical.Degrees(1, 2, 3)
|
||||
assert(v1 != v2)
|
||||
|
||||
v2 = Spherical.Degrees(4, 5, 6)
|
||||
assert(v1 == v2)
|
||||
|
||||
def test_Magnitude(self):
|
||||
v: Spherical = Spherical.Degrees(2, 30, 0)
|
||||
r: float = 0
|
||||
|
||||
r = v.Magnitude()
|
||||
assert(r == 2)
|
||||
|
||||
v = Spherical.Degrees(-2, -30, 0)
|
||||
r = v.Magnitude()
|
||||
assert(r == 2)
|
||||
|
||||
v = Spherical.Degrees(0, 0, 0)
|
||||
r = v.Magnitude()
|
||||
assert(r == 0)
|
||||
|
||||
def test_Normalize(self):
|
||||
v1: Spherical = Spherical.Degrees(2, 90, 0)
|
||||
r: Spherical = Spherical.zero
|
||||
|
||||
r = v1.Normalized()
|
||||
assert(r == Spherical.Degrees(1, 90, 0))
|
||||
|
||||
v1 = Spherical.Degrees(2, -90, 0)
|
||||
r = v1.Normalized()
|
||||
assert(r == Spherical.Degrees(1, -90, 0))
|
||||
|
||||
v1 = Spherical.Degrees(0, 0, 0)
|
||||
r = v1.Normalized()
|
||||
assert(r == Spherical.Degrees(0, 0, 0))
|
||||
|
||||
def test_Negate(self):
|
||||
v1: Spherical = Spherical.Degrees(2, 45, 0)
|
||||
r: Spherical = Spherical.zero
|
||||
|
||||
r = -v1
|
||||
assert(r == Spherical.Degrees(2, -135, 0))
|
||||
|
||||
v1 = Spherical.Degrees(2, -45, 0)
|
||||
r = -v1
|
||||
assert(r == Spherical.Degrees(2, 135, 0))
|
||||
|
||||
v1 = Spherical.Degrees(0, 0, 0)
|
||||
r = -v1
|
||||
assert(r == Spherical.Degrees(0, 180, 0))
|
||||
|
||||
def test_Subtract(self):
|
||||
r: Spherical = Spherical.zero
|
||||
|
||||
v1: Spherical = Spherical.Degrees(4, 45, 0)
|
||||
v2: Spherical = Spherical.zero
|
||||
r = v1 - v2
|
||||
assert(r == v1)
|
||||
|
||||
r = v1
|
||||
r -= v2
|
||||
assert(r == v1)
|
||||
|
||||
v2 = Spherical.Degrees(1, 45, 0)
|
||||
r = v1 - v2
|
||||
assert(r == Spherical(3, 45, 0))
|
||||
|
||||
v2 = Spherical.Degrees(1, -135, 0)
|
||||
r = v1 - v2
|
||||
assert(r == Spherical.Degrees(5, 45, 0))
|
||||
|
||||
def test_Addition(self):
|
||||
v1 = Spherical(1, Direction.Degrees(45, 0))
|
||||
v2 = Spherical.zero
|
||||
r = Spherical.zero
|
||||
|
||||
r = v1 + v2
|
||||
assert(r.distance == v1.distance)
|
||||
|
||||
r = v1
|
||||
r += v2
|
||||
assert(r.distance == v1.distance)
|
||||
|
||||
v2 = Spherical(1, Direction.Degrees(-45, 0))
|
||||
r = v1 + v2
|
||||
assert(r.distance == math.sqrt(2))
|
||||
assert(r.direction.horizontal.InDegrees() == 0)
|
||||
assert(r.direction.vertical.InDegrees() == 0)
|
||||
|
||||
v2 = Spherical(1, Direction.Degrees(0, 90))
|
||||
r = v1 + v2
|
||||
assert(r.distance == math.sqrt(2))
|
||||
assert(r.direction.horizontal.InDegrees() == 45)
|
||||
assert(r.direction.vertical.InDegrees() == 45)
|
||||
|
||||
def test_Multiply(self):
|
||||
r = Spherical.zero
|
||||
|
||||
v1 = Spherical.Degrees(4, 45, 0)
|
||||
r = v1 * 3
|
||||
assert(r == Spherical.Degrees(12, 45, 0))
|
||||
|
||||
r = v1 * -3
|
||||
assert(r == Spherical.Degrees(12, -135, 0))
|
||||
|
||||
r = v1 * 0
|
||||
assert(r == Spherical.Degrees(0, 0, 0))
|
||||
|
||||
def test_Divide(self):
|
||||
r: Spherical.zero
|
||||
|
||||
v1 = Spherical.Degrees(4, 45, 0)
|
||||
r = v1 / 2
|
||||
assert(r == Spherical.Degrees(2, 45, 0))
|
||||
|
||||
r = v1 / -2
|
||||
assert(r == Spherical.Degrees(2, -135, 0))
|
||||
|
||||
def test_Distance(self):
|
||||
r: float = 0
|
||||
|
||||
v1 = Spherical.Degrees(4, 45, 0)
|
||||
v2 = Spherical.Degrees(1, -135, 0)
|
||||
r = Spherical.Distance(v1, v2)
|
||||
assert(r == 5)
|
||||
|
||||
v2 = Spherical.Degrees(-1, -135, 0)
|
||||
r = Spherical.Distance(v1, v2)
|
||||
assert(r == 3)
|
||||
|
||||
v2 = Spherical.Degrees(0, 0, 0)
|
||||
r = Spherical.Distance(v1, v2)
|
||||
assert(r == 4)
|
||||
|
||||
def test_Angle(self):
|
||||
r = Angle.zero
|
||||
|
||||
v1 = Spherical.Degrees(4, 45, 0)
|
||||
v2 = Spherical.Degrees(1, -45, 0)
|
||||
|
||||
r = Spherical.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
v2 = Spherical.Degrees(1, 135, 0)
|
||||
r = Spherical.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
v2 = Spherical.Degrees(1, 45, 0)
|
||||
r = Spherical.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_SignedAngle(self):
|
||||
r = Angle.zero
|
||||
|
||||
v1 = Spherical.Degrees(4, 45, 0)
|
||||
v2 = Spherical.Degrees(1, -45, 0)
|
||||
|
||||
r = Spherical.SignedAngle(v1, v2, Direction.up)
|
||||
assert(r.InDegrees() == -90)
|
||||
|
||||
v2 = Spherical.Degrees(1, 135, 0)
|
||||
r = Spherical.SignedAngle(v1, v2, Direction.up)
|
||||
assert(r.InDegrees() == 90)
|
||||
|
||||
v2 = Spherical.Degrees(1, 45, 0)
|
||||
r = Spherical.SignedAngle(v1, v2, Direction.up)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_Lerp(self):
|
||||
r = Spherical.zero
|
||||
|
||||
v1 = Spherical.Degrees(5, 45, 0)
|
||||
v2 = Spherical.Degrees(1, -45, 0)
|
||||
|
||||
r = Spherical.Lerp(v1, v2, 0)
|
||||
assert(r == v1)
|
||||
|
||||
r = Spherical.Lerp(v1, v2, 1)
|
||||
assert(r == v2)
|
||||
|
||||
r = Spherical.Lerp(v1, v2, 0.5)
|
||||
assert(r == Spherical.Degrees(3, 0, 0))
|
||||
|
||||
r = Spherical.Lerp(v1, v2, -1)
|
||||
assert(r == Spherical.Degrees(9, 135, 0))
|
||||
|
||||
r = Spherical.Lerp(v1, v2, 2)
|
||||
assert(r == Spherical.Degrees(-3, -135, 0))
|
||||
|
||||
99
LinearAlgebra/test/SwingTwist_test.py
Normal file
99
LinearAlgebra/test/SwingTwist_test.py
Normal file
@ -0,0 +1,99 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from SwingTwist import *
|
||||
|
||||
class SwingTwistTest(unittest.TestCase):
|
||||
def test_Constructor(self):
|
||||
s = SwingTwist.Degrees(0, 0, 0)
|
||||
assert(s == SwingTwist.Degrees(0, 0, 0))
|
||||
|
||||
s = SwingTwist.Degrees(0, 180, 0)
|
||||
assert(s == SwingTwist.Degrees(180, 0, 180))
|
||||
|
||||
s = SwingTwist.Degrees(0, 180, 180)
|
||||
assert(s == SwingTwist.Degrees(180, 0, 0))
|
||||
|
||||
s = SwingTwist.Degrees(270, 90, 0)
|
||||
assert(s == SwingTwist.Degrees(-90, 90, 0))
|
||||
|
||||
s = SwingTwist.Degrees(270, 270, 0)
|
||||
assert(s == SwingTwist.Degrees(-90, -90, 0))
|
||||
|
||||
s = SwingTwist.Degrees(270, 225, 0)
|
||||
assert(s == SwingTwist.Degrees(90, -45, -180))
|
||||
|
||||
s = SwingTwist.Degrees(270, 0, 225)
|
||||
assert(s == SwingTwist.Degrees(-90, 0, -135))
|
||||
|
||||
def test_FromQuaternion(self):
|
||||
q = Quaternion.identity
|
||||
r = SwingTwist.FromQuaternion(q)
|
||||
assert(r == SwingTwist.Degrees(0, 0, 0))
|
||||
|
||||
q = Quaternion.Degrees(90, 0, 0)
|
||||
r = SwingTwist.FromQuaternion(q)
|
||||
assert(r == SwingTwist.Degrees(90, 0, 0))
|
||||
|
||||
q = Quaternion.Degrees(0, 90, 0)
|
||||
r = SwingTwist.FromQuaternion(q)
|
||||
assert(r == SwingTwist.Degrees(0, 90, 0))
|
||||
|
||||
q = Quaternion.Degrees(0, 0, 90)
|
||||
r = SwingTwist.FromQuaternion(q)
|
||||
assert(r == SwingTwist.Degrees(0, 0, 90))
|
||||
|
||||
q = Quaternion.Degrees(0, 180, 0)
|
||||
r = SwingTwist.FromQuaternion(q)
|
||||
assert(r == SwingTwist.Degrees(0, 180, 0))
|
||||
|
||||
q = Quaternion.Degrees(0, 135, 0)
|
||||
r = SwingTwist.FromQuaternion(q)
|
||||
assert(r == SwingTwist.Degrees(0, 135, 0))
|
||||
|
||||
def test_FromAngleAxis(self):
|
||||
r = SwingTwist.FromAngleAxis(Angle.zero, Direction.up)
|
||||
assert(r == SwingTwist.Degrees(0, 0, 0))
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.up)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(90, 0, 0))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.up)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(180, 0, 0))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.up);
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(-90, 0, 0))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.right)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 90, 0))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.right)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 180, 0))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.right)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, -90, 0))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.forward)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 90))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.forward)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 180))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.forward)
|
||||
angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, -90))
|
||||
assert(angle.InDegrees() == 0)
|
||||
|
||||
# auto r16 = SwingTwist16::AngleAxis(13, Direction16::down);
|
||||
# auto s16 = SwingTwist16::Degrees(-13, 0, 0);
|
||||
# assert(SwingTwist16::Angle(r16, s16), Angle16::Degrees(0))
|
||||
551
LinearAlgebra/test/Vector_test.py
Normal file
551
LinearAlgebra/test/Vector_test.py
Normal file
@ -0,0 +1,551 @@
|
||||
import unittest
|
||||
import sys
|
||||
from pathlib import Path
|
||||
# Add the project root to sys.path
|
||||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
from Vector import *
|
||||
|
||||
class Vector2Test(unittest.TestCase):
|
||||
def test_Equality(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
assert(v1 != v2)
|
||||
|
||||
v2 = Vector2(4, 5)
|
||||
assert(v1 == v2)
|
||||
|
||||
def test_SqrMagnitude(self):
|
||||
v: Vector2 = Vector2(1, 2)
|
||||
m: float = 0;
|
||||
|
||||
m = v.SqrMagnitude()
|
||||
assert(m == 5)
|
||||
|
||||
v = Vector2(-1, -2)
|
||||
m = v.SqrMagnitude()
|
||||
assert(m == 5)
|
||||
|
||||
v = Vector2(0, 0)
|
||||
m = v.SqrMagnitude()
|
||||
assert(m == 0)
|
||||
|
||||
def test_Magnitude(self):
|
||||
v: Vector2 = Vector2(1, 2)
|
||||
m: float = 0;
|
||||
|
||||
m = v.Magnitude()
|
||||
assert(m == 2.23606797749979)
|
||||
|
||||
v = Vector2(-1, -2)
|
||||
m = v.Magnitude()
|
||||
assert(m == 2.23606797749979)
|
||||
|
||||
v = Vector2(0, 0)
|
||||
m = v.Magnitude()
|
||||
assert(m == 0)
|
||||
|
||||
def test_Normalize(self):
|
||||
v1: Vector2 = Vector2(0, 2)
|
||||
v: Vector2 = Vector2.zero
|
||||
|
||||
v = v1.Normalized()
|
||||
assert(v == Vector2(0, 1))
|
||||
|
||||
v1 = Vector2(0, -2)
|
||||
v = v1.Normalized()
|
||||
assert(v == Vector2(0, -1))
|
||||
|
||||
v1 = Vector2(0, 0)
|
||||
v = v1.Normalized()
|
||||
assert(v == Vector2(0, 0))
|
||||
|
||||
def test_Negate(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
r: Vector2 = Vector2.zero
|
||||
|
||||
r = -v1
|
||||
assert(r == Vector2(-4, -5))
|
||||
|
||||
v1 = Vector2(-4, -5)
|
||||
r = -v1
|
||||
assert(r == Vector2(4, 5))
|
||||
|
||||
v1 = Vector2(0, 0)
|
||||
r = -v1
|
||||
assert(r == Vector2(0, 0))
|
||||
|
||||
def test_Subtract(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: Vector2 = Vector2.zero
|
||||
|
||||
|
||||
r = v1 - v2
|
||||
assert(v1 - v2 == Vector2(3, 3))
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = v1 - v2
|
||||
assert(r == Vector2(5, 7))
|
||||
|
||||
v2 = Vector2(4, 5)
|
||||
r = v1 - v2
|
||||
assert(r == Vector2(0, 0))
|
||||
|
||||
v2 = Vector2(0, 0)
|
||||
r = v1 - v2
|
||||
assert(r == Vector2(4, 5))
|
||||
|
||||
def test_Addition(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: Vector2 = Vector2.zero
|
||||
|
||||
r = v1 + v2
|
||||
assert(r == Vector2(5, 7))
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = v1 + v2
|
||||
assert(r == Vector2(3, 3))
|
||||
|
||||
v2 = Vector2(0, 0)
|
||||
r = v1 + v2
|
||||
assert(r == Vector2(4, 5))
|
||||
|
||||
def test_Scale(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: Vector2 = Vector2.zero
|
||||
|
||||
r = v1.Scale(v2)
|
||||
assert(r == Vector2(4, 10))
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = v1.Scale(v2)
|
||||
assert(r == Vector2(-4, -10))
|
||||
|
||||
v2 = Vector2(0, 0)
|
||||
r = v1.Scale(v2)
|
||||
assert(r == Vector2(0, 0))
|
||||
|
||||
def test_Multiply(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
f: float = 3
|
||||
r: Vector2 = Vector2.zero
|
||||
|
||||
r = v1 * f
|
||||
assert(r == Vector2(12, 15))
|
||||
|
||||
f = -3
|
||||
r = v1 * f
|
||||
assert(r == Vector2(-12, -15))
|
||||
|
||||
f = 0
|
||||
r = v1 * f
|
||||
assert(r == Vector2(0, 0))
|
||||
|
||||
def test_Divide(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
f: float = 2
|
||||
v: Vector2 = Vector2.zero
|
||||
|
||||
v = v1 / f
|
||||
assert(v == Vector2(2, 2.5))
|
||||
|
||||
f = -2
|
||||
v = v1 / f
|
||||
assert(v == Vector2(-2, -2.5))
|
||||
|
||||
def test_Distance(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: float = 0
|
||||
|
||||
r = Vector2.Distance(v1, v2)
|
||||
assert(r == 4.242640687119285)
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = Vector2.Distance(v1, v2)
|
||||
assert(r == 8.602325267042627)
|
||||
|
||||
v2 = Vector2(0, 0)
|
||||
r = Vector2.Distance(v1, v2)
|
||||
assert(r == 6.4031242374328485)
|
||||
|
||||
def test_Dot(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: float = 0
|
||||
|
||||
r = Vector2.Dot(v1, v2)
|
||||
assert(r == 14)
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = Vector2.Dot(v1, v2)
|
||||
assert(r, -14)
|
||||
|
||||
v2 = Vector2(0, 0)
|
||||
r = Vector2.Dot(v1, v2)
|
||||
assert(r, 0)
|
||||
|
||||
def test_Angle(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: Angle = Angle.Degrees(0)
|
||||
|
||||
r = Vector2.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 12.094757077012119)
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = Vector2.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 167.9052429229879)
|
||||
|
||||
v2 = Vector2(0, 0)
|
||||
r = Vector2.Angle(v1, v2)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
def test_SignedAngle(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: float = 0
|
||||
|
||||
r = Vector2.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees() == -12.094757077012098)
|
||||
|
||||
v2 = Vector2(-1, -2)
|
||||
r = Vector2.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees() == 167.90524292298792)
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
r = Vector2.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees() == 0)
|
||||
|
||||
v1 = Vector2(0, 1)
|
||||
v2 = Vector2(1, 0)
|
||||
r = Vector2.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees(), 90)
|
||||
|
||||
v1 = Vector2(0, 1)
|
||||
v2 = Vector2(0, -1)
|
||||
r = Vector2.SignedAngle(v1, v2)
|
||||
assert(r.InDegrees(), 180)
|
||||
|
||||
def test_Lerp(self):
|
||||
v1: Vector2 = Vector2(4, 5)
|
||||
v2: Vector2 = Vector2(1, 2)
|
||||
r: Vector2 = Vector2.zero
|
||||
|
||||
r = Vector2.Lerp(v1, v2, 0)
|
||||
assert(Vector2.Distance(r, v1), 0)
|
||||
|
||||
r = Vector2.Lerp(v1, v2, 1)
|
||||
assert(Vector2.Distance(r, v2), 0)
|
||||
|
||||
r = Vector2.Lerp(v1, v2, 0.5)
|
||||
assert(Vector2.Distance(r, Vector2(2.5, 3.5)), 0)
|
||||
|
||||
r = Vector2.Lerp(v1, v2, -1)
|
||||
assert(Vector2.Distance(r, Vector2(7.0, 8.0)), 0)
|
||||
|
||||
r = Vector2.Lerp(v1, v2, 2)
|
||||
assert(Vector2.Distance(r, Vector2(-2.0, -1.0)), 0)
|
||||
|
||||
class Vector3Test(unittest.TestCase):
|
||||
def test_Equality(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
r: bool = False
|
||||
|
||||
r = v1 == v2
|
||||
assert(r == False)
|
||||
|
||||
v2 = Vector3(4, 5, 6);
|
||||
r = v1 == v2;
|
||||
assert(r == True)
|
||||
|
||||
def test_SqrMagnitude(self):
|
||||
v: Vector3 = Vector3(1, 2, 3)
|
||||
m: float = 0;
|
||||
|
||||
m = v.SqrMagnitude()
|
||||
assert(m == 14)
|
||||
|
||||
v = Vector3(-1, -2, -3)
|
||||
m = v.SqrMagnitude()
|
||||
assert(m == 14)
|
||||
|
||||
v = Vector3(0, 0, 0)
|
||||
m = v.SqrMagnitude()
|
||||
assert(m == 0)
|
||||
|
||||
def test_Magnitude(self):
|
||||
v: Vector3 = Vector3(1, 2, 3)
|
||||
m: float = 0;
|
||||
|
||||
m = v.Magnitude()
|
||||
assert(m == 3.7416573867739413)
|
||||
|
||||
v = Vector3(-1, -2, -3)
|
||||
m = v.Magnitude()
|
||||
assert(m == 3.7416573867739413)
|
||||
|
||||
v = Vector3(0, 0, 0)
|
||||
m = v.Magnitude()
|
||||
assert(m == 0)
|
||||
|
||||
def test_Normalize(self):
|
||||
r: bool = False
|
||||
|
||||
v1: Vector3 = Vector3(0, 2, 0)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1.Normalized()
|
||||
assert(v == Vector3(0, 1, 0))
|
||||
|
||||
v1 = Vector3(0, -2, 0)
|
||||
v = v1.Normalized()
|
||||
assert(v == Vector3(0, -1, 0))
|
||||
|
||||
v1 = Vector3(0, 0, 0)
|
||||
v = v1.Normalized()
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
def test_Negate(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = -v1
|
||||
assert(v == Vector3(-4, -5, -6))
|
||||
|
||||
v1 = Vector3(-4, -5, -6)
|
||||
v = -v1
|
||||
assert(v == Vector3(4, 5, 6))
|
||||
|
||||
v1 = Vector3(0, 0, 0)
|
||||
v = -v1
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
def test_Subtract(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v3: Vector3 = Vector3.zero
|
||||
|
||||
v = v1 - v2
|
||||
assert(v == Vector3(3, 3, 3))
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
v = v1 - v2
|
||||
assert(v == Vector3(5, 7, 9))
|
||||
|
||||
v2 = Vector3(4, 5, 6)
|
||||
v = v1 - v2
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
v = v1 - v2
|
||||
assert(v == Vector3(4, 5, 6))
|
||||
|
||||
def test_Addition(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1 + v2
|
||||
assert(v == Vector3(5, 7, 9))
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
v = v1 + v2
|
||||
assert(v == Vector3(3, 3, 3))
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
v = v1 + v2
|
||||
assert(v == Vector3(4, 5, 6))
|
||||
|
||||
def test_Scale(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1.Scale(v2)
|
||||
assert(v == Vector3(4, 10, 18))
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
v = v1.Scale(v2)
|
||||
assert(v == Vector3(-4, -10, -18))
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
v = v1.Scale(v2)
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
def test_Multiply(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
f: float = 3
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1 * f
|
||||
assert(v == Vector3(12, 15, 18))
|
||||
|
||||
f = -3
|
||||
v = v1 * f
|
||||
assert(v == Vector3(-12, -15, -18))
|
||||
|
||||
f = 0
|
||||
v = v1 * f
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
def test_Divide(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
f: float = 2
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1 / f
|
||||
assert(v == Vector3(2, 2.5, 3))
|
||||
|
||||
f = -2
|
||||
v = v1 / f
|
||||
assert(v == Vector3(-2, -2.5, -3))
|
||||
|
||||
def test_Distance(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6);
|
||||
v2: Vector3 = Vector3(1, 2, 3);
|
||||
f: float = 0
|
||||
|
||||
f = Vector3.Distance(v1, v2);
|
||||
assert(f == 5.196152422706632)
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
f = Vector3.Distance(v1, v2);
|
||||
assert(f == 12.449899597988733)
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
f = Vector3.Distance(v1, v2);
|
||||
assert(f == 8.774964387392123)
|
||||
|
||||
def test_Dot(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
f: float = 0
|
||||
|
||||
f = Vector3.Dot(v1, v2)
|
||||
assert(f == 32)
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
f = Vector3.Dot(v1, v2)
|
||||
assert(f, -32)
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
f = Vector3.Dot(v1, v2)
|
||||
assert(f, 0)
|
||||
|
||||
def test_Cross(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = Vector3.Cross(v1, v2)
|
||||
assert(v == Vector3(3, -6, 3))
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
v = Vector3.Cross(v1, v2)
|
||||
assert(v == Vector3(-3, 6, -3))
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
v = Vector3.Cross(v1, v2)
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
def test_Project(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1.Project(v2)
|
||||
assert(v == Vector3(2.2857142857142856, 4.571428571428571, 6.857142857142857))
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
v = v1.Project(v2)
|
||||
assert(v == Vector3(2.2857142857142856, 4.571428571428571, 6.857142857142857))
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
v = v1.Project(v2)
|
||||
assert(v == Vector3(0, 0, 0))
|
||||
|
||||
def test_ProjectOnPlane(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v: Vector3 = Vector3.zero
|
||||
|
||||
v = v1.ProjectOnPlane(v2)
|
||||
assert(v == Vector3(1.7142857142857144, 0.4285714285714288, -0.8571428571428568))
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
v = v1.ProjectOnPlane(v2)
|
||||
assert(v == Vector3(1.7142857142857144, 0.4285714285714288, -0.8571428571428568))
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
v = v1.ProjectOnPlane(v2)
|
||||
assert(v == Vector3(4, 5, 6))
|
||||
|
||||
def test_Angle(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
f: Angle = Angle.Degrees(0)
|
||||
|
||||
f = Vector3.Angle(v1, v2)
|
||||
assert(f.InDegrees() == 12.933154491899135)
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
f = Vector3.Angle(v1, v2)
|
||||
assert(f.InDegrees() == 167.06684550810087)
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
f = Vector3.Angle(v1, v2)
|
||||
assert(f.InDegrees() == 0)
|
||||
|
||||
def test_SignedAngle(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
v3: Vector3 = Vector3(7, 8, -9)
|
||||
f: Angle = Angle.Degrees(0);
|
||||
r: bool = False
|
||||
|
||||
f = Vector3.SignedAngle(v1, v2, v3)
|
||||
assert(f.InDegrees() == -12.933154491899135)
|
||||
|
||||
v2 = Vector3(-1, -2, -3)
|
||||
f = Vector3.SignedAngle(v1, v2, v3)
|
||||
assert(f.InDegrees(), 167.06684550810087)
|
||||
|
||||
v2 = Vector3(0, 0, 0)
|
||||
f = Vector3.SignedAngle(v1, v2, v3)
|
||||
assert(f.InDegrees(), 0)
|
||||
|
||||
v2 = Vector3(1, 2, 3)
|
||||
|
||||
v3 = Vector3(-7, -8, 9)
|
||||
f = Vector3.SignedAngle(v1, v2, v3)
|
||||
assert(f.InDegrees(), 12.933154491899135)
|
||||
|
||||
v3 = Vector3(0, 0, 0)
|
||||
f = Vector3.SignedAngle(v1, v2, v3)
|
||||
assert(f.InDegrees(), 0)
|
||||
|
||||
def test_Lerp(self):
|
||||
v1: Vector3 = Vector3(4, 5, 6)
|
||||
v2: Vector3 = Vector3(1, 2, 3)
|
||||
r: Vector3 = Vector3(0, 0, 0)
|
||||
|
||||
r = Vector3.Lerp(v1, v2, 0)
|
||||
assert(Vector3.Distance(r, v1), 0)
|
||||
|
||||
r = Vector3.Lerp(v1, v2, 1)
|
||||
assert(Vector3.Distance(r, v2), 0)
|
||||
|
||||
r = Vector3.Lerp(v1, v2, 0.5)
|
||||
assert(Vector3.Distance(r, Vector3(2.5, 3.5, 4.5)), 0)
|
||||
|
||||
r = Vector3.Lerp(v1, v2, -1)
|
||||
assert(Vector3.Distance(r, Vector3(7.0, 8.0, 9.0)), 0)
|
||||
|
||||
r = Vector3.Lerp(v1, v2, 2)
|
||||
assert(Vector3.Distance(r, Vector3(-2.0, -1.0, 0.0)), 0)
|
||||
Loading…
x
Reference in New Issue
Block a user