import Sofa

def createScene(rootNode):
    rootNode.gravity=[0,0,0]
    rootNode.createObject('RequiredPlugin', pluginName="ContactMapping")
    
    rootNode.createObject('VisualStyle', displayFlags='showMechanicalMappings')


    cube = rootNode.createChild("cube")
        
    cube.createObject('MeshObjLoader', name="loader", filename="mesh/cube.obj" )
    cube.createObject('MeshTopology', name="topology", src="@loader", drawEdges=True )

    snake = rootNode.createChild("snake")

    snake.createObject('MeshObjLoader', name="loader", filename="mesh/snake_body.obj", rotation="90 0 0", scale3d=".1 .1 .1", translation="0 0 -1" )
    snake.createObject('MeshTopology', name="topology", src="@loader" )
    snake.createObject('OglModel', color="white", primitiveType="POINTS")

    snake.createObject('MechanicalObject', template="Vec3d", name="dof", position="2 2 2", velocity="0 0 0", showObject=False, showObjectScale=0.01, drawMode=1)


    snake.createObject('ClosestPointEngine', name="cp",
                       sourcePosition="@snake/dof.position", targetPosition="@cube/topology.position")

    snake.createObject('ProjectionToMeshEngine', name="pm",
                        sourcePosition="@cp.sourcePosition", targetPosition="@cp.targetPosition",
                        indexPairs="@cp.indexPairs" ,
                        triangles="@cube/topology.triangles",
                        useRestLength=False )






    positivedistance = snake.createChild("positive distance (red)")
    positivedistance.createObject('ClosestKeepPositiveEngine', name="filter",
                    closests="@../pm.output",
                    indexPairs="@../cp.indexPairs" )
    positivedistance.createObject('MechanicalObject', template="Vec1d", name="dof")
    positivedistance.createObject('DistanceToMeshMapping', template="Vec3d,Vec1d", name="mapping",
                          indexPairs="@../cp.indexPairs",
                          validPairs="@filter.output",
                          closests="@../pm.output",
                          showObjectScale=0, showColor="1 0 0 1")








    negativedistance = snake.createChild("negative distance (blue)")
    negativedistance.createObject('ClosestKeepNegativeEngine', name="filter",
                    closests="@../pm.output",
                    indexPairs="@../cp.indexPairs" )
    negativedistance.createObject('MechanicalObject', template="Vec1d", name="dof")
    negativedistance.createObject('DistanceToMeshMapping', template="Vec3d,Vec1d", name="mapping",
                          indexPairs="@../cp.indexPairs",
                          validPairs="@filter.output",
                          closests="@../pm.output",
                          showObjectScale=0, showColor="0 0 1 1")








    
    return rootNode
