1use pounce_common::types::{Index, Number};
17use pounce_nlp::tnlp::{
18 BoundsInfo, IpoptCq, IpoptData, IterStats, MetaData, NlpInfo, ScalingRequest, Solution,
19 SparsityRequest, StartingPoint, TNLP,
20};
21use std::cell::RefCell;
22use std::rc::Rc;
23
24#[derive(Debug, Clone)]
27pub enum Kernel {
28 Gauss {
31 centers: Vec<Vec<Number>>,
32 amps: Vec<Number>,
33 inv_sigma2: Vec<Number>,
34 },
35 Pole {
38 centers: Vec<Vec<Number>>,
39 eta: Number,
40 q: Number,
41 soft: Number,
42 inv_len2: Vec<Number>,
43 },
44}
45
46impl Kernel {
47 pub fn value(&self, x: &[Number]) -> Number {
49 match self {
50 Kernel::Gauss {
51 centers,
52 amps,
53 inv_sigma2,
54 } => {
55 let mut val = 0.0;
56 for (c, &a) in centers.iter().zip(amps) {
57 let mut q = 0.0;
58 for i in 0..x.len() {
59 let d = x[i] - c[i];
60 q += inv_sigma2[i] * d * d;
61 }
62 val += a * (-0.5 * q).exp();
63 }
64 val
65 }
66 Kernel::Pole {
67 centers,
68 eta,
69 q,
70 soft,
71 inv_len2,
72 } => {
73 let mut val = 0.0;
74 for c in centers {
75 let mut r2 = *soft;
76 for i in 0..x.len() {
77 let d = x[i] - c[i];
78 r2 += inv_len2[i] * d * d;
79 }
80 val += eta * r2.powf(-*q);
81 }
82 val
83 }
84 }
85 }
86
87 pub fn add_grad(&self, x: &[Number], grad: &mut [Number]) {
89 match self {
90 Kernel::Gauss {
91 centers,
92 amps,
93 inv_sigma2,
94 } => {
95 for (c, &a) in centers.iter().zip(amps) {
96 let mut q = 0.0;
97 for i in 0..x.len() {
98 let d = x[i] - c[i];
99 q += inv_sigma2[i] * d * d;
100 }
101 let g = a * (-0.5 * q).exp();
102 for i in 0..x.len() {
104 let md = inv_sigma2[i] * (x[i] - c[i]);
105 grad[i] -= g * md;
106 }
107 }
108 }
109 Kernel::Pole {
110 centers,
111 eta,
112 q,
113 soft,
114 inv_len2,
115 } => {
116 for c in centers {
117 let mut r2 = *soft;
118 for i in 0..x.len() {
119 let d = x[i] - c[i];
120 r2 += inv_len2[i] * d * d;
121 }
122 let coef1 = -2.0 * q * eta * r2.powf(-*q - 1.0);
124 for i in 0..x.len() {
125 let md = inv_len2[i] * (x[i] - c[i]);
126 grad[i] += coef1 * md;
127 }
128 }
129 }
130 }
131 }
132}
133
134pub struct PenaltyTnlp {
138 inner: Rc<RefCell<dyn TNLP>>,
139 kernel: Kernel,
140}
141
142impl PenaltyTnlp {
143 pub fn new(inner: Rc<RefCell<dyn TNLP>>, kernel: Kernel) -> Self {
144 Self { inner, kernel }
145 }
146}
147
148impl TNLP for PenaltyTnlp {
149 fn get_nlp_info(&mut self) -> Option<NlpInfo> {
150 self.inner.borrow_mut().get_nlp_info()
151 }
152 fn get_bounds_info(&mut self, b: BoundsInfo<'_>) -> bool {
153 self.inner.borrow_mut().get_bounds_info(b)
154 }
155 fn get_starting_point(&mut self, sp: StartingPoint<'_>) -> bool {
156 self.inner.borrow_mut().get_starting_point(sp)
157 }
158 fn eval_f(&mut self, x: &[Number], new_x: bool) -> Option<Number> {
159 let base = self.inner.borrow_mut().eval_f(x, new_x)?;
160 Some(base + self.kernel.value(x))
161 }
162 fn eval_grad_f(&mut self, x: &[Number], new_x: bool, grad_f: &mut [Number]) -> bool {
163 if !self.inner.borrow_mut().eval_grad_f(x, new_x, grad_f) {
164 return false;
165 }
166 self.kernel.add_grad(x, grad_f);
167 true
168 }
169 fn eval_g(&mut self, x: &[Number], new_x: bool, g: &mut [Number]) -> bool {
170 self.inner.borrow_mut().eval_g(x, new_x, g)
171 }
172 fn eval_jac_g(&mut self, x: Option<&[Number]>, new_x: bool, mode: SparsityRequest<'_>) -> bool {
173 self.inner.borrow_mut().eval_jac_g(x, new_x, mode)
174 }
175 fn eval_h(
176 &mut self,
177 x: Option<&[Number]>,
178 new_x: bool,
179 obj_factor: Number,
180 lambda: Option<&[Number]>,
181 new_lambda: bool,
182 mode: SparsityRequest<'_>,
183 ) -> bool {
184 self.inner
191 .borrow_mut()
192 .eval_h(x, new_x, obj_factor, lambda, new_lambda, mode)
193 }
194 fn finalize_solution(&mut self, sol: Solution<'_>, ip_data: &IpoptData, ip_cq: &IpoptCq) {
195 self.inner
196 .borrow_mut()
197 .finalize_solution(sol, ip_data, ip_cq)
198 }
199 fn get_var_con_metadata(&mut self, var: &mut MetaData, con: &mut MetaData) -> bool {
200 self.inner.borrow_mut().get_var_con_metadata(var, con)
201 }
202 fn get_scaling_parameters(&mut self, req: ScalingRequest<'_>) -> bool {
203 self.inner.borrow_mut().get_scaling_parameters(req)
204 }
205 fn get_number_of_nonlinear_variables(&mut self) -> Index {
206 self.inner.borrow_mut().get_number_of_nonlinear_variables()
207 }
208 fn get_list_of_nonlinear_variables(&mut self, pos: &mut [Index]) -> bool {
209 self.inner.borrow_mut().get_list_of_nonlinear_variables(pos)
210 }
211 fn intermediate_callback(
212 &mut self,
213 stats: IterStats,
214 ip_data: &IpoptData,
215 ip_cq: &IpoptCq,
216 ) -> bool {
217 self.inner
218 .borrow_mut()
219 .intermediate_callback(stats, ip_data, ip_cq)
220 }
221 fn finalize_metadata(&mut self, var: &MetaData, con: &MetaData) {
222 self.inner.borrow_mut().finalize_metadata(var, con)
223 }
224}
225
226pub struct TunnelTnlp {
230 inner: Rc<RefCell<dyn TNLP>>,
231 f_ref: Number,
232 pole: Kernel,
233}
234
235impl TunnelTnlp {
236 pub fn new(inner: Rc<RefCell<dyn TNLP>>, f_ref: Number, pole: Kernel) -> Self {
237 Self { inner, f_ref, pole }
238 }
239}
240
241impl TNLP for TunnelTnlp {
242 fn get_nlp_info(&mut self) -> Option<NlpInfo> {
243 self.inner.borrow_mut().get_nlp_info()
244 }
245 fn get_bounds_info(&mut self, b: BoundsInfo<'_>) -> bool {
246 self.inner.borrow_mut().get_bounds_info(b)
247 }
248 fn get_starting_point(&mut self, sp: StartingPoint<'_>) -> bool {
249 self.inner.borrow_mut().get_starting_point(sp)
250 }
251 fn eval_f(&mut self, x: &[Number], new_x: bool) -> Option<Number> {
252 let base = self.inner.borrow_mut().eval_f(x, new_x)?;
253 let d = base - self.f_ref;
254 Some(d * d + self.pole.value(x))
255 }
256 fn eval_grad_f(&mut self, x: &[Number], new_x: bool, grad_f: &mut [Number]) -> bool {
257 let base = match self.inner.borrow_mut().eval_f(x, new_x) {
259 Some(v) => v,
260 None => return false,
261 };
262 if !self.inner.borrow_mut().eval_grad_f(x, false, grad_f) {
263 return false;
264 }
265 let scale = 2.0 * (base - self.f_ref);
266 for g in grad_f.iter_mut() {
267 *g *= scale;
268 }
269 self.pole.add_grad(x, grad_f);
270 true
271 }
272 fn eval_g(&mut self, x: &[Number], new_x: bool, g: &mut [Number]) -> bool {
273 self.inner.borrow_mut().eval_g(x, new_x, g)
274 }
275 fn eval_jac_g(&mut self, x: Option<&[Number]>, new_x: bool, mode: SparsityRequest<'_>) -> bool {
276 self.inner.borrow_mut().eval_jac_g(x, new_x, mode)
277 }
278 fn eval_h(
279 &mut self,
280 x: Option<&[Number]>,
281 new_x: bool,
282 obj_factor: Number,
283 lambda: Option<&[Number]>,
284 new_lambda: bool,
285 mode: SparsityRequest<'_>,
286 ) -> bool {
287 self.inner
290 .borrow_mut()
291 .eval_h(x, new_x, obj_factor, lambda, new_lambda, mode)
292 }
293 fn finalize_solution(&mut self, sol: Solution<'_>, ip_data: &IpoptData, ip_cq: &IpoptCq) {
294 self.inner
295 .borrow_mut()
296 .finalize_solution(sol, ip_data, ip_cq)
297 }
298 fn get_var_con_metadata(&mut self, var: &mut MetaData, con: &mut MetaData) -> bool {
299 self.inner.borrow_mut().get_var_con_metadata(var, con)
300 }
301 fn get_scaling_parameters(&mut self, req: ScalingRequest<'_>) -> bool {
302 self.inner.borrow_mut().get_scaling_parameters(req)
303 }
304 fn get_number_of_nonlinear_variables(&mut self) -> Index {
305 self.inner.borrow_mut().get_number_of_nonlinear_variables()
306 }
307 fn get_list_of_nonlinear_variables(&mut self, pos: &mut [Index]) -> bool {
308 self.inner.borrow_mut().get_list_of_nonlinear_variables(pos)
309 }
310 fn intermediate_callback(
311 &mut self,
312 stats: IterStats,
313 ip_data: &IpoptData,
314 ip_cq: &IpoptCq,
315 ) -> bool {
316 self.inner
317 .borrow_mut()
318 .intermediate_callback(stats, ip_data, ip_cq)
319 }
320 fn finalize_metadata(&mut self, var: &MetaData, con: &MetaData) {
321 self.inner.borrow_mut().finalize_metadata(var, con)
322 }
323}