Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
// GLKGeometry.cpp: implementation of the GLKGeometry class.
//
//////////////////////////////////////////////////////////////////////
#include <stdio.h>
#include <math.h>
//#include <malloc.h>
#include "GLKLib.h"
#include "GLKMatrixLib.h"
#include "GLKGeometry.h"
//////////////////////////////////////////////////////////////////////
// Implementation
//////////////////////////////////////////////////////////////////////
bool GLKGeometry::Normalize(double n[])
{
double tt=sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
if (tt<EPS) {
n[0]=0.0; n[1]=0.0; n[2]=0.0;
return false;
}
else{
n[0]=n[0]/tt; n[1]=n[1]/tt; n[2]=n[2]/tt;
}
return true;
}
bool GLKGeometry::CalPlaneEquation( double & A, double & B, double & C, double & D,
double p1[], double p2[], double l, double m, double n)
{
A = ( p2[1] - p1[1] ) * n - ( p2[2] - p1[2] ) * m;
B = ( p2[2] - p1[2] ) * l - ( p2[0] - p1[0] ) * n;
C = ( p2[0] - p1[0] ) * m - ( p2[1] - p1[1] ) * l;
D = - ( p1[0] * A + p1[1] * B + p1[2] * C );
double tt = A*A + B*B + C*C;
tt = sqrt(tt);
if(tt < EPS) return false;
A = A/tt; B = B/tt; C = C/tt; D = D/tt;
return true;
}
void GLKGeometry::CalArealCoordinate(double p1[], double p2[], double p3[], double pp[],
double &u, double &v, double &w)
{
double area=SpatialTriangleArea(p1,p2,p3);
u=SpatialTriangleArea(pp,p2,p3)/area;
v=SpatialTriangleArea(pp,p3,p1)/area;
w=SpatialTriangleArea(pp,p1,p2)/area;
}
void GLKGeometry::CalLineEquation( double & A, double & B, double & C, double x1, double y1, double x2, double y2)
{
A=y2-y1;
B=x1-x2;
if (fabs(B)<EPS)
{
A=1;
B=0;
C=-x1;
return;
}
C=-(B*y1+A*x1);
}
bool GLKGeometry::ApproximatePlaneEquation( int n, float** p,
double & A, double & B, double & C, double & D )
{
double**pp; int i,j; bool bReturn;
GLKMatrixLib::CreateMatrix(pp,n,3);
for(i=0;i<n;i++)
for(j=0;j<3;j++)
pp[i][j]=p[i][j];
bReturn=ApproximatePlaneEquation(n,pp,A,B,C,D);
GLKMatrixLib::DeleteMatrix(pp,n,3);
return bReturn;
}
bool GLKGeometry::ApproximatePlaneEquation( int n, double** p,
double & A, double & B, double & C, double & D )
{
double **M,**U,**V;
double cp[3],minSingularValue,ll;
int i;
bool rc;
cp[0]=0.0; cp[1]=0.0; cp[2]=0.0;
for(i=0;i<n;i++) {cp[0]+=p[i][0]; cp[1]+=p[i][1]; cp[2]+=p[i][2];}
cp[0]=cp[0]/(double)n; cp[1]=cp[1]/(double)n; cp[2]=cp[2]/(double)n;
GLKMatrixLib::CreateMatrix(M,n,3);
GLKMatrixLib::CreateMatrix(V,3,3);
GLKMatrixLib::CreateMatrix(U,n,n);
for(i=0;i<n;i++) {M[i][0]=p[i][0]-cp[0]; M[i][1]=p[i][1]-cp[1]; M[i][2]=p[i][2]-cp[2];}
rc=GLKMatrixLib::SingularValueDecomposition(M,n,3,U,V);
if (rc) {
minSingularValue=M[0][0]; A=V[0][0]; B=V[0][1]; C=V[0][2];
if (M[1][1]<minSingularValue) {
minSingularValue=M[1][1]; A=V[1][0]; B=V[1][1]; C=V[1][2];
}
if (M[2][2]<minSingularValue) {A=V[2][0]; B=V[2][1]; C=V[2][2];}
ll=sqrt(A*A+B*B+C*C);
if (ll>EPS) {
A=A/ll; B=B/ll; C=C/ll;
D=-(A*cp[0]+B*cp[1]+C*cp[2]);
}
else
rc=false;
}
GLKMatrixLib::DeleteMatrix(M,n,3);
GLKMatrixLib::DeleteMatrix(V,3,3);
GLKMatrixLib::DeleteMatrix(U,n,n);
return rc;
/*
double lumda,delta[5],bb[5],ll;
double **T,**E;
int i,j,k;
bool bRC;
GLKMatrixLib::CreateMatrix(T,5,5);
GLKMatrixLib::CreateMatrix(E,4,4);
A=1.0; B=0.0; C=0.0; D=0.0; lumda=1.0;
for(k=0;k<n;k++) {
E[0][0]+=2.0*p[k][0]*p[k][0]; E[0][1]+=2.0*p[k][0]*p[k][1]; E[0][2]+=2.0*p[k][0]*p[k][2]; E[0][3]+=2.0*p[k][0];
E[1][0]+=2.0*p[k][1]*p[k][0]; E[1][1]+=2.0*p[k][1]*p[k][1]; E[1][2]+=2.0*p[k][1]*p[k][2]; E[1][3]+=2.0*p[k][1];
E[2][0]+=2.0*p[k][2]*p[k][0]; E[2][1]+=2.0*p[k][2]*p[k][1]; E[2][2]+=2.0*p[k][2]*p[k][2]; E[2][3]+=2.0*p[k][2];
E[3][0]+=2.0*p[k][0]; E[3][1]+=2.0*p[k][1]; E[3][2]+=2.0*p[k][2]; E[3][3]+=2.0;
}
//----------------------------------------------------------------------------------------
// using the non-linear optimization method to determine the equation
//----------------------------------------------------------------------------------------
for(int iter=0;iter<50;iter++) {
for(i=0;i<4;i++) for(j=0;j<4;j++) T[i][j]=E[i][j];
T[0][0]+=2.0*lumda; T[1][1]+=2.0*lumda; T[2][2]+=2.0*lumda;
T[4][0]=2.0*A; T[4][1]=2.0*B; T[4][2]=2.0*C; T[4][3]=0.0;
T[0][4]=2.0*A; T[1][4]=2.0*B; T[2][4]=2.0*C; T[3][4]=0.0;
T[4][4]=0.0;
//----------------------------------------------------------------------------------------
bb[0]=-(E[0][0]*A+E[0][1]*B+E[0][2]*C+E[0][3]*D+2.0*lumda*A);
bb[1]=-(E[1][0]*A+E[1][1]*B+E[1][2]*C+E[1][3]*D+2.0*lumda*B);
bb[2]=-(E[2][0]*A+E[2][1]*B+E[2][2]*C+E[2][3]*D+2.0*lumda*C);
bb[3]=-(E[3][0]*A+E[3][1]*B+E[3][2]*C+E[3][3]*D);
bb[4]=-(A*A+B*B+C*C-1.0);
ll=0.0;
for(i=0;i<5;i++) ll+=bb[i]*bb[i];
if (ll<1.0e-10) break;
bRC=GLKMatrixLib::Inverse(T,5);
if (!bRC) break;
GLKMatrixLib::Mul(T,bb,5,5,delta);
A+=delta[0]; B+=delta[1]; C+=delta[2]; D+=delta[3]; lumda+=delta[4];
}
GLKMatrixLib::DeleteMatrix(E,4,4);
GLKMatrixLib::DeleteMatrix(T,5,5);
if (!bRC) {
if (CalPlaneEquation(p[0],p[1],p[2],A,B,C,D)) return true;
return false;
}
ll=sqrt(A*A+B*B+C*C);
A=A/ll; B=B/ll; C=C/ll; D=D/ll;
return true;
*/
}
bool GLKGeometry::CalPlaneEquation( double p0[], double p1[], double p2[], double & A, double & B, double & C, double & D )
{
double x[3],y[3],z[3];
x[0]=p0[0]; x[1]=p1[0]; x[2]=p2[0];
y[0]=p0[1]; y[1]=p1[1]; y[2]=p2[1];
z[0]=p0[2]; z[1]=p1[2]; z[2]=p2[2];
return CalPlaneEquation(A,B,C,D,x,y,z);
}
bool GLKGeometry::CalPlaneEquation( float p0[], float p1[], float p2[], double & A, double & B, double & C, double & D )
{
double x[3],y[3],z[3];
x[0]=p0[0]; x[1]=p1[0]; x[2]=p2[0];
y[0]=p0[1]; y[1]=p1[1]; y[2]=p2[1];
z[0]=p0[2]; z[1]=p1[2]; z[2]=p2[2];
return CalPlaneEquation(A,B,C,D,x,y,z);
}
bool GLKGeometry::CalPlaneEquation( double & A, double & B, double & C, double & D, double x[], double y[], double z[])
{
A = y[0] * ( z[1] - z[2] )
+ y[1] * ( z[2] - z[0] )
+ y[2] * ( z[0] - z[1] );
B = z[0] * ( x[1] - x[2] )
+ z[1] * ( x[2] - x[0] )
+ z[2] * ( x[0] - x[1] );
C = x[0] * ( y[1] - y[2] )
+ x[1] * ( y[2] - y[0] )
+ x[2] * ( y[0] - y[1] );
D = - x[0] * ( y[1]*z[2] - y[2]*z[1] )
- x[1] * ( y[2]*z[0] - y[0]*z[2] )
- x[2] * ( y[0]*z[1] - y[1]*z[0] );
double tt = A*A + B*B + C*C;
tt = sqrt(tt);
if(tt < EPS) return false;
A = A/tt; B = B/tt; C = C/tt; D = D/tt;
return true;
}
bool GLKGeometry::CalPlaneLineIntersection( double p[], double n[],
double A, double B, double C, double D, double &mu)
{
double denom;
denom=A*n[0]+B*n[1]+C*n[2];
if (fabs(denom)<EPS) return false;
mu=-(D+A*p[0]+B*p[1]+C*p[2])/denom;
return true;
}
bool GLKGeometry::CalPlaneLineSegIntersection( double p1[], double p2[],
double A, double B, double C, double D, double &mu)
{
double denom;
denom=A*(p2[0]-p1[0])+B*(p2[1]-p1[1])+C*(p2[2]-p1[2]);
if (fabs(denom)<EPS) return false;
mu=-(D+A*p1[0]+B*p1[1]+C*p1[2])/denom;
if ((mu<0.0) || (mu>1.0)) return false;
return true;
}
void GLKGeometry::ClipPolygonByCube(double* &xp, double* &yp, double* &zp, int &pntNum,
double xmin, double ymin, double zmin, double boxSize)
{
double pp[3],normal[3];
pp[0]=xmin; pp[1]=ymin; pp[2]=zmin;
normal[0]=1.0; normal[1]=0.0; normal[2]=0.0;
ClipPolygonByHalfSpace(xp,yp,zp,pntNum,pp,normal);
normal[0]=0.0; normal[1]=1.0; normal[2]=0.0;
ClipPolygonByHalfSpace(xp,yp,zp,pntNum,pp,normal);
normal[0]=0.0; normal[1]=0.0; normal[2]=1.0;
ClipPolygonByHalfSpace(xp,yp,zp,pntNum,pp,normal);
pp[0]=xmin+boxSize; pp[1]=ymin+boxSize; pp[2]=zmin+boxSize;
normal[0]=-1.0; normal[1]=0.0; normal[2]=0.0;
ClipPolygonByHalfSpace(xp,yp,zp,pntNum,pp,normal);
normal[0]=0.0; normal[1]=-1.0; normal[2]=0.0;
ClipPolygonByHalfSpace(xp,yp,zp,pntNum,pp,normal);
normal[0]=0.0; normal[1]=0.0; normal[2]=-1.0;
ClipPolygonByHalfSpace(xp,yp,zp,pntNum,pp,normal);
}
void GLKGeometry::ClipPolygonByHalfSpace(double* &xp, double* &yp, double* &zp, int &pntNum,
double planePnt[], double planeNormal[])
{
GLKArray xc(50,50,3);
GLKArray yc(50,50,3);
GLKArray zc(50,50,3);
int i,pntIndex,thisIndex,lastIndex;
double dd;
if (pntNum==0) return;
pntIndex=-1;
//----------------------------------------------------------------------------
// Step 1: determine the first positive point' index
for(i=0;i<pntNum;i++) {
dd=(xp[i]-planePnt[0])*planeNormal[0]
+(yp[i]-planePnt[1])*planeNormal[1]
+(zp[i]-planePnt[2])*planeNormal[2];
if (dd>0.0) {pntIndex=i;break;}
}
//----------------------------------------------------------------------------
// Step 2: incrementally adding the point into the list
bool lastFlag;
if (pntIndex>=0) {
thisIndex=pntIndex;
xc.Add(xp[thisIndex]); yc.Add(yp[thisIndex]); zc.Add(zp[thisIndex]);
lastFlag=true;
for(i=1;i<=pntNum;i++) {
lastIndex=thisIndex;
thisIndex=(i+pntIndex)%pntNum;
if (lastFlag) {
dd=(xp[thisIndex]-planePnt[0])*planeNormal[0]
+(yp[thisIndex]-planePnt[1])*planeNormal[1]
+(zp[thisIndex]-planePnt[2])*planeNormal[2];
if (dd>=0.0) {
xc.Add(xp[thisIndex]); yc.Add(yp[thisIndex]); zc.Add(zp[thisIndex]);
lastFlag=true;
}
else {
double scale=dd/((xp[thisIndex]-xp[lastIndex])*planeNormal[0]
+(yp[thisIndex]-yp[lastIndex])*planeNormal[1]
+(zp[thisIndex]-zp[lastIndex])*planeNormal[2]);
xc.Add(xp[thisIndex]*(1.0-scale)+xp[lastIndex]*scale);
yc.Add(yp[thisIndex]*(1.0-scale)+yp[lastIndex]*scale);
zc.Add(zp[thisIndex]*(1.0-scale)+zp[lastIndex]*scale);
lastFlag=false;
}
}
else {
dd=(xp[thisIndex]-planePnt[0])*planeNormal[0]
+(yp[thisIndex]-planePnt[1])*planeNormal[1]
+(zp[thisIndex]-planePnt[2])*planeNormal[2];
if (dd>=0.0) {
double scale=dd/((xp[thisIndex]-xp[lastIndex])*planeNormal[0]
+(yp[thisIndex]-yp[lastIndex])*planeNormal[1]
+(zp[thisIndex]-zp[lastIndex])*planeNormal[2]);
xc.Add(xp[thisIndex]*(1.0-scale)+xp[lastIndex]*scale);
yc.Add(yp[thisIndex]*(1.0-scale)+yp[lastIndex]*scale);
zc.Add(zp[thisIndex]*(1.0-scale)+zp[lastIndex]*scale);
xc.Add(xp[thisIndex]); yc.Add(yp[thisIndex]); zc.Add(zp[thisIndex]);
lastFlag=true;
}
}
}
}
//----------------------------------------------------------------------------
// Step 3: memory allocation
delete xp; delete yp; delete zp; pntNum=xc.GetSize();
if (pntNum>0) {
pntNum--;
xp=new double[pntNum];
yp=new double[pntNum];
zp=new double[pntNum];
for(i=0;i<pntNum;i++) {
xp[i]=xc.GetDoubleAt(i);
yp[i]=yc.GetDoubleAt(i);
zp[i]=zc.GetDoubleAt(i);
}
}
}
bool GLKGeometry::CalLineFacetIntersection(double p[], double n[], double &mu,
double x[], double y[], double z[],
double A, double B, double C, double D)
{
double denom,sp[3],pa1[3],pa2[3],pa3[3],total;
double a1,a2,a3,a;
denom=A*n[0]+B*n[1]+C*n[2];
if (fabs(denom)<EPS) return false;
mu=-(D+A*p[0]+B*p[1]+C*p[2])/denom;
// Obtain the intersection point
for(int i=0;i<3;i++) sp[i]=p[i]+mu*n[i];
// Determine whether or not the intersection point is bounded by x[],y[],z[]
pa1[0]=x[0]; pa1[1]=y[0]; pa1[2]=z[0];
pa2[0]=x[1]; pa2[1]=y[1]; pa2[2]=z[1];
pa3[0]=x[2]; pa3[1]=y[2]; pa3[2]=z[2];
a1=SpatialTriangleArea(sp,pa1,pa2);
a2=SpatialTriangleArea(sp,pa2,pa3);
a3=SpatialTriangleArea(sp,pa3,pa1);
a=a1+a2+a3;
total=SpatialTriangleArea(pa1,pa2,pa3);
if (fabs(a-total)>1.0e-3) return false;
return true;
}
#define EPSILON 0.00000001
bool GLKGeometry::CalLineFacetIntersection( double p[], double n[],
double v0[], double v1[], double v2[],
double& t, double& u, double& v)
{
double edge1[3],edge2[3],tvec[3],pvec[3],qvec[3];
double det,inv_det;
double delta = 0.00000001;
for(int i =0;i<3;i++)
{
edge1[i] = v1[i]-v0[i];
edge2[i] = v2[i]-v0[i];
}
pvec[0] = n[1]*edge2[2]-n[2]*edge2[1];
pvec[1] = n[2]*edge2[0]-n[0]*edge2[2];
pvec[2] = n[0]*edge2[1]-n[1]*edge2[0];
det = edge1[0]*pvec[0]+edge1[1]*pvec[1]+edge1[2]*pvec[2];
if((det>-EPSILON)&&(det<EPSILON))
return false;
inv_det = 1.0/det;
for(int j = 0;j<3;j++)
{
tvec[j] = p[j]-v0[j];
}
u = (tvec[0]*pvec[0]+tvec[1]*pvec[1]+tvec[2]*pvec[2])*inv_det;
//if(u<0.0||u>1.0)
if(u<((-1.0)*delta)||u>(1.0+delta))
return false;
qvec[0] = tvec[1]*edge1[2]-tvec[2]*edge1[1];
qvec[1] = tvec[2]*edge1[0]-tvec[0]*edge1[2];
qvec[2] = tvec[0]*edge1[1]-tvec[1]*edge1[0];
v = (n[0]*qvec[0]+n[1]*qvec[1]+n[2]*qvec[2])*inv_det;
//if(v<0.0||u+v>1.0)
if(v<((-1.0)*delta)||u+v>(1.0+(2*delta)))
return false;
t = (edge2[0]*qvec[0]+edge2[1]*qvec[1]+edge2[2]*qvec[2])*inv_det;
return true;
}
float GLKGeometry::Distance_to_Point(float p1[], float p2[])
{
double dis=(p1[0]-p2[0])*(p1[0]-p2[0])
+(p1[1]-p2[1])*(p1[1]-p2[1])
+(p1[2]-p2[2])*(p1[2]-p2[2]);
dis=sqrt(dis);
return (float)dis;
}
double GLKGeometry::Distance_to_Point(double p1[], double p2[])
{
double dis=(p1[0]-p2[0])*(p1[0]-p2[0])
+(p1[1]-p2[1])*(p1[1]-p2[1])
+(p1[2]-p2[2])*(p1[2]-p2[2]);
dis=sqrt(dis);
return dis;
}
double GLKGeometry::Distance_to_LineSegment(double p[], double p1[], double p2[])
{
double vt[3],pt[3],proj,dist;
vt[0]=p2[0]-p1[0]; vt[1]=p2[1]-p1[1]; vt[2]=p2[2]-p1[2]; Normalize(vt);
pt[0]=p[0]-p1[0]; pt[1]=p[1]-p1[1]; pt[2]=p[2]-p1[2];
proj=pt[0]*vt[0]+pt[1]*vt[1]+pt[2]*vt[2];
dist=(p2[0]-p1[0])*vt[0]+(p2[1]-p1[1])*vt[1]+(p2[2]-p1[2])*vt[2];
if (proj<=0.0) return Distance_to_Point(p,p1);
if (proj>=dist) return Distance_to_Point(p,p2);
pt[0]=pt[0]-vt[0]*proj; pt[1]=pt[1]-vt[1]*proj; pt[2]=pt[2]-vt[2]*proj;
dist=sqrt(pt[0]*pt[0]+pt[1]*pt[1]+pt[2]*pt[2]);
return dist;
}
double GLKGeometry::Distance_to_Triangle_Approx(double p[], double p1[], double p2[], double p3[])
{
double dist,cp[3],temp;
cp[0]=(p3[0]+p1[0]+p2[0])/3.0;
cp[1]=(p3[1]+p1[1]+p2[1])/3.0;
cp[2]=(p3[2]+p1[2]+p2[2])/3.0;
dist=Distance_to_Point(cp,p);
cp[0]=(p1[0]+p2[0])/2.0;
cp[1]=(p1[1]+p2[1])/2.0;
cp[2]=(p1[2]+p2[2])/2.0;
temp=Distance_to_Point(cp,p);
if (temp<dist) {dist=temp;}
cp[0]=(p1[0]+p3[0])/2.0;
cp[1]=(p1[1]+p3[1])/2.0;
cp[2]=(p1[2]+p3[2])/2.0;
temp=Distance_to_Point(cp,p);
if (temp<dist) {dist=temp;}
cp[0]=(p3[0]+p2[0])/2.0;
cp[1]=(p3[1]+p2[1])/2.0;
cp[2]=(p3[2]+p2[2])/2.0;
temp=Distance_to_Point(cp,p);
if (temp<dist) {dist=temp;}
temp=Distance_to_Point(p1,p);
if (temp<dist) {dist=temp;}
temp=Distance_to_Point(p2,p);
if (temp<dist) {dist=temp;}
temp=Distance_to_Point(p3,p);
if (temp<dist) {dist=temp;}
return dist;
}
double GLKGeometry::Distance_to_Triangle_Approx(double p[], double p1[], double p2[], double p3[],
double closePnt[])
{
double dist,cp[3],temp;
cp[0]=(p3[0]+p1[0]+p2[0])/3.0;
cp[1]=(p3[1]+p1[1]+p2[1])/3.0;
cp[2]=(p3[2]+p1[2]+p2[2])/3.0;
dist=Distance_to_Point(cp,p);
cp[0]=(p1[0]+p2[0])/2.0;
cp[1]=(p1[1]+p2[1])/2.0;
cp[2]=(p1[2]+p2[2])/2.0;
temp=Distance_to_Point(cp,p);
if (temp<dist) {dist=temp; closePnt[0]=cp[0]; closePnt[1]=cp[1]; closePnt[2]=cp[2];}
cp[0]=(p1[0]+p3[0])/2.0;
cp[1]=(p1[1]+p3[1])/2.0;
cp[2]=(p1[2]+p3[2])/2.0;
temp=Distance_to_Point(cp,p);
if (temp<dist) {dist=temp; closePnt[0]=cp[0]; closePnt[1]=cp[1]; closePnt[2]=cp[2];}
cp[0]=(p3[0]+p2[0])/2.0;
cp[1]=(p3[1]+p2[1])/2.0;
cp[2]=(p3[2]+p2[2])/2.0;
temp=Distance_to_Point(cp,p);
if (temp<dist) {dist=temp; closePnt[0]=cp[0]; closePnt[1]=cp[1]; closePnt[2]=cp[2];}
temp=Distance_to_Point(p1,p);
if (temp<dist) {dist=temp; closePnt[0]=p1[0]; closePnt[1]=p1[1]; closePnt[2]=p1[2];}
temp=Distance_to_Point(p2,p);
if (temp<dist) {dist=temp; closePnt[0]=p2[0]; closePnt[1]=p2[1]; closePnt[2]=p2[2];}
temp=Distance_to_Point(p3,p);
if (temp<dist) {dist=temp; closePnt[0]=p3[0]; closePnt[1]=p3[1]; closePnt[2]=p3[2];}
return dist;
}
double GLKGeometry::Distance_to_Triangle(double p[], double p1[], double p2[], double p3[],
double closePnt[])
{
double dist,temp,ll,proj,normal[3],v[3],v2[3],cp[3],normal2[3];
bool bFlag=true;
//-----------------------------------------------------------------------------
// distance to face is considerred
CalPlaneEquation(p1,p2,p3,normal[0],normal[1],normal[2],dist);
v[0]=p[0]-p1[0]; v[1]=p[1]-p1[1]; v[2]=p[2]-p1[2];
dist=v[0]*normal[0]+v[1]*normal[1]+v[2]*normal[2];
cp[0]=p[0]-dist*normal[0]; closePnt[0]=cp[0];
cp[1]=p[1]-dist*normal[1]; closePnt[1]=cp[1];
cp[2]=p[2]-dist*normal[2]; closePnt[2]=cp[2];
v2[0]=p2[0]-p1[0]; v2[1]=p2[1]-p1[1]; v2[2]=p2[2]-p1[2];
v[0]=cp[0]-p1[0]; v[1]=cp[1]-p1[1]; v[2]=cp[2]-p1[2];
VectorProduct(v2,v,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
v2[0]=p3[0]-p1[0]; v2[1]=p3[1]-p1[1]; v2[2]=p3[2]-p1[2];
VectorProduct(v,v2,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
if (bFlag) {
v2[0]=p1[0]-p3[0]; v2[1]=p1[1]-p3[1]; v2[2]=p1[2]-p3[2];
v[0]=cp[0]-p3[0]; v[1]=cp[1]-p3[1]; v[2]=cp[2]-p3[2];
VectorProduct(v2,v,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
v2[0]=p2[0]-p3[0]; v2[1]=p2[1]-p3[1]; v2[2]=p2[2]-p3[2];
VectorProduct(v,v2,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
}
if (bFlag) {
v2[0]=p3[0]-p2[0]; v2[1]=p3[1]-p2[1]; v2[2]=p3[2]-p2[2];
v[0]=cp[0]-p2[0]; v[1]=cp[1]-p2[1]; v[2]=cp[2]-p2[2];
VectorProduct(v2,v,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
v2[0]=p1[0]-p2[0]; v2[1]=p1[1]-p2[1]; v2[2]=p1[2]-p2[2];
VectorProduct(v,v2,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
}
if (bFlag) {return fabs(dist);}
//-----------------------------------------------------------------------------
// distances to three points are considerred
dist=Distance_to_Point(p,p1);
closePnt[0]=p1[0]; closePnt[1]=p1[1]; closePnt[2]=p1[2];
temp=Distance_to_Point(p,p2);
if (temp<dist) {
dist=temp; closePnt[0]=p2[0]; closePnt[1]=p2[1]; closePnt[2]=p2[2];
}
temp=Distance_to_Point(p,p3);
if (temp<dist) {
dist=temp; closePnt[0]=p3[0]; closePnt[1]=p3[1]; closePnt[2]=p3[2];
}
//-----------------------------------------------------------------------------
// distances to three segments are considerred
ll=Distance_to_Point(p1,p2);
v[0]=p[0]-p1[0]; v[1]=p[1]-p1[1]; v[2]=p[2]-p1[2];
v2[0]=p2[0]-p1[0]; v2[1]=p2[1]-p1[1]; v2[2]=p2[2]-p1[2];
Normalize(v2);
proj=v[0]*v2[0]+v[1]*v2[1]+v[2]*v2[2];
if ((proj>0.0) && (proj<ll)) {
cp[0]=v[0]-proj*v2[0]; cp[1]=v[1]-proj*v2[1]; cp[2]=v[2]-proj*v2[2];
temp=sqrt(cp[0]*cp[0]+cp[1]*cp[1]+cp[2]*cp[2]);
if (temp<dist) {
dist=temp;
closePnt[0]=p[0]-cp[0]; closePnt[1]=p[1]-cp[1]; closePnt[2]=p[2]-cp[2];
}
}
ll=Distance_to_Point(p2,p3);
v[0]=p[0]-p2[0]; v[1]=p[1]-p2[1]; v[2]=p[2]-p2[2];
v2[0]=p3[0]-p2[0]; v2[1]=p3[1]-p2[1]; v2[2]=p3[2]-p2[2];
Normalize(v2);
proj=v[0]*v2[0]+v[1]*v2[1]+v[2]*v2[2];
if ((proj>0.0) && (proj<ll)) {
cp[0]=v[0]-proj*v2[0]; cp[1]=v[1]-proj*v2[1]; cp[2]=v[2]-proj*v2[2];
temp=sqrt(cp[0]*cp[0]+cp[1]*cp[1]+cp[2]*cp[2]);
if (temp<dist) {
dist=temp;
closePnt[0]=p[0]-cp[0]; closePnt[1]=p[1]-cp[1]; closePnt[2]=p[2]-cp[2];
}
}
ll=Distance_to_Point(p3,p1);
v[0]=p[0]-p3[0]; v[1]=p[1]-p3[1]; v[2]=p[2]-p3[2];
v2[0]=p1[0]-p3[0]; v2[1]=p1[1]-p3[1]; v2[2]=p1[2]-p3[2];
Normalize(v2);
proj=v[0]*v2[0]+v[1]*v2[1]+v[2]*v2[2];
if ((proj>0.0) && (proj<ll)) {
cp[0]=v[0]-proj*v2[0]; cp[1]=v[1]-proj*v2[1]; cp[2]=v[2]-proj*v2[2];
temp=sqrt(cp[0]*cp[0]+cp[1]*cp[1]+cp[2]*cp[2]);
if (temp<dist) {
dist=temp;
closePnt[0]=p[0]-cp[0]; closePnt[1]=p[1]-cp[1]; closePnt[2]=p[2]-cp[2];
}
}
return dist;
}
double GLKGeometry::Distance_to_Triangle(double p[], double p1[], double p2[], double p3[])
{
double dist,temp,ll,proj,normal[3],v[3],v2[3],cp[3],normal2[3];
bool bFlag=true;
//-----------------------------------------------------------------------------
// distance to face is considerred
CalPlaneEquation(p1,p2,p3,normal[0],normal[1],normal[2],dist);
v[0]=p[0]-p1[0]; v[1]=p[1]-p1[1]; v[2]=p[2]-p1[2];
dist=v[0]*normal[0]+v[1]*normal[1]+v[2]*normal[2];
cp[0]=v[0]-dist*normal[0]+p1[0];
cp[1]=v[1]-dist*normal[1]+p1[1];
cp[2]=v[2]-dist*normal[2]+p1[2];
v2[0]=p2[0]-p1[0]; v2[1]=p2[1]-p1[1]; v2[2]=p2[2]-p1[2];
v[0]=cp[0]-p1[0]; v[1]=cp[1]-p1[1]; v[2]=cp[2]-p1[2];
VectorProduct(v2,v,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
v2[0]=p3[0]-p1[0]; v2[1]=p3[1]-p1[1]; v2[2]=p3[2]-p1[2];
VectorProduct(v,v2,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
if (bFlag) {
v2[0]=p1[0]-p3[0]; v2[1]=p1[1]-p3[1]; v2[2]=p1[2]-p3[2];
v[0]=cp[0]-p3[0]; v[1]=cp[1]-p3[1]; v[2]=cp[2]-p3[2];
VectorProduct(v2,v,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
v2[0]=p2[0]-p3[0]; v2[1]=p2[1]-p3[1]; v2[2]=p2[2]-p3[2];
VectorProduct(v,v2,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
}
if (bFlag) {
v2[0]=p3[0]-p2[0]; v2[1]=p3[1]-p2[1]; v2[2]=p3[2]-p2[2];
v[0]=cp[0]-p2[0]; v[1]=cp[1]-p2[1]; v[2]=cp[2]-p2[2];
VectorProduct(v2,v,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
v2[0]=p1[0]-p2[0]; v2[1]=p1[1]-p2[1]; v2[2]=p1[2]-p2[2];
VectorProduct(v,v2,normal2);
temp=normal2[0]*normal[0]+normal2[1]*normal[1]+normal2[2]*normal[2];
if (temp<=0.0) bFlag=false;
}
if (bFlag) {return fabs(dist);}
//-----------------------------------------------------------------------------
// distances to three points are considerred
dist=Distance_to_Point(p,p1);
temp=Distance_to_Point(p,p2); if (temp<dist) dist=temp;
temp=Distance_to_Point(p,p3); if (temp<dist) dist=temp;
//-----------------------------------------------------------------------------
// distances to three segments are considerred
ll=Distance_to_Point(p1,p2);
v[0]=p[0]-p1[0]; v[1]=p[1]-p1[1]; v[2]=p[2]-p1[2];
v2[0]=p2[0]-p1[0]; v2[1]=p2[1]-p1[1]; v2[2]=p2[2]-p1[2];
Normalize(v2);
proj=v[0]*v2[0]+v[1]*v2[1]+v[2]*v2[2];
if ((proj>0.0) && (proj<ll)) {
cp[0]=v[0]-proj*v2[0]; cp[1]=v[1]-proj*v2[1]; cp[2]=v[2]-proj*v2[2];
temp=sqrt(cp[0]*cp[0]+cp[1]*cp[1]+cp[2]*cp[2]);
if (temp<dist) dist=temp;
}
ll=Distance_to_Point(p2,p3);
v[0]=p[0]-p2[0]; v[1]=p[1]-p2[1]; v[2]=p[2]-p2[2];
v2[0]=p3[0]-p2[0]; v2[1]=p3[1]-p2[1]; v2[2]=p3[2]-p2[2];
Normalize(v2);
proj=v[0]*v2[0]+v[1]*v2[1]+v[2]*v2[2];
if ((proj>0.0) && (proj<ll)) {
cp[0]=v[0]-proj*v2[0]; cp[1]=v[1]-proj*v2[1]; cp[2]=v[2]-proj*v2[2];
temp=sqrt(cp[0]*cp[0]+cp[1]*cp[1]+cp[2]*cp[2]);
if (temp<dist) dist=temp;
}
ll=Distance_to_Point(p3,p1);
v[0]=p[0]-p3[0]; v[1]=p[1]-p3[1]; v[2]=p[2]-p3[2];
v2[0]=p1[0]-p3[0]; v2[1]=p1[1]-p3[1]; v2[2]=p1[2]-p3[2];
Normalize(v2);
proj=v[0]*v2[0]+v[1]*v2[1]+v[2]*v2[2];
if ((proj>0.0) && (proj<ll)) {
cp[0]=v[0]-proj*v2[0]; cp[1]=v[1]-proj*v2[1]; cp[2]=v[2]-proj*v2[2];
temp=sqrt(cp[0]*cp[0]+cp[1]*cp[1]+cp[2]*cp[2]);
if (temp<dist) dist=temp;
}
return dist;
}
void GLKGeometry::SpatialPolygonCenter(double *xp, double *yp, double *zp, int pntNum,
double centerPos[])
{
centerPos[0]=0.0; centerPos[1]=0.0; centerPos[2]=0.0;
for(int i=0;i<pntNum;i++) {
centerPos[0]+=xp[i]; centerPos[1]+=yp[i]; centerPos[2]+=zp[i];
}
centerPos[0]=centerPos[0]/(double)pntNum;
centerPos[1]=centerPos[1]/(double)pntNum;
centerPos[2]=centerPos[2]/(double)pntNum;
}
double GLKGeometry::PolygonArea(double *xp, double *yp, int pntNum)
{
double p0[2], p1[2], p2[2];
double area = 0.0;
p0[0]=xp[0]; p0[1]=yp[0];
for (int i=0; i<pntNum-3; i++){
p1[0]=xp[i+1]; p1[1]=yp[i+1];
p2[0]=xp[i+2]; p2[1]=yp[i+2];
area += 0.5*fabs((p1[0]-p0[0])*(p2[1]-p0[1])-(p2[0]-p0[0])*(p1[1]-p0[1]));
}
return area;
}
double GLKGeometry::SpatialPolygonArea(double *xp, double *yp, double *zp, int pntNum)
{
double p0[3], p1[3], p2[3];
double area;
area=0.0;
p0[0]=xp[0]; p0[1]=yp[0]; p0[2]=zp[0];
for(int i=0;i<(pntNum-3);i++) {
p1[0]=xp[i+1]; p1[1]=yp[i+1]; p1[2]=zp[i+1];
p2[0]=xp[i+2]; p2[1]=yp[i+2]; p2[2]=zp[i+2];
area+=SpatialTriangleArea(p0, p1, p2);
}
return area;
}
double GLKGeometry::SpatialTriangleArea(double p0[], double p1[], double p2[])
{
double x1,y1,z1,x2,y2,z2;
double ii,jj,kk;
double area;
x1=p1[0]-p0[0]; y1=p1[1]-p0[1]; z1=p1[2]-p0[2];
x2=p2[0]-p0[0]; y2=p2[1]-p0[1]; z2=p2[2]-p0[2];
ii=y1*z2-z1*y2;
jj=x2*z1-x1*z2;
kk=x1*y2-x2*y1;
area=sqrt(ii*ii+jj*jj+kk*kk)/2.0;
return area;
}
void GLKGeometry::DiscretizationByLength(double* &x, double* &y, double* &z, int n,
double Len, int &m)
{
int *nIndex;
nIndex=new int[n];
m=0;
int startNo,endNo;
for(int j=0;j<n;j++)
{
if ((j==0) || (j==(n-1)))
{
nIndex[m++]=j;
startNo=j;
continue;
}
endNo=j;
double point1[3],point2[3];
point1[0]=x[startNo]; point1[1]=y[startNo]; point1[2]=z[startNo];
point2[0]=x[endNo]; point2[1]=y[endNo]; point2[2]=z[endNo];
double distance=Distance_to_Point(point1,point2);
if (distance>Len)
{
nIndex[m++]=endNo;
startNo=endNo;
}
}
for(int j=0;j<m;j++)
{
x[j]=x[nIndex[j]]; y[j]=y[nIndex[j]]; z[j]=z[nIndex[j]];
}
delete []nIndex;
GLPositionArray posArray;
posArray.Add(x[0],y[0],z[0]);
for(int j=1;j<m;j++)
{
double p1[3],p2[3];
p1[0]=x[j-1]; p1[1]=y[j-1]; p1[2]=z[j-1];
p2[0]=x[j]; p2[1]=y[j]; p2[2]=z[j];
double l=Distance_to_Point(p1,p2);
if (l<=Len)
{
posArray.Add(x[j],y[j],z[j]);
}
else
{
double d[3];
int num=(int)(l/Len+1);
for(int i=0;i<3;i++) d[i]=(p2[i]-p1[i])/((double)num);
for(int i=1;i<=num;i++)
posArray.Add(p1[0]+d[0]*((double)i),
p1[1]+d[1]*((double)i),
p1[2]+d[2]*((double)i));
}
}
delete []x; delete []y; delete []z;
m=posArray.GetSize();
x=new double[m]; y=new double[m]; z=new double[m];
for(int j=0;j<m;j++) posArray.ElementAt(j,x[j],y[j],z[j]);
}
void GLKGeometry::DiscretizationByChordal(double *x, double *y, double *z, int n,
double Chordal, int &m)
{
int *nIndex;
int j;
nIndex=new int[n];
m=0;
int startNo,endNo;
for(j=0;j<n;j++)
{
if ((j==0) || (j==(n-1)))
{
nIndex[m++]=j;
startNo=j;
continue;
}
endNo=j;
double point1[3],point2[3];
point1[0]=x[startNo]; point1[1]=y[startNo]; point1[2]=z[startNo];
point2[0]=x[endNo]; point2[1]=y[endNo]; point2[2]=z[endNo];
for(int k=startNo+1;k<endNo;k++)
{
double point[3];
point[0]=x[k]; point[1]=y[k]; point[2]=z[k];
double distance=Distance_to_LineSegment(point,point1,point2);;
if (distance>Chordal)
{
nIndex[m++]=endNo-1;
startNo=endNo-1;
break;
}
}
}
for(j=0;j<m;j++)
{
x[j]=x[nIndex[j]]; y[j]=y[nIndex[j]]; z[j]=z[nIndex[j]];
}
delete []nIndex;
}
bool GLKGeometry::EdgeFlipDetection(double p1[], double p2[], double p3[], double p4[])
{
double e[6],e2[6],a,minA[2];
e[0]=Distance_to_Point(p1,p2);
e[1]=Distance_to_Point(p2,p3);
e[2]=Distance_to_Point(p3,p4);
e[3]=Distance_to_Point(p4,p1);
e[4]=Distance_to_Point(p1,p3);
e[5]=Distance_to_Point(p2,p4);
for(int i=0;i<6;i++) e2[i]=e[i]*e[i];
a=acos((e2[0]+e2[1]-e2[4])/(2.0*e[0]*e[1]));
minA[0]=a;
a=acos((e2[1]+e2[4]-e2[0])/(2.0*e[1]*e[4]));
if (a<minA[0]) minA[0]=a;
a=acos((e2[0]+e2[4]-e2[1])/(2.0*e[0]*e[4]));
if (a<minA[0]) minA[0]=a;
a=acos((e2[3]+e2[4]-e2[2])/(2.0*e[3]*e[4]));
if (a<minA[0]) minA[0]=a;
a=acos((e2[2]+e2[4]-e2[3])/(2.0*e[2]*e[4]));
if (a<minA[0]) minA[0]=a;
a=acos((e2[2]+e2[3]-e2[4])/(2.0*e[2]*e[3]));
if (a<minA[0]) minA[0]=a;
a=acos((e2[0]+e2[5]-e2[3])/(2.0*e[0]*e[5]));
minA[1]=a;
a=acos((e2[5]+e2[3]-e2[0])/(2.0*e[5]*e[3]));
if (a<minA[1]) minA[1]=a;
a=acos((e2[0]+e2[3]-e2[5])/(2.0*e[0]*e[3]));
if (a<minA[1]) minA[1]=a;
a=acos((e2[1]+e2[5]-e2[2])/(2.0*e[1]*e[5]));
if (a<minA[1]) minA[1]=a;
a=acos((e2[1]+e2[2]-e2[5])/(2.0*e[1]*e[2]));
if (a<minA[1]) minA[1]=a;
a=acos((e2[2]+e2[5]-e2[1])/(2.0*e[2]*e[5]));
if (a<minA[1]) minA[1]=a;
if (minA[0]<minA[1]) return false;
return true;
}
void GLKGeometry::Get3rdPointCoord(double x1,double y1,double r1,double x2,double y2,double r2,double &x3,double &y3)
{
long double r0=sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
long double cs,sn,nx,ny,mx,my,temp;
cs=(r1*r1+r0*r0-r2*r2)/(2.0*r1*r0);
temp=1.0-cs*cs;
if ((fabs(cs)>1.0) || (fabs(cs)<0.0))
{
cs=1.0;
temp=0.0;
r1=r0/2.0;
}
sn=sqrt(temp);
nx=(x2-x1)/r0; ny=(y2-y1)/r0;
mx=nx*cs-ny*sn; my=nx*sn+ny*cs;
x3=mx*r1+x1; y3=my*r1+y1;
}
bool GLKGeometry::JugTwoLineSegmentsIntersectOrNot(double x1, double y1, double x2, double y2,
double x3, double y3, double x4, double y4)
{
if ((x1==x3) && (y1==y3)) return false;
if ((x1==x4) && (y1==y4)) return false;
if ((x2==x3) && (y2==y3)) return false;
if ((x2==x4) && (y2==y4)) return false;
double ua1,ua2,ua,ub1,ub2,ub;
ua1=(x4-x3)*(y1-y3)-(y4-y3)*(x1-x3);
ua2=(y4-y3)*(x2-x1)-(x4-x3)*(y2-y1);
ub1=(x2-x1)*(y1-y3)-(y2-y1)*(x1-x3);
ub2=ua2;
if (fabs(ua2)<EPS) return false;
if (fabs(ub2)<EPS) return false;
ua=ua1/ua2;
ub=ub1/ub2;
if ((ua<0.0) || (ua>1.0)) return false;
if ((ub<0.0) || (ub>1.0)) return false;
return true;
}
bool GLKGeometry::JugClockwiseOrNot(int pNum, double xp[], double yp[])
{
double area=0.0;
for(int i=1;i<pNum;i++) area+=(xp[i-1]-xp[i])*(yp[i-1]+yp[i]);
area+=(xp[pNum-1]-xp[0])*(yp[pNum-1]+yp[0]);
if (area<0.0) return true;
return false;
}
bool GLKGeometry::JugPointInsideOrNot(int pNum, double xp[], double yp[], double x, double y)
{
int i, j;
bool c=false;
j=pNum-1;
for(i=0;i<pNum;j=i++)
{
if ((((yp[i]<=y) && (y<yp[j])) ||
((yp[j]<=y) && (y<yp[i]))) &&
(x<(xp[j]-xp[i])*(y-yp[i])/(yp[j]-yp[i])+xp[i]))
c=!c;
}
return c;
}
bool GLKGeometry::CalTwoLinesIntersection(double a1, double b1, double c1,
double a2, double b2, double c2,
double &xx, double &yy)
{
double d=a1*b2-a2*b1;
if (fabs(d)<EPS) return false;
xx=-(c1*b2-c2*b1)/d;
yy=(c1*a2-c2*a1)/d;
return true;
}
bool GLKGeometry::CalTwoLineSegmentsIntersection(double x1, double y1, double x2, double y2,
double x3, double y3, double x4, double y4,
double &xx, double &yy)
{
double a1,b1,c1,a2,b2,c2;
CalLineEquation(a1,b1,c1,x1,y1,x2,y2);
CalLineEquation(a2,b2,c2,x3,y3,x4,y4);
if (!(CalTwoLinesIntersection(a1,b1,c1,a2,b2,c2,xx,yy))) return false;
double u1;
if (x3==x4)
u1=(yy-y3)/(y4-y3);
else
u1=(xx-x3)/(x4-x3);
double u2;
if (x1==x2)
u2=(yy-y1)/(y2-y1);
else
u2=(xx-x1)/(x2-x1);
if ((u1>=0.0) && (u1<=1.0) && (u2>=0.0) && (u2<=1.0)) return true;
return false;
}
double GLKGeometry::CalAngle(double p1[], double p[], double p2[])
{
double angle;
double a=Distance_to_Point(p,p1);
double b=Distance_to_Point(p,p2);
double c=Distance_to_Point(p1,p2);
angle=acos((a*a+b*b-c*c)/(2.0*a*b));
return ROTATE_TO_DEGREE(angle);
}
bool GLKGeometry::CalSphereEquation(double x[], double y[], double z[],
double& x0, double& y0, double& z0, double& R )
{
double a[16];
double t1,t2,t3,t4;
double M11,M12,M13,M14,M15;
t1=x[0]*x[0]+y[0]*y[0]+z[0]*z[0]; t2=x[1]*x[1]+y[1]*y[1]+z[1]*z[1];
t3=x[2]*x[2]+y[2]*y[2]+z[2]*z[2]; t4=x[3]*x[3]+y[3]*y[3]+z[0]*z[3];
a[0]=x[0]; a[1]=y[0]; a[2]=z[0]; a[3]=1.0;
a[4]=x[1]; a[5]=y[1]; a[6]=z[1]; a[7]=1.0;
a[8]=x[2]; a[9]=y[2]; a[10]=z[2]; a[11]=1.0;
a[12]=x[3]; a[13]=y[3]; a[14]=z[3]; a[15]=1.0;
M11=Determinant4(a);
a[0]=t1; a[1]=y[0]; a[2]=z[0]; a[3]=1.0;
a[4]=t2; a[5]=y[1]; a[6]=z[1]; a[7]=1.0;
a[8]=t3; a[9]=y[2]; a[10]=z[2]; a[11]=1.0;
a[12]=t4; a[13]=y[3]; a[14]=z[3]; a[15]=1.0;
M12=Determinant4(a);
a[0]=t1; a[1]=x[0]; a[2]=z[0]; a[3]=1.0;
a[4]=t2; a[5]=x[1]; a[6]=z[1]; a[7]=1.0;
a[8]=t3; a[9]=x[2]; a[10]=z[2]; a[11]=1.0;
a[12]=t4; a[13]=x[3]; a[14]=z[3]; a[15]=1.0;
M13=Determinant4(a);
a[0]=t1; a[1]=x[0]; a[2]=y[0]; a[3]=1.0;
a[4]=t2; a[5]=x[1]; a[6]=y[1]; a[7]=1.0;
a[8]=t3; a[9]=x[2]; a[10]=y[2]; a[11]=1.0;
a[12]=t4; a[13]=x[3]; a[14]=y[3]; a[15]=1.0;
M14=Determinant4(a);
a[0]=t1; a[1]=x[0]; a[2]=y[0]; a[3]=z[0];
a[4]=t2; a[5]=x[1]; a[6]=y[1]; a[7]=z[1];
a[8]=t3; a[9]=x[2]; a[10]=y[2]; a[11]=z[2];
a[12]=t4; a[13]=x[3]; a[14]=y[3]; a[15]=z[3];
M15=Determinant4(a);
if (M11<EPS) return false;
x0=M12/(2.0*M11);
y0=-M13/(2.0*M11);
z0=M14/(2.0*M11);
R=sqrt((M12*M12+M13*M13+M14*M14-4.0*M15*M11)/(4.0*M11*M11));
return true;
}
double GLKGeometry::Determinant3(double a[])
{
double r;
r=a[0]*a[4]*a[8]-a[0]*a[5]*a[7]
+a[1]*a[5]*a[6]-a[1]*a[3]*a[8]
+a[2]*a[3]*a[7]-a[2]*a[4]*a[6];
return r;
}
double GLKGeometry::Determinant4(double a[])
{
double r;
double b[9];
b[0]=a[5]; b[1]=a[6]; b[2]=a[7];
b[3]=a[9]; b[4]=a[10]; b[5]=a[11];
b[6]=a[13]; b[7]=a[14]; b[8]=a[15];
r=a[0]*Determinant3(b);
b[0]=a[4]; b[1]=a[6]; b[2]=a[7];
b[3]=a[8]; b[4]=a[10]; b[5]=a[11];
b[6]=a[12]; b[7]=a[14]; b[8]=a[15];
r=r-a[1]*Determinant3(b);
b[0]=a[4]; b[1]=a[5]; b[2]=a[7];
b[3]=a[8]; b[4]=a[9]; b[5]=a[11];
b[6]=a[12]; b[7]=a[13]; b[8]=a[15];
r=r+a[2]*Determinant3(b);
b[0]=a[4]; b[1]=a[5]; b[2]=a[6];
b[3]=a[8]; b[4]=a[9]; b[5]=a[10];
b[6]=a[12]; b[7]=a[13]; b[8]=a[14];
r=r-a[3]*Determinant3(b);
return r;
}
void GLKGeometry::VectorProduct(double n1[], double n2[], double n3[])
{
n3[0]=n1[1]*n2[2]-n1[2]*n2[1];
n3[1]=n1[2]*n2[0]-n1[0]*n2[2];
n3[2]=n1[0]*n2[1]-n1[1]*n2[0];
}
double GLKGeometry::VectorProject(double n1[], double n2[])
{
double r=n1[0]*n2[0]+n1[1]*n2[1]+n1[2]*n2[2];
return r;
}
double GLKGeometry::VectorProject(double n1[], double n2[], const int n)
{
double r = 0.0;
for(int i=0;i<n;i++) r += n1[i]*n2[i];
return r;
}
double GLKGeometry::TripleProduct(double va[], double vb[], double vc[])
{
return (va[0]*vb[1]*vc[2]+va[1]*vb[2]*vc[0]+va[2]*vb[0]*vc[1]
-va[0]*vb[2]*vc[1]-va[1]*vb[0]*vc[2]-va[2]*vb[1]*vc[0]);
}
void GLKGeometry::CoordinateTransf(double xA[], double yA[], double zA[], double p[],
double &xx, double &yy, double &zz)
{
xx=VectorProject(p,xA);
yy=VectorProject(p,yA);
zz=VectorProject(p,zA);
}
void GLKGeometry::InverseCoordinateTransf(double xA[], double yA[], double zA[],
double p[], double &xx, double &yy, double &zz)
{
xx=p[0]*xA[0]+p[1]*yA[0]+p[2]*zA[0];
yy=p[0]*xA[1]+p[1]*yA[1]+p[2]*zA[1];
zz=p[0]*xA[2]+p[1]*yA[2]+p[2]*zA[2];
}
void GLKGeometry::RotatePointAlongX(double px, double py, double pz, double angle,
double &px1, double &py1, double &pz1)
{
double a=DEGREE_TO_ROTATE(angle);
double ca=cos(a),sa=sin(a);
px1=px;
py1=py*ca-pz*sa;
pz1=py*sa+pz*ca;
}
void GLKGeometry::RotatePointAlongY(double px, double py, double pz, double angle,
double &px1, double &py1, double &pz1)
{
double a=DEGREE_TO_ROTATE(angle);
double ca=cos(a),sa=sin(a);
px1=pz*sa+px*ca;
py1=py;
pz1=pz*ca-px*sa;
}
void GLKGeometry::RotatePointAlongZ(double px, double py, double pz, double angle,
double &px1, double &py1, double &pz1)
{
double a=DEGREE_TO_ROTATE(angle);
double ca=cos(a),sa=sin(a);
px1=px*ca-py*sa;
py1=px*sa+py*ca;
pz1=pz;
}
void GLKGeometry::RotatePointAlongVector(double px, double py, double pz,
double x1, double y1, double z1, double x2, double y2, double z2,
double angle, double &px1, double &py1, double &pz1)
{
double rx,ry,rz,rrrr; double costheta,sintheta;
angle=DEGREE_TO_ROTATE(angle);
costheta=cos(angle); sintheta=sin(angle);
px1=0.0; py1=0.0; pz1=0.0; px=px-x1; py=py-y1; pz=pz-z1;
rx=x2-x1; ry=y2-y1; rz=z2-z1; rrrr=sqrt(rx*rx+ry*ry+rz*rz);
rx=rx/rrrr; ry=ry/rrrr; rz=rz/rrrr;
px1 += (costheta + (1 - costheta) * rx * rx) * px;
px1 += ((1 - costheta) * rx * ry - rz * sintheta) * py;
px1 += ((1 - costheta) * rx * rz + ry * sintheta) * pz;
py1 += ((1 - costheta) * rx * ry + rz * sintheta) * px;
py1 += (costheta + (1 - costheta) * ry * ry) * py;
py1 += ((1 - costheta) * ry * rz - rx * sintheta) * pz;
pz1 += ((1 - costheta) * rx * rz - ry * sintheta) * px;
pz1 += ((1 - costheta) * ry * rz + rx * sintheta) * py;
pz1 += (costheta + (1 - costheta) * rz * rz) * pz;
px1 += x1; py1 += y1; pz1 += z1;
}
void GLKGeometry::QuickSort(int pArr[], int d, int h, bool bAscending)
{
int i,j;
int str;
i = h;
j = d;
str = pArr[((int) ((d+h) / 2))];
do {
if (bAscending) {
while (pArr[j] < str) j++;
while (pArr[i] > str) i--;
} else {
while (pArr[j] > str) j++;
while (pArr[i] < str) i--;
}
if ( i >= j ) {
if ( i != j ) {
int zal;
zal = pArr[i];
pArr[i] = pArr[j];
pArr[j] = zal;
}
i--;
j++;
}
} while (j <= i);
if (d < i) QuickSort(pArr,d,i,bAscending);
if (j < h) QuickSort(pArr,j,h,bAscending);
}
void GLKGeometry::ConvexHull2D(Point2D *points, int n, std::vector<Point2D> &hull)
{
// There must be at least 3 points
if (n < 3) return;
hull.clear();
// Find the leftmost point
int l = 0;
for (int i = 1; i < n; i++)
if (points[i].x <= points[l].x)
l = i;
// Start from leftmost point, keep moving counterclockwise
// until reach the start point again. This loop runs O(h)
// times where h is number of points in result or output.
int p = l, q;
int number = 0;
do
{
// Add current point to result
hull.push_back(points[p]);
// Search for a point 'q' such that orientation(p, x,
// q) is counterclockwise for all points 'x'. The idea
// is to keep track of last visited most counterclock-
// wise point in q. If any point 'i' is more counterclock-
// wise than q, then update q.
q = (p+1)%n;
for (int i = 0; i < n; i++)
{
// If i is more counterclockwise than current q, then
// update q
if (orientation(points[p], points[i], points[q]) == 2)
q = i;
}
// Now q is the most counterclockwise with respect to p
// Set p as q for next iteration, so that q is added to
// result 'hull'
p = q;
number++;
if (number > n)
break;
} while (p != l); // While we don't come to first point
}
4a2d0fdf7This file is pinned to the selected release. Download the original to open it in your design tool.