FreeCAD

1
// SPDX-License-Identifier: LGPL-2.1-or-later
2
/****************************************************************************
3
 *                                                                          *
4
 *   Copyright (c) 2023 Ondsel <development@ondsel.com>                     *
5
 *                                                                          *
6
 *   This file is part of FreeCAD.                                          *
7
 *                                                                          *
8
 *   FreeCAD is free software: you can redistribute it and/or modify it     *
9
 *   under the terms of the GNU Lesser General Public License as            *
10
 *   published by the Free Software Foundation, either version 2.1 of the   *
11
 *   License, or (at your option) any later version.                        *
12
 *                                                                          *
13
 *   FreeCAD is distributed in the hope that it will be useful, but         *
14
 *   WITHOUT ANY WARRANTY; without even the implied warranty of             *
15
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU       *
16
 *   Lesser General Public License for more details.                        *
17
 *                                                                          *
18
 *   You should have received a copy of the GNU Lesser General Public       *
19
 *   License along with FreeCAD. If not, see                                *
20
 *   <https://www.gnu.org/licenses/>.                                       *
21
 *                                                                          *
22
 ***************************************************************************/
23

24
#include "PreCompiled.h"
25
#ifndef _PreComp_
26
#include <BRepAdaptor_Curve.hxx>
27
#include <BRepAdaptor_Surface.hxx>
28
#include <TopoDS.hxx>
29
#include <TopoDS_Face.hxx>
30
#include <gp_Circ.hxx>
31
#include <gp_Cylinder.hxx>
32
#include <gp_Sphere.hxx>
33
#include <cmath>
34
#include <vector>
35
#include <unordered_map>
36
#endif
37

38
#include <App/Application.h>
39
#include <App/Document.h>
40
#include <App/DocumentObjectGroup.h>
41
#include <App/FeaturePythonPyImp.h>
42
#include <App/Link.h>
43
#include <App/PropertyPythonObject.h>
44
#include <Base/Console.h>
45
#include <Base/Placement.h>
46
#include <Base/Rotation.h>
47
#include <Base/Tools.h>
48
#include <Base/Interpreter.h>
49

50
#include <Mod/Part/App/PartFeature.h>
51
#include <Mod/Part/App/TopoShape.h>
52
#include <Mod/PartDesign/App/Body.h>
53

54
#include <OndselSolver/CREATE.h>
55
#include <OndselSolver/ASMTSimulationParameters.h>
56
#include <OndselSolver/ASMTAssembly.h>
57
#include <OndselSolver/ASMTMarker.h>
58
#include <OndselSolver/ASMTPart.h>
59
#include <OndselSolver/ASMTJoint.h>
60
#include <OndselSolver/ASMTFixedJoint.h>
61
#include <OndselSolver/ASMTRevoluteJoint.h>
62
#include <OndselSolver/ASMTCylindricalJoint.h>
63
#include <OndselSolver/ASMTTranslationalJoint.h>
64
#include <OndselSolver/ASMTSphericalJoint.h>
65
#include <OndselSolver/ASMTPointInPlaneJoint.h>
66
#include <OndselSolver/ASMTPointInLineJoint.h>
67
#include <OndselSolver/ASMTLineInPlaneJoint.h>
68
#include <OndselSolver/ASMTPlanarJoint.h>
69
#include <OndselSolver/ASMTRevCylJoint.h>
70
#include <OndselSolver/ASMTCylSphJoint.h>
71
#include <OndselSolver/ASMTSphSphJoint.h>
72
#include <OndselSolver/ASMTTime.h>
73
#include <OndselSolver/ASMTConstantGravity.h>
74

75
#include "AssemblyObject.h"
76
#include "AssemblyObjectPy.h"
77
#include "JointGroup.h"
78

79
namespace PartApp = Part;
80

81
using namespace Assembly;
82
using namespace MbD;
83

84
// ================================ Assembly Object ============================
85

86
PROPERTY_SOURCE(Assembly::AssemblyObject, App::Part)
87

88
AssemblyObject::AssemblyObject()
89
    : mbdAssembly(std::make_shared<ASMTAssembly>())
90
{}
91

92
AssemblyObject::~AssemblyObject() = default;
93

94
PyObject* AssemblyObject::getPyObject()
95
{
96
    if (PythonObject.is(Py::_None())) {
97
        // ref counter is set to 1
98
        PythonObject = Py::Object(new AssemblyObjectPy(this), true);
99
    }
100
    return Py::new_reference_to(PythonObject);
101
}
102

103

104
int AssemblyObject::solve(bool enableRedo)
105
{
106
    mbdAssembly = makeMbdAssembly();
107
    objectPartMap.clear();
108

109
    std::vector<App::DocumentObject*> groundedObjs = fixGroundedParts();
110
    if (groundedObjs.empty()) {
111
        // If no part fixed we can't solve.
112
        return -6;
113
    }
114

115
    std::vector<App::DocumentObject*> joints = getJoints();
116

117
    removeUnconnectedJoints(joints, groundedObjs);
118

119
    jointParts(joints);
120

121
    if (enableRedo) {
122
        savePlacementsForUndo();
123
    }
124

125
    try {
126
        mbdAssembly->solve();
127
    }
128
    catch (...) {
129
        Base::Console().Error("Solve failed\n");
130
        return -1;
131
    }
132

133
    setNewPlacements();
134

135
    redrawJointPlacements(joints);
136

137
    return 0;
138
}
139

140
void AssemblyObject::preDrag(std::vector<App::DocumentObject*> dragParts)
141
{
142
    solve();
143

144
    dragMbdParts.clear();
145
    for (auto part : dragParts) {
146
        dragMbdParts.push_back(getMbDPart(part));
147
    }
148

149
    mbdAssembly->runPreDrag();
150
}
151

152
void AssemblyObject::doDragStep()
153
{
154
    for (auto& mbdPart : dragMbdParts) {
155
        App::DocumentObject* part = nullptr;
156
        for (auto& pair : objectPartMap) {
157
            if (pair.second == mbdPart) {
158
                part = pair.first;
159
                break;
160
            }
161
        }
162
        if (!part) {
163
            continue;
164
        }
165

166
        Base::Placement plc = getPlacementFromProp(part, "Placement");
167
        Base::Vector3d pos = plc.getPosition();
168
        mbdPart->setPosition3D(pos.x, pos.y, pos.z);
169

170
        Base::Rotation rot = plc.getRotation();
171
        Base::Matrix4D mat;
172
        rot.getValue(mat);
173
        Base::Vector3d r0 = mat.getRow(0);
174
        Base::Vector3d r1 = mat.getRow(1);
175
        Base::Vector3d r2 = mat.getRow(2);
176
        mbdPart->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z);
177
    }
178

179
    auto dragPartsVec = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>(dragMbdParts);
180
    mbdAssembly->runDragStep(dragPartsVec);
181
    setNewPlacements();
182
    redrawJointPlacements(getJoints());
183
}
184

185
void AssemblyObject::postDrag()
186
{
187
    mbdAssembly->runPostDrag();  // Do this after last drag
188
}
189

190
void AssemblyObject::savePlacementsForUndo()
191
{
192
    previousPositions.clear();
193

194
    for (auto& pair : objectPartMap) {
195
        App::DocumentObject* obj = pair.first;
196
        if (!obj) {
197
            continue;
198
        }
199

200
        std::pair<App::DocumentObject*, Base::Placement> savePair;
201
        savePair.first = obj;
202

203
        // Check if the object has a "Placement" property
204
        auto* propPlc = dynamic_cast<App::PropertyPlacement*>(obj->getPropertyByName("Placement"));
205
        if (!propPlc) {
206
            continue;
207
        }
208
        savePair.second = propPlc->getValue();
209

210
        previousPositions.push_back(savePair);
211
    }
212
}
213

214
void AssemblyObject::undoSolve()
215
{
216
    if (previousPositions.size() == 0) {
217
        return;
218
    }
219

220
    for (auto& pair : previousPositions) {
221
        App::DocumentObject* obj = pair.first;
222
        if (!obj) {
223
            continue;
224
        }
225

226
        // Check if the object has a "Placement" property
227
        auto* propPlacement =
228
            dynamic_cast<App::PropertyPlacement*>(obj->getPropertyByName("Placement"));
229
        if (!propPlacement) {
230
            continue;
231
        }
232

233
        propPlacement->setValue(pair.second);
234
    }
235
    previousPositions.clear();
236

237
    // update joint placements:
238
    getJoints();
239
}
240

241
void AssemblyObject::clearUndo()
242
{
243
    previousPositions.clear();
244
}
245

246
void AssemblyObject::exportAsASMT(std::string fileName)
247
{
248
    mbdAssembly = makeMbdAssembly();
249
    objectPartMap.clear();
250
    fixGroundedParts();
251

252
    std::vector<App::DocumentObject*> joints = getJoints();
253

254
    jointParts(joints);
255

256
    mbdAssembly->outputFile(fileName);
257
}
258

259
void AssemblyObject::setNewPlacements()
260
{
261
    for (auto& pair : objectPartMap) {
262
        App::DocumentObject* obj = pair.first;
263
        std::shared_ptr<ASMTPart> mbdPart = pair.second;
264

265
        if (!obj || !mbdPart) {
266
            continue;
267
        }
268

269
        // Check if the object has a "Placement" property
270
        auto* propPlacement =
271
            dynamic_cast<App::PropertyPlacement*>(obj->getPropertyByName("Placement"));
272
        if (!propPlacement) {
273
            continue;
274
        }
275

276
        double x, y, z;
277
        mbdPart->getPosition3D(x, y, z);
278
        // Base::Console().Warning("in set placement : (%f, %f, %f)\n", x, y, z);
279
        Base::Vector3d pos = Base::Vector3d(x, y, z);
280

281
        // TODO : replace with quaternion to simplify
282
        auto& r0 = mbdPart->rotationMatrix->at(0);
283
        auto& r1 = mbdPart->rotationMatrix->at(1);
284
        auto& r2 = mbdPart->rotationMatrix->at(2);
285
        Base::Vector3d row0 = Base::Vector3d(r0->at(0), r0->at(1), r0->at(2));
286
        Base::Vector3d row1 = Base::Vector3d(r1->at(0), r1->at(1), r1->at(2));
287
        Base::Vector3d row2 = Base::Vector3d(r2->at(0), r2->at(1), r2->at(2));
288
        Base::Matrix4D mat;
289
        mat.setRow(0, row0);
290
        mat.setRow(1, row1);
291
        mat.setRow(2, row2);
292
        Base::Rotation rot = Base::Rotation(mat);
293

294
        /*double q0, q1, q2, q3;
295
        mbdPart->getQuarternions(q0, q1, q2, q3);
296
        Base::Rotation rot = Base::Rotation(q0, q1, q2, q3);*/
297

298
        Base::Placement newPlacement = Base::Placement(pos, rot);
299

300
        propPlacement->setValue(newPlacement);
301
    }
302
}
303

304
void AssemblyObject::redrawJointPlacements(std::vector<App::DocumentObject*> joints)
305
{
306
    // Notify the joint objects that the transform of the coin object changed.
307
    for (auto* joint : joints) {
308
        auto* propPlacement =
309
            dynamic_cast<App::PropertyPlacement*>(joint->getPropertyByName("Placement1"));
310
        if (propPlacement) {
311
            propPlacement->setValue(propPlacement->getValue());
312
        }
313
        propPlacement =
314
            dynamic_cast<App::PropertyPlacement*>(joint->getPropertyByName("Placement2"));
315
        if (propPlacement) {
316
            propPlacement->setValue(propPlacement->getValue());
317
        }
318
    }
319
}
320

321
void AssemblyObject::recomputeJointPlacements(std::vector<App::DocumentObject*> joints)
322
{
323
    // The Placement1 and Placement2 of each joint needs to be updated as the parts moved.
324
    for (auto* joint : joints) {
325
        App::PropertyPythonObject* proxy = joint
326
            ? dynamic_cast<App::PropertyPythonObject*>(joint->getPropertyByName("Proxy"))
327
            : nullptr;
328

329
        if (!proxy) {
330
            continue;
331
        }
332

333
        Py::Object jointPy = proxy->getValue();
334

335
        if (!jointPy.hasAttr("updateJCSPlacements")) {
336
            continue;
337
        }
338

339
        Py::Object attr = jointPy.getAttr("updateJCSPlacements");
340
        if (attr.ptr() && attr.isCallable()) {
341
            Py::Tuple args(1);
342
            args.setItem(0, Py::asObject(joint->getPyObject()));
343
            Py::Callable(attr).apply(args);
344
        }
345
    }
346
}
347

348
std::shared_ptr<ASMTAssembly> AssemblyObject::makeMbdAssembly()
349
{
350
    auto assembly = CREATE<ASMTAssembly>::With();
351
    assembly->setName("OndselAssembly");
352

353
    return assembly;
354
}
355

356
App::DocumentObject* AssemblyObject::getJointOfPartConnectingToGround(App::DocumentObject* part,
357
                                                                      std::string& name)
358
{
359
    std::vector<App::DocumentObject*> joints = getJointsOfPart(part);
360

361
    for (auto joint : joints) {
362
        if (!joint) {
363
            continue;
364
        }
365
        App::DocumentObject* part1 = getLinkObjFromProp(joint, "Part1");
366
        App::DocumentObject* part2 = getLinkObjFromProp(joint, "Part2");
367
        if (!part1 || !part2) {
368
            continue;
369
        }
370

371
        if (part == part1 && isJointConnectingPartToGround(joint, "Part1")) {
372
            name = "Part1";
373
            return joint;
374
        }
375
        if (part == part2 && isJointConnectingPartToGround(joint, "Part2")) {
376
            name = "Part2";
377
            return joint;
378
        }
379
    }
380

381
    return nullptr;
382
}
383

384
JointGroup* AssemblyObject::getJointGroup()
385
{
386
    App::Document* doc = getDocument();
387

388
    std::vector<DocumentObject*> jointGroups =
389
        doc->getObjectsOfType(Assembly::JointGroup::getClassTypeId());
390
    if (jointGroups.empty()) {
391
        return nullptr;
392
    }
393
    for (auto jointGroup : jointGroups) {
394
        if (hasObject(jointGroup)) {
395
            return dynamic_cast<JointGroup*>(jointGroup);
396
        }
397
    }
398
    return nullptr;
399
}
400

401
std::vector<App::DocumentObject*> AssemblyObject::getJoints(bool updateJCS)
402
{
403
    std::vector<App::DocumentObject*> joints = {};
404

405
    JointGroup* jointGroup = getJointGroup();
406
    if (!jointGroup) {
407
        return {};
408
    }
409

410
    Base::PyGILStateLocker lock;
411
    for (auto joint : jointGroup->getObjects()) {
412
        if (!joint) {
413
            continue;
414
        }
415

416
        auto* prop = dynamic_cast<App::PropertyBool*>(joint->getPropertyByName("Activated"));
417
        if (prop && !prop->getValue()) {
418
            continue;
419
        }
420

421
        auto proxy = dynamic_cast<App::PropertyPythonObject*>(joint->getPropertyByName("Proxy"));
422
        if (proxy) {
423
            if (proxy->getValue().hasAttr("setJointConnectors")) {
424
                joints.push_back(joint);
425
            }
426
        }
427
    }
428

429
    // Make sure the joints are up to date.
430
    if (updateJCS) {
431
        recomputeJointPlacements(joints);
432
    }
433

434
    return joints;
435
}
436

437
std::vector<App::DocumentObject*> AssemblyObject::getGroundedJoints()
438
{
439
    std::vector<App::DocumentObject*> joints = {};
440

441
    JointGroup* jointGroup = getJointGroup();
442
    if (!jointGroup) {
443
        return {};
444
    }
445

446
    Base::PyGILStateLocker lock;
447
    for (auto obj : jointGroup->getObjects()) {
448
        if (!obj) {
449
            continue;
450
        }
451

452
        auto* propObj = dynamic_cast<App::PropertyLink*>(obj->getPropertyByName("ObjectToGround"));
453

454
        if (propObj) {
455
            joints.push_back(obj);
456
        }
457
    }
458

459
    return joints;
460
}
461

462
std::vector<App::DocumentObject*> AssemblyObject::getJointsOfObj(App::DocumentObject* obj)
463
{
464
    std::vector<App::DocumentObject*> joints = getJoints(false);
465
    std::vector<App::DocumentObject*> jointsOf;
466

467
    for (auto joint : joints) {
468
        App::DocumentObject* obj1 = getObjFromNameProp(joint, "object1", "Part1");
469
        App::DocumentObject* obj2 = getObjFromNameProp(joint, "Object2", "Part2");
470
        if (obj == obj1 || obj == obj2) {
471
            jointsOf.push_back(obj);
472
        }
473
    }
474

475
    return jointsOf;
476
}
477

478
std::vector<App::DocumentObject*> AssemblyObject::getJointsOfPart(App::DocumentObject* part)
479
{
480
    std::vector<App::DocumentObject*> joints = getJoints(false);
481
    std::vector<App::DocumentObject*> jointsOf;
482

483
    for (auto joint : joints) {
484
        App::DocumentObject* part1 = getLinkObjFromProp(joint, "Part1");
485
        App::DocumentObject* part2 = getLinkObjFromProp(joint, "Part2");
486
        if (part == part1 || part == part2) {
487
            jointsOf.push_back(joint);
488
        }
489
    }
490

491
    return jointsOf;
492
}
493

494
std::vector<App::DocumentObject*> AssemblyObject::getGroundedParts()
495
{
496
    std::vector<App::DocumentObject*> groundedJoints = getGroundedJoints();
497

498
    std::vector<App::DocumentObject*> groundedObjs;
499
    for (auto gJoint : groundedJoints) {
500
        if (!gJoint) {
501
            continue;
502
        }
503

504
        auto* propObj =
505
            dynamic_cast<App::PropertyLink*>(gJoint->getPropertyByName("ObjectToGround"));
506

507
        if (propObj) {
508
            App::DocumentObject* objToGround = propObj->getValue();
509
            groundedObjs.push_back(objToGround);
510
        }
511
    }
512
    return groundedObjs;
513
}
514

515
std::vector<App::DocumentObject*> AssemblyObject::fixGroundedParts()
516
{
517
    std::vector<App::DocumentObject*> groundedJoints = getGroundedJoints();
518

519
    std::vector<App::DocumentObject*> groundedObjs;
520
    for (auto obj : groundedJoints) {
521
        if (!obj) {
522
            continue;
523
        }
524

525
        auto* propObj = dynamic_cast<App::PropertyLink*>(obj->getPropertyByName("ObjectToGround"));
526

527
        if (propObj) {
528
            App::DocumentObject* objToGround = propObj->getValue();
529

530
            Base::Placement plc = getPlacementFromProp(obj, "Placement");
531
            std::string str = obj->getFullName();
532
            fixGroundedPart(objToGround, plc, str);
533
            groundedObjs.push_back(objToGround);
534
        }
535
    }
536
    return groundedObjs;
537
}
538

539
void AssemblyObject::fixGroundedPart(App::DocumentObject* obj,
540
                                     Base::Placement& plc,
541
                                     std::string& name)
542
{
543
    std::string markerName1 = "marker-" + obj->getFullName();
544
    auto mbdMarker1 = makeMbdMarker(markerName1, plc);
545
    mbdAssembly->addMarker(mbdMarker1);
546

547
    std::shared_ptr<ASMTPart> mbdPart = getMbDPart(obj);
548

549
    std::string markerName2 = "FixingMarker";
550
    Base::Placement basePlc = Base::Placement();
551
    auto mbdMarker2 = makeMbdMarker(markerName2, basePlc);
552
    mbdPart->addMarker(mbdMarker2);
553

554
    markerName1 = "/OndselAssembly/" + mbdMarker1->name;
555
    markerName2 = "/OndselAssembly/" + mbdPart->name + "/" + mbdMarker2->name;
556

557
    auto mbdJoint = CREATE<ASMTFixedJoint>::With();
558
    mbdJoint->setName(name);
559
    mbdJoint->setMarkerI(markerName1);
560
    mbdJoint->setMarkerJ(markerName2);
561

562
    mbdAssembly->addJoint(mbdJoint);
563
}
564

565
bool AssemblyObject::isJointConnectingPartToGround(App::DocumentObject* joint, const char* propname)
566
{
567

568
    auto* propPart = dynamic_cast<App::PropertyLink*>(joint->getPropertyByName(propname));
569
    if (!propPart) {
570
        return false;
571
    }
572
    App::DocumentObject* part = propPart->getValue();
573
    // Check if the part is disconnected even with the joint
574
    bool isConnected = isPartConnected(part);
575
    if (!isConnected) {
576
        return false;
577
    }
578

579
    // to know if a joint is connecting to ground we disable all the other joints
580
    std::vector<App::DocumentObject*> jointsOfPart = getJointsOfPart(part);
581
    std::vector<bool> activatedStates;
582

583
    for (auto jointi : jointsOfPart) {
584
        if (jointi->getFullName() == joint->getFullName()) {
585
            continue;
586
        }
587

588
        activatedStates.push_back(getJointActivated(jointi));
589
        setJointActivated(jointi, false);
590
    }
591

592
    isConnected = isPartConnected(part);
593

594
    // restore activation states
595
    for (auto jointi : jointsOfPart) {
596
        if (jointi->getFullName() == joint->getFullName() || activatedStates.empty()) {
597
            continue;
598
        }
599

600
        setJointActivated(jointi, activatedStates[0]);
601
        activatedStates.erase(activatedStates.begin());
602
    }
603

604
    return isConnected;
605
}
606

607
void AssemblyObject::removeUnconnectedJoints(std::vector<App::DocumentObject*>& joints,
608
                                             std::vector<App::DocumentObject*> groundedObjs)
609
{
610
    std::set<App::DocumentObject*> connectedParts;
611

612
    // Initialize connectedParts with groundedObjs
613
    for (auto* groundedObj : groundedObjs) {
614
        connectedParts.insert(groundedObj);
615
    }
616

617
    // Perform a traversal from each grounded object
618
    for (auto* groundedObj : groundedObjs) {
619
        traverseAndMarkConnectedParts(groundedObj, connectedParts, joints);
620
    }
621

622
    // Filter out unconnected joints
623
    joints.erase(
624
        std::remove_if(
625
            joints.begin(),
626
            joints.end(),
627
            [&connectedParts](App::DocumentObject* joint) {
628
                App::DocumentObject* obj1 = getLinkObjFromProp(joint, "Part1");
629
                App::DocumentObject* obj2 = getLinkObjFromProp(joint, "Part2");
630
                if ((connectedParts.find(obj1) == connectedParts.end())
631
                    || (connectedParts.find(obj2) == connectedParts.end())) {
632
                    Base::Console().Warning(
633
                        "%s is unconnected to a grounded part so it is ignored.\n",
634
                        joint->getFullName());
635
                    return true;  // Remove joint if any connected object is not in connectedParts
636
                }
637
                return false;
638
            }),
639
        joints.end());
640
}
641

642
void AssemblyObject::traverseAndMarkConnectedParts(App::DocumentObject* currentObj,
643
                                                   std::set<App::DocumentObject*>& connectedParts,
644
                                                   const std::vector<App::DocumentObject*>& joints)
645
{
646
    // getConnectedParts returns the objs connected to the currentObj by any joint
647
    auto connectedObjs = getConnectedParts(currentObj, joints);
648
    for (auto* nextObj : connectedObjs) {
649
        if (connectedParts.find(nextObj) == connectedParts.end()) {
650
            connectedParts.insert(nextObj);
651
            traverseAndMarkConnectedParts(nextObj, connectedParts, joints);
652
        }
653
    }
654
}
655

656
std::vector<App::DocumentObject*>
657
AssemblyObject::getConnectedParts(App::DocumentObject* part,
658
                                  const std::vector<App::DocumentObject*>& joints)
659
{
660
    std::vector<App::DocumentObject*> connectedParts;
661
    for (auto joint : joints) {
662
        App::DocumentObject* obj1 = getLinkObjFromProp(joint, "Part1");
663
        App::DocumentObject* obj2 = getLinkObjFromProp(joint, "Part2");
664
        if (obj1 == part) {
665
            connectedParts.push_back(obj2);
666
        }
667
        else if (obj2 == part) {
668
            connectedParts.push_back(obj1);
669
        }
670
    }
671
    return connectedParts;
672
}
673

674
bool AssemblyObject::isPartGrounded(App::DocumentObject* obj)
675
{
676
    std::vector<App::DocumentObject*> groundedObjs = fixGroundedParts();
677

678
    for (auto* groundedObj : groundedObjs) {
679
        if (groundedObj->getFullName() == obj->getFullName()) {
680
            return true;
681
        }
682
    }
683

684
    return false;
685
}
686

687
bool AssemblyObject::isPartConnected(App::DocumentObject* obj)
688
{
689
    std::vector<App::DocumentObject*> groundedObjs = getGroundedParts();
690
    std::vector<App::DocumentObject*> joints = getJoints(false);
691

692
    std::set<App::DocumentObject*> connectedParts;
693

694
    // Initialize connectedParts with groundedObjs
695
    for (auto* groundedObj : groundedObjs) {
696
        connectedParts.insert(groundedObj);
697
    }
698

699
    // Perform a traversal from each grounded object
700
    for (auto* groundedObj : groundedObjs) {
701
        traverseAndMarkConnectedParts(groundedObj, connectedParts, joints);
702
    }
703

704
    for (auto part : connectedParts) {
705
        if (obj == part) {
706
            return true;
707
        }
708
    }
709

710
    return false;
711
}
712

713
void AssemblyObject::jointParts(std::vector<App::DocumentObject*> joints)
714
{
715
    for (auto* joint : joints) {
716
        if (!joint) {
717
            continue;
718
        }
719

720
        std::vector<std::shared_ptr<MbD::ASMTJoint>> mbdJoints = makeMbdJoint(joint);
721
        for (auto& mbdJoint : mbdJoints) {
722
            mbdAssembly->addJoint(mbdJoint);
723
        }
724
    }
725
}
726

727
std::shared_ptr<ASMTJoint> AssemblyObject::makeMbdJointOfType(App::DocumentObject* joint,
728
                                                              JointType type)
729
{
730
    if (type == JointType::Fixed) {
731
        return CREATE<ASMTFixedJoint>::With();
732
    }
733
    else if (type == JointType::Revolute) {
734
        return CREATE<ASMTRevoluteJoint>::With();
735
    }
736
    else if (type == JointType::Cylindrical) {
737
        return CREATE<ASMTCylindricalJoint>::With();
738
    }
739
    else if (type == JointType::Slider) {
740
        return CREATE<ASMTTranslationalJoint>::With();
741
    }
742
    else if (type == JointType::Ball) {
743
        return CREATE<ASMTSphericalJoint>::With();
744
    }
745
    else if (type == JointType::Distance) {
746
        return makeMbdJointDistance(joint);
747
    }
748

749
    return nullptr;
750
}
751

752
std::shared_ptr<ASMTJoint> AssemblyObject::makeMbdJointDistance(App::DocumentObject* joint)
753
{
754
    // Depending on the type of element of the JCS, we apply the correct set of constraints.
755
    std::string type1 = getElementTypeFromProp(joint, "Element1");
756
    std::string type2 = getElementTypeFromProp(joint, "Element2");
757

758
    if (type1 == "Vertex" && type2 == "Vertex") {
759
        // Point to point distance, or ball joint if distance=0.
760
        auto mbdJoint = CREATE<ASMTSphSphJoint>::With();
761
        mbdJoint->distanceIJ = getJointDistance(joint);
762
        return mbdJoint;
763
    }
764
    else if (type1 == "Edge" && type2 == "Edge") {
765
        return makeMbdJointDistanceEdgeEdge(joint);
766
    }
767
    else if (type1 == "Face" && type2 == "Face") {
768
        return makeMbdJointDistanceFaceFace(joint);
769
    }
770
    else if ((type1 == "Vertex" && type2 == "Face") || (type1 == "Face" && type2 == "Vertex")) {
771
        if (type1 == "Vertex") {  // Make sure face is the first.
772
            swapJCS(joint);
773
        }
774
        return makeMbdJointDistanceFaceVertex(joint);
775
    }
776
    else if ((type1 == "Edge" && type2 == "Face") || (type1 == "Face" && type2 == "Edge")) {
777
        if (type1 == "Edge") {  // Make sure face is the first.
778
            swapJCS(joint);
779
        }
780
        return makeMbdJointDistanceFaceEdge(joint);
781
    }
782
    else if ((type1 == "Vertex" && type2 == "Edge") || (type1 == "Edge" && type2 == "Vertex")) {
783
        if (type1 == "Vertex") {  // Make sure edge is the first.
784
            swapJCS(joint);
785
        }
786
        return makeMbdJointDistanceEdgeVertex(joint);
787
    }
788

789
    return nullptr;
790
}
791

792
std::shared_ptr<ASMTJoint> AssemblyObject::makeMbdJointDistanceEdgeEdge(App::DocumentObject* joint)
793
{
794
    const char* elt1 = getElementFromProp(joint, "Element1");
795
    const char* elt2 = getElementFromProp(joint, "Element2");
796
    auto* obj1 = getLinkedObjFromNameProp(joint, "Object1", "Part1");
797
    auto* obj2 = getLinkedObjFromNameProp(joint, "Object2", "Part2");
798

799
    if (isEdgeType(obj1, elt1, GeomAbs_Line) || isEdgeType(obj2, elt2, GeomAbs_Line)) {
800
        if (!isEdgeType(obj1, elt1, GeomAbs_Line)) {
801
            swapJCS(joint);  // make sure that line is first if not 2 lines.
802
            std::swap(elt1, elt2);
803
            std::swap(obj1, obj2);
804
        }
805

806
        if (isEdgeType(obj2, elt2, GeomAbs_Line)) {
807
            auto mbdJoint = CREATE<ASMTRevCylJoint>::With();
808
            mbdJoint->distanceIJ = getJointDistance(joint);
809
            return mbdJoint;
810
        }
811
        else if (isEdgeType(obj2, elt2, GeomAbs_Circle)) {
812
            auto mbdJoint = CREATE<ASMTRevCylJoint>::With();
813
            mbdJoint->distanceIJ = getJointDistance(joint) + getEdgeRadius(obj2, elt2);
814
            return mbdJoint;
815
        }
816
        // TODO : other cases Ellipse, parabola, hyperbola...
817
    }
818

819
    else if (isEdgeType(obj1, elt1, GeomAbs_Circle) || isEdgeType(obj2, elt2, GeomAbs_Circle)) {
820
        if (!isEdgeType(obj1, elt1, GeomAbs_Circle)) {
821
            swapJCS(joint);  // make sure that circle is first if not 2 lines.
822
            std::swap(elt1, elt2);
823
            std::swap(obj1, obj2);
824
        }
825

826
        if (isEdgeType(obj2, elt2, GeomAbs_Circle)) {
827
            auto mbdJoint = CREATE<ASMTRevCylJoint>::With();
828
            mbdJoint->distanceIJ =
829
                getJointDistance(joint) + getEdgeRadius(obj1, elt1) + getEdgeRadius(obj2, elt2);
830
            return mbdJoint;
831
        }
832
        // TODO : other cases Ellipse, parabola, hyperbola...
833
    }
834

835
    // TODO : other cases Ellipse, parabola, hyperbola...
836

837
    return nullptr;
838
}
839

840
std::shared_ptr<ASMTJoint> AssemblyObject::makeMbdJointDistanceFaceFace(App::DocumentObject* joint)
841
{
842
    const char* elt1 = getElementFromProp(joint, "Element1");
843
    const char* elt2 = getElementFromProp(joint, "Element2");
844
    auto* obj1 = getLinkedObjFromNameProp(joint, "Object1", "Part1");
845
    auto* obj2 = getLinkedObjFromNameProp(joint, "Object2", "Part2");
846

847
    if (isFaceType(obj1, elt1, GeomAbs_Plane) || isFaceType(obj2, elt2, GeomAbs_Plane)) {
848
        if (!isFaceType(obj1, elt1, GeomAbs_Plane)) {
849
            swapJCS(joint);  // make sure plane is first if its not 2 planes.
850
            std::swap(elt1, elt2);
851
            std::swap(obj1, obj2);
852
        }
853

854
        if (isFaceType(obj2, elt2, GeomAbs_Plane)) {
855
            auto mbdJoint = CREATE<ASMTPlanarJoint>::With();
856
            mbdJoint->offset = getJointDistance(joint);
857
            return mbdJoint;
858
        }
859
        else if (isFaceType(obj2, elt2, GeomAbs_Cylinder)) {
860
            auto mbdJoint = CREATE<ASMTLineInPlaneJoint>::With();
861
            mbdJoint->offset = getJointDistance(joint) + getFaceRadius(obj2, elt2);
862
            return mbdJoint;
863
        }
864
        else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
865
            auto mbdJoint = CREATE<ASMTPointInPlaneJoint>::With();
866
            mbdJoint->offset = getJointDistance(joint) + getFaceRadius(obj2, elt2);
867
            return mbdJoint;
868
        }
869
        else if (isFaceType(obj2, elt2, GeomAbs_Cone)) {
870
            // TODO
871
        }
872
        else if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
873
            auto mbdJoint = CREATE<ASMTPlanarJoint>::With();
874
            mbdJoint->offset = getJointDistance(joint);
875
            return mbdJoint;
876
        }
877
    }
878

879
    else if (isFaceType(obj1, elt1, GeomAbs_Cylinder) || isFaceType(obj2, elt2, GeomAbs_Cylinder)) {
880
        if (!isFaceType(obj1, elt1, GeomAbs_Cylinder)) {
881
            swapJCS(joint);  // make sure cylinder is first if its not 2 cylinders.
882
            std::swap(elt1, elt2);
883
            std::swap(obj1, obj2);
884
        }
885

886
        if (isFaceType(obj2, elt2, GeomAbs_Cylinder)) {
887
            auto mbdJoint = CREATE<ASMTRevCylJoint>::With();
888
            mbdJoint->distanceIJ =
889
                getJointDistance(joint) + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2);
890
            return mbdJoint;
891
        }
892
        else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
893
            auto mbdJoint = CREATE<ASMTCylSphJoint>::With();
894
            mbdJoint->distanceIJ =
895
                getJointDistance(joint) + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2);
896
            return mbdJoint;
897
        }
898
        else if (isFaceType(obj2, elt2, GeomAbs_Cone)) {
899
            // TODO
900
        }
901
        else if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
902
            auto mbdJoint = CREATE<ASMTRevCylJoint>::With();
903
            mbdJoint->distanceIJ =
904
                getJointDistance(joint) + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2);
905
            return mbdJoint;
906
        }
907
    }
908

909
    else if (isFaceType(obj1, elt1, GeomAbs_Cone) || isFaceType(obj2, elt2, GeomAbs_Cone)) {
910
        if (!isFaceType(obj1, elt1, GeomAbs_Cone)) {
911
            swapJCS(joint);  // make sure cone is first if its not 2 cones.
912
            std::swap(elt1, elt2);
913
            std::swap(obj1, obj2);
914
        }
915

916
        if (isFaceType(obj2, elt2, GeomAbs_Cone)) {
917
            // TODO
918
        }
919
        else if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
920
            // TODO
921
        }
922
        else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
923
            // TODO
924
        }
925
    }
926

927
    else if (isFaceType(obj1, elt1, GeomAbs_Torus) || isFaceType(obj2, elt2, GeomAbs_Torus)) {
928
        if (!isFaceType(obj1, elt1, GeomAbs_Torus)) {
929
            swapJCS(joint);  // make sure torus is first if its not 2 torus.
930
            std::swap(elt1, elt2);
931
            std::swap(obj1, obj2);
932
        }
933

934
        if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
935
            auto mbdJoint = CREATE<ASMTPlanarJoint>::With();
936
            mbdJoint->offset = getJointDistance(joint);
937
            return mbdJoint;
938
        }
939
        else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
940
            auto mbdJoint = CREATE<ASMTCylSphJoint>::With();
941
            mbdJoint->distanceIJ =
942
                getJointDistance(joint) + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2);
943
            return mbdJoint;
944
        }
945
    }
946

947
    else if (isFaceType(obj1, elt1, GeomAbs_Sphere) || isFaceType(obj2, elt2, GeomAbs_Sphere)) {
948
        if (!isFaceType(obj1, elt1, GeomAbs_Sphere)) {
949
            swapJCS(joint);  // make sure sphere is first if its not 2 spheres.
950
            std::swap(elt1, elt2);
951
            std::swap(obj1, obj2);
952
        }
953

954
        if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
955
            auto mbdJoint = CREATE<ASMTSphSphJoint>::With();
956
            mbdJoint->distanceIJ =
957
                getJointDistance(joint) + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2);
958
            return mbdJoint;
959
        }
960
    }
961
    else {
962
        // by default we make a planar joint.
963
        auto mbdJoint = CREATE<ASMTPlanarJoint>::With();
964
        mbdJoint->offset = getJointDistance(joint);
965
        return mbdJoint;
966
    }
967

968
    return nullptr;
969
}
970

971
std::shared_ptr<ASMTJoint>
972
AssemblyObject::makeMbdJointDistanceFaceVertex(App::DocumentObject* joint)
973
{
974
    const char* elt1 = getElementFromProp(joint, "Element1");
975
    auto* obj1 = getLinkedObjFromNameProp(joint, "Object1", "Part1");
976

977
    if (isFaceType(obj1, elt1, GeomAbs_Plane)) {
978
        auto mbdJoint = CREATE<ASMTPointInPlaneJoint>::With();
979
        mbdJoint->offset = getJointDistance(joint);
980
        return mbdJoint;
981
    }
982
    else if (isFaceType(obj1, elt1, GeomAbs_Cylinder)) {
983
        auto mbdJoint = CREATE<ASMTCylSphJoint>::With();
984
        mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1);
985
        return mbdJoint;
986
    }
987
    else if (isFaceType(obj1, elt1, GeomAbs_Sphere)) {
988
        auto mbdJoint = CREATE<ASMTSphSphJoint>::With();
989
        mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1);
990
        return mbdJoint;
991
    }
992
    /*else if (isFaceType(obj1, elt1, GeomAbs_Cone)) {
993
        // TODO
994
    }
995
    else if (isFaceType(obj1, elt1, GeomAbs_Thorus)) {
996
        // TODO
997
    }*/
998

999
    return nullptr;
1000
}
1001

1002
std::shared_ptr<ASMTJoint>
1003
AssemblyObject::makeMbdJointDistanceEdgeVertex(App::DocumentObject* joint)
1004
{
1005
    const char* elt1 = getElementFromProp(joint, "Element1");
1006
    auto* obj1 = getLinkedObjFromNameProp(joint, "Object1", "Part1");
1007

1008
    if (isEdgeType(obj1, elt1, GeomAbs_Line)) {  // Point on line joint.
1009
        auto mbdJoint = CREATE<ASMTCylSphJoint>::With();
1010
        mbdJoint->distanceIJ = getJointDistance(joint);
1011
        return mbdJoint;
1012
    }
1013
    else {
1014
        // For other curves we do a point in plane-of-the-curve.
1015
        // Maybe it would be best tangent / distance to the conic?
1016
        // For arcs and circles we could use ASMTRevSphJoint. But is it better than pointInPlane?
1017
        auto mbdJoint = CREATE<ASMTPointInPlaneJoint>::With();
1018
        mbdJoint->offset = getJointDistance(joint);
1019
        return mbdJoint;
1020
    }
1021

1022
    return nullptr;
1023
}
1024

1025
std::shared_ptr<ASMTJoint> AssemblyObject::makeMbdJointDistanceFaceEdge(App::DocumentObject* joint)
1026
{
1027
    const char* elt2 = getElementFromProp(joint, "Element2");
1028
    auto* obj2 = getLinkedObjFromNameProp(joint, "Object2", "Part2");
1029

1030
    if (isEdgeType(obj2, elt2, GeomAbs_Line)) {
1031
        // Make line in plane joint.
1032
        auto mbdJoint = CREATE<ASMTLineInPlaneJoint>::With();
1033
        mbdJoint->offset = getJointDistance(joint);
1034
        return mbdJoint;
1035
    }
1036
    else {
1037
        // planar joint for other edges.
1038
        auto mbdJoint = CREATE<ASMTPlanarJoint>::With();
1039
        mbdJoint->offset = getJointDistance(joint);
1040
        return mbdJoint;
1041
    }
1042

1043
    return nullptr;
1044
}
1045

1046

1047
std::vector<std::shared_ptr<MbD::ASMTJoint>>
1048
AssemblyObject::makeMbdJoint(App::DocumentObject* joint)
1049
{
1050
    JointType jointType = getJointType(joint);
1051

1052
    std::shared_ptr<ASMTJoint> mbdJoint = makeMbdJointOfType(joint, jointType);
1053
    if (!mbdJoint) {
1054
        return {};
1055
    }
1056

1057
    std::string fullMarkerName1 = handleOneSideOfJoint(joint, "Object1", "Part1", "Placement1");
1058
    std::string fullMarkerName2 = handleOneSideOfJoint(joint, "Object2", "Part2", "Placement2");
1059

1060
    mbdJoint->setName(joint->getFullName());
1061
    mbdJoint->setMarkerI(fullMarkerName1);
1062
    mbdJoint->setMarkerJ(fullMarkerName2);
1063

1064
    return {mbdJoint};
1065
}
1066

1067
std::string AssemblyObject::handleOneSideOfJoint(App::DocumentObject* joint,
1068
                                                 const char* propObjName,
1069
                                                 const char* propPartName,
1070
                                                 const char* propPlcName)
1071
{
1072
    App::DocumentObject* part = getLinkObjFromProp(joint, propPartName);
1073
    App::DocumentObject* obj = getObjFromNameProp(joint, propObjName, propPartName);
1074

1075
    std::shared_ptr<ASMTPart> mbdPart = getMbDPart(part);
1076
    Base::Placement plc = getPlacementFromProp(joint, propPlcName);
1077
    // Now we have plc which is the JCS placement, but its relative to the Object, not to the
1078
    // containing Part.
1079

1080
    if (obj->getNameInDocument() != part->getNameInDocument()) {
1081
        // Make plc relative to the containing part
1082
        // plc = objPlc * plc; // this would not work for nested parts.
1083

1084
        Base::Placement obj_global_plc = getGlobalPlacement(obj, part);
1085
        plc = obj_global_plc * plc;
1086

1087
        Base::Placement part_global_plc = getGlobalPlacement(part);
1088
        plc = part_global_plc.inverse() * plc;
1089
    }
1090

1091
    std::string markerName = joint->getFullName();
1092
    auto mbdMarker = makeMbdMarker(markerName, plc);
1093
    mbdPart->addMarker(mbdMarker);
1094

1095
    return "/OndselAssembly/" + mbdPart->name + "/" + markerName;
1096
}
1097

1098
std::shared_ptr<ASMTPart> AssemblyObject::getMbDPart(App::DocumentObject* obj)
1099
{
1100
    std::shared_ptr<ASMTPart> mbdPart;
1101

1102
    Base::Placement plc = getPlacementFromProp(obj, "Placement");
1103

1104
    auto it = objectPartMap.find(obj);
1105
    if (it != objectPartMap.end()) {
1106
        // obj has been associated with an ASMTPart before
1107
        mbdPart = it->second;
1108
    }
1109
    else {
1110
        // obj has not been associated with an ASMTPart before
1111
        std::string str = obj->getFullName();
1112
        mbdPart = makeMbdPart(str, plc);
1113
        mbdAssembly->addPart(mbdPart);
1114
        objectPartMap[obj] = mbdPart;  // Store the association
1115
    }
1116

1117
    return mbdPart;
1118
}
1119

1120
std::shared_ptr<ASMTPart>
1121
AssemblyObject::makeMbdPart(std::string& name, Base::Placement plc, double mass)
1122
{
1123
    auto mbdPart = CREATE<ASMTPart>::With();
1124
    mbdPart->setName(name);
1125

1126
    auto massMarker = CREATE<ASMTPrincipalMassMarker>::With();
1127
    massMarker->setMass(mass);
1128
    massMarker->setDensity(1.0);
1129
    massMarker->setMomentOfInertias(1.0, 1.0, 1.0);
1130
    mbdPart->setPrincipalMassMarker(massMarker);
1131

1132
    Base::Vector3d pos = plc.getPosition();
1133
    mbdPart->setPosition3D(pos.x, pos.y, pos.z);
1134
    // Base::Console().Warning("MbD Part placement : (%f, %f, %f)\n", pos.x, pos.y, pos.z);
1135

1136
    // TODO : replace with quaternion to simplify
1137
    Base::Rotation rot = plc.getRotation();
1138
    Base::Matrix4D mat;
1139
    rot.getValue(mat);
1140
    Base::Vector3d r0 = mat.getRow(0);
1141
    Base::Vector3d r1 = mat.getRow(1);
1142
    Base::Vector3d r2 = mat.getRow(2);
1143
    mbdPart->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z);
1144
    /*double q0, q1, q2, q3;
1145
    rot.getValue(q0, q1, q2, q3);
1146
    mbdPart->setQuarternions(q0, q1, q2, q3);*/
1147

1148
    return mbdPart;
1149
}
1150

1151
std::shared_ptr<ASMTMarker> AssemblyObject::makeMbdMarker(std::string& name, Base::Placement& plc)
1152
{
1153
    auto mbdMarker = CREATE<ASMTMarker>::With();
1154
    mbdMarker->setName(name);
1155

1156
    Base::Vector3d pos = plc.getPosition();
1157
    mbdMarker->setPosition3D(pos.x, pos.y, pos.z);
1158

1159
    // TODO : replace with quaternion to simplify
1160
    Base::Rotation rot = plc.getRotation();
1161
    Base::Matrix4D mat;
1162
    rot.getValue(mat);
1163
    Base::Vector3d r0 = mat.getRow(0);
1164
    Base::Vector3d r1 = mat.getRow(1);
1165
    Base::Vector3d r2 = mat.getRow(2);
1166
    mbdMarker->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z);
1167
    /*double q0, q1, q2, q3;
1168
    rot.getValue(q0, q1, q2, q3);
1169
    mbdMarker->setQuarternions(q0, q1, q2, q3);*/
1170
    return mbdMarker;
1171
}
1172

1173
std::vector<App::DocumentObject*> AssemblyObject::getDownstreamParts(App::DocumentObject* part,
1174
                                                                     App::DocumentObject* joint)
1175
{
1176
    // First we deactivate the joint
1177
    bool state = getJointActivated(joint);
1178
    setJointActivated(joint, false);
1179

1180
    std::vector<App::DocumentObject*> joints = getJoints(false);
1181

1182
    std::set<App::DocumentObject*> connectedParts = {part};
1183
    traverseAndMarkConnectedParts(part, connectedParts, joints);
1184

1185
    std::vector<App::DocumentObject*> downstreamParts;
1186
    for (auto parti : connectedParts) {
1187
        if (!isPartConnected(parti) && (parti != part)) {
1188
            downstreamParts.push_back(parti);
1189
        }
1190
    }
1191

1192
    AssemblyObject::setJointActivated(joint, state);
1193
    /*if (limit > 1000) {  // Infinite loop protection
1194
        return {};
1195
    }
1196
    limit++;
1197
    Base::Console().Warning("limit %d\n", limit);
1198

1199
    std::vector<App::DocumentObject*> downstreamParts = {part};
1200
    std::string name;
1201
    App::DocumentObject* connectingJoint =
1202
        getJointOfPartConnectingToGround(part,
1203
                                         name);  // ?????????????????????????????? if we remove
1204
                                                 // connection to ground then it can't work for tom
1205
    std::vector<App::DocumentObject*> jointsOfPart = getJointsOfPart(part);
1206

1207
    // remove connectingJoint from jointsOfPart
1208
    auto it = std::remove(jointsOfPart.begin(), jointsOfPart.end(), connectingJoint);
1209
    jointsOfPart.erase(it, jointsOfPart.end());
1210
    for (auto joint : jointsOfPart) {
1211
        App::DocumentObject* part1 = getLinkObjFromProp(joint, "Part1");
1212
        App::DocumentObject* part2 = getLinkObjFromProp(joint, "Part2");
1213
        bool firstIsDown = part->getFullName() == part2->getFullName();
1214
        App::DocumentObject* downstreamPart = firstIsDown ? part1 : part2;
1215

1216
        Base::Console().Warning("looping\n");
1217
        // it is possible that the part is connected to ground by this joint.
1218
        // In which case we should not select those parts. To test we disconnect :
1219
        auto* propObj = dynamic_cast<App::PropertyLink*>(joint->getPropertyByName("Part1"));
1220
        if (!propObj) {
1221
            continue;
1222
        }
1223
        propObj->setValue(nullptr);
1224
        bool isConnected = isPartConnected(downstreamPart);
1225
        propObj->setValue(part1);
1226
        if (isConnected) {
1227
            Base::Console().Warning("continue\n");
1228
            continue;
1229
        }
1230

1231
        std::vector<App::DocumentObject*> subDownstreamParts =
1232
            getDownstreamParts(downstreamPart, limit);
1233
        for (auto downPart : subDownstreamParts) {
1234
            if (std::find(downstreamParts.begin(), downstreamParts.end(), downPart)
1235
                == downstreamParts.end()) {
1236
                downstreamParts.push_back(downPart);
1237
            }
1238
        }
1239
    }*/
1240
    return downstreamParts;
1241
}
1242

1243
std::vector<App::DocumentObject*> AssemblyObject::getUpstreamParts(App::DocumentObject* part,
1244
                                                                   int limit)
1245
{
1246
    if (limit > 1000) {  // Infinite loop protection
1247
        return {};
1248
    }
1249
    limit++;
1250

1251
    if (isPartGrounded(part)) {
1252
        return {part};
1253
    }
1254

1255
    std::string name;
1256
    App::DocumentObject* connectingJoint = getJointOfPartConnectingToGround(part, name);
1257
    App::DocumentObject* upPart =
1258
        getLinkObjFromProp(connectingJoint, name == "Part1" ? "Part2" : "Part1");
1259

1260
    std::vector<App::DocumentObject*> upstreamParts = getUpstreamParts(upPart, limit);
1261
    upstreamParts.push_back(part);
1262
    return upstreamParts;
1263
}
1264

1265
App::DocumentObject* AssemblyObject::getUpstreamMovingPart(App::DocumentObject* part)
1266
{
1267
    if (isPartGrounded(part)) {
1268
        return nullptr;
1269
    }
1270

1271
    std::string name;
1272
    App::DocumentObject* connectingJoint = getJointOfPartConnectingToGround(part, name);
1273
    JointType jointType = getJointType(connectingJoint);
1274
    if (jointType != JointType::Fixed) {
1275
        return part;
1276
    }
1277

1278
    App::DocumentObject* upPart =
1279
        getLinkObjFromProp(connectingJoint, name == "Part1" ? "Part2" : "Part1");
1280

1281
    return getUpstreamMovingPart(upPart);
1282
}
1283

1284
double AssemblyObject::getObjMass(App::DocumentObject* obj)
1285
{
1286
    for (auto& pair : objMasses) {
1287
        if (pair.first == obj) {
1288
            return pair.second;
1289
        }
1290
    }
1291
    return 1.0;
1292
}
1293

1294
void AssemblyObject::setObjMasses(std::vector<std::pair<App::DocumentObject*, double>> objectMasses)
1295
{
1296
    objMasses = objectMasses;
1297
}
1298

1299
// ======================================= Utils ======================================
1300

1301
void AssemblyObject::swapJCS(App::DocumentObject* joint)
1302
{
1303
    auto propElement1 = dynamic_cast<App::PropertyString*>(joint->getPropertyByName("Element1"));
1304
    auto propElement2 = dynamic_cast<App::PropertyString*>(joint->getPropertyByName("Element2"));
1305
    if (propElement1 && propElement2) {
1306
        auto temp = std::string(propElement1->getValue());
1307
        propElement1->setValue(propElement2->getValue());
1308
        propElement2->setValue(temp);
1309
    }
1310
    auto propVertex1 = dynamic_cast<App::PropertyString*>(joint->getPropertyByName("Vertex1"));
1311
    auto propVertex2 = dynamic_cast<App::PropertyString*>(joint->getPropertyByName("Vertex2"));
1312
    if (propVertex1 && propVertex2) {
1313
        auto temp = std::string(propVertex1->getValue());
1314
        propVertex1->setValue(propVertex2->getValue());
1315
        propVertex2->setValue(temp);
1316
    }
1317
    auto propPlacement1 =
1318
        dynamic_cast<App::PropertyPlacement*>(joint->getPropertyByName("Placement1"));
1319
    auto propPlacement2 =
1320
        dynamic_cast<App::PropertyPlacement*>(joint->getPropertyByName("Placement2"));
1321
    if (propPlacement1 && propPlacement2) {
1322
        auto temp = propPlacement1->getValue();
1323
        propPlacement1->setValue(propPlacement2->getValue());
1324
        propPlacement2->setValue(temp);
1325
    }
1326
    auto propObject1 = dynamic_cast<App::PropertyString*>(joint->getPropertyByName("Object1"));
1327
    auto propObject2 = dynamic_cast<App::PropertyString*>(joint->getPropertyByName("Object2"));
1328
    if (propObject1 && propObject2) {
1329
        auto temp = std::string(propObject1->getValue());
1330
        propObject1->setValue(propObject2->getValue());
1331
        propObject2->setValue(temp);
1332
    }
1333
    auto propPart1 = dynamic_cast<App::PropertyLink*>(joint->getPropertyByName("Part1"));
1334
    auto propPart2 = dynamic_cast<App::PropertyLink*>(joint->getPropertyByName("Part2"));
1335
    if (propPart1 && propPart2) {
1336
        auto temp = propPart1->getValue();
1337
        propPart1->setValue(propPart2->getValue());
1338
        propPart2->setValue(temp);
1339
    }
1340
}
1341

1342
bool AssemblyObject::isEdgeType(App::DocumentObject* obj,
1343
                                const char* elName,
1344
                                GeomAbs_CurveType type)
1345
{
1346
    PartApp::Feature* base = static_cast<PartApp::Feature*>(obj);
1347
    const PartApp::TopoShape& TopShape = base->Shape.getShape();
1348

1349
    // Check for valid face types
1350
    TopoDS_Edge edge = TopoDS::Edge(TopShape.getSubShape(elName));
1351
    BRepAdaptor_Curve sf(edge);
1352

1353
    if (sf.GetType() == type) {
1354
        return true;
1355
    }
1356

1357
    return false;
1358
}
1359

1360
bool AssemblyObject::isFaceType(App::DocumentObject* obj,
1361
                                const char* elName,
1362
                                GeomAbs_SurfaceType type)
1363
{
1364
    auto base = static_cast<PartApp::Feature*>(obj);
1365
    PartApp::TopoShape TopShape = base->Shape.getShape();
1366

1367
    // Check for valid face types
1368
    TopoDS_Face face = TopoDS::Face(TopShape.getSubShape(elName));
1369
    BRepAdaptor_Surface sf(face);
1370
    // GeomAbs_Plane GeomAbs_Cylinder GeomAbs_Cone GeomAbs_Sphere GeomAbs_Thorus
1371
    if (sf.GetType() == type) {
1372
        return true;
1373
    }
1374

1375
    return false;
1376
}
1377

1378
double AssemblyObject::getFaceRadius(App::DocumentObject* obj, const char* elt)
1379
{
1380
    auto base = static_cast<PartApp::Feature*>(obj);
1381
    const PartApp::TopoShape& TopShape = base->Shape.getShape();
1382

1383
    // Check for valid face types
1384
    TopoDS_Face face = TopoDS::Face(TopShape.getSubShape(elt));
1385
    BRepAdaptor_Surface sf(face);
1386

1387
    if (sf.GetType() == GeomAbs_Cylinder) {
1388
        return sf.Cylinder().Radius();
1389
    }
1390
    else if (sf.GetType() == GeomAbs_Sphere) {
1391
        return sf.Sphere().Radius();
1392
    }
1393

1394
    return 0.0;
1395
}
1396

1397
double AssemblyObject::getEdgeRadius(App::DocumentObject* obj, const char* elt)
1398
{
1399
    auto base = static_cast<PartApp::Feature*>(obj);
1400
    const PartApp::TopoShape& TopShape = base->Shape.getShape();
1401

1402
    // Check for valid face types
1403
    TopoDS_Edge edge = TopoDS::Edge(TopShape.getSubShape(elt));
1404
    BRepAdaptor_Curve sf(edge);
1405

1406
    if (sf.GetType() == GeomAbs_Circle) {
1407
        return sf.Circle().Radius();
1408
    }
1409

1410
    return 0.0;
1411
}
1412

1413
void printPlacement(Base::Placement plc, const char* name)
1414
{
1415
    Base::Vector3d pos = plc.getPosition();
1416
    Base::Vector3d axis;
1417
    double angle;
1418
    Base::Rotation rot = plc.getRotation();
1419
    rot.getRawValue(axis, angle);
1420
    Base::Console().Warning(
1421
        "placement %s : position (%.1f, %.1f, %.1f) - axis (%.1f, %.1f, %.1f) angle %.1f\n",
1422
        name,
1423
        pos.x,
1424
        pos.y,
1425
        pos.z,
1426
        axis.x,
1427
        axis.y,
1428
        axis.z,
1429
        angle);
1430
}
1431

1432
void AssemblyObject::setJointActivated(App::DocumentObject* joint, bool val)
1433
{
1434
    auto* propActivated = dynamic_cast<App::PropertyBool*>(joint->getPropertyByName("Activated"));
1435
    if (propActivated) {
1436
        propActivated->setValue(val);
1437
    }
1438
}
1439
bool AssemblyObject::getJointActivated(App::DocumentObject* joint)
1440
{
1441
    auto* propActivated = dynamic_cast<App::PropertyBool*>(joint->getPropertyByName("Activated"));
1442
    if (propActivated) {
1443
        return propActivated->getValue();
1444
    }
1445
    return false;
1446
}
1447

1448
Base::Placement AssemblyObject::getPlacementFromProp(App::DocumentObject* obj, const char* propName)
1449
{
1450
    Base::Placement plc = Base::Placement();
1451
    auto* propPlacement = dynamic_cast<App::PropertyPlacement*>(obj->getPropertyByName(propName));
1452
    if (propPlacement) {
1453
        plc = propPlacement->getValue();
1454
    }
1455
    return plc;
1456
}
1457

1458
bool AssemblyObject::getTargetPlacementRelativeTo(Base::Placement& foundPlc,
1459
                                                  App::DocumentObject* targetObj,
1460
                                                  App::DocumentObject* part,
1461
                                                  App::DocumentObject* container,
1462
                                                  bool inContainerBranch,
1463
                                                  bool ignorePlacement)
1464
{
1465
    inContainerBranch = inContainerBranch || (!ignorePlacement && part == container);
1466

1467
    if (targetObj == part && inContainerBranch && !ignorePlacement) {
1468
        foundPlc = getPlacementFromProp(targetObj, "Placement");
1469
        return true;
1470
    }
1471

1472
    if (part->isDerivedFrom(App::DocumentObjectGroup::getClassTypeId())) {
1473
        for (auto& obj : part->getOutList()) {
1474
            bool found = getTargetPlacementRelativeTo(foundPlc,
1475
                                                      targetObj,
1476
                                                      obj,
1477
                                                      container,
1478
                                                      inContainerBranch,
1479
                                                      ignorePlacement);
1480
            if (found) {
1481
                return true;
1482
            }
1483
        }
1484
    }
1485
    else if (part->isDerivedFrom(Assembly::AssemblyObject::getClassTypeId())
1486
             || part->isDerivedFrom(App::Part::getClassTypeId())
1487
             || part->isDerivedFrom(PartDesign::Body::getClassTypeId())) {
1488
        for (auto& obj : part->getOutList()) {
1489
            bool found = getTargetPlacementRelativeTo(foundPlc,
1490
                                                      targetObj,
1491
                                                      obj,
1492
                                                      container,
1493
                                                      inContainerBranch);
1494
            if (!found) {
1495
                continue;
1496
            }
1497

1498
            if (!ignorePlacement) {
1499
                foundPlc = getPlacementFromProp(part, "Placement") * foundPlc;
1500
            }
1501

1502
            return true;
1503
        }
1504
    }
1505
    else if (auto link = dynamic_cast<App::Link*>(part)) {
1506
        auto linked_obj = link->getLinkedObject();
1507

1508
        if (dynamic_cast<App::Part*>(linked_obj) || dynamic_cast<AssemblyObject*>(linked_obj)) {
1509
            for (auto& obj : linked_obj->getOutList()) {
1510
                bool found = getTargetPlacementRelativeTo(foundPlc,
1511
                                                          targetObj,
1512
                                                          obj,
1513
                                                          container,
1514
                                                          inContainerBranch);
1515
                if (!found) {
1516
                    continue;
1517
                }
1518

1519
                foundPlc = getPlacementFromProp(link, "Placement") * foundPlc;
1520
                return true;
1521
            }
1522
        }
1523

1524
        bool found = getTargetPlacementRelativeTo(foundPlc,
1525
                                                  targetObj,
1526
                                                  linked_obj,
1527
                                                  container,
1528
                                                  inContainerBranch,
1529
                                                  true);
1530

1531
        if (found) {
1532
            if (!ignorePlacement) {
1533
                foundPlc = getPlacementFromProp(link, "Placement") * foundPlc;
1534
            }
1535

1536
            return true;
1537
        }
1538
    }
1539

1540
    return false;
1541
}
1542

1543
Base::Placement AssemblyObject::getGlobalPlacement(App::DocumentObject* targetObj,
1544
                                                   App::DocumentObject* container)
1545
{
1546
    bool inContainerBranch = (container == nullptr);
1547
    auto rootObjects = App::GetApplication().getActiveDocument()->getRootObjects();
1548
    for (auto& part : rootObjects) {
1549
        Base::Placement foundPlc;
1550
        bool found =
1551
            getTargetPlacementRelativeTo(foundPlc, targetObj, part, container, inContainerBranch);
1552
        if (found) {
1553
            return foundPlc;
1554
        }
1555
    }
1556

1557
    return Base::Placement();
1558
}
1559

1560
Base::Placement AssemblyObject::getGlobalPlacement(App::DocumentObject* joint,
1561
                                                   const char* targetObj,
1562
                                                   const char* container)
1563
{
1564
    App::DocumentObject* obj = getObjFromNameProp(joint, targetObj, container);
1565
    App::DocumentObject* part = getLinkObjFromProp(joint, container);
1566
    return getGlobalPlacement(obj, part);
1567
}
1568

1569
double AssemblyObject::getJointDistance(App::DocumentObject* joint)
1570
{
1571
    double distance = 0.0;
1572

1573
    auto* prop = dynamic_cast<App::PropertyFloat*>(joint->getPropertyByName("Distance"));
1574
    if (prop) {
1575
        distance = prop->getValue();
1576
    }
1577

1578
    return distance;
1579
}
1580

1581
JointType AssemblyObject::getJointType(App::DocumentObject* joint)
1582
{
1583
    JointType jointType = JointType::Fixed;
1584

1585
    auto* prop = dynamic_cast<App::PropertyEnumeration*>(joint->getPropertyByName("JointType"));
1586
    if (prop) {
1587
        jointType = static_cast<JointType>(prop->getValue());
1588
    }
1589

1590
    return jointType;
1591
}
1592

1593
const char* AssemblyObject::getElementFromProp(App::DocumentObject* obj, const char* propName)
1594
{
1595
    auto* prop = dynamic_cast<App::PropertyString*>(obj->getPropertyByName(propName));
1596
    if (!prop) {
1597
        return "";
1598
    }
1599

1600
    return prop->getValue();
1601
}
1602

1603
std::string AssemblyObject::getElementTypeFromProp(App::DocumentObject* obj, const char* propName)
1604
{
1605
    // The prop is going to be something like 'Edge14' or 'Face7'. We need 'Edge' or 'Face'
1606
    std::string elementType;
1607
    for (char ch : std::string(getElementFromProp(obj, propName))) {
1608
        if (std::isalpha(ch)) {
1609
            elementType += ch;
1610
        }
1611
    }
1612
    return elementType;
1613
}
1614

1615
App::DocumentObject* AssemblyObject::getLinkObjFromProp(App::DocumentObject* joint,
1616
                                                        const char* propLinkName)
1617
{
1618
    auto* propObj = dynamic_cast<App::PropertyLink*>(joint->getPropertyByName(propLinkName));
1619
    if (!propObj) {
1620
        return nullptr;
1621
    }
1622
    return propObj->getValue();
1623
}
1624

1625
App::DocumentObject* AssemblyObject::getObjFromNameProp(App::DocumentObject* joint,
1626
                                                        const char* pObjName,
1627
                                                        const char* pPart)
1628
{
1629
    auto* propObjName = dynamic_cast<App::PropertyString*>(joint->getPropertyByName(pObjName));
1630
    if (!propObjName) {
1631
        return nullptr;
1632
    }
1633
    std::string objName = std::string(propObjName->getValue());
1634

1635
    App::DocumentObject* containingPart = getLinkObjFromProp(joint, pPart);
1636
    if (!containingPart) {
1637
        return nullptr;
1638
    }
1639

1640
    if (objName == containingPart->getNameInDocument()) {
1641
        return containingPart;
1642
    }
1643

1644
    /*if (containingPart->getTypeId().isDerivedFrom(App::Link::getClassTypeId())) {
1645
        App::Link* link = dynamic_cast<App::Link*>(containingPart);
1646

1647
        containingPart = link->getLinkedObject();
1648
        if (!containingPart) {
1649
            return nullptr;
1650
        }
1651
    }*/
1652

1653
    for (auto obj : containingPart->getOutListRecursive()) {
1654
        if (objName == obj->getNameInDocument()) {
1655
            return obj;
1656
        }
1657
    }
1658

1659
    return nullptr;
1660
}
1661

1662
App::DocumentObject* AssemblyObject::getLinkedObjFromNameProp(App::DocumentObject* joint,
1663
                                                              const char* pObjName,
1664
                                                              const char* pPart)
1665
{
1666
    auto* obj = getObjFromNameProp(joint, pObjName, pPart);
1667
    if (obj) {
1668
        return obj->getLinkedObject(true);
1669
    }
1670
    return nullptr;
1671
}
1672

1673

1674
/*void Part::handleChangedPropertyType(Base::XMLReader& reader, const char* TypeName, App::Property*
1675
prop)
1676
{
1677
    App::Part::handleChangedPropertyType(reader, TypeName, prop);
1678
}*/
1679

1680
/* Apparently not necessary as App::Part doesn't have this.
1681
// Python Assembly feature ---------------------------------------------------------
1682

1683
namespace App
1684
{
1685
    /// @cond DOXERR
1686
    PROPERTY_SOURCE_TEMPLATE(Assembly::AssemblyObjectPython, Assembly::AssemblyObject)
1687
        template<>
1688
    const char* Assembly::AssemblyObjectPython::getViewProviderName() const
1689
    {
1690
        return "AssemblyGui::ViewProviderAssembly";
1691
    }
1692
    template<>
1693
    PyObject* Assembly::AssemblyObjectPython::getPyObject()
1694
    {
1695
        if (PythonObject.is(Py::_None())) {
1696
            // ref counter is set to 1
1697
            PythonObject = Py::Object(new FeaturePythonPyT<AssemblyObjectPy>(this), true);
1698
        }
1699
        return Py::new_reference_to(PythonObject);
1700
    }
1701
    /// @endcond
1702

1703
    // explicit template instantiation
1704
    template class AssemblyExport FeaturePythonT<Assembly::AssemblyObject>;
1705
}// namespace App*/
1706