nx_force.nx source
↩ module page · 153 lines · 5773 B
1// nx_force.nx -- 3D force vector primitive. Layer 1 physics
2// foundation per [[feedback-engineering-sciences-bits-up-3d-print-first]].
3//
4// Coordinate / unit convention: Q14 Newtons. 1 Q14 unit = 1/16384 N
5// = 61 µN. Range fits i64 to ±5e14 N = ±5e11 kN -- vastly exceeds
6// any engineering analysis (largest civil-eng forces are GN scale).
7//
8// Foundation for downstream primitives queued per the META-CARDINAL
9// substrate hierarchy:
10// nx_stress, nx_strain, nx_modulus -- continuum mechanics
11// nx_fea, nx_finite_element -- structural analysis
12// nx_pid (already shipped) -- control loop on force
13// nx_pose, nx_torque -- robotics (force + arm)
14// nx_aerodynamic_load -- aero/CFD
15//
16// Composes nx_isqrt (magnitude via Q28 sum-of-squares → Q14 root)
17// + nx_i128 + nx_muldiv_i64 (intermediate-overflow protection in
18// dot/cross products for large magnitudes).
19//
20// Per Cardinal 6 (no float): all math integer-only. Slicer-class
21// determinism: same input bytes → same output bytes on every
22// nxc2 codegen target.
23//
24// license_tier: ORIGINAL
25
26import "nx_syscalls.nx"
27import "nx_isqrt.nx"
28import "nx_i128.nx"
29
30const NX_FORCE_Q14: i64 = 16384
31
32// Gravitational acceleration g = 9.80665 m/s² on Earth (CODATA).
33// In Q14 N/kg: 9.80665 * 16384 = 160664.9 -> 160665 (rounded).
34// Common engineering use approximates 9.81 → 160727 Q14. Use the
35// exact CODATA value here; callers needing the approximation can
36// multiply by 160727 directly.
37const NX_FORCE_GRAVITY_Q14: i64 = 160665
38
39// ===== sealed-enum verdicts ========================================
40
41const NX_FORCE_OK: i64 = 0
42const NX_FORCE_ERR_NULL_INPUT: i64 = 1
43const NX_FORCE_ERR_DEGENERATE: i64 = 2
44
45func nx_force_verdict_name(v: i64) -> *u8 {
46 if v == NX_FORCE_OK { return "OK" }
47 if v == NX_FORCE_ERR_NULL_INPUT { return "NULL_INPUT" }
48 if v == NX_FORCE_ERR_DEGENERATE { return "DEGENERATE" }
49 return "UNKNOWN"
50}
51
52// ===== struct ======================================================
53
54struct NxForce {
55 fx_q14: i64,
56 fy_q14: i64,
57 fz_q14: i64,
58}
59
60const NX_FORCE_BYTES: i64 = 24
61
62// ===== constructors ================================================
63
64func nx_force_new(fx_q14: i64, fy_q14: i64, fz_q14: i64) -> *NxForce {
65 let f: *NxForce = (sys_mmap(NX_FORCE_BYTES)) as *NxForce
66 f.fx_q14 = fx_q14
67 f.fy_q14 = fy_q14
68 f.fz_q14 = fz_q14
69 return f
70}
71
72func nx_force_zero() -> *NxForce {
73 return nx_force_new(0, 0, 0)
74}
75
76// Gravitational force on a mass m (Q14 kg) pointing -Z. Returns
77// (0, 0, -m * g) in Q14 N. -Z is the "down" convention matching
78// the slicer's bed-down orientation.
79func nx_force_gravity_on_kg(mass_kg_q14: i64) -> *NxForce {
80 // F_z_q14 = -(m_q14 * g_q14) / Q14_ONE (units cancel via div)
81 let fz: i64 = 0 - nx_muldiv_i64(mass_kg_q14, NX_FORCE_GRAVITY_Q14, NX_FORCE_Q14)
82 return nx_force_new(0, 0, fz)
83}
84
85// ===== arithmetic ==================================================
86
87func nx_force_add(a: *NxForce, b: *NxForce) -> *NxForce {
88 return nx_force_new(a.fx_q14 + b.fx_q14,
89 a.fy_q14 + b.fy_q14,
90 a.fz_q14 + b.fz_q14)
91}
92
93func nx_force_sub(a: *NxForce, b: *NxForce) -> *NxForce {
94 return nx_force_new(a.fx_q14 - b.fx_q14,
95 a.fy_q14 - b.fy_q14,
96 a.fz_q14 - b.fz_q14)
97}
98
99func nx_force_scale(f: *NxForce, scalar_q14: i64) -> *NxForce {
100 return nx_force_new(nx_muldiv_i64(f.fx_q14, scalar_q14, NX_FORCE_Q14),
101 nx_muldiv_i64(f.fy_q14, scalar_q14, NX_FORCE_Q14),
102 nx_muldiv_i64(f.fz_q14, scalar_q14, NX_FORCE_Q14))
103}
104
105// ===== magnitude ===================================================
106//
107// |F| = sqrt(fx² + fy² + fz²). In Q14: squares give Q28; sum stays
108// Q28; nx_isqrt(Q28) returns Q14. Pure integer.
109
110func nx_force_magnitude_q14(f: *NxForce) -> i64 {
111 let sq_x: i64 = f.fx_q14 * f.fx_q14
112 let sq_y: i64 = f.fy_q14 * f.fy_q14
113 let sq_z: i64 = f.fz_q14 * f.fz_q14
114 let lsq: i64 = sq_x + sq_y + sq_z
115 return nx_isqrt(lsq)
116}
117
118// ===== dot product =================================================
119//
120// a · b = ax*bx + ay*by + az*bz. In Q14: each product is Q28;
121// sum is Q28; divide by Q14_ONE to get Q14 scalar result.
122//
123// For large magnitudes use nx_muldiv_i64 path to keep intermediates
124// within i64. For slicer/engineering scales (forces in kN range)
125// plain i64 works.
126
127func nx_force_dot(a: *NxForce, b: *NxForce) -> i64 {
128 let dx: i64 = nx_muldiv_i64(a.fx_q14, b.fx_q14, NX_FORCE_Q14)
129 let dy: i64 = nx_muldiv_i64(a.fy_q14, b.fy_q14, NX_FORCE_Q14)
130 let dz: i64 = nx_muldiv_i64(a.fz_q14, b.fz_q14, NX_FORCE_Q14)
131 return dx + dy + dz
132}
133
134// ===== cross product ===============================================
135//
136// a × b = (ay*bz - az*by, az*bx - ax*bz, ax*by - ay*bx). Same
137// Q14 scaling as dot.
138
139func nx_force_cross(a: *NxForce, b: *NxForce) -> *NxForce {
140 // NOTE: NishiLang treats `expr\n - expr` as a STATEMENT BREAK,
141 // not a continuation -- the second term is silently dropped.
142 // Single-line subtractions only; use temporaries when needed.
143 let cx_lhs: i64 = nx_muldiv_i64(a.fy_q14, b.fz_q14, NX_FORCE_Q14)
144 let cx_rhs: i64 = nx_muldiv_i64(a.fz_q14, b.fy_q14, NX_FORCE_Q14)
145 let cx: i64 = cx_lhs - cx_rhs
146 let cy_lhs: i64 = nx_muldiv_i64(a.fz_q14, b.fx_q14, NX_FORCE_Q14)
147 let cy_rhs: i64 = nx_muldiv_i64(a.fx_q14, b.fz_q14, NX_FORCE_Q14)
148 let cy: i64 = cy_lhs - cy_rhs
149 let cz_lhs: i64 = nx_muldiv_i64(a.fx_q14, b.fy_q14, NX_FORCE_Q14)
150 let cz_rhs: i64 = nx_muldiv_i64(a.fy_q14, b.fx_q14, NX_FORCE_Q14)
151 let cz: i64 = cz_lhs - cz_rhs
152 return nx_force_new(cx, cy, cz)
153}