Another project
1use bone_types::{ParameterIndex, ResidualIndex};
2
3#[derive(Copy, Clone, Debug, PartialEq, Eq)]
4pub struct PointHandle {
5 pub x: ParameterIndex,
6 pub y: ParameterIndex,
7}
8
9#[derive(Copy, Clone, Debug, PartialEq, Eq)]
10pub struct LineHandle {
11 pub a: PointHandle,
12 pub b: PointHandle,
13}
14
15#[derive(Copy, Clone, Debug, PartialEq, Eq)]
16pub enum CurveRadius {
17 Explicit {
18 center: PointHandle,
19 radius: ParameterIndex,
20 },
21 FromSpoke {
22 center: PointHandle,
23 spoke: PointHandle,
24 },
25}
26
27impl CurveRadius {
28 #[must_use]
29 pub fn center(self) -> PointHandle {
30 match self {
31 Self::Explicit { center, .. } | Self::FromSpoke { center, .. } => center,
32 }
33 }
34}
35
36#[derive(Clone, Debug, PartialEq)]
37pub enum Residual {
38 Pin {
39 param: ParameterIndex,
40 target: f64,
41 },
42 Horizontal(LineHandle),
43 Vertical(LineHandle),
44 Parallel(LineHandle, LineHandle),
45 Perpendicular(LineHandle, LineHandle),
46 TangentLineCurve {
47 line: LineHandle,
48 curve: CurveRadius,
49 },
50 TangentCurveCurve {
51 a: CurveRadius,
52 b: CurveRadius,
53 },
54 CoincidentPointPoint(PointHandle, PointHandle),
55 CoincidentPointLine {
56 point: PointHandle,
57 line: LineHandle,
58 },
59 CoincidentPointCurve {
60 point: PointHandle,
61 curve: CurveRadius,
62 },
63 MidpointPointLine {
64 point: PointHandle,
65 line: LineHandle,
66 },
67 EqualLength(LineHandle, LineHandle),
68 EqualRadius(CurveRadius, CurveRadius),
69 LinearDistance {
70 a: PointHandle,
71 b: PointHandle,
72 value_mm: f64,
73 },
74 AngularBetweenLines {
75 a: LineHandle,
76 b: LineHandle,
77 angle_rad: f64,
78 },
79 RadiusCurve {
80 curve: CurveRadius,
81 value_mm: f64,
82 },
83 Symmetric {
84 a: PointHandle,
85 b: PointHandle,
86 axis: LineHandle,
87 },
88}
89
90pub type Triplet = (ResidualIndex, ParameterIndex, f64);
91
92impl Residual {
93 #[must_use]
94 pub fn rows(&self) -> usize {
95 match self {
96 Self::CoincidentPointPoint(..)
97 | Self::MidpointPointLine { .. }
98 | Self::Symmetric { .. } => 2,
99 _ => 1,
100 }
101 }
102
103 #[must_use]
104 pub fn parameters(&self) -> Vec<ParameterIndex> {
105 match *self {
106 Self::Pin { param, .. } => vec![param],
107 Self::Horizontal(l) | Self::Vertical(l) => line_params(l),
108 Self::Parallel(l1, l2) | Self::Perpendicular(l1, l2) | Self::EqualLength(l1, l2) => {
109 line_params(l1).into_iter().chain(line_params(l2)).collect()
110 }
111 Self::TangentLineCurve { line, curve } => line_params(line)
112 .into_iter()
113 .chain(curve_params(curve))
114 .collect(),
115 Self::TangentCurveCurve { a, b } | Self::EqualRadius(a, b) => {
116 curve_params(a).into_iter().chain(curve_params(b)).collect()
117 }
118 Self::CoincidentPointPoint(p, q) => {
119 point_params(p).into_iter().chain(point_params(q)).collect()
120 }
121 Self::CoincidentPointLine { point, line } | Self::MidpointPointLine { point, line } => {
122 point_params(point)
123 .into_iter()
124 .chain(line_params(line))
125 .collect()
126 }
127 Self::CoincidentPointCurve { point, curve } => point_params(point)
128 .into_iter()
129 .chain(curve_params(curve))
130 .collect(),
131 Self::LinearDistance { a, b, .. } => {
132 point_params(a).into_iter().chain(point_params(b)).collect()
133 }
134 Self::AngularBetweenLines { a, b, .. } => {
135 line_params(a).into_iter().chain(line_params(b)).collect()
136 }
137 Self::RadiusCurve { curve, .. } => curve_params(curve),
138 Self::Symmetric { a, b, axis } => point_params(a)
139 .into_iter()
140 .chain(point_params(b))
141 .chain(line_params(axis))
142 .collect(),
143 }
144 }
145
146 pub fn evaluate(&self, params: &[f64], out: &mut [f64]) {
147 match *self {
148 Self::Pin { param, target } => {
149 out[0] = params[param.as_usize()] - target;
150 }
151 Self::Horizontal(l) => out[0] = get(params, l.a.y) - get(params, l.b.y),
152 Self::Vertical(l) => out[0] = get(params, l.a.x) - get(params, l.b.x),
153 Self::Parallel(l1, l2) => {
154 let (d1x, d1y) = delta(params, l1);
155 let (d2x, d2y) = delta(params, l2);
156 out[0] = d1x * d2y - d1y * d2x;
157 }
158 Self::Perpendicular(l1, l2) => {
159 let (d1x, d1y) = delta(params, l1);
160 let (d2x, d2y) = delta(params, l2);
161 out[0] = d1x * d2x + d1y * d2y;
162 }
163 Self::TangentLineCurve { line, curve } => {
164 let (dx, dy) = delta(params, line);
165 let cx = get(params, curve.center().x);
166 let cy = get(params, curve.center().y);
167 let ex = cx - get(params, line.a.x);
168 let ey = cy - get(params, line.a.y);
169 let cross = ex * dy - ey * dx;
170 let len2 = dx * dx + dy * dy;
171 let r2 = radius_squared(params, curve);
172 out[0] = cross * cross - r2 * len2;
173 }
174 Self::TangentCurveCurve { a, b } => {
175 let ax = get(params, a.center().x);
176 let ay = get(params, a.center().y);
177 let bx = get(params, b.center().x);
178 let by = get(params, b.center().y);
179 let ra = radius_squared(params, a).sqrt();
180 let rb = radius_squared(params, b).sqrt();
181 let u = ax - bx;
182 let v = ay - by;
183 let rsum = ra + rb;
184 out[0] = u * u + v * v - rsum * rsum;
185 }
186 Self::CoincidentPointPoint(p, q) => {
187 out[0] = get(params, p.x) - get(params, q.x);
188 out[1] = get(params, p.y) - get(params, q.y);
189 }
190 Self::CoincidentPointLine { point, line } => {
191 let (dx, dy) = delta(params, line);
192 let ex = get(params, point.x) - get(params, line.a.x);
193 let ey = get(params, point.y) - get(params, line.a.y);
194 out[0] = ex * dy - ey * dx;
195 }
196 Self::MidpointPointLine { point, line } => {
197 out[0] =
198 get(params, point.x) - 0.5 * (get(params, line.a.x) + get(params, line.b.x));
199 out[1] =
200 get(params, point.y) - 0.5 * (get(params, line.a.y) + get(params, line.b.y));
201 }
202 Self::CoincidentPointCurve { point, curve } => {
203 let cx = get(params, curve.center().x);
204 let cy = get(params, curve.center().y);
205 let ex = get(params, point.x) - cx;
206 let ey = get(params, point.y) - cy;
207 let r2 = radius_squared(params, curve);
208 out[0] = ex * ex + ey * ey - r2;
209 }
210 Self::EqualLength(l1, l2) => {
211 let (d1x, d1y) = delta(params, l1);
212 let (d2x, d2y) = delta(params, l2);
213 out[0] = d1x * d1x + d1y * d1y - (d2x * d2x + d2y * d2y);
214 }
215 Self::EqualRadius(a, b) => {
216 out[0] = radius_squared(params, a) - radius_squared(params, b);
217 }
218 Self::LinearDistance { a, b, value_mm } => {
219 let dx = get(params, b.x) - get(params, a.x);
220 let dy = get(params, b.y) - get(params, a.y);
221 out[0] = dx * dx + dy * dy - value_mm * value_mm;
222 }
223 Self::AngularBetweenLines { a, b, angle_rad } => {
224 let (d1x, d1y) = delta(params, a);
225 let (d2x, d2y) = delta(params, b);
226 let dot = d1x * d2x + d1y * d2y;
227 let cross = d1x * d2y - d1y * d2x;
228 let (s, c) = angle_rad.sin_cos();
229 out[0] = s * dot - c * cross;
230 }
231 Self::RadiusCurve { curve, value_mm } => {
232 out[0] = radius_squared(params, curve) - value_mm * value_mm;
233 }
234 Self::Symmetric { a, b, axis } => {
235 let (ax, ay) = (get(params, a.x), get(params, a.y));
236 let (bx, by) = (get(params, b.x), get(params, b.y));
237 let (dx, dy) = delta(params, axis);
238 let lax = get(params, axis.a.x);
239 let lay = get(params, axis.a.y);
240 out[0] = (bx - ax) * dx + (by - ay) * dy;
241 let mx = 0.5 * (ax + bx) - lax;
242 let my = 0.5 * (ay + by) - lay;
243 out[1] = mx * dy - my * dx;
244 }
245 }
246 }
247
248 pub fn jacobian(&self, params: &[f64], row: ResidualIndex, sink: &mut Vec<Triplet>) {
249 match *self {
250 Self::Pin { param, .. } => sink.push((row, param, 1.0)),
251 Self::Horizontal(l) => {
252 sink.push((row, l.a.y, 1.0));
253 sink.push((row, l.b.y, -1.0));
254 }
255 Self::Vertical(l) => {
256 sink.push((row, l.a.x, 1.0));
257 sink.push((row, l.b.x, -1.0));
258 }
259 Self::Parallel(l1, l2) => jacobian_parallel(params, row, l1, l2, sink),
260 Self::Perpendicular(l1, l2) => jacobian_perpendicular(params, row, l1, l2, sink),
261 Self::TangentLineCurve { line, curve } => {
262 tangent_line_curve_jacobian(params, row, line, curve, sink);
263 }
264 Self::TangentCurveCurve { a, b } => {
265 tangent_curve_curve_jacobian(params, row, a, b, sink);
266 }
267 Self::CoincidentPointPoint(p, q) => {
268 let row2 = row.next();
269 sink.push((row, p.x, 1.0));
270 sink.push((row, q.x, -1.0));
271 sink.push((row2, p.y, 1.0));
272 sink.push((row2, q.y, -1.0));
273 }
274 Self::CoincidentPointLine { point, line } => {
275 jacobian_coincident_point_line(params, row, point, line, sink);
276 }
277 Self::MidpointPointLine { point, line } => {
278 let row2 = row.next();
279 sink.push((row, point.x, 1.0));
280 sink.push((row, line.a.x, -0.5));
281 sink.push((row, line.b.x, -0.5));
282 sink.push((row2, point.y, 1.0));
283 sink.push((row2, line.a.y, -0.5));
284 sink.push((row2, line.b.y, -0.5));
285 }
286 Self::CoincidentPointCurve { point, curve } => {
287 jacobian_coincident_point_curve(params, row, point, curve, sink);
288 }
289 Self::EqualLength(l1, l2) => jacobian_equal_length(params, row, l1, l2, sink),
290 Self::EqualRadius(a, b) => {
291 add_radius_squared_jacobian(params, row, a, sink);
292 subtract_radius_squared_jacobian(params, row, b, sink);
293 }
294 Self::LinearDistance { a, b, .. } => {
295 jacobian_linear_distance(params, row, a, b, sink);
296 }
297 Self::AngularBetweenLines { a, b, angle_rad } => {
298 jacobian_angular_between_lines(params, row, a, b, angle_rad, sink);
299 }
300 Self::RadiusCurve { curve, .. } => {
301 add_radius_squared_jacobian(params, row, curve, sink);
302 }
303 Self::Symmetric { a, b, axis } => {
304 jacobian_symmetric(params, row, a, b, axis, sink);
305 }
306 }
307 }
308}
309
310fn jacobian_symmetric(
311 params: &[f64],
312 row: ResidualIndex,
313 a: PointHandle,
314 b: PointHandle,
315 axis: LineHandle,
316 sink: &mut Vec<Triplet>,
317) {
318 let (dx, dy) = delta(params, axis);
319 let (ax, ay) = (get(params, a.x), get(params, a.y));
320 let (bx, by) = (get(params, b.x), get(params, b.y));
321 let lax = get(params, axis.a.x);
322 let lay = get(params, axis.a.y);
323 let mx = 0.5 * (ax + bx) - lax;
324 let my = 0.5 * (ay + by) - lay;
325 let bxa_x = bx - ax;
326 let bxa_y = by - ay;
327 sink.push((row, a.x, -dx));
328 sink.push((row, a.y, -dy));
329 sink.push((row, b.x, dx));
330 sink.push((row, b.y, dy));
331 sink.push((row, axis.a.x, -bxa_x));
332 sink.push((row, axis.a.y, -bxa_y));
333 sink.push((row, axis.b.x, bxa_x));
334 sink.push((row, axis.b.y, bxa_y));
335 let row2 = row.next();
336 sink.push((row2, a.x, 0.5 * dy));
337 sink.push((row2, a.y, -0.5 * dx));
338 sink.push((row2, b.x, 0.5 * dy));
339 sink.push((row2, b.y, -0.5 * dx));
340 sink.push((row2, axis.a.x, -dy + my));
341 sink.push((row2, axis.a.y, -mx + dx));
342 sink.push((row2, axis.b.x, -my));
343 sink.push((row2, axis.b.y, mx));
344}
345
346fn jacobian_parallel(
347 params: &[f64],
348 row: ResidualIndex,
349 l1: LineHandle,
350 l2: LineHandle,
351 sink: &mut Vec<Triplet>,
352) {
353 let (d1x, d1y) = delta(params, l1);
354 let (d2x, d2y) = delta(params, l2);
355 sink.push((row, l1.a.x, -d2y));
356 sink.push((row, l1.b.x, d2y));
357 sink.push((row, l1.a.y, d2x));
358 sink.push((row, l1.b.y, -d2x));
359 sink.push((row, l2.a.x, d1y));
360 sink.push((row, l2.b.x, -d1y));
361 sink.push((row, l2.a.y, -d1x));
362 sink.push((row, l2.b.y, d1x));
363}
364
365fn jacobian_perpendicular(
366 params: &[f64],
367 row: ResidualIndex,
368 l1: LineHandle,
369 l2: LineHandle,
370 sink: &mut Vec<Triplet>,
371) {
372 let (d1x, d1y) = delta(params, l1);
373 let (d2x, d2y) = delta(params, l2);
374 sink.push((row, l1.a.x, -d2x));
375 sink.push((row, l1.b.x, d2x));
376 sink.push((row, l1.a.y, -d2y));
377 sink.push((row, l1.b.y, d2y));
378 sink.push((row, l2.a.x, -d1x));
379 sink.push((row, l2.b.x, d1x));
380 sink.push((row, l2.a.y, -d1y));
381 sink.push((row, l2.b.y, d1y));
382}
383
384fn jacobian_coincident_point_line(
385 params: &[f64],
386 row: ResidualIndex,
387 point: PointHandle,
388 line: LineHandle,
389 sink: &mut Vec<Triplet>,
390) {
391 let (dx, dy) = delta(params, line);
392 let ex = get(params, point.x) - get(params, line.a.x);
393 let ey = get(params, point.y) - get(params, line.a.y);
394 sink.push((row, point.x, dy));
395 sink.push((row, point.y, -dx));
396 sink.push((row, line.a.x, ey - dy));
397 sink.push((row, line.a.y, dx - ex));
398 sink.push((row, line.b.x, -ey));
399 sink.push((row, line.b.y, ex));
400}
401
402fn jacobian_coincident_point_curve(
403 params: &[f64],
404 row: ResidualIndex,
405 point: PointHandle,
406 curve: CurveRadius,
407 sink: &mut Vec<Triplet>,
408) {
409 let center = curve.center();
410 let ex = get(params, point.x) - get(params, center.x);
411 let ey = get(params, point.y) - get(params, center.y);
412 sink.push((row, point.x, 2.0 * ex));
413 sink.push((row, point.y, 2.0 * ey));
414 sink.push((row, center.x, -2.0 * ex));
415 sink.push((row, center.y, -2.0 * ey));
416 subtract_radius_squared_jacobian(params, row, curve, sink);
417}
418
419fn jacobian_equal_length(
420 params: &[f64],
421 row: ResidualIndex,
422 l1: LineHandle,
423 l2: LineHandle,
424 sink: &mut Vec<Triplet>,
425) {
426 let (d1x, d1y) = delta(params, l1);
427 let (d2x, d2y) = delta(params, l2);
428 sink.push((row, l1.a.x, -2.0 * d1x));
429 sink.push((row, l1.b.x, 2.0 * d1x));
430 sink.push((row, l1.a.y, -2.0 * d1y));
431 sink.push((row, l1.b.y, 2.0 * d1y));
432 sink.push((row, l2.a.x, 2.0 * d2x));
433 sink.push((row, l2.b.x, -2.0 * d2x));
434 sink.push((row, l2.a.y, 2.0 * d2y));
435 sink.push((row, l2.b.y, -2.0 * d2y));
436}
437
438fn jacobian_linear_distance(
439 params: &[f64],
440 row: ResidualIndex,
441 a: PointHandle,
442 b: PointHandle,
443 sink: &mut Vec<Triplet>,
444) {
445 let dx = get(params, b.x) - get(params, a.x);
446 let dy = get(params, b.y) - get(params, a.y);
447 sink.push((row, a.x, -2.0 * dx));
448 sink.push((row, a.y, -2.0 * dy));
449 sink.push((row, b.x, 2.0 * dx));
450 sink.push((row, b.y, 2.0 * dy));
451}
452
453fn jacobian_angular_between_lines(
454 params: &[f64],
455 row: ResidualIndex,
456 a: LineHandle,
457 b: LineHandle,
458 angle_rad: f64,
459 sink: &mut Vec<Triplet>,
460) {
461 let (d1x, d1y) = delta(params, a);
462 let (d2x, d2y) = delta(params, b);
463 let (sin_t, cos_t) = angle_rad.sin_cos();
464 let d1_partial_x = sin_t * d2x - cos_t * d2y;
465 let d1_partial_y = sin_t * d2y + cos_t * d2x;
466 let d2_partial_x = sin_t * d1x + cos_t * d1y;
467 let d2_partial_y = sin_t * d1y - cos_t * d1x;
468 sink.push((row, a.a.x, -d1_partial_x));
469 sink.push((row, a.b.x, d1_partial_x));
470 sink.push((row, a.a.y, -d1_partial_y));
471 sink.push((row, a.b.y, d1_partial_y));
472 sink.push((row, b.a.x, -d2_partial_x));
473 sink.push((row, b.b.x, d2_partial_x));
474 sink.push((row, b.a.y, -d2_partial_y));
475 sink.push((row, b.b.y, d2_partial_y));
476}
477
478fn line_params(l: LineHandle) -> Vec<ParameterIndex> {
479 vec![l.a.x, l.a.y, l.b.x, l.b.y]
480}
481
482fn point_params(p: PointHandle) -> Vec<ParameterIndex> {
483 vec![p.x, p.y]
484}
485
486fn curve_params(c: CurveRadius) -> Vec<ParameterIndex> {
487 match c {
488 CurveRadius::Explicit { center, radius } => vec![center.x, center.y, radius],
489 CurveRadius::FromSpoke { center, spoke } => vec![center.x, center.y, spoke.x, spoke.y],
490 }
491}
492
493fn get(params: &[f64], idx: ParameterIndex) -> f64 {
494 params[idx.as_usize()]
495}
496
497fn delta(params: &[f64], line: LineHandle) -> (f64, f64) {
498 (
499 get(params, line.b.x) - get(params, line.a.x),
500 get(params, line.b.y) - get(params, line.a.y),
501 )
502}
503
504fn radius_squared(params: &[f64], curve: CurveRadius) -> f64 {
505 match curve {
506 CurveRadius::Explicit { radius, .. } => {
507 let r = get(params, radius);
508 r * r
509 }
510 CurveRadius::FromSpoke { center, spoke } => {
511 let dx = get(params, spoke.x) - get(params, center.x);
512 let dy = get(params, spoke.y) - get(params, center.y);
513 dx * dx + dy * dy
514 }
515 }
516}
517
518fn add_radius_squared_jacobian(
519 params: &[f64],
520 row: ResidualIndex,
521 curve: CurveRadius,
522 sink: &mut Vec<Triplet>,
523) {
524 match curve {
525 CurveRadius::Explicit { radius, .. } => {
526 sink.push((row, radius, 2.0 * get(params, radius)));
527 }
528 CurveRadius::FromSpoke { center, spoke } => {
529 let dx = get(params, spoke.x) - get(params, center.x);
530 let dy = get(params, spoke.y) - get(params, center.y);
531 sink.push((row, spoke.x, 2.0 * dx));
532 sink.push((row, spoke.y, 2.0 * dy));
533 sink.push((row, center.x, -2.0 * dx));
534 sink.push((row, center.y, -2.0 * dy));
535 }
536 }
537}
538
539fn subtract_radius_squared_jacobian(
540 params: &[f64],
541 row: ResidualIndex,
542 curve: CurveRadius,
543 sink: &mut Vec<Triplet>,
544) {
545 match curve {
546 CurveRadius::Explicit { radius, .. } => {
547 sink.push((row, radius, -2.0 * get(params, radius)));
548 }
549 CurveRadius::FromSpoke { center, spoke } => {
550 let dx = get(params, spoke.x) - get(params, center.x);
551 let dy = get(params, spoke.y) - get(params, center.y);
552 sink.push((row, spoke.x, -2.0 * dx));
553 sink.push((row, spoke.y, -2.0 * dy));
554 sink.push((row, center.x, 2.0 * dx));
555 sink.push((row, center.y, 2.0 * dy));
556 }
557 }
558}
559
560fn tangent_line_curve_jacobian(
561 params: &[f64],
562 row: ResidualIndex,
563 line: LineHandle,
564 curve: CurveRadius,
565 sink: &mut Vec<Triplet>,
566) {
567 let (dx, dy) = delta(params, line);
568 let cx = get(params, curve.center().x);
569 let cy = get(params, curve.center().y);
570 let ex = cx - get(params, line.a.x);
571 let ey = cy - get(params, line.a.y);
572 let cross = ex * dy - ey * dx;
573 let len2 = dx * dx + dy * dy;
574 let r2 = radius_squared(params, curve);
575 let two_cross = 2.0 * cross;
576
577 sink.push((
578 row,
579 line.a.x,
580 two_cross * (cy - get(params, line.b.y)) + r2 * 2.0 * dx,
581 ));
582 sink.push((
583 row,
584 line.a.y,
585 two_cross * (get(params, line.b.x) - cx) + r2 * 2.0 * dy,
586 ));
587 sink.push((
588 row,
589 line.b.x,
590 two_cross * (get(params, line.a.y) - cy) - r2 * 2.0 * dx,
591 ));
592 sink.push((
593 row,
594 line.b.y,
595 two_cross * (cx - get(params, line.a.x)) - r2 * 2.0 * dy,
596 ));
597 sink.push((row, curve.center().x, two_cross * dy));
598 sink.push((row, curve.center().y, -two_cross * dx));
599
600 let neg_len2 = -len2;
601 match curve {
602 CurveRadius::Explicit { radius, .. } => {
603 sink.push((row, radius, neg_len2 * 2.0 * get(params, radius)));
604 }
605 CurveRadius::FromSpoke { center, spoke } => {
606 let sdx = get(params, spoke.x) - get(params, center.x);
607 let sdy = get(params, spoke.y) - get(params, center.y);
608 sink.push((row, spoke.x, neg_len2 * 2.0 * sdx));
609 sink.push((row, spoke.y, neg_len2 * 2.0 * sdy));
610 sink.push((row, center.x, neg_len2 * -2.0 * sdx));
611 sink.push((row, center.y, neg_len2 * -2.0 * sdy));
612 }
613 }
614}
615
616fn tangent_curve_curve_jacobian(
617 params: &[f64],
618 row: ResidualIndex,
619 a: CurveRadius,
620 b: CurveRadius,
621 sink: &mut Vec<Triplet>,
622) {
623 let ra = radius_squared(params, a).sqrt();
624 let rb = radius_squared(params, b).sqrt();
625 let rsum = ra + rb;
626 let u = get(params, a.center().x) - get(params, b.center().x);
627 let v = get(params, a.center().y) - get(params, b.center().y);
628
629 sink.push((row, a.center().x, 2.0 * u));
630 sink.push((row, a.center().y, 2.0 * v));
631 sink.push((row, b.center().x, -2.0 * u));
632 sink.push((row, b.center().y, -2.0 * v));
633
634 let coef = -2.0 * rsum;
635 push_radius_linear_jacobian(params, row, a, coef, sink);
636 push_radius_linear_jacobian(params, row, b, coef, sink);
637}
638
639fn push_radius_linear_jacobian(
640 params: &[f64],
641 row: ResidualIndex,
642 curve: CurveRadius,
643 coef: f64,
644 sink: &mut Vec<Triplet>,
645) {
646 if coef == 0.0 {
647 return;
648 }
649 match curve {
650 CurveRadius::Explicit { radius, .. } => {
651 sink.push((row, radius, coef));
652 }
653 CurveRadius::FromSpoke { center, spoke } => {
654 let dx = get(params, spoke.x) - get(params, center.x);
655 let dy = get(params, spoke.y) - get(params, center.y);
656 let len = (dx * dx + dy * dy).sqrt();
657 if len == 0.0 {
658 return;
659 }
660 let k = coef / len;
661 sink.push((row, spoke.x, k * dx));
662 sink.push((row, spoke.y, k * dy));
663 sink.push((row, center.x, -k * dx));
664 sink.push((row, center.y, -k * dy));
665 }
666 }
667}