def inferPairAngle(self, obja, atta, objb, attb):
x1 = obja["tr.x"]
y1 = obja["tr.y"]
x2 = objb["tr.x"]
y2 = objb["tr.y"]
a1 = obja["rt.angle"]
a2 = objb["rt.angle"]
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = Globals.convertAttachment(atta, "p")
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = Globals.convertAttachment(att1, "r")
else:
i1 = 0
j1 = 0
att2 = Globals.convertAttachment(attb, "p")
if att2[0] != 0:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = Globals.convertAttachment(att2, "r")
else:
i2 = 0
j2 = 0
dx = sympy.sympify((x2 + i2) - (x1 + i1))
dy = sympy.sympify((y2 + j2) - (y1 + j1))
return sympy.sympify(sympy.atan2(dx, dy))
def assertPairAngle(self, dyn):
x1 = dyn.obja["tr.x"]
y1 = dyn.obja["tr.y"]
x2 = dyn.objb["tr.x"]
y2 = dyn.objb["tr.y"]
a1 = dyn.obja["rt.angle"]
a2 = dyn.objb["rt.angle"]
thetaa = dyn.thetaa
name = dyn.name
if not isTimeConstant(thetaa, self.symbols):
return True
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = dyn.getAttachment(dyn.obja, "p")
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = Globals.convertAttachment(att1, "r")
else:
i1 = 0
j1 = 0
att2 = dyn.getAttachment(dyn.objb, "p")
if att2[0] != 0:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = Globals.convertAttachment(att2, "r")
else:
i2 = 0
j2 = 0
dx = sympy.sympify((x2 + i2) - (x1 + i1))
dy = sympy.sympify((y2 + j2) - (y1 + j1))
if dx == 0 and sympy.simplify(sympy.Mod(thetaa, sympy.pi)) != 0:
self.printer.print_diagnostic(
2,
"thetaa assertion failed for dynamic %s, set=%s, inferred=%s."
% (name, str(thetaa), str(sympy.sympify(sympy.atan2(dx, dy)))),
)
return False
elif dy == 0 and sympy.simplify(sympy.Mod(thetaa + sympy.pi / 2, sympy.pi)) != 0:
self.printer.print_diagnostic(
2,
"thetaa assertion failed for dynamic %s, set=%s, inferred=%s."
% (name, str(thetaa), str(sympy.sympify(sympy.atan2(dx, dy)))),
)
return False
return True
