From 3989178aba4feb55f850aebd1c0f99e2d2f8fd0f Mon Sep 17 00:00:00 2001
From: Alex Hagiopol <alexhagiopol@gmail.com>
Date: Fri, 11 Mar 2016 20:13:51 -0500
Subject: [PATCH] Deprecated subInsert() in Vector.h

---
 gtsam/base/Vector.cpp                                  | 5 -----
 gtsam/base/Vector.h                                    | 9 +--------
 gtsam/base/tests/testVector.cpp                        | 2 +-
 gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp | 3 ++-
 4 files changed, 4 insertions(+), 15 deletions(-)

diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index d3c39eeda..3365525c4 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -181,11 +181,6 @@ ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
   return v.segment(i1,i2-i1);
 }
 
-/* ************************************************************************* */
-void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
-  fullVector.segment(i, subVector.size()) = subVector;
-}
-
 /* ************************************************************************* */
 Vector ediv_(const Vector &a, const Vector &b) {
   size_t n = a.size();
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 4c2f5e3fa..3b0634702 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -204,14 +204,6 @@ GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, doubl
  */
 GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
 
-/**
- * Inserts a subvector into a vector IN PLACE
- * @param fullVector is the vector to be changed
- * @param subVector is the vector to insert
- * @param i is the index where the subvector should be inserted
- */
-GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
-
 /**
  * elementwise division, but 0/0 = 0, not inf
  * @param a first vector
@@ -304,6 +296,7 @@ GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.siz
 GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();}
 GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();}
 GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
+GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
 GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();}
 #endif
 
diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp
index 718ce9c80..7e421573b 100644
--- a/gtsam/base/tests/testVector.cpp
+++ b/gtsam/base/tests/testVector.cpp
@@ -142,7 +142,7 @@ TEST(Vector, subInsert )
        small = ones(3);
 
   size_t i = 2;
-  subInsert(big, small, i);
+  big.segment(i,small.size()) = small;
 
   Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished();
 
diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
index 8b224f510..a3c622d9b 100644
--- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
+++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
@@ -90,7 +90,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
 TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
   Vector Fu = computeFu(gamma2, u2);
   Vector fGravity_g1 = zero(6);
-  subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame
+  Vector subVector = g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81));
+  fGravity_g1.segment(3,subVector.size()) = subVector;
   Vector Fb = Fu+fGravity_g1;
 
   Vector dV = newtonEuler(V1_g1, Fb, Inertia);