Use emplace_back where applicapable

flexible-collision-library · Aug 9, 2016 · 3693e16 · 3693e16
1 parent b749c03
commit 3693e16
Show file tree

Hide file tree

Showing 7 changed files with 53 additions and 66 deletions.
diff --git a/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h
@@ -445,7 +445,7 @@ void SaPCollisionManager<Scalar>::registerObjects(const std::vector<CollisionObj
         {
           if(pos_next == NULL) pos_next = pos_it;
           if(pos_it->aabb->cached.overlap(aabb->cached))
-            overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj));
+            overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj);
         }
         pos_it = pos_it->next[axis];
       }
@@ -520,7 +520,7 @@ void SaPCollisionManager<Scalar>::registerObject(CollisionObject<Scalar>* obj)
       {
         if(current != curr->lo)
           if(current->aabb->cached.overlap(curr->cached))
-            overlap_pairs.push_back(SaPPair(current->aabb->obj, obj));
+            overlap_pairs.emplace_back(current->aabb->obj, obj);
 
         current = current->next[coord];
       }

diff --git a/include/fcl/narrowphase/detail/box_box.h b/include/fcl/narrowphase/detail/box_box.h
@@ -618,7 +618,7 @@ int boxBox2(
 
     // Vector3<Scalar> pointInWorld((pa + pb) * 0.5);
     // contacts.push_back(ContactPoint<Scalar>(-normal, pointInWorld, -*depth));
-    contacts.push_back(ContactPoint<Scalar>(normal,pb,-*depth));
+    contacts.emplace_back(normal, pb, -*depth);
     *return_code = code;
 
     return 1;
@@ -804,7 +804,7 @@ int boxBox2(
       for(int j = 0; j < cnum; ++j)
       {
         Vector3<Scalar> pointInWorld = points[j] + (*pa);
-        contacts.push_back(ContactPoint<Scalar>(normal, pointInWorld, -dep[j]));
+        contacts.emplace_back(normal, pointInWorld, -dep[j]);
       }
     }
     else
@@ -813,7 +813,7 @@ int boxBox2(
       for(int j = 0; j < cnum; ++j)
       {
         Vector3<Scalar> pointInWorld = points[j] + (*pa) - normal * dep[j];
-        contacts.push_back(ContactPoint<Scalar>(normal, pointInWorld, -dep[j]));
+        contacts.emplace_back(normal, pointInWorld, -dep[j]);
       }
     }
   }
@@ -839,9 +839,9 @@ int boxBox2(
     {
       Vector3<Scalar> posInWorld = points[iret[j]] + (*pa);
       if(code < 4)
-        contacts.push_back(ContactPoint<Scalar>(normal, posInWorld, -dep[iret[j]]));
+        contacts.emplace_back(normal, posInWorld, -dep[iret[j]]);
       else
-        contacts.push_back(ContactPoint<Scalar>(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
+        contacts.emplace_back(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
     }
     cnum = maxc;
   }
@@ -1191,7 +1191,7 @@ int boxBox2(
 
     // Vector3<Scalar> pointInWorld((pa + pb) * 0.5);
     // contacts.push_back(ContactPoint<Scalar>(-normal, pointInWorld, -*depth));
-    contacts.push_back(ContactPoint<Scalar>(normal,pb,-*depth));
+    contacts.emplace_back(normal, pb, -*depth);
     *return_code = code;
 
     return 1;
@@ -1372,7 +1372,7 @@ int boxBox2(
       for(int j = 0; j < cnum; ++j)
       {
         Vector3<Scalar> pointInWorld = points[j] + Ta->translation();
-        contacts.push_back(ContactPoint<Scalar>(normal, pointInWorld, -dep[j]));
+        contacts.emplace_back(normal, pointInWorld, -dep[j]);
       }
     }
     else
@@ -1381,7 +1381,7 @@ int boxBox2(
       for(int j = 0; j < cnum; ++j)
       {
         Vector3<Scalar> pointInWorld = points[j] + Ta->translation() - normal * dep[j];
-        contacts.push_back(ContactPoint<Scalar>(normal, pointInWorld, -dep[j]));
+        contacts.emplace_back(normal, pointInWorld, -dep[j]);
       }
     }
   }
@@ -1407,9 +1407,9 @@ int boxBox2(
     {
       Vector3<Scalar> posInWorld = points[iret[j]] + Ta->translation();
       if(code < 4)
-        contacts.push_back(ContactPoint<Scalar>(normal, posInWorld, -dep[iret[j]]));
+        contacts.emplace_back(normal, posInWorld, -dep[iret[j]]);
       else
-        contacts.push_back(ContactPoint<Scalar>(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
+        contacts.emplace_back(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
     }
     cnum = maxc;
   }

diff --git a/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h
@@ -163,11 +163,11 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::Scalar>& sh
     Scalar theta_ = theta + thetad * (i + 1);
     for(unsigned int j = 0; j < seg; ++j)
     {
-      points.push_back(Vector3<Scalar>(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
+      points.emplace_back(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_));
     }
   }
-  points.push_back(Vector3<Scalar>(0, 0, r));
-  points.push_back(Vector3<Scalar>(0, 0, -r));
+  points.emplace_back(0, 0, r);
+  points.emplace_back(0, 0, -r);
 
   for(unsigned int i = 0; i < ring - 1; ++i)
   {
@@ -178,8 +178,8 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::Scalar>& sh
        b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
        c = (i + 1) * seg + j;
        d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
-       tri_indices.push_back(Triangle(a, c, b));
-       tri_indices.push_back(Triangle(b, c, d));
+       tri_indices.emplace_back(a, c, b);
+       tri_indices.emplace_back(b, c, d);
     }
   }
 
@@ -188,11 +188,11 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::Scalar>& sh
     unsigned int a, b;
     a = j;
     b = (j == seg - 1) ? 0 : (j + 1);
-    tri_indices.push_back(Triangle(ring * seg, a, b));
+    tri_indices.emplace_back(ring * seg, a, b);
 
     a = (ring - 1) * seg + j;
     b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
-    tri_indices.push_back(Triangle(a, ring * seg + 1, b));
+    tri_indices.emplace_back(a, ring * seg + 1, b);
   }
 
   for(unsigned int i = 0; i < points.size(); ++i)
@@ -247,11 +247,11 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::Scalar>&
     Scalar theta_ = theta + thetad * (i + 1);
     for(unsigned int j = 0; j < seg; ++j)
     {
-      points.push_back(Vector3<Scalar>(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_)));
+      points.emplace_back(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_));
     }
   }
-  points.push_back(Vector3<Scalar>(0, 0, c));
-  points.push_back(Vector3<Scalar>(0, 0, -c));
+  points.emplace_back(0, 0, c);
+  points.emplace_back(0, 0, -c);
 
   for(unsigned int i = 0; i < ring - 1; ++i)
   {
@@ -262,8 +262,8 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::Scalar>&
        b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
        c = (i + 1) * seg + j;
        d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
-       tri_indices.push_back(Triangle(a, c, b));
-       tri_indices.push_back(Triangle(b, c, d));
+       tri_indices.emplace_back(a, c, b);
+       tri_indices.emplace_back(b, c, d);
     }
   }
 
@@ -272,11 +272,11 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::Scalar>&
     unsigned int a, b;
     a = j;
     b = (j == seg - 1) ? 0 : (j + 1);
-    tri_indices.push_back(Triangle(ring * seg, a, b));
+    tri_indices.emplace_back(ring * seg, a, b);
 
     a = (ring - 1) * seg + j;
     b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
-    tri_indices.push_back(Triangle(a, ring * seg + 1, b));
+    tri_indices.emplace_back(a, ring * seg + 1, b);
   }
 
   for(unsigned int i = 0; i < points.size(); ++i)
@@ -330,33 +330,27 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::Scalar>&
   Scalar hd = h / h_num;
 
   for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vector3<Scalar>(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
+    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2);
 
   for(unsigned int i = 0; i < h_num - 1; ++i)
   {
     for(unsigned int j = 0; j < tot; ++j)
     {
-      points.push_back(Vector3<Scalar>(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
+      points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd);
     }
   }
 
   for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vector3<Scalar>(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
 
-  points.push_back(Vector3<Scalar>(0, 0, h / 2));
-  points.push_back(Vector3<Scalar>(0, 0, -h / 2));
+  points.emplace_back(0, 0, h / 2);
+  points.emplace_back(0, 0, -h / 2);
 
   for(unsigned int i = 0; i < tot; ++i)
-  {
-    Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
-    tri_indices.push_back(tmp);
-  }
+    tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
 
   for(unsigned int i = 0; i < tot; ++i)
-  {
-    Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
-    tri_indices.push_back(tmp);
-  }
+    tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
 
   for(unsigned int i = 0; i < h_num; ++i)
   {
@@ -369,8 +363,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::Scalar>&
       d = (j == tot - 1) ? tot : (j + 1 + tot);
 
       int start = i * tot;
-      tri_indices.push_back(Triangle(start + b, start + a, start + c));
-      tri_indices.push_back(Triangle(start + b, start + c, start + d));
+      tri_indices.emplace_back(start + b, start + a, start + c);
+      tri_indices.emplace_back(start + b, start + c, start + d);
     }
   }
 
@@ -430,27 +424,21 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::Scalar>& shap
     Scalar rh = r * (0.5 - h_i / h);
     for(unsigned int j = 0; j < tot; ++j)
     {
-      points.push_back(Vector3<Scalar>(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
+      points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i);
     }
   }
 
   for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vector3<Scalar>(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
 
-  points.push_back(Vector3<Scalar>(0, 0, h / 2));
-  points.push_back(Vector3<Scalar>(0, 0, -h / 2));
+  points.emplace_back(0, 0, h / 2);
+  points.emplace_back(0, 0, -h / 2);
 
   for(unsigned int i = 0; i < tot; ++i)
-  {
-    Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
-    tri_indices.push_back(tmp);
-  }
+    tri_indices.emplace_back(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
 
   for(unsigned int i = 0; i < tot; ++i)
-  {
-    Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
-    tri_indices.push_back(tmp);
-  }
+    tri_indices.emplace_back(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
 
   for(unsigned int i = 0; i < h_num - 1; ++i)
   {
@@ -463,8 +451,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::Scalar>& shap
       d = (j == tot - 1) ? tot : (j + 1 + tot);
 
       int start = i * tot;
-      tri_indices.push_back(Triangle(start + b, start + a, start + c));
-      tri_indices.push_back(Triangle(start + b, start + c, start + d));
+      tri_indices.emplace_back(start + b, start + a, start + c);
+      tri_indices.emplace_back(start + b, start + c, start + d);
     }
   }
 

diff --git a/include/fcl/traversal/collision/mesh_continuous_collision_traversal_node.h b/include/fcl/traversal/collision/mesh_continuous_collision_traversal_node.h
@@ -227,7 +227,7 @@ void MeshContinuousCollisionTraversalNode<BV>::leafTesting(int b1, int b2) const
 
   if(!(collision_time > 1)) // collision happens
   {
-    pairs.push_back(BVHContinuousCollisionPair<Scalar>(primitive_id1, primitive_id2, collision_time));
+    pairs.emplace_back(primitive_id1, primitive_id2, collision_time);
     time_of_contact = std::min(time_of_contact, collision_time);
   }
 }

diff --git a/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h
@@ -257,7 +257,7 @@ MeshConservativeAdvancementTraversalNode<BV>::BVTesting(int b1, int b2) const
   Vector3<Scalar> P1, P2;
   Scalar d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
 
-  stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+  stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }
@@ -535,8 +535,7 @@ Scalar MeshConservativeAdvancementTraversalNodeRSS<Scalar>::BVTesting(int b1, in
         this->model1->getBV(b1).bv,
         this->model2->getBV(b2).bv, &P1, &P2);
 
-  this->stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
-  // TODO(JS): use emplace_back()
+  this->stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }
@@ -610,7 +609,7 @@ BVTesting(int b1, int b2) const
         this->model1->getBV(b1).bv,
         this->model2->getBV(b2).bv, &P1, &P2);
 
-  this->stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+  this->stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }

diff --git a/include/fcl/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h
@@ -241,7 +241,7 @@ class MeshShapeConservativeAdvancementTraversalNodeRSS
     Vector3<Scalar> P1, P2;
     Scalar d = distance(this->tf1.linear(), this->tf1.translation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
 
-    this->stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+    this->stack.emplace_back(P1, P2, b1, b2, d);
 
     return d;
   }
@@ -311,7 +311,7 @@ class MeshShapeConservativeAdvancementTraversalNodeOBBRSS :
     Vector3<Scalar> P1, P2;
     Scalar d = distance(this->tf1.linear(), this->tf1.translation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
 
-    this->stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+    this->stack.emplace_back(P1, P2, b1, b2, d);
 
     return d;
   }
@@ -400,7 +400,7 @@ BVTesting(int b1, int b2) const
   Vector3<Scalar> P1, P2;
   Scalar d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1);
 
-  stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+  stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }

diff --git a/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h
@@ -185,7 +185,7 @@ BVTesting(int b1, int b2) const
   Vector3<Scalar> P1, P2;
   Scalar d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2);
 
-  stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+  stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }
@@ -342,7 +342,7 @@ BVTesting(int b1, int b2) const
   Vector3<Scalar> P1, P2;
   Scalar d = distance(this->tf2.linear(), this->tf2.translation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
 
-  this->stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+  this->stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }
@@ -442,7 +442,7 @@ BVTesting(int b1, int b2) const
   Vector3<Scalar> P1, P2;
   Scalar d = distance(this->tf2.linear(), this->tf2.translation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
 
-  this->stack.push_back(ConservativeAdvancementStackData<Scalar>(P1, P2, b1, b2, d));
+  this->stack.emplace_back(P1, P2, b1, b2, d);
 
   return d;
 }