bool IntersectPolygon (const TPolygon& p, TVector3 *v) {
TRay ray;
double d, s, nuDotProd, wec;
double distsq;
TVector3 nml = MakeNormal (p, v);
ray.pt = TVector3(0., 0., 0.);
ray.vec = nml;
nuDotProd = DotProduct (nml, ray.vec);
if (fabs(nuDotProd) < EPS)
return false;
d = - (nml.x * v[p.vertices[0]].x +
nml.y * v[p.vertices[0]].y +
nml.z * v[p.vertices[0]].z);
if (fabs (d) > 1) return false;
for (int i=0; i < p.num_vertices; i++) {
TVector3 *v0, *v1;
v0 = &v[p.vertices[i]];
v1 = &v[p.vertices[ (i+1) % p.num_vertices ]];
TVector3 edge_vec = SubtractVectors (*v1, *v0);
double edge_len = NormVector (edge_vec);
double t = - DotProduct (*v0, edge_vec);
if (t < 0) {
distsq = MAG_SQD (*v0);
} else if (t > edge_len) {
distsq = MAG_SQD (*v1);
} else {
*v0 = AddVectors (*v0, ScaleVector (t, edge_vec));
distsq = MAG_SQD (*v0);
}
if (distsq <= 1) return true;
}
s = - (d + DotProduct (nml, TVector3(ray.pt.x, ray.pt.y, ray.pt.z))) / nuDotProd;
TVector3 pt = AddVectors (ray.pt, ScaleVector (s, ray.vec));
for (int i=0; i < p.num_vertices; i++) {
TVector3 edge_nml = CrossProduct (nml,
SubtractVectors (v[p.vertices[ (i+1) % p.num_vertices ]], v[p.vertices[i]]));
wec = DotProduct (SubtractVectors (pt, v[p.vertices[i]]), edge_nml);
if (wec < 0) return false;
}
return true;
}
double VectorLength (const TVector3 &v) {
return sqrt (MAG_SQD(v));
}
/* Returns True iff the specified polygon intersections a unit-radius sphere
* centered at the origin. */
bool_t intersect_polygon( polygon_t p, point_t *v )
{
ray_t ray;
vector_t nml, edge_nml, edge_vec;
point_t pt;
double d, s, nuDotProd, wec;
double edge_len, t, distsq;
int i;
/* create a ray from origin along polygon normal */
nml = make_normal( p, v );
ray.pt = make_point( 0., 0., 0. );
ray.vec = nml;
nuDotProd = dot_product( nml, ray.vec );
if ( fabs(nuDotProd) < EPS )
return False;
/* determine distance of plane from origin */
d = -( nml.x * v[p.vertices[0]].x +
nml.y * v[p.vertices[0]].y +
nml.z * v[p.vertices[0]].z );
/* if plane's distance to origin > 1, immediately reject */
if ( fabs( d ) > 1 )
return False;
/* check distances of edges from origin */
for ( i=0; i < p.num_vertices; i++ ) {
point_t *v0, *v1;
v0 = &v[p.vertices[i]];
v1 = &v[p.vertices[ (i+1) % p.num_vertices ]];
edge_vec = subtract_points( *v1, *v0 );
edge_len = normalize_vector( &edge_vec );
/* t is the distance from v0 of the closest point on the line
to the origin */
t = - dot_product( *((vector_t *) v0), edge_vec );
if ( t < 0 ) {
/* use distance from v0 */
distsq = MAG_SQD( *v0 );
} else if ( t > edge_len ) {
/* use distance from v1 */
distsq = MAG_SQD( *v1 );
} else {
/* closest point to origin is on the line segment */
*v0 = move_point( *v0, scale_vector( t, edge_vec ) );
distsq = MAG_SQD( *v0 );
}
if ( distsq <= 1 ) {
return True;
}
}
/* find intersection point of ray and plane */
s = - ( d + dot_product( nml, make_vector(ray.pt.x, ray.pt.y, ray.pt.z) ) ) /
nuDotProd;
pt = move_point( ray.pt, scale_vector( s, ray.vec ) );
/* test if intersection point is in polygon by clipping against it
* (we are assuming that polygon is convex) */
for ( i=0; i < p.num_vertices; i++ ) {
edge_nml = cross_product( nml,
subtract_points( v[p.vertices[ (i+1) % p.num_vertices ]], v[p.vertices[i]] ) );
wec = dot_product( subtract_points( pt, v[p.vertices[i]] ), edge_nml );
if (wec < 0)
return False;
}
return True;
}
