detectcollision1 - 有限尺度粒子的碰撞检测

% detectcollision1.m

% detect the collision between a particle and obstructions

% particle center moves from (x1,y1) to (x2,y2) in a line

% particle is represented with a circle (radius = r)

% obstructions are defined in matrix 'elems', element=0: obstruction, element=1: void space

% output: ifcollide, 0: no collision, 1: collision

function ifcollide = detectcollision1(x1,y1,x2,y2,r,elems)

% upper boundary

[uby ubx]=size(elems);

uby=uby-1;

ubx=ubx-1;

% moving direction in x

sgndx=sign(x1-x2);

ifcollide=0;

if sgndx~=0

    % equation of the trajectory of particle center: y=ax+c

    a=(y2-y1)/(x2-x1);

    c=-a*x1+y1;

    sintheta=sqrt(a^2/(a^2+1));

    costheta=sqrt(1/(a^2+1));

    % equation of the perpendicular line: x==-ay+d

    d=(x1+x2)/2+a*(y1+y2)/2;

    halfdist=sqrt((x2-x1)^2+(y2-y1)^2)/2;

    % y limits

    maxy=floor(max(y2,y1)+r*costheta);

    miny=floor(min(y2,y1)-r*costheta);   

    % x limits

    x2ext=floor(x2-sgndx*r*sintheta);

    x1ext=floor(x1+sgndx*r*sintheta);   

    if a>0

        % detect collision from the trajectory of particle transverse

        % diameter (a rectangular)

        for xtrack=x2ext:sgndx:x1ext

            for ytrack=max(miny,ceil(xtrack*a+c-1-r/costheta)):min(maxy,floor((xtrack+1)*a+c+r/costheta))

                if xtrack>=-a*(ytrack+1)+d-1-halfdist/costheta && xtrack<=-a*ytrack+d+halfdist/costheta

                    if xtrack<0 || xtrack>ubx || ytrack<0 || ytrack>uby || elems(ytrack+1,xtrack+1)==0

                        ifcollide=1;

                        return

                    end

                end

            end

        end

        % detect collision from the half circle at the end of trajectory

        for ysurcir=floor(y2-r):floor(y2+r)               

            if ysurcir>maxy || ysurcir<miny

                xsurcirs=floor(x2-r):floor(x2+r);

            else

                if sgndx==1

                    xsurcirs=floor(x2-r):max(x2ext-1,floor(-a*(ysurcir+1)+d-1-halfdist/costheta));

                else

                    xsurcirs=min(x2ext+1,ceil(-a*ysurcir+d+halfdist/costheta)):floor(x2+r);

                end

            end

            for xsurcir=xsurcirs

                dmin=0;

                if x2<xsurcir

                    dmin=dmin+(xsurcir-x2)^2;

                elseif x2>xsurcir+1

                    dmin=dmin+(xsurcir+1-x2)^2;

                end

                if y2<ysurcir

                    dmin=dmin+(ysurcir-y2)^2;

                elseif y2>ysurcir+1

                    dmin=dmin+(ysurcir+1-y2)^2;

                end

                if dmin<=r^2

                    if xsurcir<0 || xsurcir>ubx || ysurcir<0 || ysurcir>uby || elems(ysurcir+1,xsurcir+1)==0 || elems(ysurcir+1,xsurcir+1)==0

                        ifcollide=1;

                        return

                    end

                end

            end

        end

    else

        for xtrack=x2ext:sgndx:x1ext

            for ytrack=max(miny,ceil((xtrack+1)*a+c-1-r/costheta)):min(maxy,floor(xtrack*a+c+r/costheta))

                if xtrack>=-a*ytrack+d-1-halfdist/costheta && xtrack<=-a*(ytrack+1)+d+halfdist/costheta

                    if xtrack<0 || xtrack>ubx || ytrack<0 || ytrack>uby || elems(ytrack+1,xtrack+1)==0

                        ifcollide=1;

                        return

                    end

                end 

            end

        end

        for ysurcir=floor(y2-r):floor(y2+r)               

            if ysurcir>maxy || ysurcir<miny

                xsurcirs=floor(x2-r):floor(x2+r);

            else

                if sgndx==1

                    xsurcirs=floor(x2-r):max(x2ext-1,floor(-a*ysurcir+d-1-halfdist/costheta));

                else

                    xsurcirs=min(x2ext+1,ceil(-a*(ysurcir+1)+d+halfdist/costheta)):floor(x2+r);

                end

            end

            for xsurcir=xsurcirs

                dmin=0;

                if x2<xsurcir

                    dmin=dmin+(xsurcir-x2)^2;

                elseif x2>xsurcir+1

                    dmin=dmin+(xsurcir+1-x2)^2;

                end

                if y2<ysurcir

                    dmin=dmin+(ysurcir-y2)^2;

                elseif y2>ysurcir+1

                    dmin=dmin+(ysurcir+1-y2)^2;

                end

                if dmin<=r^2

                    if xsurcir<0 || xsurcir>ubx || ysurcir<0 || ysurcir>uby || elems(ysurcir+1,xsurcir+1)==0 || elems(ysurcir+1,xsurcir+1)==0

                        ifcollide=1;

                        return

                    end

                end

            end

        end

    end

else

    sgndy=sign(y1-y2);

    if sgndy~=0

        for ytrack=floor(y2):sgndy:floor(y1)

            for xtrack=floor(x1-r):floor(x1+r)

                if ytrack<0 || ytrack>uby || elems(ytrack+1,xtrack+1)==0

                    ifcollide=1;

                    return

                end

            end

        end

        for xsurcir=floor(x1-r):floor(x1+r)

            if sgndy==1

                ysurcirs=floor(y2-r):(floor(y2)-1);

            else

                ysurcirs=ceil(y2):floor(y2+r);

            end

            for ysurcir=ysurcirs

                dmin=0;

                if x2<xsurcir

                    dmin=dmin+(xsurcir-x2)^2;

                elseif x2>xsurcir+1

                    dmin=dmin+(xsurcir+1-x2)^2;

                end

                if y2<ysurcir

                    dmin=dmin+(ysurcir-y2)^2;

                elseif y2>ysurcir+1

                    dmin=dmin+(ysurcir+1-y2)^2;

                end

                if dmin<=r^2

                    if ysurcir<0 || ysurcir>uby || elems(ysurcir+1,xsurcir+1)==0

                        ifcollide=1;

                        return

                    end

                end

            end

        end

    end

end