SEXP
GetMatRow(const SEXP data, const int idx)
{
Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > A = EigenXPtrToMapEigen<T>(data);
Eigen::Matrix<T, Eigen::Dynamic, 1> Am = A.row(idx-1);
return(wrap(Am));
}
// create initial points distribution
Eigen::ArrayXXd distmesh::utils::createInitialPoints(
Functional const& distanceFunction, double const initialPointDistance,
Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
// extract dimension of mesh
unsigned const dimension = boundingBox.cols();
// initially distribute points evenly in complete bounding box
Eigen::ArrayXi pointsPerDimension(dimension);
for (int dim = 0; dim < dimension; ++dim) {
pointsPerDimension(dim) = ceil((boundingBox(1, dim) - boundingBox(0, dim)) /
(initialPointDistance * (dim == 0 ? 1.0 : sqrt(3.0) / 2.0)));
}
Eigen::ArrayXXd points(pointsPerDimension.prod(), dimension);
for (int point = 0; point < points.rows(); ++point)
for (int dim = 0; dim < dimension; ++dim) {
int const pointIndex = (point / std::max(pointsPerDimension.topRows(dim).prod(), 1)) %
pointsPerDimension(dim);
points(point, dim) = boundingBox(0, dim) + (double)pointIndex * initialPointDistance *
(dim == 0 ? 1.0 : sqrt(3.0) / 2.0);
if (dim > 0) {
points(point, dim - 1) += pointIndex % 2 != 0 ? initialPointDistance / 2.0 : 0.0;
}
}
// reject points outside of region defined by distance function
points = selectMaskedArrayElements<double>(points,
distanceFunction(points) < constants::geometryEvaluationThreshold * initialPointDistance);
// clear duplicate points
Eigen::Array<bool, Eigen::Dynamic, 1> isUniquePoint =
Eigen::Array<bool, Eigen::Dynamic, 1>::Constant(points.rows(), true);
for (int i = 0; i < fixedPoints.rows(); ++i)
for (int j = 0; j < points.rows(); ++j) {
isUniquePoint(j) &= !(fixedPoints.row(i) == points.row(j)).all();
}
points = selectMaskedArrayElements<double>(points, isUniquePoint);
// calculate probability to keep points
Eigen::ArrayXd probability = 1.0 / elementSizeFunction(points).pow(dimension);
probability /= probability.maxCoeff();
// reject points with wrong probability
points = selectMaskedArrayElements<double>(points,
0.5 * (1.0 + Eigen::ArrayXd::Random(points.rows())) < probability);
// combine fixed and variable points to one array
Eigen::ArrayXXd finalPoints(points.rows() + fixedPoints.rows(), dimension);
finalPoints << fixedPoints, points;
return finalPoints;
}
// fix orientation of edges located at the boundary
Eigen::ArrayXXi distmesh::utils::fixBoundaryEdgeOrientation(
Eigen::Ref<Eigen::ArrayXXd const> const nodes,
Eigen::Ref<Eigen::ArrayXXi const> const triangulation,
Eigen::Ref<Eigen::ArrayXXi const> const _edges,
Eigen::Ref<Eigen::ArrayXXi const> const edgeIndices) {
Eigen::ArrayXXi edges = _edges;
// for the 2-D case fix orientation of boundary edges
if (nodes.cols() == 2) {
auto const boundary = utils::boundEdges(triangulation, edges, edgeIndices);
for (int edge = 0; edge < boundary.rows(); ++edge) {
// find get index of element containing boundary edge
int elementIndex = 0, edgeIndex = 0;
(edgeIndices - boundary(edge)).square().minCoeff(&elementIndex, &edgeIndex);
// get index of node not used in edge, but in the triangle
int nodeIndex = 0;
for (int node = 0; node < triangulation.cols(); ++node) {
if ((triangulation(elementIndex, node) != edges(boundary(edge), 0)) &&
(triangulation(elementIndex, node) != edges(boundary(edge), 1))) {
nodeIndex = node;
break;
}
}
// boundary edges with wrong orientation are marked with a negative sign
auto const v1 = (nodes.row(edges(boundary(edge), 1)) - nodes.row(edges(boundary(edge), 0))).eval();
auto const v2 = (nodes.row(triangulation(elementIndex, nodeIndex)) - nodes.row(edges(boundary(edge), 1))).eval();
if (v1(0) * v2(1) - v1(1) * v2(0) < 0.0) {
edges.row(boundary(edge)) = edges.row(boundary(edge)).reverse().eval();
}
}
}
return edges;
}
void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
{
const unsigned int offset = 3 * links_ * chainNum_;
out.setZero();
Eigen::VectorXd plus(3 * (links_ + 1));
plus.head(3 * links_) = x.segment(offset, 3 * links_);
plus.tail(3) = Eigen::VectorXd::Zero(3);
Eigen::VectorXd minus(3 * (links_ + 1));
minus.head(3) = offset_;
minus.tail(3 * links_) = x.segment(offset, 3 * links_);
const Eigen::VectorXd diagonal = plus - minus;
for (unsigned int i = 0; i < links_; i++)
out.row(i).segment(3 * i + offset, 3) = diagonal.segment(3 * i, 3).normalized();
out.block(1, offset, links_ - 1, 3 * links_ - 3) -= out.block(1, offset + 3, links_ - 1, 3 * links_ - 3);
}
void
SetMatRow(SEXP data, const int idx, SEXP value)
{
Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > A = EigenXPtrToMapEigen<T>(data);
A.row(idx-1) = as<Eigen::Matrix<T, Eigen::Dynamic, 1> >(value);
}