/*
Есть определенные модели на матлабе, которые написаны кем-то и когда-то. Иногда нужно что-то переписывать для ускорения, но при переписывании возникают опечатки. Да и модели не факт, что правильные, так что юнит-тесты не панацея, почему бы статической типизации не помочь?
Удобно при этом задавать исходные как они даны - в миллиметрах или метрах:
*/
//радиус диафрагмы в основании бленды
coord r0(9.5 / 2 * mm);
//радиус диафрагмы 1
coord r1(28.9503 / 2 * mm);
//радиус диафрагмы 2
coord r2(46.3001 / 2 * mm);
//расстояние между диафрагмами 0 и 1
coord d1(5.2 * mm);
//расстояние между диафрагмами 1 и 2
coord d2(4.7 * mm);
//высота диафрагмы 0 от плоскости ФПУ
coord h0(hd + f);
//высота диафрагмы 1 от плоскости ФПУ
coord h1(d1 + hd + f);
//высота диафрагмы 2 от плоскости ФПУ
coord h2(d1 + d2 + hd + f);
//Положение небесных тел
coord Hz(500e3 * meter);
coord Rz(6371e3 * meter);
Angle fi_z(asin( Rz / (Rz + Hz)));
Angle alf_z(0.0 * degree);
Angle bet_z(0.0 * degree);
/*
Что должны из себя представлять типы coord и Angle? Можно сделать
*/
typedef double coord;
typedef double Angle;
coord mm = 10E-3.
coord degree = pi / 180.0;
/*
Нормальный выход для динамического языка. Но в этом случае можно где-то в коде умножить два раза:
*/
Angle alpha = 5 * degrees;
Angle beta = alpha * degrees;
/*
На помощь приходит библиотека boost::units:
*/
#include <boost/math/constants/constants.hpp>
#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/cgs/io.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
const double pi = boost::math::constants::pi<double>();
typedef boost::units::quantity<boost::units::si::plane_angle> Angle;
typedef boost::units::quantity<boost::units::si::length> coord;
using boost::units::si::radians;
using boost::units::si::meter;
coord mm = 10e-3 * boost::units::si::meter;
Angle degree = pi / 180.0 * boost::units::si::radians;
/*
Все, в этом случае мы в большой степени застрахованы от опечаток в исходных данных.
Но код не хотелось бы закладывать на эту библиотеку - шаблоны на MPL долго собираются, собираются не везде и вообще не понятно, зачем это делать. Вся логика обобщена таким образом.
*/
template<class T> class Ray
{
private:
        bool f_intersected;
public:
        V<T> r;
        V<T> d;
        template<class Ang> bool intersectsSphere(const T& tH, const T& tR, const Ang& salf, const Ang& sbet)
        {
//Код из матлаба почти 1 в 1
                T l = tH + tR;
                T fx = l * sin(salf) / sqrt(1.0 + tan(sbet)*tan(sbet) * cos(salf)*cos(salf));
                T fy = l * sin(sbet) / sqrt(1.0 + tan(salf)*tan(salf) * cos(sbet)*cos(sbet));
                double sgn = 1;
                if (cos(salf) < 0.0 || cos(sbet) < 0.0)
                {
                        sgn = -1.0;
                }
                T fz = sgn * l / sqrt(1.0 + tan(salf) * tan(salf) + tan(sbet) * tan(sbet));

                auto as = d.x * d.x + d.y * d.y + d.z * d.z;
                auto bs = 2.0 * (d.x * (r.x - fx) + d.y * (r.y - fy) + d.z * (r.z - fz));
                auto cs = (r.x - fx) * (r.x - fx) +
                              (r.y - fy) * (r.y - fy) +
                                  (r.z - fz) * (r.z - fz)- tR * tR;

                bool intrsct = false;
                auto Ds = (bs * bs - 4.0 * as * cs);
                decltype(Ds) zero(0);
                if (Ds >= zero)
                {
                        auto tau = (-bs + sqrt(Ds)) / 2.0 / as;
                        T x = r.x + tau * d.x;
                        T y = r.y + tau * d.y;
                        T z = r.z + tau * d.z;
                        intrsct = z >= T(0);
                }
                f_intersected = f_intersected && intrsct;
                return intrsct;
        }
};

/*
Вычисления практически ортогональны языку, не считая глобального стейта f_intersected.
*/