def NM_static(self):
# print(self.K)
P = sim.fix_left_end(self.V)
p_g = np.copy(self.p)
f_ext = np.zeros(len(self.f))
sim.compute_gravity(f_ext,
self.M,
self.sortedFlatB,
self.map_nodes,
axis=0,
mult=-10)
NewtonMax = 100
for i in range(NewtonMax):
forces = f_ext + self.K.dot(p_g - self.x)
g_block = P.T.dot(forces)
grad_g_block = np.matmul(np.matmul(P.T, self.K), P)
Q, R = np.linalg.qr(grad_g_block)
Qg = Q.T.dot(-1 * g_block)
dp = np.linalg.solve(R, Qg)
# print(dp)
p_g += P.dot(dp)
# print("gblock norm")
# print(np.linalg.norm(g_block))
# print("")
if (np.linalg.norm(g_block) / len(g_block)) < 1e-2:
# print("solved in ", i)
break
if i == 10:
print("Newton Method Error: not converging NM_static")
exit()
self.p = np.copy(p_g)
self.X_to_V(self.V, self.p)
def new_nm_step(self, h=None):
P = sim.fix_left_end(self.V)
p_g = np.copy(self.p)
rayleigh = -0
NewtonMax = 100
for i in range(NewtonMax):
forces = self.f + self.K.dot(
p_g - self.x) + (rayleigh / h) * self.K.dot(p_g - self.p)
g_block = P.T.dot(p_g - self.p - h * self.v -
h * h * self.invM.dot(forces))
grad_g_block = np.matmul(
P.T,
np.matmul(
np.identity(2 * (self.nonDupSize)) -
h * h * np.matmul(self.invM, self.K), P))
Q, R = np.linalg.qr(grad_g_block)
Qg = Q.T.dot(g_block)
dp = -1 * np.linalg.solve(R, Qg)
p_g += P.dot(dp)
# print("gblock norm")
# print(np.linalg.norm(g_block))
# print("")
if (np.linalg.norm(g_block) / len(g_block)) < 1e-2:
# print("solved in ", i)
break
if i == 10:
print("Newton Error: not converging, new_nm_step")
exit()
v_g = (p_g - self.p) / h
return p_g, v_g
def step(self, h=None):
# invMhhK = np.linalg.inv(self.M - h*h*self.K)
P = sim.fix_left_end(self.V)
# print("Mass")
# print(self.M)
for i in range(100):
self.p = self.p + h * P.dot(P.T.dot(self.v))
forces = self.f + self.K.dot(self.p - self.x)
self.v = self.v + h * P.dot(
P.T.dot(np.matmul(self.invM, P).dot(P.T.dot(forces))))
# print("")
# print("o", h*self.invM.dot(forces))
# print("f", h*P.dot(P.T.dot(self.invM.dot(forces))))
# print("p", self.p)
# print("v", self.v)
# newv = np.copy(self.v)
# func = lambda x: 0.5*np.dot(x.T, self.W.dot(x))
# def constr(x):
# return x - (self.v + h*P.dot(np.matmul(np.matmul(P.T, self.invM), P).dot(P.T.dot(forces))))
# cons = ({'type': 'eq', 'fun': constr })
#
# res = scipy.optimize.minimize(func, newv, method="SLSQP", constraints=cons)
# self.v = np.copy(res.x)
self.X_to_V(self.V, self.p)
def solve(meshL, meshH):
print("Old Solve")
timestep = 1e-1
P = sim.fix_left_end(meshH.V)
meshL.NMstep(h=1e-1)
v_squiggle = P.dot(P.T.dot(meshH.Nc.T.dot(meshL.v)))
p_squiggle = meshH.p + timestep * v_squiggle
u_squiggle = p_squiggle - meshH.x
def func(E_k):
#v_squiggle(E_squiggle, F_squiggle)
meshH.resetYM(E_k)
#Term 1: 0.5*v~^T* N^T*M*N *v~
t1 = 0.5 * np.dot(
v_squiggle,
np.matmul(np.matmul(meshH.Nc.T, meshL.M),
meshH.Nc).dot(v_squiggle))
#Term 2: v~^T * N^T*M*N * v~old
t2 = -1 * np.dot(
v_squiggle,
np.matmul(np.matmul(meshH.Nc.T, meshL.M), meshH.Nc).dot(meshH.v))
#Term 3: 0.5 u~^T * N^T*K*N * u~ #Energy
# print(v_squiggle)
t3 = 0.5 * np.dot(u_squiggle, meshH.K.dot(u_squiggle))
#Term 4: -h* N^T*Fext*v~
t4 = -1 * timestep * np.dot(meshH.Nc.T.dot(meshL.f), v_squiggle)
no = t1 + t2 + t3 + t4
# print(" no",no, "t1 ",t1, "t2 ",t2, "t3 ",t3, "t4 ",t4)
# print(E_k)
return np.fabs(no)
def func_der(x):
J = nd.Gradient(func)(x)
# print(">>>>>grad", J, x)
return J.ravel()
res = minimize(func,
meshH.YM,
method='Nelder-Mead',
options={'disp': True})
meshH.resetYM(res.x)
meshH.v = v_squiggle
meshH.p = p_squiggle
meshH.X_to_V(meshH.V, meshH.p)
print("RESULT")
print(res)
return res.x
def new_verlet_step(self, h=None):
P = sim.fix_left_end(self.V)
p_g = self.p + h * np.matmul(P, P.T).dot(self.v)
forces = self.f + self.K.dot(p_g - self.x)
v_g = self.v + h * P.dot(P.T.dot(self.invM.dot(forces)))
# newv = np.copy(self.v)
# func = lambda x: 0.5*np.dot(x.T, self.W.dot(x))
# def constr(x):
# return x - (self.v + h*P.dot(np.matmul(np.matmul(P.T, self.invM), P).dot(P.T.dot(forces))))
# cons = ({'type': 'eq', 'fun': constr })
#
# res = scipy.optimize.minimize(func, newv, method="SLSQP", constraints=cons)
return p_g, v_g