(P,Q)
| 26 | plt.savefig(fn+'.png') |
| 27 | |
| 28 | def compute_distance_between_rotations(P,Q): |
| 29 | #! input two rotation matrices, output the distance between them (unit is rad) |
| 30 | #! P,Q are 3x3 numpy arrays |
| 31 | P = np.asarray(P) |
| 32 | Q = np.asarray(Q) |
| 33 | R = np.matmul(P,Q.swapaxes(1,2)) |
| 34 | theta = np.arccos(np.clip((np.trace(R,axis1 = 1,axis2 = 2) - 1)/2,-1,1)) |
| 35 | return np.mean(theta) |
| 36 | |
| 37 | def bgs(d6s): |
| 38 | bsz = d6s.shape[0] |
nothing calls this directly
no outgoing calls
no test coverage detected