BRL-CAD
util.c
Go to the documentation of this file.
1 /* U T I L . C
2  * BRL-CAD
3  *
4  * Copyright (c) 2013-2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * Copyright 2001 softSurfer, 2012 Dan Sunday
8  * This code may be freely used and modified for any purpose
9  * providing that this copyright notice is included with it.
10  * SoftSurfer makes no warranty for this code, and cannot be held
11  * liable for any real or imagined damage resulting from its use.
12  * Users of this code must verify correctness for their application.
13  *
14  */
15 /** @file util.c
16  *
17  * This file implements utility functions internal to libbn.
18  */
19
20 #include "common.h"
21 #include <stdlib.h>
22
23 #include "vmath.h"
24 #include "bn/plane_struct.h"
25 #include "bn/plane_calc.h"
26 #include "bn/tol.h"
27
28 int
29 coplanar_2d_coord_sys(point_t *origin_pnt, vect_t *u_axis, vect_t *v_axis, const point_t *points_3d, int n)
30 {
31  int i = 0;
32  int have_normal = 0;
33  plane_t plane;
34  fastf_t dist_pt_pt = 0.0;
35  fastf_t vdot = 1.0;
36  point_t p_farthest;
37  int p_farthest_index = 0;
38  vect_t normal = VINIT_ZERO;
39  const struct bn_tol tol = {BN_TOL_MAGIC, BN_TOL_DIST/2.0, BN_TOL_DIST*BN_TOL_DIST/4.0, 1.0e-6, 1.0-1.0e-6};
40
41  /* Step 1 - find center point */
42  VSETALL(*origin_pnt, 0.0);
43  for (i = 0; i < n; i++) {
44  VADD2(*origin_pnt, *origin_pnt, points_3d[i]);
45  }
46  VSCALE(*origin_pnt, *origin_pnt, 1.0/n);
47
48  /* Step 2 - find furthest points from the center point */
49  VSETALL(p_farthest, 0.0);
50  for (i = 0; i < n; i++) {
51  fastf_t curr_dist = DIST_PT_PT_SQ(*origin_pnt, points_3d[i]);
52  if (curr_dist > dist_pt_pt) {
53  dist_pt_pt = curr_dist;
54  VMOVE(p_farthest, points_3d[i]);
55  p_farthest_index = i;
56  }
57  }
58  VSUB2(*u_axis, p_farthest, *origin_pnt);
59  VUNITIZE(*u_axis);
60
61  /* Step 3 - find normal vector of plane holding points */
62  i = 0;
63  dist_pt_pt = DIST_PT_PT(*origin_pnt, p_farthest);
64  while (i < n) {
65  if (i != p_farthest_index) {
66  vect_t temp_vect;
67  fastf_t curr_vdot;
68  VSUB2(temp_vect, points_3d[i], *origin_pnt);
69  VUNITIZE(temp_vect);
70  curr_vdot = fabs(VDOT(temp_vect, *u_axis));
71  if (curr_vdot < vdot) {
72  if (!bn_mk_plane_3pts(plane, *origin_pnt, p_farthest, points_3d[i], &tol)) {
73  VSET(normal, plane[0], plane[1], plane[2]);
74  have_normal = 1;
75  vdot = curr_vdot;
76  }
77  }
78  }
79  i++;
80  }
81  if (!have_normal) return -1;
82  VUNITIZE(normal);
83
84  /* Step 4 - use vectors from steps 2 and 3 to find y axis vector */
85  VCROSS(*v_axis, *u_axis, normal);
86  VUNITIZE(*v_axis);
87
88  return 0;
89 }
90
91
92 int
93 coplanar_3d_to_2d(point2d_t **points_2d, const point_t *origin_pnt,
94  const vect_t *u_axis, const vect_t *v_axis,
95  const point_t *points_3d, int n)
96 {
97  int i = 0;
98  for (i = 0; i < n; i++) {
99  vect_t temp, c, d;
100  fastf_t u, v;
101  VSUB2(temp, points_3d[i], *origin_pnt);
102  VPROJECT(temp, *u_axis, c, d);
103  u = (VDOT(c, *u_axis) > 0.0) ? (MAGNITUDE(c)) : (-1.0 * MAGNITUDE(c));
104  v = (VDOT(d, *v_axis) > 0.0) ? (MAGNITUDE(d)) : (-1.0 * MAGNITUDE(d));
105  V2SET((*points_2d)[i], u, v);
106  }
107
108  return 0;
109 }
110
111
112 int
113 coplanar_2d_to_3d(point_t **points_3d, const point_t *origin_pnt,
114  const vect_t *u_axis, const vect_t *v_axis,
115  const point2d_t *points_2d, int n)
116 {
117 int i;
118  vect_t u_x_component, u_y_component, u_z_component;
119  vect_t v_x_component, v_y_component, v_z_component;
120  vect_t x_axis, y_axis, z_axis;
121  vect_t temp;
122  fastf_t mag_u_x, mag_u_y, mag_u_z;
123  fastf_t mag_v_x, mag_v_y, mag_v_z;
124  VSET(x_axis, 1.0, 0.0, 0.0);
125  VSET(y_axis, 0.0, 1.0, 0.0);
126  VSET(z_axis, 0.0, 0.0, 1.0);
127  /* Step 1 - find the 3d X, Y and Z components of u_axis and v_axis */
128  VPROJECT(*u_axis, x_axis, u_x_component, temp);
129  VPROJECT(temp, y_axis, u_y_component, u_z_component);
130  VPROJECT(*v_axis, x_axis, v_x_component, temp);
131  VPROJECT(temp, y_axis, v_y_component, v_z_component);
132  mag_u_x = (VDOT(u_x_component, x_axis) > 0.0) ? (MAGNITUDE(u_x_component)) : (-1.0 * MAGNITUDE(u_x_component));
133  mag_u_y = (VDOT(u_y_component, y_axis) > 0.0) ? (MAGNITUDE(u_y_component)) : (-1.0 * MAGNITUDE(u_y_component));
134  mag_u_z = (VDOT(u_z_component, z_axis) > 0.0) ? (MAGNITUDE(u_z_component)) : (-1.0 * MAGNITUDE(u_z_component));
135  mag_v_x = (VDOT(v_x_component, x_axis) > 0.0) ? (MAGNITUDE(v_x_component)) : (-1.0 * MAGNITUDE(v_x_component));
136  mag_v_y = (VDOT(v_y_component, y_axis) > 0.0) ? (MAGNITUDE(v_y_component)) : (-1.0 * MAGNITUDE(v_y_component));
137  mag_v_z = (VDOT(v_z_component, z_axis) > 0.0) ? (MAGNITUDE(v_z_component)) : (-1.0 * MAGNITUDE(v_z_component));
138
139  /* Step 2 - for each 2D point, calculate the (x,y,z) coordinates as follows:
140  * (http://math.stackexchange.com/questions/525829/how-to-find-the-3d-coordinate-of-a-2d-point-on-a-known-plane)
141  */
142  for (i = 0; i < n; i++) {
143  vect_t temp_2d;
144  VSET(temp_2d, points_2d[i][0]*mag_u_x + (points_2d)[i][1]*mag_v_x,
145  (points_2d)[i][0]*mag_u_y + (points_2d)[i][1]*mag_v_y,
146  (points_2d)[i][0]*mag_u_z + (points_2d)[i][1]*mag_v_z);
147  VADD2((*points_3d)[i], (*origin_pnt), temp_2d);
148  }
149
150  return 0;
151 }
152
153
154 /*
155  * Local Variables:
156  * mode: C
157  * tab-width: 8
158  * indent-tabs-mode: t
159  * c-file-style: "stroustrup"
160  * End:
161  * ex: shiftwidth=4 tabstop=8
162  */
#define VSET(a, b, c, d)
Definition: color.c:53
#define VSETALL(a, s)
Definition: color.c:54
#define BN_TOL_MAGIC
Definition: magic.h:74
Header file for the BRL-CAD common definitions.
int coplanar_2d_coord_sys(point_t *origin_pnt, vect_t *u_axis, vect_t *v_axis, const point_t *points_3d, int n)
Find a 2D coordinate system for a set of co-planar 3D points.
Definition: util.c:29
#define BN_TOL_DIST
Definition: tol.h:109
Support for uniform tolerances.
Definition: tol.h:71
int coplanar_3d_to_2d(point2d_t **points_2d, const point_t *origin_pnt, const vect_t *u_axis, const vect_t *v_axis, const point_t *points_3d, int n)
Find 2D coordinates for a set of co-planar 3D points.
Definition: util.c:93
int coplanar_2d_to_3d(point_t **points_3d, const point_t *origin_pnt, const vect_t *u_axis, const vect_t *v_axis, const point2d_t *points_2d, int n)
Find 3D coordinates for a set of 2D points given a coordinate system.
Definition: util.c:113
double fastf_t
Definition: defines.h:300
int bn_mk_plane_3pts(plane_t plane, const point_t a, const point_t b, const point_t c, const struct bn_tol *tol)