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