diff --git a/main.py b/main.py index 0581f41..f6b456a 100644 --- a/main.py +++ b/main.py @@ -7,7 +7,8 @@ from matplotlib.widgets import Button, Slider from mpl_toolkits.mplot3d import proj3d -connection = [ +# Configure connections between points (list of (start_index, end_index) using 0-based indices) +LINES = [ (0, 1), (0, 2), (1, 3), @@ -25,17 +26,12 @@ (8, 9), (7, 9), ] -POINT_NAMES: List[str] = [] - -# Configure vectors to draw (list of (start_index, end_index) using 0-based indices) +# Configure vectors to draw (list of (start_index, end_index, color, label) using 0-based indices) # Edit this list to add/remove vectors to display -VECTOR_PAIRS = [(4, 5), (9, 8)] -# Optional colors (will cycle if fewer colors than VECTOR_PAIRS) -VECTOR_COLORS = ["r", "g"] +VECTORS = [(9, 8, "g", "v1"), (4, 5, "r", "v2"), (4, 14, "b", "v3"), (0, 4, "m", "v4")] # Line width for vector arrows VECTOR_LW = 4 -# Optional labels for vectors (defaults to v1, v2, ... if None) -VECTOR_LABELS = None +VECTOR_LEN = 20.0 # Length of vector arrows (can be adjusted as needed) # Label offset factors: fraction of vector (x/y) and fraction of z-range to offset label position # Increase these to place labels further from the vector LABEL_OFFSET_XY = 0.12 @@ -49,12 +45,7 @@ self._verts3d = xs, ys, zs def draw(self, renderer): - xs3d, ys3d, zs3d = self._verts3d - xs, ys, _ = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.get_proj()) - # If projected endpoints are effectively identical, skip drawing to avoid path errors - if abs(xs[0] - xs[1]) < 1e-6 and abs(ys[0] - ys[1]) < 1e-6: - return - self.set_positions((xs[0], ys[0]), (xs[1], ys[1])) + self.do_3d_projection(renderer) try: super().draw(renderer) except Exception: @@ -64,6 +55,9 @@ def do_3d_projection(self, renderer=None): xs3d, ys3d, zs3d = self._verts3d xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.get_proj()) + if abs(xs[0] - xs[1]) < 1e-6 and abs(ys[0] - ys[1]) < 1e-6: + print("Degenerate arrow, skipping drawing") + return 0 self.set_positions((xs[0], ys[0]), (xs[1], ys[1])) return max(zs) @@ -79,49 +73,88 @@ self.points = points # List of (x, y, z) tuples for 14 points -# Function to read point names from the CSV header -def get_point_names(file_path: str) -> List[str]: - global POINT_NAMES - df_header = pd.read_csv(file_path, skiprows=2, nrows=1, header=None) - point_names = [] - for i in range(2, 59, 3): - cell = df_header.iloc[0, i] - if pd.notna(cell) and "New Subject:" in cell: - name = cell.split("New Subject:")[1].strip() - point_names.append(name) - POINT_NAMES = point_names - return point_names +# Function to compute vector from two points +def vector( + p1: Tuple[float, float, float], p2: Tuple[float, float, float] +) -> Tuple[float, float, float]: + return (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]) + + +# Function to normalize a vector +def normalize(v: Tuple[float, float, float]) -> Tuple[float, float, float]: + len = math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]) + if len < 1e-9: + return (0, 0, 0) + return (v[0] / len, v[1] / len, v[2] / len) + + +# Function to compute normalized outer product of two vectors +def outer_product( + v1: Tuple[float, float, float], v2: Tuple[float, float, float] +) -> Tuple[float, float, float]: + x = v1[1] * v2[2] - v1[2] * v2[1] + y = v1[2] * v2[0] - v1[0] * v2[2] + z = v1[0] * v2[1] - v1[1] * v2[0] + return (x, y, z) + + +# Function to compute inner product of two vectors +def dot(v1: Tuple[float, float, float], v2: Tuple[float, float, float]) -> float: + return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] # Function to load thumb data from CSV -def load_thumb_data(file_path: str) -> List[ThumbFrameData]: - get_point_names(file_path) +def load_thumb_data(file_path: str, n_points=14) -> List[ThumbFrameData]: # Read CSV, skip first 5 rows (0-4), use columns 0-43 - df = pd.read_csv(file_path, skiprows=5, usecols=range(44), header=None) + df = pd.read_csv( + file_path, skiprows=2, usecols=range(n_points * 3 + 2), header=None + ) data_list = [] - for _, row in df.iterrows(): - frame_number = int(row[0]) - points = [] - for i in range(14): - x = row[2 + i * 3] - y = row[3 + i * 3] - z = row[4 + i * 3] - points.append((x, y, z)) - data_list.append(ThumbFrameData(frame_number, points)) + point_names = [] + for col, row in df.iterrows(): + if col == 0: + for i in range(n_points): + cell = row[2 + i * 3] + if pd.notna(cell) and "New Subject:" in cell: + name = cell.split("New Subject:")[1].strip() + point_names.append(name) + point_names.append("P15") # Add name for the computed point - return data_list + if col > 2: + frame_number = int(row[0]) + points = [] + for i in range(n_points): + x = float(row[2 + i * 3]) + y = float(row[3 + i * 3]) + z = float(row[4 + i * 3]) + points.append((x, y, z)) + + # Compute the 15th point based on points 4 and 5 using the outer product + outer_p = normalize( + outer_product( + vector(points[0], points[4]), vector(points[5], points[4]) + ) + ) + p15 = ( + points[4][0] + outer_p[0] * VECTOR_LEN, + points[4][1] + outer_p[1] * VECTOR_LEN, + points[4][2] + outer_p[2] * VECTOR_LEN, + ) + points.append(p15) # Add the computed point as the 15th point + + data_list.append(ThumbFrameData(frame_number, points)) + + return point_names, data_list +# Function to compute angle between first two vectors in VECTORS def compute_and_format_angle(pts: List[Tuple[float, float, float]]) -> str: - if len(VECTOR_PAIRS) < 2: + if len(VECTORS) < 2: return "Angle v1-v2: N/A" - (a1, b1) = VECTOR_PAIRS[0] - (a2, b2) = VECTOR_PAIRS[1] - p1s = pts[a1] - p1e = pts[b1] - p2s = pts[a2] - p2e = pts[b2] + (a1, b1, _, _) = VECTORS[1] + (a2, b2, _, _) = VECTORS[2] + p1s, p1e, p2s, p2e = pts[a1], pts[b1], pts[a2], pts[b2] v1 = (p1e[0] - p1s[0], p1e[1] - p1s[1], p1e[2] - p1s[2]) v2 = (p2e[0] - p2s[0], p2e[1] - p2s[1], p2e[2] - p2s[2]) norm1 = math.sqrt(v1[0] ** 2 + v1[1] ** 2 + v1[2] ** 2) @@ -136,7 +169,7 @@ # Function to visualize thumb data with a slider -def visualize_thumb_data(data: List[ThumbFrameData]): +def visualize_thumb_data(data: List[ThumbFrameData], POINT_NAMES: List[str] = []): if not data: return @@ -162,7 +195,7 @@ # Draw connections lines = [] - for conn in connection: + for conn in LINES: idx1, idx2 = conn line = ax.plot( [points[idx1][0], points[idx2][0]], @@ -178,14 +211,25 @@ text = ax.text(p[0], p[1], p[2], name, fontsize=8) texts.append(text) + def vector_coordinates( + p1: Tuple[float, float, float], p2: Tuple[float, float, float] + ) -> Tuple[List[List[float]], List[float]]: + x1, y1, z1 = p1 + x2, y2, z2 = p2 + mid_x = 0.5 * (x1 + x2) + LABEL_OFFSET_XY * (x1 - x2) + mid_y = 0.5 * (y1 + y2) + LABEL_OFFSET_XY * (y1 - y2) + mid_z = 0.5 * (z1 + z2) + LABEL_OFFSET_Z_FACTOR * (z_max - z_min) + midp = [mid_x, mid_y, mid_z] + return [[x1, x2], [y1, y2], [z1, z2]], midp + # Draw vector arrows as specified in VECTOR_PAIRS arrows = [] - for i_pair, (idx1, idx2) in enumerate(VECTOR_PAIRS): - color = VECTOR_COLORS[i_pair % len(VECTOR_COLORS)] if VECTOR_COLORS else "r" + arrow_labels = [] + # z_range = z_max - z_min if z_max > z_min else 1.0 + for i_pair, (idx1, idx2, color, name) in enumerate(VECTORS): + arrow_pts, mid_pts = vector_coordinates(points[idx1], points[idx2]) a = Arrow3D( - [points[idx1][0], points[idx2][0]], - [points[idx1][1], points[idx2][1]], - [points[idx1][2], points[idx2][2]], + *arrow_pts, mutation_scale=20, lw=VECTOR_LW, arrowstyle="-|>", @@ -194,27 +238,10 @@ ax.add_artist(a) arrows.append(a) - # Create labels for each configured vector (v1, v2, ... by default) - arrow_labels = [] - labels = ( - VECTOR_LABELS - if VECTOR_LABELS is not None - else [f"v{i + 1}" for i in range(len(VECTOR_PAIRS))] - ) - z_range = z_max - z_min if z_max > z_min else 1.0 - for i_pair, (idx1, idx2) in enumerate(VECTOR_PAIRS): - x1, y1, z1 = points[idx1] - x2, y2, z2 = points[idx2] - mid_x = 0.5 * (x1 + x2) + LABEL_OFFSET_XY * (x2 - x1) - mid_y = 0.5 * (y1 + y2) + LABEL_OFFSET_XY * (y2 - y1) - mid_z = 0.5 * (z1 + z2) + LABEL_OFFSET_Z_FACTOR * z_range - label = labels[i_pair] lbl = ax.text( - mid_x, - mid_y, - mid_z, - label, - color=VECTOR_COLORS[i_pair % len(VECTOR_COLORS)] if VECTOR_COLORS else "k", + *mid_pts, + name, + color=color, fontsize=10, weight="bold", ) @@ -234,10 +261,10 @@ # Slider ax_slider = plt.axes([0.2, 0.02, 0.5, 0.03]) - slider = Slider(ax_slider, "Frame", 0, len(data) - 1, valinit=0, valstep=1) + slider = Slider(ax_slider, "Frame", 1, len(data), valinit=1, valstep=1) def update(val): - frame_idx = int(slider.val) + frame_idx = int(slider.val) - 1 points = data[frame_idx].points x = [p[0] for p in points] y = [p[1] for p in points] @@ -245,7 +272,7 @@ scat._offsets3d = (x, y, z) # Update lines - for i, conn in enumerate(connection): + for i, conn in enumerate(LINES): idx1, idx2 = conn lines[i].set_data_3d( [points[idx1][0], points[idx2][0]], @@ -258,21 +285,10 @@ texts[i].set_position((p[0], p[1], p[2])) # Update all configured arrows - for i_pair, (idx1, idx2) in enumerate(VECTOR_PAIRS): - arrows[i_pair]._verts3d = ( - [points[idx1][0], points[idx2][0]], - [points[idx1][1], points[idx2][1]], - [points[idx1][2], points[idx2][2]], - ) - - # Update arrow labels positions - for i_pair, (idx1, idx2) in enumerate(VECTOR_PAIRS): - x1, y1, z1 = points[idx1] - x2, y2, z2 = points[idx2] - mid_x = 0.5 * (x1 + x2) + LABEL_OFFSET_XY * (x2 - x1) - mid_y = 0.5 * (y1 + y2) + LABEL_OFFSET_XY * (y2 - y1) - mid_z = 0.5 * (z1 + z2) + LABEL_OFFSET_Z_FACTOR * (z_max - z_min) - arrow_labels[i_pair].set_position((mid_x, mid_y, mid_z)) + for i_pair, (idx1, idx2, color, name) in enumerate(VECTORS): + arrow_pts, mid_pts = vector_coordinates(points[idx1], points[idx2]) + arrows[i_pair]._verts3d = arrow_pts + arrow_labels[i_pair].set_position(mid_pts) # Update angle text between v1 and v2 angle_text.set_text(compute_and_format_angle(points)) @@ -282,15 +298,48 @@ slider.on_changed(update) - # Button: switch to top-down (Z軸方向＝真上) の視点に切り替える + # Button: switch view to direction from points[0] towards points[4] ax_button = plt.axes([0.02, 0.02, 0.1, 0.04]) - btn_top = Button(ax_button, "X-Y plane") + btn_top = Button(ax_button, "P0→P4") - def set_top_view(event): - ax.view_init(elev=90, azim=0) + def set_top_view(points): + # Calculate direction from points[0] to points[4] + forward = vector(points[0], points[4]) + # Calculate azimuth and elevation angles + azim = math.degrees(math.atan2(forward[1], forward[0])) + elev = math.degrees( + math.atan2(forward[2], math.sqrt(forward[0] ** 2 + forward[1] ** 2)) + ) + ax.view_init(elev=elev, azim=azim) + + # # Calculate roll so that vector from points[4] to points[5] points left + # # Target left direction: from points[4] to points[5] + # target_left = normalize(vector(points[14], points[4])) + # # Up vector (Z-axis) + # up = (0, 0, 1) + # # Current right direction: forward × up + # right = normalize(OuterProduct(forward, up)) + # # Current left direction: up × right + # left = OuterProduct(up, right) + # # Calculate roll angle between current left and target left + # cos_roll = ( + # left[0] * target_left[0] + # + left[1] * target_left[1] + # + left[2] * target_left[2] + # ) + # sin_roll = ( + # right[0] * target_left[0] + # + right[1] * target_left[1] + # + right[2] * target_left[2] + # ) + # roll = math.degrees(math.atan2(sin_roll, cos_roll)) + # ax.view_init(elev=elev, azim=azim, roll=roll) + + def btn_top_on_clicked(event): + set_top_view(data[int(slider.val) - 1].points) fig.canvas.draw_idle() - btn_top.on_clicked(set_top_view) + btn_top.on_clicked(btn_top_on_clicked) plt.show() @@ -298,11 +347,12 @@ # Main function to test loading and visualization def main(): # Test the load_thumb_data function - data = load_thumb_data("IwasakiThumbRightKapandji03.csv") + data_file = "IwasakiThumbRightKapandji03.csv" + names, data = load_thumb_data(data_file) print(f"Loaded {len(data)} frames") if data: # Visualize the data - visualize_thumb_data(data) + visualize_thumb_data(data, names) if __name__ == "__main__":