I read up the comments of the code this morning.
I came to understand what it is doing
the ones that start with the letter 'q' is a directional vector
'w' is a positional vector
// # end spiral sol.c_e = cre - D3DXVECTOR3(0, 0, -dist2*tan(gam)); sol.psi_e = theta - PI / 2; sol.lamda_e = 1; //# hyperplane H_s : switch from first spiral to line sol.w_s = w1; sol.q_s = q1; // # hyperplane H_l: switch from line to last spiral sol.w_l = w2; sol.q_l = q1; // # hyperplane H_e: end of Dubins path sol.w_e = ze; sol.q_e = dot(D3DXVECTOR3(1, 0, 0), rotY(anglend));
It is a RSR turn.
Start with the w_s, which is an end point of a spiral
and q_s is which direction it is pointing towards.
So let's say I start with position (10, 20, 30) and some direction say a radian of 1.57
If a path is found and in the case of RSR turn, there will be a spiral
between (10, 20, 30) and w_s, and the directions are already said,
which is 1.57 from the starting point and ends with q_s
So the problem reduces to finding a sampled point within the spiral itself
The other segments are found exactly the same way.
Unfortunately, I am not good at mathematics, and I couldn't find much materials
on how to go about finding the value of the sampled point...