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 (planenormalz > planenormalx && planenormalz > 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 * planenormalx;
  29         quadup[2] += d * planenormalx;
  30         // VectorNormalize(quadup);
  31         d = 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 (planenormalz > planenormalx && planenormalz > 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 * planenormalx;
  74         quadup[2] += d * planenormalx;
  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         p = inpoints + (innumpoints - 1) * 3;
 107         n = inpoints;
 108         pdist = p[0] * planenormalx + p[1] * planenormaly + p[2] * planenormalz - planedist;
 109         for (i = 0;i < innumpoints;i++)
 110         {
 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                 p = n;
 151                 n += 3;
 152                 pdist = ndist;
 153         }
 154         if (neededfrontpoints)
 155                 *neededfrontpoints = frontcount;
 156         if (neededbackpoints)
 157                 *neededbackpoints = backcount;
 158 }
 159
 160 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)
 161 {
 162         unsigned int i, frontcount, backcount;
 163         const double *n, *p;
 164         double frac, pdist, ndist;
 165         frontcount = 0;
 166         backcount = 0;
 167         p = inpoints + (innumpoints - 1) * 3;
 168         n = inpoints;
 169         pdist = p[0] * planenormalx + p[1] * planenormaly + p[2] * planenormalz - planedist;
 170         for (i = 0;i < innumpoints;i++)
 171         {
 172                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 173                 if (pdist >= -epsilon)
 174                 {
 175                         if (frontcount < outfrontmaxpoints)
 176                         {
 177                                 *outfrontpoints++ = p[0];
 178                                 *outfrontpoints++ = p[1];
 179                                 *outfrontpoints++ = p[2];
 180                         }
 181                         frontcount++;
 182                 }
 183                 if (pdist <= epsilon)
 184                 {
 185                         if (backcount < outbackmaxpoints)
 186                         {
 187                                 *outbackpoints++ = p[0];
 188                                 *outbackpoints++ = p[1];
 189                                 *outbackpoints++ = p[2];
 190                         }
 191                         backcount++;
 192                 }
 193                 if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 194                 {
 195                         frac = pdist / (pdist - ndist);
 196                         if (frontcount < outfrontmaxpoints)
 197                         {
 198                                 *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 199                                 *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 200                                 *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 201                         }
 202                         frontcount++;
 203                         if (backcount < outbackmaxpoints)
 204                         {
 205                                 *outbackpoints++ = p[0] + frac * (n[0] - p[0]);
 206                                 *outbackpoints++ = p[1] + frac * (n[1] - p[1]);
 207                                 *outbackpoints++ = p[2] + frac * (n[2] - p[2]);
 208                         }
 209                         backcount++;
 210                 }
 211                 p = n;
 212                 n += 3;
 213                 pdist = ndist;
 214         }
 215         if (neededfrontpoints)
 216                 *neededfrontpoints = frontcount;
 217         if (neededbackpoints)
 218                 *neededbackpoints = backcount;
 219 }
 220