Calculate distance between parameters and target image - image-processing

Question

How can I calculate distance between a fixed parameter and a target image/pixel?
The following code does color recognition, finds the average position, and draws circle on it. It is able to find if the target (averageX and averageY) is close to leftPd, centerPd, or rightPd. I want to change this code as lane tracking which is at least able to find distance value between leftPd parameter variable and left lane or rightPd parameter variable and right lane.
import processing.video.*;
Capture video;
float threshold = 210;
color trackColor;
PVector leftP, centerP, rightP, target;
void setup() {
leftP = new PVector (80,420);
centerP = new PVector (width/2, 380);
rightP = new PVector (560,420);
size(640, 480);
video = new Capture(this, width, height);
video.start();
trackColor = color(160,0,0); // Start off tracking for red
}
void captureEvent(Capture video) {
// Read image from the camera
video.read();
}
void draw() {
loadPixels();
video.loadPixels();
image(video, 0, 0);
float avgX = 0;
float avgY = 0;
int count = 0;
for (int x = 0; x < video.width; x ++ ) {
for (int y = 0; y < video.height; y ++ ) {
int loc = x + y*video.width;
color currentColor = video.pixels[loc];
float r1 = red(currentColor);
float g1 = green(currentColor);
float b1 = blue(currentColor);
float r2 = red(trackColor);
float g2 = green(trackColor);
float b2 = blue(trackColor);
// Using euclidean distance to compare colors
float d = distSq(r1, g1, b1, r2, g2, b2);
if (d < threshold) {
stroke(255);
strokeWeight(1);
point(x,y);
avgX += x;
avgY += y;
count++;
}
}
}
if (count > 0) {
avgX = avgX / count;
avgY = avgY / count;
// Draw a circle at the tracked pixel
fill(trackColor);
strokeWeight(4.0);
stroke(0);
ellipse(avgX, avgY, 20, 20);
text("brightnesslevel: " + trackColor, 20, 60);
text("FPS: " + frameRate, 20, 80);
}
target = new PVector (avgX, avgY);
color c = color(255, 204, 0);
fill(c);
noStroke();
ellipse(leftP.x,leftP.y,16,16); // left param
ellipse(centerP.x,centerP.y,16,16); // center param
ellipse(rightP.x,rightP.y,16,16); // right param
float leftPd = leftP.dist(target);
float centerPd = centerP.dist(target);
float rightPd = rightP.dist(target);
if ( leftPd <= 85 ){
text("To Close left " , 20, 250);
}
if ( centerPd <= 85 ){
text("To Close turn center " , 20, 275);
}
if ( rightPd <= 85 ){
text("To Close turn right " , 20, 300);
}
}
float distSq(float x1,float y1, float z1, float x2, float y2, float z2){
float d = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1);
return d;
}
void mousePressed() {
// Save color where the mouse is clicked in trackColor variable
int loc = mouseX + mouseY*video.width;
trackColor = video.pixels[loc];
}