spacefarer/src/math.zig

const std = @import("std");

/// Smooth lerp
pub inline fn lerpTime(a: anytype, b: anytype, t: f32, comptime f: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    return lerpTimeLn(a, b, t, @log(f));
}

pub fn lerpTimeLn(a: anytype, b: anytype, t: f32, lnf: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    return lerp(b, a, @exp(lnf * t));
}

pub fn lerp(a: anytype, b: anytype, f: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    const a_factor = 1.0 - f;
    const b_factor = f;

    switch (@typeInfo(@TypeOf(a, b))) {
        .float => return a_factor * a + b_factor * b,
        .vector => return @as(@TypeOf(a), @splat(a_factor)) * a + @as(@TypeOf(b), @splat(b_factor)) * b,
        else => @compileError("Can only interpolate between vector or float values"),
    }
}

/// Spherical lerp
pub inline fn slerpTime(a: anytype, b: anytype, t: f32, comptime f: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    return slerpTimeLn(a, b, t, @log(f));
}

pub fn slerpTimeLn(a: anytype, b: anytype, t: f32, lnf: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    return slerp(b, a, @exp(lnf * t));
}

pub fn slerp(a: anytype, b: anytype, f: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    const cos = @reduce(.Add, a * b);
    if (cos > 0.999) {
        return lerp(a, b, f);
    }

    const angle = std.math.acos(cos);

    const a_angle_factor = 1 - f;
    const b_angle_factor = f;

    const rev_angle_sin = 1.0 / std.math.sin(angle);
    const a_sin = std.math.sin(a_angle_factor * angle);
    const b_sin = std.math.sin(b_angle_factor * angle);

    const a_factor = a_sin * rev_angle_sin;
    const b_factor = b_sin * rev_angle_sin;

    return @as(@TypeOf(a), @splat(a_factor)) * a + @as(@TypeOf(b), @splat(b_factor)) * b;
}

// Step interpolation
pub fn step(a: f32, b: f32, l: f32) f32 {
    @setFloatMode(.optimized);

    if (b > a) {
        return @min(a + l, b);
    } else {
        return @max(b, a - l);
    }
}

pub fn stepVector(a: anytype, b: anytype, l: f32) @TypeOf(a, b) {
    @setFloatMode(.optimized);

    return a + limitLength(b - a, l);
}

pub fn limitLength(vector: anytype, max_length: f32) @TypeOf(vector) {
    @setFloatMode(.optimized);

    const length_square = @reduce(.Add, vector * vector);
    if (length_square > max_length * max_length) {
        return vector * @as(@TypeOf(vector), @splat(max_length / @sqrt(length_square)));
    }
    return vector;
}

pub fn length(vector: anytype) f32 {
    @setFloatMode(.optimized);

    return @sqrt(dot(vector, vector));
}

pub fn dot(a: anytype, b: anytype) f32 {
    @setFloatMode(.optimized);

    return @reduce(.Add, a * b);
}

pub fn lengthInt(vector: anytype) f32 {
    @setFloatMode(.optimized);

    return @sqrt(@as(f32, @floatFromInt(dotInt(vector, vector))));
}

pub fn dotInt(a: anytype, b: anytype) (@typeInfo(@TypeOf(a)).vector.child) {
    @setFloatMode(.optimized);

    return @reduce(.Add, a * b);
}

pub fn Sway(comptime T: type) type {
    const STABILIZATION = -1;
    return packed struct {
        value: T = 0,
        velocity: T = 0,
        frequency: T,
        amplitude: T,

        const Self = @This();
        pub fn update(self: *Self, delta: f32) void {
            @setFloatMode(.optimized);

            const dist = delta * -2 * std.math.pi * self.frequency;
            const sin = std.math.sin(dist);
            const cos = std.math.cos(dist);
            var len = length(@Vector(2, T){ self.value, self.velocity });
            if (len < 0.001) {
                self.value = 0.001;
                self.velocity = 0;
                len = 0.001;
            }
            const new_value = self.value * cos - self.velocity * sin;
            const new_velocity = self.value * sin + self.velocity * cos;
            const target_len = lerpTimeLn(len, self.amplitude, delta, STABILIZATION);
            const mult = target_len / len;
            self.value = new_value * mult;
            self.velocity = new_velocity * mult;
        }
    };
}

pub fn raycast(
    origin: @Vector(3, f32),
    target: @Vector(3, f32),
    plane: @Vector(4, f32),
) @Vector(3, f32) {
    @setFloatMode(.optimized);

    const offset = target - origin;
    const plane_dir = @Vector(3, f32){ plane[0], plane[1], plane[2] };
    const dist = plane[3];
    const dist_mod = dist / dot(plane_dir, plane_dir);
    const num = dot(plane_dir, plane_dir * @as(@Vector(3, f32), @splat(dist_mod)) - origin);
    var den = dot(offset, plane_dir);
    if (@abs(den) < 0.0001) {
        den = 0.0001;
    }

    return origin + offset * @as(@Vector(3, f32), @splat(num / den));
}

pub fn limit(vector: anytype, value: f32) @TypeOf(vector) {
    const max = @reduce(.Max, vector);
    if (max > value)
        return vector * @as(@TypeOf(vector), @splat(value / max))
    else
        return vector;
}

pub fn norm(vector: anytype) @TypeOf(vector) {
    const len = length(vector);
    if (len < 1.01 and len > 0.99) return vector;
    if (len < 1e-10) {
        var output = @as(@TypeOf(vector), @splat(0));
        output[@typeInfo(@TypeOf(vector)).vector.len - 1] = 1;
        return output;
    }
    return vector * @as(@TypeOf(vector), @splat(1.0 / len));
}