function

Planning.Dubins.advance

Advance a pose along one normalized Dubins segment

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`position`	`Real`	`[2]`
input	`heading`	`Real`
input	`kind`	`Planning.Dubins.SegmentType`
input	`normalizedLength`	`Real`	`(min=0.0)`
input	`turnRadius`	`Real`
output	`result`	`Planning.Dubins.Pose`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function advance "Advance a pose along one normalized Dubins segment"
  input Real position[2];
  input Real heading;
  input Planning.Dubins.SegmentType kind;
  input Real normalizedLength(min=0.0);
  input Real turnRadius;
  output Planning.Dubins.Pose result;
algorithm
  assert(turnRadius > 0.0, "Dubins turn radius must be positive");
  if kind == SegmentType.left then
    result.position[1] := position[1] + turnRadius
      * (sin(heading + normalizedLength) - sin(heading));
    result.position[2] := position[2] + turnRadius
      * (-cos(heading + normalizedLength) + cos(heading));
    result.heading := heading + normalizedLength;
  elseif kind == SegmentType.right then
    result.position[1] := position[1] + turnRadius
      * (sin(heading) - sin(heading - normalizedLength));
    result.position[2] := position[2] + turnRadius
      * (cos(heading - normalizedLength) - cos(heading));
    result.heading := heading - normalizedLength;
  else
    result.position := position + turnRadius * normalizedLength
      * {cos(heading), sin(heading)};
    result.heading := heading;
  end if;
end advance;