void Phaser::PrintCheck(my_pair ** check) {
const char separator = ' ';
const int width = 5;
if (0) {
Debug::PrintLine(2*F_len/3);
for (bool cf : {false, true}) {
for (bool ff : {false, true}) {
for (bool mf : {false, true}) {
if (!cf && !ff && !mf) {
Debug::PrintLine(2*F_len);
printf("%i %i %i\n", mf, ff, cf);
Debug::PrintLine(2*F_len);
size_t m = m_index(mf, ff, cf);
for (size_t i = 0; i <= I_len; i++) {
for (size_t j = 0; j <= J_len; j++) {
std::cout << std::left << std::setw(width) << std::setfill(separator) <<
"(" << check[m][IJ(i, j)].first << "," << check[m][IJ(i, j)].second<< ")";
}
std::cout << std::endl;
}
}
}
}
}
printf("\n");
}
}
void SMPProtocol::sendWriteBack(PAddr addr, CallbackBase *cb)
{
//cb->destroy();
IJ(0);
#if 0
IJ(cb==NULL);
SMPMemRequest *sreq = SMPMemRequest::create(pCache, addr, MemPush, true, NULL, MeshMemWriteBack);
sreq->addDstNode(pCache->getL2NodeID(addr));
sreq->msgOwner = pCache;
pCache->sendBelow(sreq);
#endif
}
void Phaser::PrintFace(score_t ** face) {
const char separator = ' ';
const int width = 5;
if (verbose) {
Debug::PrintLine(2*F_len/3);
for (bool cf : {false, true}) {
for (bool ff : {false, true}) {
for (bool mf : {false, true}) {
if (!cf && !ff && !mf) {
Debug::PrintLine(2*F_len);
printf("%i %i %i\n", mf, ff, cf);
Debug::PrintLine(2*F_len);
size_t m = m_index(mf, ff, cf);
for (size_t i = 0; i <= I_len; i++) {
for (size_t j = 0; j <= J_len; j++) {
std::cout << std::left << std::setw(width) << std::setfill(separator) <<
face[m][IJ(i, j)];
}
std::cout << std::endl;
}
}
}
}
}
printf("\n");
}
}
// `incident` must be preallocated - use mesh_count_incident().
int32 mesh_get_incident(Mesh *mesh,
MeshConnectivity *incident, int32 dim,
Indices *entities, int32 dent)
{
int32 ret = RET_OK;
uint32 ii;
uint32 D = mesh->topology->max_dim;
MeshEntityIterator it0[1], it1[1];
MeshConnectivity *conn = mesh->topology->conn[IJ(D, dent, dim)];
if (!conn->num) {
errput("connectivity %d -> %d is not avaliable!\n", dent, dim);
ERR_CheckGo(ret);
}
ii = 0;
incident->offsets[0] = 0;
for (mei_init_sub(it0, mesh, entities, dent); mei_go(it0); mei_next(it0)) {
for (mei_init_conn(it1, it0->entity, dim); mei_go(it1); mei_next(it1)) {
incident->indices[ii++] = it1->entity->ii;
}
incident->offsets[it0->it + 1] = incident->offsets[it0->it] + it1->it_end;
}
end_label:
return(ret);
}
int32 mesh_transpose(Mesh *mesh, int32 d1, int32 d2)
{
int32 ret = RET_OK;
uint32 n_incident;
uint32 ii;
uint32 *nd2 = 0;
uint32 D = mesh->topology->max_dim;
MeshEntityIterator it2[1], it1[1];
MeshConnectivity *c12 = 0; // d1 -> d2 - to compute
debprintf("transpose %d -> %d\n", d1, d2);
if (d1 >= d2) {
errput("d1 must be smaller than d2 in mesh_transpose()!\n");
ERR_CheckGo(ret);
}
c12 = mesh->topology->conn[IJ(D, d1, d2)];
// Count entities of d2 -> d1.
conn_alloc(c12, mesh->topology->num[d1], 0);
ERR_CheckGo(ret);
nd2 = c12->offsets + 1;
for (mei_init(it2, mesh, d2); mei_go(it2); mei_next(it2)) {
for (mei_init_conn(it1, it2->entity, d1); mei_go(it1); mei_next(it1)) {
nd2[it1->entity->ii]++;
}
}
// c12->offsets now contains counts - make a cumsum to get offsets.
for (ii = 1; ii < c12->num + 1; ii++) {
c12->offsets[ii] += c12->offsets[ii-1];
}
n_incident = c12->offsets[c12->num];
debprintf("transpose n_incident (%d -> %d): %d\n", d1, d2, n_incident);
// Fill in the indices.
conn_alloc(c12, 0, n_incident);
ERR_CheckGo(ret);
for (ii = 0; ii < c12->n_incident; ii++) {
c12->indices[ii] = UINT32_None; // "not set" value.
}
for (mei_init(it2, mesh, d2); mei_go(it2); mei_next(it2)) {
for (mei_init_conn(it1, it2->entity, d1); mei_go(it1); mei_next(it1)) {
conn_set_to_free(c12, it1->entity->ii, it2->entity->ii);
ERR_CheckGo(ret);
}
}
end_label:
return(ret);
}
void Particle::Update(RenderWindow &Window, Real dt)
{
dt = 0.1f;
Real x = CavitySize.x * u[IJ(Cavity_i, Cavity_j)];
Real y = -CavitySize.y * v[IJ(Cavity_i, Cavity_j)];
Real size = sqrtf(x*x + y* y);
//x /= size;
//y /= size;
x *= dt;
y *= dt;
Position.x = TopLeft.x + (Cavity_i * CavitySize.x) / GRID_SIZE;
Position.y = TopLeft.y + (Cavity_j * CavitySize.y) / GRID_SIZE;
Position.y = (BottommRight.y - Position.y) + TopLeft.y;
Vector2f EndPosition = Position + Vector2f(x, y);
sf::Vertex line[] =
{
sf::Vertex(Position, Color::Red),
sf::Vertex(EndPosition, Color::Red)
};
Window.draw(line, 2, sf::Lines);
size = 0.2f;
line[0] = sf::Vertex(EndPosition, Color::Red);
line[1] = sf::Vertex(EndPosition + Vector2f(size*(-x + y), size*(-x - y)), Color::Red);
Window.draw(line, 2, sf::Lines);
line[0] = sf::Vertex(EndPosition, Color::Red);
line[1] = sf::Vertex(EndPosition + Vector2f(size*(-x - y), size*(x - y)), Color::Red);
Window.draw(line, 2, sf::Lines);
//SetPosition(
// ((EndPosition.x - TopLeft.x) * GRID_SIZE) / CavitySize.x,
// ((EndPosition.y - TopLeft.y) * GRID_SIZE) / CavitySize.y
// );
//Circle.setPosition(Position);
//Window.draw(Circle);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
const mxArray *params;
double *t;
size_t m, n, d;
int i;
/* Check for proper number of arguments. */
if (nrhs!=1) {
mexErrMsgTxt("One input required.");
} else if (nlhs>1) {
mexErrMsgTxt("Too many output arguments.");
}
params = prhs[0];
/* Check argument types */
if (!mxIsDouble(params) || mxIsComplex(params) || mxGetNumberOfDimensions(params) != 2) {
mexErrMsgTxt("Input must be a real double row or column vector.");
}
m = mxGetM(params);
n = mxGetN(params);
if (m != 1 && n != 1) {
mexErrMsgTxt("Input must be a row or column vector.");
}
d = m > n ? m : n;
if (d != 2 && d != 3) {
mexErrMsgTxt("I can only think in two or three dimensions.");
}
plhs[0] = mxCreateDoubleMatrix(d+1, d+1, mxREAL);
t = mxGetPr(plhs[0]);
for (i = 0; i < d+1; ++i) {
IJ(t, d+1, i, i) = 1;
}
for (i = 0; i < d; ++i) {
IJ(t, d+1, i, d) = mxGetPr(params)[i];
}
}
int32 mesh_free_connectivity(Mesh *mesh, int32 d1, int32 d2)
{
uint32 D = mesh->topology->max_dim;
MeshConnectivity *conn = 0;
debprintf("free connectivity %d -> %d\n", d1, d2);
conn = mesh->topology->conn[IJ(D, d1, d2)];
conn_free(conn);
return(RET_OK);
}
void foobar()
{
for ( int i = 0; i < 5; i++ )
{
int pz_zdx = 7;
int ii,jj,kk = 0;
if (3 == 1)
{
IJ(pz_zdx, 42, ii, jj);
}
}
}
void EyeTrackerCalibration::estimateParametersMethod1(
const std::vector<cv::Point2d> & eyeData,
const std::vector<cv::Point2d> & calPointData)
{
const size_t dataCounter = eyeData.size();
cv::Mat IJ(dataCounter, 3, CV_64FC1); // positions of eye in video frame
cv::Mat X(dataCounter, 1, CV_64FC1); // positions of point on screen
cv::Mat Y(dataCounter, 1, CV_64FC1);
for(int i = 0; i < IJ.rows; i++)
{
double * Mi = IJ.ptr<double>(i);
Mi[0] = eyeData[i].x;
Mi[1] = eyeData[i].y;
Mi[2] = 1.0;
}
cv::MatIterator_<double> it;
int i;
for(i = 0, it = X.begin<double>(); it != X.end<double>(); it++)
{
*it = calPointData[i].x;
i++;
}
for(i = 0, it = Y.begin<double>(); it != Y.end<double>(); it++)
{
*it = calPointData[i].y;
i++;
}
const cv::Mat PX = (IJ.t() * IJ).inv() * IJ.t() * X;
const cv::Mat PY = (IJ.t() * IJ).inv() * IJ.t() * Y;
//g_GX = IJ*PX; //If calibration results are necessary ...
//g_GY = IJ*PY;
for(int i = 0; i < 3; i++)
{
const double * MiX = PX.ptr<double>(i);
const double * MiY = PY.ptr<double>(i);
m_paramX[i] = *MiX;
m_paramY[i] = *MiY;
}
qDebug() << "Calibration coeffs:";
qDebug() << m_paramX[0] << " " << m_paramX[1] << " " << m_paramX[2];
qDebug() << m_paramY[0] << " " << m_paramY[2] << " " << m_paramY[2];
}
SEXP
R_predict(SEXP obj1, SEXP obj2, SEXP r_x, SEXP r_y, SEXP r_ans1, SEXP r_ans2, SEXP r_dims)
{
int n, i;
double *x_ans_ptr, *y_ans_ptr;
double *x, *y;
int ix, iy;
double dx, dy;
int *dims;
double *gridx, *gridy;
n = Rf_length(r_x) + 1;
/* x_ans_ptr = REAL(VECTOR_ELT(r_ans, 0));
y_ans_ptr = REAL(VECTOR_ELT(r_ans, 1)); */
dims = INTEGER(r_dims);
x_ans_ptr = REAL(r_ans1);
y_ans_ptr = REAL(r_ans2);
/* printf("%p\n", x_ans_ptr);
printf("%p\n", y_ans_ptr); */
gridx = REAL(obj1);
gridy = REAL(obj2);
x = REAL(r_x);
y = REAL(r_y);
for(i = 0; i < n-1; i++) {
ix = (int) x[i] - 1;
iy = (int) y[i] - 1;
dx = x[i] - ix - 1.;
dy = y[i] - iy - 1.;
if(ix < 0 || ix >= dims[1] || iy < 0 || iy >= dims[0]) {
continue;
}
x_ans_ptr[i] = (1.-dx) * (1.-dy)*IJ(gridx, ix, iy) + dx*(1.-dy)*IJ(gridx, ix+1, iy)
+ (1.-dx)*dy*IJ(gridx, ix, iy+1) + dx*dy*IJ(gridx, ix+1, iy+1);
y_ans_ptr[i] = (1. - dx)*(1. - dy)*IJ(gridy, ix, iy) + dx*(1. - dy)*IJ(gridy, ix+1, iy)
+ (1. - dx)*dy*IJ(gridy, ix, iy+1) + dx*dy*IJ(gridy, ix+1, iy+1);
}
return(R_NilValue);
}
BICAPI void render_one_row (
void *volume_data1,
Data_types volume1_type,
int y,
int start_x,
int end_x,
size_t **y_offsets1,
size_t **row_offsets1,
void **start_slices1,
int n_slices1,
Real weights1[],
void *volume_data2,
Data_types volume2_type,
size_t **y_offsets2,
size_t **row_offsets2,
void **start_slices2,
int n_slices2,
Real weights2[],
unsigned short **cmode_colour_map,
Colour **rgb_colour_map,
pixels_struct *pixels )
{
int x_size;
Colour *rgb_pixel_ptr;
unsigned short *cmap_pixel_ptr;
x_size = pixels->x_size;
if( pixels->pixel_type == RGB_PIXEL )
rgb_pixel_ptr = &pixels->data.pixels_rgb[IJ(y,start_x,x_size)];
else
cmap_pixel_ptr = &pixels->data.pixels_16bit_colour_index
[IJ(y,start_x,x_size)];
#include "call_rend_f_include.c"
}
inline int32 me_get_incident(MeshEntity *entity, Indices *out, int32 dim)
{
int32 ret = RET_OK;
Mesh *mesh = entity->mesh;
uint32 D = mesh->topology->max_dim;
MeshConnectivity *conn = mesh->topology->conn[IJ(D, entity->dim, dim)];
if (!conn->num) {
errput("required connectivity is not avaliable!\n");
ERR_CheckGo(ret);
}
out->indices = conn->indices + conn->offsets[entity->ii];
out->num = conn->offsets[entity->ii + 1] - conn->offsets[entity->ii];
end_label:
return(ret);
}
int32 mesh_setup_connectivity(Mesh *mesh, int32 d1, int32 d2)
{
int32 ret = RET_OK;
int32 d3 = 0;
MeshTopology *topology = mesh->topology;
uint32 D = topology->max_dim;
debprintf("request connectivity %d -> %d\n", d1, d2);
if (topology->num[d1] == 0) {
mesh_build(mesh, d1);
ERR_CheckGo(ret);
}
if (topology->num[d2] == 0) {
mesh_build(mesh, d2);
ERR_CheckGo(ret);
}
if (topology->conn[IJ(D, d1, d2)]->num) {
return(ret);
}
if (d1 < d2) {
mesh_setup_connectivity(mesh, d2, d1);
mesh_transpose(mesh, d1, d2);
} else {
if ((d1 == 0) && (d2 == 0)) {
d3 = D;
} else {
d3 = 0;
}
if ((d1 > 0) && (d2 == 0)) {
errput("connectivity %d -> %d should already exist!\n", d1, d2);
ERR_CheckGo(ret);
}
mesh_setup_connectivity(mesh, d1, d3);
mesh_setup_connectivity(mesh, d3, d2);
mesh_intersect(mesh, d1, d2, d3);
}
ERR_CheckGo(ret);
end_label:
return(ret);
}
// `local_ids` must be preallocated to same size as `entities`.
int32 mesh_get_local_ids(Mesh *mesh, Indices *local_ids,
Indices *entities, int32 dent,
MeshConnectivity *incident, int32 dim)
{
int32 ret = RET_OK;
uint32 ii, iind, ic, found;
uint32 D = mesh->topology->max_dim;
MeshEntity entity[1];
MeshEntityIterator it1[1];
MeshConnectivity *conn = mesh->topology->conn[IJ(D, dim, dent)];
if (!conn->num) {
errput("connectivity %d -> %d is not avaliable!\n", dim, dent);
ERR_CheckGo(ret);
}
entity->mesh = mesh;
entity->dim = dim;
ii = 0;
for (iind = 0; iind < incident->num; iind++) {
for (ic = incident->offsets[iind]; ic < incident->offsets[iind+1]; ic++) {
entity->ii = incident->indices[ic];
// printf("%d: ? %d in %d\n", iind, entities->indices[iind], entity->ii);
found = 0;
for (mei_init_conn(it1, entity, dent); mei_go(it1); mei_next(it1)) {
if (entities->indices[iind] == it1->entity->ii) {
local_ids->indices[ii++] = it1->it;
// printf("%d -> %d\n", ii, it1->it);
found = 1;
break; // Degenerate cases - 1. occurrence is returned.
}
}
if (!found) {
errput("entity (%d, %d) not found in entity (%d, %d)!\n",
entities->indices[iind], dent, entity->ii, dim);
ERR_CheckGo(ret);
}
}
}
end_label:
return(ret);
}
// Allocates mask->mask.
int32 mesh_select_complete(Mesh *mesh, Mask *mask, int32 dim,
Indices *entities, int32 dent)
{
int32 ret = RET_OK;
uint32 D = mesh->topology->max_dim;
uint32 ii, inum;
char *ent_mask = 0;
MeshEntityIterator it0[1], it1[1];
MeshConnectivity *conn = mesh->topology->conn[IJ(D, dim, dent)];
if (!conn->num) {
errput("connectivity %d -> %d is not avaliable!\n", dim, dent);
ERR_CheckGo(ret);
}
mask->mask = alloc_mem(char, conn->num);
mask->num = conn->num;
mask->n_true = 0;
ent_mask = alloc_mem(char, mesh->topology->num[dent]);
for (ii = 0; ii < entities->num; ii++) {
ent_mask[entities->indices[ii]] = 1;
}
for (mei_init(it0, mesh, dim); mei_go(it0); mei_next(it0)) {
inum = 0;
for (mei_init_conn(it1, it0->entity, dent); mei_go(it1); mei_next(it1)) {
if (ent_mask[it1->entity->ii]) inum++;
}
// Check if all entities with dimension dent incident to entity it0 are set
// in ent_mask.
if (inum == it1->it_end) {
mask->mask[it0->it] = 1;
mask->n_true++;
}
}
end_label:
free_mem(ent_mask);
return(ret);
}
uint32 mesh_count_incident(Mesh *mesh, int32 dim,
Indices *entities, int32 dent)
{
uint32 ii, num = 0;
uint32 *ptr;
uint32 D = mesh->topology->max_dim;
MeshConnectivity *conn = mesh->topology->conn[IJ(D, dent, dim)];
if (!conn->num) {
errput("connectivity %d -> %d is not avaliable!\n", dent, dim);
ERR_CheckGo(num);
}
for (ii = 0; ii < entities->num; ii++) {
ptr = conn->offsets + entities->indices[ii];
num += ptr[1] - ptr[0];
}
end_label:
return(num);
}
inline int32 mei_init_conn(MeshEntityIterator *iter, MeshEntity *entity,
uint32 dim)
{
Mesh *mesh = entity->mesh;
uint32 D = mesh->topology->max_dim;
MeshConnectivity *conn = mesh->topology->conn[IJ(D, entity->dim, dim)];
iter->entity->mesh = mesh;
iter->entity->dim = dim;
iter->it = 0;
if ((conn->num > 0) && (conn->indices > 0)) {
iter->ptr = conn->indices + conn->offsets[entity->ii];
iter->it_end = conn->offsets[entity->ii+1] - conn->offsets[entity->ii];
iter->entity->ii = iter->ptr[iter->it];
} else {
iter->ptr = 0;
iter->it_end = 0;
iter->entity->ii = 0;
}
return(RET_OK);
}
int32 mesh_print(Mesh *mesh, FILE *file, int32 header_only)
{
uint32 ii, id;
uint32 *num;
uint32 D = mesh->topology->max_dim;
MeshGeometry *geometry = mesh->geometry;
MeshTopology *topology = mesh->topology;
MeshConnectivity *conn = 0;
fprintf(file, "Mesh %p (vertices: %d dimension: %d)\n",
mesh, geometry->num, geometry->dim);
fprintf(file, "topology: max_dim: %d\n", topology->max_dim);
num = topology->num;
fprintf(file, "n_cell: %d, n_face: %d, n_edge: %d, n_vertex: %d\n",
num[3], num[2], num[1], num[0]);
if (header_only == 0) { // Full print.
fprintf(file, "vertex coordinates:\n");
for (ii = 0; ii < geometry->num; ii++) {
for (id = 0; id < geometry->dim; id++) {
fprintf(file, " %.8e", geometry->coors[geometry->dim * ii + id]);
}
fprintf(file, "\n");
}
fprintf(file, "topology connectivities:\n");
for (ii = 0; ii <= D; ii++) {
for (id = 0; id <= D; id++) {
fprintf(file, "incidence %d -> %d:\n", ii, id);
conn = topology->conn[IJ(D, ii, id)];
conn_print(conn, file);
}
}
}
return(RET_OK);
}
int32 refc_find_ref_coors(FMField *ref_coors,
int32 *cells, int32 n_cells,
int32 *status, int32 n_status,
FMField *coors,
Mesh *mesh,
int32 *candidates, int32 n_candidates,
int32 *offsets, int32 n_offsets,
int32 allow_extrapolation,
float64 qp_eps,
float64 close_limit,
void *_ctx)
{
BasisContext *ctx = (BasisContext *) _ctx;
int32 ip, ic, ii, imin, ok, xi_ok, ret = RET_OK;
int32 D = mesh->topology->max_dim;
int32 nc = mesh->geometry->dim;
float64 d_min, dist;
float64 *mesh_coors = mesh->geometry->coors;
float64 buf3[3];
FMField point[1], e_coors[1], xi[1];
Indices cell_vertices[1];
MeshEntity cell_ent[1];
MeshConnectivity *cD0 = 0; // D -> 0
mesh_setup_connectivity(mesh, D, 0);
cD0 = mesh->topology->conn[IJ(D, D, 0)];
fmf_pretend_nc(point, coors->nRow, 1, 1, nc, coors->val);
fmf_pretend_nc(xi, 1, 1, 1, nc, buf3);
fmf_fillC(xi, 0.0);
ctx->is_dx = 0;
for (ip = 0; ip < coors->nRow; ip++) {
FMF_SetCell(point, ip);
if (offsets[ip] == offsets[ip+1]) {
status[ip] = 5;
cells[ip] = 0;
for (ii = 0; ii < nc; ii++) {
ref_coors->val[nc*ip+ii] = 0.0;
}
continue;
}
ok = xi_ok = 0;
d_min = 1e10;
imin = candidates[offsets[ip]];
for (ic = offsets[ip]; ic < offsets[ip+1]; ic++) {
/* output("***** %d %d %d\n", ip, ic, candidates[ic]); */
ctx->iel = candidates[ic];
cell_ent->ii = candidates[ic];
me_get_incident2(cell_ent, cell_vertices, cD0);
_get_cell_coors(e_coors, cell_vertices, mesh_coors, nc,
ctx->e_coors_max->val);
xi_ok = ctx->get_xi_dist(&dist, xi, point, e_coors, ctx);
if (xi_ok) {
if (dist < qp_eps) {
imin = cell_ent->ii;
ok = 1;
break;
} else if (dist < d_min) {
d_min = dist;
imin = cell_ent->ii;
}
} else if (dist < d_min) {
d_min = dist;
imin = cell_ent->ii;
}
}
/* output("-> %d %d %d %.3e\n", imin, xi_ok, ok, d_min); */
cells[ip] = imin;
if (ok != 1) {
if (!xi_ok) {
status[ip] = 4;
} else if (allow_extrapolation) {
if (sqrt(d_min) < close_limit) {
status[ip] = 1;
} else {
status[ip] = 2;
}
} else {
status[ip] = 3;
}
} else {
status[ip] = 0;
}
for (ii = 0; ii < nc; ii++) {
ref_coors->val[nc*ip+ii] = xi->val[ii];
}
ERR_CheckGo(ret);
}
end_label:
return(ret);
}
int32 mesh_intersect(Mesh *mesh, int32 d1, int32 d2, int32 d3)
{
int32 ret = RET_OK;
uint32 D = mesh->topology->max_dim;
uint32 n_incident, ii;
uint32 *nd2 = 0;
char *mask = 0;
MeshEntityIterator it1[1], it2[1], it3[1];
Indices ei1[1], ei2[1];
MeshConnectivity *c12 = 0; // d1 -> d2 - to compute
MeshConnectivity *c10 = 0; // d1 -> 0 - known
MeshConnectivity *c20 = 0; // d2 -> 0 - known
debprintf("intersect %d -> %d (%d)\n", d1, d2, d3);
if (d1 < d2) {
errput("d1 must be greater or equal to d2 in mesh_intersect()!\n");
ERR_CheckGo(ret);
}
c12 = mesh->topology->conn[IJ(D, d1, d2)];
if (d1 > d2) {
c10 = mesh->topology->conn[IJ(D, d1, 0)];
c20 = mesh->topology->conn[IJ(D, d2, 0)];
}
mask = alloc_mem(char, mesh->topology->num[d2]);
// Count entities of d2 -> d1.
conn_alloc(c12, mesh->topology->num[d1], 0);
ERR_CheckGo(ret);
nd2 = c12->offsets + 1;
for (mei_init(it1, mesh, d1); mei_go(it1); mei_next(it1)) {
// Clear mask for it1 incident entities of dimension d2.
for (mei_init_conn(it3, it1->entity, d3); mei_go(it3); mei_next(it3)) {
for (mei_init_conn(it2, it3->entity, d2); mei_go(it2); mei_next(it2)) {
mask[it2->entity->ii] = 0;
}
}
for (mei_init_conn(it3, it1->entity, d3); mei_go(it3); mei_next(it3)) {
for (mei_init_conn(it2, it3->entity, d2); mei_go(it2); mei_next(it2)) {
if (mask[it2->entity->ii]) continue;
mask[it2->entity->ii] = 1;
if (d1 == d2) {
if (it1->entity->ii != it2->entity->ii) {
nd2[it1->entity->ii]++;
}
} else {
// Get incident vertices.
me_get_incident2(it1->entity, ei1, c10);
me_get_incident2(it2->entity, ei2, c20);
if (contains(ei1, ei2)) {
nd2[it1->entity->ii]++;
}
}
}
}
}
// c12->offsets now contains counts - make a cumsum to get offsets.
for (ii = 1; ii < c12->num + 1; ii++) {
c12->offsets[ii] += c12->offsets[ii-1];
}
n_incident = c12->offsets[c12->num];
debprintf("intersect n_incident (%d -> %d): %d\n", d1, d2, n_incident);
// Fill in the indices.
conn_alloc(c12, 0, n_incident);
ERR_CheckGo(ret);
for (ii = 0; ii < c12->n_incident; ii++) {
c12->indices[ii] = UINT32_None; // "not set" value.
}
for (mei_init(it1, mesh, d1); mei_go(it1); mei_next(it1)) {
// Clear mask for it1 incident entities of dimension d2.
for (mei_init_conn(it3, it1->entity, d3); mei_go(it3); mei_next(it3)) {
for (mei_init_conn(it2, it3->entity, d2); mei_go(it2); mei_next(it2)) {
mask[it2->entity->ii] = 0;
}
}
for (mei_init_conn(it3, it1->entity, d3); mei_go(it3); mei_next(it3)) {
for (mei_init_conn(it2, it3->entity, d2); mei_go(it2); mei_next(it2)) {
if (mask[it2->entity->ii]) continue;
mask[it2->entity->ii] = 1;
if (d1 == d2) {
if (it1->entity->ii != it2->entity->ii) {
conn_set_to_free(c12, it1->entity->ii, it2->entity->ii);
ERR_CheckGo(ret);
}
} else {
// Get incident vertices.
me_get_incident2(it1->entity, ei1, c10);
me_get_incident2(it2->entity, ei2, c20);
if (contains(ei1, ei2)) {
conn_set_to_free(c12, it1->entity->ii, it2->entity->ii);
ERR_CheckGo(ret);
}
}
}
}
}
end_label:
free_mem(mask);
return(ret);
}
void SMPProtocol::writeBackAckHandler(SMPMemRequest *sreq)
{
IJ(0);
// sreq->ack(0);
}
int32 mesh_build(Mesh *mesh, int32 dim)
{
int32 ret = RET_OK;
uint32 n_incident, n_v_max, n_loc;
uint32 ii, ic, id, found;
uint32 D = mesh->topology->max_dim;
uint32 facet[4]; // Max. space for single facet.
uint32 *oris = 0;
uint32 loc_oris[12];
uint32 *nDd = 0;
uint32 *ptr1 = 0, *ptr2 = 0;
uint32 *cell_types = mesh->topology->cell_types;
Indices cell_vertices[1];
MeshEntityIterator it0[1], it1[1], it2[1];
MeshConnectivity *cD0 = 0; // D -> 0 - known
MeshConnectivity *cDd = 0; // D -> d - to compute
MeshConnectivity *cd0 = 0; // d -> 0 - to compute
MeshConnectivity **locs = 0;
MeshConnectivity *loc = 0;
MeshConnectivity sloc[1]; // Local connectivity with sorted global vertices.
MeshConnectivity gloc[1]; // Local connectivity with global vertices.
debprintf("build %d\n", dim);
if (!mesh->topology->conn[IJ(D, D, D)]->num) {
mesh_setup_connectivity(mesh, D, D);
}
cD0 = mesh->topology->conn[IJ(D, D, 0)];
cDd = mesh->topology->conn[IJ(D, D, dim)];
cd0 = mesh->topology->conn[IJ(D, dim, 0)];
locs = (dim == 1) ? mesh->entities->edges : mesh->entities->faces;
// Max. number of vertices in facets.
n_v_max = (dim == 1) ? 2 : 4;
// Count entities of D -> d.
conn_alloc(cDd, mesh->topology->num[D], 0);
ERR_CheckGo(ret);
nDd = cDd->offsets + 1;
for (mei_init(it0, mesh, D); mei_go(it0); mei_next(it0)) {
loc = locs[cell_types[it0->it]];
nDd[it0->it] = loc->num;
}
// cDd->offsets now contains counts - make a cumsum to get offsets.
for (ii = 1; ii < cDd->num + 1; ii++) {
cDd->offsets[ii] += cDd->offsets[ii-1];
}
n_incident = cDd->offsets[cDd->num];
debprintf("build n_incident (%d -> %d): %d\n", D, dim, n_incident);
// Cell-local orientations w.r.t. D -> d.
oris = alloc_mem(uint32, n_incident);
if (dim == 2) {
free_mem(mesh->topology->face_oris);
mesh->topology->face_oris = oris;
} else {
free_mem(mesh->topology->edge_oris);
mesh->topology->edge_oris = oris;
}
// Allocate D -> d indices.
conn_alloc(cDd, 0, n_incident);
ERR_CheckGo(ret);
for (ii = 0; ii < cDd->n_incident; ii++) {
cDd->indices[ii] = UINT32_None; // "not set" value.
}
// Allocate maximal buffers for d -> 0 arrays.
conn_alloc(cd0, n_incident, n_incident * n_v_max);
debprintf("build max. n_incident_vertex: %d\n", n_incident * n_v_max);
// Allocate maximal buffers for local connectivity with sorted global
// vertices. Counts have to be set to zero to avoid spurious calls to
// conn_free()!
sloc->num = sloc->n_incident = 0;
conn_alloc(sloc, 12, n_v_max * 12);
gloc->num = gloc->n_incident = 0;
conn_alloc(gloc, 12, n_v_max * 12);
id = 0;
for (mei_init(it0, mesh, D); mei_go(it0); mei_next(it0)) {
// Get vertex sets for local entities of current cell.
loc = locs[cell_types[it0->it]];
me_get_incident2(it0->entity, cell_vertices, cD0);
get_local_connectivity(gloc, cell_vertices, loc);
conn_set_from(sloc, gloc);
sort_local_connectivity(sloc, loc_oris, loc->num);
// Iterate over entities in the vertex sets.
for (ii = 0; ii < loc->num; ii++) {
// ii points to a vertex set in sloc/gloc.
n_loc = sloc->offsets[ii+1] - sloc->offsets[ii];
// Try to find entity in cells visited previously.
for (mei_init_conn(it1, it0->entity, D); mei_go(it1); mei_next(it1)) {
if (it1->entity->ii >= it0->entity->ii) continue;
// Iterate over facets of visited cells.
for (mei_init_conn(it2, it1->entity, dim); mei_go(it2); mei_next(it2)) {
ptr1 = cd0->indices + cd0->offsets[it2->entity->ii];
uint32_sort234_copy(facet, ptr1, n_loc);
ptr2 = sloc->indices + sloc->offsets[ii];
found = 1;
for (ic = 0; ic < n_loc; ic++) {
if (facet[ic] != ptr2[ic]) {
found = 0;
break;
}
}
if (found) {
// Assign existing entity to D -> d.
conn_set_to_free(cDd, it0->entity->ii, it2->entity->ii);
goto found_label;
}
}
}
// Entity not found - create new.
// Add it as 'id' to D -> d.
conn_set_to_free(cDd, it0->entity->ii, id);
// Add vertices in gloc to d -> 0.
cd0->offsets[id+1] = cd0->offsets[id] + n_loc;
ptr1 = cd0->indices + cd0->offsets[id];
ptr2 = gloc->indices + gloc->offsets[ii];
for (ic = 0; ic < n_loc; ic++) {
ptr1[ic] = ptr2[ic];
}
// Increment entity counter.
id++;
found_label:
// Store entity orientation key to position of the last used item in cDd.
ptr1 = cDd->offsets + it0->entity->ii;
ic = ptr1[1] - 1;
while (ic >= ptr1[0]) {
if (cDd->indices[ic] != UINT32_None) { // Not found & free slot.
break;
}
ic--;
}
// printf("%d << %d, %d\n", ic, ii, loc_oris[ii]);
oris[ic] = loc_oris[ii];
}
}
debprintf("build n_unique: %d, n_incident (%d -> 0): %d\n",
id, dim, cd0->offsets[id]);
// Update entity count in topology.
mesh->topology->num[dim] = id;
// Strip d -> 0.
conn_resize(cd0, id, cd0->offsets[id]);
end_label:
conn_free(sloc);
conn_free(gloc);
return(ret);
}
// Checkpoint algorithm.
// O(n^3) time
// O(n^2) space
score_t Phaser::partial_aligner(size_t i_ini,
size_t j_ini,
size_t k_ini,
size_t i_end,
size_t j_end,
size_t k_end,
size_t *i_med,
size_t *j_med,
size_t *k_med) {
assert(j_end >= j_ini || j_end + 1 == j_ini);
assert(i_end >= i_ini || i_end + 1 == i_ini);
assert(k_end >= k_ini);
score_t * prev_face[8];
score_t * curr_face[8];
my_pair * prev_check[8];
my_pair * curr_check[8];
// Obs: we will use local i (resp. j, k) from 0 to I_len (J_len, K_len).
// characters are stracted from i_ini+i (j, k resp.).
// checkpoint answer is shifted back at the end.
I_len = i_end - i_ini + 1;
J_len = j_end - j_ini + 1;
size_t K_len = k_end - k_ini + 1;
size_t mid_k = K_len/2;
for (size_t m = 0; m < 8; m++) {
prev_face[m] = new score_t[(I_len+1) * (J_len+1)];
prev_check[m] = new my_pair[(I_len+1) * (J_len+1)];
}
// 8 points:
for (bool cf : {false, true}) {
for (bool ff : {false, true}) {
for (bool mf : {false, true}) {
size_t m = m_index(mf, ff, cf);
prev_face[m][IJ(0, 0)] = 0;
prev_check[m][IJ(0, 0)] = my_pair(0, 0);
}
}
}
// 8 lines (j=0):
for (size_t i = 1; i <= I_len; i++) {
for (bool cf : {false, true}) {
for (bool ff : {false, true}) {
for (bool mf : {false, true}) {
size_t m = m_index(mf, ff, cf);
char m_char = mf ? M2[i_ini + i-1] : M1[i_ini + i-1];
prev_face[m][IJ(i, 0)] = std::max(prev_face[m_index(0, ff, cf)][IJ(i-1, 0)] + score(m_char, '-'), // NOLINT
prev_face[m_index(1, ff, cf)][IJ(i-1, 0)] + score(m_char, '-')); // NOLINT
prev_check[m][IJ(i, 0)] = my_pair(i, 0);
}
}
}
}
// 8 faces:
for (size_t j = 1; j <= J_len; j++) {
for (size_t i = 0; i <= I_len; i++) {
for (bool cf : {false, true}) {
for (bool ff : {false, true}) {
for (bool mf : {false, true}) {
size_t m = m_index(mf, ff, cf);
char f_char = ff ? F2[j_ini + j-1] : F1[j_ini + j-1];
prev_face[m][IJ(i, j)] = std::max(prev_face[m_index(mf, 0, cf)][IJ(i, j-1)] + score(f_char, '-'), // NOLINT
prev_face[m_index(mf, 1, cf)][IJ(i, j-1)] + score(f_char, '-')); // NOLINT
prev_check[m][IJ(i, j)] = my_pair(i, j);
}
}
}
}
}
PrintFace(prev_face);
bool malloc_opt = true;
if (malloc_opt) {
for (size_t m = 0; m < 8; m++) {
curr_face[m] = new score_t[(I_len+1) * (J_len+1)];
curr_check[m] = new my_pair[(I_len+1) * (J_len+1)];
}
}
// the rest of the faces:
for (size_t k = 1; k <= K_len; k++) {
if (!malloc_opt) {
for (size_t m = 0; m < 8; m++) {
curr_face[m] = new score_t[(I_len+1) * (J_len+1)];
curr_check[m] = new my_pair[(I_len+1) * (J_len+1)];
}
}
for (size_t j = 0; j <= J_len; j++) {
for (size_t i = 0; i <= I_len; i++) {
for (bool cf : {false, true}) {
for (bool ff : {false, true}) {
for (bool mf : {false, true}) {
// m_char anf f_char should not be used, at least i > 0 (j > 0).
// If that is not the case, we initilize with a non-accepted character that
// will trig an error if used.
char m_char, f_char;
if (i > 0) {
m_char = mf ? M2[i_ini + i-1] : M1[i_ini + i-1];
} else {
m_char = 'J'; // Not in gen alphabet, will trigger an error if used.
}
if (j > 0) {
f_char = ff ? F2[j_ini + j-1] : F1[j_ini + j-1];
} else {
f_char = 'J'; // Not in gen alphabet, will trigger an error if used.
}
char c_1 = cf ? C2[k_ini + k-1] : C1[k_ini + k-1];
char c_2 = cf ? C1[k_ini + k-1] : C2[k_ini + k-1];
UpdateGeneral(curr_face,
prev_face,
curr_check,
prev_check,
i,
j,
k,
mid_k,
mf,
ff,
cf,
m_char,
f_char,
c_1,
c_2);
}
}
}
}
}
for (size_t m = 0; m < 8; m++) {
if (malloc_opt) {
score_t * tmp_face = prev_face[m];
my_pair * tmp_check = prev_check[m];
prev_face[m] = curr_face[m];
prev_check[m] = curr_check[m];
curr_face[m] = tmp_face;
curr_check[m] = tmp_check;
} else { delete[] (prev_face[m]);
delete[] (prev_check[m]);
prev_face[m] = curr_face[m];
prev_check[m] = curr_check[m];
}
}
if (k >= mid_k) {
// verbose = true;
}
PrintFace(prev_face);
PrintCheck(prev_check);
}
// we use char_i = M[i-1]
for (int i = 0; i < 8; i++) {
assert(prev_check[i][IJ(I_len, J_len)].first <= I_len);
assert(prev_check[i][IJ(I_len, J_len)].second <= J_len);
prev_check[i][IJ(I_len, J_len)].first--;
prev_check[i][IJ(I_len, J_len)].second--;
}
mid_k--;
// extract max:
score_t ans;
bool flip_ans;
ExtractMax(prev_face, prev_check, &ans, i_med, j_med, &flip_ans);
*k_med = k_ini + mid_k;
*i_med = i_ini + (*i_med);
*j_med = j_ini + (*j_med);
assert(CorrectIniMedEnd(i_ini, *i_med, i_end));
assert(CorrectIniMedEnd(j_ini, *j_med, j_end));
assert(CorrectIniMedEnd(k_ini, *k_med, k_end));
char phase_char = flip_ans ? '1' : '0';
if (phase_string[k_end] != '?') {
assert(phase_string[k_end] == phase_char);
}
if (phase_string[k_end] == '?') {
phase_string[k_end] = phase_char;
}
for (size_t m = 0; m < 8; m++) {
delete[] (prev_face[m]);
delete[] (prev_check[m]);
if (malloc_opt) {
delete[] (curr_face[m]);
delete[] (curr_check[m]);
}
}
return ans;
}
// `normals` must be preallocated.
int32 mesh_get_facet_normals(Mesh *mesh, float64 *normals, int32 which)
{
#define VS(ic, id) (coors[nc*cell_vertices->indices[ik[ic]] + id])
uint32 D = mesh->topology->max_dim;
uint32 nc = mesh->geometry->dim;
int32 dim = D - 1;
uint32 ii, id, n_loc;
uint32 *ik;
uint32 *cell_types = mesh->topology->cell_types;
float64 *coors = mesh->geometry->coors;
float64 vv0, vv1, vv2, vv3, v0[3], v1[3], v2[3], v3[3], ndir[3], ndir1[3];
Indices cell_vertices[1];
MeshEntityIterator it0[1];
MeshConnectivity *cD0 = 0; // D -> 0
MeshConnectivity *cDd = 0; // D -> d
MeshConnectivity *loc = 0;
MeshConnectivity **locs = 0;
cD0 = mesh->topology->conn[IJ(D, D, 0)];
cDd = mesh->topology->conn[IJ(D, D, dim)];
// Local entities - reference cell edges or faces.
locs = (dim == 1) ? mesh->entities->edges : mesh->entities->faces;
for (mei_init(it0, mesh, D); mei_go(it0); mei_next(it0)) {
me_get_incident2(it0->entity, cell_vertices, cD0);
loc = locs[cell_types[it0->it]];
for (ii = 0; ii < loc->num; ii++) {
ik = loc->indices + loc->offsets[ii]; // Points to local facet vertices.
n_loc = loc->offsets[ii+1] - loc->offsets[ii];
if (n_loc == 2) { // Edge normals.
for (id = 0; id < nc; id++) {
v0[id] = VS(1, id) - VS(0, id);
}
ndir[0] = v0[1];
ndir[1] = -v0[0];
} else if (n_loc == 3) { // Triangular face normals.
for (id = 0; id < nc; id++) {
vv0 = VS(0, id);
v0[id] = VS(1, id) - vv0;
v1[id] = VS(2, id) - vv0;
}
gtr_cross_product(ndir, v0, v1);
} else if (n_loc == 4) { // Quadrilateral face normals.
for (id = 0; id < nc; id++) {
vv0 = VS(0, id);
vv1 = VS(1, id);
vv2 = VS(2, id);
vv3 = VS(3, id);
v0[id] = vv1 - vv0;
v1[id] = vv3 - vv0;
v2[id] = vv3 - vv2;
v3[id] = vv1 - vv2;
}
if (which == 0) {
gtr_cross_product(ndir, v0, v1);
} else if (which == 1) {
gtr_cross_product(ndir, v2, v3);
} else {
gtr_cross_product(ndir, v0, v1);
gtr_cross_product(ndir1, v2, v3);
for (id = 0; id < nc; id++) {
ndir[id] += ndir1[id];
}
}
}
gtr_normalize_v3(ndir, ndir, nc, 0);
for (id = 0; id < nc; id++) {
normals[nc * (cDd->offsets[it0->it] + ii) + id] = ndir[id];
}
}
}
return(RET_OK);
#undef VS
}
// With the current scheme we store the larger i, j
// that can be aligned to mid_k in the optimal alignment.
void Phaser::UpdateGeneral(score_t ** curr_face,
score_t ** prev_face,
my_pair ** curr_check,
my_pair ** prev_check,
size_t i,
size_t j,
size_t k,
size_t mid_k,
bool mf,
bool ff,
bool cf,
char m_char,
char f_char,
char c_1,
char c_2) {
size_t m = m_index(mf, ff, cf);
score_t max_score;
my_pair max_check;
// only k decreases. Two deletions from C.
score_t c_ins_1 = prev_face[m_index(mf, ff, 0)][IJ(i, j)] + score(c_1, '-') + score(c_2, '-');
score_t c_ins_2 = prev_face[m_index(mf, ff, 1)][IJ(i, j)] + score(c_1, '-') + score(c_2, '-');
my_pair check_1 = prev_check[m_index(mf, ff, 0)][IJ(i, j)];
my_pair check_2 = prev_check[m_index(mf, ff, 1)][IJ(i, j)];
if (c_ins_1 >= c_ins_2) {
max_score = c_ins_1;
max_check = check_1;
} else {
max_score = c_ins_2;
max_check = check_2;
}
if (i > 0) {
if (m_char == '-') {
score_t p1 = curr_face[m_index(0, ff, cf)][IJ(i-1, j)];
score_t p2 = curr_face[m_index(1, ff, cf)][IJ(i-1, j)];
curr_face[m][IJ(i, j)] = std::max(p1, p2);
if (k == mid_k) {
curr_check[m][IJ(i, j)] = my_pair(i, j);
} else if (k > mid_k) {
curr_check[m][IJ(i, j)] = (p1 > p2) ?
curr_check[m_index(0, ff, cf)][IJ(i-1, j)] :
curr_check[m_index(1, ff, cf)][IJ(i-1, j)];
}
return;
}
// only i decreases. Single deletion from M.
// TODO(Readability): This might be a one-level for over pre_mf.
score_t ins_val;
my_pair ins_check;
ins_val = curr_face[m_index(0, ff, cf)][IJ(i-1, j)] + score(m_char, '-');
ins_check = curr_check[m_index(0, ff, cf)][IJ(i-1, j)];
UpdateVals(ins_val, ins_check, &max_score, &max_check);
ins_val = curr_face[m_index(1, ff, cf)][IJ(i-1, j)] + score(m_char, '-');
ins_check = curr_check[m_index(1, ff, cf)][IJ(i-1, j)];
UpdateVals(ins_val, ins_check, &max_score, &max_check);
// k and i decreases: single deletions from C, M aligns.
for (bool pre_cf : {false, true}) {
for (bool pre_mf : {false, true}) {
score_t del_val = prev_face[m_index(pre_mf, ff, pre_cf)][IJ(i-1, j)] + score(c_1, m_char) + score(c_2, '-'); // NOLINT
my_pair del_check = prev_check[m_index(pre_mf, ff, pre_cf)][IJ(i-1, j)];
UpdateVals(del_val, del_check, &max_score, &max_check);
}
}
}
if (j > 0) {
if (f_char == '-') {
score_t p1 = curr_face[m_index(mf, 0, cf)][IJ(i, j-1)];
score_t p2 = curr_face[m_index(mf, 1, cf)][IJ(i, j-1)];
curr_face[m][IJ(i, j)] = std::max(p1, p2);
if (k == mid_k) {
curr_check[m][IJ(i, j)] = my_pair(i, j);
} else if (k > mid_k) {
curr_check[m][IJ(i, j)] = (p1 > p2) ?
curr_check[m_index(mf, 0, cf)][IJ(i, j-1)] :
curr_check[m_index(mf, 1, cf)][IJ(i, j-1)];
}
return;
}
score_t ins_val;
my_pair ins_check;
// only j decreases. Single deletion from F.
ins_val = curr_face[m_index(mf, 0, cf)][IJ(i, j-1)] + score(f_char, '-');
ins_check = curr_check[m_index(mf, 0, cf)][IJ(i, j-1)];
UpdateVals(ins_val, ins_check, &max_score, &max_check);
ins_val = curr_face[m_index(mf, 1, cf)][IJ(i, j-1)] + score(f_char, '-');
ins_check = curr_check[m_index(mf, 1, cf)][IJ(i, j-1)];
UpdateVals(ins_val, ins_check, &max_score, &max_check);
// k and j decreases: single deletions from C, F aligns.
for (bool pre_cf : {false, true}) {
for (bool pre_ff : {false, true}) {
score_t del_val = prev_face[m_index(mf, pre_ff, pre_cf)][IJ(i, j-1)] + score(c_1, '-') + score(c_2, f_char); // NOLINT
my_pair del_check = prev_check[m_index(mf, pre_ff, pre_cf)][IJ(i, j-1)];
UpdateVals(del_val, del_check, &max_score, &max_check);
}
}
}
if (i > 0 && j > 0) {
for (bool pre_cf : {false, true}) {
for (bool pre_ff : {false, true}) {
for (bool pre_mf : {false, true}) {
score_t aln_val = prev_face[m_index(pre_mf, pre_ff, pre_cf)][IJ(i-1, j-1)] + score(c_1, m_char) + score(c_2, f_char); // NOLINT
my_pair aln_check = prev_check[m_index(pre_mf, pre_ff, pre_cf)][IJ(i-1, j-1)];
UpdateVals(aln_val, aln_check, &max_score, &max_check);
}
}
} }
curr_face[m][IJ(i, j)] = max_score;
if (k == mid_k) {
curr_check[m][IJ(i, j)] = my_pair(i, j);
} else if (k > mid_k) {
curr_check[m][IJ(i, j)] = max_check;
}
}
void forward_prismatic_gradient(int n, double *u, double pos, double *t, double *r, double *Dr, double *Dt)
{
if (n == 2) {
IJ(Dr,4,0,0) = 0 ; IJ(Dr,4,0,1) = 0 ; IJ(Dr,4,0,2) = -sin(r[0]) ; IJ(Dr,4,0,3) = 0 ; IJ(Dr,4,0,4) = 0 ; IJ(Dr,4,0,5) = 0 ;
IJ(Dr,4,1,0) = 0 ; IJ(Dr,4,1,1) = 0 ; IJ(Dr,4,1,2) = cos(r[0]) ; IJ(Dr,4,1,3) = 0 ; IJ(Dr,4,1,4) = 0 ; IJ(Dr,4,1,5) = 0 ;
IJ(Dr,4,2,0) = 0 ; IJ(Dr,4,2,1) = 0 ; IJ(Dr,4,2,2) = -cos(r[0]) ; IJ(Dr,4,2,3) = 0 ; IJ(Dr,4,2,4) = 0 ; IJ(Dr,4,2,5) = 0 ;
IJ(Dr,4,3,0) = 0 ; IJ(Dr,4,3,1) = 0 ; IJ(Dr,4,3,2) = -sin(r[0]) ; IJ(Dr,4,3,3) = 0 ; IJ(Dr,4,3,4) = 0 ; IJ(Dr,4,3,5) = 0 ;
IJ(Dt,2,0,0) = 1 ; IJ(Dt,2,0,1) = 0 ; IJ(Dt,2,0,2) = 0 ; IJ(Dt,2,0,3) = pos ; IJ(Dt,2,0,4) = 0 ; IJ(Dt,2,0,5) = u[0] ;
IJ(Dt,2,1,0) = 0 ; IJ(Dt,2,1,1) = 1 ; IJ(Dt,2,1,2) = 0 ; IJ(Dt,2,1,3) = 0 ; IJ(Dt,2,1,4) = pos ; IJ(Dt,2,1,5) = u[1] ;
} else {
IJ(Dr,9,0,0) = 0 ; IJ(Dr,9,0,1) = 0 ; IJ(Dr,9,0,2) = 0 ; IJ(Dr,9,0,3) = - cos(r[0])*sin(r[2]) - cos(r[1])*cos(r[2])*sin(r[0]) ; IJ(Dr,9,0,4) = -cos(r[0])*cos(r[2])*sin(r[1]) ; IJ(Dr,9,0,5) = - cos(r[2])*sin(r[0]) - cos(r[0])*cos(r[1])*sin(r[2]) ; IJ(Dr,9,0,6) = 0 ; IJ(Dr,9,0,7) = 0 ; IJ(Dr,9,0,8) = 0 ; IJ(Dr,9,0,9) = 0 ;
IJ(Dr,9,1,0) = 0 ; IJ(Dr,9,1,1) = 0 ; IJ(Dr,9,1,2) = 0 ; IJ(Dr,9,1,3) = cos(r[0])*cos(r[1])*cos(r[2]) - sin(r[0])*sin(r[2]) ; IJ(Dr,9,1,4) = -cos(r[2])*sin(r[0])*sin(r[1]) ; IJ(Dr,9,1,5) = cos(r[0])*cos(r[2]) - cos(r[1])*sin(r[0])*sin(r[2]) ; IJ(Dr,9,1,6) = 0 ; IJ(Dr,9,1,7) = 0 ; IJ(Dr,9,1,8) = 0 ; IJ(Dr,9,1,9) = 0 ;
IJ(Dr,9,2,0) = 0 ; IJ(Dr,9,2,1) = 0 ; IJ(Dr,9,2,2) = 0 ; IJ(Dr,9,2,3) = 0 ; IJ(Dr,9,2,4) = cos(r[1])*cos(r[2]) ; IJ(Dr,9,2,5) = -sin(r[1])*sin(r[2]) ; IJ(Dr,9,2,6) = 0 ; IJ(Dr,9,2,7) = 0 ; IJ(Dr,9,2,8) = 0 ; IJ(Dr,9,2,9) = 0 ;
IJ(Dr,9,3,0) = 0 ; IJ(Dr,9,3,1) = 0 ; IJ(Dr,9,3,2) = 0 ; IJ(Dr,9,3,3) = cos(r[1])*sin(r[0])*sin(r[2]) - cos(r[0])*cos(r[2]) ; IJ(Dr,9,3,4) = cos(r[0])*sin(r[1])*sin(r[2]) ; IJ(Dr,9,3,5) = sin(r[0])*sin(r[2]) - cos(r[0])*cos(r[1])*cos(r[2]) ; IJ(Dr,9,3,6) = 0 ; IJ(Dr,9,3,7) = 0 ; IJ(Dr,9,3,8) = 0 ; IJ(Dr,9,3,9) = 0 ;
IJ(Dr,9,4,0) = 0 ; IJ(Dr,9,4,1) = 0 ; IJ(Dr,9,4,2) = 0 ; IJ(Dr,9,4,3) = - cos(r[2])*sin(r[0]) - cos(r[0])*cos(r[1])*sin(r[2]) ; IJ(Dr,9,4,4) = sin(r[0])*sin(r[1])*sin(r[2]) ; IJ(Dr,9,4,5) = - cos(r[0])*sin(r[2]) - cos(r[1])*cos(r[2])*sin(r[0]) ; IJ(Dr,9,4,6) = 0 ; IJ(Dr,9,4,7) = 0 ; IJ(Dr,9,4,8) = 0 ; IJ(Dr,9,4,9) = 0 ;
IJ(Dr,9,5,0) = 0 ; IJ(Dr,9,5,1) = 0 ; IJ(Dr,9,5,2) = 0 ; IJ(Dr,9,5,3) = 0 ; IJ(Dr,9,5,4) = -cos(r[1])*sin(r[2]) ; IJ(Dr,9,5,5) = -cos(r[2])*sin(r[1]) ; IJ(Dr,9,5,6) = 0 ; IJ(Dr,9,5,7) = 0 ; IJ(Dr,9,5,8) = 0 ; IJ(Dr,9,5,9) = 0 ;
IJ(Dr,9,6,0) = 0 ; IJ(Dr,9,6,1) = 0 ; IJ(Dr,9,6,2) = 0 ; IJ(Dr,9,6,3) = sin(r[0])*sin(r[1]) ; IJ(Dr,9,6,4) = -cos(r[0])*cos(r[1]) ; IJ(Dr,9,6,5) = 0 ; IJ(Dr,9,6,6) = 0 ; IJ(Dr,9,6,7) = 0 ; IJ(Dr,9,6,8) = 0 ; IJ(Dr,9,6,9) = 0 ;
IJ(Dr,9,7,0) = 0 ; IJ(Dr,9,7,1) = 0 ; IJ(Dr,9,7,2) = 0 ; IJ(Dr,9,7,3) = -cos(r[0])*sin(r[1]) ; IJ(Dr,9,7,4) = -cos(r[1])*sin(r[0]) ; IJ(Dr,9,7,5) = 0 ; IJ(Dr,9,7,6) = 0 ; IJ(Dr,9,7,7) = 0 ; IJ(Dr,9,7,8) = 0 ; IJ(Dr,9,7,9) = 0 ;
IJ(Dr,9,8,0) = 0 ; IJ(Dr,9,8,1) = 0 ; IJ(Dr,9,8,2) = 0 ; IJ(Dr,9,8,3) = 0 ; IJ(Dr,9,8,4) = -sin(r[1]) ; IJ(Dr,9,8,5) = 0 ; IJ(Dr,9,8,6) = 0 ; IJ(Dr,9,8,7) = 0 ; IJ(Dr,9,8,8) = 0 ; IJ(Dr,9,8,9) = 0 ;
IJ(Dt,3,0,0) = 1 ; IJ(Dt,3,0,1) = 0 ; IJ(Dt,3,0,2) = 0 ; IJ(Dt,3,0,3) = 0 ; IJ(Dt,3,0,4) = 0 ; IJ(Dt,3,0,5) = 0 ; IJ(Dt,3,0,6) = pos ; IJ(Dt,3,0,7) = 0 ; IJ(Dt,3,0,8) = 0 ; IJ(Dt,3,0,9) = u[0] ;
IJ(Dt,3,1,0) = 0 ; IJ(Dt,3,1,1) = 1 ; IJ(Dt,3,1,2) = 0 ; IJ(Dt,3,1,3) = 0 ; IJ(Dt,3,1,4) = 0 ; IJ(Dt,3,1,5) = 0 ; IJ(Dt,3,1,6) = 0 ; IJ(Dt,3,1,7) = pos ; IJ(Dt,3,1,8) = 0 ; IJ(Dt,3,1,9) = u[1] ;
IJ(Dt,3,2,0) = 0 ; IJ(Dt,3,2,1) = 0 ; IJ(Dt,3,2,2) = 1 ; IJ(Dt,3,2,3) = 0 ; IJ(Dt,3,2,4) = 0 ; IJ(Dt,3,2,5) = 0 ; IJ(Dt,3,2,6) = 0 ; IJ(Dt,3,2,7) = 0 ; IJ(Dt,3,2,8) = pos ; IJ(Dt,3,2,9) = u[2] ;
}
}
void forward_prismatic(int n, double *offset, double *u, double pos, double *x)
{
if (n == 2) {
IJ(x,3,0,0) = IJ(offset,3,0,0) ; IJ(x,3,0,1) = IJ(offset,3,0,1) ; IJ(x,3,0,2) = IJ(offset,3,0,2) + pos*u[0] ;
IJ(x,3,1,0) = IJ(offset,3,1,0) ; IJ(x,3,1,1) = IJ(offset,3,1,1) ; IJ(x,3,1,2) = IJ(offset,3,1,2) + pos*u[1] ;
IJ(x,3,2,0) = 0 ; IJ(x,3,2,1) = 0 ; IJ(x,3,2,2) = 1 ;
} else {
IJ(x,4,0,0) = IJ(offset,4,0,0) ; IJ(x,4,0,1) = IJ(offset,4,0,1) ; IJ(x,4,0,2) = IJ(offset,4,0,2) ; IJ(x,4,0,3) = IJ(offset,4,0,3) + pos*u[0] ;
IJ(x,4,1,0) = IJ(offset,4,1,0) ; IJ(x,4,1,1) = IJ(offset,4,1,1) ; IJ(x,4,1,2) = IJ(offset,4,1,2) ; IJ(x,4,1,3) = IJ(offset,4,1,3) + pos*u[1] ;
IJ(x,4,2,0) = IJ(offset,4,2,0) ; IJ(x,4,2,1) = IJ(offset,4,2,1) ; IJ(x,4,2,2) = IJ(offset,4,2,2) ; IJ(x,4,2,3) = IJ(offset,4,2,3) + pos*u[2] ;
IJ(x,4,3,0) = 0 ; IJ(x,4,3,1) = 0 ; IJ(x,4,3,2) = 0 ; IJ(x,4,3,3) = 1 ;
}
}
// `volumes` must be preallocated.
int32 mesh_get_volumes(Mesh *mesh, float64 *volumes, int32 dim)
{
#define VS(ic, id) (coors[nc*entity_vertices->indices[ic] + id])
int32 ret = RET_OK;
uint32 D = mesh->topology->max_dim;
uint32 nc = mesh->geometry->dim;
uint32 id;
uint32 indx2[6];
float64 vol, aux, vv0, vv1, vv2, vv3;
float64 *ptr = volumes;
float64 *coors = mesh->geometry->coors;
float64 v0[3], v1[3], v2[3], ndir[3];
Indices entity_vertices[1];
MeshEntityIterator it0[1];
MeshConnectivity *cd0 = 0; // d -> 0
if (!dim) {
errput("vertices have no volume!\n");
ERR_CheckGo(ret);
}
cd0 = mesh->topology->conn[IJ(D, dim, 0)];
for (mei_init(it0, mesh, dim); mei_go(it0); mei_next(it0)) {
me_get_incident2(it0->entity, entity_vertices, cd0);
/* mei_print(it0, stdout); */
/* ind_print(entity_vertices, stdout); */
vol = 0;
if (dim == 1) { // Edges.
for (id = 0; id < nc; id++) {
aux = VS(1, id) - VS(0, id);
vol += aux * aux;
}
ptr[0] = sqrt(vol);
} else if (entity_vertices->num == 3) { // Triangles.
ptr[0] = _tri_area(coors, entity_vertices->indices, nc);
} else if (nc == 2) { // Quadrilateral cells.
ptr[0] = _tri_area(coors, entity_vertices->indices, nc);
indx2[0] = entity_vertices->indices[2];
indx2[1] = entity_vertices->indices[3];
indx2[2] = entity_vertices->indices[0];
ptr[0] += _tri_area(coors, indx2, nc);
} else if (nc == 3) { // 3D.
if (entity_vertices->num == 4) {
if (dim == 2) { // Quadrilateral faces (approximate).
indx2[0] = entity_vertices->indices[0];
indx2[1] = entity_vertices->indices[1];
indx2[2] = entity_vertices->indices[2];
indx2[3] = entity_vertices->indices[3];
indx2[4] = entity_vertices->indices[0];
indx2[5] = entity_vertices->indices[1];
aux = _tri_area(coors, indx2, nc);
aux += _tri_area(coors, indx2 + 1, nc);
aux += _tri_area(coors, indx2 + 2, nc);
aux += _tri_area(coors, indx2 + 3, nc);
ptr[0] = 0.5 * aux;
} else { // Tetrahedral cells.
for (id = 0; id < nc; id++) {
vv0 = VS(0, id);
vv1 = VS(1, id);
vv2 = VS(2, id);
vv3 = VS(3, id);
v0[id] = vv1 - vv0;
v1[id] = vv2 - vv0;
v2[id] = vv3 - vv2;
}
gtr_cross_product(ndir, v0, v1);
gtr_dot_v3(ptr, v2, ndir, 3);
ptr[0] /= 6.0;
}
} else { // Hexahedral cells with trilinear interpolation.
// See https://math.stackexchange.com/questions/1628540/what-is-the-enclosed-volume-of-an-irregular-cube-given-the-x-y-z-coordinates-of
// Uses 0 1 3 2 4 5 7 6 ordering w.r.t. sfepy.
aux = _aux_hex(coors, entity_vertices->indices, nc, 0, 1, 3, 2);
aux -= _aux_hex(coors, entity_vertices->indices, nc, 4, 5, 7, 6);
aux -= _aux_hex(coors, entity_vertices->indices, nc, 0, 1, 4, 5);
aux += _aux_hex(coors, entity_vertices->indices, nc, 3, 2, 7, 6);
aux += _aux_hex(coors, entity_vertices->indices, nc, 0, 3, 4, 7);
aux -= _aux_hex(coors, entity_vertices->indices, nc, 1, 2, 5, 6);
ptr[0] = aux / 12.0;
}
}
ptr += 1;
}
end_label:
return(ret);
#undef VS
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
const mxArray *params;
double *r, *theta;
size_t m, n, d;
int i;
/* Check for proper number of arguments. */
if (nrhs!=1) {
mexErrMsgTxt("One input required.");
} else if (nlhs>1) {
mexErrMsgTxt("Too many output arguments.");
}
params = prhs[0];
/* Check argument types */
if (!mxIsDouble(params) || mxIsComplex(params) || mxGetNumberOfDimensions(params) != 2) {
mexErrMsgTxt("Input must be a real double scalar or 3-element vector.");
}
m = mxGetM(params);
n = mxGetN(params);
if (m != 1 && n != 1) {
mexErrMsgTxt("Input must be a scalar or 3-element vector.");
}
d = m > n ? m : n;
if (d == 1) d = 2;
if (d != 2 && d != 3) {
mexErrMsgTxt("I can only think in two or three dimensions.");
}
plhs[0] = mxCreateDoubleMatrix(d+1, d+1, mxREAL);
r = mxGetPr(plhs[0]);
theta = mxGetPr(params);
if (d == 2) {
IJ(r,d+1, 0,0) = cos(*theta); IJ(r,d+1, 0,1) = -sin(*theta); IJ(r,d+1, 0,2) = 0;
IJ(r,d+1, 1,0) = sin(*theta); IJ(r,d+1, 1,1) = cos(*theta); IJ(r,d+1, 1,2) = 0;
IJ(r,d+1, 2,0) = 0; IJ(r,d+1, 2,1) = 0; IJ(r,d+1, 2,2) = 1;
} else {
double ca = cos(theta[0]), cb = cos(theta[1]), cc = cos(theta[2]),
sa = sin(theta[0]), sb = sin(theta[1]), sc = sin(theta[2]);
IJ(r,d+1, 0,0) = ca*cb*cc - sa*sc; IJ(r,d+1, 0,1) = -cc*sa - ca*cb*sc; IJ(r,d+1, 0,2) = -ca*sb; IJ(r,d+1, 0,3) = 0;
IJ(r,d+1, 1,0) = ca*sc + cb*cc*sa; IJ(r,d+1, 1,1) = ca*cc - cb*sa*sc; IJ(r,d+1, 1,2) = -sa*sb; IJ(r,d+1, 1,3) = 0;
IJ(r,d+1, 2,0) = cc*sb; IJ(r,d+1, 2,1) = -sb*sc; IJ(r,d+1, 2,2) = cb; IJ(r,d+1, 2,3) = 0;
IJ(r,d+1, 3,0) = 0; IJ(r,d+1, 3,1) = 0; IJ(r,d+1, 3,2) = 0; IJ(r,d+1, 3,3) = 1;
}
}
