h=1/30;
%gravity=[0,-10,0];X0=read_obj('cloth.obj'); % seahorse_extended.obj');
v0=X0.xyz';  %坐标
originalv0=v0;
pre_v0=v0;
tri=X0.tri'; %三角形顺序索引
velocity=v0;
gravity=zeros(X0.node_num,3);
gravity(:,2)=-10;
velocity(:,1)=0;velocity(:,2)=0;velocity(:,3)=0;  %速度初始化
%controlpoint = [1,3,7,9];
%controlpoint = [1,11,111,121];
controlpoint = [1,217];  %固定点的索引号.
%velocity(controlpoint(1,:),:)=-1000;win_min = [999 999 999];
win_max = [-999 -999 -999];timestep=200;  %total step
xAxis=[1:timestep];
for index=1:timestepvelocity=velocity+gravity*h;v0=pre_v0+h*velocity;    %PBD中的位置预测for xx=1:3  %迭代次数v0=distanceconstraint(v0,X0,controlpoint);  %距离约束end%如果撞到地面的话（-10），按照上一时刻的位置，然后速度反向，弹回去,布料这个例子中并未用到与地面的碰撞%for i=1:X0.node_numif v0(i,2)<-20v0(i,2)=pre_v0(i,2)-velocity(i,2)*h;end%if v0(i,2)<-10%    v0(i,2)=-10;% endend%----------------------------------------------------------------%v0(controlpoint(1,:),:)= pre_v0(controlpoint(1,:),:);  %更新速度与位置velocity=(v0-pre_v0)/h;pre_v0=v0;center=mean(v0,1);trimesh(tri,v0(:,1),v0(:,2),v0(:,3),'EdgeColor','b'); axis equalaxis manualcur_V = v0(:,1:3);% scale window to fitwin_min = min([win_min; cur_V]);win_max = max([win_max; cur_V]);axis(reshape([win_min;win_max],1,2*size(cur_V,2)))pause(0.01);
end
grid on;
%figure, trimesh(tri,v0(:,1),v0(:,2),v0(:,3),'EdgeColor','b'); axis equal
%hold on, plot3(v0(controlpoint(1,:),1),v0(controlpoint(1,:),2),v0(controlpoint(1,:),3),'*r');

function P=distanceconstraint(P,X0,controlpoint)
for i=1:X0.face_numidx1=X0.tri(1,i);  %三角形的三个索引idx2=X0.tri(2,i);idx3=X0.tri(3,i);d1=norm(X0.xyz(:,idx1)-X0.xyz(:,idx2));  %原始两点间的距离c_d1=norm(P(idx1,:)-P(idx2,:));%现在两点间的距离d2=norm(X0.xyz(:,idx2)-X0.xyz(:,idx3));c_d2=norm(P(idx2,:)-P(idx3,:));d3=norm(X0.xyz(:,idx1)-X0.xyz(:,idx3));c_d3=norm(P(idx1,:)-P(idx3,:));if c_d1~=d1  %考察三角形第一条边， 如果现在的距离与原始的距离不相等，那么更新位置，就像弹簧一样test=1;%用来测试这个点是不是control point， 如果是control point 那么不更新位置test2=1;%同上for count=1:length(controlpoint)if idx1==controlpoint(count)test=0;  %这个点不是control pointendif idx2==controlpoint(count)test2=0;endendif test==0deltap1=[0,0,0];deltap2=(c_d1-d1)*(P(idx1,:)-P(idx2,:))/(norm(P(idx1,:)-P(idx2,:)));elsedeltap1=-0.5*(c_d1-d1)*(P(idx1,:)-P(idx2,:))/(norm(P(idx1,:)-P(idx2,:))); %文章中的stifness是乘在这里的，我没乘，我按质点质量相等来算的，所以是0.5endif test2==0deltap2=[0,0,0];deltap1=-(c_d1-d1)*(P(idx1,:)-P(idx2,:))/(norm(P(idx1,:)-P(idx2,:)));elsedeltap2=0.5*(c_d1-d1)*(P(idx1,:)-P(idx2,:))/(norm(P(idx1,:)-P(idx2,:)));endP(idx1,:)=P(idx1,:)+deltap1;P(idx2,:)=P(idx2,:)+deltap2;endif c_d2~=d2  %考察第二条边test=1;test2=1;for count=1:length(controlpoint)if idx2==controlpoint(count)test=0;endif idx3==controlpoint(count)test2=0;endendif test==0deltap23=[0,0,0];deltap32=(c_d2-d2)*(P(idx2,:)-P(idx3,:))/(norm(P(idx2,:)-P(idx3,:)));elsedeltap23=-0.5*(c_d2-d2)*(P(idx2,:)-P(idx3,:))/(norm(P(idx2,:)-P(idx3,:)));endif test2==0deltap32=[0,0,0];deltap23=-(c_d2-d2)*(P(idx2,:)-P(idx3,:))/(norm(P(idx2,:)-P(idx3,:)));elsedeltap32=0.5*(c_d2-d2)*(P(idx2,:)-P(idx3,:))/(norm(P(idx2,:)-P(idx3,:)));endP(idx2,:)=P(idx2,:)+deltap23;P(idx3,:)=P(idx3,:)+deltap32;endif c_d3~=d3 %考察第三条边test=1;test2=1;for count=1:length(controlpoint)if idx1==controlpoint(count)test=0;endif idx3==controlpoint(count)test2=0;endendif test==0deltap13=[0,0,0];deltap31=(c_d3-d3)*(P(idx1,:)-P(idx3,:))/(norm(P(idx1,:)-P(idx3,:)));elsedeltap13=-0.5*(c_d3-d3)*(P(idx1,:)-P(idx3,:))/(norm(P(idx1,:)-P(idx3,:)));endif test2==0deltap31=[0,0,0];deltap13=-(c_d3-d3)*(P(idx1,:)-P(idx3,:))/(norm(P(idx1,:)-P(idx3,:)));elsedeltap31=0.5*(c_d3-d3)*(P(idx1,:)-P(idx3,:))/(norm(P(idx1,:)-P(idx3,:)));endP(idx1,:)=P(idx1,:)+deltap13;P(idx3,:)=P(idx3,:)+deltap31;end
end
end