class Quaternion(CopyableMixin):
def __init__(self, w: Real = 1, x: Real = 0, y: Real = 0, z: Real = 0):
self.scalar = w
self.vector = Vector([x, y, z])
@property
def w(self) -> Real:
return self.scalar
@w.setter
def w(self, value: Real) -> None:
self.scalar = value
@property
def x(self) -> Real:
return self.vector[0]
@x.setter
def x(self, value: Real) -> None:
self.vector[0] = value
@property
def y(self) -> Real:
return self.vector[1]
@y.setter
def y(self, value: Real) -> None:
self.vector[1] = value
@property
def z(self) -> Real:
return self.vector[2]
@z.setter
def z(self, value: Real) -> None:
self.vector[2] = value
def __iter__(self) -> Generator[Real, None, None]:
yield self.scalar
yield from self.vector
def __str__(self) -> str:
return f"{self.__class__.__name__}(w={self.w:.4f}, x={self.x:.4f}, y={self.y:.4f}, z={self.z:.4f})"
def __repr__(self) -> str:
return self.__str__()
def __neg__(self) -> "Quaternion":
return Quaternion(*(-i for i in self))
def __add__(self, other: "Quaternion") -> "Quaternion":
if not isinstance(other, Quaternion):
raise TypeError(
f"Cannot add instances of type {type(other)} and {type(self)}")
return Quaternion(self.scalar + other.scalar,
*(self.vector + other.vector))
def __sub__(self, other: "Quaternion") -> "Quaternion":
if not isinstance(other, Quaternion):
raise TypeError(
f"Cannot subtract instances of type {type(other)} and {type(self)}"
)
return Quaternion(self.scalar - other.scalar,
*(self.vector - other.vector))
def __div__(self, other: "Quaternion") -> "Quaternion":
if not isinstance(other, Quaternion):
other = Quaternion.from_scalar(other)
if other.is_zero_quaternion():
raise ZeroDivisionError("other is a zero quaternion!")
return self * other.inverse()
def __idiv__(self, other: "Quaternion") -> "Quaternion":
return self.__div__(other)
def __rdiv__(self, other: "Quaternion") -> "Quaternion":
if not isinstance(other, Quaternion):
other = Quaternion.from_scalar(other)
return other * self.inverse()
def __truediv__(self, other: "Quaternion") -> "Quaternion":
return self.__div__(other)
def __itruediv__(self, other: "Quaternion") -> "Quaternion":
return self.__idiv__(other)
def __rtruediv__(self, other: "Quaternion") -> "Quaternion":
return self.__rdiv__(other)
def __mul__(self, other: "Quaternion") -> "Quaternion":
if not isinstance(other, Quaternion):
other = Quaternion.from_scalar(other)
_scalar = self.scalar * other.scalar - self.vector.dot(other.vector)
_vector = other.vector.scale(self.scalar) + self.vector.scale(
other.scalar) + self.vector.cross(other.vector)
return Quaternion(_scalar, *_vector)
def __imul__(self, other: "Quaternion") -> "Quaternion":
return self * other
def __rmul__(self, other: "Quaternion") -> "Quaternion":
if not isinstance(other, Quaternion):
other = Quaternion.from_scalar(other)
return other * self
def __pow__(self, exponent: Real) -> "Quaternion":
norm = self.norm()
if norm > 0:
vector_mag = self.vector.magnitude()
if vector_mag <= 0:
return Quaternion.from_scalar(self.scalar**exponent)
unit_vector = self.vector.scale(1 / vector_mag)
phi = acos(self.scalar / norm)
_scalar = cos(exponent * phi)
_vector = unit_vector.scale(sin(exponent * phi))
return (norm**exponent) * Quaternion(_scalar, *_vector)
return self.copy()
def __eq__(self, other: "Quaternion") -> "Quaternion":
return all(
isclose(i, j, rel_tol=1e-09, abs_tol=1e-09)
for i, j in zip(self, other))
def __hash__(self) -> int:
return hash(self.as_tuple())
def is_zero_quaternion(self) -> bool:
return self == Quaternion(0, 0, 0, 0)
def is_unit_quaternion(self) -> bool:
return isclose(self._squared_sum(), 1.0, rel_tol=1e-09)
def magnitude(self) -> Real:
return self.norm()
def _squared_sum(self) -> Real:
return self.scalar**2 + self.vector.dot(self.vector)
def norm(self) -> Real:
return sqrt(self._squared_sum())
def __len__(self):
return 4
def __getitem__(self, idx: int) -> Real:
if idx > 3:
raise IndexError(
f"Index {idx} is out of range for a Quaternion of size 4")
return self.scalar if idx == 0 else self.vector[idx - 1]
def __setitem__(self, idx: int, value: Real) -> None:
if idx > 3:
raise IndexError(
f"Index {idx} is out of range for a Quaternion of size 4")
if idx == 0:
self.scalar = value
else:
self.vector[idx - 1] = value
def normalize(self) -> "Quaternion":
if self.is_zero_quaternion():
raise ValueError("Cannot normalize a zero quaternion!")
n = self.norm()
return Quaternion(*(i / n for i in self))
def inverse(self) -> "Quaternion":
if self.is_zero_quaternion():
raise ValueError("Cannot invert a zero quaternion!")
square_sum = self._squared_sum()
return Quaternion(self.scalar / square_sum,
*-self.vector.scale(1 / square_sum))
def conjugate(self) -> "Quaternion":
return Quaternion(self.scalar, -self.vector)
def as_tuple(self) -> Tuple[Real]:
return tuple(i for i in self)
@classmethod
def from_tuple(cls, tup: Tuple[Real]) -> "Quaternion":
assert len(tup) == 4
return cls(*tup)
@classmethod
def identity(cls) -> "Quaternion":
return cls(1, 0, 0, 0)
@classmethod
def from_scalar(cls, scalar: Real) -> "Quaternion":
return cls(scalar, 0, 0, 0)
def test_vector_scaling():
v1 = Vector([1, 1, 1])
assert v1.scale(2) == Vector([2, 2, 2])
class PhysicsInterface:
def __init__(self, entity):
self.entity = entity
self.mass = 1
self.elasticity = 1
self.gravity = 0
self.friction = .75
self.velocity = Vector(entity.name + " velocity", 0, 0)
self.forces = []
self.last_position = 0, 0
def set_interface(self):
entity = self.entity
entity.get_velocity = self.get_instantaneous_velocity
entity.set_gravity = self.set_gravity
entity.set_friction = self.set_friction
entity.set_mass = self.set_mass
entity.set_elasticity = self.set_elasticity
entity.apply_force = self.apply_force
entity.scale_movement_in_direction = self.scale_movement_in_direction
def get_instantaneous_velocity(self):
entity = self.entity
x, y = entity.position
lx, ly = self.last_position
x -= lx
y -= ly
return Vector(entity.name + " instantaneous velocity", x, y)
def set_mass(self, value):
self.mass = value
def set_elasticity(self, value):
self.elasticity = value
def set_gravity(self, value):
self.gravity = value
def set_friction(self, value):
self.friction = value
def scale_movement_in_direction(self, angle, value):
self.velocity.scale_in_direction(angle, value)
def apply_force(self, i, j):
self.forces.append(Vector("acceleration force", i, j))
def integrate_forces(self):
forces = self.forces
self.forces = []
i, j = 0, 0
for f in forces:
i += f.i_hat
j += f.j_hat
self.velocity.i_hat += i
self.velocity.j_hat += j
def apply_velocity(self):
if self.mass:
movement = self.velocity.get_copy(scale=(1 /
self.mass)).get_value()
self.entity.move(movement)
def update(self):
self.last_position = self.entity.position
self.integrate_forces()
# friction
self.velocity.scale(self.friction)
# gravity
if self.gravity:
g = self.gravity * self.mass
self.apply_force(0, g)
# movement
self.apply_velocity()
@staticmethod
def wall_velocity_test(wall, sprite):
n = wall.get_normal()
n.rotate(.5)
points = sprite.get_collision_points()
v = sprite.get_velocity()
if n.check_orientation(v):
for point in points:
collision = wall.vector_collision(v, point)
if collision:
return point
@staticmethod
def test_wall_collision(wall, sprite):
v_test = PhysicsInterface.wall_velocity_test(wall, sprite)
if v_test:
return v_test
else:
s_test = PhysicsInterface.wall_skeleton_test(wall, sprite)
return s_test
@staticmethod
def wall_skeleton_test(wall, sprite):
v = sprite.get_velocity()
n = wall.get_normal()
if not n.check_orientation(v):
skeleton = sprite.get_collision_skeleton()
angle = n.get_angle() + .5
angle -= angle // 1
r = (0 <= angle < .125) or (.875 <= angle <= 1)
t = .125 <= angle < .375
l = .375 <= angle < .675
b = .675 <= angle < .875
h = l or r
v = t or b
if h:
w = skeleton[0]
collision = wall.vector_collision(w, w.origin)
if collision:
if l:
return w.origin
if r:
return w.end_point
if v:
w = skeleton[1]
collision = wall.vector_collision(w, w.origin)
if collision:
if t:
return w.origin
if b:
return w.end_point
@staticmethod
def smooth_wall_collision(wall, sprite, point):
v = sprite.get_velocity()
sprite.move(wall.get_normal_adjustment(v.apply_to_point(point)))
normal = wall.get_normal()
sprite.scale_movement_in_direction(normal.get_angle(), 0)
@staticmethod
def bounce_wall_collision(wall, sprite, point):
v = sprite.get_velocity()
sprite.move(wall.get_normal_adjustment(v.apply_to_point(point)))
normal = wall.get_normal()
sprite.scale_movement_in_direction(normal.get_angle(), -1)
@staticmethod
def test_sprite_collision(sprite, other):
r1 = sprite.get_collision_rect()
r2 = other.get_collision_rect()
return r1.get_rect_collision(r2)
@staticmethod
def handle_sprite_collision(sprite, other, collision):
if collision:
def check_orientation(s):
x, y = s.get_collision_rect().center
cx, cy = collision
heading = Vector("heading", cx - x, cy - y)
return s.get_velocity().check_orientation(heading)
def get_adjustment(o):
x, y = o.get_collision_rect().center
cx, cy = collision
v = Vector("collision adjustment", cx - x, cy - y)
return v
def do_adjustment(s, o):
v = get_adjustment(o)
if check_orientation(s):
s.scale_movement_in_direction(v.get_angle(), 0)
v.scale(1 - s.physics_interface.elasticity)
s.apply_force(*v.get_value())
do_adjustment(sprite, other)
do_adjustment(other, sprite)
