int main( int argc, char* argv[] )
{
// problem parameters
unsigned NTRAIN = 4096;
unsigned dim = 16;
unsigned NTEST = 3*NTRAIN;
size_t i,j,k;
// training and test patterns
float *train_patterns = (float*)malloc(NTRAIN*dim*sizeof(float));
float *test_patterns = (float*)malloc(NTEST*dim*sizeof(float));
// result
unsigned dist_matrix_size = NTRAIN*NTEST;
float *dist_matrix_cpu = (float*)malloc(dist_matrix_size*sizeof(float));
float *dist_matrix = (float*)malloc(dist_matrix_size*sizeof(float));
zeroMatrix(dist_matrix_cpu, dist_matrix_size);
zeroMatrix(dist_matrix, dist_matrix_size);
// initialize with some values ...
for (i=0;i<NTRAIN;i++){
for (k=0;k<dim;k++){
train_patterns[i*dim + k] = (float)sin(i);
}
}
for (i=0;i<NTEST;i++){
for (k=0;k<dim;k++){
test_patterns[i*dim + k] = (float)cos(i);
}
}
Stopwatch timer;
timer.start();
float d,tmp;
for (i=0;i<NTEST;i++) {
// compute distances to all training patterns
for (j=0;j<NTRAIN;j++) {
d = 0.0;
// for each feature
for (k=0;k<dim;k++) {
tmp = test_patterns[i*dim+k]-train_patterns[j*dim+k];
d += tmp*tmp;
}
dist_matrix_cpu[j*NTEST + i] = d;
}
}
cout << "$Sequential_time " << timer.stop() << endl;
OCLKNearestTask ocl_task;
ocl_task.RunOCLKNearestForKernel(
NTEST, NTRAIN, dim,
dist_matrix, NTEST, NTRAIN,
"gpu", test_patterns, dim, NTEST, train_patterns,
dim, NTRAIN);
return 0;
}
void
gaussianEstimator_Pred ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, Matrix (*afun)(Matrix m, float t), float *dt, Matrix *m_opt)
{
float D = elem(sizeOfMatrix(*m_opt),0,1)+1; //printf("%f\n", D);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = elem(sizeOfMatrix(*xEst),0,0); //printf("%f\n", Nx);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
/* Eigenvectors, Eigenvalues */
int dimC = elem ( sizeOfMatrix(*CEst), 0, 0 );
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val );
/* m1 = vec*sqrtf(val) */
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val);
freeMatrix(Vec);
freeMatrix(Val);
/* rotate & scale samples: m = m1*S */
Matrix m = scaledSamplePoints(m1, *m_opt);
/* x = x*ones(1,d) */
Matrix x = fillMatrix(*xEst, d);
/* shift samples: m = m + x */
m = addMatrix(m, x); //printMatrix(m);
/* afun */
/* Prediction: mean
xPredDiracs = feval(afun,m, [], [], t);
xPred = w*sum(xPredDiracs, 2);*/
Matrix xPredDiracs = (*afun) (m, *dt); //printMatrix(xPredDiracs);
Matrix xPredDiracsSum = sumMatrix(xPredDiracs, 2); //printMatrix(xPredDiracsSum);
Matrix xPred = mulScalarMatrix(w, xPredDiracsSum); //printMatrix(xPred);
//mxDiracs = xPredDiracs-repmat(xPred, 1, d);
//CPred = w_opt*mxDiracs*mxDiracs';
Matrix mxDiracs = subMatrix(xPredDiracs, fillMatrix(xPred, d)); //printMatrix(mxDiracs);
Matrix CPred = mulScalarMatrix(w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs))); //printMatrix(CPred);
//RETURN
*xEst = xPred; //printMatrix(*xEst);
*CEst = CPred; //printMatrix(*CEst);
freeMatrix(m);
freeMatrix(xPredDiracs);
freeMatrix(xPredDiracsSum);
}
Matrix
scaledSamplePoints(Matrix m, Matrix m_opt)
{
int w = m_opt->width;
int h = m->height;
Matrix res = zeroMatrix(h, h*w + 1);
Lines r1 = m->lines;
Lines r2 = m_opt->lines;
Lines r3 = res->lines;
int i, j, k, l;
for (i=0; i<h; i++)
{
l = 0;
j = 1;
while (j<h*w)
{
for (k=0; k<w; k++)
{
r3[i][j] = r1[i][l] * r2[0][k];
j++;
}
l++;
}
}
return res;
}
int blockMatrixMultiply(double* a, double* b, double* out, int n, int m){
const int s = n/m;
const int r = n - s*m;
const int matrix_size = n*n;
const int block_size = m*m;
const int string_size = m*n;
const int small_block_size = r*m;
const int smallest_block_size = r*r;
int i, j, k;
int res = zeroMatrix(out, matrix_size);
if (res!=0){
printf("zeroMatrix failed!\n\t--blockmatrixmultiply\n");
}
double* const temp_block = new double[block_size];
if (r>0){
for (i=0; i<s; i++){
for (j=0; j<s; j++){
for (k=0; k<s; k++){
res = simpleMatrixMultiply(a+i*string_size+k*block_size, b+k*string_size+j*block_size, temp_block, m, m, m);
res = addToMatrix(out+i*string_size+j*block_size, temp_block, block_size);
}
res = simpleMatrixMultiply(a+i*string_size+s*block_size, b+s*string_size+j*small_block_size, temp_block, m, r, m);
res = addToMatrix(out+i*string_size+j*block_size, temp_block, block_size);
}
for (k=0; k<s; k++){
res = simpleMatrixMultiply(a+i*string_size+k*block_size, b+k*string_size+s*block_size, temp_block, m, m, r);
res = addToMatrix(out+i*string_size+s*block_size, temp_block, small_block_size);
}
res = simpleMatrixMultiply(a+i*string_size+s*block_size, b+s*string_size+s*small_block_size, temp_block, m, r, r);
res = addToMatrix(out+i*string_size+s*block_size, temp_block, small_block_size);
}
for (j=0; j<s; j++){
for (k=0; k<s; k++){
res = simpleMatrixMultiply(a+s*string_size+k*small_block_size, b+k*string_size+j*block_size, temp_block, r, m, m);
res = addToMatrix(out+s*string_size+j*small_block_size, temp_block, small_block_size);
}
res = simpleMatrixMultiply(a+s*string_size+s*small_block_size, b+s*string_size+j*small_block_size, temp_block, r, r, m);
res = addToMatrix(out+s*string_size+j*small_block_size, temp_block, small_block_size);
}
for (k=0; k<s; k++){
res = simpleMatrixMultiply(a+s*string_size+k*small_block_size, b+k*string_size+s*block_size, temp_block, r, m, r);
res = addToMatrix(out+s*string_size+s*small_block_size, temp_block, smallest_block_size);
}
res = simpleMatrixMultiply(a+s*string_size+s*small_block_size, b+s*string_size+s*small_block_size, temp_block, r, r, r);
res = addToMatrix(out+s*string_size+s*small_block_size, temp_block, smallest_block_size);
}
else{
for (i=0; i<s; i++){
for (j=0; j<s; j++){
for (k=0; k<s; k++){
res = simpleMatrixMultiply(a+i*string_size+k*block_size, b+k*string_size+j*block_size, temp_block, m, m, m);
res = addToMatrix(out+i*string_size+j*block_size, temp_block, block_size);
}
}
}
}
delete[] temp_block;
return 0;
}
int main(void)
{
int i, j, m, n, **matrix;
while (scanf("%d %d", &m, &n) != EOF) {
// 动态申请二维数组内存空间
matrix = (int **)malloc(sizeof(int *) * m);
for (i = 0; i < m; i ++)
matrix[i] = (int *)malloc(sizeof(int) * n);
// 接收矩阵元素
for (i = 0; i < m; i ++)
for (j = 0; j < n; j ++)
scanf("%d", &matrix[i][j]);
printMatrix(matrix, m, n);
zeroMatrix(matrix, m, n);
printMatrix(matrix, m, n);
// 手动释放
for (i = 0; i < m; i ++)
free(matrix[i]);
}
return 0;
}
Matrix scale(double x, double y, double z)
{
Matrix ret = zeroMatrix();
ret.x[0][0] = x;
ret.x[1][1] = y;
ret.x[2][2] = z;
ret.x[3][3] = 1.0;
return ret;
}
void simulateJumps::init()
{
//init the vector of waiting times.
_waitingTimeParams.clear();
_waitingTimeParams.resize(_alphabetSize);
int i, j;
for (i = 0; i < _alphabetSize; ++i)
{
_waitingTimeParams[i] = -_sp.dPij_dt(i, i, 0.0);
}
//init _jumpProbs.
//_jumpProbs[i][j] = Q[i][j] / -Q[i][i]
_jumpProbs.clear();
_jumpProbs.resize(_alphabetSize);
for (i = 0; i < _alphabetSize; ++i)
{
MDOUBLE sum = 0.0;
_jumpProbs[i].resize(_alphabetSize);
for (j = 0; j < _alphabetSize; ++j)
{
if (i == j)
_jumpProbs[i][j] = 0.0;
else
{
_jumpProbs[i][j] = _sp.dPij_dt(i, j, 0.0) / _waitingTimeParams[i];
}
sum += _jumpProbs[i][j];
}
if (! DEQUAL(sum, 1.0)){
string err = "error in simulateJumps::init(): sum probabilities is not 1 and equal to ";
err+=double2string(sum);
errorMsg::reportError(err);
}
}
//init _orderNodesVec: a vector in which the branch lengths are ordered in ascending order
_tree.getAllNodes(_orderNodesVec, _tree.getRoot());
sort(_orderNodesVec.begin(), _orderNodesVec.end(), simulateJumpsAbstract::compareDist);
_nodes2JumpsExp.clear();
_nodes2JumpsProb.clear();
VVdouble zeroMatrix(getCombinedAlphabetSize());
for (i = 0; i < getCombinedAlphabetSize(); ++i)
zeroMatrix[i].resize(getCombinedAlphabetSize(), 0.0);
Vdouble zeroVector(getCombinedAlphabetSize(),0.0);
for (i = 0; i < _orderNodesVec.size(); ++i)
{
string nodeName = _orderNodesVec[i]->name();
_nodes2JumpsExp[nodeName] = zeroMatrix;
_nodes2JumpsProb[nodeName] = zeroMatrix;
for (j=0; j<getCombinedAlphabetSize();++j)
_totalTerminals[nodeName]=zeroVector;
}
}
inline Matrix scale(precision x, precision y, precision z){
Matrix ret = zeroMatrix();
ret.x[3][0] = x;
ret.x[3][1] = y;
ret.x[3][2] = z;
ret.x[3][3] = 1.0f;
return ret;
}
void Cage::paint()
{
int &width = getWidth(0);
int &height = getHeight(0);
int maxX = width/cellSize+1;
int maxY = height/cellSize+1;
if (maxX >= MATRIX_SIZE || maxY >= MATRIX_SIZE)
return;
//size(width, height);
background(backColor);
applyWave();
Areas &areas = getAreas(0);
for (unsigned int i=0; i<areas.size(); i++) {
Area &area = areas.at(i);
if (area.pt[0] > 0 && area.pt[1] > 0 && area.pt[0] < width && area.pt[1] < height) {
int x1 = (area.pt[0]-(area.width/2 ))/cellSize - influence;
int y1 = (area.pt[1]-(area.height/2))/cellSize - influence;
int x2 = (area.pt[0]+(area.width/2 ))/cellSize + influence;
int y2 = (area.pt[1]+(area.height/2))/cellSize + influence;
zeroMatrix(x1, y1, x2, y2);
}
}
m[0][0] = 1;
m[maxX-1][0] = 1;
color(boxColor);
for (int i=0; i<maxX; i++)
for (int j=0; j<maxY; j++)
{
if ( i > 0 && m[i-1][j] == 1 && chance(prob/500.0) ) {
m[i][j] = 1;
}
if ( j > 0 && m[i][j-1] == 1 && chance(prob/500.0) ) {
m[i][j] = 1;
}
if ( i < maxX-1 && m[i+1][j] == 1 && chance(prob/500.0) ) {
m[i][j] = 1;
}
if ( j < maxY-1 && m[i][j+1] == 1 && chance(prob/500.0) ) {
m[i][j] = 1;
}
if ( m[i][j] == 1) {
image(box, i*cellSize+random(boxDeviation)-boxDeviation/2,
j*cellSize+random(boxDeviation)-boxDeviation/2, boxSize, boxSize);
}
}
}
RigidBody::RigidBody(const char *name) :
massInv(0),
x(0,0,0),
q(),
P(0,0,0),
L(0,0,0),
v(0,0,0),
omega(0,0,0),
force(0,0,0),
torque(0,0,0)
{
zeroMatrix(Ibody);
zeroMatrix(Ibodyinv);
zeroMatrix(Iinv);
zeroMatrix(R);
setName(name);
shapeId = hlGenShapes(1);
}
/* Create a tmat that scales things by x in the x direction, y in the y direction,
* z in the z direction */
tmat scale(float x, float y, float z)
{
tmat mat = zeroMatrix();
mat.m[0][0] = x;
mat.m[1][1] = y;
mat.m[2][2] = z;
mat.m[3][3] = 1.0f;
return mat;
}
PERF_TEST_P(Size_MatType, Mat_Zeros,
testing::Combine(testing::Values(TYPICAL_MAT_SIZES),
testing::Values(TYPICAL_MAT_TYPES, CV_32FC3))
)
{
Size size = get<0>(GetParam());
int type = get<1>(GetParam());
Mat zeroMatrix(size.height, size.width, type);
declare.out(zeroMatrix);
int runs = (size.width <= 640) ? 15 : 5;
TEST_CYCLE_MULTIRUN(runs)
{
zeroMatrix = Mat::zeros(size, type);
}
SANITY_CHECK(zeroMatrix, 1);
}
bool
CMatrix::isZeroMatrix(
) const
{
return( *this == zeroMatrix() );
}
void
CMatrix::setZeroMatrix(
)
{
*this = zeroMatrix();
}
int main(int argc, char * argv[]) {
int rank_grid, rank_row, rank_col;
int coordinates[2];
int node_total_size;
int node_dim_size;
int elem_dim_size;
int subelem_dim_size;
int * scatter_sendcount;
int * scatter_displacement;
int gridinit_num_dims = 2;
int gridinit_dims[2] = {0,0};
int gridinit_periods[2] = {0,0};
int gridinit_reorder = 1;
MPI_Comm mpi_comm_grid, mpi_comm_row, mpi_comm_col;
MPI_Datatype mpi_type_submatrix, mpi_type_submatrix_vector;
MPI_Request fox_send_request, fox_recv_request;
int fox_sendto, fox_recfrom, fox_sendtag, fox_rectag;
int fox_broadcaster;
double *mat_a, *mat_b, *mat_c;
double *A_mine, *B_old, *B_new, *C_mine, *A_bcast;
double *mat_verify;
int i, j, k;
int verify = 0;
int verbose = 0;
MPI_Init(&argc, &argv);
MPI_Comm_size(MPI_COMM_WORLD, &node_total_size);
double starttime, endtime;
starttime = MPI_Wtime();
// Set up cartesian coordinate grid
MPI_Dims_create(node_total_size,
gridinit_num_dims,
gridinit_dims);
MPI_Cart_create(MPI_COMM_WORLD,
gridinit_num_dims,
gridinit_dims,
gridinit_periods,
gridinit_reorder,
&mpi_comm_grid);
// ** Get the grid coordinates of this process.
MPI_Comm_rank(mpi_comm_grid, &rank_grid);
MPI_Cart_coords(mpi_comm_grid, rank_grid, gridinit_num_dims, coordinates);
// ** Set up column communicators.
MPI_Comm_split(mpi_comm_grid, coordinates[1], coordinates[0], &mpi_comm_col);
MPI_Comm_rank(mpi_comm_col, &rank_col);
// ** Set up row communicators
MPI_Comm_split(mpi_comm_grid, coordinates[0], coordinates[1], &mpi_comm_row);
MPI_Comm_rank(mpi_comm_row, &rank_row);
// Get the number of processors per dimension in grid.
MPI_Comm_size(mpi_comm_row, &node_dim_size);
// ********************************************
// ** CHECK SANITY OF AND SET UP ENVIRONMENT **
// ********************************************
// Check that number of parameters is sane.
if(argc < 2) {
if(rank_grid == 0)
printf("Usage: foxmatrix N\n N = randomize NxN matrices.\n");
MPI_Finalize();
return -1;
}
// Get the number of elements per dimension in matrices from arguments.
elem_dim_size = atoi(argv[1]);
// Check that number of processors is sane.
if(sqrt(node_total_size) != (double) ((int) sqrt(node_total_size))) {
if(rank_grid == 0)
printf("Not a square number of processors.\n");
MPI_Finalize();
return -1;
}
// Check that it is possible to split matrix over the processors.
if(elem_dim_size % node_dim_size != 0) {
if(rank_grid == 0)
printf("Cannot split elements evenly over processors.\n");
MPI_Finalize();
return -1;
}
// Calculate the size (in one dimension) of the submatrices.
subelem_dim_size = elem_dim_size / node_dim_size;
// Check if the user has given the verify/verbose commands.
if(argc == 3 && strcmp(argv[2], "verify") == 0)
verify = 1;
else if(argc == 3 && strcmp(argv[2], "verbose") == 0)
verbose = 1;
else if(argc == 4 &&
strcmp(argv[2], "verbose") == 0 &&
strcmp(argv[3], "verify") == 0) {
verbose = 1;
verify = 1;
} else if(argc == 4 &&
strcmp(argv[2], "verify") == 0 &&
strcmp(argv[3], "verbose") == 0) {
verbose = 1;
verify = 1;
}
// Create datatype used for transmitting submatrices.
// Idea of using vector+struct taken from http://www.mcs.anl.gov/research/projects/mpi/tutorial/mpiexmpl/src4/scatter/C/solution.html.
MPI_Type_vector(subelem_dim_size,
subelem_dim_size,
elem_dim_size,
MPI_DOUBLE,
&mpi_type_submatrix_vector);
int sm_blocklength[2] = {1, 1};
MPI_Aint sm_displacement[2] = {0, subelem_dim_size * sizeof(double)};
MPI_Datatype sm_types[2] = {mpi_type_submatrix_vector, MPI_UB};
MPI_Type_struct(2,
sm_blocklength,
sm_displacement,
sm_types,
&mpi_type_submatrix);
MPI_Type_commit(&mpi_type_submatrix);
// ** CREATE MATRICES AND SET UP SCATTERV/GATHERV VARIABLES **
if(rank_grid == 0) {
// Create matrices on rank 0.
mat_a = (double *) malloc(elem_dim_size * elem_dim_size * sizeof(double));
mat_b = (double *) malloc(elem_dim_size * elem_dim_size * sizeof(double));
mat_c = (double *) malloc(elem_dim_size * elem_dim_size * sizeof(double));
// Randomize matrix contents.
randomMatrixInit();
randomMatrix(mat_a, elem_dim_size);
randomMatrix(mat_b, elem_dim_size);
// Allocate memory for storing scattering information.
scatter_sendcount = (int *) malloc(node_total_size * sizeof(int));
scatter_displacement = (int *) malloc(node_total_size * sizeof(int));
// Set up scatter/gather arguments.
int sit;
for(sit = 0; sit < node_total_size; sit++) {
scatter_sendcount[sit] = 1;
if(sit == 0)
scatter_displacement[sit] = 0;
else {
scatter_displacement[sit] = scatter_displacement[sit - 1] + 1;
if(sit % node_dim_size == 0)
// At end of line, go to start of next submatrix.
scatter_displacement[sit] += node_dim_size * (subelem_dim_size - 1);
}
}
}
A_mine = (double *) malloc(subelem_dim_size*subelem_dim_size*sizeof(double));
A_bcast = (double *) malloc(subelem_dim_size*subelem_dim_size*sizeof(double));
B_old = (double *) malloc(subelem_dim_size*subelem_dim_size*sizeof(double));
B_new = (double *) malloc(subelem_dim_size*subelem_dim_size*sizeof(double));
C_mine = (double *) malloc(subelem_dim_size*subelem_dim_size*sizeof(double));
zeroMatrix(C_mine, subelem_dim_size);
// ** DISTRIBUTE THE SUBMATRICES TO THE GRID NODES **
MPI_Scatterv(mat_a,
scatter_sendcount,
scatter_displacement,
mpi_type_submatrix,
A_mine,
subelem_dim_size * subelem_dim_size,
MPI_DOUBLE,
0,
mpi_comm_grid);
MPI_Scatterv(mat_b,
scatter_sendcount,
scatter_displacement,
mpi_type_submatrix,
B_new,
subelem_dim_size * subelem_dim_size,
MPI_DOUBLE,
0,
mpi_comm_grid);
// ** PERFORM FOX'S ALGORITHM FOR MATRIX MULTIPLICATION **
for(k = 0; k < node_dim_size; k++) {
// **** BROADCAST A **** //
// Decide who broadcasts this iteration.
fox_broadcaster = (k + rank_col) % node_dim_size;
// Copy matrix to the broadcast variable of the node that shall broadcast.
if(rank_row == fox_broadcaster)
copyMatrix(A_bcast, A_mine, subelem_dim_size);
// Perform the broadcasting of the A matrix.
MPI_Bcast(A_bcast,
subelem_dim_size * subelem_dim_size,
MPI_DOUBLE,
fox_broadcaster,
mpi_comm_row);
// **** CREATE COPY OF B **** //
// Wait for everyone to get their new B. If k = 0 everyone has it scattered.
if(k != 0)
MPI_Wait(&fox_recv_request, MPI_STATUS_IGNORE);
// Make a copy of B so we can overwrite the old one.
copyMatrix(B_old, B_new, subelem_dim_size);
// **** SHIFT B **** //
// Find which node to send to, and which to recieve from (B matrix).
fox_recfrom = ((rank_col + 1) % node_dim_size);
fox_sendto = ((rank_col - 1) % node_dim_size);
if(fox_sendto < 0)
fox_sendto = node_dim_size - 1;
fox_sendtag = 1000 + fox_sendto;
fox_rectag = 1000 + rank_col;
// Send the B matrix.
MPI_Isend(B_old,
subelem_dim_size * subelem_dim_size,
MPI_DOUBLE,
fox_sendto,
fox_sendtag,
mpi_comm_col,
&fox_send_request);
// Receive the B matrix.
MPI_Irecv(B_new,
subelem_dim_size * subelem_dim_size,
MPI_DOUBLE,
fox_recfrom,
fox_rectag,
mpi_comm_col,
&fox_recv_request);
// Perform matrix multiplication on the local submatrix.
naiveMatrixMult(A_bcast, B_old, C_mine, subelem_dim_size);
}
// ** GATHER DATA **
MPI_Barrier(MPI_COMM_WORLD);
// Collect C from submatrices.
MPI_Gatherv(C_mine,
subelem_dim_size * subelem_dim_size,
MPI_DOUBLE,
mat_c,
scatter_sendcount,
scatter_displacement,
mpi_type_submatrix,
0,
mpi_comm_grid);
// ** PRESENT DATA **
if(verbose && !rank_grid) {
printf("** Will print matrix C from 0:\n");
printMatrix(mat_c, elem_dim_size);
}
// ** VERIFICATION OF CORRECTNESS **
if(verify && !rank_grid) {
// Allocate memory for verification matrix.
mat_verify = (double *)
malloc(elem_dim_size * elem_dim_size * sizeof(double));
// Initialize verification matrix to zeroes.
zeroMatrix(mat_verify, elem_dim_size);
// Do the naive multiplication.
naiveMatrixMult(mat_a, mat_b, mat_verify, elem_dim_size);
// Print the correct matrix.
if(verbose) {
printf("** Print correct matrix from 0:\n");
printMatrix(mat_verify, elem_dim_size);
}
// Check equality between matrices.
if(matrixEqual(mat_c, mat_verify, elem_dim_size))
printf("\n Ok!\n\n");
else
printf("\n FAIL!\n\n");
// Free the memory used by the verification matrix.
free(mat_verify);
}
// ** FINALIZE MPI **
MPI_Barrier(MPI_COMM_WORLD);
if(rank_grid==0)
{
endtime = MPI_Wtime();
printf("%f\n", endtime - starttime);
}
MPI_Finalize();
// ** CLEANUP **
if(A_mine)
free(A_mine);
if(A_bcast)
free(A_bcast);
if(B_old)
free(B_old);
if(B_new)
free(B_new);
if(C_mine)
free(C_mine);
// Local rank 0 cleanup.
if(rank_grid == 0) {
free(mat_a);
free(mat_b);
free(mat_c);
}
return 0;
}
void
gaussianEstimator_Est (Matrix *xEst, Matrix *CEst, Matrix *y, Matrix *Cv, Matrix (*hfun)(Matrix m), Matrix *m_opt)
{
//printMatrix(*xEst);
//printMatrix(*CEst);system("PAUSE");
Matrix tmp = sizeOfMatrix(*m_opt);
float D = elem(tmp,0,1)+1; //printf("%f\n", D);
freeMatrix(tmp);
float w_opt = 1/D; //printf("%f\n", w_opt);
tmp = sizeOfMatrix(*xEst);
float Nx = elem(tmp,0,0); // printf("%f\n", Nx);
freeMatrix(tmp);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; // printf("%f\n", w);system("PAUSE");
// Eigenvectors, Eigenvalues
tmp = sizeOfMatrix(*CEst);
int dimC = elem ( tmp, 0, 0 );
freeMatrix(tmp);
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val ); //printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val); //printMatrix(m1); system("PAUSE");
freeMatrix(Vec);
freeMatrix(Val);
//* rotate & scale samples: m = m1*S
Matrix m = scaledSamplePoints(m1, *m_opt); // printMatrix(m); system("PAUSE");
Matrix mxDiracs = mulScalarMatrix(1, m);
//* x = x*ones(1,d)
Matrix x = fillMatrix(*xEst, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ) ; //printMatrix(m);
freeMatrix(tmp);
//% Predicted Measurements
//* hfun
// yPredDiracs = feval(hfun, m, [], [], t);
// yPred = w*sum(yPredDiracs, 2);
Matrix yPredDiracs = (*hfun) (m); //printMatrix(yPredDiracs );
Matrix yPredDiracsSum = sumMatrix(yPredDiracs, 2);
Matrix yPred = mulScalarMatrix(w, yPredDiracsSum);
// myDiracs = yPredDiracs-repmat(yPred, 1, d);
tmp = fillMatrix(yPred, d);
Matrix myDiracs = subMatrix(yPredDiracs, tmp);
freeMatrix(tmp);
//* CPred = w_opt*mxDiracs*mxDiracs';
// Matrix CPred = mulScalarMatrix( w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs)) );
// Matrix CPred = *CEst;
// Cxy = w_opt*mxDiracs*myDiracs';
Matrix tmp1 = transposeMatrix(myDiracs);
Matrix tmp2 = mulMatrix(mxDiracs, tmp1);
Matrix Cxy = mulScalarMatrix( w_opt, tmp2);
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = w_opt*myDiracs*myDiracs'+Cv;
tmp1 = transposeMatrix(myDiracs);
tmp2 = mulMatrix(myDiracs, tmp1);
Matrix tmp3 = mulScalarMatrix( w_opt, tmp2);
Matrix Cy = addMatrix( tmp3 , *Cv );
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
// K = Cxy / Cy;
tmp = invertCovMatrix(Cy);
Matrix K = mulMatrix( Cxy, tmp);
freeMatrix(tmp);
// I = y - yPred;
Matrix I = subMatrix( *y, yPred );
// xEst = xPred + K*I;
tmp = mulMatrix( K, I );
Matrix tmp23 = addMatrix( *xEst, tmp);
appMatrix(*xEst,0,5,0,0, tmp23,0,5,0,0);
freeMatrix(tmp);
// CEst = CPred - K*Cy*K';
tmp1 = mulMatrix(K, Cy);
tmp2 = transposeMatrix(K);
tmp3 = mulMatrix( tmp1, tmp2);
Matrix tmp24 = subMatrix(*CEst, tmp3);
appMatrix(*CEst,0,5,0,5, tmp24,0,5,0,5);
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp24);
freeMatrix(m1);
freeMatrix(m);
freeMatrix(mxDiracs);
freeMatrix(x);
freeMatrix(yPredDiracs);
freeMatrix(yPredDiracsSum);//
freeMatrix(yPred);//
freeMatrix(myDiracs);
freeMatrix(Cxy);
freeMatrix(Cy);
freeMatrix(K);//
freeMatrix(I);//
freeMatrix(tmp23);
}
void
gaussianEstimator_Pred_decomp ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, float *dt, Matrix *m_opt)
{
float r;
Matrix sizeMopt;
Matrix xn = zeroMatrix(3,1);
Matrix Cn = zeroMatrix(3,3);
Matrix xl = zeroMatrix(9,1);
Matrix Cl = zeroMatrix(9,9);
Matrix Cnl = zeroMatrix(3,9);
Matrix Cnl_T;
Matrix Cn_i;
Matrix CLN;
Matrix sizeCn;
Matrix Vec;
Matrix Val;
Matrix m1;
Matrix m;
Matrix x;
Matrix A;
Matrix Hi = zeroMatrix(12,9);
Matrix Cy = zeroMatrix(12, 12);
Matrix muy = zeroMatrix(12, 1);
Matrix zeros33 = zeroMatrix(3,3);
Matrix eye33 = unitMatrix(3,3);
Matrix Mat;
Matrix H;
Matrix gi = zeroMatrix(12,1);
Matrix Rot_vec = zeroMatrix(3,1);
Matrix mui;
Matrix muiy;
Matrix Ciy;
Matrix tmp;
Matrix tmp1;
Matrix tmp2;
Matrix tmp3;
Matrix tmp4;
Matrix tmp5;
Matrix tmp6;
Matrix tmp7;
Matrix tmp8;
Matrix tmpHi;
sizeMopt = sizeOfMatrix(*m_opt); //printf("%f\n",*dt);
float D = elem(sizeMopt,0,1)+1; //printf("%f\n", D);
freeMatrix(sizeMopt);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = 3;
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
//xn = xEst(4:6); % Rotation vector
appMatrix(xn, 0, 2, 0, 0, *xEst, 3, 5, 0, 0); //printMatrix(xn);system("PAUSE");
//Cn = CEst(4:6,4:6);
appMatrix(Cn, 0, 2, 0, 2, *CEst, 3, 5, 3, 5); //printMatrix(Cn);system("PAUSE");
//xl = [xEst(1:3) ; xEst(7:12)]; % Translation, angular velocity, linear velocity
appMatrix(xl, 0, 2, 0, 0, *xEst, 0, 2, 0, 0);
appMatrix(xl, 3, 8, 0, 0, *xEst, 6, 11, 0, 0); //printMatrix(xl);system("PAUSE");
//Cl = [CEst(1:3,1:3) CEst(1:3,7:12);
// CEst(7:12,1:3) CEst(7:12,7:12)] ;
appMatrix(Cl, 0, 2, 0, 2, *CEst, 0, 2, 0, 2);
appMatrix(Cl, 0, 2, 3, 8, *CEst, 0, 2, 6, 11);
appMatrix(Cl, 3, 8, 0, 2, *CEst, 6, 11, 0, 2);
appMatrix(Cl, 3, 8, 3, 8, *CEst, 6, 11, 6, 11); //printMatrix(Cl);system("PAUSE");
//Cnl = [CEst(4:6,1:3) CEst(4:6,7:12)];
appMatrix(Cnl, 0, 2, 0, 2, *CEst, 3, 5, 0, 2);
appMatrix(Cnl, 0, 2, 3, 8, *CEst, 3, 5, 6, 11); //printMatrix(Cnl);system("PAUSE");
//CLN = Cl - Cnl'*inv(Cn)*Cnl;
Cnl_T = transposeMatrix(Cnl);
// printMatrix(Cn);system("PAUSE");
Cn_i = invertCovMatrix(Cn); //printMatrix(Cn_i);system("PAUSE");
tmp = mulMatrix( Cnl_T, Cn_i);
tmp7 = mulMatrix(tmp, Cnl);
CLN = subMatrix ( Cl, tmp7); //printMatrix(CLN);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp7);
// Eigenvectors, Eigenvalues
sizeCn = sizeOfMatrix(Cn);
int dimC = elem ( sizeCn, 0, 0 );
freeMatrix(sizeCn);
Vec = zeroMatrix(dimC, dimC);
Val = zeroMatrix(dimC, dimC);
eig ( &Cn, &Vec, &Val ); //printMatrix(Cn);printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
m1 = mulMatrix(Vec, Val); //printMatrix(m1);system("PAUSE");
// rotate & scale samples: m = m1*S
m = scaledSamplePoints(m1, *m_opt); //printMatrix(m);system("PAUSE");
// x = x*ones(1,d)
x = fillMatrix(xn, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ); //printMatrix(m);system("PAUSE");
freeMatrix(tmp);
//A = [[eye(3,3),t*eye(3,3)];[zeros(3,3),eye(3,3)]];
A = unitMatrix(6,6);
setElem(A, 0, 3, *dt);
setElem(A, 1, 4, *dt);
setElem(A, 2, 5, *dt); //printMatrix(A);system("PAUSE");
for (i=0; i<d; i++)
{
//gi = [zeros(3,1); m(:,i); zeros(6,1)];
setElem(gi, 3, 0, elem(m, 0, i));
setElem(gi, 4, 0, elem(m, 1, i));
setElem(gi, 5, 0, elem(m, 2, i)); //printMatrix(gi);system("PAUSE");
//Rot_vec = m(:,i);
setElem(Rot_vec, 0, 0, elem(m, 0, i));
setElem(Rot_vec, 1, 0, elem(m, 1, i));
setElem(Rot_vec, 2, 0, elem(m, 2, i)); //printMatrix(Rot_vec);system("PAUSE");
//r = norm(Rot_vec);
r = sqrtf( powf((elem(Rot_vec,0,0)),2) + powf((elem(Rot_vec,1,0)),2) + powf((elem(Rot_vec,2,0)),2) ); //printf("%f\n",r);
H = zeroMatrix(3,3);
if (fmod(r, 2*pi) == 0)
{
Mat = unitMatrix(3,3);
}
else
{
// build skew symmetric Matrix
setElem(H, 0, 1, -elem(Rot_vec,2,0));
setElem(H, 0, 2, elem(Rot_vec,1,0));
setElem(H, 1, 0, elem(Rot_vec,2,0));
setElem(H, 1, 2, -elem(Rot_vec,0,0));
setElem(H, 2, 0, -elem(Rot_vec,1,0));
setElem(H, 2, 1, elem(Rot_vec,0,0)); //printMatrix(H);system("PAUSE");
// Bortz equation
// Mat = eye(3,3) + 0.5*H + (1- r*sin(r)/( 2*(1-cos(r))))/r^2*H*H;
// already declared Mat = unitMatrix(3,3);
tmp1 = mulScalarMatrix(0.5, H);
tmp4 = addMatrix( eye33 , tmp1 );
tmp2 = mulMatrix(H, H);
tmp3 = mulScalarMatrix( (1-(r*sin(r)/(2*(1-cos(r)))))/powf(r,2), tmp2);
Mat = addMatrix( tmp4, tmp3);
//printMatrix(Mat);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
}
//Hi = [[A(1:3,1:3) zeros(3,3) A(1:3,4:6)];
// [zeros(3,3), t*Mat, zeros(3,3)];
// [zeros(3,3), eye(3,3), zeros(3,3)];
// [A(4:6,1:3),zeros(3,3), A(4:6,4:6)]];
appMatrix( Hi, 0, 2, 0, 2, A, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 6, 8, A, 0, 2, 3, 5 );
appMatrix( Hi, 3, 5, 0, 2, zeros33, 0, 2, 0, 2 );
tmpHi = mulScalarMatrix(*dt, Mat);
appMatrix( Hi, 3, 5, 3, 5, tmpHi, 0, 2, 0, 2 );
freeMatrix(tmpHi);
appMatrix( Hi, 3, 5, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 0, 2, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 3, 5, eye33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 0, 2, A, 3, 5, 0, 2 );
appMatrix( Hi, 9, 11, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 6, 8, A, 3, 5, 3, 5 ); //printMatrix(Hi);system("PAUSE");
// mui = xl + Cnl'*inv(Cn)*(m(:,i)-xn); //m(:,i) -> Rot_vec
tmp = mulMatrix(Cnl_T, Cn_i );
tmp1 = subMatrix(Rot_vec, xn);
tmp2 = mulMatrix(tmp, tmp1);
mui = addMatrix(xl, tmp2);
freeMatrix(tmp);
freeMatrix(tmp1);
freeMatrix(tmp2); //printMatrix(mui);system("PAUSE");
// muiy = gi + Hi * mui;
tmp = mulMatrix(Hi, mui);
muiy = addMatrix( gi, tmp); //printMatrix(muiy);system("PAUSE");
freeMatrix(tmp);
// Ciy = Hi *CLN *Hi';
tmp1 = mulMatrix(Hi, CLN);
tmp2 = transposeMatrix(Hi);
Ciy = mulMatrix( tmp1, tmp2); //printMatrix(Ciy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = Cy + (w*Ciy + w_opt*muiy*muiy');
tmp3 = mulScalarMatrix(w, Ciy);
tmp1 = transposeMatrix(muiy);
tmp2 = mulMatrix(muiy, tmp1);
tmp4 = mulScalarMatrix( w_opt, tmp2 );
tmp5 = addMatrix( tmp3, tmp4 );
tmp6 = addMatrix( Cy, tmp5);
appMatrix(Cy,0,Cy->height-1,0,Cy->width-1,tmp6, 0,tmp6->height-1,0,tmp6->width-1); //printMatrix(Cy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
freeMatrix(tmp5);
freeMatrix(tmp6);
// muy = muy + w*muiy;
tmp = mulScalarMatrix( w, muiy );
tmp2 = addMatrix( muy, tmp );
appMatrix(muy,0,muy->height-1,0,muy->width-1, tmp2, 0, tmp2->height-1, 0, tmp2->width-1); //printMatrix(muy);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp2);
freeMatrix(H);
freeMatrix(Mat);
freeMatrix(mui);//
freeMatrix(muiy);//
freeMatrix(Ciy);
}
appMatrix(*xEst, 0, 11, 0, 0, muy, 0, 11, 0, 0 ); //printMatrix(muy);system("PAUSE");
//CEst = Cy - muy*muy' * w_opt/w + Cw;
tmp1 = transposeMatrix(muy);
tmp2 = mulMatrix(muy, tmp1);
tmp5 = mulScalarMatrix( w_opt/w, tmp2 );
tmp6 = subMatrix(Cy, tmp5);
tmp8 = addMatrix( tmp6, *Cw); //printMatrix(*CEst);system("PAUSE");
appMatrix(*CEst,0,11,0,11, tmp8, 0,11,0,11 ); //printMatrix(tmp8);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp5);
freeMatrix(tmp6);
freeMatrix(tmp8);
freeMatrix(muy);//
freeMatrix(zeros33);//
freeMatrix(Vec);
freeMatrix(Val);
freeMatrix(Cy);
freeMatrix(xn);
freeMatrix(Cn);
freeMatrix(xl);
freeMatrix(Cl);//
freeMatrix(Cnl);
freeMatrix(Cnl_T);
freeMatrix(Cn_i);
freeMatrix(CLN);//
freeMatrix(m1);
freeMatrix(m);//
freeMatrix(x);
freeMatrix(A);
freeMatrix(eye33);
freeMatrix(Hi);
freeMatrix(gi);
freeMatrix(Rot_vec);
} /* End gaussianPred_decomp */
void Cage::applyWave()
{
bool isCenter = false;
float cx = 0;
float cy = 0;
Areas &areas = getAreas(0);
if (areas.size()>0) {
cx = areas.at(0).pt[0];
cy = areas.at(0).pt[1];
isCenter = true;
}
for (unsigned int i=0; i<areas.size(); i++) {
Area &area = areas.at(i);
cx = (cx + area.pt[0])/2.0;
cy = (cy + area.pt[1])/2.0;
}
if (isCenter) {
if (isCenterPrev) {
if (isWaveDebug) {
color(1,0,0);
lineWidth(10);
line(cxPrev, cyPrev, cx, cy);
}
if (isWave) {
Wave wave;
wave.x = cx;
wave.y = cy;
wave.angle = angle(cxPrev, cyPrev, cx, cy);
wave.lenght = distance(cxPrev, cyPrev, cx, cy)*waveForce;
waves.append(wave);
}
}
isCenterPrev = true;
}
else {
isCenterPrev = false;
}
cxPrev = cx;
cyPrev = cy;
QList<Wave>::iterator i= waves.begin();
while (i != waves.end()) {
Wave &wave = *i;
if (wave.lenght > 0) {
wave.lenght -= waveSpeed;
wave.x += waveSpeed*cos(wave.angle);
wave.y += waveSpeed*sin(wave.angle);
if (isWave) {
int x1 = (wave.x - waveWidth/2.0)/cellSize;
int y1 = (wave.y - waveWidth/2.0)/cellSize;
int x2 = (wave.x + waveWidth/2.0)/cellSize;
int y2 = (wave.y + waveWidth/2.0)/cellSize;
zeroMatrix(x1, y1, x2, y2);
}
i++;
}
else {
i = waves.erase(i);
}
}
}