|
| 1 | +import struct |
| 2 | +from typing import List, Tuple, Union |
| 3 | +from zlib import crc32 |
| 4 | + |
| 5 | +import numpy as np |
| 6 | + |
| 7 | +from .frame import Frame |
| 8 | +from .point import Point |
| 9 | + |
| 10 | + |
| 11 | +class PointFrames: |
| 12 | + def __init__(self): |
| 13 | + self.points = [] |
| 14 | + self.frames = [] |
| 15 | + |
| 16 | + def add_point(self, point: Point, positions: Union[np.ndarray, List[Tuple[float, float, float]]] = None): |
| 17 | + if len(self.frames) != 0 and (positions is None or len(positions) != len(self.frames)): |
| 18 | + raise ValueError('When adding a new point to a PointFrame that already has frames, you must provide a positions array for its position at each frame') |
| 19 | + self.points.append(point) |
| 20 | + |
| 21 | + if positions is None: |
| 22 | + return |
| 23 | + |
| 24 | + for frame, position in zip(self.frames, positions): |
| 25 | + frame.positions = np.append(frame.positions, [position], axis=0) |
| 26 | + |
| 27 | + def add_frame(self, frame: Frame): |
| 28 | + if len(self.points) == 0: |
| 29 | + raise ValueError('Cannot add frames to a PointFrame with no points') |
| 30 | + |
| 31 | + if len(frame.positions) != len(self.points): |
| 32 | + raise ValueError('Frame must contain 1 position for each existing point') |
| 33 | + self.frames.append(frame) |
| 34 | + |
| 35 | + def get_position(self, point_index: int, frame_index: int) -> Tuple[float, float, float]: |
| 36 | + return self.frames[frame_index].positions[point_index] |
| 37 | + |
| 38 | + def get_positions(self, frame_index: int) -> np.ndarray: |
| 39 | + return self.frames[frame_index].positions |
| 40 | + |
| 41 | + def apply_offset(self, ax1: float, ax2: float, ax3: float): |
| 42 | + offset = (ax1, ax2, ax3) |
| 43 | + for frame in self.frames: |
| 44 | + frame.positions += np.array(offset) |
| 45 | + |
| 46 | + return self |
| 47 | + |
| 48 | + def apply_rotation(self, deg1: float, deg2: float, deg3: float): |
| 49 | + degrees = np.radians((deg1, deg2, deg3)) |
| 50 | + |
| 51 | + ax1 = np.array([[1, 0, 0], |
| 52 | + [0, np.cos(degrees[0]), -np.sin(degrees[0])], |
| 53 | + [0, np.sin(degrees[0]), np.cos(degrees[0])]]) |
| 54 | + |
| 55 | + ax2 = np.array([[np.cos(degrees[1]), 0, np.sin(degrees[1])], |
| 56 | + [0, 1, 0], |
| 57 | + [-np.sin(degrees[1]), 0, np.cos(degrees[1])]]) |
| 58 | + |
| 59 | + ax3 = np.array([[np.cos(degrees[2]), -np.sin(degrees[2]), 0], |
| 60 | + [np.sin(degrees[2]), np.cos(degrees[2]), 0], |
| 61 | + [0, 0, 1]]) |
| 62 | + |
| 63 | + rotation_matrix = np.dot(ax3, np.dot(ax2, ax1)) |
| 64 | + |
| 65 | + for frame in self.frames: |
| 66 | + frame.positions = np.dot(frame.positions, rotation_matrix.T) |
| 67 | + |
| 68 | + return self |
| 69 | + |
| 70 | + def apply_scale(self, ax1: float, ax2: float, ax3: float): |
| 71 | + scale = (ax1, ax2, ax3) |
| 72 | + for frame in self.frames: |
| 73 | + frame.positions *= np.array(scale) |
| 74 | + |
| 75 | + return self |
| 76 | + |
| 77 | + def save(self, file_path: str): |
| 78 | + with open(file_path, 'wb') as file: |
| 79 | + version = 1 |
| 80 | + |
| 81 | + file.write(b'3CPF') |
| 82 | + file.write(struct.pack('I', version)) |
| 83 | + checksum_offset = file.tell() |
| 84 | + file.write(b'\x00\x00\x00\x00') |
| 85 | + file.write(struct.pack('I', len(self.points))) |
| 86 | + file.write(struct.pack('I', len(self.frames))) |
| 87 | + |
| 88 | + point_color_data = bytearray() |
| 89 | + frame_position_data = bytearray() |
| 90 | + |
| 91 | + for point in self.points: |
| 92 | + point_color_data += struct.pack('3B', *point.color) |
| 93 | + |
| 94 | + for frame in self.frames: |
| 95 | + for position in frame.positions: |
| 96 | + frame_position_data += struct.pack('3f', *position) |
| 97 | + |
| 98 | + file.write(point_color_data) |
| 99 | + file.write(frame_position_data) |
| 100 | + |
| 101 | + checksum = crc32(point_color_data + frame_position_data) & 0xffffffff |
| 102 | + |
| 103 | + with open(file_path, 'r+b') as file: |
| 104 | + file.seek(checksum_offset) |
| 105 | + file.write(struct.pack('I', checksum)) |
| 106 | + |
| 107 | + def __str__(self): |
| 108 | + return f'PointFrames(#points={len(self.points)}, #frames={len(self.frames)})' |
| 109 | + |
| 110 | + def __repr__(self): |
| 111 | + return f'PointFrames(points={repr(self.points)}, frames={repr(self.frames)})' |
| 112 | + |
| 113 | +def load(file_path: str, coordinate_order: str = 'xyz') -> PointFrames: |
| 114 | + animation = PointFrames() |
| 115 | + |
| 116 | + coordinate_order = coordinate_order.lower() |
| 117 | + if len(coordinate_order) != 3 or set(coordinate_order) != {'x', 'y', 'z'}: |
| 118 | + raise ValueError("coordinate_order must be a 3-letter string containing 'x', 'y', and 'z'") |
| 119 | + |
| 120 | + with open(file_path, 'rb') as file: |
| 121 | + magic_number = file.read(4) |
| 122 | + if magic_number != b'3CPF': |
| 123 | + raise ValueError('Invalid file format') |
| 124 | + |
| 125 | + version_number, checksum, total_points, total_frames = struct.unpack('4I', file.read(16)) |
| 126 | + |
| 127 | + data_bytes = file.read() |
| 128 | + calculated_checksum = crc32(data_bytes) & 0xffffffff |
| 129 | + if calculated_checksum != checksum: |
| 130 | + raise ValueError('Data corruption detected') |
| 131 | + |
| 132 | + offset = 0 |
| 133 | + for _ in range(total_points): |
| 134 | + r_color, g_color, b_color = struct.unpack_from('3B', data_bytes, offset) |
| 135 | + animation.points.append(Point(r_color, g_color, b_color)) |
| 136 | + offset += 3 |
| 137 | + |
| 138 | + indices = [coordinate_order.index('x'), coordinate_order.index('y'), coordinate_order.index('z')] |
| 139 | + for _ in range(total_frames): |
| 140 | + positions = np.empty((total_points, 3), dtype=np.float32) |
| 141 | + for point_index in range(total_points): |
| 142 | + coords = struct.unpack_from('3f', data_bytes, offset) |
| 143 | + positions[point_index] = [coords[i] for i in indices] |
| 144 | + offset += 12 |
| 145 | + animation.frames.append(Frame(positions)) |
| 146 | + |
| 147 | + return animation |
0 commit comments