(*
The following pascal/delphi procedure implements the triangulation algorithm.
**********************************************************************************
procedure triangulation( dx, dy: double; xres, yres: integer; var X,Y,Z: double);
**********************************************************************************
The procedure takes as inputs: 
1. the position of the unknown point in the current camera frame (dx, dy) in pixels
2. the size of the current camera frams (xres, yres) in pixels
end returns the 3D coordinates of the unknown point in the world coordinate system

The procedure assumes three global parameters:
1. LASER_ANGLE: the fixed laser device angle with respect to the horizontal X axis in radians
2. CAMERA_ANGLE: the fixed camera angle with respect to the horizontal X axis in radians
3. CAMERA_FOV: the field of view of the camera in radians

©2006 George P. Pavlidis, gpavlid@ceti.gr
*)

{************************************* TRIANGULATION *****************************}
procedure triangulation( dx, dy: double; xres, yres: integer; var X,Y,Z: double);
{*********************************************************************************}
var
	thetax, a: double;
	thetay: double;	// not actually needed; it is not used in the algorithm
begin
	//****** compute the radius of the camera frame circumcircle
	a := sqrt( xres*xres + yres*yres) / 2;
	//****** compute the X distance of the unknown point from the center of the frame
	dx := (xres/2)-dx;
	//****** compute the Y distance of the unknown point from the center of the frame
	dy := (yres/2)-dy;
	//****** compute the angle that corresponds to the X displacement from the center
	thetax := arctan( dx / a * tan( CAMERA_FOV/2));
	//****** compute the angle that corresponds to the X displacement from the center
	thetay := arctan( dy / a * tan( CAMERA_FOV/2));
	//****** copmpute the distance of the unknown point on the measured object from the camera
	Z := LASER_CAMERA_DISTANCE * sin( LASER_ANGLE) * cos( thetax) / sin( LASER_ANGLE + CAMERA_ANGLE + thetax);
	//****** deduce the X coordinate using Z and trigonometry
	X := Z * dx/a * tan( CAMERA_FOV/2);
	//****** deduce the Y coordinate using Z and trigonometry;
	Y := Z * dy/a * tan( CAMERA_FOV/2);
end;