#include "particle_link.hpp"

namespace physics {

  float particle_link::current_length() const
  {
    vec3 relative_position = particle[0]->position - particle[1]->position;

    return magnitude(relative_position);
  }

  int particle_cable::fill_contact(particle_contact * contact, int contact_length) const
  {
    float cur_length = current_length();
    if (cur_length < max_length)
      return 0;

    contact->particle[0] = particle[0];
    contact->particle[1] = particle[1];

    vec3 normal = normalize(particle[1]->position - particle[0]->position);
    contact->contact_normal = normal;
    contact->penetration = cur_length - max_length;
    contact->restitution = restitution;

    return 1;
  }

  int particle_rod::fill_contact(particle_contact * contact, int contact_length) const
  {
    float cur_length = current_length();
    if (cur_length == length)
      return 0;

    contact->particle[0] = particle[0];
    contact->particle[1] = particle[1];

    vec3 normal = normalize(particle[1]->position - particle[0]->position);

    if (cur_length > length) {
      contact->contact_normal = normal;
      contact->penetration = cur_length - length;
    } {
      contact->contact_normal = -normal;
      contact->penetration = length - cur_length;
    }

    contact->restitution = 0;

    return 1;
  }

  float particle_constraint::current_length() const
  {
    vec3 relative_position = particle->position - anchor;

    return magnitude(relative_position);
  }

  int particle_cable_constraint::fill_contact(particle_contact * contact, int contact_length) const
  {
    float cur_length = current_length();
    if (cur_length < max_length)
      return 0;

    contact->particle[0] = particle;
    contact->particle[1] = nullptr;

    vec3 normal = normalize(anchor - particle->position);
    contact->contact_normal = normal;
    contact->penetration = cur_length - max_length;
    contact->restitution = restitution;

    return 1;
  }
}
