use na::storage::Storage;
use na::{self, Point3, RealField, U6, Vector, Vector3, Vector6, Isometry3};
use std::mem;
use std::ops::{Add, AddAssign, Mul, Neg, Sub, SubAssign};
#[repr(C)]
#[derive(Copy, Clone, Debug)]
pub struct Force3<N: RealField> {
pub linear: Vector3<N>,
pub angular: Vector3<N>,
}
impl<N: RealField> Force3<N> {
#[inline]
pub fn new(linear: Vector3<N>, angular: Vector3<N>) -> Self {
Force3 { linear, angular }
}
#[inline]
pub fn zero() -> Self {
Self::new(na::zero(), na::zero())
}
#[inline]
pub fn from_slice(data: &[N]) -> Self {
Self::new(
Vector3::new(data[0], data[1], data[2]),
Vector3::new(data[3], data[4], data[5]),
)
}
#[inline]
pub fn from_vector<S: Storage<N, U6>>(data: &Vector<N, U6, S>) -> Self {
Self::new(
Vector3::new(data[0], data[1], data[2]),
Vector3::new(data[3], data[4], data[5]),
)
}
#[inline]
pub fn torque(torque: Vector3<N>) -> Self {
Self::new(na::zero(), torque)
}
#[inline]
pub fn torque_from_vector(torque: Vector3<N>) -> Self {
Self::new(na::zero(), torque)
}
#[inline]
pub fn torque_at_point(torque: Vector3<N>, point: &Point3<N>) -> Self {
Self::new(-torque.cross(&point.coords), torque)
}
#[inline]
pub fn torque_from_vector_at_point(torque: Vector3<N>, point: &Point3<N>) -> Self {
Self::torque_at_point(torque, point)
}
#[inline]
pub fn linear(linear: Vector3<N>) -> Self {
Self::new(linear, na::zero())
}
#[inline]
pub fn linear_at_point(linear: Vector3<N>, point: &Point3<N>) -> Self {
Self::new(linear, point.coords.cross(&linear))
}
#[inline]
pub fn angular_vector(&self) -> Vector3<N> {
self.angular
}
#[inline]
pub fn as_slice(&self) -> &[N] {
self.as_vector().as_slice()
}
#[inline]
pub fn transform_by(&self, m: &Isometry3<N>) -> Self {
Self::new(m * self.linear, m * self.angular)
}
#[inline]
pub fn as_vector(&self) -> &Vector6<N> {
unsafe { mem::transmute(self) }
}
#[inline]
pub fn as_vector_mut(&mut self) -> &mut Vector6<N> {
unsafe { mem::transmute(self) }
}
}
impl<N: RealField> Add<Force3<N>> for Force3<N> {
type Output = Self;
#[inline]
fn add(self, rhs: Self) -> Self {
Force3::new(self.linear + rhs.linear, self.angular + rhs.angular)
}
}
impl<N: RealField> AddAssign<Force3<N>> for Force3<N> {
#[inline]
fn add_assign(&mut self, rhs: Self) {
self.linear += rhs.linear;
self.angular += rhs.angular;
}
}
impl<N: RealField> Sub<Force3<N>> for Force3<N> {
type Output = Self;
#[inline]
fn sub(self, rhs: Self) -> Self {
Force3::new(self.linear - rhs.linear, self.angular - rhs.angular)
}
}
impl<N: RealField> SubAssign<Force3<N>> for Force3<N> {
#[inline]
fn sub_assign(&mut self, rhs: Self) {
self.linear -= rhs.linear;
self.angular -= rhs.angular;
}
}
impl<N: RealField> Mul<N> for Force3<N> {
type Output = Self;
#[inline]
fn mul(self, rhs: N) -> Self {
Force3::new(self.linear * rhs, self.angular * rhs)
}
}
impl<N: RealField> Neg for Force3<N> {
type Output = Self;
#[inline]
fn neg(self) -> Self {
Force3::new(-self.linear, -self.angular)
}
}