Suomipelit – Fysiikkaa peliohjelmoijille - Osa 3: Törmäykset

1. Törmäystarkistus #

Voidaksemme reagoida törmäykseen, pitää se ensin havaita. Tätä varten on olemassa törmäystarkistus. Törmäystarkistus on puhtaasti geometrinen ongelma eikä fysikaalinen. Vasta, kun törmäystarkistus havaitsee törmäyksen, astuu fysiikka mukaan kuvaan törmäysvasteen muodossa.

Jotta törmäysvaste voitaisiin laskea, pitää törmäyksestä tietää seuraavat asiat: ajanhetki, kaikki kosketuspisteet ja pinnan normaalivektori kussakin kosketuspisteessä. Kuinka nämä asiat sitten lasketaan, se riippuu kappeleiden muodosta. Koska tässä artikkelisarjassa käsittelemme vain palloja ja laatikoita (ympyröitä ja suorakulmioita 2D-tapauksessa), esittelen törmäystarkistusrutiinit näille kappaleille. Törmäystarkistus voidaan tehdä kahdella eri tavalla: joko jatkuvana tai jaksottaisena.

Jatkuva törmäystarkastus kohtelee liikettä jatkumona ja pyrkii löytämään kosketuspisteet ja etenkin törmäyshetken täydellisen tarkasti. Tämä on mahdollista, jos kappaleet ovat yksinkertaisia, kuten palloja, mutta käy erittäin hankalaksi, jopa mahdottomaksi, vähänkin monimutkaisemmilla kappaleilla kuten laatikoilla. Tämän takia käytän tässä artikkelissa jaksottaista törmäystarkistusta.

Vasemmalla ideaalitapaus, jossa kappaleet vain koskettavat. Keskellä tyypillinen tapaus, jossa kappaleet pääsevät virheellisesti sisäkkäin. Oikealla tämä on estetty käyttämällä vaippoja, jotka leikkaavat vaikka kappaleet eivät sitä vielä tee.

typedef struct
{
  RIGIDBODY2D *body1;  // Kosketuksen ensimmäinen osapuoli
  RIGIDBODY2D *body2;  // Kosketuksen toinen osapuoli
  float p1[2];         // Ensimmäisen kappaleen kosketuspiste
  float p2[2];         // Toisen kappaleen kosketuspiste
  float normal[2];     // Kosketuspinnan normaali
  float ap[2];         // Kosketuspisteiden keskiarvo
} CONTACTPOINT;

typedef struct
{
  RIGIDBODY3D *body1;  // Kosketuksen ensimmäinen osapuoli
  RIGIDBODY3D *body2;  // Kosketuksen toinen osapuoli
  float p1[3];         // Ensimmäisen kappaleen kosketuspiste
  float p2[3];         // Toisen kappaleen kosketuspiste
  float normal[3];     // Kosketuspinnan normaali
  float ap[3];         // Kosketuspisteiden keskiarvo
} CONTACTPOINT;

1.1 Pallo - Pallo #

Kahden pallon (tai 2D-tapauksessa ympyrän) välisen leikkauksen havaitseminen on helppoa, joten käsittelen sen ensimmäiseksi. Kaksi palloa tai ympyrää leikkaa toisensa, jos niiden keskipisteiden välinen etäisyys on pienempi tai yhtä suuri, kuin niiden säteiden summa. Siinä se. Kosketuspisteitä on tasan yksi (kummallakin) ja pinnan normaali kosketuspisteessä on pallojen keskipisteiden kautta kulkevan suoran suuntainen.

Seuraavassa on ympyröiden leikkausfunktio koodina. Se ottaa parametrinaan ympyröiden keskipisteiden sijainnit p1 ja p2 sekä säteet r1 ja r2. Funktio palauttaa kosketuspisteet muuttujiin i1 ja i2, sekä kosketuspisteen normaalin muuttujaan normal. Funktio palauttaa TRUE, jos ympyrät leikkaavat, ja FALSE jos eivät (tällöin muuttujien i1, i2 ja normal sisältö jää määrittelemättömäksi).

BOOL circleCircleIntersection(const float p1[2], float r1,
                              const float p2[2], float r2,
                              float i1[2], float i2[2],
                              float normal[2])
{
  float d;

  // Tee vektori toisesta ympyrästä toiseen
  makeVector(normal, p2, p1);  

  // Laske vektorin pituus normalisoiden se samalla
  d=normalizeVector(normal);

  // Tarkista leikkaavatko ympyrät
  if (d>r1+r2) return FALSE;

  // Tarkista leikkaavatko ympyrät liikaa
  if (d<r1 || d<r2) return FALSE;

  // Laske leikkauspisteet
  i1[0]=p1[0]-r1*normal[0];
  i1[1]=p1[1]-r1*normal[1];

  i2[0]=p2[0]+r2*normal[0];
  i2[1]=p2[1]+r2*normal[1];

  return TRUE;
}

BOOL sphereSphereIntersection(const float p1[3], float r1,
                              const float p2[3], float r2,
                              float i1[3], float i2[3],
                              float normal[3])
{
  float d;

  // Tee vektori toisesta pallosta toiseen
  makeVector(normal, p2, p1);  

  // Laske vektorin pituus normalisoiden se samalla
  d=normalizeVector(normal);

  // Tarkista leikkaavatko pallot
  if (d>r1+r2) return FALSE;

  // Tarkista leikkaavatko pallot liikaa
  if (d<r1 || d<r2) return FALSE;

  // Laske leikkauspisteet
  i1[0]=p1[0]-r1*normal[0];
  i1[1]=p1[1]-r1*normal[1];
  i1[2]=p1[2]-r1*normal[2];

  i2[0]=p2[0]+r2*normal[0];
  i2[1]=p2[1]+r2*normal[1];
  i2[2]=p2[2]+r2*normal[2];

  return TRUE;
}

1.2 Pallo - Laatikko #

Pallon ja laatikon välinen leikkaus on jo hankalampi, mutta periaate on sama kuin pallo-pallo tapauksessa. Pitää etsiä laatikolta palloa lähin piste ja tutkia sen etäisyys pallon keskipisteestä. Jos tämä etäisyys on pienempi tai yhtä suuri kuin pallon säde, kappaleet leikkaavat, muuten eivät.

Testi olisi valtavan paljon helpompi, jos laatikko olisi suorassa koordinaattiakseleihin nähden ja sen keskipiste origossa. Niinpä ennen testiä kierrämme ja siirrämme koko systeemia niin, että pääsemme tällaiseen tilanteeseen. Teemme testin tässä helpommassa ympäristössä ja kierrämme ja siirrämme saadut tulokset tapaisin alkuperäiseen koordinaatistoon. Jos et tiedä kuinka koordinaatistomuunnos tehdään, lue artikkeli: ”Matriisimatematiikkaa peliohjelmoijille”.

Etäisyyden laskeminen ”vinoon” laatikkoon on hankalaa (vasemmalla). Kierretään ja siirretään systeemiä niin, että laatikko tulee ”suoraan” ja origoon. Lasketaan etäisyys tässä systeemissä (keskellä). Lopputulos on sama kuin, jos etäisyys olisi laskettu alkutilanteessa (oikealla).

void extractXAxisFromAngle(float x[2], float a)
{
  x[0]=cos(a);
  x[1]=sin(a);
}

void extractYAxisFromAngle(float y[2], float a)
{
  y[0]=-sin(a);
  y[1]=cos(a);
}

void extractXAxisFromQuaternion(float v[3], const float q[4])
{
  v[0]= q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
  v[1]= 2*q[1]*q[2] + 2*q[0]*q[3];
  v[2]=-2*q[0]*q[2] + 2*q[1]*q[3];
}

void extractYAxisFromQuaternion(float v[3], const float q[4])
{
  v[0]=2*q[1]*q[2] - 2*q[0]*q[3];
  v[1]=q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
  v[2]=2*q[0]*q[1] + 2*q[2]*q[3];
}

void extractZAxisFromQuaternion(float v[3], const float q[4])
{
  v[0]=2*q[0]*q[2] + 2*q[1]*q[3];
  v[1]=-2*q[0]*q[1] + 2*q[2]*q[3];
  v[2]=q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
}

BOOL circleRectIntersection(const float p1[2], float r1,
                            const float p2[2], const float r2[2], float a,
                            float i1[2], float i2[2],
                            float normal[2])
{
  float axis[2][2];
  float offset[2];
  float distance=0, d=0;
  int i, inCount=0;
  float p[2], temp[2];

  // Pura tahojen normaalit asennosta
  extractXAxisFromAngle(axis[0], a);
  extractYAxisFromAngle(axis[1], a);

  // Projisoi ympyrän keskipiste koordinaatistoon, 
  // jossa suorakulmio on "suorassa"
  offset[0]=p1[0]-p2[0];
  offset[1]=p1[1]-p2[1];
  p[0]=dotProduct(offset, axis[0]);
  p[1]=dotProduct(offset, axis[1]);

  // Laske etäisyys laatikkoon ja lähin piste temp
  // käyden läpi kaikki akselit.
  for (i=0; i<2; i++)
  {
    // Ympyrä on kummankin reunan negatiivisella puolella
    if (p[i]<-r2[i])
    {
      d=p[i]+r2[i];
      distance+=d*d;
      temp[i]=-r2[i];
    }
    else
    {
      // Ympyrä on kummankin reunan positiivisella puolella
      if (p[i]>r2[i])
      {
        d=p[i]-r2[i];
        distance+=d*d;
        temp[i]=r2[i];
      }
      // Ympyrä on reunojen välissä
      else
      {
        temp[i]=p[i];
        inCount++;
      }
    }
  }

  // Testaa leikkaavatko ympyrä ja suorakulmio
  if (distance>r1*r1) return FALSE;

  // Leikkaavatko liikaa
  if (inCount>=2) return FALSE;

  // Laske kosketuspisteet. Projisoi kuitenkin ensin lähin 
  // piste takaisin alkuperäiseen koordinaatistoon
  i2[0]=temp[0]*axis[0][0]+temp[1]*axis[1][0]+p2[0];
  i2[1]=temp[0]*axis[0][1]+temp[1]*axis[1][1]+p2[1];
  makeVector(normal, i2, p1);
  normalizeVector(normal);
  i1[0]=p1[0]-r1*normal[0];
  i1[1]=p1[1]-r1*normal[1];

  return TRUE;
}

BOOL sphereBoxIntersection(const float p1[3], float r1,
                           const float p2[3], const float r2[3], float q[4],
                           float i1[3], float i2[3],
                           float normal[3])
{
  float axis[3][3];
  float offset[3];
  float distance=0, d=0;
  int i, inCount=0;
  float p[3], temp[3];

  // Pura tahojen normaalit asennosta
  extractXAxisFromQuaternion(axis[0], q);
  extractYAxisFromQuaternion(axis[1], q);
  extractZAxisFromQuaternion(axis[2], q);

  // Projisoi pallon keskipiste koordinaatistoon, jossa laatikko on "suorassa"
  offset[0]=p1[0]-p2[0];
  offset[1]=p1[1]-p2[1];
  offset[2]=p1[2]-p2[2];
  p[0]=dotProduct(offset, axis[0]);
  p[1]=dotProduct(offset, axis[1]);
  p[2]=dotProduct(offset, axis[2]);

  // Laske etäisyys laatikkoon ja lähin piste temp
  // käyden läpi kaikki akselit.
  for (i=0; i<3; i++)
  {
    // Pallo on kummankin reunan negatiivisella puolella
    if (p[i]<-r2[i])
    {
      d=p[i]+r2[i];
      distance+=d*d;
      temp[i]=-r2[i];
    }
    else
    {
      // Pallo on kummankin reunan positiivisella puolella
      if (p[i]>r2[i])
      {
        d=p[i]-r2[i];
        distance+=d*d;
        temp[i]=r2[i];
      }
      // Pallo on reunojen välissä
      else
      {
        temp[i]=p[i];
        inCount++;
      }
    }
  }

  // Testaa leikkaavatko ympyrä ja laatikko
  if (distance>r1*r1) return FALSE;

  // Leikkaavatko liikaa
  if (inCount>=3) return FALSE;

  // Laske kosketuspisteet. Lähin piste temp siirretään
  // ensin alkuperäiseen koordinaatistoon.
  i2[0]=temp[0]*axis[0][0]+temp[1]*axis[1][0]+temp[2]*axis[2][0]+p2[0];
  i2[1]=temp[0]*axis[0][1]+temp[1]*axis[1][1]+temp[2]*axis[2][1]+p2[1];
  i2[2]=temp[0]*axis[0][2]+temp[1]*axis[1][2]+temp[2]*axis[2][2]+p2[2];
  makeVector(normal, i2, p1);
  normalizeVector(normal);
  i1[0]=p1[0]-r1*normal[0];
  i1[1]=p1[1]-r1*normal[1];
  i1[2]=p1[2]-r1*normal[2];

  return TRUE;
}

1.3 Laatikko - Laatikko #

Kaikkein hankalin tapaus, jota käsittelemme, on kahden laatikon välinen leikkaus. Toisin kuin ”pallon ja pallon” tai ”pallon ja laatikon” tapauksessa, jossa kosketuspisteitä oli vain yksi (kummallakin), kahdella laatikolla voi kosketuspisteen sijaan olla myös kosketusjana tai kosketuspinta. Janan ja pinnan käsittely pisteen sijaan on hankalaa, joten esitämme ne sarjana niitä rajoittavia pisteitä (max 2 (2D-tapaus) tai 8 (3D-tapaus)).

Puhuttaessa laatikoiden leikkauksesta useimmat artikkelit esittelevät ns. erottavan akselin algoritmin. Siinä on ideana, että jos kaksi laatikkoa EI leikkaa, on olemassa akseli (suora viiva), jolle projisoituna laatikot eivät myöskään leikkaa. Ai miksikö vaivautua projisoimaan? Koska akselille projisoituna 2D- tai 3D-laatikoista tulee yksiulotteisia, jolloin törmäyksen testaaminen on huomattavasti helpompaa. Laatikon 1D-projektio on symmetrinen keskipisteen suhteen, joten testi voidaan tehdä laskemalla projektioakselilla laatikoiden keskipisteiden välinen etäisyys ja ”säteet”. Jos etäisyys on suurempi kuin säteiden summa, laatikot eivät leikkaa, muuten leikkaavat.

Jos kaksi laatikkoa ei leikkaa on mahdollista löytää sellainen akseli (suora viiva), jolle projisoituna laatikot eivät myöskään leikkaa (vasemmalla). Jos laatikot leikkaavat tälläistä akselia ei ole olemassa (oikealla).

Kaikki 6 mahdollista laatikoiden törmäystä.

BOOL isPointInFrontOfLine(const float p[2], 
                          const float lineOrigin[2],
                          const float lineNormal[2])
{
  if (dotProduct(p, lineNormal)-dotProduct(lineOrigin, lineNormal)<0)
    return FALSE;
  return TRUE;
}

BOOL isPointInFrontOfPlane(const float p[3], 
                           const float planeOrigin[3], 
                           const float planeNormal[3])
{
  if (dotProduct(p, planeNormal)-dotProduct(planeOrigin, planeNormal)<0)
    return FALSE;
  return TRUE;
}

void closestPointOnLineSegment(float result[2],
                               const float a[2], const float b[2],
                               const float p[2])
{
  float c[2], v[2];
  float d, t;

  makeVector(c, a, p);
  makeVector(v, a, b);
  d=normalizeVector(v);
  t=dotProduct(v, c);

  if (t < 0)
  {
    result[0]=a[0];
    result[1]=a[1];
    return;
  }
  if (t > d)
  {
    result[0]=b[0];
    result[1]=b[1];
    return;
  }

  v[0]*=t;
  v[1]*=t;
  result[0]=a[0]+v[0];
  result[1]=a[1]+v[1];
}

void clipLineSegmentToRect(const float a[2], const float b[2],
                           const float rectOrigin[2],
                           const float rectXAxis[2], const float rectYAxis[2],
                           const float rectRadius[2],
                           float *result)
{
  static float temp1[4], temp2[4], o[2], n[2];

  o[0]=rectOrigin[0]+rectRadius[0]*rectXAxis[0]+rectRadius[1]*rectYAxis[0];
  o[1]=rectOrigin[1]+rectRadius[0]*rectXAxis[1]+rectRadius[1]*rectYAxis[1];

  // leikkaa positiivinen X taho
  clipLineSegmentToLine(a, b, o, rectXAxis, temp1);

  // leikkaa positiivinen Y taho
  clipLineSegmentToLine(&temp1[0], &temp1[2], o, rectYAxis, temp2);

  o[0]=rectOrigin[0]-rectRadius[0]*rectXAxis[0]-rectRadius[1]*rectYAxis[0];
  o[1]=rectOrigin[1]-rectRadius[0]*rectXAxis[1]-rectRadius[1]*rectYAxis[1];
  n[0]=-rectXAxis[0];
  n[1]=-rectXAxis[1];

  // leikkaa negatiivinen X taho
  clipLineSegmentToLine(&temp2[0], &temp2[2], o, n, temp1);

  n[0]=-rectYAxis[0];
  n[1]=-rectYAxis[1];

  // leikkaa negatiivinen Y taho
  clipLineSegmentToLine(&temp1[0], &temp1[2], o, n, temp2);

  result[0]=temp2[0];
  result[1]=temp2[1];
  result[2]=temp2[2];
  result[3]=temp2[3];
}

BOOL rectRectIntersection(const float p1[2], const float r1[2], float a1,
                          const float p2[2], const float r2[2], float a2,
                          float *i1, float *i2,
                          float normal[2], int *iCount)
{
  static float axis[2][2][2];
  static float vertex[2][4][2];
  static const int faceIndex[4][2]={{1,3},{0,2},{2,3},{0,1}};
  int i, j;
  float s[2];
  float d, t1, t2;
  float mind=-1;
  int iFace=0, iRect=0, iVertex[2];
  float iNormal[2];

  // Pura akselit
  extractXAxisFromAngle(axis[0][0], a1);
  extractYAxisFromAngle(axis[0][1], a1);
  extractXAxisFromAngle(axis[1][0], a2);
  extractYAxisFromAngle(axis[1][1], a2);

  // Tutki kaikki 4 mahdollista akselia
  makeVector(s, p1, p2);
  for (j=0; j<2; j++)
  {
    for (i=0; i<2; i++)
    {
      // Onko tämä akseli erottava akseli
      d=fabsf(dotProduct(s, axis[j][i]));
      t1=r1[0]*fabsf(dotProduct(axis[0][0], axis[j][i]))+
         r1[1]*fabsf(dotProduct(axis[0][1], axis[j][i]));
      t2=r2[0]*fabsf(dotProduct(axis[1][0], axis[j][i]))+
         r2[1]*fabsf(dotProduct(axis[1][1], axis[j][i]));
      if (d>t1+t2) return FALSE;

      // Onko tämä se akseli, jolla laatikot ovat vähiten päällekkäin
      d=t1+t2-d;
      if (mind<0 || d<mind)
      {
        // Tunnista leikkaukseen osallistuva taho
        iRect=j;
        if ((j==0)?(dotProduct(axis[j][i], s)>0):(dotProduct(axis[j][i], s)<0))
        {
          iFace=2*i;
          iNormal[0]=axis[j][i][0];
          iNormal[1]=axis[j][i][1];
        }
        else
        {
          iFace=2*i+1;
          iNormal[0]=-axis[j][i][0];
          iNormal[1]=-axis[j][i][1];
        }
        mind=d;
      }
    }
  }

  // Pura verteksit
  for (i=0; i<4; i++)
  {
    vertex[0][i][0]=p1[0]+((i%2==0)?-r1[0]:r1[0])*axis[0][0][0]+
	                      ((i/2==0)?-r1[1]:r1[1])*axis[0][1][0];
    vertex[0][i][1]=p1[1]+((i%2==0)?-r1[0]:r1[0])*axis[0][0][1]+
	                      ((i/2==0)?-r1[1]:r1[1])*axis[0][1][1];
  }
  for (i=0; i<4; i++)
  {
    vertex[1][i][0]=p2[0]+((i%2==0)?-r2[0]:r2[0])*axis[1][0][0]+
	                      ((i/2==0)?-r2[1]:r2[1])*axis[1][1][0];
    vertex[1][i][1]=p2[1]+((i%2==0)?-r2[0]:r2[0])*axis[1][0][1]+
	                      ((i/2==0)?-r2[1]:r2[1])*axis[1][1][1];
  }


  // Etsi kosketuspisteet
  // Käy läpi vastakappaleen verteksit
  *iCount=0;
  for (i=0; i<4; i++)
  {
    // Jos tämä verteksi on tahon takana tee siitä kosketuspiste
    if (!isPointInFrontOfLine(vertex[(iRect+1)%2][i], 
                              vertex[iRect][faceIndex[iFace][0]], 
                              iNormal))
    {
      // Leikkaavatko kappaleet liikaa
      if (*iCount>=2) return FALSE;

      // Lisää vertksi kosketuspisteiden listaan
      iVertex[*iCount]=i;

      // Kasvata määrää
      (*iCount)++;
    }
  }
  if (*iCount==0) return FALSE;

  // Tunnista törmäystapa
  // Taho-verteksi
  if (*iCount==1)
  {
    if (iRect==1)
    {
      i1[0]=vertex[0][iVertex[0]][0];
      i1[1]=vertex[0][iVertex[0]][1];
      closestPointOnLineSegment(i2,
                                vertex[1][faceIndex[iFace][0]],
                                vertex[1][faceIndex[iFace][1]],
                                vertex[0][iVertex[0]]);
      normal[0]=iNormal[0];
      normal[1]=iNormal[1];
    }
    else
    {
      i2[0]=vertex[1][iVertex[0]][0];
      i2[1]=vertex[1][iVertex[0]][1];
      closestPointOnLineSegment(i1,
                                vertex[0][faceIndex[iFace][0]],
                                vertex[0][faceIndex[iFace][1]],
                                vertex[1][iVertex[0]]);
      normal[0]=-iNormal[0];
      normal[1]=-iNormal[1];
    }
  }
  // Taho-taho
  else
  {
    if (iRect==1)
    {
      clipLineSegmentToRect(vertex[0][iVertex[0]], vertex[0][iVertex[1]],
                            p2, axis[1][0], axis[1][1], r2,
                            i1);
      for (i=0; i<2; i++)
      {
        closestPointOnLineSegment(&i2[2*i],
                                  vertex[1][faceIndex[iFace][0]],
                                  vertex[1][faceIndex[iFace][1]],
                                  &i1[2*i]);
      }
      normal[0]=iNormal[0];
      normal[1]=iNormal[1];
    }
    else
    {
      clipLineSegmentToRect(vertex[1][iVertex[0]], vertex[1][iVertex[1]],
                            p1, axis[0][0], axis[0][1], r1,
                            i2);
      for (i=0; i<2; i++)
      {

        closestPointOnLineSegment(&i1[2*i],
                                  vertex[0][faceIndex[iFace][0]],
                                  vertex[0][faceIndex[iFace][1]],
                                  &i2[2*i]);
      }
      normal[0]=-iNormal[0];
      normal[1]=-iNormal[1];
    }
  }

  return TRUE;
}

void closestPointOnQuad(float result[3],
                        const float c1[3],
                        const float c2[3],
                        const float c3[3],
                        const float c4[3],
                        const float p[3])
{
  float normal[3];
  float temp1[3], temp2[3];
  float mind, d;
  int i;
  const float *c[4];
  c[0]=c1; c[1]=c2; c[2]=c3; c[3]=c4;

  // Etsi lähin piste nelikulmion tasolta
  makeVector(temp1, c1, c2);
  makeVector(temp2, c1, c3);
  crossProduct(normal, temp1, temp2);
  normalizeVector(normal);
  closestPointOnPlane(result, c1, normal, p);
  
  // Jos piste on nelikulmion sisässä palauta se
  if (isPointOnQuad(result, c1, c2, c3, c4)) return;

  // Muuten etsi piste nelikulmion reunalta
  mind=-1;
  for (i=0; i<4; i++)
  {
    closestPointOnLineSegment(temp1, c[i], c[(i+1)%4], p);
    d=computeDistance(temp1, p);
    if (mind<0 || d<mind)
    {
      mind=d;
      result[0]=temp1[0];
      result[1]=temp1[1];
      result[2]=temp1[2];
    }
  }
}

void clipLineSegmentToBox(const float a[3], const float b[3],
                          const float boxOrigin[3],
                          const float boxXAxis[3], 
                          const float boxYAxis[3], 
                          const float boxZAxis[3],
                          const float boxRadius[3],
                          float *result)
{
  static float temp1[6], temp2[6], o[3], n[3];

  o[0]=boxOrigin[0]+boxRadius[0]*boxXAxis[0]+
       boxRadius[1]*boxYAxis[0]+boxRadius[2]*boxZAxis[0];
  o[1]=boxOrigin[1]+boxRadius[0]*boxXAxis[1]+
       boxRadius[1]*boxYAxis[1]+boxRadius[2]*boxZAxis[1];
  o[2]=boxOrigin[2]+boxRadius[0]*boxXAxis[2]+
       boxRadius[1]*boxYAxis[2]+boxRadius[2]*boxZAxis[2];

  // leikkaa positiivinen X taho
  clipLineSegmentToPlane(a, b, o, boxXAxis, temp1);

  // leikkaa positiivinen Y taho
  clipLineSegmentToPlane(&temp1[0], &temp1[3], o, boxYAxis, temp2);

  // Leikka positiivinen Z taho
  clipLineSegmentToPlane(&temp2[0], &temp2[3], o, boxZAxis, temp1);


  o[0]=boxOrigin[0]-boxRadius[0]*boxXAxis[0]
       -boxRadius[1]*boxYAxis[0]-boxRadius[2]*boxZAxis[0];
  o[1]=boxOrigin[1]-boxRadius[0]*boxXAxis[1]
       -boxRadius[1]*boxYAxis[1]-boxRadius[2]*boxZAxis[1];
  o[2]=boxOrigin[2]-boxRadius[0]*boxXAxis[2]
       -boxRadius[1]*boxYAxis[2]-boxRadius[2]*boxZAxis[2];
  n[0]=-boxXAxis[0];
  n[1]=-boxXAxis[1];
  n[2]=-boxXAxis[2];

  // leikkaa negatiivinen X taho
  clipLineSegmentToPlane(&temp1[0], &temp1[3], o, n, temp2);

  n[0]=-boxYAxis[0];
  n[1]=-boxYAxis[1];
  n[2]=-boxYAxis[2];

  // leikkaa negatiivinen Y taho
  clipLineSegmentToPlane(&temp2[0], &temp2[3], o, n, temp1);

  n[0]=-boxZAxis[0];
  n[1]=-boxZAxis[1];
  n[2]=-boxZAxis[2];

  // Leikkaa negatiivinen Z taho
  clipLineSegmentToPlane(&temp1[0], &temp1[3], o, n, result);
}

void clipQuadToBox(const float c1[3], const float c2[3], 
                   const float c3[3], const float c4[3],
                   const float boxOrigin[3],
                   const float boxXAxis[3], 
                   const float boxYAxis[3], 
                   const float boxZAxis[3],
                   const float boxRadius[3],
                   float *result, int *count)
{
  static float temp1[3*8], temp2[3*8], o[3], n[3];
  int count1, count2;

  temp1[0]=c1[0]; temp1[1]=c1[1]; temp1[2]=c1[2];
  temp1[3]=c2[0]; temp1[4]=c2[1]; temp1[5]=c2[2];
  temp1[6]=c3[0]; temp1[7]=c3[1]; temp1[8]=c3[2];
  temp1[9]=c4[0]; temp1[10]=c4[1]; temp1[11]=c4[2];

  o[0]=boxOrigin[0]+boxRadius[0]*boxXAxis[0]+
       boxRadius[1]*boxYAxis[0]+boxRadius[2]*boxZAxis[0];
  o[1]=boxOrigin[1]+boxRadius[0]*boxXAxis[1]+
       boxRadius[1]*boxYAxis[1]+boxRadius[2]*boxZAxis[1];
  o[2]=boxOrigin[2]+boxRadius[0]*boxXAxis[2]+
       boxRadius[1]*boxYAxis[2]+boxRadius[2]*boxZAxis[2];

  // leikkaa positiivinen X taho
  clipPolygonToPlane(temp1, 4, o, boxXAxis, temp2, &count2);

  // leikkaa positiivinen Y taho
  clipPolygonToPlane(temp2, count2, o, boxYAxis, temp1, &count1);

  // Leikka positiivinen Z taho
  clipPolygonToPlane(temp1, count1, o, boxZAxis, temp2, &count2);

  o[0]=boxOrigin[0]-boxRadius[0]*boxXAxis[0]
       -boxRadius[1]*boxYAxis[0]-boxRadius[2]*boxZAxis[0];
  o[1]=boxOrigin[1]-boxRadius[0]*boxXAxis[1]
       -boxRadius[1]*boxYAxis[1]-boxRadius[2]*boxZAxis[1];
  o[2]=boxOrigin[2]-boxRadius[0]*boxXAxis[2]
       -boxRadius[1]*boxYAxis[2]-boxRadius[2]*boxZAxis[2];
  n[0]=-boxXAxis[0];
  n[1]=-boxXAxis[1];
  n[2]=-boxXAxis[2];

  // leikkaa negatiivinen X taho
  clipPolygonToPlane(temp2, count2, o, n, temp1, &count1);

  n[0]=-boxYAxis[0];
  n[1]=-boxYAxis[1];
  n[2]=-boxYAxis[2];

  // leikkaa negatiivinen Y taho
  clipPolygonToPlane(temp1, count1, o, n, temp2, &count2);


  n[0]=-boxZAxis[0];
  n[1]=-boxZAxis[1];
  n[2]=-boxZAxis[2];

  // Leikkaa negatiivinen Z taho
  clipPolygonToPlane(temp2, count2, o, n, result, count);
}

void closestPointsOnLines(const float line1Origin[3], 
                          const float line1Direction[3],
                          const float line2Origin[3], 
                          const float line2Direction[3],
                          float result1[3], float result2[3])
{
  float u[3];
  float a, b, c, d, e, s, t;
  float det;

  makeVector(u, line2Origin, line1Origin);
  a=dotProduct(line1Direction, line1Direction);
  b=dotProduct(line1Direction, line2Direction);
  c=dotProduct(line2Direction, line2Direction);
  d=dotProduct(line1Direction, u);
  e=dotProduct(line2Direction, u);

  det=a*c-b*b;

  if (det==0)
  {
    s=0;
    t=d/b;
  }
  else
  {
    s=(b*e-c*d)/det;
    t=(a*e-b*d)/det;
  }

  result1[0]=line1Origin[0]+s*line1Direction[0];
  result1[1]=line1Origin[1]+s*line1Direction[1];
  result1[2]=line1Origin[2]+s*line1Direction[2];

  result2[0]=line2Origin[0]+t*line2Direction[0];
  result2[1]=line2Origin[1]+t*line2Direction[1];
  result2[2]=line2Origin[2]+t*line2Direction[2];
}

BOOL boxBoxIntersection(const float p1[3], const float r1[3], float q1[4],
                        const float p2[3], const float r2[3], float q2[4],
                        float *i1, float *i2,
                        float normal[3], int *iCount)
{
  static float axis[2][3][3];
  static float vertex[2][8][3];
  static const int faceIndex[6][4]={{1,3,7,5},{0,4,6,2},{7,3,2,6},
                                    {5,4,0,1},{6,4,5,7},{2,3,1,0}};
  float tempAxis[3];
  int i, j;
  float s[3];
  float d, t1, t2;
  float mind=-1;
  int iFace=0, iRect=0, iVertex[4];
  int iEdgePoint[2], iEdge[2];
  float iNormal[3];
  BOOL edgeEdge=FALSE;
  float *sorted[4];

  // Pura akselit
  extractXAxisFromQuaternion(axis[0][0], q1);
  extractYAxisFromQuaternion(axis[0][1], q1);
  extractZAxisFromQuaternion(axis[0][2], q1);
  extractXAxisFromQuaternion(axis[1][0], q2);
  extractYAxisFromQuaternion(axis[1][1], q2);
  extractZAxisFromQuaternion(axis[1][2], q2);


  // Tutki kaikki 15 mahdollista akselia
  makeVector(s, p1, p2);
  // Tutki tahojen suuntaiset akselit
  for (j=0; j<2; j++)
  {
    for (i=0; i<3; i++)
    {
      // Onko tämä akseli erottava akseli
      d=fabsf(dotProduct(s, axis[j][i]));
      t1=r1[0]*fabsf(dotProduct(axis[0][0], axis[j][i]))+
         r1[1]*fabsf(dotProduct(axis[0][1], axis[j][i]))+
         r1[2]*fabsf(dotProduct(axis[0][2], axis[j][i]));
      t2=r2[0]*fabsf(dotProduct(axis[1][0], axis[j][i]))+
         r2[1]*fabsf(dotProduct(axis[1][1], axis[j][i]))+
         r2[2]*fabsf(dotProduct(axis[1][2], axis[j][i]));
      if (d>t1+t2) return FALSE;

      // Onko tämä se akseli, jolla laatikot ovat vähiten päällekkäin
      d=t1+t2-d;
      if (mind<0 || d<mind)
      {
        // Tunnista leikkaukseen osallistuva taho
        iRect=j;
        if ((j==0)?(dotProduct(axis[j][i], s)>0):(dotProduct(axis[j][i], s)<0))
        {
          iFace=2*i;
          iNormal[0]=axis[j][i][0];
          iNormal[1]=axis[j][i][1];
          iNormal[2]=axis[j][i][2];
        }
        else
        {
          iFace=2*i+1;
          iNormal[0]=-axis[j][i][0];
          iNormal[1]=-axis[j][i][1];
          iNormal[2]=-axis[j][i][2];
        }
        mind=d;
      }
    }
  }
  // Tutki ristitulojen suuntaiset akselit
  for (i=0; i<3; i++)
  {
    for (j=0; j<3; j++)
    {
      // Ota akselien ristitulo
      crossProduct(tempAxis, axis[0][i], axis[1][j]);
      if (normalizeVector(tempAxis)<0.01) continue;

      // Onko tämä akseli erottava akseli
      d=fabsf(dotProduct(s, tempAxis));
      t1=r1[0]*fabsf(dotProduct(axis[0][0], tempAxis))+
         r1[1]*fabsf(dotProduct(axis[0][1], tempAxis))+
         r1[2]*fabsf(dotProduct(axis[0][2], tempAxis));
      t2=r2[0]*fabsf(dotProduct(axis[1][0], tempAxis))+
         r2[1]*fabsf(dotProduct(axis[1][1], tempAxis))+
         r2[2]*fabsf(dotProduct(axis[1][2], tempAxis));
      if (d>t1+t2) return FALSE;

      // Onko tämä se akseli, jolla laatikot ovat vähiten päällekkäin
      d=t1+t2-d;
      if (mind<0 || d<mind)
      {
        // Tunnista leikkaavat särmät
        edgeEdge=TRUE;
        iEdge[0]=i;
        iEdge[1]=j;
        if (dotProduct(tempAxis, s)<0)
        {
          iNormal[0]=tempAxis[0];
          iNormal[1]=tempAxis[1];
          iNormal[2]=tempAxis[2];
        }
		else
        {
          iNormal[0]=-tempAxis[0];
          iNormal[1]=-tempAxis[1];
          iNormal[2]=-tempAxis[2];
        }
        iEdgePoint[0]=((dotProduct(iNormal, axis[0][0])<0)?1:0)+
                      ((dotProduct(iNormal, axis[0][1])<0)?2:0)+
                      ((dotProduct(iNormal, axis[0][2])<0)?4:0);
        iEdgePoint[1]=((dotProduct(iNormal, axis[1][0])>0)?1:0)+
                      ((dotProduct(iNormal, axis[1][1])>0)?2:0)+
                      ((dotProduct(iNormal, axis[1][2])>0)?4:0);
        mind=d;
      }
    }
  }


  // Pura verteksit
  for (i=0; i<8; i++)
  {
    vertex[0][i][0]=p1[0]+((i%2==0)?    -r1[0]:r1[0])*axis[0][0][0]+
                          (((i/2)%2==0)?-r1[1]:r1[1])*axis[0][1][0]+
                          ((i/4==0)?    -r1[2]:r1[2])*axis[0][2][0];

    vertex[0][i][1]=p1[1]+((i%2==0)?    -r1[0]:r1[0])*axis[0][0][1]+
                          (((i/2)%2==0)?-r1[1]:r1[1])*axis[0][1][1]+
                          ((i/4==0)?    -r1[2]:r1[2])*axis[0][2][1];

    vertex[0][i][2]=p1[2]+((i%2==0)?    -r1[0]:r1[0])*axis[0][0][2]+
                          (((i/2)%2==0)?-r1[1]:r1[1])*axis[0][1][2]+
                          ((i/4==0)?    -r1[2]:r1[2])*axis[0][2][2];
  }
  for (i=0; i<8; i++)
  {
    vertex[1][i][0]=p2[0]+((i%2==0)?    -r2[0]:r2[0])*axis[1][0][0]+
                          (((i/2)%2==0)?-r2[1]:r2[1])*axis[1][1][0]+
                          ((i/4==0)?    -r2[2]:r2[2])*axis[1][2][0];

    vertex[1][i][1]=p2[1]+((i%2==0)?    -r2[0]:r2[0])*axis[1][0][1]+
                          (((i/2)%2==0)?-r2[1]:r2[1])*axis[1][1][1]+
                          ((i/4==0)?    -r2[2]:r2[2])*axis[1][2][1];

    vertex[1][i][2]=p2[2]+((i%2==0)?    -r2[0]:r2[0])*axis[1][0][2]+
                          (((i/2)%2==0)?-r2[1]:r2[1])*axis[1][1][2]+
                          ((i/4==0)?    -r2[2]:r2[2])*axis[1][2][2];
  }


  // Tunnista törmäystapa
  *iCount=0;
  // Särmä-särmä
  if (edgeEdge)
  {
    *iCount=1;

    closestPointsOnLines(vertex[0][iEdgePoint[0]], axis[0][iEdge[0]],
                         vertex[1][iEdgePoint[1]], axis[1][iEdge[1]],
                         i1, i2);

    normal[0]=iNormal[0];
    normal[1]=iNormal[1];
    normal[2]=iNormal[2];
  }
  else
  {
    // Käy läpi vastakappaleen verteksit
    for (i=0; i<8; i++)
    {
      // Jos tämä verteksi on tahon takana tee siitä kosketuspiste
      if (!isPointInFrontOfPlane(vertex[(iRect+1)%2][i],
                                 vertex[iRect][faceIndex[iFace][0]], iNormal))
      {
        // Leikkaavatko kappaleet liikaa
        if (*iCount>=4) return FALSE;

        // Lisää vertksi kosketuspisteiden listaan
        iVertex[*iCount]=i;

        // Kasvata määrää
        (*iCount)++;
      }
    }
    if (*iCount==0) return FALSE;

    // verteksi-taho
    if (*iCount==1)
    {
      if (iRect==1)
      {
        i1[0]=vertex[0][iVertex[0]][0];
        i1[1]=vertex[0][iVertex[0]][1];
        i1[2]=vertex[0][iVertex[0]][2];
        closestPointOnQuad(&i2[0],
                           vertex[1][faceIndex[iFace][0]],
                           vertex[1][faceIndex[iFace][1]],
                           vertex[1][faceIndex[iFace][2]],
                           vertex[1][faceIndex[iFace][3]],
                           vertex[0][iVertex[0]]);
        normal[0]=iNormal[0];
        normal[1]=iNormal[1];
        normal[2]=iNormal[2];
      }
      else
      {
        i2[0]=vertex[1][iVertex[0]][0];
        i2[1]=vertex[1][iVertex[0]][1];
        i2[2]=vertex[1][iVertex[0]][2];
        closestPointOnQuad(&i1[0],
                           vertex[0][faceIndex[iFace][0]],
                           vertex[0][faceIndex[iFace][1]],
                           vertex[0][faceIndex[iFace][2]],
                           vertex[0][faceIndex[iFace][3]],
                           vertex[1][iVertex[0]]);
        normal[0]=-iNormal[0];
        normal[1]=-iNormal[1];

        normal[2]=-iNormal[2];
      }
    }
    // särmä-taho
    else if (*iCount==2 || *iCount==3)
    {
      if (iRect==1)
      {
        clipLineSegmentToBox(vertex[0][iVertex[0]], vertex[0][iVertex[1]],
                             p2, axis[1][0], axis[1][1], axis[1][2], r2,
                             i1);
        for (i=0; i<2; i++)
        {
          closestPointOnQuad(&i2[3*i],
                             vertex[1][faceIndex[iFace][0]],
                             vertex[1][faceIndex[iFace][1]],
                             vertex[1][faceIndex[iFace][2]],
                             vertex[1][faceIndex[iFace][3]],
                             &i1[3*i]);
        }
        normal[0]=iNormal[0];
        normal[1]=iNormal[1];
        normal[2]=iNormal[2];
      }
      else
      {
        clipLineSegmentToBox(vertex[1][iVertex[0]], vertex[1][iVertex[1]],
                             p1, axis[0][0], axis[0][1], axis[0][2], r1,
                             i2);
        for (i=0; i<2; i++)
        {
          closestPointOnQuad(&i1[3*i],
                             vertex[0][faceIndex[iFace][0]],
                             vertex[0][faceIndex[iFace][1]],
                             vertex[0][faceIndex[iFace][2]],
                             vertex[0][faceIndex[iFace][3]],
                             &i2[3*i]);
        }
        normal[0]=-iNormal[0];
        normal[1]=-iNormal[1];
        normal[2]=-iNormal[2];
      }
    }
    // Taho-taho
    else
    {
      if (iRect==1)
      {
        sortQuadVertices(sorted,
                         vertex[0][iVertex[0]], vertex[0][iVertex[1]],
                         vertex[0][iVertex[2]], vertex[0][iVertex[3]]);

        clipQuadToBox(sorted[0], sorted[1], sorted[2], sorted[3],
                      p2, axis[1][0], axis[1][1], axis[1][2], r2,
                      i1, iCount);
        for (i=0; i<*iCount; i++)
        {
          closestPointOnQuad(&i2[3*i],
                             vertex[1][faceIndex[iFace][0]],
                             vertex[1][faceIndex[iFace][1]],
                             vertex[1][faceIndex[iFace][2]],
                             vertex[1][faceIndex[iFace][3]],
                             &i1[3*i]);
        }
        normal[0]=iNormal[0];
        normal[1]=iNormal[1];
        normal[2]=iNormal[2];
      }
      else
      {
        sortQuadVertices(sorted,
                         vertex[1][iVertex[0]], vertex[1][iVertex[1]],
                         vertex[1][iVertex[2]], vertex[1][iVertex[3]]);


        clipQuadToBox(sorted[0], sorted[1], sorted[2], sorted[3],
                      p1, axis[0][0], axis[0][1], axis[0][2], r1,
                      i2, iCount);
        for (i=0; i<*iCount; i++)
        {
          closestPointOnQuad(&i1[3*i],
                             vertex[0][faceIndex[iFace][0]],
                             vertex[0][faceIndex[iFace][1]],
                             vertex[0][faceIndex[iFace][2]],
                             vertex[0][faceIndex[iFace][3]],
                             &i2[3*i]);
        }
        normal[0]=-iNormal[0];
        normal[1]=-iNormal[1];
        normal[2]=-iNormal[2];
      }
    }
  }

  if (*iCount==0) return FALSE;
  return TRUE;
}

2. Impulssi #

Mitä mahtaakaan tapahtua, kun oikeassa maailmassa kaksi kappaletta törmää toisiinsa? Kappaleet eivät suinkaan painaudu sisäkkäin, vaikka ne ovat atomien mittakaavassa tarkasteltuna lähes pelkkää tyhjää, vaan ne kimpoavat toisistaan tai pysähtyvät paikalleen. Miksihän? Tämän saa aikaan atomien ja molekyylien mittakaavassa vaikuttava sähkömagneettinen voima.

Voima on kuitenkin niin valtavan suuri ja vaikuttaa niin tavattoman lyhyen aikavälin aikana, ettei sitä pystytä simuloimaan integraattorin avulla (varsinkaan reaaliajassa). Sen sijaan esittelemme uuden suureen nimeltä impulssi (englanniksi impulse). Impulssin symboli on I ja yksikkö Newton sekunti. Impulssi on vektori, jonka suunta esittää impulssin suuntaa ja pituus voimakkuutta. Impulssi on eräänlainen voiman kertymä, joka kertoo voiman kokonaisvaikutuksen joltain ajalta. Jos tiedämme törmäyksen aiheuttaman impulssin I, voimme laskea uuden nopeuden seuraavasta kaavasta, jossa m tarkoittaa kappaleen massaa.

void applyImpulseToBody(RIGIDBODY2D *b,
                        const float I[2], const float p[2])
{
  float temp1[3], temp2[3], c[3];

  // Vaikuta nopeuteen
  b->velocity[0]+=b->inverseMass*I[0];
  b->velocity[1]+=b->inverseMass*I[1];

  // Vaikuta pyörimisnopeuteen
  temp1[0]=p[0]-b->position[0];
  temp1[1]=p[1]-b->position[1];
  temp1[2]=0;

  temp2[0]=I[0];
  temp2[1]=I[1];
  temp2[2]=0;

  crossProduct(c, temp1, temp2);

  b->angularVelocity+=b->inverseMomentOfInertia*c[2];
}

void applyImpulseToBody(RIGIDBODY3D *b,
                        const float I[3], const float p[3])
{
  float temp1[3], c[3];
  static float qMatrix[9], tempMatrix[9], J[9];

  // Vaikuta nopeuteen
  b->velocity[0]+=b->inverseMass*I[0];
  b->velocity[1]+=b->inverseMass*I[1];
  b->velocity[2]+=b->inverseMass*I[2];

  // Vaikuta pyörimisnopeuteen
  makeVector(temp1, b->position, p);
  crossProduct(c, temp1, I);
  // Ota asento ensin huomioon hitausmomentissa
  generateQuaternionMatrix(qMatrix, b->orientation);
  multMatrix(tempMatrix, qMatrix, b->inverseMomentOfInertia);
  transposeMatrix(qMatrix);
  multMatrix(J, tempMatrix, qMatrix);

  multMatrixWithVector(temp1, J, c);

  b->angularVelocity[0]+=temp1[0];
  b->angularVelocity[1]+=temp1[1];
  b->angularVelocity[2]+=temp1[2];
}

void pointVelocity(float result[2], 
                   const float p[2], const RIGIDBODY2D *b)
{
  float W[3], P[3], temp[3];
  W[0]=0;
  W[1]=0;
  W[2]=b->angularVelocity;
  P[0]=p[0]-b->position[0];
  P[1]=p[1]-b->position[1];
  P[2]=0;
  crossProduct(temp, W, P);
  result[0]=b->velocity[0]+temp[0];
  result[1]=b->velocity[1]+temp[1];
}

void pointVelocity(float result[3], 
                   const float p[3], const RIGIDBODY3D *b)
{
  float P[3], temp[3];
  P[0]=p[0]-b->position[0];
  P[1]=p[1]-b->position[1];
  P[2]=p[2]-b->position[2];
  crossProduct(temp, b->angularVelocity, P);
  result[0]=b->velocity[0]+temp[0];
  result[1]=b->velocity[1]+temp[1];
  result[2]=b->velocity[2]+temp[2];
}

void computeImpulse(float I[2],
                    const float e,
                    const float v1[2], const float v2[2],
                    const float p[2], const float n[2],
                    const RIGIDBODY2D *b1, const RIGIDBODY2D *b2)
{
  float a, temp1[3], temp2[3], c1[3], c2[3], v[2];

  v[0]=v1[0]-v2[0];
  v[1]=v1[1]-v2[1];

  a = -(1+e)*dotProduct(v, n);

  temp1[0]=p[0]-b1->position[0];
  temp1[1]=p[1]-b1->position[1];
  temp1[2]=0;

  temp2[0]=n[0];
  temp2[1]=n[1];
  temp2[2]=0;

  crossProduct(c1, temp1, temp2);

  temp1[0]=p[0]-b2->position[0];
  temp1[1]=p[1]-b2->position[1];
  temp1[2]=0;

  crossProduct(c2, temp1, temp2);

  a/= b1->inverseMass + b2->inverseMass +
      b1->inverseMomentOfInertia*(c1[0]*c1[0]+c1[1]*c1[1]+c1[2]*c1[2]) +
      b2->inverseMomentOfInertia*(c2[0]*c2[0]+c2[1]*c2[1]+c2[2]*c2[2]);

  I[0]=a*n[0];
  I[1]=a*n[1];
}

void computeImpulse(float I[3],
                    const float e,
                    const float v1[3], const float v2[3],
                    const float p[3], const float n[3],
                    const RIGIDBODY3D *b1, const RIGIDBODY3D *b2)
{
  static float qMatrix[9], tempMatrix[9], J[9];
  static float a, P[3], c1[3], c2[3], temp1[3], temp2[2], v[3];

  v[0]=v1[0]-v2[0];
  v[1]=v1[1]-v2[1];
  v[2]=v1[2]-v2[2];

  a = -(1+e)*dotProduct(v, n);

  // Ota asento huomioon hitausmomentissa
  generateQuaternionMatrix(qMatrix, b1->orientation);
  multMatrix(tempMatrix, qMatrix, b1->inverseMomentOfInertia);
  transposeMatrix(qMatrix);
  multMatrix(J, tempMatrix, qMatrix);

  makeVector(P, p, b1->position);
  crossProduct(temp1, P, n);
  multMatrixWithVector(temp2, J, temp1);
  crossProduct(c1, temp2, P);

  // Ota asento huomioon hitausmomentissa
  generateQuaternionMatrix(qMatrix, b2->orientation);
  multMatrix(tempMatrix, qMatrix, b2->inverseMomentOfInertia);
  transposeMatrix(qMatrix);
  multMatrix(J, tempMatrix, qMatrix);

  makeVector(P, p, b2->position);
  crossProduct(temp1, P, n);
  multMatrixWithVector(temp2, J, temp1);
  crossProduct(c2, temp2, P);

  temp1[0]=c1[0]+c2[0];
  temp1[1]=c1[1]+c2[1];
  temp1[2]=c1[2]+c2[2];

  a/=b1->inverseMass + b2->inverseMass + dotProduct(temp1, n);

  I[0]=a*n[0];
  I[1]=a*n[1];
  I[2]=a*n[2];
}

3. Esimerkkiohjelma #

Esimerkkiohjelmassa muutama kappale törmäilee suljetussa huoneessa. Huoneen seininä toimivat laatikot, joiden massa on ääretön ja ovat näin ollen immuuneja impulssien vaikutuksille.

Ohjelma toimii seuraavan algoritmin mukaisesti.

Toista
{
1. Etsi törmäystarkistuksen avulla kaikki kosketuspisteet.
2. Käy kosketuspisteitä läpi käyttäen impulssia aina tarvittaessa, 
   kunnes yksikään kosketuspiste ei enää tarvitse impulssia.
3. Edistä simulaatiota seuraavan aika-askeleen verran integraattorilla.
}

2D-esimerkkiohjelma

3D-esimerkkiohjelma

4. Loppusanat #

Tässä osassa opit törmäyksistä. Huomaa, että tässä artikkelisarjassa esitellyt menetelmät eivät missään tapauksessa ole optimaalisia vaan tositilanteessa niitä käytettäessä pitää käyttää erinäisiä optimointeja. Esim. jos kappaleita on paljon pitää ne jäsentää jonkinnäköiseksi puuksi ja yrittää karsia yhdellä törmäystestillä suuria joukkoja. Lisäksi pallon ja laatikon lisäksi tarvitaan muitakin kappaleita kuten sylinteri, kartio, taso ja mielivaltainen monitahokas. Törmäystarkistus näiden kappaleiden kesken kuitenkin alkaa olla todella monimutkaista ja hidasta, joka korostaa optimoinnin tarvetta entisestään.

Seuraavassa osassa puhumme kitkasta ja helpolta kuulostavalta, mutta äärimmäisen hankalasta asiasta nimeltä lepokontakti.

Raportoithan kaikki tästä artikkelista löytämäsi virheet (niin kirjoitus-, kuin asiavirheetkin) osoitteeseen markus.ilmola@pp.inet.fi , niin korjaan ne mahdollisimman nopeasti. Myös kaikki kommentit ja kysymykset ovat tervetulleita.

Lopuksi linkkejä, joissa lisää luettavaa tähän mennessä opituista asioista.

David Baraffin artikkeli "Physically based modeling" osa 2:
http://www-2.cs.cmu.edu/~baraff/sigcours...

Chris Heckerin artikkeli "Collision response":
http://www.d6.com/users/checker/pdfs/gdm...

Kokoelma törmäystarkistusalgoritmeja:
http://www.realtimerendering.com/int/

Parhaan geometriakirjan "Geometric tools for Computer Graphics" kotisivu:
http://www.geometrictools.com/Books.html