/**
* Resizes a dynamic-size PointSet
*
* \param Points_ Number of points
*
* \throws ResizingFixedSizeEntityException
*/
void resize(int points_count)
{
if (int(weights_.size()) == points_count &&
int(points_.cols()) == points_count) return;
if (IsFixed<Points_>())
{
fl_throw(
fl::ResizingFixedSizeEntityException(weights_.size(),
points_count,
"poit set Gaussian"));
}
points_.setZero(points_.rows(), points_count);
weights_.resize(points_count, 1);
const double weight = (points_count > 0)? 1. / double(points_count) : 0;
for (int i = 0; i < points_count; ++i)
{
weights_(i).w_mean = weight;
weights_(i).w_cov = weight;
}
// std::fill(weights_.begin(), weights_.end(), Weight{weight, weight});
}
void IMesh::computeLaplace(int t)
{
int N = eff_size_;
dist.resize(N, N);
dist.setZero();
wsum.resize(N);
wsum.setZero();
int j, l;
double w;
/** Compute distance matrix (inverse proportional) */
for (j = 0; j < N; j++)
{
for (l = j + 1; l < N; l++)
{
if (!(j >= N && l >= N))
{
dist(j, l) = dist(l, j) = sqrt(
(EFFPHI.segment(j, 3) - EFFPHI.segment(l, 3)).dot(
( EFFPHI.segment(j, 3) - EFFPHI.segment(l, 3))));
}
}
}
/** Computer weight normaliser */
for (j = 0; j < N; j++)
{
for (l = 0; l < N; l++)
{
if (dist(j, l) > 0 && j != l)
{
wsum(j) += weights_(j, l) / dist(j, l);
}
}
}
/** Compute Laplace coordinates */
for (j = 0; j < N; j++)
{
PHI.segment(j, 3) = EFFPHI.segment(j, 3);
for (l = 0; l < N; l++)
{
if (j != l)
{
if (dist(j, l) > 0 && wsum(j) > 0)
{
w = weights_(j, l) / (dist(j, l) * wsum(j));
PHI.segment(j, 3) -= EFFPHI.segment(l, 3) * w;
}
}
}
}
}
/**
* Resizes a dynamic-size PointSet
*
* \param Points_ Number of points
*
* \throws ResizingFixedSizeEntityException
*/
void resize(int dim, int points_count)
{
if (dim == dimension() && points_count == count_points()) return;
points_.setZero(dim, points_count);
weights_.resize(points_count, 1);
const double weight = (points_count > 0)? 1. / double(points_count) : 0;
for (int i = 0; i < points_count; ++i)
{
weights_(i).w_mean = weight;
weights_(i).w_cov = weight;
}
}
/**
* Sets a given point at given position i along with its weights
*
* \param i Index of point
* \param p The new point
* \param weights point weights
*
* \throws OutOfBoundsException
* \throws ZeroDimensionException
*/
void point(int i, Point p, Weight weights)
{
INLINE_CHECK_POINT_SET_BOUNDS(i);
points_.col(i) = p;
weights_(i) = weights;
}
/**
* \return Returns the weights for the covariance of the points as a vector
*/
WeightVector covariance_weights_vector() const noexcept
{
const int point_count = count_points();
WeightVector weight_vec(point_count);
for (int i = 0; i < point_count; ++i)
{
weight_vec(i) = weights_(i).w_cov;
}
return weight_vec;
}
void IMesh::setWeight(int i, int j, double weight)
{
uint M = weights_.cols();
if (i < 0 || i >= M || j < 0 || j >= M)
{
throw_named("Invalid weight element (" << i << "," << j
<< "). Weight matrix " << M << "x" << M );
}
if (weight < 0)
{
throw_named("Invalid weight: " << weight );
}
weights_(i, j) = weight;
}
void
ConstitutiveModel<EvalT, Traits>::computeVolumeAverage(
Workset workset,
DepFieldMap dep_fields,
FieldMap eval_fields)
{
int const& num_dims = this->num_dims_;
int const& num_pts = this->num_pts_;
std::string cauchy = (*this->field_name_map_)["Cauchy_Stress"];
PHX::MDField<ScalarT> stress = *eval_fields[cauchy];
minitensor::Tensor<ScalarT> sig(num_dims);
minitensor::Tensor<ScalarT> I(minitensor::eye<ScalarT>(num_dims));
ScalarT volume, pbar, p;
for (int cell(0); cell < workset.numCells; ++cell) {
volume = pbar = 0.0;
for (int pt(0); pt < num_pts; ++pt) {
sig.fill(stress, cell, pt, 0, 0);
pbar += weights_(cell, pt) * (1. / num_dims) * minitensor::trace(sig);
volume += weights_(cell, pt);
}
pbar /= volume;
for (int pt(0); pt < num_pts; ++pt) {
sig.fill(stress, cell, pt, 0, 0);
p = (1. / num_dims) * minitensor::trace(sig);
sig += (pbar - p) * I;
for (int i = 0; i < num_dims; ++i) { stress(cell, pt, i, i) = sig(i, i); }
}
}
}
/**
* \return The weighted mean of all points
*
* \throws ZeroDimensionException
* \throws Exception
*/
Point mean() const
{
INLINE_CHECK_POINT_SET_DIMENSIONS();
Point weighted_mean;
weighted_mean.setZero(points_.rows());
const int point_count = points_.cols();
for (int i = 0; i < point_count; ++i)
{
weighted_mean += weights_(i).w_mean * points_.col(i);
}
return weighted_mean;
}
void ConvolutionLayer::build_tree(map<string, BoundedFunc>* func_tree) {
LOG(INFO) << "Starting to build " << this->name() << endl;
CHECK(init()) << "Cannot build tree because " << this->name()
<< " could not be initialized." << endl;
map<string, BoundedFunc>::iterator input_it =
func_tree->find(layer_param_.bottom(0));
CHECK(input_it != func_tree->end()) << "Could not find " <<
layer_param_.bottom(0) << " in the function tree" << endl;
Func& input_func = (input_it->second).first;
array<int, 4> dims = (input_it->second).second;
Func conv(layer_param_.top(0));
RDom r(0, weights_.extent(0), 0, weights_.extent(1), 0, weights_.extent(2));
conv(x, y, c, n) = sum(weights_(weights_.extent(0)-1-r.x,
weights_.extent(1)-1-r.y, r.z, c) *
input_func(x+weights_.extent(0)-1-r.x,
y+weights_.extent(1)-1-r.y, r.z, n));
if(layer_param_.convolution_param().bias_term())
conv(x, y, c, n) += bias_(c, 0, 0, 0);
dims[2] = layer_param_.blobs(0).num();
const ConvolutionParameter& conv_param = layer_param_.convolution_param();
dims[1] = (dims[1] + 2*conv_param.pad() - layer_param_.blobs(0).height())/
conv_param.stride() + 1;
dims[0] = (dims[0] + 2*conv_param.pad() - layer_param_.blobs(0).width())/
conv_param.stride() + 1;
conv.compute_root().vectorize(x, 8).parallel(n);
func_tree->insert(make_pair(layer_param_.top(0),
make_pair(conv, dims)));
LOG(INFO) << layer_param_.top(0) << ": {" << dims[0] << ", "
<< dims[1] << ", " << dims[2] << ", " << dims[3] << "}" << endl;
LOG(INFO) << "Completed building " << this->name() << endl;
}
void
Kinematics<EvalT, Traits>::evaluateFields(typename Traits::EvalData workset)
{
minitensor::Tensor<ScalarT> F(num_dims_), strain(num_dims_), gradu(num_dims_);
minitensor::Tensor<ScalarT> I(minitensor::eye<ScalarT>(num_dims_));
// Compute DefGrad tensor from displacement gradient
if (!def_grad_rc_) {
for (int cell(0); cell < workset.numCells; ++cell) {
for (int pt(0); pt < num_pts_; ++pt) {
gradu.fill(grad_u_, cell, pt, 0, 0);
F = I + gradu;
j_(cell, pt) = minitensor::det(F);
for (int i(0); i < num_dims_; ++i) {
for (int j(0); j < num_dims_; ++j) {
def_grad_(cell, pt, i, j) = F(i, j);
}
}
}
}
} else {
bool first = true;
for (int cell = 0; cell < workset.numCells; ++cell) {
for (int pt = 0; pt < num_pts_; ++pt) {
gradu.fill(grad_u_, cell, pt, 0, 0);
F = I + gradu;
for (int i = 0; i < num_dims_; ++i)
for (int j = 0; j < num_dims_; ++j)
def_grad_(cell, pt, i, j) = F(i, j);
if (first && check_det(workset, cell, pt)) first = false;
// F[n,0] = F[n,n-1] F[n-1,0].
def_grad_rc_.multiplyInto<ScalarT>(def_grad_, cell, pt);
F.fill(def_grad_, cell, pt, 0, 0);
j_(cell, pt) = minitensor::det(F);
}
}
}
if (weighted_average_) {
ScalarT jbar, weighted_jbar, volume;
for (int cell(0); cell < workset.numCells; ++cell) {
jbar = 0.0;
volume = 0.0;
for (int pt(0); pt < num_pts_; ++pt) {
jbar += weights_(cell, pt) * j_(cell, pt);
volume += weights_(cell, pt);
}
jbar /= volume;
for (int pt(0); pt < num_pts_; ++pt) {
weighted_jbar = (1 - alpha_) * jbar + alpha_ * j_(cell, pt);
F.fill(def_grad_, cell, pt, 0, 0);
const ScalarT p = std::pow((weighted_jbar / j_(cell, pt)), 1. / 3.);
F *= p;
j_(cell, pt) = weighted_jbar;
for (int i(0); i < num_dims_; ++i) {
for (int j(0); j < num_dims_; ++j) {
def_grad_(cell, pt, i, j) = F(i, j);
}
}
}
}
}
if (needs_strain_) {
if (!def_grad_rc_) {
for (int cell(0); cell < workset.numCells; ++cell) {
for (int pt(0); pt < num_pts_; ++pt) {
gradu.fill(grad_u_, cell, pt, 0, 0);
strain = 0.5 * (gradu + minitensor::transpose(gradu));
for (int i(0); i < num_dims_; ++i) {
for (int j(0); j < num_dims_; ++j) {
strain_(cell, pt, i, j) = strain(i, j);
}
}
}
}
} else {
for (int cell = 0; cell < workset.numCells; ++cell) {
for (int pt = 0; pt < num_pts_; ++pt) {
F.fill(def_grad_, cell, pt, 0, 0);
gradu = F - I;
// dU/dx[0] = dx[n]/dx[0] - dx[0]/dx[0] = F[n,0] - I.
// strain = 1/2 (dU/dx[0] + dU/dx[0]^T).
strain = 0.5 * (gradu + minitensor::transpose(gradu));
for (int i = 0; i < num_dims_; ++i)
for (int j = 0; j < num_dims_; ++j)
strain_(cell, pt, i, j) = strain(i, j);
}
}
}
}
}
void IMesh::computeIMesh(int t)
{
int M = eff_size_;
computeLaplace(t);
if (updateJacobian_)
{
double A, _A, Sk, Sl, w, _w;
int i, j, k, l, N;
N = EFFJAC.cols();
Eigen::Vector3d distance, _distance = Eigen::Vector3d::Zero(3, 1);
for (i = 0; i < N; i++)
{
for (j = 0; j < M; j++)
{
if (j < M)
JAC.block(3 * j, i, 3, 1) = EFFJAC.block(3 * j, i, 3, 1);
for (l = 0; l < M; l++)
{
if (j != l)
{
if (dist(j, l) > 0 && wsum(j) > 0 && weights_(j, l) > 0)
{
A = dist(j, l) * wsum(j);
w = weights_(j, l) / A;
_A = 0;
distance = EFFPHI.segment(j, 3) - EFFPHI.segment(l, 3);
if (j < M)
{
if (l < M)
//Both j and l are points on the robot
_distance = EFFJAC.block(3 * j, i, 3, 1)
- EFFJAC.block(3 * l, i, 3, 1);
else
//l is not on the robot
_distance = EFFJAC.block(3 * j, i, 3, 1);
}
else
{
if (l < M)
//j is not on the robot
_distance = -EFFJAC.block(3 * l, i, 3, 1);
}
Sl = distance.dot(_distance) / dist(j, l);
for (k = 0; k < M; k++)
{
if (j != k && dist(j, k) > 0 && weights_(j, k) > 0)
{
distance = EFFPHI.segment(j, 3) - EFFPHI.segment(k, 3);
if (j < M)
{
if (k < M)
_distance = EFFJAC.block(3 * j, i, 3, 1)
- EFFJAC.block(3 * k, i, 3, 1);
else
_distance = EFFJAC.block(3 * j, i, 3, 1);
}
else
{
if (k < M) _distance = -EFFJAC.block(3 * k, i, 3, 1);
}
Sk = distance.dot(_distance) / dist(j, k);
_A += weights_(j, k) * (Sl * dist(j, k) - Sk * dist(j, l))
/ (dist(j, k) * dist(j, k));
}
}
_w = -weights_(j, l) * _A / (A * A);
}
else
{
_w = 0;
w = 0;
}
if (l < M)
JAC.block(3 * j, i, 3, 1) -= EFFPHI.segment(l, 3) * _w
+ EFFJAC.block(3 * l, i, 3, 1) * w;
else
JAC.block(3 * j, i, 3, 1) -= EFFPHI.segment(l, 3) * _w;
}
}
}
}
}
}
/**
* Sets given weights of a point at position i
*
* \param i Index of point
* \param weights point weights
*
* \throws OutOfBoundsException
* \throws ZeroDimensionException
*/
void weight(int i, Weight weights)
{
INLINE_CHECK_POINT_SET_BOUNDS(i);
weights_(i) = weights;
}
/**
* \return weights of i-th point
*
* \param i Index of requested point
*
* \throws OutOfBoundsException
* \throws ZeroDimensionException
*/
const Weight& weights(int i) const
{
INLINE_CHECK_POINT_SET_BOUNDS(i);
return weights_(i);
}
/**
* \return weight of i-th point assuming both weights are the same
*
* \param i Index of requested point
*
* \throws OutOfBoundsException
* \throws ZeroDimensionException
*/
double weight(int i)
{
INLINE_CHECK_POINT_SET_BOUNDS(i);
return weights_(i).w_mean;
}
