Using new SimpleMathV2

src/SimpleMath/SimpleMath.h

@@ -48,6 +48,19 @@ struct MatrixBase {
   typedef MatrixBase<Derived, ScalarType, Rows, Cols> MatrixType;
   typedef ScalarType value_type;
 
+  Derived& operator=(const Derived& other) {
+    if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
+      for (size_t i = 0; i < other.rows(); i++) {
+        for (size_t j = 0; j < other.cols(); j++) {
+          this->operator()(i,j) = other(i,j);
+        }
+      }
+    }
+
+    return *this;
+  }
+
+
   template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
   Derived& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
     if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
@@ -64,8 +77,8 @@ struct MatrixBase {
   //
   // operators with scalars
   //
-	Derived operator*(const double& scalar) const {
+  Matrix<ScalarType, Rows, Cols> operator*(const double& scalar) const {
-		Derived result (rows(), cols());
+    Matrix<ScalarType, Rows, Cols> result (rows(), cols());
 
     unsigned int i,j;
     for (i = 0; i < rows(); i++) {
@@ -76,8 +89,8 @@ struct MatrixBase {
     return result;
   }
 
-	Derived operator*(const float& scalar) const {
+  Matrix<ScalarType, Rows, Cols> operator*(const float& scalar) const {
-		Derived result (rows(), cols());
+    Matrix<ScalarType, Rows, Cols> result (rows(), cols());
 
     unsigned int i,j;
     for (i = 0; i < rows(); i++) {
@@ -91,8 +104,8 @@ struct MatrixBase {
   //
   // operators with other matrices
   //
-	template <typename OtherDerived>
+  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
-	bool operator==(const OtherDerived& other) const {
+  bool operator==(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
     unsigned int i,j;
     for (i = 0; i < rows(); i++) {
       for (j = 0; j < cols(); j++) {
@@ -104,6 +117,11 @@ struct MatrixBase {
     return true;
   }
 
+  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+  bool operator!=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
+    return !(operator==(other));
+  }
+
   CommaInitializer<Derived> operator<< (const value_type& value) {
     return CommaInitializer<Derived> (*(static_cast<Derived*>(this)), value);
   }
@@ -147,20 +165,29 @@ struct MatrixBase {
   }
 
   template <typename OtherDerived>
-	Derived operator*=(const OtherDerived& other) {
+  Derived operator*=(const OtherDerived &other) {
-	    Derived copy (*static_cast<Derived*>(this));
+    Derived copy (*static_cast<const Derived*>(this));
-//		Derived result (Derived::Zero(rows(), other.cols()));
+    this->setZero();
 
-		unsigned int i,j,k;
+    unsigned int i, j, k;
     for (i = 0; i < rows(); i++) {
       for (j = 0; j < other.cols(); j++) {
         for (k = 0; k < other.rows(); k++) {
-					operator()(i,j) += copy.operator()(i,k) * other(k,j);
+          this->operator()(i, j) += copy.operator()(i, k) * other(k, j);
         }
       }
     }
+    return *this;
+  }
 
-  	return *this;
+  Matrix<ScalarType, Rows, Cols> operator-() const {
+    Matrix<ScalarType, Rows, Cols> copy (*static_cast<const Derived*>(this));
+    for (int i = 0; i < rows(); i++) {
+      for (int j = 0; j < cols(); j++) {
+        copy(i,j) *= static_cast<ScalarType>(-1.);
+      }
+    }
+    return copy;
   }
 
   Derived operator*=(const ScalarType& s) {
@@ -174,6 +201,45 @@ struct MatrixBase {
     return *this;
   }
 
+  void resize(unsigned int nrows, unsigned int ncols = 1) {
+    static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed.");
+
+    // Resize the this matrix (so far only possible for subclasses of the
+    // Matrix class)
+    Matrix<ScalarType, Rows, Cols>* this_matrix = static_cast<Matrix<ScalarType, Rows, Cols>*>(this);
+    this_matrix->mStorage.resize(nrows, ncols);
+  }
+
+  void conservativeResize(unsigned int nrows, unsigned int ncols = 1) {
+    static_assert(Rows == Dynamic, "Resize of fixed size matrices not allowed.");
+
+
+    Derived copy(*this);
+
+    unsigned int arows = std::min(nrows, (unsigned int) rows());
+    unsigned int acols = std::min(ncols, (unsigned int) cols());
+
+    resize(nrows, ncols);
+    setZero();
+
+    // TODO: set entries to zero within the loop
+    for (unsigned int i = 0; i < arows; i++) {
+      for (unsigned int j = 0; j < acols; j++) {
+        this->operator()(i, j) = copy(i,j);
+      }
+    }
+  }
+
+  void setZero() {
+    int nrows = rows();
+    int ncols = cols();
+    for (int i = 0; i < nrows; i++) {
+      for (int j = 0; j < ncols; j++) {
+        operator()(i,j) = static_cast<ScalarType>(0.0);
+      }
+    }
+  }
+
   void set(const ScalarType& v0) {
     static_assert(cols() * rows() == 1, "Invalid matrix size");
     data()[0] = v0;
@@ -205,13 +271,127 @@ struct MatrixBase {
     data()[3] = v3;
   }
 
+  void set(
+      const ScalarType& v0,
+      const ScalarType& v1,
+      const ScalarType& v2,
+      const ScalarType& v3,
+      const ScalarType& v4,
+      const ScalarType& v5
+      ) {
+    assert(cols() * rows() == 6);
+
+    data()[0] = v0;
+    data()[1] = v1;
+    data()[2] = v2;
+    data()[3] = v3;
+    data()[4] = v4;
+    data()[5] = v5;
+  }
+
+  void set(
+      const ScalarType& v00,
+      const ScalarType& v01,
+      const ScalarType& v02,
+      const ScalarType& v03,
+      const ScalarType& v04,
+      const ScalarType& v05,
+
+      const ScalarType& v10,
+      const ScalarType& v11,
+      const ScalarType& v12,
+      const ScalarType& v13,
+      const ScalarType& v14,
+      const ScalarType& v15,
+
+      const ScalarType& v20,
+      const ScalarType& v21,
+      const ScalarType& v22,
+      const ScalarType& v23,
+      const ScalarType& v24,
+      const ScalarType& v25,
+
+      const ScalarType& v30,
+      const ScalarType& v31,
+      const ScalarType& v32,
+      const ScalarType& v33,
+      const ScalarType& v34,
+      const ScalarType& v35,
+
+      const ScalarType& v40,
+      const ScalarType& v41,
+      const ScalarType& v42,
+      const ScalarType& v43,
+      const ScalarType& v44,
+      const ScalarType& v45,
+
+      const ScalarType& v50,
+      const ScalarType& v51,
+      const ScalarType& v52,
+      const ScalarType& v53,
+      const ScalarType& v54,
+      const ScalarType& v55
+      ) {
+    assert(cols() == 6 && rows() == 6);
+
+    operator()(0,0) = v00;
+    operator()(0,1) = v01;
+    operator()(0,2) = v02;
+    operator()(0,3) = v03;
+    operator()(0,4) = v04;
+    operator()(0,5) = v05;
+
+    operator()(1,0) = v10;
+    operator()(1,1) = v11;
+    operator()(1,2) = v12;
+    operator()(1,3) = v13;
+    operator()(1,4) = v14;
+    operator()(1,5) = v15;
+
+    operator()(2,0) = v20;
+    operator()(2,1) = v21;
+    operator()(2,2) = v22;
+    operator()(2,3) = v23;
+    operator()(2,4) = v24;
+    operator()(2,5) = v25;
+
+    operator()(3,0) = v30;
+    operator()(3,1) = v31;
+    operator()(3,2) = v32;
+    operator()(3,3) = v33;
+    operator()(3,4) = v34;
+    operator()(3,5) = v35;
+
+    operator()(4,0) = v40;
+    operator()(4,1) = v41;
+    operator()(4,2) = v42;
+    operator()(4,3) = v43;
+    operator()(4,4) = v44;
+    operator()(4,5) = v45;
+
+    operator()(5,0) = v50;
+    operator()(5,1) = v51;
+    operator()(5,2) = v52;
+    operator()(5,3) = v53;
+    operator()(5,4) = v54;
+    operator()(5,5) = v55;
+
+   }
+
+
+
   size_t rows() const {
     return static_cast<const Derived*>(this)->rows();
   }
+
   size_t cols() const {
     return static_cast<const Derived*>(this)->cols();
   }
 
+  size_t size() const {
+    return static_cast<const Derived*>(this)->rows() * static_cast<const Derived*>(this)->cols();
+  }
+
   const ScalarType& operator()(const size_t& i, const size_t& j) const {
     return static_cast<const Derived*>(this)->operator()(i,j);
   }
@@ -230,10 +410,13 @@ struct MatrixBase {
 
   operator ScalarType() const {
 #ifndef NDEBUG
+    if ( static_cast<const Derived*>(this)->cols() != 1
+        || static_cast<const Derived*>(this)->rows() != 1) {
       std::cout << "Error trying to cast to scalar type. Dimensions are: "
         << static_cast<const Derived*>(this)->rows() << ", "  
         << static_cast<const Derived*>(this)->cols() << "."  
         << std::endl;
+    }
 #endif
     assert ( static_cast<const Derived*>(this)->cols() == 1
         && static_cast<const Derived*>(this)->rows() == 1);
@@ -311,6 +494,24 @@ struct MatrixBase {
       return colPivHouseholderQr().inverse();
     }
 
+    ScalarType trace() const {
+      assert(rows() == cols());
+
+      ScalarType result = static_cast<ScalarType>(0.0);
+
+      for (unsigned int i = 0; i < rows(); i++) {
+        result += operator()(i,i);
+      }
+
+      return result;
+    }
+
+    // TODO: implement Cholesky decompositioe
+    const HouseholderQR<Derived, ScalarType, Rows, Cols> llt() const {
+      std::cerr << "LLT decomposition uses householder!" << std::endl;
+        return HouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
+    }
+
     const HouseholderQR<Derived, ScalarType, Rows, Cols> householderQr() const {
         return HouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
     }
@@ -352,7 +553,40 @@ struct MatrixBase {
     return result;
   }
 
-	// TODO: Random()
+  static Derived Constant(int NumRows, const ScalarType &value) {
+    Derived result (NumRows, 1);
+
+    for (size_t i = 0; i < NumRows; i++) {
+      result(i,0) = value;
+    }
+
+    return result;
+  }
+
+  static Derived Constant(int NumRows, int NumCols, const ScalarType &value) {
+    Derived result (NumRows, NumCols);
+
+    for (size_t i = 0; i < NumRows; i++) {
+      for (size_t j = 0; j < NumCols; j++) {
+        result(i,j) = value;
+      }
+    }
+
+    return result;
+  }
+
+  static Derived Random(int NumRows = (Rows == Dynamic) ? 1 : Rows, int NumCols = (Cols == Dynamic) ? 1 : Cols) {
+    Derived result (NumRows, NumCols);
+
+    for (size_t i = 0; i < NumRows; i++) {
+      for (size_t j = 0; j < NumCols; j++) {
+        result(i,j) = (static_cast<value_type>(rand()) / static_cast<value_type>(RAND_MAX)) * 2.0 - 1.0;
+      }
+    }
+
+    return result;
+  }
+
 
   //
   // Block accessors
@@ -398,7 +632,7 @@ struct MatrixBase {
   }
 
   const Block<
-		Derived,
+    const Derived,
     ScalarType
     > block(int block_row_index, int block_col_index,
         int block_num_rows, int block_num_cols) const {
@@ -422,22 +656,6 @@ struct MatrixBase {
 };
 
 
-
-template <typename Derived, typename ScalarType, int Rows, int Cols>
-inline Derived operator*(const ScalarType& scalar, const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix) {
-    return matrix * scalar;
-}
-
-template <typename Derived, typename ScalarType, int Rows, int Cols>
-inline Derived operator*(const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
-    return matrix * scalar;
-}
-
-template <typename Derived, typename ScalarType, int Rows, int Cols>
-inline Derived operator/(const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
-    return matrix * (1.0 / scalar);
-}
-
 template <typename ScalarType, int SizeAtCompileTime, int NumRows, int NumCols>
 struct Storage;
 
@@ -565,7 +783,9 @@ struct Storage<ScalarType, 0, Dynamic, Dynamic> {
 
 
 template <typename ScalarType, int NumRows, int NumCols>
-struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols>{
+struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols> {
+  typedef Matrix DerivedBase;
+
   enum {
     RowsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumRows,
     ColsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumCols,
@@ -580,27 +800,43 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
         SizeAtCompileTime / RowsAtCompileTime
         ) {}
 
-    explicit Matrix(int rows, int cols) :
+  explicit Matrix(int rows, int cols = 1) :
     mStorage(rows, cols) {}
 
-    explicit Matrix (size_t rows, size_t cols) :
+  explicit Matrix(unsigned int rows, unsigned int cols = 1) :
     mStorage(rows, cols) {}
 
-    template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+  explicit Matrix (size_t rows, size_t cols = 1) :
-	Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
+    mStorage(rows, cols) {}
+
+  template<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+  Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) {
     mStorage.resize(other.rows(), other.cols());
 
     for (size_t i = 0; i < rows(); i++) {
       for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) = other(i,j);
+        this->operator()(i, j) = other(i, j);
       }
     }
   }
 
+    Matrix (const Matrix& other) :
+            mStorage(other.rows(), other.cols()){
+                    memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols());
+            }
+
+
+    Matrix& operator=(const Matrix& other) {
+      if (&other != this) {
+          mStorage.resize(other.rows(), other.cols());
+          memcpy (data(), other.data(), sizeof (ScalarType) * rows() * cols());
+      }
+      return *this;
+  }
+
   //
   // Constructor for vectors
   //
-
   Matrix (
       const ScalarType& v0
       ) {
@@ -645,6 +881,24 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
     operator()(3,0) = v3;
   }
 
+  Matrix (
+      const ScalarType& v0,
+      const ScalarType& v1,
+      const ScalarType& v2,
+      const ScalarType& v3,
+      const ScalarType& v4,
+      const ScalarType& v5
+      ) {
+    static_assert (NumRows * NumCols == 6, "Invalid matrix size");
+
+    operator()(0,0) = v0;
+    operator()(1,0) = v1;
+    operator()(2,0) = v2;
+    operator()(3,0) = v3;
+    operator()(4,0) = v4;
+    operator()(5,0) = v5;
+  }
+
   //
   // Constructor for matrices
   //
@@ -715,9 +969,97 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
     operator()(3,3) = v33;
   }
 
+  Matrix (
+      const ScalarType& v00,
+      const ScalarType& v01,
+      const ScalarType& v02,
+      const ScalarType& v03,
+      const ScalarType& v04,
+      const ScalarType& v05,
+
+      const ScalarType& v10,
+      const ScalarType& v11,
+      const ScalarType& v12,
+      const ScalarType& v13,
+      const ScalarType& v14,
+      const ScalarType& v15,
+
+      const ScalarType& v20,
+      const ScalarType& v21,
+      const ScalarType& v22,
+      const ScalarType& v23,
+      const ScalarType& v24,
+      const ScalarType& v25,
+
+      const ScalarType& v30,
+      const ScalarType& v31,
+      const ScalarType& v32,
+      const ScalarType& v33,
+      const ScalarType& v34,
+      const ScalarType& v35,
+
+      const ScalarType& v40,
+      const ScalarType& v41,
+      const ScalarType& v42,
+      const ScalarType& v43,
+      const ScalarType& v44,
+      const ScalarType& v45,
+
+      const ScalarType& v50,
+      const ScalarType& v51,
+      const ScalarType& v52,
+      const ScalarType& v53,
+      const ScalarType& v54,
+      const ScalarType& v55
+        ) {
+          static_assert (NumRows == 6 && NumCols == 6, "Invalid matrix size");
+
+          operator()(0,0) = v00;
+          operator()(0,1) = v01;
+          operator()(0,2) = v02;
+          operator()(0,3) = v03;
+          operator()(0,4) = v04;
+          operator()(0,5) = v05;
+
+          operator()(1,0) = v10;
+          operator()(1,1) = v11;
+          operator()(1,2) = v12;
+          operator()(1,3) = v13;
+          operator()(1,4) = v14;
+          operator()(1,5) = v15;
+
+          operator()(2,0) = v20;
+          operator()(2,1) = v21;
+          operator()(2,2) = v22;
+          operator()(2,3) = v23;
+          operator()(2,4) = v24;
+          operator()(2,5) = v25;
+
+          operator()(3,0) = v30;
+          operator()(3,1) = v31;
+          operator()(3,2) = v32;
+          operator()(3,3) = v33;
+          operator()(3,4) = v34;
+          operator()(3,5) = v35;
+
+          operator()(4,0) = v40;
+          operator()(4,1) = v41;
+          operator()(4,2) = v42;
+          operator()(4,3) = v43;
+          operator()(4,4) = v44;
+          operator()(4,5) = v45;
+
+          operator()(5,0) = v50;
+          operator()(5,1) = v51;
+          operator()(5,2) = v52;
+          operator()(5,3) = v53;
+          operator()(5,4) = v54;
+          operator()(5,5) = v55;
+        }
+
   template <typename OtherDerived>
     Matrix& operator+=(const OtherDerived& other) {
-		assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!");
+      assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
 
       for (size_t i = 0; i < rows(); i++) {
         for (size_t j = 0; j < cols(); j++) {
@@ -958,6 +1300,17 @@ struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, S
     return *this;
   }
 
+//  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+//  Matrix<ScalarType, NumRows, OtherCols>& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
+//    unsigned int i,j,k;
+//    for (i = 0; i < rows(); i++) {
+//      for (j = 0; j < other.cols(); j++) {
+//        operator()(i,k) = other(k,j);
+//      }
+//    }
+//    return *this;
+//  }
+
   template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
   Matrix<ScalarType, NumRows, OtherCols> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
     Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
@@ -1037,13 +1390,13 @@ private:
     Derived mR;
 
 public:
-    HouseholderQR(const MatrixType &matrix) :
+    HouseholderQR(const Derived &matrix) :
             mIsFactorized(false),
             mQ(matrix.rows(), matrix.rows())
     {
         compute(matrix);
     }
-    HouseholderQR compute(const MatrixType &matrix) {
+    HouseholderQR compute(const Derived& matrix) {
         mR = matrix;
         mQ = MatrixType::Identity (mR.rows(), mR.rows());
 
@@ -1339,6 +1692,26 @@ public:
     }
 };
 
+template <typename Derived, typename ScalarType, int Rows, int Cols>
+inline Matrix<ScalarType, Rows, Cols> operator*(const ScalarType& scalar, const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix) {
+    return matrix * scalar;
+}
+
+template <typename Derived, typename ScalarType, int Rows, int Cols>
+inline Matrix<ScalarType, Rows, Cols> operator*(const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
+    return matrix * scalar;
+}
+
+template <typename Derived, typename ScalarType, int Rows, int Cols>
+inline Matrix<ScalarType, Rows, Cols> operator/(const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
+    return matrix * (1.0 / scalar);
+}
+
+template <typename Derived, typename ScalarType, int Rows, int Cols>
+inline Matrix<ScalarType, Rows, Cols> operator/=(MatrixBase<Derived, ScalarType, Rows, Cols> &matrix, const ScalarType& scalar) {
+    return matrix *= (1.0 / scalar);
+}
+
 //
 // OpenGL Matrices and Quaternions
 //
@@ -1702,7 +2075,6 @@ inline std::ostream& operator<<(std::ostream& output, const MatrixBase<Derived,
 
   for (unsigned int i = 0; i < matrix.rows(); i++) {
     output.width(0);
-		output << "[ ";
     output.width(out_width);
     for (unsigned int j = 0; j < matrix.cols(); j++) {
       std::stringstream out_stream;
@@ -1711,9 +2083,8 @@ inline std::ostream& operator<<(std::ostream& output, const MatrixBase<Derived,
       output << out_stream.str();
 
       if (j < matrix.cols() - 1)
-				output << ", ";
+        output << " ";
     }
-		output << " ]";
     
     if (matrix.rows() > 1 && i < matrix.rows() - 1)
       output << std::endl;

src/modules/RenderModule.cc

@@ -284,9 +284,9 @@ void Light::UpdateSplits(const Camera& camera) {
 	Matrix44f light_matrix = LookAt (mPosition, mPosition + mDirection, Vector3f (0.f, 1.0f, 0.0f));
 	Matrix44f light_matrix_inv = light_matrix.inverse();
 
-	mShadowSplits[0] = near + length * 0.02;
+	mShadowSplits[0] = near + length * 0.05;
-	mShadowSplits[1] = near + length * 0.1;
+	mShadowSplits[1] = near + length * 0.15;
-	mShadowSplits[2] = near + length * 0.3;
+	mShadowSplits[2] = near + length * 0.4;
 	mShadowSplits[3] = far;
 
 	float prev_split_far = near;