From ba63628ecd0dfbde209ceb2328cd1bd5d5606201 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Fri, 23 Apr 2010 01:34:40 +0000
Subject: [PATCH] Added CQP example from the matlab test, changed the linking
 to avoid warnings

---
 cpp/Makefile.am    |  2 +-
 cpp/testMatrix.cpp | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index f8f47fdd7..2872d0353 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -283,7 +283,7 @@ AM_CXXFLAGS = -I$(boost) -fPIC
 lib_LTLIBRARIES = libgtsam.la
 libgtsam_la_SOURCES = $(sources)
 libgtsam_la_CPPFLAGS = $(AM_CXXFLAGS)
-libgtsam_la_LDFLAGS = -version-info $(version) ../ldl/libldl.a -L../colamd -lcolamd # -lboost_serialization-mt
+libgtsam_la_LDFLAGS = -version-info $(version) -L../colamd -lcolamd -L../ldl -lldl # ../ldl/libldl.a -lboost_serialization-mt
 
 # enable debug if --enable-debug is set in configure
 if DEBUG
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index 05ca90bdd..76bd8505e 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -966,6 +966,39 @@ TEST( matrix, LDL_factorization ) {
 
 }
 
+
+/* ************************************************************************* */
+// Example of a single Constrained QP problem from the matlab testCQP.m file.
+TEST( matrix, CQP_example ) {
+
+	Matrix A = Matrix_(3, 2,
+			-1.0,  -1.0,
+			-2.0,   1.0,
+			 1.0,  -1.0);
+	Matrix At = trans(A),
+		   B = 2.0 * eye(3,3);
+
+	Vector b = Vector_(2, 4.0, -2.0),
+	 	   g = zero(3);
+
+	Matrix G = zeros(5,5);
+	insertSub(G, B, 0, 0);
+	insertSub(G, A, 0, 3);
+	insertSub(G, At, 3, 0);
+
+	Vector rhs = zero(5);
+	subInsert(rhs, -1.0*g, 0);
+	subInsert(rhs, -1.0*b, 3);
+
+	// solve the system with the LDL solver
+	Vector actualFull = solve_ldl(G, rhs);
+	Vector actual = sub(actualFull, 0, 3);
+
+	Vector expected = Vector_(3, 2.0/7.0, 10.0/7.0, -6.0/7.0);
+
+	CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;