Skip to content

Commit

Permalink
added rotator matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@sevec
sevec committed Jul 4, 2020
1 parent a51a569 commit cf4a9bb
Showing 1 changed file with 71 additions and 79 deletions.
150 changes: 71 additions & 79 deletions Matrix.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,95 +193,75 @@ inline Mat33f invert(const Mat33f& m) {

inline float determinant(const Mat44f& m) {
Mat44f minv; /// \todo
minv(0, 0) = m(1, 1) * m(2, 2) * m(3, 3) - m(1, 1) * m(3, 2) * m(2, 3) -
m(1, 2) * m(2, 1) * m(3, 3) + m(1, 2) * m(3, 1) * m(2, 3) +
m(1, 3) * m(2, 1) * m(3, 2) - m(1, 3) * m(3, 1) * m(2, 2);
minv(0, 0) = m(1, 1) * m(2, 2) * m(3, 3) - m(1, 1) * m(3, 2) * m(2, 3) - m(1, 2) * m(2, 1) * m(3, 3) +
m(1, 2) * m(3, 1) * m(2, 3) + m(1, 3) * m(2, 1) * m(3, 2) - m(1, 3) * m(3, 1) * m(2, 2);

minv(0, 1) = -m(0, 1) * m(2, 2) * m(3, 3) + m(0, 1) * m(3, 2) * m(2, 3) +
m(0, 2) * m(2, 1) * m(3, 3) - m(0, 2) * m(3, 1) * m(2, 3) -
m(0, 3) * m(2, 1) * m(3, 2) + m(0, 3) * m(3, 1) * m(2, 2);
minv(0, 1) = -m(0, 1) * m(2, 2) * m(3, 3) + m(0, 1) * m(3, 2) * m(2, 3) + m(0, 2) * m(2, 1) * m(3, 3) -
m(0, 2) * m(3, 1) * m(2, 3) - m(0, 3) * m(2, 1) * m(3, 2) + m(0, 3) * m(3, 1) * m(2, 2);

minv(0, 2) = m(0, 1) * m(1, 2) * m(3, 3) - m(0, 1) * m(3, 2) * m(1, 3) -
m(0, 2) * m(1, 1) * m(3, 3) + m(0, 2) * m(3, 1) * m(1, 3) +
m(0, 3) * m(1, 1) * m(3, 2) - m(0, 3) * m(3, 1) * m(1, 2);
minv(0, 2) = m(0, 1) * m(1, 2) * m(3, 3) - m(0, 1) * m(3, 2) * m(1, 3) - m(0, 2) * m(1, 1) * m(3, 3) +
m(0, 2) * m(3, 1) * m(1, 3) + m(0, 3) * m(1, 1) * m(3, 2) - m(0, 3) * m(3, 1) * m(1, 2);

minv(0, 3) = -m(0, 1) * m(1, 2) * m(2, 3) + m(0, 1) * m(2, 2) * m(1, 3) +
m(0, 2) * m(1, 1) * m(2, 3) - m(0, 2) * m(2, 1) * m(1, 3) -
m(0, 3) * m(1, 1) * m(2, 2) + m(0, 3) * m(2, 1) * m(1, 2);
minv(0, 3) = -m(0, 1) * m(1, 2) * m(2, 3) + m(0, 1) * m(2, 2) * m(1, 3) + m(0, 2) * m(1, 1) * m(2, 3) -
m(0, 2) * m(2, 1) * m(1, 3) - m(0, 3) * m(1, 1) * m(2, 2) + m(0, 3) * m(2, 1) * m(1, 2);

const float det = m(0, 0) * minv(0, 0) + m(1, 0) * minv(0, 1) + m(2, 0) * minv(0, 2) +
m(3, 0) * minv(0, 3);
const float det =
m(0, 0) * minv(0, 0) + m(1, 0) * minv(0, 1) + m(2, 0) * minv(0, 2) + m(3, 0) * minv(0, 3);
return det;
}


inline Mat44f invert(const Mat44f& m) {
Mat44f minv;
minv(0, 0) = m(1, 1) * m(2, 2) * m(3, 3) - m(1, 1) * m(3, 2) * m(2, 3) -
m(1, 2) * m(2, 1) * m(3, 3) + m(1, 2) * m(3, 1) * m(2, 3) +
m(1, 3) * m(2, 1) * m(3, 2) - m(1, 3) * m(3, 1) * m(2, 2);

minv(0, 1) = -m(0, 1) * m(2, 2) * m(3, 3) + m(0, 1) * m(3, 2) * m(2, 3) +
m(0, 2) * m(2, 1) * m(3, 3) - m(0, 2) * m(3, 1) * m(2, 3) -
m(0, 3) * m(2, 1) * m(3, 2) + m(0, 3) * m(3, 1) * m(2, 2);

minv(0, 2) = m(0, 1) * m(1, 2) * m(3, 3) - m(0, 1) * m(3, 2) * m(1, 3) -
m(0, 2) * m(1, 1) * m(3, 3) + m(0, 2) * m(3, 1) * m(1, 3) +
m(0, 3) * m(1, 1) * m(3, 2) - m(0, 3) * m(3, 1) * m(1, 2);

minv(0, 3) = -m(0, 1) * m(1, 2) * m(2, 3) + m(0, 1) * m(2, 2) * m(1, 3) +
m(0, 2) * m(1, 1) * m(2, 3) - m(0, 2) * m(2, 1) * m(1, 3) -
m(0, 3) * m(1, 1) * m(2, 2) + m(0, 3) * m(2, 1) * m(1, 2);

minv(1, 0) = -m(1, 0) * m(2, 2) * m(3, 3) + m(1, 0) * m(3, 2) * m(2, 3) +
m(1, 2) * m(2, 0) * m(3, 3) - m(1, 2) * m(3, 0) * m(2, 3) -
m(1, 3) * m(2, 0) * m(3, 2) + m(1, 3) * m(3, 0) * m(2, 2);

minv(1, 1) = m(0, 0) * m(2, 2) * m(3, 3) - m(0, 0) * m(3, 2) * m(2, 3) -
m(0, 2) * m(2, 0) * m(3, 3) + m(0, 2) * m(3, 0) * m(2, 3) +
m(0, 3) * m(2, 0) * m(3, 2) - m(0, 3) * m(3, 0) * m(2, 2);

minv(1, 2) = -m(0, 0) * m(1, 2) * m(3, 3) + m(0, 0) * m(3, 2) * m(1, 3) +
m(0, 2) * m(1, 0) * m(3, 3) - m(0, 2) * m(3, 0) * m(1, 3) -
m(0, 3) * m(1, 0) * m(3, 2) + m(0, 3) * m(3, 0) * m(1, 2);

minv(1, 3) = m(0, 0) * m(1, 2) * m(2, 3) - m(0, 0) * m(2, 2) * m(1, 3) -
m(0, 2) * m(1, 0) * m(2, 3) + m(0, 2) * m(2, 0) * m(1, 3) +
m(0, 3) * m(1, 0) * m(2, 2) - m(0, 3) * m(2, 0) * m(1, 2);

minv(2, 0) = m(1, 0) * m(2, 1) * m(3, 3) - m(1, 0) * m(3, 1) * m(2, 3) -
m(1, 1) * m(2, 0) * m(3, 3) + m(1, 1) * m(3, 0) * m(2, 3) +
m(1, 3) * m(2, 0) * m(3, 1) - m(1, 3) * m(3, 0) * m(2, 1);

minv(2, 1) = -m(0, 0) * m(2, 1) * m(3, 3) + m(0, 0) * m(3, 1) * m(2, 3) +
m(0, 1) * m(2, 0) * m(3, 3) - m(0, 1) * m(3, 0) * m(2, 3) -
m(0, 3) * m(2, 0) * m(3, 1) + m(0, 3) * m(3, 0) * m(2, 1);

minv(2, 2) = m(0, 0) * m(1, 1) * m(3, 3) - m(0, 0) * m(3, 1) * m(1, 3) -
m(0, 1) * m(1, 0) * m(3, 3) + m(0, 1) * m(3, 0) * m(1, 3) +
m(0, 3) * m(1, 0) * m(3, 1) - m(0, 3) * m(3, 0) * m(1, 1);

minv(2, 3) = -m(0, 0) * m(1, 1) * m(2, 3) + m(0, 0) * m(2, 1) * m(1, 3) +
m(0, 1) * m(1, 0) * m(2, 3) - m(0, 1) * m(2, 0) * m(1, 3) -
m(0, 3) * m(1, 0) * m(2, 1) + m(0, 3) * m(2, 0) * m(1, 1);

minv(3, 0) = -m(1, 0) * m(2, 1) * m(3, 2) + m(1, 0) * m(3, 1) * m(2, 2) +
m(1, 1) * m(2, 0) * m(3, 2) - m(1, 1) * m(3, 0) * m(2, 2) -
m(1, 2) * m(2, 0) * m(3, 1) + m(1, 2) * m(3, 0) * m(2, 1);

minv(3, 1) = m(0, 0) * m(2, 1) * m(3, 2) - m(0, 0) * m(3, 1) * m(2, 2) -
m(0, 1) * m(2, 0) * m(3, 2) + m(0, 1) * m(3, 0) * m(2, 2) +
m(0, 2) * m(2, 0) * m(3, 1) - m(0, 2) * m(3, 0) * m(2, 1);

minv(3, 2) = -m(0, 0) * m(1, 1) * m(3, 2) + m(0, 0) * m(3, 1) * m(1, 2) +
m(0, 1) * m(1, 0) * m(3, 2) - m(0, 1) * m(3, 0) * m(1, 2) -
m(0, 2) * m(1, 0) * m(3, 1) + m(0, 2) * m(3, 0) * m(1, 1);

minv(3, 3) = m(0, 0) * m(1, 1) * m(2, 2) - m(0, 0) * m(2, 1) * m(1, 2) -
m(0, 1) * m(1, 0) * m(2, 2) + m(0, 1) * m(2, 0) * m(1, 2) +
m(0, 2) * m(1, 0) * m(2, 1) - m(0, 2) * m(2, 0) * m(1, 1);
const float det = m(0, 0) * minv(0, 0) + m(1, 0) * minv(0, 1) + m(2, 0) * minv(0, 2) +
m(3, 0) * minv(0, 3);
minv(0, 0) = m(1, 1) * m(2, 2) * m(3, 3) - m(1, 1) * m(3, 2) * m(2, 3) - m(1, 2) * m(2, 1) * m(3, 3) +
m(1, 2) * m(3, 1) * m(2, 3) + m(1, 3) * m(2, 1) * m(3, 2) - m(1, 3) * m(3, 1) * m(2, 2);

minv(0, 1) = -m(0, 1) * m(2, 2) * m(3, 3) + m(0, 1) * m(3, 2) * m(2, 3) + m(0, 2) * m(2, 1) * m(3, 3) -
m(0, 2) * m(3, 1) * m(2, 3) - m(0, 3) * m(2, 1) * m(3, 2) + m(0, 3) * m(3, 1) * m(2, 2);

minv(0, 2) = m(0, 1) * m(1, 2) * m(3, 3) - m(0, 1) * m(3, 2) * m(1, 3) - m(0, 2) * m(1, 1) * m(3, 3) +
m(0, 2) * m(3, 1) * m(1, 3) + m(0, 3) * m(1, 1) * m(3, 2) - m(0, 3) * m(3, 1) * m(1, 2);

minv(0, 3) = -m(0, 1) * m(1, 2) * m(2, 3) + m(0, 1) * m(2, 2) * m(1, 3) + m(0, 2) * m(1, 1) * m(2, 3) -
m(0, 2) * m(2, 1) * m(1, 3) - m(0, 3) * m(1, 1) * m(2, 2) + m(0, 3) * m(2, 1) * m(1, 2);

minv(1, 0) = -m(1, 0) * m(2, 2) * m(3, 3) + m(1, 0) * m(3, 2) * m(2, 3) + m(1, 2) * m(2, 0) * m(3, 3) -
m(1, 2) * m(3, 0) * m(2, 3) - m(1, 3) * m(2, 0) * m(3, 2) + m(1, 3) * m(3, 0) * m(2, 2);

minv(1, 1) = m(0, 0) * m(2, 2) * m(3, 3) - m(0, 0) * m(3, 2) * m(2, 3) - m(0, 2) * m(2, 0) * m(3, 3) +
m(0, 2) * m(3, 0) * m(2, 3) + m(0, 3) * m(2, 0) * m(3, 2) - m(0, 3) * m(3, 0) * m(2, 2);

minv(1, 2) = -m(0, 0) * m(1, 2) * m(3, 3) + m(0, 0) * m(3, 2) * m(1, 3) + m(0, 2) * m(1, 0) * m(3, 3) -
m(0, 2) * m(3, 0) * m(1, 3) - m(0, 3) * m(1, 0) * m(3, 2) + m(0, 3) * m(3, 0) * m(1, 2);

minv(1, 3) = m(0, 0) * m(1, 2) * m(2, 3) - m(0, 0) * m(2, 2) * m(1, 3) - m(0, 2) * m(1, 0) * m(2, 3) +
m(0, 2) * m(2, 0) * m(1, 3) + m(0, 3) * m(1, 0) * m(2, 2) - m(0, 3) * m(2, 0) * m(1, 2);

minv(2, 0) = m(1, 0) * m(2, 1) * m(3, 3) - m(1, 0) * m(3, 1) * m(2, 3) - m(1, 1) * m(2, 0) * m(3, 3) +
m(1, 1) * m(3, 0) * m(2, 3) + m(1, 3) * m(2, 0) * m(3, 1) - m(1, 3) * m(3, 0) * m(2, 1);

minv(2, 1) = -m(0, 0) * m(2, 1) * m(3, 3) + m(0, 0) * m(3, 1) * m(2, 3) + m(0, 1) * m(2, 0) * m(3, 3) -
m(0, 1) * m(3, 0) * m(2, 3) - m(0, 3) * m(2, 0) * m(3, 1) + m(0, 3) * m(3, 0) * m(2, 1);

minv(2, 2) = m(0, 0) * m(1, 1) * m(3, 3) - m(0, 0) * m(3, 1) * m(1, 3) - m(0, 1) * m(1, 0) * m(3, 3) +
m(0, 1) * m(3, 0) * m(1, 3) + m(0, 3) * m(1, 0) * m(3, 1) - m(0, 3) * m(3, 0) * m(1, 1);

minv(2, 3) = -m(0, 0) * m(1, 1) * m(2, 3) + m(0, 0) * m(2, 1) * m(1, 3) + m(0, 1) * m(1, 0) * m(2, 3) -
m(0, 1) * m(2, 0) * m(1, 3) - m(0, 3) * m(1, 0) * m(2, 1) + m(0, 3) * m(2, 0) * m(1, 1);

minv(3, 0) = -m(1, 0) * m(2, 1) * m(3, 2) + m(1, 0) * m(3, 1) * m(2, 2) + m(1, 1) * m(2, 0) * m(3, 2) -
m(1, 1) * m(3, 0) * m(2, 2) - m(1, 2) * m(2, 0) * m(3, 1) + m(1, 2) * m(3, 0) * m(2, 1);

minv(3, 1) = m(0, 0) * m(2, 1) * m(3, 2) - m(0, 0) * m(3, 1) * m(2, 2) - m(0, 1) * m(2, 0) * m(3, 2) +
m(0, 1) * m(3, 0) * m(2, 2) + m(0, 2) * m(2, 0) * m(3, 1) - m(0, 2) * m(3, 0) * m(2, 1);

minv(3, 2) = -m(0, 0) * m(1, 1) * m(3, 2) + m(0, 0) * m(3, 1) * m(1, 2) + m(0, 1) * m(1, 0) * m(3, 2) -
m(0, 1) * m(3, 0) * m(1, 2) - m(0, 2) * m(1, 0) * m(3, 1) + m(0, 2) * m(3, 0) * m(1, 1);

minv(3, 3) = m(0, 0) * m(1, 1) * m(2, 2) - m(0, 0) * m(2, 1) * m(1, 2) - m(0, 1) * m(1, 0) * m(2, 2) +
m(0, 1) * m(2, 0) * m(1, 2) + m(0, 2) * m(1, 0) * m(2, 1) - m(0, 2) * m(2, 0) * m(1, 1);
const float det =
m(0, 0) * minv(0, 0) + m(1, 0) * minv(0, 1) + m(2, 0) * minv(0, 2) + m(3, 0) * minv(0, 3);
const float norm = 1.f / det;

for (int i = 0; i < 4; i++) {
Expand Down Expand Up @@ -324,4 +304,16 @@ inline Mat33f getRotationMatrix(const Vec3f& axis, const float angle) {
};
}

// (0,0,1) -> dir rotator
inline Mat33f getRotatorTo(const Vec3f& dir) {
const Vec3f up(0, 0, 1);
if (dir == up) {
return Mat33f::identity();
}

const Vec3f x = normalize(crossProd(up, dir));
const Vec3f y = normalize(crossProd(dir, x));
return transpose(Mat33f(x, y, dir));
}

} // namespace Pvl

0 comments on commit cf4a9bb

Please sign in to comment.