webarkitlib_rs/
ar2.rs

1/*
2 *  ar2.rs
3 *  WebARKitLib-rs
4 *
5 *  This file is part of WebARKitLib-rs - WebARKit.
6 *
7 *  WebARKitLib-rs is free software: you can redistribute it and/or modify
8 *  it under the terms of the GNU Lesser General Public License as published by
9 *  the Free Software Foundation, either version 3 of the License, or
10 *  (at your option) any later version.
11 *
12 *  WebARKitLib-rs is distributed in the hope that it will be useful,
13 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 *  GNU Lesser General Public License for more details.
16 *
17 *  You should have received a copy of the GNU Lesser General Public License
18 *  along with WebARKitLib-rs.  If not, see <http://www.gnu.org/licenses/>.
19 *
20 *  As a special exception, the copyright holders of this library give you
21 *  permission to link this library with independent modules to produce an
22 *  executable, regardless of the license terms of these independent modules, and to
23 *  copy and distribute the resulting executable under terms of your choice,
24 *  provided that you also meet, for each linked independent module, the terms and
25 *  conditions of the license of that module. An independent module is a module
26 *  which is neither derived from nor based on this library. If you modify this
27 *  library, you may extend this exception to your version of the library, but you
28 *  are not obligated to do so. If you do not wish to do so, delete this exception
29 *  statement from your version.
30 *
31 *  Copyright 2026 WebARKit.
32 *
33 *  Author(s): Walter Perdan @kalwalt https://github.com/kalwalt
34 *
35 */
36
37//! AR2 Robust Tracking Module
38//! Ported and idiomatic Rust implementation of ARToolKit's AR2 NFT tracking logic.
39//!
40//! Focus: 2D region tracking and feature matching for robust pose estimation.
41
42use crate::types::{ARParamLT, ARdouble, ARPixelFormat};
43use crate::icp::ICPHandleT;
44
45pub const AR2_TRACKING_6DOF: i32 = 1;
46pub const AR2_TRACKING_HOMOGRAPHY: i32 = 2;
47
48pub const AR2_SEARCH_FEATURE_MAX: usize = 500;
49pub const AR2_TRACKING_CANDIDATE_MAX: usize = 1000;
50pub const AR2_TRACKING_SURFACE_MAX: usize = 10;
51pub const AR2_THREAD_MAX: usize = 8; // Adjust as needed for Rust safety/concurrency
52pub const AR2_BLUR_IMAGE_MAX: usize = 5;
53pub const AR2_TEMP_SCALE: i32 = 1;
54pub const AR2_TEMPLATE_NULL_PIXEL: u16 = 0xFFFF;
55
56#[derive(Debug, Clone, Copy, PartialEq)]
57pub struct AR2TemplateCandidate {
58    pub snum: i32,
59    pub level: i32,
60    pub num: i32,
61    pub flag: i32, // -1: end of list, 0: valid, 1: selected
62    pub sx: f32,
63    pub sy: f32,
64}
65
66impl Default for AR2TemplateCandidate {
67    fn default() -> Self {
68        Self {
69            snum: 0,
70            level: 0,
71            num: 0,
72            flag: -1,
73            sx: 0.0,
74            sy: 0.0,
75        }
76    }
77}
78
79#[derive(Debug, Default, Clone)]
80pub struct AR2Image {
81    pub img_bw_blur: Vec<Option<Vec<u8>>>,
82    pub xsize: i32,
83    pub ysize: i32,
84    pub dpi: f32,
85}
86
87#[derive(Debug, Default, Clone)]
88pub struct AR2ImageSet {
89    pub scale: Vec<AR2Image>,
90}
91
92#[derive(Debug, Default, Clone)]
93pub struct AR2FeatureCoord {
94    pub x: i32,
95    pub y: i32,
96    pub mx: f32,
97    pub my: f32,
98    pub max_sim: f32,
99}
100
101#[derive(Debug, Default, Clone)]
102pub struct AR2FeaturePoints {
103    pub coord: Vec<AR2FeatureCoord>,
104    pub scale: i32,
105    pub maxdpi: f32,
106    pub mindpi: f32,
107}
108
109#[derive(Debug, Default, Clone)]
110pub struct AR2FeatureSet {
111    pub list: Vec<AR2FeaturePoints>,
112}
113
114#[derive(Debug, Default, Clone)]
115pub struct AR2Template {
116    pub xts1: i32,
117    pub xts2: i32,
118    pub yts1: i32,
119    pub yts2: i32,
120    pub xsize: i32,
121    pub ysize: i32,
122    pub img1: Vec<u16>,
123    pub vlen: i32,
124    pub sum: i32,
125    pub valid_num: usize,
126}
127
128// MarkerSet is ignored for now per user rules (no multi-marker / complex markers yet)
129// but we might need a placeholder if it's tightly coupled.
130
131#[derive(Debug, Default, Clone)]
132pub struct AR2Surface {
133    pub image_set: Option<AR2ImageSet>,
134    pub feature_set: Option<AR2FeatureSet>,
135    pub trans: [[f32; 4]; 3],
136    pub itrans: [[f32; 4]; 3],
137}
138
139#[derive(Debug, Clone)]
140pub struct AR2SurfaceSet {
141    pub surface: Vec<AR2Surface>,
142    pub trans1: [[f32; 4]; 3],
143    pub trans2: [[f32; 4]; 3],
144    pub trans3: [[f32; 4]; 3],
145    pub cont_num: i32,
146    pub prev_feature: Vec<AR2TemplateCandidate>,
147}
148
149impl Default for AR2SurfaceSet {
150    fn default() -> Self {
151        Self {
152            surface: Vec::new(),
153            trans1: [[0.0; 4]; 3],
154            trans2: [[0.0; 4]; 3],
155            trans3: [[0.0; 4]; 3],
156            cont_num: 0,
157            prev_feature: vec![AR2TemplateCandidate::default(); AR2_SEARCH_FEATURE_MAX + 1],
158        }
159    }
160}
161
162pub struct AR2Handle {
163    pub tracking_mode: i32,
164    pub xsize: i32,
165    pub ysize: i32,
166    pub cparam_lt: *mut ARParamLT, // Pointer for now consistent with C API, or Box?
167    pub icp_handle: *mut ICPHandleT,
168    pub pix_format: ARPixelFormat,
169    
170    pub blur_method: i32,
171    pub blur_level: i32,
172    pub search_size: i32,
173    pub template_size1: i32,
174    pub template_size2: i32,
175    pub search_feature_num: i32,
176    pub sim_thresh: f32,
177    pub tracking_thresh: f32,
178    
179    pub wtrans1: Vec<[[f32; 4]; 3]>,
180    pub wtrans2: Vec<[[f32; 4]; 3]>,
181    pub wtrans3: Vec<[[f32; 4]; 3]>,
182    
183    pub pos: Vec<[f32; 2]>,
184    pub pos2d: Vec<[f32; 2]>,
185    pub pos3d: Vec<[f32; 3]>,
186    
187    pub candidate: Vec<AR2TemplateCandidate>,
188    pub candidate2: Vec<AR2TemplateCandidate>,
189    pub used_feature: Vec<AR2TemplateCandidate>,
190    
191    pub templ: Vec<AR2Template>, // For sequential tracking
192    pub mf_image: Vec<u8>,
193
194    pub thread_num: i32,
195}
196
197impl AR2Handle {
198    pub fn new(xsize: i32, ysize: i32, pix_format: ARPixelFormat) -> Self {
199        Self {
200            tracking_mode: AR2_TRACKING_6DOF,
201            xsize,
202            ysize,
203            cparam_lt: std::ptr::null_mut(),
204            icp_handle: std::ptr::null_mut(),
205            pix_format,
206            blur_method: 0,
207            blur_level: 1,
208            search_size: 6,
209            template_size1: 16,
210            template_size2: 16,
211            search_feature_num: 10,
212            sim_thresh: 0.5,
213            tracking_thresh: 2.0,
214            
215            wtrans1: vec![[[0.0; 4]; 3]; AR2_TRACKING_SURFACE_MAX],
216            wtrans2: vec![[[0.0; 4]; 3]; AR2_TRACKING_SURFACE_MAX],
217            wtrans3: vec![[[0.0; 4]; 3]; AR2_TRACKING_SURFACE_MAX],
218            
219            pos: vec![[0.0; 2]; AR2_SEARCH_FEATURE_MAX + AR2_THREAD_MAX],
220            pos2d: vec![[0.0; 2]; AR2_SEARCH_FEATURE_MAX],
221            pos3d: vec![[0.0; 3]; AR2_SEARCH_FEATURE_MAX],
222            
223            candidate: vec![AR2TemplateCandidate::default(); AR2_TRACKING_CANDIDATE_MAX + 1],
224            candidate2: vec![AR2TemplateCandidate::default(); AR2_TRACKING_CANDIDATE_MAX + 1],
225            used_feature: vec![AR2TemplateCandidate::default(); AR2_SEARCH_FEATURE_MAX],
226            
227            templ: vec![AR2Template {
228                xts1: 16, xts2: 16, yts1: 16, yts2: 16,
229                xsize: 33, ysize: 33,
230                ..Default::default()
231            }; AR2_THREAD_MAX],
232            mf_image: vec![0; (xsize * ysize) as usize],
233
234            thread_num: 1,
235        }
236    }
237}
238
239pub fn marker_coord_to_screen_coord(
240    cparam_lt: Option<&ARParamLT>,
241    trans: &[[f32; 4]; 3],
242    mx: f32,
243    my: f32,
244) -> Result<(f32, f32), &'static str> {
245    let (ix, iy) = if let Some(lt) = cparam_lt {
246        let mut wtrans = [[0.0f32; 4]; 3];
247        crate::math::mat_mul_dff(&lt.param.mat, trans, &mut wtrans);
248        
249        let hx = wtrans[0][0] * mx + wtrans[0][1] * my + wtrans[0][3];
250        let hy = wtrans[1][0] * mx + wtrans[1][1] * my + wtrans[1][3];
251        let h = wtrans[2][0] * mx + wtrans[2][1] * my + wtrans[2][3];
252        (hx / h, hy / h)
253    } else {
254        let hx = trans[0][0] * mx + trans[0][1] * my + trans[0][3];
255        let hy = trans[1][0] * mx + trans[1][1] * my + trans[1][3];
256        let h = trans[2][0] * mx + trans[2][1] * my + trans[2][3];
257        return Ok((hx / h, hy / h));
258    };
259
260    if let Some(lt) = cparam_lt {
261        let (sx, sy) = lt.param_ltf.ideal2observ(ix, iy)?;
262        Ok((sx, sy))
263    } else {
264        Ok((ix, iy))
265    }
266}
267
268pub fn marker_coord_to_screen_coord2(
269    cparam_lt: Option<&ARParamLT>,
270    trans: &[[f32; 4]; 3],
271    mx: f32,
272    my: f32,
273) -> Result<(f32, f32), &'static str> {
274    let (sx, sy) = marker_coord_to_screen_coord(cparam_lt, trans, mx, my)?;
275    
276    if let Some(lt) = cparam_lt {
277        let (ix1, iy1) = lt.param_ltf.observ2ideal(sx, sy)?;
278        // Original C code does it for verification but we might just return it
279        // if( (ix-ix1)*(ix-ix1) + (iy-iy1)*(iy-iy1) > 1.0F ) return -1;
280        // In Rust we trust the LT for now or re-project?
281        // Let's stick to the C logic if ix, iy are needed
282        let (ix, iy) = {
283            let mut wtrans = [[0.0f32; 4]; 3];
284            crate::math::mat_mul_dff(&lt.param.mat, trans, &mut wtrans);
285            let hx = wtrans[0][0] * mx + wtrans[0][1] * my + wtrans[0][3];
286            let hy = wtrans[1][0] * mx + wtrans[1][1] * my + wtrans[1][3];
287            let h = wtrans[2][0] * mx + wtrans[2][1] * my + wtrans[2][3];
288            (hx / h, hy / h)
289        };
290        if (ix - ix1).powi(2) + (iy - iy1).powi(2) > 1.0 {
291            return Err("Reprojection error too high");
292        }
293    }
294    
295    Ok((sx, sy))
296}
297
298pub fn ar2_screen_coord_2_marker_coord(
299    cparam_lt: Option<&ARParamLT>,
300    trans: &[[f32; 4]; 3],
301    sx: f32,
302    sy: f32,
303) -> Result<(f32, f32), &'static str> {
304    if let Some(lt) = cparam_lt {
305        let (ix, iy) = lt.param_ltf.observ2ideal(sx, sy)?;
306        let mut wtrans = [[0.0f32; 4]; 3];
307        crate::math::mat_mul_dff(&lt.param.mat, trans, &mut wtrans);
308        
309        let c11 = wtrans[2][0] * ix - wtrans[0][0];
310        let c12 = wtrans[2][1] * ix - wtrans[0][1];
311        let c21 = wtrans[2][0] * iy - wtrans[1][0];
312        let c22 = wtrans[2][1] * iy - wtrans[1][1];
313        let b1 = wtrans[0][3] - wtrans[2][3] * ix;
314        let b2 = wtrans[1][3] - wtrans[2][3] * iy;
315        
316        let m = c11 * c22 - c12 * c21;
317        if m.abs() < 1e-10 { return Err("Singular matrix in coord conversion"); }
318        
319        let mx = (b1 * c22 - b2 * c12) / m;
320        let my = (c11 * b2 - c21 * b1) / m;
321        Ok((mx, my))
322    } else {
323        let c11 = trans[2][0] * sx - trans[0][0];
324        let c12 = trans[2][1] * sx - trans[0][1];
325        let c21 = trans[2][0] * sy - trans[1][0];
326        let c22 = trans[2][1] * sy - trans[1][1];
327        let b1 = trans[0][3] - trans[2][3] * sx;
328        let b2 = trans[1][3] - trans[2][3] * sy;
329        
330        let m = c11 * c22 - c12 * c21;
331        if m.abs() < 1e-10 { return Err("Singular matrix in coord conversion"); }
332        
333        let mx = (b1 * c22 - b2 * c12) / m;
334        let my = (c11 * b2 - c21 * b1) / m;
335        Ok((mx, my))
336    }
337}
338
339pub fn ar2_marker_coord_2_image_coord(
340    _xsize: i32,
341    ysize: i32,
342    dpi: f32,
343    mx: f32,
344    my: f32,
345) -> (f32, f32) {
346    let iix = mx * dpi / 25.4;
347    let iiy = (ysize as f32) - my * dpi / 25.4;
348    (iix, iiy)
349}
350
351pub fn ar2_get_image_value(
352    cparam_lt: Option<&ARParamLT>,
353    trans: &[[f32; 4]; 3],
354    image: &AR2Image,
355    sx: f32,
356    sy: f32,
357    blur_level: i32,
358) -> Result<u8, &'static str> {
359    let (mx, my) = ar2_screen_coord_2_marker_coord(cparam_lt, trans, sx, sy)?;
360    let (iix, iiy) = ar2_marker_coord_2_image_coord(image.xsize, image.ysize, image.dpi, mx, my);
361    
362    let ix = (iix + 0.5) as i32;
363    let iy = (iiy + 0.5) as i32;
364    
365    if ix < 0 || ix >= image.xsize || iy < 0 || iy >= image.ysize {
366        return Err("Coord out of image bounds");
367    }
368    
369    if let Some(blur_vec) = &image.img_bw_blur[blur_level as usize] {
370        Ok(blur_vec[(iy * image.xsize + ix) as usize])
371    } else {
372        Err("Blur level not available")
373    }
374}
375
376pub fn ar2_set_template_sub(
377    cparam_lt: Option<&ARParamLT>,
378    trans: &[[f32; 4]; 3],
379    image_set: &AR2ImageSet,
380    feature_points: &AR2FeaturePoints,
381    num: usize,
382    blur_level: i32,
383    templ: &mut AR2Template,
384) -> Result<(), &'static str> {
385    let mx = feature_points.coord[num].mx;
386    let my = feature_points.coord[num].my;
387    
388    let (sx, sy) = marker_coord_to_screen_coord(cparam_lt, trans, mx, my)?;
389    let ix = (sx + 0.5) as i32;
390    let iy = (sy + 0.5) as i32;
391    
392    let mut sum = 0;
393    let mut sum2 = 0;
394    let mut k = 0;
395    
396    templ.img1.clear();
397    
398    let image = &image_set.scale[feature_points.scale as usize];
399    
400    for j in -templ.yts1..=templ.yts2 {
401        let iy2 = iy + j * AR2_TEMP_SCALE;
402        for i in -templ.xts1..=templ.xts2 {
403            let ix2 = ix + i * AR2_TEMP_SCALE;
404            
405            let res = if let Some(lt) = cparam_lt {
406                match lt.param_ltf.observ2ideal(ix2 as f32, iy2 as f32) {
407                    Ok((isx, isy)) => ar2_get_image_value(cparam_lt, trans, image, isx, isy, blur_level),
408                    Err(_) => Err("Ideal error"),
409                }
410            } else {
411                ar2_get_image_value(cparam_lt, trans, image, ix2 as f32, iy2 as f32, blur_level)
412            };
413            
414            match res {
415                Ok(pixel) => {
416                    templ.img1.push(pixel as u16);
417                    sum += pixel as i32;
418                    sum2 += (pixel as i32) * (pixel as i32);
419                    k += 1;
420                }
421                Err(_) => {
422                    templ.img1.push(AR2_TEMPLATE_NULL_PIXEL);
423                }
424            }
425        }
426    }
427    
428    if k == 0 { return Err("No valid pixels in template"); }
429    
430    let vlen = (sum2 - sum * sum / (k as i32)) as f32;
431    templ.vlen = vlen.sqrt() as i32;
432    templ.sum = sum;
433    templ.valid_num = k;
434    
435    Ok(())
436}
437
438pub fn get_resolution(
439    cparam_lt: Option<&ARParamLT>,
440    trans: &[[f32; 4]; 3],
441    pos: [f32; 2],
442) -> Result<[f32; 2], &'static str> {
443    let cparam = cparam_lt.map(|lt| &lt.param);
444    get_resolution2(cparam, trans, pos)
445}
446
447pub fn get_resolution2(
448    cparam: Option<&crate::types::ARParam>,
449    trans: &[[f32; 4]; 3],
450    pos: [f32; 2],
451) -> Result<[f32; 2], &'static str> {
452    let mut mat = [[0.0f32; 4]; 3];
453    let (x0, y0, x1, y1, x2, y2);
454
455    if let Some(cp) = cparam {
456        crate::math::mat_mul_dff(&cp.mat, trans, &mut mat);
457        
458        let project = |px, py| {
459            let hx = mat[0][0] * px + mat[0][1] * py + mat[0][3];
460            let hy = mat[1][0] * px + mat[1][1] * py + mat[1][3];
461            let h = mat[2][0] * px + mat[2][1] * py + mat[2][3];
462            (hx / h, hy / h)
463        };
464        
465        let (p0x, p0y) = project(pos[0], pos[1]);
466        x0 = p0x; y0 = p0y;
467        let (p1x, p1y) = project(pos[0] + 10.0, pos[1]);
468        x1 = p1x; y1 = p1y;
469        let (p2x, p2y) = project(pos[0], pos[1] + 10.0);
470        x2 = p2x; y2 = p2y;
471    } else {
472        let project = |px, py| {
473            let hx = trans[0][0] * px + trans[0][1] * py + trans[0][3];
474            let hy = trans[1][0] * px + trans[1][1] * py + trans[1][3];
475            let h = trans[2][0] * px + trans[2][1] * py + trans[2][3];
476            (hx / h, hy / h)
477        };
478        
479        let (p0x, p0y) = project(pos[0], pos[1]);
480        x0 = p0x; y0 = p0y;
481        let (p1x, p1y) = project(pos[0] + 10.0, pos[1]);
482        x1 = p1x; y1 = p1y;
483        let (p2x, p2y) = project(pos[0], pos[1] + 10.0);
484        x2 = p2x; y2 = p2y;
485    }
486
487    let d1 = (x1 - x0).powi(2) + (y1 - y0).powi(2);
488    let d2 = (x2 - x0).powi(2) + (y2 - y0).powi(2);
489    
490    let mut dpi = [0.0; 2];
491    if d1 < d2 {
492        dpi[0] = d2.sqrt() * 2.54;
493        dpi[1] = d1.sqrt() * 2.54;
494    } else {
495        dpi[0] = d1.sqrt() * 2.54;
496        dpi[1] = d2.sqrt() * 2.54;
497    }
498
499    Ok(dpi)
500}
501
502pub fn extract_visible_features(
503    cparam_lt: &ARParamLT,
504    wtrans1: &[[[f32; 4]; 3]],
505    surface_set: &AR2SurfaceSet,
506    candidate: &mut [AR2TemplateCandidate],
507    candidate2: &mut [AR2TemplateCandidate],
508) -> Result<(), &'static str> {
509    let xsize = cparam_lt.param.xsize;
510    let ysize = cparam_lt.param.ysize;
511
512    let mut l = 0;
513    let mut l2 = 0;
514
515    for i in 0..surface_set.surface.len() {
516        let surface = &surface_set.surface[i];
517        let feature_set = match &surface.feature_set {
518            Some(fs) => fs,
519            None => continue,
520        };
521
522        let trans2 = &wtrans1[i];
523
524        for j in 0..feature_set.list.len() {
525            let feature_points = &feature_set.list[j];
526            for k in 0..feature_points.coord.len() {
527                let coord = &feature_points.coord[k];
528
529                let (sx, sy) = match marker_coord_to_screen_coord2(
530                    Some(cparam_lt),
531                    trans2,
532                    coord.mx,
533                    coord.my,
534                ) {
535                    Ok(res) => res,
536                    Err(_) => continue,
537                };
538
539                if sx < 0.0 || sx >= xsize as f32 || sy < 0.0 || sy >= ysize as f32 {
540                    continue;
541                }
542
543                // Normal vector check (backface culling)
544                let vdir0 = trans2[0][0] * coord.mx + trans2[0][1] * coord.my + trans2[0][3];
545                let vdir1 = trans2[1][0] * coord.mx + trans2[1][1] * coord.my + trans2[1][3];
546                let vdir2 = trans2[2][0] * coord.mx + trans2[2][1] * coord.my + trans2[2][3];
547                let vlen = (vdir0 * vdir0 + vdir1 * vdir1 + vdir2 * vdir2).sqrt();
548                let vd0 = vdir0 / vlen;
549                let vd1 = vdir1 / vlen;
550                let vd2 = vdir2 / vlen;
551
552                if vd0 * trans2[0][2] + vd1 * trans2[1][2] + vd2 * trans2[2][2] > -0.1 {
553                    continue;
554                }
555
556                let w = get_resolution(Some(cparam_lt), trans2, [coord.mx, coord.my])?;
557                
558                if w[1] <= feature_points.maxdpi && w[1] >= feature_points.mindpi {
559                    if l >= AR2_TRACKING_CANDIDATE_MAX {
560                        candidate[l].flag = -1;
561                        return Err("Feature candidates overflow");
562                    }
563                    candidate[l] = AR2TemplateCandidate {
564                        snum: i as i32,
565                        level: j as i32,
566                        num: k as i32,
567                        sx,
568                        sy,
569                        flag: 0,
570                    };
571                    l += 1;
572                } else if w[1] <= feature_points.maxdpi * 2.0 && w[1] >= feature_points.mindpi / 2.0 {
573                    if l2 < AR2_TRACKING_CANDIDATE_MAX {
574                        candidate2[l2] = AR2TemplateCandidate {
575                            snum: i as i32,
576                            level: j as i32,
577                            num: k as i32,
578                            sx,
579                            sy,
580                            flag: 0,
581                        };
582                        l2 += 1;
583                    }
584                }
585            }
586        }
587    }
588    candidate[l].flag = -1;
589    candidate2[l2].flag = -1;
590
591    Ok(())
592}
593
594pub fn extract_visible_features_homography(
595    xsize: i32,
596    ysize: i32,
597    wtrans1: &[[[f32; 4]; 3]],
598    surface_set: &AR2SurfaceSet,
599    candidate: &mut [AR2TemplateCandidate],
600    candidate2: &mut [AR2TemplateCandidate],
601) -> Result<(), &'static str> {
602    let mut l = 0;
603    let mut l2 = 0;
604
605    for i in 0..surface_set.surface.len() {
606        let surface = &surface_set.surface[i];
607        let feature_set = match &surface.feature_set {
608            Some(fs) => fs,
609            None => continue,
610        };
611
612        let trans2 = &wtrans1[i];
613
614        for j in 0..feature_set.list.len() {
615            let feature_points = &feature_set.list[j];
616            for k in 0..feature_points.coord.len() {
617                let coord = &feature_points.coord[k];
618
619                let (sx, sy) = match marker_coord_to_screen_coord2(
620                    None,
621                    trans2,
622                    coord.mx,
623                    coord.my,
624                ) {
625                    Ok(res) => res,
626                    Err(_) => continue,
627                };
628
629                if sx < 0.0 || sx >= xsize as f32 || sy < 0.0 || sy >= ysize as f32 {
630                    continue;
631                }
632
633                let w = get_resolution(None, trans2, [coord.mx, coord.my])?;
634                
635                if w[1] <= feature_points.maxdpi && w[1] >= feature_points.mindpi {
636                    if l >= AR2_TRACKING_CANDIDATE_MAX {
637                        candidate[l].flag = -1;
638                        return Err("Feature candidates overflow");
639                    }
640                    candidate[l] = AR2TemplateCandidate {
641                        snum: i as i32,
642                        level: j as i32,
643                        num: k as i32,
644                        sx,
645                        sy,
646                        flag: 0,
647                    };
648                    l += 1;
649                } else if w[1] <= feature_points.maxdpi * 2.0 && w[1] >= feature_points.mindpi / 2.0 {
650                    if l2 < AR2_TRACKING_CANDIDATE_MAX {
651                        candidate2[l2] = AR2TemplateCandidate {
652                            snum: i as i32,
653                            level: j as i32,
654                            num: k as i32,
655                            sx,
656                            sy,
657                            flag: 0,
658                        };
659                        l2 += 1;
660                    }
661                }
662            }
663        }
664    }
665    candidate[l].flag = -1;
666    candidate2[l2].flag = -1;
667
668    Ok(())
669}
670
671pub fn ar2_select_template(
672    candidate: &mut [AR2TemplateCandidate],
673    prev_feature: &mut [AR2TemplateCandidate],
674    num: i32,
675    pos: &mut [[f32; 2]; 4],
676    xsize: i32,
677    ysize: i32,
678) -> i32 {
679    if num < 0 {
680        return -1;
681    }
682
683    if num == 0 {
684        let mut dmax = 0.0;
685        let mut j = -1;
686        for (i, cand) in candidate.iter().enumerate() {
687            if cand.flag == -1 { break; }
688            if cand.flag != 0 { continue; }
689            if cand.sx < (xsize / 8) as f32 || cand.sx > (xsize * 7 / 8) as f32
690                || cand.sy < (ysize / 8) as f32 || cand.sy > (ysize * 7 / 8) as f32 {
691                continue;
692            }
693
694            let d = (cand.sx - (xsize / 2) as f32).powi(2) + (cand.sy - (ysize / 2) as f32).powi(2);
695            if d > dmax {
696                dmax = d;
697                j = i as i32;
698            }
699        }
700
701        if j != -1 {
702            candidate[j as usize].flag = 1;
703        }
704        return j;
705    } else if num == 1 {
706        let mut dmax = 0.0;
707        let mut j = -1;
708        for (i, cand) in candidate.iter().enumerate() {
709            if cand.flag == -1 { break; }
710            if cand.flag != 0 { continue; }
711            if cand.sx < (xsize / 8) as f32 || cand.sx > (xsize * 7 / 8) as f32
712                || cand.sy < (ysize / 8) as f32 || cand.sy > (ysize * 7 / 8) as f32 {
713                continue;
714            }
715
716            let d = (cand.sx - pos[0][0]).powi(2) + (cand.sy - pos[0][1]).powi(2);
717            if d > dmax {
718                dmax = d;
719                j = i as i32;
720            }
721        }
722
723        if j != -1 {
724            candidate[j as usize].flag = 1;
725        }
726        return j;
727    } else if num == 2 {
728        let mut dmax = 0.0;
729        let mut j = -1;
730        for (i, cand) in candidate.iter().enumerate() {
731            if cand.flag == -1 { break; }
732            if cand.flag != 0 { continue; }
733            if cand.sx < (xsize / 8) as f32 || cand.sx > (xsize * 7 / 8) as f32
734                || cand.sy < (ysize / 8) as f32 || cand.sy > (ysize * 7 / 8) as f32 {
735                continue;
736            }
737
738            let d = (cand.sx - pos[0][0]) * (pos[1][1] - pos[0][1])
739                - (cand.sy - pos[0][1]) * (pos[1][0] - pos[0][0]);
740            let d_sq = d * d;
741            if d_sq > dmax {
742                dmax = d_sq;
743                j = i as i32;
744            }
745        }
746
747        if j != -1 {
748            candidate[j as usize].flag = 1;
749        }
750        return j;
751    } else if num == 3 {
752        let (mut p2sinf, mut p2cosf) = (0.0, 0.0);
753        let (mut p3sinf, mut p3cosf) = (0.0, 0.0);
754        let (mut p4sinf, mut p4cosf) = (0.0, 0.0);
755        
756        if get_vector_angle(pos[0], pos[1], &mut p2sinf, &mut p2cosf).is_err() { return -1; }
757        if get_vector_angle(pos[0], pos[2], &mut p3sinf, &mut p3cosf).is_err() { return -1; }
758
759        let mut j = -1;
760        let mut smax = 0.0;
761        for (i, cand) in candidate.iter().enumerate() {
762            if cand.flag == -1 { break; }
763            if cand.flag != 0 { continue; }
764            if cand.sx < (xsize / 8) as f32 || cand.sx > (xsize * 7 / 8) as f32
765                || cand.sy < (ysize / 8) as f32 || cand.sy > (ysize * 7 / 8) as f32 {
766                continue;
767            }
768
769            pos[3][0] = cand.sx;
770            pos[3][1] = cand.sy;
771            if get_vector_angle(pos[0], pos[3], &mut p4sinf, &mut p4cosf).is_err() { continue; }
772
773            let q1: usize;
774            let r1: usize;
775            let r2: usize;
776
777            if (p3sinf * p2cosf - p3cosf * p2sinf >= 0.0) && (p4sinf * p2cosf - p4cosf * p2sinf >= 0.0) {
778                if p4sinf * p3cosf - p4cosf * p3sinf >= 0.0 {
779                    q1 = 1; r1 = 2; r2 = 3;
780                } else {
781                    q1 = 1; r1 = 3; r2 = 2;
782                }
783            } else if (p4sinf * p3cosf - p4cosf * p3sinf >= 0.0) && (p2sinf * p3cosf - p2cosf * p3sinf >= 0.0) {
784                if p4sinf * p2cosf - p4cosf * p2sinf >= 0.0 {
785                    q1 = 2; r1 = 1; r2 = 3;
786                } else {
787                    q1 = 2; r1 = 3; r2 = 1;
788                }
789            } else if (p2sinf * p4cosf - p2cosf * p4sinf >= 0.0) && (p3sinf * p4cosf - p3cosf * p4sinf >= 0.0) {
790                if p3sinf * p2cosf - p3cosf * p2sinf >= 0.0 {
791                    q1 = 3; r1 = 1; r2 = 2;
792                } else {
793                    q1 = 3; r1 = 2; r2 = 1;
794                }
795            } else {
796                continue;
797            }
798
799            let s = get_region_area(pos, q1, r1, r2);
800            if s > smax {
801                smax = s;
802                j = i as i32;
803            }
804        }
805
806        if j != -1 {
807            candidate[j as usize].flag = 1;
808        }
809        return j;
810    } else {
811        // Find previous feature if still valid
812        for p_cand in prev_feature.iter_mut() {
813            if p_cand.flag == -1 { break; }
814            if p_cand.flag != 0 { continue; }
815            p_cand.flag = 1;
816            
817            for (i, cand) in candidate.iter_mut().enumerate() {
818                if cand.flag == -1 { break; }
819                if cand.flag == 0 && p_cand.snum == cand.snum && p_cand.level == cand.level && p_cand.num == cand.num {
820                    cand.flag = 1;
821                    return i as i32;
822                }
823            }
824        }
825        
826        // Random fallback if no previous feature found
827        let mut available = Vec::new();
828        for (i, cand) in candidate.iter().enumerate() {
829            if cand.flag == -1 { break; }
830            if cand.flag == 0 {
831                available.push(i);
832            }
833        }
834        
835        if available.is_empty() { return -1; }
836        
837        use rand::Rng;
838        let mut rng = rand::thread_rng();
839        let idx = available[rng.gen_range(0..available.len())];
840        candidate[idx].flag = 1;
841        idx as i32
842    }
843}
844
845pub fn get_vector_angle(p1: [f32; 2], p2: [f32; 2], psinf: &mut f32, pcosf: &mut f32) -> Result<(), &'static str> {
846    let l = ((p2[0] - p1[0]).powi(2) + (p2[1] - p1[1]).powi(2)).sqrt();
847    if l == 0.0 { return Err("Zero length vector"); }
848    *psinf = (p2[1] - p1[1]) / l;
849    *pcosf = (p2[0] - p1[0]) / l;
850    Ok(())
851}
852
853pub fn get_region_area(pos: &[[f32; 2]; 4], q1: usize, r1: usize, r2: usize) -> f32 {
854    get_triangle_area(pos[0], pos[q1], pos[r1]) + get_triangle_area(pos[0], pos[r1], pos[r2])
855}
856
857pub fn get_triangle_area(p1: [f32; 2], p2: [f32; 2], p3: [f32; 2]) -> f32 {
858    let x1 = p2[0] - p1[0];
859    let y1 = p2[1] - p1[1];
860    let x2 = p3[0] - p1[0];
861    let y2 = p3[1] - p1[1];
862    ((x1 * y2 - x2 * y1) / 2.0).abs()
863}
864
865pub fn ar2_get_search_point(
866    cparam_lt: Option<&ARParamLT>,
867    trans1: Option<&[[f32; 4]; 3]>,
868    trans2: Option<&[[f32; 4]; 3]>,
869    trans3: Option<&[[f32; 4]; 3]>,
870    feature: &AR2FeatureCoord,
871    search: &mut [[i32; 2]; 3],
872) {
873    let mx = feature.mx;
874    let my = feature.my;
875
876    let ox1 = if let Some(t1) = trans1 {
877        match marker_coord_to_screen_coord(cparam_lt, t1, mx, my) {
878            Ok((x, y)) => {
879                search[0][0] = x as i32;
880                search[0][1] = y as i32;
881                Some((x, y))
882            }
883            Err(_) => None,
884        }
885    } else {
886        None
887    };
888
889    if ox1.is_none() {
890        search[0][0] = -1; search[0][1] = -1;
891        search[1][0] = -1; search[1][1] = -1;
892        search[2][0] = -1; search[2][1] = -1;
893        return;
894    }
895    let (ox1x, ox1y) = ox1.unwrap();
896
897    let ox2 = if let Some(t2) = trans2 {
898        match marker_coord_to_screen_coord(cparam_lt, t2, mx, my) {
899            Ok((x, y)) => {
900                search[1][0] = (2.0 * ox1x - x) as i32;
901                search[1][1] = (2.0 * ox1y - y) as i32;
902                Some((x, y))
903            }
904            Err(_) => None,
905        }
906    } else {
907        None
908    };
909
910    if ox2.is_none() {
911        search[1][0] = -1; search[1][1] = -1;
912        search[2][0] = -1; search[2][1] = -1;
913        return;
914    }
915    let (ox2x, ox2y) = ox2.unwrap();
916
917    if let Some(t3) = trans3 {
918        match marker_coord_to_screen_coord(cparam_lt, t3, mx, my) {
919            Ok((x, y)) => {
920                search[2][0] = (3.0 * ox1x - 3.0 * ox2x + x) as i32;
921                search[2][1] = (3.0 * ox1y - 3.0 * ox2y + y) as i32;
922            }
923            Err(_) => {
924                search[2][0] = -1;
925                search[2][1] = -1;
926            }
927        }
928    } else {
929        search[2][0] = -1;
930        search[2][1] = -1;
931    }
932}
933
934pub struct AR2Tracking2DResult {
935    pub sim: f32,
936    pub pos2d: [f32; 2],
937    pub pos3d: [f32; 3],
938}
939
940pub fn ar2_tracking_2d_sub(
941    cparam_lt: Option<&ARParamLT>,
942    pix_format: ARPixelFormat,
943    xsize: i32,
944    ysize: i32,
945    blur_level: i32,
946    search_size: i32,
947    wtrans1: &[[f32; 4]; 3],
948    wtrans2: Option<&[[f32; 4]; 3]>,
949    wtrans3: Option<&[[f32; 4]; 3]>,
950    surface_set: &AR2SurfaceSet,
951    candidate: &AR2TemplateCandidate,
952    data_ptr: &[u8],
953    mf_image: &mut [u8],
954    templ: &mut AR2Template,
955) -> Result<AR2Tracking2DResult, &'static str> {
956    let snum = candidate.snum as usize;
957    let level = candidate.level as usize;
958    let fnum = candidate.num as usize;
959
960    let surface = &surface_set.surface[snum];
961    let feature_set = surface.feature_set.as_ref().ok_or("No feature set")?;
962    let feature_points = &feature_set.list[level];
963
964    ar2_set_template_sub(
965        cparam_lt,
966        wtrans1,
967        &surface.image_set.as_ref().ok_or("No image set")?.clone(),
968        feature_points,
969        fnum,
970        blur_level,
971        templ,
972    )?;
973
974    if ((templ.vlen * templ.vlen) as f32)
975        < ((templ.xsize * templ.ysize) as f32 * 2.0 * 2.0) { // AR2_DEFAULT_TRACKING_SD_THRESH placeholder
976        return Err("Low variance template");
977    }
978
979    let mut search = [[0i32; 2]; 3];
980    let feature_coord = &feature_points.coord[fnum];
981
982    if surface_set.cont_num == 1 {
983        ar2_get_search_point(cparam_lt, Some(wtrans1), None, None, feature_coord, &mut search);
984    } else if surface_set.cont_num == 2 {
985        ar2_get_search_point(cparam_lt, Some(wtrans1), wtrans2, None, feature_coord, &mut search);
986    } else {
987        ar2_get_search_point(cparam_lt, Some(wtrans1), wtrans2, wtrans3, feature_coord, &mut search);
988    }
989
990    let mut bx = 0;
991    let mut by = 0;
992    let mut sim = 0.0;
993    
994    ar2_get_best_matching(
995        data_ptr,
996        mf_image,
997        xsize,
998        ysize,
999        pix_format,
1000        templ,
1001        search_size,
1002        search_size,
1003        search,
1004        &mut bx,
1005        &mut by,
1006        &mut sim,
1007    )?;
1008
1009    let mut result = AR2Tracking2DResult {
1010        sim,
1011        pos2d: [bx as f32, by as f32],
1012        pos3d: [0.0; 3],
1013    };
1014
1015    result.pos3d[0] = surface.trans[0][0] * feature_coord.mx + surface.trans[0][1] * feature_coord.my + surface.trans[0][3];
1016    result.pos3d[1] = surface.trans[1][0] * feature_coord.mx + surface.trans[1][1] * feature_coord.my + surface.trans[1][3];
1017    result.pos3d[2] = surface.trans[2][0] * feature_coord.mx + surface.trans[2][1] * feature_coord.my + surface.trans[2][3];
1018
1019    Ok(result)
1020}
1021
1022pub fn ar2_tracking(
1023    handle: &mut AR2Handle,
1024    surface_set: &mut AR2SurfaceSet,
1025    data_ptr: &[u8],
1026    trans: &mut [[f32; 4]; 3],
1027    err: &mut f32,
1028) -> Result<(), i32> {
1029    if surface_set.cont_num <= 0 {
1030        return Err(-2);
1031    }
1032
1033    *err = 0.0;
1034
1035    for i in 0..surface_set.surface.len() {
1036        crate::math::mat_mul_fff(&surface_set.trans1, &surface_set.surface[i].trans, &mut handle.wtrans1[i]);
1037        if surface_set.cont_num > 1 {
1038            crate::math::mat_mul_fff(&surface_set.trans2, &surface_set.surface[i].trans, &mut handle.wtrans2[i]);
1039        }
1040        if surface_set.cont_num > 2 {
1041            crate::math::mat_mul_fff(&surface_set.trans3, &surface_set.surface[i].trans, &mut handle.wtrans3[i]);
1042        }
1043    }
1044
1045    let cparam_lt = unsafe { handle.cparam_lt.as_ref() }.ok_or(-1)?;
1046
1047    if handle.tracking_mode == AR2_TRACKING_6DOF {
1048        extract_visible_features(cparam_lt, &handle.wtrans1, surface_set, &mut handle.candidate, &mut handle.candidate2).map_err(|_| -1)?;
1049    } else {
1050        extract_visible_features_homography(handle.xsize, handle.ysize, &handle.wtrans1, surface_set, &mut handle.candidate, &mut handle.candidate2).map_err(|_| -1)?;
1051    }
1052
1053    let mut candidate_ptr_idx = 0; // 0 for candidate, 1 for candidate2
1054    let mut i = 0;
1055    let mut num = 0;
1056    
1057    // Sequential tracking loop
1058    while i < handle.search_feature_num {
1059        let mut pos = [[0.0f32; 2]; 4];
1060        let mut k = ar2_select_template(
1061            if candidate_ptr_idx == 0 { &mut handle.candidate } else { &mut handle.candidate2 },
1062            &mut surface_set.prev_feature,
1063            num,
1064            &mut pos,
1065            handle.xsize,
1066            handle.ysize,
1067        );
1068
1069        if k < 0 {
1070            if candidate_ptr_idx == 0 {
1071                candidate_ptr_idx = 1;
1072                k = ar2_select_template(&mut handle.candidate2, &mut surface_set.prev_feature, num, &mut pos, handle.xsize, handle.ysize);
1073                if k < 0 { break; }
1074            } else {
1075                break;
1076            }
1077        }
1078
1079        let cand = if candidate_ptr_idx == 0 { &handle.candidate[k as usize] } else { &handle.candidate2[k as usize] };
1080        
1081        let cparam_lt = unsafe { handle.cparam_lt.as_ref() };
1082        let snum = cand.snum as usize;
1083
1084        match ar2_tracking_2d_sub(
1085            cparam_lt,
1086            handle.pix_format,
1087            handle.xsize,
1088            handle.ysize,
1089            handle.blur_level,
1090            handle.search_size,
1091            &handle.wtrans1[snum],
1092            Some(&handle.wtrans2[snum]),
1093            Some(&handle.wtrans3[snum]),
1094            surface_set,
1095            cand,
1096            data_ptr,
1097            &mut handle.mf_image,
1098            &mut handle.templ[0], // Using index 0 for sequential
1099        ) {
1100            Ok(res) => {
1101                if res.sim > handle.sim_thresh {
1102                    if handle.tracking_mode == AR2_TRACKING_6DOF {
1103                        let (ix, iy) = cparam_lt.unwrap().param_ltf.observ2ideal(res.pos2d[0], res.pos2d[1]).map_err(|_| -1)?;
1104                        handle.pos2d[num as usize][0] = ix;
1105                        handle.pos2d[num as usize][1] = iy;
1106                    } else {
1107                        handle.pos2d[num as usize][0] = res.pos2d[0];
1108                        handle.pos2d[num as usize][1] = res.pos2d[1];
1109                    }
1110                    handle.pos3d[num as usize][0] = res.pos3d[0];
1111                    handle.pos3d[num as usize][1] = res.pos3d[1];
1112                    handle.pos3d[num as usize][2] = res.pos3d[2];
1113                    
1114                    handle.used_feature[num as usize].snum = cand.snum;
1115                    handle.used_feature[num as usize].level = cand.level;
1116                    handle.used_feature[num as usize].num = cand.num;
1117                    handle.used_feature[num as usize].flag = 0;
1118                    
1119                    num += 1;
1120                }
1121            }
1122            Err(_) => {}
1123        }
1124        i += 1;
1125    }
1126
1127    for idx in 0..num {
1128        surface_set.prev_feature[idx as usize] = handle.used_feature[idx as usize];
1129    }
1130    surface_set.prev_feature[num as usize].flag = -1;
1131
1132    if num < 3 {
1133        surface_set.cont_num = 0;
1134        return Err(-3);
1135    }
1136
1137    if handle.tracking_mode == AR2_TRACKING_6DOF {
1138        unsafe {
1139            let icp_handle = handle.icp_handle.as_mut().ok_or(-1)?;
1140            *err = ar2_get_trans_mat(icp_handle, surface_set.trans1, &handle.pos2d, &handle.pos3d, num as usize, trans, false);
1141            if *err > handle.tracking_thresh {
1142                crate::icp::icp_set_inlier_probability(icp_handle, 0.8);
1143                *err = ar2_get_trans_mat(icp_handle, *trans, &handle.pos2d, &handle.pos3d, num as usize, trans, true);
1144                if *err > handle.tracking_thresh {
1145                    crate::icp::icp_set_inlier_probability(icp_handle, 0.0);
1146                    *err = ar2_get_trans_mat(icp_handle, *trans, &handle.pos2d, &handle.pos3d, num as usize, trans, true);
1147                    if *err > handle.tracking_thresh {
1148                        surface_set.cont_num = 0;
1149                        return Err(-4);
1150                    }
1151                }
1152            }
1153        }
1154    } else {
1155        // Homography pose refinement (simplified or robust)
1156        // For now, return error if not implemented, or implement a basic version
1157        return Err(-5); // Homography refinement placeholder
1158    }
1159
1160    surface_set.cont_num += 1;
1161    surface_set.trans3 = surface_set.trans2;
1162    surface_set.trans2 = surface_set.trans1;
1163    surface_set.trans1 = *trans;
1164
1165    Ok(())
1166}
1167
1168pub fn ar2_get_trans_mat(
1169    icp_handle: &mut ICPHandleT,
1170    init_conv: [[f32; 4]; 3],
1171    pos2d: &[[f32; 2]],
1172    pos3d: &[[f32; 3]],
1173    num: usize,
1174    conv: &mut [[f32; 4]; 3],
1175    robust_mode: bool,
1176) -> f32 {
1177    use crate::icp::{ICPDataT, ICP2DCoordT, ICP3DCoordT};
1178    
1179    let mut dx = 0.0;
1180    let mut dy = 0.0;
1181    let mut dz = 0.0;
1182    for i in 0..num {
1183        dx += pos3d[i][0];
1184        dy += pos3d[i][1];
1185        dz += pos3d[i][2];
1186    }
1187    dx /= num as f32;
1188    dy /= num as f32;
1189    dz /= num as f32;
1190    
1191    let mut data_2d = vec![ICP2DCoordT::default(); num];
1192    let mut data_3d = vec![ICP3DCoordT::default(); num];
1193    
1194    for i in 0..num {
1195        data_2d[i].x = pos2d[i][0] as ARdouble;
1196        data_2d[i].y = pos2d[i][1] as ARdouble;
1197        data_3d[i].x = (pos3d[i][0] - dx) as ARdouble;
1198        data_3d[i].y = (pos3d[i][1] - dy) as ARdouble;
1199        data_3d[i].z = (pos3d[i][2] - dz) as ARdouble;
1200    }
1201    
1202    let data = ICPDataT {
1203        screen_coord: data_2d,
1204        world_coord: data_3d,
1205    };
1206    
1207    let mut init_mat = [[0.0 as ARdouble; 4]; 3];
1208    for j in 0..3 {
1209        for i in 0..3 { init_mat[j][i] = init_conv[j][i] as ARdouble; }
1210    }
1211    init_mat[0][3] = (init_conv[0][0] * dx + init_conv[0][1] * dy + init_conv[0][2] * dz + init_conv[0][3]) as ARdouble;
1212    init_mat[1][3] = (init_conv[1][0] * dx + init_conv[1][1] * dy + init_conv[1][2] * dz + init_conv[1][3]) as ARdouble;
1213    init_mat[2][3] = (init_conv[2][0] * dx + init_conv[2][1] * dy + init_conv[2][2] * dz + init_conv[2][3]) as ARdouble;
1214    
1215    let mut mat = [[0.0 as ARdouble; 4]; 3];
1216    let mut err = 0.0;
1217    
1218    if !robust_mode {
1219        crate::icp::icp_point(icp_handle, &data, &init_mat, &mut mat).map(|e| err = e as f32).ok();
1220    } else {
1221        crate::icp::icp_point_robust(icp_handle, &data, &init_mat, &mut mat).map(|e| err = e as f32).ok();
1222    }
1223    
1224    for j in 0..3 {
1225        for i in 0..3 { conv[j][i] = mat[j][i] as f32; }
1226    }
1227    conv[0][3] = (mat[0][3] - mat[0][0] * dx as ARdouble - mat[0][1] * dy as ARdouble - mat[0][2] * dz as ARdouble) as f32;
1228    conv[1][3] = (mat[1][3] - mat[1][0] * dx as ARdouble - mat[1][1] * dy as ARdouble - mat[1][2] * dz as ARdouble) as f32;
1229    conv[2][3] = (mat[2][3] - mat[2][0] * dx as ARdouble - mat[2][1] * dy as ARdouble - mat[2][2] * dz as ARdouble) as f32;
1230    
1231    err as f32
1232}
1233
1234pub const KEEP_NUM: usize = 3;
1235pub const SKIP_INTERVAL: i32 = 3;
1236
1237pub fn ar2_get_best_matching(
1238    img: &[u8],
1239    mf_image: &mut [u8],
1240    xsize: i32,
1241    ysize: i32,
1242    pix_format: ARPixelFormat,
1243    mtemp: &AR2Template,
1244    rx: i32,
1245    ry: i32,
1246    search: [[i32; 2]; 3],
1247    bx: &mut i32,
1248    by: &mut i32,
1249    val: &mut f32,
1250) -> Result<(), &'static str> {
1251    // Initialise mf_image for search areas
1252    for ii in 0..3 {
1253        if search[ii][0] < 0 { break; }
1254        
1255        let px = (search[ii][0] / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
1256        let py = (search[ii][1] / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
1257        
1258        let sx = (px - rx).max(0);
1259        let ex = (px + rx).min(xsize - 1);
1260        let sy = (py - ry).max(0);
1261        let ey = (py + ry).min(ysize - 1);
1262        
1263        for j in sy..=ey {
1264            for i in sx..=ex {
1265                mf_image[(j * xsize + i) as usize] = 0;
1266            }
1267        }
1268    }
1269
1270    let mut keep_num = 0;
1271    let mut cx = [0; KEEP_NUM];
1272    let mut cy = [0; KEEP_NUM];
1273    let mut cval = [0; KEEP_NUM];
1274    let mut ret = true;
1275
1276    for ii in 0..3 {
1277        if search[ii][0] < 0 {
1278            if ret { return Err("No valid search point"); }
1279            break;
1280        }
1281
1282        let px = (search[ii][0] / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
1283        let py = (search[ii][1] / (SKIP_INTERVAL + 1)) * (SKIP_INTERVAL + 1) + (SKIP_INTERVAL + 1) / 2;
1284
1285        for j in (py - ry..=py + ry).step_by((SKIP_INTERVAL + 1) as usize) {
1286            if j - mtemp.yts1 * AR2_TEMP_SCALE < 0 { continue; }
1287            if j + mtemp.yts2 * AR2_TEMP_SCALE >= ysize { break; }
1288            
1289            for i in (px - rx..=px + rx).step_by((SKIP_INTERVAL + 1) as usize) {
1290                if i - mtemp.xts1 * AR2_TEMP_SCALE < 0 { continue; }
1291                if i + mtemp.xts2 * AR2_TEMP_SCALE >= xsize { break; }
1292                
1293                if mf_image[(j * xsize + i) as usize] != 0 { continue; }
1294                mf_image[(j * xsize + i) as usize] = 1;
1295                
1296                let mut wval = 0;
1297                if ar2_get_best_matching_sub_fine(img, xsize, ysize, pix_format, mtemp, i, j, &mut wval).is_ok() {
1298                    ret = false;
1299                    update_candidate(i, j, wval, &mut keep_num, &mut cx, &mut cy, &mut cval);
1300                }
1301            }
1302        }
1303    }
1304
1305    let mut wval2 = 0;
1306    let mut final_ret = Err("No match found");
1307
1308    // Simplified third pass (ignoring optimized SAT for now to keep it clear, can be optimized later)
1309    for l in 0..keep_num {
1310        for j in cy[l] - SKIP_INTERVAL..=cy[l] + SKIP_INTERVAL {
1311            if j - mtemp.yts1 * AR2_TEMP_SCALE < 0 { continue; }
1312            if j + mtemp.yts2 * AR2_TEMP_SCALE >= ysize { break; }
1313            
1314            for i in cx[l] - SKIP_INTERVAL..=cx[l] + SKIP_INTERVAL {
1315                if i - mtemp.xts1 * AR2_TEMP_SCALE < 0 { continue; }
1316                if i + mtemp.xts2 * AR2_TEMP_SCALE >= xsize { break; }
1317                
1318                let mut wval = 0;
1319                if ar2_get_best_matching_sub_fine(img, xsize, ysize, pix_format, mtemp, i, j, &mut wval).is_ok() {
1320                    if wval > wval2 {
1321                        *bx = i;
1322                        *by = j;
1323                        wval2 = wval;
1324                        *val = wval as f32 / 10000.0;
1325                        final_ret = Ok(());
1326                    }
1327                }
1328            }
1329        }
1330    }
1331
1332    final_ret
1333}
1334
1335pub fn ar2_get_best_matching_sub_fine(
1336    img: &[u8],
1337    xsize: i32,
1338    _ysize: i32,
1339    pix_format: ARPixelFormat,
1340    mtemp: &AR2Template,
1341    sx: i32,
1342    sy: i32,
1343    val: &mut i32,
1344) -> Result<(), &'static str> {
1345    let mut sum1 = 0;
1346    let mut sum2 = 0;
1347    let mut sum3 = 0;
1348
1349    let mut p1_idx = 0;
1350    
1351    match pix_format {
1352        ARPixelFormat::MONO | ARPixelFormat::NV21 | ARPixelFormat::FourTwoZeroV | ARPixelFormat::FourTwoZeroF => {
1353            let ssx = -mtemp.xts1;
1354            let eex = mtemp.xts2;
1355            let ssy = -mtemp.yts1;
1356            let eey = mtemp.yts2;
1357            
1358            for j in ssy..=eey {
1359                let row_start = (sy + j * AR2_TEMP_SCALE) * xsize + sx + ssx * AR2_TEMP_SCALE;
1360                for _i in ssx..=eex {
1361                    let pixel_val = img[row_start as usize + (_i - ssx) as usize * AR2_TEMP_SCALE as usize] as i32;
1362                    let template_val = mtemp.img1[p1_idx];
1363                    if template_val != AR2_TEMPLATE_NULL_PIXEL {
1364                        sum1 += pixel_val;
1365                        sum2 += pixel_val * pixel_val;
1366                        sum3 += pixel_val * template_val as i32;
1367                    }
1368                    p1_idx += 1;
1369                }
1370            }
1371        }
1372        _ => {
1373            // Placeholder for other pixel formats (RGB, etc.)
1374            // We can add them later as needed. Mono is the usual target for NFT.
1375            return Err("Unsupported pixel format for matching");
1376        }
1377    }
1378
1379    sum3 -= sum1 * mtemp.sum / (mtemp.valid_num as i32);
1380    let vlen = sum2 - sum1 * sum1 / (mtemp.valid_num as i32);
1381    if vlen == 0 {
1382        *val = 0;
1383    } else {
1384        *val = (sum3 * 100 / mtemp.vlen) * 100 / (vlen as f32).sqrt() as i32;
1385    }
1386
1387    Ok(())
1388}
1389
1390fn update_candidate(
1391    x: i32,
1392    y: i32,
1393    wval: i32,
1394    keep_num: &mut usize,
1395    cx: &mut [i32; KEEP_NUM],
1396    cy: &mut [i32; KEEP_NUM],
1397    cval: &mut [i32; KEEP_NUM],
1398) {
1399    if *keep_num == 0 {
1400        cx[0] = x;
1401        cy[0] = y;
1402        cval[0] = wval;
1403        *keep_num = 1;
1404        return;
1405    }
1406
1407    let mut insert_idx = *keep_num;
1408    for l in 0..*keep_num {
1409        if cval[l] < wval {
1410            insert_idx = l;
1411            break;
1412        }
1413    }
1414
1415    if insert_idx == *keep_num {
1416        if insert_idx < KEEP_NUM {
1417            cx[insert_idx] = x;
1418            cy[insert_idx] = y;
1419            cval[insert_idx] = wval;
1420            *keep_num += 1;
1421        }
1422        return;
1423    }
1424
1425    let m = if *keep_num == KEEP_NUM { KEEP_NUM - 1 } else { *keep_num };
1426    if *keep_num < KEEP_NUM { *keep_num += 1; }
1427
1428    for n in (insert_idx + 1..=m).rev() {
1429        cx[n] = cx[n - 1];
1430        cy[n] = cy[n - 1];
1431        cval[n] = cval[n - 1];
1432    }
1433    cx[insert_idx] = x;
1434    cy[insert_idx] = y;
1435    cval[insert_idx] = wval;
1436}
webarkitlib_rs/ar2.rs

webarkitlib_rs/
ar2.rs
