Maintain high precision of the collision checking of two rotated rectangles - SAT collision - collision

I implemented SAT to check for collisions of 2 rotated rectangles. Everything works fine, but I noticed that as the objects move, the collision check loses precision.
I set the speed of the squares by (v * 5) * dt.
I determined 5 is one meter in my simulation.
At some point, a collision is detected at that position of the objects:
Can you advise me on how to keep the high precision of collision checking? Somewhere I found information that multisampling can help. Can you explain to me what it is, how does it solve the precision loss problem and how to implement it?
I wrote my solution in fpc (pascal / lazarus) but it probably doesn't matter. Below is the code:
Creating rectangle corners:
rectangle[0].x := position.x-15;
rectangle[0].y := position.y-35;
rectangle[1].x := position.x+15;
rectangle[1].y := position.y-35;
rectangle[2].x := position.x+15;
rectangle[2].y := position.y+26;
rectangle[3].x := position.x-15;
rectangle[3].y := position.y+26;
Rotate rectangle:
for i:= 0 to 3 do
begin
tempX := rectangle[i].x - position.x;
tempY := rectangle[i].y - position.y;
// now apply rotation
rotatedX := tempX * cos(DegToRad(ang)) - tempY * sin(DegToRad(ang));
rotatedY := tempX * sin(DegToRad(ang)) + tempY * cos(DegToRad(ang));
tempRectangle[i].x := position.x + rotatedX;
tempRetangle[i].y := position.y + rotatedY;
end;
rotatedRectangle:=tempRectangle;
And collision checking. This is solution from this post: How to check intersection between 2 rotated rectangles?
ported to pascal:
Types
TRectangle = array[0..3] of TMyPoint;
TMyPoint = record
x:real;
y:real;
end;
Collision checking:
res:=true;
for i := 0 to 1 do //2 cars
begin
// for each polygon, look at each edge of the polygon, and determine if it separates
// the two shapes
polygon := polygons[i];
for i1 := 0 to Length(polygon)-1 do
begin
// grab 2 vertices to create an edge
i2 := (i1 + 1) mod 4;
p11 := polygon[i1];
p22 := polygon[i2];
// find the line perpendicular to this edge
normal.x := p22.y - p11.y;
normal.y := p11.x - p22.x;
minA.assigned := false;
maxA.assigned := false;
// for each vertex in the first shape, project it onto the line perpendicular to the edge
// and keep track of the min and max of these values
for j := 0 to 3 do
begin
projected := normal.x * rectangleA[j].x + normal.y * rectanglA[j].y;
if ((minA.assigned = false) or (projected < minA.value)) then
begin
minA.value := projected;
minA.assigned:=true;;
end;
if ((maxA.assigned = false) or (projected > maxA.value)) then
begin
maxA.value := projected;
maxA.assigned := true;
end;
end;
// for each vertex in the second shape, project it onto the line perpendicular to the edge
// and keep track of the min and max of these values
minB.assigned:=false;
maxB.assigned:=false;
for j := 0 to 3 do
begin
projected := normal.x * rectanglB[j].x + normal.y * rectanglB[j].y;
if ((minB.assigned = false) or (projected < minB.value)) then
begin
minB.value := projected;
minB.assigned :=true;
end;
if ((maxB.assigned=false) or (projected > maxB.value)) then
begin
maxB.value := projected;
maxB.assigned:=true;
end;
end;
// if there is no overlap between the projects, the edge we are looking at separates the two
// polygons, and we know there is no overlap
if ((maxA.value < minB.value) or (maxB.value < minA.value)) then
begin
res := false;
end;
end;
end;
result := res;

Related

Find block number and floor by flat number

Imagine - there's a house with 80 flats. It has 4 floors and 5 blocks. Each block has 4 flats.
User is asked to input flat number and Pascal program is supposed to calculate and output flat number. This must be calculated using some kind of formula. The only tip I have is that I have to use div and mod operations.
This is how the house looks like -
So far, I've created program, that loops through all 80 flats and after each 16 flats increases block value and after each 4 blocks increases stair.
This is my code:
program project1;
var
i, floors, blocks, flats, flat, block, floor, blockCounter, floorCounter : integer;
begin
floors := 4;
blocks := 5;
flats := 80;
while true do
begin
write('Flat number: ');
read(flat);
block := 1;
floor := 1;
blockCounter := 0;
floorCounter := 0;
for i := 1 to 80 do
begin
blockCounter := blockCounter + 1;
floorCounter := floorCounter + 1;
if (floorCounter = 4) then
begin
floorCounter := 0;
floor := floor + 1;
end;
if (blockCounter > 16) then
begin
block := block + 1;
blockCounter := 0;
floorCounter := 0;
floor := 1;
end;
if (i = flat) then
begin
writeln('Flat nr. ', flat, ' is in ', floor, '. floor and in ', block, '. block!');
end;
end;
end;
end.
Is there anyone who can help me with this?
I've finally solved my problem myself.
I finally undersood how div works, so I was able to solve this.
program Maja;
var dzivoklis, kapnutelpa, stavs : integer;
begin
while true do
begin
write('Ievadi dzivokla numuru: ');
read(dzivoklis);
kapnutelpa := ((dzivoklis - 1) div 16) + 1;
stavs := (((dzivoklis - 1) mod 16) div 4) + 1;
writeln('Kapnutelpa: ', kapnutelpa);
writeln('Stavs: ', stavs);
writeln();
end;
end.

A game with 100 oponnents, win as much money as possible

You play a game with 100 opponents. The game has k rounds. Every round you can eliminate some opponents (always atleast 1). You are rewarded for eliminating them.
The reward is: 100.000 * '# of eliminated opponents' / '# of opponents' <= in integers (rounded down)
I want to eliminate the opponents in a way, that gets me the largest amount of money possible.
Example game:
number of rounds = 3
first round we eliminate 50 opponents, so we get 100.000 * 50 / 100 = +50.000
second round we eliminate 30, so we get 100.000 * 30 / 50 = +60.000
last round we eliminate last 20 opponents, so we get 100.000 * 20 / 20 = +100.000
so the total winnings are: 210.000
I tried to write up something, but I don't think it's the most effective way to do it?
Program EliminationGame;
var
selectedHistory : array [1..10] of integer;
opponentCount,roundCount : integer;
maxOpponents,numberSelected : integer;
totalMoney : integer;
i : integer;
begin
totalMoney := 0;
maxOpponents := 100;
opponentCount := maxOpponents;
roundCount := 3; {test value}
for i:=1 to roundCount do begin
if (i = roundCount) then begin
numberSelected := opponentCount;
end else begin
numberSelected := floor(opponentCount / roundCount);
end;
selectedHistory[i] := numberSelected;
totalMoney := floor(totalMoney + (numberSelected / opponentCount * 100000));
opponentCount := opponentCount - numberSelected;
end;
writeln('Total money won:');
writeln(totalMoney);
writeln('Amount selected in rounds:');
for i:= 0 to Length(selectedHistory) do
write(selectedHistory[i],' ');
end.
Also it seems that floor function does not exist in pascal?
It seems the question has a maths answer that can be calculated in advance. As #Anton said it was obvious that the number of points given during the third round did not depend upon the number of eliminated enemies. So the third round should eliminate 1 enemy.
So We get the following function for a thre-round game.
f(x)=100000x/100+100000(99-x)/(100-x)+100000*1/1, where x- the number
of enemies eleminated at first round.
if we find the extrema (local maximum of the function) it appears equal to 90. That means the decision is the following: the first round eliminates 90 the second - 9, the third - 1 enemy.
Of course, for consideration: 90=100-sqrt(100).
In other words: the Pascal decision of the task is to loop a variable from 1 to 99 and see the maximum of this function. X-will be the answer.
program Project1;
var
x, xmax: byte;
MaxRes, tmp: real;
begin
xmax := 0;
MaxRes := 0;
for x := 1 to 99 do
begin
tmp := 100000 * x / 100 + 100000*(99 - x) / (100 - x) + 100000 * 1 / 1;
if tmp > MaxRes then
begin
MaxRes := tmp;
xmax := x;
end;
end;
writeln(xmax);
readln;
end.
The general decision for other number of enemies and rounds (using recursion) is the following (Delphi dialect):
program Project1;
{$APPTYPE CONSOLE}
{$R *.res}
Uses System.SysUtils;
var
s: string;
function Part(RemainingEnemies: byte; Depth: byte;
var OutputString: string): real;
var
i: byte;
tmp, MaxRes: real;
imax: byte;
DaughterString: string;
begin
OutputString := '';
if Depth = 0 then
exit(0);
imax := 0;
MaxRes := 0;
for i := 1 to RemainingEnemies - Depth + 1 do
begin
tmp := i / RemainingEnemies * 100000 + Part(RemainingEnemies - i, Depth - 1,
DaughterString);
if tmp > MaxRes then
begin
MaxRes := tmp;
imax := i;
OutputString := inttostr(imax) + ' ' + DaughterString;
end;
end;
result := MaxRes;
end;
begin
writeln(Part(100, 3, s):10:1);//first parameter-Enemies count,
//2-Number of rounds,
//3-output for eliminated enemies counter
writeln(s);
readln;
end.
This problem can be solved with a dynamic approach.
F(round,number_of_opponents_remained):
res = 0
opp // number_of_opponents_remained
for i in [1 opp]
res = max(res, opp/100 + F(round-1,opp - i) )
return res
I should say this not the complete solution and you add some details about it, and I am just giving you an idea. You should add some details such as base case and checking if opp>0 and some other details. The complexity of this algorithm is O(100*k).

Get color percentage OpenCv

How can i get the percentage of colors in a pIplImage (im using Delphi), like percentage of the Red(clRed), Green(clGreen), Blue(clBlue) in an image.
I wanted to simulate this img.Canvas.Pixels[0, 0] with OpenCv but didn't how to go through the image pixel by pixel but didn't know how.
exemple:
procedure TForm1.Button4Click(Sender: TObject);
var i, j, r, b, g, tot : integer;
begin
r := 0; b := 0; g := 0;
for i := 0 to Image1.Width - 1 do
for j := 0 to image1.Height - 1 do
if image1.Canvas.Pixels[i,j] = clRed then
inc(r)
else if image1.Canvas.Pixels[i,j] = clBlue then
inc(b)
else if image1.Canvas.Pixels[i,j] = clGreen then
inc(g);
tot := Image1.Width * Image1.Height;
showmessage('Red %:' + IntToStr(r div tot)+#13#10+
'Green %:'+IntToStr(g div tot)+#13#10+
'Blue %:'+IntToStr(b div tot));
end;
In my search i found out that i had to convert the pIplImage to a Mat (using the cvMat()), the split it into arrays with cvGet2D to get i guess 4 array, RGB and Gray. But didnt knew whats the next step...
var
mat : TCvMat;
res : TCvScalar;
begin
object_filename := 'd:\1.bmp';
image := cvLoadImage(pcvchar(#object_filename[1]), CV_LOAD_IMAGE_UNCHANGED);
mat := cvMat(image.width, image.height, 0, image);
for i := 0 to mat.rows - 1 do
for j := 0 to mat.cols - 1 do
begin
res := cvGet2D(#mat, i, j);
writeln('red: ', res.val[0]:2:2);
writeln('green: ', res.val[1]:2:2);
writeln('Blue: ', res.val[2]:2:2);
writeln('Gray: ', res.val[3]:2:2);
writeln;
end;
readln;
end.
But that returns only val[0] with actual values, the rest are all zeros.
Finally what is the fastest way to calculate the percentage of a color using pIplImage with OpenCV. Less time, mem, cpu...
Here is the solution:
var
img : pIplImage;
channels: array [0 .. 2] of pIplImage;
BGRSum : Array [0 .. 2] of TCvScalar;
i, total: Integer;
begin
try
img := cvLoadImage(filename, 1);
for i := 0 to 2 do
channels[i] := cvCreateImage(cvGetSize(img), 8, 1);
cvSplit(img, channels[0], channels[1], channels[2], 0);
for i := 0 to 2 do
BGRSum[i] := cvSum(channels[i]);
total := img^.width * img^.height * 255;
WriteLn('Color percentage of RGB(ex red 50%, green 25% and blue %25) in an image is');
writeln('red: ', BGRSum[2].val[0] / total * 100:2:2);
writeln('green: ', BGRSum[1].val[0] / total * 100:2:2);
writeln('blue: ', BGRSum[0].val[0] / total * 100:2:2);
readln;
except
on E: Exception do
writeln(
E.ClassName, ': ', E.Message);
end;
end.
Thanks to Laex

change a bitmap's pixel colour

I am trying to change a bit-map's pixel color if it's white. I wrote following code. But it's awfully slow!. i want to check if a pixel's color is white or not, and if it is white, change the color to black.
Can anybody suggest a better approach?
procedure TForm1.Button1Click(Sender: TObject);
var
BitMap1 : TBitmap;
X, Y, Size : Integer;
P: Cardinal;
begin
BitMap1 := TBitmap.Create;
bitMap1.LoadFromFile('image1.bmp');
for Y := 0 to Bitmap1.Height - 1 do
begin
for X := 0 to Bitmap1.width * size - 1 do
begin
p := BitMap1.Canvas.Pixels[X,Y];
if p = 255 then
BitMap1.Canvas.Pixels[X,Y] := 0;
end;
end;
Image1.Picture.Assign(BitMap1);
end;
For sure use the ScanLine property to access bitmap pixels since you're working with a large array of pixels where the Pixels access is slow. For replacing colors of your choice with support for 24-bit and 32-bit bitmaps, I would use something like this:
procedure ReplaceColor(ABitmap: TBitmap; ASource, ATarget: TColor);
type
TRGBBytes = array[0..2] of Byte;
var
I: Integer;
X: Integer;
Y: Integer;
Size: Integer;
Pixels: PByteArray;
SourceColor: TRGBBytes;
TargetColor: TRGBBytes;
const
TripleSize = SizeOf(TRGBBytes);
begin
case ABitmap.PixelFormat of
pf24bit: Size := TripleSize;
pf32bit: Size := SizeOf(TRGBQuad);
else
raise Exception.Create('Bitmap must be 24-bit or 32-bit format!');
end;
for I := 0 to TripleSize - 1 do
begin
// fill the array of bytes with color channel values in BGR order,
// the same would do for the SourceColor from ASource parameter:
// SourceColor[0] := GetBValue(ASource);
// SourceColor[1] := GetGValue(ASource);
// SourceColor[2] := GetRValue(ASource);
// but this is (just badly readable) one liner
SourceColor[I] := Byte(ASource shr (16 - (I * 8)));
// the same do for the TargetColor array from the ATarget parameter
TargetColor[I] := Byte(ATarget shr (16 - (I * 8)));
end;
for Y := 0 to ABitmap.Height - 1 do
begin
// get a pointer to the currently iterated row pixel byte array
Pixels := ABitmap.ScanLine[Y];
// iterate the row horizontally pixel by pixel
for X := 0 to ABitmap.Width - 1 do
begin
// now imagine, that you have an array of bytes in which the groups of
// bytes represent a single pixel - e.g. the used Pixels array for the
// first 2 pixels might look like this for 24-bit and 32-bit bitmaps:
// Pixels [0][1][2] [3][4][5]
// 24-bit B G R B G R
// Pixels [0][1][2][3] [4][5][6][7]
// 32-bit B G R A B G R A
// from the above you can see that you'll need to multiply the current
// pixel iterator by the count of color channels to point to the first
// (blue) color channel in that array; and that's what that (X * Size)
// is for here; X is a pixel iterator, Size is size of a single pixel:
// X * 3 (0 * 3) (1 * 3)
// ⇓ ⇓
// Pixels [0][1][2] [3][4][5]
// 24-bit B G R B G R
// X * 4 (0 * 4) (1 * 4)
// ⇓ ⇓
// Pixels [0][1][2][3] [4][5][6][7]
// 32-bit B G R A B G R A
// so let's compare a BGR value starting at the (X * Size) position of
// the Pixels array with the SourceColor array and if it matches we've
// found the same colored pixel, if so then...
if CompareMem(#Pixels[(X * Size)], #SourceColor, TripleSize) then
// copy the TargetColor color byte array values to that BGR position
// (in other words, replace the color channel bytes there)
Move(TargetColor, Pixels[(X * Size)], TripleSize);
end;
end;
end;
And the usage:
procedure TForm1.Button1Click(Sender: TObject);
var
Bitmap: TBitmap;
begin
Bitmap := TBitmap.Create;
try
Bitmap.LoadFromFile('d:\Image.bmp');
ReplaceColor(Bitmap, clWhite, clBlack);
Image1.Picture.Assign(Bitmap);
finally
Bitmap.Free;
end;
end;
For pure GDI and bitmaps having at most 256 colors you might use the CreateMappedBmp function.
You should use scanlines for this. Example:
procedure ChangeWhiteToBlack(var Bitmap: TBitmap);
var
scanline: PRGBTriple;
y: Integer;
x: Integer;
begin
Assert(Bitmap.PixelFormat = pf24bit);
for y := 0 to Bitmap.Height - 1 do
begin
scanline := Bitmap.ScanLine[y];
for x := 0 to Bitmap.Width - 1 do
begin
with scanline^ do
begin
if (rgbtBlue = 255) and (rgbtGreen = 255) and (rgbtRed = 255) then
FillChar(scanline^, sizeof(TRGBTriple), 0);
end;
inc(scanline);
end;
end;
end;
To try this:
procedure TForm5.FormCreate(Sender: TObject);
var
bm: TBitmap;
begin
bm := TBitmap.Create;
try
bm.LoadFromFile('C:\Users\Andreas Rejbrand\Desktop\test.bmp');
ChangeWhiteToBlack(bm);
bm.SaveToFile('C:\Users\Andreas Rejbrand\Desktop\test2.bmp');
finally
bm.Free;
end;
end;
Update: You need only a very minor modification of the code to make it work on 32-bit bitmaps instead:
procedure ChangeWhiteToBlack32(var Bitmap: TBitmap);
var
scanline: PRGBQuad;
y: Integer;
x: Integer;
begin
Assert(Bitmap.PixelFormat = pf32bit);
for y := 0 to Bitmap.Height - 1 do
begin
scanline := Bitmap.ScanLine[y];
for x := 0 to Bitmap.Width - 1 do
begin
with scanline^ do
begin
if (rgbBlue = 255) and (rgbGreen = 255) and (rgbRed = 255) then
FillChar(scanline^, sizeof(TRGBQuad), 0);
end;
inc(scanline);
end;
end;
end;
In fact, you could do
procedure ChangeWhiteToBlack24(var Bitmap: TBitmap);
var
scanline: PRGBTriple;
y: Integer;
x: Integer;
begin
Assert(Bitmap.PixelFormat = pf24bit);
for y := 0 to Bitmap.Height - 1 do
begin
scanline := Bitmap.ScanLine[y];
for x := 0 to Bitmap.Width - 1 do
begin
with scanline^ do
begin
if (rgbtBlue = 255) and (rgbtGreen = 255) and (rgbtRed = 255) then
FillChar(scanline^, sizeof(TRGBTriple), 0);
end;
inc(scanline);
end;
end;
end;
procedure ChangeWhiteToBlack32(var Bitmap: TBitmap);
var
scanline: PRGBQuad;
y: Integer;
x: Integer;
begin
Assert(Bitmap.PixelFormat = pf32bit);
for y := 0 to Bitmap.Height - 1 do
begin
scanline := Bitmap.ScanLine[y];
for x := 0 to Bitmap.Width - 1 do
begin
with scanline^ do
begin
if (rgbBlue = 255) and (rgbGreen = 255) and (rgbRed = 255) then
FillChar(scanline^, sizeof(TRGBQuad), 0);
end;
inc(scanline);
end;
end;
end;
procedure ChangeWhiteToBlack(var Bitmap: TBitmap);
begin
case Bitmap.PixelFormat of
pf24bit: ChangeWhiteToBlack24(Bitmap);
pf32bit: ChangeWhiteToBlack32(Bitmap);
else
raise Exception.Create('Pixel format must be pf24bit or pf32bit.');
end;
end;
if you don't want to make a single procedure that works with both 24-bit and 32-bit bitmaps, as TLama did. [One benefit of having two separate procedures is that these short procedures are easier to read (and maintain).]
procedure TForm1.Button1Click(Sender: TObject);
var
BitMap1,
BitMap2 : TBitmap;
X, Y, Size : Integer;
P: Cardinal;
begin
BitMap1 := TBitmap.Create;
BitMap1.LoadFromFile('image1.bmp');
BitMap1.Transparent := true;
BitMap1.TransparentColor := clWhite; // old color
BitMap2 := TBitMap.Create;
BitMap2.Height := BitMap1.Height;
BitMap2.Width := BitMap1.Width;
BitMap2.Canvas.Brush.Color := clBlack; // new color
BitMap2.Canvas.FillRect(
Rect(
0,
0,
BitMap2.Width,
BitMap2.Height
)
);
BitMap2.Canvas.Draw(BitMap1);
Image1.Picture.Assign(BitMap2);
BitMap1.Free;
BitMap2.Freel
end;
private void btnLoad2_Click(object sender, System.EventArgs e)
{
Bitmap myBitmap= new Bitmap(openFileDialog1.FileName);
Bitmap myBitmap1 = new Bitmap("C:\\Documents and Settings\\Lalji\\Desktop\\image.png");
for (int x = 0; x < myBitmap.Width; x++)
{
for (int y = 0; y < myBitmap.Height; y++)
{
// Get the color of a pixel within myBitmap.
Color pixelColor = myBitmap.GetPixel(x, y);
string pixelColorStringValue =
pixelColor.R.ToString("D3") + " " +
pixelColor.G.ToString("D3") + " " +
pixelColor.B.ToString("D3") + ", ";
if (pixelColor.R.Equals(0) && pixelColor.G.Equals(0) && pixelColor.B.Equals(0))
{
//MessageBox.Show("black pixel");
}
else if (pixelColor.R.Equals(255) && pixelColor.G.Equals(255) && pixelColor.B.Equals(255))
{
//MessageBox.Show("white pixel");
myBitmap1.SetPixel(x, y, Color.White);
}
//switch (pixelColorStringValue)
//{
// case "255 255 255":
// {
// // white pixel
// MessageBox.Show("white pixel");
// break;
// }
// case "000 000 000,":
// {
// // black pixel
// MessageBox.Show("black pixel");
// break;
// }
//}
}
}
myBitmap1.Save("C:\\Documents and Settings\\Lalji\\Desktop\\image1.png");
MessageBox.Show("Process done");
}

A* / Dijkstra's algorithm simple implementation (Pascal)

I'm trying to implement A* path finding algorithm (now it's Dijkstra's algorithm i.e without heuristic) using this article Link. But I can't figure out what's wrong in my code (it finds incorrect path).
instead of the empty begin ... end; it should be this step:
If it is on the open list already, check to see if this path to that
square is better, using G cost as the measure. A lower G cost means
that this is a better path. If so, change the parent of the square to
the current square, and recalculate the G and F scores of the square.
but I think it is not important because there is no diagonal movement.
uses
crt;
const
MAXX = 20;
MAXY = 25;
type
TArr = array [0..MAXY, 0..MAXX] of integer;
TCell = record
x: integer;
y: integer;
end;
TListCell = record
x: integer;
y: integer;
G: integer;
parent: TCell;
end;
TListArr = array [1..10000] of TListCell;
TList = record
arr: TListArr;
len: integer;
end;
var
i, j, minind, ind, c: integer;
start, finish: TCell;
current: TListCell;
field: TArr;
opened, closed: TList;
procedure ShowField;
var
i, j: integer;
begin
textcolor(15);
for i := 0 to MAXX do
begin
for j := 0 to MAXY do
begin
case field[j, i] of
99: textcolor(8); // not walkable
71: textcolor(14); // walkable
11: textcolor(10); // start
21: textcolor(12); // finish
15: textcolor(2); // path
14: textcolor(5);
16: textcolor(6);
end;
write(field[j, i], ' ');
end;
writeln;
end;
textcolor(15);
end;
procedure AddClosed(a: TListCell);
begin
closed.arr[closed.len + 1] := a;
inc(closed.len);
end;
procedure AddOpened(x, y, G: integer);
begin
opened.arr[opened.len + 1].x := x;
opened.arr[opened.len + 1].y := y;
opened.arr[opened.len + 1].G := G;
inc(opened.len);
end;
procedure DelOpened(n: integer);
var
i: integer;
begin
AddClosed(opened.arr[n]);
for i := n to opened.len - 1 do
opened.arr[i] := opened.arr[i + 1];
dec(opened.len);
end;
procedure SetParent(var a: TListCell; parx, pary: integer);
begin
a.parent.x := parx;
a.parent.y := pary;
end;
function GetMin(var a: TList): integer;
var
i, min, mini: integer;
begin
min := MaxInt;
mini := 0;
for i := 1 to a.len do
if a.arr[i].G < min then
begin
min := a.arr[i].G;
mini := i;
end;
GetMin := mini;
end;
function FindCell(a: TList; x, y: integer): integer;
var
i: integer;
begin
FindCell := 0;
for i := 1 to a.len do
if (a.arr[i].x = x) and (a.arr[i].y = y) then
begin
FindCell := i;
break;
end;
end;
procedure ProcessNeighbourCell(x, y: integer);
begin
if (field[current.x + x, current.y + y] <> 99) then // if walkable
if (FindCell(closed, current.x + x, current.y + y) <= 0) then // and not visited before
if (FindCell(opened, current.x + x, current.y + y) <= 0) then // and not added to list already
begin
AddOpened(current.x + x, current.y + y, current.G + 10);
SetParent(opened.arr[opened.len], current.x, current.y);
// field[opened.arr[opened.len].x, opened.arr[opened.len].y]:=16;
end
else
begin
end;
end;
begin
randomize;
for i := 0 to MAXX do
for j := 0 to MAXY do
field[j, i] := 99;
for i := 1 to MAXX - 1 do
for j := 1 to MAXY - 1 do
if random(5) mod 5 = 0 then
field[j, i] := 99
else field[j, i] := 71;
// start and finish positions coordinates
start.x := 5;
start.y := 3;
finish.x := 19;
finish.y := 16;
field[start.x, start.y] := 11;
field[finish.x, finish.y] := 21;
ShowField;
writeln;
opened.len := 0;
closed.len := 0;
AddOpened(start.x, start.y, 0);
SetParent(opened.arr[opened.len], -1, -1);
current.x := start.x;
current.y := start.y;
repeat
minind := GetMin(opened);
current.x := opened.arr[minind].x;
current.y := opened.arr[minind].y;
current.G := opened.arr[minind].G;
DelOpened(minind);
ProcessNeighbourCell(1, 0); // look at the cell to the right
ProcessNeighbourCell(-1, 0); // look at the cell to the left
ProcessNeighbourCell(0, 1); // look at the cell above
ProcessNeighbourCell(0, -1); // look at the cell below
if (FindCell(opened, finish.x, finish.y) > 0) then
break;
until opened.len = 0;
// count and mark path
c := 0;
while ((current.x <> start.x) or (current.y <> start.y)) do
begin
field[current.x, current.y] := 15;
ind := FindCell(closed, current.x, current.y);
current.x := closed.arr[ind].parent.x;
current.y := closed.arr[ind].parent.y;
inc(c);
end;
ShowField;
writeln(c);
readln;
end.
Edit Feb 1 '12: updated code, also fixed path marking (there should be or instead and), looks like it works now :)
You should rewrite the program to use a loop instead of cut-and-paste to visit each neighbor. If you do that you will avoid bugs like the following:
if (field[current.x, current.y - 1] <> 99) then
if (FindCell(closed, current.x, current.y - 1) <= 0) then
if (FindCell(opened, current.x + 1, current.y) <= 0) then
(See the inconsistent current.x + 1, current.y in the last line.)
With respect to the loop, I was thinking of something like this (pseudo-Python):
neighbor_offsets = [(0, 1), (0, -1), (1, 0), (-1, 0)]
for offset in neighbor_offsets:
neighbor = current + offset
if is_walkable(neighbor) and not is_visited(neighbor):
# Open 'neighbor' with 'current' as parent:
open(neighbor, current)
# Perhaps check if the goal is reached:
if neighbor == finish:
goal_reached = True
break
If you don't write a loop but just refactor to
ProcessCell(x+1, y);
ProcessCell(x-1, y);
ProcessCell(x, y-1);
ProcessCell(x, y-1);
then that's a great improvement too.
Youre posting quite a lot of code, have you tried narrow it down where it fails?
Have you compared your code with the pseudocode on wikipedia?
Also remember that dijkstra is just A* with a heuristic of 0.
Edit:
The article you linked (which I now realize is the very same I used to learn the A*, funny) contains illustrated steps. I would suggest that you recreate that map/grid and run your implementation on it. Then step through the images:
Are the eight initial neighbors added to the open list? Do they have the correct parent?
Is the correct open node picked as next to be scanned according to the heuristic?
Is the list of closed nodes correct?
And so on...

Resources