Class UtilityMath
Centralised math helpers that would otherwise be duplicated across vehicle controllers, path-following systems, feedback calculators, and UI layers.
public static class UtilityMath
Inheritance
Remarks
The functions here fall into four groups driven by recurring needs in the simulation:
- Unit conversions — Physics runs in SI (m/s), but research participants see speed in km/h or mph on the in-sim dashboard, and exported data may need both. Centralising the conversion factors prevents subtle inconsistencies when different systems hard-code slightly different multipliers.
- Waypoint arithmetic — Discrete waypoint paths require robust "have I arrived?" and "which way do I turn?" queries that are independent of frame rate and vehicle speed. Proximity checks alone fail at high speed; signed-projection math is more reliable.
- Range mapping — Sensor values, battery percentages, and control signals all require rescaling from one range to another. Inline arithmetic for this is error-prone and obscures intent; a named extension method makes the operation explicit.
- Numerical integration — Feedback metrics (e.g. prosocial score) require the area under a speed-response curve over a recorded interval. The trapezoidal rule gives acceptable accuracy for the smooth curves produced by physical simulation, without the complexity of higher-order methods.
Methods
CalculateProgressAlongWaypoints(Vector3, Vector3, Vector3)
Returns a scalar in [0, 1] indicating how far along the segment from
goingFromPos to goingToPos the actor currently is,
computed via dot-product projection.
public static float CalculateProgressAlongWaypoints(Vector3 carPos, Vector3 goingFromPos, Vector3 goingToPos)
Parameters
| Name | Type | Description |
|---|---|---|
carPos | Vector3 | The actor's current world position. |
goingFromPos | Vector3 | The start of the path segment. |
goingToPos | Vector3 | The end of the path segment. |
Returns
float — Normalised scalar progress along the segment. 0 = at start, 1 = at end,
<0 = before start, >1 = past end.
Remarks
The value is the normalised dot product of the actor-to-start vector onto the
segment direction: dot(carPos - from, to - from) / |to - from|². Values
below 0 mean the actor is behind the start waypoint; values above 1 mean it has
passed the end waypoint. This is used directly by Single)
and indirectly by Vector3) to locate the closest point on
the segment.
If the segment has zero length (both waypoints are identical), the function returns 1 rather than dividing by zero, treating the actor as already "past" that degenerate segment.
Convert_Kilometers_Per_Hour_To_Meters_Per_Second(float)
Converts an externally-supplied speed (e.g. from a research protocol that specifies a target speed in km/h) into the m/s value required by the physics controller or PID setpoint.
public static float Convert_Kilometers_Per_Hour_To_Meters_Per_Second(float speed)
Parameters
| Name | Type | Description |
|---|---|---|
speed | float | Speed in kilometers per hour. |
Returns
float — Speed in meters per second.
Convert_Meters_Per_Second_To_Kilometers_Per_Hour(float)
Converts a physics-layer speed value from SI units to the unit expected by the metric dashboard display and any exported data fields labelled in km/h.
public static float Convert_Meters_Per_Second_To_Kilometers_Per_Hour(float speed)
Parameters
| Name | Type | Description |
|---|---|---|
speed | float | Speed in meters per second. |
Returns
float — Speed in kilometers per hour.
Remarks
The exact multiplier is 3.6 (= 1000 m/km ÷ 3600 s/h). Using a named constant here rather than an inline magic number communicates intent at every call site.
Convert_Meters_Per_Second_To_Miles_Per_Hour(float)
Converts a physics-layer speed value from SI units to the unit expected by the North-American dashboard display and any exported data fields labelled in mph.
public static float Convert_Meters_Per_Second_To_Miles_Per_Hour(float speed)
Parameters
| Name | Type | Description |
|---|---|---|
speed | float | Speed in meters per second. |
Returns
float — Speed in miles per hour.
Remarks
All physics and controller logic in the simulation uses m/s internally. Converting only at the boundary (display, export) avoids propagating non-SI units into the control loop, where mixing units causes subtle scaling bugs.
Convert_Miles_Per_Hour_To_Meters_Per_Second(float)
Converts an externally-supplied speed (e.g. from a research protocol that specifies a target speed in mph) into the m/s value required by the physics controller or PID setpoint.
public static float Convert_Miles_Per_Hour_To_Meters_Per_Second(float speed)
Parameters
| Name | Type | Description |
|---|---|---|
speed | float | Speed in miles per hour. |
Returns
float — Speed in meters per second.
Distance2D(Vector3, Vector3)
Returns the horizontal (ground-plane) distance between two world positions, ignoring elevation differences.
public static float Distance2D(Vector3 pointA, Vector3 pointB)
Parameters
| Name | Type | Description |
|---|---|---|
pointA | Vector3 | First world position. |
pointB | Vector3 | Second world position. |
Returns
float — Distance on the XZ (ground) plane, independent of elevation.
Remarks
Road surfaces in the simulation are not flat — they follow terrain. Two actors on the same stretch of road may have significantly different Y coordinates depending on where they sit along a slope. Using full 3-D distance for proximity checks (e.g. sensor detection, gap-keeping) would report inflated distances on hills and undercount gaps between vehicles at different elevations on the same road. Zeroing Y before computing distance keeps all proximity logic consistent with the bird's-eye-view map geometry on which routes are authored.
GetCrossTrackError(Vector3, Vector3, Vector3)
Returns the perpendicular (lateral) distance between the actor's position and the ideal path line defined by two consecutive waypoints.
public static float GetCrossTrackError(Vector3 carPos, Vector3 goingFromPos, Vector3 goingToPos)
Parameters
| Name | Type | Description |
|---|---|---|
carPos | Vector3 | The actor's current world position. |
goingFromPos | Vector3 | The start of the reference path segment. |
goingToPos | Vector3 | The end of the reference path segment. |
Returns
float — Non-negative perpendicular distance from carPos to the nearest
point on the segment. Zero means the actor is exactly on the path.
Remarks
Euclidean distance to the nearest waypoint is not a useful path-following error signal: it is always positive (so a PID controller cannot tell which side of the path the actor is on) and it grows as the actor approaches the end of the segment even when tracking perfectly. Cross-track error — the distance from the actor to the nearest point on the line through the two waypoints — is the standard error metric for Stanley and pure-pursuit path-following controllers.
Implementation: the nearest point on the segment is found by projecting the actor position onto the segment direction (reusing the same dot-product projection as Vector3)), then measuring the distance from the actor to that projected point. The result is always non-negative; callers that need a signed error (left vs. right of path) should combine this with Vector3).
HasPassedWaypoint(Vector3, Vector3, Vector3, float)
Determines whether an actor has passed a target waypoint on a path segment, regardless of the actor's speed or the simulation frame rate.
public static bool HasPassedWaypoint(Vector3 carPos, Vector3 goingFromPos, Vector3 goingToPos, float PassThreshold = 1)
Parameters
| Name | Type | Description |
|---|---|---|
carPos | Vector3 | The actor's current world position. |
goingFromPos | Vector3 | The waypoint the actor is travelling from. |
goingToPos | Vector3 | The waypoint the actor is travelling toward. |
PassThreshold | float | Scalar progress value at which the waypoint is considered passed. The default of 1.0 means the actor must be at or beyond the target waypoint along the segment. Values below 1 can be used to trigger transitions slightly early. |
Returns
bool — true if the actor has passed the waypoint; otherwise, false.
Remarks
A proximity check (distance to waypoint < threshold) fails at high speed: the
actor can jump past the waypoint between frames and never enter the threshold
radius. Projecting the actor's position onto the waypoint segment (scalar progress
≥ PassThreshold) is frame-rate independent — it detects
passage based on whether the actor is geometrically ahead of the waypoint, not
how close it is.
Based on: http://www.habrador.com/tutorials/linear-algebra/2-passed-waypoint/
Integrate(Func<float, float>, params float[])
Approximates the definite integral of a function over an explicitly-provided, potentially non-uniform sequence of x-values using the trapezoidal rule.
public static float Integrate(Func<float, float> f, params float[] x)
Parameters
| Name | Type | Description |
|---|---|---|
f | Func<float, float> | The function to evaluate at each provided x-value. |
x | float[] | Ordered sequence of x-values. Must contain at least two elements. Spacing does not need to be uniform. Passing fewer than two values returns 0. |
Returns
float — Estimated area under f across the segments defined by the
provided x-values. Returns 0 if fewer than two x-values are supplied.
Remarks
Unlike Int32), which requires a uniform step size,
this overload accepts an arbitrary sequence of sample points. This is necessary
when integrating over recorded telemetry data where frames are captured at the
simulation's variable frame rate rather than at a fixed timestep. Each pair of
consecutive x-values forms one trapezoid; the interval width h = x[i+1] - x[i]
is computed per segment, so irregular spacing is handled correctly.
IntegrateCurve(AnimationCurve, float[])
Approximates the definite integral of a Unity AnimationCurve over an arbitrary set of sample points using the trapezoidal rule.
public static float IntegrateCurve(this AnimationCurve curve, float[] x)
Parameters
| Name | Type | Description |
|---|---|---|
curve | AnimationCurve | The animation curve to integrate. |
x | float[] | Ordered sample points (x-axis values) at which the curve is evaluated. More samples improve accuracy; for smooth vehicle-speed curves, 10–20 points per second of data is usually sufficient. |
Returns
float — The estimated area under the curve between x[0] and x[x.Length-1].
Remarks
This overload exists because AnimationCurve values are only
accessible through Evaluate(float t), not as a closed-form function.
Wrapping the call in a delegate lets the same Single[]) core handle
both curves and mathematical functions without duplicating the quadrature loop.
Use this when computing the area under a recorded speed or response curve that
was baked into an AnimationCurve asset (e.g. for prosocial or braking metrics).
Map(float, float, float, float, float, bool)
Linearly remaps x from the source range
[x1, x2] to the target range
[y1, y2].
public static float Map(this float x, float x1, float x2, float y1, float y2, bool clamping = false)
Parameters
| Name | Type | Description |
|---|---|---|
x | float | The value to remap. |
x1 | float | Start of the source range. Must not equal x2. |
x2 | float | End of the source range. Must not equal x1. |
y1 | float | Start of the target range. |
y2 | float | End of the target range. |
clamping | bool | When true, the result is clamped to [y1, y2] so extrapolated inputs do not produce out-of-range outputs. |
Returns
float — The remapped value in the target range (or beyond it if clamping is false).
Remarks
Linear remapping appears throughout the simulation wherever a raw physical
quantity must be scaled to a different domain: sensor voltages to normalised
weights, battery level to a UI bar width, PID error to a steering angle, etc.
Duplicating the formula y1 + (y2-y1)/(x2-x1)*(x-x1) inline is
error-prone (the division by zero case is easy to forget) and obscures which
transformation is happening. Using a named extension method makes the intent
self-documenting at every call site.
The implementation rewrites the formula in slope-intercept form
(y = m*x + c) rather than the more common expanded form because it makes
the linear relationship explicit and separates the constant term (c) from
the slope (m), which aids readability during debugging.
When clamping is false, values outside the source range
extrapolate linearly. Enable clamping whenever the physical constraint guarantees
the input is in range but floating-point noise might push it slightly outside (e.g.
a battery level that has been clamped elsewhere but arrives as 100.0001f).
Exceptions
ArgumentException — Thrown when x1 equals x2, which would require
division by zero.
Map01(float, float, float, bool)
Normalises x to a [0, 1] fraction within the range
[x1, x2].
public static float Map01(this float x, float x1, float x2, bool clamping = false)
Parameters
| Name | Type | Description |
|---|---|---|
x | float | The value to normalise. |
x1 | float | The value that should map to 0. |
x2 | float | The value that should map to 1. |
clamping | bool | When true, the result is clamped to [0, 1]. Use this when the input is expected to be in range but might exceed it due to floating-point drift. |
Returns
float — A fraction in [0, 1] representing where x falls within the
range. Values outside the range extrapolate unless clamping
is true.
Remarks
Normalisation (mapping to [0, 1]) is such a common operation that providing a
dedicated overload avoids forcing callers to write the verbose
Map(x, min, max, 0f, 1f). It also reduces the risk of accidentally
swapping the source and destination arguments, which is a silent bug when both
are numerically similar.
ShortestRotation(Quaternion, Quaternion)
Returns the minimal-arc rotation delta between two orientations, accounting for the double-cover property of quaternions.
public static Quaternion ShortestRotation(Quaternion to, Quaternion from)
Parameters
| Name | Type | Description |
|---|---|---|
to | Quaternion | The target orientation. |
from | Quaternion | The starting orientation. |
Returns
Quaternion — A quaternion representing the shortest-arc rotation from from
to to. The result is always the minimal rotation; its angle
is guaranteed to be ≤ 180°.
Remarks
Every 3-D rotation can be represented by two unit quaternions: q and
-q. When computing angular velocity or interpolating between two recorded
poses, naively computing to * Inverse(from) can take the "long way round"
(arc > 180°) if the two quaternions sit on opposite hemispheres of the 4-D unit
sphere. A negative dot product reveals this case; negating one operand before
inversion flips it onto the same hemisphere and forces the result to use the
shortest arc. This matters for the camera effector and the driving feedback
metric, both of which integrate angular displacement over time.
SteerDirection(Vector3, Vector3, Vector3)
Returns a discrete turn command (+1 right, -1 left, 0 straight) indicating which direction an actor must steer to move toward a target waypoint.
public static float SteerDirection(Vector3 rightVector, Vector3 currentPosition, Vector3 targetPosition)
Parameters
| Name | Type | Description |
|---|---|---|
rightVector | Vector3 | The actor's right-facing world direction (normalised). |
currentPosition | Vector3 | The actor's current world position. |
targetPosition | Vector3 | The world position of the next waypoint. |
Returns
float — 1 if the target is to the right, -1 if to the left,
0 if the actor is within the dead zone of straight-ahead alignment.
Remarks
The sign of the dot product between the actor's right vector and the vector toward the target encodes which side the target is on: positive means it is to the right, negative means it is to the left. Using the dot product rather than a cross product naturally handles 3-D space while keeping the logic planar (Y contributions cancel on flat roads and are minimal on gentle slopes).
A dead zone of ±0.1 suppresses oscillation when the actor is already aligned with the waypoint — without it, floating-point noise would cause rapid ±1 toggling, which transmits as jitter to the steering actuator.
Based on: http://www.habrador.com/tutorials/linear-algebra/3-turn-left-or-right
TrapezoidalIntegrate(Func<float, float>, float, float, int)
Approximates the definite integral of a function over a uniformly-spaced interval using the composite trapezoidal rule.
public static float TrapezoidalIntegrate(Func<float, float> f, float x_low, float x_high, int N_steps)
Parameters
| Name | Type | Description |
|---|---|---|
f | Func<float, float> | The function to integrate. Must be defined on [x_low, x_high]. |
x_low | float | Lower bound of the integration interval. |
x_high | float | Upper bound of the integration interval. |
N_steps | int | Number of sub-intervals. Higher values give more accurate results at the cost of more function evaluations. A value of 100 is sufficient for typical speed curves. |
Returns
float — Estimated area under f from x_low to x_high.
Remarks
The trapezoidal rule was chosen over simpler (rectangle/midpoint) and more complex (Simpson's, Gaussian) alternatives because it converges well for the smooth, continuous functions produced by physical simulation and is trivially implementable without additional data structures. For vehicle speed profiles (which are smooth because acceleration is bounded), the error is O(h²) — acceptable for behavioural scoring where single-digit percentage accuracy is sufficient.
Prefer Single[]) when the x-values are non-uniform (e.g. drawn from a recorded time series at variable frame intervals), since that overload handles arbitrary spacing.