cursor un-project with constraint

include/igl/projection_constraint.cpp

@@ -0,0 +1,50 @@
+#include "projection_constraint.h"
+
+template <
+  typename DerivedUV,
+  typename DerivedM,
+  typename DerivedVP,
+  typename DerivedA,
+  typename DerivedB>
+void igl::projection_constraint(
+  const Eigen::MatrixBase<DerivedUV> & UV,
+  const Eigen::MatrixBase<DerivedM> & _M,
+  const Eigen::MatrixBase<DerivedVP> & VP,
+  Eigen::PlainObjectBase<DerivedA> & A,
+  Eigen::PlainObjectBase<DerivedB> & B)
+{
+  typedef typename DerivedA::Scalar Scalar;
+  const Scalar u = UV(0);
+  const Scalar v = UV(1);
+  const Scalar cu = VP(0);
+  const Scalar cv = VP(1);
+  const Scalar w = VP(2);
+  const Scalar h = VP(3);
+  // u = cu + w*(0.5 + 0.5*((M.row(0)*X) / (M.row(3)*X) ))
+  // u-cu = w*(0.5 + 0.5*((M.row(0)*X) / (M.row(3)*X) ))
+  // (u-cu)/w = 0.5 + 0.5*((M.row(0)*X) / (M.row(3)*X) )
+  // (u-cu)/w - 0.5 = 0.5*((M.row(0)*X) / (M.row(3)*X) )
+  // 2.*(u-cu)/w - 1 = ((M.row(0)*X) / (M.row(3)*X) )
+  // (2.*(u - cu)/w - 1) * M.row(3)*X = M.row(0)*X
+  // (2.*(u - cu)/w - 1) * M.row(3)*X - M.row(0)*X = 0
+  // (2.*(u - cu)/w - 1) * (M.block(3,0,1,3)*x + M(3,3)) - M.block(0,0,1,3)*x - M(0,3) = 0
+  // (2.*(u - cu)/w - 1) * (M.block(3,0,1,3)*x + M(3,3)) - M.block(0,0,1,3)*x = M(0,3)
+  // ((2.*(u - cu)/w - 1) * M.block(3,0,1,3) - M.block(0,0,1,3))*x = M(0,3) - (2.*(u - cu)/w - 1)*M(3,3)
+  Eigen::Matrix<Scalar,4,4> M = _M.template cast<Scalar>();
+  A.resize(2,3);
+  A<<
+   ((2.*(u - cu)/w - 1.) * M.block(3,0,1,3) - M.block(0,0,1,3)),
+   ((2.*(v - cv)/h - 1.) * M.block(3,0,1,3) - M.block(1,0,1,3));
+  B.resize(2,1);
+  B<<
+   M(0,3) - (2.*(u - cu)/w - 1.)*M(3,3),
+   M(1,3) - (2.*(v - cv)/h - 1.)*M(3,3);
+}
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+// generated by autoexplicit.sh
+template void igl::projection_constraint<Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&);
+template void igl::projection_constraint<Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, 1, 3, 1, 1, 3>, 1, -1, false> >, Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, 1, 3, 1, 1, 3>, 1, -1, false> > > const&, Eigen::MatrixBase<Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&);
+template void igl::projection_constraint<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> >, Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> > > const&, Eigen::MatrixBase<Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&);
+#endif

include/igl/projection_constraint.h

@@ -0,0 +1,43 @@
+#ifndef IGL_PROJECTION_CONSTRAINT_H
+#define IGL_PROJECTION_CONSTRAINT_H
+
+#include <Eigen/Core>
+
+namespace igl
+{
+  // Construct two constraint equations of the form:
+  //
+  //     A z = B
+  //
+  // with A 2x3 and B 2x1, where z is the 3d position of point in the scene,
+  // given the current projection matrix (e.g. gl_proj * gl_modelview), viewport
+  // (corner u/v and width/height) and screen space point x,y. Satisfying this
+  // equation means that z projects to screen space point (x,y).
+  // 
+  // Inputs:
+  //   UV  2-long uv-coordinates of screen space point
+  //   M  4 by 4 projection matrix
+  //   VP  4-long viewport: (corner_u, corner_v, width, height)
+  // Outputs:
+  //   A  2 by 3 system matrix
+  //   B  2 by 1 right-hand side
+  template <
+    typename DerivedUV,
+    typename DerivedM,
+    typename DerivedVP,
+    typename DerivedA,
+    typename DerivedB>
+  void projection_constraint(
+    const Eigen::MatrixBase<DerivedUV> & UV,
+    const Eigen::MatrixBase<DerivedM> & M,
+    const Eigen::MatrixBase<DerivedVP> & VP,
+    Eigen::PlainObjectBase<DerivedA> & A,
+    Eigen::PlainObjectBase<DerivedB> & B);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "projection_constraint.cpp"
+#endif
+
+#endif
+

include/igl/unproject_on_line.cpp

@@ -0,0 +1,65 @@
+#include "unproject_on_line.h"
+#include "projection_constraint.h"
+
+template <
+  typename DerivedUV,
+  typename DerivedM,
+  typename DerivedVP,
+  typename Derivedorigin,
+  typename Deriveddir>
+void igl::unproject_on_line(
+  const Eigen::MatrixBase<DerivedUV> & UV,
+  const Eigen::MatrixBase<DerivedM> & M,
+  const Eigen::MatrixBase<DerivedVP> & VP,
+  const Eigen::MatrixBase<Derivedorigin> & origin,
+  const Eigen::MatrixBase<Deriveddir> & dir,
+  typename DerivedUV::Scalar & t)
+{
+  using namespace Eigen;
+  typedef typename DerivedUV::Scalar Scalar;
+  Matrix<Scalar,2,3> A;
+  Matrix<Scalar,2,1> B;
+  projection_constraint(UV,M,VP,A,B);
+  // min_z,t ‖Az - B‖²  subject to z = origin + t*dir
+  // min_t  ‖A(origin + t*dir) - B‖²
+  // min_t  ‖A*t*dir + A*origin - B‖²
+  // min_t  ‖D*t + C‖²
+  // t = -(D'D)\(D'*C)
+  Matrix<Scalar,2,1> C = A*origin.template cast<Scalar>() - B;
+  Matrix<Scalar,2,1> D =    A*dir.template cast<Scalar>();
+  // Solve least squares system directly
+  const Matrix<Scalar,1,1> t_mat = D.jacobiSvd(ComputeFullU | ComputeFullV).solve(-C);
+  t = t_mat(0,0);
+}
+
+template <
+  typename DerivedUV,
+  typename DerivedM,
+  typename DerivedVP,
+  typename Derivedorigin,
+  typename Deriveddir,
+  typename DerivedZ>
+void igl::unproject_on_line(
+  const Eigen::MatrixBase<DerivedUV> & UV,
+  const Eigen::MatrixBase<DerivedM> & M,
+  const Eigen::MatrixBase<DerivedVP> & VP,
+  const Eigen::MatrixBase<Derivedorigin> & origin,
+  const Eigen::MatrixBase<Deriveddir> & dir,
+  Eigen::PlainObjectBase<DerivedZ> & Z)
+{
+  typedef typename DerivedZ::Scalar Scalar;
+  typename DerivedUV::Scalar t;
+  unproject_on_line(UV,M,VP,origin,dir,t);
+  Z = origin + dir*Scalar(t);
+}
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+// generated by autoexplicit.sh
+template void igl::unproject_on_line<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> >, Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> >, Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> >, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> > > const&, Eigen::MatrixBase<Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> > > const&, Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
+// generated by autoexplicit.sh
+template void igl::unproject_on_line<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> >, Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> >, Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> > >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> > > const&, Eigen::MatrixBase<Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> > > const&, Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> > > const&, Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> >::Scalar&);
+// generated by autoexplicit.sh
+template void igl::unproject_on_line<Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, 1, 3, 1, 1, 3>, 1, -1, false> >, Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> >, Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> const>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, 1, 3, 1, 1, 3>, 1, -1, false> > > const&, Eigen::MatrixBase<Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> > > const&, Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 3, 1, 1, 3> const> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
+template void igl::unproject_on_line<Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>::Scalar&);
+#endif

include/igl/unproject_on_line.h

@@ -0,0 +1,56 @@
+#ifndef IGL_UNPROJECT_ON_LINE_H
+#define IGL_UNPROJECT_ON_LINE_H
+
+#include <Eigen/Dense>
+
+namespace igl
+{
+  // Given a screen space point (u,v) and the current projection matrix (e.g.
+  // gl_proj * gl_modelview) and viewport, _unproject_ the point into the scene
+  // so that it lies on given line (origin and dir) and projects as closely as
+  // possible to the given screen space point.
+  //
+  // Inputs:
+  //   UV  2-long uv-coordinates of screen space point
+  //   M  4 by 4 projection matrix
+  //   VP  4-long viewport: (corner_u, corner_v, width, height)
+  //   origin  point on line
+  //   dir  vector parallel to line
+  // Output:
+  //   t  line parameter so that closest poin on line to viewer ray through UV
+  //     lies at origin+t*dir
+  template <
+    typename DerivedUV,
+    typename DerivedM,
+    typename DerivedVP,
+    typename Derivedorigin,
+    typename Deriveddir>
+  void unproject_on_line(
+    const Eigen::MatrixBase<DerivedUV> & UV,
+    const Eigen::MatrixBase<DerivedM> & M,
+    const Eigen::MatrixBase<DerivedVP> & VP,
+    const Eigen::MatrixBase<Derivedorigin> & origin,
+    const Eigen::MatrixBase<Deriveddir> & dir,
+    typename DerivedUV::Scalar & t);
+  //   Z  3d position of closest point on line to viewing ray through UV
+  template <
+    typename DerivedUV,
+    typename DerivedM,
+    typename DerivedVP,
+    typename Derivedorigin,
+    typename Deriveddir,
+    typename DerivedZ>
+  void unproject_on_line(
+    const Eigen::MatrixBase<DerivedUV> & UV,
+    const Eigen::MatrixBase<DerivedM> & M,
+    const Eigen::MatrixBase<DerivedVP> & VP,
+    const Eigen::MatrixBase<Derivedorigin> & origin,
+    const Eigen::MatrixBase<Deriveddir> & dir,
+    Eigen::PlainObjectBase<DerivedZ> & Z);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "unproject_on_line.cpp"
+#endif
+
+#endif

include/igl/unproject_on_plane.cpp

@@ -0,0 +1,35 @@
+#include "unproject_on_plane.h"
+#include "projection_constraint.h"
+#include <Eigen/Dense>
+
+template <
+  typename DerivedUV,
+  typename DerivedM,
+  typename DerivedVP,
+  typename DerivedP,
+  typename DerivedZ>
+void igl::unproject_on_plane(
+  const Eigen::MatrixBase<DerivedUV> & UV,
+  const Eigen::MatrixBase<DerivedM> & M,
+  const Eigen::MatrixBase<DerivedVP> & VP,
+  const Eigen::MatrixBase<DerivedP> & P,
+  Eigen::PlainObjectBase<DerivedZ> & Z)
+{
+  using namespace Eigen;
+  typedef typename DerivedZ::Scalar Scalar;
+  Matrix<Scalar,2,3> A;
+  Matrix<Scalar,2,1> B;
+  projection_constraint(UV,M,VP,A,B);
+  Matrix<Scalar,3,3> AA;
+  AA.topRows(2) = A.template cast<Scalar>();
+  AA.row(2) = P.head(3).template cast<Scalar>();
+  Matrix<Scalar,3,1> BB;
+  BB.head(2) = B.template cast<Scalar>();
+  BB(2) = -P(3);
+  Z = AA.fullPivHouseholderQr().solve(BB);
+}
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+template  void igl::unproject_on_plane<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> >, Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, 1, 4, 1, 1, 4>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 1, 2, 1, 1, 2> > > const&, Eigen::MatrixBase<Eigen::CoeffBasedProduct<Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, 6> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 4, 1, 1, 4> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
+#endif

include/igl/unproject_on_plane.h

@@ -0,0 +1,37 @@
+#ifndef IGL_UNPROJECT_ON_PLANE_H
+#define IGL_UNPROJECT_ON_PLANE_H
+
+#include <Eigen/Core>
+
+namespace igl
+{
+  // Given a screen space point (u,v) and the current projection matrix (e.g.
+  // gl_proj * gl_modelview) and viewport, _unproject_ the point into the scene
+  // so that it lies on given plane.
+  //
+  // Inputs:
+  //   UV  2-long uv-coordinates of screen space point
+  //   M  4 by 4 projection matrix
+  //   VP  4-long viewport: (corner_u, corner_v, width, height)
+  //   P  4-long plane equation coefficients: P*(X 1) = 0
+  // Outputs:
+  //   Z  3-long world coordinate
+  template <
+    typename DerivedUV,
+    typename DerivedM,
+    typename DerivedVP,
+    typename DerivedP,
+    typename DerivedZ>
+  void unproject_on_plane(
+    const Eigen::MatrixBase<DerivedUV> & UV,
+    const Eigen::MatrixBase<DerivedM> & M,
+    const Eigen::MatrixBase<DerivedVP> & VP,
+    const Eigen::MatrixBase<DerivedP> & P,
+    Eigen::PlainObjectBase<DerivedZ> & Z);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "unproject_on_plane.cpp"
+#endif
+
+#endif