//=============================================================================================
/**
* @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);
}
}
//=============================================================================================
/**
* @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);
}
}
//============================================================================================
/**
* @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;
}
