Merge branch 'useful-misc-functions' into 'main'

Smoother ride rotations.

See merge request cable-mc/cobblemon!2407

CHANGELOG.md

@@ -29,6 +29,7 @@
 ### Changes
 - Updated various recipes to display in Recipe Groups within the Recipe Book
 - Updated most recipes to display more logically/consistently within the Recipe Book's categories
+- Make ride rotations less jittery in multiplayer.
 
 ### Fixes
 - Correctly grants "They're Minerals!" advancement when planting a type gem

common/src/main/kotlin/com/cobblemon/mod/common/api/orientation/OrientationController.kt

@@ -8,8 +8,10 @@
 
 package com.cobblemon.mod.common.api.orientation
 
+import com.cobblemon.mod.common.util.math.QuatSpring
 import com.cobblemon.mod.common.util.math.geometry.toDegrees
 import com.cobblemon.mod.common.util.math.geometry.toRadians
+import com.cobblemon.mod.common.util.math.simpleSpringDamperQuat
 import net.minecraft.client.Minecraft
 import net.minecraft.util.Mth
 import net.minecraft.world.entity.LivingEntity
@@ -26,6 +28,8 @@ open class OrientationController(val entity: LivingEntity) {
         val FORWARDS: Vector3f = Vector3f(0.0f, 0.0f, -1.0f)
         val UP: Vector3f = Vector3f(0.0f, 1.0f, 0.0f)
         val LEFT: Vector3f = Vector3f(-1.0f, 0.0f, 0.0f)
+        private const val TICK_DT = 1f / 20f // Minecraft server tick rate
+        private const val RENDER_SMOOTH_HALFLIFE = 0.1f // seconds to close ~half the gap
     }
 
     open var active: Boolean = false
@@ -33,6 +37,8 @@ open class OrientationController(val entity: LivingEntity) {
     var orientation: Matrix3f? = null
         private set
 
+    private val renderSpring = QuatSpring()
+
     private var renderOrientationO: Matrix3f? = null
     private var renderOrientation: Matrix3f? = null
 
@@ -54,6 +60,8 @@ open class OrientationController(val entity: LivingEntity) {
         orientation = null
         renderOrientationO = null
         renderOrientation = null
+        renderSpring.quat.identity()
+        renderSpring.angVel.set(0f, 0f, 0f)
         active = false
     }
 
@@ -77,18 +85,26 @@ open class OrientationController(val entity: LivingEntity) {
         return oldQuat
     }
 
+    /** Every tick move the current render orientation towards the last known true orientation **/
     fun tick() {
         if (!active || orientation == null) return
         renderOrientationO = renderOrientation
-        val current = orientation
-        val renderMatrix = this.renderOrientation ?: current
-        val renderQuat = Quaternionf().setFromUnnormalized(renderMatrix)
-        val targetQuat  = Quaternionf().setFromUnnormalized(current)
-        val dampingFactor = 0.66f // Smooth interpolation to reduce jitter of orientations recieved from the server.
-        renderQuat.slerp(targetQuat, dampingFactor)
+        val current = orientation!!
+        val targetQuat = Quaternionf().setFromUnnormalized(current)
+
+        if (renderOrientation == null) {
+            // First tick: snap the spring to the target instead of easing in from identity
+            renderSpring.quat.set(targetQuat)
+            renderSpring.angVel.set(0f, 0f, 0f)
+        }
+
+        // Use a "quat spring" to smoothly move the render orientation towards the goal orientation.
+        // It takes into account the orientations angular velocities and should be much more resilient
+        // to sharp changes in goal orientation or late/missed packets.
+        simpleSpringDamperQuat(renderSpring, targetQuat, 0.15f, TICK_DT)
 
         val newRenderOrientation = Matrix3f()
-        renderQuat.get(newRenderOrientation)
+        renderSpring.quat.get(newRenderOrientation)
         renderOrientation = newRenderOrientation
     }
 

common/src/main/kotlin/com/cobblemon/mod/common/api/riding/util/BlockPrimitiveCollector.kt

@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2023 Cobblemon Contributors
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
+package com.cobblemon.mod.common.api.riding.util
+
+import net.minecraft.core.BlockPos
+import net.minecraft.world.level.BlockGetter
+import net.minecraft.world.phys.AABB
+import kotlin.math.floor
+
+/**
+ * Walks every block position inside [searchBox] and turns each solid block into one
+ * full-cube [FieldPrimitive].
+ *
+ * @author Jackson
+ * @since July 27th, 2026
+ */
+
+fun collectPrimitivesFromBlocks(
+    level: BlockGetter,
+    searchBox: AABB,
+    shape: PrimitiveShape,
+    includeFluids: Boolean = false
+): List<FieldPrimitive> {
+    val primitives = mutableListOf<FieldPrimitive>()
+
+    val minX = floor(searchBox.minX).toInt()
+    val maxX = floor(searchBox.maxX).toInt()
+    val minY = floor(searchBox.minY).toInt()
+    val maxY = floor(searchBox.maxY).toInt()
+    val minZ = floor(searchBox.minZ).toInt()
+    val maxZ = floor(searchBox.maxZ).toInt()
+
+    val pos = BlockPos.MutableBlockPos()
+    for (x in minX..maxX) {
+        for (y in minY..maxY) {
+            for (z in minZ..maxZ) {
+                pos.set(x, y, z)
+                val state = level.getBlockState(pos)
+
+                val isSolid = !state.getCollisionShape(level, pos).isEmpty
+                val isFluid = includeFluids && !state.fluidState.isEmpty
+                if (!isSolid && !isFluid) continue
+
+                primitives.add(
+                    FieldPrimitive(
+                        shape = shape,
+                        centerX = x + 0.5f, centerY = y + 0.5f, centerZ = z + 0.5f,
+                        halfWidth = 0.5f, halfHeight = 0.5f, halfDepth = 0.5f
+                    )
+                )
+            }
+        }
+    }
+
+    return primitives
+}

common/src/main/kotlin/com/cobblemon/mod/common/api/riding/util/DistanceFieldMath.kt

@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2023 Cobblemon Contributors
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
+package com.cobblemon.mod.common.api.riding.util
+
+import net.minecraft.world.phys.Vec3
+import kotlin.math.abs
+import kotlin.math.max
+import kotlin.math.min
+import kotlin.math.sqrt
+
+/**
+ * Helpful Functions when working with signed distance fields/functions.
+ * https://iquilezles.org/articles/distfunctions/
+ *
+ * @author Jackson
+ * @since July 27th, 2026
+ */
+
+/** Which distance formula a primitive uses. */
+enum class PrimitiveShape {
+    BOX,
+    ROUNDED_BOX,
+    ELLIPSOID
+}
+
+/** Tunable constants for the field. */
+data class DistanceFieldTuning(
+    val smoothingRange: Float = 0.5f,
+    val roundingRadius: Float = 0.08f
+)
+
+/** A single shape in the field - a center position, half-extents, and which formula to use. */
+data class FieldPrimitive(
+    val shape: PrimitiveShape,
+    val centerX: Float, val centerY: Float, val centerZ: Float,
+    val halfWidth: Float, val halfHeight: Float, val halfDepth: Float
+)
+
+/** Fast approximate 1/sqrt(x) - the Quake III trick, cheaper than a real sqrt + divide. */
+fun fastInverseSqrt(value: Float): Float {
+    var x = value
+    val halfX = 0.5f * x
+    var bits = java.lang.Float.floatToIntBits(x)
+    bits = 0x5f3759df - (bits shr 1)
+    x = java.lang.Float.intBitsToFloat(bits)
+    x *= (1.5f - halfX * x * x)
+    return x
+}
+
+/** Exact distance from a point to an axis-aligned box. Inputs are relative to the box's own center. */
+fun boxDistance(relX: Float, relY: Float, relZ: Float, halfWidth: Float, halfHeight: Float, halfDepth: Float): Float {
+    val qx = abs(relX) - halfWidth
+    val qy = abs(relY) - halfHeight
+    val qz = abs(relZ) - halfDepth
+    val ox = max(qx, 0f)
+    val oy = max(qy, 0f)
+    val oz = max(qz, 0f)
+    val outside = sqrt(ox * ox + oy * oy + oz * oz)
+    val inside = min(max(qx, max(qy, qz)), 0f)
+    return outside + inside
+}
+
+/** Same as boxDistance but with rounded corners/edges https://iquilezles.org/articles/distfunctions/ */
+fun roundedBoxDistance(relX: Float, relY: Float, relZ: Float, halfWidth: Float, halfHeight: Float, halfDepth: Float, roundingRadius: Float): Float =
+    boxDistance(relX, relY, relZ, halfWidth - roundingRadius, halfHeight - roundingRadius, halfDepth - roundingRadius) - roundingRadius
+
+/** Approximate (lower-bound) distance to an ellipsoid https://iquilezles.org/articles/ellipsoids/. */
+fun ellipsoidDistance(relX: Float, relY: Float, relZ: Float, halfWidth: Float, halfHeight: Float, halfDepth: Float): Float {
+    val px = relX / halfWidth
+    val py = relY / halfHeight
+    val pz = relZ / halfDepth
+    val inverseMagnitude = fastInverseSqrt(px * px + py * py + pz * pz)
+    val trueDistance = sqrt(relX * relX + relY * relY + relZ * relZ)
+    return trueDistance * (1.0f - inverseMagnitude)
+}
+
+/** Distance from a point to one primitive, dispatched by its shape. */
+fun distanceToPrimitive(primitive: FieldPrimitive, sampleX: Float, sampleY: Float, sampleZ: Float, tuning: DistanceFieldTuning): Float {
+    val relX = sampleX - primitive.centerX
+    val relY = sampleY - primitive.centerY
+    val relZ = sampleZ - primitive.centerZ
+    return when (primitive.shape) {
+        PrimitiveShape.BOX -> boxDistance(relX, relY, relZ, primitive.halfWidth, primitive.halfHeight, primitive.halfDepth)
+        PrimitiveShape.ROUNDED_BOX -> roundedBoxDistance(relX, relY, relZ, primitive.halfWidth, primitive.halfHeight, primitive.halfDepth, tuning.roundingRadius)
+        PrimitiveShape.ELLIPSOID -> ellipsoidDistance(relX, relY, relZ, primitive.halfWidth, primitive.halfHeight, primitive.halfDepth)
+    }
+}
+
+///** Normalized quadratic smooth-min https://iquilezles.org/articles/smin/ */
+//fun smoothMinimum(a: Float, b: Float, smoothingRange: Float): Float {
+//    if (smoothingRange <= 0.001f) return min(a, b)
+//    val k = smoothingRange * 4.0f
+//    val h = (0.5f + 0.5f * (b - a) / k).coerceIn(0f, 1f)
+//    return b + (a - b) * h - k * h * (1f - h)
+//}
+//
+///** Folds every primitive's distance together into one field value at a point. */
+//fun sampleField(primitives: List<FieldPrimitive>, sampleX: Float, sampleY: Float, sampleZ: Float, tuning: DistanceFieldTuning): Float {
+//    var result = Float.POSITIVE_INFINITY
+//    for (primitive in primitives) {
+//        val distance = distanceToPrimitive(primitive, sampleX, sampleY, sampleZ, tuning)
+//        result = if (result == Float.POSITIVE_INFINITY) distance else smoothMinimum(result, distance, tuning.smoothingRange)
+//    }
+//    return result
+//}
+
+/** Normalized quadratic smooth-min https://iquilezles.org/articles/smin/ */
+fun smoothMinimum(a: Float, b: Float, smoothingRange: Float): Float {
+    if (smoothingRange <= 0.001f) return min(a, b)
+    val k = smoothingRange * 4.0f
+    val h = (0.5f + 0.5f * (b - a) / k).coerceIn(0f, 1f)
+    return b + (a - b) * h - k * h * (1f - h)
+}
+
+/** Folds nearby primitive distances together into one field value at a point. */
+fun sampleField(primitives: List<FieldPrimitive>, sampleX: Float, sampleY: Float, sampleZ: Float, tuning: DistanceFieldTuning): Float {
+    if (primitives.isEmpty()) return Float.POSITIVE_INFINITY
+
+    val distances = FloatArray(primitives.size) { distanceToPrimitive(primitives[it], sampleX, sampleY, sampleZ, tuning) }
+    val minDistance = distances.min()
+
+    if (tuning.smoothingRange <= 0.001f) return minDistance
+
+    // Anything more than a few blend-widths past the closest primitive is cut - its
+    // contribution would be negligible in a correct blend anyway, but left in, it still
+    // drags the chained result down the more of them there are.
+    val cutoff = tuning.smoothingRange * 3f
+
+    var result = Float.POSITIVE_INFINITY
+    for (distance in distances) {
+        if (distance - minDistance > cutoff) continue
+        result = if (result == Float.POSITIVE_INFINITY) distance else smoothMinimum(result, distance, tuning.smoothingRange)
+    }
+    return result
+}
+
+/** Finite-difference gradient of the field at a point - points away from the surface, magnitude ~1 for a clean field. */
+fun estimateGradient(primitives: List<FieldPrimitive>, point: Vec3, tuning: DistanceFieldTuning, epsilon: Float = 0.05f): Vec3 {
+    val px = point.x.toFloat()
+    val py = point.y.toFloat()
+    val pz = point.z.toFloat()
+
+    val dx = sampleField(primitives, px + epsilon, py, pz, tuning) - sampleField(primitives, px - epsilon, py, pz, tuning)
+    val dy = sampleField(primitives, px, py + epsilon, pz, tuning) - sampleField(primitives, px, py - epsilon, pz, tuning)
+    val dz = sampleField(primitives, px, py, pz + epsilon, tuning) - sampleField(primitives, px, py, pz - epsilon, tuning)
+
+    val gradient = Vec3(dx.toDouble(), dy.toDouble(), dz.toDouble())
+    return if (gradient.lengthSqr() > 1e-8) gradient.normalize() else Vec3(0.0, 1.0, 0.0)
+}

common/src/main/kotlin/com/cobblemon/mod/common/api/riding/util/SdfVisualizer.kt

@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2023 Cobblemon Contributors
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
+package com.cobblemon.mod.common.api.riding.util
+
+import net.minecraft.core.particles.DustParticleOptions
+import net.minecraft.server.level.ServerLevel
+import net.minecraft.world.entity.Entity
+import net.minecraft.world.phys.AABB
+import net.minecraft.world.phys.Vec3
+import org.joml.Vector3f
+
+/** One shell to draw - a target field value, a color, a particle size. */
+data class DistanceFieldLayer(
+    val isoValue: Float,
+    val color: Vector3f,
+    val particleSize: Float = 1.0f
+)
+
+/**
+ * Marches a grid of vertical columns through the field around [center] and spawns a
+ * particle everywhere it crosses one of [layers]' target values. Server-side only.
+ */
+fun visualizeDistanceField(
+    referenceEntity: Entity,
+    center: Vec3,
+    layers: List<DistanceFieldLayer>,
+    shape: PrimitiveShape = PrimitiveShape.BOX,
+    horizontalRadius: Double = 4.0,
+    verticalRange: Double = 4.0,
+    sampleSpacing: Double = 0.5,
+    marchStep: Double = 0.1,
+    tuning: DistanceFieldTuning = DistanceFieldTuning()
+) {
+    val level = referenceEntity.level()
+    if (level !is ServerLevel) return
+
+    val searchBox = AABB(
+        center.x - horizontalRadius, center.y - verticalRange, center.z - horizontalRadius,
+        center.x + horizontalRadius, center.y, center.z + horizontalRadius
+    )
+
+    val primitives = collectPrimitivesFromBlocks(level, searchBox, shape, includeFluids = true)
+    if (primitives.isEmpty()) return
+
+    var x = -horizontalRadius
+    while (x <= horizontalRadius) {
+        var z = -horizontalRadius
+        while (z <= horizontalRadius) {
+            marchColumn(level, primitives, center, x, z, verticalRange, marchStep, layers, tuning)
+            z += sampleSpacing
+        }
+        x += sampleSpacing
+    }
+}
+
+private fun marchColumn(
+    level: ServerLevel,
+    primitives: List<FieldPrimitive>,
+    center: Vec3,
+    offsetX: Double,
+    offsetZ: Double,
+    verticalRange: Double,
+    marchStep: Double,
+    layers: List<DistanceFieldLayer>,
+    tuning: DistanceFieldTuning
+) {
+    val previousValue = FloatArray(layers.size) { Float.NaN }
+
+    var y = verticalRange
+    while (y >= -verticalRange) {
+        val worldX = (center.x + offsetX).toFloat()
+        val worldY = (center.y + y).toFloat()
+        val worldZ = (center.z + offsetZ).toFloat()
+        val fieldValue = sampleField(primitives, worldX, worldY, worldZ, tuning)
+
+        for (layerIndex in layers.indices) {
+            val layer = layers[layerIndex]
+            val previous = previousValue[layerIndex]
+
+            if (!previous.isNaN() && crossedTarget(previous, fieldValue, layer.isoValue)) {
+                val t = (layer.isoValue - previous) / (fieldValue - previous)
+                val crossingY = (y + marchStep) - marchStep * t
+                spawnLayerParticle(level, center.add(offsetX, crossingY, offsetZ), layer)
+            }
+
+            previousValue[layerIndex] = fieldValue
+        }
+
+        y -= marchStep
+    }
+}
+
+private fun crossedTarget(previous: Float, current: Float, target: Float): Boolean =
+    (previous - target) * (current - target) < 0f
+
+private fun spawnLayerParticle(level: ServerLevel, pos: Vec3, layer: DistanceFieldLayer) {
+    val particle = DustParticleOptions(layer.color, layer.particleSize)
+    level.sendParticles(particle, pos.x, pos.y, pos.z, 1, 0.0, 0.0, 0.0, 0.0)
+}

common/src/main/kotlin/com/cobblemon/mod/common/util/math/QuaternionUtils.kt

@@ -53,4 +53,60 @@ fun Quaternionf.toEulerXYZDegrees(): Vector3f {
     vector.z = vector.z.toDegrees()
 
     return vector
+}
+
+
+/* // is not allocation free so just inline ".get(AxisAngle4f)" like in the function below this
+fun Quaternionf.toScaledAngleAxis(angAxisScaled: Vector3f) {
+    val axisAng4 = AxisAngle4f() 
+    this.get(axisAng4)
+
+    angAxisScaled.x = axisAng4.x
+    angAxisScaled.y = axisAng4.y
+    angAxisScaled.z = axisAng4.z
+    angAxisScaled.mul(axisAng4.angle)
+} */
+
+fun Quaternionf.toScaledAngleAxis(dest: Vector3f) {
+    var x = this.x
+    var y = this.y
+    var z = this.z
+    var w = this.w
+
+    if (w > 1.0f) {
+        val invNorm = 1.0f / kotlin.math.sqrt(x * x + y * y + z * z + w * w)
+        x *= invNorm
+        y *= invNorm
+        z *= invNorm
+        w *= invNorm
+    }
+
+    val angle = 2.0f * kotlin.math.acos(w)
+    val s = kotlin.math.sqrt(1.0f - w * w)
+
+    if (s < 0.001f) {
+        dest.x = x * angle
+        dest.y = y * angle
+        dest.z = z * angle
+    } else {
+        val invS = angle / s
+        dest.x = x * invS
+        dest.y = y * invS
+        dest.z = z * invS
+    }
+}
+
+
+fun Quaternionf.setFromScaledAngleAxis(angAxisScaled: Vector3f) {
+    val len = angAxisScaled.length()
+    if (len > 1e-5f) {
+        this.rotationAxis(
+            len, 
+            angAxisScaled.x / len,
+            angAxisScaled.y / len,
+            angAxisScaled.z / len
+            )
+    } else {
+        this.identity()
+    }
 }

common/src/main/kotlin/com/cobblemon/mod/common/util/math/SpringUtils.kt

@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2023 Cobblemon Contributors
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
+package com.cobblemon.mod.common.util.math
+
+import net.minecraft.util.Mth
+import kotlin.math.max
+import kotlin.math.sqrt
+import kotlin.math.abs
+import org.joml.Quaternionf
+import org.joml.Vector3f
+
+/**
+ * Much of the code here is directly taken or inspired off this great article:
+ * https://theorangeduck.com/page/spring-roll-call#critical
+ *
+ * Very much worth the read.
+ *
+ * Util funcitons for using springs to smoothly/naturally approach one value towards a target
+ */
+
+/**
+ * Need to pass in and mutate both a value and a velocity to a spring
+ */
+class FloatSpring(
+    var value: Float = 0f,
+    var velocity: Float = 0f
+)
+
+class QuatSpring(
+    var quat: Quaternionf = Quaternionf(),
+    var angVel: Vector3f = Vector3f()
+)
+
+
+/**
+ * Provides a fast negative exponent approximation for e^-x
+ */
+fun negExp(x: Float): Float {
+    return 1.0f / (1.0f + x + 0.48f*x*x + 0.235f*x*x*x)
+}
+
+fun halflifeToDamping(halflife: Float, eps: Float = 1e-5f): Float {
+    return (4.0f * 0.69314718056f) / (halflife + eps)
+}
+
+fun simpleSpringDamper(
+    spring: FloatSpring,
+    goalValue: Float,
+    halflife: Float,
+    dt: Float,
+    isAngle: Boolean = false
+) {
+    val y = halflifeToDamping(halflife) / 2.0f
+    val j0 = if (isAngle) Mth.wrapDegrees(spring.value - goalValue) else spring.value - goalValue
+    val j1 = spring.velocity + j0*y;
+    val eydt = negExp(y*dt);
+
+    spring.value = eydt*(j0 + j1*dt) + goalValue;
+    spring.velocity = eydt*(spring.velocity - j1*y*dt);
+}
+
+fun simpleSpringDamperQuat(
+    spring: QuatSpring,
+    goalQuat: Quaternionf,
+    halflife: Float,
+    dt: Float
+) {
+    val y = halflifeToDamping(halflife) / 2.0f
+
+    // Quaternions have double cover q and -q represent the same rotation.
+    // so an accidentally rotation the long way could be calculated.
+    // account for that specifically
+    val goal = if (spring.quat.dot(goalQuat) < 0f) {
+        Quaternionf(-goalQuat.x, -goalQuat.y, -goalQuat.z, -goalQuat.w)
+    } else {
+        goalQuat
+    }
+
+    // Gather the rotation from the goal to the current
+    val goalToCurrQuat = Quaternionf(spring.quat).mul(Quaternionf(goal).conjugate())
+
+    val j0 = Vector3f()
+    goalToCurrQuat.toScaledAngleAxis(j0)
+
+    val j1: Vector3f = Vector3f(spring.angVel).add(Vector3f(j0).mul(y))
+
+    val eydt: Float = negExp(y * dt)
+
+    val scaledAxis = Vector3f(j0).add(Vector3f(j1).mul(dt)).mul(eydt)
+
+    spring.quat.setFromScaledAngleAxis(scaledAxis)
+    spring.quat.mul(goal)
+
+    spring.angVel.sub(j1.mul(y * dt))
+    spring.angVel.mul(eydt)
+}
+
+/**
+* Below is essentially the same code as found here: https://github.com/orangeduck/Spring-It-On/blob/main/inertialeasing.c
+ * https://theorangeduck.com/page/inertial-easing <- the blogpost
+*/
+
+// Class to hold the "time" along the curve/function and scale of the function currently
+// Held in this class so both can be mutated and returned in one function call
+class InertialVals(
+    var t: Float = 0f,
+    var s: Float = 0f,
+    var offset: Float = 0f,
+)
+
+// Includes the negative portion
+fun smoothstep(t: Float, s: Float): Float {
+    return s * (if(t > 1.0f) 1.0f else 3*t*t - 2*t*t*t)
+}
+
+// Derivative of the above smoothstep
+fun smoothstepDt(t: Float, s: Float): Float {
+    return s * (if(t > 1.0f) 0.0f else 6*t - 6*t*t)
+}
+
+// Solve for smoothstep parameters `t` and `s` given new target `x` and velocity `v`
+fun smoothstepSolve( //TODO change these names to make more sense
+    vals: InertialVals,
+    x: Float, // Target Value x
+    v: Float,  // Goal velocity 
+    overshoot: Float = 0.05f,
+    eps: Float = 1e-8f
+) {
+    // If velocity is low enough then start from time zero
+    if (abs(v) < 1e-8f) {
+        vals.t = 0f
+        vals.s = x
+        return
+    }
+
+    // If x is negative then invert signs and re-Solve
+    if (x < 0f) {
+        smoothstepSolve(vals, -x, -v, overshoot, eps)
+        vals.s *= -1f
+        return
+    }
+
+    // Find the possible times that solve for the given `x` and `v`
+    // Quadratic formula!
+    val q: Float = sqrt(max((6*x - v)*(6*x - v) + 8*v*v, eps))
+    val t0: Float = (v - 6*x + q) / (4*v)
+    val t1: Float = (v - 6*x - q) / (4*v)
+
+    // Overshoot if the alternative time is between -0.5f and -(0.5f + overshoot)
+    vals.t = if(-0.5f > t1 && t1 > -(0.5f + overshoot)) t1 else t0;
+
+    // Find the un-scaled velocity at the fitted time
+    val vt: Float = smoothstepDt(vals.t, 1f);        
+
+    // Find the scaling factor so that the velocity at the fitted time matches
+    vals.s = if(abs(vt) < eps) 0f else v / vt;
+}