kinematicTools.h

Go to the documentation of this file.

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

#pragma once


#include <math.h>
#include <errno.h>
#include <matrix/LibMatrix.h>
#include <vl/topoView.h>
#include <matrix/vlTaitBryan.h>
#include <matrix/3dBoundingVolume.h>

#include "tdbutil/mathUtilities.h"

#include <boost/math/special_functions/fpclassify.hpp>

#define DtSIGN(x)  (((x) > (0.0)) ? (1.0) : (-1.0))

#define DtALMOST(x, y, e) (((x)<=((y)+(e))) && ((x)>=((y)-(e))))

#define DtGRAVITY_CONSTANT (9.81)
#define DtPI_OVER_TWO (1.570796327)
#define DtTWO_PI (6.283185307)
#define DtMETERS_TO_FEET (3.2808)
#define DtMETERS_PER_SEC_TO_MPH (2.23)
#define DtLN2 (0.693147181)

#define SIN(x) DtSin(x)
#define COS(x) DtCos(x)
#define TAN(x) DtTan(x)

#define ASIN(x) DtAsin(x)
#define ACOS(x) DtAcos(x)
#define ATAN(x) DtAtan(x)
#define ATAN2(y, x) DtAtan2((y), (x))

#define DtFRand() ((double) rand() / RAND_MAX)
#define DtIRandom(from, to) DtIntRandom(from, to)

class DtGeodeticCoord;
class Coordinate_System;
class DtChord;
class DtLocalVector;
class DtLocalVertexList;
class DtListItem;
class DtLocalVectorIterator;
class DtPoint;

#include "vrfutil/vrfutilDefines.h"

DT_DLL_vrfutil double gRand();

DT_DLL_vrfutil double centralLimitGaussian(int numDraws);

DT_DLL_vrfutil double gaussianRandom();

DT_DLL_vrfutil void Dt3DKinematicsTick(const DtVector& Position, 
                        const DtVector& LinearVelocity,
                        const DtVector& LinearAcceleration,
                        DtConstDcm orientation,
                        const DtVector& angVelocity,
                        const DtVector& angAcceleration,
                        double DeltaTime,
                        DtVectorRef newPosition,
                        DtVectorRef newVelocity,
                        DtDcm& newOrientation,
                        DtVectorRef newAngVelocity);

DT_DLL_vrfutil void Dt3DDynamicsTick(const DtVector& LinearForce,
                      double vehicleMass,
                      DtVectorRef newLinearAcceleration,
                      const DtVector& torque,
                      DtConstDcm vehicleInertia,
                      DtVectorRef newAngAcceleration);

DT_DLL_vrfutil void DtGravityForce(double mass, 
                    DtVectorRef returnForce);

DT_DLL_vrfutil void DtResolve(const DtVector& forcePos,    
               const DtVector& force,
               DtVectorRef resultTorqueAboutOrigin);

DT_DLL_vrfutil void DtParallelAxisTheorem(DtConstDcm inertiaInOriginalFrame,  
                           double mass,
                           const DtVector& newToOriginal,        
                           DtDcm& inertiaInNewFrame);      

DT_DLL_vrfutil void DtRotateInertia(DtDcm inertiaInOriginalFrame,  
                     DtDcm originalToNew,          
                     DtDcm& inertiaInNewFrame);     

DT_DLL_vrfutil double DtPIDServo(double error,
                  double errorIntegral,
                  double velocity,
                  double proportionalGain,
                  double integralGain,
                  double derivativeGain,
                  double biasForce = 0.0,
                  double* control = 0);

DT_DLL_vrfutil void DtBlockInertia(const double xLen, 
          const double yLen, 
          const double zLen, 
          const double mass, 
          DtDcm& inertia);

DT_DLL_vrfutil bool DtVectorIsZero(const DtVector& v,
                   double tolerance = 0.0);

DT_DLL_vrfutil void DtDcmFromColumns(DtDcm& result,
            const DtVector& col1,
            const DtVector& col2,
            const DtVector& col3);

DT_DLL_vrfutil void DtDcmFromRows(DtDcm& result,
         const DtVector& row1,
         const DtVector& row2,
         const DtVector& row3);



DT_DLL_vrfutil bool DtPointInCone(const DtVector& coneEndPosition,
         const DtVector& coneAxis,
         const double coneAngle,
         const DtVector& testPoint,
         double* testAnglePtr = 0);


DT_DLL_vrfutil bool DtPointInWedge(const DtVector& wedgeEndPosition,
                  const DtVector& wedgeAxis,
                  const double wedgeAngle,
                  const DtVector& testPoint);

DT_DLL_vrfutil DtFloat64 DtFastDist(const DtVector& pt1, const DtVector& pt2);

DT_DLL_vrfutil void DtVecLimit(DtVectorRef vec, 
      double limit);

inline double DtVecMag(const DtVector& vec)
{
    return sqrt(vec[DtX]*vec[DtX] + vec[DtY]*vec[DtY] + vec[DtZ]*vec[DtZ]);
};

inline double DtVecMagSquared(const DtVector& vec)
{
    return vec[DtX]*vec[DtX] + vec[DtY]*vec[DtY] + vec[DtZ]*vec[DtZ];
};

DT_DLL_vrfutil void DtRotX(double angle, DtDcm& rotMat);
DT_DLL_vrfutil void DtRotY(double angle, DtDcm& rotMat);
DT_DLL_vrfutil void DtRotZ(double angle, DtDcm& rotMat);
DT_DLL_vrfutil void DtBodyToLocalRef_to_Euler(DtConstDcm body2ref, DtTaitBryan *e);
DT_DLL_vrfutil void DtEuler_to_BodyToLocalRef(const DtTaitBryan &euler, DtDcm& body2ref);
DT_DLL_vrfutil const DtDcm & enu2ned();
DT_DLL_vrfutil const DtDcm & ned2enu();

DT_DLL_vrfutil void DtRotX2(double sinAng, double cosAng, DtDcm& rotMat);
DT_DLL_vrfutil void DtRotY2(double sinAng, double cosAng, DtDcm& rotMat);
DT_DLL_vrfutil void DtRotZ2(double sinAng, double cosAng, DtDcm& rotMat);

DT_DLL_vrfutil void DtRotBodyX(double angle, DtDcm& rotMat);
DT_DLL_vrfutil void DtRotBodyY(double angle, DtDcm& rotMat);
DT_DLL_vrfutil void DtRotBodyZ(double angle, DtDcm& rotMat);

class DT_DLL_vrfutil DtPoleFilterType
{
  public:
    DtPoleFilterType(double freq);
    void setPole(double freq);
    void output(DtTaitBryan* input,
      double dT,
      DtTaitBryan* output);
   void output(const DtVector& input,
      double dT,
      DtVectorRef output);
    double output(double input,
        double dT);

   double getPole() { return poleFreq; }
   const DtVector& getPrevOutput() { return prevOutput; }

  private:
   double poleFreq;
   DtVector prevOutput;
};
class DT_DLL_vrfutil DtNonLinearOutputType
{
  public:
    DtNonLinearOutputType(double nonlinearity,
           double gain = 1.0)
    {
   DtGain = gain;
   DtExponent = nonlinearity+1.0;
    }
           
    void DtSetGain(double gain) { DtGain = gain; }
    void DtSetNonLinearity(double nonlinearity) { DtExponent = nonlinearity+1.0; }

    double DtGetGain() { return DtGain; }
    double DtGetNonLinearity() { return DtExponent-1.0; }
    double DtOutput(double input)
    {
   return DtSIGN(input) * DtGain * pow(fabs(input), DtExponent);
    }

  private:
    double DtGain;
    double DtExponent;

};
   
DT_DLL_vrfutil void DtCircleVerts(double radius,
         int npts,
         double* xCoords,
         double* yCoords);



DT_DLL_vrfutil void DtSpacesToDashes(char* str);


DT_DLL_vrfutil char* DtGetFilenameExtension(char* fileName, char* extension);


// y is left
// z is up   
// Y is forward
// z is up              

DT_DLL_vrfutil void DtOrientationFromThreePoints(const DtVector& leftPoint,
                          const DtVector& rightPoint,
                          const DtVector& rearPoint,
                          DtDcm& orientation,
                          bool yIsForward=false);

DT_DLL_vrfutil void DtOrientationFromThreePointsAndHeading(const DtVector& leftPoint,
                          const DtVector& rightPoint,
                          const DtVector& otherPoint,
                          double cosHeading,
                          double sinHeading,
                          DtDcm& orientation,
                          bool upsideDown = false);

DT_DLL_vrfutil void DtOrientationFromThreePointsAndHeading(const DtVector& leftPoint,
   const DtVector& rightPoint, const DtVector& otherPoint, double heading, 
   const DtDcm& topoToLocal, DtDcm& orientation);


DT_DLL_vrfutil bool DtCalculateNormal(const DtVector& pointA, 
   const DtVector& pointB, const DtVector& pointC, DtVector& normal);


DT_DLL_vrfutil void DtCalculateOrientation(DtVector localNormal, double heading, 
   const DtDcm& topoToLocal, DtDcm& localOrientation);


DT_DLL_vrfutil double DtFindZInPlane(const double posx, const double posy, 
                 const DtVector& pointInPlane, const DtVector& normal);

DT_DLL_vrfutil DtVector DtIntersectLineWithPlane(const DtVector& point, const DtVector& direction, 
   const DtVector& pointInPlane, const DtVector& planeNormal);

DT_DLL_vrfutil void DtBodyToRefFromHeadingAndPosition(const Coordinate_System& coordSystem,
   double heading, const DtVector& position, DtDcm& bodyToRef);

DT_DLL_vrfutil void DtTopoEulerToBodyToLocal(const DtTaitBryan& topoEuler, 
   const Coordinate_System& coordSystem, const DtVector& localPosition, 
   DtDcm& bodyToLocalDcm);

DT_DLL_vrfutil DtDcm DtRotZToBodyToLocal(const Coordinate_System& coordSystem, double angle, 
   const DtVector& position);
DT_DLL_vrfutil DtDcm DtRotYToBodyToLocal(const Coordinate_System& coordSystem, double angle, 
   const DtVector& position);
DT_DLL_vrfutil DtDcm DtRotXToBodyToLocal(const Coordinate_System& coordSystem, double angle, 
   const DtVector& position);

DT_DLL_vrfutil double DtHeadingFromGeocAToGeocB(const DtVector& geocA, const DtVector& geocB);



DT_DLL_vrfutil void DtTrigInit();

DT_DLL_vrfutil double DtSin(double r);

inline double DtCos(double r)
{
   return DtSin(r + DtPI_OVER_TWO);
}

inline double DtTan(double r)
{
   double cosR = DtCos(r);

   if (DtIsZero<double>(cosR))
   {
      errno = EDOM;
      return 0.;
   }

   else
   {
      return DtSin(r) / cosR;
   }
}

DT_DLL_vrfutil double DtAsin(double x);

inline double DtAcos(double x)
{
   return DtPI_OVER_TWO + DtAsin(-x);
}

DT_DLL_vrfutil double DtAtan(double x);
DT_DLL_vrfutil double DtAtan2(double y, double x);

DT_DLL_vrfutil bool DtSameSign(double x, double y);

DT_DLL_vrfutil bool DtPointInSegment(const DtVector& testPoint, const DtVector& segStart, 
   const DtVector& segEnd);

/*******************************************************************************
 *******************************************************************************/

DT_DLL_vrfutil int DtIntersect2D_Segments( DtVector P0, DtVector P1, DtVector Q0, 
   DtVector Q1, DtVector* I0 = 0, DtVector* I1 = 0, 
   double* isectParam1 = 0, double *isectParam2 = 0, 
   bool* parallel = 0, bool* collinear = 0);

DT_DLL_vrfutil bool DtInSegment( DtVector P, DtVector P0, DtVector P1);


DT_DLL_vrfutil DtVector localGravityAcceleration(const DtVector& localPosition, 
                                  const Coordinate_System& coordinateSystem);


DT_DLL_vrfutil double altitudeOfPosition(const DtVector& localPosition,
                          const Coordinate_System& coordinateSystem);
DT_DLL_vrfutil double altitudeOfPosition(const DtPoint& localPosition,
                                         const Coordinate_System& coordinateSystem);


DT_DLL_vrfutil void setAltitudeOfPosition(DtVector& localPosition, double newAltitude,
                                         const Coordinate_System& coordinateSystem);
DT_DLL_vrfutil void setAltitudeOfPosition(DtPoint& localPosition, double newAltitude,
                                          const Coordinate_System& coordinateSystem);



DT_DLL_vrfutil double headingAngleTo(const DtVector& observerPosition, 
   const DtVector& targetPosition, const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double elevationAngleTo(const DtVector& observerPosition, 
   const DtVector& targetPosition, const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double bankAngleTo(const DtVector& observerPosition, 
   const DtVector& targetPosition, const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double distSquaredTo2D(const DtVector& position1, const DtVector& position2,
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil bool inside(const DtVector& localPoint, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil bool leftOf(const DtVector& localPoint, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double headingAngleTo(const DtVector& localPoint, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double elevationAngleTo(const DtVector& localPoint, 
  const DtLocalVertexList& localVertexList, 
  const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double localDistSquaredTo(const DtVector& localPoint, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double distSquaredTo2D(const DtVector& localPoint, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double distToBoundingVolume(const DtVector& localBaseLocation,
   const Dt3dBoundingVolume* box, const DtTaitBryan& boxToLocalOrientation,
   const DtVector& testPoint);

DT_DLL_vrfutil bool intersects(const DtVector& localPoint1, 
   const DtVector& localPoint2, const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil DtListItem* getClosestVertex2D(const DtVector& localPoint, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil bool getClosestLine2D(const DtVector& localPoint, 
   DtVector& localLinePt1, DtVector& localLinePt2, 
   const DtLocalVertexList& localVertexList, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double distanceSquaredPointToLine2D(const DtVector& localPoint,
   const DtVector& localLinePt1, const DtVector& localLinePt2, 
   const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double distanceSquaredPointToPoint2D(const DtVector& localLinePt1,
   const DtVector& localLinePt2, const Coordinate_System& coordinateSystem);

DT_DLL_vrfutil double closestPointAlongChord2D( const DtPoint& chordStart,
                                               const DtPoint& chordEnd, 
                                               const DtPoint& testPoint, DtPoint & closestPoint,
                                               const Coordinate_System& coordSys);

DT_DLL_vrfutil bool setCurrentVertexToClosestInDirectionOfTraversal(
   const DtVector& point, const Coordinate_System& coordinateSystem, 
   DtLocalVectorIterator& localVertexIterator);


inline double dotProd2D(const DtVector& v1, const DtVector& v2)
{
   return v1[0]*v2[0] + v1[1]*v2[1];
}

inline double magnitudeSquared2D(const DtVector &v)
{
   return dotProd2D(v, v);
}

inline double crossProduct2D(const DtVector& v1, const DtVector& v2)
{
   return v1[0]*v2[1] - v1[1]*v2[0];
}

inline DtVector rotateRight2D(const DtVector& v)
{
   DtVector res(v[1], -v[0], v[2]);
   return res;
}

inline void normalize2D(DtVector& v)
{
   double length = sqrt(magnitudeSquared2D(v));
   if (length > 0.0)
   {
      v[0] /= length;
      v[1] /= length;
   }
}

inline double vectorAngle2D(DtVector& v)
{
   return DtModPerLo(atan2(v[1], v[0]), -M_PI, M_PI);
}

DT_DLL_vrfutil void localVectorToGeocentricVector(const DtVector &geocentricPosition,
   const DtVector& localVector, const Coordinate_System& coordinateSystem,
   DtVector& geocentricVector);

DT_DLL_vrfutil void localOrientationToGeocentricOrientation(const DtVector geocentricPosition, 
   const DtTaitBryan& localOrientation, 
   const Coordinate_System& coordinateSystem, 
   DtTaitBryan& geocentricOrientation);

DT_DLL_vrfutil void geocentricVectorToLocalVector(const DtVector &geocentricPosition, 
   const DtVector& geocentricVector, const Coordinate_System& coordinateSystem, 
   DtVector& localVector);

DT_DLL_vrfutil void geocentricOrientationToLocalOrientation(
   const DtVector& geocentricPosition,
   const DtTaitBryan& geocentricOrientation, 
   const Coordinate_System& coordinateSystem,  
   DtTaitBryan& localOrientation);



DT_DLL_vrfutil void localVectorToGeocentricVector(const DtVector& geocPosition,
   const DtVector& local, DtVector& geoc);

DT_DLL_vrfutil void localOrientationToGeocentricOrientation(const DtVector 
   &geocPosition, const DtTaitBryan& local, DtTaitBryan& geoc);

DT_DLL_vrfutil void localVelocityToGeocentricVelocity(DtTopoView& topoView,
   const DtVector& local);

DT_DLL_vrfutil void localAccelerationToGeocentricAcceleration(DtTopoView& topoView, 
   const DtVector& local);

DT_DLL_vrfutil void localOrientationToGeocentricOrientation(DtTopoView& topoView, 
   const DtTaitBryan& local);

DT_DLL_vrfutil void geocentricVectorToLocalVector(const DtVector &geocPosition, 
   const DtVector& geoc, DtVector& local);

DT_DLL_vrfutil void geocentricOrientationToLocalOrientation(const DtVector 
   &geocPosition, const DtTaitBryan& geoc, DtTaitBryan& local);

DT_DLL_vrfutil void geocentricVelocityToLocalVelocity(DtTopoView &topoView, 
   DtVector& local);

DT_DLL_vrfutil void geocentricAccelerationToLocalAcceleration(DtTopoView& topoView,
   DtVector& local);
   
DT_DLL_vrfutil void geocentricOrientationToLocalOrientation(DtTopoView& topoView, 
   DtTaitBryan& local);

DT_DLL_vrfutil int DtIntRandom(int iFrom, int iTo);

DT_DLL_vrfutil bool DtIsValidNumber(double x);

DT_DLL_vrfutil double DtHoursMinutesSeconds2Seconds(double hours, double minutes, double seconds);

DT_DLL_vrfutil void DtSeconds2HoursMinutesSeconds(double totalSeconds, int& hoursOut, 
                                                int& minutesOut, double& secondsOut);

DT_DLL_vrfutil double calculateLengthOfLine(const std::vector<DtVector>& points);

DT_DLL_vrfutil double calculateLengthOfLine(const Coordinate_System& coordinateSystem, const std::vector<DtVector>& points);

DT_DLL_vrfutil void calculateLengthAndWidthOfShape(const Coordinate_System& coordSytem, const std::vector<DtVector>& points,
   double& length, double& width);

DT_DLL_vrfutil void calculateLengthAndWidthOfShape(const std::vector<DtVector>& points,
   double& length, double& width);

DT_DLL_vrfutil DtVector DtPoint2DtVector(const DtPoint& p);

DT_DLL_vrfutil double illuminationValueAtPosition(const DtGeodeticCoord& position, time_t time,
   double nighttimeMaximum = 0.1, double illuminationMinimum = 0.001, bool useDayNightIlluminationModel = true,
   int* sunstate = 0);

inline bool checkForNAN(const DtVector& vector)
{
   if (boost::math::isnan(vector.x()) ||
      boost::math::isnan(vector.y()) ||
      boost::math::isnan(vector.z()))
   {
      return true;
   }
   else
   {
      return false;
   }
}

inline bool checkForNAN(const DtTaitBryan& tb)
{
   if (boost::math::isnan(tb.phi()) ||
      boost::math::isnan(tb.psi()) ||
      boost::math::isnan(tb.theta()))
   {
      return true;
   }
   else
   {
      return false;
   }
}

inline bool checkForNAN(const DtDcm& dcm)
{
   if (boost::math::isnan(dcm[0].x()) ||
      boost::math::isnan(dcm[0].y()) ||
      boost::math::isnan(dcm[0].z()) ||
      boost::math::isnan(dcm[1].x()) ||
      boost::math::isnan(dcm[1].y()) ||
      boost::math::isnan(dcm[1].z()) ||
      boost::math::isnan(dcm[2].x()) ||
      boost::math::isnan(dcm[2].y()) ||
      boost::math::isnan(dcm[2].z()))
   {
      return true;
   }
   else
   {
      return false;
   }
}


#include <boost/algorithm/string.hpp>
template< typename SequenceSequenceT, typename RangeT, typename PredicateT >
inline SequenceSequenceT& oldBoostSplit(
   SequenceSequenceT& Result,
   RangeT& Input,
   PredicateT Pred,
   boost::algorithm::token_compress_mode_type eCompress=boost::algorithm::token_compress_off )
{
   if (Input.length())
   {
      ::boost::algorithm::iter_split(
            Result,
            Input,
            ::boost::algorithm::token_finder( Pred, eCompress ) );
   }

   return Result;
}