Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
2026-09-15 14:13:49 +08:00
commit 7e4ef6f98b
7002 changed files with 1452249 additions and 0 deletions
@@ -0,0 +1,207 @@
+import math
+
+
+def distance(p1, p2):
+    """
+    Calculates the distance between two points.
+
+    :param p1, p2: points
+    :return: distance between points
+    """
+    p1x, p1y = p1
+    p2x, p2y = p2
+
+    return math.sqrt((p1x - p2x) ** 2 + (p2y - p2y) ** 2)
+
+
+def knn(points, p, k):
+    """
+    Calculates the k nearest neighbours of a point.
+
+    :param points: list of points
+    :param p: reference point
+    :param k: amount of neighbours
+    :return: list of k neighbours
+    """
+    return sorted(points, key=lambda x: distance(p, x))[:k]
+
+
+def intersects(p1, p2, p3, p4):
+    """
+    Checks if the lines [p1, p2] and [p3, p4] intersect.
+
+    :param p1, p2: line
+    :param p3, p4: line
+    :return: lines intersect
+    """
+    p0x, p0y = p1
+    p1x, p1y = p2
+    p2x, p2y = p3
+    p3x, p3y = p4
+
+    s10x = p1x - p0x
+    s10y = p1y - p0y
+    s32x = p3x - p2x
+    s32y = p3y - p2y
+
+    denom = s10x * s32y - s32x * s10y
+    if denom == 0:
+        return False
+
+    denom_positive = denom > 0
+    s02x = p0x - p2x
+    s02y = p0y - p2y
+    s_numer = s10x * s02y - s10y * s02x
+    if (s_numer < 0) == denom_positive:
+        return False
+
+    t_numer = s32x * s02y - s32y * s02x
+    if (t_numer < 0) == denom_positive:
+        return False
+
+    if (s_numer > denom) == denom_positive or (t_numer > denom) == denom_positive:
+        return False
+
+    t = t_numer / denom
+    x = p0x + (t * s10x)
+    y = p0y + (t * s10y)
+
+    return (x, y) not in [p1, p2, p3, p4]
+
+
+def angle(p1, p2, previous=0):
+    """
+    Calculates the angle between two points.
+
+    :param p1, p2: points
+    :param previous: previous angle
+    :return: angle
+    """
+    p1x, p1y = p1
+    p2x, p2y = p2
+
+    return (math.atan2(p1y - p2y, p1x - p2x) - previous) % (math.pi * 2) - math.pi
+
+
+def point_in_polygon(point, polygon):
+    """
+    Checks if a point is inside a polygon.
+
+    :param point: point
+    :param polygon: polygon
+    :return: point is inside polygon
+    """
+    px, py = point
+
+    size = len(polygon)
+    for i in range(size):
+        p1x, p1y = polygon[i]
+        p2x, p2y = polygon[(i + 1) % size]
+        if min(p1x, p2x) < px <= max(p1x, p2x):
+            p = p1y - p2y
+            q = p1x - p2x
+            y = (px - p1x) * p / q + p1y
+            if y < py:
+                return True
+
+    return False
+
+
+def concave(points, k=3):
+    """
+    Calculates the concave hull for a list of points. Each point is a tuple
+    containing the x- and y-coordinate. k defines the number of considered
+    neighbours.
+
+    :param points: list of points
+    :param k: considered neighbours
+    :return: concave hull
+    """
+    dataset = list(set(points))  # Remove duplicates
+    if len(dataset) < 3:
+        raise Exception("Dataset length cannot be smaller than 3")
+    if len(dataset) == 3:
+        return dataset  # Points are a polygon already
+    
+    k = min(max(k, 3), len(dataset) - 1)  # Make sure that k neighbours can be found
+
+    first = current = min(dataset, key=lambda x: x[1])
+    hull = [first]  # Initialize hull
+    dataset.remove(first)  # Remove processed point
+    previous_angle = 0
+
+    while (current != first or len(hull) == 1) and len(dataset) > 0:
+        if len(hull) == 3:
+            dataset.append(first)  # Add first point again
+
+        neighbours = knn(dataset, current, k)
+        c_points = sorted(neighbours, key=lambda x: -angle(x, current, previous_angle))
+
+        its = True
+        i = -1
+        while its and i < len(c_points) - 1:
+            i += 1
+            last_point = 1 if c_points[i] == first else 0
+            j = 1
+            its = False
+            while not its and j < len(hull) - last_point:
+                its = intersects(hull[-1], c_points[i], hull[-j - 1], hull[-j])
+                j += 1
+
+        if its:  # All points intersect, try again with higher a number of neighbours
+            return concave(points, k + 1)
+
+        previous_angle = angle(c_points[i], current)
+        current = c_points[i]
+        hull.append(current)  # Valid candidate was found
+        dataset.remove(current)
+
+    for point in dataset:
+        if not point_in_polygon(point, hull):
+            return concave(points, k + 1)
+
+    return hull
+
+
+def cross(o, a, b):
+    """
+    Calculates cross between two vectors.
+
+    :param o, a: vector
+    :param o, b: vector
+    :return: cross product
+    """
+    ox, oy = o
+    ax, ay = a
+    bx, by = b
+
+    return (ax - ox) * (by - oy) - (ay - oy) * (bx - ox)
+
+
+def convex(points):
+    """
+    Calculates the concave hull for a list of points. Each point is a tuple
+    containing the x- and y-coordinate.
+
+    :param points: list of points
+    :return: convex hull
+    """
+    dataset = sorted(set(points))  # Remove duplicates
+    if len(dataset) <= 1:
+        return dataset
+
+    # Build lower hull
+    lower = []
+    for p in dataset:
+        while len(lower) >= 2 and cross(lower[-2], lower[-1], p) <= 0:
+            lower.pop()
+        lower.append(p)
+
+    # Build upper hull
+    upper = []
+    for p in reversed(dataset):
+        while len(upper) >= 2 and cross(upper[-2], upper[-1], p) <= 0:
+            upper.pop()
+        upper.append(p)
+
+    return lower[:-1] + upper[:-1]