예제 #1
0
void Application::OnInit()
{
  uint Width = 0, Height = 0;
  bool HasSize = false;
  bool IsFullScreen = false;
  bool DebugObject = false;
  bool DebugInfo = false;
  bool ShowFPS = false;

  
  nuiRenderer Renderer = eOpenGL;
//  nuiRenderer Renderer = eSoftware;
//  nuiRenderer Renderer = eDirect3D;

  // Accept NGL default options
  ParseDefaultArgs();

  GetLog().UseConsole(true);
  GetLog().SetLevel(_T("fps"), 100);
  GetLog().SetLevel(_T("all"), 100);

  // Manual
  if ( (GetArgCount() == 1) &&
       ((!GetArg(0).Compare(_T("-h"))) || (!GetArg(0).Compare(_T("--help")))) )
  {
    NGL_OUT(_T("no params\n"));
    Quit (0);
    return;
  }
  
  // Parse args
  int i = 0;
  while (i < GetArgCount())
  {
    nglString arg = GetArg(i);
    if ((!arg.Compare(_T("--size")) || !arg.Compare(_T("-s"))) && ((i+1) < GetArgCount()))
    {
      int w, h;

      std::string str(GetArg(i+1).GetStdString());
      sscanf(str.c_str(), "%dx%d", &w, &h);
      if (w > 0) Width  = w;
      if (h > 0) Height = h;
      HasSize = true;
      i++;
    }
    else if (!arg.Compare(_T("--showfps")) || !arg.Compare(_T("-fps"))) ShowFPS = true;
    else if (!arg.Compare(_T("--fullscreen")) || !arg.Compare(_T("-f"))) IsFullScreen = true;
    else if (!arg.Compare(_T("--debugobject")) || !arg.Compare(_T("-d"))) DebugObject = true;
    else if (!arg.Compare(_T("--debuginfo")) || !arg.Compare(_T("-i"))) DebugInfo = true;
    else if (!arg.Compare(_T("--renderer")) || !arg.Compare(_T("-r"))) 
    {
      arg = GetArg(i+1);
      if (!arg.Compare(_T("opengl"))) Renderer = eOpenGL;
      else if (!arg.Compare(_T("direct3d"))) Renderer = eDirect3D;
      else if (!arg.Compare(_T("software"))) Renderer = eSoftware;
      i++;
    }
    i++;
  }
  
  nuiMainWindow::SetRenderer(Renderer);

  if (!HasSize)
  {
    if (IsFullScreen)
    {
      nglVideoMode current_mode;

      Width = current_mode.GetWidth();
      Height = current_mode.GetHeight();
    }
    else
    {
#ifdef NUI_IPHONE
      Width = 320;
      Height = 480;
#else
      Width = 800;
      Height = 600;
#endif
    }
  }


  /* Create the nglWindow (and thus a GL context, don't even try to
   *   instantiate the gui (or nuiFont) before the nuiWin !)
   */
  nuiContextInfo ContextInfo(nuiContextInfo::StandardContext3D);
  nglWindowInfo Info;

  Info.Flags = IsFullScreen ? nglWindow::FullScreen : 0;
  Info.Width = Width;
  Info.Height = Height;
  Info.Pos = nglWindowInfo::ePosCenter;
  Info.Title = APPLICATION_TITLE;
  Info.XPos = 0;
  Info.YPos = 0;
        
  mpMainWindow = new MainWindow(ContextInfo,Info, ShowFPS);
  if ((!mpMainWindow) || (mpMainWindow->GetError()))
  {
    if (mpMainWindow) 
      NGL_OUT(_T("Error: cannot create window (%s)\n"), mpMainWindow->GetErrorStr());
    Quit (1);
    return;
  }
  mpMainWindow->Acquire();
  mpMainWindow->Acquire();
  mpMainWindow->DBG_SetMouseOverInfo(DebugInfo);  mpMainWindow->DBG_SetMouseOverObject(DebugObject);
#ifdef NUI_IPHONE
  mpMainWindow->SetState(nglWindow::eMaximize);
#else
  mpMainWindow->SetState(nglWindow::eShow);
#endif

}
예제 #2
0
void DBConverter::commitDBSet(const DBSet& set)
{
	// Adding statistics for move
	switch( set.MoveNumber ) {
		case 0: 
			data->AddPassoutMove1(set.Utilized, set.Hand, set.Move1);
			break;
		case 1:
			data->AddPassoutMove2(set.Utilized, set.Hand, set.Move1, set.Move2);
			break;
		case 2:
			data->AddPassoutMove3(set.Utilized, set.Hand, set.Move1, set.Move2, set.Move3);
			break;
		default:
			PrefAssert( false );
	}
	// Adding first move to set
	if( set.MoveNumber == 0 ) {
		vector<int> firstMove(1);
		firstMove[0] = -1;
		for( SuitForwardIterator itSuit; itSuit.HasNext(); itSuit.Next() ) { 
			RanksSet ranks = GetCardsOfSuit(set.Hand, itSuit.GetObject());
			if( ranks == EmptyRanksSet ) {
				continue;
			}
			firstMove.push_back( GetRanksSetIndex(ranks, GetCardsOfSuit(set.Utilized, itSuit.GetObject())) );
			if( itSuit.GetObject() == GetCardSuit(set.Move1) ) {
				firstMove[0] = firstMove.back();
			}
		}
		assert( firstMove[0] >= 0 );
		passoutFirstMoveData->AddSet(firstMove);
	}
	// Determining move suit and player suit...
	Suit moveSuit = GetCardSuit(set.Move1);
	Suit playerSuit = moveSuit;
	Card move = set.Move1;
	if( set.MoveNumber == 1 ) {
		playerSuit = GetCardSuit(set.Move2);
		move = set.Move2;
	} else if( set.MoveNumber == 2 ) {
		playerSuit = GetCardSuit(set.Move3);
		move = set.Move3;
	}
	if( moveSuit != playerSuit ) {
		GetLog() << "Move suit != player suit" << endl;
		// Adding learning set data about passout drop if required
		vector<int> sample(1);
		sample[0] = -1;
		for( SuitForwardIterator itSuit; itSuit.HasNext(); itSuit.Next() ) { 
			RanksSet ranksSet = GetCardsOfSuit(set.Hand, itSuit.GetObject());
			if( ranksSet == EmptyRanksSet ) {
				continue;
			}
			sample.push_back( GetRanksSetIndex(ranksSet, GetCardsOfSuit(set.Utilized, itSuit.GetObject())) ); 
			if( itSuit.GetObject() == playerSuit ) {
				sample[0] = sample.back();
			}
		}
		PrefAssert( sample[0] >= 0 );
		GetLog() << "Adding passout learning set: ";
		// dumping learning data
		for( int i = 0; i < sample.size(); i++ ) {
			GetLog() << sample[i] << " ";
		}
		GetLog() << endl;
		passoutDropData->AddSet(sample);
	}
}
예제 #3
0
// Define my frame constructor
MyFrame::MyFrame(wxFrame *frame, const wxString& title)
        : wxFrame(frame, wxID_ANY, title, wxDefaultPosition, wxSize(400, 300))
{
    // Give it an icon
    SetIcon(wxICON(sample));

    // Make a menubar
    wxMenu *file_menu = new wxMenu;

    file_menu->Append(wxID_EXIT, _T("&Quit\tCtrl-Q"));

    wxMenuBar *menu_bar = new wxMenuBar;

    menu_bar->Append(file_menu, _T("&File"));

    // Associate the menu bar with the frame
    SetMenuBar(menu_bar);

    // set a dialog background
    SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));

    // add the controls to the frame
    wxString strs4[] =
    {
        IPC_SERVICE, _T("...")
    };
    wxString strs5[] =
    {
        IPC_HOST, _T("...")
    };
    wxString strs6[] =
    {
        IPC_TOPIC, _T("...")
    };

    wxBoxSizer *item0 = new wxBoxSizer( wxVERTICAL );

    wxBoxSizer *item1 = new wxBoxSizer( wxHORIZONTAL );

    wxGridSizer *item2 = new wxGridSizer( 4, 0, 0 );

    wxButton *item3 = new wxButton( this, ID_START, wxT("Connect to server"), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item3, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    wxChoice *item5 = new wxChoice( this, ID_HOSTNAME, wxDefaultPosition, wxSize(100,-1), 2, strs5, 0 );
    item2->Add( item5, 0, wxALIGN_CENTER|wxALL, 5 );

    wxChoice *item4 = new wxChoice( this, ID_SERVERNAME, wxDefaultPosition, wxSize(100,-1), 2, strs4, 0 );
    item2->Add( item4, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    wxChoice *item6 = new wxChoice( this, ID_TOPIC, wxDefaultPosition, wxSize(100,-1), 2, strs6, 0 );
    item2->Add( item6, 0, wxALIGN_CENTER|wxALL, 5 );

    wxButton *item7 = new wxButton( this, ID_DISCONNECT, wxT("Disconnect "), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item7, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    wxButton *item8 = new wxButton( this, ID_STARTADVISE, wxT("StartAdvise"), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item8, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    wxButton *item9 = new wxButton( this, ID_STOPADVISE, wxT("StopAdvise"), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item9, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    wxButton *item10 = new wxButton( this, ID_EXECUTE, wxT("Execute"), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item10, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    wxButton *item11 = new wxButton( this, ID_POKE, wxT("Poke"), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item11, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    wxButton *item12 = new wxButton( this, ID_REQUEST, wxT("Request"), wxDefaultPosition, wxDefaultSize, 0 );
    item2->Add( item12, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    item2->Add( 20, 20, 0, wxALIGN_CENTER|wxALL, 5 );

    item1->Add( item2, 1, wxALIGN_CENTER|wxALL, 5 );

    item0->Add( item1, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    wxStaticBox *item14 = new wxStaticBox( this, -1, wxT("Client log") );
    wxStaticBoxSizer *item13 = new wxStaticBoxSizer( item14, wxVERTICAL );

    wxTextCtrl *item15 = new wxTextCtrl( this, ID_LOG, wxEmptyString, wxDefaultPosition, wxSize(500,140), wxTE_MULTILINE );
    item13->Add( item15, 1, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    item0->Add( item13, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5 );

    this->SetSizer( item0 );
    item0->SetSizeHints( this );

    // status
    m_client = NULL;
    GetServername()->SetSelection(0);
    GetHostname()->SetSelection(0);
    GetTopic()->SetSelection(0);
    wxLogTextCtrl *logWindow = new wxLogTextCtrl(GetLog());
    delete wxLog::SetActiveTarget(logWindow);
    wxLogMessage(_T("Click on Connect to connect to the server"));
    EnableControls();
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChDoubleWishboneReduced::LogConstraintViolations(VehicleSide side) {
    // Revolute joint
    {
        ChMatrix<>* C = m_revolute[side]->GetC();
        GetLog() << "Spindle revolute      ";
        GetLog() << "  " << C->GetElement(0, 0) << "  ";
        GetLog() << "  " << C->GetElement(1, 0) << "  ";
        GetLog() << "  " << C->GetElement(2, 0) << "  ";
        GetLog() << "  " << C->GetElement(3, 0) << "  ";
        GetLog() << "  " << C->GetElement(4, 0) << "\n";
    }

    // Distance constraints
    GetLog() << "UCA front distance    ";
    GetLog() << "  " << m_distUCA_F[side]->GetCurrentDistance() - m_distUCA_F[side]->GetImposedDistance() << "\n";

    GetLog() << "UCA back distance     ";
    GetLog() << "  " << m_distUCA_B[side]->GetCurrentDistance() - m_distUCA_B[side]->GetImposedDistance() << "\n";

    GetLog() << "LCA front distance    ";
    GetLog() << "  " << m_distLCA_F[side]->GetCurrentDistance() - m_distLCA_F[side]->GetImposedDistance() << "\n";

    GetLog() << "LCA back distance     ";
    GetLog() << "  " << m_distLCA_B[side]->GetCurrentDistance() - m_distLCA_B[side]->GetImposedDistance() << "\n";

    GetLog() << "Tierod distance       ";
    GetLog() << "  " << m_distTierod[side]->GetCurrentDistance() - m_distTierod[side]->GetImposedDistance() << "\n";
}
예제 #5
0
double ChLcpIterativeAPGD::Solve(
					ChLcpSystemDescriptor& sysd		///< system description with constraints and variables	
					)
{
	std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
	std::vector<ChLcpVariables*>&  mvariables	= sysd.GetVariablesList();


	

	double gdiff= 0.000001;

	double maxviolation = 0.;
	int i_friction_comp = 0;

	double theta_k=1.0;
	double theta_k1=theta_k;
	double beta_k1=0.0;

	double L_k=0.0;
	double t_k=0.0;

	tot_iterations = 0;
	// Allocate auxiliary vectors;
	
	int nc = sysd.CountActiveConstraints();
	if (verbose) GetLog() <<"\n-----Accelerated Projected Gradient Descent, solving nc=" << nc << "unknowns \n";

	//ChMatrixDynamic<> ml(nc,1);		//I made this into a class variable so I could print it easier -Hammad
	ml.Resize(nc,1);
	ChMatrixDynamic<> mx(nc,1);
	ChMatrixDynamic<> ms(nc,1);
	ChMatrixDynamic<> my(nc,1);
	ChMatrixDynamic<> ml_candidate(nc,1);
	ChMatrixDynamic<> mg(nc,1);
	ChMatrixDynamic<> mg_tmp(nc,1);
	ChMatrixDynamic<> mg_tmp1(nc,1);
	ChMatrixDynamic<> mg_tmp2(nc,1);
	//ChMatrixDynamic<> mb(nc,1);   //I made this into a class variable so I could print it easier -Hammad
	mb.Resize(nc,1);
	ChMatrixDynamic<> mb_tmp(nc,1);


	// Update auxiliary data in all constraints before starting,
	// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and  [Eq_i]=[invM_i]*[Cq_i]'
	for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
		mconstraints[ic]->Update_auxiliary();

	// Average all g_i for the triplet of contact constraints n,u,v.
	//  Can be used for the fixed point phase and/or by preconditioner.
	int j_friction_comp = 0;
	double gi_values[3];
	for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
	{
		if (mconstraints[ic]->GetMode() == CONSTRAINT_FRIC)
		{
			gi_values[j_friction_comp] = mconstraints[ic]->Get_g_i();
			j_friction_comp++;
			if (j_friction_comp==3)
			{
				double average_g_i = (gi_values[0]+gi_values[1]+gi_values[2])/3.0;
				mconstraints[ic-2]->Set_g_i(average_g_i);
				mconstraints[ic-1]->Set_g_i(average_g_i);
				mconstraints[ic-0]->Set_g_i(average_g_i);
				j_friction_comp=0;
			}
		}
	}

	// ***TO DO*** move the following thirty lines in a short function ChLcpSystemDescriptor::ShurBvectorCompute() ?

	// Compute the b_shur vector in the Shur complement equation N*l = b_shur
	// with 
	//   N_shur  = D'* (M^-1) * D
	//   b_shur  = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k
	// but flipping the sign of lambdas,  b_shur = - b_i - D'*(M^-1)*k
	// Do this in three steps:
	
	// Put (M^-1)*k    in  q  sparse vector of each variable..
	for (unsigned int iv = 0; iv< mvariables.size(); iv++)
		if (mvariables[iv]->IsActive())
			mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = [M]'*fb 

	// ...and now do  b_shur = - D'*q = - D'*(M^-1)*k ..
	int s_i = 0;
	for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
		if (mconstraints[ic]->IsActive())
		{
			mb(s_i, 0) = - mconstraints[ic]->Compute_Cq_q();
			++s_i;
		}

	// ..and finally do   b_shur = b_shur - c
	sysd.BuildBiVector(mb_tmp);	// b_i   =   -c   = phi/h
	mb.MatrDec(mb_tmp);

		// Optimization: backup the  q  sparse data computed above, 
		// because   (M^-1)*k   will be needed at the end when computing primals.
	ChMatrixDynamic<> mq; 
	sysd.FromVariablesToVector(mq, true);	


	// Initialize lambdas
	if (warm_start)
		sysd.FromConstraintsToVector(ml);
	else
		ml.FillElem(0);


	// Initial projection of ml   ***TO DO***?
	sysd.ConstraintsProject(ml);


	// Fallback solution
	double lastgoodres  = 10e30;
	double lastgoodfval = 10e30;
	ml_candidate.CopyFromMatrix(ml);

	// g = gradient of 0.5*l'*N*l-l'*b
	// g = N*l-b
	sysd.ShurComplementProduct(mg, &ml, 0);		// 1)  g = N*l ...        #### MATR.MULTIPLICATION!!!###
	mg.MatrDec(mb);								// 2)  g = N*l - b_shur ...

	//
	// THE LOOP
	//

	double mf_p =0;

	mb_tmp.FillElem(-1.0);
	mb_tmp.MatrInc(ml);
	sysd.ShurComplementProduct(mg_tmp,&mb_tmp,0); // 1)  g = N*l ...        #### MATR.MULTIPLICATION!!!###
	if(mb_tmp.NormTwo()==0){
		L_k=1;
	}else{
		L_k=mg_tmp.NormTwo()/mb_tmp.NormTwo();
	}
	t_k=1/L_k;

	double obj1=0;
	double obj2=0;

	my.CopyFromMatrix(ml);
	mx.CopyFromMatrix(ml);

	for (int iter = 0; iter < max_iterations; iter++)
	{
		sysd.ShurComplementProduct(mg_tmp1, &my, 0);	// 1)  g_tmp1 = N*yk ...        #### MATR.MULTIPLICATION!!!###
		mg.MatrSub(mg_tmp1,mb);							// 2)  g = N*yk - b_shur ...

		mx.CopyFromMatrix(mg);						// 1) xk1=g
		mx.MatrScale(-t_k); 						// 2) xk1=-tk*g
		mx.MatrInc(my);								// 3) xk1=y-tk*g
		sysd.ConstraintsProject(mx);				// 4) xk1=P(y-tk*g)

		//Now do backtracking for the steplength
		sysd.ShurComplementProduct(mg_tmp, &mx, 0);		// 1)  g_tmp = N*xk1 ...        #### MATR.MULTIPLICATION!!!###
		mg_tmp2.MatrSub(mg_tmp,mb);						// 2)  g_tmp2 = N*xk1 - b_shur ...
		mg_tmp.MatrScale(0.5);							// 3)  g_tmp=0.5*N*xk1
		mg_tmp.MatrDec(mb);								// 4)  g_tmp=0.5*N*xk1-b_shur
		obj1 = mx.MatrDot(&mx,&mg_tmp);					// 5)  obj1=xk1'*(0.5*N*x_k1-b_shur)

		mg_tmp1.MatrScale(0.5);							// 1)  g_tmp1 = 0.5*N*yk
		mg_tmp1.MatrDec(mb);							// 2)  g_tmp1 = 0.5*N*yk-b_shur
		obj2 = my.MatrDot(&my,&mg_tmp1);				// 3)  obj2 = yk'*(0.5*N*yk-b_shur)

		ms.MatrSub(mx,my);								// 1)  s=xk1-yk
		while(obj1>obj2+mg.MatrDot(&mg,&ms)+0.5*L_k*pow(ms.NormTwo(),2.0))
		{
			L_k=2*L_k;
			t_k=1/L_k;

			mx.CopyFromMatrix(mg);						// 1) xk1=g
			mx.MatrScale(-t_k);							// 2) xk1=-tk*g
			mx.MatrInc(my);								// 3) xk1=yk-tk*g
			sysd.ConstraintsProject(mx);				// 4) xk1=P(yk-tk*g)

			sysd.ShurComplementProduct(mg_tmp, &mx, 0);		// 1)  g_tmp = N*xk1 ...        #### MATR.MULTIPLICATION!!!###
			mg_tmp2.MatrSub(mg_tmp,mb);						// 2)  g_tmp2 = N*xk1 - b_shur ...
			mg_tmp.MatrScale(0.5);							// 3)  g_tmp=0.5*N*xk1
			mg_tmp.MatrDec(mb);								// 4)  g_tmp=0.5*N*xk1-b_shur
			obj1 = mx.MatrDot(&mx,&mg_tmp);					// 5)  obj1=xk1'*(0.5*N*x_k1-b_shur)

			ms.MatrSub(mx,my);								// 1)  s=xk1-yk
			if (verbose) GetLog() << "APGD halving stepsize at it " << iter  << "\n";
		}

		theta_k1=(-pow(theta_k,2)+theta_k*sqrt(pow(theta_k,2)+4))/2.0;
		beta_k1=theta_k*(1.0-theta_k)/(pow(theta_k,2)+theta_k1);

		my.CopyFromMatrix(mx);						// 1) y=xk1;
		my.MatrDec(ml);								// 2) y=xk1-xk;
		my.MatrScale(beta_k1);						// 3) y=beta_k1*(xk1-xk);
		my.MatrInc(mx);								// 4) y=xk1+beta_k1*(xk1-xk);
		ms.MatrSub(mx,ml);						    // 0) s = xk1 - xk;

		// Restarting logic if momentum is not appropriate
		if (mg.MatrDot(&mg,&ms)>0)
		{
			my.CopyFromMatrix(mx);					// 1) y=xk1
			theta_k1=1.0;							// 2) theta_k=1
			if (verbose) GetLog() << "Restarting APGD at it " << iter  << "\n";
		}

		//Allow the step to grow...
		L_k=0.9*L_k;
		t_k=1/L_k;

		ml.CopyFromMatrix(mx);						// 1) xk=xk1;
		theta_k=theta_k1;							// 2) theta_k=theta_k1;

		//****METHOD 1 for residual, same as ChLcpIterativeBB
		// Project the gradient (for rollback strategy)
		// g_proj = (l-project_orthogonal(l - gdiff*g, fric))/gdiff;
		mb_tmp.CopyFromMatrix(mg_tmp2);
		mb_tmp.MatrScale(-gdiff);
		mb_tmp.MatrInc(ml);
		sysd.ConstraintsProject(mb_tmp);
		mb_tmp.MatrDec(ml);
		mb_tmp.MatrDivScale(-gdiff);
		double g_proj_norm = mb_tmp.NormTwo(); // NormInf() is faster..
		//****End of METHOD 1 for residual, same as ChLcpIterativeBB

		//****METHOD 2 for residual, same as ChLcpIterativeSOR
		maxviolation = 0;
		i_friction_comp = 0;
		for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
		{
			if (mconstraints[ic]->IsActive())
			{
				// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
				double candidate_violation = fabs(mconstraints[ic]->Violation(mg_tmp2.ElementN(ic)));

				if (mconstraints[ic]->GetMode() == CONSTRAINT_FRIC)
				{
					candidate_violation = 0;
					i_friction_comp++;

					if (i_friction_comp==1)
						candidate_violation = fabs(ChMin(0.0,mg_tmp2.ElementN(ic)));

					if (i_friction_comp==3)
						i_friction_comp =0;
				}
				else
				{

				}
				maxviolation = ChMax(maxviolation, fabs(candidate_violation));
			}
		}
		g_proj_norm=maxviolation;
		//****End of METHOD 2 for residual, same as ChLcpIterativeSOR

		// Rollback solution: the last best candidate ('l' with lowest projected gradient)
		// in fact the method is not monotone and it is quite 'noisy', if we do not
		// do this, a prematurely truncated iteration might give a crazy result.
		if(g_proj_norm < lastgoodres)
		{
			lastgoodres  = g_proj_norm;
			ml_candidate = ml;
		}

		// METRICS - convergence, plots, etc

		if (verbose)
		{
			// f_p = 0.5*l_candidate'*N*l_candidate - l_candidate'*b  = l_candidate'*(0.5*Nl_candidate - b);
			sysd.ShurComplementProduct(mg_tmp, &ml_candidate, 0);		// 1)  g_tmp = N*l_candidate ...        #### MATR.MULTIPLICATION!!!###
			mg_tmp.MatrScale(0.5);										// 2)  g_tmp = 0.5*N*l_candidate
			mg_tmp.MatrDec(mb);											// 3)  g_tmp = 0.5*N*l_candidate-b_shur
			mf_p = ml_candidate.MatrDot(&ml_candidate,&mg_tmp);			// 4)  mf_p  = l_candidate'*(0.5*N*l_candidate-b_shur)
		}

		double maxdeltalambda = ms.NormInf();
		double maxd			  = lastgoodres;
			
		// For recording into correction/residuals/violation history, if debugging
		if (this->record_violation_history)
			AtIterationEnd(maxd, maxdeltalambda, iter);

		if (verbose) GetLog() << "  iter=" << iter << "   f=" << mf_p << "  |d|=" << maxd << "  |s|=" << maxdeltalambda  << "\n";

		tot_iterations++;

		// Terminate the loop if violation in constraints has been succesfully limited.
		// ***TO DO*** a reliable termination creterion..
		///*
		if (maxd < this->tolerance)
		{
			if (verbose) GetLog() <<"APGD premature converged at i=" << iter << "\n";
			break;
		}
		//*/

	}

	// Fallback to best found solution (might be useful because of nonmonotonicity)
	ml.CopyFromMatrix(ml_candidate);


	// Resulting DUAL variables:
	// store ml temporary vector into ChLcpConstraint 'l_i' multipliers
	sysd.FromVectorToConstraints(ml); 


	// Resulting PRIMAL variables:
	// compute the primal variables as   v = (M^-1)(k + D*l) 

		// v = (M^-1)*k  ...    (by rewinding to the backup vector computed ad the beginning)
	sysd.FromVectorToVariables(mq);


		// ... + (M^-1)*D*l     (this increment and also stores 'qb' in the ChLcpVariable items)
	for (unsigned int ic = 0; ic < mconstraints.size(); ic++)
	{	
		if (mconstraints[ic]->IsActive())
			mconstraints[ic]->Increment_q( mconstraints[ic]->Get_l_i() );
	}
	

	if (verbose) GetLog() <<"-----\n";
	current_residual = lastgoodres;
	return lastgoodres;

}