Jump to content

  • Log In with Google      Sign In   
  • Create Account


Moving Sphere vs Triangle Collision - Misunderstanding concepts!


Old topic!
Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.

  • You cannot reply to this topic
6 replies to this topic

#1 Dario1986   Members   -  Reputation: 143

Like
0Likes
Like

Posted 28 January 2014 - 09:40 AM

Hello all,

 

I'm taking as referene the next code from www.peroxide.dk/papers/collision/collision.pdf . 

This is the most famous document released and I have some doubts about the concepts involved in.

 

I have made a test myself working with a soup of triangles and:

 

The foundCollision flag return TRUE only when the centre of the sphere hits the triangle.

I think that this "foundCollision" flag should return true when the moving sphere "touches" the triangle despite the distance of the triangle to the sphere's centre.

 

What I'm doing wrong? May be, I'm not correctly passing the parameters through the function? :s

 

The implementation of the code is ok, because the algoryhtm is working, but I have a doubt about that what I posted,

 

Any suggestion?

Thanks!!


Help our doubts¡¡¡Best Regards,

Sponsor:

#2 bmarci   Members   -  Reputation: 336

Like
0Likes
Like

Posted 29 January 2014 - 07:36 AM

Hi, I remember I used that paper too, and also had some issues.

 

As I remember the whole collision algorithm is simplifed as it checks with a unit radius sphere.

So if you use bigger sphere (that seems plauseible :)) or even an ellipsoid, you must scale down your triangles, and scale back the result to the units you use.

Just an idea, though. ;)

 

Also might be helpful, I had problems with "degenerate" polygons when the ratio of edges gets too big. I had to use doubles instead of floats.

I also had problem checking with steady (not moving) sphere.



#3 Dario1986   Members   -  Reputation: 143

Like
0Likes
Like

Posted 29 January 2014 - 02:20 PM

 

So if you use bigger sphere (that seems plauseible smile.png) or even an ellipsoid, you must scale down your triangles, and scale back the result to the units you use.

Just an idea, though. ;)

 

 

 

This is good to hear.

What do you mean by scaling? You have to divide the x,y,z coords of the triangles by the sphere's radius? huh.png

Thanks in advance!

 

PS: I have checked my program with an unit sphere and no scaling, and still the same thing.


Edited by Dario1986, 29 January 2014 - 02:27 PM.

Help our doubts¡¡¡Best Regards,

#4 WiredCat   Members   -  Reputation: 93

Like
0Likes
Like

Posted 29 January 2014 - 04:24 PM

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;

}

Edited by WierdCat, 29 January 2014 - 04:31 PM.


#5 bmarci   Members   -  Reputation: 336

Like
0Likes
Like

Posted 30 January 2014 - 02:31 AM


What do you mean by scaling? You have to divide the x,y,z coords of the triangles by the sphere's radius? 

 

Yes. And the "sphere" can have 3 radiuses in case of ellipsoid.

so:

vertex.x /= sphere.x;

vertex.y /= sphere.y;

vertex.z /= sphere.z;

 

Do the collision and multiply the result pos back to the original coordinates.

Also you have to divide the sphere's position. Literaly every coordinate that you use.



#6 WarAmp   Members   -  Reputation: 673

Like
0Likes
Like

Posted 30 January 2014 - 03:00 PM

If it's a moving sphere, you can just do ray->triangle intersections, and all you have to do is for each triangle, move the origin of the ray along the normal of the triangle by -radius.

 

Like this:

 

Sphere s(center, radius)

 

foreach(T in Triangles)

{

   if (s.touching(T))

   { 

      // Already touching the triangle...do whatever.

   }

   else

   {

      Ray ray(s.center - (T.normal * s.radius), sphereDirection);

      if (ray.Intersect(T))

      {

         CollisionPosition = ray.intersection;

         SpherePositionAtCollision = CollisionPosition + (T.normal * s.radius);

      }

   }

}

 

edit: I thought about this some more and I am wrong. This *does* do some of the work, but you still need to test the sphere against the triangle edges.


Edited by WarAmp, 30 January 2014 - 05:14 PM.

Waramp.Before you insult a man, walk a mile in his shoes.That way, when you do insult him, you'll be a mile away, and you'll have his shoes.

#7 Dario1986   Members   -  Reputation: 143

Like
0Likes
Like

Posted 31 January 2014 - 08:08 PM

Hi all. Thanks for your replies. I will check all that you are suggesting me as soon as possible.

Good luck! smile.png


Help our doubts¡¡¡Best Regards,




Old topic!
Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.



PARTNERS