frcsim::detail Namespace Reference

Functions
Vector3	worldAnchor (const RigidBody *body, const Vector3 &local_anchor)
	Converts a body-local anchor point into world space.
double	clampValue (double value, double min_value, double max_value)
	Clamps a scalar into a closed range.
void	applyPositionCorrection (RigidBody *body_a, RigidBody *body_b, const Vector3 &error, double gain)
	Applies a positional correction split across two bodies by mass.
void	applyVelocityCorrection (RigidBody *body_a, RigidBody *body_b, const Vector3 &relative_velocity, double gain)
	Applies a velocity-space correction split across two bodies by mass.
Vector3	worldAxisOrFallback (const RigidBody *body, const Vector3 &local_axis, const Vector3 &fallback_axis)
	Rotates a local axis into world space and falls back if needed.
double	signedTwistAngleRad (const Quaternion &qa, const Quaternion &qb, const Vector3 &axis_world)
	Computes the signed twist between two orientations about an axis.

Variables
constexpr double	kJointEpsilon = 1e-9
const double	kPi = std::acos(-1.0)

Function Documentation

applyPositionCorrection()

void frcsim::detail::applyPositionCorrection ( RigidBody * body_a, RigidBody * body_b, const Vector3 & error, double gain )

inline

Applies a positional correction split across two bodies by mass.

The correction is distributed using inverse mass so lighter bodies move more than heavier bodies. Kinematic bodies are treated as immovable and therefore receive no positional update.

Parameters

body_a	First rigid body to correct.
body_b	Second rigid body to correct.
error	World-space position error vector in meters that should be removed.
gain	Fraction of the error to resolve this step, typically in the range [0, 1].

applyVelocityCorrection()

void frcsim::detail::applyVelocityCorrection ( RigidBody * body_a, RigidBody * body_b, const Vector3 & relative_velocity, double gain )

inline

Applies a velocity-space correction split across two bodies by mass.

This is the velocity analogue of applyPositionCorrection(): the supplied relative velocity impulse is divided by inverse mass so the lighter body receives the larger change. Kinematic bodies are left unchanged.

Parameters

body_a	First rigid body to correct.
body_b	Second rigid body to correct.
relative_velocity	World-space relative velocity vector in meters per second to reduce.
gain	Fraction of the velocity error to resolve this step.

clampValue()

double frcsim::detail::clampValue ( double value, double min_value, double max_value )

inline

Clamps a scalar into a closed range.

Parameters

value	Input value.
min_value	Lower bound.
max_value	Upper bound.

Returns

value constrained to [min_value, max_value].

signedTwistAngleRad()

double frcsim::detail::signedTwistAngleRad ( const Quaternion & qa, const Quaternion & qb, const Vector3 & axis_world )

inline

Computes the signed twist between two orientations about an axis.

The helper compares qb against qa, extracts the shortest relative rotation, and projects that rotation onto axis_world using the right-hand rule. The return value is a signed angle in radians: its magnitude is the amount of twist, and its sign indicates whether the target orientation rotates in the positive or negative direction about the supplied axis.

The intent is to match the coordinate conventions used throughout WPILib: a right-handed field frame with +X downfield, +Y left/right across the field depending on alliance perspective, and +Z upward. That convention is documented here: https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html

Keep qa, qb, and axis_world in the same world frame when calling this helper. If axis_world is degenerate, the normalized axis collapses toward zero and the returned signed twist likewise tends toward zero.

Parameters

qa	Reference orientation.
qb	Target orientation to compare against qa.
axis_world	World-space rotation axis, expressed as a unit direction vector when possible.

Returns

Signed twist angle in radians about axis_world.

worldAnchor()

Vector3 frcsim::detail::worldAnchor ( const RigidBody * body, const Vector3 & local_anchor )

inline

Converts a body-local anchor point into world space.

Parameters

body	Rigid body that owns the anchor.
local_anchor	Anchor position expressed in the body's local coordinate frame, in meters.

Returns

World-space anchor point obtained by rotating the local offset by the body orientation and adding the body position.

worldAxisOrFallback()

Vector3 frcsim::detail::worldAxisOrFallback ( const RigidBody * body, const Vector3 & local_axis, const Vector3 & fallback_axis )

inline

Rotates a local axis into world space and falls back if needed.

Parameters

body	Body whose orientation defines the local axis transform.
local_axis	Axis in the body's local coordinate frame, treated as a direction vector rather than a position and therefore unitless.
fallback_axis	World-space fallback direction, also unitless.

Returns

Normalized world-space axis, or fallback_axis if the rotated axis collapses to zero length.

Variable Documentation

kJointEpsilon

double frcsim::detail::kJointEpsilon = 1e-9

constexpr

kPi

const double frcsim::detail::kPi = std::acos(-1.0)

inline