// ULinearAlgebra.cpp: implementation of the ULinearAlgebra class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Simulator.h"
#include "ULinearAlgebra.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

ULinearAlgebra::ULinearAlgebra()
{

}

ULinearAlgebra::~ULinearAlgebra()
{

}

/************************************************************************
* Function  CPoint FindIntersection(CPoint start, CPoint end, CRect Quad)
* 
* PURPOSE
* To find the next point at which the line intersects a quadrant
* ALGORITHM
* If the line is contained in the rect return -1's;
* If not take the distance of interestion from the line to each quadrants
* the shortest one that is valid is the next point;
* INPUTS
* Start, the start of the line
* End, the end of the line
* Quad, the bounding rectange at which to look for the intersection
* This value is going to be from 1-128x 1-128y where as the point it going to be from 1-12800
* x and 1-12800y, so some formatting is going to be required.
* OUTPUTS
* A point in the next quadrant
*************************************************************************/
CPoint ULinearAlgebra::FindIntersection(CPoint start, CPoint end, CRect Quad)

{

    
    // O.K the lines goes beyond time to find out where.

    CPoint rPoint;
    CRect CalcRect;
    // note this rect has extended its values by one in all directions because the line has to go into the next quadrant
    CalcRect.top = (Quad.top-1)*100+1-1; 
    CalcRect.bottom = (Quad.bottom)*100+1;
    CalcRect.left = (Quad.left-1)*100+1-1;
    CalcRect.right = (Quad.right)*100+1;    

    // first see if there line goes beyond the rect
    if (InRect(start,CalcRect) && InRect(end,CalcRect))
    {
        CPoint temp;
        temp.x = -1;
        temp.y = -1; 
        return temp;
    }

    Matrix2X2 QuadMatrix,LineMatrix;

    // get the normal form of the line matrix
    LineMatrix = PointstoLine(start.x,start.y,end.x,end.y);

    float distance = 0;
    float sdistance = 9999999;

    bool Check[4];

    for (int ix = 0; ix < 4; ix++)
        Check[ix] = false;


    // find the direction of the line.
    int xSign,ySign;
    if (end.x > start.x)
        xSign = 1;
    else if (end.x < start.x)
        xSign = -1;
    else
        xSign = 0;
    if (end.y > start.y)
        ySign = -1;
    else if (end.y < start.y)
        ySign = 1;
    else
        ySign = 0;

    CPoint intersection;
 
    if ((xSign == 0) && (ySign == 1))
    {
        // the intersection must be in the top line
        Check[0] = true;
    }
    else if ((xSign == 0) && (ySign == -1))
    {
        // the intersection must be in the bottom line
        Check[2] = true;
    }
    else if ((ySign == 0) && (xSign == 1))
    {
        // the intersection must be in the right line
        Check[1] = true;
    }
    else if ((ySign ==0) && (xSign == -1))
    {
        // the intersection must be in the left line
        Check[3] = true;
    }
    else if ((ySign == 0) && (xSign == 0))
    {
        // should never happen because point does not pass through the first if
    }
    else
    {
        if (ySign == 1)
           Check[0] = true;
        else if (ySign == -1)
            Check[2] = true;
        if (xSign == 1)
            Check[1] = true;
        else if (xSign == -1)
            Check[3] = true;
    }

    // now that we know which lines on the rectangle to check, lets do it.
    for (ix = 0; ix < 4; ix++)
    {
        if (Check[ix] == true)
        {
            QuadMatrix = GetMatrix(CalcRect,ix+1);
            intersection = FindIntersection(QuadMatrix,LineMatrix);
            distance = GetDistance(intersection,start);
            if (distance < sdistance)
            {
                rPoint = intersection;
                sdistance = distance;

            }
        }
    }

    return rPoint;

}

/************************************************************************
* Function Matrix2X2 PointstoLine(CPoint start, CPoint end)
* 
* PURPOSE
* To convert two x,y points to the line normal format Ax+By+c
* INPUTS
* Start, the start of the line
* End, the end of the line
* OUTPUTS
* A matrix with the point normal format
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoLine(CPoint start, CPoint end)
{
    Matrix2X2 Line;    

    if ((end.x-start.x) == 0)
    {
        Line.B= 0;
        Line.A = 1;
        Line.C = -(float)start.x;
    }
    else
    {
        float slope = (float)(end.y-start.y)/(float)(end.x-start.x);
        Line.B = 1;
        Line.A = -slope;  
        Line.C = -start.y+start.x*slope;
    }

    return Line;

}

/************************************************************************
* Function float GetDistance(Matrix2X2 Line, CPoint Point)
* 
* PURPOSE
* To return the distance of a point and a line
* INPUTS
* Line, a line in normal format
* Point, a point
* OUTPUTS
* the distance of the intersection of a point and a line.
*************************************************************************/
float ULinearAlgebra::GetDistance(ULinearAlgebra::Matrix2X2 Line, CPoint Point)
{
    #ifdef USEFLOAT
        return (Line.A*(float)Point.x+Line.B*(float)Point.y+Line.C)/(float)sqrt(Line.A*Line.A+Line.B*Line.B);
    #else
        // the double precision is only needed on the intermediate calculations.
        return (float)((Line.A*(double)Point.x+Line.B*(double)Point.y+Line.C)/(double)sqrt(Line.A*Line.A+Line.B*Line.B));
    #endif



}

// wrapper for above function
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoLine(int x1, int y1, int x2, int y2)
{
    CPoint one,two;

    one.x = x1;
    one.y = y1;
    two.x = x2;
    two.y = y2;

    return PointstoLine(one,two);
        

}

/************************************************************************
* Function float FindSlope(int x1, int y1, int x2, int y2)
* 
* returns the slope of a line
*************************************************************************/
float ULinearAlgebra::FindSlope(int x1, int y1, int x2, int y2)
{
    return (float)(y2-y1)/(float)(x2-x1);


}

/************************************************************************
* Function BOOL InRect(CPoint Point, CRect Rect)
* 
* returns if a point is in a rectangle
*************************************************************************/
BOOL ULinearAlgebra::InRect(CPoint Point, CRect Rect)
{
    if ((Point.x >= Rect.left) && (Point.x <= Rect.right) && (Point.y >= Rect.top) && (Point.y <= Rect.bottom))
    {
        return true;
    }

    return false;

}

/************************************************************************
* Function BOOL InRect(FPoint Point, CRect Rect)
* 
* returns if a point is in a rectangle
*************************************************************************/
BOOL ULinearAlgebra::InRect(FPoint Point, CRect Rect)
{
    CPoint tempo;
    tempo.x = (long)Point.x;
    tempo.y = (long)Point.y;

    if ((tempo.x >= Rect.left) && (tempo.x <= Rect.right) && (tempo.y >= Rect.top) && (tempo.y <= Rect.bottom))
    {
        return true;
    }

    return false;

}

/************************************************************************
* Function BOOL InRect(FPoint Point, CRect Rect)
* 
* returns if a point is in a rectangle
*************************************************************************/
BOOL ULinearAlgebra::InRect(FPoint Point, FRect Rect)
{
    BOOL Left = ((Point.x > Rect.left) || IsEqual((float)Point.x,(float)Rect.left,(float)PLUSMINUS));
    BOOL Right = ((Point.x < Rect.right) || IsEqual((float)Point.x,(float)Rect.right,(float)PLUSMINUS));
    BOOL Top = ((Point.y > Rect.top) || IsEqual((float)Point.y,(float)Rect.top,(float)PLUSMINUS));
    BOOL Bottom = ((Point.y < Rect.bottom) || IsEqual((float)Point.y,(float)Rect.bottom,(float)PLUSMINUS));

   /* if ((Point.x >= Rect.left) && (Point.x <= Rect.right) && (Point.y >= Rect.top) && (Point.y <= Rect.bottom))
    {
        return true;
    }*/
    // could have a round error so use this with the isequal
    if (Left && Right && Top && Bottom)
        return true;
    // check if close enough
    

    return false;

}

/************************************************************************
* Function CPoint FindIntersection(Matrix2X2 Line1, Matrix2X2 Line2)
* 
* PURPOSE
* To return the point of the intersection of two lines
* ALGORITHM
* Kramer rule in linear algebra
* INPUTS
* Two lines in matrix format
* OUTPUTS
* the point of intersection
*************************************************************************/
CPoint ULinearAlgebra::FindIntersection(ULinearAlgebra::Matrix2X2 Line1, ULinearAlgebra::Matrix2X2 Line2)
{
    CPoint rPoint;
    // find the bottom determinate
    #ifdef USEFLOAT
        float DBottom = Line1.A*Line2.B-Line2.A*Line1.B;
    #else
        double DBottom = Line1.A*Line2.B-Line2.A*Line1.B;
    #endif

    // parallel lines, no intersection
    if (DBottom == 0)
    {
         rPoint.x = -1;
         rPoint.y = -1;
         return rPoint;
    }

    // find the x determinate
    rPoint.x = Round((float)(((-Line1.C)*Line2.B-(-Line2.C)*Line1.B)/DBottom));
    rPoint.y = Round((float)(((Line1.A *(-Line2.C)-Line2.A*(-Line1.C))/DBottom))); 

    return rPoint;    

}

/************************************************************************
* Function float GetDistance(CPoint one, CPoint two)
* 
* returns the distance of two points
*************************************************************************/
float ULinearAlgebra::GetDistance(CPoint one, CPoint two)
{
    // note that the use of floats is because for large slopes, the distance is going to be large
    // and it is too large to handle with the CPoints Longs
    return (float)sqrt((float)(one.x-two.x)*(float)(one.x-two.x)+(float)(one.y-two.y)*(float)(one.y-two.y));

}

/************************************************************************
* Function Matrix2X2 GetMatrix(CRect CalcRect, int whichone)
* 
* PURPOSE
* To return a matrix of the lines normal form, using a rect
* ALGORITHM
* if whichone = 1 top line
* if whichone = 2 right line
* if whichone = 3 bottom line
* if whichone = 4 left line
* OUTPUTS
* the matrix of the line
*************************************************************************/
 ULinearAlgebra::Matrix2X2 ULinearAlgebra::GetMatrix(CRect CalcRect, int whichone)
{
    // top line
    if (whichone == 1)
    {
        return PointstoLine(CalcRect.left,CalcRect.top,CalcRect.right,CalcRect.top);
    }
    // right line
    else if (whichone == 2)
    {
        return PointstoLine(CalcRect.right,CalcRect.top,CalcRect.right,CalcRect.bottom);;
    }
    // bottom line
    else if (whichone == 3)
    {
        return PointstoLine(CalcRect.left,CalcRect.bottom,CalcRect.right,CalcRect.bottom);
    }
    // left line
    else
    {
        return PointstoLine(CalcRect.left,CalcRect.top,CalcRect.left,CalcRect.bottom);
    }
    
       
}


 /************************************************************************
* Function PointListPtr GetPointsofCircle(CPoint Start, CPoint Middle, CPoint End, int nodes)
* 
* PURPOSE
* To return a list of points which can be converted into lines to form a circlular arc
* ALGORITHM
* 1) First find the line from start to middle, and then from middle to end;
* 2) Get the perpendicular of these lines
* 3) the intersection of these two perpendiculars is the center of the circle
* 4) find the radius by taking the distance from any one of the start points to the center
* 5) Once the radius and center point is found, now find to find each point, have a start angle
*    and end angle, and increment the angles, finding the circle points by using sin and cos * radius
* OUTPUTS
* a linked list of points which form a arc
*************************************************************************/

ULinearAlgebra::PointListPtr ULinearAlgebra::GetPointsofCircle(CPoint Start, CPoint Middle, CPoint End, int nodes)
{
    // find the perpindiculars midpoint lines
    Matrix2X2 Line1 = PointstoPerpMidLine(Middle,End);
    Matrix2X2 Line2 = PointstoPerpMidLine(End,Start);

    // find the intersection
    CPoint Intersection = FindIntersection(Line1,Line2);

    Intersection.y = -Intersection.y;

    // find the radius. for the second point I could use any of the three input points
    float Radius = GetDistance(Intersection,Middle);
    float R1 = GetDistance(Intersection,Start);
    float R2 = GetDistance(Intersection,End);

    int DX = End.x - Start.x;
    int DY = End.y - Start.y;

    DX = abs(DX);
    DY = abs(DY);


    double StartAngle = GetAngle(Start,Intersection);
    double MiddleAngle = GetAngle(Middle,Intersection);
    double EndAngle = GetAngle(End,Intersection);

    
    double DeltaAngle;

    // because of direction (clockwise/counterclockwise) and the fact that the angles
    // wrap around at 360 some rules need to be put in place.
    // the three extra cases had to be added because this algorithm fails when the middle.x is greater
    // then the end.x.  There are still some problems

    ///////// three extra case
    if ((StartAngle < MiddleAngle) && (MiddleAngle > EndAngle) && (Middle.x > End.x) && (Middle.y < End.y))
    {
         DeltaAngle = -(StartAngle + (360 - EndAngle))/nodes;
    }
    else if ((StartAngle > MiddleAngle) && (MiddleAngle < EndAngle) && (Middle.x > End.x) && (Middle.y > End.y))
    {
         DeltaAngle = (360.0 - (StartAngle-EndAngle))/nodes;
    }
    else if ((StartAngle < MiddleAngle) && (MiddleAngle > EndAngle) && (Middle.x > End.x) && (Middle.y > End.y))
    {
         DeltaAngle = -(StartAngle + (360 - EndAngle))/nodes;
    }
    ////////// three extra cases
    else if (StartAngle > MiddleAngle)
    {
        if (EndAngle > StartAngle)
        {
            DeltaAngle = -(StartAngle + (360 - EndAngle))/nodes;
        }
        else
        {
            DeltaAngle = -(StartAngle - EndAngle)/nodes;
        }
    }
    else
    {
        if (EndAngle > StartAngle)
        {
            DeltaAngle = (EndAngle-StartAngle)/nodes;
        }
        else
        {
            DeltaAngle = (360.0 - (StartAngle-EndAngle))/nodes;
        }
    }       


    // just in case it is a full circle, special case is needed
    if (DeltaAngle == 0)
    {
        DeltaAngle = 360.0/nodes;
    }

    // first point
    PointListPtr MainPtr = new PointList;
    PointListPtr temp;
    MainPtr->thePoint.x = Start.x;
    MainPtr->thePoint.y = Start.y;
    MainPtr->Next = NULL;

    double angle = StartAngle + DeltaAngle;

    // even though the only real lock up is when a line is placed when it is negative
    // something could happen.  If there are no valid lines in an arc, there is a screw up
    // this should prevent it.
    if (Radius > 99000) // should be lower, but I think I solved the problem which this was need to prevent
    {
        temp = new PointList;
        temp->thePoint.x = Middle.x;
        temp->thePoint.y = Middle.y;
        temp->Next = MainPtr;
        MainPtr = temp;
    }
    else
    {
    // don't do the first and last point
         for (int ix = 1; ix < nodes; ix++)
        {
            temp = new PointList;
            temp->thePoint.x = Intersection.x + Round((float)(Radius*cos(PI*angle/180.0)));
            temp->thePoint.y = Intersection.y - Round((float)(Radius*sin(PI*angle/180.0)));
            temp->Next = MainPtr;
            MainPtr = temp;
            angle += DeltaAngle;            
        }
    }

    // do last point
    temp = new PointList;
    temp->thePoint.x = End.x;
    temp->thePoint.y = End.y;
    temp->Next = MainPtr;
    MainPtr = temp;

    return MainPtr;

}






  /*  // use the x variable as the incrementer
    if (DX > DY)
    {
        float NodeDistance = (float)DX/(float)nodes;
         // the extra math is a minor offset to tip the direction to one end or another.
        float YPrev = (float)((Start.y + (float)(Middle.y - Start.y)/(float)(Middle.y+Start.y))); // used to solve the quadratic.

        PointListPtr MainPtr = new PointList;
        PointListPtr temp;
        MainPtr->thePoint.x = Start.x;
        MainPtr->thePoint.y = Start.y;
        MainPtr->Next = NULL;

        for (float ix = Start.x+NodeDistance; ix < End.x; ix = ix + NodeDistance)
        {
            float A = 1;
            float B = (float)-2*Intersection.y;
            float C = (float)((double)Intersection.y*(double)(Intersection.y) + (ix - Intersection.x)*(ix - Intersection.x) - (double)Radius*(double)Radius);

            float Y1 = (float)(-B + sqrt(B*B - 4*C))/(float)2;
            float Y2 = (float)(-B - sqrt(B*B - 4*C))/(float)2;

            // find which one is close to YPrev;

            float DY1 = Y1 - YPrev;
            float DY2 = Y2 - YPrev;

            if (DY1 < 0)
                DY1 = -DY1;
            if (DY2 < 0)
                DY2 = -DY2;

            
            if (DY1 < DY2)
            {
                temp = new PointList;
                temp->thePoint.x = Round(ix);
                temp->thePoint.y = Round(Y1);
                temp->Next = MainPtr;
                MainPtr = temp;
                YPrev = Y1;
            }
            else
            {
                temp = new PointList;
                temp->thePoint.x = Round(ix);
                temp->thePoint.y = Round(Y2);
                temp->Next = MainPtr;
                MainPtr = temp;
                YPrev = Y2;
            }
        }
        // and the last point (the end)
        temp = new PointList;
        temp->thePoint.x = End.x;
        temp->thePoint.y = End.y;
        temp->Next = MainPtr;
        MainPtr = temp;
        
        return MainPtr;
    }
    // use the y variable as the incrementer
    else
    {
        float NodeDistance = (float)DY/(float)nodes;
        // the extra math is a minor offset to tip the direction to one end or another.
        float XPrev = (float)((Start.x + (float)(Middle.x - Start.x)/(float)(Middle.x+Start.x))); // used to solve the quadratic.

        PointListPtr MainPtr = new PointList;
        PointListPtr temp;
        MainPtr->thePoint.x = Start.x;
        MainPtr->thePoint.y = Start.y;
        MainPtr->Next = NULL;

        for (float iy = Start.y+NodeDistance; iy < End.y; iy = iy + NodeDistance)
        {
            // quadratic, could be straight algebra though there would be +/- twice rather then once with this
            float A = 1;
            float B = (float)-2*Intersection.x;
            float C = (float)((double)Intersection.x*(double)(Intersection.x) + (iy - Intersection.y)*(iy - Intersection.y) - (double)Radius*(double)Radius);

            float X1 = (float)(-B + sqrt(B*B - 4*C))/(float)2;
            float X2 = (float)(-B - sqrt(B*B - 4*C))/(float)2;

          

            // find which one is close to YPrev;

            float DX1 = X1 - XPrev;
            float DX2 = X2 - XPrev;

            if (DX1 < 0)
                DX1 = -DX1;
            if (DX2 < 0)
                DX2 = -DX2;
            
            if (DX1 < DX2)
            {
                temp = new PointList;
                temp->thePoint.x = Round(X1);
                temp->thePoint.y = Round(iy);
                temp->Next = MainPtr;
                MainPtr = temp;
                XPrev = X1;
            }
            else
            {
                temp = new PointList;
                temp->thePoint.x = Round(X2);
                temp->thePoint.y = Round(iy);
                temp->Next = MainPtr;
                MainPtr = temp;
                XPrev = X2;
            }
        }
        // add the last point (the end)
        temp = new PointList;
        temp->thePoint.x = End.x;
        temp->thePoint.y = End.y;
        temp->Next = MainPtr;
        MainPtr = temp;
        
        return MainPtr;
    }*/




/************************************************************************
* Function Matrix2X2 PointtoPerpMidLine(CPoint Start, CPoint End)
* 
* PURPOSE
* To return a line, which intersects the midpoint of the input points, and is perpendicular
* to the line they form.
* USAGE
* By the circle routine
* OUTPUTS
* the matrix of the line
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoPerpMidLine(CPoint Start, CPoint End)
{

    CPoint LineStart = GetMidPoint(Start,End);

    Matrix2X2 Line;    

    if ((-End.y+Start.y) == 0)
    {
        Line.B= 0;
        Line.A = 1;
        Line.C = -(float)LineStart.x;
    }
    else
    {
        float slope = -1/((float)(-End.y+Start.y)/(float)(End.x-Start.x));
        Line.B = 1;
        Line.A = -slope;  
        Line.C = LineStart.y+LineStart.x*slope; 
    }

    
    return Line;

}

/************************************************************************
* Function CPoint GetMidPoint(CPoint Start, CPoint End)
* 
* PURPOSE
* Returns the midpoint of two points.
* USAGE
* By the circle routine
* OUTPUTS
* The midpoint
*************************************************************************/
CPoint ULinearAlgebra::GetMidPoint(CPoint Start, CPoint End)
{
    CPoint tReturn;

    tReturn.x = (Start.x+End.x)/2;
    tReturn.y = (Start.y+End.y)/2;

    return tReturn;

}


/************************************************************************
* Function int Round(float toRound)
* 
* PURPOSE
* Rounds a float to its nearest decimal.
* USAGE
* By the circle routine
* OUTPUTS
* The rounded integer
*************************************************************************/
int ULinearAlgebra::Round(float toRound)
{

    float temp = toRound - (int)(toRound);

    if (temp >= .5)
        return (int)(toRound+1);
    else
        return ((int)toRound);

}

/************************************************************************
* Function long Round(double toRound)
* 
* PURPOSE
* Rounds a float to its nearest decimal.
* USAGE
* By the circle routine
* OUTPUTS
* The rounded long integer
*************************************************************************/
long ULinearAlgebra::Round(double toRound)
{

    double temp = toRound - (long)(toRound);

    if (temp >= .5)
        return (long)(toRound+1);
    else
        return ((long)toRound);

}

/************************************************************************
* Function double GetAngle(CPoint thePoint, CPoint Center)
* 
* PURPOSE
* Gets the angle of a point of a circle (center point)
* USAGE
* By the circle routine
* INPUTS
* thePoint, the point on a circle that we want an angle
* Center, the Center point of the circle
* OUTPUTS
* The rounded integer
*************************************************************************/
double ULinearAlgebra::GetAngle(CPoint thePoint, CPoint Center)
{
    // atan returns radians
    long XLoc = thePoint.x-Center.x;
    long YLoc = +Center.y-thePoint.y;

    // special case
    if (YLoc == 0)
    {
        if ( XLoc >= 0)
            return 0.0;
        else
            return 180.0;
    }
    else
    {
        double vartan = (float)(YLoc)/(float)(XLoc);
        double Angle =  atan(vartan)*180.0/PI;

    
        // quadrent 2 error
        if ((XLoc < 0) && YLoc >= 0)
        {
            Angle = 180 + Angle;
        }
        // quadrent 3 error
        else if ((XLoc < 0) && (YLoc < 0))
        {
            Angle = 180 + Angle;
        }
        // quadrent 3 error
        else if ((XLoc >= 0) && (YLoc < 0))
        {
            Angle = 360 + Angle;
        }
        return Angle;
    }

}

/************************************************************************
* Function TransposeLine(CPoint OldStart, CPoint OldEnd, CPoint &NewStart, CPoint &NewEnd, CPoint CenterPoint, BOOL Inside)
* 
* PURPOSE
* To transpose a line on its perpidicular in the proper oirentation
* USAGE
* By on of the drafting depth functions
* INPUTS
* OldStart, OldEnd, of the old line
* CenterPoint,
* Inside, wether to use the inside or outside orientation
* OUTPUTS
* NewStart, NewEnd of the new line
* NOTE
* 1) This function wasn't thoroughly tested
* 2) This function is only used for the first line an afterward the other TransposeLine is used
*************************************************************************/
void ULinearAlgebra::TransposeLine(CPoint OldStart, CPoint OldEnd, CPoint &NewStart, CPoint &NewEnd, CPoint CenterPoint, BOOL Inside, int distance)
{
    BOOL Vertical = false;
    BOOL Horizontal = false;


    if (OldStart.x == OldEnd.x)
    {
        Horizontal = true;
    }
    else if (OldStart.y == OldEnd.y)
    {
        Vertical = true;
    }

    CPoint Start1,Start2,End1,End2;

    // get the new points
    if (Horizontal)
    {
        Start1.y = OldStart.y;
        Start2.y = OldStart.y;
        End1.y = OldEnd.y;
        End2.y = OldEnd.y;
        Start1.x = OldStart.x + distance;
        Start2.x = OldStart.x - distance;
        End1.x = OldEnd.x + distance;
        End2.x = OldEnd.x - distance;
    }
    else if (Vertical)
    {
        Start1.x = OldStart.x;
        Start2.x = OldStart.x;
        End1.x = OldEnd.x;
        End2.x = OldEnd.x;
        Start1.y = OldStart.y + distance;
        Start2.y = OldStart.y - distance;
        End1.y = OldEnd.y + distance;
        End2.y = OldEnd.y - distance;
    }
    else
    {
        double angle = GetAngle(OldEnd,OldStart); // might be the other way around, might not matter

        angle -= 90;

        Start1.x = (long)(OldStart.x + cos(angle*PI/180.0)*distance);
        Start2.x =  (long)(OldStart.x - cos(angle*PI/180.0)*distance);
        End1.x =  (long)(OldEnd.x + cos(angle*PI/180.0)*distance);
        End2.x =  (long)(OldEnd.x - cos(angle*PI/180.0)*distance);
        Start1.y = (long)(OldStart.y - sin(angle*PI/180.0)*distance);
        Start2.y = (long)(OldStart.y + sin(angle*PI/180.0)*distance);
        End1.y = (long)(OldEnd.y - sin(angle*PI/180.0)*distance);
        End2.y = (long)(OldEnd.y + sin(angle*PI/180.0)*distance);
    }

    // now find the one that is closer to the center and return it.

    float Distance1 = GetDistance(CenterPoint,Start1) + GetDistance(CenterPoint,End1);
    float Distance2 = GetDistance(CenterPoint,Start2) + GetDistance(CenterPoint,End2);

    // assign the correct points, to the return values;
    if (Distance1 > Distance2)
    {
        if (Inside)
        {
            NewStart = Start1;
            NewEnd = End1;
        }
        else
        {
            NewStart = Start2;
            NewEnd = End2;
        }
    }
    else
    {
        if (Inside)
        {
            NewStart = Start2;
            NewEnd = End2;            
        }
        else
        {
            NewStart = Start1;
            NewEnd = End1;
        }
    } 
  
}

/************************************************************************
* Function CPoint GetInsidePoint(CPoint Point1, CPoint Point2, CPoint Point3)
*
* PURPOSE
* Finds a Center Point
* USAGE
* By on of the drafting depth functions
* ALGORITHM
* Uses the Centriod of a Triangle
* INPUTS
* Any three points on a curve
* OUTPUTS
* The Center Point
*************************************************************************/
CPoint ULinearAlgebra::GetInsidePoint(CPoint Point1, CPoint Point2, CPoint Point3)
{
    CPoint tReturn;
    tReturn.x = (Point1.x + Point2.x + Point3.x)/3;
    tReturn.y = (Point1.y + Point2.y + Point3.y)/3;

    return tReturn;

}

/************************************************************************
* Function TransposeLine(CPoint OldStart, CPoint OldEnd, CPoint OldMiddle, CPoint &NewStart, CPoint &NewEnd, CPoint &NewMiddle, ULinearAlgebra::PREVLINE &PrevLine, int Distance)
* 
* PURPOSE
* To transpose a line on its perpidicular in the proper oirentation
* USAGE
* By on of the drafting depth functions
* INPUTS
* OldStart, OldEnd, OldMiddle of the old line
* PrevLine which stores which directional stuff
* OUTPUTS
* NewStart, NewEnd NewMiddle of the new line
*************************************************************************/
void ULinearAlgebra::TransposeLine(CPoint OldStart, CPoint OldEnd, CPoint OldMiddle, CPoint &NewStart, CPoint &NewEnd, CPoint &NewMiddle, ULinearAlgebra::PREVLINE &PrevLine, int Distance)
{
    BOOL Vertical = false;
    BOOL Horizontal = false;
    BOOL Reverse = false;


    if (OldStart.x == OldEnd.x)
    {
        Horizontal = true;
    }
    else if (OldStart.y == OldEnd.y)
    {
        Vertical = true;
    }

    if (PrevLine.PrevStart == OldStart)
    {
    }
    else if (PrevLine.PrevEnd == OldStart)
    {
    }
    else 
    {
        // reverse start and end
        CPoint temp;
        temp = OldStart;
        OldStart = OldEnd;
        OldEnd = temp;
        Reverse = true;
    }


    CPoint Start1,Start2,End1,End2;


    double angle = GetAngle(OldEnd,OldStart); // might be the other way around, might not matter

    if (angle < 90)
    {
        if (PrevLine.Quad == 2)
        {
            // single change in direction
            PrevLine.y = -PrevLine.y;
        }
        else if (PrevLine.Quad == 4)
        {
            //PrevLine.y = -PrevLine.y;
            PrevLine.x = -PrevLine.x;
        }
        else if (PrevLine.Quad == 3)
        {
            // double change in direction
            PrevLine.x = -PrevLine.x;
            PrevLine.y = -PrevLine.y;                   
        }
        else
        {
           
        }
        PrevLine.Quad = 1;
    }
    else if (angle < 180)
    {
        if (PrevLine.Quad == 1)
        {
             // single change in direction
             PrevLine.y = -PrevLine.y;

        }
        else if (PrevLine.Quad == 3)
        {
            PrevLine.x = -PrevLine.x;
        }

        else if (PrevLine.Quad == 4)
        {
            // double change in direction
            PrevLine.x = -PrevLine.x;
            PrevLine.y = -PrevLine.y;                   
        }
        else
        {
            // no change in direction do nothing
        }
        PrevLine.Quad = 2;
    }
    else if (angle < 270)
    {
        if (PrevLine.Quad == 1)
        {
           // double change in direction
            PrevLine.x = -PrevLine.x;
            PrevLine.y = -PrevLine.y;
        }
        else if (PrevLine.Quad == 2)
        {
            PrevLine.x = -PrevLine.x;
        }

        else if (PrevLine.Quad == 4)
        {
            PrevLine.y = -PrevLine.y; 

        }
        else
        {
            // no change in direction do nothing
        }
        PrevLine.Quad = 3;
    }
    else
    {
        if (PrevLine.Quad == 1)
        {
            PrevLine.x = -PrevLine.x;
        }
        else if (PrevLine.Quad == 2)
        {
           PrevLine.x = -PrevLine.x;
           PrevLine.y = -PrevLine.y;
        }
        else if (PrevLine.Quad == 3)
        {
            PrevLine.y = -PrevLine.y;                  
        }
        else
        {            
        }
        PrevLine.Quad = 4;
    }
    // takes it perpindicular
    if (angle >= 270)
    {
        angle = angle - 270;
    }
    else if (angle >= 180)
    {
        angle = 270-angle;
    }
    else if (angle >= 90)
    {
        angle = angle-90;
    }
    else
        angle = 90-angle;

    double Sin = sin(angle*PI/180.0);
    double Cos = cos(angle*PI/180.0);

    // the usual y reversal
    Sin = -Sin;

    NewStart.x = (long)(OldStart.x + PrevLine.x*Cos*Distance);
    NewStart.y = (long)(OldStart.y + PrevLine.y*Sin*Distance);
    NewEnd.x = (long)(OldEnd.x + PrevLine.x*Cos*Distance);
    NewEnd.y = (long)(OldEnd.y + PrevLine.y*Sin*Distance);
    NewMiddle.x = (long)(OldMiddle.x + PrevLine.x*Cos*Distance);
    NewMiddle.y = (long)(OldMiddle.y + PrevLine.y*Sin*Distance);


    if (!Reverse)
    {

        CPoint temp;
        temp = NewStart;
        NewStart = NewEnd;
        NewEnd = temp;

    }

    PrevLine.PrevStart = OldStart;
    PrevLine.PrevEnd = OldEnd;
    

}


/************************************************************************
* Function Circle ULinearAlgebra::GetCircle(CPoint Start, CPoint Middle, CPoint End)

*
* PURPOSE
* To return the equation of a circle
* RETURNS
* A structure containing the radius and center of a circle.
*************************************************************************/
ULinearAlgebra::Circle ULinearAlgebra::GetCircle(CPoint Start, CPoint Middle, CPoint End)
{

    Circle tReturn;
     // find the perpindiculars midpoint lines
    Matrix2X2 Line1 = PointstoPerpMidLine(Middle,End);
    Matrix2X2 Line2 = PointstoPerpMidLine(End,Start);

    // find the intersection
    tReturn.Center = FindIntersection(Line1,Line2);

    tReturn.Center.y = -tReturn.Center.y;

    // find the radius. for the second point I could use any of the three input points
    tReturn.Radius = GetDistance(tReturn.Center,Middle);

    return tReturn;

    

}

/************************************************************************
* Function Circle ULinearAlgebra::GetCircle(CPoint Start, CPoint Middle, CPoint End, int RInc)

*
* PURPOSE
* To return the equation of a circle, with a bigger or smaller radius as defined by RInc
* RETURNS
* A structure containing the radius and center of a circle. and ,  &NewMiddle, which contains the new middle
* The newstart and newend are created on the intersection but newmiddle has to be correct.
*************************************************************************/
ULinearAlgebra::Circle ULinearAlgebra::GetCircle(CPoint Start, CPoint Middle, CPoint End, int RInc, CPoint &NewMiddle)
{

    Circle tReturn;
     // find the perpindiculars midpoint lines
    Matrix2X2 Line1 = PointstoPerpMidLine(Middle,End);
    Matrix2X2 Line2 = PointstoPerpMidLine(End,Start);

    // find the intersection
    tReturn.Center = FindIntersection(Line1,Line2);

    tReturn.Center.y = -tReturn.Center.y;

    // find the radius. for the second point I could use any of the three input points
    float Radius = GetDistance(tReturn.Center,Middle);
    tReturn.Radius = Radius + RInc;

    double XInc;
    double YInc;

    double Angle = GetAngle(Middle,tReturn.Center);

    XInc = tReturn.Radius*cos(Angle*PI/180.0);
    YInc = -tReturn.Radius*sin(Angle*PI/180.0);
    NewMiddle.x = (long)(tReturn.Center.x + XInc);
    NewMiddle.y = (long)(tReturn.Center.y + YInc);

    return tReturn;    

}

/************************************************************************
* Function CPoint FindIntersection(ULinearAlgebra::Matrix2X2 Line, ULinearAlgebra::Circle circle, CPoint Closest)
*
* PURPOSE
* To return the point of intersection between a circle and line
* NOTE
* the parameter closest picks which point to return (two for the quadratic)
* RETURNS
* The point of intersection
*************************************************************************/
CPoint ULinearAlgebra::FindIntersection(ULinearAlgebra::Matrix2X2 Line, ULinearAlgebra::Circle circle, CPoint Closest)
{
    CPoint One,Two;  
    
    #ifdef USEFLOAT
        float A,B,C;
    #else
        double A,B,C;
    #endif  
    
    if (Line.A == 0)
    {
        // horizontal line
        One.y = (long)-Line.C;
        Two.y = (long)-Line.C;

        A = 1;
        B = (float)(-2*circle.Center.x);
        C = sqr(circle.Center.x)-sqr(circle.Radius)+sqr(-Line.C - circle.Center.y);

        One.x = (long)((-B + sqrt(sqr(B)-4*A*C)) / (2*A));
        Two.x = (long)((-B - sqrt(sqr(B)-4*A*C)) / (2*A));
        
    }
    else if (Line.B == 0)
    {
         // vertical line
        One.x = (long)-Line.C;
        Two.x = (long)-Line.C;

        A = 1;
        B = (float)(-2*circle.Center.y);
        C =  sqr(circle.Center.y)-sqr(circle.Radius)+sqr(-Line.C - circle.Center.x);

        One.y = (long)((-B + sqrt(sqr(B)-4*A*C)) / (2*A));
        Two.y = (long)((-B - sqrt(sqr(B)-4*A*C)) / (2*A));
    }
    else
    {
        A = (float)(Line.B*Line.B)/(float)(Line.A*Line.A) + 1;
        B = (float)((2*Line.B/Line.A)*(float)(Line.C/Line.A + circle.Center.x) - (float)(2*circle.Center.y));
        C = (float)(sqr(Line.C / Line.A + circle.Center.x) + sqr((float)circle.Center.y) - sqr(circle.Radius));

        One.y = (long)((-B + sqrt(sqr(B)-4*A*C)) / (2*A));
        Two.y = (long)((-B - sqrt(sqr(B)-4*A*C)) / (2*A));

        One.x = (long)((-Line.B*One.y-Line.C)/Line.A);
        Two.x = (long)((-Line.B*Two.y-Line.C)/Line.A);
    }

  

  

    float distance1 = GetDistance(One,Closest);
    float distance2 = GetDistance(Two,Closest);

    if (distance1 < distance2)
        return One;
    else
        return Two;  

}

// takes the square of a number
float ULinearAlgebra::sqr(float input)
{
    return input*input;

}

// takes the square of a number
float ULinearAlgebra::sqr(long input)
{
    return (float)(input*input);

}

// takes the square of a number
double ULinearAlgebra::sqr(double input)
{
    return (input*input);

}

/************************************************************************
* Function BOOL CheckifLineGoesinRect(ULinearAlgebra::Matrix2X2 Line, CRect Rect)
*
* PURPOSE
* To check if a line passes through a rectangle.
* USAGE
* the quadtrees select objects if in box
* ALGORITHM
* 1) Create a line out of each line in the rectange,
* 2) Check for intersection
* 3) If at least one intersection point is valid (on the border of the rectange) then return true;
* NOTE
* A possible easier way would be to use the FindIntersection(Start,End,Rect); directly but it has some problems 
* with this exact algorithm
*
*************************************************************************/
BOOL ULinearAlgebra::CheckifLineGoesinRect(CPoint LineStart, CPoint LineEnd, CRect Rect)
{
    Matrix2X2 Line = PointstoLine(LineStart,LineEnd);

    Matrix2X2 Top,Left,Right,Bottom;

    CPoint Start,End;    
    CPoint i[4];

    Start.x = Rect.left;
    Start.y = Rect.top;
    End.x = Rect.right;
    End.y = Rect.top;
    Top = PointstoLine(Start,End);

    Start.x = Rect.left;
    Start.y = Rect.bottom;
    End.x = Rect.right;
    End.y = Rect.bottom;
    Bottom = PointstoLine(Start,End);

    Start.x = Rect.left;
    Start.y = Rect.top;
    End.x = Rect.left;
    End.y = Rect.bottom;
    Left = PointstoLine(Start,End);

    Start.x = Rect.right;
    Start.y = Rect.top;
    End.x = Rect.right;
    End.y = Rect.bottom;
    Right = PointstoLine(Start,End);

    i[0] = FindIntersection(Line,Top);
    i[1] = FindIntersection(Line,Bottom);
    i[2] = FindIntersection(Line,Left);
    i[3] = FindIntersection(Line,Right);

    CPoint BorderTopLeft;
    CPoint BorderBottomRight;

    // setup of bound of the intersection
    if (LineStart.x > LineEnd.x)
    {
        BorderTopLeft.x = LineEnd.x;
        BorderBottomRight.x = LineStart.x;
    }
    else
    {
        BorderTopLeft.x = LineStart.x;
        BorderBottomRight.x = LineEnd.x;
    }

    if (LineStart.y > LineEnd.y)
    {
        BorderTopLeft.y = LineEnd.y;
        BorderBottomRight.y = LineStart.y;
    }
    else
    {
        BorderTopLeft.y = LineStart.y;
        BorderBottomRight.y = LineEnd.y;
    }

    // check if any of the intersections are actually in the rectangle.
    for (int ix = 0; ix < 4; ix++)
    {
        // is it in the selection rect?
        if ((i[ix].x <= Rect.right) && (i[ix].x >= Rect.left)
            && (i[ix].y >= Rect.top) && (i[ix].y <= Rect.bottom))
        {
            if ((i[ix].x <= BorderBottomRight.x) && (i[ix].x >= BorderTopLeft.x)
            && (i[ix].y >= BorderTopLeft.y) && (i[ix].y <= BorderBottomRight.y))
            {
                return true;
            }
        }
    }

    return false; 

}

/************************************************************************
* Function CPoint RotatePoint(CPoint OldPoint, CPoint Center, int Angle)
*
* PURPOSE
* To rotate a point about a center point and angle
* USAGE
* By the rotation function
* ALGORITHM
* 1) Find the old angle and distance from the center to the point
* 2) Create a new angle by adding Angle
* 3) Find rotated points by using the new angle and the distance
*
*************************************************************************/
CPoint ULinearAlgebra::RotatePoint(CPoint OldPoint, CPoint Center, double Angle)
{
    CPoint tReturn;

    double NewAngle = GetAngle(OldPoint,Center) + Angle;    

    float Distance = GetDistance(OldPoint,Center);    


    tReturn.x = Round(Center.x + Distance*cos(NewAngle*PI/180.0));
    tReturn.y = Round(Center.y - Distance*sin(NewAngle*PI/180.0));

    return tReturn;    

}

/************************************************************************
* Function FPoint RotatePoint(FPoint OldPoint, FPoint Center, double Angle)
*
* PURPOSE
* To rotate a point about a center point and angle
* USAGE
* By the rotation function
* ALGORITHM
* 1) Find the old angle and distance from the center to the point
* 2) Create a new angle by adding Angle
* 3) Find rotated points by using the new angle and the distance
*
*************************************************************************/
FPoint ULinearAlgebra::RotatePoint(FPoint OldPoint, FPoint Center, double Angle)
{
    FPoint tReturn;

    double NewAngle = GetAngle(OldPoint,Center) + Angle;    

    float Distance = GetDistance(OldPoint,Center);

    #ifdef USEFLOAT
        tReturn.x = (float)(Center.x + Distance*cos(NewAngle*PI/180.0));
        tReturn.y = (float)(Center.y - Distance*sin(NewAngle*PI/180.0));
    #else
        tReturn.x = (Center.x + Distance*cos(NewAngle*PI/180.0));
        tReturn.y = (Center.y - Distance*sin(NewAngle*PI/180.0));
    #endif
   
    return tReturn;    

}

/************************************************************************
* Function double GetAngle(FPoint thePoint, FPoint Center)
* 
* PURPOSE
* Gets the angle of a point of a circle (center point)
* USAGE
* By the circle routine
* INPUTS
* thePoint, the point on a circle that we want an angle
* Center, the Center point of the circle
* OUTPUTS
* The rounded integer
*************************************************************************/
double ULinearAlgebra::GetAngle(FPoint thePoint, FPoint Center)
{
    // atan returns radians
    double XLoc = thePoint.x-Center.x;
    double YLoc = +Center.y-thePoint.y;

    // special case
    if (YLoc == 0)
    {
        if ( XLoc >= 0)
            return 0.0;
        else
            return 180.0;
    }
    else
    {
        double vartan = (float)(YLoc)/(float)(XLoc);
        double Angle =  atan(vartan)*180.0/PI;

    
        // quadrent 2 error
        if ((XLoc < 0) && YLoc >= 0)
        {
            Angle = 180 + Angle;
        }
        // quadrent 3 error
        else if ((XLoc < 0) && (YLoc < 0))
        {
            Angle = 180 + Angle;
        }
        // quadrent 3 error
        else if ((XLoc >= 0) && (YLoc < 0))
        {
            Angle = 360 + Angle;
        }
        return Angle;
    }

}

/************************************************************************
* Function float GetDistance(FPoint one, FPoint two)
* 
* returns the distance of two points
*************************************************************************/
float ULinearAlgebra::GetDistance(FPoint one, FPoint two)
{
    // note that the use of floats is because for large slopes, the distance is going to be large
    // and it is too large to handle with the CPoints Longs
    return (float)sqrt((double)(one.x-two.x)*(double)(one.x-two.x)+(double)(one.y-two.y)*(double)(one.y-two.y));

}


/************************************************************************
* Function BOOL IsBetweenLines(Matrix2X2 One, Matrix2X2 Two, FPoint Point, float distance)
* 
* PURPOSE
* To check if a point is between two lines
* USAGE
* Collision Detection
* ALGORITHM
* 1) Take distance between lines and points
* 2) The two distances are the same then return true (vertical movement on a veritcal line)
* 3) If either of the distances are zero then return true
* 4) Else the distances must be of opposite sign then return true
*************************************************************************/
BOOL ULinearAlgebra::IsBetweenLines(ULinearAlgebra::Matrix2X2 One, ULinearAlgebra::Matrix2X2 Two, FPoint Point)
{

    float temp1 = GetDistance(One,Point);
    float temp2 = GetDistance(Two,Point);


     // the model fails is the lines are parallel, since the distances are equal.  So use
        // this extra check to see if the point is actually on the line, which fixes
        // this special case.
    if (isPointonLine(Point,One) || isPointonLine(Point,Two))
        return true;
    // if the slopes are still parallel, then the motion must be on the line, so if the point isn't on the line there is
    // an error
    else if ((One.A == Two.A) && (One.B == Two.B) && (One.C == Two.C))
    {    
        return false;
    }
    else if (temp1 == temp2)
        return true;
    else if (IsEqual(temp1,0,(float)SMALLPLUSMINUS) || IsEqual(temp2,0,(float)SMALLPLUSMINUS))
        return true;
    else if (temp1*temp2 < 0) // opposite sign?
        return true;

    if (One.B == 0 || Two.B == 0)
    {
        int asdf = 30;
        asdf = 50;
        return true;
    }

    return false;
}

/************************************************************************
* Function BOOL GetDistanceTwoPLLLines(FPoint Start1, FPoint End1, FPoint Start2, FPoint End2)
* 
* PURPOSE
* To check the distance between two parallel lines
* USAGE
* Collision Detection
* ALGORITHM
*************************************************************************/
float ULinearAlgebra::GetDistanceTwoPLLLines(FPoint Start1, FPoint End1, FPoint Start2, FPoint End2)
{
    Matrix2X2 Line1,Line2,PerpLine;
    FPoint Int1,Int2;

    Line1 = PointstoLine(Start1,Start2);
    Line2 = PointstoLine(End1,End2);
    PerpLine = PointstoPerpMidLine(Start1,Start2);

    Int1 = FindIntersectionF(Line1,PerpLine);
    Int2 = FindIntersectionF(Line2,PerpLine);

    return GetDistance(Int1,Int2);       

}

// wrapper for above
float ULinearAlgebra::GetDistanceTwoPLLLines(FPoint Start1, FPoint End1, FPoint Delta)
{
    FPoint Extra1, Extra2;
    
    Extra1.x = Start1.x + Delta.x;
    Extra2.x = End1.x + Delta.x;
    Extra1.y = Start1.y + Delta.y;
    Extra2.y = End1.y + Delta.y;

    return GetDistanceTwoPLLLines(Start1,Extra1,End1,Extra2);
}  

/************************************************************************
* Function Matrix2X2 PointstoLine(CPoint start, CPoint end)
* 
* PURPOSE
* To convert two x,y points to the line normal format Ax+By+c
* INPUTS
* Start, the start of the line
* End, the end of the line
* OUTPUTS
* A matrix with the point normal format
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoLine(FPoint start, FPoint end)
{
    Matrix2X2 Line;    

    if ((end.x-start.x) == 0)
    {
        Line.B= 0;
        Line.A = 1;
        Line.C = -(float)start.x;
    }
    else
    {
        float slope = (float)(end.y-start.y)/(float)(end.x-start.x);
        Line.B = 1;
        Line.A = (float)-slope;  
        Line.C = (float)(-start.y+start.x*slope);
    }

    // at this point it should be a 90 degree line and rounding errors will occur because of it.
    // note this isn't fully tested so it could cause problems
    if (Line.A > 999999)
    {
        Line.C = Line.C/Line.A;
        Line.A = 1;
        Line.B = 0;
    }

    return Line;
}

/************************************************************************
* Function Matrix2X2 PointtoPerpMidLine(FPoint Start, FPoint End)
* 
* PURPOSE
* To return a line, which intersects the midpoint of the input points, and is perpendicular
* to the line they form.
* USAGE
* By collision detection
* OUTPUTS
* the matrix of the line
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoPerpMidLine(FPoint Start, FPoint End)
{

    FPoint LineStart = GetMidPoint(Start,End);

    Matrix2X2 Line;    

    if ((-End.y+Start.y) == 0)
    {
        Line.B= 0;
        Line.A = 1;
        Line.C = -(float)LineStart.x;
    }
    else
    {
        float slope = -1/((float)(-End.y+Start.y)/(float)(End.x-Start.x));
        Line.B = 1;
        Line.A = -slope;  
        Line.C = (float)(LineStart.y+LineStart.x*slope); 
    }

    
    return Line;

}

/************************************************************************
* Function CPoint FindIntersection(Matrix2X2 Line1, Matrix2X2 Line2)
* 
* PURPOSE
* To return the point of the intersection of two lines
* ALGORITHM
* Kramer rule in linear algebra
* INPUTS
* Two lines in matrix format
* OUTPUTS
* the point of intersection
*************************************************************************/
FPoint ULinearAlgebra::FindIntersectionF(ULinearAlgebra::Matrix2X2 Line1, ULinearAlgebra::Matrix2X2 Line2)
{
    FPoint rPoint;
    // find the bottom determinate
    #ifdef USEFLOAT
        float DBottom = Line1.A*Line2.B-Line2.A*Line1.B;
    #else
        double DBottom = Line1.A*Line2.B-Line2.A*Line1.B;
    #endif

    // find the x determinate
    rPoint.x = (((-Line1.C)*Line2.B-(-Line2.C)*Line1.B)/DBottom);
    rPoint.y = ((((Line1.A *(-Line2.C)-Line2.A*(-Line1.C))/DBottom)));

    return rPoint;    

}


/************************************************************************
* Function FPoint GetMidPoint(FPoint Start, FPoint End)
* 
* PURPOSE
* Returns the midpoint of two points.
* USAGE
* By the circle routine
* OUTPUTS
* The midpoint
*************************************************************************/
FPoint ULinearAlgebra::GetMidPoint(FPoint Start, FPoint End)
{
    FPoint tReturn;

    tReturn.x = (Start.x+End.x)/2;
    tReturn.y = (Start.y+End.y)/2;

    return tReturn;

}

/************************************************************************
* Function float GetDistance(Matrix2X2 Line, FPoint Point)
* 
* PURPOSE
* To return the distance of a point and a line
* INPUTS
* Line, a line in normal format
* Point, a point
* OUTPUTS
* the distance of the intersection of a point and a line.
*************************************************************************/
float ULinearAlgebra::GetDistance(ULinearAlgebra::Matrix2X2 Line, FPoint Point)
{
    return (float)((Line.A*(double)Point.x+Line.B*(double)Point.y+Line.C)/(double)sqrt((double)Line.A*(double)Line.A+(double)Line.B*(double)Line.B));

}


/************************************************************************
* Function BOOL IsEqual(float One, Float Two, float plusminus)
* 
* PURPOSE
* To see if two numbers are the same + or - a certain amount
* INPUTS
* One, Distance 1
* Two, Distance 2
* plusminus, maximum plus or minus
* RETURNS
* True or False
*************************************************************************/
BOOL ULinearAlgebra::IsEqual(float One, float Two, float plusminus)
{
    float Delta = One - Two;
    
    if (Delta < 0)
        Delta = -Delta;
    if (plusminus < 0)
        plusminus = -plusminus;

    if (Delta < plusminus)
        return true;

    return false;

}

/************************************************************************
* Function Matrix2X2 PointtoLine(FPoint Start, Matrix2X2 Slope)
* 
* PURPOSE
* To create a line out of a point and slope (perpidicular to it)
* INPUTS
* Start, The starting point
* Slope, The Slope of the line, given by a the other line.
* RETURNS
* Returns a new line
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoPerpLine(FPoint Start, ULinearAlgebra::Matrix2X2 Slope)
{
     Matrix2X2 Line;
     
     // horionztal line, want vertical line
    if (Slope.A == 0)
    {
        Line.B= 0;
        Line.A = 1;
        Line.C = -(float)Start.x;

    }    
    else
    {
        float slope = (float)Slope.B/(float)(Slope.A);
        Line.B = 1;
        Line.A = (float)-slope;  
        Line.C = (float)(-Start.y+Start.x*slope);
    }

    return Line;

}


float ULinearAlgebra::Abs(float num)
{
    if (num < 0)
        return -num;
    else
        return num;

}

double ULinearAlgebra::Abs(double num)
{
    if (num < 0)
        return -num;
    else
        return num;

}

/************************************************************************
* Function Matrix2X2 PointoLine(FPoint Start, Matrix2X2 Slope)
* 
* PURPOSE
* To convert one point and a slope given by a line to a parallel line
* INPUTS
* Start, the start of the line
* Slope, the slope of the line
* OUTPUTS
* A matrix with the point normal format
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoLine(FPoint Start, ULinearAlgebra::Matrix2X2 Slope)
{
    Matrix2X2 Line;    

    if (Slope.B == 0)
    {
        Line.B= 0;
        Line.A = 1;
        Line.C = -(float)Start.x;
    }
    else
    {
        #ifdef USEFLOAT
            float slope = (-Slope.A)/Slope.B;
        #else
            double slope = (-Slope.A)/Slope.B;
        #endif
        Line.B = 1;
        Line.A = (float)-slope;  
        Line.C = (float)(-Start.y+Start.x*slope);
    }

    return Line;
}

/************************************************************************
* Function Matrix2X2 PointoLine(FPoint Start, Matrix2X2 Slope)
* 
* PURPOSE
* Returns true if the point is on the line
*************************************************************************/
BOOL ULinearAlgebra::isPointonLine(FPoint Point, ULinearAlgebra::Matrix2X2 Line)
{
    double sum = Line.A*Point.x + Line.B*Point.y;
    if (IsEqual((float)sum,(float)-Line.C,(float)PLUSMINUS))
        return true;
    else
        return false;
}

/************************************************************************
* Function float GetDistance(CPoint LineStart, CPoint LineEnd, FPoint thePoint)
* 
* PURPOSE
* To return the perpindicular distance from the point to the line, but it must
* be valid.  Otherwise it sends the shortest distance to either endpoint.
*************************************************************************/
float ULinearAlgebra::GetDistance(CPoint LineStart, CPoint LineEnd, FPoint thePoint)
{
    Matrix2X2 Line = PointstoLine(LineStart,LineEnd);
    Matrix2X2 Intercepter = PointstoPerpLine(thePoint,Line);

    FPoint Intersection = FindIntersectionF(Line,Intercepter);

    // check if it is one the line
    if (((Intersection.x <= LineStart.x) && (Intersection.x >= LineEnd.x))
        || ((Intersection.x >= LineStart.x) && (Intersection.x <= LineEnd.x)))
    {
        if (((Intersection.y <= LineStart.y) && (Intersection.y >= LineEnd.y))
        || ((Intersection.y >= LineStart.y) && (Intersection.y <= LineEnd.y)))
        {     
            return GetDistance(Intersection,thePoint);
        }
    }

    // o.k not on the line pick the short of the endpoints

    FPoint Tempo;
    Tempo.x = (float)LineStart.x;
    Tempo.y = (float)LineStart.y;

    float StartD = GetDistance(Tempo,thePoint);

    Tempo.x = (float)LineEnd.x;
    Tempo.y = (float)LineEnd.y;
    float EndD = GetDistance(Tempo,thePoint);

    if (StartD < EndD)
        return StartD;
    else
        return EndD;    

}

// conversion function
FPoint::operator= (CPoint RHS)
{
    x = (float)RHS.x;
    y = (float)RHS.y;
}

FPoint::operator= (GPOINT RHS)
{
    x = (float)RHS.x;
    y = (float)RHS.y;
}

// conversion function
FPoint::FPoint(GPOINT RHS)
{
    this->x = (float)RHS.x;
    this->y = (float)RHS.y;
}

// conversion function
FPoint::FPoint(CPoint RHS)
{
    this->x = (float)RHS.x;
    this->y = (float)RHS.y;
}

// conversion function
FPoint::FPoint()
{
    this->x = 0;
    this->y = 0;
}

// conversion function
FPoint::FPoint(float X, float Y)
{
    this->x = X;
    this->y = Y;
}

	

// conversion function
FRect::operator= (CRect RHS)
{
    top = (float)RHS.top;
    bottom = (float)RHS.bottom;
    left = (float)RHS.left;
    right = (float)RHS.right;
}

// conversion function
CCRect::operator= (FRect RHS)
{
    top = (long)RHS.top;
    bottom = (long)RHS.bottom;
    left =  (long)RHS.left;
    right = (long)RHS.right;
}
// conversion function
CCRect::CCRect(FRect RHS)
{
    this->top = (long)RHS.top;
    this->bottom = (long)RHS.bottom;
    this->left =  (long)RHS.left;
    this->right = (long)RHS.right;
}

// conversion function
CCPoint::operator= (FPoint RHS)
{
    x = (long)RHS.x;
    y = (long)RHS.y;
}
// conversion function
CCPoint::CCPoint(FPoint RHS)
{
    this->x = (long)RHS.x;
    this->y = (long)RHS.y;
}


/************************************************************************
* Function FormRect(FPoint Start, FPoint End)
* 
* PURPOSE
* To form a rectange out of two points making sure the upper left is lesser then
* in terms of x and y then the lower right
*************************************************************************/
FRect ULinearAlgebra::FormRect(FPoint Start, FPoint End)
{
    FRect toReturn;

    // find the top
    toReturn.top = Start.y;
    if (End.y < toReturn.top)
        toReturn.top = End.y;
    
    // find the left
    toReturn.left = Start.x;
    if (End.x < toReturn.left)
        toReturn.left = End.x;
    
    // find the bottom
    toReturn.bottom = Start.y;
    if (End.y > toReturn.bottom)
        toReturn.bottom = End.y;
   
    // find the right
    toReturn.right = Start.x;
    if (End.x > toReturn.right)
        toReturn.right = End.x;

    return toReturn;

}

/************************************************************************
* Function FormRect(FPoint Start, FPoint End)
* 
* PURPOSE
* To form a rectange out of two points making sure the upper left is lesser then
* in terms of x and y then the lower right
*************************************************************************/
CRect ULinearAlgebra::FormRect(CPoint Start, CPoint End)
{
    CRect toReturn;

    // find the top
    toReturn.top = (long)Start.y;
    if (End.y < toReturn.top)
        toReturn.top = (long)End.y;
    
    // find the left
    toReturn.left = (long)Start.x;
    if (End.x < toReturn.left)
        toReturn.left = (long)End.x;
    
    // find the bottom
    toReturn.bottom = (long)Start.y;
    if (End.y > toReturn.bottom)
        toReturn.bottom = (long)End.y;
   
    // find the right
    toReturn.right = (long)Start.x;
    if (End.x > toReturn.right)
        toReturn.right = (long)End.x;
   
    return toReturn;

}

/************************************************************************
* Function FRect FormRectF(FPoint ObjectStart, FPoint ObjectEnd, FPoint Delta) 
*
* PURPOSE
* To return a rectange which contains all of the points in between the line and its delta
* INPUTS
* ObjectStart, the start of the line which is moving
* ObjectEnd, the end of the line which is moving
* DeltaStart, the start + displacement of the line
* DeltaEnd, the end + displacement of the line 
* RETURNS
* The rect (in real coordinates) of the area incompassed
**************************************************************************/
FRect ULinearAlgebra::FormRectF(FPoint ObjectStart, FPoint ObjectEnd, FPoint DeltaStart, FPoint DeltaEnd)
{
   // FPoint DeltaStart, DeltaEnd;

    FRect toReturn;

    // find the top
    toReturn.top = ObjectStart.y;
    if (ObjectEnd.y < toReturn.top)
        toReturn.top = ObjectEnd.y;
    if (DeltaStart.y < toReturn.top)
        toReturn.top = DeltaStart.y;
    if (DeltaEnd.y < toReturn.top)
        toReturn.top = DeltaEnd.y;

    // find the left
    toReturn.left = ObjectStart.x;
    if (ObjectEnd.x < toReturn.left)
        toReturn.left = ObjectEnd.x;
    if (DeltaStart.x < toReturn.left)
        toReturn.left = DeltaStart.x;
    if (DeltaEnd.x < toReturn.left)
        toReturn.left = DeltaEnd.x;

    // find the bottom
    toReturn.bottom = ObjectStart.y;
    if (ObjectEnd.y > toReturn.bottom)
        toReturn.bottom = ObjectEnd.y;
    if (DeltaStart.y > toReturn.bottom)
        toReturn.bottom = DeltaStart.y;
    if (DeltaEnd.y > toReturn.bottom)
        toReturn.bottom = DeltaEnd.y;

    // find the right
    toReturn.right = ObjectStart.x;
    if (ObjectEnd.x > toReturn.right)
        toReturn.right = ObjectEnd.x;
    if (DeltaStart.x > toReturn.right)
        toReturn.right = DeltaStart.x;
    if (DeltaEnd.x > toReturn.right)
        toReturn.right = DeltaEnd.x;

    return toReturn;

}

/************************************************************************
* Function CRect FormRect(CPoint ObjectStart, CPoint ObjectEnd, CPoint DeltaStart, CPoint DeltaEnd)
*
* PURPOSE
* To return a rectange which contains all of the points in between the line and its delta
* INPUTS
* ObjectStart, the start of the line which is moving
* ObjectEnd, the end of the line which is moving
* DeltaStart, the start + displacement of the line
* DeltaEnd, the end + displacement of the line 
* RETURNS
* The rect (in real coordinates) of the area incompassed
**************************************************************************/
CRect ULinearAlgebra::FormRect(CPoint ObjectStart, CPoint ObjectEnd, CPoint DeltaStart, CPoint DeltaEnd)
{
   // FPoint DeltaStart, DeltaEnd;

    CRect toReturn;

    // find the top
    toReturn.top = ObjectStart.y;
    if (ObjectEnd.y < toReturn.top)
        toReturn.top = ObjectEnd.y;
    if (DeltaStart.y < toReturn.top)
        toReturn.top = DeltaStart.y;
    if (DeltaEnd.y < toReturn.top)
        toReturn.top = DeltaEnd.y;

    // find the left
    toReturn.left = ObjectStart.x;
    if (ObjectEnd.x < toReturn.left)
        toReturn.left = ObjectEnd.x;
    if (DeltaStart.x < toReturn.left)
        toReturn.left = DeltaStart.x;
    if (DeltaEnd.x < toReturn.left)
        toReturn.left = DeltaEnd.x;

    // find the bottom
    toReturn.bottom = ObjectStart.y;
    if (ObjectEnd.y > toReturn.bottom)
        toReturn.bottom = ObjectEnd.y;
    if (DeltaStart.y > toReturn.bottom)
        toReturn.bottom = DeltaStart.y;
    if (DeltaEnd.y > toReturn.bottom)
        toReturn.bottom = DeltaEnd.y;

    // find the right
    toReturn.right = ObjectStart.x;
    if (ObjectEnd.x > toReturn.right)
        toReturn.right = ObjectEnd.x;
    if (DeltaStart.x > toReturn.right)
        toReturn.right = DeltaStart.x;
    if (DeltaEnd.x > toReturn.right)
        toReturn.right = DeltaEnd.x;

    return toReturn;

}

/************************************************************************
* Function Matrix2X2 PointstoLine(FPoint Point, float Angle)
* 
* PURPOSE
* To convert one point and an angle to a line
* INPUTS
* Start, the start of the line
* Angle, the Angle of the line
* OUTPUTS
* A matrix with the point normal format
*************************************************************************/
ULinearAlgebra::Matrix2X2 ULinearAlgebra::PointstoLine(FPoint Point, float Angle)
{
    FPoint Point2;
    int SomeDistance = 100;
    Point2.x = Point.x + (float)(SomeDistance*cos(Angle*PI/180));
    Point2.y = Point.y - (float)(SomeDistance*sin(Angle*PI/180));

    return PointstoLine(Point,Point2);
    

}

/************************************************************************
* Function RenderClip(CRect Bounds, long &XStart, long &YStart, long &XEnd, long &YEnd)
*
* PURPOSE
* This function is used to clip a line so that the start and end are the start
* and end of the screen.  This solves the problems when a long line is scaled in a small area
* and the start and end values become so large that the GDI will not render them
* USAGE
* When drawing a line
* ALGORITHM
* 1) Check if any one of the points go beyond the bounds.  If yes begin
* 2) For each bounds (top,bottom,left,right) Form a line
* 3) Check the intersection with each line.
     a) If it is in the bounds, check if it is on the line
     b) If it isn't on a line, it means that the line endpoint to be used
* 4) Do this for all four bounds.  If there hasn't been any matching points it means
     that the line is not is the bounds.  Return -1;
*************************************************************************/
void ULinearAlgebra::ClipLine(CRect Bounds, long &XStart, long &YStart, long &XEnd, long &YEnd)
{
    Bounds.right -=Bounds.left;
    Bounds.left -=Bounds.left;
    Bounds.bottom -= Bounds.top;
    Bounds.top -= Bounds.top;

    // check if any of the points are out of the bounds
    if ((XStart < Bounds.left) || (XStart > Bounds.right)
        || (XEnd < Bounds.left) || (XEnd > Bounds.right)
        || (YStart < Bounds.top) || (YStart > Bounds.bottom)
        || (YEnd < Bounds.top) || (YEnd > Bounds.bottom))
    {
        // another check to see if the line is completly out of range
        // neccessary because some below code doesn't account for this case
        if (((XStart > Bounds.right) && (XEnd > Bounds.right)) ||
           ((XStart < Bounds.left) && (XEnd < Bounds.left)) ||
           ((YStart > Bounds.bottom) && (YEnd > Bounds.bottom)) ||
           ((YStart < Bounds.top) && (YEnd < Bounds.top)))
        {
            XStart = -1;
            YStart = -1;
            XEnd = -1;
            YEnd = -1;
            
            return;
        }


        FPoint One,Two;

        float XStartF = (float)XStart;
        float YStartF = (float)YStart;
        float XEndF = (float)XEnd;
        float YEndF = (float)YEnd;

        One.x = XStartF;
        One.y = YStartF;
        Two.x = XEndF;
        Two.y = YEndF;

        FRect LineBounds = FormRect(One,Two);

        CPoint Point1,Point2;

        Point1.x = -1;

        // if they are find ones in the bounds
        Matrix2X2 Line = PointstoLine(XStart,YStart,XEnd,YEnd);


        Matrix2X2 LeftLine = PointstoLine(Bounds.left,Bounds.top,Bounds.left,Bounds.bottom);
        Matrix2X2 RightLine = PointstoLine(Bounds.right,Bounds.top,Bounds.right,Bounds.bottom);
        Matrix2X2 TopLine = PointstoLine(Bounds.right,Bounds.top,Bounds.left,Bounds.top);
        Matrix2X2 BottomLine = PointstoLine(Bounds.right,Bounds.bottom,Bounds.left,Bounds.bottom);

        FPoint Int;

        // check the left intersection
        Int = FindIntersection(Line,LeftLine);
        if (Int.x != -1)
        {
            if (InRect(Int,Bounds))
            {
                // if the point is inside rect then  use original point
                if (InRect(Int,LineBounds))
                {
                }
                else
                {
                    if (XStart < XEnd)
                    {
                        Int.x = XStartF;
                        Int.y = YStartF;
                    }
                    else
                    {
                        Int.x = XEndF;
                        Int.y = YEndF;
                    }
                }
            
                Point1.x = Round(Int.x);
                Point1.y = Round(Int.y);
            }
          
        }

        // check the right intersection
        Int = FindIntersection(Line,RightLine);
        if (Int.x != -1)
        {
            if (InRect(Int,Bounds))
            {
             // if the point is inside rect then  use original point
                if (InRect(Int,LineBounds))
                {
                }
                else
                {
                    if (XStart > XEnd)
                    {
                        Int.x = XStartF;
                        Int.y = YStartF;
                    }
                    else
                    {
                        Int.x = XEndF;
                        Int.y = YEndF;
                    }
                }
          
                if (Point1.x == -1)
                {
                    Point1.x = Round(Int.x);
                    Point1.y = Round(Int.y);
                }
                else
                {
                    Point2.x = Round(Int.x);
                    Point2.y = Round(Int.y);
                }

            }
        }

        // check the top intersection
        Int = FindIntersection(Line,TopLine);
        if (Int.x != -1)
        {
            if (InRect(Int,Bounds))
            {
             // if the point is inside rect then  use original point
                if (InRect(Int,LineBounds))
                {
                }
                else
                {
                    if (YStart < YEnd)
                    {
                        Int.x = XStartF;
                        Int.y = YStartF;
                    }
                    else
                    {
                        Int.x = XEndF;
                        Int.y = YEndF;
                    }
                }
          
                if (Point1.x == -1)
                {
                    Point1.x = Round(Int.x);
                    Point1.y = Round(Int.y);
                }
                else
                {
                    Point2.x = Round(Int.x);
                    Point2.y = Round(Int.y);
                }
            }
        }

        // check the bottom intersection
        Int = FindIntersection(Line,BottomLine);
        if (Int.x != -1)
        {
            if (InRect(Int,Bounds))
            {
                  // if the point is inside rect then  use original point
                if (InRect(Int,LineBounds))
                {
                }
                else
                {
                    if (YStart > YEnd)
                    {
                        Int.x = XStartF;
                        Int.y = YStartF;
                    }
                    else
                    {
                        Int.x = XEndF;
                        Int.y = YEndF;
                    }
                }
            
                if (Point1.x == -1)
                {
                    Point1.x = Round(Int.x);
                    Point1.y = Round(Int.y);
                }
                else
                {
                    Point2.x = Round(Int.x);
                    Point2.y = Round(Int.y);
                }
            }
        }

        if (Point1.x == -1)
        {
            // the line isn't valid draw nothing
            XStart = -1;
            YStart = -1;
            XEnd = -1;
            YEnd = -1;
        }
        else
        {
            XStart = Point1.x;
            YStart = Point1.y;
            XEnd = Point2.x;
            YEnd = Point2.y;
        }
        return;

    }


}

// wrapper see above
void ULinearAlgebra::ClipLine(CRect Bounds, CPoint &Start, CPoint &End)
{
    ClipLine(Bounds,Start.x,Start.y,End.x,End.y);
}

/************************************************************************
* Function float Mod360(float Angle)
* 
* PURPOSE
* To module an angle by 360
*************************************************************************/
float ULinearAlgebra::Mod360(float Angle)
{
    
    float tReturn = Angle;

    while (tReturn > 360)
    {
        tReturn = tReturn - 360;       
    }
    while (tReturn < 0)
    {
        tReturn = 360 + tReturn;
    }

    return tReturn;
}

/************************************************************************
* Function void FormRect(FRect &Rect, FPoint Start, FPoint End)
* 
* PURPOSE
* Interativly Forms a rectange which converts all of the points
*************************************************************************/
void ULinearAlgebra::FormRect(FRect &Rect, FPoint Start, FPoint End)
{
    if (Start.x > End.x)
    {
        if (Start.x > Rect.right)        
            Rect.right = Start.x;        
        if (End.x < Rect.left)
            Rect.left = End.x;
    }
    else
    {
        if (End.x > Rect.right)
            Rect.right = End.x;
        if (Start.x < Rect.left)
            Rect.left = Start.x;
    }
    if (Start.y > End.y)
    {
        if (Start.y > Rect.bottom)
            Rect.bottom = Start.y;
        if (End.y < Rect.top)
            Rect.top = End.y;
    }
    else
    {
        if (Start.y < Rect.top)
            Rect.top = Start.y;
        if (End.y > Rect.bottom)
            Rect.bottom = End.y;
    }           

}

/************************************************************************
* Function float Sgn(float Input)
* 
* PURPOSE
* The sign function
*************************************************************************/
float ULinearAlgebra::Sgn(float Input)
{
    if (Input > 0)
        return 1.0f;
    else if (Input < 0)
        return -1.0f;

    return 1.0f;

}
