bool check_unit_vector(const char* function,
const Eigen::Matrix<T_prob,Eigen::Dynamic,1>& theta,
const char* name,
T_result* result) {
typedef typename Eigen::Matrix<T_prob,Eigen::Dynamic,1>::size_type size_t;
if (theta.size() == 0) {
std::string message(name);
message += " is not a valid unit vector. %1% elements in the vector.";
return dom_err(function,0,name,
message.c_str(),"",
result);
}
T_prob ssq = theta.squaredNorm();
if (fabs(1.0 - ssq) > CONSTRAINT_TOLERANCE) {
std::stringstream msg;
msg << "in function check_unit_vector(%1%), ";
msg << name << " is not a valid unit vector.";
msg << " The sum of the squares of the elements should be 1, but is " << ssq;
std::string tmp(msg.str());
return dom_err(function,ssq,name,
tmp.c_str(),"",
result);
}
return true;
}
void cholesky_downdate (Eigen::Matrix<double, N, N>& L, Eigen::Matrix<double, N, 1> p) {
L.template triangularView<Eigen::Lower>().solveInPlace(p);
assert(p.squaredNorm() < 1); // otherwise the downdate would destroy positive definiteness.
double rho = std::sqrt (1 - p.squaredNorm());
Eigen::JacobiRotation<double> rot;
Eigen::Matrix<double, N, 1> temp;
temp.setZero();
for (int i = N-1; i >= 0; --i) {
rot.makeGivens(rho, p(i), &rho), p(i) = 0;
apply_jacobi_rotation(temp, L.col(i), rot);
}
}
double calc_deviation(
const Eigen::Matrix<_Scalar, _Rows, _Cols>& approx,
const Eigen::Matrix<_Scalar, _Rows, _Cols>& exact )
{
assert( approx.size() == exact.size() );
double exact_square_sum = exact.squaredNorm();
double diff_square_sum = ( approx - exact ).squaredNorm();
return
exact_square_sum == 0.0 ?
sqrt( diff_square_sum ) :
sqrt( diff_square_sum / exact_square_sum );
}
Quaternion<FloatT> exp_r(const Eigen::Matrix<FloatT, 3, 1>& v)
{
// a2 = tan^2(theta/4)
FloatT a2 = v.squaredNorm();
Quaternion<FloatT> ret;
// sin(theta/2) = 2*tan(theta/4) / (1 + tan^2(theta/4))
// v == v.normalized() * tan^2(theta/4)
ret.vec() = v*(2/(1+a2));
// cos(theta/2) = (1 - tan^2(theta/4)) / (1 + tan^2(theta/4))
ret.w() = (1-a2)/(1+a2);
return ret;
}
bool check_unit_vector(const char* function,
const char* name,
const Eigen::Matrix<T_prob, Dynamic, 1>& theta) {
check_nonzero_size(function, name, theta);
T_prob ssq = theta.squaredNorm();
if (!(fabs(1.0 - ssq) <= CONSTRAINT_TOLERANCE)) {
std::stringstream msg;
msg << "is not a valid unit vector."
<< " The sum of the squares of the elements should be 1, but is ";
std::string msg_str(msg.str());
domain_error(function, name, ssq, msg_str.c_str());
}
return true;
}
double GreenStrain_LIMSolver2D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
// green strain energy
double shape = 0;
Eigen::Matrix<double,2,2> I = Eigen::Matrix<double,2,2>::Identity();
for(int t=0;t<mesh->Triangles->rows();t++)
{
Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);
Eigen::Matrix<double,2,3> V;
V.col(0) = A;
V.col(1) = B;
V.col(2) = C;
Eigen::Matrix<double,2,2> F = V*Ms.block<3,2>(0,2*t);
Eigen::Matrix<double,2,2> E = (F.transpose()*F - I);
shape += E.squaredNorm()*Divider;
}
return shape;
}
inline double dot_self(const Eigen::Matrix<double, R, C>& v) {
stan::math::check_vector("dot_self", "v", v);
return v.squaredNorm();
}
Float squared_norm(const Eigen::Matrix<Float,I,J>& mat) { return mat.squaredNorm() ; }