This is polyint.c in view mode; [Download] [Up]
#include <math.h>
#include "point3.h"
#include "transform.h"
#include "ooglutil.h"
#include "polyint.h"
static char msg[] = "polyint.c";
#define FUDGE 1.e-6
#define PT3SUB_XY(a, b, dst) { (dst).x = (a).x - (b).x; \
(dst).y = (a).y - (b).y; \
(dst).z = 0.0; }
#define PT3LENGTH_XY(a) sqrt((a).x*(a).x + (a).y*(a).y)
#define PT3DOT_XY(a, b) ((a).x*(b).x + (a).y*(b).y)
/*
* Internal routines. These should not need to be called directly.
*/
static int PolyInt_InBBox(int n_verts, Point3 *verts, float tol);
static float PolyInt_Angle(Point3 *pt1, Point3 *pt2);
static int PolyInt_Origin(int n_verts, Point3 *verts, Point3 *origin);
int PolyZInt(int n_verts, Point3 *verts, float tol,
vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
{
int i, j, flag;
int n_oldhits, n_hits = 0;
Point3 v1, v2, pt, bma;
float d, t, len, f;
float angsum = 0.0;
/* Do test for trivial rejection */
if (!PolyInt_InBBox(n_verts, verts, tol)) return 0;
n_hits = n_oldhits = VVCOUNT(*ip);
/* Go through the edges of the polygon looking for edge and vertex hits. */
for (i = 0; i < n_verts; i++) {
j = (i + 1) % n_verts;
Pt3From(&v1, (float)verts[i].x, (float)verts[i].y, (float)0.0);
Pt3From(&v2, (float)verts[j].x, (float)verts[j].y, (float)0.0);
Pt3Sub(&verts[j], &verts[i], &bma);
len = PT3LENGTH_XY(bma);
t = - PT3DOT_XY(verts[i], bma) / len;
f = t / len;
Pt3From(&pt, (float)(verts[i].x + f * bma.x),
(float)(verts[i].y + f * bma.y), (float)(verts[i].z + f * bma.z));
d = PT3LENGTH_XY(pt);
if (d < tol && t > -tol) {
if (t < tol) {
*VVINDEX(*vertices, int, n_hits) = i;
*VVINDEX(*edges, int, n_hits) = -1;
*VVINDEX(*ip, Point3, n_hits) = verts[i];
n_hits++;
} else if (t < len - tol) {
*VVINDEX(*ip, Point3, n_hits) = pt;
*VVINDEX(*vertices, int, n_hits) = -1;
*VVINDEX(*edges, int, n_hits) = i;
*VVINDEX(*ep, Point3, n_hits) = pt;
n_hits++;
}
}
angsum += PolyInt_Angle(&v1, &v2);
}
/* Look for the face hits */
if (!(n_hits - n_oldhits) && n_verts > 2 && fabs(angsum / M_2_PI) > 0.5
&& PolyInt_Origin(n_verts, verts, &pt)) {
*VVINDEX(*ip, Point3, n_hits) = pt;
*VVINDEX(*vertices, int, n_hits) = -1;
*VVINDEX(*edges, int, n_hits) = -1;
n_hits++;
}
VVCOUNT(*ip) = n_hits;
VVCOUNT(*vertices) = n_hits;
VVCOUNT(*edges) = n_hits;
return(n_hits - n_oldhits);
}
int PolyNearPosZInt(int n_verts, Point3 *verts, float tol,
Point3 *ip, int *vertex, int *edge, Point3 *ep) {
int i, closest;
vvec v_ip, v_vertices, v_edges, v_ep;
float zmax;
VVINIT(v_ip, Point3, 4);
VVINIT(v_vertices, int, 4);
VVINIT(v_edges, int, 4);
VVINIT(v_ep, Point3, 4);
closest = -1;
if (!PolyZInt(n_verts, verts, tol, &v_ip, &v_vertices, &v_edges, &v_ep))
return 0;
for (i = 0; i < VVCOUNT(v_ip); i++)
if (VVINDEX(v_ip, Point3, i)->z > -1.0 &&
(closest == -1 || VVINDEX(v_ip, Point3, i)->z > zmax)) {
closest = i;
zmax = VVINDEX(v_ip, Point3, i)->z;
}
if (closest >= 0) {
Pt3Copy(VVINDEX(v_ip, Point3, closest), ip);
*vertex = *VVINDEX(v_vertices, int, closest);
*edge = *VVINDEX(v_edges, int, closest);
Pt3Copy(VVINDEX(v_ep, Point3, closest), ep);
}
vvfree(&v_ip);
vvfree(&v_vertices);
vvfree(&v_edges);
vvfree(&v_ep);
return ((closest < 0) ? 0 : 1);
}
int PolyLineInt(Point3 *pt1, Point3 *pt2, int n_verts, Point3 *verts,
float tol, vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
{
int i;
int n_hits, old_n_hits;
Point3 *vertcopy;
Transform T, Tinv;
PolyInt_Align(pt1, pt2, T);
TmInvert(T, Tinv);
vertcopy = OOGLNewNE(Point3, n_verts, msg);
for (i = 0; i < n_verts; i++) Pt3Transform(T, &verts[i], &vertcopy[i]);
old_n_hits = VVCOUNT(*ip);
n_hits = PolyZInt(n_verts, vertcopy, tol, ip, vertices, edges, ep);
for (i = 0; i < n_hits; i++) {
Pt3Transform(Tinv, VVINDEX(*ip, Point3, old_n_hits + i),
VVINDEX(*ip, Point3, old_n_hits + i));
Pt3Transform(Tinv, VVINDEX(*ep, Point3, old_n_hits + i),
VVINDEX(*ep, Point3, old_n_hits + i));
}
OOGLFree(vertcopy);
return n_hits;
}
int PolyRayInt(Point3 *pt1, Point3 *pt2, int n_verts, Point3 *verts,
float tol, vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
{
int i, j;
int n_hits, old_n_hits;
Point3 *vertcopy;
Transform T, Tinv;
PolyInt_Align(pt1, pt2, T);
TmInvert(T, Tinv);
vertcopy = OOGLNewNE(Point3, n_verts, msg);
for (i = 0; i < n_verts; i++) Pt3Transform(T, &verts[i], &vertcopy[i]);
old_n_hits = VVCOUNT(*ip);
n_hits = PolyZInt(n_verts, vertcopy, tol, ip, vertices, edges, ep);
for (i = j = 0; i < n_hits; i++)
if (VVINDEX(*ip, Point3, old_n_hits + i)->z <= 0) {
Pt3Transform(Tinv, VVINDEX(*ip, Point3, old_n_hits + i),
VVINDEX(*ip, Point3, old_n_hits + j));
Pt3Transform(Tinv, VVINDEX(*ep, Point3, old_n_hits + i),
VVINDEX(*ep, Point3, old_n_hits + j));
j++;
}
OOGLFree(vertcopy);
return j;
}
int PolySegmentInt(Point3 *pt1, Point3 *pt2, int n_verts, Point3 *verts,
float tol, vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
{
int i, j;
int n_hits, old_n_hits;
Point3 *vertcopy;
Transform T, Tinv;
PolyInt_Align(pt1, pt2, T);
TmInvert(T, Tinv);
vertcopy = OOGLNewNE(Point3, n_verts, msg);
for (i = 0; i < n_verts; i++) Pt3Transform(T, &verts[i], &vertcopy[i]);
old_n_hits = VVCOUNT(*ip);
n_hits = PolyZInt(n_verts, vertcopy, tol, ip, vertices, edges, ep);
for (i = j = 0; i < n_hits; i++)
if ((VVINDEX(*ip, Point3, old_n_hits + i)->z <= 0) &&
(VVINDEX(*ip, Point3, old_n_hits + i)->z >= -1.0)) {
Pt3Transform(Tinv, VVINDEX(*ip, Point3, old_n_hits + i),
VVINDEX(*ip, Point3, old_n_hits + j));
Pt3Transform(Tinv, VVINDEX(*ep, Point3, old_n_hits + i),
VVINDEX(*ep, Point3, old_n_hits + j));
j++;
}
OOGLFree(vertcopy);
VVCOUNT(*ip) = old_n_hits + j;
vvtrim(ip);
return j;
}
void PolyInt_Align(Point3 *pt1, Point3 *pt2, Transform T) {
Point3 newpt2;
Transform Ttmp;
if (!bcmp(pt1, pt2, sizeof(Point3))) {
OOGLError(1, "PolyInt_Align called with identical points.");
TmIdentity(T);
return;
}
TmTranslate(T, -pt1->x, -pt1->y, -pt1->z);
Pt3Transform(T, pt2, &newpt2);
TmRotateY(Ttmp, -atan2(newpt2.x, -newpt2.z));
TmConcat(T, Ttmp, T);
Pt3Transform(T, pt2, &newpt2);
TmRotateX(Ttmp, -atan2(newpt2.y, -newpt2.z));
TmConcat(T, Ttmp, T);
Pt3Transform(T, pt2, &newpt2);
if (newpt2.z == 0.0) {
OOGLError(1, "Second point too close to first point in PolyInt_Align");
TmIdentity(T);
return;
}
TmScale(Ttmp, -1.0 / newpt2.z, -1.0 / newpt2.z, -1.0 / newpt2.z);
TmConcat(T, Ttmp, T);
}
/*
* PolyInt_InBBox
* Returns non-zero if the origin is inside the polygon described by
* verts or within tol of begin so; returns 0 otherwise.
*/
static int PolyInt_InBBox(int n_verts, Point3 *verts, float tol) {
int i;
int posx = 0, negx = 0, posy = 0, negy = 0;
for (i = 0; i < n_verts; i++, verts++) {
if (verts->x < tol) negx |= 1;
if (verts->x > -tol) posx |= 1;
if (verts->y < tol) negy |= 1;
if (verts->y > -tol) posy |= 1;
}
return (posx & negx & posy & negy);
}
static float PolyInt_Angle(Point3 *pt1, Point3 *pt2) {
float ans;
if (Pt3Distance(pt1, pt2) < FUDGE) return 0;
ans = PT3DOT_XY(*pt1, *pt2) /
sqrt((double)(PT3DOT_XY(*pt1, *pt1) * PT3DOT_XY(*pt2, *pt2)));
ans = acos(ans);
return(ans * ((pt1->x * pt2->y - pt1->y * pt2->x) >= 0 ? 1.0 : -1.0));
}
static int PolyInt_Origin(int n_verts, Point3 *verts, Point3 *origin) {
int bi, ci;
float pz, pw;
/* Find the first vertex different from the 0th vertex */
for (bi = 0; bi < n_verts && !bcmp(&verts[0], &verts[bi], sizeof(Point3));
bi++);
if (bi >= n_verts) {
Pt3Copy(&verts[0], origin);
return 0;
}
/* Find the first vertex not collinear with the other two vertices */
for (ci = bi+1; ci < n_verts; ci++) {
pz = (verts[0].x * (verts[bi].y - verts[ci].y)
- verts[0].y * (verts[bi].x - verts[ci].x)
+ (verts[bi].x * verts[ci].y - verts[bi].y * verts[ci].x));
if (fabs(pz) > FUDGE) break;
}
if (ci >= n_verts) {
Pt3Copy(&verts[0], origin);
return 0;
}
pw = (verts[0].x * (verts[bi].z*verts[ci].y - verts[bi].y*verts[ci].z)
- verts[0].y * (verts[bi].z*verts[ci].x - verts[bi].x*verts[ci].z)
+ verts[0].z * (verts[bi].y*verts[ci].x - verts[bi].x*verts[ci].y));
origin->x = origin->y = 0.0;
origin->z = -pw / pz;
return 1;
}
These are the contents of the former NiCE NeXT User Group NeXTSTEP/OpenStep software archive, currently hosted by Netfuture.ch.