Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #ifndef ApproxMVBB_MakeCoordinateSystem_hpp
00011 #define ApproxMVBB_MakeCoordinateSystem_hpp
00012
00013 #include <cmath>
00014 #include "ApproxMVBB/Config/Config.hpp"
00015 #include ApproxMVBB_TypeDefs_INCLUDE_FILE
00016
00017 namespace ApproxMVBB{
00018 namespace CoordinateSystem {
00019
00020 ApproxMVBB_DEFINE_MATRIX_TYPES
00021
00027
00035 inline void makeCoordinateSystem( Vector3 &v1,
00036 Vector3 &v2,
00037 Vector3 &v3)
00038 {
00039
00040 using std::abs;
00041 using std::sqrt;
00042
00043 v1.normalize();
00044
00045 if(abs(v1(0)) > abs(v1(2))){
00046 PREC invLen = 1.0 / sqrt(v1(0)*v1(0) + v1(2) * v1(2));
00047 v2 = MyMatrix::Vector3<PREC>(-v1(2) * invLen, 0, v1(0) *invLen);
00048 }
00049 else{
00050 PREC invLen = 1.0 / sqrt(v1(1)*v1(1) + v1(2) * v1(2));
00051 v2 = MyMatrix::Vector3<PREC>(0, v1(2) * invLen, -v1(1) *invLen);
00052 }
00053 v3 = v1.cross(v2);
00054
00055 v2.normalize();
00056 v3.normalize();
00057
00058 }
00059
00060
00061 inline void makeZAxisUp(Matrix33 & A_IK){
00062
00063
00064 if(A_IK(0,2) > A_IK(1,2) && A_IK(0,2) > A_IK(2,2)){
00065
00066 A_IK.col(0).swap(A_IK.col(2));
00067 A_IK.col(0).swap(A_IK.col(1));
00068 }else if(A_IK(1,2) > A_IK(0,2) && A_IK(1,2) > A_IK(2,2)){
00069
00070 A_IK.col(0).swap(A_IK.col(2));
00071 A_IK.col(1).swap(A_IK.col(2));
00072 }
00073 }
00074
00075
00076 inline bool checkOrthogonality(Vector3 &v1,
00077 Vector3 &v2,
00078 Vector3 &v3,
00079 PREC eps = 1e-6)
00080 {
00081
00082 if(v1.dot(v2) <= eps && v2.dot(v3) <= eps && v3.dot(v1) <= eps){
00083 return true;
00084 }
00085 return false;
00086 }
00087
00088
00089
00090 }
00091 }
00092 #endif