Collision Detection для геометрий, заданных SupportMapping'ом (комментарии) (5 стр)

public AGVector2D getSupport(AGVector2D normal){
    //поворачиваем вершину
    AGVector2D rotatedVec = _vertices[0].rotate(_orientation);
    //задаем начальное значение
    double max = normal.multiply(new AGVector2D(rotatedVec.getX()+_position.getX(), rotatedVec.getY()+_position.getY()));
    int index=0;
    //ищем по циклу максимальное
    for (int i=1;i<_size/2;i++)
    {
      //также поворачиваем
      rotatedVec = _vertices[i].rotate(_orientation);
      //и ищем произведение входящего вектора на точку
      double tmp = normal.multiply(new AGVector2D(rotatedVec.getX()+_position.getX(), rotatedVec.getY()+_position.getY()));
      if(max<tmp) 
      {
        max=tmp;
        index = i;
      }
    }
    _last_index = index;
    rotatedVec = _vertices[index].rotate(_orientation);
    //возвращаем найденную точку
    return new AGVector2D(rotatedVec.getX()+_position.getX(), rotatedVec.getY()+_position.getY());
  }

protected boolean findPD(AGObject2D inObject){
//инициализируем pd
    AGVector2D pd;
    //если сохранили значение с предыдущей итерации - кладе его
    if(direction.getLength()>0)
      pd = new AGVector2D(direction);
      //иначе - вектор разности центров объектов
    else 
      pd = (inObject.getPosition().minus(_position)).normalize();
    AGVector2D v = new AGVector2D(pd);
    double angEps = 1e-10f;
    double eps = 0.001f;
    int break1 =0;
    double OAlengthOld = 10000;
    //начинаем основной цикл
    while(true){
    //инициалзируем v
      v.setPosition(pd);
      int break2 =0;
      //решаем вспомогательную задачу из пункта 2
      while(true){
        //ищем точку S в направлении v геометрии CSO
        AGVector2D S = this.getSupport(inObject, v);
        //ищем точку пересечения прямой проходящей через S и вектором pd
        //здесь не очень понял по поводу "касательной плоскости" - решил взять прямую, перпендикулярную v и проходящую через S
        //findCross - ищет точку пересечения, calcCoefViaNormal - ищет коеф-ты прямой через нормаль и точку. 
        //calcCoefViaCenter - ищет прямую через 0 в направлении pd
        AGVector2D A = _geom.findCross(_geom.calcCoefViaNormal(v, S), _geom.calcCoefViaCenter(pd));
        //если OA приитовоположнонаправлен pd - выходим из алгоритма
        if(!A.isSameDirection(pd)) return false;
        //если длина SA меньше eps выходим
        if(A.minus(S).getLength()<=eps || break2>5) break;
        //иначе кладем в v сумму v и нормализованного SA
        else v.setPosition(v.plus((A.minus(S).multiply(eps)).normalize()));
        //здесь проверяем, укорачивается OA или нет
        if(A.getLength()>=OAlengthOld)break2++;
        else{
          break2 = 0;
          OAlengthOld = A.getLength();
        }        
      }
      //если угол между pd и v меньше eps то выходим и переходим в следующую часть алгоритма - поиск контактов
      if(pd.getAngle(v)<angEps || break1>5) break;
      //иначе кладем в pd значение v
      else pd.setPosition(v);
      break1++;
    }
    //сохраняем текущее значение pd
    direction.setPosition(pd.normalize());
    return true;
  }
  

if(direction.getLength()>0)
      pd = new AGVector2D(direction);
    else 
      pd = (inObject.getPosition().minus(_position)).normalize();
    AGVector2D v = new AGVector2D(pd);
    double angEps = 0.0001;
    double eps = 0.001f;
    int break1 =0;
    
    double OAlengthOld = 10000;
    
    while(true){
      v.setPosition(pd);
      int break2 =0;
      while(true){
        //ищем точку S в направлении v геометрии CSO
        AGVector2D S = this.getSupport(inObject, v);
        
        AGVector2D A = _geom.findCross(_geom.calcCoefViaNormal(v, S), _geom.calcCoefViaCenter(pd));
        if(!A.isSameDirection(pd)){
          direction.setPosition(v);
          return false;
        }
        if(A.minus(S).getLength()<=eps || break2>3) break;
        else v.setPosition(v.plus(A.minus(S).multiply(eps)).normalize());
        if(A.getLength()>=OAlengthOld)break2++;
        else{
          break2 = 0;
          OAlengthOld = A.getLength();
        }
        
      }
      
      if(Math.abs(pd.getAngle(v))<angEps || break1>5) break;
      else pd.setPosition(v);
      break1++;
    }

int segmentsCount = 100;
Vector2 prevDir(1, 0, 0);
for(int i = 1; i < segmentsCount; i++)
{
  Vector2 currDir(cosf(i / segmentsCount * 2.0f * pi), sinf(i / segmentsCount * 2.0f * pi));
  renderer->RenderLine(shape->GetSupportPoint(prevDir), shape->GetSupportPoint(currDir));
  prevDir = currDir;
}