Calculations/MOT Capture Process Simulation/@MOTSimulator/calculateLocalSaturationIntensity.m

function ret = calculateLocalSaturationIntensity(~, PeakIntensity, PositionVector, WaveVectorOrigin, WaveVectorEndPoint, BeamRadius, BeamWaist)
    WaveVector                          = WaveVectorEndPoint - WaveVectorOrigin; % Line
    PositionVectorFromWaveVectorOrigin  = PositionVector     - WaveVectorOrigin; % Point = PositionVector
    
    %Height of parallelogram (Distance between point and line) = Area of parallelogram / Base
    %One side of parallelogram = PositionVectorFromWaveVectorOrigin
    %Base = Wavevector
    %Area = One side of parallelogram X Base
    DistanceBetweenAtomAndLaserBeamAxis = norm(cross(PositionVectorFromWaveVectorOrigin, WaveVector))./ norm(WaveVector); % Slow
    %DistanceBetweenAtomAndLaserBeamAxis = norm((WaveVector*WaveVector')*PositionVectorFromWaveVectorOrigin-(WaveVector*PositionVectorFromWaveVectorOrigin')*WaveVector)./ ...
    %                                      (WaveVector(1)^2+WaveVector(2)^2+WaveVector(3)^2); % Faster
    
    if DistanceBetweenAtomAndLaserBeamAxis <= BeamRadius
        ret = PeakIntensity * exp(-2*DistanceBetweenAtomAndLaserBeamAxis^2 / BeamWaist^2);
    else
        ret = 0;
    end
end