vreRtdUtil.h

Go to the documentation of this file.

/*******************************************************************************
** Copyright (c) 2025 MAK Technologies
** All rights reserved.
*******************************************************************************/
 
//! \file vreRtdUtil.h
//! \ingroup vreVrfRtd
//! \brief Utility functions for VREngage RTD components
//!
//! This file provides common utility functions and macros for coordinate
//! transformations, terrain queries, and other operations used by the VREngage
//! Real-Time Dynamics (RTD) components. It includes conversions between
//! VRF (Virtual Reality Forces) and RTD coordinate systems, orientation
//! transformations, terrain height calculation, and vehicle property modifiers
//! such as wind, speed limits, and altitude limits.
 
#pragma once
 
#include <Physics/smVec3r.h>
 
RTDVRFConfigWarnsForExternalIncludes()
#include <matrix/vltaitbryan.h>
 
#include <vrfobjcore/localObject.h>
 
#pragma warning(pop)
 
 
   class DtVrfObjectStateRepository;
class DtSimulationServices;
class DtPhysicalWorld;
class DtSetDataRequest;
 
//! \brief Macro to convert geodetic coordinates to local database coordinates
//! \param geo The geodetic coordinates to convert
//! \param local The resulting local database coordinates
//!
//! Converts geodetic coordinates (latitude, longitude, altitude) to local
//! database coordinates using the simulation's physical world.
#define GEODETIC_TO_LOCAL(geo, local) simulationServices()->physicalWorld()->geodeticToDatabase((geo), (local))
 
//! \brief Macro to convert local database coordinates to geodetic coordinates
//! \param local The local database coordinates to convert
//! \param geo The resulting geodetic coordinates
//!
//! Converts local database coordinates to geodetic coordinates (latitude, longitude, altitude)
//! using the simulation's physical world.
#define LOCAL_TO_GEODETIC(local, geo) simulationServices()->physicalWorld()->databaseToGeodetic((local), (geo));
 
//! \brief Version-dependent macro for accessing object name properties
//!
//! This macro provides version compatibility between different VRF plugin versions.
//! In version 4.4.0 and later, we use markingText, while in earlier versions we use objectName.
#if RTD_VRF_PLUGIN_VER >= 440
   #define VRF_OBJECT_GET_NAME markingText
#else
   #define VRF_OBJECT_GET_NAME objectName
#endif
 
//! \brief Macro to get a formatted name for an external entity
//! \param entity Pointer to the entity object
//! \return A string containing the marking text and name of the entity
//!
//! Creates a formatted display name for an external entity that includes both
//! the marking text (in single quotes) and the entity name (in parentheses).
#define GET_EXT_ENTITY_NAME(entity) ("'" + (entity)->getMarkingText() + "'" + " (" + (entity)->getName() + ')')
 
//! \brief Macro to get a formatted name for a VRF object
//! \param object Pointer to the VRF object
//! \return A string containing the marking text and UUID of the object
//!
//! Creates a formatted display name for a VRF object that includes both
//! the marking text (in single quotes) and the object's UUID (in parentheses).
#define GET_EXT_VRF_OBJ_NAME(object)                                                                                   \
   ("'" + (object)->markingText().stdString() + "'" + " (" + (object)->uuid().uuidString().stdString() + ')')
 
 
namespace vreRtd
{
//! \brief Utility functions for VREngage Real-Time Dynamics (RTD) components
//!
//! This namespace contains various utility functions for coordinate transformations,
//! terrain queries, and other operations used by the VREngage RTD components.
 
//! \brief Sets dead reckoning thresholds for an entity
//! \param entity The entity to set thresholds for
//! \param transThreshold Translational threshold in meters
//! \param rotationThreshold Rotational threshold in radians
//!
//! Sets the translational and rotational thresholds used for dead reckoning
//! calculations for the specified entity. These thresholds control when
//! position updates are sent across the network.
void setDeadreckoningThresholds(DtLocalObject* entity, double transThreshold, double rotationThreshold);
 
//! \brief Converts from a VRF/DtTaitBryan orientation to RTD/HPR
//! \param ori The VRF orientation to convert
//! \return The equivalent RTD HPR orientation
//!
//! Converts from a VRF/DtTaitBryan orientation to RTD/HPR orientation.
//! DtTaitBryan orientation is expected to be a rotation in the coordinate system with x straight, y right, z down.
//! HPR is expected to be a rotation in the coordinate system with y straight, x right, z up.
//! A DtTaitBryan(0,0,0) is hpr(0,0,0). A DtTaitBryan(pi/2,0,0) is hpr(-90,0,0) for example.
smVec3r VRFOriToRTDHPR(const DtTaitBryan& ori);
//! \brief Converts from an RTD/HPR orientation to VRF/DtTaitBryan
//! \param hpr The RTD HPR orientation to convert
//! \return The equivalent VRF/DtTaitBryan orientation
//!
//! Inverse transform of VRFOriToRTDHPR, converting from RTD HPR to VRF orientation.
DtTaitBryan HprToVRFOri(const smVec3r& hpr);
 
//! \brief Converts airplane world orientation from VRF to RTD HPR
//! \param ori The VRF world orientation to convert
//! \return The equivalent RTD world HPR orientation
//!
//! Converts airplane world orientation from VRF to RTD HPR. Even if VRF world coordinate
//! system and RTD world coordinate system are both WGS84, airplanes are differently oriented
//! in their local coordinate systems:
//! - In RTD, airplanes point along y-axis with z-axis up
//! - In VRF, airplanes point along x-axis with z-axis down
//!
//! An RTD airplane with zero rotation (in world coordinates) points to WGS84 y-axis with
//! 'up' pointing along WGS84 z-axis. A VRF airplane with zero rotation (in world coordinates)
//! points to WGS84 x-axis with 'up' pointing along WGS84 -z-axis (upside down).
smVec3r VRFWorldOriToRTDWorldHPR(const DtTaitBryan& ori);
 
//! \brief Converts airplane world orientation from RTD HPR to VRF
//! \param hpr The RTD world HPR orientation to convert
//! \return The equivalent VRF world orientation
//!
//! Inverse of VRFWorldOriToRTDWorldHPR, converting from RTD world HPR to VRF world orientation.
DtTaitBryan RTDWorldHprToVRFWorldOri(const smVec3r& hpr);
 
//! \brief Converts rotational velocity from VRF to RTD world coordinate system
//! \param entityWorldOrientation The entity's world orientation
//! \param rotationalVelocity The rotational velocity in VRF coordinate system
//! \return The equivalent rotational velocity in RTD world coordinate system
//!
//! Converts a rotational velocity vector from VRF to RTD world coordinate system.
smVec3r VRFRotVelToRTDRotVelWCS(const DtTaitBryan& entityWorldOrientation, const DtVector& rotationalVelocity);
 
//! \brief Converts rotational velocity from RTD to VRF coordinate system
//! \param entityWorldOrientation The entity's world orientation
//! \param rotationalVelocity The rotational velocity in RTD world coordinate system
//! \return The equivalent rotational velocity in VRF coordinate system
//!
//! Converts a rotational velocity vector from RTD world coordinate system to VRF.
DtVector RTDRotVelWCSToVRFRotVel(const DtTaitBryan& entityWorldOrientation, const smVec3r& rotationalVelocity);
 
//! \brief Converts topographic orientation to local orientation
//! \param physicalWorld Pointer to the physical world
//! \param localPosition The local position to convert at
//! \param topographicOrientation The topographic orientation to convert
//! \return The equivalent local orientation with respect to the database
//!
//! Converts a topographic orientation to a local orientation with respect to the database.
DtTaitBryan TopoToLocalWrtDatabase(
   DtPhysicalWorld* physicalWorld, const DtVector& localPosition, const DtTaitBryan& topographicOrientation);
 
//! \brief Converts topographic vector to local vector
//! \param physicalWorld Pointer to the physical world
//! \param localPosition The local position to convert at
//! \param vec The topographic vector to convert
//! \return The equivalent local vector with respect to the database
//!
//! Converts a topographic vector to a local vector with respect to the database.
DtVector TopoToLocalWrtDatabase(DtPhysicalWorld* physicalWorld, const DtVector& localPosition, const DtVector& vec);
 
//! \brief Converts local orientation to topographic orientation
//! \param physicalWorld Pointer to the physical world
//! \param localPosition The local position to convert at
//! \param localOrientation The local orientation to convert
//! \return The equivalent topographic orientation
//!
//! Converts a local orientation with respect to the database to a topographic orientation.
DtTaitBryan LocalWrtDatabaseToTopo(
   DtPhysicalWorld* physicalWorld, const DtVector& localPosition, const DtTaitBryan& localOrientation);
 
//! \brief Computes the 2D distance between two points
//! \param pnt1 The first point
//! \param pnt2 The second point
//! \return The 2D distance between the two points
//!
//! Computes the 2D distance between two points.
smReal compute2DDist(const smVec3r& pnt1, const DtVector& pnt2);
 
//! \brief Gets the heading-aligned speed vector
//! \param hpr The heading, pitch, and roll of the entity
//! \param speed The speed of the entity
//! \param xyOnly Whether to only consider the x and y components
//! \return The heading-aligned speed vector
//!
//! Gets the heading-aligned speed vector for an entity.
smVec3r getHeadingAlignedSpeedVector(const smVec3r& hpr, smReal speed, bool xyOnly = false);
 
//! \brief Gets the heading-aligned speed vector for an airplane
//! \param bot The airplane entity
//! \param speed The speed of the airplane
//! \param xyOnly Whether to only consider the x and y components
//! \return The heading-aligned speed vector
//!
//! Gets the heading-aligned speed vector for an airplane entity.
smVec3r getHeadingAlignedSpeedVector(FixedWingLibCGF::AirplaneBot* bot, smReal speed, bool xyOnly = false);
 
//! \brief Gets a vector matching the direction of an entity
//! \param physicalWorld Pointer to the physical world
//! \param sr Pointer to the object state repository
//! \param vecMagnitude The magnitude of the vector
//! \return The vector matching the direction of the entity
//!
//! Gets a vector matching the direction of an entity.
DtVector getVecMatchingDirection(DtPhysicalWorld* physicalWorld, DtVrfObjectStateRepository* sr, DtReal vecMagnitude);
 
//! \brief Applies an altitude limit to a vehicle
//! \param vehicle The vehicle to apply the limit to
//! \param maxAltitude The maximum altitude
//!
//! Applies an altitude limit to a vehicle.
void applyAltitudeLimit(RTD::VehicleBase* vehicle, const smReal& maxAltitude);
 
//! \brief Applies a speed limit to a vehicle
//! \param vehicle The vehicle to apply the limit to
//! \param maxSpeed The maximum speed
//!
//! Applies a speed limit to a vehicle.
void applySpeedLimit(RTD::VehicleBase* vehicle, const smReal& maxSpeed);
 
//! \brief Applies wind to a vehicle
//! \param physicalWorld Pointer to the physical world
//! \param vehicle The vehicle to apply wind to
//!
//! Applies wind to a vehicle.
void applyWind(DtPhysicalWorld* physicalWorld, RTD::VehicleBase* vehicle);
 
//! \brief Applies wind to a vehicle
//! \param vehicle The vehicle to apply wind to
//! \param vrfWindDir The wind direction in VRF coordinates
//! \param vrfWindSpeed The wind speed
//!
//! Applies wind to a vehicle.
void applyWind(RTD::VehicleBase* vehicle, double vrfWindDir, double vrfWindSpeed);
 
//! \brief Macro to convert an RTD heading to a VRF heading
//! \param hdgInDeg RTD heading in degrees
//! \return VRF heading in degrees
//!
//! Converts a heading from RTD coordinate system to VRF coordinate system.
//! In VRF, heading increases clockwise from north, while in RTD, heading increases counterclockwise from north.
#define RTDHeadingToVrfHeading(hdgInDeg) (-hdgInDeg)
 
//! \brief Macro to convert a VRF heading to an RTD heading
//! \param hdgInDeg VRF heading in degrees
//! \return RTD heading in degrees
//!
//! Converts a heading from VRF coordinate system to RTD coordinate system.
//! In VRF, heading increases clockwise from north, while in RTD, heading increases counterclockwise from north.
#define VrfHeadingToRTDHeading(hdgInDeg) (-hdgInDeg)
 
//! \brief Macro to convert a vector to DtVector
//! \param v Vector to convert
//! \return Equivalent DtVector
#define toDtVector(v) DtVector((v)[0], (v)[1], (v)[2])
 
//! \brief Macro to convert a vector to smVec3r (single precision 3D vector)
//! \param v Vector to convert
//! \return Equivalent smVec3r
#define toSmVec3r(v) smVec3r((v)[0], (v)[1], (v)[2])
 
//! \brief Macro to convert a vector to smVec2r (single precision 2D vector)
//! \param v Vector to convert
//! \return Equivalent smVec2r
#define toSmVec2r(v) smVec2r((v)[0], (v)[1])
 
//! \brief Macro to convert a vector to smVec3d (double precision 3D vector)
//! \param v Vector to convert
//! \return Equivalent smVec3d
#define toSmVec3d(v) smVec3d((v)[0], (v)[1], (v)[2])
 
//! \brief Macro to convert a vector to smVec2d (double precision 2D vector)
//! \param v Vector to convert
//! \return Equivalent smVec2d
#define toSmVec2d(v) smVec2d((v)[0], (v)[1])
 
//! \brief Macro to convert a geodetic coordinate to RTD LLA (Latitude, Longitude, Altitude)
//! \param geodeticCoord Geodetic coordinate in radians with altitude in meters
//! \return RTD LLA with latitude and longitude in degrees, altitude in meters
#define toRTDLLA(geodeticCoord)                                                                                        \
   smVec3r(smMath::radToDeg(geodeticCoord[0]), smMath::radToDeg(geodeticCoord[1]), geodeticCoord[2])
 
//! \brief Macro to convert a geodetic coordinate to RTD LL (Latitude, Longitude)
//! \param geodeticCoord Geodetic coordinate in radians
//! \return RTD LL with latitude and longitude in degrees
#define toRTDLL(geodeticCoord) smVec2r(smMath::radToDeg(geodeticCoord[0]), smMath::radToDeg(geodeticCoord[1]))
 
//! \brief Macro to convert an RTD LLA to geodetic coordinate
//! \param lla RTD LLA with latitude and longitude in degrees, altitude in meters
//! \return Geodetic coordinate in radians with altitude in meters
#define toGeodetic(lla) DtGeodeticCoord(smMath::degToRad(lla[0]), smMath::degToRad(lla[1]), lla[2])
 
//! \brief Computes the terrain height at a specified local position
//! \param simMan Pointer to the simulation services
//! \param localPosition Local position to query terrain height at
//! \param result Reference where the computed terrain height will be stored
//! \return True if terrain height computation was successful, false otherwise
//!
//! Uses the terrain provider to compute the height of terrain at the specified local position.
//! Returns false if the terrain query fails or the position is outside the terrain bounds.
bool computeTerrainHeight(const DtSimulationServices* simMan, const DtVector& localPosition, double& result);
 
//! \brief Gets the terrain height at a specified local position
//! \param simMan Pointer to the simulation services
//! \param localPosition Local position to query terrain height at
//! \return Terrain height in meters, or 0.0 if computation fails
//!
//! Convenience function that calls computeTerrainHeight and returns the result directly.
//! Returns 0.0 if the terrain height could not be determined.
double getTerrainHeight(const DtSimulationServices* simMan, const DtVector& localPosition);
 
//! \brief Gets the height above terrain (HAT) for a geodetic position
//! \param simMan Pointer to the simulation services
//! \param pos Geodetic position to query HAT for
//! \return Height above terrain in meters
//!
//! Calculates the height above terrain by converting the geodetic position to local coordinates,
//! determining the terrain height at that location, and subtracting it from the position's altitude.
double getHAT(const DtSimulationServices* simMan, const DtGeodeticCoord& pos);
 
} // namespace vreRtd
 
//! \brief RTD namespace forward declarations
namespace RTD
{
//! \brief World class for RTD simulation environment
class World;
} // namespace RTD
 
//! \brief Forward declarations for the FixedWingLibCGF namespace
namespace FixedWingLibCGF
{
//! \brief Class representing a fixed-wing aircraft in CGF
class CGFAirplane;
} // namespace FixedWingLibCGF
 
//! \brief Forward declarations for the RotorLibCGF namespace
namespace RotorLibCGF
{
//! \brief Class representing a helicopter in CGF
class CGFHelicopter;
} // namespace RotorLibCGF
 
//! \brief Macro to check an expression and return if false, logging a console warning
//! \param expression The expression to check
//! \param complaint The message to log if the expression is false
//!
//! Evaluates the given expression and, if false, logs a warning to the object console
//! and returns from the current function (with no return value).
#define RTD_CHECK_CONSWARN_RETURN(expression, complaint)                                                               \
   if (!(expression))                                                                                                  \
   {                                                                                                                   \
      objectConsoleWarn() << (complaint) << std::endl;                                                                 \
      return;                                                                                                          \
   }
 
//! \brief Macro to check an expression and return if false, logging a DtWarn
//! \param expression The expression to check
//! \param complaint The message to log if the expression is false
//!
//! Evaluates the given expression and, if false, logs a warning using DtWarn
//! and returns from the current function (with no return value).
#define RTD_CHECK_DTWARN_RETURN(expression, complaint)                                                                 \
   if (!(expression))                                                                                                  \
   {                                                                                                                   \
      DtWarn(complaint "\n");                                                                                          \
      return;                                                                                                          \
   }
 
//! \brief Macro to check an expression and return a value if false, logging a console warning
//! \param expression The expression to check
//! \param complaint The message to log if the expression is false
//! \param retVal The value to return if the expression is false
//!
//! Evaluates the given expression and, if false, logs a warning to the object console
//! and returns the specified value from the current function.
#define RTD_CHECK_CONSWARN_RETURN2(expression, complaint, retVal)                                                      \
   if (!(expression))                                                                                                  \
   {                                                                                                                   \
      objectConsoleWarn() << (complaint) << std::endl;                                                                 \
      return retVal;                                                                                                   \
   }
 
//! \brief Macro to check an expression and return a value if false, logging a DtWarn
//! \param expression The expression to check
//! \param complaint The message to log if the expression is false
//! \param retVal The value to return if the expression is false
//!
//! Evaluates the given expression and, if false, logs a warning using DtWarn
//! and returns the specified value from the current function.
#define RTD_CHECK_DTWARN_RETURN2(expression, complaint, retVal)                                                        \
   if (!(expression))                                                                                                  \
   {                                                                                                                   \
      DtWarn(complaint "\n");                                                                                          \
      return retVal;                                                                                                   \
   }