Skip to main content

PGA_SCLERP_EVAL_WGSL

Constant PGA_SCLERP_EVAL_WGSL 

Source
pub const PGA_SCLERP_EVAL_WGSL: &str = "// PGA ScLERP Batch Compute Kernel (3D Projective Geometric Algebra)\n// Evaluates parallel Screw Linear Interpolation for N instances across SE(3) geodesics.\n\nstruct Motor {\n    r_w: f32,\n    r_x: f32,\n    r_y: f32,\n    r_z: f32,\n    d_w: f32,\n    d_x: f32,\n    d_y: f32,\n    d_z: f32,\n};\n\nstruct ScLerpParams {\n    count: u32,\n    t: f32,\n    _pad0: f32,\n    _pad1: f32,\n};\n\n@group(0) @binding(0) var<uniform> params: ScLerpParams;\n@group(0) @binding(1) var<storage, read> motors_start: array<Motor>;\n@group(0) @binding(2) var<storage, read> motors_end: array<Motor>;\n@group(0) @binding(3) var<storage, read_write> motors_out: array<Motor>;\n\nfn motor_conjugate(m: Motor) -> Motor {\n    return Motor(m.r_w, -m.r_x, -m.r_y, -m.r_z, m.d_w, -m.d_x, -m.d_y, -m.d_z);\n}\n\nfn motor_mul(a: Motor, b: Motor) -> Motor {\n    let rw = a.r_w * b.r_w - a.r_x * b.r_x - a.r_y * b.r_y - a.r_z * b.r_z;\n    let rx = a.r_w * b.r_x + a.r_x * b.r_w + a.r_y * b.r_z - a.r_z * b.r_y;\n    let ry = a.r_w * b.r_y - a.r_x * b.r_z + a.r_y * b.r_w + a.r_z * b.r_x;\n    let rz = a.r_w * b.r_z + a.r_x * b.r_y - a.r_y * b.r_x + a.r_z * b.r_w;\n\n    let dw = a.r_w * b.d_w - a.r_x * b.d_x - a.r_y * b.d_y - a.r_z * b.d_z\n           + a.d_w * b.r_w - a.d_x * b.r_x - a.d_y * b.r_y - a.d_z * b.r_z;\n    let dx = a.r_w * b.d_x + a.r_x * b.d_w + a.r_y * b.d_z - a.r_z * b.d_y\n           + a.d_w * b.r_x + a.d_x * b.r_w + a.d_y * b.r_z - a.d_z * b.r_y;\n    let dy = a.r_w * b.d_y - a.r_x * b.d_z + a.r_y * b.d_w + a.r_z * b.d_x\n           + a.d_w * b.r_y - a.d_x * b.r_z + a.d_y * b.r_w + a.d_z * b.r_x;\n    let dz = a.r_w * b.d_z + a.r_x * b.d_y - a.r_y * b.d_x + a.r_z * b.d_w\n           + a.d_w * b.r_z + a.d_x * b.r_y - a.d_y * b.r_x + a.d_z * b.r_w;\n\n    return Motor(rw, rx, ry, rz, dw, dx, dy, dz);\n}\n\n@compute @workgroup_size(64)\nfn pga_sclerp_main(@builtin(global_invocation_id) global_id: vec3<u32>) {\n    let idx = global_id.x;\n    if (idx >= params.count) {\n        return;\n    }\n\n    let m0 = motors_start[idx];\n    let m1 = motors_end[idx];\n    let t = params.t;\n\n    let m0_inv = motor_conjugate(m0);\n    var delta = motor_mul(m0_inv, m1);\n\n    if (delta.r_w < 0.0) {\n        delta = Motor(-delta.r_w, -delta.r_x, -delta.r_y, -delta.r_z,\n                      -delta.d_w, -delta.d_x, -delta.d_y, -delta.d_z);\n    }\n\n    // Logarithm -> Scale by t -> Exponential\n    let sin_sq = delta.r_x * delta.r_x + delta.r_y * delta.r_y + delta.r_z * delta.r_z;\n    var res_motor: Motor;\n\n    if (sin_sq < 1e-8) {\n        // Pure translation limit\n        let scaled_t = Motor(1.0, 0.0, 0.0, 0.0,\n                             0.0, delta.d_x * t, delta.d_y * t, delta.d_z * t);\n        res_motor = motor_mul(m0, scaled_t);\n    } else {\n        let sin_theta = sqrt(sin_sq);\n        let theta = atan2(sin_theta, delta.r_w);\n        let inv_sin = 1.0 / sin_theta;\n\n        let pitch = -delta.d_w * inv_sin;\n        let axis = vec3<f32>(delta.r_x * inv_sin, delta.r_y * inv_sin, delta.r_z * inv_sin);\n\n        let scaled_theta = theta * t;\n        let sin_scaled = sin(scaled_theta);\n        let cos_scaled = cos(scaled_theta);\n\n        let exp_rw = cos_scaled;\n        let exp_rx = axis.x * sin_scaled;\n        let exp_ry = axis.y * sin_scaled;\n        let exp_rz = axis.z * sin_scaled;\n\n        let d_trans = vec3<f32>(delta.d_x - pitch * delta.r_x,\n                                delta.d_y - pitch * delta.r_y,\n                                delta.d_z - pitch * delta.r_z) * t;\n\n        let exp_motor = Motor(exp_rw, exp_rx, exp_ry, exp_rz,\n                              0.0, d_trans.x, d_trans.y, d_trans.z);\n        res_motor = motor_mul(m0, exp_motor);\n    }\n\n    motors_out[idx] = res_motor;\n}\n";