From 48173f2b9d98c2ca1112c39673ac0f082a0e61be Mon Sep 17 00:00:00 2001 From: Paddle Date: Sat, 23 Dec 2023 21:21:22 +0100 Subject: [PATCH] Assembly: Implement special drag mode for revolute. --- src/Base/Vector3D.cpp | 16 + src/Base/Vector3D.h | 3 + src/Gui/View3DInventorViewer.cpp | 102 ++++ src/Gui/View3DInventorViewer.h | 6 + src/Mod/Assembly/App/AssemblyObject.cpp | 453 ++++++++++++++---- src/Mod/Assembly/App/AssemblyObject.h | 53 +- src/Mod/Assembly/App/AssemblyUtils.cpp | 249 ++++++++++ src/Mod/Assembly/App/AssemblyUtils.h | 72 +++ src/Mod/Assembly/App/CMakeLists.txt | 1 + src/Mod/Assembly/Gui/ViewProviderAssembly.cpp | 265 ++++++++-- src/Mod/Assembly/Gui/ViewProviderAssembly.h | 21 +- src/Mod/Assembly/JointObject.py | 63 ++- 12 files changed, 1121 insertions(+), 183 deletions(-) create mode 100644 src/Mod/Assembly/App/AssemblyUtils.cpp create mode 100644 src/Mod/Assembly/App/AssemblyUtils.h diff --git a/src/Base/Vector3D.cpp b/src/Base/Vector3D.cpp index 88a8a37397..4d6b049634 100644 --- a/src/Base/Vector3D.cpp +++ b/src/Base/Vector3D.cpp @@ -464,6 +464,22 @@ float_type Vector3::GetAngle(const Vector3& rcVect) const return float_type(acos(dot)); } +template +float_type Vector3::GetAngleOriented(const Vector3& rcVect, const Vector3& norm) const +{ + float_type angle = GetAngle(rcVect); + + Vector3 crossProduct = Cross(rcVect); + + // Use dot product to determine the sign + float_type dot = crossProduct.Dot(norm); + if (dot < 0) { + angle = 2 * traits_type::pi() - angle; + } + + return angle; +} + template void Vector3::TransformToCoordinateSystem(const Vector3& rclBase, const Vector3& rclDirX, diff --git a/src/Base/Vector3D.h b/src/Base/Vector3D.h index de9b2d19c7..926c87aaf2 100644 --- a/src/Base/Vector3D.h +++ b/src/Base/Vector3D.h @@ -194,6 +194,9 @@ public: bool IsNull() const; /// Get angle between both vectors. The returned value lies in the interval [0,pi]. float_type GetAngle(const Vector3& rcVect) const; + /// Get oriented angle between both vectors using a normal. The returned value lies in the + /// interval [0,2*pi]. + float_type GetAngleOriented(const Vector3& rcVect, const Vector3& norm) const; /** Transforms this point to the coordinate system defined by origin \a rclBase, * vector \a vector rclDirX and vector \a vector rclDirY. * \note \a rclDirX must be perpendicular to \a rclDirY, i.e. \a rclDirX * \a rclDirY = 0.. diff --git a/src/Gui/View3DInventorViewer.cpp b/src/Gui/View3DInventorViewer.cpp index f713e476b1..065327606d 100644 --- a/src/Gui/View3DInventorViewer.cpp +++ b/src/Gui/View3DInventorViewer.cpp @@ -2647,6 +2647,108 @@ SbVec2f View3DInventorViewer::getNormalizedPosition(const SbVec2s& pnt) const return {pX, pY}; } +SbVec3f View3DInventorViewer::getPointOnXYPlaneOfPlacement(const SbVec2s& pnt, Base::Placement& plc) const +{ + SbVec2f pnt2d = getNormalizedPosition(pnt); + SoCamera* pCam = this->getSoRenderManager()->getCamera(); + + if (!pCam) { + // return invalid point + return {}; + } + + SbViewVolume vol = pCam->getViewVolume(); + SbLine line; + vol.projectPointToLine(pnt2d, line); + + // Calculate the plane using plc + Base::Rotation rot = plc.getRotation(); + Base::Vector3d normalVector = rot.multVec(Base::Vector3d(0, 0, 1)); + SbVec3f planeNormal(normalVector.x, normalVector.y, normalVector.z); + + // Get the position and convert Base::Vector3d to SbVec3f + Base::Vector3d pos = plc.getPosition(); + SbVec3f planePosition(pos.x, pos.y, pos.z); + SbPlane xyPlane(planeNormal, planePosition); + + SbVec3f pt; + if (xyPlane.intersect(line, pt)) { + return pt; // Intersection point on the XY plane + } + else { + // No intersection found + return {}; + } + + return pt; +} + +SbVec3f projectPointOntoPlane(const SbVec3f& point, const SbPlane& plane) { + SbVec3f planeNormal = plane.getNormal(); + float d = plane.getDistanceFromOrigin(); + float distance = planeNormal.dot(point) + d; + return point - planeNormal * distance; +} + +// Project a line onto a plane +SbLine projectLineOntoPlane(const SbVec3f& p1, const SbVec3f& p2, const SbPlane& plane) { + SbVec3f projectedPoint1 = projectPointOntoPlane(p1, plane); + SbVec3f projectedPoint2 = projectPointOntoPlane(p2, plane); + return SbLine(projectedPoint1, projectedPoint2); +} + +SbVec3f intersection(const SbVec3f& p11, const SbVec3f& p12, const SbVec3f& p21, const SbVec3f& p22) +{ + SbVec3f da = p12 - p11; + SbVec3f db = p22 - p21; + SbVec3f dc = p21 - p11; + + double s = (dc.cross(db)).dot(da.cross(db)) / da.cross(db).sqrLength(); + return p11 + da * s; +} + +SbVec3f View3DInventorViewer::getPointOnLine(const SbVec2s& pnt, const SbVec3f& axisCenter, const SbVec3f& axis) const +{ + SbVec2f pnt2d = getNormalizedPosition(pnt); + SoCamera* pCam = this->getSoRenderManager()->getCamera(); + + if (!pCam) { + // return invalid point + return {}; + } + + // First we get pnt projection on the focal plane + SbViewVolume vol = pCam->getViewVolume(); + + float nearDist = pCam->nearDistance.getValue(); + float farDist = pCam->farDistance.getValue(); + float focalDist = pCam->focalDistance.getValue(); + + if (focalDist < nearDist || focalDist > farDist) { + focalDist = 0.5F * (nearDist + farDist); // NOLINT + } + + SbLine line; + SbVec3f pt, ptOnFocalPlaneAndOnLine, ptOnFocalPlane; + SbPlane focalPlane = vol.getPlane(focalDist); + vol.projectPointToLine(pnt2d, line); + focalPlane.intersect(line, ptOnFocalPlane); + + SbLine projectedLine = projectLineOntoPlane(axisCenter, axisCenter + axis, focalPlane); + ptOnFocalPlaneAndOnLine = projectedLine.getClosestPoint(ptOnFocalPlane); + + // now we need the intersection point between + // - the line passing by ptOnFocalPlaneAndOnLine normal to focalPlane + // - The line (axisCenter, axisCenter + axis) + + // Line normal to focal plane through ptOnFocalPlane + SbLine normalLine(ptOnFocalPlane, ptOnFocalPlane + focalPlane.getNormal()); + SbLine axisLine(axisCenter, axisCenter + axis); + pt = intersection(ptOnFocalPlane, ptOnFocalPlane + focalPlane.getNormal(), axisCenter, axisCenter + axis); + + return pt; +} + SbVec3f View3DInventorViewer::getPointOnFocalPlane(const SbVec2s& pnt) const { SbVec2f pnt2d = getNormalizedPosition(pnt); diff --git a/src/Gui/View3DInventorViewer.h b/src/Gui/View3DInventorViewer.h index 5efa45de4f..221ad4ba95 100644 --- a/src/Gui/View3DInventorViewer.h +++ b/src/Gui/View3DInventorViewer.h @@ -318,6 +318,12 @@ public: /** Returns the 3d point on the focal plane to the given 2d point. */ SbVec3f getPointOnFocalPlane(const SbVec2s&) const; + /** Returns the 3d point on a line to the given 2d point. */ + SbVec3f getPointOnLine(const SbVec2s&, const SbVec3f& axisCenter, const SbVec3f& axis) const; + + /** Returns the 3d point on the XY plane of a placement to the given 2d point. */ + SbVec3f getPointOnXYPlaneOfPlacement(const SbVec2s&, Base::Placement&) const; + /** Returns the 2d coordinates on the viewport to the given 3d point. */ SbVec2s getPointOnViewport(const SbVec3f&) const; diff --git a/src/Mod/Assembly/App/AssemblyObject.cpp b/src/Mod/Assembly/App/AssemblyObject.cpp index 14538ab331..506613640c 100644 --- a/src/Mod/Assembly/App/AssemblyObject.cpp +++ b/src/Mod/Assembly/App/AssemblyObject.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include @@ -80,103 +81,6 @@ namespace PartApp = Part; using namespace Assembly; using namespace MbD; -// ======================================= Utils ====================================== - -Base::Placement getPlacementFromProp(App::DocumentObject* obj, const char* propName) -{ - Base::Placement plc = Base::Placement(); - auto* propPlacement = dynamic_cast(obj->getPropertyByName(propName)); - if (propPlacement) { - plc = propPlacement->getValue(); - } - return plc; -} -/* // Currently unused -Base::Placement* getTargetPlacementRelativeTo( - App::DocumentObject* targetObj, App::DocumentObject* part, App::DocumentObject* container, - bool inContainerBranch, bool ignorePlacement = false) -{ - inContainerBranch = inContainerBranch || (!ignorePlacement && part == container); - - Base::Console().Warning("sub --------------\n"); - if (targetObj == part && inContainerBranch && !ignorePlacement) { - Base::Console().Warning("found0\n"); - return &getPlacementFromProp(targetObj, "Placement"); - } - - if (auto group = dynamic_cast(part)) { - for (auto& obj : group->getOutList()) { - auto foundPlacement = getTargetPlacementRelativeTo( - targetObj, obj, container, inContainerBranch, ignorePlacement - ); - if (foundPlacement != nullptr) { - return foundPlacement; - } - } - } - else if (auto assembly = dynamic_cast(part)) { - Base::Console().Warning("h3\n"); - for (auto& obj : assembly->getOutList()) { - auto foundPlacement = getTargetPlacementRelativeTo( - targetObj, obj, container, inContainerBranch - ); - if (foundPlacement == nullptr) { - continue; - } - - if (!ignorePlacement) { - *foundPlacement = getPlacementFromProp(part, "Placement") * *foundPlacement; - } - - Base::Console().Warning("found\n"); - return foundPlacement; - } - } - else if (auto link = dynamic_cast(part)) { - Base::Console().Warning("h4\n"); - auto linked_obj = link->getLinkedObject(); - - if (dynamic_cast(linked_obj) || dynamic_cast(linked_obj)) { - for (auto& obj : linked_obj->getOutList()) { - auto foundPlacement = getTargetPlacementRelativeTo( - targetObj, obj, container, inContainerBranch - ); - if (foundPlacement == nullptr) { - continue; - } - - *foundPlacement = getPlacementFromProp(link, "Placement") * *foundPlacement; - return foundPlacement; - } - } - - auto foundPlacement = getTargetPlacementRelativeTo( - targetObj, linked_obj, container, inContainerBranch, true - ); - - if (foundPlacement != nullptr && !ignorePlacement) { - *foundPlacement = getPlacementFromProp(link, "Placement") * *foundPlacement; - } - - Base::Console().Warning("found2\n"); - return foundPlacement; - } - - return nullptr; -} - -Base::Placement getGlobalPlacement(App::DocumentObject* targetObj, App::DocumentObject* container = -nullptr) { bool inContainerBranch = container == nullptr; auto rootObjects = -App::GetApplication().getActiveDocument()->getRootObjects(); for (auto& part : rootObjects) { auto -foundPlacement = getTargetPlacementRelativeTo(targetObj, part, container, inContainerBranch); if -(foundPlacement != nullptr) { Base::Placement plc(foundPlacement->toMatrix()); return plc; - } - } - - return Base::Placement(); -} -*/ - // ================================ Assembly Object ============================ PROPERTY_SOURCE(Assembly::AssemblyObject, App::Part) @@ -196,7 +100,90 @@ PyObject* AssemblyObject::getPyObject() return Py::new_reference_to(PythonObject); } -std::vector AssemblyObject::getJoints() +std::vector AssemblyObject::getJointsOfObj(App::DocumentObject* obj) +{ + std::vector joints = getJoints(false); + std::vector jointsOf; + + for (auto joint : joints) { + App::DocumentObject* obj1 = getObjFromNameProp(joint, "object1", "Part1"); + App::DocumentObject* obj2 = getObjFromNameProp(joint, "Object2", "Part2"); + if (obj == obj1 || obj == obj2) { + jointsOf.push_back(obj); + } + } + + return jointsOf; +} + +std::vector AssemblyObject::getJointsOfPart(App::DocumentObject* part) +{ + std::vector joints = getJoints(false); + std::vector jointsOf; + + for (auto joint : joints) { + App::DocumentObject* part1 = getLinkObjFromProp(joint, "Part1"); + App::DocumentObject* part2 = getLinkObjFromProp(joint, "Part2"); + if (part == part1 || part == part2) { + jointsOf.push_back(joint); + } + } + + return jointsOf; +} + +App::DocumentObject* AssemblyObject::getJointOfPartConnectingToGround(App::DocumentObject* part, + std::string& name) +{ + std::vector joints = getJointsOfPart(part); + + for (auto joint : joints) { + if (!joint) { + continue; + } + App::DocumentObject* part1 = getLinkObjFromProp(joint, "Part1"); + App::DocumentObject* part2 = getLinkObjFromProp(joint, "Part2"); + if (!part1 || !part2) { + continue; + } + + if (part == part1 && isJointConnectingPartToGround(joint, "Part1")) { + name = "Part1"; + return joint; + } + if (part == part2 && isJointConnectingPartToGround(joint, "Part2")) { + name = "Part2"; + return joint; + } + } + + return nullptr; +} + +bool AssemblyObject::isJointConnectingPartToGround(App::DocumentObject* joint, const char* propname) +{ + auto* propPart = dynamic_cast(joint->getPropertyByName(propname)); + if (!propPart) { + return false; + } + App::DocumentObject* part = propPart->getValue(); + // Check if the part is disconnected even with the joint + bool isConnected = isPartConnected(part); + if (!isConnected) { + return false; + } + + // now we disconnect this joint temporarily + propPart->setValue(nullptr); + + isConnected = isPartConnected(part); + + propPart->setValue(part); + + return !isConnected; +} + +std::vector AssemblyObject::getJoints(bool updateJCS) { std::vector joints = {}; @@ -221,7 +208,9 @@ std::vector AssemblyObject::getJoints() } // Make sure the joints are up to date. - recomputeJointPlacements(joints); + if (updateJCS) { + recomputeJointPlacements(joints); + } return joints; } @@ -318,10 +307,23 @@ AssemblyObject::getConnectedParts(App::DocumentObject* part, return connectedParts; } -bool AssemblyObject::isPartConnected(App::DocumentObject* obj) +bool AssemblyObject::isPartGrounded(App::DocumentObject* obj) { std::vector groundedObjs = fixGroundedParts(); - std::vector joints = getJoints(); + + for (auto* groundedObj : groundedObjs) { + if (groundedObj->getFullName() == obj->getFullName()) { + return true; + } + } + + return false; +} + +bool AssemblyObject::isPartConnected(App::DocumentObject* obj) +{ + std::vector groundedObjs = getGroundedParts(); + std::vector joints = getJoints(false); std::set connectedParts; @@ -344,6 +346,81 @@ bool AssemblyObject::isPartConnected(App::DocumentObject* obj) return false; } +std::vector AssemblyObject::getDownstreamParts(App::DocumentObject* part, + int limit) +{ + if (limit > 1000) { // Inifinite loop protection + return {}; + } + limit++; + + std::vector downstreamParts = {part}; + std::string name; + App::DocumentObject* connectingJoint = getJointOfPartConnectingToGround(part, name); + std::vector jointsOfPart = getJointsOfPart(part); + + // remove connectingJoint from jointsOfPart + auto it = std::remove(jointsOfPart.begin(), jointsOfPart.end(), connectingJoint); + jointsOfPart.erase(it, jointsOfPart.end()); + for (auto joint : jointsOfPart) { + App::DocumentObject* part1 = getLinkObjFromProp(joint, "Part1"); + App::DocumentObject* part2 = getLinkObjFromProp(joint, "Part2"); + App::DocumentObject* downstreamPart = + part->getFullName() == part1->getFullName() ? part2 : part1; + + std::vector subDownstreamParts = + getDownstreamParts(downstreamPart, limit); + for (auto downPart : subDownstreamParts) { + if (std::find(downstreamParts.begin(), downstreamParts.end(), downPart) + == downstreamParts.end()) { + downstreamParts.push_back(downPart); + } + } + } + return downstreamParts; +} + +std::vector AssemblyObject::getUpstreamParts(App::DocumentObject* part, + int limit) +{ + if (limit > 1000) { // Inifinite loop protection + return {}; + } + limit++; + + if (isPartGrounded(part)) { + return {part}; + } + + std::string name; + App::DocumentObject* connectingJoint = getJointOfPartConnectingToGround(part, name); + App::DocumentObject* upPart = + getLinkObjFromProp(connectingJoint, name == "Part1" ? "Part2" : "Part1"); + + std::vector upstreamParts = getUpstreamParts(upPart, limit); + upstreamParts.push_back(part); + return upstreamParts; +} + +App::DocumentObject* AssemblyObject::getUpstreamMovingPart(App::DocumentObject* part) +{ + if (isPartGrounded(part)) { + return nullptr; + } + + std::string name; + App::DocumentObject* connectingJoint = getJointOfPartConnectingToGround(part, name); + JointType jointType = getJointType(connectingJoint); + if (jointType != JointType::Fixed) { + return part; + } + + App::DocumentObject* upPart = + getLinkObjFromProp(connectingJoint, name == "Part1" ? "Part2" : "Part1"); + + return getUpstreamMovingPart(upPart); +} + JointGroup* AssemblyObject::getJointGroup() { App::Document* doc = getDocument(); @@ -361,6 +438,27 @@ JointGroup* AssemblyObject::getJointGroup() return nullptr; } +std::vector AssemblyObject::getGroundedParts() +{ + std::vector groundedJoints = getGroundedJoints(); + + std::vector groundedObjs; + for (auto gJoint : groundedJoints) { + if (!gJoint) { + continue; + } + + auto* propObj = + dynamic_cast(gJoint->getPropertyByName("ObjectToGround")); + + if (propObj) { + App::DocumentObject* objToGround = propObj->getValue(); + groundedObjs.push_back(objToGround); + } + } + return groundedObjs; +} + std::vector AssemblyObject::fixGroundedParts() { std::vector groundedJoints = getGroundedJoints(); @@ -831,7 +929,13 @@ std::string AssemblyObject::handleOneSideOfJoint(App::DocumentObject* joint, if (obj->getNameInDocument() != part->getNameInDocument()) { // Make plc relative to the containing part - plc = objPlc * plc; + // plc = objPlc * plc; // this would not work for nested parts. + + Base::Placement obj_global_plc = getGlobalPlacement(obj, part); + plc = obj_global_plc * plc; + + Base::Placement part_global_plc = getGlobalPlacement(part); + plc = part_global_plc.inverse() * plc; } std::string markerName = joint->getFullName(); @@ -1196,7 +1300,149 @@ double AssemblyObject::getEdgeRadius(App::DocumentObject* obj, const char* elt) return 0.0; } -// getters to get from properties + +// ======================================= Utils ====================================== + +void printPlacement(Base::Placement plc, const char* name) +{ + Base::Vector3d pos = plc.getPosition(); + Base::Vector3d axis; + double angle; + Base::Rotation rot = plc.getRotation(); + rot.getRawValue(axis, angle); + Base::Console().Warning( + "placement %s : position (%.1f, %.1f, %.1f) - axis (%.1f, %.1f, %.1f) angle %.1f\n", + name, + pos.x, + pos.y, + pos.z, + axis.x, + axis.y, + axis.z, + angle); +} + +Base::Placement AssemblyObject::getPlacementFromProp(App::DocumentObject* obj, const char* propName) +{ + Base::Placement plc = Base::Placement(); + auto* propPlacement = dynamic_cast(obj->getPropertyByName(propName)); + if (propPlacement) { + plc = propPlacement->getValue(); + } + return plc; +} + +bool AssemblyObject::getTargetPlacementRelativeTo(Base::Placement& foundPlc, + App::DocumentObject* targetObj, + App::DocumentObject* part, + App::DocumentObject* container, + bool inContainerBranch, + bool ignorePlacement) +{ + inContainerBranch = inContainerBranch || (!ignorePlacement && part == container); + + if (targetObj == part && inContainerBranch && !ignorePlacement) { + foundPlc = getPlacementFromProp(targetObj, "Placement"); + return true; + } + + if (part->isDerivedFrom(App::DocumentObjectGroup::getClassTypeId())) { + for (auto& obj : part->getOutList()) { + bool found = getTargetPlacementRelativeTo(foundPlc, + targetObj, + obj, + container, + inContainerBranch, + ignorePlacement); + if (found) { + return true; + } + } + } + else if (part->isDerivedFrom(Assembly::AssemblyObject::getClassTypeId()) + || part->isDerivedFrom(App::Part::getClassTypeId()) + || part->isDerivedFrom(PartDesign::Body::getClassTypeId())) { + for (auto& obj : part->getOutList()) { + bool found = getTargetPlacementRelativeTo(foundPlc, + targetObj, + obj, + container, + inContainerBranch); + if (!found) { + continue; + } + + if (!ignorePlacement) { + foundPlc = getPlacementFromProp(part, "Placement") * foundPlc; + } + + return true; + } + } + else if (auto link = dynamic_cast(part)) { + auto linked_obj = link->getLinkedObject(); + + if (dynamic_cast(linked_obj) || dynamic_cast(linked_obj)) { + for (auto& obj : linked_obj->getOutList()) { + bool found = getTargetPlacementRelativeTo(foundPlc, + targetObj, + obj, + container, + inContainerBranch); + if (!found) { + continue; + } + + foundPlc = getPlacementFromProp(link, "Placement") * foundPlc; + return true; + } + } + + bool found = getTargetPlacementRelativeTo(foundPlc, + targetObj, + linked_obj, + container, + inContainerBranch, + true); + + if (found) { + if (!ignorePlacement) { + foundPlc = getPlacementFromProp(link, "Placement") * foundPlc; + } + + return true; + } + } + + return false; +} + +Base::Placement AssemblyObject::getGlobalPlacement(App::DocumentObject* targetObj, + App::DocumentObject* container) +{ + bool inContainerBranch = (container == nullptr); + auto rootObjects = App::GetApplication().getActiveDocument()->getRootObjects(); + for (auto& part : rootObjects) { + Base::Placement foundPlc; + bool found = + getTargetPlacementRelativeTo(foundPlc, targetObj, part, container, inContainerBranch); + if (found) { + return foundPlc; + } + } + + return Base::Placement(); +} + +Base::Placement AssemblyObject::getGlobalPlacement(App::DocumentObject* joint, + const char* targetObj, + const char* container) +{ + App::DocumentObject* obj = getObjFromNameProp(joint, targetObj, container); + App::DocumentObject* part = getLinkObjFromProp(joint, container); + return getGlobalPlacement(obj, part); +} + double AssemblyObject::getJointDistance(App::DocumentObject* joint) { double distance = 0.0; @@ -1248,6 +1494,7 @@ App::DocumentObject* AssemblyObject::getLinkObjFromProp(App::DocumentObject* joi { auto* propObj = dynamic_cast(joint->getPropertyByName(propLinkName)); if (!propObj) { + Base::Console().Warning("getLinkObjFromProp nullptr\n"); return nullptr; } return propObj->getValue(); diff --git a/src/Mod/Assembly/App/AssemblyObject.h b/src/Mod/Assembly/App/AssemblyObject.h index 4cf99e2774..2f870ad09f 100644 --- a/src/Mod/Assembly/App/AssemblyObject.h +++ b/src/Mod/Assembly/App/AssemblyObject.h @@ -107,10 +107,16 @@ public: const char* propPartName, const char* propPlcName); void jointParts(std::vector joints); - std::vector getJoints(); + std::vector getJoints(bool updateJCS = true); + std::vector getJointsOfObj(App::DocumentObject* obj); + std::vector getJointsOfPart(App::DocumentObject* part); + App::DocumentObject* getJointOfPartConnectingToGround(App::DocumentObject* part, + std::string& name); + bool isJointConnectingPartToGround(App::DocumentObject* joint, const char* partPropName); std::vector getGroundedJoints(); void fixGroundedPart(App::DocumentObject* obj, Base::Placement& plc, std::string& jointName); std::vector fixGroundedParts(); + std::vector getGroundedParts(); void removeUnconnectedJoints(std::vector& joints, std::vector groundedObjs); @@ -128,7 +134,11 @@ public: void redrawJointPlacements(std::vector joints); void recomputeJointPlacements(std::vector joints); - bool isPartConnected(App::DocumentObject* obj); + bool isPartGrounded(App::DocumentObject* part); + bool isPartConnected(App::DocumentObject* part); + std::vector getDownstreamParts(App::DocumentObject* part, int limit = 0); + std::vector getUpstreamParts(App::DocumentObject* part, int limit = 0); + App::DocumentObject* getUpstreamMovingPart(App::DocumentObject* part); double getObjMass(App::DocumentObject* obj); void setObjMasses(std::vector> objectMasses); @@ -138,16 +148,6 @@ public: double getFaceRadius(App::DocumentObject* obj, const char* elName); double getEdgeRadius(App::DocumentObject* obj, const char* elName); - // getters to get from properties - double getJointDistance(App::DocumentObject* joint); - JointType getJointType(App::DocumentObject* joint); - const char* getElementFromProp(App::DocumentObject* obj, const char* propName); - std::string getElementTypeFromProp(App::DocumentObject* obj, const char* propName); - App::DocumentObject* getLinkObjFromProp(App::DocumentObject* joint, const char* propName); - App::DocumentObject* - getObjFromNameProp(App::DocumentObject* joint, const char* pObjName, const char* pPart); - App::DocumentObject* - getLinkedObjFromNameProp(App::DocumentObject* joint, const char* pObjName, const char* pPart); private: std::shared_ptr mbdAssembly; @@ -159,6 +159,35 @@ private: // void handleChangedPropertyType(Base::XMLReader &reader, const char *TypeName, App::Property // *prop) override; + +public: + // ---------------- Utils ------------------- + // Can't put the functions by themselves in AssemblyUtils.cpp : + // see https://forum.freecad.org/viewtopic.php?p=729577#p729577 + + // getters to get from properties + static double getJointDistance(App::DocumentObject* joint); + static JointType getJointType(App::DocumentObject* joint); + static const char* getElementFromProp(App::DocumentObject* obj, const char* propName); + static std::string getElementTypeFromProp(App::DocumentObject* obj, const char* propName); + static App::DocumentObject* getLinkObjFromProp(App::DocumentObject* joint, + const char* propName); + static App::DocumentObject* + getObjFromNameProp(App::DocumentObject* joint, const char* pObjName, const char* pPart); + static App::DocumentObject* + getLinkedObjFromNameProp(App::DocumentObject* joint, const char* pObjName, const char* pPart); + static Base::Placement getPlacementFromProp(App::DocumentObject* obj, const char* propName); + static bool getTargetPlacementRelativeTo(Base::Placement& foundPlc, + App::DocumentObject* targetObj, + App::DocumentObject* part, + App::DocumentObject* container, + bool inContainerBranch, + bool ignorePlacement = false); + static Base::Placement getGlobalPlacement(App::DocumentObject* targetObj, + App::DocumentObject* container = nullptr); + static Base::Placement getGlobalPlacement(App::DocumentObject* joint, + const char* targetObj, + const char* container = ""); }; // using AssemblyObjectPython = App::FeaturePythonT; diff --git a/src/Mod/Assembly/App/AssemblyUtils.cpp b/src/Mod/Assembly/App/AssemblyUtils.cpp new file mode 100644 index 0000000000..4c495628ac --- /dev/null +++ b/src/Mod/Assembly/App/AssemblyUtils.cpp @@ -0,0 +1,249 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2023 Ondsel * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD 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 * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#include "PreCompiled.h" +#ifndef _PreComp_ +#endif + +#include +#include +#include +#include +// #include +#include +// #include +#include +#include +#include + +#include + +#include "AssemblyUtils.h" + +// ======================================= Utils ====================================== +/* +namespace Assembly +{ + +Base::Placement getPlacementFromProp(App::DocumentObject* obj, const char* propName) +{ + Base::Placement plc = Base::Placement(); + auto* propPlacement = dynamic_cast(obj->getPropertyByName(propName)); + if (propPlacement) { + plc = propPlacement->getValue(); + } + return plc; +} + +/* // Currently unused +Base::Placement* getTargetPlacementRelativeTo( + App::DocumentObject* targetObj, App::DocumentObject* part, App::DocumentObject* container, + bool inContainerBranch, bool ignorePlacement = false) +{ + inContainerBranch = inContainerBranch || (!ignorePlacement && part == container); + + Base::Console().Warning("sub --------------\n"); + if (targetObj == part && inContainerBranch && !ignorePlacement) { + Base::Console().Warning("found0\n"); + return &getPlacementFromProp(targetObj, "Placement"); + } + + if (auto group = dynamic_cast(part)) { + for (auto& obj : group->getOutList()) { + auto foundPlacement = getTargetPlacementRelativeTo( + targetObj, obj, container, inContainerBranch, ignorePlacement + ); + if (foundPlacement != nullptr) { + return foundPlacement; + } + } + } + else if (auto assembly = dynamic_cast(part)) { + Base::Console().Warning("h3\n"); + for (auto& obj : assembly->getOutList()) { + auto foundPlacement = getTargetPlacementRelativeTo( + targetObj, obj, container, inContainerBranch + ); + if (foundPlacement == nullptr) { + continue; + } + + if (!ignorePlacement) { + *foundPlacement = getPlacementFromProp(part, "Placement") * *foundPlacement; + } + + Base::Console().Warning("found\n"); + return foundPlacement; + } + } + else if (auto link = dynamic_cast(part)) { + Base::Console().Warning("h4\n"); + auto linked_obj = link->getLinkedObject(); + + if (dynamic_cast(linked_obj) || dynamic_cast(linked_obj)) { + for (auto& obj : linked_obj->getOutList()) { + auto foundPlacement = getTargetPlacementRelativeTo( + targetObj, obj, container, inContainerBranch + ); + if (foundPlacement == nullptr) { + continue; + } + + *foundPlacement = getPlacementFromProp(link, "Placement") * *foundPlacement; + return foundPlacement; + } + } + + auto foundPlacement = getTargetPlacementRelativeTo( + targetObj, linked_obj, container, inContainerBranch, true + ); + + if (foundPlacement != nullptr && !ignorePlacement) { + *foundPlacement = getPlacementFromProp(link, "Placement") * *foundPlacement; + } + + Base::Console().Warning("found2\n"); + return foundPlacement; + } + + return nullptr; +} + +Base::Placement getGlobalPlacement(App::DocumentObject* targetObj, App::DocumentObject* container = +nullptr) { bool inContainerBranch = container == nullptr; auto rootObjects = +App::GetApplication().getActiveDocument()->getRootObjects(); for (auto& part : rootObjects) { auto +foundPlacement = getTargetPlacementRelativeTo(targetObj, part, container, inContainerBranch); if +(foundPlacement != nullptr) { Base::Placement plc(foundPlacement->toMatrix()); return plc; + } + } + + return Base::Placement(); +} +*/ +/* +double getJointDistance(App::DocumentObject* joint) +{ + double distance = 0.0; + + auto* prop = dynamic_cast(joint->getPropertyByName("Distance")); + if (prop) { + distance = prop->getValue(); + } + + return distance; +} + +JointType getJointType(App::DocumentObject* joint) +{ + JointType jointType = JointType::Fixed; + + auto* prop = dynamic_cast(joint->getPropertyByName("JointType")); + if (prop) { + jointType = static_cast(prop->getValue()); + } + + return jointType; +} + +const char* getElementFromProp(App::DocumentObject* obj, const char* propName) +{ + auto* prop = dynamic_cast(obj->getPropertyByName(propName)); + if (!prop) { + return ""; + } + + return prop->getValue(); +} + +std::string getElementTypeFromProp(App::DocumentObject* obj, const char* propName) +{ + // The prop is going to be something like 'Edge14' or 'Face7'. We need 'Edge' or 'Face' + std::string elementType; + for (char ch : std::string(getElementFromProp(obj, propName))) { + if (std::isalpha(ch)) { + elementType += ch; + } + } + return elementType; +} + +App::DocumentObject* getLinkObjFromProp(App::DocumentObject* joint, + const char* propLinkName) +{ + auto* propObj = dynamic_cast(joint->getPropertyByName(propLinkName)); + if (!propObj) { + return nullptr; + } + return propObj->getValue(); +} + +App::DocumentObject* getObjFromNameProp(App::DocumentObject* joint, + const char* pObjName, + const char* pPart) +{ + auto* propObjName = dynamic_cast(joint->getPropertyByName(pObjName)); + if (!propObjName) { + return nullptr; + } + std::string objName = std::string(propObjName->getValue()); + + App::DocumentObject* containingPart = getLinkObjFromProp(joint, pPart); + if (!containingPart) { + return nullptr; + } + + if (objName == containingPart->getNameInDocument()) { + return containingPart; + } + + if (containingPart->getTypeId().isDerivedFrom(App::Link::getClassTypeId())) { + App::Link* link = dynamic_cast(containingPart); + + containingPart = link->getLinkedObject(); + if (!containingPart) { + return nullptr; + } + } + + for (auto obj : containingPart->getOutList()) { + if (objName == obj->getNameInDocument()) { + return obj; + } + } + + return nullptr; +} + +App::DocumentObject* getLinkedObjFromNameProp(App::DocumentObject* joint, + const char* pObjName, + const char* pPart) + { + auto* obj = getObjFromNameProp(joint, pObjName, pPart); + if (obj) { + return obj->getLinkedObject(true); + } + return nullptr; + } + +} // namespace Assembly +*/ diff --git a/src/Mod/Assembly/App/AssemblyUtils.h b/src/Mod/Assembly/App/AssemblyUtils.h new file mode 100644 index 0000000000..c5851fe30a --- /dev/null +++ b/src/Mod/Assembly/App/AssemblyUtils.h @@ -0,0 +1,72 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2023 Ondsel * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD 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 * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + + +#ifndef ASSEMBLY_AssemblyUtils_H +#define ASSEMBLY_AssemblyUtils_H + + +#include + +#include +#include + +namespace App +{ +class DocumentObject; +} // namespace App + +namespace Base +{ +class Placement; +} // namespace Base + +namespace Assembly +{ +/* +// This enum has to be the same as the one in JointObject.py +enum class JointType +{ + Fixed, + Revolute, + Cylindrical, + Slider, + Ball, + Distance +}; + +// getters to get from properties +double getJointDistance(App::DocumentObject* joint); +JointType getJointType(App::DocumentObject* joint); +const char* getElementFromProp(App::DocumentObject* obj, const char* propName); +std::string getElementTypeFromProp(App::DocumentObject* obj, const char* propName); +App::DocumentObject* getLinkObjFromProp(App::DocumentObject* joint, const char* propName); +App::DocumentObject* getObjFromNameProp(App::DocumentObject* joint, const char* pObjName, const +char* pPart); App::DocumentObject* getLinkedObjFromNameProp(App::DocumentObject* joint, const char* +pObjName, const char* pPart); Base::Placement getPlacementFromProp(App::DocumentObject* obj, const +char* propName);*/ + +} // namespace Assembly + + +#endif // ASSEMBLY_AssemblyUtils_H diff --git a/src/Mod/Assembly/App/CMakeLists.txt b/src/Mod/Assembly/App/CMakeLists.txt index fc314577af..724f22e1f8 100644 --- a/src/Mod/Assembly/App/CMakeLists.txt +++ b/src/Mod/Assembly/App/CMakeLists.txt @@ -12,6 +12,7 @@ link_directories(${OCC_LIBRARY_DIR}) set(Assembly_LIBS Part + PartDesign FreeCADApp OndselSolver ) diff --git a/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp b/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp index 8c469973e8..174d46e6d6 100644 --- a/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp +++ b/src/Mod/Assembly/Gui/ViewProviderAssembly.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -51,6 +52,25 @@ using namespace Assembly; using namespace AssemblyGui; +void printPlacement(Base::Placement plc, const char* name) +{ + Base::Vector3d pos = plc.getPosition(); + Base::Vector3d axis; + double angle; + Base::Rotation rot = plc.getRotation(); + rot.getRawValue(axis, angle); + Base::Console().Warning( + "placement %s : position (%.1f, %.1f, %.1f) - axis (%.1f, %.1f, %.1f) angle %.1f\n", + name, + pos.x, + pos.y, + pos.z, + axis.x, + axis.y, + axis.z, + angle); +} + PROPERTY_SOURCE(AssemblyGui::ViewProviderAssembly, Gui::ViewProviderPart) ViewProviderAssembly::ViewProviderAssembly() @@ -76,7 +96,7 @@ bool ViewProviderAssembly::doubleClicked() Gui::Application::Instance->activeDocument()->resetEdit(); } else { - // assure the PartDesign workbench + // assure the Assembly workbench if (App::GetApplication() .GetUserParameter() .GetGroup("BaseApp") @@ -113,11 +133,13 @@ bool ViewProviderAssembly::canDragObject(App::DocumentObject* obj) const Gui::Command::openCommand(tr("Delete associated joints").toStdString().c_str()); for (auto joint : allJoints) { - // Assume getLinkObjFromProp can return nullptr if the property doesn't exist. - App::DocumentObject* obj1 = assemblyPart->getLinkObjFromProp(joint, "Part1"); - App::DocumentObject* obj2 = assemblyPart->getLinkObjFromProp(joint, "Part2"); - App::DocumentObject* obj3 = assemblyPart->getLinkObjFromProp(joint, "ObjectToGround"); - if (obj == obj1 || obj == obj2 || obj == obj3) { + // getLinkObjFromProp returns nullptr if the property doesn't exist. + App::DocumentObject* obj1 = AssemblyObject::getObjFromNameProp(joint, "Object1", "Part1"); + App::DocumentObject* obj2 = AssemblyObject::getObjFromNameProp(joint, "Object2", "Part2"); + App::DocumentObject* part1 = AssemblyObject::getLinkObjFromProp(joint, "Part1"); + App::DocumentObject* part2 = AssemblyObject::getLinkObjFromProp(joint, "Part2"); + App::DocumentObject* obj3 = AssemblyObject::getLinkObjFromProp(joint, "ObjectToGround"); + if (obj == obj1 || obj == obj2 || obj == part1 || obj == part2 || obj == obj3) { if (!prompted) { prompted = true; QMessageBox msgBox; @@ -166,24 +188,25 @@ bool ViewProviderAssembly::setEdit(int ModNum) void ViewProviderAssembly::unsetEdit(int ModNum) { Q_UNUSED(ModNum); - canStartDragging = false; partMoving = false; docsToMove = {}; // Check if the view is still active before trying to deactivate the assembly. - auto activeDoc = Gui::Application::Instance->activeDocument(); - if (!activeDoc) { + auto doc = getDocument(); + if (!doc) { return; } - auto activeView = activeDoc->getActiveView(); + auto activeView = doc->getActiveView(); if (!activeView) { return; } // Set the part as not 'Activated' ie not bold in the tree. Gui::Command::doCommand(Gui::Command::Gui, - "Gui.ActiveDocument.ActiveView.setActiveObject('%s', None)", + "appDoc = App.getDocument('%s')\n" + "Gui.getDocument(appDoc).ActiveView.setActiveObject('%s', None)", + this->getObject()->getDocument()->getName(), PARTKEY); } @@ -220,28 +243,106 @@ bool ViewProviderAssembly::mouseMove(const SbVec2s& cursorPos, Gui::View3DInvent canStartDragging = false; if (enableMovement && getSelectedObjectsWithinAssembly()) { - SbVec3f vec = viewer->getPointOnFocalPlane(cursorPos); - Base::Vector3d mousePosition = Base::Vector3d(vec[0], vec[1], vec[2]); + moveMode = findMoveMode(); - initMove(mousePosition); + SbVec3f vec; + if (moveMode == MoveMode::RotationOnPlane + || moveMode == MoveMode::TranslationOnAxisAndRotationOnePlane) { + vec = viewer->getPointOnXYPlaneOfPlacement(cursorPos, jcsGlobalPlc); + initialPositionRot = Base::Vector3d(vec[0], vec[1], vec[2]); + } + + if (moveMode == MoveMode::TranslationOnAxis + || moveMode == MoveMode::TranslationOnAxisAndRotationOnePlane) { + Base::Vector3d zAxis = + jcsGlobalPlc.getRotation().multVec(Base::Vector3d(0., 0., 1.)); + Base::Vector3d pos = jcsGlobalPlc.getPosition(); + SbVec3f axisCenter(pos.x, pos.y, pos.z); + SbVec3f axis(zAxis.x, zAxis.y, zAxis.z); + vec = viewer->getPointOnLine(cursorPos, axisCenter, axis); + initialPosition = Base::Vector3d(vec[0], vec[1], vec[2]); + } + else if (moveMode != MoveMode::RotationOnPlane) { + vec = viewer->getPointOnFocalPlane(cursorPos); + initialPosition = Base::Vector3d(vec[0], vec[1], vec[2]); + } + + initMove(); } } // Do the dragging of parts if (partMoving) { - SbVec3f vec = viewer->getPointOnFocalPlane(cursorPos); - Base::Vector3d mousePosition = Base::Vector3d(vec[0], vec[1], vec[2]); + Base::Vector3d newPos, newPosRot; + if (moveMode == MoveMode::RotationOnPlane + || moveMode == MoveMode::TranslationOnAxisAndRotationOnePlane) { + SbVec3f vec = viewer->getPointOnXYPlaneOfPlacement(cursorPos, jcsGlobalPlc); + newPosRot = Base::Vector3d(vec[0], vec[1], vec[2]); + } + + if (moveMode == MoveMode::TranslationOnAxis + || moveMode == MoveMode::TranslationOnAxisAndRotationOnePlane) { + Base::Vector3d zAxis = jcsGlobalPlc.getRotation().multVec(Base::Vector3d(0., 0., 1.)); + Base::Vector3d pos = jcsGlobalPlc.getPosition(); + SbVec3f axisCenter(pos.x, pos.y, pos.z); + SbVec3f axis(zAxis.x, zAxis.y, zAxis.z); + SbVec3f vec = viewer->getPointOnLine(cursorPos, axisCenter, axis); + newPos = Base::Vector3d(vec[0], vec[1], vec[2]); + } + else if (moveMode != MoveMode::RotationOnPlane) { + SbVec3f vec = viewer->getPointOnFocalPlane(cursorPos); + newPos = Base::Vector3d(vec[0], vec[1], vec[2]); + } + + for (auto& pair : docsToMove) { App::DocumentObject* obj = pair.first; auto* propPlacement = dynamic_cast(obj->getPropertyByName("Placement")); if (propPlacement) { - Base::Placement plc = propPlacement->getValue(); - // Base::Console().Warning("transl %f %f %f\n", pair.second.x, pair.second.y, - // pair.second.z); - Base::Vector3d pos = mousePosition + pair.second; - Base::Placement newPlacement = Base::Placement(pos, plc.getRotation()); - propPlacement->setValue(newPlacement); + Base::Placement plc = pair.second; + // Base::Console().Warning("newPos %f %f %f\n", newPos.x, newPos.y, newPos.z); + + if (moveMode == MoveMode::RotationOnPlane) { + Base::Vector3d center = jcsGlobalPlc.getPosition(); + Base::Vector3d norm = + jcsGlobalPlc.getRotation().multVec(Base::Vector3d(0., 0., -1.)); + double angle = + (newPosRot - center).GetAngleOriented(initialPositionRot - center, norm); + // Base::Console().Warning("angle %f\n", angle); + Base::Rotation zRotation = Base::Rotation(Base::Vector3d(0., 0., 1.), angle); + Base::Placement rotatedGlovalJcsPlc = + jcsGlobalPlc * Base::Placement(Base::Vector3d(), zRotation); + Base::Placement jcsPlcRelativeToPart = plc.inverse() * jcsGlobalPlc; + plc = rotatedGlovalJcsPlc * jcsPlcRelativeToPart.inverse(); + } + else if (moveMode == MoveMode::TranslationOnAxisAndRotationOnePlane) { + Base::Vector3d pos = plc.getPosition() + (newPos - initialPosition); + plc.setPosition(pos); + + Base::Placement newJcsGlobalPlc = jcsGlobalPlc; + newJcsGlobalPlc.setPosition(jcsGlobalPlc.getPosition() + + (newPos - initialPosition)); + Base::Vector3d center = newJcsGlobalPlc.getPosition(); + Base::Vector3d norm = + newJcsGlobalPlc.getRotation().multVec(Base::Vector3d(0., 0., -1.)); + + Base::Vector3d projInitialPositionRot = + initialPositionRot.ProjectToPlane(newJcsGlobalPlc.getPosition(), norm); + double angle = + (newPosRot - center).GetAngleOriented(initialPositionRot - center, norm); + // Base::Console().Warning("angle %f\n", angle); + Base::Rotation zRotation = Base::Rotation(Base::Vector3d(0., 0., 1.), angle); + Base::Placement rotatedGlovalJcsPlc = + newJcsGlobalPlc * Base::Placement(Base::Vector3d(), zRotation); + Base::Placement jcsPlcRelativeToPart = plc.inverse() * newJcsGlobalPlc; + plc = rotatedGlovalJcsPlc * jcsPlcRelativeToPart.inverse(); + } + else { + Base::Vector3d pos = newPos + (plc.getPosition() - initialPosition); + plc.setPosition(pos); + } + propPlacement->setValue(plc); } } @@ -315,9 +416,7 @@ bool ViewProviderAssembly::getSelectedObjectsWithinAssembly() auto* propPlacement = dynamic_cast(obj->getPropertyByName("Placement")); if (propPlacement) { - Base::Placement plc = propPlacement->getValue(); - Base::Vector3d pos = plc.getPosition(); - docsToMove.emplace_back(obj, pos); + docsToMove.emplace_back(obj, propPlacement->getValue()); } } } @@ -334,25 +433,21 @@ bool ViewProviderAssembly::getSelectedObjectsWithinAssembly() std::vector subNames = parseSubNames(subNamesStr); App::DocumentObject* preselectedObj = getObjectFromSubNames(subNames); - if (preselectedObj) { - if (assemblyPart->hasObject(preselectedObj, true)) { - bool alreadyIn = false; - for (auto& pair : docsToMove) { - App::DocumentObject* obj = pair.first; - if (obj == preselectedObj) { - alreadyIn = true; - break; - } + if (preselectedObj && assemblyPart->hasObject(preselectedObj, true)) { + bool alreadyIn = false; + for (auto& pair : docsToMove) { + App::DocumentObject* obj = pair.first; + if (obj == preselectedObj) { + alreadyIn = true; + break; } + } - if (!alreadyIn) { - auto* propPlacement = dynamic_cast( - preselectedObj->getPropertyByName("Placement")); - if (propPlacement) { - Base::Placement plc = propPlacement->getValue(); - Base::Vector3d pos = plc.getPosition(); - docsToMove.emplace_back(preselectedObj, pos); - } + if (!alreadyIn) { + auto* propPlacement = dynamic_cast( + preselectedObj->getPropertyByName("Placement")); + if (propPlacement) { + docsToMove.emplace_back(preselectedObj, propPlacement->getValue()); } } } @@ -387,13 +482,12 @@ App::DocumentObject* ViewProviderAssembly::getObjectFromSubNames(std::vector bodyOrLink should be the moving - // entity partOrLink.bodyOrLink.pad.face1 -> partOrLink should be the - // moving entity partOrLink.box.face1 -> partOrLink should - // be the moving entity partOrLink1...ParOrLinkn.bodyOrLink.pad.face1 -> partOrLink1 - // should be the moving entity assembly1.partOrLink1...ParOrLinkn.bodyOrLink.pad.face1 -> - // partOrLink1 should be the moving entity assembly1.boxOrLink1.face1 -> boxOrLink1 should be - // the moving entity + // bodyOrLink.pad.face1 -> bodyOrLink should be the moving entity + // partOrLink.bodyOrLink.pad.face1 -> partOrLink should be the moving entity + // partOrLink.box.face1 -> partOrLink should be the moving entity + // partOrLink1...ParOrLinkn.bodyOrLink.pad.face1 -> partOrLink1 should be the moving entity + // assembly1.partOrLink1...ParOrLinkn.bodyOrLink.pad.face1 -> partOrLink1 should be the moving + // entity assembly1.boxOrLink1.face1 -> boxOrLink1 should be the moving entity for (auto objName : subNames) { App::DocumentObject* obj = appDoc->getObject(objName.c_str()); @@ -429,7 +523,77 @@ App::DocumentObject* ViewProviderAssembly::getObjectFromSubNames(std::vectorgetObject(objName.c_str()); } -void ViewProviderAssembly::initMove(Base::Vector3d& mousePosition) +ViewProviderAssembly::MoveMode ViewProviderAssembly::findMoveMode() +{ + if (docsToMove.size() == 1) { + auto* assemblyPart = static_cast(getObject()); + std::string partPropName; + App::DocumentObject* joint = + assemblyPart->getJointOfPartConnectingToGround(docsToMove[0].first, partPropName); + + if (!joint) { + return MoveMode::Translation; + } + + JointType jointType = AssemblyObject::getJointType(joint); + if (jointType == JointType::Fixed) { + // If fixed joint we need to find the upstream joint to find move mode. + // For example : Gnd -(revolute)- A -(fixed)- B : if user try to move B, then we should + // actually move A + App::DocumentObject* upstreamPart = + assemblyPart->getUpstreamMovingPart(docsToMove[0].first); + + docsToMove.clear(); + auto* propPlacement = + dynamic_cast(upstreamPart->getPropertyByName("Placement")); + if (propPlacement) { + docsToMove.emplace_back(upstreamPart, propPlacement->getValue()); + } + + joint = + assemblyPart->getJointOfPartConnectingToGround(docsToMove[0].first, partPropName); + if (!joint) { + return MoveMode::Translation; + } + jointType = AssemblyObject::getJointType(joint); + } + + const char* plcPropName = (partPropName == "Part1") ? "Placement1" : "Placement2"; + const char* objPropName = (partPropName == "Part1") ? "Object1" : "Object2"; + + // jcsPlc is relative to the Object + jcsPlc = AssemblyObject::getPlacementFromProp(joint, plcPropName); + + // Make jcsGlobalPlc relative to the origin of the doc + Base::Placement global_plc = + AssemblyObject::getGlobalPlacement(joint, objPropName, partPropName.c_str()); + jcsGlobalPlc = global_plc * jcsPlc; + + // Add downstream parts so that they move together + auto downstreamParts = assemblyPart->getDownstreamParts(docsToMove[0].first); + docsToMove.clear(); // current [0] is added by the recursive getDownstreamParts. + for (auto part : downstreamParts) { + auto* propPlacement = + dynamic_cast(part->getPropertyByName("Placement")); + if (propPlacement) { + docsToMove.emplace_back(part, propPlacement->getValue()); + } + } + + if (jointType == JointType::Revolute) { + return MoveMode::RotationOnPlane; + } + else if (jointType == JointType::Slider) { + return MoveMode::TranslationOnAxis; + } + else if (jointType == JointType::Cylindrical) { + return MoveMode::TranslationOnAxisAndRotationOnePlane; + } + } + return MoveMode::Translation; +} + +void ViewProviderAssembly::initMove() { Gui::Command::openCommand(tr("Move part").toStdString().c_str()); partMoving = true; @@ -446,7 +610,6 @@ void ViewProviderAssembly::initMove(Base::Vector3d& mousePosition) objectMasses.clear(); for (auto& pair : docsToMove) { - pair.second = pair.second - mousePosition; objectMasses.push_back({pair.first, 10.0}); } diff --git a/src/Mod/Assembly/Gui/ViewProviderAssembly.h b/src/Mod/Assembly/Gui/ViewProviderAssembly.h index 32b6d8f848..151c1ff0e1 100644 --- a/src/Mod/Assembly/Gui/ViewProviderAssembly.h +++ b/src/Mod/Assembly/Gui/ViewProviderAssembly.h @@ -70,6 +70,16 @@ public: App::DocumentObject* getActivePart(); + enum class MoveMode + { + Translation, + TranslationOnAxis, + Rotation, + RotationOnPlane, + TranslationOnAxisAndRotationOnePlane, + }; + MoveMode moveMode; + /// is called when the provider is in edit and the mouse is moved bool mouseMove(const SbVec2s& pos, Gui::View3DInventorViewer* viewer) override; /// is called when the Provider is in edit and the mouse is clicked @@ -77,10 +87,11 @@ public: bool pressed, const SbVec2s& cursorPos, const Gui::View3DInventorViewer* viewer) override; - - void initMove(Base::Vector3d& mousePosition); + MoveMode findMoveMode(); + void initMove(); void endMove(); + bool getSelectedObjectsWithinAssembly(); App::DocumentObject* getObjectFromSubNames(std::vector& subNames); std::vector parseSubNames(std::string& subNamesStr); @@ -107,9 +118,13 @@ public: bool partMoving; bool enableMovement; int numberOfSel; + Base::Vector3d initialPosition; + Base::Vector3d initialPositionRot; + Base::Placement jcsPlc; + Base::Placement jcsGlobalPlc; std::vector> objectMasses; - std::vector> docsToMove; + std::vector> docsToMove; }; } // namespace AssemblyGui diff --git a/src/Mod/Assembly/JointObject.py b/src/Mod/Assembly/JointObject.py index 7883e510e6..ff9f9d15b8 100644 --- a/src/Mod/Assembly/JointObject.py +++ b/src/Mod/Assembly/JointObject.py @@ -299,6 +299,7 @@ class Joint: joint.Placement2 = self.findPlacement( joint, joint.Object2, joint.Part2, joint.Element2, joint.Vertex2, True ) + self.preventOrthogonal(joint) solveIfAllowed(assembly, True) else: @@ -446,21 +447,42 @@ class Joint: return plc def applyRotationToPlacement(self, plc, angle): + return self.applyRotationToPlacementAlongAxis(plc, angle, App.Vector(0, 0, 1)) + + def applyRotationToPlacementAlongAxis(self, plc, angle, axis): rot = plc.Rotation - zRotation = App.Rotation(App.Vector(0, 0, 1), angle) - rot = rot.multiply(zRotation) - plc.Rotation = rot + zRotation = App.Rotation(axis, angle) + plc.Rotation = rot * zRotation return plc def flipPart(self, joint): if joint.FirstPartConnected: - plc = joint.Part2.Placement.inverse() * joint.Placement2 - localXAxis = plc.Rotation.multVec(App.Vector(1, 0, 0)) - joint.Part2.Placement = flipPlacement(joint.Part2.Placement, localXAxis) + plc = joint.Placement2 # relative to obj + obj = UtilsAssembly.getObjectInPart(joint.Object2, joint.Part2) + + # we need plc to be relative to the containing part + obj_global_plc = UtilsAssembly.getGlobalPlacement(obj, joint.Part2) + part_global_plc = UtilsAssembly.getGlobalPlacement(joint.Part2) + plc = obj_global_plc * plc + plc = part_global_plc.inverse() * plc + + jcsXAxis = plc.Rotation.multVec(App.Vector(1, 0, 0)) + + joint.Part2.Placement = flipPlacement(joint.Part2.Placement, jcsXAxis) + else: - plc = joint.Part1.Placement.inverse() * joint.Placement1 - localXAxis = plc.Rotation.multVec(App.Vector(1, 0, 0)) - joint.Part1.Placement = flipPlacement(joint.Part1.Placement, localXAxis) + plc = joint.Placement1 # relative to obj + obj = UtilsAssembly.getObjectInPart(joint.Object1, joint.Part1) + + # we need plc to be relative to the containing part + obj_global_plc = UtilsAssembly.getGlobalPlacement(obj, joint.Part1) + part_global_plc = UtilsAssembly.getGlobalPlacement(joint.Part1) + plc = obj_global_plc * plc + plc = part_global_plc.inverse() * plc + + jcsXAxis = plc.Rotation.multVec(App.Vector(1, 0, 0)) + + joint.Part1.Placement = flipPlacement(joint.Part1.Placement, jcsXAxis) solveIfAllowed(self.getAssembly(joint)) @@ -486,6 +508,19 @@ class Joint: return App.Vector(res[0].X, res[0].Y, res[0].Z) return surface.Center + def preventOrthogonal(self, joint): + zAxis1 = joint.Placement1.Rotation.multVec(App.Vector(0, 0, 1)) + zAxis2 = joint.Placement2.Rotation.multVec(App.Vector(0, 0, 1)) + if abs(zAxis1.dot(zAxis2)) < Part.Precision.confusion(): + if joint.FirstPartConnected: + joint.Part2.Placement = self.applyRotationToPlacementAlongAxis( + joint.Part2.Placement, 30.0, App.Vector(1, 2, 0) + ) + else: + joint.Part1.Placement = self.applyRotationToPlacementAlongAxis( + joint.Part1.Placement, 30.0, App.Vector(1, 2, 0) + ) + class ViewProviderJoint: def __init__(self, vobj): @@ -639,8 +674,7 @@ class ViewProviderJoint: plc = joint.Placement1 self.switch_JCS1.whichChild = coin.SO_SWITCH_ALL - if joint.Part1: # prevent an unwanted call by assembly.isPartConnected - self.set_JCS_placement(self.transform1, plc, joint.Object1, joint.Part1) + self.set_JCS_placement(self.transform1, plc, joint.Object1, joint.Part1) else: self.switch_JCS1.whichChild = coin.SO_SWITCH_NONE @@ -650,15 +684,16 @@ class ViewProviderJoint: self.switch_JCS2.whichChild = coin.SO_SWITCH_ALL # if self.areJCSReversed(joint): # plc = flipPlacement(plc, App.Vector(1, 0, 0)) + self.set_JCS_placement(self.transform2, plc, joint.Object2, joint.Part2) else: self.switch_JCS2.whichChild = coin.SO_SWITCH_NONE def areJCSReversed(self, joint): - zaxis1 = joint.Placement1.Rotation.multVec(App.Vector(0, 0, 1)) - zaxis2 = joint.Placement2.Rotation.multVec(App.Vector(0, 0, 1)) + zAxis1 = joint.Placement1.Rotation.multVec(App.Vector(0, 0, 1)) + zAxis2 = joint.Placement2.Rotation.multVec(App.Vector(0, 0, 1)) - sameDir = zaxis1.dot(zaxis2) > 0 + sameDir = zAxis1.dot(zAxis2) > 0 return not sameDir def showPreviewJCS(self, visible, placement=None, objName="", part=None):