fix handling of normals to objects from points on prism faces (#14)

Author: Homer Reid

utils/geom.c

@@ -2190,28 +2190,6 @@ boolean point_in_polygon(double px, double py, vector3 *vertices, int num_vertic
   return num_side_intersections%2;
 }
 
-void add_to_list(double s, double *slist, int length)
-{
-  switch(length) 
-   { case 0: 
-       slist[0] = s;
-       break;
-     case 1: 
-       if (s>=slist[0])
-        slist[1]=s;
-       else
-        { slist[1]=slist[0]; slist[0]=s; }
-       break;
-     default:
-       if (s<slist[0])
-        { slist[1]=slist[0]; slist[0]=s; }
-       else if (s<slist[1])
-        slist[1]=s;
-       break;
-   }
-     
-}
-
 /***************************************************************/
 /* return 1 or 0 if xc lies inside or outside the prism        */
 /***************************************************************/
@@ -2322,36 +2300,25 @@ double intersect_line_segment_with_prism(prism *prsm, vector3 p, vector3 d, doub
 }
 
 /***************************************************************/
-/* compute the minimum-length vector from p to the plane       */
+/* compute the minimum distance from p to the plane            */
 /* containing o (origin) and spanned by basis vectors v1,v2    */
 /* algorithm: solve the 3x3 system p-s*v3 = o + t*v1 + u*v2    */
-/* where v3 = v1 x v2 and s,t,u are unknowns                   */
+/* where v3 = v1 x v2 and s,t,u are unknowns.                  */
+/* return value is unit normal vector to plane and *s is such  */
+/* that p-(*s)*v3 lies in the plane                            */
 /***************************************************************/
-vector3 normal_to_plane(vector3 o, vector3 v1, vector3 v2, vector3 p)
+vector3 normal_to_plane(vector3 o, vector3 v1, vector3 v2, vector3 p, double *s)
 {
-  vector3 RHS = vector3_minus(p,o);
-
-  // handle the degenerate-to-2D case
-  if ( (fabs(v1.z) + fabs(v2.z)) < 2.0e-7 && fabs(RHS.z)<1.0e-7 )
-   { vector3 zhat={0,0,1};
-     vector3 v3 = vector3_cross(zhat, v1);
-     double M00  = v1.x;     double M10  = v3.x;
-     double M01  = v1.y;     double M11  = v3.y;
-     double DetM = M00*M11 - M01*M10;
-     if ( fabs(DetM) < 1.0e-10 )
-      return vector3_scale(0.0, v3);
-     // double t = (M00*RHSy-M10*RHSx)/DetM;
-     double s= (M11*RHS.x-M01*RHS.y)/DetM;
-     return vector3_scale(-1.0*s,v3);
-   }
-
-  vector3 v3 = vector3_cross(v1, v2);
+  vector3 v3 = unit_vector3(vector3_cross(v1, v2));
+  CHECK( (vector3_norm(v3)>1.0e-6), "degenerate plane in normal_to_plane" );
   matrix3x3 M;
   M.c0 = v1;
   M.c1 = v2;
-  M.c2 = vector3_scale(-1.0, v3);
-  vector3 tus = matrix3x3_vector3_mult(matrix3x3_inverse(M),RHS);
-  return vector3_scale(-1.0*tus.z, v3);
+  M.c2 = v3;
+  vector3 RHS = vector3_minus(p,o);
+  vector3 tus = matrix3x3_vector3_mult(matrix3x3_inverse(M),RHS); // "t, u, s"
+  *s = tus.z;
+  return v3;
 }
 
 /***************************************************************/
@@ -2370,19 +2337,21 @@ vector3 normal_to_prism(prism *prsm, vector3 xc)
   vector3 axis = {0,0,0}; axis.z=height;
 
   vector3 retval;
-  double min_distance;
+  double min_distance=HUGE_VAL;
 
   // side walls
-  int nv;
-  for(nv=0; nv<num_vertices; nv++)
-   { int nvp1 = ( nv==(num_vertices-1) ? 0 : nv+1 );
-     vector3 v1 = vector3_minus(vertices[nvp1],vertices[nv]);
-     vector3 v2 = axis;
-     vector3 v3 = normal_to_plane(vertices[nv], v1, v2, xp);
-     double distance = vector3_norm(v3);
-     if (nv==0 || distance < min_distance)
-      { min_distance = distance;
-        retval = v3;
+  if (height>0.0)
+   { int nv;
+     for(nv=0; nv<num_vertices; nv++)
+      { int nvp1 = ( nv==(num_vertices-1) ? 0 : nv+1 );
+        vector3 v1 = vector3_minus(vertices[nvp1],vertices[nv]);
+        vector3 v2 = axis;
+        double s;
+        vector3 v3 = normal_to_plane(vertices[nv], v1, v2, xp, &s);
+        if (fabs(s) < min_distance)
+         { min_distance = fabs(s);
+           retval = v3;
+         }
       }
    }
 
@@ -2394,10 +2363,10 @@ vector3 normal_to_prism(prism *prsm, vector3 xc)
      vector3 v2 = vector3_cross(zhat,v1);
      vector3 o  = {0,0,0};
      if (UpperLower) o.z = height;
-     vector3 v3 = normal_to_plane(o, v1, v2, xp);
-     double distance = vector3_norm(v3);
-     if (distance < min_distance)
-      { min_distance = distance;
+     double s;
+     vector3 v3 = normal_to_plane(o, v1, v2, xp, &s);
+     if (fabs(s) < min_distance)
+      { min_distance = fabs(s);
         retval = v3;
       }
    }

utils/test-prism.c

@@ -33,13 +33,6 @@
 
 #define K_PI 3.141592653589793238462643383279502884197
 
-#define NUMPTS   10000
-#define NUMLINES 1000
-
-#define LX 0.5
-#define LY 1.0
-#define LZ 1.5
-
 // routine from geom.c that rotates the coordinate of a point
 // from the prism coordinate system to the cartesian coordinate system
 vector3 prism_coordinate_p2c(prism *prsm, vector3 vp);
@@ -63,22 +56,69 @@ static double drand()
 /************************************************************************/
 /* random point uniformly distributed over a parallelepiped             */
 /************************************************************************/
-vector3 random_point(vector3 min_corner, vector3 max_corner)
-{ return make_vector3( urand(min_corner.x, max_corner.x),
+vector3 random_point_in_box(vector3 min_corner, vector3 max_corner)
+{ 
+  return make_vector3( urand(min_corner.x, max_corner.x),
                        urand(min_corner.y, max_corner.y),
                        urand(min_corner.z, max_corner.z)
                      );
 }
 
 /************************************************************************/
-/* random unit vector uniformly distributed over a sphere               */
+/* random point uniformly distributed over a planar polygon             */
+/*  (all z coordinates are 0)                                           */
+/************************************************************************/
+vector3 random_point_in_polygon(vector3 *vertices, int num_vertices)
+{
+  // randomly choose a vertex and generate random point within the triangle 
+  // formed by that vertex, the next vertex, and the centroid
+  int which_vertex = rand() % num_vertices;
+  vector3 v0 = {0,0,0}; 
+  vector3 v1 = vertices[which_vertex];
+  vector3 v2 = vertices[(which_vertex+1)%num_vertices];
+  double xi = urand(0.0,1.0), eta = urand(0.0,1.0-xi);
+  return vector3_plus( vector3_scale(xi,  vector3_minus(v1,v0)),
+                       vector3_scale(eta, vector3_minus(v2,v0))
+                     );
+}
+
+
+/************************************************************************/
+/* random point uniformly distributed over the surface of a prism       */
+/************************************************************************/
+vector3 random_point_on_prism(geometric_object o)
+{
+  prism *prsm       = o.subclass.prism_data;
+  vector3 *vertices = prsm->vertices.items;
+  int num_vertices  = prsm->vertices.num_items;
+  double height     = prsm->height;
+
+  // choose a face
+  int num_faces     = num_vertices + 2;
+  int which_face    = rand() % num_faces;
+  if ( which_face < num_vertices ) // side face
+   { vector3 min_corner = vertices[which_face];
+     vector3 max_corner = vertices[ (which_face+1)%num_vertices ];
+     max_corner.z = height;
+     return random_point_in_box( prism_coordinate_p2c(prsm, min_corner),
+                                 prism_coordinate_p2c(prsm, max_corner) );
+   }
+  else // floor or ceiling
+   { 
+     vector3 p = random_point_in_polygon(vertices, num_vertices);
+     if (which_face==num_faces-1) p.z=height; 
+     return prism_coordinate_p2c(prsm, p);
+   }
+}
+
+/************************************************************************/
+/* random unit vector with direction uniformly distributed over unit sphere*/
 /************************************************************************/
-vector3 random_unit_vector3(void)
+vector3 random_unit_vector3()
 {
-  double z   = urand(-1.0,1.0);
-  double rho = sqrt(1 - z*z);
-  double phi = urand(0.0, 2.0*K_PI);
-  return make_vector3( rho*cos(phi), rho*sin(phi), z );
+  double cos_theta=urand(0.0,1.0), sin_theta=sqrt(1.0-cos_theta*cos_theta);
+  double phi=urand(0.0,2.0*K_PI);
+  return make_vector3(sin_theta*cos(phi), sin_theta*sin(phi), cos_theta);
 }
 
 /***************************************************************/
@@ -169,7 +209,7 @@ int test_point_inclusion(geometric_object the_block, geometric_object the_prism,
   int n;
   for(n=0; n<num_tests; n++)
    { 
-     vector3 p = random_point(min_corner, max_corner);
+     vector3 p = random_point_in_box(min_corner, max_corner);
      boolean in_block=point_in_objectp(p,the_block);
      boolean in_prism=point_in_objectp(p,the_prism);
      if (in_block!=in_prism)
@@ -185,6 +225,7 @@ int test_point_inclusion(geometric_object the_block, geometric_object the_prism,
 /************************************************************************/
 /* second unit test: check calculation of normals to objects            */
 /************************************************************************/
+#define PFACE 0.1
 int test_normal_to_object(geometric_object the_block, geometric_object the_prism,
                           int num_tests, int write_log)
 {
@@ -194,13 +235,18 @@ int test_normal_to_object(geometric_object the_block, geometric_object the_prism
   FILE *f = write_log ? fopen("/tmp/test-prism.normals","w") : 0;
 
   int num_failed=0;
-  int n;
   double tolerance=1.0e-6;
+
+  int n;
   for(n=0; n<num_tests; n++)
    { 
-     // randomly generated base point within enlarged bounding box
-     vector3 p = random_point(min_corner, max_corner);
-
+     // with probability PFACE, generate random base point lying on one
+     //  of the 6 faces of the prism.
+     // with probability 1-PFACE, generate random base point lying in the
+     //  extended volume (2x volume of block)
+     vector3 p = ( urand(0.0,1.0) < PFACE) ? random_point_on_prism(the_prism)
+                                           : random_point_in_box(min_corner, max_corner);
+      
      vector3 nhat_block=standardize(normal_to_object(p, the_block));
      vector3 nhat_prism=standardize(normal_to_object(p, the_prism));
      if (!vector3_nearly_equal(nhat_block, nhat_prism, tolerance))
@@ -234,7 +280,7 @@ int test_line_segment_intersection(geometric_object the_block, geometric_object
   for(n=0; n<num_tests; n++)
    { 
      // randomly generated base point within enlarged bounding box
-     vector3 p = random_point(min_corner, max_corner);
+     vector3 p = random_point_in_box(min_corner, max_corner);
      vector3 d = random_unit_vector3();
      double a  = urand(0.0,1.0);
      double b  = urand(0.0,1.0);
@@ -258,6 +304,13 @@ int test_line_segment_intersection(geometric_object the_block, geometric_object
 /* block and as a prism) and verify that geometric primitives  */
 /* give identical results                                      */
 /***************************************************************/
+#define NUMPTS   10000
+#define NUMLINES 1000
+
+#define LX 0.5
+#define LY 1.0
+#define LZ 1.5
+
 int run_unit_tests()
 {
   void* m = NULL;
@@ -266,16 +319,18 @@ int run_unit_tests()
   vector3 yhat = make_vector3(0,1,0);
   vector3 zhat = make_vector3(0,0,1);
   vector3 size = make_vector3(LX,LY,LZ);
-  geometric_object the_block = make_block(m, c, xhat, yhat, zhat, size);
 
   vector3 v[4]; 
   v[0].x=-0.5*LX; v[0].y=-0.5*LY; v[0].z=-0.5*LZ;
   v[1].x=+0.5*LX; v[1].y=-0.5*LY; v[1].z=-0.5*LZ;
   v[2].x=+0.5*LX; v[2].y=+0.5*LY; v[2].z=-0.5*LZ;
   v[3].x=-0.5*LX; v[3].y=+0.5*LY; v[3].z=-0.5*LZ;
+
+  geometric_object the_block = make_block(m, c, xhat, yhat, zhat, size);
   geometric_object the_prism=make_prism(m, v, 4, LZ, zhat);
 
-  int write_log=0;
+  char *s=getenv("LIBCTL_TEST_PRISM_LOG");
+  int write_log = (s && s[0]=='1') ? 1 : 0;
 
   if (write_log)
    prism2gnuplot(the_prism.subclass.prism_data, "/tmp/test-prism.prism");