Koodit: C++: Törmäysfysiikkaa

#include "object.h"

bool get_collision_normal(vector_xy &, double &, vector_xy,
                          vector_xy, vector_xy *, int);

/* Funktio selvittää kappaleen a kärkien sijainnin kappaleen b reunoihin
   nähden ja muuttaa tarvittaessa kappaleiden nopeudet. Tarkistus täytyy
   siis tehdä kahteen kertaan (a-b, b-a), mikäli törmäystä ei ensimmäisellä
   kerralla havaita. */
bool collide_objects(object *a, object *b) {
    double impulse_magnitude, // impulssin voimakkuus
           relative_velocity, /* törmäävän kärjen ja reunan välisen nopeuden
                                 törmäyspinnan normaalin suuntainen komponentti
                              */
           collision_depth,   // törmäyksen arvioitu "syvyys"
           backface_distance;

    vector_xy impulse, // impulssi
              distance, // kappaleiden painopisteiden välinen etäisyys
              vertex_position, // a:n mahdollisesti törmäävän kärjen paikka
              vertex_speed, // kärjen ratanopeus suhteessa törmäyspintaan
              torque_length, // kärjen etäisyys a:n painopisteestä
              edge_torque_length, // törmäyspinnan etäisyys b:n painopisteestä
              collision_normal, // törmäyspinnan normaali, impulssin suunta
              backface_normal;

    distance = a->position - b->position;
    // kappaleet ovat liian kaukana toisistaan, ei törmäystä
    if(distance.square()>(a->size+b->size)*(a->size+b->size)) return false;

    // tarkistetaan a:n jokaisen kärjen paikka
    for(int i=0; i<a->vertex_amount; i++) {
      torque_length = a->global_vertices[i] - a->position;
      vertex_position = a->global_vertices[i];
      /* Jos a:n kärki i on juuri törmännyt monikulmion b sivuun, se on
         (ideaalitilanteessa) hyvin lähellä törmäyspintaa. Näin ollen
         mahdollisen törmäyspinnan ratanopeus voidaan laskea suoraan kärjen
         ja b:n painopisteen välistä etäisyyttä hyödyntäen - tietämättä
         edes, mihin b:n sivuun a:n kärki on törmännyt.
      */
      edge_torque_length = vertex_position - b->position;
      // lasketaan kärjen ja mahdollisen törmäyspinnan välinen nopeus
      vertex_speed = a->speed + scalar_cross(torque_length, a->rotation) -
                     b->speed - scalar_cross(edge_torque_length, b->rotation);

      /* Selvitetään, onko kärki i joutunut monikulmion b sisään, ja jos on,
         niin minkä sivujanan se on matkallaan leikannut */
      if(get_collision_normal(collision_normal, collision_depth,
                              vertex_position, -vertex_speed,
                              b->global_vertices, b->vertex_amount)) {
       get_collision_normal(backface_normal, backface_distance,
                            vertex_position, vertex_speed,
                            b->global_vertices, b->vertex_amount);
       /* Jos monikulmion takaseinä on lähempänä kuin havaittu törmäyspinta,
          on kärki (luultavasti) jo matkalla ulos monikulmiosta, ja törmäys
          on näinollen jo tapahtunut.
       */
       if(backface_distance < collision_depth) continue;

       /* lasketaan törmäävien pisteiden välisen nopeuden
          törmäyspinnan normaalin suuntainen komponentti */
       relative_velocity = dot(collision_normal, vertex_speed);

       /* lasketaan impulssin voimakkuus oheisella kaavalla
         (löytyy osoitteesta http://www.cs.unc.edu/~ehmann/RigidTutorial/)

                                 - (1 + e) v
       I =   _______________________________r___________________________
                          _0    _   _0    _        _0    _   _0    _
            1/m  + 1/m  + n * ((r x n ) x r )/J  + n * ((r x n ) x r )/J
               a      b          a         a   a          b         b   b

          missä:
           e  = kimmokerroin (tässä laskettu a:n ja b:n kimmokerrointen tulona)
           vr = ks. "relative_velocity"
           ma = kappaleen a massa
           mb = b:n massa
           n  = törmäyspinnan normaali
           ra = törmäyspisteen etäisyys a:n painopisteeseen
           rb = vastaava vektori kappaleelle b
           Ja = kappaleen a hitausmomentti
           Jb = kappaleen b hitausmomentti

           /  = jakolasku
           *  = vektorien pistetulo
           x  = vektorien ristitulo
           0  = vektorin normalisointi
           _  = vektorimerkki

       */
       impulse_magnitude = -(1.0 + a->e*b->e)*relative_velocity /
         ( 1.0 / a->mass + 1.0 / b->mass
         + dot(
            double_cross(torque_length, collision_normal),
            collision_normal) / a->inertia
         + dot(
            double_cross(edge_torque_length, collision_normal),
            collision_normal) / b->inertia
        );

        impulse = collision_normal * impulse_magnitude;

        // päivitetään kappaleiden nopeudet ja kulmanopeudet
        a->speed += impulse * (1.0 / a->mass);
        b->speed -= impulse * (1.0 / b->mass);
        a->rotation += cross_length(torque_length, impulse) / a->inertia;
        b->rotation -= cross_length(edge_torque_length, impulse) / b->inertia;
        return true;
       }
    }
    return false;
}

/* Selvitetään, onko piste p joutunut monikulmion pg sisään, ja jos on,
   niin minkä sivujanan se on matkallaan leikannut */
bool get_collision_normal(vector_xy &normal, // törmäyspinnan normaali
                          double &distance, // törmäyksen "syvyys"
                          vector_xy p, // mahdollisesti törmäävä kärki
                          vector_xy r, // törmäyksen suunta
                          vector_xy *pg, // mahdollisesti törmäävä monikulmio
                          int n // monikulmion kulmien määrä
                          ) {

/*
    Sovelletaan "even-odd" -sääntöä:
      "Piste on suljetun monikulmion sisällä, joss pisteestä lähtevä
       mielivaltainen puolisuora leikkaa parittoman määrän monikulmion
       sivujanoja."

    Valitaan puolisuoran suunnaksi pisteen p "tulosuunta", r,
    ja etsitään lähin sivujana, jonka puolisuora leikkaa. Tämä
    on todennäköisesti sivujana, johon kärki p on törmännyt.
*/

      int intersections = 0;
      double least_distance, d, edge_length;
      vector_xy vertex, edge;

      for(int i=0; i<n; i++) {
        vertex=pg[i];
        if(i!=n-1) edge = pg[i+1] - pg[i];
        else edge = pg[0] - pg[i];

        if(edge.x==0.0) {
          if(r.x==0.0) continue;
          distance = (vertex.x - p.x) / r.x;
        } else {
          d = r.y - edge.y/edge.x*r.x;
          if(d==0.0) continue;
          distance = (vertex.y - p.y + edge.y/edge.x*(p.x - vertex.x)) / d;
        }

        edge_length = dot(r*distance+p-vertex, edge);
        if(distance<0.0 || edge_length >= edge.square() || edge_length < 0.0)
          continue;

        if(!intersections++ || distance<least_distance) {
          least_distance = distance;
          normal = edge;
        }
      }

      // jos leikkausten määrä on parillinen, piste ei ole monikulmion sisällä
      if(!(intersections & 0x1)) return false;

      // tallennetaan törmäyspinnan normaali ja törmäyksen arvioitu syvyys
      if(dot(r,edge)<0.0) normal = scalar_cross(normal, -1);
      else normal = scalar_cross(normal, 1);
      normal.normalize();
      distance = least_distance;
      return true;
}

#include "vector.h"
#define I_RES 1000

bool is_inside_polygon(vector_xy, vector_xy *, int);

/* Luokka yhtenäisille, suljetuille, kaksiulotteisille kulmikkaille
   kappaleille. Kärjet yhdistetään siinä järjestyksessä, kun ne syötetään,
   siten että viimeinen kärki yhdistetään vielä ensimmäiseen.
   Monikulmion ei siis tarvitse olla konveksi. */
class object {
  public:
  // Muuttumattomat ominaisuudet
    int vertex_amount; // kärkien määrä
    vector_xy *local_vertices; // kärkien sijainnit painopisteeseen nähden
    double mass,        // massa
           inertia,     // hitausmomentti
           size,        // suurin mahdollinen säde
           e;           // kimmokerroin

  // Muuttuvat ominaisuudet (päivitetään kerran aikayksikössä)
    double angle,       // vaihekulma (rad)
           rotation;    // kulmanopeus
    vector_xy position; // kappaleen (painopisteen) sijainti
    vector_xy speed;    // nopuesvektori
    vector_xy *global_vertices; // kärkien paikat globaalissa koordinaatistossa

    object(vector_xy *vertices, int number_of_vertices,
            double m, double cr, vector_xy pos, vector_xy sp,
            double rot, double a) {
          mass = m;
          position = pos;
          speed = sp;
          rotation = rot;
          angle = a;
          e = cr;
          vertex_amount = number_of_vertices;
          local_vertices = vertices;
          global_vertices = new vector_xy[vertex_amount];
          calculate_inertia();
    }

    ~object() { delete [] global_vertices; delete [] local_vertices; }

    void move() { // kutsutaan kerran aikayksikössä (framessa)
      position += speed;
      angle += rotation;

      if(angle>2.0*PI) angle-=2.0*PI;
      if(angle<0.0) angle+=2.0*PI;

      vector_xy v(cos(angle), sin(angle));

      // lasketaan kärkien paikat globaalissa koordinaatistossa
      for(int i=0; i<vertex_amount; i++)
        global_vertices[i] =
          vector_transformation(local_vertices[i], v) + position;
    }

    void calculate_inertia() {
    /* Selvitetään painopisteen sijainti ja hitausmomentti numeerisesti
       integroimalla ja kiinnitetään painopiste koordinaatiston origoon
       muuntamalla kärkien koordinaatteja

       Syötettyjen koordinaattien (local_vertices) on oltava positiivisia
    */
      char *buffer = new char[I_RES*I_RES];
      double max_size = local_vertices[0].x;

      for(int i=0; i<vertex_amount; i++) {
        if(local_vertices[i].x>max_size) max_size = local_vertices[i].x;
        if(local_vertices[i].y>max_size) max_size = local_vertices[i].y;
      }

      int area = 0, x_sum = 0, y_sum = 0;
      double d = (double)max_size / (double)I_RES, // "mittakaava"
             com_x, com_y, // painopisteen koordinaatit
             r; // kappaleen koko, kaukaisimman kärjen etäisyys painopisteestä

      inertia = 0.0; // hitausmomentti

      // piirretään monikulmio bittikartaksi puskuriin, ...
      for(int y=0; y<I_RES; y++)
      for(int x=0; x<I_RES; x++)
       if(is_inside_polygon(_vector_xy((double)x*d,(double)y*d),
                            local_vertices, vertex_amount)) {
          buffer[y*I_RES+x] = true;
          area++;
          x_sum += x;
          y_sum += y;
       } else buffer[y*I_RES+x] = 0;

      // ... lasketaan painopisteen koordinaatit ...
      com_x = (double)x_sum * d / (double)area;
      com_y = (double)y_sum * d / (double)area;

      // ... sekä kappaleen hitausmomentti ...
      for(int y=0; y<I_RES; y++)
      for(int x=0; x<I_RES; x++)
        if(buffer[y*I_RES+x]) {
          r = (x*d-com_x)*(x*d-com_x)+(y*d-com_y)*(y*d-com_y);
          if(r>size) size=r; // ... ja mitat
          inertia += r;
        }

      // siirretään kärkiä siten, että painopiste on origossa
      for(int i=0; i<vertex_amount; i++)
        local_vertices[i] -= _vector_xy(com_x, com_y);

      // hitausmomentti, J = sum(x² + y²) / A * m
      inertia *= mass / (double)area;
      size = sqrt(size);
      delete [] buffer;
    }
};

// Selvittää, onko piste monikulmion sisällä soveltaen "even-odd" -sääntöä
bool is_inside_polygon(vector_xy p, vector_xy *vertices, int n) {
      unsigned intersections = 0;
      double v1x, v1y, v2x, v2y;
      for(int i=0; i<n; i++) {
        v1x = vertices[i].x; v1y = vertices[i].y;
        if(i!=n-1) { v2x = vertices[i+1].x; v2y = vertices[i+1].y; }
        else { v2x = vertices[0].x; v2y = vertices[0].y; }

        if((v1x < p.x && v2x < p.x) ||
           (v1y > p.y && v2y > p.y) ||
           (v1y < p.y && v2y < p.y)) continue;
        if((v1x>=p.x && v2x>=p.x) || (v1y==v2y)) { intersections++; continue; }
        if(  -(v1y-p.y)*(v2x-v1x)/(v2y-v1y)+v1x >= p.x) intersections++;
      }
      return (intersections & 0x1);
}

#include <math.h>
#define PI 3.1415926535897932384626433832795

struct vector_xy
{
    double x;
    double y;

  vector_xy() {}
  vector_xy(double x2, double y2) { x = x2; y = y2; }
  void set(double x2, double y2) { x = x2; y = y2; }

  void operator=(vector_xy v) { x = v.x; y = v.y; }
  void operator+=(vector_xy v) { x += v.x; y += v.y; }
  void operator-=(vector_xy v) { x -= v.x; y -= v.y; }
  void operator*=(double r) { x *= r; y *= r; }

  vector_xy operator+(vector_xy v) { vector_xy p(x+v.x,y+v.y); return p; }
  vector_xy operator-(vector_xy v) { vector_xy p(x-v.x,y-v.y); return p; }
  vector_xy operator*(double a) { vector_xy p(x*a,y*a); return p; }
  vector_xy operator-() { vector_xy p(-x,-y); return p; }

  double length() { return sqrt(x*x+y*y); }
  double square() { return x*x+y*y; }
  void normalize() { double l = length(); if (l) { x /= l; y /= l; }}

};

// pistetulo
double dot(vector_xy a, vector_xy b) { return a.x*b.x + a.y*b.y; }

// | a x b |
double cross_length(vector_xy a, vector_xy b) {
  return a.x*b.y - a.y*b.x;
}

vector_xy _vector_xy(double x, double y) {
    vector_xy v(x,y);
    return v;
}

// v:n ja z-akselin suuntaisen s-pituisen vektorin ristitulo
vector_xy scalar_cross(vector_xy v, double s) {
    vector_xy u(-v.y*s, v.x*s);
    return u;
}

// a x b x a
vector_xy double_cross(vector_xy a, vector_xy b) {
    vector_xy v(a.y*a.y*b.x - a.x*a.y*b.y, a.x*a.x*b.y - a.x*a.y*b.x);
    return v;
}

// kerrotaan vektori v vektoria t vastaavalla kompleksiluvulla (t.x + t.y*i)
vector_xy vector_transformation(vector_xy v, vector_xy t) {
    vector_xy r(v.x*t.x - v.y*t.y, v.x*t.y + t.x*v.y);
    return r;
}