void MyCamera::rotateDn(float an){
///////////////////////Anton
glm::vec3 view_vec = viewC - eyePositionE;
glm::vec3 crossV = glm::cross(view_vec,upVectorU);
glm::mat4 rotm = glm::rotate(an,crossV);
////////////////////////////
glm::vec4 eyen(1.0);
eyen[0]=eyePositionE[0];
eyen[1]=eyePositionE[1];
eyen[2]=eyePositionE[2];
eyen[3]=1;
eyen = rotm*eyen;
glm::vec4 upn(1.0);
upn[0]=upVectorU[0];
upn[1]=upVectorU[1];
upn[2]=upVectorU[2];
upn[3]=1;
upn = rotm*upn;
eyePositionE[0]=eyen[0];
eyePositionE[1]=eyen[1];
eyePositionE[2]=eyen[2];
upVectorU[0]=upn[0];
upVectorU[1]=upn[1];
upVectorU[2]=upn[2];
}
void MyCamera::rotateY(float an){
glm::mat4 rotm = glm::rotate(an, glm::vec3(0.0,1.0,0.0));
glm::vec4 eyen(1.0);
eyen[0]=eyePositionE[0];
eyen[1]=eyePositionE[1];
eyen[2]=eyePositionE[2];
eyen[3]=1;
eyen = rotm*eyen;
glm::vec4 upn(1.0);
upn[0]=upVectorU[0];
upn[1]=upVectorU[1];
upn[2]=upVectorU[2];
upn[3]=1;
upn = rotm*upn;
eyePositionE[0]=eyen[0];
eyePositionE[1]=eyen[1];
eyePositionE[2]=eyen[2];
upVectorU[0]=upn[0];
upVectorU[1]=upn[1];
upVectorU[2]=upn[2];
}
inline
void cal_campose(Eigen::MatrixXf XXc,Eigen::MatrixXf XXw,
int n,Eigen::MatrixXf &R2,Eigen::VectorXf &t2)
{
//A
Eigen::MatrixXf X = XXw;
//B
Eigen::MatrixXf Y = XXc;
Eigen::MatrixXf eyen(n,n);
eyen = Eigen::MatrixXf::Identity(n,n);
Eigen::MatrixXf ones(n,n);
ones.setOnes();
Eigen::MatrixXf K(n,n);
K = eyen - ones/n;
vfloat3 ux;
for(int i =0; i < n; i++)
{
ux = ux + X.col(i);
}
ux = ux/n;
vfloat3 uy;
for(int i =0; i < n; i++)
{
uy = uy + Y.col(i);
}
uy = uy/n;
Eigen::MatrixXf XK(3,n);
XK = X*K;
Eigen::MatrixXf XKarre(3,n);
for(int i = 0 ; i < n ; i++)
{
XKarre(0,i) = XK(0,i)*XK(0,i);
XKarre(1,i) = XK(1,i)*XK(1,i);
XKarre(2,i) = XK(2,i)*XK(2,i);
}
Eigen::VectorXf sumXKarre(n);
float sigmx2 = 0;
for(int i = 0 ; i < n ; i++)
{
sumXKarre[i] = XKarre(0,i) + XKarre(1,i) + XKarre(2,i);
sigmx2 += sumXKarre[i];
}
sigmx2 /=n;
Eigen::MatrixXf SXY(3,3);
SXY = Y*K*(X.transpose())/n;
JacobiSVD<MatrixXf> svd(SXY, ComputeThinU | ComputeThinV);
Eigen::MatrixXf S(3,3);
S = Eigen::MatrixXf::Identity(3,3);
if(SXY.determinant() < 0)
{
S(3,3) = -1;
}
R2 = svd.matrixU() * S * (svd.matrixV()).transpose();
Eigen::MatrixXf D(3,3);
D.setZero();
for(int i = 0 ; i < svd.singularValues().size() ; i++)
{
D(i,i) = (svd.singularValues())[i];
}
float c2 = (D*S).trace()/sigmx2;
t2 = uy - c2*R2*ux;
vfloat3 Xx = R2.col(0);
vfloat3 Yy = R2.col(1);
vfloat3 Zz = R2.col(2);
if((x_cross(Xx,Yy)-Zz).norm()>2e-2)
{
R2.col(2) = -Zz;
}
}
