mat3 operator * (mat3& a, mat3& b) {
#define ROWCOL(i, j) \
a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j]
return mat3(vec3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
vec3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
vec3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
#undef ROWCOL
}
vec4 operator*(const mat4 &a, const vec4 &v)
{
#define ROWCOL(i) \
a.v[i].n[0]*v.n[VX] + \
a.v[i].n[1]*v.n[VY] + \
a.v[i].n[2]*v.n[VZ] + \
a.v[i].n[3]*v.n[VW]
return vec4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
#undef ROWCOL
}
// Multiply two matrices
void CMSEXPORT _cmsMAT3per(cmsMAT3* r, const cmsMAT3* a, const cmsMAT3* b)
{
#define ROWCOL(i, j) \
a->v[i].n[0]*b->v[0].n[j] + a->v[i].n[1]*b->v[1].n[j] + a->v[i].n[2]*b->v[2].n[j]
_cmsVEC3init(&r-> v[0], ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2));
_cmsVEC3init(&r-> v[1], ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2));
_cmsVEC3init(&r-> v[2], ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2));
#undef ROWCOL //(i, j)
}
void Compute_Normc(double *pdInput, int iNum_of_Rows_of_Inp, int iNum_of_Cols_of_Inp, double *pdNormc_of_Input)
{
double dNorm_of_Cur_Col;
int iRowIter, iColIter;
for (iColIter = 0 ; iColIter < iNum_of_Cols_of_Inp; iColIter++)
{
dNorm_of_Cur_Col = Compute_Norm (&pdInput[iColIter*iNum_of_Rows_of_Inp], iNum_of_Rows_of_Inp); // Computes norm of current column.
for(iRowIter = 0; iRowIter < iNum_of_Rows_of_Inp; iRowIter++)
{
pdNormc_of_Input[ROWCOL(iRowIter, iColIter, iNum_of_Rows_of_Inp)] = pdInput[ROWCOL(iRowIter, iColIter, iNum_of_Rows_of_Inp)]/dNorm_of_Cur_Col;
}
}
}
void Double_Compute_Transpose(double *pdInput, double *pdOutput, int iRows_Input, int iCols_Input)
{
int iRows_Output = iCols_Input;
int iCols_Output = iRows_Input;
int iIter_Col, iIter_Row;
for (iIter_Col = 0; iIter_Col < iCols_Input; iIter_Col++)
{
for (iIter_Row = 0; iIter_Row < iRows_Input; iIter_Row++)
{
pdOutput[ROWCOL(iIter_Col, iIter_Row, iRows_Output)] = pdInput[ROWCOL(iIter_Row, iIter_Col, iRows_Input)];
}
}
}
void rovioKalmanFilterSetUncertainty(kalmanFilter *kf, float *uncertainty)
{
// sets the process and sensor uncertainty matrices
// uncertainty is a nine element float vector
// set to {proc_x,proc_y,proc_th,ns_x,ns_y,ns_th,we_x,we_y,we_th}
// initialize the uncertainty array
kf->Q[ROWCOL(0,0)] = uncertainty[0];
kf->Q[ROWCOL(1,1)] = uncertainty[1];
kf->Q[ROWCOL(2,2)] = uncertainty[2];
kf->R1[ROWCOL(0,0)] = uncertainty[3];
kf->R1[ROWCOL(1,1)] = uncertainty[4];
kf->R1[ROWCOL(2,2)] = uncertainty[5];
kf->R2[ROWCOL(0,0)] = uncertainty[6];
kf->R2[ROWCOL(1,1)] = uncertainty[7];
kf->R2[ROWCOL(2,2)] = uncertainty[8];
}
GAE_Tiled_t* GAE_TiledParser_draw(GAE_Tiled_t* tilemap, GAE_Renderer_t* renderer, const unsigned int layerId) {
unsigned int y = 0U;
unsigned int x = 0U;
GAE_Tiled_Layer_t* layer = (GAE_Tiled_Layer_t*)GAE_Array_get(tilemap->layers, layerId);
GAE_Tiled_Tileset_t* tileset = getTileset(tilemap, *(unsigned int*)GAE_Array_begin(layer->data));
const unsigned int margin = tileset->margin;
const unsigned int srcWidth = tileset->tileWidth + margin;
const unsigned int srcHeight = tileset->tileHeight + margin;
const unsigned int dstWidth = tilemap->tileWidth + margin;
const unsigned int dstHeight = tilemap->tileHeight + margin;
const unsigned int tilemapWidthInTiles = (tileset->imageWidth / srcWidth);
const unsigned int offsetX = layer->x;
const unsigned int offsetY = layer->y;
#define ROWCOL(x,y,width) (x * width + y)
for (y = 0U; y < layer->height; ++y) {
for (x = 0U; x < layer->width; ++x) {
const unsigned int tileId = *(unsigned int*)GAE_Array_get(layer->data, ROWCOL(y, x, layer->width)) - 1U;
SDL_Rect src;
SDL_Rect dst;
src.x = (tileId % tilemapWidthInTiles) * srcWidth;
src.y = (tileId / tilemapWidthInTiles) * srcWidth;
src.w = srcWidth;
src.h = srcHeight;
dst.x = (x * dstWidth) + offsetX;
dst.y = (y * dstHeight) + offsetY;
dst.w = dstWidth;
dst.h = dstHeight;
tileset->image->srcRect = &src;
tileset->image->dstRect = &dst;
GAE_Renderer_drawSprite(renderer, tileset->image);
}
}
#undef ROWCOL
return tilemap;
}
/* Initialize the filter */
void initKalmanFilter(kalmanFilter *kf, float *initPose, float *velocity, int deltat) {
int i;
// zero the filter arrays
memset(kf->Q, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(kf->Phi, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(kf->R1, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(kf->R2, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(kf->W1, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(kf->W2, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
memset(kf->P, 0, sizeof(float) * FILTER_SIZE * FILTER_SIZE);
/* Initialize the model array */
diag(kf->Phi,1);
kf->Phi[ROWCOL(0,3)]=deltat;
kf->Phi[ROWCOL(1,4)]=deltat;
kf->Phi[ROWCOL(2,5)]=deltat;
kf->Phi[ROWCOL(3,6)]=deltat;
kf->Phi[ROWCOL(4,7)]=deltat;
kf->Phi[ROWCOL(5,8)]=deltat;
// initialize the uncertainty array
kf->Q[0] = PROCESS_UNCERTAINTY_X;
kf->Q[FILTER_SIZE+1] = PROCESS_UNCERTAINTY_Y;
kf->Q[2*FILTER_SIZE+2] = PROCESS_UNCERTAINTY_TH;
kf->R1[0] = NORTHSTAR_UNCERTAINTY_X;
kf->R1[FILTER_SIZE+1] = NORTHSTAR_UNCERTAINTY_Y;
kf->R1[2*FILTER_SIZE+2] = NORTHSTAR_UNCERTAINTY_TH;
kf->R2[0] = WHEELENC_UNCERTAINTY_X;
kf->R2[FILTER_SIZE+1] = WHEELENC_UNCERTAINTY_Y;
kf->R2[2*FILTER_SIZE+2] = WHEELENC_UNCERTAINTY_TH;
// initialize the state
kf->current_state[0] = initPose[0];
kf->current_state[1] = initPose[1];
kf->current_state[2] = initPose[2];
kf->current_state[3] = velocity[0];
kf->current_state[4] = velocity[1];
kf->current_state[5] = velocity[2];
kf->current_state[6] = 0;
kf->current_state[7] = 0;
kf->current_state[8] = 0;
return;
}
/* Initialize the filter */
void Kalman::initialize( float *initPose, float *velocity, int deltat )
{
int i;
/* Initialize the model array */
diag(Phi,1);
Phi[ROWCOL(0,3)]=deltat;
Phi[ROWCOL(1,4)]=deltat;
Phi[ROWCOL(2,5)]=deltat;
Phi[ROWCOL(3,6)]=deltat;
Phi[ROWCOL(4,7)]=deltat;
Phi[ROWCOL(5,8)]=deltat;
// initialize the uncertainty array
Q[0] = PROCESS_UNCERTAINTY_X;
Q[FILTER_SIZE+1] = PROCESS_UNCERTAINTY_Y;
Q[2*FILTER_SIZE+2] = PROCESS_UNCERTAINTY_TH;
R1[0] = NORTHSTAR_UNCERTAINTY_X;
R1[FILTER_SIZE+1] = NORTHSTAR_UNCERTAINTY_Y;
R1[2*FILTER_SIZE+2] = NORTHSTAR_UNCERTAINTY_TH;
R2[0] = WHEELENC_UNCERTAINTY_X;
R2[FILTER_SIZE+1] = WHEELENC_UNCERTAINTY_Y;
R2[2*FILTER_SIZE+2] = WHEELENC_UNCERTAINTY_TH;
// initialize the state
current_state[0] = initPose[0];
current_state[1] = initPose[1];
current_state[2] = initPose[2];
current_state[3] = velocity[0];
current_state[4] = velocity[1];
current_state[5] = velocity[2];
current_state[6] = 0;
current_state[7] = 0;
current_state[8] = 0;
}
void PreProcessDataImage(double *pdInputImage, int iNum_of_Dims_of_Input_Image, V3DLONG *piDims_InputImage, int iNum_of_pixels_InputImage,
double *pdWeights_InputImage, double *pdEigVec_Cov_InputImage, double *pdEigVal_Cov_InputImage,
double *pdNormP_EigVal_Cov, int *piMeshGridData_InputImage, char *outputDir, double sigma, int LookUpTableWidth)
{
int iDim1_InputImage, iDim2_InputImage, iDim3_InputImage;
int iIter, iRowIter, iColIter, iIdenColIter, iIdenRowIter ;
int iMinLookupTable, iMaxLookupTable, iLenLookupTable ;
//double INFINITY = DBL_MAX;
double dNormP_Element, L2;
char saveName[80];
int iIter_Dim_1, iIter_Dim_2, iIter_Dim_3;
int aiWindowDims[3] ;
int *piLookUpTable;
double *pdDistTable;
bool bAnisotropic;
double K, Beta;
double *pmxaWeights_ParseInput;
int iLen_UpTriang_of_Cov ;
int *piIndexes_of_UpTriang_of_Cov ;
double *pdUpTriag_of_Cov_of_InputImage; // C
int iNum_Elems_of_C; // to store length of C
//pthread declarations
pthread_t ptThreads[NUM_OF_PREPROCESS_THREADS_TO_CREATE];
int iThreadNumber;
int iRet_Val_Pthread_Create;
bool bAll_Threads_Completed_Successfully = TRUE;
double *adEigVec1x_Cov, *adEigVec1y_Cov, *adEigVec2x_Cov, *adEigVec2y_Cov, *adEigVal1_Cov, *adEigVal2_Cov; // Used for both 2d and 3d.
double *adEigVec1z_Cov, *adEigVec2z_Cov, *adEigVec3x_Cov, *adEigVec3y_Cov, *adEigVec3z_Cov, *adEigVal3_Cov; // Used Only for 3d.
double dGamma_of_ndims_by_2beta; // for storing: gamma(d/(2*beta))
double dGamma_of_ndims_by_2; // for storing: gamma(d/2)
double eps = 2.2204e-16;
ANISO_THREAD_ARGS astAniso_Thread_Args[NUM_OF_PREPROCESS_THREADS_TO_CREATE];
// Files generated to store test results.
FILE *fp_data, *fp_weights, *fp_final_vectors, *fp_final_lambda, *fp_final_normP; // final outputs (for both Isotropic and Anistrpoic cases)
FILE *fp_temp, *fp_temp_tr, *fp_diag_weights, *fp_cov_i, *fp_cov, *fp_eigdec_lambda, *fp_eigdec_vectors, *fp_eigdec_normP;
// Populate Image Weights.
for (iIter = 0 ; iIter < iNum_of_pixels_InputImage ; iIter++)
{
pdWeights_InputImage[iIter] = pdInputImage [iIter];
}
#if (OUTPUT_WEIGHTS == TRUE) // Save Image Weights to a file in this form: [N x 1] even though actual dimension is [1 x N]
strcpy(saveName,outputDir);
strcat(saveName,"mex_weights.txt");
fp_weights = fopen (saveName, "w");
if (fp_weights == NULL)
{
printf("\nCannot create file: mex_weights.txt\n");
}
for (iIter = 0 ; iIter < iNum_of_pixels_InputImage ; iIter++)
{
fprintf (fp_weights, "%g\n", pdWeights_InputImage[iIter]);
}
fclose (fp_weights);
#endif
iDim1_InputImage = piDims_InputImage[0];
iDim2_InputImage = piDims_InputImage[1];
iDim3_InputImage = piDims_InputImage[2];
printf ("\nImage is 3-dimensional. \nSize: [%d x %d x %d] \nTotal number of pixels: %d\n",
iDim1_InputImage, iDim2_InputImage, iDim3_InputImage, iNum_of_pixels_InputImage);
for (iIter_Dim_3 = 0 ; iIter_Dim_3 < iDim3_InputImage ; iIter_Dim_3++)
{
for (iIter_Dim_2 = 0 ; iIter_Dim_2 < iDim2_InputImage ; iIter_Dim_2++)
{
for (iIter_Dim_1 = 0 ; iIter_Dim_1 < iDim1_InputImage ; iIter_Dim_1++)
{
piMeshGridData_InputImage[ Z(iIter_Dim_1 + iIter_Dim_2*iDim1_InputImage + iIter_Dim_3*iDim1_InputImage*iDim2_InputImage) ] = iIter_Dim_1;
piMeshGridData_InputImage[ Y(iIter_Dim_1 + iIter_Dim_2*iDim1_InputImage + iIter_Dim_3*iDim1_InputImage*iDim2_InputImage) ] = iIter_Dim_2;
piMeshGridData_InputImage[ X(iIter_Dim_1 + iIter_Dim_2*iDim1_InputImage + iIter_Dim_3*iDim1_InputImage*iDim2_InputImage) ] = iIter_Dim_3;
}
}
}
aiWindowDims[0] = LookUpTableWidth;
aiWindowDims[1] = LookUpTableWidth;
aiWindowDims[2] = LookUpTableWidth;
create3DLookupTable(aiWindowDims, 3, iDim1_InputImage, iDim2_InputImage, &piLookUpTable, &pdDistTable);
iMinLookupTable = Compute_3D_Min(piLookUpTable, aiWindowDims[0], aiWindowDims[1], aiWindowDims[2]);
iMaxLookupTable = Compute_3D_Max(piLookUpTable, aiWindowDims[0], aiWindowDims[1], aiWindowDims[2]);
iLenLookupTable = Compute_3D_Len(piLookUpTable, aiWindowDims[0], aiWindowDims[1], aiWindowDims[2]);
printf ("\nLookup Min: %d \nLookup Max: %d \nLookup Len: %d\n", iMinLookupTable , iMaxLookupTable , iLenLookupTable );
parse_input (&bAnisotropic, &K, &Beta, pmxaWeights_ParseInput, iNum_of_Dims_of_Input_Image, iNum_of_pixels_InputImage, LookUpTableWidth);
printf ("\nAnisotropic = %s \nBeta = %g \nK = %g\n", (bAnisotropic ?"TRUE" :"FALSE"), Beta, K);
if (bAnisotropic)
{
// Populate uptiu.
iLen_UpTriang_of_Cov = ((iNum_of_Dims_of_Input_Image * (iNum_of_Dims_of_Input_Image +1))/2); // dData*(dData+1)/2
piIndexes_of_UpTriang_of_Cov = (int*) malloc (iLen_UpTriang_of_Cov * sizeof (int)); // uptiu
if (iNum_of_Dims_of_Input_Image == 2)
{
piIndexes_of_UpTriang_of_Cov[0] = 0;
piIndexes_of_UpTriang_of_Cov[1] = 2;
piIndexes_of_UpTriang_of_Cov[2] = 3;
}
else if (iNum_of_Dims_of_Input_Image == 3)
{
piIndexes_of_UpTriang_of_Cov[0] = 0;
piIndexes_of_UpTriang_of_Cov[1] = 3;
piIndexes_of_UpTriang_of_Cov[2] = 6;
piIndexes_of_UpTriang_of_Cov[3] = 4;
piIndexes_of_UpTriang_of_Cov[4] = 7;
piIndexes_of_UpTriang_of_Cov[5] = 8;
}
// Calculate local covariance using the lookupTable
// initialize Cov matrix
//C = zeros(N,dData*(dData+1)/2);
iNum_Elems_of_C = (iNum_of_pixels_InputImage * iLen_UpTriang_of_Cov); // length(C) = N*dData*(dData+1)/2
pdUpTriag_of_Cov_of_InputImage = (double*) malloc (iNum_Elems_of_C * sizeof(double));
for (iIter = 0; iIter < iNum_Elems_of_C; iIter++)
{
pdUpTriag_of_Cov_of_InputImage[iIter] = 0;
}
// MULTITHREADING
for(iThreadNumber = 0; iThreadNumber < NUM_OF_PREPROCESS_THREADS_TO_CREATE; iThreadNumber++)
{
// Populate input args for the each thread.
astAniso_Thread_Args[iThreadNumber].iThreadNumber = iThreadNumber; // Passes the current thread number.
astAniso_Thread_Args[iThreadNumber].pdWeights_InputImage = pdWeights_InputImage;
astAniso_Thread_Args[iThreadNumber].piMeshGridData_InputImage = piMeshGridData_InputImage;
astAniso_Thread_Args[iThreadNumber].iNum_of_pixels_InputImage = iNum_of_pixels_InputImage;
astAniso_Thread_Args[iThreadNumber].iNum_of_Dims_of_Input_Image = iNum_of_Dims_of_Input_Image;
astAniso_Thread_Args[iThreadNumber].piLookUpTable = piLookUpTable;
astAniso_Thread_Args[iThreadNumber].pdDistTable = pdDistTable;
astAniso_Thread_Args[iThreadNumber].iMinLookupTable = iMinLookupTable;
astAniso_Thread_Args[iThreadNumber].iMaxLookupTable = iMaxLookupTable;
astAniso_Thread_Args[iThreadNumber].iLenLookupTable = iLenLookupTable;
astAniso_Thread_Args[iThreadNumber].iLen_UpTriang_of_Cov = iLen_UpTriang_of_Cov;
astAniso_Thread_Args[iThreadNumber].piIndexes_of_UpTriang_of_Cov = piIndexes_of_UpTriang_of_Cov;
astAniso_Thread_Args[iThreadNumber].pdUpTriag_of_Cov_of_InputImage = pdUpTriag_of_Cov_of_InputImage;
astAniso_Thread_Args[iThreadNumber].K = K;
iRet_Val_Pthread_Create = pthread_create(&ptThreads[iThreadNumber], NULL, Anisotropic_PreProcess, &astAniso_Thread_Args[iThreadNumber]);
}
for(iThreadNumber = 0; iThreadNumber < NUM_OF_PREPROCESS_THREADS_TO_CREATE; iThreadNumber++)
{
pthread_join(ptThreads[iThreadNumber], NULL);
// printf("Thread %d returns %d\n", iThreadNumber, iRet_Val_Pthread_Create);
}
//pthread_exit(NULL);
bool bAll_Threads_Completed_Successfully = TRUE;
if (bAll_Threads_Completed_Successfully == TRUE)
{
// if dData==2
//2-d and 3-d
adEigVec1x_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec1y_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec2x_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec2y_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVal1_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVal2_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
// if dData==2
if (iNum_of_Dims_of_Input_Image == 2)
{
Compute_Eig_Dec_2D (iNum_of_pixels_InputImage, &pdUpTriag_of_Cov_of_InputImage[0], &pdUpTriag_of_Cov_of_InputImage[iNum_of_pixels_InputImage],
&pdUpTriag_of_Cov_of_InputImage[2*iNum_of_pixels_InputImage],
adEigVal1_Cov, adEigVal2_Cov, adEigVec1x_Cov, adEigVec1y_Cov, adEigVec2x_Cov, adEigVec2y_Cov);
for (iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter ++)
{
pdEigVal_Cov_InputImage[ROWCOL(0, iColIter , 2)] = adEigVal1_Cov[iColIter];
pdEigVal_Cov_InputImage[ROWCOL(1, iColIter , 2)] = adEigVal2_Cov[iColIter];
}
for (iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter ++)
{
pdEigVec_Cov_InputImage[ROWCOL(0, iColIter , 4)] = adEigVec1x_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(1, iColIter , 4)] = adEigVec1y_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(2, iColIter , 4)] = adEigVec2x_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(3, iColIter , 4)] = adEigVec2y_Cov[iColIter];
}
}
// elseif dData ==3
else if (iNum_of_Dims_of_Input_Image == 3)
{
adEigVec1z_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec2z_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec3x_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec3y_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVec3z_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
adEigVal3_Cov = (double*) malloc (iNum_of_pixels_InputImage*sizeof(double));
Compute_Eig_Dec_3D(iNum_of_pixels_InputImage, &pdUpTriag_of_Cov_of_InputImage[0], &pdUpTriag_of_Cov_of_InputImage[iNum_of_pixels_InputImage],
&pdUpTriag_of_Cov_of_InputImage[2*iNum_of_pixels_InputImage], &pdUpTriag_of_Cov_of_InputImage[3*iNum_of_pixels_InputImage],
&pdUpTriag_of_Cov_of_InputImage[4*iNum_of_pixels_InputImage], &pdUpTriag_of_Cov_of_InputImage[5*iNum_of_pixels_InputImage],
adEigVec1x_Cov, adEigVec1y_Cov, adEigVec1z_Cov,
adEigVec2x_Cov, adEigVec2y_Cov, adEigVec2z_Cov,
adEigVec3x_Cov, adEigVec3y_Cov, adEigVec3z_Cov,
adEigVal1_Cov, adEigVal2_Cov, adEigVal3_Cov);
for (iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter ++)
{
pdEigVal_Cov_InputImage[ROWCOL(0, iColIter , 3)] = adEigVal1_Cov[iColIter];
pdEigVal_Cov_InputImage[ROWCOL(1, iColIter , 3)] = adEigVal2_Cov[iColIter];
pdEigVal_Cov_InputImage[ROWCOL(2, iColIter , 3)] = adEigVal3_Cov[iColIter];
}
for (iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter ++)
{
pdEigVec_Cov_InputImage[ROWCOL(0, iColIter , 9)] = adEigVec1x_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(1, iColIter , 9)] = adEigVec1y_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(2, iColIter , 9)] = adEigVec1z_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(3, iColIter , 9)] = adEigVec2x_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(4, iColIter , 9)] = adEigVec2y_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(5, iColIter , 9)] = adEigVec2z_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(6, iColIter , 9)] = adEigVec3x_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(7, iColIter , 9)] = adEigVec3y_Cov[iColIter];
pdEigVec_Cov_InputImage[ROWCOL(8, iColIter , 9)] = adEigVec3z_Cov[iColIter];
}
free(adEigVec1z_Cov); free(adEigVec2z_Cov); free(adEigVec3x_Cov); free(adEigVec3y_Cov); free(adEigVec3z_Cov);
free(adEigVal3_Cov); // Used Only for 3d.
}
free (adEigVec1x_Cov); free(adEigVec1y_Cov); free(adEigVec2x_Cov); free(adEigVec2y_Cov);
free(adEigVal1_Cov); free(adEigVal2_Cov);
for(iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter++)
{
dNormP_Element = 1;
for(iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image; iRowIter++)
{
dNormP_Element = dNormP_Element * pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)];
if (pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] < 0)
{
pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] = 0;
}
}
dNormP_Element = 1/(pow((2*pi),(iNum_of_Dims_of_Input_Image/2))*sqrt(dNormP_Element));
if ((dNormP_Element == INFINITY)|| (dNormP_Element < eps))
{
pdNormP_EigVal_Cov[iColIter] = 0;
}
else
{
pdNormP_EigVal_Cov[iColIter] = dNormP_Element ;
}
}
} // if (bAll_Threads_Completed_Successfully == TRUE)
else
{
printf ("One or more thread failed execution.\n");
}
} // if (bAnisotropic)
else
{
for(iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter++)
{
for(iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image; iRowIter++)
{
pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] = 1;
}
dNormP_Element = 1/pow((2*pi),(iNum_of_Dims_of_Input_Image/2));
pdNormP_EigVal_Cov[iColIter] = ((dNormP_Element == INFINITY) || (dNormP_Element < eps)) ? 0 : dNormP_Element ;
//Vectors = repmat(eye(dData),1,N);
for (iIdenColIter = 0 ; iIdenColIter < iNum_of_Dims_of_Input_Image ; iIdenColIter++)
{
for (iIdenRowIter = 0 ; iIdenRowIter < iNum_of_Dims_of_Input_Image ; iIdenRowIter++)
{
/*
* (0,0) (0,1) (0,0+2) (0,1+2) (0,0+4) (0,1+4) (0,0+6) (0,1+6)
* (1,0) (1,1) (1,0+2) (1,1+2) (1,0+4) (1,1+4) (1,0+6) (1,1+6)
*
*/
pdEigVec_Cov_InputImage[iColIter*iNum_of_Dims_of_Input_Image*iNum_of_Dims_of_Input_Image +
(iIdenRowIter + iIdenColIter*iNum_of_Dims_of_Input_Image)] = (iIdenRowIter == iIdenColIter) ?1 :0;
}
}
}
} // if (bAnisotropic)
dGamma_of_ndims_by_2beta = Gamma(iNum_of_Dims_of_Input_Image/(2.0*beta)); // gamma(d/(2*beta))
dGamma_of_ndims_by_2 = Gamma(iNum_of_Dims_of_Input_Image/2.0); // gamma(d/2)
for(iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter++)
{
dNormP_Element = 1;
#if NORM_EIGENVAL
L2 = pdEigVal_Cov_InputImage[ROWCOL(0, iColIter, iNum_of_Dims_of_Input_Image)];
#endif
for(iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image; iRowIter++)
{
#if NORM_EIGENVAL
pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] /=L2;
#endif
pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] *= sigma*sigma;
dNormP_Element = dNormP_Element * pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)];
}
dNormP_Element = (dGamma_of_ndims_by_2*beta)/(pow(pi, iNum_of_Dims_of_Input_Image/2.0)*dGamma_of_ndims_by_2beta *
pow((double)2, iNum_of_Dims_of_Input_Image/(2.0*beta))*sqrt(dNormP_Element));
pdNormP_EigVal_Cov[iColIter] = ((dNormP_Element == INFINITY) || (dNormP_Element < eps)) ? 0 : dNormP_Element ;
}
#if OUTPUT_FINAL_LAMBDA == TRUE // Save the final Lambda to a file in this form: [N x dData]
strcpy(saveName,outputDir);
strcat(saveName,"mex_final_lambda.txt");
fp_final_lambda = fopen (saveName, "w");
if (fp_final_lambda == NULL)
{
printf ("\nCannot create file: mex_final_lambda.txt\n");
}
for (iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter ++)
{
for (iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image ; iRowIter ++)
{
fprintf (fp_final_lambda, "%g ", pdEigVal_Cov_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)]);
}
fprintf (fp_final_lambda, "\n");
}
fclose (fp_final_lambda);
#endif
#if OUTPUT_FINAL_VECTORS == TRUE // Save the final Lambda to a file in this form: [N x dData.dData]
strcpy(saveName,outputDir);
strcat(saveName,"mex_final_vectors.txt");
fp_final_vectors = fopen (saveName, "w");
if (fp_final_vectors == NULL)
{
printf ("\nCannot create file: mex_final_vectors.txt\n");
}
for (iColIter = 0; iColIter < iNum_of_pixels_InputImage ; iColIter ++)
{
for (iRowIter = 0; iRowIter < (iNum_of_Dims_of_Input_Image*iNum_of_Dims_of_Input_Image) ; iRowIter ++)
{
fprintf (fp_final_vectors, "%g ", pdEigVec_Cov_InputImage[ROWCOL(iRowIter, iColIter, (iNum_of_Dims_of_Input_Image*iNum_of_Dims_of_Input_Image))]);
}
fprintf (fp_final_vectors, "\n");
}
fclose (fp_final_vectors);
#endif
#if OUTPUT_FINAL_NORMP == TRUE // Save the final normP to a file in this form: [N x dData.dData]
strcpy(saveName,outputDir);
strcat(saveName,"mex_final_normP.txt");
fp_final_normP = fopen (saveName, "w");
if (fp_final_normP == NULL)
{
printf ("\nCannot create file: mex_final_normP.txt\n");
}
for (iIter = 0; iIter < iNum_of_pixels_InputImage ; iIter ++)
{
fprintf (fp_final_normP, "%g\n", pdNormP_EigVal_Cov[iIter]);
}
fclose (fp_final_normP);
#endif
return;
}
void *Anisotropic_PreProcess(void *vptrAniso_Thread_Args)
// parfor i=1:N //
{
ANISO_THREAD_ARGS *pstAniso_Thread_Args;
int iThreadNumber;
int iImagePixel_Start, iImagePixel_End;
double *pdWeights_InputImage;
int *piMeshGridData_InputImage;
int iNum_of_pixels_InputImage;
int iNum_of_Dims_of_Input_Image;
int *piLookUpTable;
double *pdDistTable;
int iMinLookupTable;
int iMaxLookupTable;
int iLenLookupTable;
int iPixelIter, iLookupIter;
int iRowIter, iColIter, iIter, iUpper_Triang_Iter;
int iNeig_Pix_Idx;
int iLen_UpTriang_of_Cov;
int *piIndexes_of_UpTriang_of_Cov;
int *aiNeig_of_MeshGridData; //D
double K;
int *aiNeig_Midpoint;
double *adTemp, *adTemp_Transpose;
double *adDiag_of_Neig_Weights;
double *adCov_Curr_Pix_Interim, *adCov_Curr_Pix;
double *pdUpTriag_of_Cov_of_InputImage;
double scale = 4;//double(LOOKUP_TABLE_WIDTH)/(4*sqrt(2*log(2)));
pstAniso_Thread_Args = (ANISO_THREAD_ARGS *) vptrAniso_Thread_Args;
iThreadNumber = pstAniso_Thread_Args->iThreadNumber;
iNum_of_pixels_InputImage = pstAniso_Thread_Args->iNum_of_pixels_InputImage;
pdWeights_InputImage = pstAniso_Thread_Args->pdWeights_InputImage;
piMeshGridData_InputImage = pstAniso_Thread_Args->piMeshGridData_InputImage;
iNum_of_Dims_of_Input_Image = pstAniso_Thread_Args->iNum_of_Dims_of_Input_Image;
piLookUpTable = pstAniso_Thread_Args->piLookUpTable ;
pdDistTable = pstAniso_Thread_Args->pdDistTable ;
iMinLookupTable = pstAniso_Thread_Args->iMinLookupTable ;
iMaxLookupTable = pstAniso_Thread_Args->iMaxLookupTable ;
iLenLookupTable = pstAniso_Thread_Args->iLenLookupTable ;
iLen_UpTriang_of_Cov = pstAniso_Thread_Args->iLen_UpTriang_of_Cov;
piIndexes_of_UpTriang_of_Cov = pstAniso_Thread_Args->piIndexes_of_UpTriang_of_Cov;
K = pstAniso_Thread_Args->K;
pdUpTriag_of_Cov_of_InputImage = pstAniso_Thread_Args->pdUpTriag_of_Cov_of_InputImage ;
iImagePixel_Start = iThreadNumber*(iNum_of_pixels_InputImage/NUM_OF_PREPROCESS_THREADS_TO_CREATE);
if (iThreadNumber == (NUM_OF_PREPROCESS_THREADS_TO_CREATE-1)) // Last part of the image processed by this thread.
{
iImagePixel_End = iNum_of_pixels_InputImage-1 ;
}
else
{
iImagePixel_End = iImagePixel_Start + (iNum_of_pixels_InputImage/NUM_OF_PREPROCESS_THREADS_TO_CREATE) - 1;
}
printf ("\nPreprocess Thread %d, processing pixels %d to %d.", iThreadNumber, iImagePixel_Start, iImagePixel_End);
aiNeig_of_MeshGridData = (int*) malloc ((iLenLookupTable*iNum_of_Dims_of_Input_Image) * sizeof(int)); // For storing D = data(:,i+LookUpTable) [iLen dData]
adTemp = (double*) malloc ((iNum_of_Dims_of_Input_Image*iLenLookupTable) * sizeof(double)); // For storing: temp [dData x iLen]
adDiag_of_Neig_Weights = (double*) malloc (iLenLookupTable*iLenLookupTable*sizeof(double)); // For storing: diag(weights(i+LookUpTable)) [iLen x iLen]
adCov_Curr_Pix_Interim = (double *) malloc (iNum_of_Dims_of_Input_Image*iLenLookupTable*sizeof(double)); // For: temp*diag(weights(i+LookUpTable)) [dData x iLen]
adTemp_Transpose = (double *) malloc (iLenLookupTable*iNum_of_Dims_of_Input_Image*sizeof(double)); // For storing: temp' [iLen x dData]
adCov_Curr_Pix = (double *) malloc (iNum_of_Dims_of_Input_Image*iNum_of_Dims_of_Input_Image*sizeof(double)); // For storing: cov_i = temp*diag(weights(i+LookUpTable))*temp'/(k-1) [dData x dData]
for (iPixelIter = iImagePixel_Start; iPixelIter <= iImagePixel_End; iPixelIter++)
//for (iPixelIter = 17342; iPixelIter < 17343; iPixelIter++)
{
/*
* if i<-minL+1 | i>N-maxL
* continue;
* end
*/
if((iPixelIter < (-iMinLookupTable)) ||
(iPixelIter > (iNum_of_pixels_InputImage - iMaxLookupTable - 1))||
(pdWeights_InputImage[iPixelIter] < W_THRESHOLD))
{
continue;
}
//D = data(:,i+LookUpTable);
for (iLookupIter = 0 ; iLookupIter < iLenLookupTable; iLookupIter++)
{
iColIter = piLookUpTable[iLookupIter] + iPixelIter;
for (iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image ; iRowIter++)
{
aiNeig_of_MeshGridData[ROWCOL(iRowIter, iLookupIter,iNum_of_Dims_of_Input_Image)] =
piMeshGridData_InputImage[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)];
}
}
// temp = repmat(D(:,(lenL+1)/2)),1,lenL)-D;
aiNeig_Midpoint = &aiNeig_of_MeshGridData[iNum_of_Dims_of_Input_Image*(iLenLookupTable/2)];// D(:,(lenL+1)/2))
for (iColIter = 0 ; iColIter < iLenLookupTable; iColIter++)
{
for (iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image ; iRowIter++)
{
adTemp [ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] = (double)(aiNeig_Midpoint[iRowIter] -
aiNeig_of_MeshGridData[ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)]); // temp [dData x lenL]
}
}
Double_Compute_Transpose(adTemp, adTemp_Transpose, iNum_of_Dims_of_Input_Image, iLenLookupTable); // temp' [lenL x dData]
#if 0
for (iColIter = 0 ; iColIter < iLenLookupTable; iColIter++)
{
iNeig_Pix_Idx = piLookUpTable[iColIter] + iPixelIter;
for (iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image ; iRowIter++)
{
adTemp [ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] =
adTemp [ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)]*pdWeights_InputImage[iNeig_Pix_Idx]*pdWeights_InputImage[iNeig_Pix_Idx]/
exp(-pow((pdDistTable[iColIter]/(scale)),0))/(K-1); // temp [dData x lenL]
}
}
#else
for (iColIter = 0 ; iColIter < iLenLookupTable; iColIter++)
{
iNeig_Pix_Idx = piLookUpTable[iColIter] + iPixelIter;
for (iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image ; iRowIter++)
{
adTemp [ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)] =
adTemp [ROWCOL(iRowIter, iColIter, iNum_of_Dims_of_Input_Image)]*pdWeights_InputImage[iNeig_Pix_Idx]/(K-1); // temp [dData x lenL]
}
}
#endif
// cov_i = temp*diag(weights(i+LookUpTable))*temp'/(k-1);
// Double_Mat_Multiply(adTemp, adDiag_of_Neig_Weights, iNum_of_Dims_of_Input_Image, iLenLookupTable,
// iLenLookupTable, iLenLookupTable, adCov_Curr_Pix_Interim);
Double_Mat_Multiply(adTemp, adTemp_Transpose, iNum_of_Dims_of_Input_Image, iLenLookupTable,
iLenLookupTable, iNum_of_Dims_of_Input_Image, adCov_Curr_Pix);
// I added this part to above loop, so we dont need this anymore
// for (iIter = 0 ; iIter < iNum_of_Dims_of_Input_Image*iNum_of_Dims_of_Input_Image ; iIter++)
// {
// adCov_Curr_Pix[iIter] = adCov_Curr_Pix[iIter]/(K-1);
// }
// C(i,:) = cov_i(uptiu);
/*
* | cov_1(1) cov_1(3) cov_1(4) |
*
*
* | cov_N(1) cov_N(3) cov_N(4) | //N rows * 3 Cols*/
for (iUpper_Triang_Iter = 0 ; iUpper_Triang_Iter < iLen_UpTriang_of_Cov ; iUpper_Triang_Iter ++)
{
pdUpTriag_of_Cov_of_InputImage[ROWCOL(iPixelIter, iUpper_Triang_Iter, iNum_of_pixels_InputImage)] = adCov_Curr_Pix[piIndexes_of_UpTriang_of_Cov[iUpper_Triang_Iter]];
}
#if OUTPUT_COV == TRUE // Output Cov to a file in this form: [N x dData*(dData+1)/2]
for (iUpper_Triang_Iter = 0 ; iUpper_Triang_Iter < iLen_UpTriang_of_Cov ; iUpper_Triang_Iter ++)
{
fprintf (fp_cov, "%g ", pdUpTriag_of_Cov_of_InputImage[ROWCOL(iPixelIter, iUpper_Triang_Iter, iNum_of_pixels_InputImage)]);
}
fprintf (fp_cov, "\n");
#endif
} // end of for loop.
free (adCov_Curr_Pix_Interim); free (adTemp_Transpose); // Since they are no longer needed for any further processing.
free (adCov_Curr_Pix); free (aiNeig_of_MeshGridData); free (adTemp); free (adDiag_of_Neig_Weights );
// If files were created for saving temp, temp', diag_weights, cov_i or cov, close them.
#if OUTPUT_TEMP == TRUE
fclose (fp_temp);
#endif
#if OUTPUT_TEMP_TRANSPOSE == TRUE
fclose (fp_temp_tr);
#endif
#if OUTPUT_DIAG_OF_WEIGHTS == TRUE
fclose (fp_diag_weights);
#endif
#if OUTPUT_COV_I == TRUE
fclose (fp_cov_i);
#endif
}
//---------------------------------------------------------------------------------------------------------------------------------------
void SortTangentialSpace(double *pdCurrT, double *pdEigVecs_of_LogHessian, double *pdEigVals_of_LogHessian, int iNum_of_Dims_of_Input_Image )
{
double *pdTranspose_of_Tangential = (double *) malloc (INTRINSIC_DIM * iNum_of_Dims_of_Input_Image * sizeof(double)); // g' [1 x dData];
double *pdTransTang_LogHess_Interim_Output = (double *) malloc (INTRINSIC_DIM * iNum_of_Dims_of_Input_Image * sizeof (double));
double *eigVec = (double *) malloc (iNum_of_Dims_of_Input_Image * iNum_of_Dims_of_Input_Image * sizeof (double));
double *eigVal = (double *) malloc (iNum_of_Dims_of_Input_Image * sizeof (double));
int *processedBasis = (int *)malloc (INTRINSIC_DIM* sizeof (int));
int iTangIter, iRowIter, iColIter, iRit, iCit, maxInd, Iter;
double currEnt, maxVal;
// initialize basis
for(Iter = 0; Iter<iNum_of_Dims_of_Input_Image; Iter++)
{
processedBasis[Iter] = 0;
}
// sort the eigen values and the eigenvectors based on the tangential space of the origin
// Tangential transpose
Double_Compute_Transpose (pdCurrT, pdTranspose_of_Tangential, iNum_of_Dims_of_Input_Image, INTRINSIC_DIM);
// innerproduct between the reference and the current space
Double_Mat_Multiply (pdTranspose_of_Tangential, pdEigVecs_of_LogHessian, INTRINSIC_DIM, iNum_of_Dims_of_Input_Image, iNum_of_Dims_of_Input_Image, iNum_of_Dims_of_Input_Image, pdTransTang_LogHess_Interim_Output);
for(iTangIter = 0; iTangIter<INTRINSIC_DIM; iTangIter++)
{
// get the matched vector for the iDim th tangential basis
maxInd = 0;
maxVal = 0;
for(iColIter = 0; iColIter < iNum_of_Dims_of_Input_Image; iColIter++)
{
currEnt = fabs(pdTransTang_LogHess_Interim_Output[ROWCOL(iTangIter, iColIter, INTRINSIC_DIM)]);
if (currEnt>maxVal)
{
maxVal = currEnt;
maxInd = iColIter;
}
}
processedBasis[maxInd] = 1;
for(iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image; iRowIter++)
{
eigVec[ROWCOL(iRowIter,iTangIter,iNum_of_Dims_of_Input_Image)]=pdEigVecs_of_LogHessian[ROWCOL(iRowIter,maxInd,iNum_of_Dims_of_Input_Image)];
}
// eigVal[ROWCOL(iTangIter,iTangIter,iNum_of_Dims_of_Input_Image)] = pdEigVals_of_LogHessian[ROWCOL(maxInd,maxInd,iNum_of_Dims_of_Input_Image)];
eigVal[iTangIter] = pdEigVals_of_LogHessian[maxInd];
}
for(Iter = 0,iTangIter = INTRINSIC_DIM; Iter<iNum_of_Dims_of_Input_Image; Iter++)
{
if(!processedBasis[Iter])
{
for(iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image; iRowIter++)
{
eigVec[ROWCOL(iRowIter,iTangIter,iNum_of_Dims_of_Input_Image)]=pdEigVecs_of_LogHessian[ROWCOL(iRowIter,Iter,iNum_of_Dims_of_Input_Image)];
}
// eigVal[ROWCOL(iTangIter,iTangIter,iNum_of_Dims_of_Input_Image)] = pdEigVals_of_LogHessian[ROWCOL(Iter,Iter,iNum_of_Dims_of_Input_Image)];
eigVal[iTangIter] = pdEigVals_of_LogHessian[Iter];
iTangIter++;
}
}
// copy the elements
for(iRowIter = 0; iRowIter < iNum_of_Dims_of_Input_Image; iRowIter++){
for(iColIter = 0; iColIter < iNum_of_Dims_of_Input_Image; iColIter++){
pdEigVecs_of_LogHessian[ROWCOL(iRowIter,iColIter,iNum_of_Dims_of_Input_Image)] = eigVec[ROWCOL(iRowIter,iColIter,iNum_of_Dims_of_Input_Image)];
}
pdEigVals_of_LogHessian[iRowIter] = eigVal[iRowIter];
}
free(pdTranspose_of_Tangential); free(pdTransTang_LogHess_Interim_Output); free(eigVec); free(eigVal);
}
GAE_Matrix4_t* GAE_Matrix4_create2dProjectionMatrix(GAE_Matrix4_t* projectionMatrix, const float left, const float bottom, const float right, const float top, const float nearClip, const float farClip) {
const float xScale = -2.0F / (right - left);
const float yScale = 2.0F / (top - bottom);
const float zScale = -2.0F / (farClip - nearClip);
const float x = -(right + left) / (right - left);
const float y = -(top + bottom) / (top - bottom);
const float z = -(farClip + nearClip) / (farClip - nearClip);
(*projectionMatrix)[ROWCOL(0, 0, 4)] = xScale;
(*projectionMatrix)[ROWCOL(0, 1, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(0, 2, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(0, 3, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(1, 0, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(1, 1, 4)] = yScale;
(*projectionMatrix)[ROWCOL(1, 2, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(1, 3, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(2, 0, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(2, 1, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(2, 2, 4)] = zScale;
(*projectionMatrix)[ROWCOL(2, 3, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(3, 0, 4)] = x;
(*projectionMatrix)[ROWCOL(3, 1, 4)] = y;
(*projectionMatrix)[ROWCOL(3, 2, 4)] = z;
(*projectionMatrix)[ROWCOL(3, 3, 4)] = 1.0F;
return projectionMatrix;
}
GAE_Matrix4_t* GAE_Matrix4_create3dProjectionMatrix(GAE_Matrix4_t* projectionMatrix, const float nearClip, const float farClip, const float fov, const float aspectRatio) {
const float radians = fov / ((2.0F * 3.14F) / 180.0F);
const float size = nearClip * tanf(radians);
const float left = -size;
const float right = size;
const float bottom = -size / aspectRatio;
const float top = size / aspectRatio;
(*projectionMatrix)[ROWCOL(0, 0, 4)] = 2 * nearClip / (right - left);
(*projectionMatrix)[ROWCOL(0, 1, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(0, 2, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(0, 3, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(1, 0, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(1, 1, 4)] = 2 * nearClip / (top - bottom);
(*projectionMatrix)[ROWCOL(1, 2, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(1, 3, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(2, 0, 4)] = (right + left) / (right - left);
(*projectionMatrix)[ROWCOL(2, 1, 4)] = (top + bottom) / (top - bottom);
(*projectionMatrix)[ROWCOL(2, 2, 4)] = (farClip + nearClip) / (farClip - nearClip);
(*projectionMatrix)[ROWCOL(2, 3, 4)] = -1.0F;
(*projectionMatrix)[ROWCOL(3, 0, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(3, 1, 4)] = 0.0F;
(*projectionMatrix)[ROWCOL(3, 2, 4)] = (2 * farClip * nearClip) / (farClip - nearClip);
(*projectionMatrix)[ROWCOL(3, 3, 4)] = 0.0F;
return projectionMatrix;
}
mat4 operator*(const mat4 &a, const mat4 &b)
{
#define ROWCOL(i, j) \
a.v[i].n[0]*b.v[0][j] + \
a.v[i].n[1]*b.v[1][j] + \
a.v[i].n[2]*b.v[2][j] + \
a.v[i].n[3]*b.v[3][j]
return mat4(
vec4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
vec4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
vec4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
vec4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3))
);
#undef ROWCOL
}
// Performs operation: AX = B, i.e X = (A inverse)*B
// Requirements:
// A must be a square matrix.
// Number of rows in B = number of rows in A.
void Compute_ML_Divide(double *pdInput_A, double *pdInput_B, int iNum_of_rows_of_A, int iNum_of_cols_of_A,
int iNum_of_rows_of_B, int iNum_of_cols_of_B, double *pdOutput)
{
double temp;
double *pd_Matrix_A, *pd_Matrix_B; // Matrix_A initialized to copy of A. Matrix_B to copy of B. Eventually, A will be reduced to Identity form, and B to the final output.
int iNum_of_row_and_cols_of_A, iNum_of_elems_of_A, iNum_of_elems_of_B ;
int iIter;
if (iNum_of_rows_of_A == iNum_of_cols_of_A) // confirms that A is square matrix.
{
iNum_of_row_and_cols_of_A = iNum_of_rows_of_A; // = iNum_of_cols_of_A
}
else
{
printf ("\nError (Compute_ML_Divide): A is not a square matrix.");
//exit (1);
}
pd_Matrix_A = (double *) malloc (iNum_of_rows_of_A * iNum_of_cols_of_A * sizeof(double)); // Allocate memory for Matrix_A
pd_Matrix_B = pdOutput; // Since memory for output matrix is allocated by the calling function.
iNum_of_elems_of_A = iNum_of_rows_of_A * iNum_of_cols_of_A ;
iNum_of_elems_of_B = iNum_of_rows_of_B * iNum_of_cols_of_B ;
for (iIter = 0; iIter < iNum_of_elems_of_A ; iIter++)
{
pd_Matrix_A[iIter] = pdInput_A[iIter];
}
for (iIter = 0; iIter < iNum_of_elems_of_B ; iIter++)
{
pd_Matrix_B[iIter] = pdInput_B[iIter];
}
/*
A -> 10x10 matrix
Sample Procedure:
In the first iteration of the main outer loop the following steps occur:
Step 1) Finds the largest element in the matrix, which then becomes the pivot.
If (20, 50) is the largest element in the matrix, then
big = A(20,50)
irow = 20, icol = 50
ipiv[icol]++ --> ipiv[50] = 1
Step 2) We put the pivot element on the diagonal, by performing only row operations.
A(20, 50) becomes A(50,50) by interchanging row 20 with row 50.
NOTE: If the pivot element was already a diagonal element (row = col), then no
swapping is required.
At the of step 2, the pivot element is at location A(iCol, iCol), i.e A(50,50) in our example.
And the row that will be normalized by the pivot elemt is iCol, i.e Row 50 in our example.
Step 3) Save the row and column index (iRow and iCol) of the pivot to unscramble the column swap
after the matrix has been reduced to its identity form.
Step 4) Normalize the row in which the pivot element is present.
i.e Divide every element in row 50 by A(50,50).
So A(50,50) = 1.
Step 5) Reduce all columns of the pivot row to zero, except for the pivot itself.
Step 6) Repeat Steps 1-5 to obtain the identity matrix in scrambled form
Step 7) Unscramble by interchanging pairs of columns in reverse order, starting with the last column that was swapped.
*/
int *piPivot_diag_positions = (int*) malloc (sizeof(int) * iNum_of_row_and_cols_of_A);
#if 0 // not needed if no column interchanges are done.
int *piPivot_Row_Index = (int*) malloc (sizeof(int) * iNum_of_row_and_cols_of_A);
int *piPivot_Col_Index = (int*) malloc (sizeof(int) * iNum_of_row_and_cols_of_A);
#endif
int iIter_Diag;
double dGreatest_element;
int iIter_Row, iIter_Col;
int iPivot_Row, iPivot_Col;
int iIter_Col_SWAP;
double dPivot_Inverse;
double dPivot_Col_Element;
for (iIter = 0;iIter < iNum_of_row_and_cols_of_A; iIter++)
{
piPivot_diag_positions[iIter]=0;
}
#if 1 // BIGGGGGG
for (iIter_Diag = 0; iIter_Diag < iNum_of_row_and_cols_of_A; iIter_Diag++)
{
// This is the main loop over the columns to be reduced.
dGreatest_element = 0.0;
for (iIter_Row = 0;iIter_Row < iNum_of_row_and_cols_of_A;iIter_Row++)
{
// This is the outer loop of the search for a pivot element.
if (piPivot_diag_positions[iIter_Row] != 1)
{
for (iIter_Col = 0;iIter_Col < iNum_of_row_and_cols_of_A; iIter_Col++)
{
if (piPivot_diag_positions[iIter_Col] == 0)
{
if (fabs(pd_Matrix_A[ROWCOL(iIter_Row, iIter_Col, iNum_of_row_and_cols_of_A)]) >= dGreatest_element)
{
dGreatest_element = fabs(pd_Matrix_A[ROWCOL(iIter_Row, iIter_Col, iNum_of_row_and_cols_of_A)]);
iPivot_Row = iIter_Row;//20
iPivot_Col = iIter_Col;//50
}
}
}
}
}
++(piPivot_diag_positions[iPivot_Col]);// ipiv[50] = 1;
/* if (iPivot_Row != iPivot_Col)
{
for (iIter_Col_SWAP = 0; iIter_Col_SWAP < iNum_of_row_and_cols_of_A; iIter_Col_SWAP++)
{
SWAP(pd_Matrix_A[ROWCOL(iPivot_Row, iIter_Col_SWAP, iNum_of_row_and_cols_of_A)],
pd_Matrix_A[ROWCOL(iPivot_Col, iIter_Col_SWAP, iNum_of_row_and_cols_of_A)]);
}
for (iIter_Col_SWAP = 0; iIter_Col_SWAP < iNum_of_cols_of_B; iIter_Col_SWAP++)
{
SWAP(pd_Matrix_B[ROWCOL(iPivot_Row, iIter_Col_SWAP, iNum_of_cols_of_B)],
pd_Matrix_B[ROWCOL(iPivot_Col, iIter_Col_SWAP, iNum_of_cols_of_B)]);
}
}*/
#if 0 // not required because we are not performing column interchanges
piPivot_Row_Index[iIter_Diag]=iPivot_Row; //We are now ready to divide the pivot row by the
piPivot_Col_Index[iIter_Diag]=iPivot_Col; //pivot element, located at irow and icol.
#endif
if ((pd_Matrix_A[ROWCOL(iPivot_Col, iPivot_Col, iNum_of_row_and_cols_of_A)]) == 0.0)
{
printf ("\nLoghessian Matrix seems singular: ");
for (iIter = 0 ; iIter < iNum_of_elems_of_A ; iIter++)
{
printf ("%g\t", pdInput_A[iIter]);
}
printf ("Pivot = %g", dGreatest_element);
//mexPrintf("\nWarning: Compute_ML_Divide: Matrix A may be singular. Results may not be accurate. ");
}
dPivot_Inverse = 1.0/(pd_Matrix_A[ROWCOL(iPivot_Col, iPivot_Col, iNum_of_row_and_cols_of_A)]) ;
pd_Matrix_A[ROWCOL(iPivot_Col, iPivot_Col, iNum_of_row_and_cols_of_A)] = 1.0;
for (iIter_Col = 0;iIter_Col < iNum_of_row_and_cols_of_A;iIter_Col++)
{
if (iIter_Col != iPivot_Col)
{
pd_Matrix_A[ROWCOL(iPivot_Col, iIter_Col, iNum_of_row_and_cols_of_A)] *= dPivot_Inverse;
}
}
for (iIter_Col = 0; iIter_Col < iNum_of_cols_of_B; iIter_Col++)
{
pd_Matrix_B[ROWCOL(iPivot_Col, iIter_Col, iNum_of_row_and_cols_of_A)] *= dPivot_Inverse;
}
for (iIter_Row = 0; iIter_Row < iNum_of_row_and_cols_of_A; iIter_Row++) //Next, we reduce the rows...
{
if (iIter_Row != iPivot_Col) //...except for the pivot one, of course.
{
dPivot_Col_Element = pd_Matrix_A[ROWCOL(iIter_Row, iPivot_Col, iNum_of_row_and_cols_of_A)];
pd_Matrix_A[ROWCOL(iIter_Row, iPivot_Col, iNum_of_row_and_cols_of_A)]= 0.0;
for (iIter_Col = 0; iIter_Col < iNum_of_row_and_cols_of_A;iIter_Col++)
{
if (iIter_Col != iPivot_Col)
{
pd_Matrix_A[ROWCOL(iIter_Row, iIter_Col, iNum_of_row_and_cols_of_A)] -=
(pd_Matrix_A[ROWCOL(iPivot_Col, iIter_Col, iNum_of_row_and_cols_of_A)] * dPivot_Col_Element);
}
}
for (iIter_Col = 0; iIter_Col < iNum_of_cols_of_B;iIter_Col++)
{
pd_Matrix_B[ROWCOL(iIter_Row, iIter_Col, iNum_of_row_and_cols_of_A)] -=
(pd_Matrix_B[ROWCOL(iPivot_Col, iIter_Col, iNum_of_row_and_cols_of_A)] * dPivot_Col_Element);
}
}
}
} // end of main for loop: for (iIter_Diag = 0...
#endif // #if 0 BIGGGGG
/*
AX = B
a11X + a12Y + a13Z = b1
a21X + a22Y + a23Z = b2
a31X + a32Y + a33Z = b3
a11P + a12Q + a13R = c1
a21P + a22Q + a23R = c2
a31P + a32Q + a33R = c3
a11L + a12M + a13N = d1
a21L + a22M + a23N = d2
a31L + a32M + a33N = d3
|a11 a12 a13| | X P L | | b1 c1 d1|
|a21 a22 a23| x | Y Q M | = | b2 c2 d2|
|a31 a32 a33| | Z R N | | b3 c3 d3|
R1 <--> R2
|a21 a22 a23| | X P L | | b2 c2 d2|
|a11 a12 a13| x | Y Q M | = | b1 c1 d1|
|a31 a32 a33| | Z R N | | b3 c3 d3|
MULTIPLYING...
a21X + a22Y + a23Z = b2
a11X + a12Y + a13Z = b1
a31X + a32Y + a33Z = b3 AND SO ON
C1 <---> C2 on the original matrix
|a12 a11 a13| | X P L | | c1 b1 d1|
|a22 a21 a23| x | Y Q M | = | c2 b2 d2|
|a32 a31 a33| | Z R N | | c3 b3 d3|
a12X + a11Y + a13Z = c1 a11X + a12Y + a13Z = b1
a22X + a21Y + a23Z = c2 a21X + a22Y + a23Z = b2
a32X + a31Y + a33Z = c3 a31X + a32Y + a33Z = b3
In order to preserve the original equantions, Column operations must not be performed on RHS (as seen in alg)
Also, we have to replace X and Y since we did a column operation on LHS Columns 1 and 2,
so they can be compensated by Row operations in the final RHS result
*/
#if 0
//unscrambling
for (iIter = (iNum_of_row_and_cols_of_A-1) ; iIter >= 0; iIter--)
{
if(piPivot_Row_Index[iIter] != piPivot_Col_Index[iIter])
{
for (iIter_Col = 0;iIter_Col < iNum_of_row_and_cols_of_A; iIter_Col++)
{
SWAP ((pd_Matrix_B[ROWCOL(piPivot_Row_Index[iIter], iIter_Col, iNum_of_row_and_cols_of_A)]),
(pd_Matrix_A[ROWCOL(piPivot_Col_Index[iIter], iIter_Col, iNum_of_row_and_cols_of_A)]));
}
}
}
#endif
free (piPivot_diag_positions);
free (pd_Matrix_A);
}
