jiri/LA_library
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
jiri
2020-01-06 20:50:34 +00:00
parent 086c2202be
commit ef02c16f28
7 changed files with 786 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
t.cc
View File

@@ -1886,7 +1886,7 @@ cout<<r<<endl;
Mat3<double> rotmat;
quat2rotmat(r,rotmat);
cout << rotmat[0][1]<<endl;
r.normalize();
r.normalize(NULL,true);
NRMat<double> rotmat2(3,3),rotmat3(3,3);
Quaternion<NRMat<double> > rotmatder;
rotmatder[0].resize(3,3);
@@ -1905,8 +1905,18 @@ cout <<"orig "<<r<<endl;
cout <<"reconstruct "<<rr<<endl;
cout <<"diff " <<r-rr<<endl;
Vec3<double> eul;
r.normquat2euler(eul);
cout<<eul[0]<<" " <<eul[1]<<" "<<eul[2]<<endl;
r.normalize(NULL,true);
r.normquat2eulerzyx(eul);
cout<< "euler "<<eul[0]<<" " <<eul[1]<<" "<<eul[2]<<endl;
Quaternion<double> rback;
rback.eulerzyx2normquat(eul);
cout <<"q from euler back "<<endl<<r<<endl<<rback<<endl;
Mat3<double> rm;
euler2rotmat((double *)eul,rm,"xyz",false,false,false);
cout <<rm;
Vec3<double> eulback;
rotmat2euler((double *)eulback,rm,"xyz",false,false,false);
cout <<"euler back from rm error "<<eulback-eul<<endl;
Vec3<double> axis={0,1/sqrt(2),1/sqrt(2)};
Quaternion<double> rrr;
rrr.axis2normquat(axis,0.5);
@@ -1924,11 +1934,31 @@ Quaternion<double> rrvec = rvec.rotateby(rrr.conjugate());
cout <<rrvec<<endl;
Vec3<double> rotvec;
rrr.rotate(rotvec,vec);
Quaternion<double> qq={1.5,2,-3,2.123};
Quaternion<double> qq={1.5,2,-2,-1.123};
cout << " test "<<qq*qq<<endl;
cout <<"exp " <<exp(qq)<<endl;
cout <<"log " <<log(qq)<<endl;
cout <<"pow " <<pow(qq,0.5)<<endl;
cout <<"exp " <<exp(qq)<< " "<< log(exp(qq))-qq<<endl;
cout <<"log " <<log(qq)<<" "<< exp(log(qq))-qq<<endl;
cout <<"pow " <<pow(qq,0.5)<<" " <<pow(qq,0.5)*pow(qq,0.5)-qq<<endl;
Quaternion<double> qq2={-.5,1,2,3.23};
Mat3<double> m(1,2,3,4,5,6,7,8,-9);
NRMat<double> mm(m,3,3);
Vec3<double> v(10,20,30);
NRVec<double> vv(v,3);
cout << m*v<<mm*vv<<endl;
cout << v*m<<vv*mm<<endl;
Mat3<double> m2 = m.inverse();
cout<<m2<<m*m2<<m2*m<<endl;
NRMat<double> mm2(3,3); mm2=1.;
multiply_by_inverse(mm2,mm);
cout <<mm<<endl;
qq.normalize(NULL,true);
Vec3<double> xeul,yeul;
qq.normquat2euler((double *)xeul,"zyx");
qq.normquat2eulerzyx((double *)yeul);
cout <<xeul<< " "<<yeul<<endl;
Quaternion<double> xqq;
xqq.euler2normquat((double *)xeul,"zyx");
cout <<"normquat2euler test "<<endl<<qq<<endl<<xqq<<endl<<qq-xqq<<endl;
}