OptitrackRigidBody.Update C# (CSharp) Method

    void Update()
    {
        //If we have received a packet from Motive then look for the rigid body ID index
        if(foundIndex == false)
        {
            if(OptitrackRigidBodyManager.instance.receivedFirstRigidBodyPacket)
            {
                if(foundIndex == false)
                {
                    for(int i = 0; i < OptitrackRigidBodyManager.instance.rigidBodyIDs.Length; i++)
                    {
                        //Looking for ID in array of rigid body IDs
                        if(OptitrackRigidBodyManager.instance.rigidBodyIDs[i] == ID)
                        {
                            index = i; //Found ID
                        }
                    }
                    foundIndex = true;
                }
            }
        }
        else
        {
            if (usePostionTracking)
            {
                if (originOverride != null)
                {
                    if( gameObject.GetComponent<Rigidbody>() )
                        gameObject.GetComponent<Rigidbody>().MovePosition( (OptitrackRigidBodyManager.instance.rigidBodyPositions[index] - OptitrackRigidBodyManager.instance.origin.position) + originOverride.transform.position );
                    else
                        transform.position = (OptitrackRigidBodyManager.instance.rigidBodyPositions[index] - OptitrackRigidBodyManager.instance.origin.position) + originOverride.transform.position;
                }
                else if( gameObject.GetComponent<Rigidbody>() )
                {
                    gameObject.GetComponent<Rigidbody>().MovePosition( OptitrackRigidBodyManager.instance.rigidBodyPositions[index] );
                }
                else
                {
                    Vector3 newPos = OptitrackRigidBodyManager.instance.rigidBodyPositions[index];
                    if( predictLostData )
                    {
                        if( Vector3.SqrMagnitude(lastPosition - newPos) < 9.99999944E-11f )//lost data
                        {
                            Vector3 velocity = pastPositions[pastPositions.Count - 1] - pastPositions[pastPositions.Count - 2];
                            newPos = pastPositions[pastPositions.Count - 1] + velocity;
                        }

                        lastPosition = OptitrackRigidBodyManager.instance.rigidBodyPositions[index];

                        pastPositions.RemoveAt(0);

                        pastPositions.Add(newPos);
                    }
                    transform.position = newPos;//OptitrackRigidBodyManager.instance.rigidBodyPositions[index];
                }
            }

            if (useRotationTracking)
            {
                if (originOverride != null)
                {
                    //Subtract the origin rotation used by OptitrackRigidBodyManager
                    transform.rotation = Quaternion.Inverse(OptitrackRigidBodyManager.instance.origin.rotation) * OptitrackRigidBodyManager.instance.rigidBodyQuaternions[index];
                    //Add the originOverride rotation applied to this rigid body
                    transform.rotation = originOverride.transform.rotation * transform.rotation;
                }
        //				else if( gameObject.GetComponent<Rigidbody>() )
        //				{
        //					gameObject.GetComponent<Rigidbody>().MoveRotation( OptitrackRigidBodyManager.instance.rigidBodyQuaternions[index] );
        //				}
                else
                {
                    Quaternion newRot = OptitrackRigidBodyManager.instance.rigidBodyQuaternions[index];
        //					float angle = Quaternion.Angle( transform.rotation, newRot );
        //					if( angle <= 120 )
                        transform.rotation = newRot;
                }
            }
        }
    }