use cgmath::{Deg, InnerSpace, Point2, Vector2};
use nalgebra;
use rapier2d::dynamics::RigidBody;

pub fn rigidbody_position(r: &RigidBody) -> cgmath::Point2<f32> {
	Point2 {
		x: r.translation()[0],
		y: r.translation()[1],
	}
}

pub fn rigidbody_angle(r: &RigidBody) -> Deg<f32> {
	Vector2 {
		x: r.rotation().re,
		y: r.rotation().im,
	}
	.angle(Vector2 { x: 1.0, y: 0.0 })
	.into()
}

pub fn rigidbody_rotation(r: &RigidBody) -> Vector2<f32> {
	Vector2 {
		x: r.rotation().im,
		y: r.rotation().re,
	}
}

pub fn rigidbody_velocity(r: &RigidBody) -> cgmath::Vector2<f32> {
	let v = r.velocity_at_point(&nalgebra::Point2::new(
		r.translation()[0],
		r.translation()[1],
	));
	Vector2 { x: v.x, y: v.y }
}