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[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 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