1609 lines
56 KiB
C++
1609 lines
56 KiB
C++
//---------------------------------------------------------------------------
|
||
//#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 (float)sqrt(sqr(p.x) + sqr(p.y) + sqr(p.z));
|
||
}
|
||
//------------------------------------------------------------------------------
|
||
//Расстояние между 2мя точками на плоскости в декартовых координатах
|
||
float fnGetDistans2d(RfPointXY PointXY1, RfPointXY PointXY2)
|
||
{
|
||
return (float)sqrt(fabs(sqr(PointXY1.x - PointXY2.x) + sqr(PointXY1.y - PointXY2.y)));
|
||
}
|
||
//------------------------------------------------------------------------------
|
||
//Расстояние между 2мя точками в пространстве в декартовых координатах
|
||
float fnGetDistans3d(RfPointXYZ PointXYZ1, RfPointXYZ PointXYZ2)
|
||
{
|
||
return (float)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 = (float)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 = (float)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 = (float)sqrt(x*x + y*y + z*z);
|
||
AngleH = (float)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 = (float)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 = (float)sqrt(x*x + y*y);
|
||
Angle = (float)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 = (float)fabs(PointXY1.x - PointXY2.x);
|
||
LengthXY.y = (float)fabs(PointXY1.y - PointXY2.y);
|
||
dist = (float)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 = (float)fabs(PointXYZ1.x - PointXYZ2.x);
|
||
LengthXYZ.y = (float)fabs(PointXYZ1.y - PointXYZ2.y);
|
||
LengthXYZ.z = (float)fabs(PointXYZ1.z - PointXYZ2.z);
|
||
|
||
dist = (float)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 = (float)fabs(PointXYZ1.x - PointXYZ2.x);
|
||
LengthXYZ.y = (float)fabs(PointXYZ1.y - PointXYZ2.y);
|
||
LengthXYZ.z = (float)fabs(PointXYZ1.z - PointXYZ2.z);
|
||
|
||
dist = (float)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;
|
||
}
|
||
//------------------------------------------------------------------------------
|