Optic_flow_RF_estimation_Zhang_et_al_2022/code/General/CPS_sampling.m

function [sph_pnt,energy] = CPS_sampling(varargin)
p = inputParser;

default.pnt_num = 240;                                                     % Number of particles; for this algorithm, more points the better
default.thre    = 'auto';                                                  % Convergence threshold
default.RepulF  = 0.1;                                                     % Repulsive force factor, the higher the stronger force
default.RstF    = 1.5;                                                     % Resistance force, higher stronger
default.MaxIter = 500;                                                     % Maximum number of iteration if not converged
default.VisIt   = false;                                                   % If make visulization

addOptional(p,'pnt_num',default.pnt_num);
addOptional(p,'thre',default.thre);
addOptional(p,'RepulF',default.RepulF,@(n) n>0&&n<=1);
addOptional(p,'RstF',default.RstF,@(n) n>0&&n<=5);
addOptional(p,'MaxIter',default.MaxIter,@(n) n>0&&(floor(n)==n));
addOptional(p,'VisIt',default.VisIt,@(n) islogical(n));

parse(p,varargin{:});
pnt_num = p.Results.pnt_num;
thre    = p.Results.thre;
RepulF   = p.Results.RepulF;
RstF    = p.Results.RstF;
MaxIter = p.Results.MaxIter;
VisIt   = p.Results.VisIt;

if strcmp(thre,"auto")
    thre = max(1/pnt_num,0.0065);
end

sph_pnt = rand(pnt_num,3)-.5;                                              % Randomly assign particle position on 3D space
sph_pnt = sph_pnt./sqrt(sum(sph_pnt.^2,2));                                % Projected particles onto sphere
sph_pntQN = quaternion([zeros(pnt_num,1),sph_pnt]);                        % Convert 3-element vector to quaternion
currentSpeed = 0;                                                          % Initial moving speed = 0
energy = 1;                                                                % The energy will be lower when the particles are more evenly distributed
iterN  = 0;                                                                % Iteration number

while energy > thre && iterN <= MaxIter
    % Core for charged particle system
    diff_vec  = sph_pntQN - (sph_pntQN.conj)';                             % Calculate the differnece vector between each pair of particle
    vec_dist  = asin(diff_vec.norm./2).*2;                                 % Calculating the angle between each pair of particles
    proj_diff_vec = (diff_vec+sph_pntQN.*diff_vec.*sph_pntQN)./2;          % Project the difference vectors onto the tangent plane for each particle
    vecForce  = 1./vec_dist.^2.*proj_diff_vec./proj_diff_vec.norm;         % Calculating the repulsive force vector from other particles weighted by 1/(the distance)^2
    vecForce  = quaternion(squeeze(nansum(reshape(vecForce.compact,[size(proj_diff_vec),4]),2))); % Calculate the sum of all force vectors as the overall force vector
    vecForce = vecForce./pnt_num.*RepulF;                                   % Adjust the force to an appriate level so the particles won't bounce around or move very slowly
    currentSpeed = currentSpeed+vecForce;                                  % Updating the moving velocity for each particle
    rotAxis   = sph_pntQN.*currentSpeed;                                   % Compute the rotation axis for particle movement on sphere
    rotAxis   = (rotAxis.compact.*[0 1 1 1]);                              % Extract only the cross product
    rotAxis   = quaternion((rotAxis./sqrt(sum(rotAxis.^2,2))+[1 0 0 0])... % Compute the rotation quaternion
                .*[cos(vecForce.norm./2),sin(vecForce.norm./2).*[1,1,1]]); % vecForce.norm is the rotation angle for the movement
    sph_pntQN = rotAxis.*sph_pntQN.*rotAxis.conj;                          % Rotate the particles by the rotation quaternion
    currentSpeed = currentSpeed./RstF;                                     % "Resistance force": prevent particles move too much
    
    energy_all = sort(vec_dist);                                           % These two lines are for computing the energy after movement
    energy = std(energy_all(2,:));
    iterN = iterN+1;
    if rem(iterN,50) ==0
        energy
    end
    if iterN>3000&&energy>.01
        break
    end
    % Visualization part below
    if VisIt
        sph_pnt   = sph_pntQN.compact;                                         
        sph_pnt   = sph_pnt(:,2:4);
        k = delaunayTriangulation(sph_pnt);
        [K,~] = convexHull(k);
        if iterN == 1
            clf
            sct = patch('vertices',sph_pnt,'faces',K,'Facecolor',[0,.25,.5],...
                'FaceAlpha',0.7,'edgecolor',[0,.1,.3],'linewidth',2);          % Visualize the solid formed with these particles
            axis equal vis3d off
            view(120,30)
        elseif rem(iterN,3) == 0
            sct.Faces = K;
            sct.Vertices = sph_pnt;
        end
        title(iterN)
        pause(0.001)
    end
end

if energy > thre
    warning("MaxIter reached; Not Converged")
end

sph_pnt   = sph_pntQN.compact;
sph_pnt   = sph_pnt(:,2:4);
end