Exemplo n.º 1
0
bool CShape::GetExtents(double* extents, const double* orig, const double* xdir, const double* ydir, const double* zdir)
{
	gp_Pnt p_orig(0, 0, 0);
	if(orig)p_orig = gp_Pnt(orig[0], orig[1], orig[2]);
	gp_Vec v_x(1, 0, 0);
	if(xdir)v_x = gp_Vec(xdir[0], xdir[1], xdir[2]);
	gp_Vec v_y(0, 1, 0);
	if(ydir)v_y = gp_Vec(ydir[0], ydir[1], ydir[2]);
	gp_Vec v_z(0, 0, 1);
	if(zdir)v_z = gp_Vec(zdir[0], zdir[1], zdir[2]);

	BRepPrimAPI_MakeBox cuboid_plus_x(gp_Ax2(gp_Pnt(p_orig.XYZ() + 2000000 * v_x.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_y.XYZ()), v_x, v_y), 1000000, 1000000, 1000000);
	BRepPrimAPI_MakeBox cuboid_minus_x(gp_Ax2(gp_Pnt(p_orig.XYZ() + (-2000000) * v_x.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_y.XYZ()), -v_x, v_z), 1000000, 1000000, 1000000);
	BRepPrimAPI_MakeBox cuboid_plus_y(gp_Ax2(gp_Pnt(p_orig.XYZ() + 2000000 * v_y.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_x.XYZ()), v_y, v_z), 1000000, 1000000, 1000000);
	BRepPrimAPI_MakeBox cuboid_minus_y(gp_Ax2(gp_Pnt(p_orig.XYZ() + (-2000000) * v_y.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_x.XYZ()), -v_y, v_x), 1000000, 1000000, 1000000);
	BRepPrimAPI_MakeBox cuboid_plus_z(gp_Ax2(gp_Pnt(p_orig.XYZ() + 2000000 * v_z.XYZ() + (-1000000) * v_x.XYZ() + (-1000000) * v_y.XYZ()), v_z, v_x), 1000000, 1000000, 1000000);
	BRepPrimAPI_MakeBox cuboid_minus_z(gp_Ax2(gp_Pnt(p_orig.XYZ() + (-2000000) * v_z.XYZ() + (-1000000) * v_x.XYZ() + (-1000000) * v_y.XYZ()), -v_z, v_y), 1000000, 1000000, 1000000);

	gp_Vec v_orig(p_orig.XYZ());

	TopoDS_Solid shape[6] =
	{
		cuboid_minus_x,
		cuboid_minus_y,
		cuboid_minus_z,
		cuboid_plus_x,
		cuboid_plus_y,
		cuboid_plus_z
	};

	gp_Vec vector[6] =
	{
		v_x,
		v_y,
		v_z,
		v_x,
		v_y,
		v_z
	};

	for(int i = 0; i<6; i++){
		BRepExtrema_DistShapeShape extrema(m_shape, shape[i]);
		extrema.Perform();
		gp_Pnt p = extrema.PointOnShape1(1);
		gp_Vec v(p.XYZ());
		double dp = v * vector[i];
		double dp_o = v_orig * vector[i];
		extents[i] = dp - dp_o;
	}

	return true;
}
Exemplo n.º 2
0
int main(int argc,char** argv)  
{  
	cv::Mat src = cv::imread("input.jpg");
    IplImage* img=cvLoadImage("input.jpg");
    IplImage* image = cvCloneImage(img);
    IplImage* temp=cvCloneImage(image);  
    cvNamedWindow("Draw Line Example", CV_WINDOW_NORMAL);  
    cvSetMouseCallback(  
        "Draw Line Example",  
        my_callback,  
        (void*)image);  

    while(1)  
    {  
        cvCopyImage(image,temp);  
        if(drawing_line) draw_line(temp,pt1,pt2);  
        cvShowImage("Draw Line Example",temp); 
		int c = cv::waitKey(20);
		if ( c == 'q' )
		{
			break;
		
		}
        switch(c){
		case 'x':
			point_vector.clear();
			pt1 = cvPoint(0,0);
			pt2 = cvPoint(0,0);
			cvCopyImage(img,image);
			break;
		case 'y':
			x_point_vector = point_vector;
			point_vector.clear();
			pt1 = cvPoint(0,0);
			pt2 = cvPoint(0,0);
			cvCopyImage(img,image); 
			break;
		case 'z':
			y_point_vector = point_vector;
			point_vector.clear();
			pt1 = cvPoint(0,0);
			pt2 = cvPoint(0,0);
			cvCopyImage(img,image); 
			break;
		case 's':
			z_point_vector = point_vector;
			point_vector.clear();
			pt1 = cvPoint(0,0);
			pt2 = cvPoint(0,0);
			cvCopyImage(img,image);
			break;
		case 'h':
			point_vector.clear();
			drawing_point = true;
			cvCopyImage(img,image); 
			break;
		default:
			break;
		}

	}

	float w = (img->width + img->height)/4.0;

	// calculate vanishing points
	for (std::vector<int>::size_type i=0;i!= x_point_vector.size();++i){
		x_point_vector[i].x -= img->width/2;
		x_point_vector[i].y -= img->height/2;
	}

	for (std::vector<int>::size_type i=0;i!= y_point_vector.size();++i){
		y_point_vector[i].x -= img->width/2;
		y_point_vector[i].y -= img->height/2;
	}

	for (std::vector<int>::size_type i=0;i!= z_point_vector.size();++i){
		z_point_vector[i].x -= img->width/2;
		z_point_vector[i].y -= img->height/2;
	}

	CvPoint3D32f x_vanish = calc_vanishing_point(x_point_vector,w);
	CvPoint3D32f y_vanish = calc_vanishing_point(y_point_vector,w);
	CvPoint3D32f z_vanish = calc_vanishing_point(z_point_vector,w);


	point_vector.clear();
	point_vector.push_back(cvPoint(2316, 3681));
	point_vector.push_back(cvPoint(1711, 3372));
	point_vector.push_back(cvPoint(2226, 3300));
	point_vector.push_back(cvPoint(2942, 3570));
	point_vector.push_back(cvPoint(2326, 3092));


	// calculate homography of x-y plane and scale in xyz axis
	if (point_vector.size() < 5)
	{
		std::cout << "Less than 5 points are clicked!\n";
		return -1;
	}

	double scale_x = 300, scale_y = 600, scale_z = 300;
	std::vector <cv::Point2d> target_point_vector;
	target_point_vector.push_back(cv::Point2d(0,0));
	target_point_vector.push_back(cv::Point2d(0, scale_y));
	target_point_vector.push_back(cv::Point2d(scale_x, scale_y));
	target_point_vector.push_back(cv::Point2d(scale_x, 0));

	std::vector <cv::Point2d> src_point_vector;
	src_point_vector.push_back(cv::Point2d(point_vector[0].x, point_vector[0].y));
	src_point_vector.push_back(cv::Point2d(point_vector[1].x, point_vector[1].y));
	src_point_vector.push_back(cv::Point2d(point_vector[2].x, point_vector[2].y));
	src_point_vector.push_back(cv::Point2d(point_vector[3].x, point_vector[3].y));
	cv::Vec3d origin(point_vector[0].x, point_vector[0].y, 1);

	cv::Mat Hxy = cv::findHomography(src_point_vector, target_point_vector);
	cv::Vec3d z_scale(point_vector[4].x, point_vector[4].y, 1);

	point_vector.clear();
	//cv::Vec3d v_x(x_vanish.x + image->width / 2, x_vanish.y + image->height / 2, 1);
	//cv::Vec3d v_y(y_vanish.x + image->width / 2, y_vanish.y + image->height / 2, 1);
	//cv::Vec3d v_z(z_vanish.x + image->width / 2, z_vanish.y + image->height / 2, 1);
	cv::Vec3d v_x(9563.7, 2403.48, 1);
	cv::Vec3d v_y(-86.75, 2409.9, 1);
	cv::Vec3d v_z(5534.12, 530677, 1);

	std::cout << "origin: " << origin << "\n";
	std::cout << "vanishing x: " << v_x << "\n";
	std::cout << "vanishing y: " << v_y << "\n";
	std::cout << "vanishing z: " << v_z << "\n";
	std::cout << "homography: " << Hxy << "\n";

	// calculate scale of z axis
	double z_temp = cal3dZ(v_x, v_y, v_z, z_scale, origin, origin, 1);
	scale_z = scale_z / z_temp;
	
	double x_coord, y_coord, z_coord;
	cv::Vec3d pt0, pt1, pt2, pt3;
	cv::Vec3d top0, bottom0, top1, bottom1, top2, bottom2, top3, bottom3;
	std::vector<cv::Vec3d> src_text_vector;
	std::vector<cv::Vec3d> target_text_vector;
	int cnt = 111;
	
	while (1)
	{
		cvCopyImage(image, temp);
		int c = cv::waitKey(20);
		if (c == 27)
		{
			break;
		}
		if (c == 'p')
		{
			if (point_vector.size() < 4) { break; }
			top0 = cv::Vec3d(point_vector[0].x, point_vector[0].y, 1);
			bottom0 = cv::Vec3d(point_vector[1].x, point_vector[1].y, 1);
			pt0 = cal3dXYZ(v_x, v_y, v_z, top0, bottom0, origin, scale_z, Hxy);

			pt1 = cal3dXYZ(v_x, v_y, v_z, bottom0, bottom0, origin, scale_z, Hxy);

			top1 = cv::Vec3d(point_vector[2].x, point_vector[2].y, 1);
			bottom1 = cv::Vec3d(point_vector[3].x, point_vector[3].y, 1);
			pt3 = cal3dXYZ(v_x, v_y, v_z, top1, bottom1, origin, scale_z, Hxy);

			pt2 = cal3dXYZ(v_x, v_y, v_z, bottom1, bottom1, origin, scale_z, Hxy);
			std::cout << "3D coordinate 0: " << pt0 << "\n";
			std::cout << "3D coordinate 1: " << pt1 << "\n";
			std::cout << "3D coordinate 2: " << pt2 << "\n";
			std::cout << "3D coordinate 3: " << pt3 << "\n";

			double text_w = distance3d(pt0, pt3);
			double text_h = distance3d(pt0, pt1);

			std::cout << "Save texture...\n";
			src_text_vector.clear();
			target_text_vector.clear();
			src_text_vector.push_back(top0);
			src_text_vector.push_back(bottom0);
			src_text_vector.push_back(bottom1);
			src_text_vector.push_back(top1);
			target_text_vector.push_back(cv::Vec3d(0, 0, 1));
			target_text_vector.push_back(cv::Vec3d(0, text_h, 1));
			target_text_vector.push_back(cv::Vec3d(text_w, text_h, 1));
			target_text_vector.push_back(cv::Vec3d(text_w, 0, 1));

			cv::Mat text = getHomo(target_text_vector, src_text_vector);
			std::cout << "texture: " << text_w << ", " << text_h << "\n";
			cv::Mat texture(text_h, text_w, src.type(), Scalar(0, 0, 0));
			getTexture(texture, src, text);
			std::string text_file = std::to_string(cnt) + ".jpg";
			cv::imwrite(text_file, texture);
			cnt++;

			std::string textures[] = { text_file };
			std::cout << textures[0] << std::endl;
			std::vector<cv::Vec3d> text_points;
			text_points.push_back(pt1);
			text_points.push_back(pt0);
			text_points.push_back(pt3);
			text_points.push_back(pt2);
			std::ofstream fout("livingroom.wrl", std::ofstream::out | std::ofstream::app);
			create_crml_file(text_points, fout, textures);
			fout.close();

			point_vector.clear();
		}


		else if (c == 'v')
		{

			if (point_vector.size() < 8) { break; }
			top0 = cv::Vec3d(point_vector[0].x, point_vector[0].y, 1);
			bottom0 = cv::Vec3d(point_vector[1].x, point_vector[1].y, 1);
			pt0 = cal3dXYZ(v_x, v_y, v_z, top0, bottom0, origin, scale_z, Hxy);

			top1 = cv::Vec3d(point_vector[2].x, point_vector[2].y, 1);
			bottom1 = cv::Vec3d(point_vector[3].x, point_vector[3].y, 1);
			pt1 = cal3dXYZ(v_x, v_y, v_z, top1, bottom1, origin, scale_z, Hxy);

			top2 = cv::Vec3d(point_vector[4].x, point_vector[4].y, 1);
			bottom2 = cv::Vec3d(point_vector[5].x, point_vector[5].y, 1);
			pt2 = cal3dXYZ(v_x, v_y, v_z, top2, bottom2, origin, scale_z, Hxy);

			top3 = cv::Vec3d(point_vector[6].x, point_vector[6].y, 1);
			bottom3 = cv::Vec3d(point_vector[7].x, point_vector[7].y, 1);
			pt3 = cal3dXYZ(v_x, v_y, v_z, top3, bottom3, origin, scale_z, Hxy);


			std::cout << "3D coordinate 0: " << pt0 << "\n";
			std::cout << "3D coordinate 1: " << pt1 << "\n";
			std::cout << "3D coordinate 2: " << pt2 << "\n";
			std::cout << "3D coordinate 3: " << pt3 << "\n";

			double text_w = distance3d(pt0, pt3);
			double text_h = distance3d(pt0, pt1);

			std::cout << "Save texture...\n";
			src_text_vector.clear();
			target_text_vector.clear();
			src_text_vector.push_back(top0);
			src_text_vector.push_back(top1);
			src_text_vector.push_back(top2);
			src_text_vector.push_back(top3);
			target_text_vector.push_back(cv::Vec3d(0, 0, 1));
			target_text_vector.push_back(cv::Vec3d(0, text_h, 1));
			target_text_vector.push_back(cv::Vec3d(text_w, text_h, 1));
			target_text_vector.push_back(cv::Vec3d(text_w, 0, 1));

			cv::Mat text = getHomo(target_text_vector, src_text_vector);
			std::cout << "texture: " << text_w << ", " << text_h << "\n";
			cv::Mat texture(text_h, text_w, src.type(), Scalar(0, 0, 0));
			getTexture(texture, src, text);
			std::string text_file = std::to_string(cnt) + ".jpg";
			cv::imwrite(text_file, texture);
			cnt++;

			std::string textures[] = { text_file };
			std::cout << textures[0] << std::endl;
			std::vector<cv::Vec3d> text_points;
			text_points.push_back(pt1);
			text_points.push_back(pt0);
			text_points.push_back(pt3);
			text_points.push_back(pt2);
			std::ofstream fout("livingroom.wrl", std::ofstream::out | std::ofstream::app);
			create_crml_file(text_points, fout, textures);
			fout.close();

			point_vector.clear();
		}
	}

	cvReleaseImage(&img);
    cvReleaseImage(&image);  
    cvReleaseImage(&temp);  
    cvDestroyAllWindows();  

	return 0;
}  
Exemplo n.º 3
0
//----------- Begin of function LinAlg::quadratic_prog ------//
//! Interior Point Quadratic Programming
//! c, Q, A, b, xNames (no global or member variable access)
//!
//! try, by BM:
//!		c = { 0,0 }
//!		Q = { {2,0}, {0,5} }
//!		A = { {5,6} }
//!		b = { 10 }
//!		xNames={x1,x2}
//!
bool LinearAlgebra::quadratic_prog(const Vector &v_c, const Matrix &m_Q, const Matrix &m_A, const Vector &v_b, Vector &v_xNames, int loopCountMultiplier) {
    // init local variables
    int maxIteration = 20;
    const REAL SIGMA    = 1/15.0;                   // 1/10.0;
    const REAL R      = 9.0/10;
    int iter    = 0;

    int n = v_c.Storage();
    int m = v_b.Storage();

    maxIteration *= loopCountMultiplier;

#ifdef DEBUG_VC
    DEBUG_LOG("----------- quadratic_prog begin -----------");
    // print_input(c,Q,A,b,xNames)
    if ( n==10 && m==12 ) {
	char s[500];

	DEBUG_LOG("c = ");
	sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
		v_c(1),v_c(2),v_c(3),
		v_c(4),v_c(5),v_c(6),
		v_c(7),v_c(8),v_c(9), v_c(10));
	DEBUG_LOG(s);
	DEBUG_LOG("Q = ");
	for (int i=1; i<=n; i++) {
	    sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
		    m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
		);
	    DEBUG_LOG(s);
	}
	DEBUG_LOG("A = ");
	for (i=1; i<=m; i++) {
	    sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
		    m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
		);
	    DEBUG_LOG(s);
	}
	DEBUG_LOG("b = ");
	sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f,",
		v_b(1),v_b(2),v_b(3),
		v_b(4),v_b(5),v_b(6),
		v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12)
	    );
	DEBUG_LOG(s);
	/*sprintf(s, "%f, %f, %f, %f, %f, %f, %f, %f, %f,",
	  v_b(10),v_b(11),v_b(12),
	  v_b(13),v_b(14),v_b(15),
	  v_b(16),v_b(17),v_b(18)
	  );
	  DEBUG_LOG(s);
	  sprintf(s, "%f, %f};",
	  v_b(19),v_b(20));
	  DEBUG_LOG(s);*/

    }
    else if ( n==9 && m==20 ) {                     // stage 1
	char s[500];

	DEBUG_LOG("c = ");
	sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
		v_c(1),v_c(2),v_c(3),
		v_c(4),v_c(5),v_c(6),
		v_c(7),v_c(8),v_c(9));
	DEBUG_LOG(s);
	DEBUG_LOG("Q = ");
	for (int i=1; i<=n; i++) {
	    sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, },",
		    m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9)
		);
	    DEBUG_LOG(s);
	}
	DEBUG_LOG("A = ");
	for (i=1; i<=m; i++) {
	    sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f },",
		    m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9)
		);
	    DEBUG_LOG(s);
	}
	DEBUG_LOG("b = ");
	sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, }, ",
		v_b(1),v_b(2),v_b(3),
		v_b(4),v_b(5),v_b(6),
		v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
		v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20)
	    );
	DEBUG_LOG(s);
    }
    else if ( n==10 && m==22 ) {                    // stage 2
	char s[500];

	DEBUG_LOG("c = ");
	sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
		v_c(1),v_c(2),v_c(3),
		v_c(4),v_c(5),v_c(6),
		v_c(7),v_c(8),v_c(9),v_c(10));
	DEBUG_LOG(s);
	DEBUG_LOG("Q = ");
	for (int i=1; i<=n; i++) {
	    sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, },",
		    m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
		);
	    DEBUG_LOG(s);
	}
	DEBUG_LOG("A = ");
	for (i=1; i<=m; i++) {
	    sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f },",
		    m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
		);
	    DEBUG_LOG(s);
	}
	DEBUG_LOG("b = ");
	sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, }, ",
		v_b(1),v_b(2),v_b(3),
		v_b(4),v_b(5),v_b(6),
		v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
		v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20), v_b(21), v_b(22)
	    );
	DEBUG_LOG(s);
    }
#endif

    Vector  v_eN(n);    v_eN = 1;
    Vector  v_eM(m);    v_eM = 1;

    Vector  v_x(n);   v_x = 1;
    Vector  v_z(n);   v_z = 1;
    Vector  v_y(m);   v_y = 1;
    Vector  v_w(m);   v_w = 1;

    // check  m_A is nxm, m_Q is nxn
    err_when(m_A.Nrows() != m || m_A.Ncols() != n );
    err_when(m_Q.Nrows() != n || m_Q.Ncols() != n );

    // init for while loop not_converged() checking
    //
    Vector v_Road = v_b - m_A*(v_x) + v_w;
    Vector v_Sigma = v_c - m_A.t() * v_y - v_z + m_Q*v_x;
    REAL  gamma   = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
    REAL  mute    = SIGMA*gamma / (n+m);

    DiagonalMatrix X(n),  Xinv(n);
    DiagonalMatrix Y(m),  Yinv(m);
    DiagonalMatrix Z(n);
    DiagonalMatrix W(m);
    Matrix T1(n,n), T2(n,m), T3(m,n), T4(m,m);

    Vector KKFvec(n+m);                             //, v_tmp1(n), v_tmp2(m);

    Vector m_temp(m+n);
    Vector v_dx(n), v_dy(m), v_dz(n), v_dw(m);
    REAL  theeta;

    X=0; Xinv=0; Z=0;
    Y=0; Yinv=0; W=0; {

	//	Matrix _t1(n,n), _t2(n,n);		_t1=1;	_t2=2;
	//	_t1 = SP(_t1,_t2);
    }

    Matrix KKFmat(m+n,m+n);
    Matrix KKFmatInverse(m+n,m+n);

    REAL min=1;

    while ( quadratic_prog_not_converged(v_x,v_y,v_Road,v_Sigma,gamma) && iter <= maxIteration && min > 0.0000000001 ) {
	iter++;
	v_Road  = v_b - m_A*v_x + v_w;
	v_Sigma = v_c - m_A.t()*v_y - v_z + m_Q*v_x;
	gamma   = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
	mute    = SIGMA*gamma / (n+m);

	X.set_diagonal(v_x);
	Y.set_diagonal(v_y);
	Z.set_diagonal(v_z);
	W.set_diagonal(v_w);

	Xinv.set_diagonal(ElmDivide(v_eN, v_x));
	Yinv.set_diagonal(ElmDivide(v_eM, v_y));

	T1 = -(Xinv * Z + m_Q);
	T2 = m_A.t();
	T3 = m_A;
	T4 = Yinv*W;
	//PRT_MAT << "T1: " << T1 << endl;
	KKFmat = (T1|T2) & (T3|T4);

	KKFvec = (v_c - T2*v_y - mute*Xinv*v_eN + m_Q*v_x)
	    & (v_b - m_A*v_x + mute*Yinv*v_eM);

	Try {
	    /*{
	      bool _f;
	      DiagonalMatrix _dmat(n+m);		_dmat=1;
	      Matrix _inv(n+m,n+m);

	      SVD(KKFmat, _dmat, _inv);

	      _dmat=1;
	      if ( KKFmat*_inv == _dmat )
	      _f = true;
	      else
	      _f = false;
	      }*/

	    min = fabs(KKFmat(1,1));

	    for (int i=2; i<=m+n; i++) {
		if ( min > fabs(KKFmat(i,i)) )            // check diagonal element
		    min = fabs(KKFmat(i,i));
	    }

	    KKFmatInverse = KKFmat.i();

	    /*
	      if ( KKFmat.LogDeterminant().Value() )
	      KKFmatInverse = KKFmat.i();
	      else
	      break;
	    */

	    m_temp =  KKFmatInverse * KKFvec;

	    v_dx  = m_temp.Rows(1,n);
	    v_dy  = m_temp.Rows(n+1,n+m);

	    v_dz  = Xinv*(mute*v_eN - X*Z*v_eN - Z*v_dx);
	    v_dw  = Yinv*(mute*v_eM - Y*W*v_eM - W*v_dy);
	    //PRT_MAT << "v_dx,z,y,w: " << (v_dx|v_dz) <<", "<< (v_dy|v_dw) <<endl;
	    //PRT_MAT << "v_x: " << v_x << endl;
	    //PRT_MAT << "ElmDivide(v_x,v_dx): " << ElmDivide(v_x,v_dx) << endl;

	    theeta =
		(R/((
		    ElmDivide(-v_dx,v_x)
		    & ElmDivide(-v_dw,v_w)
		    & ElmDivide(-v_dy,v_y)
		    & ElmDivide(-v_dz,v_z)
		    )).MaximumValue()
		    );
	    if(theeta>1)                                // theeta = min(theeta,1)
		theeta = 1;

	    v_x += theeta * v_dx;
	    v_y += theeta * v_dy;
	    v_w += theeta * v_dw;
	    v_z += theeta * v_dz;

#ifdef DEBUG_CONSOLE
	    cout << "#iter "<< iter << ": " << v_Road.Sum() << ", " << v_Sigma.Sum() << ", " << gamma << endl;
	    cout << "Ro:" << v_Road << endl;
	    PRT_MAT15 << "v_x:" << v_x << endl;
	    PRT_MAT15 << "v_y:" << v_y << endl;
	    PRT_MAT15 << "v_w:" << v_w << endl;
	    PRT_MAT15 << "v_z:" << v_z << endl;
#elif defined(DEBUG_VC)

	    char s[200];
	    sprintf(s, "#iter %d: %f, %f, %f",
		    iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
	    DEBUG_LOG(s);
	    if ( n == 9 && false ) {
		DEBUG_LOG("x = ");
		sprintf(s, "   %f, %f, %f, %f, %f, %f, %f, %f, %f",
			v_x(1),v_x(2),v_x(3),
			v_x(4),v_x(5),v_x(6),
			v_x(7),v_x(8),v_x(9));
		DEBUG_LOG(s);
	    }
#endif
	}
	CatchAll {
#ifdef DEBUG_CONSOLE
	    cout << Exception::what();
#endif
#ifdef DEBUG_VC
	    DEBUG_LOG("olinalg: failure in quad_prog");
	    DEBUG_LOG((char *)Exception::what());
#endif
	    return false;
	}
    }                                               // while

    v_xNames = v_x;

    if ( min < 0.00001 ) {                          // 1214 || KKFmat.LogDeterminant().Value() == 0 )
	//char s[200];

	//sprintf(s, "#iter %d: %f, %f, %f",
	//	iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
	//DEBUG_LOG(s);
	DEBUG_LOG("--- quad_prog early exit for min < 0.00001 ---");
    }
    else
	DEBUG_LOG("----------- quad_prog normal exit -----------");

#ifdef DEBUG_CONSOLE
    cout << "#iter: " << iter;
#endif

    return true;
}