Nav Generation Functions (navGenerationFunctions)

The Nav Generation Functions example demonstrates how to add new functions to generate nav edges in your nav data.

Nav generation functions are specified in appData/settings/vrfSim/navigationProfiles.mtl. That file specifies a list of plugins that should be loaded. By default the plugin generated by this example will automatically be loaded. Each profile in that file then specifies a list of functions for generating nav edges and nav points.

A nav edge is a line through the nav data that can connect to separate points that might not otherwise be connected. It can be useful for indicating that entities may path plan through areas of the terrain that do not look to be traversable by the Nav Generator, such as closed doors, ladders, elevators, etc.

A nav point is a point of interest within the nav data. These can be queried and used by entities at run time.

New nav generation functions can be called based on dynamic terrain data, feature data queries, or as general functions that implement a custom algorithm based on some other data.

This example adds two functions that create nav edges. The first generates edges that allow lifeforms to navigate through closed doors that are implemented using dynamic terrain features. The second can generate nav edges based on terrain path feature data.

/*******************************************************************************
** Copyright (c) 2018 MAK Technologies, Inc.
** All rights reserved.
*******************************************************************************/

#include <vrfNavigation/navGenPluginExtension.h>
#include <vrfNavigation/navGeneration.h>
#include "navGenerationFunctions.h"

// This plugin adds new nav generation functions to the VR-Forces Nav Generator.
// This can allow you to add nav data for your terrain in a custom manner based
// on feature data, dynamic terrain features, or some other mechanism of your
// choosing. This plugin is loaded by default. Nav Generator plugins are
// specified in the navigationProfiles.mtl configuration file.

extern "C" {
   

   DT_VRF_NAV_GEN_DLL_PLUGIN bool DtInitializeNavGenPlugin(DtNavDataGenerator* navGenerator)
   {
      // Register a nav edge generation function for dynamic terrain features.
      navGenerator->addNavEdgeFromDynamicTerrainFunction("from-intersection-normal", edgeFromIntersectionNormal, 0);

      // Register a nav edge generation function for terrain path features.
      navGenerator->addNavEdgeFromFeatureDataFunction("from-path-feature", edgeFromPathFeature, 0);
      
      return true;
   }
}

/*******************************************************************************
** Copyright (c) 2018 MAK Technologies, Inc.
** All rights reserved.
*******************************************************************************/

#include "navGenerationFunctions.h"
#include <vrfNavigation/navigationGeneratorInput.h>
#include <vrfutil/rwDynamicTerrainInformation.h>
#include <vrfutil/kinematicTools.h>
#include <terrainInterface/terrainInterface.h>
#include <terrainCS/coordSystem.h>
#include <matrix/topoCoord.h>

// This function tries to find the normal vector through the point specified by
// the dynamic terrain information. It was specifically designed for doors. A
// series of intersection checks is performed at different angles through the point.
// Each intersection returns the normal vector for the surface of intersection. If
// this is a door, most of the checks should result in the same normal. If so, this
// normal becomes the nav edge that defines the path through the door.
bool edgeFromIntersectionNormal(const DtRwDynamicTerrainInformation& dynTerrInfo, 
   DtNavDataGenerator* navDataGen, int sectorX, int sectorY, unsigned edgeTag,
   std::list<DtNavDataGenerator::NavEdgePoints>* edges, void* usr)
{
   const Coordinate_System* coordSys = navDataGen->terrainInterface()->coordinateSystem();

   // Find transformation matrices to and from topographic coordinates.
   DtVector topoOrig;
   DtDcm localToTopoDcm;
   DtDcm topoToLocalDcm;
   coordSys->localToTopo(dynTerrInfo.location(), localToTopoDcm);
   coordSys->topoToLocal(dynTerrInfo.location(), topoToLocalDcm);
   DtDcmVecMul(localToTopoDcm, dynTerrInfo.location(), topoOrig);

   // Start our intersection checks at some random angle.
   double nextAngle = DtDeg2Rad(DtIntRandom(0, 360));

   std::vector<DtVector> normals;

   const unsigned numTests = 7;

   // Perform a series of intersection checks and save the normal vectors.
   // The more uneven the surface is at this location (for instance if a door
   // has some complex geometry to it) the more likely we may get undesirable
   // normals. Hopefully with a sufficient number of samples we can find the
   // true normal through the door.
   for (unsigned i = 0; i < numTests; ++i)
   {
      // Find points on each side of the dynamic terrain feature at the test angle.
      DtVector topoPt1(sin(nextAngle) + topoOrig.x(), cos(nextAngle) + topoOrig.y(), topoOrig.z());
      DtVector topoPt2 = topoOrig + (topoOrig - topoPt1);

      // Convert to local coordinates and generate the test chord.
      DtVector locPt1, locPt2;
      DtDcmVecMul(topoToLocalDcm, topoPt1, locPt1);
      DtDcmVecMul(topoToLocalDcm, topoPt2, locPt2);
      DtPoint p1(locPt1);
      DtPoint p2(locPt2);
      DtChord chord(p1, p2);

      // Perform the terrain intersection check. If data is not available, keep
      // trying until it is loaded.
      bool dataAvail = false;
      bool intersects = false;
      DtVrfChordIntersectionRecord record;
      while (!dataAvail)
      {
         intersects = navDataGen->terrainInterface()->intersect(chord, record,
            DtIntersectionRecord::INT_NORMAL, &dataAvail);
      }

      // Found a normal, add it to our list.
      if (intersects && dataAvail)
      {
         normals.push_back(record.normal());
      }

      // Find our next test angle.
      nextAngle += M_2PI / numTests;
      if (nextAngle > M_2PI)
      {
         nextAngle -= M_2PI;
      }
   }

   // Now we have a number of sample normals. Try to determine the best normal
   // that passes through the dynamic terrain feature.

   // First see how many normals match each other
   std::map<DtVector, unsigned> normalCounts;
   for (unsigned i = 0; i < normals.size(); ++i)
   {
      DtVector currNormal = normals[i];

      // Find the "flipped" normal, which is the same chord but pointed in the
      // opposite direction. For our purposes these are equivalent.
      DtVector flippedCurrNormal;
      DtVecScale(currNormal, -1, flippedCurrNormal);

      // Compare this normal to the other distinct normals we have processed.
      bool foundMatch = false;
      double closestMagSq = 9999999;
      bool closestIsFlipped = false;
      std::map<DtVector, unsigned>::iterator ctItr = normalCounts.begin();
      for (; ctItr != normalCounts.end() && !foundMatch; ++ctItr)
      {
         // If normal vectors are very close, consider them the same.
         if (currNormal.approxEq(ctItr->first, 0.01) ||
            flippedCurrNormal.approxEq(ctItr->first, 0.01))
         {
            // Increase the count for this normal, since we have two samples
            // that produced the same normal.
            ++ctItr->second;
            foundMatch = true;
         }
         else
         {
            // Normals do not match. If this is the closest normal we have found
            // so far, note whether it was a closer match for the original or
            // flipped version of the normal.
            double magSq = (ctItr->first - currNormal).magnitudeSquared();
            double flippedMagSq = (ctItr->first - flippedCurrNormal).magnitudeSquared();

            if (magSq < closestMagSq)
            {
               closestMagSq = magSq;
               closestIsFlipped = false;
            }

            if (flippedMagSq < closestMagSq)
            {
               closestMagSq = flippedMagSq;
               closestIsFlipped = true;
            }
         }
      }

      if (!foundMatch)
      {
         // No match was found. Save this as a distinct normal. Save the flipped
         // version if it was closer to another distinct normal.
         if (closestIsFlipped)
         {
            normalCounts.insert(std::make_pair(flippedCurrNormal, (unsigned)1));
         }
         else
         {
            normalCounts.insert(std::make_pair(currNormal, (unsigned)1));
         }
      }
   }

   // Determine the average count received by each normal. Use this to toss out outliers.
   double avgCount = double(normals.size()) / normalCounts.size();

   // Average together the distinct normals to create our nav edge vector.
   DtVector normalSum;
   unsigned normalCountUsed = 0;
   std::map<DtVector, unsigned>::iterator ctItr = normalCounts.begin();
   for (; ctItr != normalCounts.end(); ++ctItr)
   {
      // Ignore outliers so that they do not skew the average.
      if (ctItr->second >= avgCount)
      {
         DtVector tempSum;
         DtVecScale(ctItr->first, ctItr->second, tempSum);
         normalCountUsed += ctItr->second;
         normalSum += tempSum;
      }
   }

   DtVector edgeVec;
   if (!normalSum.magnitudeIsZero())
   {
      DtVecScale(normalSum, 1.0 / normalCountUsed, edgeVec);
   }

   if (!edgeVec.magnitudeIsZero())
   {
      // Convert the edge vector to topographic coordinates and add
      // the new nav edge to the list.
      edgeVec.normalize();

      DtVector edgePoint1 = dynTerrInfo.location() + edgeVec;
      DtVector edgePoint2;

      DtVector topoEdgePoint1, topoEdgePoint2;
      DtDcmVecMul(localToTopoDcm, edgePoint1, topoEdgePoint1);
      DtVector topoEdgeVec = topoEdgePoint1 - topoOrig;
      topoEdgeVec.setZ(0);
      topoEdgeVec.normalize();
      topoEdgePoint1 = topoOrig + topoEdgeVec;
      topoEdgePoint2 = topoOrig - topoEdgeVec;

      DtDcmVecMul(topoToLocalDcm, topoEdgePoint1, edgePoint1);
      DtDcmVecMul(topoToLocalDcm, topoEdgePoint2, edgePoint2);

      // Add the nav edge.
      edges->push_back(DtNavDataGenerator::NavEdgePoints(edgePoint1, edgePoint2, edgeTag));

      // Successfully created a nav edge.
      return true;
   }

   // No nav edge was created.
   return false;
}

// Turns a path feature into nav edge(s), clipped to the current sector.
bool edgeFromPathFeature(const MAKVRinTerra::DtFeature& feature, DtNavDataGenerator* navDataGen,
   int sectorX, int sectorY, unsigned edgeTag, std::list<DtNavDataGenerator::NavEdgePoints>* edges, void* usr)
{
   // Get the geometry of the feature.
   MAKVRinTerra::DtFeatureGeometry featureGeom = feature.geometry();
   MAKVRinTerra::DtFeatureGeometry clippedGeom;

   bool edgeAdded = false;

   if (featureGeom.isValid() && featureGeom.isPath())
   {
      // Clip geometry of this feature to the current nav sector.
      featureGeom.convert(*navDataGen->terrainInterface()->localProj());
      clippedGeom = featureGeom.intersectionWith(navDataGen->extentFeatureGeometry(sectorX, sectorY));

      // Translate to geometry to local coordinates.
      clippedGeom.convert(*navDataGen->terrainInterface()->localProj());

      std::auto_ptr<MAKVRinTerra::DtFeatureGeometry::PointVector> points =
         clippedGeom.pointsWithHeight();

      // Ground clamp each point.
      std::vector<DtVector> clampedPoints(points->size(), DtVector());
      for (unsigned ptIdx = 0; ptIdx < points->size(); ++ptIdx)
      {
         DtVector pt((*points)[ptIdx].x(), (*points)[ptIdx].y(), (*points)[ptIdx].z());

         bool dataAvailable = false;
         bool intersectionFound = false;
         DtPoint intersectionPoint;

         while (!dataAvailable)
         {
            // Find ground intersection to clamp point.
            intersectionFound = navDataGen->terrainInterface()->closestIntersection(pt,
               intersectionPoint, &dataAvailable);
         }

         if (intersectionFound)
         {
            // Save the ground clamped point
            clampedPoints[ptIdx] =
               DtVector(intersectionPoint.x(), intersectionPoint.y(), intersectionPoint.z());
         }
         else
         {
            // No intersection, cannot ground clamp. Just use what we have.
            clampedPoints[ptIdx] = pt;
         }
      }

      // Traverse the set of points in this path feature, connecting each point
      // with a new nav edge.
      unsigned idx1 = 0;
      unsigned idx2 = 1;
      while (idx2 < clampedPoints.size())
      {
         // Add the nav edge between these points.
         edges->push_back(DtNavDataGenerator::NavEdgePoints(
            clampedPoints[idx1], clampedPoints[idx2], edgeTag));
         ++idx1;
         ++idx2;
         edgeAdded = true;
      }
   }

   return edgeAdded;
}