import math

import Sofa

from SofaPython.Tools import listToStr as concat
import Compliant.StructuralAPI
import Anatomy.Constraint


def createScene(rootNode):
    
    Compliant.StructuralAPI.GenericRigidJoint.PositionController = Anatomy.Constraint.ProgressivePositionController
    
    rootNode.createObject('RequiredPlugin', pluginName="Compliant")
    rootNode.createObject('RequiredPlugin', pluginName="Anatomy")
    
    rootNode.gravity='0 0 0'
    
    rootNode.createObject('CompliantAttachButtonSetting' )
    rootNode.createObject('CompliantImplicitSolver', name='odesolver', stabilization="true")
    rootNode.createObject('LDLTSolver')
    
    rigidA = Compliant.StructuralAPI.RigidBody(rootNode, "A")
    rigidA.setManually(offset = [0,0,0,0,0,0,1])
    rigidA.dofs.showObject=True
    
    rigidB = Compliant.StructuralAPI.RigidBody(rootNode, "B")
    rigidB.setManually(offset = [1,0,0,0,0,0,1])
    rigidB.dofs.showObject=True
    
    sliderAB = Compliant.StructuralAPI.SliderRigidJoint(0,"sliderAB",rigidA.node, rigidB.node)
    
    rigidA.node.createObject('FixedConstraint')
    
    sliderABController = sliderAB.addPositionController(0)
    sliderABController.progressiveTarget.step=1e-2
    sliderABController.progressiveTarget.tolerance=1e-6
    
    return rootNode
    