I didnt read that pdf, (just some scratches)
But consider something like that.
Velocity vector shows where you sphere want to go.
So you test ray from sphere to triangle intersection
But i am not sure if you test distance from sphere to triangle
not every vel vector will cross your triangle it can stop right before it (closer than sphere radius) or it can hit it during movement from A to B
See this: Its working almost in 100% i just don't know how to test triangle segmentswith that A to B journey. And maybe one other thing but i do not remember it.
//---------------------------------------------------------------------------
#pragma hdrstop
#include "COLLISION_THREAD.h"
//---------------------------------------------------------------------------
#pragma package(smart_init)
//__fastcall TMotion_Thread::TMotion_Thread(bool CreateSuspended) : TThread(CreateSuspended)
//{
//
//
//}
//
//
//__fastcall TMotion_Thread::~TMotion_Thread()
//{
//
//
//}
//__fastcall TMotion_Thread::Execute()
//{
//
//
//}
__fastcall TCollision_Thread::TCollision_Thread(bool CreateSuspended) // : TThread(CreateSuspended)
{
//s= new TStringList();
clear_stack();
wywolan = -1;
}
__fastcall TCollision_Thread::~TCollision_Thread()
{
}
void __fastcall TCollision_Thread::Execute()
{
}
// vRESULT = new sphere position
bool TCollision_Thread::SPHERE_INTERSECT_POLYGON(t3dpoint punkt,t3dpoint punktB,
float sphere_radius,
TachoGLModel* model,
int face_index,t3dpoint& vRESULT)//, t3dpoint & vectorforce, float force)
{
uproszczony_wynik = false;
wywolan = wywolan + 1;
bool result = false;
t3dpoint wektor;
int i;
bool face_arr;
int cnt;
int base_side;
int k;
float tnb = 1000000000000.0f;
int NAJBLIZSZY_BOK;
float movev;
bool BY_VERTEX;
float ODLEGLOSC_VERTEX;
float ODLEGLOSC_BOK;
float ODLEGLOSC_VOLUME;
cnt= -1;
result = false;
t3dpoint AA[2];
int kipa = model->VBO_BE[face_index].INDEX_START; //gdzie w VBO_V JESTESMY
t3dpoint FN = Normal(
model->VBO_V[kipa],model->VBO_V[kipa+1],model->VBO_V[kipa+2],true); //TEST FACE NORMAL
AA[0] = punkt;
AA[1] = punktB;
float AAdist = n3ddistance(AA[0],AA[1]) + sphere_radius*SQUARE_ROOT_2;//FUCKING IMPORTANT TO ADD SPHERE_RADIUS TO DISTANCE, BECAUSE IF WE DON'T THEN ONE OF COLLISION CASES WON'T OCCUR
t3dpoint AAvec = vectorAB(AA[0],AA[1]);
AAvec = Normalize( AAvec );
t3dpoint movedir = AAvec;
t3dpoint dota = AAvec;
dota = reverse_point(dota);
AAvec = vector_multiple( AAvec, AAdist);
AA[1] = vectors_add(AA[0], AAvec);
float proper_distance;
t3dpoint reverse_vec;
t3dpoint punktBx = PUNKT_PRZECIECIA_ODCINEK_PLASZCZYZNA(FN,model->VBO_V[kipa],AA);
t3dpoint facen = model->VBO_N[ model->VBO_BE[face_index].INDEX_START];
t3dpoint firstfacepoint = model->VBO_V[ model->VBO_BE[face_index].INDEX_START];
t3dpoint colpoint;
t3dpoint FACE_INTERSECTIONPOINT;
FACE_DIST = n3dPoint_plane_distance(punktB,model->MATH_FACE_NORMALS[face_index],
model->MATH_FACE_DISTANCES[face_index]);
t3dpoint AA2[2];
if (FACE_DIST <= sphere_radius) {
t3dpoint rn=reverse_point(facen);
rn = vector_multiple(rn,sphere_radius*2.0f);
AA2[0] = punktB;
AA2[1] = vectors_add(punktB,rn);
if (LineIntersectFaceFromModel(face_index,model,AA2,FACE_INTERSECTIONPOINT) == true) {
//go point is near the plane and its colliding with it
t3dpoint vres;
vres = vector_multiple(facen,sphere_radius);
vRESULT = vectors_add(FACE_INTERSECTIONPOINT,vres);
uproszczony_wynik = true; //simple_result = true
dotp = Dot(facen,movedir);
//w tym miejscu ponizej dystans pomiedzy old do FACE_INTERSECTIONPOINT musi byc mniejszy od sphere-RADIUS
if (dotp >= 0.0f) return false; else
{
if (n3ddistance(punktB,FACE_INTERSECTIONPOINT) <= sphere_radius)
return true; //if we want to move outwards
else return false;
}
}}
// return false;
if (LineIntersectFaceFromModel(face_index,model,AA,colpoint) == true)
{
proper_distance = sphere_radius;
//reverse_vec = vectorAB(colpoint,punkt);
// reverse_vec = Normalize(reverse_vec);
reverse_vec = vector_multiple(facen,sphere_radius);
reverse_vec = vectors_add(colpoint,reverse_vec); //refracted point
uproszczony_wynik = true;
vRESULT = reverse_vec;
return true;
//EVERYTHING IS CORRECT
}
// return false;
////////////////////////////////////////////////////////////////////////////////
// SECONS PASS COLLISION TEST FOR INTERSECTION BETWEEN VERTICES AND SIDES (SIDES ONLY :))
////////////////////////////////////////////////////////////////////////////////
// ZBIERZMY WSZYSTKIE ODLEGLOSCI BOKOW SCIANY OD NASZEGO ODCINKA RUCHU
t3dpoint tyv;
t3dpoint tyv2;
t3dpoint tyv3;
for (i = 0; i < model->VBO_BE[face_index].length; i++) {
tyv = model->VBO_V[model->VBO_BE[face_index].INDEX_START+i];
if (i != model->VBO_BE[face_index].length - 1) { //TESTING SIDE (OR VERT) IS NOT THE LAST ONE IN THIS FACE
tyv2 = model->VBO_V[model->VBO_BE[face_index].INDEX_START+i+1];
dist3D_Return_Closest_Points_Between_Segments(punkt,punktB,tyv,tyv2,PUNKTY_D[i][0],PUNKTY_D[i][1]);
} else {
tyv3 = model->VBO_V[model->VBO_BE[face_index].INDEX_START];
dist3D_Return_Closest_Points_Between_Segments(punkt,punktB,tyv,tyv3,PUNKTY_D[i][0],PUNKTY_D[i][1]);
}
t3dpoint D = PUNKTY_D[i][0];
t3dpoint K = PUNKTY_D[i][1];
float DKD = n3ddistance(D,K);
if ( DKD <= sphere_radius ) {
BL[i] = true;
t3dpoint C;
float cf;
cf = sqrt( (sphere_radius*sphere_radius)-(DKD*DKD) );
t3dpoint fromDtoBEGGINING;
fromDtoBEGGINING = vectorAB(punktB,punkt);
if ( ( fromDtoBEGGINING.x== 0.0) &&
( fromDtoBEGGINING.z== 0.0) &&
( fromDtoBEGGINING.y== 0.0) ) {ShowMessage("KURWA 2");}
fromDtoBEGGINING = Normalize( fromDtoBEGGINING );
fromDtoBEGGINING = vector_multiple(fromDtoBEGGINING, cf);
//*********************************************************************************
C = vectors_add(D,fromDtoBEGGINING);
DYSTANS_BOKI[i] = n3ddistance(punkt,C);
uber_point[i] = C;
} else BL[i] = false;
// Oto i nasz punkt na linii kierunku
}
NAJBLIZSZY_BOK = -1;
//PO ZLICZENIU WSZYSTKICH ODLEGLOSCI CZAS NA ZNALEZIENIE NAJBLIZSZEGO PUNKTU KOLIZJI
for (i = 0; i < model->VBO_BE[face_index].length; i++) {
if ( BL[i] == true )
if (DYSTANS_BOKI[i] < tnb) {
tnb = DYSTANS_BOKI[i];
NAJBLIZSZY_BOK = i;
}
}
if (NAJBLIZSZY_BOK == -1) return false;
najblizszybok = NAJBLIZSZY_BOK;
vRESULT = uber_point[ NAJBLIZSZY_BOK ];
if ( BL[ NAJBLIZSZY_BOK ] == true ) return true;
return false;
}
void TCollision_Thread::clear_frame_stack()
{
cFRAME_INDEX = -1;
}
void TCollision_Thread::add_to_frame_stack(int face_index)
{
cFRAME_INDEX = cFRAME_INDEX + 1;
COLLISION_FRAME_STACK_INDEX[cFRAME_INDEX] = face_index;
}
void TCollision_Thread::clear_stack()
{
PUNKT_KOLIZJI = triplesingletoT3DPOINT(0,0,0);
NORMALNA_KOLIZJI = triplesingletoT3DPOINT(0,0,0);
cSTACK_INDEX = -1;
COLLISION_STACK_CLOSEST_INDEX = -1;
}
void TCollision_Thread::add_to_collision_stack(int face_index,t3dpoint result)
{
cSTACK_INDEX = cSTACK_INDEX + 1;
if (cSTACK_INDEX > 100 )return;
if (uproszczony_wynik == false) {
COLLISION_STACK_POINT[cSTACK_INDEX] = PUNKT_KOLIZJI;
COLLISION_STACK_POINTD[cSTACK_INDEX] = PUNKTY_D[najblizszybok][0];
COLLISION_STACK_POINTK[cSTACK_INDEX] = PUNKTY_D[najblizszybok][1];
}
COLLISION_STACK_INDEX[cSTACK_INDEX] = face_index;
COLLISION_STACK_RESULT[cSTACK_INDEX] = result;
}
void TCollision_Thread::DrawStack()
{ return;
if (cSTACK_INDEX < 0) return;
int i;
for (i=0;i< cSTACK_INDEX;i++)
{
glColor3f(1,1,1);
CreateSphere(COLLISION_STACK_RESULT[cSTACK_INDEX].x,COLLISION_STACK_RESULT[cSTACK_INDEX].y,COLLISION_STACK_RESULT[cSTACK_INDEX].z,20.0,4);
//glColor3f(0,1,0);
//CreateSphere(COLLISION_STACK_POINTD[cSTACK_INDEX].x,COLLISION_STACK_POINTD[cSTACK_INDEX].y,COLLISION_STACK_POINTD[cSTACK_INDEX].z,10.0,4);
//glColor3f(0,0,1);
//CreateSphere(COLLISION_STACK_POINTK[cSTACK_INDEX].x,COLLISION_STACK_POINTK[cSTACK_INDEX].y,COLLISION_STACK_POINTK[cSTACK_INDEX].z,10.0,4);
}
}
//nowpos isactually newpos
tcollision_sphere_response_packet TCollision_Thread::collisionfunc(t3dpoint oldpos,t3dpoint nowpos,TachoGLModel* model,float radius)
{
tcollision_sphere_response_packet resultx;
int i,j;
t3dpoint punkt;
t3dpoint wektor;
t3dpoint wypadkowa;
float oldrad;
t3dpoint temp;
float ROZKLAD_I;
t3dpoint LADDER;
int LADDERI;
t3dpoint sWYPADKOWA;
t3dpoint wektor_predkosci;
int NUMER_TROJKATA;
t3dpoint TESTUJEMY;
t3dpoint normal;
t3dpoint TEST;
int ostatni;
t3dpoint wynik; //WYNIK KOLIZJI TO WEKTOR ZNORMALIZOWANY W KIERUNKU SFERY OD PUNKTU KOLIZJI
int INTERSECT_COUNT;
t3dpoint stack_result;
const int iloscpowtorzen = 3;
oldrad = radius;
resultx.Collision = false;
float DYSTANS_CLIP = 10000000000.0f;
kolizja = false;
//set_last_pos(nowpos);
INTERSECT_COUNT = 0; ///to sie liczy z clear stack! :U chociaz wyglada inaczej
clear_frame_stack();
clear_stack();
for (i = 0; i < model->VBO_BE.Length; i++) {
if (SPHERE_INTERSECT_POLYGON(oldpos,nowpos,radius,model,i,wynik) == true)
{
resultx.Collision = true;
resultx.NEW_POS = wynik;// vectors_add( vector_multiple(Normalize(NORMALNA_KOLIZJI),radius),PUNKT_KOLIZJI);
//return resultx;
add_to_collision_stack(i,wynik);
INTERSECT_COUNT = INTERSECT_COUNT + 1;
kolizja = true;
}
// s->Add("FINISHED FACE CHECK "+IntToStr(i));
//s->SaveToFile("E:\\FACE_CHECK.txt");
}
DYSTANS_CLIP = 10000000000.0f;
if (kolizja == true) {
//s->Clear();
for (i = 0; i <= cSTACK_INDEX; i++) {
//float ntpd = n3ddistance(COLLISION_STACK_RESULT[i],oldpos);
//s->Add(FloatToStr(ntpd));
if (n3ddistance(COLLISION_STACK_RESULT[i],oldpos) < DYSTANS_CLIP)
{
DYSTANS_CLIP = n3ddistance(COLLISION_STACK_RESULT[i],oldpos);
DYSTANSi_CLIP = i;
}
}
//s->Add("-----------");
//s->Add(FloatToStr(DYSTANS_CLIP));
COLLISION_STACK_CLOSEST_INDEX = DYSTANSi_CLIP;//COLLISION_STACK_INDEX[DYSTANSi_CLIP];
//for (i = 0; i <= cSTACK_INDEX; i++)
//s->Add(POINT_TO_BGLCMD(COLLISION_STACK_RESULT[ i ]));
//s->SaveToFile("E:\\WOOT.TXT");
wynik = COLLISION_STACK_RESULT[ COLLISION_STACK_CLOSEST_INDEX ];
}
//PUNKT_KOLIZJI = COLLISION_STACK_POINT[DYSTANSi_CLIP];
//NORMALNA_KOLIZJI = vector_multiple(wynik,100.0f);// t3dpoint;
bool MAMY_KOLIZJE = kolizja;
resultx.Collision = MAMY_KOLIZJE;
resultx.Collision_point = PUNKT_KOLIZJI;
resultx.NEW_POS = wynik;// vectors_add( vector_multiple(Normalize(NORMALNA_KOLIZJI),radius),PUNKT_KOLIZJI);
resultx.OLD_POS = nowpos;
return resultx;
}
And i will take a shortcut and post whole math unit.
//---------------------------------------------------------------------------
#pragma hdrstop
#include "DxcMath.h"
#include "Dialogs.hpp"
#include "Math.h"
//#include "System.hpp"
//---------------------------------------------------------------------------
#pragma package(smart_init)
float __fastcall ArcTan2(float x, float y)
{
float result = 0.0f;
if (abs(x) < 0.0000000010f) //Close to zero! We declare it to be zero!
{//The small margin gives a slight error, but improves reliability. nice comment btw :U => EPSILON or SPECIAL_FEAUTRE :)
if (y > 0.0)
result = pi2; //90 degrees
else
result = -pi2; //-90 degrees
} else {
if (x > 0.0f) // 1. or 4. quadrant
result = ArcTan(y / x); // Easy stuff. Normal ArcTan is valid for 1. and 4.
else
result = pi - ArcTan(-y / x);
}
return result;
}
void __fastcall tquaterion::Normalize()
{
float k;
k = x*x+y*y+z*z+w*w;
if(k > 1.020) {
long double f = k;
if (f>0.0)
f = sqrtl(f); else f = 1.0;
float magnitude = f;
w = w / magnitude;
x = x / magnitude;
y = y / magnitude;
z = z / magnitude;
}
}
bool __fastcall tquaterion::requiresNormalization()
{
float k;
k = x*x+y*y+z*z+w*w;
if(k > 1.020) return true; else return false;
}
void __fastcall tquaterion::CreateQuaterion(t3dpoint axis, float angle)
{
float fAngle = angle*imopi;
//axis is a unit vector
w = cos( fAngle/2.0);
x = axis.x * sin( fAngle/2.0 );
y = axis.y * sin( fAngle/2.0 );
z = axis.z * sin( fAngle/2.0 );
if (requiresNormalization() == true) Normalize();
}
void __fastcall tquaterion::multiple(tquaterion q2)
{
float w1=w;
float x1=x;
float y1=y;
float z1=z;
w = (w1*q2.w - x1*q2.x - y1*q2.y - z1*q2.z);
x = (w1*q2.x + x1*q2.w + y1*q2.z - z1*q2.y);
y = (w1*q2.y - x1*q2.z + y1*q2.w + z1*q2.x);
z = (w1*q2.z + x1*q2.y - y1*q2.x + z1*q2.w);
}
void __fastcall tquaterion::makematrix()
{
if (requiresNormalization() == true) Normalize();
matrix.pointer[0] = w*w+x*x-y*y-z*z;
matrix.pointer[1] = 2.0*x*y-2.0*w*z;
matrix.pointer[2] = 2.0*x*z+2.0*w*y;
matrix.pointer[3] = 0.0;
matrix.pointer[4] = 2.0*x*y+2.0*w*z;
matrix.pointer[5] = w*w-x*x+y*y-z*z;
matrix.pointer[6] = 2.0*y*z+2.0*w*x;
matrix.pointer[7] = 0.0;
matrix.pointer[8] = 2.0*x*z-2.0*w*y;
matrix.pointer[9] = 2.0*y*z-2.0*w*x;
matrix.pointer[10] = w*w-x*x-y*y+z*z;
matrix.pointer[11] = 0.0;
matrix.pointer[12] = 0.0;
matrix.pointer[13] = 0.0;
matrix.pointer[14] = 0.0;
matrix.pointer[15] = 1.0;
}
float strafe_orientation(float DEGREE_ANGLE)
{
float a = VALIDUJ( DEGREE_ANGLE );
if ((a>= 270.0) && (a <= 360.0)) return -1;
if ((a>= 0.0) && (a <= 90.0)) return -1;
if ((a>= 90.0) && (a <= 270.0)) return 1;
return 1;
}
float sins(float DEGREE_ANGLE)
{
float a = VALIDUJ( DEGREE_ANGLE );
if ((a<= 90.0) && (a>=0.0)) return a / 90.0f;
// if ((a>= 90.0) && (a<=135.0)) return (a-90.0f) / 90.0f;
if ((a>= 90.0) && (a<=180.0)) return (1.0f - (a-90.0f) / 90.0f);
if ((a>= 180.0) && (a<=270.0)) return -(a-180.0f) / 90.0f;
if ((a>= 270.0) && (a<=360.0)) return - (1.0f - ( (a-270.0f) / 90.0f));
}
long double n3dPoint_plane_distance(t3dpoint p, t3dpoint vnormalnover, float D)
{
long double d = -1.0f;
long double num = p.x * vnormalnover.x + p.y * vnormalnover.y+ p.z * vnormalnover.z + D;
num = abs( num );
long double denom = vnormalnover.x*vnormalnover.x + vnormalnover.y*vnormalnover.y+vnormalnover.z*vnormalnover.z;
denom = sqrtl(denom);
if (denom == 0.0f) return -1;
return num / denom;
}
int maxval(int * table, int length)
{
int i;
int max = -9999999999;
for (i=0; i < length; i++)
if (table[i] > max) max = table[i];
return max;
}
__fastcall TDirection::TDirection()
{
heading = 0.0f;
}
__fastcall TDirection::~TDirection()
{
glop = 0.0f;
}
void __fastcall TDirection::CALCULATE_VEC() //add to point to get Proper position
{
vec.x = 100000.0f*(sin((glop)*imopi)*cos((heading)*imopi));
vec.z = -100000.0f*(cos((glop)*imopi)*cos((heading)*imopi));
vec.y = 100000.0f*(sin((heading)*imopi));
}
t3dpoint __fastcall odbij_od_sciany(t3dpoint V,t3dpoint N)
{
return vectors_substract_v1minusv2(V,vector_multiple(vector_multiple(N, -2.0f),Dot( N ,V)));
}
float __fastcall Dystans(float x1, float x2)
{
if (x1 > x2) return x1-x2;
if (x2 > x1) return x2-x1;
return 0;
}
float __fastcall PointImagnitude2d(TPoint vector)
{
return sqrt(
(vector.x*vector.x) +
(vector.y*vector.y));
}
float dopotegi(float x)
{
return x*x;
}
float __fastcall n2ddistance(float x1,float y1,float x2,float y2)
{
return sqrt(dopotegi(x1-x2)+dopotegi(y1-y2));
}
float __fastcall n2ddistanceA(textpoint V1, textpoint V2)
{
return sqrt(dopotegi(V1.x-V2.x)+dopotegi(V1.y-V2.y));
}
textpoint __fastcall vectorAB2di(float Ax,float Ay,float Bx,float By)
{
textpoint result;
result.x = Bx-Ax;
result.y = By-Ay;
return result;
}
textpoint __fastcall vector_multiple2d(textpoint v,float k)
{
textpoint result;
result.x = v.x*k;
result.y = v.y*k;
return result;
}
t3dpointi __fastcall tripleinttopoint(int x,int y,int z)
{
t3dpointi result;
result.x = x;
result.y = y;
result.z = z;
return result;
}
t3dpoint __fastcall triplesingletoT3DPOINTA(float x,float y, float z)
{
t3dpoint result;
result.x = x;
result.y = y;
result.z = z;
return result;
}
//get magnitude of our vector and then divide m,n,k factors by it
textpoint __fastcall Normalize2d(textpoint v)
{
float magni;// : single;
textpoint res;
magni = magnitude2d(v);
if (magni == 0.0f) magni = 1.0f;
res.x = v.x/magni;
res.y = v.y/magni;
return res;
}
textpoint __fastcall PointINormalize(TPoint v)
{
float result;
float magni;// : single;
textpoint res;
magni = PointImagnitude2d(v);
if (magni == 0.0f) magni = 1.0f;
res.x = v.x/magni;
res.y = v.y/magni;
return res; //dopotegi
}
float __fastcall magnitude2d(textpoint vector)
{
return sqrt(
(vector.x*vector.x) +
(vector.y*vector.y));
}
float __fastcall PLUSORMINUS(float k)
{
if (k >= 0) return 1.0f; else return -1.0f;
}
//float __fastcall floatabs(float k)
//{
// if (k >= 0) return k; else return -k;
//}
double SingleMagnitude(double fx)
{
return sqrt(fx*fx);
}
double SingleNormalize(double fx)
{
return fx / SingleMagnitude(fx); //always return 1.0 ? :/
}
int __fastcall Utnij(float k)
{
int r = int(k);
return r;
//result := Trunc(k);
}
textpoint __fastcall odwroc2d(textpoint v)
{
textpoint result;
result.x = -v.x;
result.y = -v.y;
return result;
}
t3dpoint __fastcall odwrocV(t3dpoint v)
{
t3dpoint result;
result.x = -v.x;
result.y = -v.y;
result.z = -v.z;
return result;
}
textpoint __fastcall vectors_add2d(textpoint v, textpoint v2)
{
textpoint result;
result.x = v.x+v2.x;
result.y = v.y+v2.y;
return result;
}
float __fastcall n2dGetPolarCoordAngleA(float x,float y)
{
if ( (x > 0) && (y >= 0) ) { return ArcTan(y/x); }
if ( (x > 0) && (y < 0) ) { return ArcTan(y/x)+2.0*3.1415926535897932384626433832795; }
if (x < 0) { return ArcTan(y/x)+3.1415926535897932384626433832795; }
if ( (x == 0) && (y > 0) ) { return 3.1415926535897932384626433832795/2.0; }
if ( (x == 0) && (y < 0) ) { return 3.0*3.1415926535897932384626433832795/2.0; }
//last versions were without .0 just simple 2 division
return - 1000.0;
}
long double __fastcall n2dGetPolarCoordAngleAD(float x,float y)
{
if ( (x == 0) && (y > 0) ) { return 3.1415926535897932384626433832795/2.0; }
if ( (x == 0) && (y < 0) ) { return 3.0*3.1415926535897932384626433832795/2.0; }
if (x == 0) return - 1000.0;
long double k; k = (long double)y / (long double)x;
if ( (x > 0) && (y >= 0) ) { return ArcTan(k); }
if ( (x > 0) && (y < 0) ) { return ArcTan(k)+2.0*3.1415926535897932384626433832795; }
if (x < 0) { return ArcTan(k)+3.1415926535897932384626433832795; }
//last versions were without .0 just simple 2 division
return - 1000.0;
}
float __fastcall n2dGetPolarCoordAngleB(float x,float y)
{
if ( (x > 0)) { return ArcTan(y/x); }
if ( (x < 0) && (y >= 0) ) { return ArcTan(y/x)+pi; }
if ( (x < 0) && (y < 0) ) { return ArcTan(y/x)-pi; }
if ( (x == 0) && (y > 0) ) { return pi2; }
if ( (x == 0) && (y < 0) ) { return -pi2; }
if ( (x == 0) && (y == 0) ) { return 0.0f; }
return - 1000.0;
}
//variable that informs computer to change vert index
bool __fastcall n2disPointinRect(float X,float Y,float X1,float Y1,float X2,float Y2,bool validate)// : Boolean;
{
float maxx,minx;
float maxy,miny;
bool result = false;
if (validate == true){
if( X1 > X2 ){ maxx = X1; minx = X2; } else { maxx = X2; minx = X1; }
if (Y1 > Y2) { maxy = Y1; miny = Y2; } else { maxy = Y2; miny = Y1; }
if ((X > minx) && (X < maxx) && (Y > miny) && (Y < maxy))
return true;
} else {
if ((X > X1) && (X < X2) && (Y > Y1) && (Y < Y2))
return true;
}
return false;
}
///-----------------------------3D----------------------------------------------
float __fastcall n3ddistance(t3dpoint first_point,t3dpoint second_point )
{
long double val = dopotegi(first_point.x-second_point.x) + dopotegi(first_point.y-second_point.y) + dopotegi(first_point.z-second_point.z);
if (val > 0.0) val = sqrtl(val); else val = -1.0f;
return float(val);
}
float __fastcall n3ddistanceA(float x1,float y1,float z1,float x2,float y2,float z2)
{
float val = dopotegi(x1-x2)+dopotegi(y1-y2)+dopotegi(z1-z2);
if (val != 0.0) val = sqrt(val);
return val;
}
void __fastcall n3ddistanceB(float x1,float y1,float z1,float x2,float y2,float z2, float & val)
{
val = dopotegi(x1-x2)+dopotegi(y1-y2)+dopotegi(z1-z2);
if (val != 0.0) val = sqrt(val);
}
t3dpoint __fastcall triplesingletoT3DPOINT(float x,float y,float z)
{
t3dpoint result;
result.x = x;
result.y = y;
result.z = z;
return result;
}
float __fastcall VALIDUJ2(float angle2, double Value)
{
float angle = angle2;
int kat=(int)angle2;
kat=int(kat/Value);
if (angle < 0 )
{
angle = angle + (float)(kat+1)*Value;
}
if (angle >= 360)
{
angle = angle - (float)kat*Value;
}
return angle;
}
float __fastcall VALIDUJ(float angle2)
{
float angle = angle2;
int kat=(int)angle2;
kat=kat/360;
if (angle < 0 )
{
angle = angle + (float)(kat+1)*360.0f;
}
if (angle >= 360)
{
angle = angle - (float)kat*360.0f;
}
return angle;
}
long double __fastcall VALIDUJD(long double angle2)
{
long double angle = angle2;
int kat=(int)angle2;
kat=kat/360;
if (angle < 0 )
{
angle = angle + (long double)(kat+1)*360.0;
}
if (angle >= 360)
{
angle = angle - (long double)kat*360.0;
}
return angle;
}
t3dpoint __fastcall vector_multiple(t3dpoint v, float k)
{
t3dpoint result;
result.x = v.x*k;
result.y = v.y*k;
result.z = v.z*k;
return result;
}
t3dpoint __fastcall vectors_frontadd(t3dpoint v1, t3dpoint v2) //plaszczyzna XY
{
t3dpoint result;
result.x = v1.x+v2.x;
result.y = v1.y-v2.y;
result.z = v1.z-v2.z;
return result;
}
t3dpoint __fastcall vectors_frontaddXZ(t3dpoint v1, t3dpoint v2) //plaszczyzna XZ
{
t3dpoint result;
result.x = v1.x+v2.x;
result.y = v1.y-v2.y;
result.z = v1.z-v2.z;
return result;
}
float __fastcall sdotPlaneVecDist(t3dpoint plane, t3dpoint point)
{
return (plane.x * point.x) + (plane.y * point.y) + (plane.z * point.z) + getplaneD(plane,point);
}
float __fastcall iloczynskalarny3d(t3dpoint vVector1, t3dpoint vVector2)
{
return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) );
}
t3dpoint __fastcall vectors_frontaddZY(t3dpoint v1, t3dpoint v2) //plaszczyzna ZY
{
t3dpoint result;
result.x = v1.x-v2.x;
result.y = v1.y+v2.y;
result.z = v1.z-v2.z;
return result;
}
t3dpoint __fastcall vectors_add(t3dpoint v1,t3dpoint v2)
{
t3dpoint result;
result.x = v1.x+v2.x;
result.y = v1.y+v2.y;
result.z = v1.z+v2.z;
return result;
}
t3dpoint __fastcall vectorAB(t3dpoint A,t3dpoint B)
{
t3dpoint result;
result.x = B.x-A.x;
result.y = B.y-A.y;
result.z = B.z-A.z;
return result;
}
t3dpoint __fastcall vectors_substract_v1minusv2(t3dpoint v1,t3dpoint v2)
{
t3dpoint result;
result.x = v1.x-v2.x;
result.y = v1.y-v2.y;
result.z = v1.z-v2.z;
return result;
}
float __fastcall magnitude(t3dpoint Vector)
{
long double k;
k = (Vector.x*Vector.x) +(Vector.y*Vector.y) +(Vector.z*Vector.z);
k = sqrtl( k );
float s = float(k);
return s;
}
t3dpoint __fastcall Normalize(t3dpoint v)
{
float magni;
t3dpoint result;
magni = magnitude(v);
// = 1.0;
if (IsNan(magni) == true) magni = 1.0;
if (magni <= 0.0) magni = 1.0;
result.x = v.x/magni;
result.y = v.y/magni;
result.z = v.z/magni;
// = v;
return result;
}
t3dpoint __fastcall VectorA(t3dpoint vPoint1,t3dpoint vPoint2)
{
t3dpoint vVector;
vVector.x = 0.0; vVector.y = 0.0; vVector.z = 0.0; // Initialize our variable to zero
// In order to get a vector from 2 points (a direction) we need to
// subtract the second point from the first point.
vVector.x = vPoint2.x - vPoint1.x; // Get the X value of our new vector
vVector.y = vPoint2.y - vPoint1.y; // Get the Y value of our new vector
vVector.z = vPoint2.z - vPoint1.z; // Get the Z value of our new vector
return vVector;
}
t3dpoint __fastcall Normal(t3dpoint point1,t3dpoint point2, t3dpoint point3, bool NormalizeNormal)
{
t3dpoint v1, v2;// : t3dpoint;
t3dpoint vn;
t3dpoint Result;
vn.x = 0.0;
vn.y = 0.0;
vn.z = 0.0;
Result = vn;
v1 = VectorA(point2, point1);
v2 = VectorA(point3, point1);
vn = vectorcross(v1,v2);
if (NormalizeNormal == true) vn = Normalize(vn);
Result = vn;
return vn;
}
// vNormal.x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y));
// vNormal.y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z));
// vNormal.z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x));
AnsiString BIGGEST_DimMat(t3dpoint vec)
{
if ( (vec.x > vec.y) && (vec.x > vec.z) ) return "X";
if ( (vec.y > vec.x) && (vec.y > vec.z) ) return "Y";
if ( (vec.z > vec.y) && (vec.z > vec.x) ) return "Z";
return "W";
}
AnsiString BIGGEST_DimMat_XZ(t3dpoint vec)
{
if ( (abs(vec.x) >= abs(vec.z)) ) return "X"; else return "Z";
}
//AnsiString ABSOLUTE_BIGGEST_DimMat(t3dpoint vec)
//{
//if ( (abs(vec.x) > abs(vec.y)) && (abs(vec.x) > abs(vec.z)) ) return "X";
//if ( (abs(vec.y) > abs(vec.x)) && (abs(vec.y) > abs(vec.z)) ) return "Y";
//if ( (abs(vec.z) > abs(vec.y)) && (abs(vec.z) > abs(vec.x)) ) return "Z";
// return "W";
//
//}
t3dpoint __fastcall vectorcross(t3dpoint v1,t3dpoint v2)
{
t3dpoint crossproduct;
crossproduct.x = (v1.y*v2.z)-(v1.z*v2.y);
crossproduct.y = (v1.z*v2.x)-(v1.x*v2.z);
crossproduct.z = (v1.x*v2.y)-(v1.y*v2.x);
return crossproduct;
}
t3dpoint __fastcall PUNKT_PRZECIECIA_ODCINEK_PLASZCZYZNA(t3dpoint vNormal,t3dpoint pointonplane,
t3dpoint vLine[2])
{
const Extended MATCH_FACTOR = 0.9999999999;
t3dpoint vIntersection;// : t3dpoint;
Extended originDistance;
Extended distance1;
Extended distance2;
double m_magnitude;// : Double;
t3dpoint vPoint;// : t3dpoint;
t3dpoint vLineDir;// : t3dpoint;
Extended Numerator;// : Extended;
Extended Denominator;// : Extended;
Extended dist;// : Extended;
Extended Angle,tempangle;// : Extended;
t3dpoint vA, vB;// : t3dpoint;
int I;
Extended dotProduct;// : Extended;
Extended vectorsMagnitude;// : Extended;
t3dpoint result;
originDistance = 0;
distance1 = 0;
distance2 = 0;
vPoint.x = 0;
vPoint.y = 0;
vPoint.z = 0;
vLineDir.x = 0;
vLineDir.y = 0;
vLineDir.z = 0;
Numerator = 0.0;
Denominator = 0.0;
dist = 0.0;
Angle = 0.0;
originDistance = -1 * ((vNormal.x * pointonplane.x) +
(vNormal.y * pointonplane.y) +
(vNormal.z * pointonplane.z));
distance1 = ((vNormal.x * vLine[0].x) +
(vNormal.y * vLine[0].y) +
(vNormal.z * vLine[0].z)) + originDistance;
distance2 = ((vNormal.x * vLine[1].x) +
(vNormal.y * vLine[1].y) +
(vNormal.z * vLine[1].z)) + originDistance;
if(distance1 * distance2 >= 0) {
t3dpoint r; //linia nie przebija plaszczyzny
r.x = 1.0f; r.y = 1.0f; r.z = 1.0f;
return r;
}
vLineDir.x = vLine[1].x - vLine[0].x;
vLineDir.y = vLine[1].y - vLine[0].y;
vLineDir.z = vLine[1].z - vLine[0].z;
m_magnitude = sqrt((vLineDir.x * vLineDir.x) +
(vLineDir.y * vLineDir.y) +
(vLineDir.z * vLineDir.z) );
if (m_magnitude == 0.0) m_magnitude = 1.0f;
vLineDir.x = vLineDir.x/m_magnitude;
vLineDir.y = vLineDir.y/m_magnitude;
vLineDir.z = vLineDir.z/m_magnitude;
Numerator = -1 * (vNormal.x * vLine[0].x +
vNormal.y * vLine[0].y +
vNormal.z * vLine[0].z + originDistance);
Denominator = ( (vNormal.x * vLineDir.x) + (vNormal.y * vLineDir.y) + (vNormal.z * vLineDir.z) );
if( Denominator == 0.0) {
vIntersection = vLine[0];
return vIntersection;
} else {
dist = Numerator / Denominator;
vPoint.x = (vLine[0].x + (vLineDir.x * dist));
vPoint.y = (vLine[0].y + (vLineDir.y * dist));
vPoint.z = (vLine[0].z + (vLineDir.z * dist));
result.x = vPoint.x; // Return the intersection point
result.y = vPoint.y;
result.z = vPoint.z;
}
return result;
}
bool __fastcall PassIF_GREATERorEQUAL(double value, double maxval, bool minvalue)
{
//if (abs(maxval) >= abs(value)) return true; else return false;
if (maxval == value) return true;
if (minvalue == true)
if (value < maxval) return true;
else
if (value > maxval) return true;
return false;
}
float __fastcall Dot(t3dpoint vVector1,t3dpoint vVector2)
{
return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) );
}
long double __fastcall Dotd(t3ddoublepoint vVector1,t3ddoublepoint vVector2)
{
return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) );
}
t3dpoint __fastcall ClosestPointOnLine (t3dpoint vA,t3dpoint vB,t3dpoint vPoint)
{
t3dpoint vVector1, vVector2, vVector3;// : t3dpoint;
t3dpoint vClosestPoint;// : t3dpoint;
float D, T;// : Single;
//First, we create a vector from our end point vA to our point vPoint
vVector1.x = vPoint.x - vA.x;
vVector1.y = vPoint.y - vA.y;
vVector1.z = vPoint.z - vA.z;
//Now we create a normalized direction vector from end point vA to end point vB
vVector2.x = vB.x - vA.x;
vVector2.y = vB.y - vA.y;
vVector2.z = vB.z - vA.z;
vVector2 = Normalize(vVector2);
//Now we use the distance formula to find the distance of the line segment
D = n3ddistance(vA, vB);
//Using the dot product, we project the vVector1 onto the vector vVector2. This essentially
//gives us the distance of our projected vector from vA
T = Dot(vVector2, vVector1);
//If our projected distance from vA, "t", is greater than or equal to 0, it must be closest to the end point
//vA. So we return this end point.
if (T<=0) return vA;
//If our projected distance from vA, "t", is greater than or equal to the magnitude or distance of the line
//segment, it must be closest to the end point vB, so we return vB.
if (T>=0) return vB;
//Here we create a vector that is of length T and in the direction of vVector2
vVector3.x = vVector2.x * T;
vVector3.y = vVector2.y * T;
vVector3.z = vVector2.z * T;
//To find the closest point on the line, we just add vVector3 to the original end point vA
vClosestPoint.x = vA.x + vVector3.x;
vClosestPoint.y = vA.y + vVector3.y;
vClosestPoint.z = vA.z + vVector3.z;
return vClosestPoint;
}
int __fastcall AStringToInt(AnsiString i)
{
AnsiString g;
//g = i;
//g.Delete(Pos(".",g)+4,20);
return StrToInt(i);
}
AnsiString __fastcall FloatTruncate(AnsiString value)
{
AnsiString g = value;
///g = StringReplace(g,".",",",TReplaceFlags() << rfReplaceAll);
if (Pos(",",g) > 0) g.Delete(Pos(",",g)+4,20000);
if (Pos(".",g) > 0) g.Delete(Pos(".",g)+4,20000);
return g;
}
int __fastcall pstrtoint(AnsiString value)
{
return StrToFloat(value);
}
void __fastcall init_string_float_conversion_rule()
{
AnsiString f;
float p = 10.250f;
f = FloatToStr(p);
if (Pos(".",f) > 0)
FLOAT_CONVERSION = tFCDot; else FLOAT_CONVERSION = tFCcomma;
}
Extended __fastcall pstrtofloat(AnsiString value)
{
AnsiString temp = value;
if (FLOAT_CONVERSION == tFCDot) { //English rule (floats are written as 10.25)
temp = StringReplace(value,",",".",TReplaceFlags() << rfReplaceAll);
temp = FloatTruncate(temp);
} else { //Polish rule (floats are written as 10,25)
temp = StringReplace(value,".",",",TReplaceFlags() << rfReplaceAll);
temp = FloatTruncate(temp);
}
return StrToFloat(temp);
}
AnsiString __fastcall pfloattostr(Extended value)
{
AnsiString temp;
temp = FloatToStr(value);
temp = StringReplace(temp,",",".",TReplaceFlags() << rfReplaceAll);
return temp;
}
AnsiString __fastcall POINT_TO_BGLCMD(t3dpoint point)
{
AnsiString result;
result = pfloattostr(point.x)+"#"+pfloattostr(point.y)+"#"+pfloattostr(point.z);
result = StringReplace(result,".",",",TReplaceFlags() << rfReplaceAll);
result = StringReplace(result,"#",".",TReplaceFlags() << rfReplaceAll);
return result;
}
AnsiString __fastcall TEXTPOINT_TO_BGLCMD(textpoint point)
{
AnsiString result;
result = pfloattostr(point.x)+"#"+pfloattostr(point.y);
result = StringReplace(result,".",",",TReplaceFlags() << rfReplaceAll);
result = StringReplace(result,"#",".",TReplaceFlags() << rfReplaceAll);
return result;
}
bool __fastcall t3i2xequal(int a1, int a2, int a3, int b1, int b2, int b3)
{
if ( (a1 == b1) && (a2 == b2) && (a3 == b3) ) return true; else return false;
}
bool __fastcall t3dpointsequal(t3dpoint p1,t3dpoint p2)
{
if ( (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z) ) return true; else return false;
}
t3dpoint __fastcall reverse_point(t3dpoint p)
{
t3dpoint result;
result.x = -p.x;
result.y = -p.y;
result.z = -p.z;
return result;
}
//----------------------------------------
float __fastcall getplaneD(t3dpoint vn,t3dpoint point)
{
return -((vn.x*point.x)+(vn.y*point.y)+(vn.z*point.z));
}
float __fastcall getplaneDISTANCE(t3dpoint normal,t3dpoint v1)
{
return dotproduct(normal,v1);
}
bool __fastcall between(float x1,float x2,float x0)
{
bool result = false;
if (smaller(x1,x2) < x0)
if (greater(x1,x2) > x0) result = true; else result = false;
return result;
}
bool __fastcall closepoint(float closedistance, t3dpoint base, t3dpoint checked)
{
float d;
d = n3ddistance(base,checked);
if (d = -1.0f) return true;
if (d <= closedistance) return true; else return false;
}
bool __fastcall betweenorequal(float x1,float x2,float x0)
{
bool result = false;
if (smaller(x1,x2) <= x0)
if (greater(x1,x2) >= x0) result = true; else result = false;
return result;
}
//default returns true
float __fastcall greaterABS(float a, float b)
{
if (abs(a)==abs(b)) return a;
if (abs(a)>abs(b)) return a; else return b;
}
float __fastcall greater(float a, float b)
{
if (a==b) return a;
if (a>b) return a; else return b;
}
float __fastcall smaller(float a,float b)
{
if (a==b) return a;
if (a>b) return b; else return a;
}
//przy tym sieu wypierdala
int __fastcall classifyapointagainstaplane(t3dpoint point,t3dpoint normal,float distance)
{
float costam;
int result;
costam = dotproduct(point,normal)+distance;
if (costam > 0) result = isFront;
if (costam < 0) result = isBack;
if (betweenorequal(-SPECIAL_FEAUTRE,SPECIAL_FEAUTRE,costam) == true) result = isOnPlane;
return result;
}
float __fastcall dotproduct(t3dpoint v1,t3dpoint v2)
{
return (v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z);
}
t3dpoint __fastcall dotproductP(float n,t3dpoint v1)
{
t3dpoint Result;
Result.x = n*v1.x;
Result.y = n*v1.y;
Result.z = n*v1.z;
return Result;
}
textpoint __fastcall float2textpoint(float x, float y)
{
textpoint result;
result.x = x;
result.y = y;
return result;
}
////////////////////////////////////////////////////////////////////////////////////////////
//:) pq
bool __fastcall n2disPointinTriangle(textpoint P, textpoint P1,textpoint P2,textpoint P3)
{
float t1, t2, t3;
t1 = (P3.y-P1.y)*(P.x-P1.x) - (P3.x-P1.x)*(P.y-P1.y);
t2 = (P2.y-P3.y)*(P.x-P3.x) - (P2.x-P3.x)*(P.y-P3.y);
t3 = (P1.y-P2.y)*(P.x-P2.x) - (P1.x-P2.x)*(P.y-P2.y);
if ( ((t1>0)&&(t2>0)&&(t3>0)) || ((t1<0)&&(t2<0)&&(t3<0)) )
return true;
else
return false;
}
t3dpoint sum(LIGHT_TRAIL tab)
{
int i;
t3dpoint result;
result.x = 0.0f;result.y = 0.0f;result.z = 0.0f;
for (i = 0; i < tab.Length; i++) {
result = vectors_add(result,tab[i]);
}
result.x = result.x / float(tab.Length);
result.y = result.y / float(tab.Length);
result.z = result.z / float(tab.Length);
return result;
}
bool RAY_POLY_TRACE(LIGHT_TRAIL vPoly, t3dpoint normal, t3dpoint vLine[2], t3dpoint & colpoint)
{
LIGHT_TRAIL boki;
FLOAT_ARR bokiD;
colpoint = PUNKT_PRZECIECIA_ODCINEK_PLASZCZYZNA(normal,vPoly[0],vLine);
float Distance = getplaneD(normal,vPoly[0]);
int i;
t3dpoint nv = vector_multiple(normal,1000.0f);
boki.set_length( vPoly.Length );
bokiD.set_length( vPoly.Length );
t3dpoint vec;
int l = vPoly.Length;
for (i = 0; i < l; i++) {
if (i == l-1) {
vec = vectorAB( vPoly[i], vPoly[0] );
} else {
vec = vectorAB( vPoly[i], vPoly[i+1] );
}
boki[i] = vectorcross( vec, nv);
bokiD[i] = getplaneD(boki[i], vPoly[i]);
}
t3dpoint facecenter = sum( vPoly );
int baseside = classifyapointagainstaplane( facecenter, boki[0], bokiD[0]);
for (i = 0; i < l; i++)
if (baseside != classifyapointagainstaplane( facecenter, boki[i], bokiD[i]) ) {
boki[i].x = -1.0* boki[i].x;
boki[i].y = -1.0* boki[i].y;
boki[i].z = -1.0* boki[i].z;
bokiD[i] = getplaneD(boki[i], vPoly[i]);
}
baseside = classifyapointagainstaplane( colpoint, boki[0], bokiD[0]);
for (i = 1; i < l; i++)
if (baseside != classifyapointagainstaplane( colpoint, boki[i], bokiD[i]) ) {
boki.set_length( 0 );
bokiD.set_length( 0 );
return false;
}
boki.set_length( 0 );
bokiD.set_length( 0 );
return true;
}
bool __fastcall IntersectedPolygon(LIGHT_TRAIL vPoly, t3dpoint vLine[2],int verticeCount)
{
Extended MATCH_FACTOR = 0.9999999999;
t3dpoint vNormal;
t3dpoint vIntersection;
Extended originDistance;
Extended distance1;
Extended distance2;
t3dpoint vVector1;
t3dpoint vVector2;
double m_magnitude;
t3dpoint vPoint;
t3dpoint vLineDir;
Extended Numerator;
Extended Denominator;
Extended dist;
Extended Angle,tempangle;
t3dpoint vA, vB;
int i;
Extended dotProduct;
Extended vectorsMagnitude;
vNormal.x = 0.0;
vNormal.y = 0.0;
vNormal.z = 0.0;
originDistance = 0.0;
distance1 = 0.0;
distance2 = 0.0;
vPoint.x = 0.0f;
vPoint.y = 0.0f;
vPoint.z = 0.0f;
vLineDir.x = 0.0f;
vLineDir.y = 0.0f;
vLineDir.z = 0.0f;
Numerator = 0.0;
Denominator = 0.0;
dist = 0.0;
Angle = 0.0;
vVector1.x = vPoly[2].x - vPoly[0].x;
vVector1.y = vPoly[2].y - vPoly[0].y;
vVector1.z = vPoly[2].z - vPoly[0].z;
vVector2.x = vPoly[1].x - vPoly[0].x;
vVector2.y = vPoly[1].y - vPoly[0].y;
vVector2.z = vPoly[1].z - vPoly[0].z;
vNormal.x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y));
vNormal.y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z));
vNormal.z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x));
m_magnitude = sqrt((vNormal.x * vNormal.x) +
(vNormal.y * vNormal.y) +
(vNormal.z * vNormal.z) );
vNormal.x = vNormal.x/m_magnitude;
vNormal.y = vNormal.y/m_magnitude;
vNormal.z = vNormal.z/m_magnitude;
if ( (IsNan(vNormal.x)== true) || (IsNan(vNormal.y)== true) || (IsNan(vNormal.z)== true) )
{
return false;
}
originDistance = -1.0f * ((vNormal.x * vPoly[0].x) +
(vNormal.y * vPoly[0].y) +
(vNormal.z * vPoly[0].z));
distance1 = ((vNormal.x * vLine[0].x) +
(vNormal.y * vLine[0].y) +
(vNormal.z * vLine[0].z)) + originDistance;
distance2 = ((vNormal.x * vLine[1].x) +
(vNormal.y * vLine[1].y) +
(vNormal.z * vLine[1].z)) + originDistance;
if(distance1 * distance2 >= 0.0f)
return false;
vLineDir.x = vLine[1].x - vLine[0].x;
vLineDir.y = vLine[1].y - vLine[0].y;
vLineDir.z = vLine[1].z - vLine[0].z;
m_magnitude = sqrt((vLineDir.x * vLineDir.x) +
(vLineDir.y * vLineDir.y) +
(vLineDir.z * vLineDir.z) );
vLineDir.x = vLineDir.x/m_magnitude;
vLineDir.y = vLineDir.y/m_magnitude;
vLineDir.z = vLineDir.z/m_magnitude;
Numerator = -1.0f * (vNormal.x * vLine[0].x +
vNormal.y * vLine[0].y +
vNormal.z * vLine[0].z + originDistance);
Denominator = ( (vNormal.x * vLineDir.x) + (vNormal.y * vLineDir.y) + (vNormal.z * vLineDir.z) );
if( Denominator == 0.0f) {
vIntersection = vLine[0];
temp_intersect_v = vIntersection;
} else {
dist = Numerator / Denominator;
vPoint.x = (vLine[0].x + (vLineDir.x * dist));
vPoint.y = (vLine[0].y + (vLineDir.y * dist));
vPoint.z = (vLine[0].z + (vLineDir.z * dist));
temp_intersect_v = vPoint; // Return the intersection point
}
//temp_intersect_v.x := vIntersection.x;
//temp_intersect_v.y := vIntersection.y;
//temp_intersect_v.z := vIntersection.z;
//temp_intersect_v2 := vPoint;
//CLIP_INTERSECT_POS := temp_intersect_v;
for (i = 0; i < verticeCount; i++) {
vA.x = vPoly[i].x - temp_intersect_v.x;
vA.y = vPoly[i].y - temp_intersect_v.y;
vA.z = vPoly[i].z - temp_intersect_v.z;
vB.x = vPoly[(i + 1) % verticeCount].x - temp_intersect_v.x;
vB.y = vPoly[(i + 1) % verticeCount].y - temp_intersect_v.y;
vB.z = vPoly[(i + 1) % verticeCount].z - temp_intersect_v.z;
dotProduct = ( (vA.x * vB.x) +
(vA.y * vB.y) +
(vA.z * vB.z) );
vectorsMagnitude = sqrt(
(vA.x * vA.x) +
(vA.y * vA.y) +
(vA.z * vA.z)
)
*
sqrt(
(vB.x * vB.x) +
(vB.y * vB.y) +
(vB.z * vB.z)
);
tempangle = ArcCos( dotProduct / vectorsMagnitude );
if(IsNan(tempangle) == true) tempangle = 1990.0f;
Angle = Angle + tempangle;
}
if(Angle >= (MATCH_FACTOR * (2.0f * 3.140f)) ) return true;
return false;
}
//------------------------------------------------------------------------------
/*
procedure AddVertex(const x: t3dpoint; var res: TPolygon);
begin
// Append a vertex to a polygon.
if (res.Count = 0) or
(magnitude(vectors_substract_v1minusv2(x, res.V[res.Count-1])) > 0.01) then
begin
INC(res.Count);
res.V[res.Count-1] := x;
end;
end;
function ClipEdge(const v1, v2: t3dpoint; const plane: t3dpoint): t3dpoint;
var
v: t3dpoint;
t: Single;
begin
// Clip a line segment to a plane, return the new end point.
v := vectors_substract_v1minusv2(v2, v1);
// t := -sdotPlaneVecDist(plane, v1) / iloczynskalarny3d(plane, v);
Result := vectors_add(v1, vector_multiple(v, t));
end;
function dotVecScalarMult3(v: TDotVector3; s: Single): TDotVector3;
begin
Result.x := v.x * s;
Result.y := v.y * s;
Result.z := v.z * s;
end;
function ClipPoly(const poly: TPolygon; const plane: t3dpoint): TPolygon;
var
a, b: t3dpoint;
count, i: Integer;
da, db: Integer;
clipped: TPolygon;
begin
// Clip a polygon to a plane.
count := poly.Count;
clipped.Count := 0;
for i := 0 to count - 1 do
begin
a := poly.V[i];
b := poly.V[(i+1) mod count];
da := classifyapointagainstaplane(a,plane,getplaned(plane,a));
db := classifyapointagainstaplane(b,plane,getplaned(plane,b));
if da < 1 then
begin
AddVertex(a, clipped);
if db = 1 then AddVertex(ClipEdge(a, b, plane), clipped);
end
else if db = -1 then
begin
AddVertex(ClipEdge(a, b, plane), clipped);
AddVertex(b, clipped);
end;
end;
Result := clipped;
result.normal := poly.normal;
end;
*/
// CreateBigPoly(normal,point on plane,wielkosc);
TPolygon CreatePoly(t3dpoint p,t3dpoint punkt_na_plaszczyznie, float SIZE)
{
// SIZE = 16384.0;
TPolygon Result;
t3dpoint n, tmp;
float a, b, c,d;
d = getplaneD(p,punkt_na_plaszczyznie);
// Create a huge polygon on the given plane.
//Result.Count := 4;
a = abs(p.x);
b = abs(p.y);
c = abs(p.z);
if ((a >= b) && (a >= c) ) {
// Plane is mostly YZ-aligned.
Result.V[0] = triplesingletoT3DPOINT(-(p.y*-SIZE + p.z*-SIZE + d)/p.x, -SIZE, -SIZE);
Result.V[1] = triplesingletoT3DPOINT(-(p.y*SIZE + p.z*-SIZE + d)/p.x, SIZE, -SIZE);
Result.V[2] = triplesingletoT3DPOINT(-(p.y*SIZE + p.z*SIZE + d)/p.x, SIZE, SIZE);
Result.V[3] = triplesingletoT3DPOINT(-(p.y*-SIZE + p.z*SIZE + d)/p.x, -SIZE, SIZE);
}
if ( (b >= a) && (b >= c) ) {
// Plane is mostly XZ-aligned.
Result.V[0] = triplesingletoT3DPOINT(-SIZE, -(p.x*-SIZE + p.z*-SIZE +d)/p.y, -SIZE);
Result.V[1] = triplesingletoT3DPOINT(SIZE, -(p.x*SIZE + p.z*-SIZE + d)/p.y, -SIZE);
Result.V[2] = triplesingletoT3DPOINT(SIZE, -(p.x*SIZE + p.z*SIZE + d)/p.y, SIZE);
Result.V[3] = triplesingletoT3DPOINT(-SIZE, -(p.x*-SIZE + p.z*SIZE + d)/p.y, SIZE);
}
if ( (c >= a) && (c >= b) ) {
// Plane is mostly XY-aligned.
Result.V[0] = triplesingletoT3DPOINT(-SIZE, -SIZE, -(p.x*-SIZE + p.y*-SIZE + d)/p.z);
Result.V[1] = triplesingletoT3DPOINT(SIZE, -SIZE, -(p.x*SIZE + p.y*-SIZE + d)/p.z);
Result.V[2] = triplesingletoT3DPOINT(SIZE, SIZE, -(p.x*SIZE + p.y*SIZE + d)/p.z);
Result.V[3] = triplesingletoT3DPOINT(-SIZE, SIZE, -(p.x*-SIZE + p.y*SIZE + d)/p.z);
}
n = vectorcross(vectors_substract_v1minusv2(Result.V[0], Result.V[1]),
vectors_substract_v1minusv2(Result.V[2], Result.V[1]));
Result.normal = p;
Result.Count = 4;
if (Dot(p, n) <= 0 ) {
tmp = Result.V[1];
Result.V[1] = Result.V[3];
Result.V[3] = tmp;
}
return Result;
}
t3dpoint __fastcall return_Screen_move_vec(float glop, float heading,t3dpoint pos, int oldx,int oldy,int newx,int newy, int &przeciaganieT)
{
int i;
t3dpoint upvector;
t3dpoint rightvector;
float vecx,vecy;
t3dpoint p;
bool g;
g=false;
upvector = triplesingletoT3DPOINTA(0.0f,0.0f,0.0f);
upvector.y = -1000.0f*(sin(abs(heading+90.0f)*imopi));
upvector= Normalize(upvector);
rightvector = triplesingletoT3DPOINTA(0.0f,0.0f,0.0f);
rightvector.x = 1000.0f*(sin((glop+90.0f)*imopi));
rightvector.z = -1000.0f*(cos((glop+90.0f)*imopi));
rightvector= Normalize(rightvector);
//1 PIKSEL TO PRZESUNIECIE O 0.5
vecx = (newx-oldx)*0.50f;
vecy = (newy-oldy)*0.50f;
upvector = vector_multiple(upvector,vecy);
rightvector = vector_multiple(rightvector,vecx);
rightvector.y = 0.0;
float Force = float(GetTickCount()-przeciaganieT)/1000.0;
// if (Force == 0.0) Frrce = 1.0f;
p = vector_multiple(vectors_add(upvector,rightvector),Force);
//p = validate_vector_snap_to_grid(p,MAX,grid_size);
przeciaganieT = GetTickCount();
return p;
}
//------------------------------------------------------------------------------
t3dpoint __fastcall PUNKT_PRZECIECIA_ODCINEK_PLASZCZYZNA2(t3dpoint vNormal,Extended originDistance,
t3dpoint vLine[2])
{
const Extended MATCH_FACTOR = 0.9999999999;
t3dpoint vIntersection;// : t3dpoint;
Extended distance1;
Extended distance2;
double m_magnitude;// : Double;
t3dpoint vPoint;// : t3dpoint;
t3dpoint vLineDir;// : t3dpoint;
Extended Numerator;// : Extended;
Extended Denominator;// : Extended;
Extended dist;// : Extended;
Extended Angle,tempangle;// : Extended;
t3dpoint vA, vB;// : t3dpoint;
int I;
Extended dotProduct;// : Extended;
Extended vectorsMagnitude;// : Extended;
t3dpoint result;
originDistance = 0;
distance1 = 0;
distance2 = 0;
vPoint.x = 0;
vPoint.y = 0;
vPoint.z = 0;
vLineDir.x = 0;
vLineDir.y = 0;
vLineDir.z = 0;
Numerator = 0.0;
Denominator = 0.0;
dist = 0.0;
Angle = 0.0;
// originDistance = -1 * originDistance;
distance1 = ((vNormal.x * vLine[0].x) +
(vNormal.y * vLine[0].y) +
(vNormal.z * vLine[0].z)) + originDistance;
distance2 = ((vNormal.x * vLine[1].x) +
(vNormal.y * vLine[1].y) +
(vNormal.z * vLine[1].z)) + originDistance;
if(distance1 * distance2 >= 0) {
t3dpoint r; //linia nie przebija plaszczyzny
r.x = 1.0f; r.y = 1.0f; r.z = 1.0f;
return r;
}
vLineDir.x = vLine[1].x - vLine[0].x;
vLineDir.y = vLine[1].y - vLine[0].y;
vLineDir.z = vLine[1].z - vLine[0].z;
m_magnitude = sqrt((vLineDir.x * vLineDir.x) +
(vLineDir.y * vLineDir.y) +
(vLineDir.z * vLineDir.z) );
if (m_magnitude == 0.0) m_magnitude = 1.0f;
vLineDir.x = vLineDir.x/m_magnitude;
vLineDir.y = vLineDir.y/m_magnitude;
vLineDir.z = vLineDir.z/m_magnitude;
Numerator = -1 * (vNormal.x * vLine[0].x +
vNormal.y * vLine[0].y +
vNormal.z * vLine[0].z + originDistance);
Denominator = ( (vNormal.x * vLineDir.x) + (vNormal.y * vLineDir.y) + (vNormal.z * vLineDir.z) );
if( Denominator == 0.0) {
vIntersection = vLine[0];
return vIntersection;
} else {
dist = Numerator / Denominator;
vPoint.x = (vLine[0].x + (vLineDir.x * dist));
vPoint.y = (vLine[0].y + (vLineDir.y * dist));
vPoint.z = (vLine[0].z + (vLineDir.z * dist));
result.x = vPoint.x; // Return the intersection point
result.y = vPoint.y;
result.z = vPoint.z;
}
return result;
}
//----TABLE LOGICAL INTEGER ARRAY-----------------------------------------------
void __fastcall ADD_TO_ARRAY_INTEGER(INDEX_ARR & table, int value)
{
INDEX_ARR temp;
temp.set_length(table.Length+1);
int i;
for (i = 0; i < table.Length; i++)
temp[i] = table[i];
temp[temp.Length - 1] = value;
table.set_length(temp.Length);
for (i = 0; i < table.Length; i++)
table[i] = temp[i];
temp.set_length(0);
}
bool __fastcall FOUND_IN_INTEGER_ARRAY(INDEX_ARR table, int value)
{
int i;
for (i = 0; i < table.Length; i++)
if( table[i] == value ) return true;
return false;
}
void __fastcall DELETE_FROM_INTEGER_ARRAY(INDEX_ARR & table, int value)
{
INDEX_ARR temp;
temp.set_length(table.Length-1);
int cnt = -1;
int i;
for (i = 0; i < table.Length; i++)
if (table[i] != value) {
cnt = cnt + 1;
temp[cnt] = table[i];
}
table.set_length(temp.Length);
for (i = 0; i < table.Length; i++)
table[i] = temp[i];
temp.set_length(0);
}
float ReturnHeading(t3dpoint vector)
{
float k;
try {
k = ArcCos(sqrt(1-(vector.y*vector.y)));
k = 180* (k / 3.14);
k = VALIDUJ(k);
if ((vector.y < 0) && (vector.z >= 0)) return k;
if ((vector.y < 0) && (vector.z < 0)) return k;
if ((vector.y >= 0) && (vector.z < 0)) return -k;
if ((vector.y >= 0) && (vector.z >= 0)) return -k;
return 0;
} catch (...) {
return 0;
}
return 0;
}
float ReturnGlop(t3dpoint vector)
{
float k;
float result=0.0f;
try {
k = ArcCos(sqrt((vector.y*vector.y)+(vector.z*vector.z)));
k = 180* (k / 3.14);
k = VALIDUJ(k);
if ((vector.x >= 0) && (vector.z >= 0)) return k;
if ((vector.x < 0) && (vector.z >= 0)) return 360 - k;
if ((vector.x < 0) && (vector.z < 0)) return 180 + k;
if ((vector.x >= 0) && (vector.z < 0)) return 180 - k;
//if (vector.z < 0) { k = k+180; result = k;}// - 180;
return 0;
} catch (...) {
return 0;
}
return 0;
}
//----TABLE LOGICAL INTEGER ARRAY-----------------------------------------------
void __fastcall ReturnCOLOR_DIST_AND_ANGLE(int r1,int g1, int b1, int r2, int g2, int b2, float &dist, float &angle)
{
n3ddistanceB(r1,g1,b1,r2,g2,b2,dist);
angle = abs(RGB_TO_HUE_ANGLE(r1,g1,b1) - RGB_TO_HUE_ANGLE(r2,g2,b2));
}
double __fastcall RGB_TO_HUE_ANGLE(int r, int g, int b)
{
hr = r / 255.0;
hg = g / 255.0;
hb = b / 255.0;
paris = 0.0;
//r is the maximmum component
if ( (r >= g) && (r >= b) ) max = r;
if ( (g >= r) && (g >= b) ) max = g;
if ( (b >= r) && (b >= g) ) max = b;
if ( (r <= g) && (r <= b) ) min = r;
if ( (g <= r) && (g <= b) ) min = g;
if ( (b <= r) && (b <= g) ) min = b;
if (max == min) return -10000.0;
if ( r == max ) paris = fmod((60.0* ((g-b) / (max-min) )+360.0) , 360.0);
if ( g == max ) paris = (60.0 * ((b-r) / (max-min) )) + 120.0;
if ( b == max ) paris = (60.0 * ((r-g) / (max-min) )) + 240.0;
return paris;
}
int __fastcall TRCcolor(Byte r,Byte g,Byte b,Byte cr,Byte cg,Byte cb)
{
return abs( (r+b+g) - (cr+cg+cb) );
}
bool __fastcall fimilar_color(Byte r,Byte g,Byte b,Byte cr,Byte cg,Byte cb, int tolerance3x)
{
if ( abs((r+b+g) - (cr+cg+cb)) > tolerance3x ) return false; else return true;
}
bool __fastcall isalreadyInTable(double * RedChan,double * GreenChan, double * BlueChan, int Length,
double r,double g,double b)
{
for (int i = 0; i < Length; i++)
if ( (r == RedChan[i]) && (g == GreenChan[i]) && (b == BlueChan[i]) )
return true;
return false;
}
//--------------OPENGL H---------------------
AnsiString __fastcall matrixtostring(tmatrixtype matrix)
{
if( matrix == mtTriangles ) return "GL_TRIANGLES";
if( matrix == mtQuads ) return "GL_QUADS";
if (matrix == mtPolygon ) return "GL_POLYGON";
if (matrix == mtLines ) return "GL_LINES";
if (matrix == mtLineLoop ) return "GL_LINE_LOOP";
if (matrix == mtLineStrip ) return "GL_LINE_STRIP";
if (matrix == mtTriangleStrip ) return "GL_TRIANGLE_STRIP";
if (matrix == mtTriangleFan ) return "GL_TRIANGLE_FAN";
if (matrix == mtQuadStrip ) return "GL_QUAD_STRIP";
if (matrix == mtPoints ) return "GL_POINTS";
if (matrix == mtNone ) return "GL_POINTS";
return "GL_TRIANGLES";
}
tmatrixtype __fastcall stringtomatrix(AnsiString text)
{
if (LowerCase(text) == "gl_triangles" )return mtTriangles;
if (LowerCase(text) == "gl_quads" )return mtQuads;
if (LowerCase(text) == "gl_polygon" )return mtPolygon;
if (LowerCase(text) == "gl_lines" )return mtLines;
if (LowerCase(text) == "gl_line_loop" )return mtLineLoop;
if (LowerCase(text) == "gl_line_strip" )return mtLineStrip;
if (LowerCase(text) == "gl_triangle_strip" )return mtTriangleStrip;
if (LowerCase(text) == "gl_triangle_fan" )return mtTriangleFan;
if (LowerCase(text) == "gl_quad_strip" )return mtQuadStrip;
if (LowerCase(text) == "gl_points" )return mtPoints;
return mtNone;
}
void __fastcall matrixtoglenable(tmatrixtype matrix)
{
if (matrix == mtTriangles) glBegin(GL_TRIANGLES);
if (matrix == mtQuads ) glBegin(GL_QUADS);
if (matrix == mtPolygon ) glBegin(GL_POLYGON);
if (matrix == mtLines ) glBegin(GL_LINES);
if (matrix == mtLineLoop ) glBegin(GL_LINE_LOOP);
if (matrix == mtLineStrip ) glBegin(GL_LINE_STRIP);
if (matrix == mtTriangleStrip ) glBegin(GL_TRIANGLE_STRIP);
if (matrix == mtTriangleFan ) glBegin(GL_TRIANGLE_FAN);
if (matrix == mtQuadStrip ) glBegin(GL_QUAD_STRIP);
if (matrix == mtPoints ) glBegin(GL_POINTS);
if (matrix == mtNone ) glBegin(GL_POINTS);
}
unsigned __fastcall matrixtogl(tmatrixtype matrix)
{
if (matrix == mtTriangles) return GL_TRIANGLES;
if (matrix == mtQuads ) return GL_QUADS;
if (matrix == mtPolygon ) return GL_POLYGON;
if (matrix == mtLines ) return GL_LINES;
if (matrix == mtLineLoop ) return GL_LINE_LOOP;
if (matrix == mtLineStrip ) return GL_LINE_STRIP;
if (matrix == mtTriangleStrip ) return GL_TRIANGLE_STRIP;
if (matrix == mtTriangleFan ) return GL_TRIANGLE_FAN;
if (matrix == mtQuadStrip ) return GL_QUAD_STRIP;
return GL_POINTS;
}
float __fastcall VectorLength(t3dpoint vec)
{
return sqrt(Dot(vec,vec));
}
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
*/
int dist3D_Return_Closest_Points_Between_Segments(
t3dpoint p1,t3dpoint p2,t3dpoint p3,t3dpoint p4,t3dpoint &pa,t3dpoint &pb)
{
t3dpoint p13,p43,p21;
double d1343,d4321,d1321,d4343,d2121;
double numer,denom;
double mua; double mub;
p13.x = p1.x - p3.x;
p13.y = p1.y - p3.y;
p13.z = p1.z - p3.z;
p43.x = p4.x - p3.x;
p43.y = p4.y - p3.y;
p43.z = p4.z - p3.z;
if (Abs(p43.x) < SPECIAL_FEAUTRE && Abs(p43.y) < SPECIAL_FEAUTRE && Abs(p43.z) < SPECIAL_FEAUTRE)
return(FALSE);
p21.x = p2.x - p1.x;
p21.y = p2.y - p1.y;
p21.z = p2.z - p1.z;
if (Abs(p21.x) < SPECIAL_FEAUTRE && Abs(p21.y) < SPECIAL_FEAUTRE && Abs(p21.z) < SPECIAL_FEAUTRE)
return(FALSE);
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
denom = d2121 * d4343 - d4321 * d4321;
if (denom == 0.0f) return(FALSE);
if (d4343 == 0.0f) return(FALSE);
if (Abs(denom) <= epsilona) return(FALSE);
numer = d1343 * d4321 - d1321 * d4343;
mua = numer / denom;
mub = (d1343 + d4321 * mua) / d4343;
pa.x = p1.x + mua * p21.x;
pa.y = p1.y + mua * p21.y;
pa.z = p1.z + mua * p21.z;
pb.x = p3.x + mub * p43.x;
pb.y = p3.y + mub * p43.y;
pb.z = p3.z + mub * p43.z;
return(TRUE);
}
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------
float __fastcall dist3D_Segment_to_Segment( t3dpoint S1p0, t3dpoint S1p1, t3dpoint S2p0, t3dpoint S2p1)
{
t3dpoint u = vectorAB(S1p0,S1p1);// S1.P1 - S1.P0; vectorAB equals to B - A
t3dpoint v = vectorAB(S2p0,S2p1);//S2.P1 - S2.P0;
t3dpoint w = vectorAB(S2p0,S1p0);//S1.P0 - S2.P0;
float a = Dot(u,u); // always >= 0
float b = Dot(u,v); //jak duzo u jest na V
float c = Dot(v,v); // always >= 0
float d = Dot(u,w);
float e = Dot(v,w);
float D = a*c - b*b; // always >= 0
float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < SPECIAL_FEAUTRE) { // the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0) sN = 0.0; else if (-d > a) sN = sD; else {
sN = -d;
sD = a;
}
} else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (abs(sN) < SPECIAL_FEAUTRE ? 0.0 : sN / sD);
tc = (abs(tN) < SPECIAL_FEAUTRE ? 0.0 : tN / tD);
// get the difference of the two closest points
t3dpoint dP = vectors_add( w,
vectors_substract_v1minusv2(vector_multiple(u,sc),
vector_multiple(v,tc))); // = S1(sc) - S2(tc)
// ShowMessage(POINT_TO_BGLCMD());
// KA = vector_multiple(S1p0,sc);
// KA.x = sc;
// KB = vector_multiple(S2p0,tc);
// KB.x = tc;
return VectorLength(dP); // return the closest distance
}
float __fastcall AngleBetweenVectors(t3dpoint Vector1,t3dpoint Vector2)
{
float dotProduct = 0.0f;
float vectorsMagnitude = 0.0f;
float angle = 0.0f;
dotProduct = Dot(Vector1, Vector2);
vectorsMagnitude = magnitude(Vector1) * magnitude(Vector2) ;
if (IsNan(vectorsMagnitude) || (vectorsMagnitude == 0.0f) ) return 0;
angle = ArcCos( dotProduct / vectorsMagnitude );
if(IsNan(angle) == true) return 0.0f;
return angle;
}
t3dpoint __fastcall VectorMultiple(t3dpoint A, t3dpoint B)
{
t3dpoint r;
r.x=0.0;r.y=0.0;r.z=0.0;
r.x = A.x * B.x;
r.y = A.y * B.y;
r.z = A.z * B.z;
return r;
}
t3dpoint __fastcall VectorProjectionBDIR(t3dpoint A, t3dpoint B,bool NormalizedB)
{
t3dpoint i = B;
if ( NormalizedB == false ) i = Normalize( i );
float dotp = Dot(A,i);
// float k = 1.0f / (magnitude(A) * magnitude(A));
t3dpoint r = vector_multiple(i,dotp);
// r = vector_multiple(i,k);
return r;
}
void __fastcall MakeIdentity(MATRIX4X4 &t)
{
t.pointer[0] = 1.0;t.pointer[1] = 0.0; t.pointer[2] = 0.0; t.pointer[3] = 0.0;
t.pointer[4] = 0.0;t.pointer[5] = 1.0; t.pointer[6] = 0.0; t.pointer[7] = 0.0;
t.pointer[8] = 0.0;t.pointer[9] = 0.0; t.pointer[10] = 1.0; t.pointer[11] = 0.0;
t.pointer[12] = 0.0;t.pointer[13] = 0.0; t.pointer[14] = 0.0; t.pointer[15] = 1.0;
}
MATRIX4X4 __fastcall MatrixEqual4x4(MATRIX4X4 source)
{
MATRIX4X4 t;
int i;
for (i=0; i < 16; i++)
t.pointer[i] = source.pointer[i];
return t;
}
MATRIX4X4 __fastcall MatrixScale4x4(MATRIX4X4 source, float scale)
{
MATRIX4X4 t;
int i;
for (i=0; i < 16; i++)
t.pointer[i] = source.pointer[i]*scale;
return t;
}
void __fastcall MatrixMulti4x4(float src1[16], float src2[16], MATRIX4X4 &dest)
{
dest.pointer[0] = (src1[0]*src2[0])+(src1[4]*src2[1])+(src1[8]*src2[2])+(src1[12]*src2[3]);
dest.pointer[1] = (src1[1]*src2[0])+(src1[5]*src2[1])+(src1[9]*src2[2])+(src1[13]*src2[3]);
dest.pointer[2] = (src1[2]*src2[0])+(src1[6]*src2[1])+(src1[10]*src2[2])+(src1[14]*src2[3]);
dest.pointer[3] = (src1[3]*src2[0])+(src1[7]*src2[1])+(src1[11]*src2[2])+(src1[15]*src2[3]);
dest.pointer[4] = (src1[0]*src2[4])+(src1[4]*src2[5])+(src1[8]*src2[6])+(src1[12]*src2[7]);
dest.pointer[5] = (src1[1]*src2[4])+(src1[5]*src2[5])+(src1[9]*src2[6])+(src1[13]*src2[7]);
dest.pointer[6] = (src1[2]*src2[4])+(src1[6]*src2[5])+(src1[10]*src2[6])+(src1[14]*src2[7]);
dest.pointer[7] = (src1[3]*src2[4])+(src1[7]*src2[5])+(src1[11]*src2[6])+(src1[15]*src2[7]);
dest.pointer[8] = (src1[0]*src2[8])+(src1[4]*src2[9])+(src1[8]*src2[10])+(src1[12]*src2[11]);
dest.pointer[9] = (src1[1]*src2[8])+(src1[5]*src2[9])+(src1[9]*src2[10])+(src1[13]*src2[11]);
dest.pointer[10] = (src1[2]*src2[8])+(src1[6]*src2[9])+(src1[10]*src2[10])+(src1[14]*src2[11]);
dest.pointer[11] = (src1[3]*src2[8])+(src1[7]*src2[9])+(src1[11]*src2[10])+(src1[15]*src2[11]);
dest.pointer[12] = (src1[0]*src2[12])+(src1[4]*src2[13])+(src1[8]*src2[14])+(src1[12]*src2[15]);
dest.pointer[13] = (src1[1]*src2[12])+(src1[5]*src2[13])+(src1[9]*src2[14])+(src1[13]*src2[15]);
dest.pointer[14] = (src1[2]*src2[12])+(src1[6]*src2[13])+(src1[10]*src2[14])+(src1[14]*src2[15]);
dest.pointer[15] = (src1[3]*src2[12])+(src1[7]*src2[13])+(src1[11]*src2[14])+(src1[15]*src2[15]);
}
void __fastcall MatrixTranspose4x4(MATRIX4X4 &dest)
{
MATRIX4X4 temp;
temp = MatrixEqual4x4(dest);
dest.pointer[0] = temp.pointer[0];
dest.pointer[1] = temp.pointer[4];
dest.pointer[2] = temp.pointer[8];
dest.pointer[3] = temp.pointer[12];
dest.pointer[4] = temp.pointer[1];
dest.pointer[5] = temp.pointer[5];
dest.pointer[6] = temp.pointer[9];
dest.pointer[7] = temp.pointer[13];
dest.pointer[8] = temp.pointer[2];
dest.pointer[9] = temp.pointer[6];
dest.pointer[10] = temp.pointer[10];
dest.pointer[11] = temp.pointer[14];
dest.pointer[12] = temp.pointer[3];
dest.pointer[13] = temp.pointer[7];
dest.pointer[14] = temp.pointer[11];
dest.pointer[15] = temp.pointer[15];
}
MATRIX4X4 __fastcall Transpose(MATRIX4X4 s)
{
MATRIX4X4 tmp;
// chuj
// int i;
// for (i=0; i < 16; i++)
// tmp.pointer[i] = k[i];
return tmp;
}
MATRIX4X4 __fastcall FloatToMatrix(float k[16])
{
MATRIX4X4 tmp;
int i;
for (i=0; i < 16; i++)
tmp.pointer[i] = k[i];
return tmp;
}
MATRIX4X4 __fastcall InverseTranspose(MATRIX4X4 from)
{
MATRIX4X4 result;
float tmp[12]; //temporary pair storage
float det; //determinant
//
// //calculate pairs for first 8 elements (cofactors)
tmp[0] = from.pointer[10] * from.pointer[15];
tmp[1] = from.pointer[11] * from.pointer[14];
tmp[2] = from.pointer[9] * from.pointer[15];
tmp[3] = from.pointer[11] * from.pointer[13];
tmp[4] = from.pointer[9] * from.pointer[14];
tmp[5] = from.pointer[10] * from.pointer[13];
tmp[6] = from.pointer[8] * from.pointer[15];
tmp[7] = from.pointer[11] * from.pointer[12];
tmp[8] = from.pointer[8] * from.pointer[14];
tmp[9] = from.pointer[10] * from.pointer[12];
tmp[10] = from.pointer[8] * from.pointer[13];
tmp[11] = from.pointer[9] * from.pointer[12];
//calculate first 8 elements (cofactors)
result.pointer[0] = tmp[0]*from.pointer[5] + tmp[3]*from.pointer[6] + tmp[4]*from.pointer[7]
- tmp[1]*from.pointer[5] - tmp[2]*from.pointer[6] - tmp[5]*from.pointer[7];
result.pointer[1]= tmp[1]*from.pointer[4] + tmp[6]*from.pointer[6] + tmp[9]*from.pointer[7]
- tmp[0]*from.pointer[4] - tmp[7]*from.pointer[6] - tmp[8]*from.pointer[7];
result.pointer[2]= tmp[2]*from.pointer[4] + tmp[7]*from.pointer[5] + tmp[10]*from.pointer[7]
- tmp[3]*from.pointer[4] - tmp[6]*from.pointer[5] - tmp[11]*from.pointer[7];
result.pointer[3]= tmp[5]*from.pointer[4] + tmp[8]*from.pointer[5] + tmp[11]*from.pointer[6]
- tmp[4]*from.pointer[4] - tmp[9]*from.pointer[5] - tmp[10]*from.pointer[6];
//
result.pointer[4]= tmp[1]*from.pointer[1] + tmp[2]*from.pointer[2] + tmp[5]*from.pointer[3]
- tmp[0]*from.pointer[1] - tmp[3]*from.pointer[2] - tmp[4]*from.pointer[3];
result.pointer[5]= tmp[0]*from.pointer[0] + tmp[7]*from.pointer[2] + tmp[8]*from.pointer[3]
- tmp[1]*from.pointer[0] - tmp[6]*from.pointer[2] - tmp[9]*from.pointer[3];
result.pointer[6]= tmp[3]*from.pointer[0] + tmp[6]*from.pointer[1] + tmp[11]*from.pointer[3]
- tmp[2]*from.pointer[0] - tmp[7]*from.pointer[1] - tmp[10]*from.pointer[3];
result.pointer[7]= tmp[4]*from.pointer[0] + tmp[9]*from.pointer[1] + tmp[10]*from.pointer[2]
- tmp[5]*from.pointer[0] - tmp[8]*from.pointer[1] - tmp[11]*from.pointer[2];
//
//calculate pairs for second 8 elements (cofactors)
tmp[0] = from.pointer[2]*from.pointer[7];
tmp[1] = from.pointer[3]*from.pointer[6];
tmp[2] = from.pointer[1]*from.pointer[7];
tmp[3] = from.pointer[3]*from.pointer[5];
tmp[4] = from.pointer[1]*from.pointer[6];
tmp[5] = from.pointer[2]*from.pointer[5];
tmp[6] = from.pointer[0]*from.pointer[7];
tmp[7] = from.pointer[3]*from.pointer[4];
tmp[8] = from.pointer[0]*from.pointer[6];
tmp[9] = from.pointer[2]*from.pointer[4];
tmp[10] = from.pointer[0]*from.pointer[5];
tmp[11] = from.pointer[1]*from.pointer[4];
// //calculate second 8 elements (cofactors)
result.pointer[8]= tmp[0]*from.pointer[13] + tmp[3]*from.pointer[14] + tmp[4]*from.pointer[15]
- tmp[1]*from.pointer[13] - tmp[2]*from.pointer[14] - tmp[5]*from.pointer[15];
result.pointer[9]= tmp[1]*from.pointer[12] + tmp[6]*from.pointer[14] + tmp[9]*from.pointer[15]
- tmp[0]*from.pointer[12] - tmp[7]*from.pointer[14] - tmp[8]*from.pointer[15];
result.pointer[10]= tmp[2]*from.pointer[12] + tmp[7]*from.pointer[13] + tmp[10]*from.pointer[15]
- tmp[3]*from.pointer[12] - tmp[6]*from.pointer[13] - tmp[11]*from.pointer[15];
result.pointer[11]= tmp[5]*from.pointer[12] + tmp[8]*from.pointer[13] + tmp[11]*from.pointer[14]
- tmp[4]*from.pointer[12] - tmp[9]*from.pointer[13] - tmp[10]*from.pointer[14];
result.pointer[12]= tmp[2]*from.pointer[10] + tmp[5]*from.pointer[11] + tmp[1]*from.pointer[9]
- tmp[4]*from.pointer[11] - tmp[0]*from.pointer[9] - tmp[3]*from.pointer[10];
result.pointer[13]= tmp[8]*from.pointer[11] + tmp[0]*from.pointer[8] + tmp[7]*from.pointer[10]
- tmp[6]*from.pointer[10] - tmp[9]*from.pointer[11] - tmp[1]*from.pointer[8];
result.pointer[14]= tmp[6]*from.pointer[9] + tmp[11]*from.pointer[11] + tmp[3]*from.pointer[8]
- tmp[10]*from.pointer[11] - tmp[2]*from.pointer[8] - tmp[7]*from.pointer[9];
result.pointer[15]= tmp[10]*from.pointer[10] + tmp[4]*from.pointer[8] + tmp[9]*from.pointer[9]
- tmp[8]*from.pointer[9] - tmp[11]*from.pointer[10] - tmp[5]*from.pointer[8];
// calculate determinant
det = from.pointer[0]*result.pointer[0]
+from.pointer[1]*result.pointer[1]
+from.pointer[2]*result.pointer[2]
+from.pointer[3]*result.pointer[3];
//
if(det==0.0f)
{
MATRIX4X4 id;
return id;
}
result=MatrixScale4x4 ( result, 1.0f / det);
return result;
}
MATRIX4X4 __fastcall generateShadowMatrix( t3dpoint normal,t3dpoint point,
float lightX, float lightY, float lightZ, float lightW )
{
MATRIX4X4 ShadowMatrix;
Extended d;
Extended dot;
d = -normal.x*point.x - normal.y*point.y - normal.z*point.z;
dot =normal.x*lightX + normal.y*lightY + normal.z*lightZ + d*lightW;
ShadowMatrix.pointer[0] = -lightX*normal.x + dot;
ShadowMatrix.pointer[4] = -lightX*normal.y;
ShadowMatrix.pointer[8] = -lightX*normal.z;
ShadowMatrix.pointer[12] = -lightX*d;
ShadowMatrix.pointer[1] = -lightY*normal.x;
ShadowMatrix.pointer[5] = -lightY*normal.y + dot;
ShadowMatrix.pointer[9] = -lightY*normal.z;
ShadowMatrix.pointer[13] = -lightY*d;
ShadowMatrix.pointer[2] = -lightZ*normal.x;
ShadowMatrix.pointer[6] = -lightZ*normal.y;
ShadowMatrix.pointer[10] = -lightZ*normal.z + dot;
ShadowMatrix.pointer[14] = -lightZ*d;
ShadowMatrix.pointer[3] = -lightW*normal.x;
ShadowMatrix.pointer[7] = -lightW*normal.y;
ShadowMatrix.pointer[11] = -lightW*normal.z;
ShadowMatrix.pointer[15] = -lightW*d + dot;
return ShadowMatrix;
}
void Matrix44::makedimensions(int dim)
{
DimMat = dim;
numb.set_length(DimMat*DimMat);
nula1.set_length(DimMat*DimMat);
ipiv.set_length(DimMat*DimMat);
indxr.set_length(DimMat*DimMat);
indxc.set_length(DimMat*DimMat);
}
Matrix44::~Matrix44()
{
numb.set_length(0);
nula1.set_length(0);
ipiv.set_length(0);
indxr.set_length(0);
indxc.set_length(0);
}
Matrix44::Matrix44()
{
makedimensions(4);
m[0][0] = 0.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
m[1][0] = 0.0f; m[1][1] = 0.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 0.0f; m[2][3] = 0.0f;
m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 0.0f;
}
Matrix44::Matrix44(float m00, float m01, float m02, float m03,
float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23,
float m30, float m31, float m32, float m33)
{
makedimensions(4);
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23;
m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33;
}
Matrix44 Matrix44::operator +(Matrix44 mat)
{
return Matrix44(m[0][0] + mat.m[0][0], m[0][1] + mat.m[0][1], m[0][2] + mat.m[0][2], m[0][3] + mat.m[0][3],
m[1][0] + mat.m[1][0], m[1][1] + mat.m[1][1], m[1][2] + mat.m[1][2], m[1][3] + mat.m[1][3],
m[2][0] + mat.m[2][0], m[2][1] + mat.m[2][1], m[2][2] + mat.m[2][2], m[2][3] + mat.m[2][3],
m[3][0] + mat.m[3][0], m[3][1] + mat.m[3][1], m[3][2] + mat.m[3][2], m[3][3] + mat.m[3][3]);
}
Matrix44 Matrix44::operator -(Matrix44 mat)
{
return Matrix44(m[0][0] - mat.m[0][0], m[0][1] - mat.m[0][1], m[0][2] - mat.m[0][2], m[0][3] - mat.m[0][3],
m[1][0] - mat.m[1][0], m[1][1] - mat.m[1][1], m[1][2] - mat.m[1][2], m[1][3] - mat.m[1][3],
m[2][0] - mat.m[2][0], m[2][1] - mat.m[2][1], m[2][2] - mat.m[2][2], m[2][3] - mat.m[2][3],
m[3][0] - mat.m[3][0], m[3][1] - mat.m[3][1], m[3][2] - mat.m[3][2], m[3][3] - mat.m[3][3]);
}
Matrix44 Matrix44::operator *(Matrix44 mat)
{
return Matrix44(
mat.m[0][0] * m[0][0] + mat.m[0][1] * m[1][0] + mat.m[0][2] * m[2][0] + mat.m[0][3] * m[3][0],
mat.m[0][0] * m[0][1] + mat.m[0][1] * m[1][1] + mat.m[0][2] * m[2][1] + mat.m[0][3] * m[3][1],
mat.m[0][0] * m[0][2] + mat.m[0][1] * m[1][2] + mat.m[0][2] * m[2][2] + mat.m[0][3] * m[3][2],
mat.m[0][0] * m[0][3] + mat.m[0][1] * m[1][3] + mat.m[0][2] * m[2][3] + mat.m[0][3] * m[3][3],
mat.m[1][0] * m[0][0] + mat.m[1][1] * m[1][0] + mat.m[1][2] * m[2][0] + mat.m[1][3] * m[3][0],
mat.m[1][0] * m[0][1] + mat.m[1][1] * m[1][1] + mat.m[1][2] * m[2][1] + mat.m[1][3] * m[3][1],
mat.m[1][0] * m[0][2] + mat.m[1][1] * m[1][2] + mat.m[1][2] * m[2][2] + mat.m[1][3] * m[3][2],
mat.m[1][0] * m[0][3] + mat.m[1][1] * m[1][3] + mat.m[1][2] * m[2][3] + mat.m[1][3] * m[3][3],
mat.m[2][0] * m[0][0] + mat.m[2][1] * m[1][0] + mat.m[2][2] * m[2][0] + mat.m[2][3] * m[3][0],
mat.m[2][0] * m[0][1] + mat.m[2][1] * m[1][1] + mat.m[2][2] * m[2][1] + mat.m[2][3] * m[3][1],
mat.m[2][0] * m[0][2] + mat.m[2][1] * m[1][2] + mat.m[2][2] * m[2][2] + mat.m[2][3] * m[3][2],
mat.m[2][0] * m[0][3] + mat.m[2][1] * m[1][3] + mat.m[2][2] * m[2][3] + mat.m[2][3] * m[3][3],
mat.m[3][0] * m[0][0] + mat.m[3][1] * m[1][0] + mat.m[3][2] * m[2][0] + mat.m[3][3] * m[3][0],
mat.m[3][0] * m[0][1] + mat.m[3][1] * m[1][1] + mat.m[3][2] * m[2][1] + mat.m[3][3] * m[3][1],
mat.m[3][0] * m[0][2] + mat.m[3][1] * m[1][2] + mat.m[3][2] * m[2][2] + mat.m[3][3] * m[3][2],
mat.m[3][0] * m[0][3] + mat.m[3][1] * m[1][3] + mat.m[3][2] * m[2][3] + mat.m[3][3] * m[3][3]);
}
void Matrix44::operator =(Matrix44 mat)
{
memcpy(m, mat.m, sizeof(float) * 16);
}
void Matrix44::operator =(t3dpoint v)
{
this->LoadIdentity();
m[0][3] = v.x;
m[1][3] = v.y;
m[2][3] = v.z;
}
void Matrix44::RotateAxis(t3dpoint Axis, float angle)
{
float u = Axis.x, v = Axis.y, w = Axis.z;
float L2 = Dot(Axis, Axis);
long double f = L2;
if (f>0.0)
f = sqrtl(f); else f = 1.0;
float L = f;
float cosA = cos(angle);
float sinA = sin(angle);
Matrix44 rot;
rot.m[0][0] = (u * u + (v * v + w * w) * cosA) / L2;
rot.m[0][1] = (u * v * (1 - cosA) - w * L * sinA) / L2;
rot.m[0][2] = (u * w * (1 - cosA) + v * L * sinA) / L2;
rot.m[0][3] = 0;
rot.m[1][0] = (u * v * (1 - cosA) + w * L * sinA) / L2;
rot.m[1][1] = (v * v + (u * u + w * w) * cosA) / L2;
rot.m[1][2] = (v * w * (1 - cosA) - u * L * sinA) / L2;
rot.m[1][3] = 0;
rot.m[2][0] = (u * w * (1 - cosA) - v * L * sinA) / L2;
rot.m[2][1] = (v * w * (1 - cosA) + u * L * sinA) / L2;
rot.m[2][2] = (w * w + (u * u + v * v) * cosA) / L2;
rot.m[2][3] = 0;
rot.m[3][0] = 0;
rot.m[3][1] = 0;
rot.m[3][2] = 0;
rot.m[3][3] = 1;
(*this) = (*this) * rot;
}
void Matrix44::RotateX(float angle)
{
Matrix44 rot(1, 0, 0, 0,
0, cos(angle), -sin(angle), 0,
0, sin(angle), cos(angle), 0,
0, 0, 0, 1);
(*this) = (*this) * rot;
}
void Matrix44::RotateY(float angle)
{
Matrix44 rot(cos(angle), 0, -sin(angle), 0,
0, 1, 0, 0,
sin(angle), 0, cos(angle), 0,
0, 0, 0, 1);
(*this) = (*this) * rot;
}
void Matrix44::RotateZ(float angle)
{
Matrix44 rot(cos(angle), -sin(angle), 0, 0,
sin(angle), cos(angle), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
(*this) = (*this) * rot;
}
void Matrix44::Translate(float x, float y, float z)
{
Matrix44 tran(1, 0, 0, x,
0, 1, 0, y,
0, 0, 1, z,
0, 0, 0, 1);
(*this) = (*this) * tran;
}
void Matrix44::Scale(float x, float y, float z)
{
Matrix44 sc(x, 0, 0, 0,
0, y, 0, 0,
0, 0, z, 0,
0, 0, 0, 1);
(*this) = (*this) * sc;
}
void Matrix44::LoadBiasIdentity()
{
(*this) = Matrix44(0.50, 0.0, 0.0, 0.0,
0.0, 0.50, 0.0, 0.0,
0.0, 0.0, 0.50, 0.0,
0.50, 0.50, 0.50, 1.0);
}
void Matrix44::Load1DBiasIdentity()
{
(*this) = Matrix44(0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f,
0.5f, 0.0f, 0.0f, 0.0f,
0.5f, 0.0f, 0.0f, 1.0f);
}
void Matrix44::LoadIdentity()
{
(*this) = Matrix44(1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0);
}
float * Matrix44::return_gl()
{
float* v = new float[16];
v[ 0] = m[0][0];
v[ 1] = m[1][0];
v[ 2] = m[2][0];
v[ 3] = m[3][0];
v[ 4] = m[0][1];
v[ 5] = m[1][1];
v[ 6] = m[2][1];
v[ 7] = m[3][1];
v[ 8] = m[0][2];
v[ 9] = m[1][2];
v[10] = m[2][2];
v[11] = m[3][2];
v[12] = m[0][3];
v[13] = m[1][3];
v[14] = m[2][3];
v[15] = m[3][3];
return v;
}
void Matrix44::GetGLMatrix(float* v)
{
v[ 0] = m[0][0];
v[ 1] = m[1][0];
v[ 2] = m[2][0];
v[ 3] = m[3][0];
v[ 4] = m[0][1];
v[ 5] = m[1][1];
v[ 6] = m[2][1];
v[ 7] = m[3][1];
v[ 8] = m[0][2];
v[ 9] = m[1][2];
v[10] = m[2][2];
v[11] = m[3][2];
v[12] = m[0][3];
v[13] = m[1][3];
v[14] = m[2][3];
v[15] = m[3][3];
}
void Matrix44::Show()
{
FloatToStr(m[0][0])
+ " " +FloatToStr(m[1][0])
+ " " +FloatToStr(m[2][0])
+ " " +FloatToStr(m[3][0])
+ "\n" +FloatToStr(m[0][1])
+ " " +FloatToStr(m[1][1])
+ " " +FloatToStr(m[2][1])
+ " " +FloatToStr(m[3][1])
+ "\n" +FloatToStr(m[0][2])
+ " " +FloatToStr(m[1][2])
+ " " +FloatToStr(m[2][2])
+ " " +FloatToStr(m[3][2])
+ "\n" +FloatToStr(m[0][3])
+ " " +FloatToStr(m[1][3])
+ " " +FloatToStr(m[2][3])
+ " " +FloatToStr(m[3][3]);
AnsiString f;
ShowMessage(f);
}
//I notice 2 things; OpenGL uses column major matrix notation, so the matrix elements looks like this;
//M[0] = s[0];
//M[4] = s[1];
//M[8] = s[2];
void Matrix44::LoadGLMatrix(float *v)
{
// [r1 u1 l1 px]
// [r2 u2 l2 py]
// [r3 u3 l3 pz]
// [0 0 0 1]
m[0][0] = v[ 0]; // 0 1 2 3
m[1][0] = v[ 1]; // 4 5 6 7
m[2][0] = v[ 2]; // 8 9 10 11
m[3][0] = v[ 3]; // 12 13 14 15
m[0][1] = v[ 4];
m[1][1] = v[ 5]; // 0 4 8 12
m[2][1] = v[ 6]; // 1 5 9 13
m[3][1] = v[ 7]; // 2 6 10 14
m[0][2] = v[ 8]; // 3 7 11 15
m[1][2] = v[ 9];
m[2][2] = v[10];
m[3][2] = v[11];
m[0][3] = v[12];
m[1][3] = v[13];
m[2][3] = v[14];
m[3][3] = v[15];
}
t3dpoint Matrix44::TransformVertex(t3dpoint v)
{
t3dpoint result;
result.x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3];
result.y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3];
result.z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3];
return result;
}
void Matrix44::Transpose()
{
m[0][0] = m[0][0];
m[0][1] = m[1][0];
m[0][2] = m[2][0];
m[0][3] = m[3][0];
m[1][0] = m[0][1];
m[1][1] = m[1][1];
m[1][2] = m[2][1];
m[1][3] = m[3][1];
m[2][0] = m[0][2];
m[2][1] = m[1][2];
m[2][2] = m[2][2];
m[2][3] = m[3][2];
m[3][0] = m[0][3];
m[3][1] = m[1][3];
m[3][2] = m[2][3];
m[3][3] = m[3][3];
}
void Matrix44::Invert()
{
int i, j, l, kod, jmax, k, ll, icol, irow;//: Integer;
double amax, d, c, pomos, big, dum, pivinv;
bool ind; double opek;
for (j=0; j < DimMat; j++) ipiv[j] = 0;
// for j := 0 to Pred(DimMat) do ipiv[j] := 0;
irow = 1;
icol = 1;
// for i := 0 to Pred(DimMat) do
// begin
for (i=0; i < DimMat; i++) {
big = 0;
for (j=0; j < DimMat; j++)
{
if (ipiv[j] != 1.0) {
for (k=0; k < DimMat; k++)
{
if (ipiv[k] == 0)
if (abs(m[j][k]) >= big) {
big = abs(m[j][k]);
irow = j;
icol = k;
}
}
}
}
ipiv[icol] = ipiv[icol] + 1;
if (irow != icol) {
for (l=0; l < DimMat; l++)
{
dum = m[irow][l];
m[irow][l] = m[icol][l];
m[icol][l] = dum;
}
for (l=0; l < DimMat; l++)
{
dum = m[irow + DimMat + 1][l];
m[irow + DimMat + 1][l] = m[icol + DimMat + 1][l];
m[icol + DimMat + 1][l] = dum;
}
}
indxr[i] = irow;
indxc[i] = icol;
if (m[icol][icol] == 0.0)
pivinv = 1.0 / m[icol][icol];
m[icol][icol] = 1.0;
for (l=0; l < DimMat; l++) m[icol][l] = m[icol][l] * pivinv;
for (l=0; l < DimMat; l++) m[icol + DimMat + 1][l] = m[icol + DimMat + 1][l] * pivinv;
for (ll=0; ll < DimMat; ll++)
{
if (ll != icol) {
dum = m[ll][icol];
m[ll][icol] = 0.0;
for (l=0; l < DimMat; l++) m[ll][l] = m[ll][l] - m[icol][l] * dum;
for (l=0; l < DimMat; l++) m[ll + DimMat + 1][l] = m[ll + DimMat + 1][l] - m[icol + DimMat + 1][l] * dum;
}
}
}
for (l=DimMat-1; l >= 0; l--)
{
if (indxr[l] != indxc[l])
{
for (k=0; k < DimMat; k++)
{
dum = m[k][indxr[l]];
m[k][indxr[l]] = m[k][indxc[l]];
m[k][indxc[l]] = dum;
}
}
}
}
//------------------------------------------------------------------------------
float __fastcall mps_to_kph(float meters_per_second)
{
return meters_per_second * 3.60f;
}
float __fastcall kph_to_mps(float kilometers_per_hour)
{
return ((kilometers_per_hour * 5.0f) / 18.0f);
}
float __fastcall kph_to_machspeed(float kilometers_per_hour)
{
if (kilometers_per_hour <= 0.0f) return 0.0f;
return 1225.0f / kilometers_per_hour;
}
float __fastcall celsius_to_kelvin(float celsius)
{
return celsius + 273.15;
}
float __fastcall kelvin_to_celsius(float kelvin)
{
return kelvin - 273.15;
}
float __fastcall asqrtf(float k)
{
float result;
long double squareroot;
squareroot = k;
squareroot = sqrtl( squareroot );
result = float(squareroot);
return result;
}
) or even an ellipsoid, you must scale down your triangles, and scale back the result to the units you use.
