-
Notifications
You must be signed in to change notification settings - Fork 298
/
SpringInteractor.cpp
68 lines (55 loc) · 2.24 KB
/
SpringInteractor.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#include <iostream>
#include "SpringInteractor.h"
#include "PickedPoint.h"
#include <sofa/core/SofaLibrary.h>
#include <sofa/simulation/Simulation.h>
#include <sofa/component/constraint/projective/FixedConstraint.h>
namespace sofa::simplegui
{
typedef sofa::component::constraint::projective::FixedConstraint<sofa::defaulttype::Vec3Types> FixedConstraint3;
SpringInteractor::SpringInteractor(const PickedPoint &picked, SReal stiffness)
: Interactor(picked)
{
// get the DOF of the picked object
MechanicalObject3* pickedDof=dynamic_cast<MechanicalObject3*>(picked.state.get()); assert(pickedDof);
// create DOF to represent the actuator
_interactorDof = sofa::core::objectmodel::New<MechanicalObject3>();
_interactionNode->addObject(_interactorDof);
_interactorDof->setName("interactorDOF");
_interactorDof->addTag(std::string("Interactor"));
MechanicalObject3::WriteVecCoord xanchor = _interactorDof->writePositions();
xanchor[0] = picked.point;
FixedConstraint3::SPtr fixed= sofa::core::objectmodel::New<FixedConstraint3>(); // Makes it unmovable through forces
_interactionNode->addObject(fixed);
fixed->init();
// create spring to drag the picked object
_spring = sofa::core::objectmodel::New<StiffSpringForceField3>(_interactorDof.get(),pickedDof);
_interactionNode->addObject(_spring);
_spring->addSpring(0,picked.index,stiffness,0.1,0.);
// cout << "SpringInteractor set spring to " << pickedDof->getName() << ", " << picked.index << endl;
}
Vec3 SpringInteractor::getPoint()
{
MechanicalObject3::ReadVecCoord xanchor = _interactorDof->readPositions();
return xanchor[0];
}
void SpringInteractor::setPoint( const Vec3& p )
{
MechanicalObject3::WriteVecCoord xanchor = _interactorDof->writePositions();
xanchor[0] = p;
}
void SpringInteractor::attach(SofaScene *scene)
{
Inherited::attach(scene);
_interactionNode->removeObject(_spring);
Node* targetParent = dynamic_cast<Node*>(_spring->getMState2()->getContext());
targetParent->addObject(_spring);
}
void SpringInteractor::detach()
{
Inherited::detach();
Node* parent = dynamic_cast<Node*>(_spring->getMState2()->getContext());
parent->removeObject(_spring);
_interactionNode->addObject(_spring);
}
}