You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
493 lines
10 KiB
493 lines
10 KiB
/*
|
|
|
|
Copyright (C) 2011 Tao Ju
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU Lesser General Public License
|
|
(LGPL) as published by the Free Software Foundation; either
|
|
version 2.1 of the License, or (at your option) any later version.
|
|
|
|
This library is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
Lesser General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public
|
|
License along with this library; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
|
|
|
|
#ifndef INTERSECTION_H
|
|
#define INTERSECTION_H
|
|
|
|
#include "GeoCommon.h"
|
|
#include "PLYReader.h"
|
|
#include "PLYWriter.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
class Intersection
|
|
{
|
|
public:
|
|
Intersection(){} ;
|
|
|
|
static void cross( float a[3], float b[3], float c[3] )
|
|
{
|
|
c[0] = a[1] * b[2] - a[2] * b[1] ;
|
|
c[1] = a[2] * b[0] - a[0] * b[2] ;
|
|
c[2] = a[0] * b[1] - a[1] * b[0] ;
|
|
}
|
|
|
|
static float dot( float a[3], float b[3] )
|
|
{
|
|
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] ;
|
|
}
|
|
|
|
|
|
static int separating( float axes[3], Triangle* t1, Triangle* t2 )
|
|
{
|
|
//float mag1 = sqrt(dot( axes, axes )) ;
|
|
//axes[0] /= mag1 ;
|
|
//axes[1] /= mag1 ;
|
|
//axes[2] /= mag1 ;
|
|
|
|
float min1 = dot( axes, t1->vt[0] ), max1;
|
|
max1 = min1 ;
|
|
float min2 = dot( axes, t2->vt[0] ), max2;
|
|
max2 = min2 ;
|
|
|
|
float temp ;
|
|
for ( int i = 1 ; i < 3 ; i ++ )
|
|
{
|
|
temp = dot( axes, t1->vt[i] ) ;
|
|
if ( temp < min1 )
|
|
{
|
|
min1 = temp ;
|
|
}
|
|
else if ( temp > max1 )
|
|
{
|
|
max1 = temp ;
|
|
}
|
|
|
|
temp = dot( axes, t2->vt[i] ) ;
|
|
if ( temp < min2 )
|
|
{
|
|
min2 = temp ;
|
|
}
|
|
else if ( temp > max2 )
|
|
{
|
|
max2 = temp ;
|
|
}
|
|
}
|
|
|
|
if ( min1 >= max2 || min2 >= max1 )
|
|
{
|
|
return 1 ;
|
|
}
|
|
else
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
|
|
static int separating( float axes[3], Triangle* t1, Triangle* t2, int printout )
|
|
{
|
|
//float mag1 = sqrt(dot( axes, axes )) ;
|
|
//axes[0] /= mag1 ;
|
|
//axes[1] /= mag1 ;
|
|
//axes[2] /= mag1 ;
|
|
|
|
float min1 = dot( axes, t1->vt[0] ), max1;
|
|
max1 = min1 ;
|
|
float min2 = dot( axes, t2->vt[0] ), max2;
|
|
max2 = min2 ;
|
|
|
|
float temp ;
|
|
for ( int i = 1 ; i < 3 ; i ++ )
|
|
{
|
|
temp = dot( axes, t1->vt[i] ) ;
|
|
if ( temp < min1 )
|
|
{
|
|
min1 = temp ;
|
|
}
|
|
else if ( temp > max1 )
|
|
{
|
|
max1 = temp ;
|
|
}
|
|
|
|
temp = dot( axes, t2->vt[i] ) ;
|
|
if ( temp < min2 )
|
|
{
|
|
min2 = temp ;
|
|
}
|
|
else if ( temp > max2 )
|
|
{
|
|
max2 = temp ;
|
|
}
|
|
}
|
|
|
|
if ( printout )
|
|
{
|
|
for ( int k = 0 ; k < 3 ; k ++ )
|
|
{
|
|
printf("(%f %f %f),", t1->vt[k][0], t1->vt[k][1],t1->vt[k][2]);
|
|
}
|
|
printf("\n");
|
|
for ( int k = 0 ; k < 3 ; k ++ )
|
|
{
|
|
printf("(%f %f %f),", t2->vt[k][0], t2->vt[k][1],t2->vt[k][2]);
|
|
}
|
|
|
|
printf("\n(%f, %f), (%f, %f)\n", min1, max1, min2, max2) ;
|
|
}
|
|
|
|
if ( min1 >= max2 || min2 >= max1 )
|
|
{
|
|
return 1 ;
|
|
}
|
|
else
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
|
|
static int separating( float axes[3], Triangle* t1, float v1[3], float v2[3] )
|
|
{
|
|
float mag1 = sqrt(dot( axes, axes )) ;
|
|
axes[0] /= mag1 ;
|
|
axes[1] /= mag1 ;
|
|
axes[2] /= mag1 ;
|
|
|
|
float min1 = dot( axes, t1->vt[0] ), max1 = min1 ;
|
|
float temp ;
|
|
for ( int i = 1 ; i < 3 ; i ++ )
|
|
{
|
|
temp = dot( axes, t1->vt[i] ) ;
|
|
if ( temp < min1 )
|
|
{
|
|
min1 = temp ;
|
|
}
|
|
else if ( temp > max1 )
|
|
{
|
|
max1 = temp ;
|
|
}
|
|
}
|
|
|
|
float min2 = dot( axes, v1 ) ;
|
|
float max2 = dot( axes, v2 ) ;
|
|
if ( min2 > max2 )
|
|
{
|
|
temp = min2 ;
|
|
min2 = max2 ;
|
|
max2 = temp ;
|
|
}
|
|
|
|
|
|
if ( min1 > max2 + 0.00001f || min2 > max1 + 0.00001f )
|
|
{
|
|
return 1 ;
|
|
}
|
|
else
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
|
|
static int testIntersection( Triangle* t1, Triangle* t2 )
|
|
{
|
|
// Two face normals
|
|
float v1[3][3] = {{ t1->vt[1][0] - t1->vt[0][0], t1->vt[1][1] - t1->vt[0][1], t1->vt[1][2] - t1->vt[0][2] },
|
|
{ t1->vt[2][0] - t1->vt[1][0], t1->vt[2][1] - t1->vt[1][1], t1->vt[2][2] - t1->vt[1][2] },
|
|
{ t1->vt[0][0] - t1->vt[2][0], t1->vt[0][1] - t1->vt[2][1], t1->vt[0][2] - t1->vt[2][2] }};
|
|
float v2[3][3] = {{ t2->vt[1][0] - t2->vt[0][0], t2->vt[1][1] - t2->vt[0][1], t2->vt[1][2] - t2->vt[0][2] },
|
|
{ t2->vt[2][0] - t2->vt[1][0], t2->vt[2][1] - t2->vt[1][1], t2->vt[2][2] - t2->vt[1][2] },
|
|
{ t2->vt[0][0] - t2->vt[2][0], t2->vt[0][1] - t2->vt[2][1], t2->vt[0][2] - t2->vt[2][2] } };
|
|
float n1[3], n2[3] ;
|
|
cross( v1[0], v1[1], n1 ) ;
|
|
cross( v2[0], v2[1], n2 ) ;
|
|
|
|
float n[3] ;
|
|
cross( n1, n2, n ) ;
|
|
int i, j ;
|
|
if ( n[0] == 0 && n[1] == 0 && n[2] == 0 )
|
|
{
|
|
// Co-planar
|
|
|
|
if ( dot( n1, t1->vt[0] ) != dot( n1, t2->vt[0] ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
|
|
float axes[3] ;
|
|
for ( i = 0 ; i < 3 ; i ++ )
|
|
{
|
|
cross( n1, v1[i], axes ) ;
|
|
if ( separating( axes, t1, t2 ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
for ( i = 0 ; i < 3 ; i ++ )
|
|
{
|
|
cross( n2, v2[i], axes ) ;
|
|
if ( separating( axes, t1, t2 ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Non co-planar
|
|
if ( separating( n1, t1, t2 ) || separating( n2, t1, t2 ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
|
|
float axes[3] ;
|
|
for ( i = 0 ; i < 3 ; i ++ )
|
|
for ( j = 0 ; j < 3 ; j ++ )
|
|
{
|
|
cross( v1[i], v2[j], axes ) ;
|
|
if ( separating( axes, t1, t2 ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 1 ;
|
|
}
|
|
|
|
static int testIntersection( Triangle* t1, BoundingBox b1, Triangle* t2, BoundingBox b2, int printout )
|
|
{
|
|
// Bounding box
|
|
if ( b1.begin.x > b2.end.x || b1.begin.y > b2.end.y || b1.begin.z > b2.end.z ||
|
|
b2.begin.x > b1.end.x || b2.begin.y > b1.end.y || b2.begin.z > b1.end.z )
|
|
{
|
|
return 0 ;
|
|
}
|
|
|
|
// Two face normals
|
|
float v1[3][3] = {{ t1->vt[1][0] - t1->vt[0][0], t1->vt[1][1] - t1->vt[0][1], t1->vt[1][2] - t1->vt[0][2] },
|
|
{ t1->vt[2][0] - t1->vt[1][0], t1->vt[2][1] - t1->vt[1][1], t1->vt[2][2] - t1->vt[1][2] },
|
|
{ t1->vt[0][0] - t1->vt[2][0], t1->vt[0][1] - t1->vt[2][1], t1->vt[0][2] - t1->vt[2][2] }};
|
|
float v2[3][3] = {{ t2->vt[1][0] - t2->vt[0][0], t2->vt[1][1] - t2->vt[0][1], t2->vt[1][2] - t2->vt[0][2] },
|
|
{ t2->vt[2][0] - t2->vt[1][0], t2->vt[2][1] - t2->vt[1][1], t2->vt[2][2] - t2->vt[1][2] },
|
|
{ t2->vt[0][0] - t2->vt[2][0], t2->vt[0][1] - t2->vt[2][1], t2->vt[0][2] - t2->vt[2][2] } };
|
|
float n1[3], n2[3] ;
|
|
cross( v1[0], v1[1], n1 ) ;
|
|
cross( v2[0], v2[1], n2 ) ;
|
|
|
|
float n[3] ;
|
|
cross( n1, n2, n ) ;
|
|
int i, j ;
|
|
if ( n[0] == 0 && n[1] == 0 && n[2] == 0 )
|
|
{
|
|
// Co-planar, regard it as not intersecting -- Hack!
|
|
return 0 ;
|
|
|
|
if ( printout )
|
|
{
|
|
printf("Co-planar!\n") ;
|
|
}
|
|
if ( dot( n1, t1->vt[0] ) != dot( n1, t2->vt[0] ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
|
|
float axes[3] ;
|
|
for ( i = 0 ; i < 3 ; i ++ )
|
|
{
|
|
cross( n1, v1[i], axes ) ;
|
|
if ( separating( axes, t1, t2, printout ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
for ( i = 0 ; i < 3 ; i ++ )
|
|
{
|
|
cross( n2, v2[i], axes ) ;
|
|
if ( separating( axes, t1, t2, printout ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Non co-planar
|
|
if ( separating( n1, t1, t2, printout ) || separating( n2, t1, t2, printout ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
|
|
float axes[3] ;
|
|
for ( i = 0 ; i < 3 ; i ++ )
|
|
for ( j = 0 ; j < 3 ; j ++ )
|
|
{
|
|
cross( v1[i], v2[j], axes ) ;
|
|
if ( separating( axes, t1, t2, printout ) )
|
|
{
|
|
return 0 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 1 ;
|
|
}
|
|
|
|
static int testIntersection( char* fname, char* outname )
|
|
{
|
|
Triangle *intertris[100000];
|
|
|
|
// Read triangles
|
|
PLYReader* myreader = new PLYReader( fname, 1 ) ;
|
|
int numpoly = myreader->getNumTriangles() ;
|
|
int num = 0 ;
|
|
Triangle** tris = new Triangle *[ numpoly ] ;
|
|
Triangle* tri = NULL ;
|
|
myreader->reset() ;
|
|
while( (tri = myreader->getNextTriangle()) != NULL )
|
|
{
|
|
tris[ num ] = tri ;
|
|
num ++ ;
|
|
}
|
|
myreader->close() ;
|
|
printf("Reading %d polygons, %d triangles.\n", numpoly, num ) ;
|
|
|
|
// Build bounding boxes
|
|
printf("Building bounding boxes...\n") ;
|
|
int i ;
|
|
BoundingBox* boxes = new BoundingBox[ num ] ;
|
|
for ( i = 0 ; i < num ; i ++ )
|
|
{
|
|
Triangle* t = tris[ i ] ;
|
|
|
|
boxes[i].begin.x = 100000 ;
|
|
boxes[i].begin.y = 100000 ;
|
|
boxes[i].begin.z = 100000 ;
|
|
boxes[i].end.x = -100000 ;
|
|
boxes[i].end.y = -100000 ;
|
|
boxes[i].end.z = -100000 ;
|
|
|
|
for ( int j = 0 ; j < 3 ; j ++ )
|
|
{
|
|
if ( t->vt[j][0] < boxes[i].begin.x )
|
|
{
|
|
boxes[i].begin.x = t->vt[j][0] ;
|
|
}
|
|
if ( t->vt[j][0] > boxes[i].end.x )
|
|
{
|
|
boxes[i].end.x = t->vt[j][0] ;
|
|
}
|
|
|
|
if ( t->vt[j][1] < boxes[i].begin.y )
|
|
{
|
|
boxes[i].begin.y = t->vt[j][1] ;
|
|
}
|
|
if ( t->vt[j][1] > boxes[i].end.y )
|
|
{
|
|
boxes[i].end.y = t->vt[j][1] ;
|
|
}
|
|
|
|
if ( t->vt[j][2] < boxes[i].begin.z )
|
|
{
|
|
boxes[i].begin.z = t->vt[j][2] ;
|
|
}
|
|
if ( t->vt[j][2] > boxes[i].end.z )
|
|
{
|
|
boxes[i].end.z = t->vt[j][2] ;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// Test intersections
|
|
printf("Pairwise searching...\n") ;
|
|
int numinters = 0 ;
|
|
for ( i = 0 ; i < num ; i ++ )
|
|
{
|
|
for ( int j = 0 ; j < i ; j ++ )
|
|
{
|
|
if ( testIntersection( tris[i], boxes[i], tris[j], boxes[j], 0 ) )
|
|
{
|
|
intertris[ 2 * numinters ] = tris[i] ;
|
|
intertris[ 2 * numinters + 1 ] = tris[j] ;
|
|
numinters ++ ;
|
|
|
|
/*
|
|
printf("First tri: ");
|
|
for ( int k = 0 ; k < 3 ; k ++ )
|
|
{
|
|
printf("(%f %f %f),", tris[i]->vt[k][0], tris[i]->vt[k][1],tris[i]->vt[k][2]);
|
|
}
|
|
printf("\nSecond tri: ");
|
|
for ( k = 0 ; k < 3 ; k ++ )
|
|
{
|
|
printf("(%f %f %f),", tris[j]->vt[k][0], tris[j]->vt[k][1],tris[j]->vt[k][2]);
|
|
}
|
|
// printf("\n") ;
|
|
*/
|
|
}
|
|
}
|
|
}
|
|
|
|
// Write out
|
|
printf("Write out...\n") ;
|
|
if ( numinters > 0 )
|
|
{
|
|
FILE* intout = fopen( outname, "wb" ) ;
|
|
PLYWriter::writeHeader( intout, 6 * numinters, 2 * numinters ) ;
|
|
Triangle* t ;
|
|
|
|
// Write vertices
|
|
for ( i = 0 ; i < numinters ; i ++ )
|
|
{
|
|
t = intertris[ 2 * i ] ;
|
|
for ( int j = 0 ; j < 3 ; j ++ )
|
|
{
|
|
PLYWriter::writeVertex( intout, t->vt[j] ) ;
|
|
}
|
|
t = intertris[ 2 * i + 1 ] ;
|
|
for (int j = 0 ; j < 3 ; j ++ )
|
|
{
|
|
PLYWriter::writeVertex( intout, t->vt[j] ) ;
|
|
}
|
|
}
|
|
|
|
// Write triangles
|
|
for (int i = 0 ; i < numinters ; i ++ )
|
|
{
|
|
int tind1[] = { 6 * i, 6 * i + 1, 6 * i + 2 } ;
|
|
PLYWriter::writeFace( intout, 3, tind1 ) ;
|
|
int tind2[] = { 6 * i + 3, 6 * i + 4, 6 * i + 5 } ;
|
|
PLYWriter::writeFace( intout, 3, tind2 ) ;
|
|
}
|
|
|
|
fclose( intout ) ;
|
|
}
|
|
|
|
return numinters ;
|
|
}
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|