1 //------------------------------------------------------------------------------
3 //------------------------------------------------------------------------------
4 // GLVU : Copyright 1997 - 2002
5 // The University of North Carolina at Chapel Hill
6 //------------------------------------------------------------------------------
7 // Permission to use, copy, modify, distribute and sell this software and its
8 // documentation for any purpose is hereby granted without fee, provided that
9 // the above copyright notice appear in all copies and that both that copyright
10 // notice and this permission notice appear in supporting documentation.
11 // Binaries may be compiled with this software without any royalties or
14 // The University of North Carolina at Chapel Hill makes no representations
15 // about the suitability of this software for any purpose. It is provided
16 // "as is" without express or implied warranty.
18 //=========================================================================
20 //=========================================================================
24 //--------------------------------------------------------------------------
25 // Calculates the plane equation coeffients P = [ A B C D ] for the plane
26 // equation: Ax+By+Cz+D=0 given three vertices defining the plane. The
27 // normalized plane normal (A,B,C) is defined using the right-hand rule.
28 //--------------------------------------------------------------------------
29 float* PlaneEquation(float P[4],
30 const float p0[3], const float p1[3], const float p2[3])
32 // CALCULATE PLANE NORMAL - FIRST THREE COEFFICIENTS (A,B,C)
34 u[0]=p1[0]-p0[0]; u[1]=p1[1]-p0[1]; u[2]=p1[2]-p0[2];
35 v[0]=p2[0]-p0[0]; v[1]=p2[1]-p0[1]; v[2]=p2[2]-p0[2];
37 P[0] = u[1]*v[2] - u[2]*v[1]; // CROSS-PROD BETWEEN u AND v
38 P[1] = u[2]*v[0] - u[0]*v[2]; // DEFINES UNNORMALIZED NORMAL
39 P[2] = u[0]*v[1] - u[1]*v[0];
41 float l = (float)sqrt(P[0]*P[0] + P[1]*P[1] + P[2]*P[2]); // NORMALIZE NORMAL
42 P[0]/=l; P[1]/=l; P[2]/=l;
44 // CALCULATE D COEFFICIENT (USING FIRST PT ON PLANE - p0)
45 P[3] = -(P[0]*p0[0] + P[1]*p0[1] + P[2]*p0[2]);
51 //--------------------------------------------------------------------------
52 // Calculates the plane equation coeffients P = [ A B C D ] for the plane
53 // equation: Ax+By+Cz+D=0 given an unnormalized normal N to the plane and
54 // the distance along the normal to the plane Dist. The dot product between
55 // the normalized normal and a point on the plane should equal the distance
56 // along the normal to the plane (Dist): Ax+By+Cz=Dist, so D=-Dist.
57 //--------------------------------------------------------------------------
58 float* PlaneEquation(float P[4], const float N[3], float Dist)
60 // COPY AND NORMALIZE NORMAL
61 P[0]=N[0]; P[1]=N[1]; P[2]=N[2];
62 float l = (float)sqrt(P[0]*P[0] + P[1]*P[1] + P[2]*P[2]);
63 P[0]/=l; P[1]/=l; P[2]/=l;
65 // CALCULATE D COEFFICIENT
72 //--------------------------------------------------------------------------
73 // Calculates the plane equation coeffients P = [ A B C D ] for the plane
74 // equation: Ax+By+Cz+D=0 given an unnormalized normal N to the plane and
75 // a point pt on the plane. The dot product between the normalized normal
76 // and the point should equal the distance along the normal to the plane,
77 // thus equalling negative D.
78 //--------------------------------------------------------------------------
79 float* PlaneEquation(float P[4], const float N[3], const float pt[3])
81 // COPY AND NORMALIZE NORMAL
82 P[0]=N[0]; P[1]=N[1]; P[2]=N[2];
83 float l = (float)sqrt(P[0]*P[0] + P[1]*P[1] + P[2]*P[2]);
84 P[0]/=l; P[1]/=l; P[2]/=l;
86 // CALCULATE D COEFFICIENT (USING PT ON PLANE)
87 P[3] = -(P[0]*pt[0] + P[1]*pt[1] + P[2]*pt[2]);
92 //--------------------------------------------------------------------------
93 // Transforms a plane by a 4x4 matrix (col-major order, assumes premult/col vect).
94 // The given plane coefficients are altered. The normal (A,B,C) is normalized.
95 // see HOFF tech report: "4x4 matrix transformation of the implicit form of the plane"
96 //--------------------------------------------------------------------------
97 void XformPlane(const float M[16], float P[4])
100 NewP[0] = M[0]*P[0] + M[4]*P[1] + M[8]*P[2];
101 NewP[1] = M[1]*P[0] + M[5]*P[1] + M[9]*P[2];
102 NewP[2] = M[2]*P[0] + M[6]*P[1] + M[10]*P[2];
103 L2 = NewP[0]*NewP[0] + NewP[1]*NewP[1] + NewP[2]*NewP[2];
108 P[3] = P[3]*L2 - (NewP[0]*M[12] + NewP[1]*M[13] + NewP[2]*M[14]);
111 //--------------------------------------------------------------------------
112 // return 1 if point is "outside" (normal side) of plane, 0 if "inside" or on
113 //--------------------------------------------------------------------------
114 int PlanePtOutTest(const float P[4], const float Pt[3])
116 return( (P[0]*Pt[0] + P[1]*Pt[1] + P[2]*Pt[2]) > -P[3] );
119 //--------------------------------------------------------------------------
120 // returns -1 inside, 0 on, 1 outside (normal side)
121 //--------------------------------------------------------------------------
122 int PlanePtInOutTest(const float P[4], const float Pt[3])
124 float DotProd = P[0]*Pt[0] + P[1]*Pt[1] + P[2]*Pt[2];
125 if (DotProd < -P[3]) return(-1);
126 if (DotProd > -P[3]) return(1);
130 //--------------------------------------------------------------------------
131 // Calculates the "signed" distance of a point from a plane (in units of
132 // the normal length, if not normalized). +Dist is on normal dir side of plane.
133 //--------------------------------------------------------------------------
134 float PlaneDistToPt(const float P[4], const float Pt[3])
136 return( P[0]*Pt[0] + P[1]*Pt[1] + P[2]*Pt[2] + P[3] );
139 //--------------------------------------------------------------------------
140 // returns 1 if ray (Start,Dir) intersects plane, 0 if not
141 //--------------------------------------------------------------------------
142 int PlaneRayIsect(const float P[4], const float Start[3], const float Dir[3])
144 float NdotDir = P[0]*Dir[0] + P[1]*Dir[1] + P[2]*Dir[2];
145 float NdotStart = P[0]*Start[0] + P[1]*Start[1] + P[2]*Start[2];
146 if (NdotDir==(float)0) return(0);
147 float t = (-P[3] - NdotStart) / NdotDir;
152 //--------------------------------------------------------------------------
153 // Also computes intersection point, if possible
154 //--------------------------------------------------------------------------
155 int PlaneRayIsect(const float P[4], const float Start[3], const float Dir[3],
158 float NdotDir = P[0]*Dir[0] + P[1]*Dir[1] + P[2]*Dir[2];
159 float NdotStart = P[0]*Start[0] + P[1]*Start[1] + P[2]*Start[2];
160 if (NdotDir==(float)0) return(0);
161 float t = (-P[3] - NdotStart) / NdotDir;
164 IsectPt[0] = Start[0] + Dir[0]*t;
165 IsectPt[1] = Start[1] + Dir[1]*t;
166 IsectPt[2] = Start[2] + Dir[2]*t;
172 //--------------------------------------------------------------------------
173 // Intersects a line with a plane with normal (0,0,-1) with distance d
174 // from the origin. The line is given by position Start and direction Dir.
175 //--------------------------------------------------------------------------
176 int ZPlaneLineIsect(float d, const float Start[3], const float Dir[3],
179 float NdotDir = -Dir[2];
180 if (NdotDir==(float)0) return(0);
181 float t = (d + Start[2]) / NdotDir;
182 IsectPt[0] = Start[0] + Dir[0]*t;
183 IsectPt[1] = Start[1] + Dir[1]*t;
184 IsectPt[2] = Start[2] + Dir[2]*t;
188 //--------------------------------------------------------------------------
189 // returns 1 if edge AB intersects plane, 0 if not
190 //--------------------------------------------------------------------------
191 int PlaneEdgeIsect(const float P[4], const float A[3], const float B[3])
193 float Dir[3] = { B[0]-A[0], B[1]-A[1], B[2]-A[2] };
194 float NdotDir = P[0]*Dir[0] + P[1]*Dir[1] + P[2]*Dir[2];
195 float NdotA = P[0]*A[0] + P[1]*A[1] + P[2]*A[2];
196 if (NdotDir==(float)0) return(0);
197 float t = (-P[3] - NdotA) / NdotDir;
198 if (t>=0 && t<=1) return(1);
202 //--------------------------------------------------------------------------
203 // Also computes intersection point, if possible
204 //--------------------------------------------------------------------------
205 int PlaneEdgeIsect(const float P[4], const float A[3], const float B[3],
208 float Dir[3] = { B[0]-A[0], B[1]-A[1], B[2]-A[2] };
209 float NdotDir = P[0]*Dir[0] + P[1]*Dir[1] + P[2]*Dir[2];
210 float NdotA = P[0]*A[0] + P[1]*A[1] + P[2]*A[2];
211 if (NdotDir==(float)0) return(0);
212 float t = (-P[3] - NdotA) / NdotDir;
215 IsectPt[0] = A[0] + Dir[0]*t;
216 IsectPt[1] = A[1] + Dir[1]*t;
217 IsectPt[2] = A[2] + Dir[2]*t;
223 //---------------------------------------------------------------------------
224 // Box (m,M)/ Plane P overlap test.
225 // returns type of overlap: 1=OUT (side of normal dir), -1=IN, 0=Overlapping
226 // Finds the "closest" and "farthest" points from the plane with respect
227 // to the plane normal dir (the "extremes" of the aabb) and tests for overlap.
228 // m and M are the Min and Max extents of the AABB.
229 //---------------------------------------------------------------------------
230 int PlaneMinMaxBoxOverlap(const float P[4], const float m[3], const float M[3])
232 #define SET(v,x,y,z) v[0]=x; v[1]=y; v[2]=z;
234 // CALC EXTREME PTS (Neg,Pos) ALONG NORMAL AXIS (Pos in dir of norm, etc.)
235 float Neg[3], Pos[3];
237 if(P[1]>0) { if(P[2]>0) { SET(Pos,M[0],M[1],M[2]); SET(Neg,m[0],m[1],m[2]); }
238 else { SET(Pos,M[0],M[1],m[2]); SET(Neg,m[0],m[1],M[2]); } }
239 else { if(P[2]>0) { SET(Pos,M[0],m[1],M[2]); SET(Neg,m[0],M[1],m[2]); }
240 else { SET(Pos,M[0],m[1],m[2]); SET(Neg,m[0],M[1],M[2]); } }
242 if(P[1]>0) { if(P[2]>0) { SET(Pos,m[0],M[1],M[2]); SET(Neg,M[0],m[1],m[2]); }
243 else { SET(Pos,m[0],M[1],m[2]); SET(Neg,M[0],m[1],M[2]); } }
244 else { if(P[2]>0) { SET(Pos,m[0],m[1],M[2]); SET(Neg,M[0],M[1],m[2]); }
245 else { SET(Pos,m[0],m[1],m[2]); SET(Neg,M[0],M[1],M[2]); } }
247 // CHECK DISTANCE TO PLANE FROM EXTREMAL POINTS TO DETERMINE OVERLAP
248 if (PlaneDistToPt(P,Neg) > 0) return(1);
249 else if (PlaneDistToPt(P,Pos) <= 0) return(-1);
253 //---------------------------------------------------------------------------
254 // Edge/Planes intersection test. Returns 0 or 1, calcs In-Out "HitTimes"
255 // IsectPts can be calculated as:
256 // InIsectPt = Start + Dir * InT (if InT>0)
257 // OutIsectPt = Start + Dir * OutT (if OutT>0)
258 // Planes is defined as: float Planes[n][4] for n planes.
259 //---------------------------------------------------------------------------
260 int PlanesRayIsect(const float Planes[][4], int NumPlanes,
261 const float Start[3], const float Dir[3],
262 float *InT, float *OutT)
264 *InT=-99999, *OutT=99999; // INIT INTERVAL T-VAL ENDPTS TO -/+ INFINITY
265 float NdotDir, NdotStart; // STORAGE FOR REPEATED CALCS NEEDED FOR NewT CALC
267 for (int i=0; i<NumPlanes; i++) // CHECK INTERSECTION AGAINST EACH VF PLANE
269 NdotDir = Planes[i][0]*Dir[0] + Planes[i][1]*Dir[1] + Planes[i][2]*Dir[2];
270 NdotStart = Planes[i][0]*Start[0] + Planes[i][1]*Start[1] + Planes[i][2]*Start[2];
271 if (NdotDir == 0) // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
273 if (NdotStart > -Planes[i][3]) return(0); // IF STARTS "OUTSIDE", NO INTERSECTION
277 NewT = (-Planes[i][3] - NdotStart) / NdotDir; // FIND HIT "TIME" (DISTANCE)
278 if (NdotDir < 0) { if (NewT > *InT) *InT=NewT; } // IF "FRONTFACING", MUST BE NEW IN "TIME"
279 else { if (NewT < *OutT) *OutT=NewT; } // IF "BACKFACING", MUST BE NEW OUT "TIME"
281 if (*InT > *OutT) return(0); // CHECK FOR EARLY EXITS (INTERSECTION INTERVAL "OUTSIDE")
284 // IF AT LEAST ONE THE Tvals ARE IN THE INTERVAL [0,1] WE HAVE INTERSECTION
285 if (*InT>=0 || *OutT>=0) return(1);
289 //--------------------------------------------------------------------------
290 // returns whether or not an edge AB intersects a set of planes. The hit
291 // times are returned just as in the ray isect test.
292 //--------------------------------------------------------------------------
293 int PlanesEdgeIsect(const float Planes[][4], int NumPlanes,
294 const float A[3], const float B[3],
295 float *InT, float *OutT)
297 float Dir[3] = {B[0]-A[0],B[1]-A[1],B[2]-A[2]};
298 if ( PlanesRayIsect(Planes,NumPlanes,A,Dir,InT,OutT) )
299 if (*InT>=0 && *InT<=1) return(1);
300 else if (*OutT>=0 && *OutT<=1) return(1);