vmml::vector<3, double> TerosCam::rotatePointAroundAxis(vmml::vector<3, double> axis, vmml::vector<3, double> point, double degrees){
double angle;
double u, v, w;
double rotationMatrix[4][4];
double inputMatrix[4][1] = {0.0, 0.0, 0.0, 0.0};
double outputMatrix[4][1] = {0.0, 0.0, 0.0, 0.0};
inputMatrix[0][0] = point.x();
inputMatrix[1][0] = point.y();
inputMatrix[2][0] = point.z();
inputMatrix[3][0] = 1.0;
u = axis.x();
v = axis.y();
w = axis.z();
double L = (u*u + v * v + w * w);
angle = degrees * M_PI / 180.0; //converting to radian value
double u2 = u * u;
double v2 = v * v;
double w2 = w * w;
rotationMatrix[0][0] = (u2 + (v2 + w2) * cos(angle)) / L;
rotationMatrix[0][1] = (u * v * (1 - cos(angle)) - w * sqrt(L) * sin(angle)) / L;
rotationMatrix[0][2] = (u * w * (1 - cos(angle)) + v * sqrt(L) * sin(angle)) / L;
rotationMatrix[0][3] = 0.0;
rotationMatrix[1][0] = (u * v * (1 - cos(angle)) + w * sqrt(L) * sin(angle)) / L;
rotationMatrix[1][1] = (v2 + (u2 + w2) * cos(angle)) / L;
rotationMatrix[1][2] = (v * w * (1 - cos(angle)) - u * sqrt(L) * sin(angle)) / L;
rotationMatrix[1][3] = 0.0;
rotationMatrix[2][0] = (u * w * (1 - cos(angle)) - v * sqrt(L) * sin(angle)) / L;
rotationMatrix[2][1] = (v * w * (1 - cos(angle)) + u * sqrt(L) * sin(angle)) / L;
rotationMatrix[2][2] = (w2 + (u2 + v2) * cos(angle)) / L;
rotationMatrix[2][3] = 0.0;
rotationMatrix[3][0] = 0.0;
rotationMatrix[3][1] = 0.0;
rotationMatrix[3][2] = 0.0;
rotationMatrix[3][3] = 1.0;
for(int i = 0; i < 4; i++ ){
for(int j = 0; j < 1; j++){
outputMatrix[i][j] = 0;
for(int k = 0; k < 4; k++){
outputMatrix[i][j] += rotationMatrix[i][k] * inputMatrix[k][j];
}
}
}
return vmml::vector<3, double>(outputMatrix[0][0], outputMatrix[1][0], outputMatrix[2][0]);
}
void testPerf(int nLevels, int levelCube, vmml::vector<3,int> offset)
{
int dimV = exp2(nLevels);
float * cubeG = 0;
vmml::vector<3,int> sP;
vmml::vector<3,int> eP;
vmml::vector<3,int> mD = hdf5File.getRealDimension();
sP[0] = offset.x() - CUBE_INC < 0 ? 0 : offset.x() - CUBE_INC;
sP[1] = offset.y() - CUBE_INC < 0 ? 0 : offset.y() - CUBE_INC;
sP[2] = offset.z() - CUBE_INC < 0 ? 0 : offset.z() - CUBE_INC;
eP[0] = offset.x() + dimV + CUBE_INC >= mD.x() ? mD.x() : offset.x() + dimV + CUBE_INC;
eP[1] = offset.y() + dimV + CUBE_INC >= mD.y() ? mD.y() : offset.y() + dimV + CUBE_INC;
eP[2] = offset.z() + dimV + CUBE_INC >= mD.z() ? mD.z() : offset.z() + dimV + CUBE_INC;
std::cout<<"ReSize Plane Cache "<<sP<<" "<<eP<<std::endl;
std::cout<<"Subset volume "<<offset - vmml::vector<3,int>(CUBE_INC,CUBE_INC,CUBE_INC)<<" "<<offset+vmml::vector<3,int>(dimV+CUBE_INC, dimV+CUBE_INC,dimV+CUBE_INC)<<std::endl;
std::cout<<"ReSize Cube Cache nLevels "<<nLevels<<" level cube "<<levelCube<<" offset "<<offset<<std::endl;
if (!ccc.freeCacheAndPause() || !ccc.reSizeCacheAndContinue(offset, eP, levelCube, nLevels))
{
std::cerr<<"Error, resizing cube cpu cache"<<std::endl;
return;
}
eqMivt::index_node_t idS = eqMivt::coordinateToIndex(vmml::vector<3,int>(0,0,0), levelCube, nLevels);
eqMivt::index_node_t idF = eqMivt::coordinateToIndex(vmml::vector<3,int>(dimV-1, dimV-1, dimV-1), levelCube, nLevels);
#ifndef DISK_TIMING
boost::progress_display show_progress(idF - idS + 1);
#endif
for(eqMivt::index_node_t id=idS; id<=idF; id++)
{
vmml::vector<3,int> coord = eqMivt::getMinBoxIndex2(id, levelCube, nLevels) + offset - vmml::vector<3,int>(CUBE_INC, CUBE_INC, CUBE_INC);
if (coord.x() < mD.x() && coord.y() < mD.y() && coord.z() < mD.z())
{
do
{
cubeG = ccc.getAndBlockElement(id);
}
while(cubeG == 0);
ccc.unlockElement(id);
}
#ifndef DISK_TIMING
++show_progress;
#endif
}
}
bool test(int nLevels, int levelCube, vmml::vector<3,int> offset)
{
int dim = exp2(nLevels - levelCube);
int dimC = dim + 2 * CUBE_INC;
int dimV = exp2(nLevels);
float * cubeC = new float[dimC*dimC*dimC];
float * cube = 0;
vmml::vector<3,int> sP;
vmml::vector<3,int> eP;
vmml::vector<3,int> mD = hdf5File.getRealDimension();
sP[0] = offset.x() - CUBE_INC < 0 ? 0 : offset.x() - CUBE_INC;
sP[1] = offset.y() - CUBE_INC < 0 ? 0 : offset.y() - CUBE_INC;
sP[2] = offset.z() - CUBE_INC < 0 ? 0 : offset.z() - CUBE_INC;
eP[0] = offset.x() + dimV + CUBE_INC >= mD.x() ? mD.x() : offset.x() + dimV + CUBE_INC;
eP[1] = offset.y() + dimV + CUBE_INC >= mD.y() ? mD.y() : offset.y() + dimV + CUBE_INC;
eP[2] = offset.z() + dimV + CUBE_INC >= mD.z() ? mD.z() : offset.z() + dimV + CUBE_INC;
std::cout<<"ReSize Plane Cache "<<sP<<" "<<eP<<std::endl;
std::cout<<"Subset volume "<<offset - vmml::vector<3,int>(CUBE_INC,CUBE_INC,CUBE_INC)<<" "<<offset+vmml::vector<3,int>(dimV+CUBE_INC, dimV+CUBE_INC,dimV+CUBE_INC)<<std::endl;
std::cout<<"ReSize Cube Cache nLevels "<<nLevels<<" level cube "<<levelCube<<" offset "<<offset<<std::endl;
if (!ccc.freeCacheAndPause() || !ccc.reSizeCacheAndContinue(offset, eP, levelCube, nLevels))
{
std::cerr<<"Error, resizing cube cpu cache"<<std::endl;
return true;
}
eqMivt::index_node_t idS = eqMivt::coordinateToIndex(vmml::vector<3,int>(0,0,0), levelCube, nLevels);
eqMivt::index_node_t idF = eqMivt::coordinateToIndex(vmml::vector<3,int>(dimV-1, dimV-1, dimV-1), levelCube, nLevels);
bool error = false;
#ifndef DISK_TIMING
boost::progress_display show_progress(idF - idS + 1);
#endif
for(eqMivt::index_node_t id=idS; id<=idF && !error; id++)
{
vmml::vector<3,int> coord = eqMivt::getMinBoxIndex2(id, levelCube, nLevels) + offset - vmml::vector<3,int>(CUBE_INC, CUBE_INC, CUBE_INC);
do
{
cube = ccc.getAndBlockElement(id);
lunchbox::sleep(50);
}
while(cube == 0);
hdf5File.readCube(id, cubeC, levelCube, nLevels, vmml::vector<3,int>(dimC,dimC,dimC), offset);
for(int i= 0; i<dimC; i++)
for(int j=0; j<dimC; j++)
for(int k=0; k<dimC; k++)
{
if (cube[i*dimC*dimC+j*dimC+k] != cubeC[i*dimC*dimC+j*dimC+k])
{
std::cerr<<"Not coincidence("<<coord.x() + i<<","<<coord.y() + j<<","<<coord.z() + k<<") "<<cube[i*dimC*dimC+j*dimC+k]<<" "<<cubeC[i*dimC*dimC+j*dimC+k]<<std::endl;
error = true;
}
}
ccc.unlockElement(id);
if (error)
{
vmml::vector<3, int> cs = eqMivt::getMinBoxIndex2(id,levelCube, nLevels) + offset - vmml::vector<3, int>(CUBE_INC, CUBE_INC, CUBE_INC);
vmml::vector<3, int> ce = cs + vmml::vector<3, int>(dimC,dimC, dimC);
std::cerr<<"Cube id "<<id<<" coordinates "<<cs<<" "<<ce<<" nLevels "<<nLevels<<" levelCube "<<levelCube<<" offset "<<offset<<std::endl;
}
#ifndef DISK_TIMING
++show_progress;
#endif
}
delete[] cubeC;
return error;
}