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 int PolygonF_Clip(int innumpoints, const float *inpoints, float planenormalx, float planenormaly, float planenormalz, float planedist, float epsilon, int outfrontmaxpoints, float *outfrontpoints)
 100 {
 101         int i, frontcount = 0;
 102         const float *n, *p;
 103         float frac, pdist, ndist;
 104         if (innumpoints < 1)
 105                 return 0;
 106         n = inpoints;
 107         ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 108         for(i = 0;i < innumpoints;i++)
 109         {
 110                 p = n;
 111                 pdist = ndist;
 112                 n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 113                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 114                 if (pdist >= -epsilon)
 115                 {
 116                         if (frontcount < outfrontmaxpoints)
 117                         {
 118                                 *outfrontpoints++ = p[0];
 119                                 *outfrontpoints++ = p[1];
 120                                 *outfrontpoints++ = p[2];
 121                         }
 122                         frontcount++;
 123                 }
 124                 if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 125                 {
 126                         frac = pdist / (pdist - ndist);
 127                         if (frontcount < outfrontmaxpoints)
 128                         {
 129                                 *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 130                                 *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 131                                 *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 132                         }
 133                         frontcount++;
 134                 }
 135         }
 136         return frontcount;
 137 }
 138
 139 int PolygonD_Clip(int innumpoints, const double *inpoints, double planenormalx, double planenormaly, double planenormalz, double planedist, double epsilon, int outfrontmaxpoints, double *outfrontpoints)
 140 {
 141         int i, frontcount = 0;
 142         const double *n, *p;
 143         double frac, pdist, ndist;
 144         if (innumpoints < 1)
 145                 return 0;
 146         n = inpoints;
 147         ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 148         for(i = 0;i < innumpoints;i++)
 149         {
 150                 p = n;
 151                 pdist = ndist;
 152                 n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 153                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 154                 if (pdist >= -epsilon)
 155                 {
 156                         if (frontcount < outfrontmaxpoints)
 157                         {
 158                                 *outfrontpoints++ = p[0];
 159                                 *outfrontpoints++ = p[1];
 160                                 *outfrontpoints++ = p[2];
 161                         }
 162                         frontcount++;
 163                 }
 164                 if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 165                 {
 166                         frac = pdist / (pdist - ndist);
 167                         if (frontcount < outfrontmaxpoints)
 168                         {
 169                                 *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 170                                 *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 171                                 *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 172                         }
 173                         frontcount++;
 174                 }
 175         }
 176         return frontcount;
 177 }
 178
 179 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)
 180 {
 181         int i, frontcount = 0, backcount = 0, oncount = 0;
 182         const float *n, *p;
 183         float frac, pdist, ndist;
 184         if (innumpoints)
 185         {
 186                 n = inpoints;
 187                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 188                 for(i = 0;i < innumpoints;i++)
 189                 {
 190                         p = n;
 191                         pdist = ndist;
 192                         n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 193                         ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 194                         if (pdist >= -epsilon)
 195                         {
 196                                 if (pdist <= epsilon)
 197                                         oncount++;
 198                                 if (frontcount < outfrontmaxpoints)
 199                                 {
 200                                         *outfrontpoints++ = p[0];
 201                                         *outfrontpoints++ = p[1];
 202                                         *outfrontpoints++ = p[2];
 203                                 }
 204                                 frontcount++;
 205                         }
 206                         if (pdist <= epsilon)
 207                         {
 208                                 if (backcount < outbackmaxpoints)
 209                                 {
 210                                         *outbackpoints++ = p[0];
 211                                         *outbackpoints++ = p[1];
 212                                         *outbackpoints++ = p[2];
 213                                 }
 214                                 backcount++;
 215                         }
 216                         if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 217                         {
 218                                 oncount++;
 219                                 frac = pdist / (pdist - ndist);
 220                                 if (frontcount < outfrontmaxpoints)
 221                                 {
 222                                         *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 223                                         *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 224                                         *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 225                                 }
 226                                 frontcount++;
 227                                 if (backcount < outbackmaxpoints)
 228                                 {
 229                                         *outbackpoints++ = p[0] + frac * (n[0] - p[0]);
 230                                         *outbackpoints++ = p[1] + frac * (n[1] - p[1]);
 231                                         *outbackpoints++ = p[2] + frac * (n[2] - p[2]);
 232                                 }
 233                                 backcount++;
 234                         }
 235                 }
 236         }
 237         if (neededfrontpoints)
 238                 *neededfrontpoints = frontcount;
 239         if (neededbackpoints)
 240                 *neededbackpoints = backcount;
 241         if (oncountpointer)
 242                 *oncountpointer = oncount;
 243 }
 244
 245 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)
 246 {
 247         int i = 0, frontcount = 0, backcount = 0, oncount = 0;
 248         const double *n, *p;
 249         double frac, pdist, ndist;
 250         if (innumpoints)
 251         {
 252                 n = inpoints;
 253                 ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 254                 for(i = 0;i < innumpoints;i++)
 255                 {
 256                         p = n;
 257                         pdist = ndist;
 258                         n = inpoints + ((i + 1) < innumpoints ? (i + 1) : 0) * 3;
 259                         ndist = n[0] * planenormalx + n[1] * planenormaly + n[2] * planenormalz - planedist;
 260                         if (pdist >= -epsilon)
 261                         {
 262                                 if (pdist <= epsilon)
 263                                         oncount++;
 264                                 if (frontcount < outfrontmaxpoints)
 265                                 {
 266                                         *outfrontpoints++ = p[0];
 267                                         *outfrontpoints++ = p[1];
 268                                         *outfrontpoints++ = p[2];
 269                                 }
 270                                 frontcount++;
 271                         }
 272                         if (pdist <= epsilon)
 273                         {
 274                                 if (backcount < outbackmaxpoints)
 275                                 {
 276                                         *outbackpoints++ = p[0];
 277                                         *outbackpoints++ = p[1];
 278                                         *outbackpoints++ = p[2];
 279                                 }
 280                                 backcount++;
 281                         }
 282                         if ((pdist > epsilon && ndist < -epsilon) || (pdist < -epsilon && ndist > epsilon))
 283                         {
 284                                 oncount++;
 285                                 frac = pdist / (pdist - ndist);
 286                                 if (frontcount < outfrontmaxpoints)
 287                                 {
 288                                         *outfrontpoints++ = p[0] + frac * (n[0] - p[0]);
 289                                         *outfrontpoints++ = p[1] + frac * (n[1] - p[1]);
 290                                         *outfrontpoints++ = p[2] + frac * (n[2] - p[2]);
 291                                 }
 292                                 frontcount++;
 293                                 if (backcount < outbackmaxpoints)
 294                                 {
 295                                         *outbackpoints++ = p[0] + frac * (n[0] - p[0]);
 296                                         *outbackpoints++ = p[1] + frac * (n[1] - p[1]);
 297                                         *outbackpoints++ = p[2] + frac * (n[2] - p[2]);
 298                                 }
 299                                 backcount++;
 300                         }
 301                 }
 302         }
 303         if (neededfrontpoints)
 304                 *neededfrontpoints = frontcount;
 305         if (neededbackpoints)
 306                 *neededbackpoints = backcount;
 307         if (oncountpointer)
 308                 *oncountpointer = oncount;
 309 }
 310