polygon.c

   1
   2 /*
   3 Polygon clipping routines written by Forest Hale and placed into public domain.
   4 */
   5
   6 #include <math.h>
   7 #include "polygon.h"
   8
   9 void PolygonF_QuadForPlane(float *outpoints, float planenormalx, float planenormaly, float planenormalz, float planedist, float quadsize)
  10 {
  11         float d, quadright[3], quadup[3];
  12         if (fabs(planenormalz) > fabs(planenormalx) && fabs(planenormalz) > fabs(planenormaly))
  13         {
  14                 quadup[0] = 1;
  15                 quadup[1] = 0;
  16                 quadup[2] = 0;
  17         }
  18         else
  19         {
  20                 quadup[0] = 0;
  21                 quadup[1] = 0;
  22                 quadup[2] = 1;
  23         }
  24         // d = -DotProduct(quadup, planenormal);
  25         d = -(quadup[0] * planenormalx + quadup[1] * planenormaly + quadup[2] * planenormalz);
  26         // VectorMA(quadup, d, planenormal, quadup);
  27         quadup[0] += d * planenormalx;
  28         quadup[1] += d * planenormaly;
  29         quadup[2] += d * planenormalz;
  30         // VectorNormalize(quadup);
  31         d = (float)(1.0 / sqrt(quadup[0] * quadup[0] + quadup[1] * quadup[1] + quadup[2] * quadup[2]));
  32         quadup[0] *= d;
  33         quadup[1] *= d;
  34         quadup[2] *= d;
  35         // CrossProduct(quadup,planenormal,quadright);
  36         quadright[0] = quadup[1] * planenormalz - quadup[2] * planenormaly;
  37         quadright[1] = quadup[2] * planenormalx - quadup[0] * planenormalz;
  38         quadright[2] = quadup[0] * planenormaly - quadup[1] * planenormalx;
  39         // make the points
  40         outpoints[0] = planedist * planenormalx - quadsize * quadright[0] + quadsize * quadup[0];
  41         outpoints[1] = planedist * planenormaly - quadsize * quadright[1] + quadsize * quadup[1];
  42         outpoints[2] = planedist * planenormalz - quadsize * quadright[2] + quadsize * quadup[2];
  43         outpoints[3] = planedist * planenormalx + quadsize * quadright[0] + quadsize * quadup[0];
  44         outpoints[4] = planedist * planenormaly + quadsize * quadright[1] + quadsize * quadup[1];
  45         outpoints[5] = planedist * planenormalz + quadsize * quadright[2] + quadsize * quadup[2];
  46         outpoints[6] = planedist * planenormalx + quadsize * quadright[0] - quadsize * quadup[0];
  47         outpoints[7] = planedist * planenormaly + quadsize * quadright[1] - quadsize * quadup[1];
  48         outpoints[8] = planedist * planenormalz + quadsize * quadright[2] - quadsize * quadup[2];
  49         outpoints[9] = planedist * planenormalx - quadsize * quadright[0] - quadsize * quadup[0];
  50         outpoints[10] = planedist * planenormaly - quadsize * quadright[1] - quadsize * quadup[1];
  51         outpoints[11] = planedist * planenormalz - quadsize * quadright[2] - quadsize * quadup[2];
  52 }
  53
  54 void PolygonD_QuadForPlane(double *outpoints, double planenormalx, double planenormaly, double planenormalz, double planedist, double quadsize)
  55 {
  56         double d, quadright[3], quadup[3];
  57         if (fabs(planenormalz) > fabs(planenormalx) && fabs(planenormalz) > fabs(planenormaly))
  58         {
  59                 quadup[0] = 1;
  60                 quadup[1] = 0;
  61                 quadup[2] = 0;
  62         }
  63         else
  64         {
  65                 quadup[0] = 0;
  66                 quadup[1] = 0;
  67                 quadup[2] = 1;
  68         }
  69         // d = -DotProduct(quadup, planenormal);
  70         d = -(quadup[0] * planenormalx + quadup[1] * planenormaly + quadup[2] * planenormalz);
  71         // VectorMA(quadup, d, planenormal, quadup);
  72         quadup[0] += d * planenormalx;
  73         quadup[1] += d * planenormaly;
  74         quadup[2] += d * planenormalz;
  75         // VectorNormalize(quadup);
  76         d = 1.0 / sqrt(quadup[0] * quadup[0] + quadup[1] * quadup[1] + quadup[2] * quadup[2]);
  77         quadup[0] *= d;
  78         quadup[1] *= d;
  79         quadup[2] *= d;
  80         // CrossProduct(quadup,planenormal,quadright);
  81         quadright[0] = quadup[1] * planenormalz - quadup[2] * planenormaly;
  82         quadright[1] = quadup[2] * planenormalx - quadup[0] * planenormalz;
  83         quadright[2] = quadup[0] * planenormaly - quadup[1] * planenormalx;
  84         // make the points
  85         outpoints[0] = planedist * planenormalx - quadsize * quadright[0] + quadsize * quadup[0];
  86         outpoints[1] = planedist * planenormaly - quadsize * quadright[1] + quadsize * quadup[1];
  87         outpoints[2] = planedist * planenormalz - quadsize * quadright[2] + quadsize * quadup[2];
  88         outpoints[3] = planedist * planenormalx + quadsize * quadright[0] + quadsize * quadup[0];
  89         outpoints[4] = planedist * planenormaly + quadsize * quadright[1] + quadsize * quadup[1];
  90         outpoints[5] = planedist * planenormalz + quadsize * quadright[2] + quadsize * quadup[2];
  91         outpoints[6] = planedist * planenormalx + quadsize * quadright[0] - quadsize * quadup[0];
  92         outpoints[7] = planedist * planenormaly + quadsize * quadright[1] - quadsize * quadup[1];
  93         outpoints[8] = planedist * planenormalz + quadsize * quadright[2] - quadsize * quadup[2];
  94         outpoints[9] = planedist * planenormalx - quadsize * quadright[0] - quadsize * quadup[0];
  95         outpoints[10] = planedist * planenormaly - quadsize * quadright[1] - quadsize * quadup[1];
  96         outpoints[11] = planedist * planenormalz - quadsize * quadright[2] - quadsize * quadup[2];
  97 }
  98
  99 void PolygonF_Divide(unsigned int innumpoints, const float *inpoints, float planenormalx, float planenormaly, float planenormalz, float planedist, float epsilon, unsigned int outfrontmaxpoints, float *outfrontpoints, unsigned int *neededfrontpoints, unsigned int outbackmaxpoints, float *outbackpoints, unsigned int *neededbackpoints)
 100 {
 101         unsigned int i, frontcount, backcount;
 102         const float *n, *p;
 103         float frac, pdist, ndist;
 104         frontcount = 0;
 105         backcount = 0;
 106         for (i = 0;i < innumpoints;i++)
 107         {
 108                 p = inpoints + i * 3;
 109                 n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 110                 pdist = p[0] * planenormalx + p[1] * planenormaly + p[2] * planenormalz - planedist;
 111                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 112                 if (pdist >= -epsilon)
 113                 {
 114                         if (frontcount < outfrontmaxpoints)
 115                         {
 116                                 *outfrontpoints++ = p[0];
 117                                 *outfrontpoints++ = p[1];
 118                                 *outfrontpoints++ = p[2];
 119                         }
 120                         frontcount++;
 121                 }
 122                 if (pdist <= epsilon)
 123                 {
 124                         if (backcount < outbackmaxpoints)
 125                         {
 126                                 *outbackpoints++ = p[0];
 127                                 *outbackpoints++ = p[1];
 128                                 *outbackpoints++ = p[2];
 129                         }
 130                         backcount++;
 131                 }
 132                 if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 133                 {
 134                         frac = pdist / (pdist - ndist);
 135                         if (frontcount < outfrontmaxpoints)
 136                         {
 137                                 *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 138                                 *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 139                                 *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 140                         }
 141                         frontcount++;
 142                         if (backcount < outbackmaxpoints)
 143                         {
 144                                 *outbackpoints++ = p[0] + frac * (n[0] - p[0]);
 145                                 *outbackpoints++ = p[1] + frac * (n[1] - p[1]);
 146                                 *outbackpoints++ = p[2] + frac * (n[2] - p[2]);
 147                         }
 148                         backcount++;
 149                 }
 150         }
 151         if (neededfrontpoints)
 152                 *neededfrontpoints = frontcount;
 153         if (neededbackpoints)
 154                 *neededbackpoints = backcount;
 155 }
 156
 157 void PolygonD_Divide(unsigned int innumpoints, const double *inpoints, double planenormalx, double planenormaly, double planenormalz, double planedist, double epsilon, unsigned int outfrontmaxpoints, double *outfrontpoints, unsigned int *neededfrontpoints, unsigned int outbackmaxpoints, double *outbackpoints, unsigned int *neededbackpoints)
 158 {
 159         unsigned int i, frontcount, backcount;
 160         const double *n, *p;
 161         double frac, pdist, ndist;
 162         frontcount = 0;
 163         backcount = 0;
 164         for (i = 0;i < innumpoints;i++)
 165         {
 166                 p = inpoints + i * 3;
 167                 n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 168                 pdist = p[0] * planenormalx + p[1] * planenormaly + p[2] * planenormalz - planedist;
 169                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 170                 if (pdist >= -epsilon)
 171                 {
 172                         if (frontcount < outfrontmaxpoints)
 173                         {
 174                                 *outfrontpoints++ = p[0];
 175                                 *outfrontpoints++ = p[1];
 176                                 *outfrontpoints++ = p[2];
 177                         }
 178                         frontcount++;
 179                 }
 180                 if (pdist <= epsilon)
 181                 {
 182                         if (backcount < outbackmaxpoints)
 183                         {
 184                                 *outbackpoints++ = p[0];
 185                                 *outbackpoints++ = p[1];
 186                                 *outbackpoints++ = p[2];
 187                         }
 188                         backcount++;
 189                 }
 190                 if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 191                 {
 192                         frac = pdist / (pdist - ndist);
 193                         if (frontcount < outfrontmaxpoints)
 194                         {
 195                                 *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 196                                 *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 197                                 *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 198                         }
 199                         frontcount++;
 200                         if (backcount < outbackmaxpoints)
 201                         {
 202                                 *outbackpoints++ = p[0] + frac * (n[0] - p[0]);
 203                                 *outbackpoints++ = p[1] + frac * (n[1] - p[1]);
 204                                 *outbackpoints++ = p[2] + frac * (n[2] - p[2]);
 205                         }
 206                         backcount++;
 207                 }
 208         }
 209         if (neededfrontpoints)
 210                 *neededfrontpoints = frontcount;
 211         if (neededbackpoints)
 212                 *neededbackpoints = backcount;
 213 }
 214