//---------------------------------------------------------------------------
//#pragma hdrstop //Управление пред компиляцией
//#include "stdafx.h"
//---------------------------------------------------------------------------
#include <stdexcept>      // std::invalid_argument
#include <stdlib.h> //rand
#include <math.h>

//---------------------------------------------------------------------------
#include "mathTools.h"
#include "object3d.h"
//---------------------------------------------------------------------------
//Большее из 2х интеджеров
int MaxI4(int v1,int v2)
{	if(v1>v2) return v1; else return v2;
}
//---------------------------------------------------------------------------
/*int MaxI4_2(int v1,int v2)
{	if(v1>v2) return v1; else return v2;
}*/
//------------------------------------------------------------------------------
//Преобразовать HEX символ в число
int hexCharToInt(char input)
{
  if(input >= '0' && input <= '9')
    return input - '0';
  if(input >= 'A' && input <= 'F')
    return input - 'A' + 10;
  if(input >= 'a' && input <= 'f')
    return input - 'a' + 10;
  throw std::invalid_argument("Invalid input string");
}
//------------------------------------------------------------------------------
//проверка значения бита в массиве (нумерация с лева на право) pos - 0..n
bool testBit(const unsigned char *mas,const unsigned char pos)
{
	unsigned char mask=128;
	unsigned char loc=pos/8;
	mask=mask >> (pos-loc*8);
	return (mask & mas[loc])==mask;
}
//------------------------------------------------------------------------------
//установить заданный бит в 1 или в 0
void setBit(unsigned char *mas,const unsigned char pos,bool val)
{
	unsigned char mask=128;
	unsigned char loc=pos/8;
	mask=mask >> (pos-loc*8);
	if(val) mas[loc]=mas[loc] | mask;
	else
	{
		mask=~mask; //Отрицание
		mas[loc]=mas[loc] & mask;
	}
}
//---------------------------------------------------------------------------
//Установит знначение бита в заданную позицию
///pos - Позиция 7..0
uint1 setBitVal(uint1 bit,uint1 pos,bool val)
{
	uint1 v=1;
	v=0x1<<pos;
	if(val)
	{
		bit=bit | v;
	}else
	{
		v=v ^ 0xFF;
		bit=bit & v;
	}
	return bit;
}
//---------------------------------------------------------------------------
//Вернёт значение бита на заданной позиции
///pos - Позиция 7..0
bool getBitVal(uint1 bit,uint1 pos)
{
	uint1 v=1;
	v=v<<pos;
	return (bit & v) == v;
}
//---------------------------------------------------------------------------
//Скорость ком порта по номеру
long getBaudRate(long s)
{	
	if(s==0) return 1200;
	if(s==1) return 2400;
	if(s==2) return 4800;
	if(s==3) return 9600;
	if(s==4) return 19200;
	if(s==5) return 38400;
	if(s==6) return 57600;
	if(s==7) return 115200;
	return 0;
}
//---------------------------------------------------------------------------
//Шифровать XOR
void Encrypt(unsigned char* key, unsigned short keylen, unsigned char* data, unsigned short datalen)
{
	for(int i=0;i<datalen;i++)
		for(int j=0;j<keylen;j++)
			data[i]= data[i] ^ key[j];
}
//---------------------------------------------------------------------------
//Дешифровать XOR
void Decrypt(unsigned char* key, unsigned short keylen, unsigned char* data, unsigned short datalen)
{
  /*res = "";
  // побайтное шифрование без ключа
  for(int i = 1; i <= key.Length(); i++)
    res += " ";
  res[1] = key[1] ^ key[key.Length()];
  for(int i = 1; i < key.Length();i++){
    res[i + 1] = key[i+1] ^ res[i];
  }*/
}
//------------------------------------------------------------------------------
//Простое вычисление CRC (как для GPS)
unsigned char CRC8(const char *pcBlock, unsigned char len, unsigned char crc)
{
	for(unsigned char i=0; i < len; i++)
		crc^=pcBlock[i];
	return crc;
}
// ----------------------------------------------------------------------------
/*
  Name  : CRC-16 CCITT
  Poly  : 0x1021    x^16 + x^12 + x^5 + 1
  Init  : 0xFFFF
  Revert: false
  XorOut: 0x0000
  Check : 0x29B1 ("123456789")
  MaxLen: 4095 байт (32767 бит) - обнаружение
    одинарных, двойных, тройных и всех нечетных ошибок
*/
unsigned short Crc16(unsigned char ch, unsigned short crc)
{
	unsigned char i;
	crc ^= ch << 8;
	for (i = 0; i < 8; i++)
		crc = crc & 0x8000 ? (crc << 1) ^ 0x1021 : crc << 1;
	return crc;
}
// ----------------------------------------------------------------------------
/*
  Name  : CRC-16 CCITT
  Poly  : 0x1021    x^16 + x^12 + x^5 + 1
  Init  : 0xFFFF
  Revert: false
  XorOut: 0x0000
  Check : 0x29B1 ("123456789")
  MaxLen: 4095 байт (32767 бит) - обнаружение
    одинарных, двойных, тройных и всех нечетных ошибок
*/
unsigned short Crc16(const char *pcBlock, unsigned short len, unsigned short crc)
{
    //unsigned short crc = 0xFFFF;
    unsigned char i;
 
    while (len--)
    {
        crc ^= *pcBlock++ << 8;
        for (i = 0; i < 8; i++)
            crc = crc & 0x8000 ? (crc << 1) ^ 0x1021 : crc << 1;
    }
 
    return crc;
}
//------------------------------------------------------------------------------
int RandInt(int a, int b)
{
	return a + rand() % (b - a + 1);
}
//------------------------------------------------------------------------------
//Расстояние от начала декартовых координат до заданной точки
float fnGetDistans(RfPointXYZ p)
{
	return sqrt(sqr(p.x) + sqr(p.y) + sqr(p.z));
}
//------------------------------------------------------------------------------
//Расстояние между 2мя точками на плоскости в декартовых координатах
float fnGetDistans2d(RfPointXY PointXY1, RfPointXY PointXY2)
{
	return sqrt(fabs(sqr(PointXY1.x - PointXY2.x) + sqr(PointXY1.y - PointXY2.y)));
}
//------------------------------------------------------------------------------
//Расстояние между 2мя точками в пространстве в декартовых координатах
float fnGetDistans3d(RfPointXYZ PointXYZ1, RfPointXYZ PointXYZ2)
{
	return sqrt(fabs(sqr(PointXYZ1.x - PointXYZ2.x) + sqr(PointXYZ1.y - PointXYZ2.y) + sqr(PointXYZ1.z - PointXYZ2.z)));
}
//------------------------------------------------------------------------------
float sqr(float val)
{
	return val*val;
}
//------------------------------------------------------------------------------
double sqr(double val)
{
	return val*val;
}
//------------------------------------------------------------------------------
int roundf2(float val)
{
	if (val>0)
	{
		if (val - floor(val) >= 0.5) return (int)floor(val) + 1; else return (int)floor(val);
	}
	else
		if (val <= 0)
		{
			if (val - floor(val) <= 0.5) return (int)floor(val) - 1; else return (int)floor(val);
		}
	return 0;
}
//------------------------------------------------------------------------------
//Чтоб угол был в пределах 0..2PI градусов
double fnCorrectAngle(double angle)
{
	double num;
	if (angle>2 * M_PI)
	{
		num = floor(angle / (2.0*M_PI));
		angle = (angle - 2.0*M_PI*num);
	};
	if (angle<0)
	{
		num = floor(-angle / (2.0*M_PI)) + 1;
		angle = ((2.0*M_PI)*num + angle);
	}
	return angle;
};
//------------------------------------------------------------------------------
//Корректировать угол не дает превысить 2PI, приводит к положительному значению если оно отрицательно -45°=360°-45°=315°
double CorrectAngleRad(double angle)
{
	int num; //количество оборотов
	if (angle>2 * PI)
	{
		num = (int)floor(angle / (2 * PI));
		angle = (angle - 2 * PI*num);
	}
	else
		if (angle<0)
		{
			num = (int)floor(-angle / (2 * PI)) + 1;
			angle = ((2 * PI)*num + angle);
		}
	return angle;
}
/*************************************************************************
Проверка лежат ли две точки по одну сторону от прямой.
Если одна или обе точки лежат на прямой, функция возвращает True.

function IsPointsOnOneSide(
x1,y1,x2,y2:real;
xL1,yL1,xL2,yL2:real):Boolean

This function check is 2 points lies on same side from line
(x1,y1),(x2,y2) checked points
(xL1,yL1),(xL2,yL2)-two points for definition line;
*************************************************************************/
bool isPointsOnOneSide(const float x1,
	const float y1,
	const float x2,
	const float y2,
	const float xL1,
	const float yL1,
	const float xL2,
	const float yL2)
{
	if ((xL1<xL2) || (xL1>xL2))
	{
		return (y1 - yL1 + (yL1 - yL2)*(x1 - xL1) / (xL2 - xL1))*(y2 - yL1 + (yL1 - yL2)*(x2 - xL1) / (xL2 - xL1)) >= 0;
	}
	else
	{
		return (x1 - xL1)*(x2 - xL2) >= 0;
	}
}
//------------------------------------------------------------------------------
//Просчитать нормаль к плоскости плоскость заданна 3мя точками против часовой стрелки
void CalcNormals(float x0, float y0, float z0, float x1, float y1, float z1, float x2, float y2, float z2, float &xr, float &yr, float &zr)
{
	float a, b, c, r;
	a = (y1 - y0)*(z2 - z0) - (y2 - y0)*(z1 - z0);
	b = (x2 - x0)*(z1 - z0) - (x1 - x0)*(z2 - z0);
	c = (x1 - x0)*(y2 - y0) - (y1 - y0)*(x2 - x0);
	r = sqrt(a*a + b*b + c*c);
	if (r != 0) r = 1 / r;
	xr = a*r;
	yr = b*r;
	zr = c*r;
}
//------------------------------------------------------------------------------
//уравнение окружности по 3м точкам
// a - x, b - y, r - радиус
void fnCalcCircle(RfPointXYZ p1, RfPointXYZ p2, RfPointXYZ p3, float &a, float &b, float &r)
{
	double a1, b1, a2, b2, c1, c2;
	double k1, k2;

	a1 = 2 * (p1.x - p2.x);
	b1 = 2 * (p2.y - p1.y);
	c1 = ((p2.y - p1.y)*p2.y + (p2.y - p1.y)*p1.y) - ((p1.x - p2.x)*p1.x + (p1.x - p2.x)*p2.x);
	a2 = 2 * (p3.x - p2.x);
	b2 = 2 * (p2.y - p3.y);
	c2 = ((p2.y - p3.y)*p2.y + (p2.y - p3.y)*p3.y) - ((p3.x - p2.x)*p3.x + (p3.x - p2.x)*p2.x);
	if (b1 != 0)
	{
		k1 = (b2*a1) / b1 - a2;
		if (k1 == 0) //3 точки в 1 линии
		{
			r = 0;
			return;
		}
		a = (float)((c2 - (b2*c1) / b1) / k1);
		b = (float)((c1 + a1*a) / b1);
	}
	else
		if (b2 != 0)
		{
			k2 = (b1*a2) / b2 - a1;
			if (k2 == 0) //3 точки в 1 линии
			{
				r = 0;
				return;
			}
			a = (float)((c1 - (b1*c2) / b2) / k2);
			b = (float)((c2 + a2*a) / b2);
		}
		else
		{ //всё в 1 точке
			r = 0;
		}
	r = sqrt((a - p1.x)*(a - p1.x) + (b - p1.y)*(b - p1.y));
}
//------------------------------------------------------------------------------
/** Из декартовых в полярные
x,y,z - вектор исходящий из 0,0,0 координат
AngleH - угол в плоскости XY по часовой в радианах начиная с +X
AngleV - угол по вертикали от плоскости XY до Z, +Z плюсовые значения -Z минусовые
r - радиус
*/
void NormalInPolar3d(float x, float y, float z, float &AngleH, float &AngleV, float &r)
{
	if (x == 0.0) x = 0.0000001f;
	r = sqrt(x*x + y*y + z*z);
	AngleH = fabs(atan(y / x));
	if ((x <= 0) && (y <= 0)) AngleH = (float)(PI - AngleH);
	if ((x<0) && (y>0)) AngleH = (float)(AngleH + PI);
	if ((x >= 0) && (y >= 0)) AngleH = (float)(2.0f*PI - AngleH);
	if (AngleH>PI*2.0f) AngleH = (float)(AngleH - PI*2.0f);
	AngleV = fabs(atan(z / sqrt(x*x + y*y)));
	if (z<0) AngleV = -AngleV;
}
//------------------------------------------------------------------------------
/** Из декартовых в полярные
x,y - вектор исходящий из 0,0 координат
Angle - угол в плоскости XY по часовой в радианах начиная с +X
r - радиус
*/
void NormalInPolar2d(float x, float y, float &Angle, float &r)
{
	if (x == 0.0) x = 0.0000001f;
	r = sqrt(x*x + y*y);
	Angle = fabs(atan(y / x));
	if ((x <= 0) && (y <= 0)) Angle = (float)(PI - Angle);
	if ((x<0) && (y>0)) Angle = (float)(Angle + PI);
	if ((x >= 0) && (y >= 0)) Angle = (float)(2.0f*PI - Angle);
	if (Angle>PI*2.0f) Angle = (float)(Angle - PI*2.0f);
}
//------------------------------------------------------------------------------
void PolarInNormal3d(double AngleH, double AngleV, double r, float &x, float &y, float &z)
{
	AngleH = fnCorrectAngle(AngleH);
	AngleV = fnCorrectAngle(AngleV);

	double r2, a;
	char nx, ny;
	nx = 1; ny = 1; a = 0;
	z = (float)(r*sin(AngleV));
	r2 = r*sin(M_PI / 2.0 - fabs(AngleV));
	if (AngleH == 0) AngleH = 0.000000001;
	if (AngleH <= M_PI / 2.0)
	{
		a = AngleH; nx = 1; ny = -1;
	}
	if ((AngleH>M_PI / 2.0) && (AngleH <= M_PI))
	{
		a = M_PI - AngleH; nx = -1; ny = -1;
	}
	if ((AngleH>M_PI) && (AngleH <= 3.0 / 2.0*M_PI))
	{
		a = AngleH - M_PI; nx = -1; ny = 1;
	}
	if ((AngleH>3.0 / 2.0*M_PI) && (AngleH<2.0*M_PI))
	{
		a = 2.0*M_PI - AngleH; nx = 1; ny = 1;
	}
	x = (float)(r2*sin(M_PI / 2.0 - a)*nx);
	y = (float)(r2*sin(a)*ny);
}
//------------------------------------------------------------------------------
void PolarInNormal3d(double AngleH, double AngleV, double r, double &x, double &y, double &z)
{
	AngleH = fnCorrectAngle(AngleH);
	AngleV = fnCorrectAngle(AngleV);

	double r2, a;
	char nx, ny;
	nx = 1; ny = 1; a = 0;
	z = r*sin(AngleV);
	r2 = r*sin(M_PI / 2.0 - fabs(AngleV));
	if (AngleH == 0) AngleH = 0.000000001;
	if (AngleH <= M_PI / 2.0)
	{
		a = AngleH; nx = 1; ny = -1;
	}
	if ((AngleH>M_PI / 2.0) && (AngleH <= M_PI))
	{
		a = M_PI - AngleH; nx = -1; ny = -1;
	}
	if ((AngleH>M_PI) && (AngleH <= 3.0 / 2.0*M_PI))
	{
		a = AngleH - M_PI; nx = -1; ny = 1;
	}
	if ((AngleH>3.0 / 2.0*M_PI) && (AngleH<2.0*M_PI))
	{
		a = 2.0*M_PI - AngleH; nx = 1; ny = 1;
	}
	x = r2*sin(M_PI / 2.0 - a)*nx;
	y = r2*sin(a)*ny;
}
//------------------------------------------------------------------------------
/* уравнение плоскости по 3м точкам
P0,P1,P2 -предпологаемые точки на плоскости
a,b,c,d - коэфициенты уравнения плоскости
*/
void CalcPlane(RfPointXYZ p0, RfPointXYZ p1, RfPointXYZ p2, float &a, float &b, float &c, float &d)
{
	//просчитаем коефициенты в уравнении плоскости треугольника
	a = ((p1.y - p0.y)*(p2.z - p0.z) - (p2.y - p0.y)*(p1.z - p0.z));
	b = ((p2.x - p0.x)*(p1.z - p0.z) - (p1.x - p0.x)*(p2.z - p0.z));
	c = ((p1.x - p0.x)*(p2.y - p0.y) - (p1.y - p0.y)*(p2.x - p0.x));
	d = -(p1.y - p0.y)*(p2.z - p0.z)*p0.x - (p1.x - p0.x)*(p2.y - p0.y)*p0.z - (p2.x - p0.x)*(p1.z - p0.z)*p0.y;
	d = d + (p1.y - p0.y)*(p2.x - p0.x)*p0.z + (p1.x - p0.x)*(p2.z - p0.z)*p0.y + (p2.y - p0.y)*(p1.z - p0.z)*p0.x;
	//  d=-a*p0.x-b*p0.y-c*p0.z;
}
//------------------------------------------------------------------------------
/*уравнение плоскости по вектору и точке на плоскости
v - вектор
p - точка на плоскости
a,b,c,d - коэфициенты уравнения плоскости
*/
void CalcPlane2(RfPointXYZ v, RfPointXYZ p, float &a, float &b, float &c, float &d)
{
	normalized(v); //В единичный вектор
	a = v.x;
	b = v.y;
	c = v.z;//c=v.x;
	d = -a*p.x - b*p.y - c*p.z;
}
//------------------------------------------------------------------------------
/*уравнение плоскости по вектору и точке на плоскости
v - вектор
p - точка на плоскости
a,b,c,d - коэфициенты уравнения плоскости
*/
void CalcPlane2(RdPointXYZ v, RdPointXYZ p, double &a, double &b, double &c, double &d)
{
	normalized(v);
	a = v.x;
	b = v.y;
	c = v.z;//c=v.x;
	d = -a*p.x - b*p.y - c*p.z;
}
//------------------------------------------------------------------------------
float Max(float val1, float val2)
{
	if (val1>val2)
		return val1;
	else
		return val2;
}
//------------------------------------------------------------------------------
float Min(float val1, float val2)
{
	if (val1<val2)
		return val1;
	else
		return val2;
}
//------------------------------------------------------------------------------
RfPointXYZ getMaxPoint(RfPointXYZ point1, RfPointXYZ point2, RfPointXYZ point3)
{
	RfPointXYZ result;
	result.x = Max(Max(point1.x, point2.x), point3.x);
	result.y = Max(Max(point1.y, point2.y), point3.y);
	result.z = Max(Max(point1.z, point2.z), point3.z);
	return result;
}
//------------------------------------------------------------------------------
RfPointXYZ getMinPoint(RfPointXYZ point1, RfPointXYZ point2, RfPointXYZ point3)
{
	RfPointXYZ result;
	result.x = Min(Min(point1.x, point2.x), point3.x);
	result.y = Min(Min(point1.y, point2.y), point3.y);
	result.z = Min(Min(point1.z, point2.z), point3.z);
	return result;
}
//------------------------------------------------------------------------------
//Находиться ли точка внутри куба заданного минимальной и максимальной точками включая эти точки
bool inBox(RfPointXYZ pMin, RfPointXYZ pMax, RfPointXYZ p)
{
	if (pMin.x <= p.x && pMin.y <= p.y && pMin.z <= p.z && pMax.x >= p.x && pMax.y >= p.y && pMax.z >= p.z) return true;
	return false;
}
//------------------------------------------------------------------------------
//Неходиться ли точка в нутри куба заданного минимальной и максимальной точками
bool inBox(RfPointXYZ pMin, RfPointXYZ pMax, RdPointXYZ p)
{
	if (pMin.x <= p.x && pMin.y <= p.y && pMin.z <= p.z && pMax.x >= p.x && pMax.y >= p.y && pMax.z >= p.z) return true;
	return false;
}
//------------------------------------------------------------------------------
//Перевести из градусов в радианы
double DegToRad(double deg)
{
	return CorrectAngleRad(deg / 180.0*M_PI);
}
//------------------------------------------------------------------------------
//Перевести из радиан в градусы
double RadToDeg(double deg)
{
	return CorrectAngleRad(deg) / M_PI*180.0;
}
//------------------------------------------------------------------------------
RfPointXYZ getMaxPointXYZ(RfPointXYZ poiny1, RfPointXYZ poiny2, RfPointXYZ poiny3)
{
	RfPointXYZ PointXYZ;
	PointXYZ.x = Max(Max(poiny1.x, poiny2.x), poiny3.x);
	PointXYZ.y = Max(Max(poiny1.y, poiny2.y), poiny3.y);
	PointXYZ.z = Max(Max(poiny1.z, poiny2.z), poiny3.z);
	return PointXYZ;
};
//------------------------------------------------------------------------------
RfPointXYZ getMinPointXYZ(RfPointXYZ poiny1, RfPointXYZ poiny2, RfPointXYZ poiny3)
{
	RfPointXYZ PointXYZ;
	PointXYZ.x = Min(Min(poiny1.x, poiny2.x), poiny3.x);
	PointXYZ.y = Min(Min(poiny1.y, poiny2.y), poiny3.y);
	PointXYZ.z = Min(Min(poiny1.z, poiny2.z), poiny3.z);
	return PointXYZ;
};
//------------------------------------------------------------------------------
//Приблизить либо отдалить первую точку ко второй на заданное растояние
RfPointXY ApproachPoint2d(RfPointXY PointXY1, RfPointXY PointXY2, float Distans)
{
	float dist;
	RfPointXY LengthXY;

	LengthXY.x = fabs(PointXY1.x - PointXY2.x);
	LengthXY.y = fabs(PointXY1.y - PointXY2.y);
	dist = sqrt(sqr(LengthXY.x) + sqr(LengthXY.y));

	dist = Distans / dist;
	PointXY2.x = PointXY1.x*(1 - dist) + PointXY2.x*dist;
	PointXY2.y = PointXY1.y*(1 - dist) + PointXY2.y*dist;

	return PointXY2;
}
//------------------------------------------------------------------------------
//Приблизить или отдолить первую точку ко второй на указанное растояние
RfPointXYZ ApproachPoint3d(RfPointXYZ PointXYZ1, RfPointXYZ PointXYZ2, float Distans)
{
	float dist;
	RfPointXYZ LengthXYZ;

	LengthXYZ.x = fabs(PointXYZ1.x - PointXYZ2.x);
	LengthXYZ.y = fabs(PointXYZ1.y - PointXYZ2.y);
	LengthXYZ.z = fabs(PointXYZ1.z - PointXYZ2.z);

	dist = sqrt(sqr(LengthXYZ.x) + sqr(LengthXYZ.y) + sqr(LengthXYZ.z));
	dist = Distans / dist;
	PointXYZ2.x = PointXYZ1.x*(1 - dist) + PointXYZ2.x*dist;
	PointXYZ2.y = PointXYZ1.y*(1 - dist) + PointXYZ2.y*dist;
	PointXYZ2.z = PointXYZ1.z*(1 - dist) + PointXYZ2.z*dist;

	return PointXYZ2;
}
//------------------------------------------------------------------------------
//задать расстояние на которой должна находиться первая точка относительно второй
RfPointXYZ setDistancePoint3d(RfPointXYZ PointXYZ1, RfPointXYZ PointXYZ2, float Distans)
{
	float dist;
	RfPointXYZ LengthXYZ;

	LengthXYZ.x = fabs(PointXYZ1.x - PointXYZ2.x);
	LengthXYZ.y = fabs(PointXYZ1.y - PointXYZ2.y);
	LengthXYZ.z = fabs(PointXYZ1.z - PointXYZ2.z);

	dist = sqrt(sqr(LengthXYZ.x) + sqr(LengthXYZ.y) + sqr(LengthXYZ.z));
	dist = 1 - Distans / dist;
	PointXYZ2.x = PointXYZ1.x*(1 - dist) + PointXYZ2.x*dist;
	PointXYZ2.y = PointXYZ1.y*(1 - dist) + PointXYZ2.y*dist;
	PointXYZ2.z = PointXYZ1.z*(1 - dist) + PointXYZ2.z*dist;

	return PointXYZ2;
}
//------------------------------------------------------------------------------
//Угол между 2мя точкам и X координатой против часовой в радианах
float fnGetAngle(RfPointXY CenterPoint, RfPointXY ResearchedPoint)
{
	double A, B, C, Angle;
	Angle = 0;
	A = fabs(ResearchedPoint.y - CenterPoint.y)*1.0;
	B = fabs(ResearchedPoint.x - CenterPoint.x)*1.0;
	C = sqrt(A*A + B*B);
	if (C>0)
	{
		Angle = asin(1.00*B / C);
		if ((ResearchedPoint.x>CenterPoint.x) && (ResearchedPoint.y <= CenterPoint.y)) Angle = M_PI / 2.0 - Angle; else
			if ((ResearchedPoint.x <= CenterPoint.x) && (ResearchedPoint.y<CenterPoint.y)) Angle = M_PI / 2.0 + Angle; else
				if ((ResearchedPoint.x<CenterPoint.x) && (ResearchedPoint.y >= CenterPoint.y)) Angle = 3.0 / 2.0*M_PI - Angle; else
					if ((ResearchedPoint.x >= CenterPoint.x) && (ResearchedPoint.y>CenterPoint.y)) Angle = 3.0 / 2.0*M_PI + Angle;
	}
	return (float)Angle;
}
//------------------------------------------------------------------------------
//Угол между 2мя точкам и X координатой против часовой в радианах
float fnGetAngleXY(RfPointXYZ CenterPoint, RfPointXYZ ResearchedPoint)
{
	double A, B, C, Angle;
	Angle = 0;
	A = fabs(ResearchedPoint.y - CenterPoint.y)*1.0;
	B = fabs(ResearchedPoint.x - CenterPoint.x)*1.0;
	C = sqrt(A*A + B*B);
	if (C>0)
	{
		Angle = asin(1.00*B / C);
		if ((ResearchedPoint.x>CenterPoint.x) && (ResearchedPoint.y <= CenterPoint.y)) Angle = M_PI / 2.0 - Angle; else
			if ((ResearchedPoint.x <= CenterPoint.x) && (ResearchedPoint.y<CenterPoint.y)) Angle = M_PI / 2.0 + Angle; else
				if ((ResearchedPoint.x<CenterPoint.x) && (ResearchedPoint.y >= CenterPoint.y)) Angle = 3.0 / 2.0*M_PI - Angle; else
					if ((ResearchedPoint.x >= CenterPoint.x) && (ResearchedPoint.y>CenterPoint.y)) Angle = 3.0 / 2.0*M_PI + Angle;
	}
	return (float)Angle;
}
//------------------------------------------------------------------------------
//Угол между 2мя точкам и X координатой против часовой в радианах
float fnGetAngle(float cx, float cy, float x, float y)
{
	RfPointXY CenterPoint, ResearchedPoint;
	CenterPoint.x = cx;
	CenterPoint.y = cy;
	ResearchedPoint.x = x;
	ResearchedPoint.y = y;
	return fnGetAngle(CenterPoint, ResearchedPoint);
}
//------------------------------------------------------------------------------
//разбить поверхность на треугольники в плоскости XY
//inPos - позиция с которой начинаем читать count точек из PointsXYZ
//count - количество точек читаемых из в PointsXYZ
//PointsXYZ - ссылка на массив точек
//outPos - позиция с которой надо записывать в Vector3i
//Vector3i - ссылка на созданный вектор в который будем писать count-2 треугольников
bool Tessellated2d(unsigned int inPos, unsigned int count, RfPointXYZ *PointsXYZ, unsigned int outPos, RsFacesABC *Vector3i)
{
	unsigned int i;
	struct TessTri
	{
		TessTri *next; //ссылка на следующую точку
		unsigned int n; //номер точки
	};
	TessTri *tessTri0, *tessTri1, *tessTri2;
	//точки по которым мы собираемся пройтись представляем в виде связанного списка
	tessTri0 = new TessTri;
	tessTri0->n = inPos;
	tessTri1 = tessTri0;
	for (i = inPos + 1; i<inPos + count; i++)
	{
		tessTri2 = new TessTri;
		tessTri2->n = i;
		tessTri1->next = tessTri2;
		tessTri1 = tessTri2;
	}
	tessTri1->next = tessTri0;
	//алгоритм работает пока не будут открыты 3 точки
	int p[3];
	unsigned int pos = 0; //найденно треугольников
	i = 0;
	while (tessTri0 != tessTri0->next->next) //пока текущйй и следующий не ссылаються друг на друга (протреангулированно)
	{
		if (i >= count) break;//если сделали круг и не создали ни одного треугольника то дело дрянь выходим из цикла
							  //выбираем 3 точки которые пытаемся соеденить
		p[0] = tessTri0->n;
		p[1] = tessTri0->next->n;
		p[2] = tessTri0->next->next->n;
		//проверка на вогнутость
		float Angle1 = fnGetAngleXY(PointsXYZ[p[1]], PointsXYZ[p[0]]);
		float Angle2 = fnGetAngleXY(PointsXYZ[p[1]], PointsXYZ[p[2]]);
		//вогнутые и лижащие на одной линии пропускаем
		if (fnCorrectAngle(Angle2 - Angle1) <= PI)
		{
			//проверка на вхождение остальных точек в этот 3х угольник
			//максимальная и минимальная точки треугольника
			RfPointXYZ PointMax = getMaxPoint(PointsXYZ[p[0]], PointsXYZ[p[1]], PointsXYZ[p[2]]);
			RfPointXYZ PointMin = getMinPoint(PointsXYZ[p[0]], PointsXYZ[p[1]], PointsXYZ[p[2]]);
			//точка в не 3х угольника
			bool bool1 = false; bool bool2 = false; bool bool3 = false;
			tessTri1 = tessTri0->next->next->next; //первая точка за пределпми проверяемого треугольника
			while (tessTri0 != tessTri1)
			{
				RfPointXY Point;
				Point.x = PointsXYZ[tessTri1->n].x; //сравниваем каждую точку
				Point.y = PointsXYZ[tessTri1->n].y;
				if (!((PointMin.x <= Point.x) && (PointMax.x >= Point.x) && (PointMin.y <= Point.y) && (PointMax.y >= Point.y)))
				{
					bool1 = false; bool2 = false; bool3 = false;
					tessTri1 = tessTri1->next;
					continue;
				}
				bool1 = isPointsOnOneSide(Point.x, Point.y, PointsXYZ[p[0]].x, PointsXYZ[p[0]].y, PointsXYZ[p[1]].x, PointsXYZ[p[1]].y, PointsXYZ[p[2]].x, PointsXYZ[p[2]].y);
				bool2 = isPointsOnOneSide(Point.x, Point.y, PointsXYZ[p[1]].x, PointsXYZ[p[1]].y, PointsXYZ[p[2]].x, PointsXYZ[p[2]].y, PointsXYZ[p[0]].x, PointsXYZ[p[0]].y);
				bool3 = isPointsOnOneSide(Point.x, Point.y, PointsXYZ[p[2]].x, PointsXYZ[p[2]].y, PointsXYZ[p[0]].x, PointsXYZ[p[0]].y, PointsXYZ[p[1]].x, PointsXYZ[p[1]].y);
				if (bool1 && bool2 && bool3) break; //попалась точка в нутри 3х угольника
				tessTri1 = tessTri1->next;
			}
			if (!(bool1 && bool2 && bool3))
			{
				//удаляем точку из списка и в следующий раз её не брать
				tessTri1 = tessTri0->next;
				tessTri0->next = tessTri0->next->next;
				delete tessTri1;

				i = 0; //обнуляем счётчик круга без новых треугольников
				count--; //точек стало меньше

				Vector3i[outPos + pos].a = p[0];
				Vector3i[outPos + pos].b = p[1];
				Vector3i[outPos + pos].c = p[2];
				pos++;
				continue; //если удачный треугольник то не двигаемся вероятно что с этого положения можно найти ещё
			}
		}
		i++;
		tessTri0 = tessTri0->next; //если не удачный треугольник то двигаемся на 1 точку в перёд
	}
	//осталось 2 не использованные точки значит всё OK
	if (count == 2)
	{
		delete tessTri0->next;
		delete tessTri0;
		return true;
	}
	else //не использованные будут ссылаться на 0ю точку
	{
		while (tessTri0 != tessTri0->next)
		{
			tessTri1 = tessTri0->next;
			tessTri0->next = tessTri0->next->next;
			delete tessTri1;
		}
		delete tessTri0;
		for (i = outPos + pos; i<outPos + pos + count - 2; i++)
		{
			Vector3i[i].a = 0;
			Vector3i[i].b = 0;
			Vector3i[i].c = 0;
		}
		return false;
	}
}
//------------------------------------------------------------------------------
//Триангуляция набора точек по делоне
void TrianglesDeloneXY(unsigned int countP, RfPointXYZ *points, TSimpleList<RTriangle*>* ListEdge)
{
	RTriangle *Triangle;
	RSotrPoint *sp, *sp1, *sp2;
	RfPointXYZ addpoint, rez;
	bool b;
	unsigned int i, j, k;

	if (countP<3) return;
	TSimpleList<RSotrPoint*>* listsort = new TSimpleList<RSotrPoint*>(10, true); //список открытых отсортированных точек по часовой стрелки

																				 //первоначальный 3х угольник
	Triangle = new RTriangle;
	Triangle->del = false;
	Triangle->pnt[0] = 0;
	Triangle->pnt[1] = 1;
	Triangle->pnt[2] = 2;
	Triangle->center.z = 0;
	fnCalcCircle(points[Triangle->pnt[0]], points[Triangle->pnt[1]], points[Triangle->pnt[2]], Triangle->center.x, Triangle->center.y, Triangle->r);
	ListEdge->add(Triangle);
	for (i = 3; i<countP; i++)
	{
		addpoint = points[i];
		//помечаем на удаление те треугольники в писанную окружность которых попала новая точка
		for (j = 0; j<ListEdge->count(); j++)
		{
			if ((ListEdge->get(j)->center.x + ListEdge->get(j)->r>addpoint.x) && (ListEdge->get(j)->center.x - ListEdge->get(j)->r<addpoint.x) &&
				(ListEdge->get(j)->center.y + ListEdge->get(j)->r>addpoint.y) && (ListEdge->get(j)->center.y - ListEdge->get(j)->r<addpoint.y) &&
				(fnGetDistans3d(ListEdge->get(j)->center, addpoint)<ListEdge->get(j)->r))
			{
				ListEdge->get(j)->del = true;
			}
		}
		//вычисляем градус до каждой открытой(нет пересичения с существующими рёбрами) точки из новой
		listsort->clear();
		for (j = 0; j<i; j++)
		{
			//проверяем на пересичение с существующими треугольниками
			b = false;
			for (k = 0; k<ListEdge->count(); k++)
			{
				if (!ListEdge->get(k)->del)
				{
					if (!((j == ListEdge->get(k)->pnt[0]) || (j == ListEdge->get(k)->pnt[1])))
						b = CrossingLineXY(addpoint, points[j], points[ListEdge->get(k)->pnt[0]], points[ListEdge->get(k)->pnt[1]], rez);
					if (b) break;
					if (!((j == ListEdge->get(k)->pnt[1]) || (j == ListEdge->get(k)->pnt[2])))
						b = CrossingLineXY(addpoint, points[j], points[ListEdge->get(k)->pnt[1]], points[ListEdge->get(k)->pnt[2]], rez);
					if (b) break;
					if (!((j == ListEdge->get(k)->pnt[2]) || (j == ListEdge->get(k)->pnt[0])))
						b = CrossingLineXY(addpoint, points[j], points[ListEdge->get(k)->pnt[2]], points[ListEdge->get(k)->pnt[0]], rez);
					if (b) break;
				}
			}
			if (!b)
			{
				sp = new RSotrPoint;
				sp->n = j; //номер точки в массиве
				sp->angle = fnGetAngleXY(addpoint, points[j]); //её градус в радианах
				listsort->add(sp);
			}
		}
		//сортируем все открытые точки по углу (пузырьком)
		for (j = 0; j<listsort->count(); j++)
		{
			for (k = j; k<listsort->count(); k++)
			{
				if (listsort->get(j)->angle > listsort->get(k)->angle)
				{
					sp = listsort->get(j);
					listsort->get(j) = listsort->get(k);
					listsort->get(k) = sp;
				}
			}
		}
		//строим треугольники по откр. оточкам пропускаем те где улог >180 градусов
		for (j = 0; j<listsort->count(); j++)
		{
			if (j<listsort->count() - 1)
			{
				sp1 = listsort->get(j);
				sp2 = listsort->get(j + 1);
			}
			else
			{
				sp1 = listsort->get(listsort->count() - 1);
				sp2 = listsort->get(0);
			}
			if (fnCorrectAngle(sp2->angle + (2 * PI) - sp1->angle)>PI) continue; //потому что область выпуклая
			Triangle = new RTriangle;
			Triangle->del = false;
			Triangle->pnt[0] = i;
			Triangle->pnt[1] = sp1->n;
			Triangle->pnt[2] = sp2->n;
			Triangle->center.z = 0;
			fnCalcCircle(points[Triangle->pnt[0]], points[Triangle->pnt[1]], points[Triangle->pnt[2]], Triangle->center.x, Triangle->center.y, Triangle->r);
			ListEdge->add(Triangle);
		}
		//удаляем помечанные на удаление
		j = 0;
		while (j<ListEdge->count())
		{
			if (ListEdge->get(j)->del)
			{
				ListEdge->del(j);
				j--;
			}
			j++;
		}
	}

	//вычисляем центр треугольника и растояние каждой точки до центра треугольника
	for (i = 0; i<ListEdge->count(); i++)
	{
		ListEdge->get(i)->center.x = (points[ListEdge->get(i)->pnt[0]].x + points[ListEdge->get(i)->pnt[1]].x + points[ListEdge->get(i)->pnt[2]].x) / 3;
		ListEdge->get(i)->center.y = (points[ListEdge->get(i)->pnt[0]].y + points[ListEdge->get(i)->pnt[1]].y + points[ListEdge->get(i)->pnt[2]].y) / 3;
		ListEdge->get(i)->center.z = 0;
		ListEdge->get(i)->cr[0] = fnGetDistans3d(points[ListEdge->get(i)->pnt[0]], ListEdge->get(i)->center);
		ListEdge->get(i)->cr[1] = fnGetDistans3d(points[ListEdge->get(i)->pnt[1]], ListEdge->get(i)->center);
		ListEdge->get(i)->cr[2] = fnGetDistans3d(points[ListEdge->get(i)->pnt[2]], ListEdge->get(i)->center);
	}
	delete listsort;
}
//------------------------------------------------------------------------------
//Подвинуть точку на заданный угол
void MovePointOnAngle(float Angle, RfPointXY CenterPointXY, RfPointXY PointXY, RfPointXY & RezPointXY)
{
	PointXY.x = PointXY.x - CenterPointXY.x;
	PointXY.y = PointXY.y - CenterPointXY.y;
	RezPointXY.x = PointXY.x*cos(Angle) + PointXY.y*sin(Angle);
	RezPointXY.y = PointXY.y*cos(Angle) - PointXY.x*sin(Angle);
	RezPointXY.x = RezPointXY.x + CenterPointXY.x;
	RezPointXY.y = RezPointXY.y + CenterPointXY.y;
}
/*------------------------------------------------------------------------------
PHead0,PTail0 - координаты первой линии (отрезка)
PHead1,PTail1 - координаты второй линии (отрезка)
PRez - точка пересичения
*/
bool CrossingLine(RfPointXY PHead0, RfPointXY PTail0, RfPointXY PHead1, RfPointXY PTail1, RfPointXY & PRez)
{
	float a0, b0, c0, a1, b1, c1;
	a0 = PTail0.y - PHead0.y;
	b0 = PHead0.x - PTail0.x;
	c0 = PTail0.x*PHead0.y - PHead0.x*PTail0.y;

	a1 = PTail1.y - PHead1.y;
	b1 = PHead1.x - PTail1.x;
	c1 = PTail1.x*PHead1.y - PHead1.x*PTail1.y;

	if ((b1 == 0) || ((b0*a1 / b1) - a0 == 0)) PRez.x = PHead1.x;
	else PRez.x = (-(b0*c1 / b1) + c0) / ((b0*a1 / b1) - a0);
	if ((a1 == 0) || ((a0*b1 / a1) - b0 == 0)) PRez.y = PHead1.y;
	else PRez.y = (-(c1*a0 / a1) + c0) / ((a0*b1 / a1) - b0);

	if ((PRez.x<Min(PHead0.x, PTail0.x)) || (PRez.x>Max(PHead0.x, PTail0.x))) return false;
	if ((PRez.x<Min(PHead1.x, PTail1.x)) || (PRez.x>Max(PHead1.x, PTail1.x))) return false;

	if ((PRez.y<Min(PHead0.y, PTail0.y)) || (PRez.y>Max(PHead0.y, PTail0.y))) return false;
	if ((PRez.y<Min(PHead1.y, PTail1.y)) || (PRez.y>Max(PHead1.y, PTail1.y))) return false;
	return true;
}
//------------------------------------------------------------------------------
//Точка пересечения 2х линий
bool CrossingLineXY(RfPointXYZ PHead0, RfPointXYZ PTail0, RfPointXYZ PHead1, RfPointXYZ PTail1, RfPointXYZ & PRez)
{
	RfPointXY qPHead0;
	qPHead0.x = PHead0.x;
	qPHead0.y = PHead0.y;
	RfPointXY qPTail0;
	qPTail0.x = PTail0.x;
	qPTail0.y = PTail0.y;
	RfPointXY qPHead1;
	qPHead1.x = PHead1.x;
	qPHead1.y = PHead1.y;
	RfPointXY qPTail1;
	qPTail1.x = PTail1.x;
	qPTail1.y = PTail1.y;
	RfPointXY qPRez;
	bool rez = CrossingLine(qPHead0, qPTail0, qPHead1, qPTail1, qPRez);
	PRez.x = qPRez.x;
	PRez.y = qPRez.y;
	PRez.z = PHead0.z;
	return rez;
}
//------------------------------------------------------------------------------
/*пересичение сферы и линии
r - радиус, p0 - центр шара
p1,p2 - координаты линии
rp1,rp2 - 2 точки пересичения с шаром*/
bool CrossingLineAndSphere(double r, RdPointXYZ p0, RdPointXYZ p1, RdPointXYZ p2, RdPointXYZ &rp1, RdPointXYZ &rp2)
{
	double a1, a2, b1, b2;
	double d, b, a, c;
	double rz1, rz2;

	if (p1.z == p2.z) return false;
	a1 = (p2.x - p1.x) / (p2.z - p1.z);
	a2 = (p2.y - p1.y) / (p2.z - p1.z);
	b1 = (-p1.z*p2.x + p1.z*p1.x) / (p2.z - p1.z) + p1.x - p0.x;
	b2 = (-p1.z*p2.y + p1.z*p1.y) / (p2.z - p1.z) + p1.y - p0.y;
	//дискриминант
	b = 2.0*a1*b1 + 2 * a2*b2 + 2.0*p0.z;
	a = a1*a1 + a2*a2 + 1;
	c = b1*b1 + b2*b2 - r*r + p0.z*p0.z;
	d = sqr(b) - 4.0*a*c;
	if (d<0) return false;
	d = sqrt(d);
	rz1 = (-(2.0*a1*b1 + 2 * a2*b2 + 2 * p0.z) - d) / (2.0*(a1*a1 + a2*a2 + 1));
	rz2 = (-(2.0*a1*b1 + 2 * a2*b2 + 2 * p0.z) + d) / (2.0*(a1*a1 + a2*a2 + 1));
	//подставляем уравнения и находим ответы
	rp1.x = (rz2 - p1.z) / (p2.z - p1.z)*(p2.x - p1.x) + p1.x;
	rp1.y = (rz2 - p1.z) / (p2.z - p1.z)*(p2.y - p1.y) + p1.y;
	rp1.z = rz2;
	rp2.x = (rz1 - p1.z) / (p2.z - p1.z)*(p2.x - p1.x) + p1.x;
	rp2.y = (rz1 - p1.z) / (p2.z - p1.z)*(p2.y - p1.y) + p1.y;
	rp2.z = rz1;
	return true;
}
//------------------------------------------------------------------------------
/*пересичение сферы и линии
r - радиус, p0 - центр шара
p1,p2 - координаты линии
rp1,rp2 - 2 точки пересичения с шаром*/
bool CrossingLineAndSphere(float r, RfPointXYZ p0, RfPointXYZ p1, RfPointXYZ p2, RfPointXYZ &rp1, RfPointXYZ &rp2)
{
	double rr;
	RdPointXYZ p00, p11, p22, rp11, rp22;
	bool b = true;
	rr = r;
	p00.x = p0.x; p00.y = p0.y; p00.z = p0.z;
	p11.x = p1.x; p11.y = p1.y; p11.z = p1.z;
	p22.x = p2.x; p22.y = p2.y; p22.z = p2.z;
	CrossingLineAndSphere(rr, p00, p11, p22, rp11, rp22);
	rp1.x = (float)rp11.x; rp1.y = (float)rp11.y; rp1.z = (float)rp11.z;
	rp2.x = (float)rp22.x; rp2.y = (float)rp22.y; rp2.z = (float)rp22.z;
	return b;
}
//------------------------------------------------------------------------------
/*пересичении линии и плоскости (линия заданна 2мя точками)
p1,p2 - точки линии
a,b,c,d - коэфициенты в уранении плоскости*/
RdPointXYZ CrossingLineAndPlane(RdPointXYZ p1, RdPointXYZ p2, double a, double b, double c, double d)
{
	double buf;
	RdPointXYZ result;
	// вычисляем расстояния между концами отрезка и плоскостью треугольника.
	//float r1,r2;
	//  r1 = PHead0.x*a+PHead0.y*b+PHead0.z*c+d;
	//  r2 = PTail0.x*a+PTail0.y*b+PTail0.z*c+d;
	//  if(((r1<0) and (r2<0))or((r1>0) or (r2>0))) then Exit; // если оба конца отрезка лежат по одну сторону от плоскости, то отрезок не пересекает треугольник.
	// вычисляем точку пересечения отрезка с плоскостью
	buf = p2.x*a - p1.x*a + p2.z*c - p1.z*c + p2.y*b - p1.y*b; //надо выяснить что означает геометрически buf=0 TODO
	if ((p1.y == p2.y) || (buf == 0.0)) result.y = p1.y; else result.y = (float)(((p1.y*p2.x*a - p1.y*p1.x*a + p1.y*p2.z*c - p1.y*p1.z*c) / (p2.y - p1.y) - a*p1.x - c*p1.z - d) / (buf / (p2.y - p1.y)));
	if (p1.x == p2.x) result.x = p1.x; else
	{
		if (p1.y == p2.y) result.x = p1.x / (p2.x - p1.x)*(p2.x - p1.x);
		else result.x = ((result.y - p1.y) / (p2.y - p1.y) + p1.x / (p2.x - p1.x))*(p2.x - p1.x);
	}
	if (p1.z == p2.z) result.z = p1.z; else
	{
		if (p1.y == p2.y) result.z = p1.z / (p2.z - p1.z)*(p2.z - p1.z);
		else result.z = ((result.y - p1.y) / (p2.y - p1.y) + p1.z / (p2.z - p1.z))*(p2.z - p1.z);
	}
	return result;
}
//------------------------------------------------------------------------------
/*пересичении линии и плоскости (линия заданна 2мя точками)
p1,p2 - точки линии
a,b,c,d - коэфициенты в уранении плоскости*/
RfPointXYZ CrossingLineAndPlane(RfPointXYZ p1, RfPointXYZ p2, float a, float b, float c, float d)
{
	RfPointXYZ result;
	RdPointXYZ r, p11, p22;
	double aa, bb, cc, dd;
	p11.x = p1.x; p11.y = p1.y; p11.z = p1.z;
	p22.x = p2.x; p22.y = p2.y; p22.z = p2.z;
	aa = a; bb = b; cc = c; dd = d;
	r = CrossingLineAndPlane(p11, p22, aa, bb, cc, dd);
	result.x = (float)r.x; result.y = (float)r.y; result.z = (float)r.z;
	return result;
}
//------------------------------------------------------------------------------
//точка пересичения 3х плоскостей
RfPointXYZ CrossingPlanes(RfPlaneABCD p1, RfPlaneABCD p2, RfPlaneABCD p3)
{
	float a1, a2, a3, a4;
	a1 = (p1.a*p2.b*p3.c + p1.b*p2.c*p3.a + p2.a*p3.b*p1.c) - (p1.c*p2.b*p3.a + p2.a*p1.b*p3.c + p1.a*p2.c*p3.b);
	a2 = (p1.d*p2.b*p3.c + p1.b*p2.c*p3.d + p2.d*p3.b*p1.c) - (p1.c*p2.b*p3.d + p1.b*p3.c*p2.d + p1.d*p2.c*p3.b);
	a3 = (p1.a*p2.d*p3.c + p2.a*p3.d*p1.c + p3.a*p2.c*p1.d) - (p1.c*p2.d*p3.a + p2.a*p3.c*p1.d + p1.a*p3.d*p2.c);
	a4 = (p1.a*p2.b*p3.d + p1.b*p2.d*p3.a + p2.a*p3.b*p1.d) - (p1.d*p2.b*p3.a + p1.b*p3.d*p2.a + p1.a*p2.d*p3.b);
	RfPointXYZ result;
	result.x = -a2 / a1;
	result.y = -a3 / a1;
	result.z = -a4 / a1;
	return result;
}
//------------------------------------------------------------------------------
//Для определения проходит ли линия в нутри заданого радиуса
//r - радиус
//cx,cy - центр радиуса
//lx1, ly1 - начало линии
//lx2, ly2 - конец линии
//Coincides - Проходит или нет
//x, y - Ближайшая точка на линии к центру радиуса
void Distance(float r, float cx, float cy, float lx1, float ly1, float lx2, float ly2, bool& Coincides, float& x, float& y)
{
	double ang, angcos, angsin;
	double ncx, ncy, nlx2, nr;

	x = 0.0; y = 0.0;

	cx = cx - lx1;
	cy = cy - ly1;
	lx2 = lx2 - lx1;
	ly2 = ly2 - ly1;
	ang = atan(ly2 / lx2);
	if (lx2<0) ang = ang + M_PI;
	if ((lx2>0) && (ly2<0)) ang = ang + (2 * M_PI);
	angcos = cos(ang);
	angsin = sin(ang);
	ncx = cx*angcos + cy*angsin;
	ncy = cy*angcos - (cx*angsin);
	nlx2 = lx2*angcos + ly2*angsin;
	nr = fabs(ncy);
	Coincides = false;
	if (nr<r)
	{
		Coincides = true;
		if (ncx<0)
		{
			if (ncx<-r)
			{
				Coincides = false;
				return;
			}
			nr = sqrt(ncx*ncx + ncy*ncy);
			if (nr<r)
			{
				x = lx1;
				y = ly1;
				return;
			}
			Coincides = false;
			return;
		}
		else
			if (ncx>nlx2)
			{
				if (ncx>nlx2 + r)
				{
					Coincides = false;
					return;
				}
				nr = sqrt((ncx - nlx2)*(ncx - nlx2) + ncy*ncy);
				if (nr<r)
				{
					x = lx2 + lx1;
					y = ly2 + ly1;
					return;
				}
				Coincides = false;
				return;
			}
			else
			{
				angcos = cos(2 * M_PI - ang);
				angsin = sin(2 * M_PI - ang);
				x = (float)(ncx*angcos + lx1);
				y = (float)(-(ncx*angsin) + ly1);
				return;
			}
	}
}
//------------------------------------------------------------------------------
//умножение векторов
float ScalarProduct(RfPointXYZ t, RfPointXYZ p)
{
	return p.x*t.x + p.y*t.y + p.z*t.z;
}
//------------------------------------------------------------------------------
//нормализовать точку сделать еденичной длины
void normalized(RfPointXYZ &p)
{
	float l = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
	if (l == 0)
	{
		p.x = 0.0;
		p.y = 0.0;
		p.z = 1.0;
		return;
	}
	p.x = p.x / l;
	p.y = p.y / l;
	p.z = p.z / l;
}
//------------------------------------------------------------------------------
//нормализовать точку сделать еденичной длины
void normalized(RdPointXYZ &p)
{
	double l = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
	if (l == 0)
	{
		p.x = 0.0;
		p.y = 0.0;
		p.z = 1.0;
		return;
	}
	p.x = p.x / l;
	p.y = p.y / l;
	p.z = p.z / l;
}
//------------------------------------------------------------------------------
//пересичение векторов
RfPointXYZ CrossProduct(RfPointXYZ p, RfPointXYZ t)
{
	RfPointXYZ result;
	result.x = p.y*t.z - p.z*t.y;
	result.y = t.x*p.z - p.x*t.z;
	result.z = p.x*t.y - p.y*t.x;
	return result;
}
//------------------------------------------------------------------------------
//пересичение векторов
RdPointXYZ CrossProduct(RdPointXYZ p, RdPointXYZ t)
{
	RdPointXYZ result;
	result.x = p.y*t.z - p.z*t.y;
	result.y = t.x*p.z - p.x*t.z;
	result.z = p.x*t.y - p.y*t.x;
	return result;
}
//------------------------------------------------------------------------------
//dot product умножить вектор на значение
RfPointXYZ dotProduct(RfPointXYZ p, float t)
{
	RfPointXYZ result;
	result.x = p.x*t;
	result.y = p.y*t;
	result.z = p.z*t;
	return result;
}
//------------------------------------------------------------------------------
/*умножение 2х видовых матриц 4x4
dest - матрица из 16 элементов результат
left, float - умножаемые матрицы из 16 элементов
*/
void quatnext(float* dest, float* left, float* right)
{
	dest[0] = left[0] * right[0] + left[1] * right[4] + left[2] * right[8];
	dest[1] = left[0] * right[1] + left[1] * right[5] + left[2] * right[9];
	dest[2] = left[0] * right[2] + left[1] * right[6] + left[2] * right[10];
	dest[4] = left[4] * right[0] + left[5] * right[4] + left[6] * right[8];
	dest[5] = left[4] * right[1] + left[5] * right[5] + left[6] * right[9];
	dest[6] = left[4] * right[2] + left[5] * right[6] + left[6] * right[10];
	dest[8] = left[8] * right[0] + left[9] * right[4] + left[10] * right[8];
	dest[9] = left[8] * right[1] + left[9] * right[5] + left[10] * right[9];
	dest[10] = left[8] * right[2] + left[9] * right[6] + left[10] * right[10];
}
//------------------------------------------------------------------------------
//reset the rotation matrix
void quatidentity(float* q)
{
	q[0] = 1;  q[1] = 0;  q[2] = 0;  q[3] = 0;
	q[4] = 0;  q[5] = 1;  q[6] = 0;  q[7] = 0;
	q[8] = 0;  q[9] = 0;  q[10] = 1; q[11] = 0;
	q[12] = 0; q[13] = 0; q[14] = 0; q[15] = 1;
}
//------------------------------------------------------------------------------
//reset the rotation matrix
void quatidentity(double* q)
{	//0,5,10 - масштаб
	q[0] = 1;  q[1] = 0;  q[2] = 0;  q[3] = 0;
	q[4] = 0;  q[5] = 1;  q[6] = 0;  q[7] = 0;
	q[8] = 0;  q[9] = 0;  q[10] = 1; q[11] = 0;
	q[12] = 0; q[13] = 0; q[14] = 0; q[15] = 1; // <-сдвиг
}
//------------------------------------------------------------------------------
//длина от начала коордионат
float pythagoreanlength(RfPointXYZ p)
{
	return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}
//------------------------------------------------------------------------------
/*умножить точку на матрицу поворота
point - точка
mas - матрица из 16 элементов
*/
RfPointXYZ MultPointOnRotArray(RfPointXYZ point, double* mas)
{
	RfPointXYZ result;
	result.x = (float)(point.x*mas[0] + point.y*mas[1] + point.z*mas[2]);
	result.y = (float)(point.x*mas[4] + point.y*mas[5] + point.z*mas[6]);
	result.z = (float)(point.x*mas[8] + point.y*mas[9] + point.z*mas[10]);
	return result;
}
//------------------------------------------------------------------------------
/*умножить точку на матрицу поворота
point - точка
mas - матрица из 16 элементов
*/
RdPointXYZ MultPointOnRotArray(RdPointXYZ point, double* mas)
{
	RdPointXYZ result;
	result.x = point.x*mas[0] + point.y*mas[1] + point.z*mas[2];
	result.y = point.x*mas[4] + point.y*mas[5] + point.z*mas[6];
	result.z = point.x*mas[8] + point.y*mas[9] + point.z*mas[10];
	return result;
}
//------------------------------------------------------------------------------
/* Транспонировать только матрицу вращения (=обратной)
q - матрица 4x4
*/
void TransposeRotMat(double* q)
{
	double buf;
	buf = q[1]; q[1] = q[4]; q[4] = buf;
	buf = q[2]; q[2] = q[8]; q[8] = buf;
	buf = q[6]; q[6] = q[9]; q[9] = buf;
}
//------------------------------------------------------------------------------
//заполнить матрицу поворота по заданному вектору  q[16]
void LookAt(RfPointXYZ vecEye, RfPointXYZ vecTop, double* q)
{
	normalized(vecEye);
	normalized(vecTop);
	//float m[16];
	double x[3], y[3], z[3];
	//double mag;
	/* Make rotation matrix */
	/* Z vector */
	z[0] = vecEye.x;
	z[1] = vecEye.y;
	z[2] = vecEye.z;

	/* Y vector */
	y[0] = vecTop.x;
	y[1] = vecTop.y;
	y[2] = vecTop.z;

	/* X vector = Y cross Z */
	x[0] = y[1] * z[2] - y[2] * z[1];
	x[1] = -y[0] * z[2] + y[2] * z[0];
	x[2] = y[0] * z[1] - y[1] * z[0];

	/* Recompute Y = Z cross X */
	y[0] = z[1] * x[2] - z[2] * x[1];
	y[1] = -z[0] * x[2] + z[2] * x[0];
	y[2] = z[0] * x[1] - z[1] * x[0];

	/* mpichler, 19950515 */
	/* cross product gives area of parallelogram, which is < 1.0 for
	* non-perpendicular unit-length vectors; so normalize x, y here
	*/
	/*mag = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
	if (mag) {
	x[0] /= mag;
	x[1] /= mag;
	x[2] /= mag;
	}

	mag = sqrt(y[0] * y[0] + y[1] * y[1] + y[2] * y[2]);
	if (mag) {
	y[0] /= mag;
	y[1] /= mag;
	y[2] /= mag;
	}*/

#define M(row,col)  q[col*4+row]
	M(0, 0) = x[0];
	M(0, 1) = x[1];
	M(0, 2) = x[2];
	M(0, 3) = 0.0;
	M(1, 0) = y[0];
	M(1, 1) = y[1];
	M(1, 2) = y[2];
	M(1, 3) = 0.0;
	M(2, 0) = z[0];
	M(2, 1) = z[1];
	M(2, 2) = z[2];
	M(2, 3) = 0.0;
	M(3, 0) = 0.0;
	M(3, 1) = 0.0;
	M(3, 2) = 0.0;
	M(3, 3) = 1.0;
#undef M
	//glMultMatrixf(m);       
	/* Translate Eye to Origin */
	//glTranslatef(-eyex, -eyey, -eyez);
}
//------------------------------------------------------------------------------
//Построить: икосандр 12 точек 20 граней
void buildIcosahedron(double r, TSimpleList2<RdPointXYZ>* Points, TSimpleList2<RsFacesABC>* Faces)
{
	const double vX = 0.525731112119133606*r;
	const double vZ = 0.850650808352039932*r;

	RdPointXYZ vPoints;
	RsFacesABC vFaces;

	vPoints.x = -vX; vPoints.y = 0.0; vPoints.z = vZ;
	Points->add(vPoints);
	vPoints.x = vX; vPoints.y = 0.0; vPoints.z = vZ;
	Points->add(vPoints);
	vPoints.x = -vX; vPoints.y = 0.0; vPoints.z = -vZ;
	Points->add(vPoints);
	vPoints.x = vX; vPoints.y = 0.0; vPoints.z = -vZ;
	Points->add(vPoints);
	vPoints.x = 0.0; vPoints.y = vZ; vPoints.z = vX;
	Points->add(vPoints);
	vPoints.x = 0.0; vPoints.y = vZ; vPoints.z = -vX;
	Points->add(vPoints);
	vPoints.x = 0.0; vPoints.y = -vZ; vPoints.z = vX;
	Points->add(vPoints);
	vPoints.x = 0.0; vPoints.y = -vZ; vPoints.z = -vX;
	Points->add(vPoints);
	vPoints.x = vZ; vPoints.y = vX; vPoints.z = 0.0;
	Points->add(vPoints);
	vPoints.x = -vZ; vPoints.y = vX; vPoints.z = 0.0;
	Points->add(vPoints);
	vPoints.x = vZ; vPoints.y = -vX; vPoints.z = 0.0;
	Points->add(vPoints);
	vPoints.x = -vZ; vPoints.y = -vX; vPoints.z = 0.0;
	Points->add(vPoints);

	vFaces.a = 0; vFaces.c = 4; vFaces.b = 1;
	Faces->add(vFaces);
	vFaces.a = 0; vFaces.c = 9; vFaces.b = 4;
	Faces->add(vFaces);
	vFaces.a = 9; vFaces.c = 5; vFaces.b = 4;
	Faces->add(vFaces);
	vFaces.a = 4; vFaces.c = 5; vFaces.b = 8;
	Faces->add(vFaces);
	vFaces.a = 4; vFaces.c = 8; vFaces.b = 1;
	Faces->add(vFaces);
	vFaces.a = 8; vFaces.c = 10; vFaces.b = 1;
	Faces->add(vFaces);
	vFaces.a = 8; vFaces.c = 3; vFaces.b = 10;
	Faces->add(vFaces);
	vFaces.a = 5; vFaces.c = 3; vFaces.b = 8;
	Faces->add(vFaces);
	vFaces.a = 5; vFaces.c = 2; vFaces.b = 3;
	Faces->add(vFaces);
	vFaces.a = 2; vFaces.c = 7; vFaces.b = 3;
	Faces->add(vFaces);
	vFaces.a = 7; vFaces.c = 10; vFaces.b = 3;
	Faces->add(vFaces);
	vFaces.a = 7; vFaces.c = 6; vFaces.b = 10;
	Faces->add(vFaces);
	vFaces.a = 7; vFaces.c = 11; vFaces.b = 6;
	Faces->add(vFaces);
	vFaces.a = 11; vFaces.c = 0; vFaces.b = 6;
	Faces->add(vFaces);
	vFaces.a = 0; vFaces.c = 1; vFaces.b = 6;
	Faces->add(vFaces);
	vFaces.a = 6; vFaces.c = 1; vFaces.b = 10;
	Faces->add(vFaces);
	vFaces.a = 9; vFaces.c = 0; vFaces.b = 11;
	Faces->add(vFaces);
	vFaces.a = 9; vFaces.c = 11; vFaces.b = 2;
	Faces->add(vFaces);
	vFaces.a = 9; vFaces.c = 2; vFaces.b = 5;
	Faces->add(vFaces);
	vFaces.a = 7; vFaces.c = 2; vFaces.b = 11;
	Faces->add(vFaces);
}
//------------------------------------------------------------------------------
//Построить: октаэдр 6 точек 8 граней
void buildOctahedron(double r, TSimpleList2<RdPointXYZ>* Points, TSimpleList2<RsFacesABC>* Faces)
{
	RdPointXYZ vPoints;
	RsFacesABC vFaces;

	vPoints.x = r; vPoints.y = 0; vPoints.z = 0; //0
	Points->add(vPoints);
	vPoints.x = -r; vPoints.y = 0; vPoints.z = 0; //1
	Points->add(vPoints);
	vPoints.x = 0; vPoints.y = r; vPoints.z = 0; //2
	Points->add(vPoints);
	vPoints.x = 0; vPoints.y = -r; vPoints.z = 0; //3
	Points->add(vPoints);
	vPoints.x = 0; vPoints.y = 0; vPoints.z = r; //4
	Points->add(vPoints);
	vPoints.x = 0; vPoints.y = 0; vPoints.z = -r; //5
	Points->add(vPoints);

	vFaces.a = 0; vFaces.c = 4; vFaces.b = 2;
	Faces->add(vFaces);
	vFaces.a = 2; vFaces.c = 4; vFaces.b = 1;
	Faces->add(vFaces);
	vFaces.a = 1; vFaces.c = 4; vFaces.b = 3;
	Faces->add(vFaces);
	vFaces.a = 3; vFaces.c = 4; vFaces.b = 0;
	Faces->add(vFaces);

	vFaces.a = 2; vFaces.c = 5; vFaces.b = 0;
	Faces->add(vFaces);
	vFaces.a = 1; vFaces.c = 5; vFaces.b = 2;
	Faces->add(vFaces);
	vFaces.a = 3; vFaces.c = 5; vFaces.b = 1;
	Faces->add(vFaces);
	vFaces.a = 0; vFaces.c = 5; vFaces.b = 3;
	Faces->add(vFaces);
}
//------------------------------------------------------------------------------
//Переместить шпиндель на заданое количество точек по линии
// От LONG_MIN до LONG_MAX -2,147,483,648 до 2,147,483,647
// xDis, yDis, zDis - количество шагов в ту или другую сторону
TSimpleList2<RcPointXYZ>* Line3D(signed long int xDis, signed long int yDis, signed long int zDis)
{
	TSimpleList2<RcPointXYZ>* result = new TSimpleList2<RcPointXYZ>();

	signed long int i, l, m, n, dx2, dy2, dz2, err_1, err_2;
	signed char x_inc, y_inc, z_inc;
	RcPointXYZ pixel;

	pixel.x = 0;
	pixel.y = 0;
	pixel.z = 0;

	if (xDis < 0) x_inc = -1; else x_inc = 1;
	if (xDis < 0) xDis = -xDis;
	l = xDis;

	if (yDis < 0) y_inc = -1; else 	y_inc = 1;
	if (yDis < 0) yDis = -yDis;
	m = yDis;

	if (zDis < 0) z_inc = -1; else 	z_inc = 1;
	if (zDis < 0) zDis = -zDis;
	n = zDis;

	dx2 = l << 1;
	dy2 = m << 1;
	dz2 = n << 1;

	if ((l >= m) && (l >= n))
	{
		err_1 = dy2 - l;
		err_2 = dz2 - l;
		for (i = 0; i < l; i++)
		{
			// step here
			//StepMotors(pixel[0], pixel[1], pixel[2]);
			result->add(pixel);

			pixel.x = 0;
			pixel.y = 0;
			pixel.z = 0;

			if (err_1 > 0)
			{
				pixel.y = y_inc;
				err_1 -= dx2;
			}
			if (err_2 > 0)
			{
				pixel.z = z_inc;
				err_2 -= dx2;
			}
			err_1 += dy2;
			err_2 += dz2;

			pixel.x = x_inc;
		}
	}
	else if ((m >= l) && (m >= n))
	{
		err_1 = dx2 - m;
		err_2 = dz2 - m;
		for (i = 0; i < m; i++)
		{
			// step here
			//StepMotors(pixel[0], pixel[1], pixel[2]);
			result->add(pixel);

			pixel.x = 0;
			pixel.y = 0;
			pixel.z = 0;

			if (err_1 > 0)
			{
				pixel.x = x_inc;
				err_1 -= dy2;
			}
			if (err_2 > 0)
			{
				pixel.z = z_inc;
				err_2 -= dy2;
			}
			err_1 += dx2;
			err_2 += dz2;

			pixel.y = y_inc;
		}
	}
	else
	{
		err_1 = dy2 - n;
		err_2 = dx2 - n;
		for (i = 0; i < n; i++)
		{
			// step here
			//StepMotors(pixel[0], pixel[1], pixel[2]);
			result->add(pixel);

			pixel.x = 0;
			pixel.y = 0;
			pixel.z = 0;

			if (err_1 > 0)
			{
				pixel.y = y_inc;
				err_1 -= dz2;
			}
			if (err_2 > 0)
			{
				pixel.x = x_inc;
				err_2 -= dz2;
			}
			err_1 += dy2;
			err_2 += dx2;

			pixel.z = z_inc;
		}
	}
	// step here
	//StepMotors(pixel[0], pixel[1], pixel[2]);
	result->add(pixel);

	return result;
}
//------------------------------------------------------------------------------