Too slow shortest path algorithm

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 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 priortity 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.

Here is my code http://pastebin.com/pQS29Vw9 Is there any option how to make 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;
}

Solution

Three possible algorithmic improvements to consider:

Improvement to search

Djikstra's algorithm will explore all points within S of the start node, where S is the shortest distance between the start and the end.

If you use an A* search (e.g. with a heuristic of the Euclidean distance to the goal) then you should find that many fewer points need to be explored.

Improvement to edge construction

Depending on how the points are distributed, you may find it better to find the edges within a distance D by:

Imagine a grid of side length D being overlaid on the plane
Add each point into a bucket corresponding to which grid square it belongs to
When you need to find the neighbours of a point, you only need to test points in the neighbouring buckets instead of every point.

Improvement to preprocessing

Depending on the distribution of the points, you may well find that it is more efficient to only construct the valid edges when you reach a vertex, rather than precalculating all edges.

This potentially saves a lot of time if the start and destination are close.