 //			mg_tmp2[index + number_of_contacts * 1] = _mg_tmp2.y;
 //			mg_tmp2[index + number_of_contacts * 2] = _mg_tmp2.z;
 
-		lm[index] = _mg_tmp2.x;
-
-		real3 _obj1_tmp = 0.5 * _mg_tmp - _b;
-
-		_mx.x = mx[index+ number_of_contacts * 0];
-		_mx.y = mx[index+ number_of_contacts * 1];
-		_mx.z = mx[index+ number_of_contacts * 2];
-
-		_my.x = my[index+ number_of_contacts * 0];
-		_my.y = my[index+ number_of_contacts * 1];
-		_my.z = my[index+ number_of_contacts * 2];
-
-		obj1_tmp[index + number_of_contacts * 0] = _obj1_tmp.x * _mx.x;
-		obj1_tmp[index + number_of_contacts * 1] = _obj1_tmp.y * _mx.y;
-		obj1_tmp[index + number_of_contacts * 2] = _obj1_tmp.z * _mx.z;
-
-		_ms = _mx - _my;
-
-		ms[index + number_of_contacts * 0] = _ms.x;
-		ms[index + number_of_contacts * 1] = _ms.y;
-		ms[index + number_of_contacts * 2] = _ms.z;
-
-		_mg.x = mg[index+ number_of_contacts * 0];
-		_mg.y = mg[index+ number_of_contacts * 1];
-		_mg.z = mg[index+ number_of_contacts * 2];
-
-		obj3_tmp1[index+ number_of_contacts * 0] = _mg.x * _ms.x;
-		obj3_tmp1[index+ number_of_contacts * 1] = _mg.y * _ms.y;
-		obj3_tmp1[index+ number_of_contacts * 2] = _mg.z * _ms.z;
-
-		obj3_tmp2[index+ number_of_contacts * 0] = _ms.x * _ms.x;
-		obj3_tmp2[index+ number_of_contacts * 1] = _ms.y * _ms.y;
-		obj3_tmp2[index+ number_of_contacts * 2] = _ms.z * _ms.z;
-
-	}
-
-	min_val = Thrust_Min(lm);
-
-	real _obj1 = 0;
-
-	real temp1 = 0;
-	real temp2 = 0;
+			lm[index] = _mg_tmp2.x;
+
+			real3 _obj1_tmp = 0.5 * _mg_tmp - _b;
+
+			_mx.x = mx[index+ number_of_contacts * 0];
+			_mx.y = mx[index+ number_of_contacts * 1];
+			_mx.z = mx[index+ number_of_contacts * 2];
+
+			_my.x = my[index+ number_of_contacts * 0];
+			_my.y = my[index+ number_of_contacts * 1];
+			_my.z = my[index+ number_of_contacts * 2];
+
+			obj1_tmp[index + number_of_contacts * 0] = _obj1_tmp.x * _mx.x;
+			obj1_tmp[index + number_of_contacts * 1] = _obj1_tmp.y * _mx.y;
+			obj1_tmp[index + number_of_contacts * 2] = _obj1_tmp.z * _mx.z;
+
+			_ms = _mx - _my;
+
+			ms[index + number_of_contacts * 0] = _ms.x;
+			ms[index + number_of_contacts * 1] = _ms.y;
+			ms[index + number_of_contacts * 2] = _ms.z;
+
+			_mg.x = mg[index+ number_of_contacts * 0];
+			_mg.y = mg[index+ number_of_contacts * 1];
+			_mg.z = mg[index+ number_of_contacts * 2];
+
+			obj3_tmp1[index+ number_of_contacts * 0] = _mg.x * _ms.x;
+			obj3_tmp1[index+ number_of_contacts * 1] = _mg.y * _ms.y;
+			obj3_tmp1[index+ number_of_contacts * 2] = _mg.z * _ms.z;
+
+			obj3_tmp2[index+ number_of_contacts * 0] = _ms.x * _ms.x;
+			obj3_tmp2[index+ number_of_contacts * 1] = _ms.y * _ms.y;
+			obj3_tmp2[index+ number_of_contacts * 2] = _ms.z * _ms.z;
+
+		}
+
+		min_val = Thrust_Min(lm);
+
+		real _obj1 = 0;
+
+		real temp1 = 0;
+		real temp2 = 0;
 
 #pragma omp parallel for reduction(+:_obj1,temp1,temp2)
 		for (int i = 0; i < size; i++) {


		shurA(mx);

#pragma omp parallel for
		for (uint index = 0; index < number_of_contacts; index++) {
			real3 temp = R3(0);
			int2 id_ = ids[index];

	real theta_k=1.0;
	real theta_k1=theta_k;
	real beta_k1=0.0;
//custom_vector<real> ml = x;
		Project(x);
//custom_vector<real> ml_candidate = ml;

		custom_vector<real> mg(x.size());
		ShurProduct(x,mg);
