webarkitlib_rs/
pose.rs

1/*
2 *  pose.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
37use crate::types::{AR3DHandle, ARParam, ARMarkerInfo, ARdouble};
38use crate::icp::{icp_create_handle, icp_delete_handle, ICP2DCoordT, ICP3DCoordT, ICPDataT, icp_get_init_xw2xc_from_planar_data, icp_point};
39// use log::debug; // Removed unused import
40
41pub fn ar_3d_create_handle(ar_param: &ARParam) -> Result<*mut AR3DHandle, &'static str> {
42    let icp_handle = icp_create_handle(&ar_param.mat)?;
43    let mut handle = Box::new(AR3DHandle::default());
44    handle.icp_handle = icp_handle;
45    Ok(Box::into_raw(handle))
46}
47
48pub fn ar_3d_delete_handle(handle: &mut *mut AR3DHandle) -> Result<(), &'static str> {
49    if handle.is_null() {
50        return Err("Null AR3DHandle");
51    }
52    unsafe {
53        let mut bx = Box::from_raw(*handle);
54        icp_delete_handle(&mut bx.icp_handle)?;
55        drop(bx);
56    }
57    *handle = std::ptr::null_mut();
58    Ok(())
59}
60
61pub fn ar_get_trans_mat_square(
62    handle: &AR3DHandle,
63    marker_info: &ARMarkerInfo,
64    width: ARdouble,
65    conv: &mut [[ARdouble; 4]; 3]
66) -> Result<ARdouble, &'static str> {
67    if handle.icp_handle.is_null() {
68        return Err("Null ICPHandleT within AR3DHandle");
69    }
70    
71    let dir = if marker_info.id_matrix < 0 {
72        marker_info.dir_patt
73    } else if marker_info.id_patt < 0 {
74        marker_info.dir_matrix
75    } else {
76        marker_info.dir
77    };
78    
79    let mut screen_coord = vec![ICP2DCoordT::default(); 4];
80    screen_coord[0].x = marker_info.vertex[((4 - dir) % 4) as usize][0];
81    screen_coord[0].y = marker_info.vertex[((4 - dir) % 4) as usize][1];
82    screen_coord[1].x = marker_info.vertex[((5 - dir) % 4) as usize][0];
83    screen_coord[1].y = marker_info.vertex[((5 - dir) % 4) as usize][1];
84    screen_coord[2].x = marker_info.vertex[((6 - dir) % 4) as usize][0];
85    screen_coord[2].y = marker_info.vertex[((6 - dir) % 4) as usize][1];
86    screen_coord[3].x = marker_info.vertex[((7 - dir) % 4) as usize][0];
87    screen_coord[3].y = marker_info.vertex[((7 - dir) % 4) as usize][1];
88    
89    let mut world_coord = vec![ICP3DCoordT::default(); 4];
90    world_coord[0].x = -width / 2.0; world_coord[0].y = width / 2.0;  world_coord[0].z = 0.0;
91    world_coord[1].x = width / 2.0;  world_coord[1].y = width / 2.0;  world_coord[1].z = 0.0;
92    world_coord[2].x = width / 2.0;  world_coord[2].y = -width / 2.0; world_coord[2].z = 0.0;
93    world_coord[3].x = -width / 2.0; world_coord[3].y = -width / 2.0; world_coord[3].z = 0.0;
94    
95    let mut init_mat_xw2xc = [[0.0; 4]; 3];
96    
97    let icp_handle_ref = unsafe { &*handle.icp_handle };
98    
99    match icp_get_init_xw2xc_from_planar_data(
100        &icp_handle_ref.mat_xc2u,
101        &screen_coord,
102        &world_coord,
103        4,
104        &mut init_mat_xw2xc
105    ) {
106        Ok(_) => {},
107        Err(_) => return Ok(100000000.0) // Arbitrary high error
108    }
109    
110    let data = ICPDataT {
111        screen_coord,
112        world_coord
113    };
114    
115    match icp_point(icp_handle_ref, &data, &init_mat_xw2xc, conv) {
116        Ok(err) => Ok(err),
117        Err(_) => Ok(100000000.0)
118    }
119}
120
121pub fn ar_get_trans_mat_square_cont(
122    handle: &AR3DHandle,
123    marker_info: &ARMarkerInfo,
124    init_conv: &[[ARdouble; 4]; 3],
125    width: ARdouble,
126    conv: &mut [[ARdouble; 4]; 3]
127) -> Result<ARdouble, &'static str> {
128    if handle.icp_handle.is_null() {
129        return Err("Null ICPHandleT within AR3DHandle");
130    }
131    
132    let dir = if marker_info.id_matrix < 0 {
133        marker_info.dir_patt
134    } else if marker_info.id_patt < 0 {
135        marker_info.dir_matrix
136    } else {
137        marker_info.dir
138    };
139    
140    let mut screen_coord = vec![ICP2DCoordT::default(); 4];
141    let mut world_coord = vec![ICP3DCoordT::default(); 4];
142    
143    screen_coord[0].x = marker_info.vertex[((4 - dir) % 4) as usize][0];
144    screen_coord[0].y = marker_info.vertex[((4 - dir) % 4) as usize][1];
145    screen_coord[1].x = marker_info.vertex[((5 - dir) % 4) as usize][0];
146    screen_coord[1].y = marker_info.vertex[((5 - dir) % 4) as usize][1];
147    screen_coord[2].x = marker_info.vertex[((6 - dir) % 4) as usize][0];
148    screen_coord[2].y = marker_info.vertex[((6 - dir) % 4) as usize][1];
149    screen_coord[3].x = marker_info.vertex[((7 - dir) % 4) as usize][0];
150    screen_coord[3].y = marker_info.vertex[((7 - dir) % 4) as usize][1];
151    
152    world_coord[0].x = -width / 2.0; world_coord[0].y = width / 2.0;  world_coord[0].z = 0.0;
153    world_coord[1].x = width / 2.0;  world_coord[1].y = width / 2.0;  world_coord[1].z = 0.0;
154    world_coord[2].x = width / 2.0;  world_coord[2].y = -width / 2.0; world_coord[2].z = 0.0;
155    world_coord[3].x = -width / 2.0; world_coord[3].y = -width / 2.0; world_coord[3].z = 0.0;
156    
157    let icp_handle_ref = unsafe { &*handle.icp_handle };
158    
159    let data = ICPDataT {
160        screen_coord,
161        world_coord
162    };
163    
164    match icp_point(icp_handle_ref, &data, init_conv, conv) {
165        Ok(err) => Ok(err),
166        Err(_) => Ok(100000000.0)
167    }
168}
169
170pub fn ar_get_trans_mat(
171    handle: &AR3DHandle,
172    init_conv: &[[ARdouble; 4]; 3],
173    pos2d: &[[ARdouble; 2]],
174    pos3d: &[[ARdouble; 3]],
175    num: usize,
176    conv: &mut [[ARdouble; 4]; 3]
177) -> Result<ARdouble, &'static str> {
178    if handle.icp_handle.is_null() {
179        return Err("Null ICPHandleT within AR3DHandle");
180    }
181
182    if pos2d.len() < num || pos3d.len() < num {
183        return Err("Not enough coordinate data provided");
184    }
185
186    let mut screen_coord = Vec::with_capacity(num);
187    let mut world_coord = Vec::with_capacity(num);
188
189    for i in 0..num {
190        screen_coord.push(ICP2DCoordT { x: pos2d[i][0], y: pos2d[i][1] });
191        world_coord.push(ICP3DCoordT { x: pos3d[i][0], y: pos3d[i][1], z: pos3d[i][2] });
192    }
193
194    let data = ICPDataT {
195        screen_coord,
196        world_coord
197    };
198
199    let icp_handle_ref = unsafe { &*handle.icp_handle };
200
201    match icp_point(icp_handle_ref, &data, init_conv, conv) {
202        Ok(err) => Ok(err),
203        Err(_) => Ok(100000000.0)
204    }
205}
webarkitlib_rs/pose.rs

webarkitlib_rs/
pose.rs
