using UnityEngine;
using System.Collections;

public class DebugPointVelocity : MonoBehaviour {
    private Physics2DEx _rb;
    public Vector3 offset = Vector3.up;
	// Use this for initialization
	void Start () {
        _rb = GetComponent<Physics2DEx>();
	}
	
	// Update is called once per frame
	void Update () {
	
	}

    void OnDrawGizmos() {
        if (_rb == null) {
            _rb = GetComponent<Physics2DEx>();
        }

        if (_rb != null) {
            var worldPoint = transform.localToWorldMatrix.MultiplyPoint(offset);
            var velocity = _rb.GetWorldPointVelocity(worldPoint);
            Gizmos.DrawLine(worldPoint, (Vector3)velocity + worldPoint);
            AdvancedGizmo.DrawCircle(worldPoint, Vector3.forward, 0.1f, Color.red);

            var r = GetComponent<Rigidbody2D>();
            var v = r.GetPointVelocity(worldPoint);

            Gizmos.color = Color.blue;
            Gizmos.DrawLine(worldPoint, (Vector3) v + worldPoint);

            //Debug.LogFormat("Unity: {0},  Custom: {1}, Angular velocity: {2}", v.magnitude, velocity.magnitude, _rb.AngularVelocity);
        }
      
    }
}