Nearest point between two lines

nearest_point_between_two_lines.cpp

//normal1: a 3d double vector indicating the normal vector of line1
//point1:  a 3d double point indicating any point on line1
//normal2: a 3d double vector indicating the normal vector of line2
//point2:  a 3d double point indicating any point on line2
//nearest_point: a 3d double point as the nearest point between line1 and line2
void nearest_point_between_two_lines(
	const double *normal1, const double *point1,
	const double *normal2, const double *point2,
	double *nearest_point)
{
	double f1ab = normal1[0] * normal1[0] + normal1[1] * normal1[1] + normal1[2] * normal1[2];
	double f1cd = normal2[0] * normal2[0] + normal2[1] * normal2[1] + normal2[2] * normal2[2];
	double f2 = normal1[0] * normal2[0] + normal1[1] * normal2[1] + normal1[2] * normal2[2];
	double f3ab = normal1[0] * (point2[0] - point1[0])
		        + normal1[1] * (point2[1] - point1[1])
				+ normal1[2] * (point2[2] - point1[2]);
	double f3cd = normal2[0] * (point2[0] - point1[0])
		        + normal2[1] * (point2[1] - point1[1])
		        + normal2[2] * (point2[2] - point1[2]);

	double t1 = (f3ab * f1cd - f3cd * f2) / (f1ab * f1cd - f2 * f2);
	double t2 = (f3ab * f2 - f3cd * f1ab) / (f1ab * f1cd - f2 * f2);

	double xm = t1 * normal1[0] + point1[0];
	double ym = t1 * normal1[1] + point1[1];
	double zm = t1 * normal1[2] + point1[2];

	double xn = t2 * normal2[0] + point2[0];
	double yn = t2 * normal2[1] + point2[1];
	double zn = t2 * normal2[2] + point2[2];

	nearest_point[0] = (xm + xn) / 2;
	nearest_point[1] = (ym + yn) / 2;
	nearest_point[2] = (zm + zn) / 2;
}