PhysicsSceneManager.cs

using System.Collections.Generic;
using UnityEngine;
using UnityStandardAssets.Characters.FirstPerson;
using System;

[ExecuteInEditMode]

public class PhysicsSceneManager : MonoBehaviour {
    public Dictionary<string, SimObjPhysics> ObjectIdToSimObjPhysics = new Dictionary<string, SimObjPhysics>();

#if UNITY_EDITOR
    private bool m_Started = false;
#endif

    private Vector3 gizmopos;
    private Vector3 gizmoscale;
    private Quaternion gizmoquaternion;

    // keep track of if the physics autosimulation has been paused or not
    public bool physicsSimulationPaused = false;

    // this is used to report if the scene is at rest in metadata, and also to
    // automatically resume Physics Autosimulation if physics simulation was
    // paused
    public bool isSceneAtRest;                // if any object in the scene has a non zero
                                              // velocity, set to false
    public List<Rigidbody> rbsInScene = null; // list of all active rigidbodies in the scene

    public ExperimentRoomSceneManager experimentRoomSceneManager;

    private void OnEnable() {
        // clear this on start so that the CheckForDuplicates function doesn't
        // check pre-existing lists
        SetupScene();

        if (!GameObject.Find("Objects")) {
            GameObject c = new GameObject("Objects");
            Debug.Log(c.transform.name + " was missing and is now added");
        }
    }

    public void SetupScene() {
        experimentRoomSceneManager = GetComponent<ExperimentRoomSceneManager>();
        ObjectIdToSimObjPhysics.Clear();
        GatherSimObjPhysInScene();
        GatherAllRBsInScene();
    }
    // Use this for initialization
    void Start() { GatherAllRBsInScene(); }

    private void GatherAllRBsInScene() {
        // cache all rigidbodies that are in the scene by default
        // NOTE: any rigidbodies created from actions such as Slice/Break or
        // spawned in should be added to this!
        rbsInScene = new List<Rigidbody>(FindObjectsOfType<Rigidbody>());
    }

    // Update is called once per frame
    void Update() {}

    public virtual void LateUpdate() {
        // check what objects in the scene are currently in motion
        foreach (Rigidbody rb in rbsInScene) {
            if (rb == null)
                return;

            SimObjPhysics sop = rb.GetComponentInParent<SimObjPhysics>();
            // if this rigidbody is part of a SimObject, calculate rest using
            // lastVelocity/currentVelocity comparisons
            if (sop != null && rb.transform.gameObject.activeSelf) // make sure the object is actually active,
                                                                   // otherwise skip the check
            {
                float currentVelocity = Math.Abs(rb.angularVelocity.sqrMagnitude + rb.velocity.sqrMagnitude);
                float accel = (currentVelocity - sop.lastVelocity) / Time.fixedDeltaTime;

                if (Mathf.Abs(accel) <= 0.0001f) {
                    sop.inMotion = false;
                }

                else {
                    // the rb's velocities are not 0, so it is in motion and the
                    // scene is not at rest
                    sop.inMotion = true;
                    isSceneAtRest = false;
                }
            }

            // this rigidbody is not a SimOBject, and might be a piece of a
            // shattered sim object spawned in, or something
            else {
                if (rb.transform.gameObject.activeSelf) {
                    // is the rigidbody at non zero velocity? then the scene is
                    // not at rest
                    if (!(Math.Abs(rb.angularVelocity.sqrMagnitude + rb.velocity.sqrMagnitude) < 0.01)) {
                        isSceneAtRest = false;
                        // make sure the rb's drag values are not at 0 exactly
                        rb.drag += 0.01f;
                        rb.angularDrag += 0.01f;

#if UNITY_EDITOR
                        print(rb.transform.name + " is still in motion!");
#endif
                    }

                    // the velocities are small enough, assume object has come
                    // to rest and force this one to sleep
                    else {
                        rb.drag = 1.0f;
                        rb.angularDrag = 1.0f;
                    }

                    // if the shard/broken piece gets out of bounds somehow and
                    // begins falling forever, get rid of it with this check
                    if (rb.transform.position.y < -50f) {
                        rb.transform.gameObject.SetActive(false);
                        // note: we might want to remove these from the list of
                        // rbs at some point but for now it'll be fine
                    }
                }
            }
        }
    }

    // used to add a reference to a rigidbody created after the scene was
    // started
    public void AddToRBSInScene(Rigidbody rb) { rbsInScene.Add(rb); }

    public void RemoveFromRBSInScene(Rigidbody rb) { rbsInScene.Remove(rb); }

    public void ResetObjectIdToSimObjPhysics() {
        ObjectIdToSimObjPhysics.Clear();
        foreach (SimObjPhysics so in GameObject.FindObjectsOfType<SimObjPhysics>()) {
            ObjectIdToSimObjPhysics[so.ObjectID] = so;
        }
    }

    public void GatherSimObjPhysInScene() {
        List<SimObjPhysics> allPhysObjects = new List<SimObjPhysics>();

        allPhysObjects.AddRange(FindObjectsOfType<SimObjPhysics>());
        allPhysObjects.Sort((x, y) => (x.Type.ToString().CompareTo(y.Type.ToString())));

        foreach (SimObjPhysics o in allPhysObjects) {
            Generate_ObjectID(o);
/// debug in editor, make sure no two object share ids for some reason
#if UNITY_EDITOR
            if (CheckForDuplicateObjectIDs(o) && !(o.name.Contains("floor"))) {
                Debug.Log("Yo there are duplicate ObjectIDs! Check" + o.ObjectID + "in scene " + UnityEngine.SceneManagement.SceneManager.GetActiveScene().name);
            } else {
                AddToObjectsInScene(o);
                continue;
            }
#endif

            AddToObjectsInScene(o);
        }

        BaseFPSAgentController fpsController = GameObject.FindObjectOfType<BaseFPSAgentController>();
        foreach (Collider c in fpsController.GetComponents<Collider>())
            c.enabled = true;
        if (fpsController.imageSynthesis != null) {
            fpsController.imageSynthesis.OnSceneChange();
        }
    }

    public virtual void Generate_ObjectID(SimObjPhysics o) {
        // check if this object require's it's parent simObj's ObjectID as a
        // prefix
        if (ReceptacleRestrictions.UseParentObjectIDasPrefix.Contains(o.Type)) {
            SimObjPhysics parent = o.transform.parent.GetComponent<SimObjPhysics>();
            if (parent == null) {
                Debug.LogWarning("Object " + o + " requires a SimObjPhysics " + "parent to create its object ID but none exists. Using 'None' instead.");
                o.ObjectID = "None|" + o.Type.ToString();
                return;
            }

            if (parent.ObjectID == null) {
                Vector3 ppos = parent.transform.position;
                string xpPos = (ppos.x >= 0 ? "+" : "") + ppos.x.ToString("00.00");
                string ypPos = (ppos.y >= 0 ? "+" : "") + ppos.y.ToString("00.00");
                string zpPos = (ppos.z >= 0 ? "+" : "") + ppos.z.ToString("00.00");
                parent.ObjectID = parent.Type.ToString() + "|" + xpPos + "|" + ypPos + "|" + zpPos;
            }

            o.ObjectID = parent.ObjectID + "|" + o.Type.ToString();
            return;
        }

        Vector3 pos = o.transform.position;
        string xPos = (pos.x >= 0 ? "+" : "") + pos.x.ToString("00.00");
        string yPos = (pos.y >= 0 ? "+" : "") + pos.y.ToString("00.00");
        string zPos = (pos.z >= 0 ? "+" : "") + pos.z.ToString("00.00");
        o.ObjectID = o.Type.ToString() + "|" + xPos + "|" + yPos + "|" + zPos;
    }

    // used to create object id for an object created as result of a state
    // change of another object ie: bread - >breadslice1, breadslice 2 etc
    public void Generate_InheritedObjectID(SimObjPhysics sourceObject, SimObjPhysics createdObject, int count) {
        createdObject.ObjectID = sourceObject.ObjectID + "|" + createdObject.ObjType + "_" + count;
        AddToObjectsInScene(createdObject);
    }

    private bool CheckForDuplicateObjectIDs(SimObjPhysics sop) {
        if (ObjectIdToSimObjPhysics.ContainsKey(sop.ObjectID))
            return true;

        else
            return false;
    }

    public void AddToObjectsInScene(SimObjPhysics sop) {
        ObjectIdToSimObjPhysics[sop.ObjectID] = sop;
        if (sop.myRigidbody)
            AddToRBSInScene(sop.myRigidbody);
    }

#if UNITY_EDITOR
    void OnDrawGizmos() {
        Gizmos.color = Color.magenta;
        if (m_Started) {
            Matrix4x4 cubeTransform = Matrix4x4.TRS(gizmopos, gizmoquaternion, gizmoscale);
            Matrix4x4 oldGizmosMatrix = Gizmos.matrix;

            Gizmos.matrix *= cubeTransform;

            Gizmos.DrawWireCube(Vector3.zero, Vector3.one);

            Gizmos.matrix = oldGizmosMatrix;
        }
    }
#endif
}