cyclone2d 0.1.2

use crate::particle::Particle;
use crate::vek2d::Vec2d;
use crate::FP;
use std::ptr;

#[derive(Copy, Clone)]
pub struct Contact {
    pub first:          ptr::NonNull<Particle>,
    pub second:         Option<ptr::NonNull<Particle>>,
    pub first_move:     Vec2d<FP>,
    pub second_move:    Vec2d<FP>,
    pub restitution:    FP,
    pub contact_normal: Vec2d<FP>,
    pub penetration:    FP,
}

impl Default for Contact {
    fn default() -> Contact {
        Contact {
            first:          ptr::NonNull::dangling(),
            second:         None,
            first_move:     Vec2d::default(),
            second_move:    Vec2d::default(),
            restitution:    0.0,
            contact_normal: Vec2d::default(),
            penetration:    0.0,
        }
    }
}

/// The generator itself can have any kind of structure, but
/// must implement this trait to be able to add any found contacts
/// to the contact list. This method is called on each generator in
/// turn per physics 'tick'
pub trait ContactGenerator {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        particles: &mut Vec<Option<Particle>>,
        limit: u32,
    ) -> u32;
    fn contains_ptr(&self, ptr: usize) -> bool;
}

/// Simulates a basic ground surface constraint
pub struct GroundContact {}

impl GroundContact {
    pub fn new() -> GroundContact {
        GroundContact {}
    }
}

impl ContactGenerator for GroundContact {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        particles: &mut Vec<Option<Particle>>,
        limit: u32,
    ) -> u32 {
        let mut index = 0;
        let mut count = 0;
        for maybe in particles.iter_mut() {
            if let Some(particle) = maybe {
                let y = particle.position().y();
                if y < 0.0 {
                    contacts[index].contact_normal = Vec2d::new(0.0, 1.0);
                    contacts[index].first = particle.as_non_null_ptr();
                    contacts[index].second = None;
                    contacts[index].penetration = -y;
                    contacts[index].restitution = 0.3;
                    count += 1;
                    index += 1;
                }
                if count >= limit {
                    return count;
                }
            }
        }
        count
    }

    fn contains_ptr(&self, _id: usize) -> bool {
        false
    }
}

/// Simulates a containment
pub struct AreaContact {
    left:   FP,
    right:  FP,
    top:    FP,
    bottom: FP,
}

impl AreaContact {
    pub fn new(min: Vec2d<FP>, max: Vec2d<FP>) -> AreaContact {
        AreaContact {
            left:   min.x(),
            right:  max.x(),
            top:    max.y(),
            bottom: min.y(),
        }
    }
}

impl ContactGenerator for AreaContact {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        particles: &mut Vec<Option<Particle>>,
        limit: u32,
    ) -> u32 {
        let mut index = 0;
        let mut count = 0;
        for maybe in particles.iter_mut() {
            if let Some(particle) = maybe {
                let pos = particle.position();
                let mut contact_made = false;
                if pos.x() > self.right {
                    contacts[index].contact_normal = Vec2d::new(-1.0, 0.0);
                    contacts[index].penetration = -pos.x();
                    contact_made = true;
                } else if pos.x() < self.left {
                    contacts[index].contact_normal = Vec2d::new(1.0, 0.0);
                    contacts[index].penetration = -pos.x();
                    contact_made = true;
                } else if pos.y() < self.bottom {
                    contacts[index].contact_normal = Vec2d::new(0.0, 1.0);
                    contacts[index].penetration = -pos.y();
                    contact_made = true;
                } else if pos.y() > self.top {
                    contacts[index].contact_normal = Vec2d::new(0.0, -1.0);
                    contacts[index].penetration = -pos.y();
                    contact_made = true;
                }

                if contact_made {
                    contacts[index].first = particle.as_non_null_ptr();
                    contacts[index].second = None;
                    contacts[index].restitution = 0.3;
                    count += 1;
                    index += 1;
                }
                if count >= limit {
                    return count;
                }
            }
        }
        count
    }

    fn contains_ptr(&self, _id: usize) -> bool {
        false
    }
}

/// Simulates a cable which is slack until it reaches its maximum length.
/// The cable joins two particles together.
pub struct Cable {
    particles:   [ptr::NonNull<Particle>; 2],
    max_length:  FP,
    restitution: FP,
}

impl Cable {
    pub fn new(
        p1: ptr::NonNull<Particle>,
        p2: ptr::NonNull<Particle>,
        len: FP,
        restitution: FP,
    ) -> Cable {
        Cable {
            particles: [p1, p2],
            max_length: len,
            restitution,
        }
    }

    fn get_current_len(&self) -> FP {
        let p0 = unsafe { self.particles[0].as_ref() };
        let p1 = unsafe { self.particles[1].as_ref() };
        (p0.position() - p1.position()).magnitude()
    }
}

impl ContactGenerator for Cable {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        _: &mut Vec<Option<Particle>>,
        _limit: u32,
    ) -> u32 {
        let length = self.get_current_len();
        if length < self.max_length {
            return 0;
        }

        let mut contact = &mut contacts[0];

        let p0 = unsafe { self.particles[0].as_ref() };
        let p1 = unsafe { self.particles[1].as_ref() };

        contact.first = self.particles[0];
        contact.second = Some(self.particles[1]);
        contact.contact_normal = (p1.position() - p0.position()).normalise();
        contact.penetration = length - self.max_length;
        contact.restitution = self.restitution;

        1
    }
    fn contains_ptr(&self, ptr: usize) -> bool {
        for p in self.particles.iter() {
            if p.as_ptr() as usize == ptr {
                return true;
            }
        }
        false
    }
}

/// Simulates a cable which is slack until it reaches its maximum length.
/// This version can anchor to any point specified by a `Vec2d`.
pub struct CableConstraint {
    particle:    ptr::NonNull<Particle>,
    anchor:      Vec2d<FP>,
    max_length:  FP,
    restitution: FP,
}

impl CableConstraint {
    pub fn new(
        particle: ptr::NonNull<Particle>,
        anchor: Vec2d<FP>,
        len: FP,
        restitution: FP,
    ) -> CableConstraint {
        CableConstraint {
            particle,
            anchor,
            max_length: len,
            restitution,
        }
    }

    fn get_current_len(&self) -> FP {
        let p = unsafe { self.particle.as_ref() };
        (p.position() - self.anchor).magnitude()
    }
}

impl ContactGenerator for CableConstraint {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        _: &mut Vec<Option<Particle>>,
        _limit: u32,
    ) -> u32 {
        let length = self.get_current_len();
        if length < self.max_length {
            return 0;
        }

        let mut contact = &mut contacts[0];

        let p = unsafe { self.particle.as_ref() };

        contact.first = self.particle;
        contact.second = None;
        contact.contact_normal = (self.anchor - p.position()).normalise();
        contact.penetration = length - self.max_length;
        contact.restitution = self.restitution;

        1
    }
    fn contains_ptr(&self, ptr: usize) -> bool {
        self.particle.as_ptr() as usize == ptr
    }
}

/// A rod simulates a hard constraint where the distance of two particles
/// can not be above or below the length specified.
pub struct Rod {
    particles:   [ptr::NonNull<Particle>; 2],
    length:      FP,
    restitution: FP,
}

impl Rod {
    pub fn new(
        p1: ptr::NonNull<Particle>,
        p2: ptr::NonNull<Particle>,
        length: FP,
        restitution: FP,
    ) -> Rod {
        Rod {
            particles: [p1, p2],
            length,
            restitution,
        }
    }

    fn get_current_len(&self) -> FP {
        let p0 = unsafe { self.particles[0].as_ref() };
        let p1 = unsafe { self.particles[1].as_ref() };
        (p0.position() - p1.position()).magnitude()
    }
}

impl ContactGenerator for Rod {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        _: &mut Vec<Option<Particle>>,
        _limit: u32,
    ) -> u32 {
        let current_length = self.get_current_len();
        if (current_length - self.length).abs() < 0.001 {
            return 0;
        }

        let mut contact = &mut contacts[0];
        contact.first = self.particles[0];
        contact.second = Some(self.particles[1]);
        contact.restitution = self.restitution;

        let p0 = unsafe { self.particles[0].as_ref() };
        let p1 = unsafe { self.particles[1].as_ref() };

        let normal = (p1.position() - p0.position()).normalise();

        if current_length > self.length {
            contact.contact_normal = normal;
            contact.penetration = current_length - self.length;
        } else {
            contact.contact_normal = normal * -1.0;
            contact.penetration = self.length - current_length;
        }
        1
    }
    fn contains_ptr(&self, ptr: usize) -> bool {
        for p in self.particles.iter() {
            if p.as_ptr() as usize == ptr {
                return true;
            }
        }
        false
    }
}

/// A rod simulates a hard constraint where the distance of two particles
/// can not be above or below the length specified. This version may be
/// anchored to any point specified by a `Vec2d`
pub struct RodConstraint {
    particle:    ptr::NonNull<Particle>,
    anchor:      Vec2d<FP>,
    length:      FP,
    restitution: FP,
}

impl RodConstraint {
    pub fn new(
        particle: ptr::NonNull<Particle>,
        anchor: Vec2d<FP>,
        length: FP,
        restitution: FP,
    ) -> RodConstraint {
        RodConstraint {
            particle,
            anchor,
            length,
            restitution,
        }
    }

    fn get_current_len(&self) -> FP {
        let p = unsafe { self.particle.as_ref() };
        (p.position() - self.anchor).magnitude()
    }
}

impl ContactGenerator for RodConstraint {
    fn add_contact(
        &self,
        contacts: &mut [Contact],
        _: &mut Vec<Option<Particle>>,
        _limit: u32,
    ) -> u32 {
        let current_length = self.get_current_len();
        if (current_length - self.length).abs() < 0.001 {
            return 0;
        }

        let mut contact = &mut contacts[0];
        contact.first = self.particle;
        contact.second = None;
        contact.restitution = self.restitution;

        let p = unsafe { self.particle.as_ref() };
        let normal = (self.anchor - p.position()).normalise();

        if current_length > self.length {
            contact.contact_normal = normal;
            contact.penetration = current_length - self.length;
        } else {
            contact.contact_normal = normal * -1.0;
            contact.penetration = self.length - current_length;
        }
        1
    }
    fn contains_ptr(&self, ptr: usize) -> bool {
        self.particle.as_ptr() as usize == ptr
    }
}