Example for quaternion usage and relation to rotation matrix
- Author
- MIP
#include "../Quaternion.hh"
#include <Base/Common/CompareFloatingPoint.hh>
using namespace std;
using namespace BIAS;
int main()
{
cout << "q = " << q << ", p = " << p << endl;
cout << "q*p = " << qp << endl;
cout << "L(q)*p = " << Lp << endl;
cout << "R(p)*q = " << Rq << endl;
#ifdef BIASASSERT_ISACTIVE
BIASASSERT(res_GetQuat == 0);
#endif
q.MultVec(v, qvq);
cout << "v = " << v << ", q*v*q' = " << qvq << endl;
BIASASSERT(
BIAS::Equal((qvq-Rv).NormL2(), 0.0, 1e-12));
return 0;
}