import Sofa
from Compliant import StructuralAPI, Tools
import SofaPython.Quaternion as quat

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

    rootNode.dt=0.1
    rootNode.gravity=[0,0,0]
    
    rootNode.createObject('CompliantAttachButtonSetting', compliance=1E-10)
    
    rootNode.createObject('CompliantImplicitSolver', name="odesolver", stabilization=True)
    rootNode.createObject('LDLTSolver', name="numsolver")
    rootNode.createObject('LDLTResponse', name="numresponse")



    # target is a fixed topology
    targetNode = rootNode.createChild( "target")
    targetNode.createObject("MeshObjLoader",filename='mesh/raptor_35kp.obj',name="loader", scale3d=".1 .1 .1" )
    targetTopology = targetNode.createObject("MeshTopology",src="@loader",name="topology")
    vm = targetNode.createObject("OglModel",fileMesh='mesh/raptor_35kp.obj',name="vm", scale3d=".1 .1 .1", color='1 0 0 1' )



    # source is a rigid body
    s = StructuralAPI.RigidBody( rootNode, "source" )
    s.setManually(offset = [1,0,0]+quat.from_euler([1,1,1]).tolist() )
    s.addCollisionMesh( 'mesh/raptor_8kp.obj', scale3d=[.1,.1,.1] )
    s.collision.addNormals()
    vs=s.collision.addVisualModel()
    vs.model.setColor(0,1,0,1)




    # registration using improved ICP
    distance = s.collision.node.createChild("distance")
    distance.createObject('MechanicalObject', template="Vec1d", name="dof")

    distance.createObject('ClosestPointEngine', name="cp", parallel=True,
                          sourcePosition=s.collision.node.getLinkPath()+"/topology.position",
                          sourceRigidPosition=s.node.getLinkPath()+"/dofs.position",
                          targetPosition=targetTopology.getLinkPath()+".position",
                          printLog=True,drawMode="0",method="1",
                          useBV=False, cacheSize=5, targetROISize=5000, sourceROISize=5000)

    distance.createObject('ProjectionToMeshEngine', name="pm", printLog=True, template="Vec3d",
                    sourcePosition=s.collision.dofs.getLinkPath()+".position",
                    targetPosition=targetTopology.getLinkPath()+".position",
                    indexPairs="@cp.indexPairs" ,
                    triangles2=targetTopology.getLinkPath()+".triangles",
                    quads2=targetTopology.getLinkPath()+".quads",
                    useRestLength=True, restLength='0' )


    distance.createObject('DistanceToMeshMapping', template="Vec3d,Vec1d", name="mapping",
                          input=s.collision.node.getLinkPath(),
                          output="@.",
                          indexPairs="@cp.indexPairs",
                          closests="@pm.output",
                          printLog=True)

    distance.createObject('UniformCompliance', compliance=1E-2,rayleighStiffness=0.1,isCompliance='0',resizable=True)
