The solver works pretty well, though it will probably work better if you can pass in a reasonable guess at z.
Haven't tested the last couple lines with the returns - should work, but if it complains about broadcasting the function call with **try_solve in the arguments, you may have to make those assignments one at a time.
Code: import scipy.optimize as scop
class Unknown(object):
def __init__(self,name,sterm=0,mterm=1,reciprocal=False):
self.name = name
self.reciprocal = reciprocal
self.sterm=sterm
self.mterm=mterm
def __repr__(self):
return "%s((%s*Unknown(%s))+%s)" % ("1./" if self.reciprocal else "",repr(self.mterm),repr(self.name),repr(self.sterm))
def __str__(self):
return "%s((%s*%s)+%s)" % ("1./" if self.reciprocal else "",str(self.mterm),str(self.name),str(self.sterm))
def __mul__(self, other):
oterm = (1./other) if self.reciprocal else other
return Unknown(name=self.name,sterm=self.sterm*oterm,mterm=self.mterm*oterm,reciprocal=self.reciprocal)
def __div__(self, other):
return self.__mul__(1./other)
def __add__(self, other):
if self.reciprocal:
# This is where it gets hairy
denom = Unknown(name=self.name,sterm=self.sterm,mterm=self.mterm,reciprocal=True)
return Unknown(name=self.name,sterm=(self.sterm*other+1)*denom,mterm=(self.mterm*other)*denom,reciprocal=False)
else:
return Unknown(name=self.name,sterm=self.sterm+other,mterm=self.mterm,reciprocal=False)
self.sterm = self.sterm + other
return self
def __sub__(self, other):
return self.__add__(-other)
def __rmul__(self, other):
return self.__mul__(other)
# Division is not commutative!
def __rdiv__(self, other):
return Unknown(name=self.name,sterm=self.sterm,mterm=self.mterm,reciprocal = not self.reciprocal).__rmul__(other)
def __radd__(self, other):
return self.__add__(other)
def __rsub__(self, other):
return self.__neg__().__radd__(other)
def __neg__(self):
return Unknown(name=self.name,sterm=-self.sterm,mterm=-self.mterm,reciprocal=self.reciprocal)
#Evaluate one level deep
def __call__(self, **variables):
tterm = Unknown(self.name)
mdict = {}
sdict = {}
for n,v in variables.iteritems():
if self.name == n:
tterm = v
if type(self.mterm) == Unknown and n in self.mterm:
mdict[n] = v
if type(self.sterm) == Unknown and n in self.sterm:
sdict[n] = v
term = (self.sterm(**sdict) if sdict else self.sterm) + (tterm * (self.mterm(**mdict) if mdict else self.mterm))
return 1/term if self.reciprocal else term
def __contains__(self, key):
ret = False
if self.name == key:
ret = True
if type(self.mterm) == Unknown:
ret |= self.mterm.__contains__(key)
if type(self.sterm) == Unknown:
ret |= self.sterm.__contains__(key)
return ret
def findaroot(self, **guesses):
args = sorted(guesses.keys())
init_vals = array([guesses[a] for a in args])
def redict(argorder,values):
return {a:v for a,v in zip(args,values)}
def solvable(array_values):
return self.__call__(**redict(args,array_values))
def sstatus(x,f):
print "Trying %s to get %f" % (str(x),self.__call__(**redict(args,x)))
result = scop.root(solvable,init_vals,method='excitingmixing',tol=1e-10)
if result.success:
ret = redict(args,result.x)
else:
ret = {}
return ret
if x == None:
a = 1
b = 2
c = 0
elif y == None:
a = 0
b = 2
c = 1
else:
a = 0
b = 1
c = 2
reindex = array([a,b,c])
trimatrix = matrix(tris[reindex]).transpose()
P = matrix([[x],[y],[z]])[reindex]
P[2]=Unknown('z')
offset = trimatrix[:,0]
centered = trimatrix - offset # translate to origin
theta_x = atan2(centered[2,2],centered[1,2])
rx = matrix([[1,0,0],[0,cos(-theta_x),-sin(-theta_x)],[0,sin(-theta_x),cos(-theta_x)]])
intermediate1 = rx*centered
theta_z = atan2(intermediate1[1,2],intermediate1[0,2])
rz = matrix([[cos(-theta_z),-sin(-theta_z),0],[sin(-theta_z),cos(-theta_z),0],[0,0,1]])
intermediate2=rz*intermediate1
theta_x2 = atan2(intermediate2[2,1],intermediate2[1,1])
rx2 = matrix([[1,0,0],[0,cos(-theta_x2),-sin(-theta_x2)],[0,sin(-theta_x2),cos(-theta_x2)]])
planar = rx2*intermediate2
Pplanar = rx2*rz*rx*(P-offset)
# Do something here to set tolerances
# and take care of rounding error, if it seems like it matters
T = matrix([[(planar[0,0]-planar[0,2]),(planar[0,1]-planar[0,2])],[(planar[1,0]-planar[1,2]),(planar[1,1]-planar[1,2])]])
det = numpy.linalg.det(T)
if det == 0: # Incoming div-by-0
print("Determinant: Div-by-0 warning")
return [0., 0., 0., x, y, z]
lam12 = inv(T) * (Pplanar[:2]-planar[:2,2])
lambda1 = lam12[0,0]
lambda2 = lam12[1,0]
lambda3 = 1 - lambda1 - lambda2
rval = [lambda1,lambda2,lambda3,x,y,lambda1*trimatrix[2,0]+lambda2*trimatrix[2,1]+lambda3*trimatrix[2,2]]
rval = array([lambda1,lambda2,lambda3,x,y,z])
rval[3+c] = lambda1*trimatrix[2,0]+lambda2*trimatrix[2,1]+lambda3*trimatrix[2,2]
# feel free to put better guesses if you have them
# but it's linear so I think it should be okay?
try_solve = (rval[3+c] - P[2,0]).findaroot(z=0)
if try_solve:
unsolved_indices = array([1,2,3,3+c])
rval[unsolved_indices] = rval[unsolved_indices](**try_solve)
else:
print "Found no solution to equation %s=0" % (str(rval[3+c] - P[2]))
return [0., 0., 0., x, y, z]
return rval