Submitted version of the thesis - Airlab, the Artificial Intelligence ...

B.1. Microprocessor Code 109
}
Inverse(H, InvH);
double position[3] = { 0, 0, 0 };
double image_point[3] = { point.X, point.Y, 1 };
vectorByMatrix3x1(InvH, image_point, position);
result[0] = (s16) (position[0] / position[2]);
result[1] = (s16) (position[1] / position[2]);
result[2] = (s16) (position[2] / position[2]);
void CalculateBallPosition(u8 X, u8 Y, u8 Dpx,
double result[4]) {
double image_point[3] = { X, Y, 1 };
double inv_K[3][3] = { { 0.005216086284917, 0,
-0.419160051539105 }, { 0, 0.005228129700905,
-0.320361489617536 }, { 0, 0, 1 } };
double position[3] = { 0, 0, 0 };
vectorByMatrix3x1(inv_K, image_point, position);
double norm;
norm = sqrt((position[0] * position[0]) + (position[1]
* position[1]) + (position[2] * position[2]));
position[0] = position[0] / norm;
position[1] = position[1] / norm;
position[2] = position[2] / norm;
int X_axis = 300;
int Y_axis = -150;
int Dreal = 62; //red ball w 6.2 cm diam
double fstar = 160.9969;
double Pc[4] = { (fstar * Dreal / Dpx) * position[0],
(fstar * Dreal / Dpx) * position[1], (fstar
* Dreal / Dpx) * position[2], 1 };
double T_wr[4][4] = { { 1, 0, 0, X_axis }, { 0, 1, 0,
Y_axis }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
double T_wc[4][4] = { { 0.0012, -0.3587, 0.9334,
-350.8669 }, { -1.0000, -0.0039, -0.0002,