How to use tf for custom rotation

edit

Hi!

I am trying to generate the starting points and the ending points of the following arrows:

The idea is that I get the start (yellow, 0,0,2) and the end (orange 1, 1, 5).
The arrows are the path of ray tracing to find obstacles.
These arrows should all be parallel amongst themselves but also orthogonal to the vector start to goal. The search should be within a given safety margin.

I given a detailed description of what I've done bellow but here is what I wanted to ask Since we can specify frames just by giving the offset and the angles and then tf can calculate the transform, I thought there might be a way to create dynamic frames and then use the same functions as tf to find the transform. Is this possible?

The approach I used to generate the image was to imagine a 2D square (over x and z) around both start and goal. Then to find the yaw and roll that make the squares parallel and perform the rotation. This approach fails when the points need to be generated over both x and y ( you can see in the picture that square should be rotated in the direction of the camera). The code is at https://github.com/margaridaCF/Flying...

Then I thought of generating the points in an auxiliary frame at the origin. Always generating the points in x and z and then rotating them back. This would be done combining the following

    final_transform_start = translation_from_center_start * rotation_roll * rotation_yaw;
    final_transform_end   = translation_from_center_end   * rotation_roll * rotation_yaw;

In this approach, I run into issues with angles being interpreted clockwise and counterclockwise (I think) and also how to handle 90º yaw and roll. The code can be found here https://github.com/margaridaCF/Flying...

UPDATE: (Building on the answer from @PeteBlackerThe3rd )

I started with this process but got hanged on point 3 because there was too much freedom to choose and I couldn't figure out how to decide when I got the right one.

Later a colleague pointed out that all I need to do is check if the norm of the normalization of the dot product of the axis with the direction vector is below 0.9 (more or less).
When it is 1 they are parallel and when it's over ~0.9 the cross product will lose accuracy.
If this is the case just use another unit axis. As unit axis are orthogonal, if it is close to parallel in one axis it won't be in the others.

| direction * î | < ~0.9