simgear/scene/sky/clouds3d/plane.cpp

   1 //------------------------------------------------------------------------------
   2 // File : plane.cpp
   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
  12 // restrictions.
  13 //
  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.
  17
  18 //=========================================================================
  19 // plane.cpp
  20 //=========================================================================
  21
  22 #include <math.h>
  23
  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])
  31 {
  32   // CALCULATE PLANE NORMAL - FIRST THREE COEFFICIENTS (A,B,C)
  33   float u[3], v[3];
  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];
  36
  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];
  40
  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;
  43
  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]);
  46
  47   return(P);
  48 }
  49
  50
  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)
  59 {
  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;
  64
  65   // CALCULATE D COEFFICIENT
  66   P[3]=-Dist;
  67
  68   return(P);
  69 }
  70
  71
  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])
  80 {
  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;
  85
  86   // CALCULATE D COEFFICIENT (USING PT ON PLANE)
  87   P[3] = -(P[0]*pt[0] + P[1]*pt[1] + P[2]*pt[2]);
  88
  89   return(P);
  90 }
  91
  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])
  98 {
  99   float NewP[4], L2, L;
 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];
 104         L = (float)sqrt(L2);
 105   P[0] = NewP[0] / L;
 106   P[1] = NewP[1] / L;
 107   P[2] = NewP[2] / L;
 108   P[3] = P[3]*L2 - (NewP[0]*M[12] + NewP[1]*M[13] + NewP[2]*M[14]);
 109 }
 110
 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])
 115 {
 116   return( (P[0]*Pt[0] + P[1]*Pt[1] + P[2]*Pt[2]) > -P[3] );
 117 }
 118
 119 //--------------------------------------------------------------------------
 120 // returns -1 inside, 0 on, 1 outside (normal side)
 121 //--------------------------------------------------------------------------
 122 int PlanePtInOutTest(const float P[4], const float Pt[3])
 123 {
 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);
 127   return(0);
 128 }
 129
 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])
 135 {
 136   return( P[0]*Pt[0] + P[1]*Pt[1] + P[2]*Pt[2] + P[3] );
 137 }
 138
 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])
 143 {
 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;
 148   if (t>=0) return(1);
 149   return(0);
 150 }
 151
 152 //--------------------------------------------------------------------------
 153 // Also computes intersection point, if possible
 154 //--------------------------------------------------------------------------
 155 int PlaneRayIsect(const float P[4], const float Start[3], const float Dir[3],
 156                   float IsectPt[3])
 157 {
 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;
 162   if (t>=0)
 163   {
 164     IsectPt[0] = Start[0] + Dir[0]*t;
 165     IsectPt[1] = Start[1] + Dir[1]*t;
 166     IsectPt[2] = Start[2] + Dir[2]*t;
 167     return(1);
 168   }
 169   return(0);
 170 }
 171
 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],
 177                     float IsectPt[3])
 178 {
 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;
 185   return(1);
 186 }
 187
 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])
 192 {
 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);
 199   return(0);
 200 }
 201
 202 //--------------------------------------------------------------------------
 203 // Also computes intersection point, if possible
 204 //--------------------------------------------------------------------------
 205 int PlaneEdgeIsect(const float P[4], const float A[3], const float B[3],
 206                    float IsectPt[3])
 207 {
 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;
 213   if (t>=0 && t<=1)
 214   {
 215     IsectPt[0] = A[0] + Dir[0]*t;
 216     IsectPt[1] = A[1] + Dir[1]*t;
 217     IsectPt[2] = A[2] + Dir[2]*t;
 218     return(1);
 219   }
 220   return(0);
 221 }
 222
 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])
 231 {
 232   #define SET(v,x,y,z) v[0]=x; v[1]=y; v[2]=z;
 233
 234   // CALC EXTREME PTS (Neg,Pos) ALONG NORMAL AXIS (Pos in dir of norm, etc.)
 235   float Neg[3], Pos[3];
 236   if(P[0]>0)
 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]); } }
 241   else
 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]); } }
 246
 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);
 250   return(0);
 251 }
 252
 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)
 263 {
 264   *InT=-99999, *OutT=99999;          // INIT INTERVAL T-VAL ENDPTS TO -/+ INFINITY
 265   float NdotDir, NdotStart;          // STORAGE FOR REPEATED CALCS NEEDED FOR NewT CALC
 266   float NewT;
 267   for (int i=0; i<NumPlanes; i++)    // CHECK INTERSECTION AGAINST EACH VF PLANE
 268   {
 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
 272     {
 273       if (NdotStart > -Planes[i][3]) return(0); // IF STARTS "OUTSIDE", NO INTERSECTION
 274     }
 275     else
 276     {
 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"
 280     }
 281     if (*InT > *OutT) return(0);   // CHECK FOR EARLY EXITS (INTERSECTION INTERVAL "OUTSIDE")
 282   }
 283
 284   // IF AT LEAST ONE THE Tvals ARE IN THE INTERVAL [0,1] WE HAVE INTERSECTION
 285   if (*InT>=0 || *OutT>=0) return(1);
 286   return(0);
 287 }
 288
 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)
 296 {
 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);
 301   return(0);
 302 }