Initial structure for refactoring, some conversions still missing. Especially required further test cases.

samples/blackmiscquantities/samplesphysicalquantities.cpp

@@ -46,6 +46,10 @@ int CSamplesPhysicalQuantities::samples()
     l3 *= 1.5;// 3km now
     qDebug() << l2 << l3;
 
+    l3 = l3 * 2;
+    qDebug() << "doubled l3:" << l3;
+
+
     CFrequency f1(1E6, CFrequencyUnit::Hz()); // 1MHz
     qDebug() << f1 << f1.valueRoundedWithUnit(CFrequencyUnit::MHz()) << f1.valueRoundedWithUnit(CFrequencyUnit::GHz(), 3);
 

samples/blackmiscvectorgeo/samplesgeo.cpp

@@ -11,15 +11,15 @@ namespace BlackMiscTest
  */
 int CSamplesGeo::samples()
 {
-    CGeoLatitude lat1(20.0, CAngleUnit::deg());
+    CLatitude lat1(20.0, CAngleUnit::deg());
-    CGeoLatitude lat2 = lat1;
+    CLatitude lat2 = lat1;
-    CGeoLatitude lat3 = lat1 - lat2;
+    CLatitude lat3 = lat1 - lat2;
 
     qDebug() << lat1 << lat2 << lat3;
     qDebug() << (lat1 + lat2) << (lat1 - lat2);
 
     lat3 += lat1;
-    CGeoLongitude lon1(33.0, CAngleUnit::deg());
+    CLongitude lon1(33.0, CAngleUnit::deg());
     qDebug() << lon1 << lat3;
 
 //    lat3 += lon1; // must not work

src/blackmisc/blackmisc.pro

@@ -72,7 +72,9 @@ HEADERS += \
     coordinategeodetic.h \
     coordinateecef.h \
     coordinatened.h \
-    geoearthangle.h
+    geoearthangle.h \
+    coordinatetransformation.h \
+    mathmatrix1x3.h
 
 SOURCES += \
     logmessage.cpp \
@@ -104,6 +106,7 @@ SOURCES += \
     mathvector3dbase.cpp \
     mathmatrixbase.cpp \
     mathmatrix3x3.cpp \
-    mathematics.cpp
+    mathematics.cpp \
+    coordinatetransformation.cpp
 
 DESTDIR = ../../lib

src/blackmisc/coordinategeodetic.h

@@ -21,8 +21,8 @@ class CCoordinateGeodetic : public CBaseStreamStringifier<CCoordinateGeodetic>
 {
 
 private:
-    CGeoLatitude m_latitude; //!< Latitude
+    CLatitude m_latitude; //!< Latitude
-    CGeoLongitude m_longitude; //!< Longitude
+    CLongitude m_longitude; //!< Longitude
     BlackMisc::PhysicalQuantities::CLength m_height; //!< height
 
 protected:
@@ -55,7 +55,7 @@ public:
      * \param longitude
      * \param height
      */
-    CCoordinateGeodetic(CGeoLatitude latitude, CGeoLongitude longitude, BlackMisc::PhysicalQuantities::CLength height) :
+    CCoordinateGeodetic(CLatitude latitude, CLongitude longitude, BlackMisc::PhysicalQuantities::CLength height) :
         m_latitude(latitude), m_longitude(longitude), m_height(height) {}
 
     /*!
@@ -72,7 +72,7 @@ public:
      * \brief Latitude
      * \return
      */
-    CGeoLatitude latitude() const
+    CLatitude latitude() const
     {
         return this->m_latitude;
     }
@@ -81,7 +81,7 @@ public:
      * \brief Longitude
      * \return
      */
-    CGeoLongitude longitude() const
+    CLongitude longitude() const
     {
         return this->m_longitude;
     }
@@ -99,7 +99,7 @@ public:
      * \brief Set latitude
      * \param latitude
      */
-    void setLatitude(CGeoLatitude latitude)
+    void setLatitude(CLatitude latitude)
     {
         this->m_latitude = latitude;
     }
@@ -108,7 +108,7 @@ public:
      * \brief Set longitude
      * \param latitude
      */
-    void setLongitude(CGeoLongitude longitude)
+    void setLongitude(CLongitude longitude)
     {
         this->m_longitude = longitude;
     }

src/blackmisc/coordinatened.h

@@ -6,6 +6,8 @@
 #ifndef BLACKMISC_COORDINATENED_H
 #define BLACKMISC_COORDINATENED_H
 #include "blackmisc/mathvector3dbase.h"
+#include "blackmisc/mathmatrix3x3.h"
+#include "blackmisc/coordinategeodetic.h"
 
 namespace BlackMisc
 {
@@ -16,19 +18,94 @@ namespace Geo
  */
 class CCoordinateNed : public BlackMisc::Math::CVector3DBase<CCoordinateNed>
 {
+
+private:
+    CCoordinateGeodetic m_referencePosition; //!< geodetic reference position
+    bool m_hasReferencePosition; //!< valid reference position?
+
 public:
     /*!
      * \brief Default constructor
      */
-    CCoordinateNed() : CVector3DBase() {}
+    CCoordinateNed() : CVector3DBase(), m_hasReferencePosition(false) {}
+
+    /*!
+     * \brief Constructor with reference position
+     * \param referencePosition
+     */
+    CCoordinateNed(const CCoordinateGeodetic &referencePosition) : CVector3DBase(), m_referencePosition(referencePosition), m_hasReferencePosition(true) {}
 
     /*!
      * \brief Constructor by values
+     * \param referencePosition
      * \param north
      * \param east
      * \param down
      */
-    CCoordinateNed(qreal north, qreal east, qreal down) : CVector3DBase(north, east, down) {}
+    CCoordinateNed(const CCoordinateGeodetic &referencePosition, qreal north, qreal east, qreal down) : CVector3DBase(north, east, down), m_referencePosition(referencePosition), m_hasReferencePosition(true) {}
+
+    /*!
+     * \brief Copy constructor
+     * \param otherNed
+     */
+    CCoordinateNed(const CCoordinateNed &otherNed) :
+        CVector3DBase(otherNed) , m_hasReferencePosition(otherNed.m_hasReferencePosition), m_referencePosition(otherNed.m_referencePosition) {}
+
+    /*!
+     * \brief Equal operator ==
+     * \param otherNed
+     * \return
+     */
+    bool operator ==(const CCoordinateNed &otherNed) const
+    {
+        if (this == &otherNed) return true;
+        return this->m_hasReferencePosition == otherNed.m_hasReferencePosition &&
+               this->m_referencePosition == otherNed.m_referencePosition &&
+               CVector3DBase::operator== (otherNed);
+    }
+
+    /*!
+     * \brief Unequal operator !=
+     * \param otherNed
+     * \return
+     */
+    bool operator !=(const CCoordinateNed &otherNed) const
+    {
+        if (this == &otherNed) return false;
+        return !((*this) == otherNed);
+    }
+
+    /*!
+     * \brief Assigment operator =
+     * \param otherNed
+     * \return
+     */
+    CCoordinateNed &operator =(const CCoordinateNed &otherNed)
+    {
+        if (this == &otherNed)  return *this; // Same object?
+        CVector3DBase::operator = (otherNed);
+        this->m_hasReferencePosition = otherNed.m_hasReferencePosition;
+        this->m_referencePosition = otherNed.m_referencePosition;
+        return (*this);
+    }
+
+    /*!
+     * \brief Corresponding reference position
+     * \return
+     */
+    CCoordinateGeodetic referencePosition() const
+    {
+        return this->m_referencePosition;
+    }
+
+    /*!
+     * \brief Corresponding reference position
+     * \return
+     */
+    bool hasReferencePosition() const
+    {
+        return this->m_hasReferencePosition;
+    }
 
     /*!
      * \brief North
@@ -83,6 +160,17 @@ public:
     {
         this->m_vector.setZ(down);
     }
+
+    /*!
+     * \brief Corresponding reference position
+     * \param referencePosition
+     */
+    void setReferencePosition(const CCoordinateGeodetic &referencePosition)
+    {
+        this->m_referencePosition = referencePosition;
+        this->m_hasReferencePosition = true;
+    }
+
 };
 
 } // namespace

src/blackmisc/coordinatetransformation.cpp

@@ -0,0 +1,63 @@
+/*  Copyright (C) 2013 VATSIM Community / authors
+ *  This Source Code Form is subject to the terms of the Mozilla Public
+ *  License, v. 2.0. If a copy of the MPL was not distributed with this
+ *  file, You can obtain one at http://mozilla.org/MPL/2.0/. */
+
+#include "coordinatetransformation.h"
+using namespace BlackMisc::PhysicalQuantities;
+using namespace BlackMisc::Math;
+
+namespace BlackMisc
+{
+namespace Geo
+{
+
+/*
+ * NED to ECEF
+ */
+CCoordinateEcef CCoordinateTransformation::toEcef(const CCoordinateNed &ned)
+{
+    CLatitude lat = ned.referencePosition().latitude();
+    lat.switchUnit(CAngleUnit::rad());
+    CLongitude lon = ned.referencePosition().longitude();
+    lon.switchUnit(CAngleUnit::rad());
+
+    double angleRad = - (lat.unitValueToDouble()) - BlackMisc::Math::PI / 2;
+    CMatrix3x3 dcm1;
+    CMatrix3x3 dcm2;
+    CMatrix3x3 dcm3;
+    CMatrix3x3 dcm;
+    CMatrix3x3 invDcm;
+    dcm1.setToIdentity();
+    dcm2.setZero();
+    dcm3.setZero();
+
+    dcm2(0, 0) = cos(angleRad);
+    dcm2(0, 2) = -sin(angleRad);
+    dcm2(1, 1) = 1;
+    dcm2(2, 0) = sin(angleRad);
+    dcm2(2, 2) = cos(angleRad);
+
+    angleRad = lon.unitValueToDouble();
+
+    dcm3(0, 0) = cos(angleRad);
+    dcm3(0, 1) = sin(angleRad);
+    dcm3(1, 0) = -sin(angleRad);
+    dcm3(1, 1) = cos(angleRad);
+    dcm3(2, 2) = 1;
+
+    dcm = dcm1 * dcm2 * dcm3;
+
+    invDcm.setZero();
+    invDcm = dcm.inverse();
+
+    CCoordinateNed tempResult(ned);
+    tempResult.matrixMultiplication(invDcm);
+    CCoordinateEcef result(tempResult.north(), tempResult.east(), tempResult.down());
+
+    return result;
+
+}
+
+} // namespace
+} // namespace

src/blackmisc/coordinatetransformation.h

@@ -0,0 +1,44 @@
+/*  Copyright (C) 2013 VATSIM Community / authors
+ *  This Source Code Form is subject to the terms of the Mozilla Public
+ *  License, v. 2.0. If a copy of the MPL was not distributed with this
+ *  file, You can obtain one at http://mozilla.org/MPL/2.0/. */
+
+#ifndef BLACKMISC_COORDINATETRANSFORMATION_H
+#define BLACKMISC_COORDINATETRANSFORMATION_H
+
+#include "blackmisc/coordinateecef.h"
+#include "blackmisc/coordinatened.h"
+#include "blackmisc/coordinategeodetic.h"
+#include "blackmisc/mathmatrix3x3.h"
+#include "blackmisc/mathematics.h"
+
+namespace BlackMisc
+{
+namespace Geo
+{
+
+/*!
+ * \brief Coordinate transformation class between different systems
+ */
+class CCoordinateTransformation
+{
+private:
+    /*!
+     * \brief Default constructor, avoid object instantiation
+     */
+    CCoordinateTransformation() {}
+
+public:
+    /*!
+     * \brief NED to ECEF
+     * \param ned
+     * \return
+     */
+    static CCoordinateEcef toEcef(const CCoordinateNed &ned);
+
+};
+
+} // namespace
+} // namespace
+
+#endif // guard

src/blackmisc/geoearthangle.h

@@ -14,33 +14,33 @@ namespace Geo
 /*!
  * \brief Base class for latitude / longitude
  */
-template <class LATorLON> class CGeoEarthAngle : public BlackMisc::PhysicalQuantities::CAngle
+template <class LATorLON> class CEarthAngle : public BlackMisc::PhysicalQuantities::CAngle
 {
 protected:
     /*!
      * \brief Default constructor
      */
-    CGeoEarthAngle() : CAngle() {}
+    CEarthAngle() : CAngle() {}
 
     /*!
      * \brief Copy constructor
      * \param latOrLon
      */
-    CGeoEarthAngle(const LATorLON &latOrLon) : CAngle(latOrLon) { }
+    CEarthAngle(const LATorLON &latOrLon) : CAngle(latOrLon) { }
 
     /*!
      * \brief Init by double value
      * \param value
      * \param unit
      */
-    CGeoEarthAngle(double value, const BlackMisc::PhysicalQuantities::CAngleUnit &unit): CAngle(value, unit) {}
+    CEarthAngle(double value, const BlackMisc::PhysicalQuantities::CAngleUnit &unit): CAngle(value, unit) {}
 
 public:
 
     /*!
      * \brief Virtual destructor
      */
-    virtual ~CGeoEarthAngle() {}
+    virtual ~CEarthAngle() {}
 
     /*!
      * \brief Equal operator ==
@@ -67,7 +67,7 @@ public:
      * \param latOrLon
      * \return
      */
-    CGeoEarthAngle &operator +=(const CGeoEarthAngle &latOrLon)
+    CEarthAngle &operator +=(const CEarthAngle &latOrLon)
     {
         CAngle::operator +=(latOrLon);
         return (*this);
@@ -78,7 +78,7 @@ public:
      * \param latOrLon
      * \return
      */
-    CGeoEarthAngle &operator -=(const CGeoEarthAngle &latOrLon)
+    CEarthAngle &operator -=(const CEarthAngle &latOrLon)
     {
         CAngle::operator -=(latOrLon);
         return (*this);
@@ -129,7 +129,7 @@ public:
      * \param latOrLon
      * \return
      */
-    CGeoEarthAngle &operator =(const LATorLON &latOrLon)
+    CEarthAngle &operator =(const LATorLON &latOrLon)
     {
         CAngle::operator =(latOrLon);
         return (*this);

src/blackmisc/geolatitude.h

@@ -10,7 +10,7 @@ namespace Geo
 /*!
  * \brief Latitude
  */
-class CGeoLatitude : public CGeoEarthAngle<CGeoLatitude>
+class CLatitude : public CEarthAngle<CLatitude>
 {
 protected:
     /*!
@@ -19,32 +19,32 @@ protected:
     virtual QString stringForConverter() const
     {
         QString s = "latitude ";
-        return s.append(CGeoEarthAngle::stringForConverter());
+        return s.append(CEarthAngle::stringForConverter());
     }
 
 public:
     /*!
      * \brief Default constructor
      */
-    CGeoLatitude() : CGeoEarthAngle() {}
+    CLatitude() : CEarthAngle() {}
 
     /*!
      * \brief Copy constructor
      * \param latitude
      */
-    CGeoLatitude(const CGeoLatitude &latitude) : CGeoEarthAngle(latitude) {}
+    CLatitude(const CLatitude &latitude) : CEarthAngle(latitude) {}
 
     /*!
      * \brief Init by double value
      * \param value
      * \param unit
      */
-    CGeoLatitude(double value, const BlackMisc::PhysicalQuantities::CAngleUnit &unit): CGeoEarthAngle(value, unit) {}
+    CLatitude(double value, const BlackMisc::PhysicalQuantities::CAngleUnit &unit): CEarthAngle(value, unit) {}
 
     /*!
      * \brief Virtual destructor
      */
-    virtual ~CGeoLatitude() {}
+    virtual ~CLatitude() {}
 };
 
 } // namespace

src/blackmisc/geolongitude.h

@@ -10,7 +10,7 @@ namespace Geo
 /*!
  * \brief Longitude
  */
-class CGeoLongitude : public CGeoEarthAngle<CGeoLongitude>
+class CLongitude : public CEarthAngle<CLongitude>
 {
 protected:
     /*!
@@ -19,32 +19,32 @@ protected:
     virtual QString stringForConverter() const
     {
         QString s = "longitude ";
-        return s.append(CGeoEarthAngle::stringForConverter());
+        return s.append(CEarthAngle::stringForConverter());
     }
 
 public:
     /*!
      * \brief Default constructor
      */
-    CGeoLongitude() : CGeoEarthAngle() {}
+    CLongitude() : CEarthAngle() {}
 
     /*!
      * \brief Copy constructor
      * \param Longitude
      */
-    CGeoLongitude(const CGeoLongitude &Longitude) : CGeoEarthAngle(Longitude) {}
+    CLongitude(const CLongitude &Longitude) : CEarthAngle(Longitude) {}
 
     /*!
      * \brief Init by double value
      * \param value
      * \param unit
      */
-    CGeoLongitude(double value, const BlackMisc::PhysicalQuantities::CAngleUnit &unit): CGeoEarthAngle(value, unit) {}
+    CLongitude(double value, const BlackMisc::PhysicalQuantities::CAngleUnit &unit): CEarthAngle(value, unit) {}
 
     /*!
      * \brief Virtual destructor
      */
-    virtual ~CGeoLongitude() {}
+    virtual ~CLongitude() {}
 };
 
 } // namespace

src/blackmisc/mathematics.cpp

@@ -14,7 +14,7 @@ namespace Math
 /*
  * Hypotenuse
  */
-double CMath::hypot(double x, double y)
+qreal CMath::hypot(qreal x, qreal y)
 {
     x = abs(x);
     y = abs(y);
@@ -27,7 +27,7 @@ double CMath::hypot(double x, double y)
 /*
  * Real part of cubic root
  */
-double CMath::cubicRootReal(const double x)
+qreal CMath::cubicRootReal(qreal x)
 {
     double result;
     result = std::pow(std::abs(x), (double)1 / 3);

src/blackmisc/mathematics.h

@@ -5,32 +5,42 @@
 
 #ifndef BLACKMISC_MATHEMATICS_H
 #define BLACKMISC_MATHEMATICS_H
+#include <QtCore/qmath.h>
 
 namespace BlackMisc
 {
 namespace Math
 {
 
+// Some namespace wide constant values
+
+//! Mathematical constant Pi
+const qreal PI = 4.0 * qAtan(1.0);
+
+//! 2 * Pi
+const qreal TwoPI  = 2.0 * PI;
+
 /*!
  * \brief Math utils
  */
 class CMath
 {
 public:
+
     /*!
      * \brief Calculates the hypotenuse of x and y without overflow
      * \param x
      * \param y
      * \return
      */
-    static double hypot(double x, double y);
+    static qreal hypot(qreal x, qreal y);
 
     /*!
      * \brief Calculates the square of x
      * \param x
      * \return
      */
-    static inline double square(const double x)
+    static inline qreal square(qreal x)
     {
         return x * x;
     }
@@ -40,7 +50,7 @@ public:
      * \param x
      * \return
      */
-    static inline double cubic(const double x)
+    static inline qreal cubic(const qreal x)
     {
         return x * x * x;
     }
@@ -50,7 +60,7 @@ public:
      * \param x
      * \return
      */
-    static double cubicRootReal(const double x);
+    static qreal cubicRootReal(qreal x);
 
 private:
     /*!

src/blackmisc/mathmatrix1x3.h

@@ -0,0 +1,58 @@
+/*  Copyright (C) 2013 VATSIM Community / authors
+ *  This Source Code Form is subject to the terms of the Mozilla Public
+ *  License, v. 2.0. If a copy of the MPL was not distributed with this
+ *  file, You can obtain one at http://mozilla.org/MPL/2.0/. */
+
+#ifndef BLACKMISC_POSMATRIX1x3_H
+#define BLACKMISC_POSMATRIX1x3_H
+
+#include "blackmisc/mathmatrixbase.h"
+#include "blackmisc/mathvector3d.h"
+
+namespace BlackMisc
+{
+namespace Math
+{
+
+/*!
+ * \brief 3D matrix
+ */
+class CMatrix1x3 : public CMatrixBase<CMatrix1x3, 1, 3>
+{
+public:
+    /*!
+     * \brief Matrix 1x3
+     */
+    CMatrix1x3() : CMatrixBase() {}
+
+    /*!
+     * \brief init with value
+     * \param fillValue
+     */
+    CMatrix1x3(qreal fillValue) : CMatrixBase(fillValue) {}
+
+    /*!
+     * \brief Copy constructor
+     * \param other
+     */
+    CMatrix1x3(const CMatrix1x3 &otherMatrix) : CMatrixBase(otherMatrix) {}
+
+    /*!
+     * \brief CMatrix 3x1
+     * \param c1
+     * \param c2
+     * \param c3
+     */
+    CMatrix1x3(qreal c1, qreal c2, qreal c3) : CMatrixBase()
+    {
+        this->m_matrix(0, 0) = c1;
+        this->m_matrix(0, 1) = c2;
+        this->m_matrix(0, 2) = c3;
+    }
+};
+
+} // namespace
+
+} // namespace
+
+#endif // guard

src/blackmisc/mathmatrix3x1.h

@@ -15,16 +15,31 @@ namespace Math
 {
 
 /*!
- * \brief 3D matrix
+ * \brief Matrix 3x1
  */
 class CMatrix3x1 : public CMatrixBase<CMatrix3x1, 3, 1>
 {
+    friend class CMatrix3x3; // for matrix multiplicaion to access m_matrix
+
 public:
     /*!
-     * \brief CMatrix3D
+     * \brief CMatrix 3x1
      */
     CMatrix3x1() : CMatrixBase() {}
 
+    /*!
+     * \brief CMatrix 3x1
+     * \param r1
+     * \param r2
+     * \param r3
+     */
+    CMatrix3x1(qreal r1, qreal r2, qreal r3) : CMatrixBase()
+    {
+        this->m_matrix(0, 0) = r1;
+        this->m_matrix(1, 0) = r2;
+        this->m_matrix(2, 0) = r3;
+    }
+
     /*!
      * \brief init with value
      * \param fillValue
@@ -52,9 +67,9 @@ public:
      */
     void fromVector3D(const CVector3D &vector)
     {
-        this->m_matrix[0][0] = vector.x();
+        this->m_matrix(0, 0) = vector.i();
-        this->m_matrix[1][0] = vector.y();
+        this->m_matrix(1, 0) = vector.j();
-        this->m_matrix[2][0] = vector.z();
+        this->m_matrix(2, 0) = vector.k();
     }
 };
 

src/blackmisc/mathmatrix3x3.cpp

@@ -52,6 +52,30 @@ CMatrix3x3 CMatrix3x3::inverse() const
     return inverse;
 }
 
+/*
+ * Get a row
+ */
+CMatrix1x3 CMatrix3x3::getRow(int row) const
+{
+    bool valid = row >= 0 && row <= 3;
+    Q_ASSERT_X(valid, "getRow", "invalid row");
+    if (!valid) throw new std::range_error("invalid row");
+    return CMatrix1x3(this->getElement(row, 0), this->getElement(row, 1), this->getElement(row, 2));
+}
+
+/*
+ * Get a column
+ */
+CMatrix3x1 CMatrix3x3::getColumn(int column) const
+{
+    bool valid = column >= 0 && column <= 3;
+    Q_ASSERT_X(valid, "getColumn", "invalid column");
+    if (!valid) throw new std::range_error("invalid column");
+    return CMatrix3x1(this->getElement(0, column), this->getElement(1, column), this->getElement(2, column));
+}
+
+
+
 } // namespace
 
 } // namespace

src/blackmisc/mathmatrix3x3.h

@@ -5,8 +5,8 @@
 
 #ifndef BLACKMISC_POSMATRIX3X3_H
 #define BLACKMISC_POSMATRIX3X3_H
-
+#include "blackmisc/mathmatrix1x3.h"
-#include "blackmisc/mathmatrixbase.h"
+#include "blackmisc/mathmatrix3x1.h"
 
 namespace BlackMisc
 {
@@ -14,7 +14,7 @@ namespace Math
 {
 
 /*!
- * \brief 3x1 matrix
+ * \brief 3x3 matrix
  */
 class CMatrix3x3 : public CMatrixBase<CMatrix3x3, 3, 3>
 {
@@ -48,6 +48,66 @@ public:
      */
     CMatrix3x3 inverse() const;
 
+    /*!
+     * \brief Operator *=
+     * \param otherMatrix
+     * \return
+     */
+    CMatrix3x3 &operator *=(const CMatrix3x3 &otherMatrix)
+    {
+        this->m_matrix = this->m_matrix * otherMatrix.m_matrix;
+        return (*this);
+    }
+
+    /*!
+     * \brief Operator *
+     * \param otherMatrix
+     * \return
+     */
+    CMatrix3x1 operator *(const CMatrix3x1 &otherMatrix) const
+    {
+        CMatrix3x1 m;
+        m.m_matrix = this->m_matrix * otherMatrix.m_matrix;
+        return m;
+    }
+
+    /*!
+     * \brief Operator *
+     * \param multiply
+     * \return
+     */
+    CMatrix3x3 operator *(const CMatrix3x3 &otherMatrix) const
+    {
+        CMatrix3x3 m(otherMatrix);
+        m *= otherMatrix;
+        return m;
+    }
+
+    /*!
+     * \brief Transposed matrix
+     * \return
+     */
+    CMatrix3x3 transposed() const
+    {
+        CMatrix3x3 m(0.0);
+        m.m_matrix = this->m_matrix.transposed();
+        return m;
+    }
+
+    /*!
+     * \brief Get column
+     * \param column
+     * \return
+     */
+    CMatrix3x1 getColumn(int column) const;
+
+    /*!
+     * \brief Get row
+     * \param column
+     * \return
+     */
+    CMatrix1x3 getRow(int column) const;
+
 };
 
 } // namespace

src/blackmisc/mathmatrixbase.cpp

@@ -14,12 +14,29 @@ namespace Math
 /*
  * Get element by column / row
  */
-template<class ImplMatrix, int Rows, int Columns> double CMatrixBase<ImplMatrix, Rows, Columns>::getElement(size_t row, size_t column) const
+template<class ImplMatrix, int Rows, int Columns> qreal CMatrixBase<ImplMatrix, Rows, Columns>::getElement(size_t row, size_t column) const
+{
+    this->checkRange(row, column);
+    return this->m_matrix(row, column);
+}
+
+/*
+ * Set element by column / row
+ */
+template<class ImplMatrix, int Rows, int Columns> void CMatrixBase<ImplMatrix, Rows, Columns>::setElement(size_t row, size_t column, qreal value)
+{
+    this->checkRange(row, column);
+    this->m_matrix(row, column) = value;
+}
+
+/*
+ * Check range
+ */
+template<class ImplMatrix, int Rows, int Columns> void CMatrixBase<ImplMatrix, Rows, Columns>::checkRange(size_t row, size_t column) const
 {
     bool valid = (row >= 0 && column >= 0 && row < Rows && column < Columns);
     Q_ASSERT_X(valid, "getElement()", "Row or column invalid");
-    std::range_error("Row or column invalid");
+    if (!valid) throw std::range_error("Row or column invalid");
-    return this->m_matrix(row, column);
 }
 
 /*
@@ -57,11 +74,11 @@ template <class ImplMatrix, int Rows, int Columns> QString CMatrixBase<ImplMatri
     return s;
 }
 
-
 // see here for the reason of thess forward instantiations
 // http://www.parashift.com/c++-faq/separate-template-class-defn-from-decl.html
 template class CMatrixBase<CMatrix3x3, 3, 3>;
 template class CMatrixBase<CMatrix3x1, 3, 1>;
+template class CMatrixBase<CMatrix1x3, 1, 3>;
 
 } // namespace
 

src/blackmisc/mathmatrixbase.h

@@ -7,6 +7,7 @@
 #define BLACKMISC_MATHMATRIXBASE_H
 
 #include "blackmisc/basestreamstringifier.h"
+#include "blackmisc/mathvector3dbase.h"
 #include <QGenericMatrix>
 
 namespace BlackMisc
@@ -17,11 +18,12 @@ namespace Math
 /*!
  * \brief Base functionality of a matrix
  */
-template<class ImplMatrix, int Rows, int Columns> class CMatrixBase :
+template<class ImplMatrix, int Rows, int Columns> class CMatrixBase : public BlackMisc::CBaseStreamStringifier<ImplMatrix>
-    public BlackMisc::CBaseStreamStringifier<ImplMatrix>
 {
+
 protected:
-    QGenericMatrix<Rows, Columns, qreal> m_matrix; //!< backing data
+    // no bug, Qt expects columns rows
+    QGenericMatrix<Columns, Rows, qreal> m_matrix; //!< backing data
 
     /*!
      * \brief Conversion to string
@@ -72,7 +74,6 @@ public:
     {
         if (this == &otherMatrix) return false;
         return !((*this) == otherMatrix);
-
     }
 
     /*!
@@ -87,30 +88,6 @@ public:
         return (*this);
     }
 
-    /*!
-     * \brief Operator *=
-     * \param multiply
-     * \return
-     */
-    CMatrixBase &operator *=(const CMatrixBase &otherMatrix)
-    {
-        this->m_matrix = this->m_matrix * otherMatrix.m_matrix;
-        return (*this);
-    }
-
-    /*!
-     * \brief Operator *
-     * \param multiply
-     * \return
-     */
-    ImplMatrix operator *(const ImplMatrix &otherMatrix) const
-    {
-        ImplMatrix m(0.0);
-        m += (*this);
-        m *= otherMatrix;
-        return m;
-    }
-
     /*!
      * \brief Operator *=
      * \param factor
@@ -135,6 +112,13 @@ public:
         return m;
     }
 
+    /*!
+     * \brief Multiply with 3D vector operator *
+     * \param vector
+     * \return
+     */
+    template<class ImplVector> ImplVector operator*(const ImplVector vector) const;
+
     /*!
      * \brief Operator /=
      * \param factor
@@ -207,17 +191,6 @@ public:
         return m;
     }
 
-    /*!
-     * \brief Transposed matrix
-     * \return
-     */
-    ImplMatrix transposed() const
-    {
-        ImplMatrix m(0.0);
-        m.m_matrix = this->m_matrix.transposed();
-        return m;
-    }
-
     /*!
      * \brief Is identity matrix?
      * \return
@@ -255,9 +228,49 @@ public:
     /*!
      * \brief Get element
      * \param row
+     * \param column
      * \return
      */
-    double getElement(size_t row, size_t column) const;
+    qreal getElement(size_t row, size_t column) const;
+
+    /*!
+     * \brief Get element
+     * \param row
+     * \param column
+     * \param value
+     */
+    void setElement(size_t row, size_t column, qreal value);
+
+    /*!
+     * \brief Get element by ()
+     * \param column
+     * \return
+     */
+    qreal &operator()(size_t row, size_t column)
+    {
+        this->checkRange(row, column);
+        return this->m_matrix(row, column);
+    }
+
+    /*!
+     * \brief Get element by ()
+     * \param column
+     * \return
+     */
+    qreal operator()(size_t row, size_t column) const
+    {
+        return this->getElement(row, column);
+    }
+
+private:
+    /*!
+     * \brief Check range of row / column
+     * \param row
+     * \param column
+     * \throws std::range_error if index out of bounds
+     */
+    void checkRange(size_t row, size_t column) const;
+
 };
 
 } // namespace

src/blackmisc/mathvector3dbase.cpp

@@ -3,8 +3,8 @@
  *  License, v. 2.0. If a copy of the MPL was not distributed with this
  *  file, You can obtain one at http://mozilla.org/MPL/2.0/. */
 
-#include "blackmisc/mathvector3dbase.h"
 #include "blackmisc/mathvector3d.h"
+#include "blackmisc/mathmatrix3x3.h"
 #include "blackmisc/coordinateecef.h"
 #include "blackmisc/coordinatened.h"
 
@@ -46,10 +46,7 @@ template <class ImplClass> void CVector3DBase<ImplClass>::fill(qreal value)
  */
 template <class ImplClass> qreal CVector3DBase<ImplClass>::getElement(size_t row) const
 {
-    bool validIndex = (row < 3 && row >= 0);
+    qreal d;
-    Q_ASSERT_X(validIndex, "getElement", "Wrong index");
-    if (!validIndex) throw std::range_error("Invalid index vor 3D vector");
-    double d;
     switch (row)
     {
     case 0:
@@ -69,6 +66,48 @@ template <class ImplClass> qreal CVector3DBase<ImplClass>::getElement(size_t row
     return d;
 }
 
+/*
+ * Set given element
+ */
+template <class ImplClass> void CVector3DBase<ImplClass>::setElement(size_t row, qreal value)
+{
+    switch (row)
+    {
+    case 0:
+        this->m_vector.setX(value);
+        break;
+    case 1:
+        this->m_vector.setY(value);
+        break;
+    case 2:
+        this->m_vector.setZ(value);
+        break;
+    default:
+        Q_ASSERT_X(true, "setElement", "Detected invalid index in 3D vector");
+        throw std::range_error("Detected invalid index in 3D vector");
+        break;
+    }
+}
+
+/*
+ * Multiply with matrix
+ */
+template <class ImplClass> void CVector3DBase<ImplClass>::matrixMultiplication(const CMatrix3x3 &matrix)
+{
+    CMatrix3x1 m = matrix * (this->toMatrix3x1());
+    this->m_vector.setX(m(0,0));
+    this->m_vector.setY(m(1,0));
+    this->m_vector.setZ(m(2,0));
+}
+
+/*
+ * Convert to matrix
+ */
+template <class ImplClass> CMatrix3x1 CVector3DBase<ImplClass>::toMatrix3x1() const
+{
+    return CMatrix3x1(this->m_vector.x(), this->m_vector.y(), this->m_vector.z());
+}
+
 // see here for the reason of thess forward instantiations
 // http://www.parashift.com/c++-faq/separate-template-class-defn-from-decl.html
 template class CVector3DBase<CVector3D>;

src/blackmisc/mathvector3dbase.h

@@ -14,6 +14,10 @@ namespace BlackMisc
 namespace Math
 {
 
+class CMatrix3x3; // forward declaration
+class CMatrix3x1; // forward declaration
+
+
 /*!
  * \brief 3D vector base (x, y, z)
  */
@@ -75,6 +79,13 @@ public:
      */
     qreal getElement(size_t row) const;
 
+    /*!
+     * \brief Set element
+     * \param row
+     * \param value
+     */
+    void setElement(size_t row, qreal value);
+
     /*!
      * \brief Operator []
      * \param row
@@ -82,6 +93,14 @@ public:
      */
     qreal operator[](size_t row) const { return this->getElement(row); }
 
+
+    /*!
+     * \brief Get element by ()
+     * \param column
+     * \return
+     */
+    qreal operator()(size_t row) const { return this->getElement(row); }
+
     /*!
      * \brief Equal operator ==
      * \param otherVector
@@ -106,7 +125,7 @@ public:
 
     /*!
      * \brief Assigment operator =
-     * \param multiply
+     * \param otherVector
      * \return
      */
     CVector3DBase &operator =(const CVector3DBase &otherVector)
@@ -165,7 +184,7 @@ public:
     }
 
     /*!
-     * \brief Operator *=, just x*x, y*y, z*z neither vector nor dot product
+     * \brief Operator *=, just x*x, y*y, z*z neither vector nor dot product (like a matrix produc)
      * \param otherVector
      * \return
      */
@@ -176,7 +195,7 @@ public:
     }
 
     /*!
-     * \brief Operator, just x*x, y*y, z*z neither vector nor dot product
+     * \brief Operator, just x*x, y*y, z*z neither vector nor dot product, (like a matrix produc)
      * \param otherVector
      * \return
      */
@@ -223,7 +242,7 @@ public:
     }
 
     /*!
-     * \brief Dot product
+     * \brief Cross product
      * \param otherVector
      * \return
      */
@@ -235,7 +254,14 @@ public:
     }
 
     /*!
-     * \brief Dot product
+     * \brief Matrix * this vector
+     * \param matrix
+     * \return
+     */
+    void matrixMultiplication(const CMatrix3x3 &matrix);
+
+    /*!
+     * \brief Reciporcal value
      * \param otherVector
      * \return
      */
@@ -266,6 +292,12 @@ public:
         return this->m_vector.lengthSquared();
     }
 
+    /*!
+     * \brief Converted to matrix
+     * \return
+     */
+    CMatrix3x1 toMatrix3x1() const;
+
     /*!
      * \brief Magnitude
      * \return

src/blackmisc/pqphysicalquantity.cpp

@@ -57,16 +57,22 @@ template <class MU, class PQ> bool CPhysicalQuantity<MU, PQ>::operator ==(const
     // some special cases for best quality
     double diff;
     const double lenient = 1.001; // even diff already has a rounding issue to be avoided
-    if (this->m_unit == otherQuantity.m_unit) {
+    if (this->m_unit == otherQuantity.m_unit)
+    {
         // same unit
-        if (this->m_isIntegerBaseValue && otherQuantity.m_isIntegerBaseValue) {
+        if (this->m_isIntegerBaseValue && otherQuantity.m_isIntegerBaseValue)
+        {
             // pure integer comparison, no rounding issues
             return this->m_unitValueI == otherQuantity.m_unitValueI;
-        } else {
+        }
+        else
+        {
             diff = abs(this->m_unitValueD - otherQuantity.m_unitValueD);
             return diff <= (lenient * this->m_unit.getEpsilon());
         }
-    } else {
+    }
+    else
+    {
         // based on SI value
         diff = abs(this->m_convertedSiUnitValueD - otherQuantity.m_convertedSiUnitValueD);
         return diff <= (lenient * this->m_unit.getEpsilon());
@@ -105,15 +111,21 @@ template <class MU, class PQ> CPhysicalQuantity<MU, PQ>& CPhysicalQuantity<MU, P
  */
 template <class MU, class PQ> CPhysicalQuantity<MU, PQ> &CPhysicalQuantity<MU, PQ>::operator +=(const CPhysicalQuantity<MU, PQ> &otherQuantity)
 {
-    if (this->m_unit == otherQuantity.m_unit) {
+    if (this->m_unit == otherQuantity.m_unit)
+    {
         // same unit
-        if (this->m_isIntegerBaseValue && otherQuantity.m_isIntegerBaseValue) {
+        if (this->m_isIntegerBaseValue && otherQuantity.m_isIntegerBaseValue)
+        {
             // pure integer, no rounding issues
             this->setUnitValue(otherQuantity.m_unitValueI + this->m_unitValueI);
-        } else {
+        }
+        else
+        {
             this->setUnitValue(otherQuantity.m_unitValueD + this->m_unitValueD);
         }
-    } else {
+    }
+    else
+    {
         double v = otherQuantity.value(this->m_unit);
         this->setUnitValue(v + this->m_unitValueD);
     }
@@ -152,15 +164,21 @@ template <class MU, class PQ> void CPhysicalQuantity<MU, PQ>::substractUnitValue
  */
 template <class MU, class PQ> CPhysicalQuantity<MU, PQ> &CPhysicalQuantity<MU, PQ>::operator -=(const CPhysicalQuantity<MU, PQ> &otherQuantity)
 {
-    if (this->m_unit == otherQuantity.m_unit) {
+    if (this->m_unit == otherQuantity.m_unit)
+    {
         // same unit
-        if (this->m_isIntegerBaseValue && otherQuantity.m_isIntegerBaseValue) {
+        if (this->m_isIntegerBaseValue && otherQuantity.m_isIntegerBaseValue)
+        {
             // pure integer, no rounding issues
             this->setUnitValue(otherQuantity.m_unitValueI - this->m_unitValueI);
-        } else {
+        }
+        else
+        {
             this->setUnitValue(otherQuantity.m_unitValueD - this->m_unitValueD);
         }
-    } else {
+    }
+    else
+    {
         double v = otherQuantity.value(this->m_unit);
         this->setUnitValue(v - this->m_unitValueD);
     }
@@ -258,14 +276,13 @@ template <class MU, class PQ> bool CPhysicalQuantity<MU, PQ>::operator <=(const
 /*
  * Switch to another unit
  */
-template <class MU, class PQ> bool CPhysicalQuantity<MU, PQ>::switchUnit(const MU &newUnit)
+template <class MU, class PQ> CPhysicalQuantity<MU, PQ> &CPhysicalQuantity<MU, PQ>::switchUnit(const MU &newUnit)
 {
-    if (this->m_unit == newUnit) return true;
+    if (this->m_unit == newUnit) return *this;
-    if (this->m_unit.getType() != newUnit.getType()) return false; // not possible
     double cf = this->m_unit.conversionToUnit(this->m_unitValueD, newUnit);
     this->m_unit = newUnit;
     this->setUnitValue(cf);
-    return true;
+    return *this;
 }
 
 /*

src/blackmisc/pqphysicalquantity.h

@@ -111,7 +111,7 @@ public:
      * \param newUnit
      * \return
      */
-    bool switchUnit(const MU &newUnit);
+    CPhysicalQuantity &switchUnit(const MU &newUnit);
 
     /*!
      * \brief Value in SI base unit? Meter is an SI base unit, hertz not!