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;
}
}
}
Nested<const int> SegmentSoup::neighbors() const {
if (nodes() && !neighbors_.size()) {
Array<int> lengths(nodes());
for(int s=0;s<elements.size();s++)
for(int a=0;a<2;a++)
lengths[elements[s][a]]++;
neighbors_ = Nested<int>(lengths);
for(int s=0;s<elements.size();s++) {
int i,j;elements[s].get(i,j);
neighbors_(i,neighbors_.size(i)-lengths[i]--) = j;
neighbors_(j,neighbors_.size(j)-lengths[j]--) = i;
}
// Sort and remove duplicates if necessary
bool need_copy = false;
for(int i=0;i<nodes();i++) {
RawArray<int> n = neighbors_[i];
sort(n);
int* last = std::unique(n.begin(),n.end());
if(last!=n.end())
need_copy = true;
lengths[i] = int(last-n.begin());
}
if (need_copy) {
Nested<int> copy(lengths);
for(int i=0;i<nodes();i++)
copy[i] = neighbors_[i].slice(0,lengths[i]);
neighbors_ = copy;
}
}
return neighbors_;
}
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();
}
}
}
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<> 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 reverse_arcs(RawArray<CircleArc> arcs) {
if(arcs.empty()) return;
arcs.reverse();
const auto temp_q = arcs.front().q;
for(int i = 0,j = 1; j<arcs.size(); i=j++) {
arcs[i].q = -arcs[j].q;
}
arcs.back().q = -temp_q;
}
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_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]];}
}
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;}
}
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;
}
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);
}
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();
}
}
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<> 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;
}
void inplace_partial_permute(UntypedArray& x, RawArray<const int> perm, Array<char>& work, const int block) {
const int m = perm.size();
const int b_size = block*x.t_size();
GEODE_ASSERT(x.size()==block*m);
const int space = m ? (perm.max()+1)*b_size : 0;
work.resize(space);
for (int i=0;i<m;i++) {
const int pi = perm[i];
if (pi >= 0)
memcpy(work.data()+pi*b_size,x.data()+i*b_size,b_size);
}
x.resize(space/x.t_size());
memcpy(x.data(),work.data(),space);
}
void RawArray::wipecopy(RawArray& src) {
long long k,kmax;
if(&src==NULL)return;
if(src.rawNum==0) return; // ignore empty inputs.
if(src.getrawNum()!=rawNum || src.getZsize() != z[0].getZsize() || src.getYsize() != z[0].getYsize() ||
src.getXsize() != getXsize()) {
sizer(&src);
}
kmax = getrawNum();
for(k=0; k< kmax; k++) { // copy each field;
z[k].wipecopy(&(src.z[k]));
}
}
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();
}
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];
}
}
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
}
}
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_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
}
}
}
// 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;
}
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];
}
}
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;}
}
