00001 #include <octree_instanciation.h>
00002
00003 namespace animal
00004 {
00005 namespace octree
00006 {
00007
00008
00009 Vec3d HVecToVec3d( const HVec v )
00010 {
00011 Require( v[3] != 0 );
00012
00013 Vec3d res;
00014 res[0] = v[0];
00015 res[1] = v[1];
00016 res[2] = v[2];
00017 res /= v[3];
00018
00019 return res;
00020 }
00021
00022 HVec Vec3dToHVec( const Vec3d v )
00023 {
00024 HVec res(4);
00025
00026 res[0] = v[0];
00027 res[1] = v[1];
00028 res[2] = v[2];
00029 res[3] = 1;
00030
00031 return res;
00032 }
00033
00034
00035 FloatingPointType *getMatrixInverse( unsigned short size, FloatingPointType *data )
00036 {
00037 HMat A(size,size);
00038
00039 for( unsigned int j=0 ; j<size ; ++j )
00040 {
00041 for(unsigned int i=0 ; i<size ; ++i )
00042 {
00043 A[j][i] = data[i+j*size];
00044 }
00045 }
00046
00047 HMat LU(size,size), LU_1(size,size);
00048 m_eq_ludcmp( LU, A );
00049
00050 HVec aux(size);
00051 m_eq_luinv( LU_1, LU, aux );
00052
00053 FloatingPointType *res = new FloatingPointType(size*size);
00054
00055 for( unsigned int j=0 ; j<size ; ++j )
00056 {
00057 for(unsigned int i=0 ; i<size ; ++i )
00058 {
00059 res[i+j*size] = LU_1[j][i];
00060 }
00061 }
00062
00063 return res;
00064 }
00065
00066 HMat getMatrixInverse( HMat A )
00067 {
00068 Require( A.nrows() == A.ncols() );
00069
00070 unsigned short size = A.ncols();
00071 HMat LU(size,size), LU_1(size,size);
00072 m_eq_ludcmp( LU, A );
00073
00074 HVec aux(size);
00075 m_eq_luinv( LU_1, LU, aux );
00076
00077 return LU_1;
00078 }
00079
00080 }
00081 }
00082