I have a box (
projectile) floating in the world with a arbitrary transformation in between. Attached to this box is a cylinder (
rope) which is mounted using a ball joint with no parent/child transformation. I now want to let the cylinder point from the projectile to a specific body
p given by its three coordinates.
I tried the following snippet:
axis = self.projectile.getCOM(p) matrix = util.matrix_from_vectors(np.asarray([1, 0, 0]), axis) angles = matrixToEulerXYZ(matrix) joint.setPositions(angles)
But this gives me some weird values which do not seem to be correct.
util.matrix_from_vectors calculates the rotation matrix to align the first vector to the second. The code is as follows (taken somewehere from stackoverflow):
def matrix_from_vectors(vec1, vec2): """ Find the rotation matrix that aligns vec1 to vec2 :param vec1: A 3d "source" vector :param vec2: A 3d "destination" vector :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2. """ a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3) v = np.cross(a, b) c = np.dot(a, b) s = np.linalg.norm(v) if c == -1: return -np.eye(3) elif c == 1: return np.eye(3) kmat = np.array([[0, -v, v], [v, 0, -v], [-v, v, 0]]) rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2)) return rotation_matrix
I have the feeling I am fundamentally wrong with something here, but I just cannot get the hang of it. Help is appreciated