/*
* This file is part of Physics 2D Library.
* 
* Physics 2D Library is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* 
* Physics 2D Library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
* 
* You should have received a copy of the GNU General Public License
* along with Physics 2D Library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "phys2d.h"

BOOL APIENTRY DllMain (HINSTANCE hInst, DWORD reason, LPVOID reserved)
{
return true;
}

P2DPhysics::P2DPhysics()
{
maxObjects = MAXOBJECTS;
maxEffects = MAXEFFECTS;
for(int i=0; i<maxObjects; i++)
 object[i] = NULL;
for(int i=0; i<maxEffects; i++)
 effect[i] = NULL;
}

P2DPhysics::~P2DPhysics()
{
for(int i=0; i<maxObjects; i++)
 delete object[i];
}

void P2DPhysics::CalcStep(double step)
{
for(int i=0; i<maxObjects; i++)
 if(object[i])
  {
  for(int j=0; j<maxEffects; j++)
   if(effect[j])
    if(!effect[j]->disabled)
     effect[j]->ApplyEffect(this, object[i], step);
  if(!object[i]->unyielding)
   {
   object[i]->x += object[i]->vx * step;
   object[i]->y += object[i]->vy * step;
   object[i]->rotation += object[i]->vr * step;
   while(fabs(object[i]->rotation)>360)
    object[i]->rotation -= sign(object[i]->rotation)*360;
   }
  ApplyTransform(object[i]);
  }
}

void P2DPhysics::ApplyForce(P2DObject* obj, double dir, double strength)
{
//Радианы нужны потому как стандартные ф-ции принимают именно радианы
//А минус - чтобы переделать направление против часовой в по
//Формулы работают против часовой, а параметр - по часовой стрелке.
//Dividing by mass is needed becaose of inertia
//Speed = Strength / Mass
if(!obj->unyielding)
 {
 if(dir<0)
  dir = 360 + dir;
 if(fabs(dir)>360)
  abort();
 double rad = dir/(180/M_PI);
 obj->vx += strength * sin(rad) / obj->mass;
 obj->vy += strength * cos(rad) / obj->mass;
 }
}

int P2DPhysics::AddObject(P2DObject *obj)
{
int lastNull = -1;
for(int i=0; i<maxObjects; i++)
 if(object[i]==NULL)
  {
  lastNull = i;
  break;
  }
if(lastNull >= 0)
 {
 object[lastNull] = obj;
 for(int i=0; i<obj->pointCount; i++)
  {
  obj->splinePoints[i].x = obj->splinePointsSrc[i].x;
  obj->splinePoints[i].y = obj->splinePointsSrc[i].y;
  }
 FindCenterOfMass(obj);
 return 1;
 }
else
 return -1;
}

int P2DPhysics::AddEffect(P2DEffect *obj)
{
int lastNull = -1;
for(int i=0; i<maxEffects; i++)
 if(effect[i]==NULL)
  {
  lastNull = i;
  break;
  }
if(lastNull >= 0)
 {
 effect[lastNull] = obj;
 return 1;
 }
else
 return -1;
}

void P2DPhysics::ApplyTransform(P2DObject* obj)
{
if(fabs(obj->rotation)>360)
 abort();
double rad = obj->rotation / (180 / M_PI);
for(int i=0; i<obj->pointCount; i++)
 {
 // without this in 2nd eq is used modified X and Y!!
 double oldx = obj->splinePointsSrc[i].x - obj->cx;
 double oldy = obj->splinePointsSrc[i].y - obj->cy;
 obj->splinePoints[i].x = 
   oldx * cos(rad) + 
   oldy * sin(rad) + obj->cx;
 obj->splinePoints[i].y = 
   -oldx * sin(rad) + 
   oldy * cos(rad) + obj->cy;
 }
}

void P2DPhysics::InitObject(P2DObject* obj, int pointCount, int massPointCount, double mass, bool unyielding)
{
obj->x = 0;
obj->y = 0;
obj->splinePoints = new P2DPoint[pointCount];
obj->splinePointsSrc = new P2DPoint[pointCount];
if(massPointCount>0)
 {
 obj->massPoints = new P2DPoint[massPointCount];
 obj->massPointMass = new double[massPointCount];
 }
obj->triangulated = new bool[pointCount];
for(int i=0; i<pointCount; i++)
 {
 obj->triangulated[i] = false;
 obj->splinePointsSrc[i].x = 0;
 obj->splinePointsSrc[i].y = 0;
 obj->splinePoints[i].x = 0;
 obj->splinePoints[i].y = 0;
 if(massPointCount>0)
  {
  obj->massPoints[i].x = 0;
  obj->massPoints[i].y = 0;
  obj->massPointMass[i] = 0;
  }
 }
obj->pointCount = pointCount;
obj->massPointCount = massPointCount;
obj->mass = mass;
obj->vx = 0;
obj->vy = 0;
obj->vr = 0;
obj->cx = 0;
obj->cy = 0;
obj->unyielding = unyielding;
obj->rotation = 0;
}

PTL P2DPhysics::PointToLine(P2DPoint p, P2DPoint ls, P2DPoint le)
{
P2DPoint a = { le.x - ls.x, le.y - ls.y };
P2DPoint b = { p.x - ls.x, p.y - ls.y };

double sa = a. x * b.y - b.x * a.y;
if (sa > 0.0)
 return ptlLeft;
if (sa < 0.0)
 return ptlRight;
if ((a.x * b.x < 0.0) || (a.y * b.y < 0.0))
 return ptlBehind;
if (sqrt(a.x*a.x + a.y*a.y) < sqrt(b.x*b.x + b.y*b.y))
 return ptlBeyond;
if (p.x == ls.x && p.y == ls.y)
 return ptlOrigin;
if (p.x == le.x && p.y == ls.y)
 return ptlDestination;
return ptlBetween;
}

bool P2DPhysics::LinesCrossing(P2DPoint p1, P2DPoint p2, P2DPoint p3, P2DPoint p4, bool alsoBorder)
{
double maxx1 = max(p1.x, p2.x), maxy1 = max(p1.y, p2.y);
double minx1 = min(p1.x, p2.x), miny1 = min(p1.y, p2.y);
double maxx2 = max(p3.x, p4.x), maxy2 = max(p3.y, p4.y);
double minx2 = min(p3.x, p4.x), miny2 = min(p3.y, p4.y);

if (minx1 > maxx2 || maxx1 < minx2 || miny1 > maxy2 || maxy1 < miny2)
  return alsoBorder;  // Момент, когда линии имеют одну общую вершину...
 

double dx1 = p2.x-p1.x, dy1 = p2.y-p1.y; // Длина проекций первой линии на ось x и y
double dx2 = p4.x-p3.x, dy2 = p4.y-p3.y; // Длина проекций второй линии на ось x и y
double dxx = p1.x-p3.x, dyy = p1.y-p3.y;
double div, mul;


if ((div = (double)dy2*dx1-(double)dx2*dy1) == 0) 
 return false; // Линии параллельны...
if (div > 0) 
 {
 if ((mul = (double)dx1*dyy-(double)dy1*dxx) < 0 || mul > div)
  return false; // Первый отрезок пересекается за своими границами...
 if ((mul = (double)dx2*dyy-(double)dy2*dxx) < 0 || mul > div)
  return false; // Второй отрезок пересекается за своими границами...
 }

if ((mul = -((double)dx1*dyy-(double)dy1*dxx)) < 0 || mul > -div)
  return false; // Первый отрезок пересекается за своими границами...
if ((mul = -((double)dx2*dyy-(double)dy2*dxx)) < 0 || mul > -div)
  return false; // Второй отрезок пересекается за своими границами...

return TRUE;
}

/*
 Точка внутри многоугольника.
 Метод трассировки лучей:
       если точка внутри, луч из нее пересечет границу НЕЧЕТНОЕ число раз
       иначе, ЧЕТНОЕ число раз
 Выбрал горизонтальный луч, для простоты, так как подходит любой
*/
PTP P2DPhysics::PointInPolygon(P2DPoint a, P2DObject p)
{
int parity = 0;
for(int i=0;  i<p.pointCount-1; i++) 
 switch(EdgeToRay(a, p.splinePoints[i], p.splinePoints[i+1]))
  {
  case etrTouching:
  return ptpBoundary;
  
  case etrCrossing:
  parity = 1 - parity;
  }
switch(EdgeToRay(a, p.splinePoints[0], p.splinePoints[p.pointCount-1]))
 {
 case etrTouching:
 return ptpBoundary;
 
 case etrCrossing:
 parity = 1 - parity;
 }
return (parity ? ptpInside : ptpOutside);
}

ETR P2DPhysics::EdgeToRay (P2DPoint a, P2DPoint v, P2DPoint w)
{
switch (PointToLine(a, v, w)) 
 {
 case ptlLeft:
 return ((v.y < a.y)&&(a.y <= w.y)) ? etrCrossing : etrInessential; 
 
 case ptlRight:
 return ((w.y < a.y)&&(a.y <= v.y)) ? etrCrossing : etrInessential; 
 
 case ptlBetween:
 case ptlOrigin:
 case ptlDestination:
 return etrTouching;
 
 default:
 return etrInessential;
 }
}

bool P2DPhysics::PointInTriangle (P2DPoint p, P2DPoint a, P2DPoint b, P2DPoint c)
{
return ((PointToLine(p, a, b) != ptlLeft) && 
        (PointToLine(p, b, c) != ptlLeft) && 
        (PointToLine(p, c, a) != ptlLeft));
}

/*
 Поиск центра массы триангуляцией
*/
void P2DPhysics::FindCenterOfMass(P2DObject* obj)
{
if(obj->unyielding) return;
int tri[obj->pointCount][3];
for(int i=0; i<obj->pointCount; i++)
 obj->triangulated[i] = false;
int triCount = 0;
while(triCount < obj->pointCount-2)
 {
 int convex = 0, a, b;
 while(1)
  {
  do
   {
   convex = FindNextTri(obj, convex, true);
   a = FindNextTri(obj, convex, false);
   b = FindNextTri(obj, convex, true);
   } while(PointToLine(obj->splinePoints[a], 
     obj->splinePoints[convex], obj->splinePoints[b]) != ptlRight); // Выпуклый угол
     
  bool hasIntruding = false;   //если другая вторгается, пошлем этот треугольник к черту и поищем другой
  for(int i=0; i<obj->pointCount; i++)
   if(PointInTriangle(obj->splinePoints[i], obj->splinePoints[a], 
                                            obj->splinePoints[convex], obj->splinePoints[b])
      && i!=a && i!=b && i!=convex)
    {
    hasIntruding = true;
    break;
    }
  if(!hasIntruding) break;
  }
 obj->triangulated[convex] = true;
 tri[triCount][0] = a;
 tri[triCount][1] = convex;
 tri[triCount++][2] = b;
 }
double cX = 0, cY = 0, S = 0;
if(obj->massPointCount > 0)
 {
 /* TODO: Implement mass points! */
 }
else
 {
 for(int i=0; i<triCount; i++)
  {
  P2DPoint a = obj->splinePoints[tri[i][0]];
  P2DPoint b = obj->splinePoints[tri[i][1]];
  P2DPoint c = obj->splinePoints[tri[i][2]];
  double al = sqrt(sqr(b.x - a.x) + sqr(b.y - a.y));
  double bl = sqrt(sqr(c.x - a.x) + sqr(c.y - a.y));
  double cl = sqrt(sqr(c.x - b.x) + sqr(c.y - b.y));
  double ptri = (al + bl + cl) * 0.5;
  double Stri = sqrt(ptri * (ptri - al) * (ptri - bl) * (ptri - cl));
  S +=  Stri;
  cX += Stri*(a.x + b.x + c.x)/3; 
  cY += Stri*(a.y + b.y + c.y)/3;
  }
 cX /= S;
 cY /= S;
 }
obj->cx = cX;
obj->cy = cY;
}

/*
 Поиск первой вершины после(перед) данной, для к-рой не производилась триангуляция
*/
int P2DPhysics::FindNextTri(P2DObject* obj, int v, bool next)
{
if(next)
 {
 for(int i=v+1; i<obj->pointCount; i++)  // v ---> count
  if(!obj->triangulated[i])
   return i;
 for(int i=0; i<v; i++)                  // 0 ---> v
  if(!obj->triangulated[i])
   return i;
 }
else
 {
 for(int i=v-1; i>=0; i--)               // 0 <--- v
  if(!obj->triangulated[i])
   return i;
 for(int i=obj->pointCount-1; i>v; i--)  // v <--- count
  if(!obj->triangulated[i])
   return i;
 }
return -1;
}

//------------------------------------------------------------------------------
//----------------------------E F F E C T S-------------------------------------
//------------------------------------------------------------------------------
P2DGravity::P2DGravity(double _strength) : P2DEffect()
{
strength = _strength;
}

void P2DGravity::ApplyEffect(P2DPhysics* ph, P2DObject* obj, double step)
{
ph->ApplyForce(obj, 180, strength * obj->mass * step);
}
 
P2DEnvResistance::P2DEnvResistance(double _strength) : P2DEffect()
{
strength = fabs(_strength) > 0.999 ? 0.999 : fabs(_strength);
}

void P2DEnvResistance::ApplyEffect(P2DPhysics* ph, P2DObject* obj, double step)
{
obj->vr = obj->vr-strength*obj->vr*obj->vr*step > 0 ? obj->vr-strength*obj->vr*obj->vr*step : 0;
}
