matrix: apply PX4 astyle

Tools/astyle/files_to_check_code_style.sh

@@ -17,7 +17,6 @@ exec find boards msg src platforms test \
     -path src/drivers/uavcannode_gps_demo/libcanard -prune -o \
     -path src/lib/crypto/monocypher -prune -o \
     -path src/lib/events/libevents -prune -o \
-    -path src/lib/matrix -prune -o \
     -path src/lib/parameters/uthash -prune -o \
     -path src/modules/ekf2/EKF -prune -o \
     -path src/modules/gyro_fft/CMSIS_5 -prune -o \

src/lib/matrix/matrix/AxisAngle.hpp

@@ -30,131 +30,134 @@ template<typename Type>
 class AxisAngle : public Vector<Type, 3>
 {
 public:
-    using Matrix31 = Matrix<Type, 3, 1>;
+	using Matrix31 = Matrix<Type, 3, 1>;
 
-    /**
+	/**
-     * Constructor from array
+	 * Constructor from array
-     *
+	 *
-     * @param data_ array
+	 * @param data_ array
-     */
+	 */
-    explicit AxisAngle(const Type data_[3]) :
+	explicit AxisAngle(const Type data_[3]) :
-        Vector<Type, 3>(data_)
+		Vector<Type, 3>(data_)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Standard constructor
+	 * Standard constructor
-     */
+	 */
-    AxisAngle() = default;
+	AxisAngle() = default;
 
-    /**
+	/**
-     * Constructor from Matrix31
+	 * Constructor from Matrix31
-     *
+	 *
-     * @param other Matrix31 to copy
+	 * @param other Matrix31 to copy
-     */
+	 */
-    AxisAngle(const Matrix31 &other) :
+	AxisAngle(const Matrix31 &other) :
-        Vector<Type, 3>(other)
+		Vector<Type, 3>(other)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Constructor from quaternion
+	 * Constructor from quaternion
-     *
+	 *
-     * This sets the instance from a quaternion representing coordinate transformation from
+	 * This sets the instance from a quaternion representing coordinate transformation from
-     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
+	 * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
-     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+	 * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
-     *
+	 *
-     * @param q quaternion
+	 * @param q quaternion
-     */
+	 */
-    AxisAngle(const Quaternion<Type> &q)
+	AxisAngle(const Quaternion<Type> &q)
-    {
+	{
-        AxisAngle &v = *this;
+		AxisAngle &v = *this;
-        Type mag = q.imag().norm();
+		Type mag = q.imag().norm();
-        if (fabs(mag) >= Type(1e-10)) {
-            v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
-        } else {
-            v = q.imag() * Type(Type(2) * Type(sign(q(0))));
-        }
-    }
 
-    /**
+		if (fabs(mag) >= Type(1e-10)) {
-     * Constructor from dcm
+			v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
-     *
-     * Instance is initialized from a dcm representing coordinate transformation
-     * from frame 2 to frame 1.
-     *
-     * @param dcm dcm to set quaternion to
-     */
-    AxisAngle(const Dcm<Type> &dcm)
-    {
-        AxisAngle &v = *this;
-        v = AxisAngle<Type>(Quaternion<Type>(dcm));
-    }
 
-    /**
+		} else {
-     * Constructor from euler angles
+			v = q.imag() * Type(Type(2) * Type(sign(q(0))));
-     *
+		}
-     * This sets the instance to a quaternion representing coordinate transformation from
+	}
-     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
-     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
-     *
-     * @param euler euler angle instance
-     */
-    AxisAngle(const Euler<Type> &euler)
-    {
-        AxisAngle &v = *this;
-        v = AxisAngle<Type>(Quaternion<Type>(euler));
-    }
 
-    /**
+	/**
-     * Constructor from 3 axis angle values (unit vector * angle)
+	 * Constructor from dcm
-     *
+	 *
-     * @param x r_x*angle
+	 * Instance is initialized from a dcm representing coordinate transformation
-     * @param y r_y*angle
+	 * from frame 2 to frame 1.
-     * @param z r_z*angle
+	 *
-     */
+	 * @param dcm dcm to set quaternion to
-    AxisAngle(Type x, Type y, Type z)
+	 */
-    {
+	AxisAngle(const Dcm<Type> &dcm)
-        AxisAngle &v = *this;
+	{
-        v(0) = x;
+		AxisAngle &v = *this;
-        v(1) = y;
+		v = AxisAngle<Type>(Quaternion<Type>(dcm));
-        v(2) = z;
+	}
-    }
 
-    /**
+	/**
-     * Constructor from axis and angle
+	 * Constructor from euler angles
-     *
+	 *
-     * @param axis An axis of rotation, normalized if not unit length
+	 * This sets the instance to a quaternion representing coordinate transformation from
-     * @param angle The amount to rotate
+	 * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
-     */
+	 * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
-    AxisAngle(const Matrix31 & axis_, Type angle_)
+	 *
-    {
+	 * @param euler euler angle instance
-        AxisAngle &v = *this;
+	 */
-        // make sure axis is a unit vector
+	AxisAngle(const Euler<Type> &euler)
-        Vector<Type, 3> a = axis_;
+	{
-        a = a.unit();
+		AxisAngle &v = *this;
-        v(0) = a(0)*angle_;
+		v = AxisAngle<Type>(Quaternion<Type>(euler));
-        v(1) = a(1)*angle_;
+	}
-        v(2) = a(2)*angle_;
+
-    }
+	/**
+	 * Constructor from 3 axis angle values (unit vector * angle)
+	 *
+	 * @param x r_x*angle
+	 * @param y r_y*angle
+	 * @param z r_z*angle
+	 */
+	AxisAngle(Type x, Type y, Type z)
+	{
+		AxisAngle &v = *this;
+		v(0) = x;
+		v(1) = y;
+		v(2) = z;
+	}
+
+	/**
+	 * Constructor from axis and angle
+	 *
+	 * @param axis An axis of rotation, normalized if not unit length
+	 * @param angle The amount to rotate
+	 */
+	AxisAngle(const Matrix31 &axis_, Type angle_)
+	{
+		AxisAngle &v = *this;
+		// make sure axis is a unit vector
+		Vector<Type, 3> a = axis_;
+		a = a.unit();
+		v(0) = a(0) * angle_;
+		v(1) = a(1) * angle_;
+		v(2) = a(2) * angle_;
+	}
 
 
-    Vector<Type, 3> axis() {
+	Vector<Type, 3> axis()
-        if (Vector<Type, 3>::norm() > 0) {
+	{
-            return Vector<Type, 3>::unit();
+		if (Vector<Type, 3>::norm() > 0) {
-        } else {
+			return Vector<Type, 3>::unit();
-            return Vector3<Type>(1, 0, 0);
-        }
-    }
 
-    Type angle() {
+		} else {
-        return Vector<Type, 3>::norm();
+			return Vector3<Type>(1, 0, 0);
-    }
+		}
+	}
+
+	Type angle()
+	{
+		return Vector<Type, 3>::norm();
+	}
 };
 
 using AxisAnglef = AxisAngle<float>;
 using AxisAngled = AxisAngle<double>;
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Dcm.hpp

@@ -39,149 +39,147 @@ template<typename Type>
 class Dcm : public SquareMatrix<Type, 3>
 {
 public:
-    using Vector3 = Matrix<Type, 3, 1>;
+	using Vector3 = Matrix<Type, 3, 1>;
 
-    /**
+	/**
-     * Standard constructor
+	 * Standard constructor
-     *
+	 *
-     * Initializes to identity
+	 * Initializes to identity
-     */
+	 */
-    Dcm() : SquareMatrix<Type, 3>(eye<Type, 3>()) {}
+	Dcm() : SquareMatrix<Type, 3>(eye<Type, 3>()) {}
 
-    /**
+	/**
-     * Constructor from array
+	 * Constructor from array
-     *
+	 *
-     * @param _data pointer to array
+	 * @param _data pointer to array
-     */
+	 */
-    explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
+	explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Constructor from array
+	 * Constructor from array
-     *
+	 *
-     * @param _data pointer to array
+	 * @param _data pointer to array
-     */
+	 */
-    explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
+	explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Copy constructor
+	 * Copy constructor
-     *
+	 *
-     * @param other Matrix33 to set dcm to
+	 * @param other Matrix33 to set dcm to
-     */
+	 */
-    Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(other)
+	Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(other)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Constructor from quaternion
+	 * Constructor from quaternion
-     *
+	 *
-     * Instance is initialized from quaternion representing
+	 * Instance is initialized from quaternion representing
-     * coordinate transformation from frame 2 to frame 1.
+	 * coordinate transformation from frame 2 to frame 1.
-     *
+	 *
-     * @param q quaternion to set dcm to
+	 * @param q quaternion to set dcm to
-     */
+	 */
-    Dcm(const Quaternion<Type> &q)
+	Dcm(const Quaternion<Type> &q)
-    {
+	{
-        Dcm &dcm = *this;
+		Dcm &dcm = *this;
-        const Type a = q(0);
+		const Type a = q(0);
-        const Type b = q(1);
+		const Type b = q(1);
-        const Type c = q(2);
+		const Type c = q(2);
-        const Type d = q(3);
+		const Type d = q(3);
-        const Type aa = a * a;
+		const Type aa = a * a;
-        const Type ab = a * b;
+		const Type ab = a * b;
-        const Type ac = a * c;
+		const Type ac = a * c;
-        const Type ad = a * d;
+		const Type ad = a * d;
-        const Type bb = b * b;
+		const Type bb = b * b;
-        const Type bc = b * c;
+		const Type bc = b * c;
-        const Type bd = b * d;
+		const Type bd = b * d;
-        const Type cc = c * c;
+		const Type cc = c * c;
-        const Type cd = c * d;
+		const Type cd = c * d;
-        const Type dd = d * d;
+		const Type dd = d * d;
-        dcm(0, 0) = aa + bb - cc - dd;
+		dcm(0, 0) = aa + bb - cc - dd;
-        dcm(0, 1) = Type(2) * (bc - ad);
+		dcm(0, 1) = Type(2) * (bc - ad);
-        dcm(0, 2) = Type(2) * (ac + bd);
+		dcm(0, 2) = Type(2) * (ac + bd);
-        dcm(1, 0) = Type(2) * (bc + ad);
+		dcm(1, 0) = Type(2) * (bc + ad);
-        dcm(1, 1) = aa - bb + cc - dd;
+		dcm(1, 1) = aa - bb + cc - dd;
-        dcm(1, 2) = Type(2) * (cd - ab);
+		dcm(1, 2) = Type(2) * (cd - ab);
-        dcm(2, 0) = Type(2) * (bd - ac);
+		dcm(2, 0) = Type(2) * (bd - ac);
-        dcm(2, 1) = Type(2) * (ab + cd);
+		dcm(2, 1) = Type(2) * (ab + cd);
-        dcm(2, 2) = aa - bb - cc + dd;
+		dcm(2, 2) = aa - bb - cc + dd;
-    }
+	}
 
-    /**
+	/**
-     * Constructor from euler angles
+	 * Constructor from euler angles
-     *
+	 *
-     * This sets the transformation matrix from frame 2 to frame 1 where the rotation
+	 * This sets the transformation matrix from frame 2 to frame 1 where the rotation
-     * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+	 * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
-     *
+	 *
-     *
+	 *
-     * @param euler euler angle instance
+	 * @param euler euler angle instance
-     */
+	 */
-    Dcm(const Euler<Type> &euler)
+	Dcm(const Euler<Type> &euler)
-    {
+	{
-        Dcm &dcm = *this;
+		Dcm &dcm = *this;
-        Type cosPhi = Type(cos(euler.phi()));
+		Type cosPhi = Type(cos(euler.phi()));
-        Type sinPhi = Type(sin(euler.phi()));
+		Type sinPhi = Type(sin(euler.phi()));
-        Type cosThe = Type(cos(euler.theta()));
+		Type cosThe = Type(cos(euler.theta()));
-        Type sinThe = Type(sin(euler.theta()));
+		Type sinThe = Type(sin(euler.theta()));
-        Type cosPsi = Type(cos(euler.psi()));
+		Type cosPsi = Type(cos(euler.psi()));
-        Type sinPsi = Type(sin(euler.psi()));
+		Type sinPsi = Type(sin(euler.psi()));
 
-        dcm(0, 0) = cosThe * cosPsi;
+		dcm(0, 0) = cosThe * cosPsi;
-        dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+		dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
-        dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+		dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
 
-        dcm(1, 0) = cosThe * sinPsi;
+		dcm(1, 0) = cosThe * sinPsi;
-        dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+		dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
-        dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+		dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
 
-        dcm(2, 0) = -sinThe;
+		dcm(2, 0) = -sinThe;
-        dcm(2, 1) = sinPhi * cosThe;
+		dcm(2, 1) = sinPhi * cosThe;
-        dcm(2, 2) = cosPhi * cosThe;
+		dcm(2, 2) = cosPhi * cosThe;
-    }
+	}
 
 
-    /**
+	/**
-     * Constructor from axis angle
+	 * Constructor from axis angle
-     *
+	 *
-     * This sets the transformation matrix from frame 2 to frame 1 where the rotation
+	 * This sets the transformation matrix from frame 2 to frame 1 where the rotation
-     * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+	 * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
-     *
+	 *
-     *
+	 *
-     * @param euler euler angle instance
+	 * @param euler euler angle instance
-     */
+	 */
-    Dcm(const AxisAngle<Type> &aa)
+	Dcm(const AxisAngle<Type> &aa)
-    {
+	{
-        Dcm &dcm = *this;
+		Dcm &dcm = *this;
-        dcm = Quaternion<Type>(aa);
+		dcm = Quaternion<Type>(aa);
-    }
+	}
 
-    Vector<Type, 3> vee() const      // inverse to Vector.hat() operation
+	Vector<Type, 3> vee() const      // inverse to Vector.hat() operation
-    {
+	{
-        const Dcm &A(*this);
+		const Dcm &A(*this);
-        Vector<Type, 3> v;
+		Vector<Type, 3> v;
-        v(0) = -A(1, 2);
+		v(0) = -A(1, 2);
-        v(1) =  A(0, 2);
+		v(1) =  A(0, 2);
-        v(2) = -A(0, 1);
+		v(2) = -A(0, 1);
-        return v;
+		return v;
-    }
+	}
 
-    void renormalize()
+	void renormalize()
-    {
+	{
-        /* renormalize rows */
+		/* renormalize rows */
-        for (size_t r = 0; r < 3; r++) {
+		for (size_t r = 0; r < 3; r++) {
-            matrix::Vector3<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose());
+			matrix::Vector3<Type> rvec(Matrix<Type, 1, 3>(this->Matrix<Type, 3, 3>::row(r)).transpose());
-            this->Matrix<Type,3,3>::row(r) = rvec.normalized();
+			this->Matrix<Type, 3, 3>::row(r) = rvec.normalized();
-        }
+		}
-    }
+	}
 };
 
 using Dcmf = Dcm<float>;
 using Dcmd = Dcm<double>;
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Euler.hpp

@@ -36,116 +36,116 @@ template<typename Type>
 class Euler : public Vector<Type, 3>
 {
 public:
-    /**
+	/**
-     * Standard constructor
+	 * Standard constructor
-     */
+	 */
-    Euler() = default;
+	Euler() = default;
 
-    /**
+	/**
-     * Copy constructor
+	 * Copy constructor
-     *
+	 *
-     * @param other vector to copy
+	 * @param other vector to copy
-     */
+	 */
-    Euler(const Vector<Type, 3> &other) :
+	Euler(const Vector<Type, 3> &other) :
-        Vector<Type, 3>(other)
+		Vector<Type, 3>(other)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Constructor from Matrix31
+	 * Constructor from Matrix31
-     *
+	 *
-     * @param other Matrix31 to copy
+	 * @param other Matrix31 to copy
-     */
+	 */
-    Euler(const Matrix<Type, 3, 1> &other) :
+	Euler(const Matrix<Type, 3, 1> &other) :
-        Vector<Type, 3>(other)
+		Vector<Type, 3>(other)
-    {
+	{
-    }
+	}
 
-    /**
+	/**
-     * Constructor from euler angles
+	 * Constructor from euler angles
-     *
+	 *
-     * Instance is initialized from an 3-2-1 intrinsic Tait-Bryan
+	 * Instance is initialized from an 3-2-1 intrinsic Tait-Bryan
-     * rotation sequence representing transformation from frame 1
+	 * rotation sequence representing transformation from frame 1
-     * to frame 2.
+	 * to frame 2.
-     *
+	 *
-     * @param phi_ rotation angle about X axis
+	 * @param phi_ rotation angle about X axis
-     * @param theta_ rotation angle about Y axis
+	 * @param theta_ rotation angle about Y axis
-     * @param psi_ rotation angle about Z axis
+	 * @param psi_ rotation angle about Z axis
-     */
+	 */
-    Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
+	Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
-    {
+	{
-        phi() = phi_;
+		phi() = phi_;
-        theta() = theta_;
+		theta() = theta_;
-        psi() = psi_;
+		psi() = psi_;
-    }
+	}
 
-    /**
+	/**
-     * Constructor from DCM matrix
+	 * Constructor from DCM matrix
-     *
+	 *
-     * Instance is set from Dcm representing transformation from
+	 * Instance is set from Dcm representing transformation from
-     * frame 2 to frame 1.
+	 * frame 2 to frame 1.
-     * This instance will hold the angles defining the 3-2-1 intrinsic
+	 * This instance will hold the angles defining the 3-2-1 intrinsic
-     * Tait-Bryan rotation sequence from frame 1 to frame 2.
+	 * Tait-Bryan rotation sequence from frame 1 to frame 2.
-     *
+	 *
-     * @param dcm Direction cosine matrix
+	 * @param dcm Direction cosine matrix
-    */
+	*/
-    Euler(const Dcm<Type> &dcm)
+	Euler(const Dcm<Type> &dcm)
-    {
+	{
-        theta() = asin(-dcm(2, 0));
+		theta() = asin(-dcm(2, 0));
 
-        if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
+		if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
-            phi() = 0;
+			phi() = 0;
-            psi() = atan2(dcm(1, 2), dcm(0, 2));
+			psi() = atan2(dcm(1, 2), dcm(0, 2));
 
-        } else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
+		} else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
-            phi() = 0;
+			phi() = 0;
-            psi() = atan2(-dcm(1, 2), -dcm(0, 2));
+			psi() = atan2(-dcm(1, 2), -dcm(0, 2));
 
-        } else {
+		} else {
-            phi() = atan2(dcm(2, 1), dcm(2, 2));
+			phi() = atan2(dcm(2, 1), dcm(2, 2));
-            psi() = atan2(dcm(1, 0), dcm(0, 0));
+			psi() = atan2(dcm(1, 0), dcm(0, 0));
-        }
+		}
-    }
+	}
 
-    /**
+	/**
-     * Constructor from quaternion instance.
+	 * Constructor from quaternion instance.
-     *
+	 *
-     * Instance is set from a quaternion representing transformation
+	 * Instance is set from a quaternion representing transformation
-     * from frame 2 to frame 1.
+	 * from frame 2 to frame 1.
-     * This instance will hold the angles defining the 3-2-1 intrinsic
+	 * This instance will hold the angles defining the 3-2-1 intrinsic
-     * Tait-Bryan rotation sequence from frame 1 to frame 2.
+	 * Tait-Bryan rotation sequence from frame 1 to frame 2.
-     *
+	 *
-     * @param q quaternion
+	 * @param q quaternion
-    */
+	*/
-    Euler(const Quaternion<Type> &q) : Vector<Type, 3>(Euler(Dcm<Type>(q)))
+	Euler(const Quaternion<Type> &q) : Vector<Type, 3>(Euler(Dcm<Type>(q)))
-    {
+	{
-    }
+	}
 
-    inline Type phi() const
+	inline Type phi() const
-    {
+	{
-        return (*this)(0);
+		return (*this)(0);
-    }
+	}
-    inline Type theta() const
+	inline Type theta() const
-    {
+	{
-        return (*this)(1);
+		return (*this)(1);
-    }
+	}
-    inline Type psi() const
+	inline Type psi() const
-    {
+	{
-        return (*this)(2);
+		return (*this)(2);
-    }
+	}
 
-    inline Type &phi()
+	inline Type &phi()
-    {
+	{
-        return (*this)(0);
+		return (*this)(0);
-    }
+	}
-    inline Type &theta()
+	inline Type &theta()
-    {
+	{
-        return (*this)(1);
+		return (*this)(1);
-    }
+	}
-    inline Type &psi()
+	inline Type &psi()
-    {
+	{
-        return (*this)(2);
+		return (*this)(2);
-    }
+	}
 
 };
 
@@ -153,5 +153,3 @@ using Eulerf = Euler<float>;
 using Eulerd = Euler<double>;
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/LeastSquaresSolver.hpp

@@ -16,131 +16,149 @@
 
 #include "math.hpp"
 
-namespace matrix {
+namespace matrix
+{
 
 template<typename Type, size_t M, size_t N>
 class LeastSquaresSolver
 {
 public:
 
-    /**
+	/**
-     * @brief Class calculates QR decomposition which can be used for linear
+	 * @brief Class calculates QR decomposition which can be used for linear
-     * least squares
+	 * least squares
-     * @param A Matrix of size MxN
+	 * @param A Matrix of size MxN
-     *
+	 *
-     * Initialize the class with a MxN matrix. The constructor starts the
+	 * Initialize the class with a MxN matrix. The constructor starts the
-     * QR decomposition. This class does not check the rank of the matrix.
+	 * QR decomposition. This class does not check the rank of the matrix.
-     * The user needs to make sure that rank(A) = N and M >= N.
+	 * The user needs to make sure that rank(A) = N and M >= N.
-     */
+	 */
-    LeastSquaresSolver(const Matrix<Type, M, N> &A)
+	LeastSquaresSolver(const Matrix<Type, M, N> &A)
-    {
+	{
-        static_assert(M >= N, "Matrix dimension should be M >= N");
+		static_assert(M >= N, "Matrix dimension should be M >= N");
 
-        // Copy contentents of matrix A
+		// Copy contentents of matrix A
-        _A = A;
+		_A = A;
 
-        for (size_t j = 0; j < N; j++) {
+		for (size_t j = 0; j < N; j++) {
-            Type normx = Type(0);
+			Type normx = Type(0);
-            for (size_t i = j; i < M; i++) {
-                normx += _A(i,j) * _A(i,j);
-            }
-            normx = sqrt(normx);
-            Type s = _A(j,j) > 0 ? Type(-1) : Type(1);
-            Type u1 = _A(j,j) - s*normx;
-            // prevent divide by zero
-            // also covers u1. normx is never negative
-            if (normx < Type(1e-8)) {
-                break;
-            }
-            Type w[M] = {};
-            w[0] = Type(1);
-            for (size_t i = j+1; i < M; i++) {
-                w[i-j] = _A(i,j) / u1;
-                _A(i,j) = w[i-j];
-            }
-            _A(j,j) = s*normx;
-            _tau(j) = -s*u1/normx;
 
-            for (size_t k = j+1; k < N; k++) {
+			for (size_t i = j; i < M; i++) {
-                Type tmp = Type(0);
+				normx += _A(i, j) * _A(i, j);
-                for (size_t i = j; i < M; i++) {
+			}
-                    tmp += w[i-j] * _A(i,k);
-                }
-                for (size_t i = j; i < M; i++) {
-                    _A(i,k) -= _tau(j) * w[i-j] * tmp;
-                }
-            }
 
-        }
+			normx = sqrt(normx);
-    }
+			Type s = _A(j, j) > 0 ? Type(-1) : Type(1);
+			Type u1 = _A(j, j) - s * normx;
 
-    /**
+			// prevent divide by zero
-     * @brief qtb Calculate Q^T * b
+			// also covers u1. normx is never negative
-     * @param b
+			if (normx < Type(1e-8)) {
-     * @return Q^T*b
+				break;
-     *
+			}
-     * This function calculates Q^T * b. This is useful for the solver
-     * because R*x = Q^T*b.
-     */
-    Vector<Type, M> qtb(const Vector<Type, M> &b) {
-        Vector<Type, M> qtbv = b;
 
-        for (size_t j = 0; j < N; j++) {
+			Type w[M] = {};
-            Type w[M];
+			w[0] = Type(1);
-            w[0] = Type(1);
-            // fill vector w
-            for (size_t i = j+1; i < M; i++) {
-                w[i-j] = _A(i,j);
-            }
-            Type tmp = Type(0);
-            for (size_t i = j; i < M; i++) {
-                tmp += w[i-j] * qtbv(i);
-            }
 
-            for (size_t i = j; i < M; i++) {
+			for (size_t i = j + 1; i < M; i++) {
-                qtbv(i) -= _tau(j) * w[i-j] * tmp;
+				w[i - j] = _A(i, j) / u1;
-            }
+				_A(i, j) = w[i - j];
-        }
+			}
-        return qtbv;
-    }
 
-    /**
+			_A(j, j) = s * normx;
-     * @brief Solve Ax=b for x
+			_tau(j) = -s * u1 / normx;
-     * @param b
-     * @return Vector x
-     *
-     * Find x in the equation Ax = b.
-     * A is provided in the initializer of the class.
-     */
-    Vector<Type, N> solve(const Vector<Type, M> &b) {
-        Vector<Type, M> qtbv = qtb(b);
-        Vector<Type, N> x;
 
-        // size_t is unsigned and wraps i = 0 - 1 to i > N
+			for (size_t k = j + 1; k < N; k++) {
-        for (size_t i = N - 1; i < N; i--) {
+				Type tmp = Type(0);
-            printf("i %d\n", static_cast<int>(i));
+
-            x(i) = qtbv(i);
+				for (size_t i = j; i < M; i++) {
-            for (size_t r = i+1; r < N; r++) {
+					tmp += w[i - j] * _A(i, k);
-                x(i) -= _A(i,r) * x(r);
+				}
-            }
+
-            // divide by zero, return vector of zeros
+				for (size_t i = j; i < M; i++) {
-            if (isEqualF(_A(i,i), Type(0), Type(1e-8))) {
+					_A(i, k) -= _tau(j) * w[i - j] * tmp;
-                for (size_t z = 0; z < N; z++) {
+				}
-                    x(z) = Type(0);
+			}
-                }
+
-                break;
+		}
-            }
+	}
-            x(i) /= _A(i,i);
+
-        }
+	/**
-        return x;
+	 * @brief qtb Calculate Q^T * b
-    }
+	 * @param b
+	 * @return Q^T*b
+	 *
+	 * This function calculates Q^T * b. This is useful for the solver
+	 * because R*x = Q^T*b.
+	 */
+	Vector<Type, M> qtb(const Vector<Type, M> &b)
+	{
+		Vector<Type, M> qtbv = b;
+
+		for (size_t j = 0; j < N; j++) {
+			Type w[M];
+			w[0] = Type(1);
+
+			// fill vector w
+			for (size_t i = j + 1; i < M; i++) {
+				w[i - j] = _A(i, j);
+			}
+
+			Type tmp = Type(0);
+
+			for (size_t i = j; i < M; i++) {
+				tmp += w[i - j] * qtbv(i);
+			}
+
+			for (size_t i = j; i < M; i++) {
+				qtbv(i) -= _tau(j) * w[i - j] * tmp;
+			}
+		}
+
+		return qtbv;
+	}
+
+	/**
+	 * @brief Solve Ax=b for x
+	 * @param b
+	 * @return Vector x
+	 *
+	 * Find x in the equation Ax = b.
+	 * A is provided in the initializer of the class.
+	 */
+	Vector<Type, N> solve(const Vector<Type, M> &b)
+	{
+		Vector<Type, M> qtbv = qtb(b);
+		Vector<Type, N> x;
+
+		// size_t is unsigned and wraps i = 0 - 1 to i > N
+		for (size_t i = N - 1; i < N; i--) {
+			printf("i %d\n", static_cast<int>(i));
+			x(i) = qtbv(i);
+
+			for (size_t r = i + 1; r < N; r++) {
+				x(i) -= _A(i, r) * x(r);
+			}
+
+			// divide by zero, return vector of zeros
+			if (isEqualF(_A(i, i), Type(0), Type(1e-8))) {
+				for (size_t z = 0; z < N; z++) {
+					x(z) = Type(0);
+				}
+
+				break;
+			}
+
+			x(i) /= _A(i, i);
+		}
+
+		return x;
+	}
 
 private:
-    Matrix<Type, M, N> _A;
+	Matrix<Type, M, N> _A;
-    Vector<Type, N> _tau;
+	Vector<Type, N> _tau;
 
 };
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/PseudoInverse.hpp

@@ -21,38 +21,44 @@ namespace matrix
  * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
  */
 template<typename Type, size_t M, size_t N>
-bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
+bool geninv(const Matrix<Type, M, N> &G, Matrix<Type, N, M> &res)
 {
-    size_t rank;
+	size_t rank;
-    if (M <= N) {
-        SquareMatrix<Type, M> A = G * G.transpose();
-        SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
 
-        A = L.transpose() * L;
+	if (M <= N) {
-        SquareMatrix<Type, M> X;
+		SquareMatrix<Type, M> A = G * G.transpose();
-        if (!inv(A, X, rank)) {
+		SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
-            res = Matrix<Type, N, M>();
-            return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
-        }
-        // doing an intermediate assignment reduces stack usage
-        A = X * X * L.transpose();
-        res = G.transpose() * (L * A);
 
-    } else {
+		A = L.transpose() * L;
-        SquareMatrix<Type, N> A = G.transpose() * G;
+		SquareMatrix<Type, M> X;
-        SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
 
-        A = L.transpose() * L;
+		if (!inv(A, X, rank)) {
-        SquareMatrix<Type, N> X;
+			res = Matrix<Type, N, M>();
-        if(!inv(A, X, rank)) {
+			return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
-            res = Matrix<Type, N, M>();
+		}
-            return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+
-        }
+		// doing an intermediate assignment reduces stack usage
-        // doing an intermediate assignment reduces stack usage
+		A = X * X * L.transpose();
-        A = X * X * L.transpose();
+		res = G.transpose() * (L * A);
-        res = (L * A) * G.transpose();
+
-    }
+	} else {
-    return true;
+		SquareMatrix<Type, N> A = G.transpose() * G;
+		SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
+
+		A = L.transpose() * L;
+		SquareMatrix<Type, N> X;
+
+		if (!inv(A, X, rank)) {
+			res = Matrix<Type, N, M>();
+			return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+		}
+
+		// doing an intermediate assignment reduces stack usage
+		A = X * X * L.transpose();
+		res = (L * A) * G.transpose();
+	}
+
+	return true;
 }
 
 
@@ -62,58 +68,60 @@ Type typeEpsilon();
 template<> inline
 float typeEpsilon<float>()
 {
-    return FLT_EPSILON;
+	return FLT_EPSILON;
 }
 
 /**
  * Full rank Cholesky factorization of A
  */
 template<typename Type, size_t N>
-SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
+SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> &A,
-                                       size_t& rank)
+				       size_t &rank)
 {
-    // Loses one ulp accuracy per row of diag, relative to largest magnitude
+	// Loses one ulp accuracy per row of diag, relative to largest magnitude
-    const Type tol = N * typeEpsilon<Type>() * A.diag().max();
+	const Type tol = N * typeEpsilon<Type>() * A.diag().max();
 
-    Matrix<Type, N, N> L;
+	Matrix<Type, N, N> L;
 
-    size_t r = 0;
+	size_t r = 0;
-    for (size_t k = 0; k < N; k++) {
 
-        if (r == 0) {
+	for (size_t k = 0; k < N; k++) {
-            for (size_t i = k; i < N; i++) {
-                L(i, r) = A(i, k);
-            }
 
-        } else {
+		if (r == 0) {
-            for (size_t i = k; i < N; i++) {
+			for (size_t i = k; i < N; i++) {
-                // Compute LL = L[k:n, :r] * L[k, :r].T
+				L(i, r) = A(i, k);
-                Type LL = Type();
+			}
-                for (size_t j = 0; j < r; j++) {
-                    LL += L(i, j) * L(k, j);
-                }
-                L(i, r) = A(i, k) - LL;
-            }
-        }
-        if (L(k, r) > tol) {
-            L(k, r) = sqrt(L(k, r));
 
-            if (k < N - 1) {
+		} else {
-                for (size_t i = k + 1; i < N; i++) {
+			for (size_t i = k; i < N; i++) {
-                    L(i, r) = L(i, r) / L(k, r);
+				// Compute LL = L[k:n, :r] * L[k, :r].T
-                }
+				Type LL = Type();
-            }
 
-            r = r + 1;
+				for (size_t j = 0; j < r; j++) {
-        }
+					LL += L(i, j) * L(k, j);
-    }
+				}
 
-    // Return rank
+				L(i, r) = A(i, k) - LL;
-    rank = r;
+			}
+		}
 
-    return L;
+		if (L(k, r) > tol) {
+			L(k, r) = sqrt(L(k, r));
+
+			if (k < N - 1) {
+				for (size_t i = k + 1; i < N; i++) {
+					L(i, r) = L(i, r) / L(k, r);
+				}
+			}
+
+			r = r + 1;
+		}
+	}
+
+	// Return rank
+	rank = r;
+
+	return L;
 }
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Scalar.hpp

@@ -17,36 +17,38 @@ template<typename Type>
 class Scalar
 {
 public:
-    Scalar() = delete;
+	Scalar() = delete;
 
-    Scalar(const Matrix<Type, 1, 1> & other) :
+	Scalar(const Matrix<Type, 1, 1> &other) :
-        _value{other(0,0)}
+		_value{other(0, 0)}
-    {
+	{
-    }
+	}
 
-    Scalar(Type other) : _value(other)
+	Scalar(Type other) : _value(other)
-    {
+	{
-    }
+	}
 
-    operator const Type &()
+	operator const Type &()
-    {
+	{
-        return _value;
+		return _value;
-    }
+	}
 
-    operator Matrix<Type, 1, 1>() const {
+	operator Matrix<Type, 1, 1>() const
-        Matrix<Type, 1, 1> m;
+	{
-        m(0, 0) = _value;
+		Matrix<Type, 1, 1> m;
-        return m;
+		m(0, 0) = _value;
-    }
+		return m;
+	}
 
-    operator Vector<Type, 1>() const {
+	operator Vector<Type, 1>() const
-        Vector<Type, 1> m;
+	{
-        m(0) = _value;
+		Vector<Type, 1> m;
-        return m;
+		m(0) = _value;
-    }
+		return m;
+	}
 
 private:
-    const Type _value;
+	const Type _value;
 
 };
 
@@ -54,5 +56,3 @@ using Scalarf = Scalar<float>;
 using Scalard = Scalar<double>;
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/SparseVector.hpp

@@ -12,175 +12,213 @@
 
 #include "math.hpp"
 
-namespace matrix {
+namespace matrix
+{
 template<int N> struct force_constexpr_eval {
-    static const int value = N;
+	static const int value = N;
 };
 
 // Vector that only store nonzero elements,
 // which indices are specified as parameter pack
 template<typename Type, size_t M, size_t... Idxs>
-class SparseVector {
+class SparseVector
+{
 private:
-    static constexpr size_t N = sizeof...(Idxs);
+	static constexpr size_t N = sizeof...(Idxs);
-    static constexpr size_t _indices[N] {Idxs...};
+	static constexpr size_t _indices[N] {Idxs...};
 
-    static constexpr bool duplicateIndices() {
+	static constexpr bool duplicateIndices()
-        for (size_t i = 0; i < N; i++) {
+	{
-            for (size_t j = 0; j < i; j++) {
+		for (size_t i = 0; i < N; i++) {
-                if (_indices[i] == _indices[j]) {
+			for (size_t j = 0; j < i; j++) {
-                    return true;
+				if (_indices[i] == _indices[j]) {
-                }
+					return true;
-            }
+				}
-        }
+			}
-        return false;
+		}
-    }
-    static constexpr size_t findMaxIndex() {
-        size_t maxIndex = 0;
-        for (size_t i = 0; i < N; i++) {
-            if (maxIndex < _indices[i]) {
-                maxIndex = _indices[i];
-            }
-        }
-        return maxIndex;
-    }
 
-    static_assert(!duplicateIndices(), "Duplicate indices");
+		return false;
-    static_assert(N < M, "More entries than elements, use a dense vector");
+	}
-    static_assert(N > 0, "A sparse vector needs at least one element");
+	static constexpr size_t findMaxIndex()
-    static_assert(findMaxIndex() < M, "Largest entry doesn't fit in sparse vector");
+	{
+		size_t maxIndex = 0;
 
-    Type _data[N] {};
+		for (size_t i = 0; i < N; i++) {
+			if (maxIndex < _indices[i]) {
+				maxIndex = _indices[i];
+			}
+		}
 
-    static constexpr int findCompressedIndex(size_t index) {
+		return maxIndex;
-        int compressedIndex = -1;
+	}
-        for (size_t i = 0; i < N; i++) {
+
-            if (index == _indices[i]) {
+	static_assert(!duplicateIndices(), "Duplicate indices");
-                compressedIndex = static_cast<int>(i);
+	static_assert(N < M, "More entries than elements, use a dense vector");
-            }
+	static_assert(N > 0, "A sparse vector needs at least one element");
-        }
+	static_assert(findMaxIndex() < M, "Largest entry doesn't fit in sparse vector");
-        return compressedIndex;
+
-    }
+	Type _data[N] {};
+
+	static constexpr int findCompressedIndex(size_t index)
+	{
+		int compressedIndex = -1;
+
+		for (size_t i = 0; i < N; i++) {
+			if (index == _indices[i]) {
+				compressedIndex = static_cast<int>(i);
+			}
+		}
+
+		return compressedIndex;
+	}
 
 public:
-    constexpr size_t non_zeros() const {
+	constexpr size_t non_zeros() const
-        return N;
+	{
-    }
+		return N;
+	}
 
-    constexpr size_t index(size_t i) const {
+	constexpr size_t index(size_t i) const
-        return SparseVector::_indices[i];
+	{
-    }
+		return SparseVector::_indices[i];
+	}
 
-    SparseVector() = default;
+	SparseVector() = default;
 
-    SparseVector(const matrix::Vector<Type, M>& data) {
+	SparseVector(const matrix::Vector<Type, M> &data)
-        for (size_t i = 0; i < N; i++) {
+	{
-            _data[i] = data(_indices[i]);
+		for (size_t i = 0; i < N; i++) {
-        }
+			_data[i] = data(_indices[i]);
-    }
+		}
+	}
 
-    explicit SparseVector(const Type data[N]) {
+	explicit SparseVector(const Type data[N])
-        memcpy(_data, data, sizeof(_data));
+	{
-    }
+		memcpy(_data, data, sizeof(_data));
+	}
 
-    template <size_t i>
+	template <size_t i>
-    inline Type at() const {
+	inline Type at() const
-        static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
+	{
-        static_assert(compressed_index >= 0, "cannot access unpopulated indices");
+		static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
-        return _data[compressed_index];
+		static_assert(compressed_index >= 0, "cannot access unpopulated indices");
-    }
+		return _data[compressed_index];
+	}
 
-    template <size_t i>
+	template <size_t i>
-    inline Type& at() {
+	inline Type &at()
-        static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
+	{
-        static_assert(compressed_index >= 0, "cannot access unpopulated indices");
+		static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
-        return _data[compressed_index];
+		static_assert(compressed_index >= 0, "cannot access unpopulated indices");
-    }
+		return _data[compressed_index];
+	}
 
-    inline Type atCompressedIndex(size_t i) const {
+	inline Type atCompressedIndex(size_t i) const
-        assert(i < N);
+	{
-        return _data[i];
+		assert(i < N);
-    }
+		return _data[i];
+	}
 
-    inline Type& atCompressedIndex(size_t i) {
+	inline Type &atCompressedIndex(size_t i)
-        assert(i < N);
+	{
-        return _data[i];
+		assert(i < N);
-    }
+		return _data[i];
+	}
 
-    void setZero() {
+	void setZero()
-        for (size_t i = 0; i < N; i++) {
+	{
-            _data[i] = Type(0);
+		for (size_t i = 0; i < N; i++) {
-        }
+			_data[i] = Type(0);
-    }
+		}
+	}
 
-    Type dot(const matrix::Vector<Type, M>& other) const {
+	Type dot(const matrix::Vector<Type, M> &other) const
-        Type accum (0);
+	{
-        for (size_t i = 0; i < N; i++) {
+		Type accum(0);
-            accum += _data[i] * other(_indices[i]);
-        }
-        return accum;
-    }
 
-    matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M>& other) const {
+		for (size_t i = 0; i < N; i++) {
-        matrix::Vector<Type, M> vec = other;
+			accum += _data[i] * other(_indices[i]);
-        for (size_t i = 0; i < N; i++) {
+		}
-            vec(_indices[i]) +=  _data[i];
-        }
-        return vec;
-    }
 
-    SparseVector& operator+=(Type t) {
+		return accum;
-        for (size_t i = 0; i < N; i++) {
+	}
-            _data[i] += t;
-        }
-        return *this;
-    }
 
-    Type norm_squared() const
+	matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M> &other) const
-    {
+	{
-        Type accum(0);
+		matrix::Vector<Type, M> vec = other;
-        for (size_t i = 0; i < N; i++) {
-            accum += _data[i] * _data[i];
-        }
-        return accum;
-    }
 
-    Type norm() const
+		for (size_t i = 0; i < N; i++) {
-    {
+			vec(_indices[i]) +=  _data[i];
-        return matrix::sqrt(norm_squared());
+		}
-    }
 
-    bool longerThan(Type testVal) const
+		return vec;
-    {
+	}
-        return norm_squared() > testVal*testVal;
+
-    }
+	SparseVector &operator+=(Type t)
+	{
+		for (size_t i = 0; i < N; i++) {
+			_data[i] += t;
+		}
+
+		return *this;
+	}
+
+	Type norm_squared() const
+	{
+		Type accum(0);
+
+		for (size_t i = 0; i < N; i++) {
+			accum += _data[i] * _data[i];
+		}
+
+		return accum;
+	}
+
+	Type norm() const
+	{
+		return matrix::sqrt(norm_squared());
+	}
+
+	bool longerThan(Type testVal) const
+	{
+		return norm_squared() > testVal * testVal;
+	}
 };
 
 template<typename Type, size_t Q, size_t M, size_t ... Idxs>
-matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const matrix::SparseVector<Type, M, Idxs...>& vec) {
+matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M> &mat,
-    matrix::Vector<Type, Q> res;
+				  const matrix::SparseVector<Type, M, Idxs...> &vec)
-    for (size_t i = 0; i < Q; i++) {
+{
-        const Vector<Type, M> row = mat.row(i);
+	matrix::Vector<Type, Q> res;
-        res(i) = vec.dot(row);
+
-    }
+	for (size_t i = 0; i < Q; i++) {
-    return res;
+		const Vector<Type, M> row = mat.row(i);
+		res(i) = vec.dot(row);
+	}
+
+	return res;
 }
 
 // returns x.T * A * x
 template<typename Type, size_t M, size_t ... Idxs>
-Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& x) {
+Type quadraticForm(const matrix::SquareMatrix<Type, M> &A, const matrix::SparseVector<Type, M, Idxs...> &x)
-    Type res = Type(0);
+{
-    for (size_t i = 0; i < x.non_zeros(); i++) {
+	Type res = Type(0);
-        Type tmp = Type(0);
+
-        for (size_t j = 0; j < x.non_zeros(); j++) {
+	for (size_t i = 0; i < x.non_zeros(); i++) {
-            tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
+		Type tmp = Type(0);
-        }
+
-        res += x.atCompressedIndex(i) * tmp;
+		for (size_t j = 0; j < x.non_zeros(); j++) {
-    }
+			tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
-    return res;
+		}
+
+		res += x.atCompressedIndex(i) * tmp;
+	}
+
+	return res;
 }
 
-template<typename Type,size_t M, size_t... Idxs>
+template<typename Type, size_t M, size_t... Idxs>
 constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];
 
 template<size_t M, size_t ... Idxs>

src/lib/matrix/matrix/Vector.hpp

@@ -20,117 +20,134 @@ template<typename Type, size_t M>
 class Vector : public Matrix<Type, M, 1>
 {
 public:
-    using MatrixM1 = Matrix<Type, M, 1>;
+	using MatrixM1 = Matrix<Type, M, 1>;
 
-    Vector() = default;
+	Vector() = default;
 
-    Vector(const MatrixM1 & other) :
+	Vector(const MatrixM1 &other) :
-        MatrixM1(other)
+		MatrixM1(other)
-    {
+	{
-    }
+	}
 
-    explicit Vector(const Type data_[M]) :
+	explicit Vector(const Type data_[M]) :
-        MatrixM1(data_)
+		MatrixM1(data_)
-    {
+	{
-    }
+	}
 
-    template<size_t P, size_t Q>
+	template<size_t P, size_t Q>
-    Vector(const Slice<Type, M, 1, P, Q>& slice_in) :
+	Vector(const Slice<Type, M, 1, P, Q> &slice_in) :
-        Matrix<Type, M, 1>(slice_in)
+		Matrix<Type, M, 1>(slice_in)
-    {
+	{
-    }
+	}
 
-    template<size_t P, size_t Q, size_t DUMMY = 1>
+	template<size_t P, size_t Q, size_t DUMMY = 1>
-    Vector(const Slice<Type, 1, M, P, Q>& slice_in)
+	Vector(const Slice<Type, 1, M, P, Q> &slice_in)
-    {
+	{
-        Vector &self(*this);
+		Vector &self(*this);
-        for (size_t i = 0; i<M; i++) {
-            self(i) = slice_in(0, i);
-        }
-    }
 
-    inline const Type &operator()(size_t i) const
+		for (size_t i = 0; i < M; i++) {
-    {
+			self(i) = slice_in(0, i);
-        assert(i < M);
+		}
+	}
 
-        const MatrixM1 &v = *this;
+	inline const Type &operator()(size_t i) const
-        return v(i, 0);
+	{
-    }
+		assert(i < M);
 
-    inline Type &operator()(size_t i)
+		const MatrixM1 &v = *this;
-    {
+		return v(i, 0);
-        assert(i < M);
+	}
 
-        MatrixM1 &v = *this;
+	inline Type &operator()(size_t i)
-        return v(i, 0);
+	{
-    }
+		assert(i < M);
 
-    Type dot(const MatrixM1 & b) const {
+		MatrixM1 &v = *this;
-        const Vector &a(*this);
+		return v(i, 0);
-        Type r(0);
+	}
-        for (size_t i = 0; i<M; i++) {
-            r += a(i)*b(i,0);
-        }
-        return r;
-    }
 
-    inline Type operator*(const MatrixM1 & b) const {
+	Type dot(const MatrixM1 &b) const
-        const Vector &a(*this);
+	{
-        return a.dot(b);
+		const Vector &a(*this);
-    }
+		Type r(0);
 
-    inline Vector operator*(Type b) const {
+		for (size_t i = 0; i < M; i++) {
-        return Vector(MatrixM1::operator*(b));
+			r += a(i) * b(i, 0);
-    }
+		}
 
-    Type norm() const {
+		return r;
-        const Vector &a(*this);
+	}
-        return Type(matrix::sqrt(a.dot(a)));
-    }
 
-    Type norm_squared() const {
+	inline Type operator*(const MatrixM1 &b) const
-        const Vector &a(*this);
+	{
-        return a.dot(a);
+		const Vector &a(*this);
-    }
+		return a.dot(b);
+	}
 
-    inline Type length() const {
+	inline Vector operator*(Type b) const
-        return norm();
+	{
-    }
+		return Vector(MatrixM1::operator*(b));
+	}
 
-    inline void normalize() {
+	Type norm() const
-        (*this) /= norm();
+	{
-    }
+		const Vector &a(*this);
+		return Type(matrix::sqrt(a.dot(a)));
+	}
 
-    Vector unit() const {
+	Type norm_squared() const
-        return (*this) / norm();
+	{
-    }
+		const Vector &a(*this);
+		return a.dot(a);
+	}
 
-    Vector unit_or_zero(const Type eps = Type(1e-5)) const {
+	inline Type length() const
-        const Type n = norm();
+	{
-        if (n > eps) {
+		return norm();
-            return (*this) / n;
+	}
-        }
-        return Vector();
-    }
 
-    inline Vector normalized() const {
+	inline void normalize()
-        return unit();
+	{
-    }
+		(*this) /= norm();
+	}
 
-    bool longerThan(Type testVal) const {
+	Vector unit() const
-        return norm_squared() > testVal*testVal;
+	{
-    }
+		return (*this) / norm();
+	}
 
-    Vector sqrt() const {
+	Vector unit_or_zero(const Type eps = Type(1e-5)) const
-        const Vector &a(*this);
+	{
-        Vector r;
+		const Type n = norm();
-        for (size_t i = 0; i<M; i++) {
+
-            r(i) = Type(matrix::sqrt(a(i)));
+		if (n > eps) {
-        }
+			return (*this) / n;
-        return r;
+		}
-    }
+
+		return Vector();
+	}
+
+	inline Vector normalized() const
+	{
+		return unit();
+	}
+
+	bool longerThan(Type testVal) const
+	{
+		return norm_squared() > testVal * testVal;
+	}
+
+	Vector sqrt() const
+	{
+		const Vector &a(*this);
+		Vector r;
+
+		for (size_t i = 0; i < M; i++) {
+			r(i) = Type(matrix::sqrt(a(i)));
+		}
+
+		return r;
+	}
 };
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Vector2.hpp

@@ -21,53 +21,55 @@ class Vector2 : public Vector<Type, 2>
 {
 public:
 
-    using Matrix21 = Matrix<Type, 2, 1>;
+	using Matrix21 = Matrix<Type, 2, 1>;
-    using Vector3 = Vector<Type, 3>;
+	using Vector3 = Vector<Type, 3>;
 
-    Vector2() = default;
+	Vector2() = default;
 
-    Vector2(const Matrix21 & other) :
+	Vector2(const Matrix21 &other) :
-        Vector<Type, 2>(other)
+		Vector<Type, 2>(other)
-    {
+	{
-    }
+	}
 
-    explicit Vector2(const Type data_[2]) :
+	explicit Vector2(const Type data_[2]) :
-        Vector<Type, 2>(data_)
+		Vector<Type, 2>(data_)
-    {
+	{
-    }
+	}
 
-    Vector2(Type x, Type y)
+	Vector2(Type x, Type y)
-    {
+	{
-        Vector2 &v(*this);
+		Vector2 &v(*this);
-        v(0) = x;
+		v(0) = x;
-        v(1) = y;
+		v(1) = y;
-    }
+	}
 
-    template<size_t P, size_t Q>
+	template<size_t P, size_t Q>
-    Vector2(const Slice<Type, 2, 1, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
+	Vector2(const Slice<Type, 2, 1, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
-    {
+	{
-    }
+	}
 
-    template<size_t P, size_t Q>
+	template<size_t P, size_t Q>
-    Vector2(const Slice<Type, 1, 2, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
+	Vector2(const Slice<Type, 1, 2, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
-    {
+	{
-    }
+	}
 
-    explicit Vector2(const Vector3 & other)
+	explicit Vector2(const Vector3 &other)
-    {
+	{
-        Vector2 &v(*this);
+		Vector2 &v(*this);
-        v(0) = other(0);
+		v(0) = other(0);
-        v(1) = other(1);
+		v(1) = other(1);
-    }
+	}
 
-    Type cross(const Matrix21 & b) const {
+	Type cross(const Matrix21 &b) const
-        const Vector2 &a(*this);
+	{
-        return a(0)*b(1, 0) - a(1)*b(0, 0);
+		const Vector2 &a(*this);
-    }
+		return a(0) * b(1, 0) - a(1) * b(0, 0);
+	}
 
-    Type operator%(const Matrix21 & b) const {
+	Type operator%(const Matrix21 &b) const
-        return (*this).cross(b);
+	{
-    }
+		return (*this).cross(b);
+	}
 
 };
 
@@ -76,5 +78,3 @@ using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Vector3.hpp

@@ -30,121 +30,127 @@ class Vector3 : public Vector<Type, 3>
 {
 public:
 
-    using Matrix31 = Matrix<Type, 3, 1>;
+	using Matrix31 = Matrix<Type, 3, 1>;
 
-    Vector3() = default;
+	Vector3() = default;
 
-    Vector3(const Matrix31 & other) :
+	Vector3(const Matrix31 &other) :
-        Vector<Type, 3>(other)
+		Vector<Type, 3>(other)
-    {
+	{
-    }
+	}
 
-    explicit Vector3(const Type data_[3]) :
+	explicit Vector3(const Type data_[3]) :
-        Vector<Type, 3>(data_)
+		Vector<Type, 3>(data_)
-    {
+	{
-    }
+	}
 
-    Vector3(Type x, Type y, Type z) {
+	Vector3(Type x, Type y, Type z)
-        Vector3 &v(*this);
+	{
-        v(0) = x;
+		Vector3 &v(*this);
-        v(1) = y;
+		v(0) = x;
-        v(2) = z;
+		v(1) = y;
-    }
+		v(2) = z;
+	}
 
-    template<size_t P, size_t Q>
+	template<size_t P, size_t Q>
-    Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
+	Vector3(const Slice<Type, 3, 1, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
-    {
+	{
-    }
+	}
 
-    template<size_t P, size_t Q>
+	template<size_t P, size_t Q>
-    Vector3(const Slice<Type, 1, 3, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
+	Vector3(const Slice<Type, 1, 3, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
-    {
+	{
-    }
+	}
 
-    Vector3 cross(const Matrix31 & b) const {
+	Vector3 cross(const Matrix31 &b) const
-        const Vector3 &a(*this);
+	{
-        return {a(1)*b(2,0) - a(2)*b(1,0), -a(0)*b(2,0) + a(2)*b(0,0), a(0)*b(1,0) - a(1)*b(0,0)};
+		const Vector3 &a(*this);
-    }
+		return {a(1) *b(2, 0) - a(2) *b(1, 0), -a(0) *b(2, 0) + a(2) *b(0, 0), a(0) *b(1, 0) - a(1) *b(0, 0)};
+	}
 
-    /**
+	/**
-     * Override matrix ops so Vector3 type is returned
+	 * Override matrix ops so Vector3 type is returned
-     */
+	 */
 
-    inline Vector3 operator+(Vector3 other) const
+	inline Vector3 operator+(Vector3 other) const
-    {
+	{
-        return Matrix31::operator+(other);
+		return Matrix31::operator+(other);
-    }
+	}
 
-    inline Vector3 operator+(Type scalar) const
+	inline Vector3 operator+(Type scalar) const
-    {
+	{
-        return Matrix31::operator+(scalar);
+		return Matrix31::operator+(scalar);
-    }
+	}
 
-    inline Vector3 operator-(Vector3 other) const
+	inline Vector3 operator-(Vector3 other) const
-    {
+	{
-        return Matrix31::operator-(other);
+		return Matrix31::operator-(other);
-    }
+	}
 
-    inline Vector3 operator-(Type scalar) const
+	inline Vector3 operator-(Type scalar) const
-    {
+	{
-        return Matrix31::operator-(scalar);
+		return Matrix31::operator-(scalar);
-    }
+	}
 
-    inline Vector3 operator-() const
+	inline Vector3 operator-() const
-    {
+	{
-        return Matrix31::operator-();
+		return Matrix31::operator-();
-    }
+	}
 
-    inline Vector3 operator*(Type scalar) const
+	inline Vector3 operator*(Type scalar) const
-    {
+	{
-        return Matrix31::operator*(scalar);
+		return Matrix31::operator*(scalar);
-    }
+	}
 
-    inline Type operator*(Vector3 b) const
+	inline Type operator*(Vector3 b) const
-    {
+	{
-        return Vector<Type, 3>::operator*(b);
+		return Vector<Type, 3>::operator*(b);
-    }
+	}
 
-    inline Vector3 operator%(const Matrix31 & b) const {
+	inline Vector3 operator%(const Matrix31 &b) const
-        return (*this).cross(b);
+	{
-    }
+		return (*this).cross(b);
+	}
 
-    /**
+	/**
-     * Override vector ops so Vector3 type is returned
+	 * Override vector ops so Vector3 type is returned
-     */
+	 */
-    inline Vector3 unit() const {
+	inline Vector3 unit() const
-        return Vector3(Vector<Type, 3>::unit());
+	{
-    }
+		return Vector3(Vector<Type, 3>::unit());
+	}
 
-    inline Vector3 normalized() const {
+	inline Vector3 normalized() const
-        return unit();
+	{
-    }
+		return unit();
+	}
 
-    const Slice<Type, 2, 1, 3, 1> xy() const
+	const Slice<Type, 2, 1, 3, 1> xy() const
-    {
+	{
-        return Slice<Type, 2, 1, 3, 1>(0, 0, this);
+		return Slice<Type, 2, 1, 3, 1>(0, 0, this);
-    }
+	}
 
-    Slice<Type, 2, 1, 3, 1> xy()
+	Slice<Type, 2, 1, 3, 1> xy()
-    {
+	{
-        return Slice<Type, 2, 1, 3, 1>(0, 0, this);
+		return Slice<Type, 2, 1, 3, 1>(0, 0, this);
-    }
+	}
 
 
-    Dcm<Type> hat() const {    // inverse to Dcm.vee() operation
+	Dcm<Type> hat() const      // inverse to Dcm.vee() operation
-        const Vector3 &v(*this);
+	{
-        Dcm<Type> A;
+		const Vector3 &v(*this);
-        A(0,0) = 0;
+		Dcm<Type> A;
-        A(0,1) = -v(2);
+		A(0, 0) = 0;
-        A(0,2) = v(1);
+		A(0, 1) = -v(2);
-        A(1,0) = v(2);
+		A(0, 2) = v(1);
-        A(1,1) = 0;
+		A(1, 0) = v(2);
-        A(1,2) = -v(0);
+		A(1, 1) = 0;
-        A(2,0) = -v(1);
+		A(1, 2) = -v(0);
-        A(2,1) = v(0);
+		A(2, 0) = -v(1);
-        A(2,2) = 0;
+		A(2, 1) = v(0);
-        return A;
+		A(2, 2) = 0;
-    }
+		return A;
+	}
 
 };
 
@@ -152,5 +158,3 @@ using Vector3f = Vector3<float>;
 using Vector3d = Vector3<double>;
 
 } // namespace matrix
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/filter.hpp

@@ -2,25 +2,26 @@
 
 #include "math.hpp"
 
-namespace matrix {
+namespace matrix
+{
 
 template<typename Type, size_t M, size_t N>
 int kalman_correct(
-    const Matrix<Type, M, M> & P,
+	const Matrix<Type, M, M> &P,
-    const Matrix<Type, N, M> & C,
+	const Matrix<Type, N, M> &C,
-    const Matrix<Type, N, N> & R,
+	const Matrix<Type, N, N> &R,
-    const Matrix<Type, N, 1> &r,
+	const Matrix<Type, N, 1> &r,
-    Matrix<Type, M, 1> & dx,
+	Matrix<Type, M, 1> &dx,
-    Matrix<Type, M, M> & dP,
+	Matrix<Type, M, M> &dP,
-    Type & beta
+	Type &beta
 )
 {
-    SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
+	SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C * P * C.T() + R).I();
-    Matrix<Type, M, N> K = P*C.T()*S_I;
+	Matrix<Type, M, N> K = P * C.T() * S_I;
-    dx = K*r;
+	dx = K * r;
-    beta = Scalar<Type>(r.T()*S_I*r);
+	beta = Scalar<Type>(r.T() * S_I * r);
-    dP = K*C*P*(-1);
+	dP = K * C * P * (-1);
-    return 0;
+	return 0;
 }
 
 } // namespace matrix

src/lib/matrix/matrix/helper_functions.hpp

@@ -10,13 +10,14 @@ namespace matrix
 {
 
 template<typename Type>
-bool is_finite(Type x) {
+bool is_finite(Type x)
+{
 #if defined (__PX4_NUTTX)
-    return PX4_ISFINITE(x);
+	return PX4_ISFINITE(x);
 #elif defined (__PX4_QURT)
-    return __builtin_isfinite(x);
+	return __builtin_isfinite(x);
 #else
-    return std::isfinite(x);
+	return std::isfinite(x);
 #endif
 }
 
@@ -34,25 +35,26 @@ bool is_finite(Type x) {
 template<typename Type>
 bool isEqualF(const Type x, const Type y, const Type eps = Type(1e-4f))
 {
-    return (matrix::fabs(x - y) <= eps)
+	return (matrix::fabs(x - y) <= eps)
-           || (isnan(x) && isnan(y))
+	       || (isnan(x) && isnan(y))
-           || (isinf(x) && isinf(y) && isnan(x - y));
+	       || (isinf(x) && isinf(y) && isnan(x - y));
 }
 
 namespace detail
 {
 
 template<typename Floating>
-Floating wrap_floating(Floating x, Floating low, Floating high) {
+Floating wrap_floating(Floating x, Floating low, Floating high)
-    // already in range
+{
-    if (low <= x && x < high) {
+	// already in range
-        return x;
+	if (low <= x && x < high) {
-    }
+		return x;
+	}
 
-    const auto range = high - low;
+	const auto range = high - low;
-    const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime
+	const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime
-    const auto num_wraps = floor((x - low) * inv_range);
+	const auto num_wraps = floor((x - low) * inv_range);
-    return x - range * num_wraps;
+	return x - range * num_wraps;
 }
 
 }  // namespace detail
@@ -65,8 +67,9 @@ Floating wrap_floating(Floating x, Floating low, Floating high) {
  * @param high upper limit of the allowed range
  * @return wrapped value inside the range
  */
-inline float wrap(float x, float low, float high) {
+inline float wrap(float x, float low, float high)
-    return matrix::detail::wrap_floating(x, low, high);
+{
+	return matrix::detail::wrap_floating(x, low, high);
 }
 
 /**
@@ -77,8 +80,9 @@ inline float wrap(float x, float low, float high) {
  * @param high upper limit of the allowed range
  * @return wrapped value inside the range
  */
-inline double wrap(double x, double low, double high) {
+inline double wrap(double x, double low, double high)
-    return matrix::detail::wrap_floating(x, low, high);
+{
+	return matrix::detail::wrap_floating(x, low, high);
 }
 
 /**
@@ -90,14 +94,15 @@ inline double wrap(double x, double low, double high) {
  * @return wrapped value inside the range
  */
 template<typename Integer>
-Integer wrap(Integer x, Integer low, Integer high) {
+Integer wrap(Integer x, Integer low, Integer high)
-    const auto range = high - low;
+{
+	const auto range = high - low;
 
-    if (x < low) {
+	if (x < low) {
-        x += range * ((low - x) / range + 1);
+		x += range * ((low - x) / range + 1);
-    }
+	}
 
-    return low + (x - low) % range;
+	return low + (x - low) % range;
 }
 
 /**
@@ -106,7 +111,7 @@ Integer wrap(Integer x, Integer low, Integer high) {
 template<typename Type>
 Type wrap_pi(Type x)
 {
-    return wrap(x, Type(-M_PI), Type(M_PI));
+	return wrap(x, Type(-M_PI), Type(M_PI));
 }
 
 /**
@@ -115,13 +120,13 @@ Type wrap_pi(Type x)
 template<typename Type>
 Type wrap_2pi(Type x)
 {
-    return wrap(x, Type(0), Type(M_TWOPI));
+	return wrap(x, Type(0), Type(M_TWOPI));
 }
 
 template<typename T>
 int sign(T val)
 {
-    return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
+	return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
 }
 
 } // namespace matrix

src/lib/matrix/matrix/integration.hpp

@@ -2,41 +2,45 @@
 
 #include "math.hpp"
 
-namespace matrix {
+namespace matrix
+{
 
 template<typename Type, size_t M, size_t N>
 int integrate_rk4(
-    Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> & u),
+	Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> &u),
-    const Matrix<Type, M, 1> & y0,
+	const Matrix<Type, M, 1> &y0,
-    const Matrix<Type, N, 1> & u,
+	const Matrix<Type, N, 1> &u,
-    Type t0,
+	Type t0,
-    Type tf,
+	Type tf,
-    Type h0,
+	Type h0,
-    Matrix<Type, M, 1> & y1
+	Matrix<Type, M, 1> &y1
 )
 {
-    // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
+	// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
-    Type t1 = t0;
+	Type t1 = t0;
-    y1 = y0;
+	y1 = y0;
-    Type h = h0;
+	Type h = h0;
-    Vector<Type, M> k1, k2, k3, k4;
+	Vector<Type, M> k1, k2, k3, k4;
-    if (tf < t0) return -1; // make sure t1 > t0
+
-    while (t1 < tf) {
+	if (tf < t0) { return -1; } // make sure t1 > t0
-        if (t1 + h0 < tf) {
+
-            h = h0;
+	while (t1 < tf) {
-        } else {
+		if (t1 + h0 < tf) {
-            h = tf - t1;
+			h = h0;
-        }
+
-        k1 = f(t1, y1, u);
+		} else {
-        k2 = f(t1 + h/2, y1 + k1*h/2, u);
+			h = tf - t1;
-        k3 = f(t1 + h/2, y1 + k2*h/2, u);
+		}
-        k4 = f(t1 + h, y1 + k3*h, u);
+
-        y1 += (k1 + k2*2 + k3*2 + k4)*(h/6);
+		k1 = f(t1, y1, u);
-        t1 += h;
+		k2 = f(t1 + h / 2, y1 + k1 * h / 2, u);
-    }
+		k3 = f(t1 + h / 2, y1 + k2 * h / 2, u);
-    return 0;
+		k4 = f(t1 + h, y1 + k3 * h, u);
+		y1 += (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
+		t1 += h;
+	}
+
+	return 0;
 }
 
 } // namespace matrix
-
-// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :

src/lib/matrix/matrix/stdlib_imports.hpp

@@ -20,7 +20,8 @@
 #define M_TWOPI (M_PI * 2.0)
 #endif
 
-namespace matrix {
+namespace matrix
+{
 
 #if !defined(FLT_EPSILON)
 #define FLT_EPSILON     __FLT_EPSILON__

src/lib/matrix/test/copyto.cpp

@@ -3,70 +3,76 @@
 
 using namespace matrix;
 
-namespace {
+namespace
-void doTheCopy(const Matrix<float, 2, 3>& A, float array_A[6])
 {
-    A.copyTo(array_A);
+void doTheCopy(const Matrix<float, 2, 3> &A, float array_A[6])
+{
+	A.copyTo(array_A);
 }
 }
 
 int main()
 {
-    float eps = 1e-6f;
+	float eps = 1e-6f;
 
-    // Vector3 copyTo
+	// Vector3 copyTo
-    const Vector3f v(1, 2, 3);
+	const Vector3f v(1, 2, 3);
-    float dst3[3] = {};
+	float dst3[3] = {};
-    v.copyTo(dst3);
+	v.copyTo(dst3);
-    for (size_t i = 0; i < 3; i++) {
-        TEST(fabs(v(i) - dst3[i]) < eps);
-    }
 
-    // Quaternion copyTo
+	for (size_t i = 0; i < 3; i++) {
-    Quatf q(1, 2, 3, 4);
+		TEST(fabs(v(i) - dst3[i]) < eps);
-    float dst4[4] = {};
+	}
-    q.copyTo(dst4);
-    for (size_t i = 0; i < 4; i++) {
-        TEST(fabs(q(i) - dst4[i]) < eps);
-    }
 
-    // Matrix copyTo
+	// Quaternion copyTo
-    Matrix<float, 2, 3> A;
+	Quatf q(1, 2, 3, 4);
-    A(0,0) = 1;
+	float dst4[4] = {};
-    A(0,1) = 2;
+	q.copyTo(dst4);
-    A(0,2) = 3;
-    A(1,0) = 4;
-    A(1,1) = 5;
-    A(1,2) = 6;
-    float array_A[6] = {};
-    doTheCopy(A, array_A);
-    float array_row[6] = {1, 2, 3, 4, 5, 6};
-    for (size_t i = 0; i < 6; i++) {
-        TEST(fabs(array_A[i] - array_row[i]) < eps);
-    }
 
-    // Matrix copyToColumnMajor
+	for (size_t i = 0; i < 4; i++) {
-    A.copyToColumnMajor(array_A);
+		TEST(fabs(q(i) - dst4[i]) < eps);
-    float array_column[6] = {1, 4, 2, 5, 3, 6};
+	}
-    for (size_t i = 0; i < 6; i++) {
-        TEST(fabs(array_A[i] - array_column[i]) < eps);
-    }
 
-    // Slice copyTo
+	// Matrix copyTo
-    float dst5[2] = {};
+	Matrix<float, 2, 3> A;
-    v.slice<2,1>(0,0).copyTo(dst5);
+	A(0, 0) = 1;
-    for (size_t i = 0; i < 2; i++) {
+	A(0, 1) = 2;
-        TEST(fabs(v(i) - dst5[i]) < eps);
+	A(0, 2) = 3;
-    }
+	A(1, 0) = 4;
+	A(1, 1) = 5;
+	A(1, 2) = 6;
+	float array_A[6] = {};
+	doTheCopy(A, array_A);
+	float array_row[6] = {1, 2, 3, 4, 5, 6};
 
-    float subarray_A[4] = {};
+	for (size_t i = 0; i < 6; i++) {
-    A.slice<2,2>(0,0).copyToColumnMajor(subarray_A);
+		TEST(fabs(array_A[i] - array_row[i]) < eps);
-    float subarray_column[4] = {1,4,2,5};
+	}
-    for (size_t i = 0; i < 4; i++) {
-        TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
-    }
 
-    return 0;
+	// Matrix copyToColumnMajor
+	A.copyToColumnMajor(array_A);
+	float array_column[6] = {1, 4, 2, 5, 3, 6};
+
+	for (size_t i = 0; i < 6; i++) {
+		TEST(fabs(array_A[i] - array_column[i]) < eps);
+	}
+
+	// Slice copyTo
+	float dst5[2] = {};
+	v.slice<2, 1>(0, 0).copyTo(dst5);
+
+	for (size_t i = 0; i < 2; i++) {
+		TEST(fabs(v(i) - dst5[i]) < eps);
+	}
+
+	float subarray_A[4] = {};
+	A.slice<2, 2>(0, 0).copyToColumnMajor(subarray_A);
+	float subarray_column[4] = {1, 4, 2, 5};
+
+	for (size_t i = 0; i < 4; i++) {
+		TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
+	}
+
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/filter.cpp

@@ -5,25 +5,24 @@ using namespace matrix;
 
 int main()
 {
-    const size_t n_x = 6;
+	const size_t n_x = 6;
-    const size_t n_y = 5;
+	const size_t n_y = 5;
-    SquareMatrix<float, n_x> P = eye<float, n_x>();
+	SquareMatrix<float, n_x> P = eye<float, n_x>();
-    SquareMatrix<float, n_y> R = eye<float, n_y>();
+	SquareMatrix<float, n_y> R = eye<float, n_y>();
-    Matrix<float, n_y, n_x> C;
+	Matrix<float, n_y, n_x> C;
-    C.setIdentity();
+	C.setIdentity();
-    float data[] = {1,2,3,4,5};
+	float data[] = {1, 2, 3, 4, 5};
-    Vector<float, n_y> r(data);
+	Vector<float, n_y> r(data);
 
-    Vector<float, n_x> dx;
+	Vector<float, n_x> dx;
-    SquareMatrix<float, n_x> dP;
+	SquareMatrix<float, n_x> dP;
-    float beta = 0;
+	float beta = 0;
-    kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
+	kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
 
-    float data_check[] = {0.5,1,1.5,2,2.5,0};
+	float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
-    Vector<float, n_x> dx_check(data_check);
+	Vector<float, n_x> dx_check(data_check);
-    TEST(isEqual(dx, dx_check));
+	TEST(isEqual(dx, dx_check));
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/hatvee.cpp

@@ -5,15 +5,14 @@ using namespace matrix;
 
 int main()
 {
-    Euler<float> euler(0.1f, 0.2f, 0.3f);
+	Euler<float> euler(0.1f, 0.2f, 0.3f);
-    Dcm<float> R(euler);
+	Dcm<float> R(euler);
-    Dcm<float> skew = R - R.T();
+	Dcm<float> skew = R - R.T();
-    Vector3<float> w = skew.vee();
+	Vector3<float> w = skew.vee();
-    Vector3<float> w_check(0.1348f, 0.4170f, 0.5647f);
+	Vector3<float> w_check(0.1348f, 0.4170f, 0.5647f);
 
-    TEST(isEqual(w, w_check));
+	TEST(isEqual(w, w_check));
-    TEST(isEqual(skew, w.hat()));
+	TEST(isEqual(skew, w.hat()));
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/helper.cpp

@@ -5,74 +5,73 @@ using namespace matrix;
 
 int main()
 {
-    // general wraps
+	// general wraps
-    TEST(fabs(wrap(4., 0., 10.) - 4.) < FLT_EPSILON);
+	TEST(fabs(wrap(4., 0., 10.) - 4.) < FLT_EPSILON);
-    TEST(fabs(wrap(4., 0., 1.)) < FLT_EPSILON);
+	TEST(fabs(wrap(4., 0., 1.)) < FLT_EPSILON);
-    TEST(fabs(wrap(-4., 0., 10.) - 6.) < FLT_EPSILON);
+	TEST(fabs(wrap(-4., 0., 10.) - 6.) < FLT_EPSILON);
-    TEST(fabs(wrap(-18., 0., 10.) - 2.) < FLT_EPSILON);
+	TEST(fabs(wrap(-18., 0., 10.) - 2.) < FLT_EPSILON);
-    TEST(fabs(wrap(-1.5, 3., 5.) - 4.5) < FLT_EPSILON);
+	TEST(fabs(wrap(-1.5, 3., 5.) - 4.5) < FLT_EPSILON);
-    TEST(fabs(wrap(15.5, 3., 5.) - 3.5) < FLT_EPSILON);
+	TEST(fabs(wrap(15.5, 3., 5.) - 3.5) < FLT_EPSILON);
-    TEST(fabs(wrap(-1., 30., 40.) - 39.) < FLT_EPSILON);
+	TEST(fabs(wrap(-1., 30., 40.) - 39.) < FLT_EPSILON);
-    TEST(fabs(wrap(-8000., -555., 1.) - (-216.)) < FLT_EPSILON);
+	TEST(fabs(wrap(-8000., -555., 1.) - (-216.)) < FLT_EPSILON);
-    TEST(fabs(wrap(0., 0., 360.)) < FLT_EPSILON);
+	TEST(fabs(wrap(0., 0., 360.)) < FLT_EPSILON);
-    TEST(fabs(wrap(0. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
+	TEST(fabs(wrap(0. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
-    TEST(fabs(wrap(0. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
+	TEST(fabs(wrap(0. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
-    TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
+	TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
-    TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
+	TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
-    TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
+	TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
 
-    // integer wraps
+	// integer wraps
-    TEST(wrap(-10, 0, 10) == 0);
+	TEST(wrap(-10, 0, 10) == 0);
-    TEST(wrap(-4, 0, 10) == 6);
+	TEST(wrap(-4, 0, 10) == 6);
-    TEST(wrap(0, 0, 10) == 0)
+	TEST(wrap(0, 0, 10) == 0)
-    TEST(wrap(4, 0, 10) == 4);
+	TEST(wrap(4, 0, 10) == 4);
-    TEST(wrap(10, 0, 10) == 0);
+	TEST(wrap(10, 0, 10) == 0);
 
-    // wrap pi
+	// wrap pi
-    TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);
-    TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON);
-    TEST(fabs(wrap_pi(-4.) - (-4. + M_TWOPI)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(-4.) - (-4. + M_TWOPI)) < FLT_EPSILON);
-    TEST(fabs(wrap_pi(3.) - (3.)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(3.) - (3.)) < FLT_EPSILON);
-    TEST(fabs(wrap_pi(100.) - (100. - 32. * M_PI)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(100.) - (100. - 32. * M_PI)) < FLT_EPSILON);
-    TEST(fabs(wrap_pi(-100.) - (-100. + 32. * M_PI)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(-100.) - (-100. + 32. * M_PI)) < FLT_EPSILON);
-    TEST(fabs(wrap_pi(-101.) - (-101. + 32. * M_PI)) < FLT_EPSILON);
+	TEST(fabs(wrap_pi(-101.) - (-101. + 32. * M_PI)) < FLT_EPSILON);
-    TEST(!is_finite(wrap_pi(NAN)));
+	TEST(!is_finite(wrap_pi(NAN)));
 
-    // wrap 2pi
+	// wrap 2pi
-    TEST(fabs(wrap_2pi(0.)) < FLT_EPSILON);
+	TEST(fabs(wrap_2pi(0.)) < FLT_EPSILON);
-    TEST(fabs(wrap_2pi(-4.) - (-4. + 2. * M_PI)) < FLT_EPSILON);
+	TEST(fabs(wrap_2pi(-4.) - (-4. + 2. * M_PI)) < FLT_EPSILON);
-    TEST(fabs(wrap_2pi(3.) - (3.)) < FLT_EPSILON);
+	TEST(fabs(wrap_2pi(3.) - (3.)) < FLT_EPSILON);
-    TEST(fabs(wrap_2pi(200.) - (200. - 31. * M_TWOPI)) < FLT_EPSILON);
+	TEST(fabs(wrap_2pi(200.) - (200. - 31. * M_TWOPI)) < FLT_EPSILON);
-    TEST(fabs(wrap_2pi(-201.) - (-201. + 32. * M_TWOPI)) < FLT_EPSILON);
+	TEST(fabs(wrap_2pi(-201.) - (-201. + 32. * M_TWOPI)) < FLT_EPSILON);
-    TEST(!is_finite(wrap_2pi(NAN)));
+	TEST(!is_finite(wrap_2pi(NAN)));
 
-    // Equality checks
+	// Equality checks
-    TEST(isEqualF(1., 1.));
+	TEST(isEqualF(1., 1.));
-    TEST(!isEqualF(1., 2.));
+	TEST(!isEqualF(1., 2.));
-    TEST(!isEqualF(NAN, 1.f));
+	TEST(!isEqualF(NAN, 1.f));
-    TEST(!isEqualF(1.f, NAN));
+	TEST(!isEqualF(1.f, NAN));
-    TEST(!isEqualF(INFINITY, 1.f));
+	TEST(!isEqualF(INFINITY, 1.f));
-    TEST(!isEqualF(1.f, INFINITY));
+	TEST(!isEqualF(1.f, INFINITY));
-    TEST(isEqualF(NAN, NAN));
+	TEST(isEqualF(NAN, NAN));
-    TEST(isEqualF(NAN, -NAN));
+	TEST(isEqualF(NAN, -NAN));
-    TEST(isEqualF(-NAN, NAN));
+	TEST(isEqualF(-NAN, NAN));
-    TEST(isEqualF(INFINITY, INFINITY));
+	TEST(isEqualF(INFINITY, INFINITY));
-    TEST(!isEqualF(INFINITY, -INFINITY));
+	TEST(!isEqualF(INFINITY, -INFINITY));
-    TEST(!isEqualF(-INFINITY, INFINITY));
+	TEST(!isEqualF(-INFINITY, INFINITY));
-    TEST(isEqualF(-INFINITY, -INFINITY));
+	TEST(isEqualF(-INFINITY, -INFINITY));
 
-    Vector3f a(1, 2, 3);
+	Vector3f a(1, 2, 3);
-    Vector3f b(4, 5, 6);
+	Vector3f b(4, 5, 6);
-    TEST(!isEqual(a, b));
+	TEST(!isEqual(a, b));
-    TEST(isEqual(a, a));
+	TEST(isEqual(a, a));
 
-    Vector3f c(1, 2, 3);
+	Vector3f c(1, 2, 3);
-    Vector3f d(1, 2, NAN);
+	Vector3f d(1, 2, NAN);
-    TEST(!isEqual(c, d));
+	TEST(!isEqual(c, d));
-    TEST(isEqual(c, c));
+	TEST(isEqual(c, c));
-    TEST(isEqual(d, d));
+	TEST(isEqual(d, d));
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/integration.cpp

@@ -5,22 +5,22 @@ using namespace matrix;
 
 Vector<float, 6> f(float t, const Matrix<float, 6, 1> &  /*y*/, const Matrix<float, 3, 1> &  /*u*/);
 
-Vector<float, 6> f(float t, const Matrix<float, 6, 1> &  /*y*/, const Matrix<float, 3, 1> &  /*u*/) {
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &  /*y*/, const Matrix<float, 3, 1> &  /*u*/)
-    float v = -sin(t);
+{
-    return v*ones<float, 6, 1>();
+	float v = -sin(t);
+	return v * ones<float, 6, 1>();
 }
 
 int main()
 {
-    Vector<float, 6> y = ones<float, 6, 1>();
+	Vector<float, 6> y = ones<float, 6, 1>();
-    Vector<float, 3> u = ones<float, 3, 1>();
+	Vector<float, 3> u = ones<float, 3, 1>();
-    float t0 = 0;
+	float t0 = 0;
-    float tf = 2;
+	float tf = 2;
-    float h = 0.001f;
+	float h = 0.001f;
-    integrate_rk4(f, y, u, t0, tf, h, y);
+	integrate_rk4(f, y, u, t0, tf, h, y);
-    float v = 1 + cos(tf) - cos(t0);
+	float v = 1 + cos(tf) - cos(t0);
-    TEST(isEqual(y, (ones<float, 6, 1>()*v)));
+	TEST(isEqual(y, (ones<float, 6, 1>()*v)));
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/inverse.cpp

@@ -7,158 +7,158 @@ static const size_t n_large = 50;
 
 int main()
 {
-    float data[9] = {0, 2, 3,
+	float data[9] = {0, 2, 3,
-                     4, 5, 6,
+			 4, 5, 6,
-                     7, 8, 10
+			 7, 8, 10
-                    };
+			};
-    float data_check[9] = {
+	float data_check[9] = {
-        -0.4f, -0.8f,  0.6f,
+		-0.4f, -0.8f,  0.6f,
-        -0.4f,  4.2f, -2.4f,
+			-0.4f,  4.2f, -2.4f,
-        0.6f, -2.8f,  1.6f
+			0.6f, -2.8f,  1.6f
-    };
+		};
 
-    SquareMatrix<float, 3> A(data);
+	SquareMatrix<float, 3> A(data);
-    SquareMatrix<float, 3> A_I = inv(A);
+	SquareMatrix<float, 3> A_I = inv(A);
-    SquareMatrix<float, 3> A_I_check(data_check);
+	SquareMatrix<float, 3> A_I_check(data_check);
-    TEST((A_I - A_I_check).abs().max() < 1e-6f);
+	TEST((A_I - A_I_check).abs().max() < 1e-6f);
 
-    float data_2x2[4] = {12, 2,
+	float data_2x2[4] = {12, 2,
-                         -7, 5
+			     -7, 5
-                        };
+			    };
-    float data_2x2_check[4] = {
+	float data_2x2_check[4] = {
-        0.0675675675f, -0.02702702f,
+		0.0675675675f, -0.02702702f,
-        0.0945945945f, 0.162162162f
+		0.0945945945f, 0.162162162f
-    };
+	};
 
-    SquareMatrix<float, 2> A2x2(data_2x2);
+	SquareMatrix<float, 2> A2x2(data_2x2);
-    SquareMatrix<float, 2> A2x2_I = inv(A2x2);
+	SquareMatrix<float, 2> A2x2_I = inv(A2x2);
-    SquareMatrix<float, 2> A2x2_I_check(data_2x2_check);
+	SquareMatrix<float, 2> A2x2_I_check(data_2x2_check);
-    TEST(isEqual(A2x2_I, A2x2_I_check));
+	TEST(isEqual(A2x2_I, A2x2_I_check));
 
-    SquareMatrix<float, 2> A2x2_sing = ones<float, 2, 2>();
+	SquareMatrix<float, 2> A2x2_sing = ones<float, 2, 2>();
-    SquareMatrix<float, 2> A2x2_sing_I;
+	SquareMatrix<float, 2> A2x2_sing_I;
-    TEST(inv(A2x2_sing, A2x2_sing_I) == false);
+	TEST(inv(A2x2_sing, A2x2_sing_I) == false);
 
-    SquareMatrix<float, 3> A3x3_sing = ones<float, 3, 3>();
+	SquareMatrix<float, 3> A3x3_sing = ones<float, 3, 3>();
-    SquareMatrix<float, 3> A3x3_sing_I;
+	SquareMatrix<float, 3> A3x3_sing_I;
-    TEST(inv(A3x3_sing, A3x3_sing_I) == false)
+	TEST(inv(A3x3_sing, A3x3_sing_I) == false)
 
-    // stess test
+	// stess test
-    SquareMatrix<float, n_large> A_large;
+	SquareMatrix<float, n_large> A_large;
-    A_large.setIdentity();
+	A_large.setIdentity();
-    SquareMatrix<float, n_large> A_large_I;
+	SquareMatrix<float, n_large> A_large_I;
-    A_large_I.setZero();
+	A_large_I.setZero();
 
-    for (size_t i = 0; i < n_large; i++) {
+	for (size_t i = 0; i < n_large; i++) {
-        A_large_I = inv(A_large);
+		A_large_I = inv(A_large);
-        TEST(isEqual(A_large, A_large_I));
+		TEST(isEqual(A_large, A_large_I));
-    }
+	}
 
-    SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
+	SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
-    TEST(isEqual(inv(zero_test), zeros<float, 3, 3>()));
+	TEST(isEqual(inv(zero_test), zeros<float, 3, 3>()));
 
-    // test pivotting
+	// test pivotting
-    float data2[81] = {
+	float data2[81] = {
-        -2,   1,   1,  -1,  -5,   1,   2,  -1,   0,
+		-2,   1,   1,  -1,  -5,   1,   2,  -1,   0,
-        -3,   2,  -1,   0,   2,   2,  -1,  -5,   3,
+			-3,   2,  -1,   0,   2,   2,  -1,  -5,   3,
-        0,   0,   0,   1,   4,  -3,   3,   0,  -2,
+			0,   0,   0,   1,   4,  -3,   3,   0,  -2,
-        2,   2,  -1,  -2,  -1,   0,   3,   0,   1,
+			2,   2,  -1,  -2,  -1,   0,   3,   0,   1,
-        -1,   2,  -1,  -1,  -3,   3,   0,  -2,   3,
+			-1,   2,  -1,  -1,  -3,   3,   0,  -2,   3,
-        0,   1,   1,  -3,   3,  -2,   0,  -4,   0,
+			0,   1,   1,  -3,   3,  -2,   0,  -4,   0,
-        1,   0,   0,   0,   0,   0,  -2,   4,  -3,
+			1,   0,   0,   0,   0,   0,  -2,   4,  -3,
-        1,  -1,   0,  -1,  -1,   1,  -1,  -3,   4,
+			1,  -1,   0,  -1,  -1,   1,  -1,  -3,   4,
-        0,   3,  -1,  -2,   2,   1,  -2,   0,  -1
+			0,   3,  -1,  -2,   2,   1,  -2,   0,  -1
-    };
+		};
 
-    float data2_check[81] = {
+	float data2_check[81] = {
-        6, -4,   3, -3, -9, -8, -10,   8,  14,
+		6, -4,   3, -3, -9, -8, -10,   8,  14,
-        -2, -7,  -5, -3, -2, -2, -16,  -5,   8,
+		-2, -7,  -5, -3, -2, -2, -16,  -5,   8,
-        -2,  0, -23,  7, -24, -5, -28, -14,   9,
+		-2,  0, -23,  7, -24, -5, -28, -14,   9,
-        3, -7,   2, -5,  -4, -6, -13,   4,  13,
+		3, -7,   2, -5,  -4, -6, -13,   4,  13,
-        -1,  4,  -8,  5,  -8,  0,  -3,  -5,  -2,
+		-1,  4,  -8,  5,  -8,  0,  -3,  -5,  -2,
-        6,  7,  -7,  7, -21, -7,  -5,   3,   6,
+		6,  7,  -7,  7, -21, -7,  -5,   3,   6,
-        1,  4,  -4,  4,  -7, -1,   0,  -1,  -1,
+		1,  4,  -4,  4,  -7, -1,   0,  -1,  -1,
-        -7,  3, -11,  5,   1,  6,  -1, -13, -10,
+		-7,  3, -11,  5,   1,  6,  -1, -13, -10,
-        -8,  0, -11,  3,   3,  6,  -5, -14,  -8
+		-8,  0, -11,  3,   3,  6,  -5, -14,  -8
-    };
+	};
-    SquareMatrix<float, 9> A2(data2);
+	SquareMatrix<float, 9> A2(data2);
-    SquareMatrix<float, 9> A2_I = inv(A2);
+	SquareMatrix<float, 9> A2_I = inv(A2);
-    SquareMatrix<float, 9> A2_I_check(data2_check);
+	SquareMatrix<float, 9> A2_I_check(data2_check);
-    TEST((A2_I - A2_I_check).abs().max() < 1e-3f);
+	TEST((A2_I - A2_I_check).abs().max() < 1e-3f);
 
-    float data3[9] = {
+	float data3[9] = {
-        0, 1, 2,
+		0, 1, 2,
-        3, 4, 5,
+		3, 4, 5,
-        6, 7, 9
+		6, 7, 9
-    };
+	};
-    float data3_check[9] = {
+	float data3_check[9] = {
-        -0.3333333f, -1.6666666f, 1,
+		-0.3333333f, -1.6666666f, 1,
-        -1, 4, -2,
+			-1, 4, -2,
-        1, -2, 1
+			1, -2, 1
-    };
+		};
-    SquareMatrix<float, 3> A3(data3);
+	SquareMatrix<float, 3> A3(data3);
-    SquareMatrix<float, 3> A3_I = inv(A3);
+	SquareMatrix<float, 3> A3_I = inv(A3);
-    SquareMatrix<float, 3> A3_I_check(data3_check);
+	SquareMatrix<float, 3> A3_I_check(data3_check);
-    TEST(isEqual(inv(A3), A3_I_check));
+	TEST(isEqual(inv(A3), A3_I_check));
-    TEST(isEqual(A3_I, A3_I_check));
+	TEST(isEqual(A3_I, A3_I_check));
-    TEST(A3.I(A3_I));
+	TEST(A3.I(A3_I));
-    TEST(isEqual(A3_I, A3_I_check));
+	TEST(isEqual(A3_I, A3_I_check));
 
-    // cover singular matrices
+	// cover singular matrices
-    A3(0, 0) = 0;
+	A3(0, 0) = 0;
-    A3(0, 1) = 0;
+	A3(0, 1) = 0;
-    A3(0, 2) = 0;
+	A3(0, 2) = 0;
-    A3_I = inv(A3);
+	A3_I = inv(A3);
-    SquareMatrix<float, 3>  Z3 = zeros<float, 3, 3>();
+	SquareMatrix<float, 3>  Z3 = zeros<float, 3, 3>();
-    TEST(!A3.I(A3_I));
+	TEST(!A3.I(A3_I));
-    TEST(!Z3.I(A3_I));
+	TEST(!Z3.I(A3_I));
-    TEST(isEqual(A3_I, Z3));
+	TEST(isEqual(A3_I, Z3));
-    TEST(isEqual(A3.I(), Z3));
+	TEST(isEqual(A3.I(), Z3));
 
-    for(size_t i = 0; i < 9; i++) {
+	for (size_t i = 0; i < 9; i++) {
-        A2(0, i) = 0;
+		A2(0, i) = 0;
-    }
+	}
-    A2_I = inv(A2);
-    SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
-    TEST(!A2.I(A2_I));
-    TEST(!Z9.I(A2_I));
-    TEST(isEqual(A2_I, Z9));
-    TEST(isEqual(A2.I(), Z9));
 
-    // cover NaN
+	A2_I = inv(A2);
-    A3(0, 0) = NAN;
+	SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
-    A3(0, 1) = 0;
+	TEST(!A2.I(A2_I));
-    A3(0, 2) = 0;
+	TEST(!Z9.I(A2_I));
-    A3_I = inv(A3);
+	TEST(isEqual(A2_I, Z9));
-    TEST(isEqual(A3_I, Z3));
+	TEST(isEqual(A2.I(), Z9));
-    TEST(isEqual(A3.I(), Z3));
 
-    A2(0, 0) = NAN;
+	// cover NaN
-    A2_I = inv(A2);
+	A3(0, 0) = NAN;
-    TEST(isEqual(A2_I, Z9));
+	A3(0, 1) = 0;
-    TEST(isEqual(A2.I(), Z9));
+	A3(0, 2) = 0;
+	A3_I = inv(A3);
+	TEST(isEqual(A3_I, Z3));
+	TEST(isEqual(A3.I(), Z3));
 
-    float data4[9] = {
+	A2(0, 0) = NAN;
-        1.33471626f,  0.74946721f, -0.0531679f,
+	A2_I = inv(A2);
-        0.74946721f,  1.07519593f,  0.08036323f,
+	TEST(isEqual(A2_I, Z9));
-        -0.0531679f,  0.08036323f,  1.01618474f
+	TEST(isEqual(A2.I(), Z9));
-    };
-    SquareMatrix<float, 3> A4(data4);
 
-    float data4_cholesky[9] = {
+	float data4[9] = {
-        1.15529921f,  0.f,  0.f,
+		1.33471626f,  0.74946721f, -0.0531679f,
-        0.6487213f,  0.80892311f,  0.f,
+		0.74946721f,  1.07519593f,  0.08036323f,
-        -0.04602089f,  0.13625271f,  0.99774847f
+		-0.0531679f,  0.08036323f,  1.01618474f
-    };
+	};
-    SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
+	SquareMatrix<float, 3> A4(data4);
-    SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
-    TEST(isEqual(A4_cholesky_check, A4_cholesky));
 
-    SquareMatrix<float, 3> I3;
+	float data4_cholesky[9] = {
-    I3.setIdentity();
+		1.15529921f,  0.f,  0.f,
-    TEST(isEqual(choleskyInv(A4)*A4, I3));
+		0.6487213f,  0.80892311f,  0.f,
-    TEST(isEqual(cholesky(Z3), Z3));
+		-0.04602089f,  0.13625271f,  0.99774847f
-    return 0;
+	};
+	SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
+	SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
+	TEST(isEqual(A4_cholesky_check, A4_cholesky));
+
+	SquareMatrix<float, 3> I3;
+	I3.setIdentity();
+	TEST(isEqual(choleskyInv(A4)*A4, I3));
+	TEST(isEqual(cholesky(Z3), Z3));
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/least_squares.cpp

@@ -10,91 +10,97 @@ int test_div_zero(void);
 
 int main()
 {
-    int ret;
+	int ret;
 
-    ret = test_4x4<float>();
+	ret = test_4x4<float>();
-    if (ret != 0) return ret;
 
-    ret = test_4x4<double>();
+	if (ret != 0) { return ret; }
-    if (ret != 0) return ret;
 
-    ret = test_4x3();
+	ret = test_4x4<double>();
-    if (ret != 0) return ret;
 
-    ret = test_div_zero();
+	if (ret != 0) { return ret; }
-    if (ret != 0) return ret;
 
-    return 0;
+	ret = test_4x3();
+
+	if (ret != 0) { return ret; }
+
+	ret = test_div_zero();
+
+	if (ret != 0) { return ret; }
+
+	return 0;
 }
 
-int test_4x3() {
+int test_4x3()
-    // Start with an (m x n) A matrix
+{
-    float data[12] = {20.f, -10.f, -13.f,
+	// Start with an (m x n) A matrix
-                      17.f,  16.f, -18.f,
+	float data[12] = {20.f, -10.f, -13.f,
-                      0.7f,  -0.8f,   0.9f,
+			  17.f,  16.f, -18.f,
-                      -1.f,  -1.1f,  -1.2f
+			  0.7f,  -0.8f,   0.9f,
-                     };
+			  -1.f,  -1.1f,  -1.2f
-    Matrix<float, 4, 3> A(data);
+			 };
+	Matrix<float, 4, 3> A(data);
 
-    float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
+	float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
-    Vector<float, 4> b(b_data);
+	Vector<float, 4> b(b_data);
 
-    float x_check_data[3] = {-0.69168233f,
+	float x_check_data[3] = {-0.69168233f,
-                             -0.26227593f,
+				 -0.26227593f,
-                             -1.03767522f
+				 -1.03767522f
-                            };
+				};
-    Vector<float, 3> x_check(x_check_data);
+	Vector<float, 3> x_check(x_check_data);
 
-    LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
+	LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
 
-    Vector<float, 3> x = qrd.solve(b);
+	Vector<float, 3> x = qrd.solve(b);
-    TEST(isEqual(x, x_check));
+	TEST(isEqual(x, x_check));
-    return 0;
+	return 0;
 }
 
 template<typename Type>
-int test_4x4() {
+int test_4x4()
-    // Start with an (m x n) A matrix
+{
-    const Type data[16] = { 20.f, -10.f, -13.f,  21.f,
+	// Start with an (m x n) A matrix
-                            17.f,  16.f, -18.f, -14.f,
+	const Type data[16] = { 20.f, -10.f, -13.f,  21.f,
-                            0.7f,  -0.8f,   0.9f,  -0.5f,
+				17.f,  16.f, -18.f, -14.f,
-                            -1.f,  -1.1f,  -1.2f,  -1.3f
+				0.7f,  -0.8f,   0.9f,  -0.5f,
-                          };
+				-1.f,  -1.1f,  -1.2f,  -1.3f
-    Matrix<Type, 4, 4> A(data);
+			      };
+	Matrix<Type, 4, 4> A(data);
 
-    Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
+	Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
-    Vector<Type, 4> b(b_data);
+	Vector<Type, 4> b(b_data);
 
-    Type x_check_data[4] = { 0.97893433f,
+	Type x_check_data[4] = { 0.97893433f,
-                             -2.80798701f,
+				 -2.80798701f,
-                             -0.03175765f,
+				 -0.03175765f,
-                             -2.19387649f
+				 -2.19387649f
-                           };
+			       };
-    Vector<Type, 4> x_check(x_check_data);
+	Vector<Type, 4> x_check(x_check_data);
 
-    LeastSquaresSolver<Type, 4, 4> qrd = LeastSquaresSolver<Type, 4, 4>(A);
+	LeastSquaresSolver<Type, 4, 4> qrd = LeastSquaresSolver<Type, 4, 4>(A);
 
-    Vector<Type, 4> x = qrd.solve(b);
+	Vector<Type, 4> x = qrd.solve(b);
-    TEST(isEqual(x, x_check));
+	TEST(isEqual(x, x_check));
-    return 0;
+	return 0;
 }
 
-int test_div_zero() {
+int test_div_zero()
-    float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+{
-    Matrix<float, 2, 2> A(data);
+	float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+	Matrix<float, 2, 2> A(data);
 
-    float b_data[2] = {1.0f, 1.0f};
+	float b_data[2] = {1.0f, 1.0f};
-    Vector<float, 2> b(b_data);
+	Vector<float, 2> b(b_data);
 
-    // Implement such that x returns zeros if it reaches div by zero
+	// Implement such that x returns zeros if it reaches div by zero
-    float x_check_data[2] = {0.0f, 0.0f};
+	float x_check_data[2] = {0.0f, 0.0f};
-    Vector<float, 2> x_check(x_check_data);
+	Vector<float, 2> x_check(x_check_data);
 
-    LeastSquaresSolver<float, 2, 2> qrd = LeastSquaresSolver<float, 2, 2>(A);
+	LeastSquaresSolver<float, 2, 2> qrd = LeastSquaresSolver<float, 2, 2>(A);
 
-    Vector<float, 2> x = qrd.solve(b);
+	Vector<float, 2> x = qrd.solve(b);
-    TEST(isEqual(x, x_check));
+	TEST(isEqual(x, x_check));
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/matrixAssignment.cpp

@@ -7,254 +7,264 @@ template class matrix::Matrix<float, 3, 2>;
 
 int main()
 {
-    Matrix3f m;
+	Matrix3f m;
-    m.setZero();
+	m.setZero();
-    m.zero();
+	m.zero();
-    m(0, 0) = 1;
+	m(0, 0) = 1;
-    m(0, 1) = 2;
+	m(0, 1) = 2;
-    m(0, 2) = 3;
+	m(0, 2) = 3;
-    m(1, 0) = 4;
+	m(1, 0) = 4;
-    m(1, 1) = 5;
+	m(1, 1) = 5;
-    m(1, 2) = 6;
+	m(1, 2) = 6;
-    m(2, 0) = 7;
+	m(2, 0) = 7;
-    m(2, 1) = 8;
+	m(2, 1) = 8;
-    m(2, 2) = 9;
+	m(2, 2) = 9;
 
-    float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-    Matrix3f m2(data);
+	Matrix3f m2(data);
 
-    for(size_t i=0; i<3; i++) {
+	for (size_t i = 0; i < 3; i++) {
-        for (size_t j = 0; j < 3; j++) {
+		for (size_t j = 0; j < 3; j++) {
-            TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
+			TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON);
-        }
+		}
-    }
+	}
 
-    Matrix3f m_nan;
+	Matrix3f m_nan;
-    m_nan.setNaN();
+	m_nan.setNaN();
-    for(size_t i=0; i<3; i++) {
-        for (size_t j = 0; j < 3; j++) {
-            TEST(isnan(m_nan(i,j)));
-        }
-    }
-    TEST(m_nan.isAllNan());
 
-    float data2d[3][3] = {
+	for (size_t i = 0; i < 3; i++) {
-        {1, 2, 3},
+		for (size_t j = 0; j < 3; j++) {
-        {4, 5, 6},
+			TEST(isnan(m_nan(i, j)));
-        {7, 8, 9}
+		}
-    };
+	}
-    m2 = Matrix3f(data2d);
-    for(size_t i=0; i<3; i++) {
-        for (size_t j = 0; j < 3; j++) {
-            TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
-        }
-    }
-    TEST(!m2.isAllNan());
 
-    float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	TEST(m_nan.isAllNan());
-    Matrix3f m3(data_times_2);
 
-    TEST(isEqual(m, m2));
+	float data2d[3][3] = {
-    TEST(!(isEqual(m, m3)));
+		{1, 2, 3},
+		{4, 5, 6},
+		{7, 8, 9}
+	};
+	m2 = Matrix3f(data2d);
 
-    m2 *= 2;
+	for (size_t i = 0; i < 3; i++) {
-    TEST(isEqual(m2, m3));
+		for (size_t j = 0; j < 3; j++) {
+			TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON);
+		}
+	}
 
-    m2 /= 2;
+	TEST(!m2.isAllNan());
-    m2 -= 1;
-    float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
-    TEST(isEqual(Matrix3f(data_minus_1), m2));
 
-    m2 += 1;
+	float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
-    TEST(isEqual(Matrix3f(data), m2));
+	Matrix3f m3(data_times_2);
 
-    m3 -= m2;
+	TEST(isEqual(m, m2));
+	TEST(!(isEqual(m, m3)));
 
-    TEST(isEqual(m3, m2));
+	m2 *= 2;
+	TEST(isEqual(m2, m3));
 
-    // set rows and columns to value
+	m2 /= 2;
-    Matrix3f m2e(data2d);
+	m2 -= 1;
+	float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+	TEST(isEqual(Matrix3f(data_minus_1), m2));
 
-    float data2e_check1[3][3] = {
+	m2 += 1;
-        {1, 11, 3},
+	TEST(isEqual(Matrix3f(data), m2));
-        {4, 11, 6},
-        {7, 11, 9}
-    };
-    Matrix3f m2e_check1(data2e_check1);
 
-    float data2e_check2[3][3] = {
+	m3 -= m2;
-        {1, 11, 3},
-        {4, 11, 6},
-        {0, 0, 0}
-    };
-    Matrix3f m2e_check2(data2e_check2);
 
-    m2e.setCol(1, 11);
+	TEST(isEqual(m3, m2));
-    TEST(isEqual(m2e, m2e_check1));
-    m2e.setRow(2, 0);
-    TEST(isEqual(m2e, m2e_check2));
 
-    float data_row_02_swap[9] = {
+	// set rows and columns to value
-        7, 8, 9,
+	Matrix3f m2e(data2d);
-        4, 5, 6,
-        1, 2, 3,
-    };
 
-    float data_col_02_swap[9] = {
+	float data2e_check1[3][3] = {
-        3, 2, 1,
+		{1, 11, 3},
-        6, 5, 4,
+		{4, 11, 6},
-        9, 8, 7
+		{7, 11, 9}
-    };
+	};
+	Matrix3f m2e_check1(data2e_check1);
 
-    Matrix3f m4(data);
+	float data2e_check2[3][3] = {
+		{1, 11, 3},
+		{4, 11, 6},
+		{0, 0, 0}
+	};
+	Matrix3f m2e_check2(data2e_check2);
 
-    TEST(isEqual(-m4, m4*(-1)));
+	m2e.setCol(1, 11);
+	TEST(isEqual(m2e, m2e_check1));
+	m2e.setRow(2, 0);
+	TEST(isEqual(m2e, m2e_check2));
 
-    // col swap
+	float data_row_02_swap[9] = {
-    m4.swapCols(0, 2);
+		7, 8, 9,
-    TEST(isEqual(m4, Matrix3f(data_col_02_swap)));
+		4, 5, 6,
-    m4.swapCols(0, 2);
+		1, 2, 3,
+	};
 
-    // row swap
+	float data_col_02_swap[9] = {
-    m4.swapRows(0, 2);
+		3, 2, 1,
-    TEST(isEqual(m4, Matrix3f(data_row_02_swap)));
+		6, 5, 4,
-    m4.swapRows(0, 2);
+		9, 8, 7
+	};
 
-    // swapping with same row should do nothing
+	Matrix3f m4(data);
-    m4.swapRows(0, 0);
-    m4.swapRows(1, 1);
-    m4.swapRows(2, 2);
-    TEST(isEqual(m4, Matrix3f(data)));
 
-    // swapping with same col should do nothing
+	TEST(isEqual(-m4, m4 * (-1)));
-    m4.swapCols(0, 0);
-    m4.swapCols(1, 1);
-    m4.swapCols(2, 2);
-    TEST(isEqual(m4, Matrix3f(data)));
 
-    TEST(fabs(m4.min() - 1) < FLT_EPSILON);
+	// col swap
-    TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
+	m4.swapCols(0, 2);
+	TEST(isEqual(m4, Matrix3f(data_col_02_swap)));
+	m4.swapCols(0, 2);
 
-    Scalar<float> s = 1;
+	// row swap
-    const Vector<float, 1> & s_vect = s;
+	m4.swapRows(0, 2);
-    TEST(fabs(s - 1) < FLT_EPSILON);
+	TEST(isEqual(m4, Matrix3f(data_row_02_swap)));
-    TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
+	m4.swapRows(0, 2);
 
-    Matrix<float, 1, 1> m5 = s;
+	// swapping with same row should do nothing
-    TEST(fabs(m5(0,0) - s) < FLT_EPSILON);
+	m4.swapRows(0, 0);
+	m4.swapRows(1, 1);
+	m4.swapRows(2, 2);
+	TEST(isEqual(m4, Matrix3f(data)));
 
-    Matrix<float, 2, 2> m6;
+	// swapping with same col should do nothing
-    m6.setRow(0, Vector2f(1, 2));
+	m4.swapCols(0, 0);
-    float m7_array[] = {1,2,0,0};
+	m4.swapCols(1, 1);
-    Matrix<float, 2, 2> m7(m7_array);
+	m4.swapCols(2, 2);
-    TEST(isEqual(m6, m7));
+	TEST(isEqual(m4, Matrix3f(data)));
-    m6.setCol(0, Vector2f(3, 4));
-    float m8_array[] = {3,2,4,0};
-    Matrix<float, 2, 2> m8(m8_array);
-    TEST(isEqual(m6, m8));
 
-    m7.setNaN();
+	TEST(fabs(m4.min() - 1) < FLT_EPSILON);
-    TEST(m7 != m8);
+	TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
 
-    // min, max, constrain matrix values with scalar
+	Scalar<float> s = 1;
-    float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	const Vector<float, 1> &s_vect = s;
-    float lower_bound = 7;
+	TEST(fabs(s - 1) < FLT_EPSILON);
-    float upper_bound = 11;
+	TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
-    float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18};
-    float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11};
-    float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11};
-    Matrix3f m9(data_m9);
-    Matrix3f m9_lower_bounded(data_m9_lower_bounded);
-    Matrix3f m9_upper_bounded(data_m9_upper_bounded);
-    Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained);
-    TEST(isEqual(max(m9, lower_bound), m9_lower_bounded));
-    TEST(isEqual(max(lower_bound, m9), m9_lower_bounded));
-    TEST(isEqual(min(m9, upper_bound), m9_upper_bounded));
-    TEST(isEqual(min(upper_bound, m9), m9_upper_bounded));
-    TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained));
-    TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan));
 
-    // min, max, constrain matrix values with matrix of same size
+	Matrix<float, 1, 1> m5 = s;
-    float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	TEST(fabs(m5(0, 0) - s) < FLT_EPSILON);
-    float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18};
-    float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18};
-    float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18};
-    float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18};
-    float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18};
-    Matrix3f m10(data_m10);
-    Matrix3f m10_lower_bound(data_m10_lower_bound);
-    Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref);
-    Matrix3f m10_upper_bound(data_m10_upper_bound);
-    Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref);
-    Matrix3f m10_constrained_ref(data_m10_constrained_ref);
-    TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref));
-    TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref));
-    TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref));
-    TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref));
-    TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref));
 
-    // min, max, constrain with NAN
+	Matrix<float, 2, 2> m6;
-    TEST(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f));
+	m6.setRow(0, Vector2f(1, 2));
-    TEST(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f));
+	float m7_array[] = {1, 2, 0, 0};
-    TEST(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN));
+	Matrix<float, 2, 2> m7(m7_array);
-    TEST(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f));
+	TEST(isEqual(m6, m7));
-    TEST(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f));
+	m6.setCol(0, Vector2f(3, 4));
-    TEST(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN));
+	float m8_array[] = {3, 2, 4, 0};
-    TEST(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN));
+	Matrix<float, 2, 2> m8(m8_array);
-    TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN));
+	TEST(isEqual(m6, m8));
-    TEST(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f));
-    TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f));
-    Vector2f v1{NAN, 5.0f};
-    Vector2f v1_min = min(v1, 1.f);
-    Matrix3f m11 = min(m10_constrained_ref,NAN);
-    TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0))));
-    TEST(isEqual(m11, m10_constrained_ref));
 
-    // check write_string()
+	m7.setNaN();
-    float comma[6] = {
+	TEST(m7 != m8);
-        1.f, 12345.123f,
-        12345.1228f, .1234567891011f,
-        12345678910.123456789f, 1234567891011.123456789101112f
-    };
-    Matrix<float, 3, 2> Comma(comma);
-    const size_t len = 15*2*3 + 2 + 1;
-    char buffer[len];
-    Comma.print(); // for debugging in case of failure
-    Comma.write_string(buffer, len);
-    printf("%s\n", buffer); // for debugging in case of failure
-    char output[] = "\t       1\t12345.123\n\t12345.123\t0.12345679\n\t1.2345679e+10\t1.234568e+12\n";
-    printf("%s\n", output); // for debugging in case of failure
-    for (size_t i = 0; i < len; i++) {
-        if(buffer[i] != output[i]) { // for debugging in case of failure
-            printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure
-        }
-        TEST(buffer[i] == output[i]);
-        if (buffer[i] == '\0') {
-            break;
-        }
-    }
 
-    // check print()
+	// min, max, constrain matrix values with scalar
-    // Redirect stdout
+	float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
-    TEST(freopen("testoutput.txt", "w", stdout) != NULL);
+	float lower_bound = 7;
-    // write
+	float upper_bound = 11;
-    Comma.print();
+	float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18};
-    fclose(stdout);
+	float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11};
-    // read
+	float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11};
-    FILE *fp = fopen("testoutput.txt", "r");
+	Matrix3f m9(data_m9);
-    TEST(fp != nullptr);
+	Matrix3f m9_lower_bounded(data_m9_lower_bounded);
-    TEST(!fseek(fp, 0, SEEK_SET));
+	Matrix3f m9_upper_bounded(data_m9_upper_bounded);
-    for (size_t i = 0; i < len; i++) {
+	Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained);
-        char c = static_cast<char>(fgetc(fp));
+	TEST(isEqual(max(m9, lower_bound), m9_lower_bounded));
-        if (c == '\n') {
+	TEST(isEqual(max(lower_bound, m9), m9_lower_bounded));
-            break;
+	TEST(isEqual(min(m9, upper_bound), m9_upper_bounded));
-        }
+	TEST(isEqual(min(upper_bound, m9), m9_upper_bounded));
-        printf("%d %d %d\n", static_cast<int>(i), output[i], c);
+	TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained));
-        TEST(c == output[i]);
+	TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan));
-    }
-    TEST(!fclose(fp));
 
-    return 0;
+	// min, max, constrain matrix values with matrix of same size
+	float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18};
+	float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18};
+	float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18};
+	float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18};
+	float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18};
+	Matrix3f m10(data_m10);
+	Matrix3f m10_lower_bound(data_m10_lower_bound);
+	Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref);
+	Matrix3f m10_upper_bound(data_m10_upper_bound);
+	Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref);
+	Matrix3f m10_constrained_ref(data_m10_constrained_ref);
+	TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref));
+	TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref));
+	TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref));
+	TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref));
+	TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref));
+
+	// min, max, constrain with NAN
+	TEST(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f));
+	TEST(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f));
+	TEST(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN));
+	TEST(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f));
+	TEST(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f));
+	TEST(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN));
+	TEST(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN));
+	TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN));
+	TEST(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f));
+	TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f));
+	Vector2f v1{NAN, 5.0f};
+	Vector2f v1_min = min(v1, 1.f);
+	Matrix3f m11 = min(m10_constrained_ref, NAN);
+	TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0))));
+	TEST(isEqual(m11, m10_constrained_ref));
+
+	// check write_string()
+	float comma[6] = {
+		1.f, 12345.123f,
+		12345.1228f, .1234567891011f,
+		12345678910.123456789f, 1234567891011.123456789101112f
+	};
+	Matrix<float, 3, 2> Comma(comma);
+	const size_t len = 15 * 2 * 3 + 2 + 1;
+	char buffer[len];
+	Comma.print(); // for debugging in case of failure
+	Comma.write_string(buffer, len);
+	printf("%s\n", buffer); // for debugging in case of failure
+	char output[] = "\t       1\t12345.123\n\t12345.123\t0.12345679\n\t1.2345679e+10\t1.234568e+12\n";
+	printf("%s\n", output); // for debugging in case of failure
+
+	for (size_t i = 0; i < len; i++) {
+		if (buffer[i] != output[i]) { // for debugging in case of failure
+			printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure
+		}
+
+		TEST(buffer[i] == output[i]);
+
+		if (buffer[i] == '\0') {
+			break;
+		}
+	}
+
+	// check print()
+	// Redirect stdout
+	TEST(freopen("testoutput.txt", "w", stdout) != NULL);
+	// write
+	Comma.print();
+	fclose(stdout);
+	// read
+	FILE *fp = fopen("testoutput.txt", "r");
+	TEST(fp != nullptr);
+	TEST(!fseek(fp, 0, SEEK_SET));
+
+	for (size_t i = 0; i < len; i++) {
+		char c = static_cast<char>(fgetc(fp));
+
+		if (c == '\n') {
+			break;
+		}
+
+		printf("%d %d %d\n", static_cast<int>(i), output[i], c);
+		TEST(c == output[i]);
+	}
+
+	TEST(!fclose(fp));
+
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/matrixMult.cpp

@@ -5,67 +5,66 @@ using namespace matrix;
 
 int main()
 {
-    float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
+	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-    Matrix3f A(data);
+	Matrix3f A(data);
 
-    float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
+	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-    Matrix3f A_I(data_check);
+	Matrix3f A_I(data_check);
-    Matrix3f I;
+	Matrix3f I;
-    I.setIdentity();
+	I.setIdentity();
-    Matrix3f R = A * A_I;
+	Matrix3f R = A * A_I;
-    TEST(isEqual(R, I));
+	TEST(isEqual(R, I));
 
-    Matrix3f R2 = A;
+	Matrix3f R2 = A;
-    R2 *= A_I;
+	R2 *= A_I;
-    TEST(isEqual(R2, I));
+	TEST(isEqual(R2, I));
 
-    TEST(R2==I);
+	TEST(R2 == I);
-    TEST(A!=A_I);
+	TEST(A != A_I);
-    Matrix3f A2 = eye<float, 3>()*2;
+	Matrix3f A2 = eye<float, 3>() * 2;
-    Matrix3f B = A2.emult(A2);
+	Matrix3f B = A2.emult(A2);
-    Matrix3f B_check = eye<float, 3>()*4;
+	Matrix3f B_check = eye<float, 3>() * 4;
-    Matrix3f C_check = eye<float, 3>()*2;
+	Matrix3f C_check = eye<float, 3>() * 2;
-    TEST(isEqual(B, B_check));
+	TEST(isEqual(B, B_check));
-    Matrix3f C = B_check.edivide(C_check);
+	Matrix3f C = B_check.edivide(C_check);
 
-    float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2};
+	float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2};
-    // off diagonal are NANs because division by 0
+	// off diagonal are NANs because division by 0
-    TEST(C == Matrix3f(off_diagonal_nan));
+	TEST(C == Matrix3f(off_diagonal_nan));
 
-    // Test non-square matrix
+	// Test non-square matrix
-    float data_43[12] = {1,3,2,
+	float data_43[12] = {1, 3, 2,
-                         2,2,1,
+			     2, 2, 1,
-                         5,2,1,
+			     5, 2, 1,
-                         2,3,4
+			     2, 3, 4
-                        };
+			    };
-    float data_32[6] = {2,3,
+	float data_32[6] = {2, 3,
-                        1,7,
+			    1, 7,
-                        5,4
+			    5, 4
-                       };
+			   };
 
-    Matrix<float, 4, 3> m43(data_43);
+	Matrix<float, 4, 3> m43(data_43);
-    Matrix<float, 3, 2> m32(data_32);
+	Matrix<float, 3, 2> m32(data_32);
 
-    Matrix<float, 4, 2> m42 = m43 * m32;
+	Matrix<float, 4, 2> m42 = m43 * m32;
 
-    float data_42[8] = {15,32,
+	float data_42[8] = {15, 32,
-                        11,24,
+			    11, 24,
-                        17,33,
+			    17, 33,
-                        27,43
+			    27, 43
-                       };
+			   };
-    Matrix<float, 4, 2> m42_check(data_42);
+	Matrix<float, 4, 2> m42_check(data_42);
-    TEST(isEqual(m42, m42_check))
+	TEST(isEqual(m42, m42_check))
 
-    float data_42_plus2[8] = {17,34,
+	float data_42_plus2[8] = {17, 34,
-                              13,26,
+				  13, 26,
-                              19,35,
+				  19, 35,
-                              29,45
+				  29, 45
-                             };
+				 };
-    Matrix<float, 4, 2> m42_plus2_check(data_42_plus2);
+	Matrix<float, 4, 2> m42_plus2_check(data_42_plus2);
-    Matrix<float, 4, 2> m42_plus2 = m42 - (-2);
+	Matrix<float, 4, 2> m42_plus2 = m42 - (-2);
-    TEST(isEqual(m42_plus2, m42_plus2_check));
+	TEST(isEqual(m42_plus2, m42_plus2_check));
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/matrixScalarMult.cpp

@@ -6,13 +6,12 @@ using namespace matrix;
 
 int main()
 {
-    float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-    Matrix3f A(data);
+	Matrix3f A(data);
-    A = A * 2;
+	A = A * 2;
-    float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
-    Matrix3f A_check(data_check);
+	Matrix3f A_check(data_check);
-    TEST(isEqual(A, A_check));
+	TEST(isEqual(A, A_check));
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/pseudoInverse.cpp

@@ -7,147 +7,146 @@ static const size_t n_large = 20;
 
 int main()
 {
-    // 3x4 Matrix test
+	// 3x4 Matrix test
-    float data0[12] = {
+	float data0[12] = {
-        0.f, 1.f,  2.f,  3.f,
+		0.f, 1.f,  2.f,  3.f,
-        4.f, 5.f,  6.f,  7.f,
+		4.f, 5.f,  6.f,  7.f,
-        8.f, 9.f, 10.f, 11.f
+		8.f, 9.f, 10.f, 11.f
-    };
+	};
 
-    float data0_check[12] = {
+	float data0_check[12] = {
-        -0.3375f, -0.1f,  0.1375f,
+		-0.3375f, -0.1f,  0.1375f,
-        -0.13333333f, -0.03333333f,  0.06666667f,
+			-0.13333333f, -0.03333333f,  0.06666667f,
-        0.07083333f,  0.03333333f, -0.00416667f,
+			0.07083333f,  0.03333333f, -0.00416667f,
-        0.275f,  0.1f, -0.075f
+			0.275f,  0.1f, -0.075f
-    };
+		};
 
-    Matrix<float, 3, 4> A0(data0);
+	Matrix<float, 3, 4> A0(data0);
-    Matrix<float, 4, 3> A0_I;
+	Matrix<float, 4, 3> A0_I;
-    bool ret = geninv(A0, A0_I);
+	bool ret = geninv(A0, A0_I);
-    TEST(ret);
+	TEST(ret);
-    Matrix<float, 4, 3> A0_I_check(data0_check);
+	Matrix<float, 4, 3> A0_I_check(data0_check);
 
-    TEST((A0_I - A0_I_check).abs().max() < 1e-5);
+	TEST((A0_I - A0_I_check).abs().max() < 1e-5);
 
-    // 4x3 Matrix test
+	// 4x3 Matrix test
-    float data1[12] = {
+	float data1[12] = {
-        0.f, 4.f, 8.f,
+		0.f, 4.f, 8.f,
-        1.f, 5.f, 9.f,
+		1.f, 5.f, 9.f,
-        2.f, 6.f, 10.f,
+		2.f, 6.f, 10.f,
-        3.f, 7.f, 11.f
+		3.f, 7.f, 11.f
-    };
+	};
 
-    float data1_check[12] = {
+	float data1_check[12] = {
-        -0.3375f, -0.13333333f,  0.07083333f,  0.275f,
+		-0.3375f, -0.13333333f,  0.07083333f,  0.275f,
-        -0.1f, -0.03333333f,  0.03333333f,  0.1f,
+			-0.1f, -0.03333333f,  0.03333333f,  0.1f,
-        0.1375f,  0.06666667f, -0.00416667f, -0.075f
+			0.1375f,  0.06666667f, -0.00416667f, -0.075f
-    };
+		};
 
-    Matrix<float, 4, 3> A1(data1);
+	Matrix<float, 4, 3> A1(data1);
-    Matrix<float, 3, 4> A1_I;
+	Matrix<float, 3, 4> A1_I;
-    ret = geninv(A1, A1_I);
+	ret = geninv(A1, A1_I);
-    TEST(ret);
+	TEST(ret);
-    Matrix<float, 3, 4> A1_I_check(data1_check);
+	Matrix<float, 3, 4> A1_I_check(data1_check);
 
-    TEST((A1_I - A1_I_check).abs().max() < 1e-5);
+	TEST((A1_I - A1_I_check).abs().max() < 1e-5);
 
-    // Stess test
+	// Stess test
-    Matrix<float, n_large, n_large - 1> A_large;
+	Matrix < float, n_large, n_large - 1 > A_large;
-    A_large.setIdentity();
+	A_large.setIdentity();
-    Matrix<float, n_large - 1, n_large> A_large_I;
+	Matrix < float, n_large - 1, n_large > A_large_I;
 
-    for (size_t i = 0; i < n_large; i++) {
+	for (size_t i = 0; i < n_large; i++) {
-        ret = geninv(A_large, A_large_I);
+		ret = geninv(A_large, A_large_I);
-        TEST(ret);
+		TEST(ret);
-        TEST(isEqual(A_large, A_large_I.T()));
+		TEST(isEqual(A_large, A_large_I.T()));
-    }
+	}
 
-    // Square matrix test
+	// Square matrix test
-    float data2[9] = {0, 2, 3,
+	float data2[9] = {0, 2, 3,
-                      4, 5, 6,
+			  4, 5, 6,
-                      7, 8, 10
+			  7, 8, 10
-                     };
+			 };
-    float data2_check[9] = {
+	float data2_check[9] = {
-        -0.4f, -0.8f,  0.6f,
+		-0.4f, -0.8f,  0.6f,
-        -0.4f,  4.2f, -2.4f,
+			-0.4f,  4.2f, -2.4f,
-        0.6f, -2.8f,  1.6f
+			0.6f, -2.8f,  1.6f
-    };
+		};
 
-    SquareMatrix<float, 3> A2(data2);
+	SquareMatrix<float, 3> A2(data2);
-    SquareMatrix<float, 3> A2_I;
+	SquareMatrix<float, 3> A2_I;
-    ret = geninv(A2, A2_I);
+	ret = geninv(A2, A2_I);
-    TEST(ret);
+	TEST(ret);
-    SquareMatrix<float, 3> A2_I_check(data2_check);
+	SquareMatrix<float, 3> A2_I_check(data2_check);
-    TEST((A2_I - A2_I_check).abs().max() < 1e-3);
+	TEST((A2_I - A2_I_check).abs().max() < 1e-3);
 
-    // Null matrix test
+	// Null matrix test
-    Matrix<float, 6, 16> A3;
+	Matrix<float, 6, 16> A3;
-    Matrix<float, 16, 6> A3_I;
+	Matrix<float, 16, 6> A3_I;
-    ret = geninv(A3, A3_I);
+	ret = geninv(A3, A3_I);
-    TEST(ret);
+	TEST(ret);
-    Matrix<float, 16, 6> A3_I_check;
+	Matrix<float, 16, 6> A3_I_check;
-    TEST((A3_I - A3_I_check).abs().max() < 1e-5);
+	TEST((A3_I - A3_I_check).abs().max() < 1e-5);
 
-    // Mock-up effectiveness matrix
+	// Mock-up effectiveness matrix
-    const float B_quad_w[6][16] = {
+	const float B_quad_w[6][16] = {
-        {-0.5717536f,  0.43756646f,  0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-0.5717536f,  0.43756646f,  0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-        { 0.35355328f, -0.35355328f,  0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.35355328f, -0.35355328f,  0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-        { 0.28323701f,  0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.28323701f,  0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-        { 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-        { 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-        {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+		{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
-    };
+	};
-    Matrix<float, 6, 16> B = Matrix<float, 6, 16>(B_quad_w);
+	Matrix<float, 6, 16> B = Matrix<float, 6, 16>(B_quad_w);
-    const float A_quad_w[16][6] = {
+	const float A_quad_w[16][6] = {
-        { -0.495383f,  0.707107f,  0.765306f,  0.0f, 0.0f, -1.000000f },
+		{ -0.495383f,  0.707107f,  0.765306f,  0.0f, 0.0f, -1.000000f },
-        {  0.495383f, -0.707107f,  1.000000f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f, -0.707107f,  1.000000f,  0.0f, 0.0f, -1.000000f },
-        {  0.495383f,  0.707107f, -0.765306f,  0.0f, 0.0f, -1.000000f },
+		{  0.495383f,  0.707107f, -0.765306f,  0.0f, 0.0f, -1.000000f },
-        { -0.495383f, -0.707107f, -1.000000f,  0.0f, 0.0f, -1.000000f },
+		{ -0.495383f, -0.707107f, -1.000000f,  0.0f, 0.0f, -1.000000f },
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
-        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
+		{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
-    };
+	};
-    Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
+	Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
-    Matrix<float, 16, 6> A;
+	Matrix<float, 16, 6> A;
-    ret = geninv(B, A);
+	ret = geninv(B, A);
-    TEST(ret);
+	TEST(ret);
-    TEST((A - A_check).abs().max() < 1e-5);
+	TEST((A - A_check).abs().max() < 1e-5);
 
-    // Real-world test case
+	// Real-world test case
-    const float real_alloc[5][6] = {
+	const float real_alloc[5][6] = {
-        { 0.794079,  0.794079,  0.794079,  0.794079,  0.0000,  0.0000},
+		{ 0.794079,  0.794079,  0.794079,  0.794079,  0.0000,  0.0000},
-        { 0.607814,  0.607814,  0.607814,  0.607814,  1.0000,  1.0000},
+		{ 0.607814,  0.607814,  0.607814,  0.607814,  1.0000,  1.0000},
-        {-0.672516,  0.915642, -0.915642,  0.672516,  0.0000,  0.0000},
+		{-0.672516,  0.915642, -0.915642,  0.672516,  0.0000,  0.0000},
-        { 0.159704,  0.159704,  0.159704,  0.159704, -0.2500, -0.2500},
+		{ 0.159704,  0.159704,  0.159704,  0.159704, -0.2500, -0.2500},
-        { 0.607814, -0.607814,  0.607814, -0.607814,  1.0000,  1.0000}
+		{ 0.607814, -0.607814,  0.607814, -0.607814,  1.0000,  1.0000}
-    };
+	};
-    Matrix<float, 5, 6> real ( real_alloc);
+	Matrix<float, 5, 6> real(real_alloc);
-    Matrix<float, 6, 5> real_pinv;
+	Matrix<float, 6, 5> real_pinv;
-    ret = geninv(real, real_pinv);
+	ret = geninv(real, real_pinv);
-    TEST(ret);
+	TEST(ret);
 
-    // from SVD-based inverse
+	// from SVD-based inverse
-    const float real_pinv_expected_alloc[6][5] = {
+	const float real_pinv_expected_alloc[6][5] = {
-        { 2.096205,  -2.722267,   2.056547,   1.503279,   3.098087},
+		{ 2.096205,  -2.722267,   2.056547,   1.503279,   3.098087},
-        { 1.612621,  -1.992694,   2.056547,   1.131090,   2.275467},
+		{ 1.612621,  -1.992694,   2.056547,   1.131090,   2.275467},
-        {-1.062688,   2.043479,  -2.056547,  -0.927950,  -2.275467},
+		{-1.062688,   2.043479,  -2.056547,  -0.927950,  -2.275467},
-        {-1.546273,   2.773052,  -2.056547,  -1.300139,  -3.098087},
+		{-1.546273,   2.773052,  -2.056547,  -1.300139,  -3.098087},
-        {-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000},
+		{-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000},
-        {-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000}
+		{-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000}
-    };
+	};
-    Matrix<float, 6, 5> real_pinv_expected(real_pinv_expected_alloc);
+	Matrix<float, 6, 5> real_pinv_expected(real_pinv_expected_alloc);
-    TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4);
+	TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4);
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/setIdentity.cpp

@@ -5,35 +5,34 @@ using namespace matrix;
 
 int main()
 {
-    Matrix3f A;
+	Matrix3f A;
-    A.setIdentity();
+	A.setIdentity();
 
-    for (size_t i = 0; i < 3; i++) {
+	for (size_t i = 0; i < 3; i++) {
-        for (size_t j = 0; j < 3; j++) {
+		for (size_t j = 0; j < 3; j++) {
-            if (i == j) {
+			if (i == j) {
-                TEST(fabs(A(i, j) -  1) < FLT_EPSILON);
+				TEST(fabs(A(i, j) -  1) < FLT_EPSILON);
 
-            } else {
+			} else {
-                TEST(fabs(A(i, j) -  0) < FLT_EPSILON);
+				TEST(fabs(A(i, j) -  0) < FLT_EPSILON);
-            }
+			}
-        }
+		}
-    }
+	}
 
-    Matrix3f B;
+	Matrix3f B;
-    B.identity();
+	B.identity();
 
-    for (size_t i = 0; i < 3; i++) {
+	for (size_t i = 0; i < 3; i++) {
-        for (size_t j = 0; j < 3; j++) {
+		for (size_t j = 0; j < 3; j++) {
-            if (i == j) {
+			if (i == j) {
-                TEST(fabs(B(i, j) -  1) < FLT_EPSILON);
+				TEST(fabs(B(i, j) -  1) < FLT_EPSILON);
 
-            } else {
+			} else {
-                TEST(fabs(B(i, j) -  0) < FLT_EPSILON);
+				TEST(fabs(B(i, j) -  0) < FLT_EPSILON);
-            }
+			}
-        }
+		}
-    }
+	}
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/slice.cpp

@@ -8,228 +8,229 @@ template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage
 
 int main()
 {
-    float data[9] = {0, 2, 3,
+	float data[9] = {0, 2, 3,
-                     4, 5, 6,
+			 4, 5, 6,
-                     7, 8, 10
+			 7, 8, 10
-                    };
+			};
-    SquareMatrix<float, 3> A(data);
+	SquareMatrix<float, 3> A(data);
 
-    // Test row slicing
+	// Test row slicing
-    Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
+	Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
-    float data_check_rowslice[6] = {
+	float data_check_rowslice[6] = {
-        4, 5, 6,
+		4, 5, 6,
-        7, 8, 10
+		7, 8, 10
-    };
+	};
-    Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
+	Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
-    TEST(isEqual(B_rowslice, B_check_rowslice));
+	TEST(isEqual(B_rowslice, B_check_rowslice));
 
-    // Test column slicing
+	// Test column slicing
-    Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
+	Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
-    float data_check_colslice[6] = {
+	float data_check_colslice[6] = {
-        2, 3,
+		2, 3,
-        5, 6,
+		5, 6,
-        8, 10
+		8, 10
-    };
+	};
-    Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
+	Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
-    TEST(isEqual(B_colslice, B_check_colslice));
+	TEST(isEqual(B_colslice, B_check_colslice));
 
-    // Test slicing both
+	// Test slicing both
-    Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
+	Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
-    float data_check_bothslice[4] = {
+	float data_check_bothslice[4] = {
-        5, 6,
+		5, 6,
-        8, 10
+		8, 10
-    };
+	};
-    Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
+	Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
-    TEST(isEqual(B_bothslice, B_check_bothslice));
+	TEST(isEqual(B_bothslice, B_check_bothslice));
 
-    //Test block writing
+	//Test block writing
-    float data_2[4] = {
+	float data_2[4] = {
-        11, 12,
+		11, 12,
-        13, 14
+		13, 14
-    };
+	};
 
-    Matrix<float, 2, 2> C(data_2);
+	Matrix<float, 2, 2> C(data_2);
-    A.slice<2, 2>(1, 1) = C;
+	A.slice<2, 2>(1, 1) = C;
 
-    float data_2_check[9] = {
+	float data_2_check[9] = {
-        0, 2, 3,
+		0, 2, 3,
-        4, 11, 12,
+		4, 11, 12,
-        7, 13, 14
+		7, 13, 14
-    };
+	};
-    Matrix<float, 3, 3> D(data_2_check);
+	Matrix<float, 3, 3> D(data_2_check);
-    TEST(isEqual(A, D));
+	TEST(isEqual(A, D));
 
-    //Test writing to slices
+	//Test writing to slices
-    Matrix<float, 3, 1> E;
+	Matrix<float, 3, 1> E;
-    E(0,0) = -1;
+	E(0, 0) = -1;
-    E(1,0) = 1;
+	E(1, 0) = 1;
-    E(2,0) = 3;
+	E(2, 0) = 3;
 
-    Matrix<float, 2, 1> F;
+	Matrix<float, 2, 1> F;
-    F(0,0) = 9;
+	F(0, 0) = 9;
-    F(1,0) = 11;
+	F(1, 0) = 11;
 
-    E.slice<2,1>(0,0) = F;
+	E.slice<2, 1>(0, 0) = F;
 
-    float data_3_check[3] = {9, 11, 3};
+	float data_3_check[3] = {9, 11, 3};
-    Matrix<float, 3, 1> G (data_3_check);
+	Matrix<float, 3, 1> G(data_3_check);
-    TEST(isEqual(E, G));
+	TEST(isEqual(E, G));
-    TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
+	TEST(isEqual(E, Matrix<float, 3, 1>(E.slice<3, 1>(0, 0))));
 
-    Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
+	Matrix<float, 2, 1> H = E.slice<2, 1>(0, 0);
-    TEST(isEqual(H,F));
+	TEST(isEqual(H, F));
 
-    float data_4_check[5] = {3, 11, 9, 0, 0};
+	float data_4_check[5] = {3, 11, 9, 0, 0};
-    {   // assigning row slices to each other
+	{
-        const Matrix<float, 3, 1> J (data_3_check);
+		// assigning row slices to each other
-        Matrix<float, 5, 1> K;
+		const Matrix<float, 3, 1> J(data_3_check);
-        K.row(2) = J.row(0);
+		Matrix<float, 5, 1> K;
-        K.row(1) = J.row(1);
+		K.row(2) = J.row(0);
-        K.row(0) = J.row(2);
+		K.row(1) = J.row(1);
+		K.row(0) = J.row(2);
 
-        Matrix<float, 5, 1> K_check(data_4_check);
+		Matrix<float, 5, 1> K_check(data_4_check);
-        TEST(isEqual(K, K_check));
+		TEST(isEqual(K, K_check));
-    }
+	}
-    {   // assigning col slices to each other
+	{
-        const Matrix<float, 1, 3> J (data_3_check);
+		// assigning col slices to each other
-        Matrix<float, 1, 5> K;
+		const Matrix<float, 1, 3> J(data_3_check);
-        K.col(2) = J.col(0);
+		Matrix<float, 1, 5> K;
-        K.col(1) = J.col(1);
+		K.col(2) = J.col(0);
-        K.col(0) = J.col(2);
+		K.col(1) = J.col(1);
+		K.col(0) = J.col(2);
 
-        Matrix<float, 1, 5> K_check(data_4_check);
+		Matrix<float, 1, 5> K_check(data_4_check);
-        TEST(isEqual(K, K_check));
+		TEST(isEqual(K, K_check));
-    }
+	}
 
-    // check that slice of a slice works for reading
+	// check that slice of a slice works for reading
-    const Matrix<float, 3, 3> cm33(data);
+	const Matrix<float, 3, 3> cm33(data);
-    Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2);
+	Matrix<float, 2, 1> topRight = cm33.slice<2, 3>(0, 0).slice<2, 1>(0, 2);
-    float top_right_check[2] = {3,6};
+	float top_right_check[2] = {3, 6};
-    TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
+	TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
 
-    // check that slice of a slice works for writing
+	// check that slice of a slice works for writing
-    Matrix<float, 3, 3> m33(data);
+	Matrix<float, 3, 3> m33(data);
-    m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>();
+	m33.slice<2, 3>(0, 0).slice<2, 1>(0, 2) = Matrix<float, 2, 1>();
-    const float data_check[9] = {0, 2, 0,
+	const float data_check[9] = {0, 2, 0,
-                                 4, 5, 0,
+				     4, 5, 0,
-                                 7, 8, 10
+				     7, 8, 10
-                                };
+				    };
-    TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
+	TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
 
-    // longerThan
+	// longerThan
-    Vector3f v5;
+	Vector3f v5;
-    v5(0) = 3;
+	v5(0) = 3;
-    v5(1) = 4;
+	v5(1) = 4;
-    v5(2) = 9;
+	v5(2) = 9;
-    TEST(v5.xy().longerThan(4.99f));
+	TEST(v5.xy().longerThan(4.99f));
-    TEST(!v5.xy().longerThan(5.f));
+	TEST(!v5.xy().longerThan(5.f));
-    TEST(isEqualF(5.f, v5.xy().norm()));
+	TEST(isEqualF(5.f, v5.xy().norm()));
 
-    // min/max
+	// min/max
-    TEST(m33.row(1).max() == 5);
+	TEST(m33.row(1).max() == 5);
-    TEST(m33.col(0).min() == 0);
+	TEST(m33.col(0).min() == 0);
-    TEST((m33.slice<2,2>(1,1).max()) == 10);
+	TEST((m33.slice<2, 2>(1, 1).max()) == 10);
 
-    // assign scalar value to slice
+	// assign scalar value to slice
-    Matrix<float, 3, 1> L;
+	Matrix<float, 3, 1> L;
-    L(0,0) = -1;
+	L(0, 0) = -1;
-    L(1,0) = 1;
+	L(1, 0) = 1;
-    L(2,0) = 3;
+	L(2, 0) = 3;
 
-    L.slice<2,1>(0,0) = 0.0f;
+	L.slice<2, 1>(0, 0) = 0.0f;
 
-    float data_5_check[3] = {0, 0, 3};
+	float data_5_check[3] = {0, 0, 3};
-    Matrix<float, 3, 1> M (data_5_check);
+	Matrix<float, 3, 1> M(data_5_check);
-    TEST(isEqual(L, M));
+	TEST(isEqual(L, M));
 
-    // return diagonal elements
+	// return diagonal elements
-    float data_6[9] = {0, 2, 3,
+	float data_6[9] = {0, 2, 3,
-                       4, 5, 6,
+			   4, 5, 6,
-                       7, 8, 10
+			   7, 8, 10
-                      };
+			  };
-    SquareMatrix<float, 3> N(data_6);
+	SquareMatrix<float, 3> N(data_6);
 
-    Vector3f v6 = N.slice<3,3>(0,0).diag();
+	Vector3f v6 = N.slice<3, 3>(0, 0).diag();
-    Vector3f v6_check = {0, 5, 10};
+	Vector3f v6_check = {0, 5, 10};
-    TEST(isEqual(v6,v6_check));
+	TEST(isEqual(v6, v6_check));
-    Vector2f v7 = N.slice<2,3>(1,0).diag();
+	Vector2f v7 = N.slice<2, 3>(1, 0).diag();
-    Vector2f v7_check = {4, 8};
+	Vector2f v7_check = {4, 8};
-    TEST(isEqual(v7,v7_check));
+	TEST(isEqual(v7, v7_check));
-    Vector2f v8 = N.slice<3,2>(0,1).diag();
+	Vector2f v8 = N.slice<3, 2>(0, 1).diag();
-    Vector2f v8_check = {2, 6};
+	Vector2f v8_check = {2, 6};
-    TEST(isEqual(v8,v8_check));
+	TEST(isEqual(v8, v8_check));
-    Vector2f v9(N.slice<1,2>(1,1));
+	Vector2f v9(N.slice<1, 2>(1, 1));
-    Vector2f v9_check = {5, 6};
+	Vector2f v9_check = {5, 6};
-    TEST(isEqual(v9,v9_check));
+	TEST(isEqual(v9, v9_check));
-    Vector3f v10(N.slice<1,3>(1,0));
+	Vector3f v10(N.slice<1, 3>(1, 0));
-    Vector3f v10_check = {4, 5, 6};
+	Vector3f v10_check = {4, 5, 6};
-    TEST(isEqual(v10,v10_check));
+	TEST(isEqual(v10, v10_check));
 
-    // Different assignment operators
+	// Different assignment operators
-    SquareMatrix3f O(data);
+	SquareMatrix3f O(data);
-    float operand_data [4] = {2, 1, -3, -1};
+	float operand_data [4] = {2, 1, -3, -1};
-    const SquareMatrix<float, 2> operand(operand_data);
+	const SquareMatrix<float, 2> operand(operand_data);
 
-    O.slice<2,2>(1,0) += operand;
+	O.slice<2, 2>(1, 0) += operand;
-    float O_check_data_1 [9] = {0, 2, 3,  6, 6, 6,  4, 7, 10};
+	float O_check_data_1 [9] = {0, 2, 3,  6, 6, 6,  4, 7, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<2,1>(1,1) += operand.slice<2,1>(0,0);
+	O.slice<2, 1>(1, 1) += operand.slice<2, 1>(0, 0);
-    float O_check_data_2 [9] = {0, 2, 3,  4, 7, 6,  7, 5, 10};
+	float O_check_data_2 [9] = {0, 2, 3,  4, 7, 6,  7, 5, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<3,3>(0,0) += -1;
+	O.slice<3, 3>(0, 0) += -1;
-    float O_check_data_3 [9] = {-1, 1, 2,  3, 4, 5,  6, 7, 9};
+	float O_check_data_3 [9] = {-1, 1, 2,  3, 4, 5,  6, 7, 9};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.col(1) += Vector3f{1, -2, 3};
+	O.col(1) += Vector3f{1, -2, 3};
-    float O_check_data_4 [9] = {0, 3, 3,  4, 3, 6,  7, 11, 10};
+	float O_check_data_4 [9] = {0, 3, 3,  4, 3, 6,  7, 11, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<2,2>(1,0) -= operand;
+	O.slice<2, 2>(1, 0) -= operand;
-    float O_check_data_5 [9] = {0, 2, 3,  2, 4, 6,  10, 9, 10};
+	float O_check_data_5 [9] = {0, 2, 3,  2, 4, 6,  10, 9, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<2,1>(1,1) -= operand.slice<2,1>(0,0);
+	O.slice<2, 1>(1, 1) -= operand.slice<2, 1>(0, 0);
-    float O_check_data_6 [9] = {0, 2, 3,  4, 3, 6,  7, 11, 10};
+	float O_check_data_6 [9] = {0, 2, 3,  4, 3, 6,  7, 11, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<3,3>(0,0) -= -1;
+	O.slice<3, 3>(0, 0) -= -1;
-    float O_check_data_7 [9] = {1, 3, 4,  5, 6, 7,  8, 9, 11};
+	float O_check_data_7 [9] = {1, 3, 4,  5, 6, 7,  8, 9, 11};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.col(1) -= Vector3f{1, -2, 3};
+	O.col(1) -= Vector3f{1, -2, 3};
-    float O_check_data_8 [9] = {0, 1, 3,  4, 7, 6,  7, 5, 10};
+	float O_check_data_8 [9] = {0, 1, 3,  4, 7, 6,  7, 5, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<2,1>(1,1) *= 5.f;
+	O.slice<2, 1>(1, 1) *= 5.f;
-    float O_check_data_9 [9] = {0, 2, 3,  4, 25, 6,  7, 40, 10};
+	float O_check_data_9 [9] = {0, 2, 3,  4, 25, 6,  7, 40, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    O.slice<2,1>(1,1) /= 2.f;
+	O.slice<2, 1>(1, 1) /= 2.f;
-    float O_check_data_10 [9] = {0, 2, 3,  4, 2.5, 6,  7, 4, 10};
+	float O_check_data_10 [9] = {0, 2, 3,  4, 2.5, 6,  7, 4, 10};
-    TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
+	TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
 
-    // Different operations
+	// Different operations
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    SquareMatrix<float, 2> res_11(O.slice<2,2>(1,1) * 2.f);
+	SquareMatrix<float, 2> res_11(O.slice<2, 2>(1, 1) * 2.f);
-    float O_check_data_11 [4] = {10, 12, 16, 20};
+	float O_check_data_11 [4] = {10, 12, 16, 20};
-    TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11)));
+	TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11)));
 
-    O = SquareMatrix3f(data);
+	O = SquareMatrix3f(data);
-    SquareMatrix<float, 2> res_12(O.slice<2,2>(1,1) / 2.f);
+	SquareMatrix<float, 2> res_12(O.slice<2, 2>(1, 1) / 2.f);
-    float O_check_data_12 [4] = {2.5, 3, 4, 5};
+	float O_check_data_12 [4] = {2.5, 3, 4, 5};
-    TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12)));
+	TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12)));
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/sparseVector.cpp

@@ -3,117 +3,131 @@
 
 using namespace matrix;
 
-TEST(sparseVectorTest, defaultConstruction) {
+TEST(sparseVectorTest, defaultConstruction)
-    SparseVectorf<24, 4, 6> a;
+{
-    EXPECT_EQ(a.non_zeros(), 2);
+	SparseVectorf<24, 4, 6> a;
-    EXPECT_EQ(a.index(0), 4);
+	EXPECT_EQ(a.non_zeros(), 2);
-    EXPECT_EQ(a.index(1), 6);
+	EXPECT_EQ(a.index(0), 4);
-    a.at<4>() = 1.f;
+	EXPECT_EQ(a.index(1), 6);
-    a.at<6>() = 2.f;
+	a.at<4>() = 1.f;
+	a.at<6>() = 2.f;
 }
 
-TEST(sparseVectorTest, initializationWithData) {
+TEST(sparseVectorTest, initializationWithData)
-    const float data[3] = {1.f, 2.f, 3.f};
+{
-    SparseVectorf<24, 4, 6, 22> a(data);
+	const float data[3] = {1.f, 2.f, 3.f};
-    EXPECT_EQ(a.non_zeros(), 3);
+	SparseVectorf<24, 4, 6, 22> a(data);
-    EXPECT_EQ(a.index(0), 4);
+	EXPECT_EQ(a.non_zeros(), 3);
-    EXPECT_EQ(a.index(1), 6);
+	EXPECT_EQ(a.index(0), 4);
-    EXPECT_EQ(a.index(2), 22);
+	EXPECT_EQ(a.index(1), 6);
-    EXPECT_FLOAT_EQ(a.at<4>(), data[0]);
+	EXPECT_EQ(a.index(2), 22);
-    EXPECT_FLOAT_EQ(a.at<6>(), data[1]);
+	EXPECT_FLOAT_EQ(a.at<4>(), data[0]);
-    EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
+	EXPECT_FLOAT_EQ(a.at<6>(), data[1]);
+	EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
 }
 
-TEST(sparseVectorTest, initialisationFromVector) {
+TEST(sparseVectorTest, initialisationFromVector)
-    const Vector3f vec(1.f, 2.f, 3.f);
+{
-    const SparseVectorf<3, 0, 2> a(vec);
+	const Vector3f vec(1.f, 2.f, 3.f);
-    EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
+	const SparseVectorf<3, 0, 2> a(vec);
-    EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
+	EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
+	EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
 }
 
-TEST(sparseVectorTest, accessDataWithCompressedIndices) {
+TEST(sparseVectorTest, accessDataWithCompressedIndices)
-    const Vector3f vec(1.f, 2.f, 3.f);
+{
-    SparseVectorf<3, 0, 2> a(vec);
+	const Vector3f vec(1.f, 2.f, 3.f);
-    for (size_t i = 0; i < a.non_zeros(); i++) {
+	SparseVectorf<3, 0, 2> a(vec);
-        a.atCompressedIndex(i) = static_cast<float>(i);
+
-    }
+	for (size_t i = 0; i < a.non_zeros(); i++) {
-    EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
+		a.atCompressedIndex(i) = static_cast<float>(i);
-    EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
+	}
+
+	EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
+	EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
 }
 
-TEST(sparseVectorTest, setZero) {
+TEST(sparseVectorTest, setZero)
-    const float data[3] = {1.f, 2.f, 3.f};
+{
-    SparseVectorf<24, 4, 6, 22> a(data);
+	const float data[3] = {1.f, 2.f, 3.f};
-    a.setZero();
+	SparseVectorf<24, 4, 6, 22> a(data);
-    EXPECT_FLOAT_EQ(a.at<4>(), 0.f);
+	a.setZero();
-    EXPECT_FLOAT_EQ(a.at<6>(), 0.f);
+	EXPECT_FLOAT_EQ(a.at<4>(), 0.f);
-    EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
+	EXPECT_FLOAT_EQ(a.at<6>(), 0.f);
+	EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
 }
 
-TEST(sparseVectorTest, additionWithDenseVector) {
+TEST(sparseVectorTest, additionWithDenseVector)
-    Vector<float, 4> dense_vec;
+{
-    dense_vec.setAll(1.f);
+	Vector<float, 4> dense_vec;
-    const float data[3] = {1.f, 2.f, 3.f};
+	dense_vec.setAll(1.f);
-    const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+	const float data[3] = {1.f, 2.f, 3.f};
-    const Vector<float, 4> res = sparse_vec + dense_vec;
+	const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
-    EXPECT_FLOAT_EQ(res(0), 1.f);
+	const Vector<float, 4> res = sparse_vec + dense_vec;
-    EXPECT_FLOAT_EQ(res(1), 2.f);
+	EXPECT_FLOAT_EQ(res(0), 1.f);
-    EXPECT_FLOAT_EQ(res(2), 3.f);
+	EXPECT_FLOAT_EQ(res(1), 2.f);
-    EXPECT_FLOAT_EQ(res(3), 4.f);
+	EXPECT_FLOAT_EQ(res(2), 3.f);
+	EXPECT_FLOAT_EQ(res(3), 4.f);
 }
 
-TEST(sparseVectorTest, addScalar) {
+TEST(sparseVectorTest, addScalar)
-    const float data[3] = {1.f, 2.f, 3.f};
+{
-    SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+	const float data[3] = {1.f, 2.f, 3.f};
-    sparse_vec += 2.f;
+	SparseVectorf<4, 1, 2, 3> sparse_vec(data);
-    EXPECT_FLOAT_EQ(sparse_vec.at<1>(), 3.f);
+	sparse_vec += 2.f;
-    EXPECT_FLOAT_EQ(sparse_vec.at<2>(), 4.f);
+	EXPECT_FLOAT_EQ(sparse_vec.at<1>(), 3.f);
-    EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
+	EXPECT_FLOAT_EQ(sparse_vec.at<2>(), 4.f);
+	EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
 }
 
-TEST(sparseVectorTest, dotProductWithDenseVector) {
+TEST(sparseVectorTest, dotProductWithDenseVector)
-    Vector<float, 4> dense_vec;
+{
-    dense_vec.setAll(3.f);
+	Vector<float, 4> dense_vec;
-    const float data[3] = {1.f, 2.f, 3.f};
+	dense_vec.setAll(3.f);
-    const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+	const float data[3] = {1.f, 2.f, 3.f};
-    float res = sparse_vec.dot(dense_vec);
+	const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
-    EXPECT_FLOAT_EQ(res, 18.f);
+	float res = sparse_vec.dot(dense_vec);
+	EXPECT_FLOAT_EQ(res, 18.f);
 }
 
-TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
+TEST(sparseVectorTest, multiplicationWithDenseMatrix)
-    Matrix<float, 2, 3> dense_matrix;
+{
-    dense_matrix.setAll(2.f);
+	Matrix<float, 2, 3> dense_matrix;
-    dense_matrix(1, 1) = 3.f;
+	dense_matrix.setAll(2.f);
-    const Vector3f dense_vec(0.f, 1.f, 5.f);
+	dense_matrix(1, 1) = 3.f;
-    const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
+	const Vector3f dense_vec(0.f, 1.f, 5.f);
-    const Vector<float, 2> res_sparse = dense_matrix * sparse_vec;
+	const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
-    const Vector<float, 2> res_dense = dense_matrix * dense_vec;
+	const Vector<float, 2> res_sparse = dense_matrix * sparse_vec;
-    EXPECT_TRUE(isEqual(res_dense, res_sparse));
+	const Vector<float, 2> res_dense = dense_matrix * dense_vec;
+	EXPECT_TRUE(isEqual(res_dense, res_sparse));
 }
 
-TEST(sparseVectorTest, quadraticForm) {
+TEST(sparseVectorTest, quadraticForm)
-    float matrix_data[9] = {1, 2, 3,
+{
-                            2, 4, 5,
+	float matrix_data[9] = {1, 2, 3,
-                            3, 5, 6
+				2, 4, 5,
-                           };
+				3, 5, 6
-    const SquareMatrix<float, 3> dense_matrix(matrix_data);
+			       };
-    const Vector3f dense_vec(0.f, 1.f, 5.f);
+	const SquareMatrix<float, 3> dense_matrix(matrix_data);
-    const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
+	const Vector3f dense_vec(0.f, 1.f, 5.f);
-    EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
+	const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
+	EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
 }
 
-TEST(sparseVectorTest, norms) {
+TEST(sparseVectorTest, norms)
-    const float data[2] = {3.f, 4.f};
+{
-    const SparseVectorf<4, 1, 3> sparse_vec(data);
+	const float data[2] = {3.f, 4.f};
-    EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
+	const SparseVectorf<4, 1, 3> sparse_vec(data);
-    EXPECT_FLOAT_EQ(sparse_vec.norm(), 5.f);
+	EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
-    EXPECT_TRUE(sparse_vec.longerThan(4.5f));
+	EXPECT_FLOAT_EQ(sparse_vec.norm(), 5.f);
-    EXPECT_FALSE(sparse_vec.longerThan(5.5f));
+	EXPECT_TRUE(sparse_vec.longerThan(4.5f));
+	EXPECT_FALSE(sparse_vec.longerThan(5.5f));
 }
 
 
-int main(int argc, char **argv) {
+int main(int argc, char **argv)
-    testing::InitGoogleTest(&argc, argv);
+{
-    std::cout << "Run SparseVector tests" << std::endl;
+	testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
+	std::cout << "Run SparseVector tests" << std::endl;
+	return RUN_ALL_TESTS();
 }

src/lib/matrix/test/squareMatrix.cpp

@@ -6,143 +6,142 @@ using namespace matrix;
 
 int main()
 {
-    float data[9] = {1, 2, 3,
+	float data[9] = {1, 2, 3,
-                     4, 5, 6,
+			 4, 5, 6,
-                     7, 8, 10
+			 7, 8, 10
-                    };
+			};
-    SquareMatrix<float, 3> A(data);
+	SquareMatrix<float, 3> A(data);
-    Vector3<float> diag_check(1, 5, 10);
+	Vector3<float> diag_check(1, 5, 10);
 
-    TEST(isEqual(A.diag(), diag_check));
+	TEST(isEqual(A.diag(), diag_check));
-    TEST(A.trace() - 16 < FLT_EPSILON);
+	TEST(A.trace() - 16 < FLT_EPSILON);
 
-    float data_check[9] = {
+	float data_check[9] = {
-        1.01158503f,  0.02190432f,  0.03238144f,
+		1.01158503f,  0.02190432f,  0.03238144f,
-        0.04349195f,  1.05428524f,  0.06539627f,
+		0.04349195f,  1.05428524f,  0.06539627f,
-        0.07576783f,  0.08708946f,  1.10894048f
+		0.07576783f,  0.08708946f,  1.10894048f
-    };
+	};
 
-    float dt = 0.01f;
+	float dt = 0.01f;
-    SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A*dt), 5);
+	SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
-    SquareMatrix<float, 3> eA_check(data_check);
+	SquareMatrix<float, 3> eA_check(data_check);
-    TEST((eA - eA_check).abs().max() < 1e-3f);
+	TEST((eA - eA_check).abs().max() < 1e-3f);
 
-    SquareMatrix<float, 2> A_bottomright = A.slice<2,2>(1,1);
+	SquareMatrix<float, 2> A_bottomright = A.slice<2, 2>(1, 1);
-    SquareMatrix<float, 2> A_bottomright2;
+	SquareMatrix<float, 2> A_bottomright2;
-    A_bottomright2 = A.slice<2,2>(1,1);
+	A_bottomright2 = A.slice<2, 2>(1, 1);
 
-    float data_bottomright[4] = {5, 6,
+	float data_bottomright[4] = {5, 6,
-                                 8, 10
+				     8, 10
-                                };
+				    };
-    SquareMatrix<float, 2> bottomright_check(data_bottomright);
+	SquareMatrix<float, 2> bottomright_check(data_bottomright);
-    TEST(isEqual(A_bottomright, bottomright_check));
+	TEST(isEqual(A_bottomright, bottomright_check));
-    TEST(isEqual(A_bottomright2, bottomright_check));
+	TEST(isEqual(A_bottomright2, bottomright_check));
 
-    // test diagonal functions
+	// test diagonal functions
-    float data_4x4[16] = {1, 2, 3, 4,
+	float data_4x4[16] = {1, 2, 3, 4,
-                          5, 6, 7, 8,
+			      5, 6, 7, 8,
-                          9, 10, 11, 12,
+			      9, 10, 11, 12,
-                          13, 14,15, 16
+			      13, 14, 15, 16
-                         };
+			     };
 
-    SquareMatrix<float, 4> B(data_4x4);
+	SquareMatrix<float, 4> B(data_4x4);
-    B.uncorrelateCovariance<1>(1);
+	B.uncorrelateCovariance<1>(1);
-    float data_B_check[16] = {1, 0, 3, 4,
+	float data_B_check[16] = {1, 0, 3, 4,
-                              0, 6, 0, 0,
+				  0, 6, 0, 0,
-                              9, 0, 11, 12,
+				  9, 0, 11, 12,
-                              13, 0,15, 16
+				  13, 0, 15, 16
-                             };
+				 };
-    SquareMatrix<float, 4> B_check(data_B_check);
+	SquareMatrix<float, 4> B_check(data_B_check);
-    TEST(isEqual(B, B_check))
+	TEST(isEqual(B, B_check))
 
-    SquareMatrix<float, 4> C(data_4x4);
+	SquareMatrix<float, 4> C(data_4x4);
-    C.uncorrelateCovariance<2>(1);
+	C.uncorrelateCovariance<2>(1);
-    float data_C_check[16] = {1, 0, 0, 4,
+	float data_C_check[16] = {1, 0, 0, 4,
-                              0, 6, 0, 0,
+				  0, 6, 0, 0,
-                              0, 0, 11, 0,
+				  0, 0, 11, 0,
-                              13, 0,0, 16
+				  13, 0, 0, 16
-                             };
+				 };
-    SquareMatrix<float, 4> C_check(data_C_check);
+	SquareMatrix<float, 4> C_check(data_C_check);
-    TEST(isEqual(C, C_check))
+	TEST(isEqual(C, C_check))
 
-    SquareMatrix<float, 4> D(data_4x4);
+	SquareMatrix<float, 4> D(data_4x4);
-    D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20,21});
+	D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20, 21});
-    float data_D_check[16] = {20, 0, 0, 0,
+	float data_D_check[16] = {20, 0, 0, 0,
-                              0, 21, 0, 0,
+				  0, 21, 0, 0,
-                              0, 0, 11, 12,
+				  0, 0, 11, 12,
-                              0, 0,15, 16
+				  0, 0, 15, 16
-                             };
+				 };
-    SquareMatrix<float, 4> D_check(data_D_check);
+	SquareMatrix<float, 4> D_check(data_D_check);
-    TEST(isEqual(D, D_check))
+	TEST(isEqual(D, D_check))
 
-    SquareMatrix<float, 4> E(data_4x4);
+	SquareMatrix<float, 4> E(data_4x4);
-    E.uncorrelateCovarianceSetVariance<3>(1, 33);
+	E.uncorrelateCovarianceSetVariance<3>(1, 33);
-    float data_E_check[16] = {1, 0, 0, 0,
+	float data_E_check[16] = {1, 0, 0, 0,
-                              0, 33, 0, 0,
+				  0, 33, 0, 0,
-                              0, 0, 33, 0,
+				  0, 0, 33, 0,
-                              0, 0,0, 33
+				  0, 0, 0, 33
-                             };
+				 };
-    SquareMatrix<float, 4> E_check(data_E_check);
+	SquareMatrix<float, 4> E_check(data_E_check);
-    TEST(isEqual(E, E_check))
+	TEST(isEqual(E, E_check))
 
-    // test symmetric functions
+	// test symmetric functions
-    SquareMatrix<float, 4> F(data_4x4);
+	SquareMatrix<float, 4> F(data_4x4);
-    F.makeBlockSymmetric<2>(1);
+	F.makeBlockSymmetric<2>(1);
-    float data_F_check[16] = {1, 2, 3, 4,
+	float data_F_check[16] = {1, 2, 3, 4,
-                              5, 6, 8.5, 8,
+				  5, 6, 8.5, 8,
-                              9, 8.5, 11, 12,
+				  9, 8.5, 11, 12,
-                              13, 14,15, 16
+				  13, 14, 15, 16
-                             };
+				 };
-    SquareMatrix<float, 4> F_check(data_F_check);
+	SquareMatrix<float, 4> F_check(data_F_check);
-    TEST(isEqual(F, F_check))
+	TEST(isEqual(F, F_check))
-    TEST(F.isBlockSymmetric<2>(1));
+	TEST(F.isBlockSymmetric<2>(1));
-    TEST(!F.isRowColSymmetric<2>(1));
+	TEST(!F.isRowColSymmetric<2>(1));
 
-    SquareMatrix<float, 4> G(data_4x4);
+	SquareMatrix<float, 4> G(data_4x4);
-    G.makeRowColSymmetric<2>(1);
+	G.makeRowColSymmetric<2>(1);
-    float data_G_check[16] = {1, 3.5, 6, 4,
+	float data_G_check[16] = {1, 3.5, 6, 4,
-                              3.5, 6, 8.5, 11,
+				  3.5, 6, 8.5, 11,
-                              6, 8.5, 11, 13.5,
+				  6, 8.5, 11, 13.5,
-                              13, 11,13.5, 16
+				  13, 11, 13.5, 16
-                             };
+				 };
-    SquareMatrix<float, 4> G_check(data_G_check);
+	SquareMatrix<float, 4> G_check(data_G_check);
-    TEST(isEqual(G, G_check));
+	TEST(isEqual(G, G_check));
-    TEST(G.isBlockSymmetric<2>(1));
+	TEST(G.isBlockSymmetric<2>(1));
-    TEST(G.isRowColSymmetric<2>(1));
+	TEST(G.isRowColSymmetric<2>(1));
 
-    SquareMatrix<float, 4> H(data_4x4);
+	SquareMatrix<float, 4> H(data_4x4);
-    H.makeBlockSymmetric<1>(1);
+	H.makeBlockSymmetric<1>(1);
-    float data_H_check[16] = {1, 2, 3, 4,
+	float data_H_check[16] = {1, 2, 3, 4,
-                              5, 6, 7, 8,
+				  5, 6, 7, 8,
-                              9, 10, 11, 12,
+				  9, 10, 11, 12,
-                              13, 14,15, 16
+				  13, 14, 15, 16
-                             };
+				 };
-    SquareMatrix<float, 4> H_check(data_H_check);
+	SquareMatrix<float, 4> H_check(data_H_check);
-    TEST(isEqual(H, H_check))
+	TEST(isEqual(H, H_check))
-    TEST(H.isBlockSymmetric<1>(1));
+	TEST(H.isBlockSymmetric<1>(1));
-    TEST(!H.isRowColSymmetric<1>(1));
+	TEST(!H.isRowColSymmetric<1>(1));
 
-    SquareMatrix<float, 4> J(data_4x4);
+	SquareMatrix<float, 4> J(data_4x4);
-    J.makeRowColSymmetric<1>(1);
+	J.makeRowColSymmetric<1>(1);
-    float data_J_check[16] = {1, 3.5, 3, 4,
+	float data_J_check[16] = {1, 3.5, 3, 4,
-                              3.5, 6, 8.5, 11,
+				  3.5, 6, 8.5, 11,
-                              9, 8.5, 11, 12,
+				  9, 8.5, 11, 12,
-                              13, 11,15, 16
+				  13, 11, 15, 16
-                             };
+				 };
-    SquareMatrix<float, 4> J_check(data_J_check);
+	SquareMatrix<float, 4> J_check(data_J_check);
-    TEST(isEqual(J, J_check));
+	TEST(isEqual(J, J_check));
-    TEST(J.isBlockSymmetric<1>(1));
+	TEST(J.isBlockSymmetric<1>(1));
-    TEST(J.isRowColSymmetric<1>(1));
+	TEST(J.isRowColSymmetric<1>(1));
-    TEST(!J.isBlockSymmetric<3>(1));
+	TEST(!J.isBlockSymmetric<3>(1));
 
-    float data_K[16] = {1, 2, 3, 4,
+	float data_K[16] = {1, 2, 3, 4,
-                        2, 3, 4, 11,
+			    2, 3, 4, 11,
-                        3, 4, 11, 12,
+			    3, 4, 11, 12,
-                        4, 11,15, 16
+			    4, 11, 15, 16
-                       };
+			   };
-    SquareMatrix<float, 4> K(data_K);
+	SquareMatrix<float, 4> K(data_K);
-    TEST(!K.isRowColSymmetric<1>(2));
+	TEST(!K.isRowColSymmetric<1>(2));
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/test_data.py

@@ -145,5 +145,3 @@ print('b:')
 pprint(b)
 print('x:')
 pprint(x)
-
-# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 : 

src/lib/matrix/test/transpose.cpp

@@ -6,14 +6,13 @@ using namespace matrix;
 
 int main()
 {
-    float data[6] = {1, 2, 3, 4, 5, 6};
+	float data[6] = {1, 2, 3, 4, 5, 6};
-    Matrix<float, 2, 3> A(data);
+	Matrix<float, 2, 3> A(data);
-    Matrix<float, 3, 2> A_T = A.transpose();
+	Matrix<float, 3, 2> A_T = A.transpose();
-    float data_check[6] = {1, 4, 2, 5, 3, 6};
+	float data_check[6] = {1, 4, 2, 5, 3, 6};
-    Matrix<float, 3, 2> A_T_check(data_check);
+	Matrix<float, 3, 2> A_T_check(data_check);
-    TEST(isEqual(A_T, A_T_check));
+	TEST(isEqual(A_T, A_T_check));
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/upperRightTriangle.cpp

@@ -5,19 +5,18 @@ using namespace matrix;
 
 int main()
 {
-    float data[9] = {1, 2, 3,
+	float data[9] = {1, 2, 3,
-                     4, 5, 6,
+			 4, 5, 6,
-                     7, 8, 10
+			 7, 8, 10
-                    };
+			};
-    float urt[6] = {1, 2, 3, 5, 6, 10};
+	float urt[6] = {1, 2, 3, 5, 6, 10};
 
-    SquareMatrix<float, 3> A(data);
+	SquareMatrix<float, 3> A(data);
 
-    for(size_t i=0; i<6; i++) {
+	for (size_t i = 0; i < 6; i++) {
-        TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON);
+		TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON);
-    }
+	}
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vector.cpp

@@ -6,43 +6,42 @@ using namespace matrix;
 
 int main()
 {
-    // test data
+	// test data
-    float data1[] = {1,2,3,4,5};
+	float data1[] = {1, 2, 3, 4, 5};
-    float data2[] = {6,7,8,9,10};
+	float data2[] = {6, 7, 8, 9, 10};
-    Vector<float, 5> v1(data1);
+	Vector<float, 5> v1(data1);
-    Vector<float, 5> v2(data2);
+	Vector<float, 5> v2(data2);
 
-    // copy constructor
+	// copy constructor
-    Vector<float, 5> v3(v2);
+	Vector<float, 5> v3(v2);
-    TEST(isEqual(v2, v3));
+	TEST(isEqual(v2, v3));
 
-    // norm, dot product
+	// norm, dot product
-    TEST(isEqualF(v1.norm(), 7.416198487095663f));
+	TEST(isEqualF(v1.norm(), 7.416198487095663f));
-    TEST(isEqualF(v1.norm_squared(), v1.norm() * v1.norm()));
+	TEST(isEqualF(v1.norm_squared(), v1.norm() * v1.norm()));
-    TEST(isEqualF(v1.norm(), v1.length()));
+	TEST(isEqualF(v1.norm(), v1.length()));
-    TEST(isEqualF(v1.dot(v2), 130.0f));
+	TEST(isEqualF(v1.dot(v2), 130.0f));
-    TEST(isEqualF(v1.dot(v2), v1 * v2));
+	TEST(isEqualF(v1.dot(v2), v1 * v2));
 
-    // unit, unit_zero, normalize
+	// unit, unit_zero, normalize
-    TEST(isEqualF(v2.unit().norm(), 1.f));
+	TEST(isEqualF(v2.unit().norm(), 1.f));
-    TEST(isEqualF(v2.unit_or_zero().norm(), 1.f));
+	TEST(isEqualF(v2.unit_or_zero().norm(), 1.f));
-    TEST(isEqualF(Vector<float, 5>().unit_or_zero().norm(), 0.f));
+	TEST(isEqualF(Vector<float, 5>().unit_or_zero().norm(), 0.f));
-    v2.normalize();
+	v2.normalize();
-    TEST(isEqualF(v2.norm(), 1.f));
+	TEST(isEqualF(v2.norm(), 1.f));
 
-    // sqrt
+	// sqrt
-    float data1_sq[] = {1,4,9,16,25};
+	float data1_sq[] = {1, 4, 9, 16, 25};
-    Vector<float, 5> v4(data1_sq);
+	Vector<float, 5> v4(data1_sq);
-    TEST(isEqual(v1, v4.sqrt()));
+	TEST(isEqual(v1, v4.sqrt()));
 
-    // longerThan
+	// longerThan
-    Vector<float, 2> v5;
+	Vector<float, 2> v5;
-    v5(0) = 3;
+	v5(0) = 3;
-    v5(1) = 4;
+	v5(1) = 4;
-    TEST(v5.longerThan(4.99f));
+	TEST(v5.longerThan(4.99f));
-    TEST(!v5.longerThan(5.f));
+	TEST(!v5.longerThan(5.f));
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vector2.cpp

@@ -8,33 +8,32 @@ using namespace matrix;
 
 int main()
 {
-    Vector2f a(1, 0);
+	Vector2f a(1, 0);
-    Vector2f b(0, 1);
+	Vector2f b(0, 1);
-    TEST(fabs(a % b - 1.0f) < FLT_EPSILON);
+	TEST(fabs(a % b - 1.0f) < FLT_EPSILON);
 
-    Vector2f c;
+	Vector2f c;
-    TEST(fabs(c(0) - 0) < FLT_EPSILON);
+	TEST(fabs(c(0) - 0) < FLT_EPSILON);
-    TEST(fabs(c(1) - 0) < FLT_EPSILON);
+	TEST(fabs(c(1) - 0) < FLT_EPSILON);
 
-    Matrix<float, 2, 1> d(a);
+	Matrix<float, 2, 1> d(a);
-    TEST(fabs(d(0,0) - 1) < FLT_EPSILON);
+	TEST(fabs(d(0, 0) - 1) < FLT_EPSILON);
-    TEST(fabs(d(1,0) - 0) < FLT_EPSILON);
+	TEST(fabs(d(1, 0) - 0) < FLT_EPSILON);
 
-    Vector2f e(d);
+	Vector2f e(d);
-    TEST(fabs(e(0) - 1) < FLT_EPSILON);
+	TEST(fabs(e(0) - 1) < FLT_EPSILON);
-    TEST(fabs(e(1) - 0) < FLT_EPSILON);
+	TEST(fabs(e(1) - 0) < FLT_EPSILON);
 
-    float data[] = {4,5};
+	float data[] = {4, 5};
-    Vector2f f(data);
+	Vector2f f(data);
-    TEST(fabs(f(0) - 4) < FLT_EPSILON);
+	TEST(fabs(f(0) - 4) < FLT_EPSILON);
-    TEST(fabs(f(1) - 5) < FLT_EPSILON);
+	TEST(fabs(f(1) - 5) < FLT_EPSILON);
 
-    Vector3f g(1.23f, 423.4f, 3221.f);
+	Vector3f g(1.23f, 423.4f, 3221.f);
-    Vector2f h(g);
+	Vector2f h(g);
-    TEST(fabs(h(0) - 1.23f) < FLT_EPSILON);
+	TEST(fabs(h(0) - 1.23f) < FLT_EPSILON);
-    TEST(fabs(h(1) - 423.4f) < FLT_EPSILON);
+	TEST(fabs(h(1) - 423.4f) < FLT_EPSILON);
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vector3.cpp

@@ -6,56 +6,55 @@ using namespace matrix;
 
 int main()
 {
-    Vector3f a(1, 0, 0);
+	Vector3f a(1, 0, 0);
-    Vector3f b(0, 1, 0);
+	Vector3f b(0, 1, 0);
-    Vector3f c = a.cross(b);
+	Vector3f c = a.cross(b);
-    TEST(isEqual(c, Vector3f(0,0,1)));
+	TEST(isEqual(c, Vector3f(0, 0, 1)));
-    c = a % b;
+	c = a % b;
-    TEST(isEqual(c, Vector3f(0,0,1)));
+	TEST(isEqual(c, Vector3f(0, 0, 1)));
-    Matrix<float, 3, 1> d(c);
+	Matrix<float, 3, 1> d(c);
-    Vector3f e(d);
+	Vector3f e(d);
-    TEST(isEqual(e, d));
+	TEST(isEqual(e, d));
-    float data[] = {4, 5, 6};
+	float data[] = {4, 5, 6};
-    Vector3f f(data);
+	Vector3f f(data);
-    TEST(isEqual(f, Vector3f(4, 5, 6)));
+	TEST(isEqual(f, Vector3f(4, 5, 6)));
 
-    TEST(isEqual(a + b, Vector3f(1, 1, 0)));
+	TEST(isEqual(a + b, Vector3f(1, 1, 0)));
-    TEST(isEqual(a - b, Vector3f(1, -1, 0)));
+	TEST(isEqual(a - b, Vector3f(1, -1, 0)));
-    TEST(isEqualF(a * b, 0.0f));
+	TEST(isEqualF(a * b, 0.0f));
-    TEST(isEqual(-a, Vector3f(-1, 0, 0)));
+	TEST(isEqual(-a, Vector3f(-1, 0, 0)));
-    TEST(isEqual(a.unit(), a));
+	TEST(isEqual(a.unit(), a));
-    TEST(isEqual(a.unit(), a.normalized()));
+	TEST(isEqual(a.unit(), a.normalized()));
-    TEST(isEqual(a*2.0, Vector3f(2, 0, 0)));
+	TEST(isEqual(a * 2.0, Vector3f(2, 0, 0)));
 
-    Vector2f g2(1,3);
+	Vector2f g2(1, 3);
-    Vector3f g3(7, 11, 17);
+	Vector3f g3(7, 11, 17);
-    g3.xy() = g2;
+	g3.xy() = g2;
-    TEST(isEqual(g3, Vector3f(1, 3, 17)));
+	TEST(isEqual(g3, Vector3f(1, 3, 17)));
 
-    const Vector3f g4(g3);
+	const Vector3f g4(g3);
-    Vector2f g5 = g4.xy();
+	Vector2f g5 = g4.xy();
-    TEST(isEqual(g5,g2));
+	TEST(isEqual(g5, g2));
-    TEST(isEqual(g2,Vector2f(g4.xy())));
+	TEST(isEqual(g2, Vector2f(g4.xy())));
 
-    Vector3f h;
+	Vector3f h;
-    TEST(isEqual(h,Vector3f(0,0,0)));
+	TEST(isEqual(h, Vector3f(0, 0, 0)));
 
-    Vector<float, 4> j;
+	Vector<float, 4> j;
-    j(0) = 1;
+	j(0) = 1;
-    j(1) = 2;
+	j(1) = 2;
-    j(2) = 3;
+	j(2) = 3;
-    j(3) = 4;
+	j(3) = 4;
 
-    Vector3f k = j.slice<3,1>(0,0);
+	Vector3f k = j.slice<3, 1>(0, 0);
-    Vector3f k_test(1,2,3);
+	Vector3f k_test(1, 2, 3);
-    TEST(isEqual(k,k_test));
+	TEST(isEqual(k, k_test));
 
-    Vector3f m1(1, 2, 3);
+	Vector3f m1(1, 2, 3);
-    Vector3f m2(3.1f, 4.1f, 5.1f);
+	Vector3f m2(3.1f, 4.1f, 5.1f);
-    TEST(isEqual(m2, m1 + 2.1f));
+	TEST(isEqual(m2, m1 + 2.1f));
-    TEST(isEqual(m2 - 2.1f, m1));
+	TEST(isEqual(m2 - 2.1f, m1));
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vectorAssignment.cpp

@@ -6,29 +6,28 @@ using namespace matrix;
 
 int main()
 {
-    Vector3f v;
+	Vector3f v;
-    v(0) = 1;
+	v(0) = 1;
-    v(1) = 2;
+	v(1) = 2;
-    v(2) = 3;
+	v(2) = 3;
 
-    static const float eps = 1e-7f;
+	static const float eps = 1e-7f;
 
-    TEST(fabs(v(0) - 1) < eps);
+	TEST(fabs(v(0) - 1) < eps);
-    TEST(fabs(v(1) - 2) < eps);
+	TEST(fabs(v(1) - 2) < eps);
-    TEST(fabs(v(2) - 3) < eps);
+	TEST(fabs(v(2) - 3) < eps);
 
-    Vector3f v2(4, 5, 6);
+	Vector3f v2(4, 5, 6);
 
-    TEST(fabs(v2(0) - 4) < eps);
+	TEST(fabs(v2(0) - 4) < eps);
-    TEST(fabs(v2(1) - 5) < eps);
+	TEST(fabs(v2(1) - 5) < eps);
-    TEST(fabs(v2(2) - 6) < eps);
+	TEST(fabs(v2(2) - 6) < eps);
 
-    SquareMatrix<float, 3> m = diag(Vector3f(1,2,3));
+	SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
-    TEST(fabs(m(0, 0) - 1) < eps);
+	TEST(fabs(m(0, 0) - 1) < eps);
-    TEST(fabs(m(1, 1) - 2) < eps);
+	TEST(fabs(m(1, 1) - 2) < eps);
-    TEST(fabs(m(2, 2) - 3) < eps);
+	TEST(fabs(m(2, 2) - 3) < eps);
 
-    return 0;
+	return 0;
 }
 
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */