KDIS/KDIS/Extras/DeadReckoningCalculator.cpp

   1 /*********************************************************************
   2 Copyright 2013 Karl Jones
   3 All rights reserved.
   4
   5 Redistribution and use in source and binary forms, with or without
   6 modification, are permitted provided that the following conditions are met:
   7
   8 1. Redistributions of source code must retain the above copyright notice, this
   9    list of conditions and the following disclaimer.
  10 2. Redistributions in binary form must reproduce the above copyright notice,
  11    this list of conditions and the following disclaimer in the documentation
  12    and/or other materials provided with the distribution.
  13
  14 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  15 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  16 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  17 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
  18 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  19 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  20 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  21 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  22 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  23 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  24
  25 For Further Information Please Contact me at
  26 Karljj1@yahoo.com
  27 http://p.sf.net/kdis/UserGuide
  28 *********************************************************************/
  29
  30 #include "./DeadReckoningCalculator.h"
  31 #include <cmath>
  32 #include <limits>
  33
  34 #ifndef KDIS_PI
  35     #define KDIS_PI 3.14159265358979323846
  36 #endif
  37
  38 #ifndef KDIS_PI_2
  39     #define KDIS_PI_2 1.57079632679489661923
  40 #endif
  41
  42 //////////////////////////////////////////////////////////////////////////
  43
  44 using namespace std;
  45 using namespace KDIS;
  46 using namespace DATA_TYPE;
  47 using namespace ENUMS;
  48 using namespace UTILS;
  49
  50 #define MAGIC_EPSILON 0.00001
  51 const float PI_TIMES_TWO = 2.0f * KDIS_PI;
  52
  53 //////////////////////////////////////////////////////////////////////////
  54
  55 template <typename T>
  56 inline KBOOL equivalent( const T lhs, const T rhs, T epsilon = std::numeric_limits<T>::epsilon() )
  57 {
  58     T delta = rhs - lhs;
  59     return ( delta < 0 ) ? delta > -epsilon : delta < epsilon;
  60 }
  61
  62 /////////////////////////////////////////////////////////////////////////
  63
  64 inline KFLOAT32 checkAngleHR( const KFLOAT32 a )
  65 {
  66     if (a < -KDIS_PI) return (a + PI_TIMES_TWO);
  67     else if (a > KDIS_PI) return (a - PI_TIMES_TWO);
  68     return a;
  69 }
  70
  71 //////////////////////////////////////////////////////////////////////////
  72
  73 inline KFLOAT32 checkAngleP( const KFLOAT32 a )
  74 {
  75     if (a < -KDIS_PI_2)  return (-KDIS_PI-a);
  76     else if (a > KDIS_PI_2) return (KDIS_PI-a);
  77     return a;
  78 }
  79
  80 /////////////////////////////////////////////////////////////////////////
  81
  82 template <typename T>
  83 inline const T clampVal( const T val, T CutOff )
  84 {
  85     if( CutOff < 0 )CutOff = -CutOff;
  86
  87     if( val > CutOff )return CutOff;
  88     else if( val < -CutOff )return -CutOff;
  89
  90     return val;
  91 }
  92
  93 //////////////////////////////////////////////////////////////////////////
  94
  95 void matrixToHPR_new( const TMATRIX& rotation, EulerAngles & hpr )
  96 {
  97     static const float PRECISION = MAGIC_EPSILON;
  98     KFLOAT64 sin_pitch = clampVal( -rotation.Data[0][2], 1.0f ); // -mat(0,2)
  99     KFLOAT64 pitch = asin( sin_pitch );
 100     hpr.SetThetaInRadians( pitch );
 101
 102     KFLOAT64 cp = cos( pitch );
 103     if (equivalent<double>(cp, 0, PRECISION))
 104     {
 105         KFLOAT64 cr = clampVal(rotation.Data[1][1],1.0f);
 106         KFLOAT64 sr = clampVal(-rotation.Data[2][1],1.0f);
 107         hpr.SetPsiInRadians( 0.0f );
 108         hpr.SetPhiInRadians( atan2( sr,cr ) );
 109     } else {
 110         KFLOAT64 sr = clampVal(rotation.Data[1][2],1.0f);
 111         KFLOAT64 cr = clampVal(rotation.Data[2][2],1.0f);
 112         float sgnr = (sr < 0) ? -1.0f : 1.0f;
 113
 114         KFLOAT64 sh = clampVal(rotation.Data[0][1],1.0f);
 115         KFLOAT64 ch = clampVal(rotation.Data[0][0],1.0f);
 116         float sgnh = (sh < 0) ? -1.0f : 1.0f;
 117
 118         //hpr.SetPsiInRadians( acos ( ch / cp )*sgnh );
 119         //hpr.SetPhiInRadians( acos( cr / cp)*sgnr);
 120         hpr.SetPsiInRadians( atan2 (sh,ch ) );
 121         hpr.SetPhiInRadians( atan2 (sr,cr ) );
 122     }
 123 }
 124
 125 //////////////////////////////////////////////////////////////////////////
 126 // protected:
 127 //////////////////////////////////////////////////////////////////////////
 128
 129 void DeadReckoningCalculator::positionReset( const WorldCoordinates & Position )
 130 {
 131     m_initPosition = Position;
 132 }
 133
 134 //////////////////////////////////////////////////////////////////////////
 135
 136 void DeadReckoningCalculator::orientationReset( const EulerAngles & Orientation )
 137 {
 138     m_initOrientation = Orientation;
 139
 140     const KFLOAT32 f32Psi = Orientation.GetPsiInRadians();
 141     const KFLOAT32 f32Theta = Orientation.GetThetaInRadians();
 142     const KFLOAT32 f32Phi = Orientation.GetPhiInRadians();
 143
 144     const KFLOAT64 f32CosPsi = cos( f32Psi );
 145     const KFLOAT64 f32CosTheta = cos( f32Theta );
 146     const KFLOAT64 f32CosPhi = cos( f32Phi );
 147     const KFLOAT64 f32SinPsi = sin( f32Psi );
 148     const KFLOAT64 f32SinTheta = sin( f32Theta );
 149     const KFLOAT64 f32SinPhi = sin( f32Phi );
 150
 151     // Create the orientation matrix
 152
 153     // RI - could use osg mathematics for Euler to rotation matrix computation
 154     const KFLOAT64 sinTheta_cosPsi = f32SinTheta * f32CosPsi;
 155     const KFLOAT64 sinTheta_sinPsi = f32SinTheta * f32SinPsi;
 156
 157     m_initOrientationMatrix.Data[0][0] = f32CosTheta * f32CosPsi;
 158     m_initOrientationMatrix.Data[0][1] = f32CosTheta * f32SinPsi;
 159     m_initOrientationMatrix.Data[0][2] = -f32SinTheta;
 160
 161     m_initOrientationMatrix.Data[1][0] = ( f32SinPhi * sinTheta_cosPsi ) - ( f32CosPhi * f32SinPsi );
 162     m_initOrientationMatrix.Data[1][1] = ( f32SinPhi * sinTheta_sinPsi ) + ( f32CosPhi * f32CosPsi );
 163     m_initOrientationMatrix.Data[1][2] = f32SinPhi * f32CosTheta;
 164
 165     m_initOrientationMatrix.Data[2][0] = ( f32CosPhi * sinTheta_cosPsi ) + ( f32SinPhi * f32SinPsi );
 166     m_initOrientationMatrix.Data[2][1] = ( f32CosPhi * sinTheta_sinPsi ) - ( f32SinPhi * f32CosPsi );
 167     m_initOrientationMatrix.Data[2][2] = f32CosPhi * f32CosTheta;
 168
 169     m_initOrientationMatrixTranspose = m_initOrientationMatrix;
 170     m_initOrientationMatrixTranspose.inPlanceTranspose();
 171
 172     computeRotationAxis(m_initOrientationMatrix);
 173 }
 174
 175 //////////////////////////////////////////////////////////////////////////
 176
 177 void DeadReckoningCalculator::angularVelocityReset( const Vector & AngularVelocity )
 178 {
 179     m_initAngularVelocity = AngularVelocity;
 180
 181     m_initEulerAngularVelocity = toEulerAngularVelocity(AngularVelocity, m_initOrientation);
 182
 183     // Create the ww matrix
 184     m_wwMatrix.Data[0][0] = m_initAngularVelocity[0] * m_initAngularVelocity[0];
 185     m_wwMatrix.Data[0][1] = m_initAngularVelocity[0] * m_initAngularVelocity[1];
 186     m_wwMatrix.Data[0][2] = m_initAngularVelocity[0] * m_initAngularVelocity[2];
 187
 188     m_wwMatrix.Data[1][0] = m_initAngularVelocity[1] * m_initAngularVelocity[0];
 189     m_wwMatrix.Data[1][1] = m_initAngularVelocity[1] * m_initAngularVelocity[1];
 190     m_wwMatrix.Data[1][2] = m_initAngularVelocity[1] * m_initAngularVelocity[2];
 191
 192     m_wwMatrix.Data[2][0] = m_initAngularVelocity[2] * m_initAngularVelocity[0];
 193     m_wwMatrix.Data[2][1] = m_initAngularVelocity[2] * m_initAngularVelocity[1];
 194     m_wwMatrix.Data[2][2] = m_initAngularVelocity[2] * m_initAngularVelocity[2];
 195
 196     m_SkewOmegaMatrix.Data[0][0] = 0;
 197     m_SkewOmegaMatrix.Data[0][1] = - m_initAngularVelocity[2];
 198     m_SkewOmegaMatrix.Data[0][2] = m_initAngularVelocity[1];
 199
 200     m_SkewOmegaMatrix.Data[1][0] = m_initAngularVelocity[2];
 201     m_SkewOmegaMatrix.Data[1][1] = 0;
 202     m_SkewOmegaMatrix.Data[1][2] = - m_initAngularVelocity[0];
 203
 204     m_SkewOmegaMatrix.Data[2][0] = - m_initAngularVelocity[1];
 205     m_SkewOmegaMatrix.Data[2][1] = m_initAngularVelocity[0];
 206     m_SkewOmegaMatrix.Data[2][2] = 0;
 207
 208     m_f64Magnitude = sqrt( m_initAngularVelocity[0] * m_initAngularVelocity[0] +
 209                            m_initAngularVelocity[1] * m_initAngularVelocity[1] +
 210                            m_initAngularVelocity[2] * m_initAngularVelocity[2] );
 211 }
 212
 213 //////////////////////////////////////////////////////////////////////////
 214
 215 void DeadReckoningCalculator::velocityReset( const Vector & LinearVelocity )
 216 {
 217     m_initLinearVelocity = LinearVelocity;
 218 }
 219
 220 //////////////////////////////////////////////////////////////////////////
 221
 222 void DeadReckoningCalculator::accelerationReset( const Vector & LinearAcceleration )
 223 {
 224     m_initLinearAcceleration = LinearAcceleration;
 225 }
 226
 227 //////////////////////////////////////////////////////////////////////////
 228
 229 EulerAngles DeadReckoningCalculator::toEulerAngularVelocity( const Vector & angVel, const EulerAngles & Orientation )
 230 {
 231     KFLOAT32 dpsi, dtheta, dphi;
 232
 233     const KFLOAT32 phi = Orientation.GetPhiInRadians();
 234     const KFLOAT32 theta = Orientation.GetThetaInRadians();
 235     const KFLOAT32 psi = Orientation.GetPsiInRadians();
 236
 237     KFLOAT32 cos_phi = cos( phi );
 238     KFLOAT32 sin_phi = sin( phi );
 239
 240     dtheta = angVel[1] * cos_phi - angVel[2] * sin_phi;
 241     dpsi  = ( angVel[1] * sin_phi + angVel[2] * cos_phi ) / cos( theta ); //could be unstable when cos(theta) is close to 0
 242     dphi = angVel[0] + dpsi * sin( theta );
 243
 244     return EulerAngles( ( const KFLOAT32 )dpsi, ( const KFLOAT32 )dtheta, ( const KFLOAT32 )dphi );
 245 }
 246
 247
 248 void DeadReckoningCalculator::calcOrientation_simplified( EulerAngles & OrientationOut, const KFLOAT32 totalTimeSinceReset )
 249 {
 250     /* Orientation */
 251     // A simpler way from the IEEE method:
 252     /*
 253         m_curOrientation += ( toEulerAngularVelocity( m_initAngularVelocity, m_curOrientation ) * DeltaTime );
 254     */
 255     OrientationOut = m_initOrientation + m_initEulerAngularVelocity * totalTimeSinceReset;
 256     // make sure the Pitch is between -90 and +90 degrees
 257     // also handle the discontinuity when Pitch is very close to +-90
 258     if( equivalent<KFLOAT32>( OrientationOut[1], KDIS_PI_2, MAGIC_EPSILON * 100 ) ) {
 259         OrientationOut[0] = KDIS_PI - OrientationOut[0]; //reflecting the heading angle, to avoid discontinuity
 260         if (OrientationOut[1] < 0)
 261             OrientationOut[1] =  (-KDIS_PI_2-OrientationOut[1]);
 262         else if (OrientationOut[1] > 0)
 263             OrientationOut[1] = (KDIS_PI_2-OrientationOut[1]);
 264     }
 265
 266     // Make sure the value is within -180 and +180 degrees for Heading and Roll.
 267     OrientationOut[0] = checkAngleHR( OrientationOut[0]);
 268     OrientationOut[2] = checkAngleHR( OrientationOut[2]);
 269 }
 270
 271 /////////////////////////////////////////////////////////////////////////
 272
 273 void DeadReckoningCalculator::computeRotationAxis( const TMATRIX& curOrientationMatrix )
 274 {
 275     if (!m_bQuaxAxisSet)
 276     {
 277         KFLOAT32 x,y,z;
 278         x = curOrientationMatrix.Data[1][2] - curOrientationMatrix.Data[2][1];
 279         y = curOrientationMatrix.Data[2][0] - curOrientationMatrix.Data[0][2];
 280         z = curOrientationMatrix.Data[0][1] - curOrientationMatrix.Data[1][0];
 281
 282         // normalise
 283         KFLOAT32 len = sqrt(x * x + y * y + z * z);
 284
 285         if (len != 0)
 286         {
 287             x /= len;
 288             y /= len;
 289             z /= len;
 290         }
 291
 292         m_quatAxis.SetX(x);
 293         m_quatAxis.SetY(y);
 294         m_quatAxis.SetZ(z);
 295
 296         m_bQuaxAxisSet = true;
 297     }
 298 }
 299
 300 void DeadReckoningCalculator::calcOrientation( EulerAngles & OrientationOut, const KFLOAT32 totalTimeSinceReset  )
 301 {
 302     // compute the DR matrix
 303     TMATRIX DR;
 304     if (m_bQuaxAxisSet)
 305         computeDRMatrix_asQuat( DR, totalTimeSinceReset );
 306     else
 307         computeDRMatrix( DR, totalTimeSinceReset );
 308
 309     TMATRIX curOrientationMatrix = DR * m_initOrientationMatrix;
 310     //TMATRIX curOrientationMatrix = m_initOrientationMatrix;
 311
 312     matrixToHPR_new( curOrientationMatrix, OrientationOut );
 313     //matrixToHPR( curOrientationMatrix, m_curOrientation );
 314     OrientationOut[0] = checkAngleHR( OrientationOut[0]);
 315     OrientationOut[1] = checkAngleP(OrientationOut[1]);
 316     OrientationOut[2] = checkAngleHR( OrientationOut[2]);
 317
 318     computeRotationAxis(curOrientationMatrix);
 319 }
 320
 321 //////////////////////////////////////////////////////////////////////////
 322
 323 void DeadReckoningCalculator::computeDRMatrix( TMATRIX& res, const KFLOAT32 totalTimeSinceReset )
 324 {
 325     TMATRIX I;
 326     const KFLOAT64 f64DeltaMag = totalTimeSinceReset * m_f64Magnitude;
 327     if( f64DeltaMag > MAGIC_EPSILON )
 328     {
 329         const KFLOAT32 f64CosMagDelta = cos( f64DeltaMag );
 330         const KFLOAT64 fwwScalar = ( 1 - f64CosMagDelta ) / ( m_f64Magnitude * m_f64Magnitude );
 331         res = m_wwMatrix;
 332         res *= fwwScalar;
 333         I *= f64CosMagDelta;
 334         const KFLOAT64 f64SkewScalar = sin( f64DeltaMag ) / m_f64Magnitude;
 335         res += I;
 336         res -= ( m_SkewOmegaMatrix * f64SkewScalar );
 337     }
 338     else
 339     {
 340         res = I; // Identity matrix
 341     }
 342 }
 343
 344 void DeadReckoningCalculator::computeDRMatrix_asQuat( TMATRIX& res, const KFLOAT32 totalTimeSinceReset )
 345 {
 346     // to be implemented
 347     // assume that m_quatAxis is set
 348     KFLOAT32 qx, qy, qz;
 349     KFLOAT32 q0;
 350     const KFLOAT64 beta = totalTimeSinceReset * m_f64Magnitude;
 351
 352     KFLOAT32 sin_beta_2 = sin(beta/2.0f);
 353     qx = sin_beta_2 * m_quatAxis.GetX();
 354     qy = sin_beta_2 * m_quatAxis.GetY();
 355     qz = sin_beta_2 * m_quatAxis.GetZ();
 356     q0 = cos(beta/2.0f);
 357
 358     res.Data[0][0] = q0*q0 + qx*qx - 0.5f;
 359     res.Data[0][1] = qx*qy - q0*qz;
 360     res.Data[0][2] = qx*qz + q0*qy;
 361
 362     res.Data[1][0] = qx*qy + q0*qz;
 363     res.Data[1][1] = q0*q0 + qy*qy - 0.5f;
 364     res.Data[1][2] = qy*qz - q0*qx;
 365
 366     res.Data[2][0] = qx*qz - q0*qy;
 367     res.Data[2][1] = qy*qz + q0*qx;
 368     res.Data[2][2] = q0*q0 + qz*qz - 0.5f;
 369 }
 370
 371 //////////////////////////////////////////////////////////////////////////
 372 // Dead Reckoning Algorithm Implementations
 373 //////////////////////////////////////////////////////////////////////////
 374
 375 void DeadReckoningCalculator::calcDeadReckoningFPW( WorldCoordinates & PositionOut, const KFLOAT32 totalTimeSinceReset)
 376 {
 377     /* Position */
 378     PositionOut = ( m_initPosition + ( m_initLinearVelocity * totalTimeSinceReset ) );
 379 }
 380
 381 //////////////////////////////////////////////////////////////////////////
 382
 383 void DeadReckoningCalculator::calcDeadReckoningRPW( WorldCoordinates & PositionOut, EulerAngles & OrientationOut, const KFLOAT32 totalTimeSinceReset )
 384 {
 385     /* Position */
 386     PositionOut = ( m_initPosition + ( m_initLinearVelocity * totalTimeSinceReset ) );
 387     if (m_f64Magnitude < MAGIC_EPSILON)
 388     {
 389         OrientationOut = m_initOrientation;
 390         return;
 391     }
 392     //calcOrientation(OrientationOut);
 393     calcOrientation_simplified( OrientationOut, totalTimeSinceReset );
 394 }
 395
 396 //////////////////////////////////////////////////////////////////////////
 397
 398 void DeadReckoningCalculator::calcDeadReckoningRVW(  WorldCoordinates & PositionOut, EulerAngles & OrientationOut, const KFLOAT32 totalTimeSinceReset )
 399 {
 400     /* Position */
 401     PositionOut = ( m_initPosition + ( m_initLinearVelocity * totalTimeSinceReset ) +
 402                     ( m_initLinearAcceleration * ( 0.5 * totalTimeSinceReset * totalTimeSinceReset ) ) );
 403     if (m_f64Magnitude < MAGIC_EPSILON)
 404     {
 405         OrientationOut = m_initOrientation;
 406         return;
 407     }
 408     //calcOrientation(OrientationOut);
 409     calcOrientation_simplified( OrientationOut, totalTimeSinceReset );
 410 }
 411
 412 //////////////////////////////////////////////////////////////////////////
 413
 414 void DeadReckoningCalculator::calcDeadReckoningFVW( WorldCoordinates & PositionOut, const KFLOAT32 totalTimeSinceReset )
 415 {
 416     /* Position */
 417     PositionOut = ( m_initPosition + ( m_initLinearVelocity * totalTimeSinceReset ) +
 418                     ( m_initLinearAcceleration * ( 0.5 * totalTimeSinceReset * totalTimeSinceReset ) ) );
 419 }
 420
 421 //////////////////////////////////////////////////////////////////////////
 422
 423 void DeadReckoningCalculator::calcDeadReckoningFPB( WorldCoordinates & PositionOut, const KFLOAT32 totalTimeSinceReset )
 424 {
 425     // See page 124 of IEEE 1278.1 - 1995 for more details on this and other dead reckoning equations.
 426
 427     // Calculate Magnitude, scalars etc
 428     const KFLOAT64 f64DeltaMag = totalTimeSinceReset * m_f64Magnitude;
 429     if( f64DeltaMag > MAGIC_EPSILON )
 430     {
 431         KFLOAT32 m_f32MagPow2 = m_f64Magnitude * m_f64Magnitude;
 432         KFLOAT32 m_f32MagPow3 = m_f32MagPow2 * m_f64Magnitude;
 433
 434         const KFLOAT64 f64Scalar = ( ( f64DeltaMag ) - sin( f64DeltaMag ) ) / m_f32MagPow3;
 435         const KFLOAT64 f64IScalar = sin( f64DeltaMag ) / m_f64Magnitude;
 436         const KFLOAT64 f64SkewScale = ( 1 - cos( f64DeltaMag ) ) / m_f32MagPow2 ;
 437         TMATRIX R1( m_wwMatrix ), IMatrix;
 438         R1 *= f64Scalar;
 439         IMatrix *= f64IScalar;
 440         R1 += IMatrix;
 441         R1 += ( m_SkewOmegaMatrix * f64SkewScale ); // RI - should be + instead of - IEEE 1278.1-1995.pdf page 128
 442
 443         PositionOut = ( m_initPosition + ( m_initOrientationMatrixTranspose * ( R1 * m_initLinearVelocity ) ) );
 444     }
 445     else
 446     {
 447         PositionOut = ( m_initPosition + ( m_initOrientationMatrixTranspose * m_initLinearVelocity * totalTimeSinceReset ) );
 448     }
 449 }
 450
 451 //////////////////////////////////////////////////////////////////////////
 452
 453 void DeadReckoningCalculator::calcDeadReckoningRPB(  WorldCoordinates & PositionOut, EulerAngles & OrientationOut, const KFLOAT32 totalTimeSinceReset)
 454 {
 455     /* Position */
 456     // Position is calculated the same as FPB
 457     calcDeadReckoningFPB( PositionOut, totalTimeSinceReset );
 458     //calcOrientation(OrientationOut);
 459     calcOrientation_simplified( OrientationOut, totalTimeSinceReset );
 460 }
 461
 462
 463 //////////////////////////////////////////////////////////////////////////
 464
 465 void DeadReckoningCalculator::calcDeadReckoningRVB( WorldCoordinates & PositionOut, EulerAngles & OrientationOut, const KFLOAT32 totalTimeSinceReset )
 466 {
 467     /* Position */
 468     calcDeadReckoningFVB( PositionOut, totalTimeSinceReset );
 469     //calcOrientation(OrientationOut);
 470     calcOrientation_simplified( OrientationOut, totalTimeSinceReset );
 471 }
 472
 473 //////////////////////////////////////////////////////////////////////////
 474
 475 void DeadReckoningCalculator::calcDeadReckoningFVB( WorldCoordinates & PositionOut, const KFLOAT32 totalTimeSinceReset )
 476 {
 477     // the first part of the equation
 478     calcDeadReckoningFPB( PositionOut, totalTimeSinceReset);
 479
 480     // now adding to the position R0t_w->b * R2 * Ab
 481     // Calculate Magnitude, scalars etc
 482     const KFLOAT64 f64DeltaMag = totalTimeSinceReset * m_f64Magnitude;
 483
 484     if( f64DeltaMag > MAGIC_EPSILON )
 485     {
 486
 487         KFLOAT32 m_f32MagPow2 = m_f64Magnitude * m_f64Magnitude;
 488         KFLOAT32 m_f32MagPow3 = m_f32MagPow2 * m_f64Magnitude;
 489         KFLOAT32 m_f32MagPow4 = m_f32MagPow3 * m_f64Magnitude;
 490         KFLOAT32 m_f32DeltaPow2 = f64DeltaMag * f64DeltaMag;
 491
 492         KFLOAT64 f64R2Scale = (0.5 * m_f32DeltaPow2 - cos( f64DeltaMag )- f64DeltaMag * sin( f64DeltaMag ) + 1)/m_f32MagPow4;
 493
 494         KFLOAT64 f64R2IScalar = ( cos( f64DeltaMag ) + f64DeltaMag * sin( f64DeltaMag ) - 1.0 ) / m_f32MagPow2;
 495
 496         KFLOAT64 f64R2SkewScale = ( sin( f64DeltaMag ) - f64DeltaMag * cos( f64DeltaMag ) ) / m_f32MagPow3;
 497
 498         // Create the ww matrix
 499         TMATRIX R2Matrix( m_wwMatrix ), IR2Matrix;
 500         R2Matrix *= f64R2Scale;
 501         IR2Matrix *= f64R2IScalar;
 502         R2Matrix += ( IR2Matrix + ( m_SkewOmegaMatrix * f64R2SkewScale ) );
 503
 504         PositionOut = PositionOut + ( m_initOrientationMatrixTranspose * ( R2Matrix * m_Ab ) );
 505     } else
 506         PositionOut = PositionOut + ( m_initOrientationMatrixTranspose * m_Ab ) * totalTimeSinceReset;
 507 }
 508
 509 /////////////////////////////////////////////////////////////////////////
 510
 511 void DeadReckoningCalculator::RunAlgorithm( const KFLOAT32 totalTimeSinceReset, WorldCoordinates& PositionOut, EulerAngles& OrientationOut )
 512 {
 513     switch( m_DRA )
 514     {
 515     case DRM_F_P_W:
 516         calcDeadReckoningFPW( PositionOut, totalTimeSinceReset );
 517         break;
 518
 519     case DRM_R_P_W:
 520             calcDeadReckoningRPW( PositionOut, OrientationOut, totalTimeSinceReset );
 521         break;
 522
 523     case DRM_R_V_W:
 524             calcDeadReckoningRVW( PositionOut, OrientationOut, totalTimeSinceReset );
 525         break;
 526
 527     case DRM_F_V_W:
 528         calcDeadReckoningFVW( PositionOut, totalTimeSinceReset );
 529         break;
 530
 531     case DRM_F_P_B:
 532         calcDeadReckoningFPB( PositionOut, totalTimeSinceReset );
 533         break;
 534
 535     case DRM_R_P_B:
 536         calcDeadReckoningRPB( PositionOut, OrientationOut, totalTimeSinceReset );
 537         break;
 538
 539     case DRM_R_V_B:
 540         calcDeadReckoningRVB( PositionOut, OrientationOut, totalTimeSinceReset );
 541         break;
 542
 543     case DRM_F_V_B:
 544         calcDeadReckoningFVB( PositionOut, totalTimeSinceReset );
 545         break;
 546     }
 547 }
 548
 549 //////////////////////////////////////////////////////////////////////////
 550 // public:
 551 //////////////////////////////////////////////////////////////////////////
 552
 553 DeadReckoningCalculator::DeadReckoningCalculator() :
 554     m_bQuaxAxisSet( false ),
 555     m_f64Magnitude( 0 ),
 556     m_DRA( Other_DRA )
 557 {
 558 }
 559
 560 //////////////////////////////////////////////////////////////////////////
 561
 562 DeadReckoningCalculator::~DeadReckoningCalculator()
 563 {
 564 }
 565
 566 //////////////////////////////////////////////////////////////////////////
 567
 568 void DeadReckoningCalculator::Reset( const Vector & LinearVelocity, const Vector & LinearAcceleration, const Vector & AngularVelocity,
 569                                      const WorldCoordinates & Position, const EulerAngles & Orientation, const Vector & QuatAxis, DeadReckoningAlgorithm DRA )
 570 {
 571     m_DRA = DRA;
 572
 573     positionReset( Position );
 574     if ((QuatAxis.GetX() + QuatAxis.GetY() + QuatAxis.GetZ()) > 0.0f) // must set m_bQuaxAxisSet before orientationReset
 575         quatAxisReset(QuatAxis);
 576     else
 577         m_bQuaxAxisSet = false;
 578     orientationReset( Orientation );
 579     angularVelocityReset( AngularVelocity );
 580     velocityReset( LinearVelocity );
 581     accelerationReset( LinearAcceleration );
 582
 583     m_Ab = ( m_initLinearAcceleration + ( m_SkewOmegaMatrix * m_initLinearVelocity ) );
 584 }
 585
 586 //////////////////////////////////////////////////////////////////////////
 587
 588 vector<WorldCoordinates> DeadReckoningCalculator::GenerateSmoothingPoints( const WorldCoordinates & StartPosition, const WorldCoordinates & EndPosition,
 589                                                                            KUINT32 NumberOfPoints )
 590 {
 591     vector<WorldCoordinates> Points;
 592     Points.reserve( NumberOfPoints );
 593
 594     GenerateSmoothingPoints( StartPosition, EndPosition, NumberOfPoints, Points );
 595
 596     return Points;
 597 }
 598
 599 //////////////////////////////////////////////////////////////////////////
 600
 601 void DeadReckoningCalculator::GenerateSmoothingPoints( const WorldCoordinates & StartPosition, const WorldCoordinates & EndPosition,
 602                                                        KUINT32 NumberOfPoints, vector<WorldCoordinates> & v)
 603 {
 604     v.reserve( NumberOfPoints );
 605     WorldCoordinates delta_loc = EndPosition - StartPosition;
 606
 607     for( KINT32 i = 1; i <= NumberOfPoints; ++i )
 608     {
 609         v.push_back( StartPosition + ( delta_loc *  ( KFLOAT32( i ) / KFLOAT32( NumberOfPoints ) ) ) );
 610     }
 611 }
 612
 613 //////////////////////////////////////////////////////////////////////////
 614
 615 void DeadReckoningCalculator::quatAxisReset( const Vector & QuatAxis )
 616 {
 617     m_quatAxis = QuatAxis;
 618     m_bQuaxAxisSet = true;
 619 }
 620
 621 //////////////////////////////////////////////////////////////////////////
 622
 623 #undef KDIS_PI
 624 #undef KDIS_PI_2