75 lines
8.4 KiB
C++
75 lines
8.4 KiB
C++
|
#include "signed_angle.h"
|
||
|
|
#include "PI.h"
|
||
|
|
#include <cmath>
|
||
|
|
|
||
|
|
template <
|
||
|
|
typename DerivedA,
|
||
|
|
typename DerivedB,
|
||
|
|
typename DerivedP>
|
||
|
|
IGL_INLINE typename DerivedA::Scalar igl::signed_angle(
|
||
|
|
const Eigen::MatrixBase<DerivedA> & A,
|
||
|
|
const Eigen::MatrixBase<DerivedB> & B,
|
||
|
|
const Eigen::MatrixBase<DerivedP> & P)
|
||
|
|
{
|
||
|
|
typedef typename DerivedA::Scalar SType;
|
||
|
|
// Gather vectors to source and destination
|
||
|
|
SType o2A[2];
|
||
|
|
SType o2B[2];
|
||
|
|
// and lengths
|
||
|
|
SType o2Al = 0;
|
||
|
|
SType o2Bl = 0;
|
||
|
|
for(int i = 0;i<2;i++)
|
||
|
|
{
|
||
|
|
o2A[i] = P(i) - A(i);
|
||
|
|
o2B[i] = P(i) - B(i);
|
||
|
|
o2Al += o2A[i]*o2A[i];
|
||
|
|
o2Bl += o2B[i]*o2B[i];
|
||
|
|
}
|
||
|
|
o2Al = sqrt(o2Al);
|
||
|
|
o2Bl = sqrt(o2Bl);
|
||
|
|
// Normalize
|
||
|
|
for(int i = 0;i<2;i++)
|
||
|
|
{
|
||
|
|
// Matlab crashes on NaN
|
||
|
|
if(o2Al!=0)
|
||
|
|
{
|
||
|
|
o2A[i] /= o2Al;
|
||
|
|
}
|
||
|
|
if(o2Bl!=0)
|
||
|
|
{
|
||
|
|
o2B[i] /= o2Bl;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return
|
||
|
|
-atan2(o2B[0]*o2A[1]-o2B[1]*o2A[0],o2B[0]*o2A[0]+o2B[1]*o2A[1])/
|
||
|
|
(2.*igl::PI);
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef IGL_STATIC_LIBRARY
|
||
|
|
// Explicit template instantiation
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1> const, 1, -1, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Matrix<float, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Matrix<float, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false>, Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false>, Eigen::Matrix<float, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&);
|
||
|
|
// generated by autoexplicit.sh
|
||
|
|
template Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false>, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&);
|
||
|
|
template Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false>, Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false>, Eigen::Matrix<float, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 0, -1, 3> const, 1, 3, false> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 2, 1, 1, 2> > const&);
|
||
|
|
template Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true>::Scalar igl::signed_angle<Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true>, Eigen::Matrix<float, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<float, -1, 3, 1, -1, 3> const, 1, 3, true> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 1, 2, 1, 1, 2> > const&);
|
||
|
|
#ifdef WIN32
|
||
|
|
template float igl::signed_angle<class Eigen::Block<class Eigen::Matrix<float,-1,3,1,-1,3> const ,1,3,1>,class Eigen::Block<class Eigen::Matrix<float,-1,3,1,-1,3> const ,1,3,1>,class Eigen::Matrix<float,1,2,1,1,2> >(class Eigen::MatrixBase<class Eigen::Block<class Eigen::Matrix<float,-1,3,1,-1,3> const ,1,3,1> > const &,class Eigen::MatrixBase<class Eigen::Block<class Eigen::Matrix<float,-1,3,1,-1,3> const ,1,3,1> > const &,class Eigen::MatrixBase<class Eigen::Matrix<float,1,2,1,1,2> > const &);
|
||
|
|
template float igl::signed_angle<class Eigen::Block<class Eigen::Matrix<float,-1,3,0,-1,3> const ,1,3,0>,class Eigen::Block<class Eigen::Matrix<float,-1,3,0,-1,3> const ,1,3,0>,class Eigen::Matrix<float,1,2,1,1,2> >(class Eigen::MatrixBase<class Eigen::Block<class Eigen::Matrix<float,-1,3,0,-1,3> const ,1,3,0> > const &,class Eigen::MatrixBase<class Eigen::Block<class Eigen::Matrix<float,-1,3,0,-1,3> const ,1,3,0> > const &,class Eigen::MatrixBase<class Eigen::Matrix<float,1,2,1,1,2> > const &);
|
||
|
|
#endif
|
||
|
|
#endif
|