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])