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; }