Eigen::Matrix<double, 3, 3> Filter::unvectorizeMatrix(Eigen::Block<FilterState::StateType, 9, 1> vec) {
Eigen::Matrix<double, 3, 3> mat;
mat.row(0) = vec.block<3, 1>(0, 0);
mat.row(1) = vec.block<3, 1>(3, 0);
mat.row(2) = vec.block<3, 1>(6, 0);
return mat;
}
Eigen::Matrix<double, 9, 1> Filter::vectorizeMatrix(const Eigen::Matrix<double, 3, 3> &mat) {
Eigen::Matrix<double, 9, 1> vec;
vec.block<3, 1>(0, 0) = mat.row(0);
vec.block<3, 1>(3, 0) = mat.row(1);
vec.block<3, 1>(6, 0) = mat.row(2);
return vec;
}
void
factor_U(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& U,
Eigen::Array<T, Eigen::Dynamic, 1>& CPCs) {
size_t K = U.rows();
size_t position = 0;
size_t pull = K - 1;
if (K == 2) {
CPCs(0) = atanh(U(0, 1));
return;
}
Eigen::Array<T, 1, Eigen::Dynamic> temp = U.row(0).tail(pull);
CPCs.head(pull) = temp;
Eigen::Array<T, Eigen::Dynamic, 1> acc(K);
acc(0) = -0.0;
acc.tail(pull) = 1.0 - temp.square();
for (size_t i = 1; i < (K - 1); i++) {
position += pull;
pull--;
temp = U.row(i).tail(pull);
temp /= sqrt(acc.tail(pull) / acc(i));
CPCs.segment(position, pull) = temp;
acc.tail(pull) *= 1.0 - temp.square();
}
CPCs = 0.5 * ( (1.0 + CPCs) / (1.0 - CPCs) ).log(); // now unbounded
}
inline Eigen::Matrix<double, R1, 1>
rows_dot_product(const Eigen::Matrix<double, R1, C1>& v1,
const Eigen::Matrix<double, R2, C2>& v2) {
validate_matching_sizes(v1,v2,"rows_dot_product");
Eigen::Matrix<double, R1, 1> ret(v1.rows(),1);
for (size_type j = 0; j < v1.rows(); ++j) {
ret(j) = v1.row(j).dot(v2.row(j));
}
return ret;
}
void ParticleManager::disperseStateForParticle(int particleNum, Eigen::Matrix< double, Eigen::Dynamic, 1 >& newState, bool fSingleParticle)
{
int currentRow = fSingleParticle? 0 : particleNum * 6;
for (int i = 0; i < m_particles[particleNum]->mPosition.rows(); i++)
{
m_particles[particleNum]->mPosition.row(i) = newState.row(currentRow++);
}
for (int i = 0; i < m_particles[particleNum]->mVelocity.rows(); i++)
{
m_particles[particleNum]->mVelocity.row(i) = newState.row(currentRow++);
}
}
Eigen::Matrix<typename Derived::Scalar, 3, 3> rpy2rotmat(const Eigen::MatrixBase<Derived>& rpy)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
auto rpy_array = rpy.array();
auto s = rpy_array.sin();
auto c = rpy_array.cos();
Eigen::Matrix<typename Derived::Scalar, 3, 3> R;
R.row(0) << c(2) * c(1), c(2) * s(1) * s(0) - s(2) * c(0), c(2) * s(1) * c(0) + s(2) * s(0);
R.row(1) << s(2) * c(1), s(2) * s(1) * s(0) + c(2) * c(0), s(2) * s(1) * c(0) - c(2) * s(0);
R.row(2) << -s(1), c(1) * s(0), c(1) * c(0);
return R;
}
void compute_motor_0w_polynomial_coefficients(Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m0w_, const Eigen::Matrix <
double, N_SEGMENTS + 1, N_MOTORS> motor_interpolations_)
{
// Zero all matrix coefficients.
m0w_ = Eigen::Matrix <double, N_SEGMENTS, N_MOTORS>::Zero(N_SEGMENTS, N_MOTORS);
// Compute 03w for all segments.
for (int sgt = 0; sgt < N_SEGMENTS; ++sgt) {
m0w_.row(sgt) = motor_interpolations_.row(sgt);
}
#if 0
cout << "m0w:\n" << m0w_ << endl;
#endif
}
static typename DerivedF::Scalar add_vertex(const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &values,
const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 3> &points,
unsigned int i0,
unsigned int i1,
PointMatrixType &vertices,
int &num_vertices,
MyMap &edge2vertex)
{
// find vertex if it has been computed already
MyMapIterator it = edge2vertex.find(EdgeKey(i0, i1));
if (it != edge2vertex.end())
return it->second;
;
// generate new vertex
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> & p0 = points.row(i0);
const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> & p1 = points.row(i1);
typename DerivedV::Scalar s0 = fabs(values[i0]);
typename DerivedV::Scalar s1 = fabs(values[i1]);
typename DerivedV::Scalar t = s0 / (s0+s1);
num_vertices++;
if (num_vertices > vertices.rows())
vertices.conservativeResize(vertices.rows()+10000, Eigen::NoChange);
vertices.row(num_vertices-1) = (1.0f-t)*p0 + t*p1;
edge2vertex[EdgeKey(i0, i1)] = num_vertices-1;
return num_vertices-1;
}
std::shared_ptr<Geometry> generateTexQuadGeometry(
float width, float height, Eigen::Vector3f pos, Eigen::Matrix3f rot) {
Eigen::Matrix<float, 6, 3 + 2, Eigen::RowMajor> vertex;
GLfloat vertex_pos_uv[] = {
-1.0f, 0, -1.0f, 0, 1,
1.0f, 0, 1.0f, 1, 0,
-1.0f, 0, 1.0f, 0, 0,
1.0f, 0, 1.0f, 1, 0,
-1.0f, 0, -1.0f, 0, 1,
1.0f, 0, -1.0f, 1, 1,
};
for(int i = 0; i < 6; i++) {
Eigen::Vector3f p_local(
vertex_pos_uv[i * 5 + 0] * width / 2,
vertex_pos_uv[i * 5 + 1],
vertex_pos_uv[i * 5 + 2] * height / 2);
vertex.row(i).head(3) = rot * p_local + pos;
vertex.row(i).tail(2) = Eigen::Vector2f(
vertex_pos_uv[i * 5 + 3], vertex_pos_uv[i * 5 + 4]);
}
return Geometry::createPosUV(vertex.rows(), vertex.data());
}
Eigen::Matrix<typename Derived::Scalar, 3, 3> quat2rotmat(const Eigen::MatrixBase<Derived>& q)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
auto q_normalized = q.normalized();
auto w = q_normalized(0);
auto x = q_normalized(1);
auto y = q_normalized(2);
auto z = q_normalized(3);
Eigen::Matrix<typename Derived::Scalar, 3, 3> M;
M.row(0) << w * w + x * x - y * y - z * z, 2.0 * x * y - 2.0 * w * z, 2.0 * x * z + 2.0 * w * y;
M.row(1) << 2.0 * x * y + 2.0 * w * z, w * w + y * y - x * x - z * z, 2.0 * y * z - 2.0 * w * x;
M.row(2) << 2.0 * x * z - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, w * w + z * z - x * x - y * y;
return M;
}
IGL_INLINE void igl::mat_min(
const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & X,
const int dim,
Eigen::Matrix<T,Eigen::Dynamic,1> & Y,
Eigen::Matrix<int,Eigen::Dynamic,1> & I)
{
assert(dim==1||dim==2);
// output size
int n = (dim==1?X.cols():X.rows());
// resize output
Y.resize(n);
I.resize(n);
// loop over dimension opposite of dim
for(int j = 0;j<n;j++)
{
typename Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Index PHONY;
typename Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Index i;
T m;
if(dim==1)
{
m = X.col(j).minCoeff(&i,&PHONY);
}else
{
m = X.row(j).minCoeff(&PHONY,&i);
}
Y(j) = m;
I(j) = i;
}
}
void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
const int numNodes = mesh->InitalVertices->rows();
// Compute deformation gradients
int numTets = mesh->Tetrahedra->rows();
Ms.resize(4,3*numTets);
MMTs.resize(4,4*numTets);
Eigen::Matrix<double,4,3> SelectorM;
SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
for(int t=0;t<numTets;t++)
{
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();
Eigen::Matrix3d V;
V << A-D,B-D,C-D;
Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
Ms.block<4,3>(0,3*t) = Mtemp;
MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
}
}
void GreenStrain_LIMSolver2D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
// Compute deformation gradients
int numTets = mesh->Triangles->rows();
Ms.resize(3,2*numTets);
MMTs.resize(3,3*numTets);
Eigen::Matrix<double,3,2> SelectorM;
SelectorM.block<2,2>(0,0) = Eigen::Matrix<double,2,2>::Identity();
SelectorM.row(2) = Eigen::Vector2d::Ones()*-1;
for(int t=0;t<numTets;t++)
{
Eigen::Vector2d A,B,C;
if(mesh->IsCorotatedTriangles)
{
A = mesh->CorotatedTriangles->row(t).block<1,2>(0,0).cast<double>();
B = mesh->CorotatedTriangles->row(t).block<1,2>(0,2).cast<double>();
C = mesh->CorotatedTriangles->row(t).block<1,2>(0,4).cast<double>();
}
else
{
A = mesh->InitalVertices->row(mesh->Triangles->coeff(t,0)).block<1,2>(0,0).cast<double>();
B = mesh->InitalVertices->row(mesh->Triangles->coeff(t,1)).block<1,2>(0,0).cast<double>();
C = mesh->InitalVertices->row(mesh->Triangles->coeff(t,2)).block<1,2>(0,0).cast<double>();
}
Eigen::Matrix2d V;
V << A-C,B-C;
Eigen::Matrix<double,3,2> Mtemp = SelectorM*V.inverse().cast<double>();
Ms.block<3,2>(0,2*t) = Mtemp;
MMTs.block<3,3>(0,3*t) = Mtemp*Mtemp.transpose();
}
}
inline
Eigen::Matrix<T, 1, Eigen::Dynamic>
row(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& m,
size_t i) {
stan::math::check_row_index("row", "j", m, i);
return m.row(i - 1);
}
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
//ds A matrix for system: A*X=0
Eigen::Matrix4d matAHomogeneous;
matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);
//ds solve system subject to ||A*x||=0 ||x||=1
const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );
//ds solution x is the last column of V
const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );
return vecX.head( 3 )/vecX(3);
}
inline Eigen::Matrix<fvar<T>,R,1>
rows_dot_self(const Eigen::Matrix<fvar<T>,R,C>& x) {
Eigen::Matrix<fvar<T>,R,1> ret(x.rows(),1);
for (size_type i = 0; i < x.rows(); i++) {
Eigen::Matrix<fvar<T>,1,C> crow = x.row(i);
ret(i,0) = dot_self(crow);
}
return ret;
}
void NuTo::CollidableWallBase::VisualizationStatic(NuTo::Visualize::UnstructuredGrid& rVisualizer) const
{
double size = 1.;
if (*mBoxes.begin() == mOutsideBox)
size = 2.;
Eigen::Matrix<double, 4, 3> corners;
// get some vector != mDirection
Eigen::Vector3d random;
random << 1, 0, 0;
if (std::abs(random.dot(mDirection)) == 1)
{
random << 0, 1, 0;
}
Eigen::Vector3d transversal = random.cross(mDirection);
Eigen::Vector3d transversal2 = transversal.cross(mDirection);
// normalize to size/2;
transversal.normalize();
transversal2.normalize();
transversal *= size / 2;
transversal2 *= size / 2;
corners.row(0) = (mPosition + transversal + transversal2).transpose();
corners.row(1) = (mPosition + transversal - transversal2).transpose();
corners.row(2) = (mPosition - transversal - transversal2).transpose();
corners.row(3) = (mPosition - transversal + transversal2).transpose();
std::vector<int> cornerIndex(4);
for (int i = 0; i < 4; ++i)
{
cornerIndex[i] = rVisualizer.AddPoint(corners.row(i));
}
int insertIndex = rVisualizer.AddCell(cornerIndex, eCellTypes::QUAD);
rVisualizer.SetCellData(insertIndex, "Direction", mDirection);
}
IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F,
const Eigen::Matrix<T, Eigen::Dynamic, 1>&X,
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G )
{
G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3);
for (int i = 0; i<F.rows(); ++i)
{
// renaming indices of vertices of triangles for convenience
int i1 = F(i,0);
int i2 = F(i,1);
int i3 = F(i,2);
// #F x 3 matrices of triangle edge vectors, named after opposite vertices
Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2);
Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3);
Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1);
// area of parallelogram is twice area of triangle
// area of parallelogram is || v1 x v2 ||
Eigen::Matrix<T, 1, 3> n = v32.cross(v13);
// This does correct l2 norm of rows, so that it contains #F list of twice
// triangle areas
double dblA = std::sqrt(n.dot(n));
// now normalize normals to get unit normals
Eigen::Matrix<T, 1, 3> u = n / dblA;
// rotate each vector 90 degrees around normal
double norm21 = std::sqrt(v21.dot(v21));
double norm13 = std::sqrt(v13.dot(v13));
Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21);
eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21));
eperp21 *= norm21;
Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13);
eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13));
eperp13 *= norm13;
G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA;
};
}
void ParticleManager::accumulateDerivativeForParticle(int particleNum, Eigen::Matrix< double, Eigen::Dynamic, 1 >& derivative, bool fVelocity, bool fAcceleration, bool fSingleParticle)
{
assert( fVelocity == true || fAcceleration == true );
int stateSize = (fVelocity & fAcceleration) ? 6 : 3;
int currentRow = fSingleParticle? 0 : particleNum * stateSize;
if ( fVelocity )
{
for (int i = 0; i < m_particles[particleNum]->mVelocity.rows(); i++)
{
derivative.row(currentRow++) = m_particles[particleNum]->mVelocity.row(i);
}
}
if ( fAcceleration )
{
for (int i = 0; i < m_particles[particleNum]->mAccumulatedForce.rows(); i++)
{
derivative.row(currentRow++) = (m_particles[particleNum]->mAccumulatedForce.row(i)/m_particles[particleNum]->mMass);
}
}
}
void ParticleManager::accumulateStateForParticle(int particleNum, Eigen::Matrix< double, Eigen::Dynamic, 1>& currentState, bool fPosition, bool fVelocity, bool fSingleParticle)
{
assert( fPosition == true || fVelocity == true );
int stateSize = (fPosition & fVelocity) ? 6 : 3;
int currentRow = fSingleParticle? 0 : particleNum * stateSize;
if ( fPosition )
{
for (int i = 0; i < m_particles[particleNum]->mPosition.rows(); i++)
{
currentState.row(currentRow++) = m_particles[particleNum]->mPosition.row(i);
}
}
if ( fVelocity )
{
for (int i = 0; i < m_particles[particleNum]->mVelocity.rows(); i++)
{
currentState.row(currentRow++) = m_particles[particleNum]->mVelocity.row(i);
}
}
}
void compute_motor_3w_polynomial_coefficients(Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m3w_, const Eigen::Matrix <
double, N_SEGMENTS, N_MOTORS> m2w_, const Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> motor_deltas_, const Eigen::Matrix <
double, N_SEGMENTS, 1> taus_)
{
// Zero all matrix coefficients.
m3w_ = Eigen::Matrix <double, N_SEGMENTS, N_MOTORS>::Zero(N_SEGMENTS, N_MOTORS);
// Compute 3w for last segment.
m3w_.row(N_SEGMENTS - 1) = -m2w_.row(N_SEGMENTS - 1) / (taus_(N_SEGMENTS - 1) * 2.0) - motor_deltas_.row(N_SEGMENTS
- 1) / (taus_(N_SEGMENTS - 1) * taus_(N_SEGMENTS - 1) * taus_(N_SEGMENTS - 1) * 2.0);
// Compute 3w for other segments.
for (int sgt = 0; sgt < N_SEGMENTS - 1; ++sgt) {
m3w_.row(sgt) = (m2w_.row(sgt + 1) - m2w_.row(sgt)) / (taus_(sgt) * 3.0);
}
#if 0
cout << "3w:\n" << m3w_ << endl;
#endif
}
IGL_INLINE void igl::average_onto_vertices(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V,
const Eigen::Matrix<I, Eigen::Dynamic, Eigen::Dynamic> &F,
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &S,
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &SV)
{
SV = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(V.rows(),S.cols());
Eigen::Matrix<T, Eigen::Dynamic, 1> COUNT = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(V.rows());
for (int i = 0; i <F.rows(); ++i)
{
for (int j = 0; j<F.cols(); ++j)
{
SV.row(F(i,j)) += S.row(i);
COUNT[F(i,j)] ++;
}
}
for (int i = 0; i <V.rows(); ++i)
SV.row(i) /= COUNT[i];
};
Eigen::Matrix<double,3,3>
Triangle::localStiffMatrix() const{
Eigen::Matrix<double,3,2> n;
Triangle const & t(*this);
double meas2=1/(2.0*t.measure());
for (int i=0;i<3;++i){
int j = (i+1)% 3;
int k = (j+1) % 3;
n.row(i)<< (-t[k][1]+t[j][1]),(t[k][0]-t[j][0]);
}
n*=meas2;
double off01=n.row(0).dot(n.row(1));
double off02=n.row(0).dot(n.row(2));
double off12=n.row(1).dot(n.row(2));
Eigen::Matrix<double,3,3> s;
s<<-(off01+off02),off01,off02,
off01,-(off01+off12),off12,
off02,off12,-(off02+off12);
return s;
}
//TODO msati: Optimize this portion
void ParticleManager::getInvMassMatrix( Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& invMassMatrix )
{
invMassMatrix.resize( m_particles.size() * 3, m_particles.size() * 3 );
invMassMatrix.setZero();
for (int i = 0; i < m_particles.size(); i++)
{
for (int j = 0; j < 3; j++)
{
invMassMatrix.row(3*i+j).array()[3*i+j] = 1/(m_particles[i]->mMass);
}
}
}
typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
multi_gp_cholesky_log(const Eigen::Matrix
<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
const Eigen::Matrix
<T_covar, Eigen::Dynamic, Eigen::Dynamic>& L,
const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) {
static const char* function("multi_gp_cholesky_log");
typedef
typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type T_lp;
T_lp lp(0.0);
check_size_match(function,
"Size of random variable (rows y)", y.rows(),
"Size of kernel scales (w)", w.size());
check_size_match(function,
"Size of random variable", y.cols(),
"rows of covariance parameter", L.rows());
check_finite(function, "Kernel scales", w);
check_positive(function, "Kernel scales", w);
check_finite(function, "Random variable", y);
if (y.rows() == 0)
return lp;
if (include_summand<propto>::value) {
lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
}
if (include_summand<propto, T_covar>::value) {
lp -= L.diagonal().array().log().sum() * y.rows();
}
if (include_summand<propto, T_w>::value) {
lp += 0.5 * y.cols() * sum(log(w));
}
if (include_summand<propto, T_y, T_w, T_covar>::value) {
T_lp sum_lp_vec(0.0);
for (int i = 0; i < y.rows(); i++) {
Eigen::Matrix<T_y, Eigen::Dynamic, 1> y_row(y.row(i));
Eigen::Matrix<typename boost::math::tools::promote_args
<T_y, T_covar>::type,
Eigen::Dynamic, 1>
half(mdivide_left_tri_low(L, y_row));
sum_lp_vec += w(i) * dot_self(half);
}
lp -= 0.5*sum_lp_vec;
}
return lp;
}
IGL_INLINE Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> barycentric_to_global(
const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> & V,
const Eigen::Matrix<Index,Eigen::Dynamic,Eigen::Dynamic> & F,
const Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> & bc)
{
Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> R;
R.resize(bc.rows(),3);
for (unsigned i=0; i<R.rows(); ++i)
{
unsigned id = round(bc(i,0));
double u = bc(i,1);
double v = bc(i,2);
if (id != -1)
R.row(i) = V.row(F(id,0)) +
((V.row(F(id,1)) - V.row(F(id,0))) * u +
(V.row(F(id,2)) - V.row(F(id,0))) * v );
else
R.row(i) << 0,0,0;
}
return R;
}
bool
check_cholesky_factor_corr(const char* function,
const char* name,
const Eigen::Matrix<T_y, Dynamic, Dynamic>& y) {
check_square(function, name, y);
check_lower_triangular(function, name, y);
for (int i = 0; i < y.rows(); ++i)
check_positive(function, name, y(i, i));
for (int i = 0; i < y.rows(); ++i) {
Eigen::Matrix<T_y, Dynamic, 1>
y_i = y.row(i).transpose();
check_unit_vector(function, name, y_i);
}
return true;
}
inline
Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R1,C2>
multiply(const Eigen::Matrix<fvar<T1>,R1,C1>& m1,
const Eigen::Matrix<T2,R2,C2>& m2) {
stan::math::validate_multiplicable(m1,m2,"multiply");
Eigen::Matrix<fvar<typename stan::return_type<T1,T2>::type>,R1,C2> result(m1.rows(),m2.cols());
for (size_type i = 0; i < m1.rows(); i++) {
Eigen::Matrix<fvar<T1>,1,C1> crow = m1.row(i);
for (size_type j = 0; j < m2.cols(); j++) {
Eigen::Matrix<T2,R2,1> ccol = m2.col(j);
result(i,j) = stan::agrad::dot_product(crow,ccol);
}
}
return result;
}
inline
Eigen::Matrix<fvar<T>,R1,C2>
multiply(const Eigen::Matrix<fvar<T>,R1,C1>& m1,
const Eigen::Matrix<double,R2,C2>& m2) {
stan::math::check_multiplicable("multiply(%1%)",m1,"m1",
m2,"m2",(double*)0);
Eigen::Matrix<fvar<T>,R1,C2> result(m1.rows(),m2.cols());
for (size_type i = 0; i < m1.rows(); i++) {
Eigen::Matrix<fvar<T>,1,C1> crow = m1.row(i);
for (size_type j = 0; j < m2.cols(); j++) {
Eigen::Matrix<double,R2,1> ccol = m2.col(j);
result(i,j) = stan::agrad::dot_product(crow,ccol);
}
}
return result;
}
void compute_motor_deltas_for_segments(Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & motor_deltas_for_segments_, const Eigen::Matrix <
double, N_SEGMENTS + 1, N_MOTORS> motor_interpolations_)
{
for (int segment = 0; segment < N_SEGMENTS; ++segment) {
for (int axis = 0; axis < N_MOTORS; ++axis) {
motor_deltas_for_segments_(segment, axis) = motor_interpolations_(segment + 1, axis)
- motor_interpolations_(segment, axis);
}
}
#if 0
// Display all motor increments.
for (unsigned int l = 0; l < N_SEGMENTS; ++l) {
cout << "Motor increments for segment " << l << ": " << motor_deltas_for_segments_.row(l) << endl;
}
#endif
}