nx_pc_test.nx source

1// nx_pc_test.nx -- smoke for nx_pc primitives. 2 3import "syscalls.nx" 4import "nx_pc.nx" 5 6func main() -> i64 { 7 // === Test 1: centroid === 8 let sq: *PointCloud2D = nx_pc_alloc(4) 9 nx_pc_set(sq, 0, 0, 0) 10 nx_pc_set(sq, 1, 10, 0) 11 nx_pc_set(sq, 2, 10, 10) 12 nx_pc_set(sq, 3, 0, 10) 13 let cx: *i64 = (sys_mmap(8)) as *i64 14 let cy: *i64 = (sys_mmap(8)) as *i64 15 nx_pc_centroid(sq, cx, cy) 16 if cx[0] != 5 { return 1 } 17 if cy[0] != 5 { return 2 } 18 19 // === Test 2: bounding box === 20 let xmn: *i64 = (sys_mmap(8)) as *i64 21 let ymn: *i64 = (sys_mmap(8)) as *i64 22 let xmx: *i64 = (sys_mmap(8)) as *i64 23 let ymx: *i64 = (sys_mmap(8)) as *i64 24 nx_pc_bbox(sq, xmn, ymn, xmx, ymx) 25 if xmn[0] != 0 { return 10 } 26 if ymn[0] != 0 { return 11 } 27 if xmx[0] != 10 { return 12 } 28 if ymx[0] != 10 { return 13 } 29 30 // === Test 3: nearest neighbor === 31 let cloud: *PointCloud2D = nx_pc_alloc(3) 32 nx_pc_set(cloud, 0, 100, 100) 33 nx_pc_set(cloud, 1, 200, 200) 34 nx_pc_set(cloud, 2, 50, 50) 35 let nn: i64 = nx_pc_nearest(cloud, 60, 60) 36 if nn != 2 { return 20 } // closest is (50,50) 37 let nn2: i64 = nx_pc_nearest(cloud, 190, 190) 38 if nn2 != 1 { return 21 } // closest is (200,200) 39 40 // === Test 4: integer sqrt === 41 if nx_math_isqrt(100) != 10 { return 30 } 42 if nx_math_isqrt(1000000) != 1000 { return 31 } 43 44 // === Test 5: apply identity transform leaves points unchanged === 45 let p1: *PointCloud2D = nx_pc_alloc(2) 46 nx_pc_set(p1, 0, 100, 50) 47 nx_pc_set(p1, 1, -30, 80) 48 nx_pc_apply_transform(p1, 1024, 0, 0, 0) // cos=1, sin=0, t=0 49 if nx_pc_get_x(p1, 0) != 100 { return 40 } 50 if nx_pc_get_y(p1, 0) != 50 { return 41 } 51 if nx_pc_get_x(p1, 1) != -30 { return 42 } 52 if nx_pc_get_y(p1, 1) != 80 { return 43 } 53 54 // === Test 6: apply translation === 55 nx_pc_apply_transform(p1, 1024, 0, 5, -7) 56 if nx_pc_get_x(p1, 0) != 105 { return 50 } 57 if nx_pc_get_y(p1, 0) != 43 { return 51 } 58 59 // === Test 7: ICP aligns translated cloud === 60 // Source: 4 corners of a 100x100 square at origin. 61 // Target: same square translated by (50, 30). 62 // After ICP, source should align with target (low MSE). 63 let src: *PointCloud2D = nx_pc_alloc(4) 64 nx_pc_set(src, 0, 0, 0) 65 nx_pc_set(src, 1, 100, 0) 66 nx_pc_set(src, 2, 100, 100) 67 nx_pc_set(src, 3, 0, 100) 68 let tgt: *PointCloud2D = nx_pc_alloc(4) 69 nx_pc_set(tgt, 0, 50, 30) 70 nx_pc_set(tgt, 1, 150, 30) 71 nx_pc_set(tgt, 2, 150, 130) 72 nx_pc_set(tgt, 3, 50, 130) 73 let mse_before: i64 = nx_pc_mse(src, tgt) 74 nx_pc_icp(src, tgt, 5) 75 let mse_after: i64 = nx_pc_mse(src, tgt) 76 if mse_after >= mse_before { return 60 } 77 // After alignment, MSE should be very small (translation is exact). 78 if mse_after > 10 { return 61 } 79 80 // === Test 8: ICP on already-aligned clouds does nothing harmful === 81 let s2: *PointCloud2D = nx_pc_alloc(4) 82 nx_pc_set(s2, 0, 0, 0) 83 nx_pc_set(s2, 1, 100, 0) 84 nx_pc_set(s2, 2, 100, 100) 85 nx_pc_set(s2, 3, 0, 100) 86 let t2: *PointCloud2D = nx_pc_alloc(4) 87 nx_pc_set(t2, 0, 0, 0) 88 nx_pc_set(t2, 1, 100, 0) 89 nx_pc_set(t2, 2, 100, 100) 90 nx_pc_set(t2, 3, 0, 100) 91 nx_pc_icp(s2, t2, 3) 92 // Points should still be near originals. 93 var dx: i64 = nx_pc_get_x(s2, 0) - 0 94 if dx < 0 { dx = -dx } 95 if dx > 5 { return 70 } 96 97 return 0 98}