def solveSystemWithMode(self,doc, mode):
self.level_of_accuracy=1
startTime = int(round(time.time() * 1000))
self.loadSystem(doc)
if self.status == "loadingDependencyError":
return
self.assignParentship(doc)
loadTime = int(round(time.time() * 1000))
while True:
systemSolved = self.calculateChain(doc, mode)
totalTime = int(round(time.time() * 1000))
DebugMsg(A2P_DEBUG_1, "Total steps used: %d\n" % self.stepCount)
DebugMsg(A2P_DEBUG_1, "LoadTime (ms): %d\n" % (loadTime - startTime) )
DebugMsg(A2P_DEBUG_1, "CalcTime (ms): %d\n" % (totalTime - loadTime) )
DebugMsg(A2P_DEBUG_1, "TotalTime (ms): %d\n" % (totalTime - startTime) )
if systemSolved:
self.mySOLVER_SPIN_ACCURACY *= 1e-1
self.mySOLVER_POS_ACCURACY *= 1e-1
self.level_of_accuracy+=1
if self.level_of_accuracy == 4:
self.solutionToParts(doc)
break
self.prepareRestart()
else:
break
self.mySOLVER_SPIN_ACCURACY = SOLVER_SPIN_ACCURACY
self.mySOLVER_POS_ACCURACY = SOLVER_POS_ACCURACY
return systemSolved
def enable(self, workList):
if self.dependedRigid not in workList:
DebugMsg(
A2P_DEBUG_2,
"{} - not in working list\n".format(self)
)
return
self.Enabled = True
self.foreignDependency.Enabled = True
DebugMsg(
A2P_DEBUG_2,
"{} - enabled\n".format(self)
)
if callingFuncName != None:
'''
print (
"autoSolveConstraints called from '{}'".format(
callingFuncName
)
)
'''
solveConstraints(doc, useTransaction)
class a2p_SolverCommand:
def Activated(self):
solveConstraints(FreeCAD.ActiveDocument) #the new iterative solver
def GetResources(self):
return {
'Pixmap': path_a2p + '/icons/a2p_Solver.svg',
'MenuText': 'Solve constraints',
'ToolTip': 'Solves constraints'
}
FreeCADGui.addCommand('a2p_SolverCommand', a2p_SolverCommand())
#------------------------------------------------------------------------------
if __name__ == "__main__":
DebugMsg(A2P_DEBUG_1, "Starting solveConstraints latest script...\n")
doc = FreeCAD.activeDocument()
solveConstraints(doc)
def Create(doc, constraint, solver, rigid1, rigid2):
DebugMsg(
A2P_DEBUG_2,
"Creating dependencies between {}-{}, type {}\n".format(
rigid1.label,
rigid2.label,
constraint.Type
)
)
c = constraint
if c.Type == "pointIdentity":
dep1 = DependencyPointIdentity(c, "point")
dep2 = DependencyPointIdentity(c, "point")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
vert1 = getObjectVertexFromName(ob1, c.SubElement1)
vert2 = getObjectVertexFromName(ob2, c.SubElement2)
dep1.refPoint = vert1.Point
dep2.refPoint = vert2.Point
elif c.Type == "sphereCenterIdent":
dep1 = DependencyPointIdentity(c, "point")
dep2 = DependencyPointIdentity(c, "point")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
vert1 = getPos(ob1, c.SubElement1)
vert2 = getPos(ob2, c.SubElement2)
dep1.refPoint = vert1
dep2.refPoint = vert2
elif c.Type == "pointOnLine":
dep1 = DependencyPointOnLine(c, "point")
dep2 = DependencyPointOnLine(c, "pointAxis")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
vert1 = getObjectVertexFromName(ob1, c.SubElement1)
line2 = getObjectEdgeFromName(ob2, c.SubElement2)
dep1.refPoint = vert1.Point
dep2.refPoint = getPos(ob2, c.SubElement2)
axis2 = getAxis(ob2, c.SubElement2)
dep2.refAxisEnd = dep2.refPoint.add(axis2)
elif c.Type == "pointOnPlane":
dep1 = DependencyPointOnPlane(c, "point")
dep2 = DependencyPointOnPlane(c, "plane")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
vert1 = getObjectVertexFromName(ob1, c.SubElement1)
plane2 = getObjectFaceFromName(ob2, c.SubElement2)
dep1.refPoint = vert1.Point
dep2.refPoint = plane2.Faces[0].BoundBox.Center
normal2 = plane2.Surface.Axis
dep2.refAxisEnd = dep2.refPoint.add(normal2)
elif c.Type == "circularEdge":
dep1 = DependencyCircularEdge(c, "pointAxis")
dep2 = DependencyCircularEdge(c, "pointAxis")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
circleEdge1 = getObjectEdgeFromName(ob1, c.SubElement1)
circleEdge2 = getObjectEdgeFromName(ob2, c.SubElement2)
dep1.refPoint = circleEdge1.Curve.Center
dep2.refPoint = circleEdge2.Curve.Center
axis1 = circleEdge1.Curve.Axis
axis2 = circleEdge2.Curve.Axis
if dep2.direction == "opposed":
axis2.multiply(-1.0)
dep1.refAxisEnd = dep1.refPoint.add(axis1)
dep2.refAxisEnd = dep2.refPoint.add(axis2)
#
if abs(dep2.offset) > solver.mySOLVER_SPIN_ACCURACY * 1e-1:
offsetAdjustVec = Base.Vector(axis2.x,axis2.y,axis2.z)
offsetAdjustVec.multiply(dep2.offset)
dep2.refPoint = dep2.refPoint.add(offsetAdjustVec)
dep2.refAxisEnd = dep2.refAxisEnd.add(offsetAdjustVec)
elif c.Type == "planesParallel":
dep1 = DependencyParallelPlanes(c, "pointNormal")
dep2 = DependencyParallelPlanes(c, "pointNormal")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
plane1 = getObjectFaceFromName(ob1, c.SubElement1)
plane2 = getObjectFaceFromName(ob2, c.SubElement2)
dep1.refPoint = plane1.Faces[0].BoundBox.Center
dep2.refPoint = plane2.Faces[0].BoundBox.Center
normal1 = plane1.Surface.Axis
normal2 = plane2.Surface.Axis
if dep2.direction == "opposed":
normal2.multiply(-1.0)
dep1.refAxisEnd = dep1.refPoint.add(normal1)
dep2.refAxisEnd = dep2.refPoint.add(normal2)
elif c.Type == "angledPlanes":
dep1 = DependencyAngledPlanes(c, "pointNormal")
dep2 = DependencyAngledPlanes(c, "pointNormal")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
plane1 = getObjectFaceFromName(ob1, c.SubElement1)
plane2 = getObjectFaceFromName(ob2, c.SubElement2)
dep1.refPoint = plane1.Faces[0].BoundBox.Center
dep2.refPoint = plane2.Faces[0].BoundBox.Center
normal1 = plane1.Surface.Axis
normal2 = plane2.Surface.Axis
dep1.refAxisEnd = dep1.refPoint.add(normal1)
dep2.refAxisEnd = dep2.refPoint.add(normal2)
elif c.Type == "plane":
dep1 = DependencyPlane(c, "pointNormal")
dep2 = DependencyPlane(c, "pointNormal")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
plane1 = getObjectFaceFromName(ob1, c.SubElement1)
plane2 = getObjectFaceFromName(ob2, c.SubElement2)
dep1.refPoint = plane1.Faces[0].BoundBox.Center
dep2.refPoint = plane2.Faces[0].BoundBox.Center
normal1 = plane1.Surface.Axis
normal2 = plane2.Surface.Axis
if dep2.direction == "opposed":
normal2.multiply(-1.0)
dep1.refAxisEnd = dep1.refPoint.add(normal1)
dep2.refAxisEnd = dep2.refPoint.add(normal2)
#
if abs(dep2.offset) > solver.mySOLVER_SPIN_ACCURACY * 1e-1:
offsetAdjustVec = Base.Vector(normal2.x,normal2.y,normal2.z)
offsetAdjustVec.multiply(dep2.offset)
dep2.refPoint = dep2.refPoint.add(offsetAdjustVec)
dep2.refAxisEnd = dep2.refAxisEnd.add(offsetAdjustVec)
elif c.Type == "axial":
dep1 = DependencyAxial(c, "pointAxis")
dep2 = DependencyAxial(c, "pointAxis")
ob1 = doc.getObject(c.Object1)
ob2 = doc.getObject(c.Object2)
dep1.refPoint = getPos(ob1,c.SubElement1)
dep2.refPoint = getPos(ob2,c.SubElement2)
axis1 = getAxis(ob1, c.SubElement1)
axis2 = getAxis(ob2, c.SubElement2)
if dep2.direction == "opposed":
axis2.multiply(-1.0)
dep1.refAxisEnd = dep1.refPoint.add(axis1)
dep2.refAxisEnd = dep2.refPoint.add(axis2)
else:
raise NotImplementedError("Constraint type {} was not implemented!".format(c.Type))
# Assignments
dep1.currentRigid = rigid1
dep1.dependedRigid = rigid2
dep1.foreignDependency = dep2
dep2.currentRigid = rigid2
dep2.dependedRigid = rigid1
dep2.foreignDependency = dep1
rigid1.dependencies.append(dep1)
rigid2.dependencies.append(dep2)
def calculateWorkList(self, doc, workList, mode):
if A2P_DEBUG_LEVEL >= A2P_DEBUG_1:
self.printList("WorkList", workList)
for rig in workList:
rig.enableDependencies(workList)
self.lastPositionError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.lastAxisError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE
self.convergencyCounter = 0
calcCount = 0
goodAccuracy = False
while not goodAccuracy:
maxPosError = 0.0
maxAxisError = 0.0
calcCount += 1
self.stepCount += 1
self.convergencyCounter += 1
# First calculate all the movement vectors
for w in workList:
w.calcMoveData(doc, self)
if w.maxPosError > maxPosError:
maxPosError = w.maxPosError
if w.maxAxisError > maxAxisError:
maxAxisError = w.maxAxisError
# Perform the move
for w in workList:
w.move(doc)
# Enable those 2 lines to see the computation progress on screen
#w.applySolution(doc, self)
#FreeCADGui.updateGui()
# The accuracy is good, apply the solution to FreeCAD's objects
if (maxPosError <= self.mySOLVER_POS_ACCURACY and
maxAxisError <= self.mySOLVER_SPIN_ACCURACY):
# The accuracy is good, we're done here
goodAccuracy = True
# Mark the rigids as tempfixed and add its constrained rigids to pending list to be processed next
DebugMsg(A2P_DEBUG_1, "{} counts \n".format(calcCount) )
for r in workList:
r.applySolution(doc, self)
r.tempfixed = True
if self.convergencyCounter > SOLVER_STEPS_CONVERGENCY_CHECK:
if (
maxPosError >= self.lastPositionError or
maxAxisError >= self.lastAxisError
):
if mode == 'magnetic':
Msg( "System not solvable, convergency is incorrect!\n" )
return False
self.lastPositionError = maxPosError
self.lastAxisError = maxAxisError
self.convergencyCounter = 0
if self.stepCount > SOLVER_MAXSTEPS:
if mode == 'magnetic':
Msg( "Reached max calculations count ({})\n".format(SOLVER_MAXSTEPS) )
return False
return True