void IpoptAlgorithm::calc_number_of_bounds(
const Vector& x,
const Vector& x_L,
const Vector& x_U,
const Matrix& Px_L,
const Matrix& Px_U,
Index& n_tot,
Index& n_only_lower,
Index& n_both,
Index& n_only_upper)
{
DBG_START_METH("IpoptAlgorithm::calc_number_of_bounds",
dbg_verbosity);
n_tot = x.Dim();
SmartPtr<Vector> tmpx = x.MakeNew();
SmartPtr<Vector> tmpxL = x_L.MakeNew();
SmartPtr<Vector> tmpxU = x_U.MakeNew();
tmpxL->Set(-1.);
tmpxU->Set(2.);
Px_L.MultVector(1.0, *tmpxL, 0.0, *tmpx);
Px_U.MultVector(1.0, *tmpxU, 1.0, *tmpx);
// Now, x has elements
// -1 : if component has only lower bound
// 0 : if component has no bound
// 1 : if component has both lower and upper bound
// 2 : if component has only upper bound
DBG_PRINT_VECTOR(2, "x-indicator", *tmpx);
SmartPtr<Vector> tmpx0 = x.MakeNew();
tmpx0->Set(0.);
SmartPtr<Vector> tmpx2 = x.MakeNew();
tmpx2->Set(-1.0);
tmpx2->Axpy(1.0, *tmpx);
tmpx2->ElementWiseMax(*tmpx0); // tmpx2 is now 1 in those
// components with only upper bounds
n_only_upper = (Index)tmpx2->Asum();
tmpx->Axpy(-2., *tmpx2); // now make all those entries for
// only upper bounds zero in tmpx
tmpx2->Copy(*tmpx);
tmpx2->ElementWiseMax(*tmpx0); // tmpx2 is now 1 in those
// components with both bounds
n_both = (Index)tmpx2->Asum();
tmpx->Axpy(-1., *tmpx2);
tmpx->ElementWiseMin(*tmpx); // tmpx is now -1 in those with only
// lower bounds
n_only_lower = (Index)tmpx->Asum();
}
SmartPtr<Vector> NLPScalingObject::apply_vector_scaling_d_LU_NonConst(
const Matrix& Pd_LU,
const SmartPtr<const Vector>& lu,
const VectorSpace& d_space
)
{
DBG_START_METH("NLPScalingObject::apply_vector_scaling_d_LU_NonConst", dbg_verbosity);
SmartPtr<Vector> scaled_d_LU = lu->MakeNew();
if( have_d_scaling() )
{
SmartPtr<Vector> tmp_d = d_space.MakeNew();
// move to full d space
Pd_LU.MultVector(1.0, *lu, 0.0, *tmp_d);
// scale in full x space
tmp_d = apply_vector_scaling_d_NonConst(ConstPtr(tmp_d));
// move back to x_L space
Pd_LU.TransMultVector(1.0, *tmp_d, 0.0, *scaled_d_LU);
}
else
{
scaled_d_LU->Copy(*lu);
}
return scaled_d_LU;
}
PointPerturber::PointPerturber(const Vector& x0,
Number random_pert_radius,
const Matrix& Px_L, const Vector& x_L,
const Matrix& Px_U, const Vector& x_U)
{
DBG_START_METH("PointPerturber::PointPerturber", dbg_verbosity);
const Number very_large = 1e300;
// First we compute full-space lower and upper bounds
SmartPtr<Vector> full_x_L = x0.MakeNew();
full_x_L->Set(-very_large);
SmartPtr<Vector> tmp = x_L.MakeNew();
tmp->Set(very_large);
Px_L.MultVector(1., *tmp, 1., *full_x_L);
DBG_PRINT_VECTOR(1,"full_x_L1", *full_x_L);
Px_L.MultVector(1., x_L, 1., *full_x_L);
DBG_PRINT_VECTOR(1,"full_x_L2", *full_x_L);
SmartPtr<Vector> full_x_U = x0.MakeNew();
full_x_U->Set(very_large);
tmp = x_U.MakeNew();
tmp->Set(-very_large);
Px_U.MultVector(1., *tmp, 1., *full_x_U);
DBG_PRINT_VECTOR(1,"full_x_U1", *full_x_U);
Px_U.MultVector(1., x_U, 1., *full_x_U);
DBG_PRINT_VECTOR(1,"full_x_U2", *full_x_U);
pert_dir_ = full_x_U->MakeNew();
pert_dir_->AddTwoVectors(.5, *full_x_U, -.5, *full_x_L, 0.);
tmp = full_x_U->MakeNew();
tmp->Set(random_pert_radius);
pert_dir_->ElementWiseMin(*tmp);
DBG_PRINT_VECTOR(1,"pert_dir", *pert_dir_);
ref_point_ = x0.MakeNewCopy();
DBG_PRINT_VECTOR(1,"ref_point1", *ref_point_);
full_x_U->AddOneVector(-1., *pert_dir_, 1.);
ref_point_->ElementWiseMin(*full_x_U);
DBG_PRINT_VECTOR(1,"ref_point2", *ref_point_);
full_x_L->AddOneVector(1., *pert_dir_, 1.);
ref_point_->ElementWiseMax(*full_x_L);
DBG_PRINT_VECTOR(1,"ref_point3", *ref_point_);
}
SmartPtr<const Vector>
AugRestoSystemSolver::Neg_Omega_d_plus_D_d(
const Matrix& Pd_L,
const SmartPtr<const Vector>& sigma_tilde_n_d_inv,
const Matrix& neg_Pd_U,
const SmartPtr<const Vector>& sigma_tilde_p_d_inv,
const Vector* D_d,
const Vector& any_vec_in_d)
{
DBG_START_METH("AugRestoSystemSolver::Neg_Omega_d_plus_D_d",dbg_verbosity);
SmartPtr<Vector> retVec;
if (IsValid(sigma_tilde_n_d_inv) || IsValid(sigma_tilde_p_d_inv) || D_d) {
std::vector<const TaggedObject*> deps(5);
std::vector<Number> scalar_deps;
deps[0] = &Pd_L;
deps[1] = GetRawPtr(sigma_tilde_n_d_inv);
deps[2] = &neg_Pd_U;
deps[3] = GetRawPtr(sigma_tilde_p_d_inv);
deps[4] = D_d;
if (!neg_omega_d_plus_D_d_cache_.
GetCachedResult(retVec, deps, scalar_deps)) {
DBG_PRINT((1,"Not found in cache\n"));
retVec = any_vec_in_d.MakeNew();
retVec->Set(0.0);
if (IsValid(sigma_tilde_n_d_inv)) {
Pd_L.MultVector(-1.0, *sigma_tilde_n_d_inv, 1.0, *retVec);
}
if (IsValid(sigma_tilde_p_d_inv)) {
neg_Pd_U.MultVector(1.0, *sigma_tilde_p_d_inv, 1.0, *retVec);
}
if (D_d) {
retVec->Copy(*D_d);
}
neg_omega_d_plus_D_d_cache_.
AddCachedResult(retVec, deps, scalar_deps);
}
}
return ConstPtr(retVec);
}
SmartPtr<const Vector> AugRestoSystemSolver::Rhs_dR(const Vector& rhs_d,
const SmartPtr<const Vector>& sigma_tilde_n_d_inv, const Vector& rhs_n_d, const Matrix& pd_L,
const SmartPtr<const Vector>& sigma_tilde_p_d_inv, const Vector& rhs_p_d, const Matrix& neg_pd_U)
{
DBG_START_METH("AugRestoSystemSolver::Rhs_dR",dbg_verbosity);
SmartPtr<Vector> retVec;
std::vector<const TaggedObject*> deps(7);
std::vector<Number> scalar_deps;
deps[0] = &rhs_d;
deps[1] = GetRawPtr(sigma_tilde_n_d_inv);
deps[2] = &rhs_n_d;
deps[3] = &pd_L;
deps[4] = GetRawPtr(sigma_tilde_p_d_inv);
deps[5] = &rhs_p_d;
deps[6] = &neg_pd_U;
if (!rhs_dR_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
DBG_PRINT((1,"Not found in cache\n"));
retVec = rhs_d.MakeNew();
retVec->Copy(rhs_d);
if (IsValid(sigma_tilde_n_d_inv)) {
SmartPtr<Vector> tmpn = sigma_tilde_n_d_inv->MakeNew();
tmpn->Copy(*sigma_tilde_n_d_inv);
tmpn->ElementWiseMultiply(rhs_n_d);
pd_L.MultVector(-1.0, *tmpn, 1.0, *retVec);
}
if (IsValid(sigma_tilde_p_d_inv)) {
SmartPtr<Vector> tmpp = sigma_tilde_p_d_inv->MakeNew();
tmpp->Copy(*sigma_tilde_p_d_inv);
tmpp->ElementWiseMultiply(rhs_p_d);
neg_pd_U.MultVector(-1.0, *tmpp, 1.0, *retVec);
}
rhs_dR_cache_.AddCachedResult(retVec, deps, scalar_deps);
}
return ConstPtr(retVec);
}
void WarmStartIterateInitializer::process_target_mu(Number factor,
const Vector& curr_vars,
const Vector& curr_slacks,
const Vector& curr_mults,
const Matrix& P,
SmartPtr<const Vector>& ret_vars,
SmartPtr<const Vector>& ret_mults)
{
SmartPtr<Vector> new_slacks = curr_slacks.MakeNewCopy();
SmartPtr<Vector> new_mults = curr_mults.MakeNewCopy();
adapt_to_target_mu(*new_slacks, *new_mults, warm_start_target_mu_);
new_slacks->Axpy(-1, curr_slacks); // this is now correction step
SmartPtr<Vector> new_vars = curr_vars.MakeNew();
new_vars->Copy(curr_vars);
P.MultVector(factor, *new_slacks, 1., *new_vars);
ret_vars = ConstPtr(new_vars);
ret_mults = ConstPtr(new_mults);
}
void MapObject::ScreenPos2MapPos(int ScreenX, int ScreenY, Matrix MVP, float &MapX, float &MapY)
{
__UINT32 W = VIEWCLASS::GetInstance()->GetWidth();
__UINT32 H = VIEWCLASS::GetInstance()->GetHeight();
SPosition2D<float> fpos;
SPosition2D<float> fpos2;
MVP.Inverse();
fpos.X = (float(ScreenX)*2 - W)/W;
fpos.Y = (float(ScreenY)*2 - H)/H;
//Use ray to detect the point that cursor point to
Vector v1( fpos.X,
fpos.Y,
0,
1);
Vector v2( fpos.X,
fpos.Y,
10,
1);
float* v1data = v1.GetData();
float* v2data = v2.GetData();
//Mult inverse matrix
v1 = MVP.MultVector(v1);v1.Normalize();
v2 = MVP.MultVector(v2);v2.Normalize();
v1data = v1.GetData();
v2data = v2.GetData();
//Calculate position in object coordinate
float t = (v1data[2])/(v1data[2]-v2data[2]);
fpos2.X = v1data[0] + (v2data[0]-v1data[0])*t;
fpos2.Y = v1data[1] + (v2data[1]-v1data[1])*t;
MapX = fpos2.X;
MapY = fpos2.Y;
}
SmartPtr<Vector> NLPScalingObject::apply_vector_scaling_x_LU_NonConst(
const Matrix& Px_LU,
const SmartPtr<const Vector>& lu,
const VectorSpace& x_space)
{
SmartPtr<Vector> scaled_x_LU = lu->MakeNew();
if (have_x_scaling()) {
SmartPtr<Vector> tmp_x = x_space.MakeNew();
// move to full x space
Px_LU.MultVector(1.0, *lu, 0.0, *tmp_x);
// scale in full x space
tmp_x = apply_vector_scaling_x_NonConst(ConstPtr(tmp_x));
// move back to x_L space
Px_LU.TransMultVector(1.0, *tmp_x, 0.0, *scaled_x_LU);
}
else {
scaled_x_LU->Copy(*lu);
}
return scaled_x_LU;
}
void PDFullSpaceSolver::ComputeResiduals(
const SymMatrix& W,
const Matrix& J_c,
const Matrix& J_d,
const Matrix& Px_L,
const Matrix& Px_U,
const Matrix& Pd_L,
const Matrix& Pd_U,
const Vector& z_L,
const Vector& z_U,
const Vector& v_L,
const Vector& v_U,
const Vector& slack_x_L,
const Vector& slack_x_U,
const Vector& slack_s_L,
const Vector& slack_s_U,
const Vector& sigma_x,
const Vector& sigma_s,
Number alpha,
Number beta,
const IteratesVector& rhs,
const IteratesVector& res,
IteratesVector& resid)
{
DBG_START_METH("PDFullSpaceSolver::ComputeResiduals", dbg_verbosity);
DBG_PRINT_VECTOR(2, "res", res);
IpData().TimingStats().ComputeResiduals().Start();
// Get the current sizes of the perturbation factors
Number delta_x;
Number delta_s;
Number delta_c;
Number delta_d;
perturbHandler_->CurrentPerturbation(delta_x, delta_s, delta_c, delta_d);
SmartPtr<Vector> tmp;
// x
W.MultVector(1., *res.x(), 0., *resid.x_NonConst());
J_c.TransMultVector(1., *res.y_c(), 1., *resid.x_NonConst());
J_d.TransMultVector(1., *res.y_d(), 1., *resid.x_NonConst());
Px_L.MultVector(-1., *res.z_L(), 1., *resid.x_NonConst());
Px_U.MultVector(1., *res.z_U(), 1., *resid.x_NonConst());
resid.x_NonConst()->AddTwoVectors(delta_x, *res.x(), -1., *rhs.x(), 1.);
// s
Pd_U.MultVector(1., *res.v_U(), 0., *resid.s_NonConst());
Pd_L.MultVector(-1., *res.v_L(), 1., *resid.s_NonConst());
resid.s_NonConst()->AddTwoVectors(-1., *res.y_d(), -1., *rhs.s(), 1.);
if (delta_s!=0.) {
resid.s_NonConst()->Axpy(delta_s, *res.s());
}
// c
J_c.MultVector(1., *res.x(), 0., *resid.y_c_NonConst());
resid.y_c_NonConst()->AddTwoVectors(-delta_c, *res.y_c(), -1., *rhs.y_c(), 1.);
// d
J_d.MultVector(1., *res.x(), 0., *resid.y_d_NonConst());
resid.y_d_NonConst()->AddTwoVectors(-1., *res.s(), -1., *rhs.y_d(), 1.);
if (delta_d!=0.) {
resid.y_d_NonConst()->Axpy(-delta_d, *res.y_d());
}
// zL
resid.z_L_NonConst()->Copy(*res.z_L());
resid.z_L_NonConst()->ElementWiseMultiply(slack_x_L);
tmp = z_L.MakeNew();
Px_L.TransMultVector(1., *res.x(), 0., *tmp);
tmp->ElementWiseMultiply(z_L);
resid.z_L_NonConst()->AddTwoVectors(1., *tmp, -1., *rhs.z_L(), 1.);
// zU
resid.z_U_NonConst()->Copy(*res.z_U());
resid.z_U_NonConst()->ElementWiseMultiply(slack_x_U);
tmp = z_U.MakeNew();
Px_U.TransMultVector(1., *res.x(), 0., *tmp);
tmp->ElementWiseMultiply(z_U);
resid.z_U_NonConst()->AddTwoVectors(-1., *tmp, -1., *rhs.z_U(), 1.);
// vL
resid.v_L_NonConst()->Copy(*res.v_L());
resid.v_L_NonConst()->ElementWiseMultiply(slack_s_L);
tmp = v_L.MakeNew();
Pd_L.TransMultVector(1., *res.s(), 0., *tmp);
tmp->ElementWiseMultiply(v_L);
resid.v_L_NonConst()->AddTwoVectors(1., *tmp, -1., *rhs.v_L(), 1.);
// vU
resid.v_U_NonConst()->Copy(*res.v_U());
resid.v_U_NonConst()->ElementWiseMultiply(slack_s_U);
tmp = v_U.MakeNew();
Pd_U.TransMultVector(1., *res.s(), 0., *tmp);
tmp->ElementWiseMultiply(v_U);
resid.v_U_NonConst()->AddTwoVectors(-1., *tmp, -1., *rhs.v_U(), 1.);
DBG_PRINT_VECTOR(2, "resid", resid);
if (Jnlst().ProduceOutput(J_MOREVECTOR, J_LINEAR_ALGEBRA)) {
resid.Print(Jnlst(), J_MOREVECTOR, J_LINEAR_ALGEBRA, "resid");
}
if (Jnlst().ProduceOutput(J_MOREDETAILED, J_LINEAR_ALGEBRA)) {
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_x %e\n", resid.x()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_s %e\n", resid.s()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_c %e\n", resid.y_c()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_d %e\n", resid.y_d()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_zL %e\n", resid.z_L()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_zU %e\n", resid.z_U()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_vL %e\n", resid.v_L()->Amax());
Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
"max-norm resid_vU %e\n", resid.v_U()->Amax());
}
IpData().TimingStats().ComputeResiduals().End();
}
