drecon-unity/Assets/2_MarathonController/Scripts/ROMparserSwingTwist.cs

using System.Collections;
using System.Collections.Generic;
using System;

using UnityEngine;
using System.Linq;


public class ROMparserSwingTwist : MonoBehaviour
{

    //we assume a decomposition where the twist is in the X axis.
    //This seems consistent with how ArticulationBody works (see constraints in the inspector and definition of ReducedCoordinates)

    public
    Animator theAnimator;

    public
    Transform skeletonRoot;

    Transform[] joints;


    float duration;


    [SerializeField]
    //public ROMinfoCollector info2store;
    public RangeOfMotionValues info2store;


    //those are to generate a prefab from a bunch of articulated bodies and the constraints parsed

    //[SerializeField]
    public ArticulationBody targetRagdollRoot;


    [Tooltip("Learning Environment where to integrate the constrained ragdoll. Leave blanc if you do not want to generate any training environment")]
    public
    ManyWorlds.SpawnableEnv trainingEnv;



    [Tooltip("Leave blanc if you want to apply on all the children of targetRoot")]
    [SerializeField]
    ArticulationBody[] targetJoints;

    string animationEndMessage = "First animation played.If there are no more animations, the constraints have been stored.If there are, wait until the ROM info collector file does not update anymore";

  

//    [SerializeField]

 //   public RangeOfMotionValue[] RangeOfMotionPreview;

    MapAnim2Ragdoll _mocapControllerArtanim;
    Vector3 _rootStartPosition;
    Quaternion _rootStartRotation;

    public bool MimicMocap;
    [Range(0, 359)]
    public int MaxROM = 180;

    [Range(0, 500)]
    public int MimicSkipPhysicsSteps = 50;
    int _physicsStepsToNextMimic = 0;
    public float stiffness = 40000f;
    public float damping = 0f;
    public float forceLimit = float.MaxValue;



    // Start is called before the first frame update
    void Start()
    {

     

        joints = skeletonRoot.GetComponentsInChildren<Transform>();


        foreach (Transform j in joints) {
            info2store.addJoint(j);

        }
   

        try
        {

            AnimatorClipInfo[] info = theAnimator.GetCurrentAnimatorClipInfo(0);
            AnimationClip theClip = info[0].clip;
            duration = theClip.length;
            Debug.Log("The animation " + theClip.name + " has a duration of: " + duration);

        }
        catch {
            Debug.Log("the character does not seem to have an animator. Make sure it is moving in some way to extract the Range of Motion");

        }


        _mocapControllerArtanim = theAnimator.GetComponent<MapAnim2Ragdoll>();

        // get root start position and rotation
        var articulationBodies = targetRagdollRoot.GetComponentsInChildren<ArticulationBody>(true);
        if (articulationBodies.Length == 0)
            return;
        var root = articulationBodies.First(x => x.isRoot);
        _rootStartPosition = root.transform.position;
        _rootStartRotation = root.transform.rotation;

        // if no joints specified get joints using
        // not root (is static), begins with 'articulation:'
        if (targetJoints.Length == 0)
        {
            targetJoints = targetRagdollRoot
                .GetComponentsInChildren<ArticulationBody>(true)
                .Where(x => x.isRoot == false)
                .Where(x => x.name.StartsWith("articulation:"))
                .ToArray();
            SetJointsToMaxROM();
        }
    }


    void CopyMocap()
    {
        if (_mocapControllerArtanim != null &&
            targetRagdollRoot != null &&
            _mocapControllerArtanim.enabled)
        {
            var atriculationBodies = targetRagdollRoot.GetComponentsInChildren<ArticulationBody>();
            if (atriculationBodies.Length == 0)
                return;
            var root = atriculationBodies.First(x => x.isRoot);
            CopyMocapStatesTo(root.gameObject, _rootStartPosition);
            // teleport back to start position
            // var curRotation = root.transform.rotation;
            // root.TeleportRoot(_rootStartPosition, curRotation);
            // Vector3 offset = _rootStartPosition - root.transform.position;
            // root.gameObject.SetActive(false);
            // foreach (var t in root.GetComponentsInChildren<Transform>())
            // {
            //     t.position = t.position + offset;
            // }
            // root.transform.position = _rootStartPosition;
            // root.gameObject.SetActive(true);

            // foreach (var body in atriculationBodies)
            // {
            //     if (body.twistLock == ArticulationDofLock.LimitedMotion)
            //     {
            //         var xDrive = body.xDrive;
            //         List<float> targets = new List<float>();
            //         var bb = body.GetDriveTargets(targets);
            //         var cc = 22;
            //     }
            // }
        }
    }
    void CopyMocapStatesTo(GameObject target, Vector3 rootPosition)
    {

        var targets = target.GetComponentsInChildren<ArticulationBody>().ToList();
        if (targets?.Count == 0)
            return;
        var root = targets.First(x => x.isRoot);
        root.gameObject.SetActive(false);
        var mocapRoot = _mocapControllerArtanim.GetComponentsInChildren<Rigidbody>().First(x => x.name == root.name);
        Vector3 offset = rootPosition - mocapRoot.transform.position;
        foreach (var body in targets)
        {
            var stat = _mocapControllerArtanim.GetComponentsInChildren<Rigidbody>().First(x => x.name == body.name);
            body.transform.position = stat.position + offset;
            body.transform.rotation = stat.rotation;
            if (body.isRoot)
            {
                body.TeleportRoot(stat.position + offset, stat.rotation);
            }
        }
        root.gameObject.SetActive(true);
        foreach (var body in targets)
        {
            // body.AddForce(new Vector3(0.1f, -200f, 3f));
            // body.AddTorque(new Vector3(0.1f, 200f, 3f));
            body.velocity = (new Vector3(0.1f, 4f, .3f));
            body.angularVelocity = (new Vector3(0.1f, 20f, 3f));
        }
    }




    void FixedUpdate()
    {
        for (int i = 0; i < joints.Length; i++)
        {


            Quaternion localRotation = joints[i].localRotation;

           Vector3 candidates4storage = Utils.GetSwingTwist(localRotation);

            

            if (info2store.Values[i].upper.x < candidates4storage.x)
                info2store.Values[i].upper.x = candidates4storage.x;
            if (info2store.Values[i].upper.y < candidates4storage.y)
                info2store.Values[i].upper.y = candidates4storage.y;
            if (info2store.Values[i].upper.z < candidates4storage.z)
                info2store.Values[i].upper.z = candidates4storage.z;


            if (info2store.Values[i].lower.x > candidates4storage.x)
                info2store.Values[i].lower.x = candidates4storage.x;
            if (info2store.Values[i].lower.y > candidates4storage.y)
                info2store.Values[i].lower.y = candidates4storage.y;
            if (info2store.Values[i].lower.z > candidates4storage.z)
                info2store.Values[i].lower.z = candidates4storage.z;


          
        }

        if (duration < Time.time)
        {
            if(animationEndMessage.Length > 0) { 
                Debug.Log(animationEndMessage);
                animationEndMessage = "";
            }

        }

   //     CalcPreview();//this only stores the ROM value?


        CalculateOscillatorParameters();


    }
    // void FixedUpdate()
    void OnRenderObject()
    {
        if (MimicMocap)
        {
            if (_physicsStepsToNextMimic-- < 1)
            {
                CopyMocap();
                _physicsStepsToNextMimic = MimicSkipPhysicsSteps;
            }
        }
    }

    // preview range of motion

    /*
    void CalcPreview()
    {
        ArticulationBody[] articulationBodies = targetJoints;
        //we want them all:
        if (articulationBodies.Length == 0)
            articulationBodies = targetRagdollRoot.GetComponentsInChildren<ArticulationBody>(true);

        List<RangeOfMotionValue> preview = new List<RangeOfMotionValue>();

        //List<string> jNames = new List<string>(info2store.jointNames);
        List<string> jNames = new List<string>(info2store.getNames());
        for (int i = 0; i < articulationBodies.Length; i++)
        {
            string s = articulationBodies[i].name;
            string[] parts = s.Split(':');
            //we assume the articulationBodies have a name structure of hte form ANYNAME:something-in-the-targeted-joint

            int index = -1;

            index = jNames.FindIndex(x => x.Contains(parts[1]));

            if (index < 0)
                    Debug.Log("Could not find a joint name matching " + s + " and specifically: " + parts[1]);
            else
            {
                preview.Add(info2store.Values[index]);
            }
        }
       // RangeOfMotionPreview = preview.ToArray();
    }
    */

    //Not needed, the previous function already does that
    //public void WriteRangeOfMotion()
    //{
    //    if (RangeOfMotion2Store == null)
    //        RangeOfMotion2Store = RangeOfMotion004.CreateInstance<RangeOfMotion004>();
    //    RangeOfMotion2Store.Values = RangeOfMotionPreview;
    //}

    // Make all joints use Max Range of Motion 
    public void SetJointsToMaxROM()
    {
        //these are the articulationBodies that we want to parse and apply the constraints to
        ArticulationBody[] articulationBodies;

        ArticulationBody[] joints = targetJoints;
        //we want them all:
        if (joints.Length == 0)
            joints = targetRagdollRoot.GetComponentsInChildren<ArticulationBody>();

        articulationBodies = joints.ToArray();

        foreach (var body in articulationBodies)
        {
            // root has no DOF
            if (body.isRoot)
                continue;
            body.jointType = ArticulationJointType.SphericalJoint;
            body.twistLock = ArticulationDofLock.LimitedMotion;
            body.swingYLock = ArticulationDofLock.LimitedMotion;
            body.swingZLock = ArticulationDofLock.LimitedMotion;

            var drive = new ArticulationDrive();
            drive.lowerLimit = -(float)MaxROM;
            drive.upperLimit = (float)MaxROM;
            drive.stiffness = stiffness;
            drive.damping = damping;
            drive.forceLimit = forceLimit;
            body.xDrive = drive;

            drive = new ArticulationDrive();
            drive.lowerLimit = -(float)MaxROM;
            drive.upperLimit = (float)MaxROM;
            drive.stiffness = stiffness;
            drive.damping = damping;
            drive.forceLimit = forceLimit;
            body.yDrive = drive;

            drive = new ArticulationDrive();
            drive.lowerLimit = -(float)MaxROM;
            drive.upperLimit = (float)MaxROM;
            drive.stiffness = stiffness;
            drive.damping = damping;
            drive.forceLimit = forceLimit;
            body.zDrive = drive;

            // body.useGravity = false;
        }
    }




    public void CalculateOscillatorParameters() { 
    
        
    
    
    }



    //we assume the constraints have been well applied 
    //This function is called from an Editor Script
    public void Prepare4PrefabStorage(out ProcRagdollAgent rda, out ManyWorlds.SpawnableEnv envPrefab)
    {


        ArticulationBody targetRagdollPrefab = GameObject.Instantiate(targetRagdollRoot);

        //if there is a spawnableEnv, there is a ragdollAgent:
        rda = targetRagdollPrefab.GetComponent<ProcRagdollAgent>();

        if (rda != null)
            Debug.Log("Setting up the  ragdoll agent");

        envPrefab = null;





        //these are all the articulationBodies in the ragdoll prefab
        ArticulationBody[] articulationBodies;

        Transform[] joints = targetRagdollPrefab.GetComponentsInChildren<Transform>();


        List<ArticulationBody> temp = new List<ArticulationBody>();
        for (int i = 0; i < joints.Length; i++)
        {
            ArticulationBody a = joints[i].GetComponent<ArticulationBody>();
            if (a != null)
                temp.Add(a);
        }

        articulationBodies = temp.ToArray();


        //We also prepare everything inside the ragdoll agent :
        for (int i = 0; i < articulationBodies.Length; i++)
        {
            articulationBodies[i].transform.localRotation = Quaternion.identity;
            if (articulationBodies[i].isRoot)
            {
                articulationBodies[i].immovable = false;
                if (rda != null)
                    rda.CameraTarget = articulationBodies[i].transform;


                if (trainingEnv)
                {
                    envPrefab = GameObject.Instantiate(trainingEnv);

                    Animator target = envPrefab.transform.GetComponentInChildren<Animator>();



                    //we assume the environment has an animated character, and in this there is a son which is the root of a bunch of rigidBodies forming a humanoid.
                    //TODO: replace this function with something that creates the rigidBody humanoid such a thing procedurally
                    activateMarathonManTarget(target);

                    //we also need our target animation to have this:
                    TrackBodyStatesInWorldSpace tracker = target.GetComponent<TrackBodyStatesInWorldSpace>();
                    if (tracker == null)
                        target.gameObject.AddComponent<TrackBodyStatesInWorldSpace>();



                    if (rda != null)
                    {
                        rda.transform.parent = envPrefab.transform;
                        rda.name = targetRagdollRoot.name;
                        rda.enabled = true;//this should already be the case, but just ot be cautious
                    }

                    MapRagdoll2Anim agentOutcome = envPrefab.GetComponentInChildren<MapRagdoll2Anim>(true);
                    if (agentOutcome != null)
                    {
                        agentOutcome.gameObject.SetActive(true);
                        //agentOutcome.enabled = true;
                        agentOutcome.ArticulationBodyRoot = articulationBodies[i];
                    }
                }
            }

        }

    }



    static void activateMarathonManTarget(Animator target)
    {

        Transform[] rbs = target.GetComponentsInChildren<Transform>(true);


        //Rigidbody[] rbs = target.GetComponentsInChildren<Rigidbody>(true);
        for (int i = 0; i < rbs.Length; i++)
        {
            rbs[i].gameObject.SetActive(true);

        }



        //the animation source is a son of the SpawnableEnv, or it does not find the MocapControllerArtanim when it initializes
        MapAnim2Ragdoll mca = target.GetComponent<MapAnim2Ragdoll>();
        mca.enabled = true;






    }



}