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(int innumpoints, const float *inpoints, float planenormalx, float planenormaly, float planenormalz, float planedist, float epsilon, int outfrontmaxpoints, float *outfrontpoints, int *neededfrontpoints, int outbackmaxpoints, float *outbackpoints, int *neededbackpoints, int *oncountpointer)
 100 {
 101         int i, frontcount = 0, backcount = 0, oncount = 0;
 102         const float *n, *p;
 103         double frac, pdist, ndist;
 104         for (i = 0;i < innumpoints;i++)
 105         {
 106                 p = inpoints + i * 3;
 107                 n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 108                 pdist = p[0] * planenormalx + p[1] * planenormaly + p[2] * planenormalz - planedist;
 109                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 110                 if (pdist >= -epsilon)
 111                 {
 112                         if (pdist <= epsilon)
 113                                 oncount++;
 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                         oncount++;
 135                         frac = pdist / (pdist - ndist);
 136                         if (frontcount < outfrontmaxpoints)
 137                         {
 138                                 *outfrontpoints++ = (float)(p[0] + frac * (n[0] - p[0]));
 139                                 *outfrontpoints++ = (float)(p[1] + frac * (n[1] - p[1]));
 140                                 *outfrontpoints++ = (float)(p[2] + frac * (n[2] - p[2]));
 141                         }
 142                         frontcount++;
 143                         if (backcount < outbackmaxpoints)
 144                         {
 145                                 *outbackpoints++ = (float)(p[0] + frac * (n[0] - p[0]));
 146                                 *outbackpoints++ = (float)(p[1] + frac * (n[1] - p[1]));
 147                                 *outbackpoints++ = (float)(p[2] + frac * (n[2] - p[2]));
 148                         }
 149                         backcount++;
 150                 }
 151         }
 152         if (neededfrontpoints)
 153                 *neededfrontpoints = frontcount;
 154         if (neededbackpoints)
 155                 *neededbackpoints = backcount;
 156         if (oncountpointer)
 157                 *oncountpointer = oncount;
 158 }
 159
 160 void PolygonD_Divide(int innumpoints, const double *inpoints, double planenormalx, double planenormaly, double planenormalz, double planedist, double epsilon, int outfrontmaxpoints, double *outfrontpoints, int *neededfrontpoints, int outbackmaxpoints, double *outbackpoints, int *neededbackpoints, int *oncountpointer)
 161 {
 162         int i, frontcount = 0, backcount = 0, oncount = 0;
 163         const double *n, *p;
 164         double frac, pdist, ndist;
 165         for (i = 0;i < innumpoints;i++)
 166         {
 167                 p = inpoints + i * 3;
 168                 n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 169                 pdist = p[0] * planenormalx + p[1] * planenormaly + p[2] * planenormalz - planedist;
 170                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 171                 if (pdist >= -epsilon)
 172                 {
 173                         if (pdist <= epsilon)
 174                                 oncount++;
 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                         oncount++;
 196                         frac = pdist / (pdist - ndist);
 197                         if (frontcount < outfrontmaxpoints)
 198                         {
 199                                 *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 200                                 *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 201                                 *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 202                         }
 203                         frontcount++;
 204                         if (backcount < outbackmaxpoints)
 205                         {
 206                                 *outbackpoints++ = p[0] + frac * (n[0] - p[0]);
 207                                 *outbackpoints++ = p[1] + frac * (n[1] - p[1]);
 208                                 *outbackpoints++ = p[2] + frac * (n[2] - p[2]);
 209                         }
 210                         backcount++;
 211                 }
 212         }
 213         if (neededfrontpoints)
 214                 *neededfrontpoints = frontcount;
 215         if (neededbackpoints)
 216                 *neededbackpoints = backcount;
 217         if (oncountpointer)
 218                 *oncountpointer = oncount;
 219 }
 220