void KinematicMotion::addRotation(const double *vec0, const double *vec1, bool normalized) { // not unique, but shortest path
// rotation is defined in the CURRENT coordinates system
double e0[3];
copyVector(vec0, e0);
double e1[3];
copyVector(vec1, e1);
if (!normalized) {
normalizeVector(e0);
normalizeVector(e1);
}
// algorithm form Non-linear Modeling and Analysis of Solids and Structures (Steen Krenk) P54
double e2[3];
e2[0] = e0[0] + e1[0];
e2[1] = e0[1] + e1[1];
e2[2] = e0[2] + e1[2];
double e2_square = vectorProduct(e2, e2);
double newRotation[9];
newRotation[0] = 1.0 + 2.0 * e1[0] * e0[0] - 2.0 / e2_square * e2[0] * e2[0];
newRotation[1] = 0.0 + 2.0 * e1[0] * e0[1] - 2.0 / e2_square * e2[0] * e2[1];
newRotation[2] = 0.0 + 2.0 * e1[0] * e0[2] - 2.0 / e2_square * e2[0] * e2[2];
newRotation[3] = 0.0 + 2.0 * e1[1] * e0[0] - 2.0 / e2_square * e2[1] * e2[0];
newRotation[4] = 1.0 + 2.0 * e1[1] * e0[1] - 2.0 / e2_square * e2[1] * e2[1];
newRotation[5] = 0.0 + 2.0 * e1[1] * e0[2] - 2.0 / e2_square * e2[1] * e2[2];
newRotation[6] = 0.0 + 2.0 * e1[2] * e0[0] - 2.0 / e2_square * e2[2] * e2[0];
newRotation[7] = 0.0 + 2.0 * e1[2] * e0[1] - 2.0 / e2_square * e2[2] * e2[1];
newRotation[8] = 1.0 + 2.0 * e1[2] * e0[2] - 2.0 / e2_square * e2[2] * e2[2];
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
int GaussJacobiPoisson1D(Vector u, double tol, int maxit)
{
int it=0, i;
Vector b = cloneVector(u);
Vector e = cloneVector(u);
copyVector(b, u);
fillVector(u, 0.0);
double max = tol+1;
while (max > tol && ++it < maxit) {
copyVector(e, u);
collectVector(e);
copyVector(u, b);
#pragma omp parallel for schedule(static)
for (i=1;i<e->len-1;++i) {
u->data[i] += e->data[i-1];
u->data[i] += e->data[i+1];
u->data[i] /= (2.0+alpha);
}
axpy(e, u, -1.0);
e->data[0] = e->data[e->len-1] = 0.0;
max = maxNorm(e);
}
freeVector(b);
freeVector(e);
return it;
}
int cg(Matrix A, Vector b, double tolerance)
{
int i=0, j;
double rl;
Vector r = createVector(b->len);
Vector p = createVector(b->len);
Vector buffer = createVector(b->len);
double dotp = 1000;
double rdr = dotp;
copyVector(r,b);
fillVector(b, 0.0);
rl = sqrt(dotproduct(r,r));
while (i < b->len && rdr > tolerance*rl) {
++i;
if (i == 1) {
copyVector(p,r);
dotp = dotproduct(r,r);
} else {
double dotp2 = dotproduct(r,r);
double beta = dotp2/dotp;
dotp = dotp2;
scaleVector(p,beta);
axpy(p,r,1.0);
}
MxV(buffer, p);
double alpha = dotp/dotproduct(p,buffer);
axpy(b,p,alpha);
axpy(r,buffer,-alpha);
rdr = sqrt(dotproduct(r,r));
}
freeVector(r);
freeVector(p);
freeVector(buffer);
return i;
}
SEXP kz1d(SEXP x, SEXP window, SEXP iterations)
{
int p;
int i, k;
int m;
SEXP ans, tmp;
// int n;
m = (2 * INTEGER_VALUE(window)) + 1;
p = (m-1)/2;
// dim = GET_DIM(x);
// n = LENGTH(x);
PROTECT(tmp = allocVector(REALSXP, LENGTH(x)));
PROTECT(ans = allocVector(REALSXP, LENGTH(x)));
copyVector(tmp, x);
for(k=0; k<INTEGER_VALUE(iterations); k++) {
for(i=0;i<LENGTH(x);i++) {
REAL(ans)[i] = mavg1d(tmp, i, p);
}
/* copyMatrix (destination, source, byrow) */
copyVector(tmp, ans);
}
UNPROTECT(2);
return ans;
}
int dataExchange(unit_t tmin, unit_t tmax, pvector_t *v, pvector_t tmp) {
index_t i, x, pos = 0;
unit_t t;
pvector_t data;
if (initVector(&data, ELEMENTS_PER_PROCESS << 1) == NULL)
return ERRORCODE_CANT_MALLOC;
for(i = 0; i < PROCESS_NUMBER; i++) {
if (ID == i) copyVector(*v, tmp, (*v)->length);
if (MPI_Bcast(tmp->vector, listLength(i), MPI_UNIT, i, MPI_COMM_WORLD) != MPI_SUCCESS)
return ERRORCODE_MPI_ERROR;
for(x = 0; x < listLength(i); x++) {
t = getFromVector(tmp, x);
if (t <= tmax) {
if (t > tmin)
setInVector(data, pos++, t);
} else
x+= ELEMENTS_NUMBER;
}
}
freeVector(v);
if (initVector(v, pos) == NULL)
return ERRORCODE_CANT_MALLOC;
copyVector(data, *v, pos);
freeVector(&data);
return ERRORCODE_NOERRORS;
}
void cg(eval_t A, Matrix b, double tolerance, void* ctx)
{
Matrix r = createMatrix(b->rows, b->cols);
Matrix p = createMatrix(b->rows, b->cols);
Matrix buffer = createMatrix(b->rows, b->cols);
double dotp = 1000;
double rdr = dotp;
copyVector(r->as_vec,b->as_vec);
fillVector(b->as_vec, 0.0);
int i=0;
while (i < b->as_vec->len && rdr > tolerance) {
++i;
if (i == 1) {
copyVector(p->as_vec,r->as_vec);
dotp = innerproduct(r->as_vec,r->as_vec);
} else {
double dotp2 = innerproduct(r->as_vec,r->as_vec);
double beta = dotp2/dotp;
dotp = dotp2;
scaleVector(p->as_vec,beta);
axpy(p->as_vec,r->as_vec,1.0);
}
A(buffer,p,ctx);
double alpha = dotp/innerproduct(p->as_vec,buffer->as_vec);
axpy(b->as_vec,p->as_vec,alpha);
axpy(r->as_vec,buffer->as_vec,-alpha);
rdr = sqrt(innerproduct(r->as_vec,r->as_vec));
}
printf("%i iterations\n",i);
freeMatrix(r);
freeMatrix(p);
freeMatrix(buffer);
}
/* Efficiently transposes a sparse matrix. */
SMat svdTransposeS(SMat S) {
int r, c, i, j;
SMat N = svdNewSMat(S->h.cols, S->h.rows, S->h.vals);
/* Count number nz in each row. */
for (i = 0; i < S->h.vals; i++)
N->pointr[S->rowind[i]]++;
/* Fill each cell with the starting point of the previous row. */
N->pointr[S->h.rows] = S->h.vals - N->pointr[S->h.rows - 1];
for (r = S->h.rows - 1; r > 0; r--)
N->pointr[r] = N->pointr[r+1] - N->pointr[r-1];
N->pointr[0] = 0;
/* Assign the new columns and values. */
for (c = 0, i = 0; c < S->h.cols; c++) {
for (; i < S->pointr[c+1]; i++) {
r = S->rowind[i];
j = N->pointr[r+1]++;
N->rowind[j] = c;
N->value[j] = S->value[i];
}
}
/* Transpose the row and column offsets also. */
if (S->offset_for_col)
N->offset_for_row = copyVector(S->offset_for_col, S->h.cols, "svdTransposeS: offset_for_row");
if (S->offset_for_row)
N->offset_for_col = copyVector(S->offset_for_row, S->h.rows, "svdTransposeS: offset_for_col");
return N;
}
int GaussJacobiPoisson1D(Vector u, double tol, int maxit)
{
int it=0, i;
double rl;
double max = tol+1;
Vector b = createVector(u->len);
Vector e = createVector(u->len);
copyVector(b, u);
fillVector(u, 0.0);
rl = maxNorm(b);
while (max > tol*rl && ++it < maxit) {
copyVector(e, u);
copyVector(u, b);
#pragma omp parallel for schedule(static)
for (i=0;i<e->len;++i) {
if (i > 0)
u->data[i] += e->data[i-1];
if (i < e->len-1)
u->data[i] += e->data[i+1];
u->data[i] /= (2.0+alpha);
}
axpy(e, u, -1.0);
max = maxNorm(e);
}
freeVector(b);
freeVector(e);
return it;
}
int GaussSeidelPoisson1D(Vector u, double tol, int maxit)
{
int it=0, i, j;
double max = tol+1;
double rl = maxNorm(u);
Vector b = createVector(u->len);
Vector r = createVector(u->len);
Vector v = createVector(u->len);
copyVector(b, u);
fillVector(u, 0.0);
while (max > tol*rl && ++it < maxit) {
copyVector(v, u);
copyVector(u, b);
for (i=0;i<r->len;++i) {
if (i > 0)
u->data[i] += v->data[i-1];
if (i < r->len-1)
u->data[i] += v->data[i+1];
r->data[i] = u->data[i]-(2.0+alpha)*v->data[i];
u->data[i] /= (2.0+alpha);
v->data[i] = u->data[i];
}
max = maxNorm(r);
}
freeVector(b);
freeVector(r);
freeVector(v);
return it;
}
void KinematicMotion::addRotation(const double *xAxisNew, const double *yAxisNew,
const double *zAxisNew, bool normalized) {
double e0[3];
double e1[3];
double e2[3];
copyVector(xAxisNew, e0);
copyVector(yAxisNew, e1);
copyVector(zAxisNew, e2);
if (!normalized) {
normalizeVector(e0);
normalizeVector(e1);
normalizeVector(e2);
}
double newRotation[9];
for (int i = 0; i < 3; i++) {
newRotation[i * 3 + 0] = e0[i];
newRotation[i * 3 + 1] = e1[i];
newRotation[i * 3 + 2] = e2[i];
}
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
int GaussSeidelPoisson1Drb(Vector u, double tol, int maxit)
{
int it=0, i, j;
double max = tol+1;
double rl = maxNorm(u);
Vector b = createVector(u->len);
Vector r = createVector(u->len);
Vector v = createVector(u->len);
copyVector(b, u);
fillVector(u, 0.0);
while (max > tol && ++it < maxit) {
copyVector(v, u);
copyVector(u, b);
for (j=0;j<2;++j) {
#pragma omp parallel for schedule(static)
for (i=j;i<r->len;i+=2) {
if (i > 0)
u->data[i] += v->data[i-1];
if (i < r->len-1)
u->data[i] += v->data[i+1];
r->data[i] = u->data[i]-(2.0+alpha)*v->data[i];
u->data[i] /= (2.0+alpha);
v->data[i] = u->data[i];
}
}
max = maxNorm(r);
}
freeVector(b);
freeVector(r);
freeVector(v);
return it;
}
void updateBoids()
{
unsigned long j;
kd_clear(k3);
for( j=0;j<simParameters.numberOfBoids;j++)
{
copyVector(&(boidSet[j].nextPosition), &(boidSet[j].currentPosition));
copyVector(&(boidSet[j].nextVelocity), &(boidSet[j].currentVelocity));
// kdtree update
kd_insert3(k3,boidSet[j].currentPosition.x, boidSet[j].currentPosition.y, boidSet[j].currentPosition.z, &boidSet[j]);
}
}
// single boid initialization
void initBoid(const Vector *p, const Vector *v, const Vector *a, double speed, double acceleration, double mForce, double bMass, long int id, Boid *boid)
{
copyVector(p, &(boid->currentPosition));
copyVector(p, &(boid->nextPosition));
copyVector(v, &(boid->currentVelocity));
copyVector(v, &(boid->nextVelocity));
copyVector(a, &(boid->acceleration));
boid->id = id;
boid->maxSpeed = speed;
boid->maxAcceleration = acceleration;
// for future implementation
boid->maxForce = acceleration;
boid->mass = bMass;
}
void KinematicMotion::addRotation(const double *axis, bool normalized, double angle) {
// rotation is defined in the CURRENT coordinates system
double u[3];
copyVector(axis, u);
if (!normalized) {
normalizeVector(u);
}
// algorithm from http://en.wikipedia.org/wiki/Rotation_matrix
double c = cos(angle);
double s = sin(angle);
double newRotation[9];
newRotation[0] = c + u[0] * u[0] * (1.0 - c);
newRotation[1] = u[0] * u[1] * (1.0 - c) - u[2] * s;
newRotation[2] = u[0] * u[2] * (1.0 - c) + u[1] * s;
newRotation[3] = u[1] * u[0] * (1.0 - c) + u[2] * s;
newRotation[4] = c + u[1] * u[1] * (1.0 - c);
newRotation[5] = u[1] * u[2] * (1.0 - c) - u[0] * s;
newRotation[6] = u[2] * u[0] * (1.0 - c) - u[1] * s;
newRotation[7] = u[2] * u[1] * (1.0 - c) + u[0] * s;
newRotation[8] = c + u[2] * u[2] * (1.0 - c);
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
void collectAfterPre(Vector u, const Vector v)
{
int source, dest;
if (u->comm_rank == 0) {
int len=u->len-1;
dcopy(&len, v->data, &v->stride, u->data+1, &u->stride);
} else if (u->comm_rank == u->comm_size-1) {
int len=v->len-1;
dcopy(&len, v->data+1, &v->stride, u->data+1, &u->stride);
} else
copyVector(u, v);
// west
double recv;
MPI_Cart_shift(*u->comm, 0, -1, &source, &dest);
MPI_Sendrecv(v->data, 1, MPI_DOUBLE, dest, 0,
u->data, 1, MPI_DOUBLE, source, 0, *u->comm, MPI_STATUS_IGNORE);
if (source > -1)
u->data[u->len-2] += u->data[0];
// east
MPI_Cart_shift(*u->comm, 0, 1, &source, &dest);
MPI_Sendrecv(v->data+v->len-1, 1, MPI_DOUBLE, dest, 1,
u->data, 1, MPI_DOUBLE, source, 1, *u->comm, MPI_STATUS_IGNORE);
if (source > -1)
u->data[1] += u->data[0];
u->data[0] = u->data[u->len-1] = 0.0;
}
void DiagonalizationPoisson1Dfst(Vector u, const Vector lambda)
{
Vector btilde = createVector(u->len);
Vector buf = createVector(4*(u->len+1));
int i;
int N=u->len+1;
int NN=4*N;
copyVector(btilde, u);
fst(btilde->data, &N, buf->data, &NN);
for (i=0;i<btilde->len;++i)
btilde->data[i] /= (lambda->data[i]+alpha);
fstinv(btilde->data, &N, buf->data, &NN);
copyVector(u, btilde);
freeVector(btilde);
freeVector(buf);
}
void copyDirection(const Vector *inputVector, Vector *outputVector)
{
double m = magnitude(outputVector);
Vector copyV;
copyVector(inputVector, ©V);
normalize(©V);
multiply(©V, m, outputVector);
}
void setDirection(Vector *v)
{
double m = magnitude(v);
Vector *d = v;
normalize(d);
multiply(d, m, d);
copyVector(d, v);
}
/******************************
* Interface to copy a vector *
******************************/
ALGEB lbCopyVector (MKernelVector kv, ALGEB *argv){
try {
const VectorKey *key = &MapleToVectorKey(kv, argv[1]);
return VectorKeyToMaple(kv, copyVector(*key));
}
catch (lb_runtime_error &t)
{ lbRaiseError(kv, t);}
return ToMapleNULL(kv);
}
vector_t* getBeta(const mxArray* array, size_t L) {
if (mxGetN(array) != L) {
mexErrMsgTxt("Invalid dimension of beta vector.");
}
vector_t* beta = alloc_vector(L);
copyVector(beta, array);
return beta;
}
SimpleVector<DataType>::SimpleVector
(
const SimpleVector<DataType> & copiedVector // in: vector to copy
)
: vectorCapacity( copiedVector.vectorCapacity ),
vectorSize( copiedVector.vectorSize ),
vectorData( new DataType[ vectorCapacity ] )
{
copyVector( vectorData, copiedVector.vectorData, vectorCapacity );
}
void earliestDueDate(int* p, int* due_date, int n){
int i;
int *dueDateCopy = malloc(n*sizeof(int*));
copyVector(due_date, dueDateCopy,n);
for (i = 0; i < n; i++) {
p[i] = getArgmin(dueDateCopy,n);
dueDateCopy[p[i]] = -1;
}
free(dueDateCopy);
}
void GS(Matrix u, double tolerance, int maxit)
{
int it=0;
Matrix b = cloneMatrix(u);
Matrix e = cloneMatrix(u);
Matrix v = cloneMatrix(u);
int* sizes, *displ;
splitVector(u->rows-2, 2*max_threads(), &sizes, &displ);
copyVector(b->as_vec, u->as_vec);
fillVector(u->as_vec, 0.0);
double max = tolerance+1;
while (max > tolerance && ++it < maxit) {
copyVector(e->as_vec, u->as_vec);
copyVector(u->as_vec, b->as_vec);
for (int color=0;color<2;++color) {
for (int i=1;i<u->cols-1;++i) {
#pragma omp parallel
{
int cnt=displ[get_thread()*2+color]+1;
for (int j=0;j<sizes[get_thread()*2+color];++j, ++cnt) {
u->data[i][cnt] += v->data[i][cnt-1];
u->data[i][cnt] += v->data[i][cnt+1];
u->data[i][cnt] += v->data[i-1][cnt];
u->data[i][cnt] += v->data[i+1][cnt];
u->data[i][cnt] /= 4.0;
v->data[i][cnt] = u->data[i][cnt];
}
}
}
}
axpy(e->as_vec, u->as_vec, -1.0);
max = sqrt(innerproduct(e->as_vec, e->as_vec));
}
printf("number of iterations %i %f\n", it, max);
freeMatrix(b);
freeMatrix(e);
freeMatrix(v);
free(sizes);
free(displ);
}
//-------------------------Função Principal-------------------
int main(){
int menu, dado, i;
TpContato *vect = NULL, *control;
TpList *head = NULL, *tail, *manipulation;
/*
Ponteiro do tipo listaconjunto para armazenarem:
head - Primeiro conjunto criado. Indica conjunto inicio da fila;
tail - Ultimo conjunto criado. Indica conjunto no final da fila;
conjuntoatual - Manipulação;
*/
do{
printf("\nTrabalho_NP2\n\n1 - Criar Lista..\n2 - Criar Vetor..\n3 - Algoritmo a ser estudado..\n");
printf("4 - Método logarítmico..\n0 - Sair\n");
scanf("%d", &menu);
__fpurge(stdin);
getchar();
printf("\033[H\033[J"); //Limpar Tela no Linux
//system("cls");// Limpar tela no Windows
switch (menu) {
case 1:
if(head == NULL){
printf("How many contacts do you want to create?\n");
scanf("%d", &dado);
head = createList(dado);
printf("List created sucessfully!\n");
printList(head);
}
else{
printf("List already has been created!\n");
}
break;
case 2:
if(vect == NULL){
vect = createVector();
printf("Vector created sucessfully!\n\n");
printVector(vect);
}
else{
printf("Vector already has been created!\n");
}
break;
case 3:
control = copyVector(vect);
cocktailSort(control);
printVector(control);
break;
}
}while(menu != 0);
}
KinematicMotion *KinematicMotion::newInverse() {
// x2 = R*x1 + T ==> x1 = R^{-1} * (x2-T) = R^{-1}*x2 - R^{-1}*T
double rotationMatrixInverse[9];
copyMatrix(rotationMatrix, rotationMatrixInverse);
matrixTanspose(rotationMatrixInverse);
double translationVectorInverse[3];
copyVector(translationVector, translationVectorInverse);
vectorInverse(translationVectorInverse);
KinematicMotion *t_inverse = new KinematicMotion();
t_inverse->addTranslation(translationVectorInverse); // step 1
t_inverse->addRotation(rotationMatrixInverse); // step 2, order cannot be changed
return t_inverse;
}
int GaussSeidel(Matrix A, Vector u, int maxit)
{
int it=0, i, j;
Vector b = createVector(u->len);
Vector v = createVector(u->len);
copyVector(b, u);
fillVector(u, 0.0);
while (++it < maxit) {
copyVector(v, u);
copyVector(u, b);
for (i=0;i<A->rows;++i) {
for (j=0;j<A->cols;++j) {
if (j != i)
u->data[i] -= A->data[j][i]*v->data[j];
}
u->data[i] /= A->data[i][i];
v->data[i] = u->data[i];
}
}
freeVector(b);
freeVector(v);
return it;
}
static std::vector<double> * kmeans(std::vector<unsigned int>::const_iterator vita, uint64_t const rn, uint64_t const k, uint64_t const runs, bool const debug = false)
{
try
{
KMdata datapoints(1, rn);
for ( uint64_t i = 0; i < rn; ++i )
datapoints[i][0] = vita[i];
std::vector<double> const Q = computeKMeans(datapoints,k,runs,debug);
return copyVector(Q);
}
catch(std::exception const & ex)
{
std::cerr << ex.what() << std::endl;
return 0;
}
}
void SimpleVector<DataType>::grow
(
int growBy // in: grow by amount
)
{
DataType *newData;
int oldVectorCapacity = vectorCapacity;
vectorCapacity += growBy;
newData = new DataType[ vectorCapacity ];
copyVector( newData, vectorData, oldVectorCapacity );
delete [] vectorData;
vectorData = newData;
}
void rotateVector()
{
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
std::mt19937 generator(seed);
std::uniform_int_distribution<int> distribution(0, 9);
const int size = 3;
std::vector<int> vector(size);
for (int iteration = 0; iteration < size; ++iteration) {
vector[iteration] = distribution(generator);
}
print(vector.begin(), vector.end());
for (int k = -5; k < 5; ++k) {
std::vector<int> copyVector(vector);
rotate(copyVector.begin(), copyVector.end() - 1, k);
std::cout << "k = " << k << " : ";
print(copyVector.begin(), copyVector.end());
}
}
void rspfValueAssignImageSourceFilter::validateArrays()
{
if(theOutputValueArray.size() != theInputValueArray.size())
{
rspf_uint32 index = std::min((rspf_uint32)theOutputValueArray.size(),
(rspf_uint32)theInputValueArray.size());
vector<double> copyVector(theOutputValueArray.begin(),
theOutputValueArray.begin() + index);
theOutputValueArray = copyVector;
for(rspf_uint32 index2 = index; index < theInputValueArray.size(); ++index)
{
theOutputValueArray.push_back(theInputValueArray[index2]);
}
}
}
