<!-- 
Step 3:
Instrument Control, and Python script
-->
<Node name="root" dt="0.05">

    <RequiredPlugin pluginName="SofaPython"/>

    <BackgroundSetting color="0.5 0.5 0.5" />

    <FreeMotionAnimationLoop />
    <LCPConstraintSolver tolerance="0.001" maxIt="1000"/>

    <Node name="InstrControl">
        <MechanicalObject template="Rigid" name="DOFs" position="0.05 0.5 -7.65  0 0 0 1" />
        <PythonScriptController filename="keyboardControl.py" classname="KeyboardControl"/>

        <Node name="RefModel">
            <MeshObjLoader filename="data/mesh/dental_instrument_centerline.obj" name="loader" />
            <Mesh src="@loader" />
            <MechanicalObject src="@loader" name="instrumentCollisionState" rz="90" dx="2"/>
            <RigidMapping input="@../DOFs" output="@instrumentCollisionState"/>
        </Node> 
    </Node>
    <Node name="Instrument">
        <EulerImplicitSolver name="ODE solver" rayleighStiffness="0.01" rayleighMass="1.0" />
        <CGLinearSolver name="linear solver" iterations="25" tolerance="1e-10" threshold="10e-10" />
        <MechanicalObject name="instrumentState" template="Rigid" position="0.05 0.5 -7.65  0 0 0 1" />
        <UniformMass name="mass" totalmass="0.05" filename="BehaviorModels/dental_instrument.rigid" />
        <UncoupledConstraintCorrection />
        <Node name="VisualModel">
            <OglModel name="InstrumentVisualModel" fileMesh="data/mesh/dental_instrument.obj" color="0.8 0.8 0.8 1.0" />
            <RigidMapping name="MM-VM mapping" input="@../instrumentState" output="@InstrumentVisualModel" />
        </Node>
        <Node name="CollisionModel">
            <MeshObjLoader filename="data/mesh/dental_instrument_centerline.obj" name="loader" />
            <Mesh src="@loader" name="InstrumentCollisionModel" />
            <MechanicalObject src="@loader" name="instrumentCollisionState" translation="0 -2 0" />
            <Line name="instrument" contactStiffness="10" />
            <Point name="instrument" contactStiffness="10" />
            <RigidMapping name="MM-CM mapping" input="@../instrumentState" output="@instrumentCollisionState" />
        </Node>

        <VectorSpringForceField object1="@InstrControl/RefModel/instrumentCollisionState" object2="@Instrument/CollisionModel/instrumentCollisionState" stiffness="1000" viscosity="0" />

    </Node>

</Node>
