Attempt to stabilize ODE. Now using constant step (sys_ticrate set steptime) - on...
[divverent/darkplaces.git] / 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