nx_body_rest_registration.nx source
↩ module page · 87 lines · 4580 B
1// Versioned full-XYZ rest registration over nx_skelgen J rows. Legacy consumers unchanged.
2import "nx_buf_dyn.nx"
3const BR_ROW_WORDS:i64=6
4const BR_ROW_BYTES:i64=48
5const BR_INVALID:i64=-201
6const BR_RANGE:i64=-202
7const BR_SCHEMA:i64=-203
8const BR_HIERARCHY:i64=-204
9func br_space(c:i64)->i64{return (c==32||c==9||c==13) as i64}
10// Negative accumulation admits INT64_MIN without negating it.
11func br_integer(b:*u8,end:i64,p:*i64,out:*i64)->i64{
12 var i:i64=p[0];while i<end&&br_space(b[i] as i64)==1{i=i+1};if i>=end{return BR_SCHEMA}
13 var neg:i64=0;if b[i]==45 as u8{neg=1;i=i+1};if i>=end{return BR_SCHEMA}
14 var limit:i64=-9223372036854775807;if neg==1{limit=limit-1}
15 var v:i64=0;var digits:i64=0
16 while i<end&&b[i]>=48 as u8&&b[i]<=57 as u8{
17 let d:i64=(b[i] as i64)-48
18 if v<limit/10||(v==limit/10&&d>0-limit%10){return BR_RANGE}
19 v=v*10-d;digits=digits+1;i=i+1
20 }
21 if digits==0{return BR_SCHEMA};if i<end&&br_space(b[i] as i64)!=1{return BR_SCHEMA}
22 if neg==0{v=0-v};out[0]=v;p[0]=i;return 0
23}
24func br_joint_index(rows:*i64,n:i64,id:i64)->i64{var i:i64=0;while i<n{if rows[i*BR_ROW_WORDS]==id{return i};i=i+1};return -1}
25func br_validate(rows:*i64,n:i64,coordinate_limit:i64)->i64{
26 if n<=0||coordinate_limit<=0{return BR_INVALID};if coordinate_limit>NX_BO_I64_MAX/2{return BR_RANGE}
27 var i:i64=0;var roots:i64=0
28 while i<n{
29 let r:*i64=((rows as i64)+i*BR_ROW_BYTES) as *i64
30 if r[0]<0||r[1]<-1||r[2]<-1||r[2]>1{return BR_SCHEMA}
31 var k:i64=3;while k<6{if r[k]<0-coordinate_limit||r[k]>coordinate_limit{return BR_RANGE};k=k+1}
32 k=0;while k<i{if rows[k*BR_ROW_WORDS]==r[0]{return BR_SCHEMA};k=k+1}
33 if r[1]==-1{roots=roots+1}else{if br_joint_index(rows,n,r[1])<0{return BR_HIERARCHY}}
34 i=i+1
35 }
36 if roots==0{return BR_HIERARCHY}
37 i=0;while i<n{
38 var at:i64=i;var depth:i64=0
39 while at>=0{
40 if depth>=n{return BR_HIERARCHY};depth=depth+1
41 let parent:i64=rows[at*BR_ROW_WORDS+1]
42 if parent<0{at=-1}else{at=br_joint_index(rows,n,parent)}
43 };i=i+1
44 };return 0
45}
46// All J fields retained, record order may differ from parent order. B constraint
47// records remain in the caller's hash-bound original file; this function only owns J rows.
48// Failure leaves the caller's previous complete buffer unchanged.
49func br_parse(b:*u8,n:i64,max_bytes:i64,coordinate_limit:i64,out:*NxBufOwned)->i64{
50 if (b as i64)<=0||n<=0||max_bytes<BR_ROW_BYTES{return BR_INVALID}
51 let state:i64=nx_bo_state(out);if state<0{return state}
52 let scratch:*NxBufOwned=sys_mmap_try(NX_BO_BYTES) as *NxBufOwned
53 let row:*i64=sys_mmap_try(BR_ROW_BYTES) as *i64
54 let cursor:*i64=sys_mmap_try(16) as *i64
55 if (scratch as i64)<=0||(row as i64)<=0||(cursor as i64)<=0{
56 if (scratch as i64)>0{sys_munmap_direct(scratch as *u8,NX_BO_BYTES)}
57 if (row as i64)>0{sys_munmap_direct(row as *u8,BR_ROW_BYTES)}
58 if (cursor as i64)>0{sys_munmap_direct(cursor as *u8,16)};return NX_BO_ALLOCATION
59 }
60 var rc:i64=0;var pos:i64=0
61 while pos<n&&rc==0{
62 var end:i64=pos;while end<n&&b[end]!=10 as u8{end=end+1}
63 var start:i64=pos;while start<end&&br_space(b[start] as i64)==1{start=start+1}
64 if start<end{
65 let kind:i64=b[start] as i64
66 if kind==74||kind==66{
67 if start+1>=end||br_space(b[start+1] as i64)!=1{rc=BR_SCHEMA}else{
68 var count:i64=4;if kind==74{count=BR_ROW_WORDS};cursor[0]=start+1
69 var i:i64=0;while i<count&&rc==0{rc=br_integer(b,end,cursor,(row as i64+i*8) as *i64);i=i+1}
70 while cursor[0]<end&&br_space(b[cursor[0]] as i64)==1{cursor[0]=cursor[0]+1}
71 if rc==0&&cursor[0]!=end{rc=BR_SCHEMA}
72 if rc==0&&kind==74{rc=nx_bo_append(scratch,row as *u8,BR_ROW_BYTES,max_bytes)}
73 }
74 }else{if kind!=35{rc=BR_SCHEMA}}
75 };pos=end+1
76 }
77 if rc==0{rc=br_validate(scratch.buf as *i64,scratch.len/BR_ROW_BYTES,coordinate_limit)}
78 if rc==0{rc=nx_bo_release(out);if rc==0{out.buf=scratch.buf;out.len=scratch.len;out.cap=scratch.cap;scratch.buf=0 as *u8;scratch.len=0;scratch.cap=0}}
79 nx_bo_release(scratch);sys_munmap_direct(scratch as *u8,NX_BO_BYTES);sys_munmap_direct(row as *u8,BR_ROW_BYTES);sys_munmap_direct(cursor as *u8,16);return rc
80}
81// Exact local translation in source millimetres. Orientation is supplied by the
82// versioned region frame recipe; joint positions alone do not determine twist.
83func br_local_offset(rows:*i64,n:i64,index:i64,out:*i64)->i64{
84 if index<0||index>=n{return BR_INVALID};let r:*i64=((rows as i64)+index*BR_ROW_BYTES) as *i64
85 var parent:i64=-1;if r[1]>=0{parent=br_joint_index(rows,n,r[1]);if parent<0{return BR_HIERARCHY}}
86 var k:i64=0;while k<3{var v:i64=r[k+3];if parent>=0{v=v-rows[parent*BR_ROW_WORDS+k+3]};out[k]=v;k=k+1};return 0
87}