Пример #1
0
        //=============================================================================================

        /**
         *  @brief
         *
         *********************************************************************************************/
        public static void SetRelativeTo(TrajectoryPoint[] a_trajectory, Transform a_transform, int a_startIndex = 0)
        {
            float rot = a_transform.rotation.eulerAngles.y;

            for (int i = a_startIndex; i < a_trajectory.Length; ++i)
            {
                Vector3 newPos = a_transform.InverseTransformVector(a_trajectory[i].Position);
                float   newRot = a_trajectory[i].FacingAngle - rot;

                a_trajectory[i] = new TrajectoryPoint(newPos, newRot);
            }
        }
Пример #2
0
        //=============================================================================================

        /**
         *  @brief
         *
         *********************************************************************************************/
        public static void LerpGoal(TrajectoryPoint[] _target, TrajectoryPoint[] _startGoal,
                                    TrajectoryPoint[] _endGoal, float _lerpVal)
        {
            for (int i = 0; i < _target.Length; ++i)
            {
                TrajectoryPoint fromPoint = _startGoal[i];
                TrajectoryPoint toPoint   = _endGoal[i];

                Vector3 pos    = Vector3.Lerp(fromPoint.Position, toPoint.Position, _lerpVal);
                float   fAngle = Mathf.LerpAngle(fromPoint.FacingAngle, toPoint.FacingAngle, _lerpVal);

                _target[i] = new TrajectoryPoint(pos, fAngle);
            }
        }
Пример #3
0
        //============================================================================================

        /**
         *  @brief
         *
         *********************************************************************************************/
        public static void Lerp(ref TrajectoryPoint a_from, ref TrajectoryPoint a_to, float a_step, out TrajectoryPoint a_result)
        {
            a_result.Position    = Vector3.Lerp(a_from.Position, a_to.Position, a_step);
            a_result.FacingAngle = Mathf.LerpAngle(a_from.FacingAngle, a_to.FacingAngle, a_step);
        }
        //============================================================================================

        /**
         *  @brief Draws the debugging gizmos for the MxMAnimator.
         *
         *  This includes drawing of the desired trajectory, animation trajectory and pose joint data.
         *
         *********************************************************************************************/
        private void OnDrawGizmos()
        {
            if (p_trajectoryGenerator != null && CurrentAnimData != null && m_animMixerConnected && enabled)
            {
                if (m_fsm.CurrentStateId == (uint)EMxMStates.Event)
                {
                    Gizmos.color = Color.green;
                    Gizmos.DrawSphere(m_desiredEventRootWorld.Position, 0.1f);
                    Quaternion rotation = Quaternion.AngleAxis(m_desiredEventRootWorld.RotationY, Vector3.up);
                    DrawArrow.ForGizmo(m_desiredEventRootWorld.Position, rotation * Vector3.forward, 0.8f, 20f);
                    Gizmos.color = Color.blue;
                    Gizmos.DrawSphere(m_currentEventRootWorld.Position, 0.1f);
                    rotation = Quaternion.AngleAxis(m_currentEventRootWorld.RotationY, Vector3.up);
                    DrawArrow.ForGizmo(m_currentEventRootWorld.Position, rotation * Vector3.forward, 0.8f, 20f);

                    Quaternion contactRotation = Quaternion.AngleAxis(m_desiredEventRootWorld.RotationY, Vector3.up);

                    if (CurEventContacts != null && CurEventContacts.Length > m_curEventContactIndex)
                    {
                        Gizmos.color = Color.red;
                        Gizmos.DrawSphere(CurEventContacts[m_curEventContactIndex].Position, 0.05f);
                    }

                    Gizmos.color = Color.cyan;
                    Gizmos.DrawSphere(transform.position, 0.05f);
                    DrawArrow.ForGizmo(transform.position, transform.rotation * Vector3.forward, 0.4f, 20f);
                }

                if (m_debugGoal)
                {
                    Array.Copy(m_desiredGoal, m_debugDesiredGoal, m_debugDesiredGoal.Length);
                    float rot = transform.rotation.eulerAngles.y;
                    for (int i = 0; i < m_debugDesiredGoal.Length; ++i)
                    {
                        Vector3 newPos = transform.TransformVector(m_debugDesiredGoal[i].Position);
                        float   newRot = m_debugDesiredGoal[i].FacingAngle + rot;

                        m_debugDesiredGoal[i] = new TrajectoryPoint(newPos, newRot);
                    }

                    //Draw Desired Trajectory
                    int doneCenter = 0;
                    for (int i = 0; i < m_debugDesiredGoal.Length; ++i)
                    {
                        TrajectoryPoint curPoint = m_debugDesiredGoal[i];

                        if (CurrentAnimData.PosePredictionTimes[i] < 0f)
                        {
                            Gizmos.color = new Color(0f, 0.5f, 0f);
                        }
                        else
                        {
                            if (doneCenter == 0)
                            {
                                Gizmos.color  = Color.cyan;
                                Handles.color = Color.cyan;
                                Handles.DrawWireDisc(transform.position, Vector3.up, 0.3f);
                                Gizmos.color = Color.blue;
                                Vector3 centerPos   = transform.position;
                                Vector3 centerPosUp = centerPos + Vector3.up * 0.14f;

                                if (m_debugArrowMesh != null)
                                {
                                    Gizmos.DrawMesh(m_debugArrowMesh, centerPos, Quaternion.LookRotation(transform.forward, Vector3.up));
                                }
                                else
                                {
                                    Gizmos.DrawSphere(centerPos, 0.06f);
                                    Gizmos.DrawLine(centerPos, centerPosUp);
                                    DrawArrow.ForGizmo(centerPosUp, transform.forward * 0.2f, 0.05f, 20f);
                                }

                                doneCenter = 1;

                                Vector3 point1;
                                Vector3 point2;
                                if (m_transformGoal)
                                {
                                    point1 = transform.position + m_debugDesiredGoal[i - 1].Position;
                                    point2 = transform.position + m_debugDesiredGoal[i].Position;
                                }
                                else
                                {
                                    point1 = transform.TransformPoint(m_debugDesiredGoal[i - 1].Position);
                                    point2 = transform.TransformPoint(m_debugDesiredGoal[i].Position);
                                }

                                Gizmos.color = new Color(0f, 0.5f, 0f);
                                Gizmos.DrawLine(centerPos, point1);
                                Gizmos.color = Color.green;
                                Gizmos.DrawLine(centerPos, point2);
                            }

                            Gizmos.color = Color.green;
                        }

                        Vector3 pointPos;

                        float angle = curPoint.FacingAngle * Mathf.Deg2Rad;

                        var direction = new Vector3(0.2f * Mathf.Sin(angle), 0f, 0.2f * Mathf.Cos(angle));

                        if (m_transformGoal)
                        {
                            pointPos = transform.position + curPoint.Position;
                        }
                        else
                        {
                            pointPos  = transform.TransformPoint(curPoint.Position);
                            direction = transform.TransformDirection(direction);
                        }

                        if (m_debugArrowMesh != null)
                        {
                            Gizmos.DrawMesh(m_debugArrowMesh, pointPos, Quaternion.LookRotation(direction, Vector3.up));
                        }
                        else
                        {
                            Gizmos.DrawSphere(pointPos, 0.06f);
                            Gizmos.DrawLine(pointPos, new Vector3(pointPos.x, pointPos.y + 0.14f, pointPos.z));
                            DrawArrow.ForGizmo(new Vector3(pointPos.x, pointPos.y + 0.14f, pointPos.z),
                                               direction, 0.05f, 20f);
                        }

                        if (i != 0)
                        {
                            Vector3 pointPosPrev;

                            if (m_transformGoal)
                            {
                                pointPosPrev = transform.position + m_debugDesiredGoal[i - 1].Position;
                            }
                            else
                            {
                                pointPosPrev = transform.TransformPoint(m_debugDesiredGoal[i - 1].Position);
                            }

                            if (doneCenter != 1)
                            {
                                Gizmos.DrawLine(pointPos, pointPosPrev);
                            }
                            else
                            {
                                doneCenter = 2;
                            }
                        }
                    }
                }

                if (m_debugCurrentPose)
                {
                    Gizmos.color = Color.yellow;

                    for (int i = 0; i < m_curInterpolatedPose.JointsData.Length; ++i)
                    {
                        Vector3 pos = transform.TransformPoint(m_curInterpolatedPose.JointsData[i].Position);
                        Gizmos.DrawWireSphere(pos, 0.04f);
                        DrawArrow.ForGizmo(pos, transform.TransformVector(m_curInterpolatedPose.JointsData[i].Velocity), 0.05f);
                    }
                }

                if (m_debugChosenTrajectory)
                {
                    TrajectoryPoint[] curGoal = CurrentAnimData.Poses[m_curInterpolatedPose.PoseId].Trajectory;

                    int doneCenter = 0;
                    for (int i = 0; i < curGoal.Length; ++i)
                    {
                        TrajectoryPoint curPoint = curGoal[i];

                        if (CurrentAnimData.PosePredictionTimes[i] < 0f)
                        {
                            Gizmos.color = new Color(0.5f, 0f, 0f);
                        }
                        else
                        {
                            if (doneCenter == 0)
                            {
                                Gizmos.color  = Color.cyan;
                                Handles.color = Color.cyan;
                                Handles.DrawWireDisc(transform.position, Vector3.up, 0.3f);

                                DrawArrow.ForGizmo(transform.position,
                                                   transform.TransformVector(m_curInterpolatedPose.LocalVelocity), 0.4f, 20f);

                                Gizmos.color = Color.blue;
                                Vector3 centerPos   = transform.position;
                                Vector3 centerPosUp = centerPos + Vector3.up * 0.14f;

                                if (m_debugArrowMesh != null)
                                {
                                    Gizmos.DrawMesh(m_debugArrowMesh, centerPos, Quaternion.LookRotation(transform.forward, Vector3.up));
                                }
                                else
                                {
                                    Gizmos.DrawSphere(centerPos, 0.06f);
                                    Gizmos.DrawLine(centerPos, centerPosUp);
                                    DrawArrow.ForGizmo(centerPosUp, transform.forward * 0.2f, 0.05f, 20f);
                                }

                                doneCenter = 1;

                                Gizmos.color = new Color(0.5f, 0f, 0f);
                                Gizmos.DrawLine(centerPos, transform.TransformPoint(curGoal[i - 1].Position));
                                Gizmos.color = Color.red;
                                Gizmos.DrawLine(centerPos, transform.TransformPoint(curGoal[i].Position));
                            }

                            Gizmos.color = Color.red;
                        }

                        Vector3 pointPos = transform.TransformPoint(curPoint.Position);
                        float   angle    = curPoint.FacingAngle * Mathf.Deg2Rad;

                        Vector3 direction = transform.TransformDirection(new Vector3(0.2f * Mathf.Sin(angle),
                                                                                     0f, 0.2f * Mathf.Cos(angle)));

                        if (m_debugArrowMesh != null)
                        {
                            Gizmos.DrawMesh(m_debugArrowMesh, pointPos, Quaternion.LookRotation(direction, Vector3.up));
                        }
                        else
                        {
                            Gizmos.DrawSphere(pointPos, 0.06f);
                            Gizmos.DrawLine(pointPos, new Vector3(pointPos.x, pointPos.y + 0.14f, pointPos.z));
                            DrawArrow.ForGizmo(new Vector3(pointPos.x, pointPos.y + 0.14f, pointPos.z), direction, 0.05f, 20f);
                        }

                        if (i != 0)
                        {
                            if (doneCenter != 1)
                            {
                                Gizmos.DrawLine(pointPos, transform.TransformPoint(curGoal[i - 1].Position));
                            }
                            else
                            {
                                doneCenter = 2;
                            }
                        }
                    }
                }
            }
            else
            {
                //Draw Chosen Pose Trajectory
                if (m_debugPoses && m_debugPosesActive && CurrentAnimData != null)
                {
                    if (m_debugPoseId >= CurrentAnimData.Poses.Length)
                    {
                        m_debugPoseId = CurrentAnimData.Poses.Length - 1;
                    }

                    PoseData pose = CurrentAnimData.Poses[m_debugPoseId];
                    UpdatePoseDebug();
                    Gizmos.color = Color.yellow;
                    for (int i = 0; i < pose.JointsData.Length; ++i)
                    {
                        Vector3 pos = transform.TransformPoint(pose.JointsData[i].Position);
                        Gizmos.DrawWireSphere(pos, 0.04f);
                        DrawArrow.ForGizmo(pos, transform.TransformVector(
                                               pose.JointsData[i].Velocity), 0.05f);
                    }

                    if (m_debugPoseId < CurrentAnimData.Poses.Length)
                    {
                        m_chosenPose = CurrentAnimData.Poses[m_debugPoseId];
                    }
                    else
                    {
                        m_chosenPose = CurrentAnimData.Poses[CurrentAnimData.Poses.Length - 1];
                    }

                    TrajectoryPoint[] curGoal = m_chosenPose.Trajectory;

                    int doneCenter = 0;
                    for (int i = 0; i < curGoal.Length; ++i)
                    {
                        TrajectoryPoint curPoint = curGoal[i];

                        if (CurrentAnimData.PosePredictionTimes[i] < 0f)
                        {
                            Gizmos.color = new Color(0.5f, 0f, 0f);
                        }
                        else
                        {
                            if (doneCenter == 0)
                            {
                                Gizmos.color  = Color.cyan;
                                Handles.color = Color.cyan;
                                Handles.DrawWireDisc(transform.position, Vector3.up, 0.3f);
                                DrawArrow.ForGizmo(transform.position,
                                                   transform.TransformVector(m_chosenPose.LocalVelocity), 0.4f, 20f);

                                Gizmos.color = Color.blue;
                                Vector3 centerPos   = transform.position;
                                Vector3 centerPosUp = centerPos + Vector3.up * 0.14f;

                                if (m_debugArrowMesh != null)
                                {
                                    Gizmos.DrawMesh(m_debugArrowMesh, centerPos, Quaternion.LookRotation(transform.forward, Vector3.up));
                                }
                                else
                                {
                                    Gizmos.DrawSphere(centerPos, 0.06f);
                                    Gizmos.DrawLine(centerPos, centerPosUp);
                                    DrawArrow.ForGizmo(centerPosUp, transform.forward * 0.2f, 0.05f, 20f);
                                }
                                doneCenter = 1;

                                Gizmos.color = new Color(0.5f, 0f, 0f);
                                Gizmos.DrawLine(centerPos, transform.TransformPoint(curGoal[i - 1].Position));
                                Gizmos.color = Color.red;
                                Gizmos.DrawLine(centerPos, transform.TransformPoint(curGoal[i].Position));
                            }

                            Gizmos.color = Color.red;
                        }

                        Vector3 pointPos = transform.TransformPoint(curPoint.Position);
                        float   angle    = curPoint.FacingAngle * Mathf.Deg2Rad;


                        Gizmos.DrawSphere(pointPos, 0.06f);
                        DrawArrow.ForGizmo(new Vector3(pointPos.x, pointPos.y + 0.14f, pointPos.z),
                                           transform.TransformDirection(new Vector3(0.2f * Mathf.Sin(angle),
                                                                                    0f, 0.2f * Mathf.Cos(angle))), 0.05f, 20f);
                        Gizmos.DrawLine(pointPos, new Vector3(pointPos.x, pointPos.y + 0.14f, pointPos.z));

                        if (i != 0)
                        {
                            if (doneCenter != 1)
                            {
                                Gizmos.DrawLine(pointPos, transform.TransformPoint(curGoal[i - 1].Position));
                            }
                            else
                            {
                                doneCenter = 2;
                            }
                        }
                    }
                }
            }
        }
        //===========================================================================================

        /**
         *  @brief This function is called by the MxMAnimator when it requires a goal to run an
         *  animation search. Essentially it extracts a course set of trajectory points from the granular
         *  recorded past and predicted future. The goal extracted is based on the trajectory configuration
         *  specified in the Pre-Processing stage
         *
         *  This function is an implementation of IMxMTrajectory
         *
         *********************************************************************************************/
        public void ExtractGoal()
        {
            p_trajectoryGenerateJobHandle.Complete();

            Vector3 transformPosition = transform.position;

            //Extract data from the trajectory for the goal times
            for (int i = 0; i < p_goal.Length; ++i)
            {
                float timeDelay = p_predictionTimes[i];

                if (timeDelay < 0f) //Past trajectory search
                {
                    int   curIndex = 1;
                    float lerp     = 0f;
                    float curTime  = Time.time;
                    for (int k = 1; k < p_recordedPastTimes.Count; ++k)
                    {
                        float time = p_recordedPastTimes[k];

                        if (time < curTime + timeDelay)
                        {
                            curIndex = k;


                            float timeError = (curTime + timeDelay) - time;
                            float deltaTime = p_recordedPastTimes[k - 1] - time;

                            lerp = timeError / deltaTime;
                            break;
                        }
                    }

                    if (curIndex < p_recordedPastTimes.Count)
                    {
                        Vector3 position    = Vector3.Lerp(p_recordedPastPositions[curIndex], p_recordedPastPositions[curIndex - 1], lerp);
                        float   facingAngle = Mathf.LerpAngle(p_recordedPastFacingAngles[curIndex], p_recordedPastFacingAngles[curIndex - 1], lerp);

                        if (p_flattenTrajectory)
                        {
                            position.y = transformPosition.y;
                        }

                        //p_goal[i] = new TrajectoryPoint(position - transformPosition, p_recordedPastFacingAngles[curIndex]);
                        p_goal[i] = new TrajectoryPoint(position - transformPosition, facingAngle);
                    }
                    else
                    {
                        p_goal[i] = new TrajectoryPoint();
                    }
                }
                else
                {
                    //int index = Mathf.RoundToInt(timeDelay / p_timeStep);
                    int index = Mathf.RoundToInt(timeDelay / p_timeHorizon * p_trajPositions.Length) - 1;

                    if (index >= p_trajPositions.Length)
                    {
                        index = p_trajPositions.Length - 1;
                    }

                    Vector3 position = p_trajPositions[index];

                    if (p_flattenTrajectory)
                    {
                        position.y = 0f;
                    }

                    p_goal[i] = new TrajectoryPoint(position, p_trajFacingAngles[index]);
                }
            }

            m_extractedThisFrame = true;
        }