I have this two formulas in Matlab:
the_p=atan2((-y(2)+obs(2)),(-y(1)+obs(1)));
phi_p=acos((y(3)-obs(3))/(norm(y-obs)));
... where y is a 3d point and obs is a 3d point also, representing the position of the ellipsoidal obstacle.
After that, I have:
r_p=r(1)*r(2)*r(3)/sqrt((r(2)*r(3)*cos(the_p)*sin(phi_p))^2+(r(1)*r(3)*sin(the_p)*sin(phi_p))^2+(r(1)*r(2)*cos(phi_p))^2);
... that finds the point on the surface. The ellipsoid is defined in this way:
ellipsoid(obs(1),obs(2),obs(3), r(1), r(2), r(3), 50);
... where r is a vector of the size of ellipsoid. This two formulas should be calculate the angle to the center and nearest point on the surface. Can you confirm that? If yes,how do this two formulas change for a obstacle with cubical shape?