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)