Repulsion method based on Helbing model

Question

I’m developing a crowd simulator following Helbing’s Social Forces model, only I’m having a problem at the moment of the collision between agents.

When the collision occurs the repulsion method is called to calculate the new position for agent A and agent B, only when calculating the new position it is giving a very misplaced position from the position where the collision occurred or whether my agents are giving back jumps, sides and etc.

The algorithm should work equal fluid that video or that.

Follow my code of repulsion:

    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));
        }
    }