Cloth.py
import numpy as np
import math
def create(nW, nH, width, height) :
dx = width/(nW-1)
dz = height / (nH-1)
dDiag = math.sqrt(dx**2.0 + dz**2.0)
Verts = np.zeros(shape=(nW * nH, 3))
Springs = np.zeros(shape=((nW - 1) * nH + (nH - 1) * nW + (nW - 1) * (nH - 1) * 2, 2), dtype=np.int32)
for i in range(0, nW * nH):
Verts[i] = [(i % nW) * dx, 1, (int(i / nW)) * dz]
L0 = np.zeros(shape=(len(Springs),))
for i in range(0, (nW - 1) * nH):
row = int(i / (nW - 1))
col = i % (nW - 1)
Springs[i] = [row * nW + col, row * nW + col + 1]
L0[i] = dx
for i in range(0, (nH - 1) * nW):
row = int(i / (nW))
col = i % (nW)
Springs[(nW - 1) * nH + i] = [row * nW + col, (row + 1) * nW + col]
L0[(nW - 1) * nH + i] = dz
for i in range(0, (nW - 1) * (nH - 1)):
row = int(i / (nW - 1))
col = i % (nW - 1)
Springs[(nW - 1) * nH + (nH - 1) * nW + 2 * i] = [row * nW + col, (row + 1) * nW + col + 1]
Springs[(nW - 1) * nH + (nH - 1) * nW + 2 * i + 1] = [row * nW + col + 1, (row + 1) * nW + col]
L0[(nW - 1) * nH + (nH - 1) * nW + 2 * i] = dDiag
L0[(nW - 1) * nH + (nH - 1) * nW + 2 * i + 1] = dDiag
return Verts, Springs, L0
MatrixVector.py
import numpy as np
def convertLongVector(x) :
longX = np.zeros(shape=(len(x)*3,))
for i in range(0, len(x)) :
for j in range(0,3) :
longX[i*3+j] = x[i,j]
return longX
def set33SubMatrix(M, i, j, m) :
for row in range(0,3) :
for col in range(0,3) :
M[i*3+row, j*3+col] = m[row,col]
def set33SubMatrixSymetric(M, i, j, m):
for row in range(0, 3):
for col in range(0, 3):
M[i * 3 + row, j * 3 + col] = m[row, col]
M[j * 3 + row, i * 3 + col] = m[row, col]
ImplicitMethod.py
from OpenGL.GLUT import *
from OpenGL.GL import *
from OpenGL.GLU import *
import numpy as np
import Cloth
import MatrixVector as MV
nW = 15
nH = 15
clothWidth = 10
clothHeight = 10
Verts, Springs, L0 = Cloth.create(nW, nH, clothWidth, clothHeight)
Vel = np.zeros(shape=(len(Verts),3), dtype = np.float64)
Force = np.zeros(shape=(len(Verts),3), dtype = np.float64)
Mass = [1.0 for x in range(len(Verts))]
gravity = np.array([0, -9.8, 0])
K = 500000.0
damp = 2.0
dt = 0.01
def computeForces(springs, verts) :
global Vel, K
force = np.zeros((len(verts),3), dtype=np.float64)
for i in range(0, len(springs)):
idx0, idx1 = springs[i][0], springs[i][1]
p0 , p1 = verts[idx0], verts[idx1]
L = np.linalg.norm(p1 - p0)
dir = (p1 - p0) / L
Vrel = Vel[idx0] - Vel[idx1]
F = K * (L-L0[i])* dir - damp*Vrel # spring force + damping force
force[idx0] = force[idx0] + F
force[idx1] = force[idx1] - F
return force
def Integrate(locs, force, vel, h) :
global Springs, L0, Mass, gravity, K, nW
for i in range(0, len(locs)) :
force[i] = force[i] + gravity
longF = MV.convertLongVector(force)
bigM = np.zeros(shape=(len(locs) * 3, len(locs) * 3))
Jx = np.zeros(shape=(len(locs) * 3, len(locs) * 3))
Jv = np.zeros(shape=(len(locs) * 3, len(locs) * 3))
for i in range(0, len(locs)) :
MV.set33SubMatrix(bigM, i, i, Mass[i] * np.identity(3))
Jxii = np.zeros(shape=(len(locs), 3, 3))
Jvii = np.zeros(shape=(len(locs), 3, 3))
for spring in range(0, len(Springs)) :
i,j = Springs[spring][0], Springs[spring][1]
xji = locs[j]-locs[i]
lt = np.linalg.norm(xji)
Jxij = K*(((lt-L0[spring])/lt)*np.identity(3) + (L0[spring]/(lt**3.0))*np.outer(xji, xji))
Jvij = damp*np.identity(3)
Jxii[i], Jxii[j] = Jxii[i] - Jxij, Jxii[j] - Jxij
Jvii[i], Jvii[j] = Jvii[i] - Jvij, Jvii[j] - Jvij
MV.set33SubMatrixSymetric(Jx, i, j, Jxij)
MV.set33SubMatrixSymetric(Jv, i, j, Jvij)
for i in range(0, len(locs)):
MV.set33SubMatrix(Jx, i, i, Jxii[i])
MV.set33SubMatrix(Jv, i, i, Jvii[i])
bigM = bigM - h*h*Jx - h*Jv
bigMinv = np.linalg.inv(bigM)
longV = MV.convertLongVector(vel)
deltaV = bigMinv.dot(longF*h + (h**2)*Jx.dot(longV))
for i in range(0, len(locs)):
vel[i] = vel[i] + deltaV[i*3:i*3+3]
if i is not 0 and i is not nW-1:
locs[i] = locs[i] + vel[i] * h
else :
vel[i] = np.zeros(shape=(3,))
def moveImplicit():
global Verts, Springs, Vel, Force, Mass, K, damp, L0, dt, gravity
# Force Initialization
Force = computeForces(Springs, Verts)
Integrate(Verts, Force, Vel, dt)
glutPostRedisplay() ## Hey! Draw Function! Draw!
def drawSpring(loc1, loc2):
glColor3f(1, 1, 1)
glBegin(GL_LINES)
glVertex3f(loc1[0], loc1[1], loc1[2])
glVertex3f(loc2[0], loc2[1], loc2[2])
glEnd()
def draw():
global clothWidth, clothHeight
glClear(GL_COLOR_BUFFER_BIT)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(60, 1.0, 0.1, 1000)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
gluLookAt(clothWidth*0.5, 1.0 - clothHeight*0.5, clothHeight*1.5, clothWidth*0.5, 1.0 - clothHeight*0.5, 0.0, 0.0, 1.0, 0.0)
for i in range(0, len(Springs)):
drawSpring(Verts[Springs[i][0]], Verts[Springs[i][1]])
glFlush()
glutInit(sys.argv)
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA)
glutInitWindowSize(250, 250)
glutInitWindowPosition(100, 100)
glutCreateWindow(b"OpenGL with Python")
glClearColor(0.5, 0.5, 0.5, 1)
glutDisplayFunc(draw)
glutIdleFunc(moveImplicit)
glutMainLoop()
# End of program
ApproxImplicit.py
from OpenGL.GLUT import *
from OpenGL.GL import *
from OpenGL.GLU import *
import numpy as np
import Cloth
import MatrixVector as MV
nW = 15
nH = 15
clothWidth = 3
clothHeight = 3
Verts, Springs, L0 = Cloth.create(nW, nH, clothWidth, clothHeight)
Vel = np.zeros(shape=(len(Verts),3), dtype = np.float64)
Force = np.zeros(shape=(len(Verts),3), dtype = np.float64)
Mass = [1.0 for x in range(len(Verts))]
gravity = np.array([0, -9.8, 0])
K = 5000.0
damp = 2.0
dt = 0.01
def computeForces(springs, verts) :
global Vel, K
force = np.zeros((len(verts),3), dtype=np.float64)
for i in range(0, len(springs)):
idx0, idx1 = springs[i][0], springs[i][1]
p0 , p1 = verts[idx0], verts[idx1]
L = np.linalg.norm(p1 - p0)
dir = (p1 - p0) / L
Vrel = Vel[idx0] - Vel[idx1]
F = K * (L-L0[i])* dir - damp*Vrel # spring force + damping force
force[idx0] = force[idx0] + F
force[idx1] = force[idx1] - F
return force
def Integrate(locs, springs, force, vel, h) :
global L0, Mass, gravity, K, nW
for i in range(0, len(locs)) :
force[i] = force[i] + gravity
Mii = np.zeros(shape=(len(locs), 3, 3))
Jxii = np.zeros(shape=(len(locs), 3, 3))
Jvii = np.zeros(shape=(len(locs), 3, 3))
Jxij = np.zeros(shape=(len(springs), 3, 3))
Jvij = np.zeros(shape=(len(springs), 3, 3))
for i in range(0, len(locs)) :
Mii[i] = Mass[i] * np.identity(3)
for s in range(0, len(springs)) :
i,j = springs[s][0], springs[s][1]
xji = locs[j]-locs[i]
lt = np.linalg.norm(xji)
Jxij[s] = K*(((lt-L0[s])/lt)*np.identity(3) + (L0[s]/(lt**3.0))*np.outer(xji, xji))
Jvij[s] = damp*np.identity(3)
Jxii[i], Jxii[j] = Jxii[i] - Jxij[s], Jxii[j] - Jxij[s]
Jvii[i], Jvii[j] = Jvii[i] - Jvij[s], Jvii[j] - Jvij[s]
b = np.zeros(shape=(len(locs), 3), dtype = np.float64)
deltaV = np.zeros(shape=(len(locs), 3), dtype=np.float64)
for i in range(len(locs)) :
b[i] = force[i]*h + h*h*Jxii[i].dot(vel[i])
for s in range(len(springs)) :
i,j = springs[s][0], springs[s][1]
b[i] = b[i] + h * h * Jxij[s].dot(vel[j])
b[j] = b[j] + h * h * Jxij[s].dot(vel[i])
Minv = np.zeros(shape=(len(locs), 3, 3))
for i in range(len(locs)) :
Minv[i] = np.linalg.inv(Mii[i] - h * h * Jxii[i] - h * Jvii[i])
#deltaV[i] = Minv[i].dot(b[i])
if i is not 0 and i is not nW - 1:
deltaV[i] = Minv[i].dot(b[i])
else:
deltaV[i] = np.zeros(shape=(3,))
for iter in range(1) :
for s in range(len(springs)):
i, j = springs[s][0], springs[s][1]
OffDiag = -h*h * Jxij[s] - h*Jvij[s]
b[i] -= OffDiag.dot(deltaV[j])
b[j] -= OffDiag.dot(deltaV[i])
for i in range(len(locs)):
if i is not 0 and i is not nW-1 :
deltaV[i] = Minv[i].dot(b[i])
else :
deltaV[i] = np.zeros(shape=(3,))
for i in range(0, len(locs)):
vel[i] = vel[i] + deltaV[i]
if i is not 0 and i is not nW-1:
locs[i] = locs[i] + vel[i] * h
else :
vel[i] = np.zeros(shape=(3,))
def moveImplicit():
global Verts, Springs, Vel, Force, Mass, K, damp, L0, dt, gravity
# Force Initialization
Force = computeForces(Springs, Verts)
Integrate(Verts, Springs, Force, Vel, dt)
glutPostRedisplay() ## Hey! Draw Function! Draw!
def drawSpring(loc1, loc2):
glColor3f(1, 1, 1)
glBegin(GL_LINES)
glVertex3f(loc1[0], loc1[1], loc1[2])
glVertex3f(loc2[0], loc2[1], loc2[2])
glEnd()
def draw():
global clothWidth, clothHeight
glClear(GL_COLOR_BUFFER_BIT)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(60, 1.0, 0.1, 1000)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
gluLookAt(clothWidth*0.5, 1.0 - clothHeight*0.5, clothHeight*1.5, clothWidth*0.5, 1.0 - clothHeight*0.5, 0.0, 0.0, 1.0, 0.0)
for i in range(0, len(Springs)):
drawSpring(Verts[Springs[i][0]], Verts[Springs[i][1]])
glFlush()
glutInit(sys.argv)
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA)
glutInitWindowSize(250, 250)
glutInitWindowPosition(100, 100)
glutCreateWindow(b"OpenGL with Python")
glClearColor(0.5, 0.5, 0.5, 1)
glutDisplayFunc(draw)
glutIdleFunc(moveImplicit)
glutMainLoop()
# End of program