double Geo3d_Line_Line_Dist(double line1_start[], double line1_end[],
double line2_start[], double line2_end[])
{
double intersect1, intersect2;
double dc1[3], dc2[3];
double lamda;
switch (geo3d_line_line_dist(line1_start, line1_end, line2_start, line2_end,
dc1, dc2, &intersect1, &intersect2,
TZ_DIST_3D_EPS)) {
case -3: /* point to point */
return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
line2_start[0], line2_start[1], line2_start[2]);
case -1: /* point to line2 */
geo3d_point_line_dist(line1_start, line2_start, line2_end, &lamda,
TZ_DIST_3D_EPS);
return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
line2_start[0] + lamda * dc2[0],
line2_start[1] + lamda * dc2[1],
line2_start[2] + lamda * dc2[2]);
case -2: /* point to line 1 */
geo3d_point_line_dist(line2_start, line1_start, line1_end, &lamda,
TZ_DIST_3D_EPS);
return Geo3d_Dist(line2_start[0], line2_start[1], line2_start[2],
line1_start[0] + lamda * dc1[0],
line1_start[1] + lamda * dc1[1],
line1_start[2] + lamda * dc1[2]);
case 0: /* parallel */
geo3d_point_line_dist(line1_start, line2_start, line2_end, &lamda,
TZ_DIST_3D_EPS);
return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
line2_start[0] + lamda * dc2[0],
line2_start[1] + lamda * dc2[1],
line2_start[2] + lamda * dc2[2]);
default: /* line to line */
return sqrt(Geo3d_Dist_Sqr(line1_start[0] + intersect1 * dc1[0],
line1_start[1] + intersect1 * dc1[1],
line1_start[2] + intersect1 * dc1[2],
line2_start[0] + intersect2 * dc2[0],
line2_start[1] + intersect2 * dc2[1],
line2_start[2] + intersect2 * dc2[2]));
}
}
double Geo3d_Point_Lineseg_Dist(const double *point, const double *line_start,
const double *line_end, double *lamda)
{
double tmp_lamda;
int status = geo3d_point_line_dist(point, line_start, line_end, &tmp_lamda,
GEO3D_POINT_LINESEG_EPS);
double dist;
if (status == 0) {
dist = Geo3d_Dist(line_start[0], line_start[1], line_start[2],
point[0], point[1], point[2]);
} else {
if ((tmp_lamda >= 0.0) && (tmp_lamda <= 1.0)) {
dist =
Geo3d_Dist(point[0], point[1], point[2],
(1.0 - tmp_lamda) * line_start[0] + tmp_lamda * line_end[0],
(1.0 - tmp_lamda) * line_start[1] + tmp_lamda * line_end[1],
(1.0 - tmp_lamda) * line_start[2] + tmp_lamda * line_end[2]);
} else if (tmp_lamda < 0.0) {
tmp_lamda = 0.0;
dist = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2],
point[0], point[1], point[2]));
} else {
tmp_lamda = 1.0;
dist = sqrt(Geo3d_Dist_Sqr(line_end[0], line_end[1], line_end[2],
point[0], point[1], point[2]));
}
}
/*
double line_vec[3];
double point_vec[3];
int i;
for (i = 0; i < 3; i++) {
line_vec[i] = line_end[i] - line_start[i];
point_vec[i] = point[i] - line_start[i];
}
double length_sqr = Geo3d_Orgdist_Sqr(line_vec[0], line_vec[1], line_vec[2]);
double tmp_lamda = 0.0;
double dist = -1.0;
if (length_sqr <= GEO3D_POINT_LINESEG_EPS) {
dist = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2],
point[0], point[1], point[2]));
} else {
double dot = point_vec[0] * line_vec[0] + point_vec[1] * line_vec[1] +
point_vec[2] * line_vec[2];
double point_vec_lensqr = Geo3d_Orgdist_Sqr(point_vec[0], point_vec[1],
point_vec[2]);
if (point_vec_lensqr <= GEO3D_POINT_LINESEG_EPS) {
dist = 0.0;
} else {
tmp_lamda = dot / length_sqr;
if ((tmp_lamda >= 0.0) && (tmp_lamda <= 1.0)) {
dist = sqrt(point_vec_lensqr - dot * dot / length_sqr);
} else if (tmp_lamda < 0.0) {
dist = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2],
point[0], point[1], point[2]));
} else {
dist = sqrt(Geo3d_Dist_Sqr(line_end[0], line_end[1], line_end[2],
point[0], point[1], point[2]));
}
}
}
*/
if (lamda != NULL) {
*lamda = tmp_lamda;
}
return dist;
}
double Geo3d_Lineseg_Lineseg_Dist(double line1_start[], double line1_end[],
double line2_start[], double line2_end[],
double *intersect1, double *intersect2,
int *cond)
{
double dc1[3]; /*21*/
double dc2[3]; /*43*/
double d1, d2;
switch (geo3d_line_line_dist(line1_start, line1_end, line2_start, line2_end,
dc1, dc2, intersect1, intersect2,
TZ_DIST_3D_EPS)) {
case 0: /* parallel */
*cond = 9;
d1 = Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
intersect1);
d2 = Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
intersect2);
if (d1 <= d2) {
*intersect2 = *intersect1;
*intersect1 = 1.0;
return d1;
} else {
*intersect1 = 0.0;
return d2;
}
case -1:
*intersect1 = 0.0;
*cond = 10;
return Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
intersect2);
case -2:
*intersect2= 0.0;
*cond = 10;
return Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
intersect1);
case -3:
*cond = 10;
*intersect1 = 0.0;
*intersect2 = 0.0;
return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
line2_start[0], line2_start[1], line2_start[2]);
default:
#if defined BREAK_IS_IN_RANGE
# undef BREAK_IS_IN_RANGE
#endif
#define BREAK_IS_IN_RANGE(mu) ((mu >= 0.0) && (mu <= 1.0))
if (BREAK_IS_IN_RANGE(*intersect1) && BREAK_IS_IN_RANGE(*intersect2)) {
*cond = 0;
return sqrt(Geo3d_Dist_Sqr(line1_start[0] + *intersect1 * dc1[0],
line1_start[1] + *intersect1 * dc1[1],
line1_start[2] + *intersect1 * dc1[2],
line2_start[0] + *intersect2 * dc2[0],
line2_start[1] + *intersect2 * dc2[1],
line2_start[2] + *intersect2 * dc2[2]));
} else if ((*intersect1 < 0.0) && BREAK_IS_IN_RANGE(*intersect2)) {
*cond = 1;
*intersect1 = 0.0;
return Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
intersect2);
} else if ((*intersect1 > 0.0) && BREAK_IS_IN_RANGE(*intersect2)) {
*cond = 2;
*intersect1 = 1.0;
return Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
intersect2);
} else if ((*intersect2 < 0.0) && BREAK_IS_IN_RANGE(*intersect1)) {
*cond = 3;
*intersect2 = 0.0;
return Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
intersect1);
} else if ((*intersect2 > 1.0) && BREAK_IS_IN_RANGE(*intersect1)) {
*cond = 4;
*intersect2 = 1.0;
return Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
intersect1);
} else if ((*intersect1 < 0.0) && (*intersect2 < 0.0)) {
*cond = 5;
d1 = Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
intersect2);
d2 = Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
intersect1);
if (d1 <= d2) {
*intersect1 = 0.0;
return d1;
} else {
*intersect2 = 0.0;
return d2;
}
} else if ((*intersect1 > 1.0) && (*intersect2 < 0.0)) {
*cond = 6;
d1 = Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
intersect2);
d2 = Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
intersect1);
if (d1 <= d2) {
*intersect1 = 1.0;
return d1;
} else {
*intersect2 = 0.0;
return d2;
}
} else if ((*intersect1 < 0.0) && (*intersect2 > 1.0)) {
*cond = 7;
d1 = Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
intersect2);
d2 = Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
intersect1);
if (d1 <= d2) {
*intersect1 = 0.0;
return d1;
} else {
*intersect2 = 1.0;
return d2;
}
} else if ((*intersect1 > 1.0) && (*intersect2 > 1.0)) {
*cond = 8;
d1 = Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
intersect2);
d2 = Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
intersect1);
if (d1<= d2) {
*intersect1 = 1.0;
return d1;
} else {
*intersect2 = 1.0;
return d2;
}
}
}
return -1.0;
}
int main(int argc, char *argv[])
{
static char *Spec[] = {"[-t]", NULL};
Process_Arguments(argc, argv, Spec, 1);
if (Is_Arg_Matched("-t")) {
coordinate_3d_t coord = {0, 0, 0};
/* Translate coordinates. */
Geo3d_Translate_Coordinate(coord, coord+1, coord+2, 1.0, 2.0, 3.0);
if ((coord[0] != 1.0) || (coord[1] != 2.0) || (coord[2] != 3.0)) {
Print_Coordinate_3d(coord);
PRINT_EXCEPTION(":( Bug?", "Unexpected coordinate values.");
return 1;
}
coordinate_3d_t coord2 = {0, 0, 0};
double dx, dy, dz;
Geo3d_Coordinate_Offset(coord[0], coord[1], coord[2], coord2[0], coord2[1],
coord2[2], &dx, &dy, &dz);
if ((dx != -coord[0]) || (dy != -coord[1]) || (dz != -coord[2])) {
PRINT_EXCEPTION(":( Bug?", "Unexpected offset values.");
return 1;
}
Coordinate_3d_Copy(coord2, coord);
/* Rotate coordinates. */
Geo3d_Rotate_Coordinate(coord, coord+1, coord+2, 0.1, 0.5, 0);
if (fabs(Geo3d_Orgdist_Sqr(coord[0], coord[1], coord[2]) -
Geo3d_Orgdist_Sqr(coord2[0], coord2[1], coord2[2])) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Rotation failed.");
return 1;
}
/* inverse rotation */
Geo3d_Rotate_Coordinate(coord, coord+1, coord+2, 0.1, 0.5, 1);
if ((fabs(coord[0] - coord2[0]) > 0.01) ||
(fabs(coord[1] - coord2[1]) > 0.01) ||
(fabs(coord[2] - coord2[2]) > 0.01)){
PRINT_EXCEPTION(":( Bug?", "Rotation failed.");
return 1;
}
/* Normal vector of an orientation. This is a canonical representaion
* of an orientation. */
Geo3d_Orientation_Normal(0.1, 0.5, coord, coord+1, coord+2);
/* We can also turn the normal vector into orientation. */
double theta, psi;
Geo3d_Normal_Orientation(coord[0], coord[1], coord[2], &theta, &psi);
if (fabs(theta - 0.1) > 0.01 || fabs(Normalize_Radian(psi) - 0.5) > 0.01) {
printf("%g, %g\n", theta, psi);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
/* If we rotate the vector back, we should get (0, 0, 1). */
Geo3d_Rotate_Coordinate(coord, coord+1, coord+2, 0.1, 0.5, 1);
if ((fabs(coord[0]) > 0.01) || (fabs(coord[1]) > 0.01) ||
(fabs(coord[2] - 1.0) > 0.01)){
PRINT_EXCEPTION(":( Bug?", "Rotation failed.");
return 1;
}
Geo3d_Coord_Orientation(1, 0, 0, &theta, &psi);
double in[3] = {0, 0, 1};
double out[3] = {0, 0, 0};
Rotate_XZ(in, out, 1, theta, psi, 0);
if (fabs(out[0] - 1.0) > 0.01 || fabs(out[1]) > 0.01 ||
fabs(out[2]) > 0.01) {
printf("%g, %g, %g\n", out[0], out[1], out[2]);
printf("%g, %g\n", theta, psi);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
/*
if (fabs(theta - TZ_PI_2) > 0.01 ||
fabs(Normalize_Radian(psi) - TZ_PI_2) > 0.01) {
printf("%g, %g\n", theta, psi);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
*/
Geo3d_Coord_Orientation(5, 0, 0, &theta, &psi);
{
double in[3] = {0, 0, 5};
double out[3] = {0, 0, 0};
Rotate_XZ(in, out, 1, theta, psi, 0);
if (fabs(out[0] - 5.0) > 0.01 || fabs(out[1]) > 0.01 ||
fabs(out[2]) > 0.01) {
printf("%g, %g, %g\n", out[0], out[1], out[2]);
printf("%g, %g\n", theta, psi);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
}
/*
if (fabs(theta - TZ_PI_2) > 0.01 ||
fabs(Normalize_Radian(psi) - TZ_PI_2) > 0.01) {
printf("%g, %g\n", theta, psi);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
*/
/* Rotate the orientation. */
theta = TZ_PI_2;
psi = TZ_PI;
Geo3d_Rotate_Orientation(-TZ_PI, TZ_PI_2, &theta, &psi);
if (fabs(theta + TZ_PI_2) > 0.01 ||
fabs(psi + TZ_PI_2) > 0.01) {
printf("%g, %g\n", theta, psi);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
/* More extensive test on rotation */
Random_Seed(time(NULL) - getpid());
int i;
for (i = 0; i < 100; i++) {
double x = Unifrnd() * (1 - Bernrnd(0.5) * 2.0);
double y = Unifrnd() * (1 - Bernrnd(0.5) * 2.0);
double z = Unifrnd() * (1 - Bernrnd(0.5) * 2.0);
double theta, psi;
Geo3d_Coord_Orientation(x, y, z, &theta, &psi);
double x2 = 0.0;
double y2 = 0.0;
double z2 = Geo3d_Orgdist(x, y, z);
Geo3d_Rotate_Coordinate(&x2, &y2, &z2, theta, psi, 0);
if (fabs(x - x2) > 0.01 || fabs(y - y2) > 0.01 || fabs(z - z2) > 0.01) {
printf("%g, %g\n", theta, psi);
printf("%g, %g, %g\n", x, y, z);
printf("%g, %g, %g\n", x2, y2, z2);
PRINT_EXCEPTION(":( Bug?", "Unexpected orientation values.");
return 1;
}
}
/* Dot product. */
if (Geo3d_Dot_Product(1.0, 2.0, 3.0, 3.0, 2.0, 1.0) != 10.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected dot product.");
return 1;
}
/* Cross product. */
Geo3d_Cross_Product(1.0, 2.0, 3.0, 3.0, 1.0, 2.0, coord, coord+1, coord+2);
if (fabs(Geo3d_Dot_Product(1.0, 2.0, 3.0, coord[0], coord[1], coord[2])) >
0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected dot product.");
return 1;
}
if (fabs(Geo3d_Dot_Product(3.0, 1.0, 2.0, coord[0], coord[1], coord[2])) >
0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected dot product.");
return 1;
}
/* Distance calculations. */
if (Geo3d_Orgdist_Sqr(1.0, 2.0, 3.0) != 14.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected value.");
return 1;
}
if (fabs(Geo3d_Orgdist(1.0, 2.0, 3.0) - sqrt(14.0)) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected value.");
return 1;
}
if (Geo3d_Dist(1.0, 2.0, 3.0, 1.0, 2.0, 3.0) != 0.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected value.");
return 1;
}
if (Geo3d_Dist_Sqr(1.0, 2.0, 3.0, 0.0, 0.0, 0.0) != 14.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected value.");
return 1;
}
/* Angle between two vectors. */
if (fabs(Geo3d_Angle2(1, 0, 0, 0, 0, 1) - TZ_PI_2) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected value.");
return 1;
}
/* Distance between two lines */
coordinate_3d_t line1_start = {0, 0, 0};
coordinate_3d_t line1_end = {1, 0, 0};
coordinate_3d_t line2_start = {0, 1, 1};
coordinate_3d_t line2_end = {0, 2, 1};
double d = Geo3d_Line_Line_Dist(line1_start, line1_end,
line2_start, line2_end);
if (fabs(d - 1.0) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected distance.");
return 1;
}
/* Distance between two line segments. */
double intersect1, intersect2;
int cond;
d = Geo3d_Lineseg_Lineseg_Dist(line1_start, line1_end,
line2_start, line2_end,
&intersect1, &intersect2, &cond);
if (fabs(d - sqrt(2.0)) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected distance.");
return 1;
}
/* Distance between a point and a line segment. */
coord[0] = 0.5;
coord[1] = 1.0;
coord[2] = 1.0;
d = Geo3d_Point_Lineseg_Dist(coord, line1_start, line1_end, &intersect1);
if (fabs(d - sqrt(2.0)) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected distance.");
return 1;
}
/* Get a break point of a line segment. */
Geo3d_Lineseg_Break(line1_start, line1_end, 0.1, coord);
if ((coord[0] != 0.1) || (coord[1] != 0.0) || (coord[2] != 0.0)) {
PRINT_EXCEPTION(":( Bug?", "Unexpected break point.");
return 1;
}
/* Orientations of a set of points */
/* Orientation of a covariance matrix */
/* 3D coordinates routines */
/* Unitize a point. */
Coordinate_3d_Unitize(coord);
if (fabs(Coordinate_3d_Norm(coord) - 1.0) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected norm.");
return 1;
}
Set_Coordinate_3d(coord, 0.0, 0.0, 0.0);
/* origin point is not affected by unitization */
Coordinate_3d_Unitize(coord);
if (Coordinate_3d_Norm(coord) != 0.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected norm.");
return 1;
}
Set_Coordinate_3d(coord, 1.0, 2.0, 3.0);
/* Square length */
if (Coordinate_3d_Length_Square(coord) != 14) {
PRINT_EXCEPTION(":( Bug?", "Unexpected square length");
}
/* Angle between two vectors */
coordinate_3d_t coord_a1 = { 1, 0, 0 };
coordinate_3d_t coord_a2 = { 0, 0, 1 };
if (fabs(Coordinate_3d_Angle2(coord_a1, coord_a2) - TZ_PI_2) > 0.01) {
PRINT_EXCEPTION(":( Bug?", "Unexpected angle");
}
/* Scale coordinates */
Coordinate_3d_Scale(coord, 2.0);
if ((coord[0] != 2.0) || (coord[1] != 4.0) || (coord[2] != 6.0)) {
PRINT_EXCEPTION(":( Bug?", "Unexpected coordinate value.");
return 1;
}
/* Normalizd dot product */
if (Coordinate_3d_Normalized_Dot(coord, coord2) > 1.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected dot product.");
return 1;
}
Set_Coordinate_3d(coord, 1.0, 2.0, 3.0);
Coordinate_3d_Copy(coord2, coord);
Coordinate_3d_Scale(coord2, 2.0);
coordinate_3d_t coord3;
Coordinate_3d_Copy(coord3, coord);
Coordinate_3d_Scale(coord3, 3.0);
/* cos value of an angle formed by 3 points */
if (Coordinate_3d_Cos3(coord, coord2, coord3) != 1.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected cos value.");
return 1;
}
/* cos value of an angle formed by 4 points */
coordinate_3d_t coord4;
Coordinate_3d_Copy(coord4, coord);
Coordinate_3d_Scale(coord4, 4.0);
if (Coordinate_3d_Cos4(coord, coord2, coord3, coord4) != 1.0) {
PRINT_EXCEPTION(":( Bug?", "Unexpected cos value.");
return 1;
}
printf(":) Testing passed.\n");
return 0;
}
#if 0
Geo3d_Vector v1 = {1.1, 0, 0};
Geo3d_Vector v2 = {5, 0, 0};
double theta1 = 3.83812;
double psi1 = 3.53202;
double theta2 = 0.72657;
double psi2 = 3.61214;
Geo3d_Orientation_Normal(theta1, psi1, &(v1.x), &(v1.y), &(v1.z));
Geo3d_Orientation_Normal(theta2, psi2, &(v2.x), &(v2.y), &(v2.z));
Print_Geo3d_Vector(&v1);
Print_Geo3d_Vector(&v2);
printf("%g\n", Geo3d_Vector_Dot(&v1, &v2));
printf("%g\n", Geo3d_Vector_Angle2(&v1, &v2));
double angle = Vector_Angle(-1.0, -1.0);
theta1 = -3.0;
psi1 = -5.0;
double x, y, z;
Geo3d_Orientation_Normal(theta1, psi1, &x, &y, &z);
printf("%g, %g, %g\n", x, y, z);
Geo3d_Normal_Orientation(x, y, z, &theta1, &psi1);
printf("%g\n", angle * 180.0 / TZ_PI);
printf("%g, %g\n", theta1, psi1);
Geo3d_Orientation_Normal(theta1, psi1, &x, &y, &z);
printf("%g, %g, %g\n", x, y, z);
#endif
#if 0
Geo3d_Scalar_Field *field =
Read_Geo3d_Scalar_Field("../data/mouse_neuron/seeds.bn");
Print_Geo3d_Scalar_Field_Info(field);
Stack *stack = Read_Stack("../data/mouse_neuron/mask.tif");
Geo3d_Scalar_Field_Draw_Stack(field, stack, NULL, NULL);
Write_Stack("../data/test.tif", stack);
#endif
#if 0
double line_start[3] = {2.0, 3.0, 4.0};
double line_end[3] = {1.0, 1.0, 6.0};
double point[3] = {10.5, 20.5, 40.7};
double lamda;
printf("%g\n", Geo3d_Point_Lineseg_Dist(point, line_start, line_end, &lamda));
double line_point[3];
double length = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2], line_end[0], line_end[1], line_end[2]));
line_point[0] = line_start[0] + lamda * (line_end[0] - line_start[0]);
line_point[1] = line_start[1] + lamda * (line_end[1] - line_start[1]);
line_point[2] = line_start[2] + lamda * (line_end[2] - line_start[2]);
printf("%g\n", lamda);
printf("%g\n", sqrt(Geo3d_Dist_Sqr(line_point[0], line_point[1], line_point[2], point[0], point[1], point[2])));
darray_sub(point, line_point, 3);
darray_sub(line_end, line_point, 3);
printf("%g\n", Geo3d_Dot_Product(point[0], point[1], point[2],
line_end[0], line_end[1], line_end[2]));
#endif
#if 0
Geo3d_Scalar_Field *field = Read_Geo3d_Scalar_Field("../data/mouse_neuron3_org/seeds");
//Print_Geo3d_Scalar_Field(field);
darray_write("../data/pts.bn", (double *)field->points, field->size * 3);
darray_write("../data/ws.bn", field->values, field->size);
coordinate_3d_t centroid;
Geo3d_Scalar_Field_Centroid(field, centroid);
Print_Coordinate_3d(centroid);
double cov[9];
Geo3d_Scalar_Field_Cov(field, cov);
darray_print2(cov, 3, 3);
darray_write("../data/cov.bn", cov, 9);
double vec[3];
Geo3d_Cov_Orientation(cov, vec);
darray_print2(vec, 3, 1);
#endif
#if 0
double mu1, mu2;
# if 0 /* parallel case */
double line1_start[3] = {0, 0, 0};
double line1_end[3] = {1, 1, 10};
double line2_start[3] = {0, 1, 0};
double line2_end[3] = {1, 2, 10};
# endif
# if 0 /* parallel case */
double line1_start[3] = {0, 0, 0};
double line1_end[3] = {1, 1, 10};
double line2_start[3] = {-1, -1, -1};
double line2_end[3] = {-3, -3, -21};
# endif
# if 0 /* i-i case */
double line1_start[3] = {0, 0, 0};
double line1_end[3] = {3, 4, 5};
double line2_start[3] = {-1, 8, 9};
double line2_end[3] = {1, -3, -4};
# endif
# if 0 /* point to line case */
double line1_start[3] = {3, 4, 5};
double line1_end[3] = {3, 4, 5};
double line2_start[3] = {-1, 8, 9};
double line2_end[3] = {1, -3, -4};
# endif
# if 0 /* point to line case */
double line2_start[3] = {3, 4, 5};
double line2_end[3] = {3, 4, 5};
double line1_start[3] = {-1, 8, 9};
double line1_end[3] = {1, -3, -4};
# endif
# if 0 /* point to line case */
double line2_start[3] = {3, 4, 5};
double line2_end[3] = {3, 4, 5};
double line1_start[3] = {-1, 8, 9};
double line1_end[3] = {-1, 8, 9};
# endif
# if 0
double line1_start[3] = {256.062, 328.674, 67.9029};
double line1_end[3] = {246.162, 330.078, 67.9896};
double line2_start[3] = {262.368, 336.609, 68.3076};
double line2_end[3] = {259.956, 326.944, 67.4325};
# endif
int cond;
double dist = Geo3d_Lineseg_Lineseg_Dist(line1_start, line1_end,
line2_start, line2_end,
&mu1, &mu2, &cond);
printf("%d, %g\n", cond, dist);
dist = Geo3d_Line_Line_Dist(line1_start, line1_end, line2_start, line2_end);
printf("%g\n", dist);
#endif
#if 1
double *d = Unifrnd_Double_Array(12000000, NULL);
double *d2 = Unifrnd_Double_Array(12000000, NULL);
tic();
Rotate_XZ(d, d, 4000000, 0.1, 0.2, 1);
printf("%llu\n", toc());
tic();
Rotate_XZ2(d, d, 4000000, 0.1, 0.2, 0);
printf("%llu\n", toc());
#endif
#if 0
double d[3] = {0.1, 0.2, 0.3};
Rotate_XZ(d, d, 1, 0.1, 0.2, 0);
darray_print2(d, 3, 1);
#endif
#if 0
printf("%g\n", Ellipse_Point_Distance(5, 5, 3, 5));
#endif
#if 0
Geo3d_Ellipse *ellipse = New_Geo3d_Ellipse();
ellipse->scale = 0.5;
ellipse->orientation[0] = 1.0;
Print_Geo3d_Ellipse(ellipse);
FILE *fp = fopen("../data/test.swc", "w");
Swc_Node node;
Geo3d_Ellipse_To_Swc_Node(ellipse, 1, -1, 1.0, 2, &node);
Swc_Node_Fprint(fp, &node);
/*
double pt[3] = {0.0, 0.0, 1.0};
printf("%g\n", Geo3d_Ellipse_Point_Distance(ellipse, pt));
*/
Geo3d_Ellipse *ellipse2 = Copy_Geo3d_Ellipse(ellipse);
ellipse2->center[0] += 10.0;
ellipse2->orientation[0] = 2.0;
ellipse2->radius += 3.0;
Geo3d_Ellipse *ellipse3 = Geo3d_Ellipse_Interpolate(ellipse, ellipse2, 0.2,
NULL);
Geo3d_Ellipse_To_Swc_Node(ellipse3, 2, 1, 1.0, 2, &node);
Swc_Node_Fprint(fp, &node);
Geo3d_Ellipse_To_Swc_Node(ellipse2, 3, 2, 1.0, 2, &node);
Swc_Node_Fprint(fp, &node);
Print_Geo3d_Ellipse(ellipse3);
fclose(fp);
#endif
#if 0
Geo3d_Ellipse *ellipse = New_Geo3d_Ellipse();
ellipse->scale = 0.5;
FILE *fp = fopen("../data/test.swc", "w");
coordinate_3d_t *pts = Geo3d_Ellipse_Sampling(ellipse, 20, 0, NULL);
Geo3d_Point_Array_Swc_Fprint(fp, pts, 20, 1, -1, 1.0, 2);
ellipse->orientation[0] = 1.0;
pts = Geo3d_Ellipse_Sampling(ellipse, 20, 0, NULL);
Geo3d_Point_Array_Swc_Fprint(fp, pts, 20, 21, -1, 1.0, 3);
ellipse->center[2] = 5.0;
pts = Geo3d_Ellipse_Sampling(ellipse, 20, 0, NULL);
Geo3d_Point_Array_Swc_Fprint(fp, pts, 20, 41, -1, 1.0, 4);
coordinate_3d_t vec[2];
Coordinate_3d_Copy(vec[0], ellipse->center);
Geo3d_Ellipse_First_Vector(ellipse, vec[1]);
Coordinate_3d_Add(vec[0], vec[1], vec[1]);
Geo3d_Point_Array_Swc_Fprint(fp, vec, 2, 101, -1, 1.0, 5);
Geo3d_Ellipse_Second_Vector(ellipse, vec[1]);
Coordinate_3d_Add(vec[0], vec[1], vec[1]);
Geo3d_Point_Array_Swc_Fprint(fp, vec, 2, 111, -1, 1.0, 6);
Geo3d_Ellipse_Normal_Vector(ellipse, vec[1]);
Coordinate_3d_Add(vec[0], vec[1], vec[1]);
Geo3d_Point_Array_Swc_Fprint(fp, vec, 2, 121, -1, 1.0, 7);
fclose(fp);
#endif
#if 0
Geo3d_Ellipse ep_array[10];
Geo3d_Ellipse *ellipse = New_Geo3d_Ellipse();
ellipse->scale = 0.5;
Geo3d_Ellipse_Copy(ep_array, ellipse);
FILE *fp = fopen("../data/test.swc", "w");
ellipse->orientation[0] += 0.5;
ellipse->center[2] += 3.0;
ellipse->center[0] += 1.0;
Geo3d_Ellipse_Copy(ep_array + 1, ellipse);
ellipse->center[2] += 3.0;
Geo3d_Ellipse_Copy(ep_array + 2, ellipse);
coordinate_3d_t *pts = Geo3d_Ellipse_Array_Sampling(ep_array, 3, 20, NULL);
Geo3d_Point_Array_Swc_Fprint(fp, pts, 60, 1, -1, 1.0, 2);
fclose(fp);
#endif
#if 0
printf("%g\n", Vector_Angle(1.0, 1.0) * 180 / TZ_PI);
#endif
#if 0
double theta, psi;
coordinate_3d_t coord;
coord[0] = 0.0822;
coord[1] = 0.1515;
coord[2] = 0.1369;
Geo3d_Coord_Orientation(coord[0], coord[1], coord[2], &theta, &psi);
printf("%g, %g\n", theta, psi);
Geo3d_Orientation_Normal(theta, psi, coord, coord+1, coord+2);
Print_Coordinate_3d(coord);
#endif
#if 0
double theta, psi;
double x = -1;
double y = 1.83697019872103e-16;
double z = 3;
Geo3d_Coord_Orientation(x, y, z, &theta, &psi);
#endif
#if 0
double theta = Vector_Angle2(0, 1, 1, 0, TRUE);
printf("angle: %g\n", theta);
theta = Vector_Angle2(0, 1, -1, 0, FALSE);
printf("angle: %g\n", theta);
theta = Vector_Angle2(-1, 0, -1, -1, TRUE);
printf("angle: %g\n", theta);
#endif
return 0;
}
Int_Arraylist *Stack_Route(const Stack *stack, int start[], int end[],
Stack_Graph_Workspace *sgw)
{
if (sgw->gw == NULL) {
sgw->gw = New_Graph_Workspace();
}
if (sgw->range == NULL) {
double dist = Geo3d_Dist(start[0], start[1], start[2], end[0], end[1],
end[2]);
int margin[3];
int i = 0;
for (i = 0; i < 3; ++i) {
margin[i] = iround(dist - abs(end[i] - start[i] + 1));
if (margin[i] < 0) {
margin[i] = 0;
}
}
Stack_Graph_Workspace_Set_Range(sgw, start[0], end[0], start[1], end[1],
start[2], end[2]);
Stack_Graph_Workspace_Expand_Range(sgw, margin[0], margin[0],
margin[1], margin[1], margin[2], margin[2]);
Stack_Graph_Workspace_Validate_Range(sgw, stack->width, stack->height,
stack->depth);
}
int swidth = sgw->range[1] - sgw->range[0] + 1;
int sheight = sgw->range[3] - sgw->range[2] + 1;
int sdepth = sgw->range[5] - sgw->range[4] + 1;
int start_index = Stack_Util_Offset(start[0] - sgw->range[0],
start[1] - sgw->range[2],
start[2] - sgw->range[4],
swidth, sheight, sdepth);
int end_index = Stack_Util_Offset(end[0] - sgw->range[0],
end[1] - sgw->range[2],
end[2] - sgw->range[4],
swidth, sheight, sdepth);
if (start_index > end_index) {
int tmp;
SWAP2(start_index, end_index, tmp);
}
ASSERT(start_index >= 0, "Invalid starting index.");
ASSERT(end_index >= 0, "Invalid ending index.");
tic();
Graph *graph = Stack_Graph_W(stack, sgw);
ptoc();
tic();
int *path = NULL;
switch (sgw->sp_option) {
case 0:
path = Graph_Shortest_Path_E(graph, start_index, end_index, sgw->gw);
break;
case 1:
{
//printf("%g\n", sgw->intensity[start_index]);
sgw->intensity[end_index] = 4012;
sgw->intensity[start_index] = 4012;
path = Graph_Shortest_Path_Maxmin(graph, start_index, end_index,
sgw->intensity, sgw->gw);
}
break;
}
sgw->value = sgw->gw->dlist[end_index];
Kill_Graph(graph);
if (isinf(sgw->value)) {
return NULL;
}
#ifdef _DEBUG_2
{
Graph_Update_Edge_Table(graph, sgw->gw);
Stack *stack = Make_Stack(GREY, swidth, sheight, sdepth);
Zero_Stack(stack);
int nvoxel = (int) Stack_Voxel_Number(stack);
int index = end_index;
printf("%d -> %d\n", start_index, end_index);
while (index >= 0) {
if (index < nvoxel) {
stack->array[index] = 255;
}
int x, y, z;
Stack_Util_Coord(index, swidth, sheight, &x, &y, &z);
printf("%d (%d, %d, %d), %g\n", index, x, y, z, sgw->gw->dlist[index]);
index = path[index];
}
Write_Stack("../data/test2.tif", stack);
Kill_Stack(stack);
}
#endif
Int_Arraylist *offset_path =
Parse_Stack_Shortest_Path(path, start_index, end_index,
stack->width, stack->height, sgw);
int org_start = Stack_Util_Offset(start[0], start[1], start[2], stack->width,
stack->height, stack->depth);
if (org_start != offset_path->array[0]) {
iarray_reverse(offset_path->array, offset_path->length);
}
int org_end = Stack_Util_Offset(end[0], end[1], end[2], stack->width,
stack->height, stack->depth);
//printf("%d, %d\n", org_end, offset_path->array[offset_path->length -]);
ASSERT(org_start == offset_path->array[0], "Wrong path head.");
if (org_end != offset_path->array[offset_path->length - 1]) {
printf("debug here\n");
}
ASSERT(org_end == offset_path->array[offset_path->length - 1],
"Wrong path tail.");
ptoc();
return offset_path;
}
