import numpy as np # import scipy.sparse as sparse import visualization.Animation as Animation """ Family Functions """ def joints(parents): """ Parameters ---------- parents : (J) ndarray parents array Returns ------- joints : (J) ndarray Array of joint indices """ return np.arange(len(parents), dtype=int) def joints_list(parents): """ Parameters ---------- parents : (J) ndarray parents array Returns ------- joints : [ndarray] List of arrays of joint idices for each joint """ return list(joints(parents)[:, np.newaxis]) def parents_list(parents): """ Parameters ---------- parents : (J) ndarray parents array Returns ------- parents : [ndarray] List of arrays of joint idices for the parents of each joint """ return list(parents[:, np.newaxis]) def children_list(parents): """ Parameters ---------- parents : (J) ndarray parents array Returns ------- children : [ndarray] List of arrays of joint indices for the children of each joint """ def joint_children(i): return [j for j, p in enumerate(parents) if p == i] return list(map(lambda j: np.array(joint_children(j)), joints(parents))) def descendants_list(parents): """ Parameters ---------- parents : (J) ndarray parents array Returns ------- descendants : [ndarray] List of arrays of joint idices for the descendants of each joint """ children = children_list(parents) def joint_descendants(i): return sum([joint_descendants(j) for j in children[i]], list(children[i])) return list(map(lambda j: np.array(joint_descendants(j)), joints(parents))) def ancestors_list(parents): """ Parameters ---------- parents : (J) ndarray parents array Returns ------- ancestors : [ndarray] List of arrays of joint idices for the ancestors of each joint """ decendants = descendants_list(parents) def joint_ancestors(i): return [j for j in joints(parents) if i in decendants[j]] return list(map(lambda j: np.array(joint_ancestors(j)), joints(parents))) """ Mask Functions """ def mask(parents, filter): """ Constructs a Mask for a give filter A mask is a (J, J) ndarray truth table for a given condition over J joints. For example there may be a mask specifying if a joint N is a child of another joint M. This could be constructed into a mask using `m = mask(parents, children_list)` and the condition of childhood tested using `m[N, M]`. Parameters ---------- parents : (J) ndarray parents array filter : (J) ndarray -> [ndarray] function that outputs a list of arrays of joint indices for some condition Returns ------- mask : (N, N) ndarray boolean truth table of given condition """ m = np.zeros((len(parents), len(parents))).astype(bool) jnts = joints(parents) fltr = filter(parents) for i, f in enumerate(fltr): m[i, :] = np.any(jnts[:, np.newaxis] == f[np.newaxis, :], axis=1) return m def joints_mask(parents): return np.eye(len(parents)).astype(bool) def children_mask(parents): return mask(parents, children_list) def parents_mask(parents): return mask(parents, parents_list) def descendants_mask(parents): return mask(parents, descendants_list) def ancestors_mask(parents): return mask(parents, ancestors_list) """ Search Functions """ def joint_chain_ascend(parents, start, end): chain = [] while start != end: chain.append(start) start = parents[start] chain.append(end) return np.array(chain, dtype=int) """ Constraints """ def constraints(anim, **kwargs): """ Constraint list for Animation This constraint list can be used in the VerletParticle solver to constrain a animation global joint positions. Parameters ---------- anim : Animation Input animation masses : (F, J) ndarray Optional list of masses for joints J across frames F defaults to weighting by vertical height Returns ------- constraints : [(int, int, (F, J) ndarray, (F, J) ndarray, (F, J) ndarray)] A list of constraints in the format: (Joint1, Joint2, Masses1, Masses2, Lengths) """ masses = kwargs.pop('masses', None) children = children_list(anim.parents) constraints = [] points_offsets = Animation.offsets_global(anim) points = Animation.positions_global(anim) if masses is None: masses = 1.0 / (0.1 + np.absolute(points_offsets[:, 1])) masses = masses[np.newaxis].repeat(len(anim), axis=0) for j in range(anim.shape[1]): """ Add constraints between all joints and their children """ for c0 in children[j]: dists = np.sum((points[:, c0] - points[:, j]) ** 2.0, axis=1) ** 0.5 constraints.append((c0, j, masses[:, c0], masses[:, j], dists)) """ Add constraints between all children of joint """ for c1 in children[j]: if c0 == c1: continue dists = np.sum((points[:, c0] - points[:, c1]) ** 2.0, axis=1) ** 0.5 constraints.append((c0, c1, masses[:, c0], masses[:, c1], dists)) return constraints """ Graph Functions """ def graph(anim): """ Generates a weighted adjacency matrix using local joint distances along the skeletal structure. Joints which are not connected are assigned the weight `0`. Joints which actually have zero distance between them, but are still connected, are perturbed by some minimal amount. The output of this routine can be used with the `scipy.sparse.csgraph` routines for graph analysis. Parameters ---------- anim : Animation input animation Returns ------- graph : (N, N) ndarray weight adjacency matrix using local distances along the skeletal structure from joint N to joint M. If joints are not directly connected are assigned the weight `0`. """ graph = np.zeros(anim.shape[1], anim.shape[1]) lengths = np.sum(anim.offsets ** 2.0, axis=1) ** 0.5 + 0.001 for i, p in enumerate(anim.parents): if p == -1: continue graph[i, p] = lengths[p] graph[p, i] = lengths[p] return graph def distances(anim): """ Generates a distance matrix for pairwise joint distances along the skeletal structure Parameters ---------- anim : Animation input animation Returns ------- distances : (N, N) ndarray array of pairwise distances along skeletal structure from some joint N to some joint M """ distances = np.zeros((anim.shape[1], anim.shape[1])) generated = distances.copy().astype(bool) joint_lengths = np.sum(anim.offsets ** 2.0, axis=1) ** 0.5 joint_children = children_list(anim) joint_parents = parents_list(anim) def find_distance(distances, generated, prev, i, j): """ If root, identity, or already generated, return """ if j == -1: return (0.0, True) if j == i: return (0.0, True) if generated[i, j]: return (distances[i, j], True) """ Find best distances along parents and children """ par_dists = [(joint_lengths[j], find_distance(distances, generated, j, i, p)) for p in joint_parents[j] if p != prev] out_dists = [(joint_lengths[c], find_distance(distances, generated, j, i, c)) for c in joint_children[j] if c != prev] """ Check valid distance and not dead end """ par_dists = [a + d for (a, (d, f)) in par_dists if f] out_dists = [a + d for (a, (d, f)) in out_dists if f] """ All dead ends """ if (out_dists + par_dists) == []: return (0.0, False) """ Get minimum path """ dist = min(out_dists + par_dists) distances[i, j] = dist; distances[j, i] = dist generated[i, j] = True; generated[j, i] = True for i in range(anim.shape[1]): for j in range(anim.shape[1]): find_distance(distances, generated, -1, i, j) return distances def edges(parents): """ Animation structure edges Parameters ---------- parents : (J) ndarray parents array Returns ------- edges : (M, 2) ndarray array of pairs where each pair contains two indices of a joints which corrisponds to an edge in the joint structure going from parent to child. """ return np.array(list(zip(parents, joints(parents)))[1:]) def incidence(parents): """ Incidence Matrix Parameters ---------- parents : (J) ndarray parents array Returns ------- incidence : (N, M) ndarray Matrix of N joint positions by M edges which each entry is either 1 or -1 and multiplication by the joint positions returns the an array of vectors along each edge of the structure """ es = edges(parents) inc = np.zeros((len(parents) - 1, len(parents))).astype(np.int) for i, e in enumerate(es): inc[i, e[0]] = 1 inc[i, e[1]] = -1 return inc.T