diff --git a/src/basics/matrix.cc b/src/basics/matrix.cc index 5a3798b..6c24450 100644 --- a/src/basics/matrix.cc +++ b/src/basics/matrix.cc @@ -10,6 +10,7 @@ #include "basics/matrix.hh" #include "basics/util.hh" +#include "basics/vector.hh" namespace charles { @@ -47,9 +48,9 @@ Matrix4::Identity() * charles::basics::TranslationMatrix -- */ /* static */ Matrix4 -Translation(Double x, - Double y, - Double z) +Matrix4::Translation(Double x, + Double y, + Double z) { Matrix4 m = Matrix4::Identity(); m(0,3) = x; @@ -129,14 +130,15 @@ Matrix4::operator!=(const Matrix4 &rhs) * charles::basics::Matrix4::operator() -- */ Double& -Matrix4::operator()(UInt i, UInt j) +Matrix4::operator()(UInt i, + UInt j) { if (i >= 4 || j >= 4) { std::stringstream ss; ss << "matrix index out of bounds: i = " << i << ", j = " << j; throw std::out_of_range(ss.str()); } - return mData[i * 4 + j]; + return mData[i*4 + j]; } @@ -144,10 +146,16 @@ Matrix4::operator()(UInt i, UInt j) * charles::basics::Matrix4::operator() -- */ Double -Matrix4::operator()(UInt i, UInt j) +Matrix4::operator()(UInt i, + UInt j) const { - return operator()(i, j); + if (i >= 4 || j >= 4) { + std::stringstream ss; + ss << "matrix index out of bounds: i = " << i << ", j = " << j; + throw std::out_of_range(ss.str()); + } + return mData[i*4 + j]; } @@ -229,6 +237,24 @@ Matrix4::operator*(const Matrix4& rhs) } +/* + * charles::basics::Matrix4::operator* -- + */ +Vector4 +Matrix4::operator*(const Vector4 &rhs) + const +{ + Vector4 result; + for (UInt i = 0; i < 4; i++) { + result(i) = 0.0; + for (UInt k = 0; k < 4; k++) { + result(i) += mData[i*4 + k] * rhs(k); + } + } + return result; +} + + /* * charles::basics::operator* -- */ diff --git a/src/basics/matrix.hh b/src/basics/matrix.hh index c7224a9..2128e45 100644 --- a/src/basics/matrix.hh +++ b/src/basics/matrix.hh @@ -80,7 +80,7 @@ protected: /** Scalar multiplication, scalar factor on the left. */ -Matrix4 operator*(const Double &lhs, const Matrix4 &rhs); +Matrix4 operator*(Double lhs, const Matrix4 &rhs); } /* namespace basics */ } /* namespace charles */