template<> void CubicHinges<TV3>::add_elastic_differential(RawArray<TV> dF, RawArray<const TV> dX) const {
GEODE_ASSERT(dF.size()>=nodes_ && dX.size()>=nodes_);
if (simple_hessian) // In the simple case, the force is linear and the differential is easy
add_force_helper<true>(bends,info,stiffness,dF,dX);
else { // Otherwise, we need custom code
GEODE_ASSERT(dF.size()>=nodes_ && dX.size()>=nodes_);
const T scale = stiffness;
if (!scale) return;
RawArray<const TV> X = this->X;
for (int b=0;b<bends.size();b++) {
const auto& I = info[b];
int i0,i1,i2,i3;bends[b].get(i0,i1,i2,i3);
const TV x0 = X[i0], x1 = X[i1], x2 = X[i2], x3 = X[i3];
const TV dx0 = dX[i0], dx1 = dX[i1], dx2 = dX[i2], dx3 = dX[i3];
const TV dstress = scale*I.dot*(I.c[0]*dx0+I.c[1]*dx1+I.c[2]*dx2+I.c[3]*dx3);
const T cubic = scale*I.det;
const TV ce0 = cubic*(x2-x1),
e1 = x3-x1,
e2 = x0-x1,
dce0 = cubic*(dx2-dx1),
de1 = dx3-dx1,
de2 = dx0-dx1,
dcross01 = cross(ce0,de1)+cross(dce0,e1),
dcross12 = cubic*(cross(e1,de2)+cross(de1,e2)),
dcross20 = cross(e2,dce0)+cross(de2,ce0);
dF[i0] -= I.c[0]*dstress+dcross01;
dF[i1] -= I.c[1]*dstress-dcross01-dcross12-dcross20;
dF[i2] -= I.c[2]*dstress+dcross12;
dF[i3] -= I.c[3]*dstress+dcross20;
}
}
}
GEODE_UNUSED static void check_bsp(const TriangleTopology& mesh, RawArray<const Node> bsp, RawField<const Vector<int,2>,FaceId> face_to_bsp, RawField<const Perturbed2,VertexId> X_) {
if (self_check) {
cout << "bsp:\n";
#define CHILD(c) format("%c%d",(c<0?'f':'n'),(c<0?~c:c))
for (const int n : range(bsp.size()))
cout << " "<<n<<" : test "<<bsp[n].test<<", children "<<CHILD(bsp[n].children[0])<<" "<<CHILD(bsp[n].children[1])<<endl;
cout << "tris = "<<mesh.elements()<<endl;
cout << "X = "<<X_.flat<<endl;
}
for (const int n : range(bsp.size()))
for (const int i : range(2))
if (bsp[n].children[i]<0)
GEODE_ASSERT(face_to_bsp[FaceId(~bsp[n].children[i])].contains(2*n+i));
auto X = X_.copy();
for (const auto f : mesh.faces()) {
int i0,i1;
face_to_bsp[f].get(i0,i1);
if (bsp.size())
GEODE_ASSERT(bsp[i0/2].children[i0&1]==~f.id);
if (i1>=0)
GEODE_ASSERT(bsp[i1/2].children[i1&1]==~f.id);
const auto v = mesh.vertices(f);
if (!is_sentinel(X[v.x]) && !is_sentinel(X[v.y]) && !is_sentinel(X[v.z])) {
const auto center = X.append(Perturbed2(X.size(),(X[v.x].value()+X[v.y].value()+X[v.z].value())/3));
const auto found = bsp_search(bsp,X,center);
if (found!=f) {
cout << "bsp search failed: f "<<f<<", v "<<v<<", found "<<found<<endl;
GEODE_ASSERT(false);
}
X.flat.pop();
}
}
}
Ref<SparseMatrix> TriangleSubdivision::loop_matrix() const {
if (loop_matrix_)
return ref(loop_matrix_);
// Build matrix of Loop subdivision weights
Hashtable<Vector<int,2>,T> A;
int offset = coarse_mesh->nodes();
RawArray<const Vector<int,2> > segments = coarse_mesh->segment_soup()->elements;
Nested<const int> neighbors = coarse_mesh->sorted_neighbors();
Nested<const int> boundary_neighbors = coarse_mesh->boundary_mesh()->neighbors();
unordered_set<int> corners_set(corners.begin(),corners.end());
// Fill in node weights
for (int i=0;i<offset;i++){
if (!neighbors.valid(i) || !neighbors.size(i) || corners_set.count(i) || (boundary_neighbors.valid(i) && boundary_neighbors.size(i) && boundary_neighbors.size(i)!=2))
A.set(vec(i,i),1);
else if (boundary_neighbors.valid(i) && boundary_neighbors.size(i)==2) { // Regular boundary node
A.set(vec(i,i),(T).75);
for (int a=0;a<2;a++)
A.set(vec(i,boundary_neighbors(i,a)),(T).125);
} else { // Interior node
RawArray<const int> ni = neighbors[i];
T alpha = new_loop_alpha(ni.size());
A.set(vec(i,i),alpha);
T other = (1-alpha)/ni.size();
for (int j : ni)
A.set(vec(i,j),other);
}
}
// Fill in edge values
for (int s=0;s<segments.size();s++) {
int i = offset+s;
Vector<int,2> e = segments[s];
RawArray<const int> n[2] = {neighbors.valid(e[0])?neighbors[e[0]]:RawArray<const int>(),
neighbors.valid(e[1])?neighbors[e[1]]:RawArray<const int>()};
if (boundary_neighbors.valid(e[0]) && boundary_neighbors.valid(e[1]) && boundary_neighbors.size(e[0]) && boundary_neighbors.size(e[1]) && boundary_neighbors[e[0]].contains(e[1])) // Boundary edge
for (int a=0;a<2;a++)
A.set(vec(i,e[a]),(T).5);
else if (n[0].size()==6 && n[1].size()==6) { // Edge between regular vertices
int j = n[0].find(e[1]);
int c[2] = {n[0][(j-1+n[0].size())%n[0].size()],
n[0][(j+1)%n[0].size()]};
for (int a=0;a<2;a++) {
A.set(vec(i,e[a]),(T).375);
A.set(vec(i,c[a]),(T).125);
}
} else { // Edge between one or two irregular vertices
T factor = n[0].size()!=6 && n[1].size()!=6 ?.5:1;
for (int k=0;k<2;k++)
if (n[k].size()!=6) {
A[vec(i,e[k])] += factor*(1-new_loop_beta(n[k].size()));
int start = n[k].find(e[1-k]);
for (int j=0;j<n[k].size();j++)
A[vec(i,n[k][(start+j)%n[k].size()])] += factor*new_loop_weight(n[k].size(),j);
}
}
}
// Finalize construction
loop_matrix_ = new_<SparseMatrix>(A,vec(offset+segments.size(),offset));
return ref(loop_matrix_);
}
template<> void CubicHinges<TV2>::add_elastic_differential(RawArray<TV> dF, RawArray<const TV> dX) const {
// 2D forces are unconditionally linear, so we can always reuse force computation
GEODE_ASSERT(dF.size()>=nodes_ && dX.size()>=nodes_);
if (simple_hessian)
add_force_helper<true>(bends,info,stiffness,dF,dX);
else
add_force_helper<false>(bends,info,stiffness,dF,dX);
}
void SurfacePins::add_damping_force(RawArray<TV> F, RawArray<const TV> V) const {
GEODE_ASSERT(V.size()==mass.size());
GEODE_ASSERT(F.size()==mass.size());
for (int i=0;i<particles.size();i++) {
int p = particles[i];
F[p] -= kd[i]*dot(V[p],info[i].normal)*info[i].normal;
}
}
void SurfacePins::add_elastic_differential(RawArray<TV> dF, RawArray<const TV> dX) const {
GEODE_ASSERT(dF.size()==mass.size());
GEODE_ASSERT(dX.size()==mass.size());
for (int i=0;i<particles.size();i++) {
int p = particles[i];
dF[p] -= k[i]*dot(dX[p],info[i].normal)*info[i].normal; // Ignores a curvature term if the closest point is on an edge or vertex
}
}
template<int d,int m> static Array<UpperTriangularMatrix<T,d>> compute_Dm_inverse(RawArray<const Vector<int,d+1>> elements, RawArray<const Vector<T,m>> X) {
Array<UpperTriangularMatrix<T,d>> Dm_inverse(elements.size(),uninit);
for (int t=0;t<elements.size();t++) {
const auto R = StrainMeasure<T,d>::Ds(X,elements[t]).R_from_QR_factorization();
if (R.determinant()<=0)
throw RuntimeError("StrainMeasure: Inverted or degenerate rest state");
Dm_inverse[t] = R.inverse();
}
return Dm_inverse;
}
void SparseMatrix::
solve_forward_substitution(RawArray<const T> b,RawArray<T> x) const
{
GEODE_ASSERT(cholesky && rows()<=x.size() && rows()<=b.size());
// The result of Incomplete_Cholesky_Factorization has unit diagonals in the lower triangle.
for(int i=0;i<rows();i++){
T sum=0;
for(int index=J.offsets[i];index<diagonal_index[i];index++)
sum+=A.flat[index]*x[J.flat[index]];
x[i]=b[i]-sum;}
}
void SparseMatrix::
solve_backward_substitution(RawArray<const T> b,RawArray<T> x) const
{
GEODE_ASSERT(cholesky && rows()<=x.size() && rows()<=b.size());
// The result of Incomplete_Cholesky_Factorization has an inverted diagonal for the upper triangle.
for(int i=rows()-1;i>=0;i--){
T sum=0;
for(int index=diagonal_index[i]+1;index<J.offsets[i+1];index++)
sum+=A.flat[index]*x[J.flat[index]];
x[i]=(b[i]-sum)*A.flat[diagonal_index[i]];}
}
template<class TV> CubicHinges<TV>::CubicHinges(Array<const Vector<int,d+2>> bends, RawArray<const T> angles, RawArray<const TV> X)
: bends(bends)
, stiffness(0)
, damping(0)
, simple_hessian(false)
, nodes_(bends.size()?scalar_view(bends).max()+1:0)
, info(bends.size()) {
GEODE_ASSERT(bends.size()==angles.size());
GEODE_ASSERT(X.size()>=nodes_);
compute_info(bends,angles,X,info);
}
template<> Array<T> CubicHinges<TV2>::angles(RawArray<const Vector<int,3>> bends, RawArray<const TV2> X) {
if (bends.size())
GEODE_ASSERT(X.size()>scalar_view(bends).max());
Array<T> angles(bends.size(),uninit);
for (int b=0;b<bends.size();b++) {
int i0,i1,i2;bends[b].get(i0,i1,i2);
const TV x0 = X[i0], x1 = X[i1], x2 = X[i2];
angles[b] = angle_between(x1-x0,x2-x1);
}
return angles;
}
Array<T> TriangleSoup::vertex_areas(RawArray<const TV3> X) const {
GEODE_ASSERT(X.size()>=nodes());
Array<T> areas(X.size());
for (int t=0;t<elements.size();t++) {
int i,j,k;elements[t].get(i,j,k);
T area = T(1./6)*magnitude(cross(X[j]-X[i],X[k]-X[i]));
areas[i] += area;
areas[j] += area;
areas[k] += area;
}
return areas;
}
Array<TV3> TriangleSoup::vertex_normals(RawArray<const TV3> X) const {
GEODE_ASSERT(X.size()>=nodes());
Array<TV3> normals(X.size());
for(int t=0;t<elements.size();t++){
int i,j,k;elements[t].get(i,j,k);
TV3 n = cross(X[j]-X[i],X[k]-X[i]);
normals[i]+=n;normals[j]+=n;normals[k]+=n;
}
for(int i=0;i<X.size();i++)
normals[i].normalize();
return normals;
}
Array<int> SegmentSoup::nonmanifold_nodes(bool allow_boundary) const {
Array<int> nonmanifold;
Nested<const int> incident_elements = this->incident_elements();
for (int i=0;i<incident_elements.size();i++) {
RawArray<const int> incident = incident_elements[i];
if ( incident.size()>2 // Too many segments
|| (incident.size()==1 && !allow_boundary) // Disallowed boundary
|| (incident.size()==2 && (elements[incident[0]][0]==i)==(elements[incident[1]][0]==i))) // Inconsistent orientations
nonmanifold.append(i);
}
return nonmanifold;
}
static void endgame_sparse_verify(RawArray<const board_t> boards, RawArray<const Vector<super_t,2>> wins, Random& random, int samples) {
GEODE_ASSERT(boards.size()==wins.size());
GEODE_ASSERT((unsigned)samples<=(unsigned)boards.size());
// Check samples in random order
Array<int> permutation = arange(boards.size()).copy();
ProgressIndicator progress(samples,true);
for (int i=0;i<samples;i++) {
swap(permutation[i],permutation[random.uniform<int>(i,boards.size())]);
endgame_verify_board("endgame sparse verify",boards[permutation[i]],wins[permutation[i]],true);
progress.progress();
}
}
template<> Array<T> CubicHinges<TV3>::angles(RawArray<const Vector<int,4>> bends, RawArray<const TV3> X) {
if (bends.size())
GEODE_ASSERT(X.size()>scalar_view(bends).max());
Array<T> angles(bends.size(),uninit);
for (int b=0;b<bends.size();b++) {
int i0,i1,i2,i3;bends[b].get(i0,i1,i2,i3);
const TV x0 = X[i0], x1 = X[i1], x2 = X[i2], x3 = X[i3],
n0 = normal(x0,x2,x1),
n1 = normal(x1,x2,x3);
angles[b] = copysign(acos(clamp(dot(n0,n1),-1.,1.)),dot(n1-n0,x3-x0));
}
return angles;
}
Array<const Vector<int,3>> SegmentSoup::bending_tuples() const {
if (!bending_tuples_valid) {
Nested<const int> neighbors = this->neighbors();
Array<Vector<int,3>> tuples;
for (const int p : range(nodes())) {
RawArray<const int> near = neighbors[p];
for (int i=0;i<near.size();i++) for(int j=i+1;j<near.size();j++)
tuples.append(vec(near[i],p,near[j]));
}
bending_tuples_ = tuples;
}
return bending_tuples_;
}
template<class TV> static inline Tuple<T,TV> weighted_average(RawArray<const T> mass, RawArray<const TV> X) {
const int n = X.size();
if (mass.size()) {
GEODE_ASSERT(mass.size()==X.size());
const T total = mass.sum();
TV center;
for (int i=0;i<n;i++)
center += mass[i]*X[i];
return tuple(total,center/total);
} else {
// Unweighted
return tuple(n,X.mean());
}
}
template<class TV> void SparseMatrix::
multiply_helper(RawArray<const TV> x,RawArray<TV> result) const
{
const int rows = this->rows();
GEODE_ASSERT(columns()<=x.size() && rows<=result.size());
RawArray<const int> offsets = J.offsets;
RawArray<const int> J_flat = J.flat;
RawArray<const T> A_flat = A.flat;
for(int i=0;i<rows;i++){
int end=offsets[i+1];TV sum=TV();
for(int index=offsets[i];index<end;index++) sum+=A_flat[index]*x[J_flat[index]];
result[i]=sum;}
result.slice(rows,result.size()).zero();
}
// Prepare a list of points for Delaunay triangulation: randomly assign into logarithmic bins, sort within bins, and add sentinels.
// For details, see Amenta et al., Incremental Constructions con BRIO.
static Array<Perturbed2> partially_sorted_shuffle(RawArray<const EV> Xin) {
const int n = Xin.size();
Array<Perturbed2> X(n+3,uninit);
// Randomly assign input points into bins. Bin k has 2**k = 1,2,4,8,... and starts at index 2**k-1 = 0,1,3,7,...
// We fill points into bins as sequentially as possible to maximize cache coherence.
const int bins = integer_log(n);
Array<int> bin_counts(bins);
for (int i=0;i<n;i++) {
int j = (int)random_permute(n,key,i);
const int bin = min(integer_log(j+1),bins-1);
j = (1<<bin)-1+bin_counts[bin]++;
X[j] = Perturbed2(i,Xin[i]);
}
// Spatially sort each bin down to clusters of size 64.
const int leaf_size = 64;
for (int bin=0;bin<bins;bin++) {
const int start = (1<<bin)-1,
end = bin==bins-1?n:start+(1<<bin);
assert(bin_counts[bin]==end-start);
spatial_sort(X.slice(start,end),leaf_size,new_<Random>(key+bin));
}
// Add 3 sentinel points at infinity
X[n+0] = Perturbed2(n+0,EV(-bound,-bound));
X[n+1] = Perturbed2(n+1,EV( bound, 0) );
X[n+2] = Perturbed2(n+2,EV(-bound, bound));
return X;
}
void SurfacePins::add_elastic_force(RawArray<TV> F) const {
GEODE_ASSERT(F.size()==mass.size());
for (int i=0;i<particles.size();i++) {
int p = particles[i];
F[p] -= k[i]*info[i].phi*info[i].normal;
}
}
template<bool simple> static void add_gradient_helper(RawArray<const Vector<int,4>> bends, RawArray<const CubicHinges<TV3>::Info> info, const T scale, RawArray<const TV3> X, SolidMatrix<TV3>& matrix) {
if (!scale) return;
for (int b=0;b<bends.size();b++) {
const auto& I = info[b];
int i0,i1,i2,i3;bends[b].get(i0,i1,i2,i3);
const T quad = -scale*I.dot;
matrix.add_entry(i0,quad*sqr(I.c[0]));
matrix.add_entry(i1,quad*sqr(I.c[1]));
matrix.add_entry(i2,quad*sqr(I.c[2]));
matrix.add_entry(i3,quad*sqr(I.c[3]));
if (simple) {
matrix.add_entry(i0,i1,quad*I.c[0]*I.c[1]);
matrix.add_entry(i0,i2,quad*I.c[0]*I.c[2]);
matrix.add_entry(i0,i3,quad*I.c[0]*I.c[3]);
matrix.add_entry(i1,i2,quad*I.c[1]*I.c[2]);
matrix.add_entry(i1,i3,quad*I.c[1]*I.c[3]);
matrix.add_entry(i2,i3,quad*I.c[2]*I.c[3]);
} else {
const T cubic = -scale*I.det;
const TV3 x0 = cubic*X[i0], x1 = cubic*X[i1], x2 = cubic*X[i2], x3 = cubic*X[i3];
matrix.add_entry(i0,i1,quad*I.c[0]*I.c[1]+cross_product_matrix(x3-x2)); // e3
matrix.add_entry(i0,i2,quad*I.c[0]*I.c[2]+cross_product_matrix(x1-x3)); // -e1
matrix.add_entry(i0,i3,quad*I.c[0]*I.c[3]+cross_product_matrix(x2-x1)); // e0
matrix.add_entry(i1,i2,quad*I.c[1]*I.c[2]+cross_product_matrix(x3-x0));
matrix.add_entry(i1,i3,quad*I.c[1]*I.c[3]+cross_product_matrix(x0-x2)); // e4
matrix.add_entry(i2,i3,quad*I.c[2]*I.c[3]+cross_product_matrix(x1-x0)); // -e2
}
}
}
real circle_arc_area(RawArray<const CircleArc> arcs) {
const int n = arcs.size();
real area = 0;
for (int i=n-1,j=0;j<n;i=j++)
area += .5*cross(arcs[i].x,arcs[j].x) + .25*sqr_magnitude(arcs[j].x-arcs[i].x)*q_factor(arcs[i].q); // Triangle area plus circular sector area
return .5*area;
}
void SurfacePins::add_elastic_gradient_block_diagonal(RawArray<SymmetricMatrix<T,m>> dFdX) const {
GEODE_ASSERT(dFdX.size()==mass.size());
for (int i=0;i<particles.size();i++) {
int p = particles[i];
dFdX[p] -= scaled_outer_product(k[i],info[i].normal); // Ignores a curvature term if the closest point is on an edge or vertex
}
}
void SurfacePins::add_frequency_squared(RawArray<T> frequency_squared) const {
GEODE_ASSERT(frequency_squared.size()==mass.size());
for (int i=0;i<particles.size();i++) {
int p = particles[i];
frequency_squared[p]+=k[i]/mass[p];
}
}
template<bool simple> static void add_force_helper(RawArray<const Vector<int,4>> bends, RawArray<const CubicHinges<TV3>::Info> info, const T scale, RawArray<TV3> F, RawArray<const TV3> X) {
if (!scale) return;
for (int b=0;b<bends.size();b++) {
const auto& I = info[b];
int i0,i1,i2,i3;bends[b].get(i0,i1,i2,i3);
const TV3 x0 = X[i0], x1 = X[i1], x2 = X[i2], x3 = X[i3],
stress = scale*I.dot*(I.c[0]*x0+I.c[1]*x1+I.c[2]*x2+I.c[3]*x3);
if (simple) {
F[i0] -= I.c[0]*stress;
F[i1] -= I.c[1]*stress;
F[i2] -= I.c[2]*stress;
F[i3] -= I.c[3]*stress;
} else {
const T cubic = scale*I.det;
const TV3 x0 = X[i0], x1 = X[i1], x2 = X[i2], x3 = X[i3];
const TV3 ce0 = cubic*(x2-x1),
e1 = x3-x1,
e2 = x0-x1,
cross01 = cross(ce0,e1),
cross12 = cubic*cross(e1,e2),
cross20 = cross(e2,ce0);
F[i0] -= I.c[0]*stress+cross01;
F[i1] -= I.c[1]*stress-cross01-cross12-cross20;
F[i2] -= I.c[2]*stress+cross12;
F[i3] -= I.c[3]*stress+cross20;
}
}
}
void reduce(RawArray<const T> xe, RawArray<T> xr) const {
for (int k=0;k<=bcs.size();k++) {
const int lo = k ? bcs[k-1].x+1 : 0,
hi = k<bcs.size() ? bcs[k].x : xe.size();
for (int i=lo;i<hi;i++)
xr[i-k] = xe[i];
}
}
static void nasty_denominator(RawArray<mp_limb_t> result, RawArray<const Vector<Exact<1>,2>> X) {
assert(X.size()==2);
typename remove_const_reference<decltype(X[0])>::type p1;
for (int i=0;i<2;i++)
mpz_set(asarray(p1[i].n),Exact<1>(perturbation<2>(1,nasty_index)[i]));
const auto d = edet(X[0],p1);
mpz_set(result,d);
}
void SparseMatrix::
gauss_seidel_solve(RawArray<T> x,RawArray<const T> b,const T tolerance,const int max_iterations) const
{
GEODE_ASSERT(rows()==columns() && x.size()==rows() && b.size()==rows());
const T sqr_tolerance=sqr(tolerance);
for(int iteration=0;iteration<max_iterations;iteration++){
T sqr_residual=0;
for(int i=0;i<rows();i++){
T rho=0;T diagonal_entry=0;
for(int index=J.offsets[i];index<J.offsets[i+1];index++){
if(J.flat[index]==i) diagonal_entry=A.flat[index];
else rho+=A.flat[index]*x[J.flat[index]];}
T new_x=x[i]=(b[i]-rho)/diagonal_entry;
sqr_residual+=sqr(new_x-x[i]);
x[i]=new_x;}
if(sqr_residual <= sqr_tolerance) break;}
}
template<class TV> static Matrix<T,TV::m+1> affine_register_helper(RawArray<const TV> X0, RawArray<const TV> X1, RawArray<const T> mass) {
typedef typename TV::Scalar T;
static const int d = TV::m;
const int n = X0.size();
GEODE_ASSERT(n==X1.size() && (mass.size()==0 || mass.size()==n));
const bool weighted = mass.size() == n;
const T total = weighted ? mass.sum() : n;
if (!total)
return Matrix<T,d+1>::identity_matrix();
// Compute centers of mass
TV c0, c1;
if (weighted) {
for (int i=0;i<n;i++) {
c0 += mass[i]*X0[i];
c1 += mass[i]*X1[i];
}
} else {
c0 = X0.sum();
c1 = X1.sum();
}
c0 /= total;
c1 /= total;
// Compute covariances
SymmetricMatrix<T,d> cov00 = scaled_outer_product(-total,c0);
Matrix<T,d> cov01 = outer_product(-total*c1,c0);
if (weighted)
for(int i=0;i<n;i++) {
cov00 += scaled_outer_product(mass[i],X0[i]);
cov01 += outer_product(mass[i]*X1[i],X0[i]);
}
else
for (int i=0;i<n;i++) {
cov00 += outer_product(X0[i]);
cov01 += outer_product(X1[i],X0[i]);
}
// Compute transform
const Matrix<T,d> A = cov01*cov00.inverse();
const TV t = c1-A*c0;
auto tA = Matrix<T,d+1>::from_linear(A);
tA.set_translation(t);
return tA;
}
