/*
* Replica of STEP function:
* FUNCTION first_proj_axis()
*/
void
Axis2Placement3D::FirstProjAxis(double *proj,double *zaxis, double *refdir) {
double z[3] = VINIT_ZERO;
double v[3] = VINIT_ZERO;
double TOL = 1e-9;
if (zaxis == NULL)
return;
VMOVE(z,zaxis);
VUNITIZE(z);
if (refdir == NULL) {
double xplus[3]= {1.0,0.0,0.0};
double xminus[3]= {-1.0,0.0,0.0};
if (!VNEAR_EQUAL(z, xplus, TOL) &&
!VNEAR_EQUAL(z, xminus, TOL)) {
VSET(v,1.0,0.0,0.0);
} else {
VSET(v,0.0,1.0,0.0);
}
} else {
double cross[3];
double mag;
VCROSS(cross, refdir, z);
mag = MAGNITUDE(cross);
if (NEAR_ZERO(mag,TOL)) {
return;
} else {
VMOVE(v,refdir);
VUNITIZE(v);
}
}
double x_vec[3];
double aproj[3];
double dot = VDOT(v,z);
ScalarTimesVector(x_vec, dot, z);
VectorDifference(aproj,v,x_vec);
VSCALE(x_vec,z,dot);
VSUB2(aproj,v, x_vec);
VUNITIZE(aproj);
VMOVE(proj,aproj);
return;
}
int PointIndex::InsertPoint(
ON_3dPoint P
)
{
/* This will eventually be implemented in a much more efficient way */
for (int i = 0; i < mesh->m_V.Count(); i++) {
if (VNEAR_EQUAL(mesh->m_V[i], P, tol)) {
return i;
}
}
mesh->m_V.Append(ON_3fPoint(P));
return mesh->m_V.Count();
}
static int
test_bn_eigen2x2(int argc, char *argv[])
{
fastf_t expected_val1, expected_val2, actual_val1, actual_val2;
fastf_t a, b, c;
vect_t expected_vec1, expected_vec2, actual_vec1, actual_vec2;
if (argc != 9) {
bu_exit(1, "<args> format: a b c <expected_val1> <expected_val2> <expected_vec1> <expected_vec2> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg", &a);
sscanf(argv[3], "%lg", &b);
sscanf(argv[4], "%lg", &c);
sscanf(argv[5], "%lg", &expected_val1);
sscanf(argv[6], "%lg", &expected_val2);
sscanf(argv[7], "%lg,%lg,%lg", &expected_vec1[0], &expected_vec1[1], &expected_vec1[2]);
sscanf(argv[8], "%lg,%lg,%lg", &expected_vec2[0], &expected_vec2[1], &expected_vec2[2]);
bn_eigen2x2(&actual_val1, &actual_val2, actual_vec1, actual_vec2, a, b, c);
return !(VNEAR_EQUAL(expected_vec1, actual_vec1, BN_TOL_DIST) && VNEAR_EQUAL(expected_vec2, actual_vec2, BN_TOL_DIST) && NEAR_EQUAL(expected_val1, actual_val1, BN_TOL_DIST) && NEAR_EQUAL(expected_val2, actual_val2, BN_TOL_DIST));
}
static int
test_bn_htov_move(int argc, char *argv[])
{
hvect_t h;
vect_t expected, actual;
if (argc != 4) {
bu_exit(1, "<args> format: H <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg,%lg,%lg,%lg", &h[0], &h[1], &h[2], &h[3]);
sscanf(argv[3], "%lg,%lg,%lg", &expected[0], &expected[1], &expected[2]);
bn_htov_move(actual, h);
return !VNEAR_EQUAL(expected, actual, BN_TOL_DIST);
}
static int
test_bn_vec_ae(int argc, char *argv[])
{
vect_t expected, actual;
fastf_t az, el;
if (argc != 5) {
bu_exit(1, "<args> format: az el <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg", &az);
sscanf(argv[3], "%lg", &el);
sscanf(argv[4], "%lg,%lg,%lg", &expected[0], &expected[1], &expected[2]);
bn_vec_ae(actual, az, el);
return !VNEAR_EQUAL(expected, actual, BN_TOL_DIST);
}
static void
plot_ray_img(struct application *ap,
const struct partition *pp,
double dist,
struct bbd_img *bi)
{
static int plot_num;
FILE *pfd;
char name[256];
point_t pt;
sprintf(name, "bbd_%d.plot3", plot_num++);
bu_log("plotting %s\n", name);
if ((pfd = fopen(name, "wb")) == (FILE *)NULL) {
bu_bomb("can't open plot3 file\n");
}
/* red line from ray origin to hit point */
VJOIN1(pp->pt_inhit->hit_point, ap->a_ray.r_pt, pp->pt_inhit->hit_dist,
ap->a_ray.r_dir);
if (VNEAR_EQUAL(ap->a_ray.r_pt, pp->pt_inhit->hit_point, 0.125)) {
/* start and hit point identical, make special allowance */
vect_t vtmp;
pl_color(pfd, 255, 0, 128);
VREVERSE(vtmp, ap->a_ray.r_dir);
VJOIN1(vtmp, ap->a_ray.r_pt, 5.0, vtmp);
pdv_3line(pfd, vtmp, pp->pt_inhit->hit_point);
} else {
pl_color(pfd, 255, 0, 0);
pdv_3line(pfd, ap->a_ray.r_pt, pp->pt_inhit->hit_point);
}
/* yellow line from hit point to plane point */
VJOIN1(pt, ap->a_ray.r_pt, dist, ap->a_ray.r_dir); /* point on plane */
pl_color(pfd, 255, 255, 0);
pdv_3line(pfd, pp->pt_inhit->hit_point, pt);
/* green line from image origin to plane point */
pl_color(pfd, 0, 255, 0);
pdv_3line(pfd, pt, bi->img_origin);
fclose(pfd);
}
HIDDEN int
avpp_val_compare(const char *val1, const char *val2, const struct bn_tol *diff_tol)
{
/* We need to look for numbers to do tolerance based comparisons */
int num_compare = 1;
int color_compare = 1;
int pnt_compare = 1;
double dval1, dval2;
int c1val1, c1val2, c1val3;
int c2val1, c2val2, c2val3;
float p1val1, p1val2, p1val3;
float p2val1, p2val2, p2val3;
char *endptr;
/* Don't try a numerical comparison unless the strings differ -
* numerical attempts when they are not needed can introduce
* invalid changes */
int retval = BU_STR_EQUAL(val1, val2);
if (!retval) {
/* First, check for individual numbers */
errno = 0;
dval1 = strtod(val1, &endptr);
if (errno == EINVAL || *endptr != '\0') num_compare--;
errno = 0;
dval2 = strtod(val2, &endptr);
if (errno == EINVAL || *endptr != '\0') num_compare--;
if (num_compare == 1) {return NEAR_EQUAL(dval1, dval2, diff_tol->dist);}
/* If we didn't find numbers, try for colors (3 integer numbers) */
if (sscanf(val1, "%d %d %d", &c1val1, &c1val2, &c1val3) == 3) color_compare--;
if (sscanf(val2, "%d %d %d", &c2val1, &c2val2, &c2val3) == 3) color_compare--;
if (color_compare == 1) return retval;
/* If we didn't find numbers, try for points (3 floating point numbers) */
if (sscanf(val1, "%f %f %f", &p1val1, &p1val2, &p1val3) == 3) pnt_compare--;
if (sscanf(val2, "%f %f %f", &p2val1, &p2val2, &p2val3) == 3) pnt_compare--;
if (pnt_compare == 1) {
vect_t v1, v2;
VSET(v1, p1val1, p1val2, p1val3);
VSET(v2, p2val1, p2val2, p2val3);
return VNEAR_EQUAL(v1, v2, diff_tol->dist);
}
}
return retval;
}
int TriIntersections::Faces(
ON_ClassArray<ON_3dPoint[3]> UNUSED(faces)
)
{
if (intersections.Count() == 0) {
return 0;
}
/* first we get an array of all the segments we can use to make
* our faces.
*/
ON_SimpleArray<ON_Line> segments; /*the segments we have to make faces */
ON_SimpleArray<bool> flippable; /* whether or not the segment has direction */
ON_SimpleArray<bool> segexternal; /* whether or not the segment is from the edge */
for (int i = 0; i < intersections.Count(); i++) {
segments.Append(intersections[i]);
segments.Append(intersections[i]);
flippable.Append(false);
flippable.Append(false);
segexternal.Append(false);
segexternal.Append(false);
}
for (int i = 0; i < 3; i++) {
if (edges[i].Count() == 2) { /* the edge was never intersected */
segments.Append(ON_Line(edges[i][0], edges[i][0]));
flippable.Append(true);
segexternal.Append(true);
} else {
for (int j = 0; j < (edges[i].Count() - 1); j++) {
if (dir[i][j] == dir[i][j + 1]) {
/* this indicates an error in the intersection data */
return -1;
} else if (dir[i][j] == 0 || dir[i][j+1] == 1) {
segments.Append(ON_Line(edges[i][j], edges[i][j+1]));
flippable.Append(false);
segexternal.Append(true);
} else {
segments.Append(ON_Line(edges[i][j+1], edges[i][j]));
flippable.Append(false);
segexternal.Append(true);
}
}
}
}
/* Now that the segments are all set up it's time to make them
* into faces.
*/
ON_ClassArray<ON_Polyline> outlines;
ON_SimpleArray<bool> line_external; /* stores whether each polyline is internal */
ON_Polyline outline;
while (segments.Count() != 0) {
outline.Append(segments[0].from);
outline.Append(segments[0].to);
segments.Remove(0);
int i = 0;
bool ext = false; /* keeps track of the ternality of the path we're assembling */
while (!outline.IsClosed(tol)) {
if (i >= segments.Count()) {
return -1;
} else if (VNEAR_EQUAL(segments[i].from, outline[outline.Count() - 1], tol)) {
outline.Append(segments[i].to);
} else if (VNEAR_EQUAL(segments[i].to, outline[0], tol)) {
outline.Insert(0, segments[i].from);
} else if (VNEAR_EQUAL(segments[i].from, outline[0], tol) && flippable[i]) {
outline.Insert(0, segments[i].to);
} else if (VNEAR_EQUAL(segments[i].to, outline[outline.Count() - 1], tol) && flippable[i]) {
outline.Append(segments[i].from);
} else {
i++;
continue;
}
/* only executed when we append edge i */
segments.Remove(i);
flippable.Remove(i);
ext &= segexternal[i];
segexternal.Remove(i);
i = 0;
}
outlines.Append(outline);
line_external.Append(ext);
}
/* XXX - now we need to setup the ternality tree for the paths */
return 0;
}
/**
* tests whether a point is inside of the triangle using vector math
* the point has to be in the same plane as the triangle, otherwise
* it returns false.
*/
bool PointInTriangle(
const ON_3dPoint& a,
const ON_3dPoint& b,
const ON_3dPoint& c,
const ON_3dPoint& P,
double tol
)
{
/* First we check to make sure that the point is in the plane */
double normal[3];
VCROSS(normal, b - a, c - a);
VUNITIZE(normal);
if (!NEAR_ZERO(VDOT(normal, P - a), tol))
return false;
/* we have a point that we know is in the plane,
* but we need to check that it's in the triangle
* the cleanest way to check this is to check that
* the crosses of edges and vectors from vertices to P are all parallel or 0
* The reader could try to prove this if s/he were ambitious
*/
double v1[3];
VCROSS(v1, b - a, P - a);
if (VNEAR_ZERO(v1, tol)) {
VSETALL(v1, 0.0);
} else
VUNITIZE(v1);
double v2[3];
VCROSS(v2, c - b, P - b);
if (VNEAR_ZERO(v2, tol)) {
VSETALL(v2, 0.0);
} else
VUNITIZE(v2);
double v3[3];
VCROSS(v3, a - c, P - c);
if (VNEAR_ZERO(v3, tol)) {
VSETALL(v3, 0.0);
} else
VUNITIZE(v3);
/* basically we need to check that v1 == v2 == v3, and 0 vectors get in for free
* if 2 of them are 0 vectors, leaving the final vector with nothing to be equal too
* then P is in the triangle (this actually means P is a vertex of our triangle)
* I can't think of any slick way to do this, so it gets kinda ugly
*/
if (VNEAR_ZERO(v1, tol)) {
if (VNEAR_ZERO(v2, tol)) {
return true;
} else if (VNEAR_ZERO(v3, tol)) {
return true;
} else if (VNEAR_EQUAL(v2, v3, tol)) {
return true;
} else
return false;
} else if (VNEAR_ZERO(v2, tol)) {
if (VNEAR_ZERO(v3, tol)) {
return true;
} else if (VNEAR_EQUAL(v1, v3, tol)) {
return true;
} else
return false;
} else if (VNEAR_EQUAL(v1, v2, tol)) {
if (VNEAR_ZERO(v3, tol)) {
return true;
} else if (VNEAR_EQUAL(v2, v3, tol)) {
return true;
} else
return false;
} else
return false;
}
int TriangleTriangleIntersect(
const ON_3dPoint a,
const ON_3dPoint b,
const ON_3dPoint c,
const ON_3dPoint d,
const ON_3dPoint e,
const ON_3dPoint f,
ON_3dPoint out[6], /* indicates the points of intersection */
char edge[6], /* indicates which edge the intersection points lie on ab is 0 bc is 1 etc. */
double tol
)
{
ON_3dPoint abc[3] = {a, b, c};
ON_3dPoint def[3] = {d, e, f};
ON_3dPoint result[2];
int rv;
ON_3dPoint p1, p2;
int number_found = 0; /* number_found <= 2*/
int i, j, k; /* iterators */
/* intersect the edges of triangle abc with triangle def*/
for (i = 0; i < 3; i++) {
rv = SegmentTriangleIntersect(d, e, f, abc[i], abc[(i+1)%3], result, tol);
for (j = 0; j < rv; j++) {
ON_3dPoint P = result[j];
bool dup = false;
for (k = 0; k < number_found; k++) {
if (VNEAR_EQUAL(out[k], P, tol)) {
dup = true;
break;
}
}
if (!dup) {
out[number_found] = P;
edge[number_found] = i;
number_found++;
}
}
}
/* intersect the edges of triangle def with triangle abc*/
for (i = 0; i < 3; i++) {
rv = SegmentTriangleIntersect(a, b, c, def[i], def[(i + 1) % 3], result, tol);
for (j = 0; j < rv; j++) {
ON_3dPoint P = result[j];
bool dup = false;
for (k = 0; k < number_found; k++) {
if (VNEAR_EQUAL(out[k], P, tol)) {
dup = true;
break;
}
}
if (!dup) {
out[number_found] = P;
edge[number_found] = i+3;
number_found++;
}
}
}
/* now we check if the points need to be reordered to meet our
* condition, and reorder them if necessary
*/
if (number_found == 2) {
double T1norm[3];
VCROSS(T1norm, b - a, c - a);
VUNITIZE(T1norm);
double T2norm[3];
VCROSS(T2norm, b - a, c - a);
VUNITIZE(T2norm);
double T2normXT1norm[3];
VCROSS(T2normXT1norm, T1norm, T2norm);
if (VDOT(T2normXT1norm, (out[1] - out[2])) < 0) {
/* the points are in the wrong order swap them */
ON_3dPoint tmpP = out[1];
out[1] = out[0];
out[0] = tmpP;
/* and swap the edges they came from */
char tmpC = edge[1];
edge[1] = edge[0];
edge[0] = tmpC;
}
}
return number_found;
}
/**
* intersects a triangle ABC with a line PQ
*
* return values:
* -1: error
* 0: no intersection
* 1: intersects in a point
* 2: intersects in a line
*/
int SegmentTriangleIntersect(
const ON_3dPoint& a,
const ON_3dPoint& b,
const ON_3dPoint& c,
const ON_3dPoint& p,
const ON_3dPoint& q,
ON_3dPoint out[2],
double tol
)
{
ON_3dPoint triangle[3] = {a, b, c}; /* it'll be nice to have this as an array too*/
/* First we need to get our plane into point normal form (N \dot (P - P0) = 0)
* Where N is a normal vector, and P0 is a point in the plane
* P0 can be any of {a, b, c} so that's easy
* Finding N
*/
double normal[3];
VCROSS(normal, b - a, c - a);
VUNITIZE(normal);
ON_3dPoint P0 = a; /* could be b or c*/
/* Now we've got our plane in a manageable form (two of them actually)
* So here's the rest of the plan:
* Every point P on the line can be written as: P = p + u (q - p)
* We just need to find u
* We know that when u is correct:
* normal dot (q + u * (q-p) = N dot P0
* N dot (P0 - p)
* so u = --------------
* N dot (q - p)
*/
if (!NEAR_ZERO(VDOT(normal, (p-q)), tol)) {/* if this is 0 it indicates the line and plane are parallel*/
double u = VDOT(normal, (P0 - p))/VDOT(normal, (q - p));
if (u < 0.0 || u > 1.0) /* this means we're on the line but not the line segment*/
return 0; /* so we can return early*/
ON_3dPoint P = p + u * (q - p);
if (PointInTriangle(a, b, c, P, tol)) {
out[0] = P;
return 1;
}
return 0;
} else {
/* If we're here it means that the line and plane are parallel*/
if (NEAR_ZERO(VDOT(normal, p-P0), tol)) {/* yahtzee!!*/
/* The line segment is in the same plane as the triangle*/
/* So first we check if the points are inside or outside the triangle*/
bool p_in = PointInTriangle(a, b, c, p, tol);
bool q_in = PointInTriangle(a, b , c , q , tol);
ON_3dPoint x[2]; /* a place to put our results*/
if (q_in && p_in) {
out[0] = p;
out[1] = q;
return 2;
} else if (q_in || p_in) {
if (q_in)
out[0] = q;
else
out[0] = p;
int i;
int rv;
for (i=0; i<3; i++) {
rv = SegmentSegmentIntersect(triangle[i], triangle[(i+1)%3], p, q, x, tol);
if (rv == 1) {
out[1] = x[0];
return 1;
} else if (rv == 2) {
out[0] = x[0];
out[1] = x[1];
return 2;
}
}
} else {
/* neither q nor p is in the triangle*/
int i;
int points_found = 0;
int rv;
for (i = 0; i < 3; i++) {
rv = SegmentSegmentIntersect(triangle[i], triangle[(i+1)%3], p, q, x, tol);
if (rv == 1) {
if (points_found == 0 || !VNEAR_EQUAL(out[0], x[0], tol)) { /* in rare cases we can get the same point twice*/
out[points_found] = x[0];
points_found++;
}
} else if (rv == 2) {
out[0] = x[0];
out[1] = x[1];
return 2;
}
}
return points_found;
}
} else
return 0;
}
return -1;
}
/**
* finds the intersection point between segments x1, x2 and x3, x4 and
* stores the result in x we assume that the segments are coplanar.
*
* return values:
*
* 0: no intersection
* 1: intersection in a point
* 2: intersection in a line
*
*
* x1* *x3
* \ /
* \ /
* \ /
* \ /
* \ /
* \ /
* \ /
* \ /
* * <----x
* / \
* / \
* / \
* / \
* / \
* / \
* x4* *x2
*
*
*
* the equations for the lines are:
*
* P(s) = x1 + s (x2 - x1) s in [0, 1]
* Q(t) = x3 + t (x4 - x3) t in [0, 1]
*
* so we need to find s and t s.t. P(s) = Q(t)
* So some vector calculus tells us that:
*
* (CXB) dot (AXB)
* s = ---------------
* |AXB|^2
*
* (-CXA) dot (BXA)
* t = ----------------
* |BXA|^2
*
*
* Where we define:
*
* A = (x2-x1)
* B = (x4-x3)
* C = (x3-x1)
*
* This equation blows up if |AXB|^2 is 0 (in which case |BXA|^2 is
* also 0), which indicates that the lines are parallel which is kind
* of a pain.
*/
int SegmentSegmentIntersect(
const ON_3dPoint& x1,
const ON_3dPoint& x2,
const ON_3dPoint& x3,
const ON_3dPoint& x4,
ON_3dPoint x[2], /* segments could in degenerate cases intersect in another segment*/
double tol
)
{
ON_3dPoint A = (x2 - x1);
ON_3dPoint B = (x4 - x3);
ON_3dPoint C = (x3 - x1);
double AXB[3];
VCROSS(AXB, A, B);
double BXA[3];
VCROSS(BXA, B, A);
double CXB[3];
VCROSS(CXB, C, B);
double negC[3];
VSCALE(negC, C, -1.0);
double negCXA[3];
VCROSS(negCXA, negC, A);
if (VNEAR_ZERO(AXB, tol)) {/* the lines are parallel **commence sad music*/
/* this is a potential bug if someone gets cheeky and passes us x2==x1*/
double coincident_test[3];
VCROSS(coincident_test, x4 - x2, x4 - x1);
if (VNEAR_ZERO(coincident_test, tol)) {
/* the lines are coincident, meaning the segments lie on the same
* line but they could:
* --not intersect at all
* --intersect in a point
* --intersect in a segment
* So here's the plan we. We're going to use dot products,
* The aspect of dot products that's important:
* A dot B is positive if A and B point the same way
* and negative when they point in opposite directions
* so --> dot --> is positive, but <-- dot --> is negative
* so if (x3-x1) dot (x4-x1) is negative, then x1 lies on the segment (x3, x4)
* which means that x1 should be one of the points we return so we just go
* through and find which points are contained in the other segment
* and those are our return values
*/
int points = 0;
if (x1 == x3 || x1 == x4) {
x[points] = x1;
points++;
}
if (x2 == x3 || x2 == x4) {
x[points] = x2;
points++;
}
if (VDOT((x3 - x1), (x4 - x1)) < 0) {
x[points] = x1;
points++;
}
if (VDOT((x3 - x2), (x4 - x2)) < 0) {
x[points] = x2;
points++;
}
if (VDOT((x1 - x3), (x2 - x3)) < 0) {
x[points] = x3;
points++;
}
if (VDOT((x1 - x4), (x2 - x4)) < 0) {
x[points] = x4;
points++;
}
assert(points <= 2);
return points;
}
} else {
double s = VDOT(CXB, AXB)/MAGSQ(AXB);
double t = VDOT(negCXA, BXA)/MAGSQ(BXA);
/* now we need to perform some tests to make sure we're not
* outside these bounds by tiny little amounts */
if (-tol <= s && s <= 1.0 + tol && -tol <= t && t <= 1.0 + tol) {
ON_3dPoint Ps = x1 + s * (x2 - x1); /* The answer according to equation P*/
ON_3dPoint Qt = x3 + t * (x4 - x3); /* The answer according to equation Q*/
assert(VNEAR_EQUAL(Ps, Qt, tol)); /* check to see if they agree, just a sanity check*/
x[0] = Ps;
return 1;
} else {
/* this happens when the lines through x1, x2 and x3, x4 intersect but not the segments*/
return 0;
}
}
return 0;
}
/* returns 0 to continue, 1 if the left face was split, 2 if the right face was
* split */
int
split_face(struct soup_s *left, unsigned long int left_face, struct soup_s *right, unsigned long int right_face, const struct bn_tol *tol) {
struct face_s *lf, *rf;
vect_t isectpt[2] = {{0, 0, 0}, {0, 0, 0}};
int coplanar, r = 0;
lf = left->faces+left_face;
rf = right->faces+right_face;
splitz++;
if (gcv_tri_tri_intersect_with_isectline(left, right, lf, rf, &coplanar, (point_t *)isectpt, tol) != 0 && !VNEAR_EQUAL(isectpt[0], isectpt[1], tol->dist)) {
splitty++;
if (split_face_single(left, left_face, isectpt, &right->faces[right_face], tol) > 1) r|=0x1;
if (split_face_single(right, right_face, isectpt, &left->faces[left_face], tol) > 1) r|=0x2;
}
return r;
}