Shortest path algorithm is too slow [closed]

I am solving one problem with shortest path algorithm, but it is too slow.

The problem is that I have N points and these can be connected only if the distance between them is smaller or equal than the D. I have a start index and finish(ciel in code) index and have to return the shortest path in double format.

Firstly I thought that the sqrt is too slow, but when I changed it, it was still too slow. I am backtracking the distance and using sqrt just there for better speed, but it is too slow. I have used a priority queue. For more information, the input consists of the X and Y of the points , D maximal distance to make edge, start index and finish index. There can be max 1000 points.

// The main problem is that, it crashes of memory, I do not think I use so much of it but, it crashes from bad_alloc

Here is my code.

Are there any ways of making it faster, please?

#include <iostream>
#include <stdio.h>
#include <queue>
#include <vector>
#include <math.h>
#include <stdlib.h>
#include <utility>

using namespace std;

const int MAX = 1001;
const int INF = 1e9;

std::vector< std::pair<int, int> > edges[MAX]; // hrany a vzdialenosti medzi bodmi a hranami
int N; // pocet vrcholov
int start, ciel; // start a ciel index

double dijkstra() {
    int vis[N]; // pocet navstiveni daneho bodu
    int prevNodes[N][2];
    for(int i=0;i < N;i++)
        prevNodes[i][1] = INF;

    std::priority_queue< std::pair<int, int> > heap; // halda
    for(int i = 0; i < N; i++) vis[i] = 0;
    heap.push(pair<int, int>(0, start));
    while(!heap.empty())
    {
        pair<int, int> min = heap.top(); // vybratie dalsieho
        heap.pop(); // vyhodenie pozreteho
        min.first *= -1.0; // kvoli spravnemu fungovaniu priority
        int v = min.second; // len pre oko

        vis[v]++;
        if (v == ciel && vis[v] == 1)
        {
            double d = 0.0; 

            int prevIndex = ciel, nextIndex = prevNodes[ciel][0]; 

            while(1)
            {

                for(int j=0;j < edges[nextIndex].size();j++)
                    if(edges[nextIndex][j].first == prevIndex)
                    {

                        d += sqrt(double( edges[nextIndex][j].second ));
                        break;
                    }

                prevIndex = nextIndex; // posunutie
                if(nextIndex == start) // ak sme uz na zaciatku
                    break;
                else
                    nextIndex = prevNodes[nextIndex][0];// posun dalej
            }
            return d; // najkratsia cesta
        }

        for (int i = 0; i < (int) edges[v].size(); i++)
        {
            if (vis[edges[v][i].first] < 1)
            {
                if(prevNodes[edges[v][i].first][1] > min.first + edges[v][i].second)
                {
                    prevNodes[edges[v][i].first][0] = min.second;

                    prevNodes[edges[v][i].first][1] = min.first + edges[v][i].second;
                }
                heap.push(pair<int, int>(-(min.first + edges[v][i].second), edges[v][i].first)); 
            }
        }
    }
    return -1;
}

int main()
{
    int X;
    scanf("%d",&X);
    double answers[X];
    for(int i=0;i < X;i++)
    {
        int D, sIndex, eIndex; // N je globalne
        scanf("%d %d", &N, &D); // N
        int DD = D * D;
        for(int j=0;j < N;j++)
            edges[j].clear();

        int V[N][2]; // N
        int x, y;
        for(int k=0;k < N;k++) // N
        {
            scanf("%d %d", &x, &y);
            V[k][0] = x;
            V[k][1] = y;
        }

        for(int a=0;a < N;a++)
            for(int b=0;b < N;b++)
            {

                int v = (((V[a][0] - V[b][0]) * (V[a][0] - V[b][0]) +
                                (V[a][1] - V[b][1]) * (V[a][1] - V[b][1])));
                if(v > DD)
                    continue;
                else
                {
                    edges[a].push_back(pair<int, int>(b, v));
                    edges[b].push_back(pair<int, int>(a, v));
                }
            }

        scanf("%d %d", &start, &ciel);
        start--;
        ciel--;
        double dijLen = dijkstra();
        if(dijLen < 0)
            answers[i] = -1;
        else
            answers[i] = dijLen;
    }
    for(int i=0;i < X;i++)
        if(answers[i] < 0)
            printf("Plan B\n");
        else
            printf("%.2f\n", answers[i]);

    return 0;
}