Search Unity

  1. Unity 2019.2 is now released.
    Dismiss Notice

The idea to calculate Motion, Direction, Speed, Angle

Discussion in 'Scripting' started by canis, Oct 11, 2019.

  1. canis

    canis

    Joined:
    Oct 25, 2013
    Posts:
    65
    Since I'm programmer but not the mathematician, I would like to share the way I used to calculate all those things.

    In return I expect some of you will give me feedback, discussion or challenge, to learn more...
    or even make it faster. (I bet you know some trick)

    My major purpose to write this script ?
    I usually need to develop enemy AI, apply fake physics on object...etc
    In order to do that, I found myself need to re-calculate { speed, time, distance, direction, angle } across multiple script.
    some of them are costly, e.g. vector3.magnitude, tranformVector...
    This make me try to collect the common factor together into a single script with debug tools.

    and here is one of the case, that I use that CMotionCache.cs it to debug some advance feature on top of it.


    And yes. code here.
    >> also can found the missing files append on this post
    >> CMotionCache.cs & GizmosExtend.cs

    Code (CSharp):
    1. using UnityEngine;
    2. using Kit;
    3.  
    4. public class CMotionCache : MonoBehaviour
    5. {
    6.     [Header("Setting")]
    7.     [SerializeField, Tooltip("To define the world up for this object. require normalize vector (length = 1f)")]
    8.     Vector3 m_BiasGroundNormal = Vector3.up;
    9.     public Vector3 biasGroundNormal => m_BiasGroundNormal; // will normalize at OnEnable.
    10.  
    11.     [SerializeField] bool pauseOnTimeScaleZero = true;
    12.  
    13.     [Header("Debug")]
    14.     [SerializeField] DebugInfo debugInfo;
    15.  
    16.     [System.Flags]
    17.     private enum eDebugFlag
    18.     {
    19.         motion = 1 << 0,
    20.         rotation = 1 << 1,
    21.         speed = 1 << 2,
    22.     }
    23.  
    24.     [System.Serializable]
    25.     private class DebugInfo
    26.     {
    27.         [MaskField(typeof(eDebugFlag))] public eDebugFlag flag;
    28.         public Vector2 offset;
    29.  
    30.         public Color colorMotionVelocity = Color.green;
    31.         public Color colorMotionVector = Color.yellow;
    32.         public Color colorMotionAngle = Color.cyan;
    33.     }
    34.  
    35.     /// <summary>used for frame lock, prevent double calculation.</summary>
    36.     private int m_CacheFrameCount = -1;
    37.  
    38.     /// <summary>The <see cref="Time.deltaTime"/>, used for calculation.</summary>
    39.     public float deltaTime { get; private set; }
    40.  
    41.     /// <summary>World position on last frame</summary>
    42.     public Vector3 lastPosition { get; private set; }
    43.  
    44.     /// <summary>the rotation on last frame</summary>
    45.     public Quaternion lastRotation { get; private set; }
    46.  
    47.     /// <summary>The last forward from <see cref="lastRotation"/></summary>
    48.     public Vector3 lastForward { get; private set; }
    49.  
    50.     /// <summary>The motion between <see cref="lastPosition"/> and current position</summary>
    51.     public Vector3 motionVector { get; private set; }
    52.  
    53.     /// <summary>The motion vector based on local space <see cref="motionVector"/></summary>
    54.     public Vector3 localMotionVector { get; private set; }
    55.  
    56.     /// <summary>The motion vector project on <see cref="m_BiasGroundNormal"/></summary>
    57.     public Vector3 motionVectorIgnoreY { get; private set; }
    58.  
    59.     /// <summary>The local motion vector project on <see cref="m_BiasGroundNormal"/></summary>
    60.     public Vector3 localMotionVectorIgnoreY { get; private set; }
    61.  
    62.     /// <summary>The motion direction (Normalize)</summary>
    63.     public Vector3 motionDirection { get; private set; }
    64.  
    65.     /// <summary>The moving direction on local space</summary>
    66.     public Vector3 localMotionDirection { get; private set; }
    67.  
    68.     /// <summary>The distance moved from <see cref="lastPosition"/></summary>
    69.     public float motionDistance { get; private set; }
    70.  
    71.     /// <summary>The distance moved from <see cref="lastPosition"/>
    72.     /// Ignore Y - (local space), are based on <see cref="m_BiasGroundNormal"/>.</summary>
    73.     public float motionDistanceIgnoreY { get; private set; }
    74.  
    75.     /// <summary>The distance moved from <see cref="lastPosition"/>
    76.     /// Ignore Y - (local space), are based on <see cref="m_BiasGroundNormal"/>.</summary>
    77.     public float localMotionDistanceIgnoreY { get; private set; }
    78.  
    79.     /// <summary>The speed moved from <see cref="lastPosition"/> to current position.
    80.     /// calculate by engine <see cref="Time.deltaTime"/></summary>
    81.     public float motionSpeed { get; private set; }
    82.  
    83.     /// <summary>The speed project on <see cref="m_BiasGroundNormal"/> as floor,
    84.     /// and separate into horizontal direction (XZ)</summary>
    85.     public float localMotionSpeedXZ { get; private set; }
    86.  
    87.     /// <summary>The speed project on <see cref="m_BiasGroundNormal"/> itself
    88.     /// and extract the vertical direction (Y)</summary>
    89.     public float localMotionSpeedY { get; private set; }
    90.  
    91.     /// <summary>Velocity - The <see cref="motionSpeed"/> represent in vector</summary>
    92.     public Vector3 motionVelocity { get; private set; }
    93.  
    94.     /// <summary>The steeringRotation between <see cref="lastRotation"/> and current rotation,
    95.     /// <seealso cref="Quaternion.FromToRotation(Vector3, Vector3)"/></summary>
    96.     public Quaternion steeringRotation { get; private set; }
    97.  
    98.     /// <summary>The different between <see cref="lastForward"/> and current forward</summary>
    99.     public float steeringAngle { get; private set; }
    100.  
    101.     /// <summary>The angle between
    102.     /// moving direction <see cref="motionVector"/>
    103.     /// and actually facing direction <see cref="Transform.forward"/>
    104.     /// 0 = align forward, -90 = left, +90 = right, -180/180 = backward</summary>
    105.     public float angleBetweenMotionLookAt { get; private set; }
    106.  
    107.     /// <summary>0 = forward, Normalize -180~180 degree to -1f ~ 1f</summary>
    108.     public float angleBetweenMotionLookAtNormalize { get; private set; }
    109.  
    110.     private void OnEnable()
    111.     {
    112.         ResetParams();
    113.         DeltaCalculation();
    114.     }
    115.  
    116.     private void Update()
    117.     {
    118.         DeltaCalculation();
    119.     }
    120.  
    121.     private void LateUpdate()
    122.     {
    123.         // Final, update transform record
    124.         lastPosition = transform.position;
    125.         lastRotation = transform.rotation;
    126.     }
    127.  
    128.     System.Text.StringBuilder sb = new System.Text.StringBuilder(100);
    129.     private void OnDrawGizmos()
    130.     {
    131.         if (debugInfo.flag.HasFlag(eDebugFlag.motion))
    132.         {
    133.             sb.AppendLine($"Last position : {lastPosition:F1}");
    134.             sb.AppendLine($"Motion distance : {motionDistance:F2}");
    135.             if (motionVector != Vector3.zero)
    136.                 GizmosExtend.DrawDirection(transform.position, motionVector, color: debugInfo.colorMotionVector);
    137.         }
    138.  
    139.         if (debugInfo.flag.HasFlag(eDebugFlag.rotation))
    140.         {
    141.             sb.AppendLine($"Steering angle : {steeringAngle:F2}");
    142.             sb.AppendLine($"Angle between Motion & LookAt : {angleBetweenMotionLookAt:F1}");
    143.             if (motionVector != Vector3.zero)
    144.                 GizmosExtend.DrawAngleBetween(transform.position, motionVector, transform.forward, biasGroundNormal, 1f, debugInfo.colorMotionAngle);
    145.         }
    146.  
    147.         if (debugInfo.flag.HasFlag(eDebugFlag.speed))
    148.         {
    149.             sb.AppendLine($"Speed : {motionSpeed:F2}");
    150.             sb.AppendLine($"Local Speed XZ : {localMotionSpeedXZ:F1}, Y : {localMotionSpeedY:F1}");
    151.             if (motionVelocity != Vector3.zero)
    152.                 GizmosExtend.DrawDirection(transform.position, motionVelocity, color: debugInfo.colorMotionVelocity);
    153.         }
    154.  
    155.         if (sb.Length > 0)
    156.         {
    157.             GizmosExtend.DrawLabel(transform.position,
    158.                 sb.ToString(),
    159.                 offsetX: debugInfo.offset.x,
    160.                 offsetY: debugInfo.offset.y);
    161.             sb.Clear();
    162.         }
    163.     }
    164.  
    165.     private void DeltaCalculation()
    166.     {
    167.         if (m_CacheFrameCount == Time.frameCount || Time.deltaTime == 0f)
    168.             return;
    169.         m_CacheFrameCount = Time.frameCount;
    170.  
    171.         if (pauseOnTimeScaleZero && Time.timeScale == 0f)
    172.             return;
    173.  
    174.         // time
    175.         deltaTime = Time.deltaTime;
    176.  
    177.         // motion's
    178.         motionVector = transform.position - lastPosition;
    179.         localMotionVector = transform.InverseTransformVector(motionVector);
    180.  
    181.         // distance first, we have ZERO cases.
    182.         motionDistance = motionVector.magnitude; // Cost
    183.         if (motionDistance == 0f)
    184.         {
    185.             // didn't moved, in this case we can't calculate movement direction.
    186.             // Fallback : align to current forward.
    187.             motionDirection = localMotionDirection = transform.forward; // Gain
    188.             motionVectorIgnoreY = localMotionVectorIgnoreY = Vector3.zero; // Gain
    189.             motionDistanceIgnoreY = localMotionDistanceIgnoreY = 0f; // Gain
    190.         }
    191.         else
    192.         {
    193.             motionDirection = motionVector.normalized;
    194.             localMotionDirection = localMotionVector.normalized; // Gain
    195.             motionVectorIgnoreY = Vector3.ProjectOnPlane(motionVector, biasGroundNormal);
    196.             motionDistanceIgnoreY = motionVectorIgnoreY.magnitude;
    197.             localMotionVectorIgnoreY = new Vector3(localMotionVector.x, 0f, localMotionVector.z); // Gain - no calculation
    198.             localMotionDistanceIgnoreY = new Vector2(localMotionVector.x, localMotionVector.z).magnitude; // Gain - Vector2
    199.         }
    200.  
    201.         // Speed
    202.         motionSpeed = motionDistance / deltaTime;
    203.         motionVelocity = motionDirection * motionSpeed;
    204.         localMotionSpeedXZ = motionDistanceIgnoreY / deltaTime;
    205.         // math hack, since local's Y is actually moved distance along Y axis with correct sign.
    206.         // cost was paid on previous matrix convertion, now we gain some back.
    207.         /*
    208.         // full version
    209.         Vector3 y = Vector3.Project(motionVector, m_BiasGroundNormal);
    210.         localMotionSpeedY = y.magnitude / deltaTime * Vector3.Dot(y.normalized, biasGroundNormal);
    211.         */
    212.         localMotionSpeedY = localMotionVector.y / deltaTime; // Gain
    213.  
    214.         // Rotation
    215.         lastForward = lastRotation * Vector3.forward;
    216.         steeringAngle = Vector3.SignedAngle(lastForward, transform.forward, biasGroundNormal);
    217.         steeringRotation = Quaternion.FromToRotation(lastForward, transform.forward);
    218.         if (motionDirection == Vector3.zero)
    219.         {
    220.             angleBetweenMotionLookAtNormalize = angleBetweenMotionLookAt = 0f;
    221.         }
    222.         else
    223.         {
    224.             angleBetweenMotionLookAt = Vector3.SignedAngle(motionDirection, transform.forward, biasGroundNormal);
    225.             angleBetweenMotionLookAtNormalize = angleBetweenMotionLookAt / 180f;
    226.         }
    227.     }
    228.  
    229.     public void ResetParams()
    230.     {
    231.         m_BiasGroundNormal.Normalize();
    232.  
    233.         lastPosition = transform.position;
    234.         lastForward = transform.forward;
    235.         lastRotation = transform.rotation;
    236.         steeringRotation = Quaternion.identity;
    237.  
    238.         motionVector = localMotionVector = motionVectorIgnoreY = localMotionVectorIgnoreY = motionDirection = localMotionDirection = motionVelocity = Vector3.zero;
    239.      
    240.         motionDistance = motionDistanceIgnoreY = localMotionDistanceIgnoreY = motionSpeed = localMotionSpeedXZ = localMotionSpeedY =
    241.         steeringAngle = angleBetweenMotionLookAt = angleBetweenMotionLookAtNormalize = deltaTime = motionSpeed = 0f;
    242.     }
    243. }
     

    Attached Files:

    Last edited: Oct 11, 2019