28 lines
1.1 KiB
Swift
28 lines
1.1 KiB
Swift
|
import simd
|
||
|
|
import Foundation
|
||
|
|
|
||
|
|
struct PoseInterpolator {
|
||
|
|
/// Interpolates between two timestamped poses at the given timestamp.
|
||
|
|
/// Uses linear interpolation for translation and spherical linear interpolation for rotation.
|
||
|
|
static func interpolate(from a: TimestampedPose, to b: TimestampedPose, at timestamp: TimeInterval) -> simd_float4x4 {
|
||
|
|
let t = Float((timestamp - a.timestamp) / (b.timestamp - a.timestamp))
|
||
|
|
let clamped = max(0, min(1, t))
|
||
|
|
return lerp(from: a.transform, to: b.transform, t: clamped)
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Interpolates between two simd_float4x4 transforms with a given t in [0, 1].
|
||
|
|
static func lerp(from a: simd_float4x4, to b: simd_float4x4, t: Float) -> simd_float4x4 {
|
||
|
|
let tA = simd_float3(a.columns.3.x, a.columns.3.y, a.columns.3.z)
|
||
|
|
let tB = simd_float3(b.columns.3.x, b.columns.3.y, b.columns.3.z)
|
||
|
|
let qA = simd_quatf(a)
|
||
|
|
let qB = simd_quatf(b)
|
||
|
|
|
||
|
|
let tInterp = tA + (tB - tA) * t
|
||
|
|
let qInterp = simd_slerp(qA, qB, t)
|
||
|
|
|
||
|
|
var result = simd_float4x4(qInterp)
|
||
|
|
result.columns.3 = simd_float4(tInterp.x, tInterp.y, tInterp.z, 1.0)
|
||
|
|
return result
|
||
|
|
}
|
||
|
|
}
|