commit e03821f3c7bf02b1d5a20e36c07904476e846053
parent 0079f843e65e18d488bcdb11dcd411df204c6615
Author: umhau <umhau@example.invalid>
Date: Wed, 21 Oct 2026 12:00:19 +0000
Apply Reverb factory lens and chromatic calibration
Diffstat:
12 files changed, 998 insertions(+), 21 deletions(-)
diff --git a/DEPENDENCIES.md b/DEPENDENCIES.md
@@ -16,3 +16,17 @@
## Drivers
- None. Headset HID devices are driven from user space via IOKit (IOHIDManager).
+
+## Third-party code and provenance
+- **Monado** (https://gitlab.freedesktop.org/monado/monado), Boost Software License 1.0, full text in
+ `LICENSES/BSL-1.0.txt`. Used for:
+ - HP Reverb G1 HID protocol (wake sequence, IMU report layout, config-store read sequence): adapted from
+ `src/xrt/drivers/wmr/wmr_hmd.c` (Copyright 2018 Philipp Zabel; 2020-2021 N Madsen; 2020-2023 Collabora, Ltd.)
+ and `wmr_protocol.h`. Notices retained in `Sources/VRWorkstation/HMD.swift` (vr0).
+ - Factory calibration decoding + Polynomial-3K lens distortion: ported in `Sources/VRWorkstation/Calibration.swift` (offline checks: `Tests/CalibrationChecks.swift`) from
+ `wmr_config.c/.h`, `wmr_config_key.h` (Copyright 2021 Jan Schmidt; key derived with
+ https://github.com/pH5/wmr-config) and `src/xrt/auxiliary/util/u_distortion_mesh.c`
+ (Copyright 2019-2021 Collabora, Ltd.). Notices retained in the file header.
+- **Linux tipd driver** (drivers/usb/typec/tipd, GPL-2.0): consulted for TI/Apple CD321x register and command
+ semantics only; no code copied.
+- **Wintch/reverb-g2** docs: consulted for headset behaviour notes; no code copied.
diff --git a/LICENSES/BSL-1.0.txt b/LICENSES/BSL-1.0.txt
@@ -0,0 +1,23 @@
+Boost Software License - Version 1.0 - August 17th, 2003
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by this
+license (the "Software") to use, reproduce, display, distribute, execute, and
+transmit the Software, and to prepare derivative works of the Software, and to
+permit third-parties to whom the Software is furnished to do so, all subject to
+the following:
+
+The copyright notices in the Software and this entire statement, including the
+above license grant, this restriction and the following disclaimer, must be
+included in all copies of the Software, in whole or in part, and all derivative
+works of the Software, unless such copies or derivative works are solely in the
+form of machine-executable object code generated by a source language
+processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY
+DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
diff --git a/NOTES.md b/NOTES.md
@@ -91,3 +91,36 @@ for 60 Hz-only operation).
- GPU warp check (Tests/LensWarpChecks.swift, evidence/lenswarp-gpu-checks.txt): 1632 channel samples, max error
0.0041511 against the Double CPU model. Resolution is limited by 8-bit coordinate gradients (~12 texels per step);
catches gross/channel-swap errors, not sub-12-texel ones; blue position is not checked.
+
+## Factory optical correction (2026-10-20)
+
+Optical compensation is separate from head tracking. The factory store was read twice (10,737 bytes),
+and both reads matched byte for byte. The saved JSON supplies the per-eye affine transform, asymmetric field of view,
+and separate Polynomial-3K coefficients for red, green, and blue. Source: Monado WMR driver and distortion utilities;
+see DEPENDENCIES.md and LICENSES/BSL-1.0.txt for provenance.
+
+The renderer now always uses that calibration: 6048x3024 offscreen stereo rendering, then a per-color correction pass
+into native 4320x2160. The optics grid uses angular coordinates, so its straight-ahead cross agrees between eyes.
+Raw output snapshots intentionally show curved lines and separated colors: the physical lenses should cancel that
+pre-compensation. That is a software/model expectation, not proof that all observed optical defocus is corrected.
+
+Validation: offline calibration fixtures pass; the production Metal coordinate function agrees with the Double CPU
+model over 3,750 RGB coordinate pairs (max normalized error 4.574e-7). The additional end-to-end 8-bit gradient sampling
+check passes (1,632 channel samples, max error 0.004152, limited by 8-bit quantization). Asymmetric projection edges
+also pass. The 180 s calibrated hardware trial exits cleanly: 89.996 presented fps in the 160 s settled interval,
+zero display removes, one additional skipped frame, 0 GPU errors, 5.97 ms GPU/frame. Detailed results are in evidence/.
+Physical optical validation remains separate from software/model checks. Optical focus/fit and the headset's intrinsic peripheral limits
+may remain after correct geometric/chromatic compensation; do not claim blur has been eliminated based on GPU tests.
+
+Offline checks:
+```sh
+swiftc -O Sources/VRWorkstation/Calibration.swift Tests/CalibrationChecks.swift -o /tmp/vr-calibration-checks
+/tmp/vr-calibration-checks
+swiftc -O -D LENS_WARP_CHECKS Sources/VRWorkstation/Calibration.swift Sources/VRWorkstation/LensWarp.swift Tests/LensWarpChecks.swift -framework Metal -o /tmp/vr-lenswarp-checks
+/tmp/vr-lenswarp-checks "$HOME/Library/Application Support/VRWorkstation/factory.json"
+```
+
+The calibrated live build also passed a forced panel-off recovery: display ADD after 4.576 s, then the known
+0.654 s post-reset retrain healed automatically; native 90 Hz rendering resumed with no GPU errors.
+See `evidence/calibrated-recovery-trial.json` and `evidence/calibrated-recovery-live.txt`.
+The running demo logs to `/tmp/vr-calibrated-live.txt`; Q or Escape in its window quits it.
diff --git a/SETUP.md b/SETUP.md
@@ -79,6 +79,17 @@ zsh tools/build.sh # builds the app + the signe
(`SLSWaitThroughReconfigurationCycleForcingNotificationHandling`). Any headset display ADD cancels a pending reset.
- The helper reports failure (exit 1) unless the controller's DATA1 result is 0. A result of 0x04 has been seen after
an accepted DRST that still restored the display; the app treats ADD, not the helper's exit code, as proof.
+- First launch reads the headset's factory optical calibration before enabling the rendering session, then caches it at
+ `~/Library/Application Support/VRWorkstation/factory.json`. Later launches validate and reuse that file.
+ `--calibration /path/to/factory.json` explicitly selects another saved calibration; invalid calibration stops startup.
+- Each eye renders with its calibrated asymmetric field of view to a 3024x3024 target. A final pass applies the factory
+ Polynomial-3K lens correction separately to red, green, and blue before native 4320x2160 output. There is no
+ uncalibrated rendering fallback. Eye spacing remains 63 mm; head tracking is not connected to the scene yet.
+- `--optics-grid` shows a stationary angular grid, with its green cross at straight ahead (tangent 0,0).
+ `--snapshot /tmp/calibrated.png` renders through the same correction without opening the headset.
+- Calibrated rendering passed a 180 s trial: 89.996 presented fps over the settled health interval, no display drops,
+ 0 GPU errors, approximately 5.97 ms GPU/frame, clean shutdown. See `evidence/calibrated-90hz-trial.json`.
+ Software validation is complete; physical optical validation remains separate from software/model checks.
- The app captures the headset display (no desktop on it) and uses its native unscaled 4320x2160 @ 90 Hz mode.
If macOS changes the mode by itself, the app restores 90 Hz (also checked every 10 s, CoreGraphics only).
- Rendering runs on a dedicated "Headset Metal rendering" thread: a `CAMetalDisplayLink` on that thread's run loop
diff --git a/Sources/VRWorkstation/App.swift b/Sources/VRWorkstation/App.swift
@@ -265,6 +265,9 @@ final class HeadsetSession: NSObject, NSApplicationDelegate {
}
func runApp() {
+ do { try prepareFactoryOptics() } catch {
+ print("cannot load headset optical calibration:", error); exit(1)
+ }
let app = NSApplication.shared
app.setActivationPolicy(.regular)
let session: HeadsetSession
@@ -289,3 +292,20 @@ func runApp() {
}
withExtendedLifetime((session, signals)) { app.run() }
}
+
+/// Read once before opening the rendering session; later starts use the validated local cache.
+private func prepareFactoryOptics() throws {
+ if option("--calibration") != nil || FileManager.default.fileExists(atPath: HeadsetOptics.cacheURL.path) {
+ _ = try HeadsetOptics.loadCached()
+ return
+ }
+ let reader = HMD()
+ try reader.start(mode: .observe)
+ defer { reader.stop() }
+ let block = try WMRConfigBlock(raw: [UInt8](reader.readFactoryConfiguration()))
+ _ = try HeadsetOptics(json: block.json)
+ let cache = HeadsetOptics.cacheURL
+ try FileManager.default.createDirectory(at: cache.deletingLastPathComponent(), withIntermediateDirectories: true)
+ try block.json.write(to: cache, options: .atomic)
+ log("cached headset factory optical calibration")
+}
diff --git a/Sources/VRWorkstation/Calibration.swift b/Sources/VRWorkstation/Calibration.swift
@@ -0,0 +1,405 @@
+// HP Reverb G1 (Windows Mixed Reality) factory calibration: config-block decoding, JSON parsing, and the
+// Polynomial-3K lens distortion model. Pure Swift (Foundation + simd), no hardware access.
+//
+// Ported from Monado (https://gitlab.freedesktop.org/monado/monado), src/xrt/drivers/wmr/{wmr_hmd.c,
+// wmr_config.c, wmr_config.h, wmr_config_key.h, wmr_protocol.h} and src/xrt/auxiliary/util/u_distortion_mesh.c.
+// Upstream copyright notices (each file is SPDX-License-Identifier: BSL-1.0):
+// wmr_hmd.c: Copyright 2018, Philipp Zabel. Copyright 2020-2021, N Madsen. Copyright 2020-2023, Collabora, Ltd.
+// wmr_protocol.h: Copyright 2018, Philipp Zabel. Copyright 2020-2021, N Madsen. Copyright 2020-2021, Collabora, Ltd.
+// wmr_config.c/.h, wmr_config_key.h: Copyright 2021, Jan Schmidt.
+// u_distortion_mesh.c: Copyright 2019-2021, Collabora, Ltd.
+// Full license text: LICENSES/BSL-1.0.txt
+// The XOR key (wmr_config_key.h) was derived by the Monado/OpenHMD authors from config blocks read with
+// https://github.com/pH5/wmr-config.
+//
+// Boost Software License - Version 1.0 - August 17th, 2003
+// Permission is hereby granted, free of charge, to any person or organization obtaining a copy of the software and
+// accompanying documentation covered by this license (the "Software") to use, reproduce, display, distribute,
+// execute, and transmit the Software, and to prepare derivative works of the Software, and to permit third-parties to
+// whom the Software is furnished to do so, all subject to the following: The copyright notices in the Software and
+// this entire statement, including the above license grant, this restriction and the following disclaimer, must be
+// included in all copies of the Software, in whole or in part, and all derivative works of the Software, unless such
+// copies or derivative works are solely in the form of machine-executable object code generated by a source language
+// processor. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
+// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO
+// EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER LIABILITY,
+// WHETHER IN CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+// Self-test (synthetic fixtures only):
+// swiftc -O Sources/VRWorkstation/Calibration.swift Tests/CalibrationChecks.swift -o /tmp/calibration-selftest && /tmp/calibration-selftest
+import Foundation
+import simd
+
+// MARK: - HID transport protocol (HoloLens Sensors 045e:0659), Monado wmr_read_config_part / wmr_read_config_raw
+
+/// Request/reply layout for reading the config store. The caller owns the HID device and its threading:
+/// - request: OUTPUT report, 64 bytes, `[0x02, command, 0, 0, ...]` (report id 0x02)
+/// - reply: INPUT report with id 0x02 (WMR_MS_HOLOLENS_MSG_CONTROL), 33 bytes. Other input reports (IMU id 0x01,
+/// controller traffic) must be ignored while waiting. Monado waits up to 100 ms per read.
+/// Sequence to read one "part" of type T (T = 0x06 metadata, then T = 0x04 data):
+/// 1. send 0x0b; expect a 33-byte reply with byte0 == 0x02
+/// 2. send T; expect a 33-byte reply with byte0 == 0x02
+/// 3. repeat: send 0x08; reply byte1 == 0x01 => chunk: length = byte2, payload = bytes[3 ..< 3+length];
+/// reply byte1 == 0x02 => end of part; anything else => error.
+/// Whole config: part(0x06) -> 84-byte metadata; dataSize = meta[0] | meta[1] << 8; then part(0x04) -> dataSize bytes.
+/// Read the config BEFORE enabling the IMU (`[0x02, 0x07]` uses the same report id), or filter replies carefully.
+enum WMRConfigProtocol {
+ static let reportID: UInt8 = 0x02
+ static let replyLength = 33
+ static let selectCommand: UInt8 = 0x0b
+ static let readChunkCommand: UInt8 = 0x08
+ static let metadataPart: UInt8 = 0x06
+ static let dataPart: UInt8 = 0x04
+
+ /// 64-byte output report for `command`.
+ static func request(_ command: UInt8) -> [UInt8] {
+ var report = [UInt8](repeating: 0, count: 64)
+ report[0] = reportID; report[1] = command
+ return report
+ }
+
+ enum ChunkReply: Equatable { case data([UInt8]), end, invalid }
+
+ /// Interprets the reply to a 0x08 (read chunk) request.
+ static func parseChunk(_ reply: [UInt8]) -> ChunkReply {
+ guard reply.count == replyLength, reply[0] == reportID else { return .invalid }
+ switch reply[1] {
+ case 0x01:
+ let length = Int(reply[2])
+ guard 3 + length <= reply.count else { return .invalid }
+ return .data(Array(reply[3 ..< 3 + length]))
+ case 0x02: return .end
+ default: return .invalid
+ }
+ }
+
+ /// True for a valid acknowledgement of a 0x0b or part-select request.
+ static func isAck(_ reply: [UInt8]) -> Bool { reply.count == replyLength && reply[0] == reportID }
+
+ /// Size of the data part, from the metadata part.
+ static func dataSize(fromMetadata meta: [UInt8]) -> Int? {
+ meta.count >= 2 ? Int(meta[0]) | Int(meta[1]) << 8 : nil
+ }
+}
+
+// MARK: - Config block (header + XOR-obfuscated JSON), Monado wmr_read_config / struct wmr_config_header
+
+struct WMRConfigBlock {
+ var manufacturer = "", device = "", serial = "", uid = "", name = "", revision = "", revisionDate = ""
+ var json: Data
+
+ enum DecodeError: Error, CustomStringConvertible {
+ case tooShort(Int), badJSONRange(start: Int, size: Int, total: Int)
+ var description: String {
+ switch self {
+ case .tooShort(let n): return "config block too short (\(n) bytes)"
+ case .badJSONRange(let s, let z, let t): return "invalid JSON range start=\(s) size=\(z) total=\(t)"
+ }
+ }
+ }
+
+ // struct wmr_config_header (packed by construction: u32, u32, then char arrays)
+ static let headerSize = 8 + 0x40 + 0x40 + 0x40 + 0x26 + 0xd5 + 0x40 + 0x20 + 0x20
+
+ /// Decodes the raw data part read from the headset.
+ init(raw: [UInt8]) throws {
+ guard raw.count >= Self.headerSize else { throw DecodeError.tooShort(raw.count) }
+ func u32(_ o: Int) -> Int { Int(raw[o]) | Int(raw[o + 1]) << 8 | Int(raw[o + 2]) << 16 | Int(raw[o + 3]) << 24 }
+ func text(_ o: Int, _ n: Int) -> String {
+ String(decoding: raw[o ..< o + n].prefix { $0 != 0 }, as: UTF8.self)
+ }
+ let jsonStart = u32(0), jsonSize = u32(4)
+ // Monado: json_start < data_size and (data_size - json_start) >= json_size
+ guard jsonStart < raw.count, raw.count - jsonStart >= jsonSize, jsonSize > 2 else {
+ throw DecodeError.badJSONRange(start: jsonStart, size: jsonSize, total: raw.count)
+ }
+ var o = 8
+ manufacturer = text(o, 0x40); o += 0x40
+ device = text(o, 0x40); o += 0x40
+ serial = text(o, 0x40); o += 0x40
+ uid = text(o, 0x26); o += 0x26 + 0xd5
+ name = text(o, 0x40); o += 0x40
+ revision = text(o, 0x20); o += 0x20
+ revisionDate = text(o, 0x20)
+ // The JSON starts 2 bytes after json_start and is XORed with the repeating key.
+ let start = jsonStart + 2, count = jsonSize - 2
+ var bytes = [UInt8](repeating: 0, count: count)
+ let key = wmrConfigKey
+ for i in 0 ..< count { bytes[i] = raw[start + i] ^ key[i % key.count] }
+ json = Data(bytes.prefix { $0 != 0 })
+ }
+
+ /// Inverse of the decoder, for fixtures: XORs `json` into a block whose header points at it.
+ static func encodeForTesting(json: Data, name: String = "Test Headset") -> [UInt8] {
+ var raw = [UInt8](repeating: 0, count: headerSize)
+ let jsonStart = headerSize
+ let payload = [UInt8](json) + [0]
+ let jsonSize = payload.count + 2
+ for (i, b) in [UInt32(jsonStart), UInt32(jsonSize)].enumerated() {
+ for k in 0 ..< 4 { raw[i * 4 + k] = UInt8((b >> (8 * UInt32(k))) & 0xff) }
+ }
+ let nameOffset = 8 + 0x40 * 3 + 0x26 + 0xd5
+ for (i, c) in name.utf8.prefix(0x3f).enumerated() { raw[nameOffset + i] = c }
+ raw += [0, 0] // the two bytes skipped before the JSON
+ for (i, b) in payload.enumerated() { raw.append(b ^ wmrConfigKey[i % wmrConfigKey.count]) }
+ return raw
+ }
+}
+
+// MARK: - Parsed calibration, Monado wmr_config_parse / wmr_config_parse_display / wmr_inertial_sensor_config_parse
+
+/// One color channel of the Polynomial-3K model: radial distortion around `eyeCenter` (full-panel pixels).
+struct Poly3KChannel {
+ var eyeCenter: SIMD2<Double> // ModelParameters[0], [1]
+ var k: SIMD3<Double> // ModelParameters[2...4]
+ var displaySize: SIMD2<Double> // full panel, e.g. 4320 x 2160
+}
+
+struct EyeCalibration {
+ var displaySize: SIMD2<Double>
+ var visibleCenter: SIMD2<Double>
+ var visibleRadius: Double
+ var affine: simd_double3x3 // as stored: row-major JSON loaded into rows
+ var inverseAffine: simd_double3x3
+ var rotation: simd_double3x3 // Rt.Rotation, row-major (to HT0)
+ var translation: SIMD3<Double> // Rt.Translation, metres (to HT0)
+ var channels: [Poly3KChannel] // red, green, blue
+}
+
+struct InertialSensorCalibration {
+ enum Kind: String { case gyro = "CALIBRATION_InertialSensorType_Gyro",
+ accelerometer = "CALIBRATION_InertialSensorType_Accelerometer",
+ magnetometer = "CALIBRATION_InertialSensorType_Magnetometer" }
+ var kind: Kind
+ var rotation: simd_double3x3 // row-major (to HT0)
+ var translation: SIMD3<Double>
+ var biasOffsets: SIMD3<Double> // constant terms of BiasTemperatureModel
+ var mixMatrix: simd_double3x3 // constant terms of MixingMatrixTemperatureModel
+}
+
+struct HeadsetCalibration {
+ var eyes: [EyeCalibration] // [left, right]
+ var sensors: [InertialSensorCalibration]
+
+ enum ParseError: Error, CustomStringConvertible {
+ case missing(String), badValue(String), unsupportedModel(String)
+ var description: String {
+ switch self {
+ case .missing(let k): return "calibration JSON: missing \(k)"
+ case .badValue(let k): return "calibration JSON: bad value for \(k)"
+ case .unsupportedModel(let m): return "calibration JSON: unsupported distortion model \(m)"
+ }
+ }
+ }
+
+ init(json: Data) throws {
+ guard let root = try JSONSerialization.jsonObject(with: json) as? [String: Any],
+ let info = root["CalibrationInformation"] as? [String: Any] else { throw ParseError.missing("CalibrationInformation") }
+ guard let displays = info["Displays"] as? [[String: Any]] else { throw ParseError.missing("Displays") }
+ var left: EyeCalibration?, right: EyeCalibration?
+ for d in displays {
+ let eye = try Self.parseDisplay(d)
+ switch d["AssignedEye"] as? String {
+ case "CALIBRATION_DisplayEyeLeft": left = eye
+ case "CALIBRATION_DisplayEyeRight": right = eye
+ default: throw ParseError.badValue("AssignedEye")
+ }
+ }
+ guard let left, let right else { throw ParseError.missing("left and right displays") }
+ eyes = [left, right]
+ sensors = try ((info["InertialSensors"] as? [[String: Any]]) ?? []).compactMap(Self.parseSensor)
+ }
+
+ // MARK: helpers
+
+ static func numbers(_ value: Any?, _ key: String, count: Int? = nil) throws -> [Double] {
+ guard let array = value as? [Any] else { throw ParseError.missing(key) }
+ let values = try array.map { v -> Double in
+ guard let n = v as? NSNumber else { throw ParseError.badValue(key) }
+ return n.doubleValue
+ }
+ if let count, values.count < count { throw ParseError.badValue(key) }
+ return values
+ }
+
+ static func number(_ dict: [String: Any], _ key: String) throws -> Double {
+ guard let n = dict[key] as? NSNumber else { throw ParseError.missing(key) }
+ return n.doubleValue
+ }
+
+ /// Nine row-major values -> matrix whose rows are those values (simd is column-major, so build from rows).
+ static func rowMajor3x3(_ v: [Double]) -> simd_double3x3 {
+ simd_double3x3(rows: [SIMD3(v[0], v[1], v[2]), SIMD3(v[3], v[4], v[5]), SIMD3(v[6], v[7], v[8])])
+ }
+
+ static func parseRt(_ dict: [String: Any]) throws -> (simd_double3x3, SIMD3<Double>) {
+ guard let rt = dict["Rt"] as? [String: Any] else { throw ParseError.missing("Rt") }
+ let r = try numbers(rt["Rotation"], "Rt.Rotation", count: 9)
+ let t = try numbers(rt["Translation"], "Rt.Translation", count: 3)
+ return (rowMajor3x3(r), SIMD3(t[0], t[1], t[2]))
+ }
+
+ static func parseDisplay(_ d: [String: Any]) throws -> EyeCalibration {
+ let affine = rowMajor3x3(try numbers(d["Affine"], "Affine", count: 9))
+ let size = SIMD2(try number(d, "DisplayWidth"), try number(d, "DisplayHeight"))
+ guard let vc = d["VisibleAreaCenter"] as? [String: Any] else { throw ParseError.missing("VisibleAreaCenter") }
+ let (rotation, translation) = try parseRt(d)
+ var channels: [Poly3KChannel] = []
+ for name in ["DistortionRed", "DistortionGreen", "DistortionBlue"] {
+ guard let dist = d[name] as? [String: Any] else { throw ParseError.missing(name) }
+ let model = dist["ModelType"] as? String ?? "?"
+ guard model == "CALIBRATION_DisplayDistortionModelPolynomial3K" else { throw ParseError.unsupportedModel(model) }
+ let p = try numbers(dist["ModelParameters"], "\(name).ModelParameters", count: 5)
+ channels.append(Poly3KChannel(eyeCenter: SIMD2(p[0], p[1]), k: SIMD3(p[2], p[3], p[4]), displaySize: size))
+ }
+ return EyeCalibration(displaySize: size, visibleCenter: SIMD2(try number(vc, "X"), try number(vc, "Y")),
+ visibleRadius: try number(d, "VisibleAreaRadius"), affine: affine,
+ inverseAffine: affine.inverse, rotation: rotation, translation: translation,
+ channels: channels)
+ }
+
+ static func parseSensor(_ s: [String: Any]) throws -> InertialSensorCalibration? {
+ guard let kindName = s["SensorType"] as? String, let kind = InertialSensorCalibration.Kind(rawValue: kindName) else { return nil }
+ let (rotation, translation) = try parseRt(s)
+ // Monado takes only the constant coefficient of each temperature model (the others are 0 in practice).
+ // MixingMatrixTemperatureModel: 3x3 entries x 4 coefficients; BiasTemperatureModel: 3 axes x 4 coefficients.
+ let mix = (try? numbers(s["MixingMatrixTemperatureModel"], "MixingMatrixTemperatureModel", count: 36)) ?? []
+ let bias = (try? numbers(s["BiasTemperatureModel"], "BiasTemperatureModel", count: 12)) ?? []
+ let mixMatrix = mix.count >= 36 ? rowMajor3x3((0 ..< 9).map { mix[$0 * 4] }) : matrix_identity_double3x3
+ let biasOffsets = bias.count >= 12 ? SIMD3(bias[0], bias[4], bias[8]) : .zero
+ return InertialSensorCalibration(kind: kind, rotation: rotation, translation: translation,
+ biasOffsets: biasOffsets, mixMatrix: mixMatrix)
+ }
+}
+
+// MARK: - Poly3K distortion, Monado u_compute_distortion_bounds_poly_3k / u_compute_distortion_poly_3k
+
+/// Render-target bounds for one eye: tangents of the half-angles (Y down, as in Monado's distortion space).
+struct Poly3KBounds {
+ var texXRange: SIMD2<Double> // (tan left, tan right); left is negative
+ var texYRange: SIMD2<Double> // (tan up, tan down) in Y-down space; up is negative
+ /// OpenXR-style angles in radians (left/down negative), for building an asymmetric projection.
+ var angleLeft: Double { atan(texXRange.x) }
+ var angleRight: Double { atan(texXRange.y) }
+ var angleUp: Double { -atan(texYRange.x) }
+ var angleDown: Double { -atan(texYRange.y) }
+}
+
+extension EyeCalibration {
+ /// `view` 0 = left, 1 = right (the right eye's panel X starts at displaySize.x / 2).
+ func distortionBounds(view: Int) -> Poly3KBounds {
+ var left = 0.0, right = 0.0, up = 0.0, down = 0.0
+ for c in channels {
+ let half = c.displaySize.x / 2, v = Double(view)
+ let pixCoords: [SIMD2<Double>] = [
+ SIMD2(v * half - c.eyeCenter.x, 0), // -eye_center_x, 0
+ SIMD2(0, -c.eyeCenter.y), // 0, -eye_center_y
+ SIMD2((1 + v) * half - c.eyeCenter.x, 0), // width - eye_center_x, 0
+ SIMD2(0, c.displaySize.y - c.eyeCenter.y), // 0, height - eye_center_y
+ ]
+ for pix in pixCoords {
+ let vp = viewPlane(pix, c)
+ if pix.x < 0 { left = min(left, vp.x) } else { right = max(right, vp.x) }
+ if pix.y < 0 { up = min(up, vp.y) } else { down = max(down, vp.y) }
+ }
+ }
+ return Poly3KBounds(texXRange: SIMD2(left, right), texYRange: SIMD2(up, down))
+ }
+
+ /// For an output position (u, v) in 0...1 across THIS eye's half of the panel (v = 0 at the top), returns the
+ /// red/green/blue sample coordinates in 0...1 of the eye's undistorted render target (rendered with `bounds`).
+ func distort(view: Int, u: Double, v: Double, bounds: Poly3KBounds) -> (r: SIMD2<Double>, g: SIMD2<Double>, b: SIMD2<Double>) {
+ var out: [SIMD2<Double>] = []
+ for c in channels {
+ // Scale the 0..1 UV back to pixels relative to the distortion center (right eye starts at width / 2).
+ let pix = SIMD2((u + Double(view)) * (c.displaySize.x / 2) - c.eyeCenter.x, v * c.displaySize.y - c.eyeCenter.y)
+ let vp = viewPlane(pix, c)
+ out.append(SIMD2((vp.x - bounds.texXRange.x) / (bounds.texXRange.y - bounds.texXRange.x),
+ (vp.y - bounds.texYRange.x) / (bounds.texYRange.y - bounds.texYRange.x)))
+ }
+ return (out[0], out[1], out[2])
+ }
+
+ /// Distort a pixel offset radially, then map back to view-plane tangents with the inverse affine transform.
+ private func viewPlane(_ pix: SIMD2<Double>, _ c: Poly3KChannel) -> SIMD2<Double> {
+ let r2 = simd_dot(pix, pix)
+ let d = 1 + r2 * (c.k.x + r2 * (c.k.y + r2 * c.k.z))
+ let p = SIMD3(pix.x * d + c.eyeCenter.x, pix.y * d + c.eyeCenter.y, 1)
+ let vp = inverseAffine * p // Monado's math_matrix_3x3_transform_vec3 on a row-major matrix = M * p
+ return SIMD2(vp.x / vp.z, vp.y / vp.z)
+ }
+
+ /// Interpupillary distance contribution from the per-eye translations (metres).
+ static func ipd(_ left: EyeCalibration, _ right: EyeCalibration) -> Double { simd_length(right.translation - left.translation) }
+}
+
+// MARK: - XOR key, Monado wmr_config_key.h (1024 bytes)
+
+let wmrConfigKey: [UInt8] = [
+ 0x2F, 0xC8, 0x0F, 0x38, 0xDD, 0x00, 0xF6, 0x5C, 0xA1, 0x31, 0xEF, 0xF1, 0xEA, 0x6F, 0xA0, 0xF8,
+ 0x26, 0xB5, 0x9B, 0x39, 0xCF, 0x3A, 0x88, 0xC8, 0x2E, 0x17, 0xC0, 0x63, 0x5B, 0x46, 0x27, 0xBB,
+ 0x98, 0x2F, 0x0E, 0x2A, 0x90, 0x4B, 0x28, 0x2D, 0x82, 0x76, 0xE5, 0x28, 0x72, 0x50, 0x8A, 0xF0,
+ 0xBF, 0x84, 0x54, 0x3B, 0xA8, 0x77, 0x91, 0xCE, 0x87, 0x80, 0x53, 0x2F, 0x07, 0xAD, 0x1B, 0x3F,
+ 0x8C, 0x67, 0x33, 0x2E, 0xEB, 0x6A, 0x2A, 0x52, 0x77, 0x7C, 0x1F, 0x02, 0x11, 0x9E, 0x2A, 0x59,
+ 0x5C, 0x94, 0x0E, 0x4F, 0xF5, 0x44, 0x54, 0x01, 0xE7, 0x8F, 0x66, 0xF0, 0xAD, 0x68, 0x71, 0x3C,
+ 0x6D, 0x2E, 0x1C, 0xE3, 0x11, 0x46, 0xF7, 0x7F, 0x02, 0x6C, 0x15, 0xA0, 0x10, 0xEE, 0x3B, 0x14,
+ 0xAE, 0x6C, 0xA7, 0x3F, 0xAF, 0x83, 0x6A, 0xD7, 0x12, 0x88, 0x53, 0xFE, 0xEB, 0x5C, 0x78, 0x85,
+ 0xAF, 0x1F, 0x80, 0x7F, 0xB6, 0xDA, 0x7C, 0x0E, 0x84, 0xB5, 0x02, 0x8E, 0x92, 0xA3, 0x5B, 0x83,
+ 0x56, 0x11, 0x7B, 0xDF, 0x80, 0xB3, 0x4C, 0x13, 0x8E, 0x61, 0x61, 0xE6, 0x82, 0x8C, 0xDA, 0x08,
+ 0x76, 0x88, 0xBF, 0x85, 0x7F, 0xE4, 0x28, 0x26, 0x1F, 0xB5, 0x67, 0x80, 0x63, 0xD9, 0x26, 0xD4,
+ 0x91, 0xD1, 0xC1, 0x51, 0xCE, 0x61, 0x64, 0x2B, 0x56, 0xAE, 0x3D, 0x06, 0x5D, 0xCD, 0xF7, 0x05,
+ 0x9A, 0x6F, 0xEB, 0x2E, 0xC2, 0x69, 0x42, 0x86, 0xBE, 0x78, 0x32, 0xC3, 0xA8, 0x94, 0xA3, 0x97,
+ 0x84, 0x07, 0xF1, 0x6E, 0x3F, 0x10, 0xDE, 0x2B, 0xB1, 0x41, 0x1A, 0x59, 0xE3, 0x7F, 0x23, 0xDE,
+ 0xF3, 0x13, 0x23, 0xD1, 0x60, 0x1C, 0xBB, 0xC5, 0x4A, 0xB1, 0xC6, 0x02, 0x38, 0x7B, 0xFE, 0xEF,
+ 0xB6, 0x50, 0x16, 0x23, 0x4B, 0xD4, 0xEF, 0xEA, 0x67, 0xEC, 0x44, 0x2C, 0xC0, 0xA6, 0x2F, 0x0D,
+ 0x6E, 0x17, 0x52, 0xC8, 0x26, 0xCB, 0x63, 0x85, 0x72, 0x8D, 0xBA, 0xD8, 0x09, 0xA3, 0x89, 0x64,
+ 0x70, 0x12, 0xC6, 0xDF, 0x4C, 0x28, 0xD4, 0xB8, 0x49, 0x18, 0x69, 0x22, 0x36, 0xF1, 0x00, 0xC3,
+ 0x91, 0xB3, 0x7C, 0xA0, 0xAA, 0x7D, 0x9E, 0x27, 0x65, 0xCD, 0x16, 0x3C, 0x71, 0x3C, 0xCC, 0xF5,
+ 0x02, 0xD1, 0xA5, 0x06, 0x95, 0xB2, 0x1E, 0x71, 0x92, 0x6F, 0xC2, 0xD2, 0xEF, 0x58, 0x7B, 0xD0,
+ 0x53, 0x5E, 0xE9, 0xB6, 0xCA, 0x1C, 0x13, 0x96, 0xAC, 0xF1, 0xF5, 0x19, 0xD9, 0x8A, 0x1D, 0xA9,
+ 0x0D, 0xAE, 0xD0, 0xF4, 0xB3, 0xDD, 0x2D, 0x43, 0x6E, 0x41, 0x22, 0xDC, 0x09, 0xA4, 0x92, 0x42,
+ 0xC5, 0x32, 0x7D, 0xD7, 0xA2, 0x57, 0xA5, 0xA5, 0x11, 0xD2, 0x22, 0xD0, 0xD7, 0x75, 0xFF, 0xFC,
+ 0x2F, 0x66, 0xB5, 0xA9, 0xA3, 0x2B, 0xB4, 0x2B, 0x14, 0xEF, 0xC4, 0xB4, 0x18, 0xF0, 0x56, 0x55,
+ 0x93, 0x6A, 0x30, 0xF6, 0xDF, 0x14, 0x23, 0xF7, 0x2A, 0xDC, 0x4C, 0xE7, 0x78, 0x2B, 0x66, 0x22,
+ 0xB4, 0xAC, 0x2E, 0x03, 0xF2, 0xEF, 0xEC, 0x35, 0xAF, 0x22, 0x6D, 0x05, 0x12, 0x6D, 0xC5, 0x2C,
+ 0x12, 0x9B, 0x7D, 0x66, 0x47, 0x8F, 0xC0, 0x81, 0xCD, 0xFE, 0x97, 0x4C, 0x01, 0xC1, 0xD7, 0x24,
+ 0xD8, 0x46, 0xFD, 0x29, 0xF7, 0xE1, 0x61, 0xF3, 0xA0, 0x69, 0xBD, 0x23, 0x35, 0x7E, 0x66, 0xB1,
+ 0xF6, 0x3A, 0xD9, 0xF8, 0x29, 0xA2, 0x99, 0x8E, 0xF7, 0xBC, 0xCC, 0xD6, 0x37, 0x11, 0x09, 0xC8,
+ 0x07, 0x68, 0x5D, 0xF6, 0xC2, 0x73, 0xE8, 0xE5, 0x2D, 0xA4, 0x62, 0x0E, 0x9D, 0xC2, 0x76, 0xA9,
+ 0x94, 0x06, 0x19, 0x39, 0x9F, 0x8F, 0x83, 0x62, 0xE9, 0xDE, 0xED, 0x76, 0xFD, 0xBC, 0x42, 0x77,
+ 0x1E, 0x49, 0x18, 0xD8, 0x15, 0x22, 0x55, 0x5C, 0xD3, 0xF2, 0x7F, 0xD0, 0x8A, 0x27, 0x82, 0x05,
+ 0xD3, 0xBD, 0x27, 0x0C, 0x2F, 0xB9, 0x72, 0xE9, 0x9D, 0x6B, 0xD3, 0xD6, 0xD6, 0x84, 0xA4, 0x1F,
+ 0x6C, 0x26, 0x7C, 0x61, 0xE0, 0x7E, 0x58, 0x05, 0xAD, 0xC5, 0xE1, 0x14, 0x3A, 0xD7, 0x40, 0x6A,
+ 0x52, 0x57, 0x82, 0xAA, 0x9B, 0xF0, 0xCA, 0x60, 0x5D, 0x6C, 0xC0, 0xA4, 0x6B, 0xF3, 0x87, 0x6D,
+ 0x04, 0x80, 0x2C, 0x7B, 0xEB, 0x9F, 0xD4, 0x03, 0x81, 0x91, 0xD0, 0xB9, 0x74, 0xAE, 0x19, 0xBF,
+ 0x48, 0x63, 0x8F, 0x8C, 0xEE, 0xBC, 0xB4, 0xC0, 0x16, 0x4A, 0xF5, 0x5E, 0x1C, 0x7A, 0xDB, 0xD5,
+ 0xA4, 0x16, 0x92, 0xCB, 0x52, 0x86, 0xCB, 0xD1, 0x1E, 0x1D, 0xEE, 0x90, 0x01, 0x90, 0x52, 0x52,
+ 0x52, 0x8C, 0x25, 0x0A, 0xB7, 0xDE, 0x10, 0x51, 0xB8, 0x23, 0x5C, 0xCB, 0x32, 0x6A, 0xB0, 0xB9,
+ 0xA4, 0x58, 0xB6, 0x14, 0x28, 0xF0, 0xFB, 0xC2, 0xCD, 0x6F, 0x5E, 0x10, 0x48, 0xAD, 0x1F, 0xC8,
+ 0xCE, 0x4F, 0x09, 0xDA, 0xF8, 0xD0, 0x84, 0x44, 0x8C, 0x57, 0x4B, 0xE1, 0x87, 0x5B, 0x79, 0xD0,
+ 0x93, 0x38, 0x57, 0x65, 0x31, 0x55, 0xF2, 0xD6, 0x1F, 0x6C, 0xC9, 0xD1, 0x3A, 0x17, 0x3C, 0x4F,
+ 0x97, 0x23, 0x07, 0xB9, 0xB6, 0xB5, 0x32, 0x28, 0x24, 0x0E, 0xCC, 0x1A, 0xA1, 0x74, 0x39, 0x06,
+ 0xD9, 0x52, 0xD6, 0x38, 0xFC, 0x95, 0xBF, 0x84, 0x3A, 0x76, 0xA3, 0xC3, 0x54, 0xF2, 0x71, 0x4D,
+ 0x2D, 0xE8, 0x9F, 0x58, 0x19, 0xE9, 0xD3, 0x5A, 0xCE, 0x30, 0x1E, 0xB5, 0xEE, 0xB5, 0x83, 0xF4,
+ 0xB9, 0x23, 0xF3, 0xA1, 0xFC, 0xEA, 0x68, 0x2F, 0xAF, 0x22, 0x73, 0xF2, 0x21, 0x66, 0x8C, 0x29,
+ 0xF2, 0x34, 0x7A, 0x39, 0xB9, 0x3C, 0x2C, 0x96, 0x54, 0x7A, 0x7E, 0xA5, 0x24, 0x98, 0xF7, 0x06,
+ 0x78, 0x28, 0x70, 0x7A, 0x3C, 0x73, 0x8D, 0x82, 0xB1, 0x9C, 0x1E, 0xD9, 0xDB, 0xBB, 0xEF, 0x3F,
+ 0xC2, 0x0F, 0xAF, 0x73, 0x0E, 0xC0, 0x01, 0x2E, 0x5B, 0x8A, 0xC4, 0x39, 0x5A, 0x71, 0xA9, 0x2B,
+ 0xD9, 0xD3, 0x9A, 0x0D, 0x28, 0x95, 0xFE, 0x7E, 0xD8, 0xC7, 0x73, 0xDD, 0x77, 0x52, 0x56, 0x94,
+ 0x80, 0x93, 0xD1, 0xFF, 0x02, 0x28, 0xE0, 0x18, 0xA1, 0xF2, 0x7E, 0x9A, 0x1C, 0xF2, 0x7B, 0x76,
+ 0x2C, 0xF0, 0xB7, 0x39, 0xF3, 0x10, 0x08, 0x90, 0x8F, 0xA6, 0xEB, 0x5F, 0xF5, 0x1A, 0xB1, 0x72,
+ 0xF0, 0x1B, 0x7A, 0xF4, 0xF7, 0x4D, 0x5C, 0xC0, 0x82, 0x1F, 0x27, 0xCE, 0xA4, 0x52, 0xB2, 0xE8,
+ 0x24, 0xC7, 0xCA, 0x8C, 0xB9, 0xCB, 0x6C, 0xC5, 0xA0, 0x42, 0x18, 0x7F, 0xE5, 0xFA, 0xA9, 0x8E,
+ 0xA0, 0xF4, 0x58, 0x78, 0xB9, 0x30, 0x86, 0x49, 0x01, 0x15, 0x8E, 0xB0, 0x22, 0x8C, 0xF5, 0x12,
+ 0x64, 0xE6, 0x69, 0x90, 0xD6, 0x86, 0x92, 0x9B, 0x83, 0xD4, 0xF7, 0x01, 0x15, 0x9A, 0x7C, 0xF8,
+ 0xB3, 0xCD, 0x0A, 0xA1, 0x3D, 0x49, 0x90, 0x21, 0x69, 0xD7, 0x25, 0xFC, 0x1A, 0x64, 0x22, 0x77,
+ 0x7A, 0xBF, 0x3C, 0x1C, 0x4B, 0x06, 0x6E, 0x83, 0x03, 0x5D, 0x5C, 0x76, 0xEA, 0x84, 0x29, 0xB5,
+ 0x7C, 0xC0, 0x74, 0xBC, 0x4A, 0x21, 0x7B, 0xDC, 0xFE, 0x1B, 0x1F, 0x77, 0x64, 0x20, 0x59, 0x6A,
+ 0x0B, 0x48, 0xC2, 0x0E, 0x2D, 0xFF, 0xCE, 0x4C, 0x06, 0xED, 0x0E, 0x1C, 0xB6, 0x1A, 0x62, 0x79,
+ 0xEC, 0x25, 0xD6, 0x89, 0xBF, 0x4F, 0x16, 0x75, 0x82, 0xD7, 0x98, 0x5C, 0xBA, 0x75, 0xBA, 0xD3,
+ 0x2D, 0xC7, 0x47, 0xF3, 0xB6, 0x31, 0x54, 0xE0, 0x86, 0xFE, 0x29, 0x8E, 0xE2, 0x92, 0x79, 0x89,
+ 0xE4, 0x43, 0xB4, 0x9C, 0xF7, 0xED, 0x1B, 0xA6, 0x0B, 0x0C, 0x69, 0x23, 0xF4, 0x7D, 0x0A, 0xA2,
+ 0x8C, 0xEC, 0xD5, 0x2C, 0x8E, 0xB6, 0x20, 0x8F, 0xA6, 0xB9, 0x86, 0xB8, 0xBA, 0x59, 0xA3, 0xA7,
+]
+
diff --git a/Sources/VRWorkstation/HMD.swift b/Sources/VRWorkstation/HMD.swift
@@ -1,3 +1,8 @@
+// Protocol adaptation from Monado wmr_hmd.c (BSL-1.0).
+// Copyright 2018, Philipp Zabel.
+// Copyright 2020-2021, N Madsen.
+// Copyright 2020-2023, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
// HP Reverb G1 over plain IOKit HID: wakes the display and streams the IMU.
// Protocol ported from Monado (src/xrt/drivers/wmr/wmr_hmd.c, wmr_protocol.c).
// Requires boot-arg hdcp-fuse-mask=0, otherwise macOS starts HDCP and the headset blanks.
@@ -31,6 +36,7 @@ final class HMD {
return control != nil && sensors != nil
}
private var generation = 0
+ private var configReply: [UInt8]?
private let controlBuf = UnsafeMutablePointer<UInt8>.allocate(capacity: 64)
private let sensorsBuf = UnsafeMutablePointer<UInt8>.allocate(capacity: 512)
@@ -90,6 +96,57 @@ final class HMD {
try setFeature([0x04, 0x01])
}
+ /// Read the factory store using Monado's WMR config protocol. This changes only
+ /// the sensor device's read cursor; it does not write calibration or toggle the panel.
+ /// Run on main after start(.observe), before a rendering session owns this device.
+ func readFactoryConfiguration() throws -> Data {
+ precondition(Thread.isMainThread)
+ let deadline = Date().addingTimeInterval(15)
+ func failure(_ message: String) -> NSError {
+ NSError(domain: "HMD calibration", code: 1, userInfo: [NSLocalizedDescriptionKey: message])
+ }
+ func command(_ type: UInt8) throws -> [UInt8] {
+ guard Date() < deadline else { throw failure("factory read exceeded 15 seconds") }
+ configReply = nil
+ let request = WMRConfigProtocol.request(type)
+ ioLock.lock()
+ guard let sensors else { ioLock.unlock(); throw NotFound(name: "HoloLens Sensors") }
+ let result = IOHIDDeviceSetReport(sensors, kIOHIDReportTypeOutput, 0x02, request, request.count)
+ ioLock.unlock()
+ guard result == kIOReturnSuccess else { throw hidError("factory read command", result) }
+ let replyDeadline = min(deadline, Date().addingTimeInterval(0.5))
+ while configReply == nil && Date() < replyDeadline {
+ RunLoop.main.run(until: min(replyDeadline, Date().addingTimeInterval(0.002)))
+ }
+ guard let reply = configReply, WMRConfigProtocol.isAck(reply) else {
+ throw failure(String(format: "no valid reply to config command 0x%02x", type))
+ }
+ return reply
+ }
+ func part(_ type: UInt8, limit: Int) throws -> [UInt8] {
+ _ = try command(WMRConfigProtocol.selectCommand)
+ _ = try command(type)
+ var bytes: [UInt8] = []
+ for _ in 0..<4096 {
+ switch WMRConfigProtocol.parseChunk(try command(WMRConfigProtocol.readChunkCommand)) {
+ case .end: return bytes
+ case .data(let payload):
+ guard bytes.count + payload.count <= limit else { throw failure("oversized factory config") }
+ bytes.append(contentsOf: payload)
+ case .invalid: throw failure("malformed factory config chunk")
+ }
+ }
+ throw failure("too many factory config chunks")
+ }
+ let metadata = try part(WMRConfigProtocol.metadataPart, limit: 84)
+ guard let length = WMRConfigProtocol.dataSize(fromMetadata: metadata) else { throw failure("short factory metadata") }
+ guard length > 0 else { throw failure("empty factory configuration") }
+ let bytes = try part(WMRConfigProtocol.dataPart, limit: length)
+ guard bytes.count == length else { throw failure("incomplete factory configuration") }
+ log("read factory configuration: \(length) bytes")
+ return Data(bytes)
+ }
+
/// Close host handles without sending panel-off, which can strand the DP link.
func stop() {
ioLock.lock(); defer { ioLock.unlock() }
@@ -141,6 +198,10 @@ final class HMD {
}
private func handleSensors(_ p: UnsafeMutablePointer<UInt8>, _ n: Int) {
+ if n == 33, p[0] == 0x02 {
+ configReply = Array(UnsafeBufferPointer(start: p, count: n))
+ return
+ }
guard (n == 381 || n == 497), p[0] == 0x01 else { return }
lastIMUAt = Date()
func i16(_ o: Int) -> Int32 { Int32(Int16(bitPattern: UInt16(p[o]) | UInt16(p[o + 1]) << 8)) }
diff --git a/Sources/VRWorkstation/LensWarp.swift b/Sources/VRWorkstation/LensWarp.swift
@@ -0,0 +1,211 @@
+// Polynomial-3K equations adapted from Monado u_distortion_mesh.c.
+// Copyright 2019-2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0 (see LICENSES/BSL-1.0.txt)
+import Foundation
+import Metal
+import simd
+
+struct HeadsetOptics {
+ let calibration: HeadsetCalibration
+ let bounds: [Poly3KBounds]
+ static var cacheURL: URL {
+ FileManager.default.urls(for: .applicationSupportDirectory, in: .userDomainMask)[0]
+ .appendingPathComponent("VRWorkstation/factory.json")
+ }
+
+ init(json: Data) throws {
+ calibration = try HeadsetCalibration(json: json)
+ bounds = calibration.eyes.enumerated().map { $0.element.distortionBounds(view: $0.offset) }
+ for (index, eye) in calibration.eyes.enumerated() {
+ let b = bounds[index]
+ guard eye.displaySize == SIMD2(4320, 2160),
+ b.texXRange.x.isFinite, b.texXRange.y.isFinite,
+ b.texYRange.x.isFinite, b.texYRange.y.isFinite,
+ b.texXRange.x < 0, b.texXRange.y > 0,
+ b.texYRange.x < 0, b.texYRange.y > 0 else {
+ throw NSError(domain: "Optics", code: 1, userInfo: [NSLocalizedDescriptionKey: "Invalid Reverb G1 optical calibration for eye \(index)"])
+ }
+ }
+ }
+
+ static func loadCached() throws -> HeadsetOptics {
+ let url = option("--calibration").map { URL(fileURLWithPath: $0) } ?? cacheURL
+ return try HeadsetOptics(json: Data(contentsOf: url))
+ }
+}
+
+private struct WarpUniforms {
+ var inverseAffine: simd_float3x3
+ var red: SIMD4<Float>
+ var green: SIMD4<Float>
+ var blue: SIMD4<Float>
+ var k3: SIMD4<Float>
+ var ranges: SIMD4<Float>
+ var panel: SIMD4<Float>
+}
+
+/// Render the scene to an oversampled target, then correct geometry and each color separately.
+final class LensWarp {
+ let color: MTLTexture
+ let depth: MTLTexture
+ private let pipeline: MTLRenderPipelineState
+ private let gridPipeline: MTLRenderPipelineState
+ private var uniforms: [WarpUniforms] = []
+
+ init(device: MTLDevice, optics: HeadsetOptics) throws {
+ // 1.4x per axis preserves more central detail through the nonlinear resampling.
+ let width = 6048, height = 3024
+ func texture(_ format: MTLPixelFormat, _ usage: MTLTextureUsage) throws -> MTLTexture {
+ let d = MTLTextureDescriptor.texture2DDescriptor(pixelFormat: format, width: width, height: height, mipmapped: false)
+ d.storageMode = .private; d.usage = usage
+ guard let t = device.makeTexture(descriptor: d) else {
+ throw NSError(domain: "Optics", code: 2, userInfo: [NSLocalizedDescriptionKey: "Cannot allocate optical render target"])
+ }
+ return t
+ }
+ color = try texture(.bgra8Unorm, [.renderTarget, .shaderRead])
+ depth = try texture(.depth32Float, .renderTarget)
+ let library = try device.makeLibrary(source: lensWarpShader, options: nil)
+ func makePipeline(_ fragment: String) throws -> MTLRenderPipelineState {
+ let p = MTLRenderPipelineDescriptor()
+ p.vertexFunction = library.makeFunction(name: "warp_vertex")
+ p.fragmentFunction = library.makeFunction(name: fragment)
+ p.colorAttachments[0].pixelFormat = .bgra8Unorm
+ if fragment == "optics_grid" { p.depthAttachmentPixelFormat = .depth32Float }
+ return try device.makeRenderPipelineState(descriptor: p)
+ }
+ pipeline = try makePipeline("warp_rgb")
+ gridPipeline = try makePipeline("optics_grid")
+ for index in 0..<2 {
+ let eye = optics.calibration.eyes[index], b = optics.bounds[index]
+ let m = eye.inverseAffine
+ let inverse = simd_float3x3(columns: (SIMD3<Float>(m.columns.0), SIMD3<Float>(m.columns.1), SIMD3<Float>(m.columns.2)))
+ func channel(_ i: Int) -> SIMD4<Float> {
+ let c = eye.channels[i]
+ return SIMD4(Float(c.eyeCenter.x), Float(c.eyeCenter.y), Float(c.k.x), Float(c.k.y))
+ }
+ uniforms.append(WarpUniforms(inverseAffine: inverse, red: channel(0), green: channel(1), blue: channel(2),
+ k3: SIMD4(Float(eye.channels[0].k.z), Float(eye.channels[1].k.z), Float(eye.channels[2].k.z), 0),
+ ranges: SIMD4(Float(b.texXRange.x), Float(b.texXRange.y), Float(b.texYRange.x), Float(b.texYRange.y)),
+ panel: SIMD4(4320, 2160, Float(index), Float(width / 2))))
+ }
+ }
+
+ #if LENS_WARP_CHECKS
+ /// Test-only float output from the exact production shader function, before sampling/quantization.
+ func coordinateSamples(device: MTLDevice, points: [SIMD2<Float>], eye: Int) throws -> [SIMD2<Float>] {
+ let kernel = """
+ kernel void check_coordinates(constant WarpUniforms &u [[buffer(0)]],
+ device const float2 *points [[buffer(1)]], device float2 *out [[buffer(2)]], uint i [[thread_position_in_grid]]) {
+ out[3*i] = channel_uv(points[i], u.red, u.k3.x, u);
+ out[3*i+1] = channel_uv(points[i], u.green, u.k3.y, u);
+ out[3*i+2] = channel_uv(points[i], u.blue, u.k3.z, u);
+ }
+ """
+ let lib = try device.makeLibrary(source: lensWarpShader + kernel, options: nil)
+ let pipeline = try device.makeComputePipelineState(function: lib.makeFunction(name: "check_coordinates")!)
+ let input = device.makeBuffer(bytes: points, length: points.count * MemoryLayout<SIMD2<Float>>.stride)!
+ let output = device.makeBuffer(length: points.count * 3 * MemoryLayout<SIMD2<Float>>.stride, options: .storageModeShared)!
+ let command = device.makeCommandQueue()!.makeCommandBuffer()!
+ let enc = command.makeComputeCommandEncoder()!
+ var u = uniforms[eye]
+ enc.setComputePipelineState(pipeline)
+ enc.setBytes(&u, length: MemoryLayout<WarpUniforms>.stride, index: 0)
+ enc.setBuffer(input, offset: 0, index: 1)
+ enc.setBuffer(output, offset: 0, index: 2)
+ enc.dispatchThreads(MTLSize(width: points.count, height: 1, depth: 1), threadsPerThreadgroup: MTLSize(width: min(32, points.count), height: 1, depth: 1))
+ enc.endEncoding(); command.commit(); command.waitUntilCompleted()
+ precondition(command.status == .completed)
+ return Array(UnsafeBufferPointer(start: output.contents().assumingMemoryBound(to: SIMD2<Float>.self), count: points.count * 3))
+ }
+ #endif
+
+ func encodeGrid(_ cb: MTLCommandBuffer, pass: MTLRenderPassDescriptor) {
+ guard let enc = cb.makeRenderCommandEncoder(descriptor: pass) else { return }
+ enc.setRenderPipelineState(gridPipeline)
+ for eye in 0..<2 {
+ enc.setViewport(MTLViewport(originX: Double(color.width / 2 * eye), originY: 0,
+ width: Double(color.width / 2), height: Double(color.height), znear: 0, zfar: 1))
+ var ranges = uniforms[eye].ranges
+ enc.setFragmentBytes(&ranges, length: MemoryLayout<SIMD4<Float>>.stride, index: 0)
+ enc.drawPrimitives(type: .triangle, vertexStart: 0, vertexCount: 3)
+ }
+ enc.endEncoding()
+ }
+
+ func encode(_ cb: MTLCommandBuffer, target: MTLTexture) {
+ let pass = MTLRenderPassDescriptor()
+ pass.colorAttachments[0].texture = target
+ pass.colorAttachments[0].loadAction = .clear
+ pass.colorAttachments[0].storeAction = .store
+ pass.colorAttachments[0].clearColor = MTLClearColorMake(0, 0, 0, 1)
+ guard let enc = cb.makeRenderCommandEncoder(descriptor: pass) else { return }
+ enc.setRenderPipelineState(pipeline)
+ enc.setFragmentTexture(color, index: 0)
+ for eye in 0..<2 {
+ enc.setViewport(MTLViewport(originX: Double(target.width / 2 * eye), originY: 0,
+ width: Double(target.width / 2), height: Double(target.height), znear: 0, zfar: 1))
+ var u = uniforms[eye]
+ enc.setFragmentBytes(&u, length: MemoryLayout<WarpUniforms>.stride, index: 0)
+ enc.drawPrimitives(type: .triangle, vertexStart: 0, vertexCount: 3)
+ }
+ enc.endEncoding()
+ }
+}
+
+private let lensWarpShader = """
+#include <metal_stdlib>
+using namespace metal;
+struct WarpUniforms {
+ float3x3 inverseAffine;
+ float4 red, green, blue, k3, ranges, panel;
+};
+struct WarpVertex { float4 position [[position]]; float2 uv; };
+vertex WarpVertex warp_vertex(uint id [[vertex_id]]) {
+ float2 p = float2(id == 1 ? 3.0 : -1.0, id == 2 ? 3.0 : -1.0);
+ WarpVertex o; o.position = float4(p, 0, 1); o.uv = float2((p.x + 1) * .5, (1 - p.y) * .5); return o;
+}
+float2 channel_uv(float2 uv, float4 channel, float k3, constant WarpUniforms &u) {
+ float2 pix = float2((uv.x + u.panel.z) * u.panel.x * .5, uv.y * u.panel.y) - channel.xy;
+ float r2 = dot(pix, pix);
+ float d = 1 + r2 * (channel.z + r2 * (channel.w + r2 * k3));
+ float3 vp = u.inverseAffine * float3(pix * d + channel.xy, 1);
+ return (vp.xy / vp.z - u.ranges.xz) / (u.ranges.yw - u.ranges.xz);
+}
+float color_sample(texture2d<float> source, float2 tc, uint channel, constant WarpUniforms &u) {
+ if (any(tc < 0) || any(tc > 1)) return 0;
+ // Clamp within this eye's target so interpolation cannot bleed across the stereo seam.
+ float2 halfPixel = float2(.5 / u.panel.w, .5 / source.get_height());
+ tc = clamp(tc, halfPixel, 1 - halfPixel);
+ tc.x = (tc.x + u.panel.z) * .5;
+ constexpr sampler linearSampler(coord::normalized, address::clamp_to_edge, filter::linear);
+ return source.sample(linearSampler, tc)[channel];
+}
+fragment float4 warp_rgb(WarpVertex in [[stage_in]], texture2d<float> source [[texture(0)]], constant WarpUniforms &u [[buffer(0)]]) {
+ float2 r = channel_uv(in.uv, u.red, u.k3.x, u);
+ float2 g = channel_uv(in.uv, u.green, u.k3.y, u);
+ float2 b = channel_uv(in.uv, u.blue, u.k3.z, u);
+ return float4(color_sample(source, r, 0, u), color_sample(source, g, 1, u), color_sample(source, b, 2, u), 1);
+}
+fragment float4 optics_grid(WarpVertex in [[stage_in]], constant float4 &ranges [[buffer(0)]]) {
+ float2 tangent = mix(ranges.xz, ranges.yw, in.uv);
+ float2 cell = abs(fract(tangent * 10 + .5) - .5);
+ float2 aa = fwidth(tangent * 10);
+ float line = 1 - min(smoothstep(aa.x, aa.x * 2, cell.x), smoothstep(aa.y, aa.y * 2, cell.y));
+ float cross = min(abs(tangent.x), abs(tangent.y));
+ float3 color = mix(float3(.01), float3(.8), line);
+ if (cross < .001) color = float3(0, .8, .2);
+ return float4(color, 1);
+}
+"""
+
+func perspective(bounds: Poly3KBounds, near: Float, far: Float) -> simd_float4x4 {
+ let left = Float(bounds.texXRange.x) * near, right = Float(bounds.texXRange.y) * near
+ let top = -Float(bounds.texYRange.x) * near, bottom = -Float(bounds.texYRange.y) * near
+ return simd_float4x4(columns: (
+ SIMD4(2 * near / (right - left), 0, 0, 0),
+ SIMD4(0, 2 * near / (top - bottom), 0, 0),
+ SIMD4((right + left) / (right - left), (top + bottom) / (top - bottom), far / (near - far), -1),
+ SIMD4(0, 0, near * far / (near - far), 0)
+ ))
+}
diff --git a/Sources/VRWorkstation/Renderer.swift b/Sources/VRWorkstation/Renderer.swift
@@ -25,16 +25,21 @@ final class Renderer: NSObject, CAMetalDisplayLinkDelegate {
let sceneBuffer: MTLBuffer
let items: [SceneItem]
let start = CACurrentMediaTime()
+ let optics: HeadsetOptics
+ private let lensWarp: LensWarp
+ private let opticsGrid = CommandLine.arguments.contains("--optics-grid")
/// Head orientation (world-from-head). Identity when head tracking is off.
var headOrientation: () -> simd_quatf = { simd_quatf(angle: 0, axis: [0, 1, 0]) }
var ipd: Float = 0.063
- var fovY: Float = 100 * .pi / 180
init(device: MTLDevice?) throws {
guard let dev = device, let q = dev.makeCommandQueue() else {
throw NSError(domain: "Renderer", code: 1, userInfo: [NSLocalizedDescriptionKey: "No Metal device"])
}
+ let opticalModel = try HeadsetOptics.loadCached()
+ optics = opticalModel
+ lensWarp = try LensWarp(device: dev, optics: opticalModel)
self.device = dev; queue = q
let lib = try dev.makeLibrary(source: shaderSource, options: nil)
let pd = MTLRenderPipelineDescriptor()
@@ -54,7 +59,6 @@ final class Renderer: NSObject, CAMetalDisplayLinkDelegate {
private let metricsLock = NSLock()
private var metrics = FrameMetrics()
- private var displayDepth: MTLTexture? // serial render-worker ownership
func frameMetrics() -> FrameMetrics {
metricsLock.lock(); defer { metricsLock.unlock() }
return metrics
@@ -67,18 +71,12 @@ final class Renderer: NSObject, CAMetalDisplayLinkDelegate {
private func draw(_ drawable: CAMetalDrawable, presentationTime: Double) {
let width = drawable.texture.width, height = drawable.texture.height
- if displayDepth?.width != width || displayDepth?.height != height {
- let descriptor = MTLTextureDescriptor.texture2DDescriptor(pixelFormat: .depth32Float, width: width, height: height, mipmapped: false)
- descriptor.usage = .renderTarget
- descriptor.storageMode = .private
- displayDepth = device.makeTexture(descriptor: descriptor)
- }
- guard let displayDepth, let cb = queue.makeCommandBuffer() else { return }
+ guard let cb = queue.makeCommandBuffer() else { return }
let rpd = MTLRenderPassDescriptor()
- rpd.colorAttachments[0].texture = drawable.texture
+ rpd.colorAttachments[0].texture = lensWarp.color
rpd.colorAttachments[0].loadAction = .clear
rpd.colorAttachments[0].storeAction = .store
- rpd.depthAttachment.texture = displayDepth
+ rpd.depthAttachment.texture = lensWarp.depth
rpd.depthAttachment.loadAction = .clear
rpd.depthAttachment.storeAction = .dontCare
rpd.depthAttachment.clearDepth = 1
@@ -88,7 +86,10 @@ final class Renderer: NSObject, CAMetalDisplayLinkDelegate {
metricsLock.unlock()
if frame == 1 { log("render worker main=\(Thread.isMainThread)") }
if frame == 1 || frame % 300 == 0 { print("frame \(frame) drawable \(width)x\(height)") }
- encode(cb, rpd, size: CGSize(width: width, height: height), time: Float(presentationTime - start))
+ if opticsGrid { lensWarp.encodeGrid(cb, pass: rpd) } else {
+ encode(cb, rpd, size: CGSize(width: lensWarp.color.width, height: lensWarp.color.height), time: Float(presentationTime - start))
+ }
+ lensWarp.encode(cb, target: drawable.texture)
drawable.addPresentedHandler { [weak self] presented in
guard let self else { return }
self.metricsLock.lock(); defer { self.metricsLock.unlock() }
@@ -125,9 +126,9 @@ final class Renderer: NSObject, CAMetalDisplayLinkDelegate {
let eyeW = size.width / 2
let spin = simd_float4x4(simd_quatf(angle: t * 0.9, axis: normalize(SIMD3<Float>(0.3, 1, 0.2))))
let head = simd_float4x4(headOrientation())
- let proj = perspective(fovY: fovY, aspect: Float(eyeW / size.height), near: 0.05, far: 100)
for eye in 0..<2 {
+ let proj = perspective(bounds: optics.bounds[eye], near: 0.05, far: 100)
enc.setViewport(MTLViewport(originX: Double(eyeW) * Double(eye), originY: 0,
width: Double(eyeW), height: Double(size.height), znear: 0, zfar: 1))
let eyeOffset: Float = (eye == 0 ? -0.5 : 0.5) * ipd
@@ -150,17 +151,23 @@ final class Renderer: NSObject, CAMetalDisplayLinkDelegate {
d.usage = usage; d.storageMode = mode
return device.makeTexture(descriptor: d)!
}
- let color = tex(.bgra8Unorm, .renderTarget, .shared), depthTex = tex(.depth32Float, .renderTarget, .private)
+ let color = tex(.bgra8Unorm, .renderTarget, .shared)
let rpd = MTLRenderPassDescriptor()
- rpd.colorAttachments[0].texture = color
+ rpd.colorAttachments[0].texture = lensWarp.color
rpd.colorAttachments[0].loadAction = .clear
rpd.colorAttachments[0].storeAction = .store
- rpd.depthAttachment.texture = depthTex
+ rpd.depthAttachment.texture = lensWarp.depth
rpd.depthAttachment.loadAction = .clear
rpd.depthAttachment.clearDepth = 1
let cb = queue.makeCommandBuffer()!
- encode(cb, rpd, size: CGSize(width: width, height: height), time: time)
+ if opticsGrid { lensWarp.encodeGrid(cb, pass: rpd) } else {
+ encode(cb, rpd, size: CGSize(width: lensWarp.color.width, height: lensWarp.color.height), time: time)
+ }
+ lensWarp.encode(cb, target: color)
cb.commit(); cb.waitUntilCompleted()
+ guard cb.status == .completed else {
+ throw cb.error ?? NSError(domain: "Renderer", code: 2, userInfo: [NSLocalizedDescriptionKey: "Snapshot GPU command failed"])
+ }
var px = [UInt8](repeating: 0, count: width * height * 4)
color.getBytes(&px, bytesPerRow: width * 4, from: MTLRegionMake2D(0, 0, width, height), mipmapLevel: 0)
let rep = NSBitmapImageRep(bitmapDataPlanes: nil, pixelsWide: width, pixelsHigh: height, bitsPerSample: 8,
@@ -176,7 +183,3 @@ func translation(_ t: SIMD3<Float>) -> simd_float4x4 {
var m = matrix_identity_float4x4; m.columns.3 = SIMD4<Float>(t, 1); return m
}
func scale(_ s: Float) -> simd_float4x4 { simd_float4x4(diagonal: [s, s, s, 1]) }
-func perspective(fovY: Float, aspect: Float, near: Float, far: Float) -> simd_float4x4 {
- let y = 1 / tan(fovY / 2), x = y / aspect, z = far / (near - far)
- return simd_float4x4(columns: ([x, 0, 0, 0], [0, y, 0, 0], [0, 0, z, -1], [0, 0, z * near, 0]))
-}
diff --git a/Sources/VRWorkstation/main.swift b/Sources/VRWorkstation/main.swift
@@ -8,6 +8,23 @@ func option(_ name: String) -> String? {
guard let i = CommandLine.arguments.firstIndex(of: name), CommandLine.arguments.count > i + 1 else { return nil }
return CommandLine.arguments[i + 1]
}
+if let path = option("--read-calibration") {
+ let hmd = HMD()
+ do {
+ try hmd.start(mode: .observe)
+ defer { hmd.stop() }
+ let data = try hmd.readFactoryConfiguration()
+ let block = try WMRConfigBlock(raw: [UInt8](data))
+ let calibration = try HeadsetCalibration(json: block.json)
+ let url = URL(fileURLWithPath: path)
+ try data.write(to: url, options: .atomic)
+ let jsonURL = url.deletingPathExtension().appendingPathExtension("json")
+ try block.json.write(to: jsonURL, options: .atomic)
+ print("saved factory configuration to", path, "and", jsonURL.path)
+ print("validated factory optical calibration (unit-specific geometry omitted)")
+ } catch { print("factory calibration read failed:", error); exit(1) }
+ exit(0)
+}
if CommandLine.arguments.contains("--observe") {
guard let value = option("--observe"), let seconds = Double(value), seconds > 0, seconds.isFinite else {
print("usage: VRWorkstation --observe <positive seconds>"); exit(2)
diff --git a/Tests/CalibrationChecks.swift b/Tests/CalibrationChecks.swift
@@ -0,0 +1,88 @@
+import Foundation
+import simd
+func check(_ ok: Bool, _ what: String) { print(ok ? "PASS" : "FAIL", what); if !ok { exit(1) } }
+func near(_ a: Double, _ b: Double, _ eps: Double = 1e-9) -> Bool { abs(a - b) <= eps }
+
+/// 3x3 identity mixing matrix, 4 temperature coefficients per entry (constant term first).
+func identityMixModel() -> [Double] {
+ var v: [Double] = []
+ for i in 0 ..< 9 { v += [i % 4 == 0 ? 1 : 0, 0, 0, 0] }
+ return v
+}
+
+/// Synthetic G1-like panel: 4320x2160, eye centers in each half, focal ~1000 px, mild positive k1.
+func fixtureJSON(k1: Double, chroma: Double = 0) -> Data {
+ func display(_ eye: String, _ cx: Double, _ tx: Double) -> [String: Any] {
+ let f = 1000.0
+ let affine = [f, 0, cx, 0, f, 1080, 0, 0, 1] // tangent-plane -> pixels; inverse maps pixels -> tangents
+ func dist(_ k: Double) -> [String: Any] {
+ ["ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K", "ModelParameterCount": 5,
+ "ModelParameters": [cx, 1080, k, 0, 0]]
+ }
+ return ["AssignedEye": eye, "Affine": affine, "DisplayWidth": 4320, "DisplayHeight": 2160,
+ "VisibleAreaCenter": ["X": cx, "Y": 1080], "VisibleAreaRadius": 1050,
+ "Rt": ["Rotation": [1, 0, 0, 0, 1, 0, 0, 0, 1], "Translation": [tx, 0, 0]],
+ "DistortionRed": dist(k1 * (1 + chroma)), "DistortionGreen": dist(k1), "DistortionBlue": dist(k1 * (1 - chroma))]
+ }
+ let gyro: [String: Any] = ["SensorType": "CALIBRATION_InertialSensorType_Gyro",
+ "Rt": ["Rotation": [0, 1, 0, -1, 0, 0, 0, 0, 1], "Translation": [0, 0, 0]],
+ "BiasTemperatureModel": [0.01, 0, 0, 0, 0.02, 0, 0, 0, 0.03, 0, 0, 0],
+ "MixingMatrixTemperatureModel": identityMixModel()]
+ let root: [String: Any] = ["CalibrationInformation": [
+ "Displays": [display("CALIBRATION_DisplayEyeLeft", 1080, -0.0315), display("CALIBRATION_DisplayEyeRight", 3240, 0.0315)],
+ "InertialSensors": [gyro], "Cameras": [] as [Any]]]
+ return try! JSONSerialization.data(withJSONObject: root)
+}
+
+@main
+struct CalibrationChecks {
+static func main() {
+
+do {
+ check(wmrConfigKey.count == 1024 && wmrConfigKey.prefix(4) == [0x2F, 0xC8, 0x0F, 0x38], "key: 1024 bytes, upstream prefix")
+ check(WMRConfigProtocol.request(0x0b) == [0x02, 0x0b] + [UInt8](repeating: 0, count: 62), "request layout")
+ var chunk = [UInt8](repeating: 0, count: 33); chunk[0] = 2; chunk[1] = 1; chunk[2] = 3; chunk[3] = 7; chunk[4] = 8; chunk[5] = 9
+ check(WMRConfigProtocol.parseChunk(chunk) == .data([7, 8, 9]), "chunk reply parse")
+ chunk[1] = 2; check(WMRConfigProtocol.parseChunk(chunk) == .end, "end reply parse")
+ check(WMRConfigProtocol.dataSize(fromMetadata: [0x34, 0x12]) == 0x1234, "metadata size LE")
+
+ // 1. Block round trip: encode synthetic JSON like the headset store, decode it back.
+ let json = fixtureJSON(k1: 0)
+ let block = try WMRConfigBlock(raw: WMRConfigBlock.encodeForTesting(json: json))
+ check(block.json == json && block.name == "Test Headset", "config block XOR/header round trip")
+
+ // 2. Parse and identity model: k = 0 and a pure scale affine => sample coords equal output coords.
+ let cal = try HeadsetCalibration(json: block.json)
+ check(cal.eyes.count == 2 && cal.sensors.count == 1 && cal.sensors[0].kind == .gyro, "parse displays + gyro")
+ check(near(EyeCalibration.ipd(cal.eyes[0], cal.eyes[1]), 0.063), "IPD from Rt translations")
+ check(near(cal.sensors[0].biasOffsets.y, 0.02), "gyro bias constant terms")
+ for view in 0 ..< 2 {
+ let eye = cal.eyes[view], b = eye.distortionBounds(view: view)
+ check(near(b.texXRange.x, -1.08) && near(b.texXRange.y, 1.08) && near(b.texYRange.x, -1.08) && near(b.texYRange.y, 1.08),
+ "view \(view): identity bounds = +-1080/1000")
+ for (u, v) in [(0.0, 0.0), (0.5, 0.5), (0.25, 0.8), (1.0, 1.0)] {
+ let s = eye.distort(view: view, u: u, v: v, bounds: b)
+ check(near(s.g.x, u, 1e-9) && near(s.g.y, v, 1e-9) && s.r == s.g && s.b == s.g, "view \(view) identity (\(u), \(v))")
+ }
+ }
+
+ // 3. Barrel/pincushion behaviour: k1 > 0 pushes edge samples outward, centre unchanged, symmetric.
+ let k1 = 1e-7
+ let cal2 = try HeadsetCalibration(json: fixtureJSON(k1: k1, chroma: 0.05))
+ let eye = cal2.eyes[0], b = eye.distortionBounds(view: 0)
+ let centre = eye.distort(view: 0, u: 0.5, v: 0.5, bounds: b)
+ check(near(centre.g.x, 0.5) && near(centre.g.y, 0.5), "k1>0: centre maps to centre")
+ let edge = eye.distort(view: 0, u: 0.9, v: 0.5, bounds: b), mirror = eye.distort(view: 0, u: 0.1, v: 0.5, bounds: b)
+ check(near(edge.g.x + mirror.g.x, 1, 1e-9), "k1>0: left/right symmetric")
+ // Hand-computed expectation for u = 0.9: pix.x = 864, d = 1 + k1 * 864^2, tangent = 864 * d / 1000.
+ let d = 1 + k1 * 864 * 864, tan = 864 * d / 1000
+ check(near(edge.g.x, (tan - b.texXRange.x) / (b.texXRange.y - b.texXRange.x), 1e-12), "k1>0: matches hand calculation")
+ check(edge.r.x > edge.g.x && edge.b.x < edge.g.x, "chromatic: red samples farther out than green, blue nearer")
+ check(b.angleLeft < 0 && b.angleRight > 0 && b.angleUp > 0 && b.angleDown < 0, "bounds -> asymmetric FOV angles")
+ print("all calibration self-tests passed")
+} catch {
+ print("FAIL", error); exit(1)
+}
+
+}
+}
diff --git a/Tests/LensWarpChecks.swift b/Tests/LensWarpChecks.swift
@@ -0,0 +1,91 @@
+// Compare the production Metal warp against the independent Double-precision CPU model.
+// Takes a saved factory JSON path; opens no hardware devices.
+import Foundation
+import Metal
+import simd
+func option(_ name: String) -> String? { nil }
+
+@main
+struct LensWarpChecks {
+ static func main() throws {
+ let optics = try HeadsetOptics(json: Data(contentsOf: URL(fileURLWithPath: CommandLine.arguments[1])))
+ let device = MTLCreateSystemDefaultDevice()!, queue = device.makeCommandQueue()!
+ let warp = try LensWarp(device: device, optics: optics)
+ var coordinateWorst = 0.0
+ var points: [SIMD2<Float>] = []
+ for y in stride(from: 0, through: 2160, by: 90) {
+ for x in stride(from: 0, through: 2160, by: 90) {
+ points.append(SIMD2(Float(x) / 2160, Float(y) / 2160))
+ }
+ }
+ for eye in 0..<2 {
+ let actual = try warp.coordinateSamples(device: device, points: points, eye: eye)
+ for (i, p) in points.enumerated() {
+ let cpu = optics.calibration.eyes[eye].distort(view: eye, u: Double(p.x), v: Double(p.y), bounds: optics.bounds[eye])
+ for (c, expected) in [cpu.r, cpu.g, cpu.b].enumerated() {
+ let error = simd_abs(SIMD2<Double>(actual[i * 3 + c]) - expected)
+ coordinateWorst = max(coordinateWorst, error.x, error.y)
+ }
+ }
+ }
+ precondition(coordinateWorst < 1e-5, "Float shader coordinates differ from Double model: \(coordinateWorst)")
+ print("PASS: \(points.count * 2 * 3) RGB float-coordinate pairs, max GPU/CPU error \(coordinateWorst)")
+ let w = warp.color.width, h = warp.color.height, eyeW = w / 2
+ let bytes = UnsafeMutablePointer<UInt8>.allocate(capacity: w * h * 4)
+ defer { bytes.deallocate() }
+ for y in 0..<h {
+ for x in 0..<w {
+ let p = (y * w + x) * 4
+ bytes[p] = 128
+ bytes[p + 1] = UInt8(Double(y) / Double(h - 1) * 255)
+ bytes[p + 2] = UInt8(Double(x % eyeW) / Double(eyeW - 1) * 255)
+ bytes[p + 3] = 255
+ }
+ }
+ func texture(_ width: Int, _ height: Int) -> MTLTexture {
+ let d = MTLTextureDescriptor.texture2DDescriptor(pixelFormat: .bgra8Unorm, width: width, height: height, mipmapped: false)
+ d.storageMode = .shared; d.usage = .renderTarget
+ return device.makeTexture(descriptor: d)!
+ }
+ let source = texture(w, h), target = texture(4320, 2160)
+ source.replace(region: MTLRegionMake2D(0, 0, w, h), mipmapLevel: 0, withBytes: bytes, bytesPerRow: w * 4)
+ let command = queue.makeCommandBuffer()!, blit = command.makeBlitCommandEncoder()!
+ blit.copy(from: source, sourceSlice: 0, sourceLevel: 0, sourceOrigin: .init(x: 0, y: 0, z: 0), sourceSize: .init(width: w, height: h, depth: 1), to: warp.color, destinationSlice: 0, destinationLevel: 0, destinationOrigin: .init(x: 0, y: 0, z: 0))
+ blit.endEncoding()
+ warp.encode(command, target: target)
+ command.commit(); command.waitUntilCompleted()
+ precondition(command.status == .completed, "Metal command failed")
+ let outW = target.width, outH = target.height
+ let output = UnsafeMutablePointer<UInt8>.allocate(capacity: outW * outH * 4)
+ defer { output.deallocate() }
+ target.getBytes(output, bytesPerRow: outW * 4, from: MTLRegionMake2D(0, 0, outW, outH), mipmapLevel: 0)
+ var worst = 0.0, count = 0
+ for eye in 0..<2 {
+ let model = optics.calibration.eyes[eye], bounds = optics.bounds[eye]
+ for y in stride(from: 53, to: 2160, by: 137) {
+ for x in stride(from: 29, to: 2160, by: 131) {
+ let tc = model.distort(view: eye, u: (Double(x) + 0.5) / 2160, v: (Double(y) + 0.5) / 2160, bounds: bounds)
+ func expected(_ uv: SIMD2<Double>, _ value: Double) -> Double {
+ uv.x < 0 || uv.x > 1 || uv.y < 0 || uv.y > 1 ? 0 : value
+ }
+ let p = (y * outW + eye * 2160 + x) * 4
+ worst = max(worst, abs(Double(output[p + 2]) / 255 - expected(tc.r, tc.r.x)),
+ abs(Double(output[p + 1]) / 255 - expected(tc.g, tc.g.y)),
+ abs(Double(output[p]) / 255 - expected(tc.b, 128.0 / 255)))
+ count += 3
+ }
+ }
+ let projection = perspective(bounds: bounds, near: 0.05, far: 100)
+ for (tangent, expectedX) in [(Float(bounds.texXRange.x), Float(-1)), (Float(bounds.texXRange.y), Float(1))] {
+ let clip = projection * SIMD4(tangent * 0.05, 0, -0.05, 1)
+ precondition(abs(clip.x / clip.w - expectedX) < 1e-5, "projection X edge incorrect")
+ }
+ for (tangent, expectedY) in [(-Float(bounds.texYRange.x), Float(1)), (-Float(bounds.texYRange.y), Float(-1))] {
+ let clip = projection * SIMD4(0, tangent * 0.05, -0.05, 1)
+ precondition(abs(clip.y / clip.w - expectedY) < 1e-5, "projection Y edge incorrect")
+ }
+ }
+ precondition(worst < 0.012, "GPU warp differs from CPU model: \(worst)")
+ print("PASS: \(count) RGB channel samples, max GPU/CPU error \(worst); asymmetric projection edges correct")
+ }
+}