def floyd_warshall(W: np.matrix, print_k: bool = False): W = np.copy(W) P = create_P(W) n = len(W) print("starting P") print(f"{P=}") for k in range(1, n + 1): for i in range(1, n + 1): for j in range(1, n + 1): current = W.item((i - 1, j - 1)) candidate = W.item((i - 1, k - 1)) + W.item((k - 1, j - 1)) if current > candidate: debug and print( f"Currently doing {i} -> {j} " + f"for a cost of {current} " + f"but we can do better going through {k} " + f"for a cost of {candidate} instead. " f"current value at {P.item((k - 1, j - 1))=}") W.itemset((i - 1, j - 1), candidate) P.itemset((i - 1, j - 1), P.item((k - 1, j - 1))) if print_k: print(f"for {k=}") print(f"{W=}") print(f"{P=}") return W, P
def setRotation(cls, m: np.matrix, rotation: list): rmat = quaternion_matrix(rotation) m.itemset(0, 0, rmat.item(0, 0)) m.itemset(0, 1, rmat.item(0, 1)) m.itemset(0, 2, rmat.item(0, 2)) m.itemset(1, 0, rmat.item(1, 0)) m.itemset(1, 1, rmat.item(1, 1)) m.itemset(1, 2, rmat.item(1, 2)) m.itemset(2, 0, rmat.item(2, 0)) m.itemset(2, 1, rmat.item(2, 1)) m.itemset(2, 2, rmat.item(2, 2))
def setScale(cls, m: np.matrix, scale: list): m.itemset(0, 0, scale[0]) m.itemset(1, 1, scale[1]) m.itemset(2, 2, scale[2])
def setTranslation(cls, m: np.matrix, pos: list): m.itemset(3, 0, pos[0]) m.itemset(3, 1, pos[1]) m.itemset(3, 2, pos[2])