Geometry Package

This package provides features for geometric calculation. Before reading the code in geometry package, please understand these words.

Cartesian Coordinate System

Generally, it is also called a rectangular coordinate system or orthogonal coordinate system. In geometry package, we treat 3-dimension cartesian coordinate system with x,y,z axis.

Frenet Coordinate System

The frenet coordinate system is a coordinate system that is defined along a curve. In the geometry package, a curve consisted of 3 3rd order polynomials about each x,y, and z axis in a cartesian coordinate system. We define the shape of the curve by changing s value of the 3rd order polynomials as^3+bs^2+cs+d in range s = [0,1]. In this coordinate system, s value is normalized in range s = [0,1], s value does not consider the length when the curve is stretched.

If the coefficient a,b = 0, the shape of the curve is a line segment. In this case, the position of the s = 0 is the start point, and the position of the s = 1 is the end position.

If the coefficient a,b,c = 0, the shape of the curve is a point. In this case, only s = 0 case is valid, all of the other cases are invalid. If you find a case that the shape is a point but s is not equal to 0, please notify to developers.

Some of functions in the geometry package have denormalize_s argument with bool type. If the denormalize_s = false, the return value s of the function is normalized and in range s = [0,1]. If the denormalize_s = true, the return value s is denormalized and it returns the value s times the length of the curve.

\warning A catmull-rom spline curve consists of multiple hermite curves. Each hermite curve has a different length. So, the catmull-rom spline curve can not normalize its length and frenet coordinate of catmull-rom spline curve is always denormalized.

Changelog for package geometry

21.2.2 (2025-12-19)

21.2.1 (2025-12-18)

• Revert "Bump version of scenario_simulator_v2 from version 21.2.0 to version 22.0.0" This reverts commit 95b2ebee5ed7df8b52a058e92f05e121e2bf75b4.
• Bump version of scenario_simulator_v2 from version 21.2.0 to version 22.0.0
• Contributors: Kotaro Yoshimoto, Release Bot

21.2.0 (2025-12-16)

• Merge pull request #1760 from tier4/ref/RJD-1921-simplify-follow-trajectory
• Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1921-simplify-follow-trajectory
• Merge branch 'master' into ref/RJD-1921-simplify-follow-trajectory
• Refactor follow_trajectory - replace trajectory iteration with sliding window model and structured state handlers
  • add initial waypoint insertion in simulator_core.hpp applyFollowTrajectoryAction() (insert current entity pose as vertices[0])
  • expose remaining_distance_tolerance as public constant in follow_waypoint_controller.hpp (change value from 1e-2 to 0.1 for more lenient final waypoint arrival threshold)
  • add geometry/vector3/is_finite.hpp with isfinite() overload for Vector3 types
  • add geometry/vector3/scalar_to_direction_vector.hpp with scalarToDirectionVector() function
  • introduce structured waypoint queries: previous_waypoint(), target_waypoint(), nearest_timed_waypoint() lambda accessors
  • replace linear trajectory iteration with explicit two-waypoint sliding window (vertices[0] = previous, vertices[1] = target)
  • implement comprehensive validation suite: validate_entity_status(), validate_trajectory(), validate_arrival_time(), validate_acceleration_constraints(), validate_desired_motion()
  • refactor distance calculation into composable functions: distance_to_waypoint(), distance_to_target_waypoint(), distance_to_timed_or_final_waypoint()
  • add special case handling for vertices[0] in distance_to_waypoint() (direct entity->previous distance)
  • separate intermediate/final waypoint handling into handle_intermediate_waypoint() and handle_final_waypoint() with explicit state tables
  • add exhaustive documentation tables mapping all entity state/timing combinations to return values (nullopt/EntityStatus/throw/recurse)
  • add structured logging: log_waypoint_action lambda with verbose_action and verbose_move_action flags
  • add verbose_input_state debug logging with entity state, waypoints, distances, and trajectory metadata
  • fix immobile validation: only throw error when entity progressed past previous waypoint but stopped too far from target (allows waiting at trajectory start with target_speed=0.0)
  • replace std::isnan()/std::isinf() with isfinite() for unified validation
  • use 'not' and 'and' operators consistently instead of '!' and '&&'
  • reorganize code into logical sections: WAYPOINT QUERIES, DISTANCE, TIME, VALIDATION, ENTITY UPDATE, VELOCITY, WAYPOINT HANDLING, EXECUTION
• Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto

21.1.1 (2025-12-10)

• Merge branch 'master' into update-sonarqube-settings
• Merge branch 'master' into update-sonarqube-settings
• Contributors: Kotaro Yoshimoto

21.1.0 (2025-12-10)

• Merge branch 'master' into feature/unknown
• Merge branch 'master' into feature/unknown
• Contributors: Kotaro Yoshimoto

21.0.0 (2025-12-10)

• Merge pull request #1731 from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance-step-2-part1
• Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance-step-2-part1
• Fix subspline altitude range Get approximation of altitude range from control points (like the spline itself) only in the subspline segment
• Add comment explaining the altitiude range is an approximation
• Merge remote-tracking branch 'tier4/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance-step-2-part1
• Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance-step-2-part1
• Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance-step-2-part1

File truncated at 100 lines see the full file