Method of repulsion based on Helbing model

I'm developing a crowd simulator following the Helbing Social Forces model, but I'm having a problem the moment the collision between the agents happens.

When the collision occurs the repulsion method is called to calculate the new position for agent A and agent B, but when calculating the new position it is giving a very displaced position of the position where the collision occurred, ie my agents they are jumping backwards, sides and so on.

The algorithm should work the same fluid this video or this .

Follow my repulsion code:

    private double SIGMA = 0.3f; //a distância no qual o efeito territorial será válido
    private double VELOCIDADE_MAX = 1.34f;
    private double LAMBDA = 0.2f;
    private double FORCACORPO = 3f; //12
    private double FORCAFRICCAO = 3f;
    private double LIMITE = 2.0f;
    private double R = 0.2f;
    private double T = 0.5f;
    private double EPSILON = 0.55f;
    private double intensidadeInteracao = 2.5;  

public void RepulsiveEffect(Humano a, Humano b, int k) {
        Point2D vDistancia = new Point2D(a.getCx() - b.getCx(), a.getCy() - b.getCy());

        //Distancia dos pedestres entre seus centros de massa
        //Distance of pedestrians between their centers of mass
        double distanciaAB = Math.sqrt(Math.pow(vDistancia.getX(), 2) + Math.pow(vDistancia.getY(), 2));

        //Normalizacao do vetor
        //Vector normalization
        Point2D vNormalizado = new Point2D(vDistancia.getX() / distanciaAB, vDistancia.getY() / distanciaAB);
        double rab = a.getRaio() + b.getRaio();
        double forcaRepulsiva = intensidadeInteracao * (Math.pow(Math.E, (rab - distanciaAB)) / SIGMA);
        double anisA = 1.0f;
        double anisB = 1.0f;

        //Fatores de Direcao de a
        //Direction Factors of a
        Point2D direcaoADestino = new Point2D(a.getDestino().getX() - a.getCx(), a.getDestino().getY() - a.getCy());
        double distanciaADestino = Math.sqrt(Math.pow(direcaoADestino.getX(), 2) + Math.pow(direcaoADestino.getY(), 2));
        double cosa = (-direcaoADestino.getX() * vDistancia.getX() - direcaoADestino.getY() * vDistancia.getY()) / (distanciaADestino * distanciaAB);
        anisA = LAMBDA + (1 - LAMBDA) * (1 + cosa) * 0.5;

        //Fatores de Direcao de b
        //Direction Factors of b
        Point2D direcaoBDestino = new Point2D(b.getDestino().getX() - b.getCx(), b.getDestino().getY() - b.getCy());
        double distanciaBDestino = Math.sqrt(Math.pow(direcaoBDestino.getX(), 2) + Math.pow(direcaoBDestino.getY(), 2));
        double cosb = (-direcaoBDestino.getX() * vDistancia.getX() + direcaoBDestino.getY() * vDistancia.getY()) / (distanciaBDestino * distanciaAB);
        anisB = LAMBDA + (1 - LAMBDA) * (1 + cosb) * 0.5;

        //Atualizacao da Velocidade de a
        //Speed update of a
        double nxa = atual.get(a.getID()).getX() + (forcaRepulsiva * vNormalizado.getX()) * anisA;
        double nya = atual.get(a.getID()).getY() + (forcaRepulsiva * vNormalizado.getY()) * anisA;
        atual.set(a.getID(), new Point2D(nxa, nya));

        //Atualizacao da Velocidade de B (subtracao pois vetor normal é oposto)
        //Speed update of B (Subtraction since normal vector is opposite)
        double nxb = atual.get(b.getID()).getX() - (forcaRepulsiva * vNormalizado.getX()) * anisB;
        double nyb = atual.get(b.getID()).getY() - (forcaRepulsiva * vNormalizado.getY()) * anisB;
        atual.set(b.getID(), new Point2D(nxb, nyb));

        //Colisao
        //Colision
        if (k == 1) {
            //Forca corpo
            //Force body
            double k1 = FORCACORPO * (rab - distanciaAB);
            nxa = atual.get(a.getID()).getX();
            nya = atual.get(a.getID()).getY();

            nxa += k1 * vNormalizado.getX();
            nya += k1 * vNormalizado.getY();

            atual.set(a.getID(), new Point2D(nxa, nya));

            nxb = atual.get(b.getID()).getX();
            nyb = atual.get(b.getID()).getY();
            nxb -= k1 * vNormalizado.getX();
            nyb -= k1 * vNormalizado.getY();

            atual.set(b.getID(), new Point2D(nxb, nyb));

            //Forca friccao de a
            //Force friction of a
            double kba = 1.0f;
            Point2D tab = new Point2D(-vNormalizado.getX(), vNormalizado.getY());
            Point2D vba = new Point2D(b.getVetorVelocidade().getX() - a.getVetorVelocidade().getX(), b.getVetorVelocidade().getY() - a.getVetorVelocidade().getY());
            double vbaf = Math.sqrt(Math.pow(vba.getX(), 2) + Math.pow(vba.getY(), 2));

            kba = FORCAFRICCAO * (rab - distanciaAB) * vbaf;
            kba = FORCAFRICCAO * (rab - distanciaAB) * (a.getVetorVelocidade().getX() * tab.getX() + a.getVetorVelocidade().getY() * tab.getY());

            nxa = atual.get(a.getID()).getX();
            nya = atual.get(a.getID()).getY();

            nxa += kba * tab.getX();
            nya += kba * tab.getY();
            atual.set(a.getID(), new Point2D(nxa, nya));

            //Forca fricção de b
            //Force friction of a
            double kab = 1.0f;
            Point2D tba = new Point2D(vNormalizado.getX(), -vNormalizado.getY());
            Point2D vab = new Point2D(a.getVetorVelocidade().getX() - b.getVetorVelocidade().getX(), a.getVetorVelocidade().getY() - b.getVetorVelocidade().getY());
            double vabf = Math.sqrt(Math.pow(vab.getX(), 2) + Math.pow(vab.getY(), 2));

            kab = FORCAFRICCAO * (rab - distanciaAB) * vabf;
            kab = FORCAFRICCAO * (rab - distanciaAB) * (b.getVetorVelocidade().getX() * tab.getX() + b.getVetorVelocidade().getY() * tab.getY());

            nxb = atual.get(b.getID()).getX();
            nyb = atual.get(b.getID()).getY();

            nxb += kab * tba.getX();
            nyb += kab * tba.getY();
            atual.set(b.getID(), new Point2D(nxb, nyb));
        }
    }