From 38cd0f56e192cfeaf12d6293fffccb08371198a1 Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sat, 4 Aug 2018 11:46:17 +0200 Subject: [PATCH 1/7] created separate file for dependencies --- dependencies.py | 520 ++++++++++++++++++++++++++++++++++++++++++++++++ solversystem.py | 472 +------------------------------------------ 2 files changed, 521 insertions(+), 471 deletions(-) create mode 100644 dependencies.py diff --git a/dependencies.py b/dependencies.py new file mode 100644 index 00000000..c381dacd --- /dev/null +++ b/dependencies.py @@ -0,0 +1,520 @@ +#*************************************************************************** +#* * +#* Copyright (c) 2018 kbwbe * +#* * +#* This program is free software; you can redistribute it and/or modify * +#* it under the terms of the GNU Lesser General Public License (LGPL) * +#* as published by the Free Software Foundation; either version 2 of * +#* the License, or (at your option) any later version. * +#* for detail see the LICENCE text file. * +#* * +#* This program is distributed in the hope that it will be useful, * +#* but WITHOUT ANY WARRANTY; without even the implied warranty of * +#* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +#* GNU Library General Public License for more details. * +#* * +#* You should have received a copy of the GNU Library General Public * +#* License along with this program; if not, write to the Free Software * +#* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * +#* USA * +#* * +#*************************************************************************** + +import random +import time +import traceback +import math +import copy +import FreeCAD, FreeCADGui, Part +from PySide import QtGui, QtCore +from FreeCAD import Base +import a2plib +from a2plib import ( + drawVector, + path_a2p, + getObjectVertexFromName, + getObjectEdgeFromName, + getObjectFaceFromName, + isLine, + getPos, + getAxis, + appVersionStr, + Msg, + DebugMsg, + A2P_DEBUG_LEVEL, + A2P_DEBUG_1, + A2P_DEBUG_2, + A2P_DEBUG_3, + ) +import os, sys + +#------------------------------------------------------------------------------ +class Dependency(): + def __init__(self, constraint, refType, axisRotation): + self.Enabled = False + self.Type = None + self.refType = refType + self.refPoint = None + self.refAxisEnd = None + self.direction = None + self.offset = None + self.angle = None + self.foreignDependency = None + self.moveVector = None # TODO: Not used? + self.currentRigid = None + self.dependedRigid = None + self.constraint = constraint # TODO: remove, probably not needed + self.axisRotationEnabled = axisRotation + + self.Type = constraint.Type + try: + self.direction = constraint.directionConstraint + except: + pass # not all constraints do have direction-Property + try: + self.offset = constraint.offset + except: + pass # not all constraints do have offset-Property + try: + self.angle = constraint.angle + except: + pass # not all constraints do have angle-Property + + + def clear(self): + self.Type = None + self.refType = None + self.refPoint = None + self.refAxisEnd = None + self.direction = None + self.offset = None + self.angle = None + self.foreignDependency = None + self.moveVector = None + self.currentRigid = None + self.dependedRigid = None + self.constraint = None + self.axisRotationEnabled = False + + def __str__(self): + return "Dependencies between {}-{}, type {}".format( + self.currentRigid.label, + self.dependedRigid.label, + self.Type + ) + + @staticmethod + 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 applyPlacement(self, placement): + if self.refPoint != None: + self.refPoint = placement.multVec(self.refPoint) + if self.refAxisEnd != None: + self.refAxisEnd = placement.multVec(self.refAxisEnd) + + 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) + ) + + def disable(self): + self.Enabled = False + self.foreignDependency.Enabled = False + + def getMovement(self): + raise NotImplementedError("Dependecly class {} doesn't implement movement, use inherited classes instead!".format(self.__class__.__name__)) + + def getRotation(self, solver): + if not self.Enabled: return None + if not self.axisRotationEnabled: return None + + # The rotation is the same for all dependinties that enabled it + # Special dependency cases are implemented in its own class + + axis = None # Rotation axis to be returned + + if self.direction != "none": + rigAxis = self.refAxisEnd.sub(self.refPoint) + foreignDep = self.foreignDependency + foreignAxis = foreignDep.refAxisEnd.sub(foreignDep.refPoint) + # + #do we have wrong alignment of axes ?? + dot = rigAxis.dot(foreignAxis) + if abs(dot+1.0) < solver.mySOLVER_SPIN_ACCURACY*1e-1: #both axes nearly aligned but false orientation... + x = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) + y = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) + z = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) + disturbVector = Base.Vector(x,y,z) + foreignAxis = foreignAxis.add(disturbVector) + + #axis = foreignAxis.cross(rigAxis) + axis = rigAxis.cross(foreignAxis) + try: + axis.normalize() + angle = foreignAxis.getAngle(rigAxis) + axis.multiply(math.degrees(angle)) + except: + axis = None + + else: #if dep.direction... (== none) + rigAxis = self.refAxisEnd.sub(self.refPoint) + foreignDep = self.foreignDependency + foreignAxis = foreignDep.refAxisEnd.sub(foreignDep.refPoint) + angle1 = abs(foreignAxis.getAngle(rigAxis)) + angle2 = math.pi-angle1 + # + if angle1<=angle2: + axis = rigAxis.cross(foreignAxis) + else: + foreignAxis.multiply(-1.0) + axis = rigAxis.cross(foreignAxis) + try: + axis.normalize() + angle = foreignAxis.getAngle(rigAxis) + axis.multiply(math.degrees(angle)) + except: + axis = None + + return axis + +#------------------------------------------------------------------------------ + +class DependencyPointIdentity(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, False) + + def getMovement(self): + if not self.Enabled: return None, None + + moveVector = self.foreignDependency.refPoint.sub(self.refPoint) + return self.refPoint, moveVector + +class DependencyPointOnLine(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, False) + + def getMovement(self): + if not self.Enabled: return None, None + + if self.refType == "point": + vec1 = self.foreignDependency.refPoint.sub(self.refPoint) + axis1 = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) + dot = vec1.dot(axis1) + axis1.multiply(dot) #projection of vec1 on axis1 + moveVector = vec1.sub(axis1) + return self.refPoint, moveVector + + elif self.refType == "pointAxis": + # refPoint is calculated in special way below + vec1 = self.foreignDependency.refPoint.sub(self.refPoint) + axis1 = self.refAxisEnd.sub(self.refPoint) + dot = vec1.dot(axis1) + axis1.multiply(dot) #projection of vec1 on axis1 + verticalRefOnLine = self.refPoint.add(axis1) #makes spinning around possible + moveVector = vec1.sub(axis1) + return verticalRefOnLine, moveVector + + else: + raise NotImplementedError("Wrong refType for class {}".format(self.__class__.__name__)) + + +class DependencyPointOnPlane(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, False) + + def getMovement(self): + if not self.Enabled: return None, None + + if self.refType == "point": + vec1 = self.foreignDependency.refPoint.sub(self.refPoint) + # Now move along foreign axis + normal1 = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) + dot = vec1.dot(normal1) + normal1.multiply(dot) + moveVector = normal1 + return self.refPoint, moveVector + + elif self.refType == "plane": + # refPoint is calculated in special way below + vec1 = self.foreignDependency.refPoint.sub(self.refPoint) + normal1 = self.refAxisEnd.sub(self.refPoint) # move along own axis + dot = vec1.dot(normal1) + normal1.multiply(dot) + moveVector = normal1 + verticalRefPointOnPlane = vec1.sub(moveVector) #makes spinning around possible + return verticalRefPointOnPlane, moveVector + + else: + raise NotImplementedError("Wrong refType for class {}".format(self.__class__.__name__)) + +class DependencyCircularEdge(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, True) + + def getMovement(self): + if not self.Enabled: return None, None + + moveVector = self.foreignDependency.refPoint.sub(self.refPoint) + return self.refPoint, moveVector + +class DependencyParallelPlanes(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, True) + + def getMovement(self): + if not self.Enabled: return None, None + + return self.refPoint, Base.Vector(0,0,0) + +class DependencyAngledPlanes(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, True) + + def getMovement(self): + if not self.Enabled: return None, None + + return self.refPoint, Base.Vector(0,0,0) + + def getRotation(self, solver): + if not self.Enabled: return None + + axis = None # Rotation axis to be returned + + rigAxis = self.refAxisEnd.sub(self.refPoint) + foreignDep = self.foreignDependency + foreignAxis = foreignDep.refAxisEnd.sub(foreignDep.refPoint) + recentAngle = math.degrees(foreignAxis.getAngle(rigAxis)) + deltaAngle = abs(self.angle.Value) - recentAngle + if abs(deltaAngle) < 1e-6: + # do not change spin, not necessary.. + axis = None + else: + try: + axis = rigAxis.cross(foreignAxis) + axis.normalize() + axis.multiply(math.degrees(-deltaAngle)) + except: #axis = Vector(0,0,0) and cannot be normalized... + x = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) + y = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) + z = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) + axis = Base.Vector(x,y,z) + #DebugMsg(A2P_DEBUG_3, "{} - rotate by {}\n".format(self, axis.Length)) + return axis + +class DependencyPlane(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, True) + + def getMovement(self): + if not self.Enabled: return None, None + + vec1 = self.foreignDependency.refPoint.sub(self.refPoint) + # move along foreign axis... + normal1 = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) + dot = vec1.dot(normal1) + normal1.multiply(dot) + moveVector = normal1 + #DebugMsg(A2P_DEBUG_3,"{} - move by {}\n".format(self, moveVector.Length)) + return self.refPoint, moveVector + +class DependencyAxial(Dependency): + def __init__(self, constraint, refType): + Dependency.__init__(self, constraint, refType, True) + + def getMovement(self): + if not self.Enabled: return None, None + + vec1 = self.foreignDependency.refPoint.sub(self.refPoint) + destinationAxis = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) + dot = vec1.dot(destinationAxis) + parallelToAxisVec = destinationAxis.normalize().multiply(dot) + moveVector = vec1.sub(parallelToAxisVec) + #DebugMsg(A2P_DEBUG_3, "{} - move by {}\n".format(self, moveVector.Length)) + return self.refPoint, moveVector + \ No newline at end of file diff --git a/solversystem.py b/solversystem.py index ca231261..8a2330bf 100644 --- a/solversystem.py +++ b/solversystem.py @@ -46,6 +46,7 @@ A2P_DEBUG_2, A2P_DEBUG_3, ) +from dependencies import Dependency import os, sys from os.path import expanduser @@ -646,477 +647,6 @@ def move(self,doc): #------------------------------------------------------------------------------ -class Dependency(): - def __init__(self, constraint, refType, axisRotation): - self.Enabled = False - self.Type = None - self.refType = refType - self.refPoint = None - self.refAxisEnd = None - self.direction = None - self.offset = None - self.angle = None - self.foreignDependency = None - self.moveVector = None # TODO: Not used? - self.currentRigid = None - self.dependedRigid = None - self.constraint = constraint # TODO: remove, probably not needed - self.axisRotationEnabled = axisRotation - - self.Type = constraint.Type - try: - self.direction = constraint.directionConstraint - except: - pass # not all constraints do have direction-Property - try: - self.offset = constraint.offset - except: - pass # not all constraints do have offset-Property - try: - self.angle = constraint.angle - except: - pass # not all constraints do have angle-Property - - - def clear(self): - self.Type = None - self.refType = None - self.refPoint = None - self.refAxisEnd = None - self.direction = None - self.offset = None - self.angle = None - self.foreignDependency = None - self.moveVector = None - self.currentRigid = None - self.dependedRigid = None - self.constraint = None - self.axisRotationEnabled = False - - def __str__(self): - return "Dependencies between {}-{}, type {}".format( - self.currentRigid.label, - self.dependedRigid.label, - self.Type - ) - - @staticmethod - 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 applyPlacement(self, placement): - if self.refPoint != None: - self.refPoint = placement.multVec(self.refPoint) - if self.refAxisEnd != None: - self.refAxisEnd = placement.multVec(self.refAxisEnd) - - 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) - ) - - def disable(self): - self.Enabled = False - self.foreignDependency.Enabled = False - - def getMovement(self): - raise NotImplementedError("Dependecly class {} doesn't implement movement, use inherited classes instead!".format(self.__class__.__name__)) - - def getRotation(self, solver): - if not self.Enabled: return None - if not self.axisRotationEnabled: return None - - # The rotation is the same for all dependinties that enabled it - # Special dependency cases are implemented in its own class - - axis = None # Rotation axis to be returned - - if self.direction != "none": - rigAxis = self.refAxisEnd.sub(self.refPoint) - foreignDep = self.foreignDependency - foreignAxis = foreignDep.refAxisEnd.sub(foreignDep.refPoint) - # - #do we have wrong alignment of axes ?? - dot = rigAxis.dot(foreignAxis) - if abs(dot+1.0) < solver.mySOLVER_SPIN_ACCURACY*1e-1: #both axes nearly aligned but false orientation... - x = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) - y = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) - z = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) - disturbVector = Base.Vector(x,y,z) - foreignAxis = foreignAxis.add(disturbVector) - - #axis = foreignAxis.cross(rigAxis) - axis = rigAxis.cross(foreignAxis) - try: - axis.normalize() - angle = foreignAxis.getAngle(rigAxis) - axis.multiply(math.degrees(angle)) - except: - axis = None - - else: #if dep.direction... (== none) - rigAxis = self.refAxisEnd.sub(self.refPoint) - foreignDep = self.foreignDependency - foreignAxis = foreignDep.refAxisEnd.sub(foreignDep.refPoint) - angle1 = abs(foreignAxis.getAngle(rigAxis)) - angle2 = math.pi-angle1 - # - if angle1<=angle2: - axis = rigAxis.cross(foreignAxis) - else: - foreignAxis.multiply(-1.0) - axis = rigAxis.cross(foreignAxis) - try: - axis.normalize() - angle = foreignAxis.getAngle(rigAxis) - axis.multiply(math.degrees(angle)) - except: - axis = None - - return axis - -#------------------------------------------------------------------------------ - -class DependencyPointIdentity(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, False) - - def getMovement(self): - if not self.Enabled: return None, None - - moveVector = self.foreignDependency.refPoint.sub(self.refPoint) - return self.refPoint, moveVector - -class DependencyPointOnLine(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, False) - - def getMovement(self): - if not self.Enabled: return None, None - - if self.refType == "point": - vec1 = self.foreignDependency.refPoint.sub(self.refPoint) - axis1 = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) - dot = vec1.dot(axis1) - axis1.multiply(dot) #projection of vec1 on axis1 - moveVector = vec1.sub(axis1) - return self.refPoint, moveVector - - elif self.refType == "pointAxis": - # refPoint is calculated in special way below - vec1 = self.foreignDependency.refPoint.sub(self.refPoint) - axis1 = self.refAxisEnd.sub(self.refPoint) - dot = vec1.dot(axis1) - axis1.multiply(dot) #projection of vec1 on axis1 - verticalRefOnLine = self.refPoint.add(axis1) #makes spinning around possible - moveVector = vec1.sub(axis1) - return verticalRefOnLine, moveVector - - else: - raise NotImplementedError("Wrong refType for class {}".format(self.__class__.__name__)) - - -class DependencyPointOnPlane(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, False) - - def getMovement(self): - if not self.Enabled: return None, None - - if self.refType == "point": - vec1 = self.foreignDependency.refPoint.sub(self.refPoint) - # Now move along foreign axis - normal1 = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) - dot = vec1.dot(normal1) - normal1.multiply(dot) - moveVector = normal1 - return self.refPoint, moveVector - - elif self.refType == "plane": - # refPoint is calculated in special way below - vec1 = self.foreignDependency.refPoint.sub(self.refPoint) - normal1 = self.refAxisEnd.sub(self.refPoint) # move along own axis - dot = vec1.dot(normal1) - normal1.multiply(dot) - moveVector = normal1 - verticalRefPointOnPlane = vec1.sub(moveVector) #makes spinning around possible - return verticalRefPointOnPlane, moveVector - - else: - raise NotImplementedError("Wrong refType for class {}".format(self.__class__.__name__)) - -class DependencyCircularEdge(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, True) - - def getMovement(self): - if not self.Enabled: return None, None - - moveVector = self.foreignDependency.refPoint.sub(self.refPoint) - return self.refPoint, moveVector - -class DependencyParallelPlanes(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, True) - - def getMovement(self): - if not self.Enabled: return None, None - - return self.refPoint, Base.Vector(0,0,0) - -class DependencyAngledPlanes(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, True) - - def getMovement(self): - if not self.Enabled: return None, None - - return self.refPoint, Base.Vector(0,0,0) - - def getRotation(self, solver): - if not self.Enabled: return None - - axis = None # Rotation axis to be returned - - rigAxis = self.refAxisEnd.sub(self.refPoint) - foreignDep = self.foreignDependency - foreignAxis = foreignDep.refAxisEnd.sub(foreignDep.refPoint) - recentAngle = math.degrees(foreignAxis.getAngle(rigAxis)) - deltaAngle = abs(self.angle.Value) - recentAngle - if abs(deltaAngle) < 1e-6: - # do not change spin, not necessary.. - axis = None - else: - try: - axis = rigAxis.cross(foreignAxis) - axis.normalize() - axis.multiply(math.degrees(-deltaAngle)) - except: #axis = Vector(0,0,0) and cannot be normalized... - x = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) - y = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) - z = random.uniform(-solver.mySOLVER_SPIN_ACCURACY*1e-1,solver.mySOLVER_SPIN_ACCURACY*1e-1) - axis = Base.Vector(x,y,z) - #DebugMsg(A2P_DEBUG_3, "{} - rotate by {}\n".format(self, axis.Length)) - return axis - -class DependencyPlane(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, True) - - def getMovement(self): - if not self.Enabled: return None, None - - vec1 = self.foreignDependency.refPoint.sub(self.refPoint) - # move along foreign axis... - normal1 = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) - dot = vec1.dot(normal1) - normal1.multiply(dot) - moveVector = normal1 - #DebugMsg(A2P_DEBUG_3,"{} - move by {}\n".format(self, moveVector.Length)) - return self.refPoint, moveVector - -class DependencyAxial(Dependency): - def __init__(self, constraint, refType): - Dependency.__init__(self, constraint, refType, True) - - def getMovement(self): - if not self.Enabled: return None, None - - vec1 = self.foreignDependency.refPoint.sub(self.refPoint) - destinationAxis = self.foreignDependency.refAxisEnd.sub(self.foreignDependency.refPoint) - dot = vec1.dot(destinationAxis) - parallelToAxisVec = destinationAxis.normalize().multiply(dot) - moveVector = vec1.sub(parallelToAxisVec) - #DebugMsg(A2P_DEBUG_3, "{} - move by {}\n".format(self, moveVector.Length)) - return self.refPoint, moveVector - - - #------------------------------------------------------------------------------ def solveConstraints( doc, cache=None ): doc.openTransaction("a2p_systemSolving") From 72f5d22991311b5b8e80db9a91a93075415f9f9c Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sat, 4 Aug 2018 11:57:56 +0200 Subject: [PATCH 2/7] new separate file for class Rigid() --- rigid.py | 320 ++++++++++++++++++++++++++++++++++++++++++++++++ solversystem.py | 274 +---------------------------------------- 2 files changed, 321 insertions(+), 273 deletions(-) create mode 100644 rigid.py diff --git a/rigid.py b/rigid.py new file mode 100644 index 00000000..7cb0cb8e --- /dev/null +++ b/rigid.py @@ -0,0 +1,320 @@ +#*************************************************************************** +#* * +#* Copyright (c) 2018 kbwbe * +#* * +#* This program is free software; you can redistribute it and/or modify * +#* it under the terms of the GNU Lesser General Public License (LGPL) * +#* as published by the Free Software Foundation; either version 2 of * +#* the License, or (at your option) any later version. * +#* for detail see the LICENCE text file. * +#* * +#* This program is distributed in the hope that it will be useful, * +#* but WITHOUT ANY WARRANTY; without even the implied warranty of * +#* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +#* GNU Library General Public License for more details. * +#* * +#* You should have received a copy of the GNU Library General Public * +#* License along with this program; if not, write to the Free Software * +#* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * +#* USA * +#* * +#*************************************************************************** + +import random +import time +import traceback +import math +import copy +import FreeCAD, FreeCADGui, Part +from PySide import QtGui, QtCore +from FreeCAD import Base +import a2plib +from a2plib import ( + drawVector, + path_a2p, + getObjectVertexFromName, + getObjectEdgeFromName, + getObjectFaceFromName, + isLine, + getPos, + getAxis, + appVersionStr, + Msg, + DebugMsg, + A2P_DEBUG_LEVEL, + A2P_DEBUG_1, + A2P_DEBUG_2, + A2P_DEBUG_3, + ) +import os, sys + + +SPINSTEP_DIVISOR = 12.0 +WEIGHT_LINEAR_MOVE = 0.5 +WEIGHT_REFPOINT_ROTATION = 8.0 + + + +class Rigid(): + ''' All data necessary for one rigid body''' + def __init__(self, + name, + label, + fixed, + placement + ): + self.objectName = name + self.label = label + self.fixed = fixed + self.tempfixed = fixed + self.placement = placement + self.savedPlacement = placement + self.dependencies = [] + self.linkedRigids = [] + self.parentRigids = [] + self.childRigids = [] + self.disatanceFromFixed = None + self.spinCenter = None + self.spin = None + self.moveVectorSum = None + self.maxPosError = 0.0 + self.maxAxisError = 0.0 + self.refPointsBoundBoxSize = 0.0 + self.countSpinVectors = 0 + + def prepareRestart(self): + self.tempfixed = False + for d in self.dependencies: + d.disable() + + + def enableDependencies(self, workList): + for dep in self.dependencies: + dep.enable(workList) + + # The function only sets parentship for childrens that are distant+1 from fixed rigid + # The function should be called in a loop with increased distance until it return False + def assignParentship(self, distance): + #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") + #FreeCAD.Console.PrintMessage("In {}:{}, distance {}\n".format(self.label, self.disatanceFromFixed, distance)) + # Current rigid was already set, pass the call to childrens + if self.disatanceFromFixed < distance: + haveMore = False + for rig in self.childRigids: + #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") + #FreeCAD.Console.PrintMessage(" passing to {}:{}, distance {}\n".format(rig.label, rig.disatanceFromFixed, distance)) + if rig.assignParentship(distance): + haveMore = True + return haveMore + elif self.disatanceFromFixed == distance: + while len(self.linkedRigids) > 0: + rig = self.linkedRigids[0] + # Got to a new rigid, set current as it's father + if rig.disatanceFromFixed is None: + #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") + #FreeCAD.Console.PrintMessage(" setting {}:{} with distance {}\n".format(rig.label, rig.disatanceFromFixed, distance+1)) + rig.parentRigids.append(self) + self.childRigids.append(rig) + rig.linkedRigids.remove(self) + self.linkedRigids.remove(rig) + rig.disatanceFromFixed = distance+1 + # That child was already assigned by another (and closer to fixed) father + # Leave only child relationship, but don't add current as a father + else: + #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") + #FreeCAD.Console.PrintMessage(" the {}:{} was already set, ignore\n".format(rig.label, rig.disatanceFromFixed)) + self.childRigids.append(rig) + rig.linkedRigids.remove(self) + self.linkedRigids.remove(rig) + + if len(self.childRigids) + len(self.linkedRigids) > 0: return True + else: return False +# else: +# FreeCAD.Console.PrintMessage("Should not happen: {}:{} got distance {}\n".format(self.label, self.disatanceFromFixed, distance)) + + + def printHierarchy(self, level): + Msg((level*3)*" ") + Msg("{} - distance {}\n".format(self.label, self.disatanceFromFixed)) + for rig in self.childRigids: + rig.printHierarchy(level+1) + + def getCandidates(self): + candidates = [] + for rig in self.childRigids: + if not rig.tempfixed and rig.areAllParentTempFixed(): + candidates.append(rig) + return set(candidates) + + def addChildrenByDistance(self, addList, distance): + # Current rigid is the father of the needed distance, so it might have needed children + if self.disatanceFromFixed == distance-1: + # No children + if len(self.childRigids) == 0: return False + else: + # There are some childrens, add with the matching distance + for rig in self.childRigids: + if rig.disatanceFromFixed == distance: + addList.append(rig) + # That rigid have children for needed distance + else: return False + + def areAllParentTempFixed(self): + for rig in self.parentRigids: + if not rig.tempfixed: + return False + return True + + def applyPlacementStep(self, pl): + self.placement = pl.multiply(self.placement) + self.spinCenter = pl.multVec(self.spinCenter) + # Update dependencies + for dep in self.dependencies: + dep.applyPlacement(pl) + + def clear(self): + for d in self.dependencies: + d.clear() + self.dependencies = [] + + def applySolution(self, doc, solver): + if self.tempfixed or self.fixed: return + + # Update FreeCAD's placements if deltaPlacement above Tolerances + base1 = self.placement.Base + base2 = self.savedPlacement.Base + absPosMove = base1.sub(base2).Length + + axis1 = self.placement.Rotation.Axis + axis2 = self.savedPlacement.Rotation.Axis + angle = math.degrees(axis2.getAngle(axis1)) + + if absPosMove >= solver.mySOLVER_POS_ACCURACY*1e-2 or angle >= solver.mySOLVER_SPIN_ACCURACY*1e-1: + ob1 = doc.getObject(self.objectName) + ob1.Placement = self.placement + + def calcSpinCenter(self): + newSpinCenter = Base.Vector(0,0,0) + countRefPoints = 0 + for dep in self.dependencies: + if dep.refPoint != None: + newSpinCenter = newSpinCenter.add(dep.refPoint) + countRefPoints += 1 + if countRefPoints > 0: + newSpinCenter.multiply(1.0/countRefPoints) + self.spinCenter = newSpinCenter + + def calcRefPointsBoundBoxSize(self): + xmin = 0 + xmax = 0 + ymin = 0 + ymax = 0 + zmin = 0 + zmax = 0 + for dep in self.dependencies: + if dep.refPoint.x < xmin: xmin=dep.refPoint.x + if dep.refPoint.x > xmax: xmax=dep.refPoint.x + if dep.refPoint.y < ymin: ymin=dep.refPoint.y + if dep.refPoint.y > ymax: ymax=dep.refPoint.y + if dep.refPoint.z < zmin: zmin=dep.refPoint.z + if dep.refPoint.z > zmax: zmax=dep.refPoint.z + self.refPointsBoundBoxSize = math.sqrt( (xmax-xmin)**2 + (ymax-ymin)**2 + (zmax-zmin)**2 ) + + def calcMoveData(self, doc, solver): + if self.tempfixed or self.fixed: return + depRefPoints = [] + depMoveVectors = [] #collect Data to compute central movement of rigid + # + self.maxPosError = 0.0 + self.maxAxisError = 0.0 + self.countSpinVectors = 0 + self.moveVectorSum = Base.Vector(0,0,0) + + for dep in self.dependencies: + refPoint, moveVector = dep.getMovement() + if refPoint is None or moveVector is None: continue # Should not happen + + depRefPoints.append(refPoint) + depMoveVectors.append(moveVector) + + # Calculate max move error + if moveVector.Length > self.maxPosError: self.maxPosError = moveVector.Length + + # Accomulate all the movements for later average calculations + self.moveVectorSum = self.moveVectorSum.add(moveVector) + + # Calculate the average of all the movements + if len(depMoveVectors) > 0: + self.moveVectorSum = self.moveVectorSum.multiply(1.0/len(depMoveVectors)) + + #compute rotation caused by refPoint-attractions and axes mismatch + if len(depMoveVectors) > 0 and self.spinCenter != None: + self.spin = Base.Vector(0,0,0) + + #realMoveVectorSum = FreeCAD.Vector(self.moveVectorSum) + #realMoveVectorSum.multiply(WEIGHT_LINEAR_MOVE) + for i in range(0, len(depRefPoints)): + try: + vec1 = depRefPoints[i].sub(self.spinCenter) # 'aka Radius' + vec2 = depMoveVectors[i].sub(self.moveVectorSum) # 'aka Force' + axis = vec1.cross(vec2) #torque-vector + + vec1.normalize() + vec1.multiply(self.refPointsBoundBoxSize) + vec3 = vec1.add(vec2) + beta = vec3.getAngle(vec1) + + axis.normalize() + axis.multiply(math.degrees(beta)*WEIGHT_REFPOINT_ROTATION) #here use degrees + self.spin = self.spin.add(axis) + self.countSpinVectors += 1 + except: + pass #numerical exception above, no spin ! + + #adjust axis' of the dependencies //FIXME (align,opposed,none) + + for dep in self.dependencies: + rotation = dep.getRotation(solver) + + if rotation is None: continue # No rotation for that dep + + # Calculate max rotation error + axisErr = self.spin.Length + if axisErr > self.maxAxisError : self.maxAxisError = axisErr + + # Accumulate all rotations for later average calculation + self.spin = self.spin.add(rotation) + self.countSpinVectors += 1 + + def move(self,doc): + if self.tempfixed or self.fixed: return + # + #Linear moving of a rigid + moveDist = Base.Vector(0,0,0) + if self.moveVectorSum != None: + moveDist = Base.Vector(self.moveVectorSum) + moveDist.multiply(WEIGHT_LINEAR_MOVE) # stabilize computation, adjust if needed... + # + #Rotate the rigid... + center = None + rotation = None + if (self.spin != None and self.spin.Length != 0.0 and self.countSpinVectors != 0): + spinAngle = self.spin.Length / self.countSpinVectors + if spinAngle>15.0: spinAngle=15.0 # do not accept more degrees + if spinAngle> 1e-8: + try: + spinStep = spinAngle/(SPINSTEP_DIVISOR) #it was 250.0 + self.spin.normalize() + rotation = FreeCAD.Rotation(self.spin, spinStep) + center = self.spinCenter + except: + pass + + if center != None and rotation != None: + pl = FreeCAD.Placement(moveDist,rotation,center) + self.applyPlacementStep(pl) + else: + if moveDist.Length > 1e-8: + pl = FreeCAD.Placement() + pl.move(moveDist) + self.applyPlacementStep(pl) diff --git a/solversystem.py b/solversystem.py index 8a2330bf..c0b05c76 100644 --- a/solversystem.py +++ b/solversystem.py @@ -47,6 +47,7 @@ A2P_DEBUG_3, ) from dependencies import Dependency +from rigid import Rigid import os, sys from os.path import expanduser @@ -55,10 +56,6 @@ SOLVER_POS_ACCURACY = 1.0e-1 # gets to smaller values during solving SOLVER_SPIN_ACCURACY = 1.0e-1 # gets to smaller values during solving -SPINSTEP_DIVISOR = 12.0 -WEIGHT_LINEAR_MOVE = 0.5 -WEIGHT_REFPOINT_ROTATION = 8.0 - SOLVER_STEPS_CONVERGENCY_CHECK = 1000 SOLVER_CONVERGENCY_ERROR_INIT_VALUE = 1.0e+20 @@ -378,275 +375,6 @@ def solutionToParts(self,doc): for rig in self.rigids: rig.applySolution(doc, self); -#------------------------------------------------------------------------------ -class Rigid(): - ''' All data necessary for one rigid body''' - def __init__(self, - name, - label, - fixed, - placement - ): - self.objectName = name - self.label = label - self.fixed = fixed - self.tempfixed = fixed - self.placement = placement - self.savedPlacement = placement - self.dependencies = [] - self.linkedRigids = [] - self.parentRigids = [] - self.childRigids = [] - self.disatanceFromFixed = None - self.spinCenter = None - self.spin = None - self.moveVectorSum = None - self.maxPosError = 0.0 - self.maxAxisError = 0.0 - self.refPointsBoundBoxSize = 0.0 - self.countSpinVectors = 0 - - def prepareRestart(self): - self.tempfixed = False - for d in self.dependencies: - d.disable() - - - def enableDependencies(self, workList): - for dep in self.dependencies: - dep.enable(workList) - - # The function only sets parentship for childrens that are distant+1 from fixed rigid - # The function should be called in a loop with increased distance until it return False - def assignParentship(self, distance): - #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") - #FreeCAD.Console.PrintMessage("In {}:{}, distance {}\n".format(self.label, self.disatanceFromFixed, distance)) - # Current rigid was already set, pass the call to childrens - if self.disatanceFromFixed < distance: - haveMore = False - for rig in self.childRigids: - #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") - #FreeCAD.Console.PrintMessage(" passing to {}:{}, distance {}\n".format(rig.label, rig.disatanceFromFixed, distance)) - if rig.assignParentship(distance): - haveMore = True - return haveMore - elif self.disatanceFromFixed == distance: - while len(self.linkedRigids) > 0: - rig = self.linkedRigids[0] - # Got to a new rigid, set current as it's father - if rig.disatanceFromFixed is None: - #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") - #FreeCAD.Console.PrintMessage(" setting {}:{} with distance {}\n".format(rig.label, rig.disatanceFromFixed, distance+1)) - rig.parentRigids.append(self) - self.childRigids.append(rig) - rig.linkedRigids.remove(self) - self.linkedRigids.remove(rig) - rig.disatanceFromFixed = distance+1 - # That child was already assigned by another (and closer to fixed) father - # Leave only child relationship, but don't add current as a father - else: - #FreeCAD.Console.PrintMessage((self.disatanceFromFixed*3)*" ") - #FreeCAD.Console.PrintMessage(" the {}:{} was already set, ignore\n".format(rig.label, rig.disatanceFromFixed)) - self.childRigids.append(rig) - rig.linkedRigids.remove(self) - self.linkedRigids.remove(rig) - - if len(self.childRigids) + len(self.linkedRigids) > 0: return True - else: return False -# else: -# FreeCAD.Console.PrintMessage("Should not happen: {}:{} got distance {}\n".format(self.label, self.disatanceFromFixed, distance)) - - - def printHierarchy(self, level): - Msg((level*3)*" ") - Msg("{} - distance {}\n".format(self.label, self.disatanceFromFixed)) - for rig in self.childRigids: - rig.printHierarchy(level+1) - - def getCandidates(self): - candidates = [] - for rig in self.childRigids: - if not rig.tempfixed and rig.areAllParentTempFixed(): - candidates.append(rig) - return set(candidates) - - def addChildrenByDistance(self, addList, distance): - # Current rigid is the father of the needed distance, so it might have needed children - if self.disatanceFromFixed == distance-1: - # No children - if len(self.childRigids) == 0: return False - else: - # There are some childrens, add with the matching distance - for rig in self.childRigids: - if rig.disatanceFromFixed == distance: - addList.append(rig) - # That rigid have children for needed distance - else: return False - - def areAllParentTempFixed(self): - for rig in self.parentRigids: - if not rig.tempfixed: - return False - return True - - def applyPlacementStep(self, pl): - self.placement = pl.multiply(self.placement) - self.spinCenter = pl.multVec(self.spinCenter) - # Update dependencies - for dep in self.dependencies: - dep.applyPlacement(pl) - - def clear(self): - for d in self.dependencies: - d.clear() - self.dependencies = [] - - def applySolution(self, doc, solver): - if self.tempfixed or self.fixed: return - - # Update FreeCAD's placements if deltaPlacement above Tolerances - base1 = self.placement.Base - base2 = self.savedPlacement.Base - absPosMove = base1.sub(base2).Length - - axis1 = self.placement.Rotation.Axis - axis2 = self.savedPlacement.Rotation.Axis - angle = math.degrees(axis2.getAngle(axis1)) - - if absPosMove >= solver.mySOLVER_POS_ACCURACY*1e-2 or angle >= solver.mySOLVER_SPIN_ACCURACY*1e-1: - ob1 = doc.getObject(self.objectName) - ob1.Placement = self.placement - - def calcSpinCenter(self): - newSpinCenter = Base.Vector(0,0,0) - countRefPoints = 0 - for dep in self.dependencies: - if dep.refPoint != None: - newSpinCenter = newSpinCenter.add(dep.refPoint) - countRefPoints += 1 - if countRefPoints > 0: - newSpinCenter.multiply(1.0/countRefPoints) - self.spinCenter = newSpinCenter - - def calcRefPointsBoundBoxSize(self): - xmin = 0 - xmax = 0 - ymin = 0 - ymax = 0 - zmin = 0 - zmax = 0 - for dep in self.dependencies: - if dep.refPoint.x < xmin: xmin=dep.refPoint.x - if dep.refPoint.x > xmax: xmax=dep.refPoint.x - if dep.refPoint.y < ymin: ymin=dep.refPoint.y - if dep.refPoint.y > ymax: ymax=dep.refPoint.y - if dep.refPoint.z < zmin: zmin=dep.refPoint.z - if dep.refPoint.z > zmax: zmax=dep.refPoint.z - self.refPointsBoundBoxSize = math.sqrt( (xmax-xmin)**2 + (ymax-ymin)**2 + (zmax-zmin)**2 ) - - def calcMoveData(self, doc, solver): - if self.tempfixed or self.fixed: return - depRefPoints = [] - depMoveVectors = [] #collect Data to compute central movement of rigid - # - self.maxPosError = 0.0 - self.maxAxisError = 0.0 - self.countSpinVectors = 0 - self.moveVectorSum = Base.Vector(0,0,0) - - for dep in self.dependencies: - refPoint, moveVector = dep.getMovement() - if refPoint is None or moveVector is None: continue # Should not happen - - depRefPoints.append(refPoint) - depMoveVectors.append(moveVector) - - # Calculate max move error - if moveVector.Length > self.maxPosError: self.maxPosError = moveVector.Length - - # Accomulate all the movements for later average calculations - self.moveVectorSum = self.moveVectorSum.add(moveVector) - - # Calculate the average of all the movements - if len(depMoveVectors) > 0: - self.moveVectorSum = self.moveVectorSum.multiply(1.0/len(depMoveVectors)) - - #compute rotation caused by refPoint-attractions and axes mismatch - if len(depMoveVectors) > 0 and self.spinCenter != None: - self.spin = Base.Vector(0,0,0) - - #realMoveVectorSum = FreeCAD.Vector(self.moveVectorSum) - #realMoveVectorSum.multiply(WEIGHT_LINEAR_MOVE) - for i in range(0, len(depRefPoints)): - try: - vec1 = depRefPoints[i].sub(self.spinCenter) # 'aka Radius' - vec2 = depMoveVectors[i].sub(self.moveVectorSum) # 'aka Force' - axis = vec1.cross(vec2) #torque-vector - - vec1.normalize() - vec1.multiply(self.refPointsBoundBoxSize) - vec3 = vec1.add(vec2) - beta = vec3.getAngle(vec1) - - axis.normalize() - axis.multiply(math.degrees(beta)*WEIGHT_REFPOINT_ROTATION) #here use degrees - self.spin = self.spin.add(axis) - self.countSpinVectors += 1 - except: - pass #numerical exception above, no spin ! - - #adjust axis' of the dependencies //FIXME (align,opposed,none) - - for dep in self.dependencies: - rotation = dep.getRotation(solver) - - if rotation is None: continue # No rotation for that dep - - # Calculate max rotation error - axisErr = self.spin.Length - if axisErr > self.maxAxisError : self.maxAxisError = axisErr - - # Accumulate all rotations for later average calculation - self.spin = self.spin.add(rotation) - self.countSpinVectors += 1 - - def move(self,doc): - if self.tempfixed or self.fixed: return - # - #Linear moving of a rigid - moveDist = Base.Vector(0,0,0) - if self.moveVectorSum != None: - moveDist = Base.Vector(self.moveVectorSum) - moveDist.multiply(WEIGHT_LINEAR_MOVE) # stabilize computation, adjust if needed... - # - #Rotate the rigid... - center = None - rotation = None - if (self.spin != None and self.spin.Length != 0.0 and self.countSpinVectors != 0): - spinAngle = self.spin.Length / self.countSpinVectors - if spinAngle>15.0: spinAngle=15.0 # do not accept more degrees - if spinAngle> 1e-8: - try: - spinStep = spinAngle/(SPINSTEP_DIVISOR) #it was 250.0 - self.spin.normalize() - rotation = FreeCAD.Rotation(self.spin, spinStep) - center = self.spinCenter - except: - pass - - if center != None and rotation != None: - pl = FreeCAD.Placement(moveDist,rotation,center) - self.applyPlacementStep(pl) - else: - if moveDist.Length > 1e-8: - pl = FreeCAD.Placement() - pl.move(moveDist) - self.applyPlacementStep(pl) - - - - -#------------------------------------------------------------------------------ #------------------------------------------------------------------------------ def solveConstraints( doc, cache=None ): doc.openTransaction("a2p_systemSolving") From 5c4ab3a5360e982aea0a0e6694ae4260c1aa8912 Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sat, 4 Aug 2018 12:24:06 +0200 Subject: [PATCH 3/7] files renamed --- InitGui.py | 10 +++++----- dependencies.py => a2p_dependencies.py | 0 a2p_importpart.py | 2 +- observers.py => a2p_observers.py | 0 rigid.py => a2p_rigid.py | 0 solversystem.py => a2p_solversystem.py | 4 ++-- a2p_viewProviderProxies.py | 2 +- 7 files changed, 9 insertions(+), 9 deletions(-) rename dependencies.py => a2p_dependencies.py (100%) rename observers.py => a2p_observers.py (100%) rename rigid.py => a2p_rigid.py (100%) rename solversystem.py => a2p_solversystem.py (99%) diff --git a/InitGui.py b/InitGui.py index 417122f4..199bbf24 100644 --- a/InitGui.py +++ b/InitGui.py @@ -48,7 +48,7 @@ def Initialize(self): import a2p_pointOnLineConstraint import a2p_pointOnPlaneConstraint import a2p_sphericalConnection - import solversystem + import a2p_solversystem import a2p_MuxAssembly commandslist = [ @@ -95,12 +95,12 @@ def Initialize(self): def Activated(self): - import observers - FreeCAD.addDocumentObserver(observers.redoUndoObserver) + import a2p_observers + FreeCAD.addDocumentObserver(a2p_observers.redoUndoObserver) def Deactivated(self): - import observers - FreeCAD.removeDocumentObserver(observers.redoUndoObserver) + import a2p_observers + FreeCAD.removeDocumentObserver(a2p_observers.redoUndoObserver) def ContextMenu(self, recipient): import FreeCAD, FreeCADGui diff --git a/dependencies.py b/a2p_dependencies.py similarity index 100% rename from dependencies.py rename to a2p_dependencies.py diff --git a/a2p_importpart.py b/a2p_importpart.py index 07b37330..5816bfb0 100644 --- a/a2p_importpart.py +++ b/a2p_importpart.py @@ -29,7 +29,7 @@ from a2p_MuxAssembly import muxObjectsWithKeys, createTopoInfo, Proxy_muxAssemblyObj from a2p_viewProviderProxies import * from a2p_versionmanagement import SubAssemblyWalk, A2P_VERSION -import solversystem +import a2p_solversystem from a2plib import ( appVersionStr, AUTOSOLVE_ENABLED diff --git a/observers.py b/a2p_observers.py similarity index 100% rename from observers.py rename to a2p_observers.py diff --git a/rigid.py b/a2p_rigid.py similarity index 100% rename from rigid.py rename to a2p_rigid.py diff --git a/solversystem.py b/a2p_solversystem.py similarity index 99% rename from solversystem.py rename to a2p_solversystem.py index c0b05c76..f856183e 100644 --- a/solversystem.py +++ b/a2p_solversystem.py @@ -46,8 +46,8 @@ A2P_DEBUG_2, A2P_DEBUG_3, ) -from dependencies import Dependency -from rigid import Rigid +from a2p_dependencies import Dependency +from a2p_rigid import Rigid import os, sys from os.path import expanduser diff --git a/a2p_viewProviderProxies.py b/a2p_viewProviderProxies.py index 75982882..0644ff69 100644 --- a/a2p_viewProviderProxies.py +++ b/a2p_viewProviderProxies.py @@ -235,7 +235,7 @@ def reduceDirectionChoices( self, obj, value): obj.directionConstraint = value def callSolveConstraints(self): - from solversystem import autoSolveConstraints + from a2p_solversystem import autoSolveConstraints autoSolveConstraints( FreeCAD.activeDocument(), cache = None ) From be601d5a3ca517c364da2df595863cf3615ed7ac Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sat, 4 Aug 2018 15:00:34 +0200 Subject: [PATCH 4/7] prevent solving of broken constraints --- a2p_importpart.py | 2 +- a2p_solversystem.py | 40 ++++++++++++++++++++++++++++++++++++++-- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/a2p_importpart.py b/a2p_importpart.py index 5816bfb0..fad64936 100644 --- a/a2p_importpart.py +++ b/a2p_importpart.py @@ -296,7 +296,7 @@ def updateImportedParts(doc): sub.showMaximized() objectCache.cleanUp(doc) - solversystem.solveConstraints(doc) + a2p_solversystem.autoSolveConstraints(doc) doc.recompute() diff --git a/a2p_solversystem.py b/a2p_solversystem.py index f856183e..04d79a43 100644 --- a/a2p_solversystem.py +++ b/a2p_solversystem.py @@ -77,6 +77,7 @@ def __init__(self): self.lastPositionError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE self.lastAxisError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE self.convergencyCounter = 0 + self.status = "created" def clear(self): for r in self.rigids: @@ -95,6 +96,7 @@ def getRigid(self,objectName): def loadSystem(self,doc): self.clear() self.doc = doc + self.status = "loading" # self.convergencyCounter = 0 self.lastPositionError = SOLVER_CONVERGENCY_ERROR_INIT_VALUE @@ -127,6 +129,7 @@ def loadSystem(self,doc): self.rigids.append(rig) # #link constraints to rigids using dependencies + deleteList = [] # a list to collect broken constraints for c in self.constraints: rigid1 = self.getRigid(c.Object1) rigid2 = self.getRigid(c.Object2) @@ -134,11 +137,37 @@ def loadSystem(self,doc): rigid1.linkedRigids.append(rigid2); rigid2.linkedRigids.append(rigid1); - Dependency.Create(doc, c, self, rigid1, rigid2) - + try: + Dependency.Create(doc, c, self, rigid1, rigid2) + except: + self.status = "loadingDependencyError" + deleteList.append(c) + + if len(deleteList) > 0: + msg = "The following constraints are broken:\n" + for c in deleteList: + msg += "{}\n".format(c.Label) + msg += "Do you want to delete them ?" + + flags = QtGui.QMessageBox.StandardButton.Yes | QtGui.QMessageBox.StandardButton.No + response = QtGui.QMessageBox.critical( + QtGui.QApplication.activeWindow(), + "Delete broken constraints?", + msg, + flags + ) + if response == QtGui.QMessageBox.Yes: + for c in deleteList: + a2plib.removeConstraint(c) + + if self.status == "loadingDependencyError": + return + for rig in self.rigids: rig.calcSpinCenter() rig.calcRefPointsBoundBoxSize() + + self.status = "loaded" # TODO: maybe instead of traversing from the root every time, save a list of objects on current distance @@ -216,6 +245,8 @@ def solveSystemWithMode(self,doc, mode): 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: @@ -249,13 +280,18 @@ def solveSystem(self,doc): mode = 'magnetic' systemSolved = self.solveSystemWithMode(doc,mode) + if self.status == "loadingDependencyError": + return + if not systemSolved and mode == 'partial': Msg( "Could not solve system with partial processing, switch to 'magnetic' mode \n" ) mode = 'magnetic' systemSolved = self.solveSystemWithMode(doc,mode) if systemSolved: + self.status = "solved" Msg( "===== System solved ! ====== \n" ) else: + self.status = "unsolved" Msg( "===== Could not solve system ====== \n" ) msg = \ ''' From c45b69d1c51a691294b77a23b0076a758265962b Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sun, 5 Aug 2018 14:04:20 +0200 Subject: [PATCH 5/7] reworked viewproviders --- a2p_viewProviderProxies.py | 63 +++++++++++++++++++++++++++++++------- a2plib.py | 4 +-- 2 files changed, 54 insertions(+), 13 deletions(-) diff --git a/a2p_viewProviderProxies.py b/a2p_viewProviderProxies.py index 0644ff69..ecc6ba66 100644 --- a/a2p_viewProviderProxies.py +++ b/a2p_viewProviderProxies.py @@ -26,6 +26,8 @@ from PySide import QtGui, QtCore from pivy import coin import traceback +import a2plib + #--------------------------------------------------------------------------- # Module global vars, automatically managed, hands off !! @@ -43,7 +45,11 @@ class ImportedPartViewProviderProxy: def claimChildren(self): if hasattr(self,'Object'): - return self.Object.InList + try: + return self.Object.InList + except: + #FreeCAD has already deleted self.Object !! + return[] else: return [] @@ -59,13 +65,10 @@ def onDelete(self, viewObject, subelements): # subelements is a tuple of strings if 'ConstraintInfo' in c.Content: # a related Constraint if obj.Name in [ c.Object1, c.Object2 ]: deleteList.append(c) - if 'ConstraintNfo' in c.Content: # a related mirror-Constraint - if obj.Name in [ c.Object1, c.Object2 ]: - deleteList.append(c) if len(deleteList) > 0: for c in deleteList: - doc.removeObject(c.Name) + a2plib.removeConstraint(c) #also deletes the mirrors... return True # If False is returned the object won't be deleted @@ -117,6 +120,7 @@ def __init__( mirrorLabel, extraLabel ) + self.enableDeleteCounterPart = True #allow to delete the mirror def getIcon(self): return self.iconPath @@ -134,25 +138,62 @@ def onDelete(self, viewObject, subelements): # subelements is a tuple of strings if FreeCAD.activeDocument() != viewObject.Object.Document: return False + if not hasattr(self,'enableDeleteCounterPart'): + self.enableDeleteCounterPart = True + + if not self.enableDeleteCounterPart: return True # nothing more to do... + + # first delete the mirror... obj = viewObject.Object doc = obj.Document - if isinstance( obj.Proxy, ConstraintMirrorObjectProxy ): + if hasattr( obj.Proxy, 'mirror_name'): try: - doc.removeObject( obj.Proxy.constraintObj_name ) # also delete the original constraint which obj mirrors + m = doc.getObject(obj.Proxy.mirror_name) + m.Proxy.enableDeleteCounterPart = False + doc.removeObject( obj.Proxy.mirror_name ) # also delete mirror except: - pass # bloede Fehlermeldung weg, wenn Original fehlt! (Klaus) - elif hasattr( obj.Proxy, 'mirror_name'): # the original constraint, #isinstance( obj.Proxy, ConstraintObjectProxy ) not done since ConstraintObjectProxy not defined in namespace - doc.removeObject( obj.Proxy.mirror_name ) # also delete mirror + pass # if mirror is already deleted... return True -class ConstraintMirrorViewProviderProxy( ConstraintViewProviderProxy ): +class ConstraintMirrorViewProviderProxy: def __init__( self, constraintObj, iconPath ): self.iconPath = iconPath self.constraintObj_name = constraintObj.Name + self.enableDeleteCounterPart = True #allow to delete the original of the mirror + + def getIcon(self): + return self.iconPath + def attach(self, vobj): vobj.addDisplayMode( coin.SoGroup(),"Standard" ) + def getDisplayModes(self,obj): + return ["Standard"] + + def getDefaultDisplayMode(self): + return "Standard" + + def onDelete(self, viewObject, subelements): # subelements is a tuple of strings + if FreeCAD.activeDocument() != viewObject.Object.Document: + return False + + if not hasattr(self,'enableDeleteCounterPart'): + self.enableDeleteCounterPart = True + + if not self.enableDeleteCounterPart: return True # nothing more to do... + + # First delete the original... + obj = viewObject.Object + doc = obj.Document + try: + c = doc.getObject(obj.Proxy.constraintObj_name) + c.Proxy.enableDeleteCounterPart = False + doc.removeObject(obj.Proxy.constraintObj_name) # also delete the original + except: + pass # if original has already been removed... + return True + def create_constraint_mirror( constraintObj, iconPath, origLabel= '', mirrorLabel='', extraLabel = '' ): #FreeCAD.Console.PrintMessage("creating constraint mirror\n") diff --git a/a2plib.py b/a2plib.py index 694f8cc1..32a29f4e 100644 --- a/a2plib.py +++ b/a2plib.py @@ -435,8 +435,8 @@ def getObjectVertexFromName( obj, name ): def removeConstraint( constraint ): 'required as constraint.Proxy.onDelete only called when deleted through GUI' doc = constraint.Document - if constraint.ViewObject != None: #do not this check is actually nessary ... - constraint.ViewObject.Proxy.onDelete( constraint.ViewObject, None ) + if constraint.ViewObject != None: + constraint.ViewObject.Proxy.onDelete( constraint.ViewObject, None ) # also removes mirror... doc.removeObject( constraint.Name ) #------------------------------------------------------------------------------ def getPos(obj, subElementName): From 00fdec0554c8c92e11ccbee5207a63ef43b15653 Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sun, 5 Aug 2018 15:36:57 +0200 Subject: [PATCH 6/7] small code clean-up --- a2p_viewProviderProxies.py | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/a2p_viewProviderProxies.py b/a2p_viewProviderProxies.py index ecc6ba66..62a4216e 100644 --- a/a2p_viewProviderProxies.py +++ b/a2p_viewProviderProxies.py @@ -35,12 +35,6 @@ a2p_NeedToSolveSystem = False #--------------------------------------------------------------------------- -def group_constraints_under_parts(): - return True - -def allow_deletetion_when_activice_doc_ne_object_doc(): - return False - class ImportedPartViewProviderProxy: def claimChildren(self): @@ -92,6 +86,7 @@ def __init__( self, proxy, menu, label, Freecad_cmd ): action = menu.addAction(label) action.triggered.connect( self.execute ) proxy.pop_up_menu_items.append( self ) + def execute( self ): try: FreeCADGui.runCommand( self.Freecad_cmd ) From aa61ef10a678fe82872785302e65e70cf88d33a9 Mon Sep 17 00:00:00 2001 From: kbwbe Date: Sun, 5 Aug 2018 16:34:41 +0200 Subject: [PATCH 7/7] fixed wrong axisrotation in class Rigid --- a2p_rigid.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/a2p_rigid.py b/a2p_rigid.py index 7cb0cb8e..bb9e744c 100644 --- a/a2p_rigid.py +++ b/a2p_rigid.py @@ -278,14 +278,14 @@ def calcMoveData(self, doc, solver): if rotation is None: continue # No rotation for that dep - # Calculate max rotation error - axisErr = self.spin.Length - if axisErr > self.maxAxisError : self.maxAxisError = axisErr - # Accumulate all rotations for later average calculation self.spin = self.spin.add(rotation) self.countSpinVectors += 1 + # Calculate max rotation error + axisErr = self.spin.Length + if axisErr > self.maxAxisError : self.maxAxisError = axisErr + def move(self,doc): if self.tempfixed or self.fixed: return #