src/DataTypes/LE_EulerAngles.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 "KDIS/DataTypes/LE_EulerAngles.hpp"
  31
  32 #include "KDIS/Extras/KUtils.hpp"
  33
  34 using namespace KDIS;
  35 using namespace DATA_TYPE;
  36 using namespace UTILS;
  37
  38 //////////////////////////////////////////////////////////////////////////
  39 // Public:
  40 //////////////////////////////////////////////////////////////////////////
  41
  42 LE_EulerAngles::LE_EulerAngles() {}
  43
  44 //////////////////////////////////////////////////////////////////////////
  45
  46 LE_EulerAngles::LE_EulerAngles(KFIXED8_3 Psi, KFIXED8_3 Theta, KFIXED8_3 Phi)
  47     : m_Psi(Psi), m_Theta(Theta), m_Phi(Phi) {}
  48
  49 //////////////////////////////////////////////////////////////////////////
  50
  51 LE_EulerAngles::LE_EulerAngles(KDataStream& stream) { Decode(stream); }
  52
  53 //////////////////////////////////////////////////////////////////////////
  54
  55 LE_EulerAngles::~LE_EulerAngles() {}
  56
  57 //////////////////////////////////////////////////////////////////////////
  58
  59 void LE_EulerAngles::SetPsiInRadians(KFIXED8_3 Psi) { m_Psi = Psi; }
  60
  61 //////////////////////////////////////////////////////////////////////////
  62
  63 KFIXED8_3 LE_EulerAngles::GetPsiInRadians() const { return m_Psi; }
  64
  65 //////////////////////////////////////////////////////////////////////////
  66
  67 void LE_EulerAngles::SetPsiInDegrees(KFIXED8_3 Psi) { m_Psi = DegToRad(Psi); }
  68
  69 //////////////////////////////////////////////////////////////////////////
  70
  71 KFIXED8_3 LE_EulerAngles::GetPsiInDegrees() const { return RadToDeg(m_Psi); }
  72
  73 //////////////////////////////////////////////////////////////////////////
  74
  75 void LE_EulerAngles::SetThetaInRadians(KFIXED8_3 Theta) { m_Theta = Theta; }
  76
  77 //////////////////////////////////////////////////////////////////////////
  78
  79 KFIXED8_3 LE_EulerAngles::GetThetaInRadians() const { return m_Theta; }
  80
  81 //////////////////////////////////////////////////////////////////////////
  82
  83 void LE_EulerAngles::SetThetaInDegrees(KFIXED8_3 Theta) {
  84   m_Theta = DegToRad(Theta);
  85 }
  86
  87 //////////////////////////////////////////////////////////////////////////
  88
  89 KFIXED8_3 LE_EulerAngles::GetThetaInDegrees() const {
  90   return RadToDeg(m_Theta);
  91 }
  92
  93 //////////////////////////////////////////////////////////////////////////
  94
  95 void LE_EulerAngles::SetPhiInRadians(KFIXED8_3 Phi) { m_Phi = Phi; }
  96
  97 //////////////////////////////////////////////////////////////////////////
  98
  99 KFIXED8_3 LE_EulerAngles::GetPhiInRadians() const { return m_Phi; }
 100
 101 //////////////////////////////////////////////////////////////////////////
 102
 103 void LE_EulerAngles::SetPhiInDegrees(KFIXED8_3 Phi) { m_Phi = DegToRad(Phi); }
 104
 105 //////////////////////////////////////////////////////////////////////////
 106
 107 KFIXED8_3 LE_EulerAngles::GetPhiInDegrees() const { return RadToDeg(m_Phi); }
 108
 109 //////////////////////////////////////////////////////////////////////////
 110
 111 KString LE_EulerAngles::GetAsString() const {
 112   KStringStream ss;
 113
 114   ss << "Psi: " << m_Psi.GetAsFloat32()
 115      << ",  Theta: " << m_Theta.GetAsFloat32()
 116      << ",  Phi: " << m_Phi.GetAsFloat32() << "\n";
 117
 118   return ss.str();
 119 }
 120
 121 //////////////////////////////////////////////////////////////////////////
 122
 123 void LE_EulerAngles::Decode(KDataStream& stream) {
 124   if (stream.GetBufferSize() < LE_EULER_ANGLES_SIZE)
 125     throw KException(__FUNCTION__, NOT_ENOUGH_DATA_IN_BUFFER);
 126
 127   stream >> KDIS_STREAM m_Psi >> KDIS_STREAM m_Theta >> KDIS_STREAM m_Phi;
 128 }
 129
 130 //////////////////////////////////////////////////////////////////////////
 131
 132 KDataStream LE_EulerAngles::Encode() const {
 133   KDataStream stream;
 134
 135   LE_EulerAngles::Encode(stream);
 136
 137   return stream;
 138 }
 139
 140 //////////////////////////////////////////////////////////////////////////
 141
 142 void LE_EulerAngles::Encode(KDataStream& stream) const {
 143   stream << KDIS_STREAM m_Psi << KDIS_STREAM m_Theta << KDIS_STREAM m_Phi;
 144 }
 145
 146 //////////////////////////////////////////////////////////////////////////
 147
 148 KBOOL LE_EulerAngles::operator==(const LE_EulerAngles& Value) const {
 149   if (m_Psi != Value.m_Psi) return false;
 150   if (m_Theta != Value.m_Theta) return false;
 151   if (m_Phi != Value.m_Phi) return false;
 152   return true;
 153 }
 154
 155 //////////////////////////////////////////////////////////////////////////
 156
 157 KBOOL LE_EulerAngles::operator!=(const LE_EulerAngles& Value) const {
 158   return !(*this == Value);
 159 }
 160
 161 //////////////////////////////////////////////////////////////////////////
 162
 163 LE_EulerAngles LE_EulerAngles::operator*(const LE_EulerAngles& Value) const {
 164   LE_EulerAngles tmp = *this;
 165   tmp.m_Psi = tmp.m_Psi * Value.m_Psi;
 166   tmp.m_Theta = tmp.m_Theta * Value.m_Theta;
 167   tmp.m_Phi = tmp.m_Phi * Value.m_Phi;
 168   return tmp;
 169 }
 170
 171 //////////////////////////////////////////////////////////////////////////
 172
 173 LE_EulerAngles KDIS::DATA_TYPE::LE_EulerAngles::operator*(
 174     KFLOAT64 Value) const {
 175   LE_EulerAngles tmp = *this;
 176   tmp.m_Psi = tmp.m_Psi * Value;
 177   tmp.m_Theta = tmp.m_Theta * Value;
 178   tmp.m_Phi = tmp.m_Phi * Value;
 179   return tmp;
 180 }
 181
 182 //////////////////////////////////////////////////////////////////////////
 183
 184 LE_EulerAngles LE_EulerAngles::operator+(const LE_EulerAngles& Value) const {
 185   LE_EulerAngles tmp = *this;
 186   tmp.m_Psi = tmp.m_Psi + Value.m_Psi;
 187   tmp.m_Theta = tmp.m_Theta + Value.m_Theta;
 188   tmp.m_Phi = tmp.m_Phi + Value.m_Phi;
 189   return tmp;
 190 }
 191
 192 //////////////////////////////////////////////////////////////////////////
 193
 194 LE_EulerAngles& LE_EulerAngles::operator+=(const LE_EulerAngles& Value) {
 195   m_Psi = m_Psi + Value.m_Psi;
 196   m_Theta = m_Theta + Value.m_Theta;
 197   m_Phi = m_Phi + Value.m_Phi;
 198   return *this;
 199 }
 200
 201 //////////////////////////////////////////////////////////////////////////
 202
 203 LE_EulerAngles LE_EulerAngles::operator-(const LE_EulerAngles& Value) const {
 204   LE_EulerAngles tmp = *this;
 205   tmp.m_Psi = tmp.m_Psi - Value.m_Psi;
 206   tmp.m_Theta = tmp.m_Theta - Value.m_Theta;
 207   tmp.m_Phi = tmp.m_Phi - Value.m_Phi;
 208   return tmp;
 209 }
 210
 211 //////////////////////////////////////////////////////////////////////////
 212
 213 LE_EulerAngles& LE_EulerAngles::operator-=(const LE_EulerAngles& Value) {
 214   m_Psi = m_Psi - Value.m_Psi;
 215   m_Theta = m_Theta - Value.m_Theta;
 216   m_Phi = m_Phi - Value.m_Phi;
 217   return *this;
 218 }
 219
 220 //////////////////////////////////////////////////////////////////////////
 221
 222 KFIXED8_3& LE_EulerAngles::operator[](KUINT16 i) {
 223   switch (i) {
 224     case 0:
 225       return m_Psi;
 226     case 1:
 227       return m_Theta;
 228     case 2:
 229       return m_Phi;
 230     default:
 231       throw KException(__FUNCTION__, OUT_OF_BOUNDS);
 232   }
 233 }
 234
 235 //////////////////////////////////////////////////////////////////////////
 236
 237 const KFIXED8_3& LE_EulerAngles::operator[](KUINT16 i) const {
 238   switch (i) {
 239     case 0:
 240       return m_Psi;
 241     case 1:
 242       return m_Theta;
 243     case 2:
 244       return m_Phi;
 245     default:
 246       throw KException(__FUNCTION__, OUT_OF_BOUNDS);
 247   }
 248 }
 249
 250 //////////////////////////////////////////////////////////////////////////