function

LieGroups.SO3.Quat.normalize

Normalize quaternion to unit length

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`q`	`Real`	`[4]`
output	`q_n`	`Real`	`[4]`

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

function normalize "Normalize quaternion to unit length"
  input Real q[4];
  output Real q_n[4];
protected
  Real n;
  constant Real tolerance = 1.0e-12;
algorithm
  n := sqrt(q[1]^2 + q[2]^2 + q[3]^2 + q[4]^2);
  // Keep the zero-input fallback total so generated-code evaluators do not
  // divide by zero while constructing inactive branches.
  q_n := if n > tolerance then q / n else {1.0, 0.0, 0.0, 0.0};
end normalize;