typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const
{
TransformationParameters ortho = parameters;
if(ortho.cols() == 4)
{
const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized();
const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();
ortho.block(0, 0, 3, 1) = col1.cross(col2);
ortho.block(0, 1, 3, 1) = col2.cross(col0);
ortho.block(0, 2, 3, 1) = col2;
}
else if(ortho.cols() == 3)
{
// R = [ a b]
// [-b a]
// mean of a and b
T a = (parameters(0,0) + parameters(1,1))/2;
T b = (-parameters(1,0) + parameters(0,1))/2;
T sum = sqrt(pow(a,2) + pow(b,2));
a = a/sum;
b = b/sum;
ortho(0,0) = a; ortho(0,1) = b;
ortho(1,0) = -b; ortho(1,1) = a;
}
return ortho;
}
IGL_INLINE void igl::per_face_normals(
const Eigen::MatrixBase<DerivedV>& V,
const Eigen::MatrixBase<DerivedF>& F,
const Eigen::MatrixBase<DerivedZ> & Z,
Eigen::PlainObjectBase<DerivedN> & N)
{
N.resize(F.rows(),3);
// loop over faces
int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
for(int i = 0; i < Frows;i++)
{
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
N.row(i) = v1.cross(v2);//.normalized();
typename DerivedV::Scalar r = N.row(i).norm();
if(r == 0)
{
N.row(i) = Z;
}else
{
N.row(i) /= r;
}
}
}
IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F,
const Eigen::Matrix<T, Eigen::Dynamic, 1>&X,
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G )
{
G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3);
for (int i = 0; i<F.rows(); ++i)
{
// renaming indices of vertices of triangles for convenience
int i1 = F(i,0);
int i2 = F(i,1);
int i3 = F(i,2);
// #F x 3 matrices of triangle edge vectors, named after opposite vertices
Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2);
Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3);
Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1);
// area of parallelogram is twice area of triangle
// area of parallelogram is || v1 x v2 ||
Eigen::Matrix<T, 1, 3> n = v32.cross(v13);
// This does correct l2 norm of rows, so that it contains #F list of twice
// triangle areas
double dblA = std::sqrt(n.dot(n));
// now normalize normals to get unit normals
Eigen::Matrix<T, 1, 3> u = n / dblA;
// rotate each vector 90 degrees around normal
double norm21 = std::sqrt(v21.dot(v21));
double norm13 = std::sqrt(v13.dot(v13));
Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21);
eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21));
eperp21 *= norm21;
Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13);
eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13));
eperp13 *= norm13;
G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA;
};
}
Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) {
Eigen::Matrix<double, 3, 1> v = A.cross(B);
double s = v.norm();
double c = A.dot(B);
Eigen::Matrix<double, 3, 3> vx;
vx << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx;
}
// returns the 90 deg rotation of a (around n) most similar to target b
/// a and b should be in the same plane orthogonal to N
static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b,
const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n)
{
Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized();
typename DerivedV::Scalar scorea = a.dot(b);
typename DerivedV::Scalar scorec = c.dot(b);
if (fabs(scorea)>=fabs(scorec))
return a*Sign(scorea);
else
return c*Sign(scorec);
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
signed_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{
/*
This method calcultes the sign of the angle which is used in the calculation of a dihedral angle.
As such, this method is not validated for other uses. It will fail if the basis atoms for the
dihedral (atom 2 and atom 3) overlap.
*/
float sa = 180.0 ;
float angle ;
float pi = acos(-1.0) ;
float ada = coor1.dot(coor1) ;
float bdb = coor2.dot(coor2) ;
if(ada*bdb <= 0.0)
{
std::cout << "; " ;
return sa ;
}
else
{
float adb = coor1.dot(coor2) ;
float argument = adb/sqrt(ada*bdb) ;
angle = (180.0/pi) * acos(argument) ;
}
Eigen::Matrix<float,3,1> cp ; cp = coor1.cross(coor2) ;
float dp = coor3.dot(cp) ;
float sign = 0.0 ;
if(dp < 0.0)
{
sign = -1.0 ;
}
else if(dp > 0.0)
{
sign = 1.0 ;
}
sa = sign*angle ;
return sa ;
}
IGL_INLINE Eigen::Matrix<Scalar, 3, 3> igl::rotation_matrix_from_directions(
const Eigen::Matrix<Scalar, 3, 1> v0,
const Eigen::Matrix<Scalar, 3, 1> v1)
{
Eigen::Matrix<Scalar, 3, 3> rotM;
const double epsilon=1e-8;
Scalar dot=v0.normalized().dot(v1.normalized());
///control if there is no rotation
if ((v0-v1).norm()<epsilon)
{
rotM = Eigen::Matrix<Scalar, 3, 3>::Identity();
return rotM;
}
if ((v0+v1).norm()<epsilon)
{
rotM = -Eigen::Matrix<Scalar, 3, 3>::Identity();
rotM(0,0) = 1.;
std::cerr<<"igl::rotation_matrix_from_directions: rotating around x axis by 180o"<<std::endl;
return rotM;
}
///find the axis of rotation
Eigen::Matrix<Scalar, 3, 1> axis;
axis=v0.cross(v1);
axis.normalize();
///construct rotation matrix
Scalar u=axis(0);
Scalar v=axis(1);
Scalar w=axis(2);
Scalar phi=acos(dot);
Scalar rcos = cos(phi);
Scalar rsin = sin(phi);
rotM(0,0) = rcos + u*u*(1-rcos);
rotM(1,0) = w * rsin + v*u*(1-rcos);
rotM(2,0) = -v * rsin + w*u*(1-rcos);
rotM(0,1) = -w * rsin + u*v*(1-rcos);
rotM(1,1) = rcos + v*v*(1-rcos);
rotM(2,1) = u * rsin + w*v*(1-rcos);
rotM(0,2) = v * rsin + u*w*(1-rcos);
rotM(1,2) = -u * rsin + v*w*(1-rcos);
rotM(2,2) = rcos + w*w*(1-rcos);
return rotM;
}
