diff --git a/.gitignore b/.gitignore index 7327629..ff2a733 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ *.user *.user.* compile_commands.json +*.autosave diff --git a/src/core/include/pw/core/axisangle.hpp b/src/core/include/pw/core/axisangle.hpp index 77744ee..fcc5fdb 100644 --- a/src/core/include/pw/core/axisangle.hpp +++ b/src/core/include/pw/core/axisangle.hpp @@ -47,51 +47,57 @@ struct axisangle_ { { } + static const axisangle_ from_matrix(const matrix_<4,4,T>& m) + { + using std::acos; + using std::sqrt; + + axisangle_ aa_res; + aa_res.angle = acos((m(0,0) + m(1,1) + m(2,2) - 1) / T(2)); + const T m2112 = m(2,1) - m(1,2); + const T m0220 = m(0,2) - m(2,0); + const T m1001 = m(1,0) - m(0,1); + // no singularity check here ... + const T mrot_denom = sqrt( m2112 * m2112 + m0220 * m0220 + m1001 * m1001 ); + + aa_res.axis.x() = m2112 / mrot_denom; + aa_res.axis.y() = m0220 / mrot_denom; + aa_res.axis.z() = m1001 / mrot_denom; + + return aa_res; + } + matrix_<4,4,T> to_matrix() const { - - // TODO ... Somehow buggy! using std::cos; using std::sin; // result matrix_<4,4,T> rot_mat; rot_mat.set_identity(); + axis_type axis_n = axis.normalized(); // always normalize - // return as identity - if (axis.norm() < std::numeric_limits::epsilon()) return rot_mat; + const T cos_a = cos(angle); + const T sin_a = sin(angle); + const T cos_1_a = T(1) - cos_a; - const T f_cos = cos(angle); + rot_mat(0,0) = cos_a + axis_n.x() * axis_n.x() * cos_1_a; + rot_mat(1,1) = cos_a + axis_n.y() * axis_n.y() * cos_1_a; + rot_mat(2,2) = cos_a + axis_n.z() * axis_n.z() * cos_1_a; - const vector3_ cos_axis = axis * (T(1) - f_cos); - const vector3_ sin_axis = axis * sin(angle); + T v1 = axis_n.x() * axis_n.y() * cos_1_a; + T v2 = axis_n.z() * sin_a; + rot_mat(1,0) = v1 + v2; + rot_mat(0,1) = v1 - v2; - T val = cos_axis.x() * axis.y(); - rot_mat(0,1) = val - sin_axis.z(); - rot_mat(1,0) = val + sin_axis.z(); + v1 = axis_n.x() * axis_n.z() * cos_1_a; + v2 = axis_n.y() * sin_a; + rot_mat(2,0) = v1 - v2; + rot_mat(0,2) = v1 + v2; - val = cos_axis.x() * axis.z(); - rot_mat(0,2) = val + sin_axis.z(); - rot_mat(2,0) = val - sin_axis.z(); - - val = cos_axis.y() * axis.z(); - rot_mat(1,2) = val - sin_axis.x(); - rot_mat(2,1) = val + sin_axis.x(); - - rot_mat(0,0) = cos_axis[0] * axis[0] + f_cos; - rot_mat(1,1) = cos_axis[1] * axis[1] + f_cos; - rot_mat(2,2) = cos_axis[2] * axis[2] + f_cos; - -// R.at(0) = axis[0] * _vCos(0,0)) + f_cos; -// R.at(4) = axis[0] * _vCos(1,0)) - _vSin(2,0)); -// R.at(8) = axis[0] * _vCos(2,0)) + _vSin(1,0)); - -// R.at(1) = (T) ((vec(1,0) * _vCos(0,0)) + _vSin(2,0)); -// R.at(5) = (T) ((vec(1,0) * _vCos(1,0)) + f_cos); -// R.at(9) = (T) ((vec(1,0) * _vCos(2,0)) - _vSin(0,0)); - -// R.at(2) = (T) ((vec(2,0) * _vCos(0,0)) - _vSin(1,0)); -// R.at(6) = (T) ((vec(2,0) * _vCos(1,0)) + _vSin(0,0)); -// R.at(10)= (T) ((vec(2,0) * _vCos(2,0)) + f_cos); + v1 = axis_n.y() * axis_n.z() * cos_1_a; + v2 = axis_n.x() * sin_a; + rot_mat(2,1) = v1 + v2; + rot_mat(1,2) = v1 - v2; return rot_mat; } diff --git a/src/core/include/pw/core/mesh.hpp b/src/core/include/pw/core/mesh.hpp index 8843179..56345db 100644 --- a/src/core/include/pw/core/mesh.hpp +++ b/src/core/include/pw/core/mesh.hpp @@ -56,6 +56,8 @@ public: void apply(const matrix4x4& m); + void bounds() const; + void reset(); protected: diff --git a/src/core/src/mesh.cpp b/src/core/src/mesh.cpp index 030d37c..07892c4 100644 --- a/src/core/src/mesh.cpp +++ b/src/core/src/mesh.cpp @@ -6,7 +6,21 @@ namespace pw { void mesh::apply(const matrix4x4 &m) { for (auto &v : _vertices) - v = vector4(m * v.homogenous()).project(); + v = vector4(m * v.homogenous()).project(); +} + +void mesh::bounds() const +{ + vector3 max_vec = _vertices.front(),min_vec = _vertices.front(); + // compute the AABB + for (auto const & v : _vertices) { + max_vec.x() = std::max(v.x(),max_vec.x()); + max_vec.y() = std::max(v.y(),max_vec.y()); + max_vec.z() = std::max(v.z(),max_vec.z()); + min_vec.x() = std::min(v.x(),min_vec.x()); + min_vec.y() = std::min(v.y(),min_vec.y()); + min_vec.z() = std::min(v.z(),min_vec.z()); + } } diff --git a/src/core/tests/pwcore_test_axisangle.cpp b/src/core/tests/pwcore_test_axisangle.cpp index 21f95b2..34b32c0 100644 --- a/src/core/tests/pwcore_test_axisangle.cpp +++ b/src/core/tests/pwcore_test_axisangle.cpp @@ -28,6 +28,26 @@ int main(int argc,char **argv) { + std::cout << "z-axis" << std::endl; + + aa.axis = pw::vector3::z_axis(); + aa.angle = pw::deg_to_rad(45.f); + + std::cout << "aa.matrix() = " << std::endl << pw::serialize::matrix(aa.to_matrix()) << std::endl; + + + std::cout << "from matrix" << std::endl; + + pw::matrix4x4f mrot; mrot.zero(); + mrot(0,0) = 1; + mrot(2,1) = 1; + mrot(1,2) = -1; + + pw::axisanglef aa_fm = pw::axisangle::from_matrix(mrot); + + std::cout << pw::serialize::matrix(aa_fm.axis.transposed()) << " " << pw::rad_to_deg(aa_fm.angle) << "deg" << std::endl; + + return 0; }