void ParticleSystem::computeForces() { for (size_t i = 0; i < pts.size(); i++) { pts[i].f = /*pts[i].m * */mg + pts[i].fu + airDamping * pts[i].v; } for (size_t i = 0; i < springs.size(); i++) { if (springs[i].p1 == 14 || springs[i].p2 == 14) { Vec3d b(0,0,0); } int p1 = springs[i].p1; int p2 = springs[i].p2; double l0 = springs[i].l0; double ks = springs[i].ks; double kd = springs[i].kd; Vec3d dx = pts[p1].cp - pts[p2].cp; Vec3d dv = pts[p1].v - pts[p2].v; double d = sqrt(dx.ddot(dx)); Vec3d dxdir = dx / d; Vec3d f = -ks * (d - l0) * dxdir + kd * dv.ddot(dxdir) * dxdir; pts[p1].f += f; pts[p2].f -= f; } }
void ParticleSystem::applyProvotDynamicInverse() { for (size_t i = 0; i < springs.size(); i++) { int p1 = springs[i].p1; int p2 = springs[i].p2; Vec3d dx = pts[p1].cp - pts[p2].cp; double d = sqrt(dx.ddot(dx)); dx = dx / d; if (d > springs[i].l0) { d -= springs[i].l0; d /= 2.0; dx *= d; if (pts[p1].isPinned) { pts[p2].v += dx; } else if (pts[p2].isPinned) { pts[p1].v -= dx; } else { pts[p1].v -= dx; pts[p2].v += dx; } } } }
void ParticleSystem::addSpring( int p1, int p2, double ks, double kd, Spring::SpringType t ) { Vec3d dx = pts[p2].cp - pts[p1].cp; double l0 = sqrt(dx.ddot(dx)); springs.push_back(Spring(p1, p2, l0, ks, kd, t)); }
void ParticleSystem::computeForces( LargeVector<Matx33d> &K ) { K.resize(pts.size()); K.clear(); for (size_t i = 0; i < pts.size(); i++) { pts[i].f = pts[i].m * g + pts[i].fu + airDamping * pts[i].v; } for (size_t i = 0; i < springs.size(); i++) { int p1 = springs[i].p1; int p2 = springs[i].p2; double l0 = springs[i].l0; double ks = springs[i].ks; double kd = springs[i].kd; Vec3d dx = pts[p1].cp - pts[p2].cp; Vec3d dv = pts[p1].v - pts[p2].v; double d2 = dx.ddot(dx); double d = sqrt(d2); Vec3d dxdir = dx / d; Matx31d tmpDx = Matx31d(dx); Matx33d tmpK = ks * (-I33d + l0 / d * (I33d - 1 / d2 * tmpDx * tmpDx.t())); K[p1] += tmpK; K[p2] += tmpK; Vec3d f = -ks * (d - l0) * dxdir + kd * dv.ddot(dxdir) * dxdir; pts[p1].f += f; pts[p2].f -= f; } }
void ParticleSystem::constraintSpring( double dt ) { for (int iter = 0; iter < 20; iter++) { for (size_t i = 0; i < springs.size(); i++) { if (springs[i].type == Spring::SPRING_BEND) { continue; } int p1 = springs[i].p1; int p2 = springs[i].p2; double l0 = springs[i].l0; Vec3d dx = pts[p1].cp - pts[p2].cp; Vec3d c = (pts[p1].cp + pts[p2].cp) / 2; double d = sqrt(dx.ddot(dx)); if (d > maxSpringLen * l0) { pts[p1].cp = c + dx * maxSpringLen * l0 / (d * 2); pts[p2].cp = c - dx * maxSpringLen * l0 / (d * 2); } if (d < minSpringLen * l0) { pts[p1].cp = c + dx * minSpringLen * l0 / (d * 2); pts[p2].cp = c - dx * minSpringLen * l0 / (d * 2); } } } // recalculate the velocity for (size_t i = 0; i < pts.size(); i++) { if (pts[i].isPinned) { pts[i].cp = pts[i].op; } pts[i].v = (pts[i].cp - pts[i].op) / dt; } }
void Mesh::inner( int i ) { Vertex v = m_vertices[i]; if (v.m_neighbors[0] == 0) { m_vertices[i].m_isInner = true; return; } int pcnt = 0, ncnt = 0; for (int j = 1; j < v.m_neighbors[0]; j++) { if (m_vertices[v.m_neighbors[j]].m_neighbors[0] == 0) { continue; } Vec3d vec = m_vertices[v.m_neighbors[j]].m_xyz - v.m_xyz; double val = vec.ddot(v.m_normal); if (val > 0) { pcnt++; } else { ncnt++; } } if (ncnt < pcnt) { m_vertices[i].m_normal = -m_vertices[i].m_normal; int tmp = ncnt; ncnt = pcnt; pcnt = tmp; } if (double(pcnt) / double(pcnt + ncnt) > INNER_THRESHOLD) { m_vertices[i].m_isInner = true; return; } }
void Delaunay::computeDelaunay(Mesh &mesh) { int size = (int)mesh.getVerticesSize(); if (size == 0) { return; } mesh.computeVerticesNormals(); m_preSize = mesh.m_curIndex; TriangleSet triSet; // 依次遍历每个点,寻找最近邻,进行三角化 for (; mesh.m_curIndex < size; mesh.m_curIndex++) { Vertex v = mesh.getVertex(mesh.m_curIndex); if (v.m_isInner) { mesh.pushTriBeginIndex((int)triSet.size()); continue; } Vec3d normal = v.m_normal; int id = 2; // 判断法向量哪个不为0,z->y->x if (normal[2] != 0) // z { id = 2; } else if (normal[1] != 0)// y { id = 1; } else if (normal[0] != 0)// x { id = 0; } else // 法向量为(0, 0, 0), { mesh.pushTriBeginIndex((int)triSet.size()); continue; } double minDistance = -1; int cnt = v.m_neighbors[0]; // 最近邻数目 double dists[k]; for (int j = 1; j < cnt + 1; j++) { Vec3d dv = mesh.getVertex(v.m_neighbors[j]).m_xyz - v.m_xyz; dists[j] = dv.ddot(dv); } minDistance = dists[1]; VertexVector vVector, tmpvVector; // 将最近邻点投射到该点的切平面上 for (int j = 1; j < cnt + 1; j++) { Vertex tmpv = mesh.getVertex(v.m_neighbors[j]); if (dists[j] < u * minDistance || // 去除非常接近的点 (tmpv.m_index < v.m_index && tmpv.m_index >= m_preSize) || // 去除已遍历过的点 tmpv.m_isInner) // 去除内点 { continue; } Vec3d vv = tmpv.m_xyz - v.m_xyz; double dist2 = dists[j] * 0.75f; // sqrt double alpha = vv.dot(normal); alpha = alpha * alpha; if (alpha > dist2) // 去除与法向量夹角小于30度或大于150度的点 { continue; } Vec3d proj = tmpv.m_xyz - alpha * normal; // 投射到切平面 tmpvVector.push_back(Vertex(proj, v.m_neighbors[j])); } if (tmpvVector.size() < 3) // 少于3个不能构成三角形 { mesh.pushTriBeginIndex((int)triSet.size()); continue; } // 将切平面转换为x-y平面进行三角形计算 vVector.push_back(Vertex(Vec3d(0, 0, 0), mesh.m_curIndex)); // 原点 Vec3d vx = tmpvVector[0].m_xyz - v.m_xyz; // x轴 vx = normalize(vx); for (int j = 0; j < tmpvVector.size(); j++) { Vec3d vv = tmpvVector[j].m_xyz - v.m_xyz; double x = vv.dot(vx); double y = vx.cross(vv)[id] / normal[id]; Vec3d proj(x, y, 0); vVector.push_back(Vertex(proj, tmpvVector[j].m_index)); } TriangleVector tVector; computeDelaunay(vVector, tVector); // cout << vVector.size() << " " << tVector.size() << endl; // drawTrianglesOnPlane(tVector); for (int j = 0; j < tVector.size(); j++) { Triangle t = tVector[j]; t.m_vertices[0] = mesh.getVertex(t.m_vertices[0].m_index); t.m_vertices[1] = mesh.getVertex(t.m_vertices[1].m_index); t.m_vertices[2] = mesh.getVertex(t.m_vertices[2].m_index); triSet.insert(t); } mesh.pushTriBeginIndex((int)triSet.size()); } for (TriangleSet::iterator iter = triSet.begin(); iter != triSet.end(); iter++) { mesh.m_triangles.push_back(*iter); } }
MatrixXd SolveRobustMatting( const Mat& src, const Mat& trimap, const double sigma=0.1, const double gamma=0.1 ) { const Size sz( src.size() ); const Mat fg( trimap == eFG ), bg( trimap != eBG ); const Mat Known( trimap != eUnknown ), Unknown( trimap == eUnknown ) ; const vector<Point> ctf( GetMaskPoints ( erode(fg)!=fg ) ), ctb( GetMaskPoints ( dilate(bg)!=bg ) ); typedef Triplet<double> Trip; typedef deque< Trip > Trips; Trips triplets; BuildProblem<3>( triplets, src, Known ); double SumWF(0), SumWB(0) ; const int N( sz.area( ) ); MatrixXd Alpha(sz.height,sz.width); # define Push(r,c,v) if(v!=0) triplets.push_back(Trip(r,c,v)) for (int I(0), x(0); x < sz.width; ++x) for( int y(0); y < sz.height; ++y, ++I ) { double &alpha( Alpha.coeffRef(y,x) ), conf(0); if( Unknown.at<uchar>( y, x ) ){ const double sigma( 0.1 ), sigma2( sigma * sigma ); const Point p(x,y); const Vec3i ucolor( src.at<Vec3b>( p ) ) ; const Vec3d dcolor( ucolor ); const vector< Point > fg_samp ( CollectSampleSet(p,ctf) ); const SampleColorWeights_t fg_scw( GetSampleColorWeights( src, p, fg_samp ) ); const vector< Point > bg_samp( CollectSampleSet(p,ctb) ); const SampleColorWeights_t bg_scw( GetSampleColorWeights( src, p, bg_samp ) ); typedef std::pair< double, double > AE; std::vector< AE > ae; ae.reserve( fg_scw.size( ) * bg_scw.size( ) ); for( SampleColorWeights_t::const_iterator f(fg_scw.begin()); f != fg_scw.end( ); ++f ){ for( SampleColorWeights_t::const_iterator b(bg_scw.begin()); b != bg_scw.end( ); ++b ){ const Vec3i usubb( ucolor - b->first ); const Vec3i fsubb( f->first - b->first ); const double ff( fsubb.dot(fsubb) ), ffx( max(ff,1.) ), uf( usubb.dot(fsubb) ), alpha( uf/ffx ); // alpha = (usubb*fsubb')/max(fsubb*fsubb',DMin); const Vec3d difference( dcolor - alpha*Vec3d(f->first) - (1-alpha)*Vec3d(b->first) ); //difference = ucolor - (alpha*fgsampcolors(i,:) + (1-alpha)*bgsampcolors(j,:)); const double distanceratio2( difference.ddot(difference)/ffx ); //(difference*difference')/max(fsubb*fsubb', DMin); const double Exp( exp( -distanceratio2 * f->second * b->second / sigma2 ) ); const double Alpha( max(min( alpha, 1. ), 0. ) ); ae.push_back( std::make_pair( Exp, Alpha ) ); } } enum{ M=3 }; struct LT{ bool operator()( const AE& a, const AE& b ){ return b<a; } }; nth_element( ae.begin( ), ae.begin( )+M, ae.end( ), LT() ); std::accumulate( ae.begin( ), ae.begin( )+M, AE(0,0) ); const AE v( std::accumulate( ae.begin( ), ae.begin( )+M, AE(0,0) ) ); conf=v.first/M; alpha=v.second/M; } else { alpha = fg.at<uchar>(y,x)? 1:0; } const double wf ( conf*alpha + (1-conf)*(alpha>0.5?1:0) ), WF(gamma*wf), WB(gamma*(1-wf)) ; SumWF += WF; SumWB += WB; Push( I, I , gamma ); Push( I, N , -WF ); Push( N, I , -WF ); Push( I, N+1 , -WB ); Push( N+1, I , -WB ); } Push( N, N , SumWF ); Push( N+1, N+1 , SumWB ); # undef Push // const int Total(N+2); typedef std::pair< int, bool > UIF; typedef deque<UIF> UIFS; UIFS unknowns, knowns, * const uk[]={ &unknowns, &knowns }; for( int i=0; i<Total; ++i ){ const uchar t( i<N? trimap.at<uchar>( i%sz.height, i/sz.height ):0 ); const UIF uif( std::make_pair( i, ( i<N? t == eFG : i==N) ) ); uk[ ( i<N && t == eUnknown ) ? 0 : 1 ] -> push_back( uif ); } const int last_unknown( unknowns.size( ) ); VectorXi Ak( knowns.size( ) ), indmap(Total), Indeces(Total) ; transform( knowns.begin( ), knowns.end( ), Ak.data( ), Second( ) ); for( int k(0), j(0); k<2; ++k ){ const UIFS &uifs( *uk[k] ); for( UIFS::const_iterator i( uifs.begin( ) ); i != uifs.end( ); ++i, ++j ){ const UIF uif( *i ); const int index( uif.first ); Indeces.coeffRef(j) = index ; indmap.coeffRef(index) = j ; } } Trips lu, rt; for( Trips::const_iterator i( triplets.begin( ) ); i != triplets.end( ); ++i ){ const int row( indmap[i->row()] ), col( indmap[i->col()] ); const Trip t( indmap[i->row()], indmap[i->col()], i->value() ); if( row < last_unknown ){ if( col<last_unknown ){ lu.push_back( Trip( row, col,i->value( ) ) ); } else { rt.push_back( Trip( row, col-last_unknown,i->value( ) ) ); } } } typedef SparseMatrix<double> SpMat; SpMat Lu(last_unknown,last_unknown); Lu.setFromTriplets( lu.begin( ), lu.end( ) ); SpMat Rt(last_unknown,Total-last_unknown); Rt.setFromTriplets( rt.begin( ), rt.end( ) ); const VectorXd Au( SimplicialCholesky<SpMat>(Lu).solve( -Rt*Ak.cast<double>() ) ); for( int i(0); i< last_unknown; ++i ){ const int ind( Indeces.coeff(i) ); const double auki( Au.coeff( i ) ), alpha( min( 1., max(0., auki) ) ); Alpha.coeffRef( ind % sz.height, ind / sz.height ) = alpha; } return Alpha; }