Fix the computation of shadow points (#22)

Clear the intersections vector before each intersectsRay() call so that intersections[0] subsequently corresponds to the j-th body. Previously, only the dot product resulting from the first body intersecting the ray has been considered, ignoring all further intersections which have been appended to the intersections vector. Co-authored-by: chris20623 <[email protected]>
PR2 · Aug 13, 2020 · b24715a · b24715a
1 parent b20cfb5
commit b24715a
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/include/robot_self_filter/self_mask.h b/include/robot_self_filter/self_mask.h
@@ -371,7 +371,7 @@ struct LinkInfo
           if (out == OUTSIDE)
           {
 
-            // we check it the point is a shadow point 
+            // we check if the point is a shadow point 
             tf::Vector3 dir(sensor_pos_ - pt);
             tfScalar  lng = dir.length();
             if (lng < min_sensor_dist_)
@@ -382,15 +382,20 @@ struct LinkInfo
 
               std::vector<tf::Vector3> intersections;
               for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+              {
+                // get the 1st intersection of ray pt->sensor
+                intersections.clear(); // intersectsRay doesn't clear the vector...
 		if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
 		{
+                  // is the intersection between point and sensor?
                   if (dir.dot(sensor_pos_ - intersections[0]) >= 0.0)
                   {
                     if (intersectionCallback)
                       intersectionCallback(intersections[0]);
                     out = SHADOW;
                   }
 		}
+              }
 
               // if it is not a shadow point, we check if it is inside the scaled body
               for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
@@ -597,7 +602,7 @@ struct LinkInfo
             // if the point is not inside the unscaled body,
             if (out == OUTSIDE)
             {
-              // we check it the point is a shadow point 
+              // we check if the point is a shadow point 
               tf::Vector3 dir(sensor_pos_ - pt);
               tfScalar  lng = dir.length();
               if (lng < min_sensor_dist_) {
@@ -610,8 +615,11 @@ struct LinkInfo
 
 		std::vector<tf::Vector3> intersections;
 		for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j) {
+                  // get the 1st intersection of ray pt->sensor
+                  intersections.clear(); // intersectsRay doesn't clear the vector...
                   if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
                   {
+                    // is the intersection between point and sensor?
                     if (dir.dot(sensor_pos_ - intersections[0]) >= 0.0)
                     {
                       if (callback)