using UnityEngine;
using System.Collections;

public static class Physics2DExtensions {
    public static Vector2 GetWorldPointVelocityEx(this Rigidbody2D body, Vector2 worldPoint) {
        var physics = body.GetComponent<Physics2DEx>();
        if (physics != null) {
            return physics.GetWorldPointVelocity(worldPoint);
        }
        return body.GetPointVelocity(worldPoint);
    }
}

[RequireComponent(typeof(Rigidbody2D))]
public class Physics2DEx : MonoBehaviour {
    private Vector2 _velocity;
    private float _angularVelocity;
    public float AngularVelocity {
        get { return _angularVelocity; }
    }
    private Rigidbody2D _rigidBody;

	// Use this for initialization
	void Start () {
	    _rigidBody = GetComponent<Rigidbody2D>();
	}

    void FixedUpdate() {
        _velocity = _rigidBody.velocity;
        _angularVelocity = _rigidBody.angularVelocity;
    }

    public Vector2 GetWorldPointVelocity(Vector2 worldPoint) {
        if (_rigidBody == null) {
            _rigidBody = GetComponent<Rigidbody2D>();
            if (_rigidBody == null) {
                Debug.LogErrorFormat(gameObject, "Physics2DEx requires component RigidBody2D! {0}", name);
                return Vector2.zero;
            }
        }
       
        var radiusVector = worldPoint - _rigidBody.worldCenterOfMass;
        return (Vector3)_velocity + Vector3.Cross(Vector3.forward * (_angularVelocity * Mathf.Deg2Rad), (Vector3)radiusVector);
    }

	// Update is called once per frame
	void Update () {
	
	}
}