source: GTP/trunk/App/Demos/Vis/CHC_revisited/MathStuff.c @ 2642

Revision 2642, 25.5 KB checked in by mattausch, 17 years ago (diff)

new demo

Line 
1//Copyright and Disclaimer:
2//This code is copyright Vienna University of Technology, 2004.
3
4#include <memory.h>
5#include <math.h>
6#include <float.h>
7#include <stdlib.h>
8#include <malloc.h>
9#include <stdio.h>
10#include "MathStuff.h"
11
12#ifndef M_PI
13#define M_PI (double)3.14159265358979323846
14#endif
15
16const double DOUBLE_MAX = DBL_MAX;
17const double PI = M_PI;
18const double PI_2 = M_PI/2;
19const double PI_180 = M_PI/180;
20const Vector3 ZERO = {0.0, 0.0, 0.0};
21const Vector3 UNIT_X = {1.0, 0.0, 0.0};
22const Vector3 UNIT_Y = {0.0, 1.0, 0.0};
23const Vector3 UNIT_Z = {0.0, 0.0, 1.0};
24
25const Matrix4x4 IDENTITY = {1.0, 0.0, 0.0, 0.0,
26                                                        0.0, 1.0, 0.0, 0.0,
27                                                        0.0, 0.0, 1.0, 0.0,
28                                                        0.0, 0.0, 0.0, 1.0};
29
30double maximum(const double a, const double b) {
31        return ( (a > b)? (a):(b) );
32
33}
34
35void clamp(double* value, const double min, const double max) {
36        if( (*value) > max) {
37                (*value) = max;
38        }
39        else {
40                if( (*value) < min) {
41                        (*value) = min;
42                }
43        }
44}
45
46double absDouble(const double a) {
47        return ( (a < 0.0)? (-a):(a) );
48}
49
50double signDouble(const double a) {
51        return ( (a < 0.0)? (-1.0f): ( (a == 0.0)? (0.0f):(1.0f) ) );
52}
53
54double coTan(const double vIn) {
55        return (double)-tan(vIn+PI_2);
56}
57
58double relativeEpsilon(const double a, const double epsilon) {
59        double relEpsilon = maximum(absDouble(a*epsilon),epsilon);
60        return relEpsilon;
61}
62
63int alike(const double a, const double b, const double epsilon) {
64        if(a == b) {
65                return 1;
66        }
67        {
68                double relEps = relativeEpsilon(a,epsilon);
69                return (a-relEps <= b) && (b <= a+relEps);
70        }
71}
72
73
74
75int alikeVector3(const Vector3 a, const Vector3 b, const double epsilon) {
76        return
77                alike(a[0],b[0],epsilon) &&
78                alike(a[1],b[1],epsilon) &&
79                alike(a[2],b[2],epsilon);
80}
81
82void linCombVector3(Vector3 result, const Vector3 pos, const Vector3 dir, const double t) {
83        int i;
84        for(i = 0; i < 3; i++) {
85                result[i] = pos[i]+t*dir[i];
86        }
87}
88
89double squaredLength(const Vector3 vec) {
90        int i;
91        double tmp = 0.0;
92        for(i = 0; i < 3; i++) {
93                tmp += vec[i]*vec[i];
94        }
95        return tmp;
96}
97
98void normalize(Vector3 vec) {
99        int i;
100        const double len = (double)(1.0/sqrt(squaredLength(vec)));
101        for(i = 0; i < 3; i++) {
102                vec[i] *= len;
103        }
104}
105
106void copyNormalize(Vector3 vec, const Vector3 input) {
107        copyVector3(vec,input),
108        normalize(vec);
109}
110
111/*      | a1 a2 |
112        | b1 b2 | calculate the determinent of a 2x2 matrix*/
113double det2x2(const double a1, const double a2,
114                        const double b1, const double b2) {
115        return a1*b2 - b1*a2;
116}
117
118/*      | a1 a2 a3 |
119        | b1 b2 b3 |
120        | c1 c2 c3 | calculate the determinent of a 3x3 matrix*/
121double det3x3(const double a1, const double a2, const double a3,
122                          const double b1, const double b2, const double b3,
123                          const double c1, const double c2, const double c3) {
124        return a1*det2x2(b2,b3,c2,c3) - b1*det2x2(a2,a3,c2,c3) +
125                        c1*det2x2(a2,a3,b2,b3);
126}
127void cross(Vector3 result, const Vector3 a, const Vector3 b) {
128        result[0] =  det2x2(a[1],b[1],a[2],b[2]);
129        result[1] = -det2x2(a[0],b[0],a[2],b[2]);
130        result[2] =  det2x2(a[0],b[0],a[1],b[1]);
131}
132
133double dot(const Vector3 a, const Vector3 b) {
134        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
135}
136
137void look(Matrix4x4 output, const Vector3 pos, const Vector3 dir, const Vector3 up) {
138        Vector3 dirN;
139        Vector3 upN;
140        Vector3 lftN;
141
142        cross(lftN,dir,up);
143        normalize(lftN);
144
145        cross(upN,lftN,dir);
146        normalize(upN);
147        copyNormalize(dirN,dir);
148
149        output[ 0] = lftN[0];
150        output[ 1] = upN[0];
151        output[ 2] = -dirN[0];
152        output[ 3] = 0.0;
153
154        output[ 4] = lftN[1];
155        output[ 5] = upN[1];
156        output[ 6] = -dirN[1];
157        output[ 7] = 0.0;
158
159        output[ 8] = lftN[2];
160        output[ 9] = upN[2];
161        output[10] = -dirN[2];
162        output[11] = 0.0;
163
164        output[12] = -dot(lftN,pos);
165        output[13] = -dot(upN,pos);
166        output[14] = dot(dirN,pos);
167        output[15] = 1.0;
168}
169
170void makeScaleMtx(Matrix4x4 output, const double x, const double y, const double z) {
171        output[ 0] = x;
172        output[ 1] = 0.0;
173        output[ 2] = 0.0;
174        output[ 3] = 0.0;
175
176        output[ 4] = 0.0;
177        output[ 5] = y;
178        output[ 6] = 0.0;
179        output[ 7] = 0.0;
180
181        output[ 8] = 0.0;
182        output[ 9] = 0.0;
183        output[10] = z;
184        output[11] = 0.0;
185
186        output[12] = 0.0;
187        output[13] = 0.0;
188        output[14] = 0.0;
189        output[15] = 1.0;
190}
191
192void makeTranslationMtx(Matrix4x4 output, Vector3 translation) {
193        output[ 0] = 1.0;
194        output[ 1] = 0.0;
195        output[ 2] = 0.0;
196        output[ 3] = 0;//translation[0];
197
198        output[ 4] = 0.0;
199        output[ 5] = 1.0;
200        output[ 6] = 0.0;
201        output[ 7] = 0;//translation[1];
202
203        output[ 8] = 0.0;
204        output[ 9] = 0.0;
205        output[10] = 1.0;
206        output[11] = 0;//translation[2];
207
208        output[12] = translation[0];//0.0;
209        output[13] = translation[1];//0.0;
210        output[14] = translation[2];//0.0;
211        output[15] = 1.0;
212}
213
214void makeRotationZMtx(Matrix4x4 output, const double rad)
215{
216        output[ 0] = cos(rad);
217        output[ 1] = sin(rad);
218        output[ 2] = 0.0;
219        output[ 3] = 0.0;
220
221        output[ 4] = -sin(rad);
222        output[ 5] = cos(rad);
223        output[ 6] = 0.0;
224        output[ 7] = 0.0;
225
226        output[ 8] = 0.0;
227        output[ 9] = 0.0;
228        output[10] = 1.0;
229        output[11] = 0.0;
230
231        output[12] = 0.0;
232        output[13] = 0.0;
233        output[14] = 0.0;
234        output[15] = 1.0;
235}
236
237void makeRotationXMtx(Matrix4x4 output, const double rad)
238{
239        output[ 0] = 1.0;
240        output[ 1] = 0.0;
241        output[ 2] = 0.0;
242        output[ 3] = 0.0;
243
244        output[ 4] = 0.0;
245        output[ 5] = cos(rad);
246        output[ 6] = sin(rad);
247        output[ 7] = 0.0;
248
249        output[ 8] = 0.0;
250        output[ 9] = -sin(rad);
251        output[10] = cos(rad);
252        output[11] = 0.0;
253
254        output[12] = 0.0;
255        output[13] = 0.0;
256        output[14] = 0.0;
257        output[15] = 1.0;
258}
259
260void makeRotationYMtx(Matrix4x4 output, const double rad)
261{
262        output[ 0] = cos(rad);
263        output[ 1] = 0.0;
264        output[ 2] = -sin(rad);
265        output[ 3] = 0.0;
266
267        output[ 4] = 0.0;
268        output[ 5] = 1.0;
269        output[ 6] = 0.0;
270        output[ 7] = 0.0;
271
272        output[ 8] = sin(rad);
273        output[ 9] = 0.0;
274        output[10] = cos(rad);
275        output[11] = 0.0;
276
277        output[12] = 0.0;
278        output[13] = 0.0;
279        output[14] = 0.0;
280        output[15] = 1.0;
281}
282
283void scaleTranslateToFit(Matrix4x4 output, const Vector3 vMin, const Vector3 vMax) {
284        output[ 0] = 2/(vMax[0]-vMin[0]);
285        output[ 4] = 0;
286        output[ 8] = 0;
287        output[12] = -(vMax[0]+vMin[0])/(vMax[0]-vMin[0]);
288
289        output[ 1] = 0;
290        output[ 5] = 2/(vMax[1]-vMin[1]);
291        output[ 9] = 0;
292        output[13] = -(vMax[1]+vMin[1])/(vMax[1]-vMin[1]);
293
294        output[ 2] = 0;
295        output[ 6] = 0;
296        output[10] = 2/(vMax[2]-vMin[2]);
297        output[14] = -(vMax[2]+vMin[2])/(vMax[2]-vMin[2]);
298
299        output[ 3] = 0;
300        output[ 7] = 0;
301        output[11] = 0;
302        output[15] = 1;
303}
304
305void perspectiveRad(Matrix4x4 output, const double vFovy, const double vAspect,
306                                                  const double vNearDis, const double vFarDis) {
307        const double f = coTan(vFovy/2);
308        const double dif = 1/(vNearDis-vFarDis);
309
310        output[ 0] = f/vAspect;
311        output[ 4] = 0;
312        output[ 8] = 0;
313        output[12] = 0;
314
315        output[ 1] = 0;
316        output[ 5] = f;
317        output[ 9] = 0;
318        output[13] = 0;
319
320        output[ 2] = 0;
321        output[ 6] = 0;
322        output[10] = (vFarDis+vNearDis)*dif;
323        output[14] = 2*vFarDis*vNearDis*dif;
324
325        output[ 3] = 0;
326        output[ 7] = 0;
327        output[11] = -1;
328        output[15] = 0;
329}
330
331void perspectiveDeg(Matrix4x4 output, const double vFovy, const double vAspect,
332                                        const double vNearDis, const double vFarDis) {
333        perspectiveRad(output,vFovy*PI_180,vAspect,vNearDis,vFarDis);
334}
335
336void invert(Matrix4x4 output, const Matrix4x4 i) {
337        double a11 =  det3x3(i[5],i[6],i[7],i[9],i[10],i[11],i[13],i[14],i[15]);
338        double a21 = -det3x3(i[1],i[2],i[3],i[9],i[10],i[11],i[13],i[14],i[15]);
339        double a31 =  det3x3(i[1],i[2],i[3],i[5],i[6],i[7],i[13],i[14],i[15]);
340        double a41 = -det3x3(i[1],i[2],i[3],i[5],i[6],i[7],i[9],i[10],i[11]);
341
342        double a12 = -det3x3(i[4],i[6],i[7],i[8],i[10],i[11],i[12],i[14],i[15]);
343        double a22 =  det3x3(i[0],i[2],i[3],i[8],i[10],i[11],i[12],i[14],i[15]);
344        double a32 = -det3x3(i[0],i[2],i[3],i[4],i[6],i[7],i[12],i[14],i[15]);
345        double a42 =  det3x3(i[0],i[2],i[3],i[4],i[6],i[7],i[8],i[10],i[11]);
346
347        double a13 =  det3x3(i[4],i[5],i[7],i[8],i[9],i[11],i[12],i[13],i[15]);
348        double a23 = -det3x3(i[0],i[1],i[3],i[8],i[9],i[11],i[12],i[13],i[15]);
349        double a33 =  det3x3(i[0],i[1],i[3],i[4],i[5],i[7],i[12],i[13],i[15]);
350        double a43 = -det3x3(i[0],i[1],i[3],i[4],i[5],i[7],i[8],i[9],i[11]);
351
352        double a14 = -det3x3(i[4],i[5],i[6],i[8],i[9],i[10],i[12],i[13],i[14]);
353        double a24 =  det3x3(i[0],i[1],i[2],i[8],i[9],i[10],i[12],i[13],i[14]);
354        double a34 = -det3x3(i[0],i[1],i[2],i[4],i[5],i[6],i[12],i[13],i[14]);
355        double a44 =  det3x3(i[0],i[1],i[2],i[4],i[5],i[6],i[8],i[9],i[10]);
356
357        double det = (i[0]*a11) + (i[4]*a21) + (i[8]*a31) + (i[12]*a41);
358        double oodet = 1/det;
359
360        output[ 0] = a11*oodet;
361        output[ 1] = a21*oodet;
362        output[ 2] = a31*oodet;
363        output[ 3] = a41*oodet;
364
365        output[ 4] = a12*oodet;
366        output[ 5] = a22*oodet;
367        output[ 6] = a32*oodet;
368        output[ 7] = a42*oodet;
369
370        output[ 8] = a13*oodet;
371        output[ 9] = a23*oodet;
372        output[10] = a33*oodet;
373        output[11] = a43*oodet;
374
375        output[12] = a14*oodet;
376        output[13] = a24*oodet;
377        output[14] = a34*oodet;
378        output[15] = a44*oodet;
379}
380
381void multUnSave(Matrix4x4 output, const Matrix4x4 a, const Matrix4x4 b) {
382        const int SIZE = 4;
383        int iCol;
384        for(iCol = 0; iCol < SIZE; iCol++) {
385                const int cID = iCol*SIZE;
386                int iRow;
387                for(iRow = 0; iRow < SIZE; iRow++) {
388                        const int id = iRow+cID;
389                        int k;
390                        output[id] = a[iRow]*b[cID];
391                        for(k = 1; k < SIZE; k++) {
392                                output[id] += a[iRow+k*SIZE]*b[k+cID];
393                        }
394                }
395        }
396}
397
398void mult(Matrix4x4 output, const Matrix4x4 a, const Matrix4x4 b) {
399        if(a == output) {
400                Matrix4x4 tmpA;
401                copyMatrix(tmpA,a);
402                if(b == output) {
403                        multUnSave(output,tmpA,tmpA);
404                }
405                else {
406                        multUnSave(output,tmpA,b);
407                }
408        }
409        else {
410                if(b == output) {
411                        Matrix4x4 tmpB;
412                        copyMatrix(tmpB,b);
413                        multUnSave(output,a,tmpB);
414                }
415                else {
416                        multUnSave(output,a,b);
417                }
418        }
419}
420
421void mulHomogenPoint(Vector3 output, const Matrix4x4 m, const Vector3 v) {
422        //if v == output -> overwriting problems -> so store in temp
423        double x = m[0]*v[0] + m[4]*v[1] + m[ 8]*v[2] + m[12];
424        double y = m[1]*v[0] + m[5]*v[1] + m[ 9]*v[2] + m[13];
425        double z = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14];
426        double w = m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15];
427
428        output[0] = x/w;
429        output[1] = y/w;
430        output[2] = z/w;
431}
432
433void appendToCubicHull(Vector3 min, Vector3 max, const Vector3 v) {
434        int j;
435        for(j = 0; j < 3; j++) {
436                if(v[j] < min[j]) {
437                        min[j] = v[j];
438                }
439                else if(v[j] > max[j]) {
440                        max[j] = v[j];
441                }
442        }
443}
444
445void calcCubicHull(Vector3 min, Vector3 max, const Vector3* ps, const int size) {
446        if(size > 0) {
447                int i;
448                copyVector3(min,ps[0]);
449                copyVector3(max,ps[0]);
450                for(i = 1; i < size; i++) {
451                        appendToCubicHull(min,max,ps[i]);
452                }
453        }
454}
455
456void calcViewFrustumWorldCoord(Vector3x8 points, const Matrix4x4 invEyeProjView) {
457        const struct AABox box = {
458                {-1, -1, -1},
459                {1, 1, 1}
460        };
461        int i;
462        calcAABoxPoints(points,box); //calc unit cube corner points
463        for(i = 0; i < 8; i++) {
464                mulHomogenPoint(points[i],invEyeProjView,points[i]); //camera to world frame
465        }
466        //viewFrustumWorldCoord[0] == near-bottom-left
467        //viewFrustumWorldCoord[1] == near-bottom-right
468        //viewFrustumWorldCoord[2] == near-top-right
469        //viewFrustumWorldCoord[3] == near-top-left
470        //viewFrustumWorldCoord[4] == far-bottom-left
471        //viewFrustumWorldCoord[5] == far-bottom-right
472        //viewFrustumWorldCoord[6] == far-top-right
473        //viewFrustumWorldCoord[7] == far-top-left
474}
475
476void calcViewFrustObject(struct Object* obj, const Vector3x8 p) {
477        int i;
478        Vector3* ps;
479        objectSetSize(obj,6);
480        for(i = 0; i < 6; i++) {
481                vecPointSetSize(&obj->poly[i],4);
482        }
483        //near poly ccw
484        ps = obj->poly[0].points;
485        for(i = 0; i < 4; i++) {
486                copyVector3(ps[i],p[i]);
487        }
488        //far poly ccw
489        ps = obj->poly[1].points;
490        for(i = 4; i < 8; i++) {
491                copyVector3(ps[i-4],p[11-i]);
492        }
493        //left poly ccw
494        ps = obj->poly[2].points;
495        copyVector3(ps[0],p[0]);
496        copyVector3(ps[1],p[3]);
497        copyVector3(ps[2],p[7]);
498        copyVector3(ps[3],p[4]);
499        //right poly ccw
500        ps = obj->poly[3].points;
501        copyVector3(ps[0],p[1]);
502        copyVector3(ps[1],p[5]);
503        copyVector3(ps[2],p[6]);
504        copyVector3(ps[3],p[2]);
505        //bottom poly ccw
506        ps = obj->poly[4].points;
507        copyVector3(ps[0],p[4]);
508        copyVector3(ps[1],p[5]);
509        copyVector3(ps[2],p[1]);
510        copyVector3(ps[3],p[0]);
511        //top poly ccw
512        ps = obj->poly[5].points;
513        copyVector3(ps[0],p[6]);
514        copyVector3(ps[1],p[7]);
515        copyVector3(ps[2],p[3]);
516        copyVector3(ps[3],p[2]);
517}
518
519void transformVecPoint(struct VecPoint* poly, const Matrix4x4 xForm) {
520        if(0 != poly) {
521                int i;
522                for(i = 0; i < poly->size; i++) {
523                        mulHomogenPoint(poly->points[i],xForm,poly->points[i]);
524                }
525        }
526}
527
528void transformObject(struct Object* obj, const Matrix4x4 xForm) {
529        if(0 != obj) {
530                int i;
531                for(i = 0; i < obj->size; i++) {
532                        transformVecPoint( &(obj->poly[i]) ,xForm);
533                }
534        }
535}
536
537void calcObjectCubicHull(Vector3 min, Vector3 max, const struct Object obj) {
538        if(0 < obj.size) {
539                int i;
540                calcCubicHull(min,max,obj.poly[0].points,obj.poly[0].size);
541                for(i = 1; i < obj.size; i++) {
542                        struct VecPoint* p = &(obj.poly[i]);
543                        int j;
544                        for(j = 0; j < p->size; j++) {
545                                appendToCubicHull(min,max,p->points[j]);
546                        }
547                }
548        }
549}
550
551int findSamePointInVecPoint(const struct VecPoint poly, const Vector3 p) {
552        int i;
553        for(i = 0; i < poly.size; i++) {
554                if(alikeVector3(poly.points[i],p,(double)0.001)) {
555                        return i;
556                }
557        }
558        return -1;
559}
560
561int findSamePointInObjectAndSwapWithLast(struct Object *inter, const Vector3 p) {
562        int i;
563        if(0 == inter) {
564                return -1;
565        }
566        if(1 > inter->size) {
567                return -1;
568        }
569        for(i = inter->size; i > 0; i--) {
570                struct VecPoint* poly = &(inter->poly[i-1]);
571                if(2 == poly->size) {
572                        const int nr = findSamePointInVecPoint(*poly,p);
573                        if(0 <= nr) {
574                                //swap with last
575                                swapVecPoint(poly, &(inter->poly[inter->size-1]) );
576                                return nr;
577                        }
578                }
579        }
580        return -1;
581}
582
583void appendIntersectionVecPoint(struct Object* obj, struct Object* inter) {
584        struct VecPoint *polyOut;
585        struct VecPoint *polyIn;
586        int size = obj->size;
587        int i;
588        //you need at least 3 sides for a polygon
589        if(3 > inter->size) {
590                return;
591        }
592        //compact inter: remove poly.size != 2 from end on forward
593        for(i = inter->size; 0 < i; i--) {
594                if(2 == inter->poly[i-1].size) {
595                        break;
596                }
597        }
598        objectSetSize(inter,i);
599        //you need at least 3 sides for a polygon
600        if(3 > inter->size) {
601                return;
602        }
603        //make place for one additional polygon in obj
604        objectSetSize(obj,size+1);
605        polyOut = &(obj->poly[size]);
606        //we have line segments in each poly of inter
607        //take last linesegment as first and second point
608        polyIn = &(inter->poly[inter->size-1]);
609        append2VecPoint(polyOut,polyIn->points[0]);
610        append2VecPoint(polyOut,polyIn->points[1]);
611        //remove last poly from inter, because it is already saved
612        objectSetSize(inter,inter->size-1);
613
614        //iterate over inter until their is no line segment left
615        while(0 < inter->size) {
616                //pointer on last point to compare
617                Vector3 *lastPt = &(polyOut->points[polyOut->size-1]);
618                //find same point in inter to continue polygon
619                const int nr = findSamePointInObjectAndSwapWithLast(inter,*lastPt);
620                if(0 <= nr) {
621                        //last line segment
622                        polyIn = &(inter->poly[inter->size-1]);
623                        //get the other point in this polygon and save into polyOut
624                        append2VecPoint(polyOut,polyIn->points[(nr+1)%2]);
625                }
626                //remove last poly from inter, because it is already saved or degenerated
627                objectSetSize(inter,inter->size-1);
628        }
629        //last point can be deleted, because he is the same as the first (closes polygon)
630        vecPointSetSize(polyOut,polyOut->size-1);
631}
632
633double pointPlaneDistance(const struct Plane A, const Vector3 p) {
634        return dot(A.n,p)+(A.d);
635}
636
637int pointBeforePlane(const struct Plane A, const Vector3 p) {
638        return pointPlaneDistance(A,p) > 0.0;
639}
640
641int intersectPlaneEdge(Vector3 output, const struct Plane A, const Vector3 a, const Vector3 b) {
642        Vector3 diff;
643        double t;
644        diffVector3(diff,b,a);
645        t = dot(A.n,diff);
646        if(0.0 == t)
647                return 0;
648        t = (A.d-dot(A.n,a))/t;
649        if(t < 0.0 || 1.0 < t)
650                return 0;
651        linCombVector3(output,a,diff,t);
652        return 1;
653}
654
655void clipVecPointByPlane(const struct VecPoint poly, const struct Plane A, struct VecPoint* polyOut, struct VecPoint* interPts) {
656        int i;
657        int *outside = 0;
658        if(poly.size < 3 || 0 == polyOut) {
659                return;
660        }
661        outside = (int*)realloc(outside,sizeof(int)*poly.size);
662        //for each point
663        for(i = 0; i < poly.size; i++) {
664                outside[i] = pointBeforePlane(A,poly.points[i]);
665        }
666        for(i = 0; i < poly.size; i++) {
667                int idNext = (i+1) % poly.size;
668                //both outside -> save none
669                if(outside[i] && outside[idNext]) {
670                        continue;
671                }
672                //outside to inside -> calc intersection save intersection and save i+1
673                if(outside[i]) {
674                        Vector3 inter;
675                        if(intersectPlaneEdge(inter,A,poly.points[i],poly.points[idNext])) {
676                                append2VecPoint(polyOut,inter);
677                                if(0 != interPts) {
678                                        append2VecPoint(interPts,inter);
679                                }
680                        }
681                        append2VecPoint(polyOut,poly.points[idNext]);
682                        continue;
683                }
684                //inside to outside -> calc intersection save intersection
685                if(outside[idNext]) {
686                        Vector3 inter;
687                        if(intersectPlaneEdge(inter,A,poly.points[i],poly.points[idNext])) {
688                                append2VecPoint(polyOut,inter);
689                                if(0 != interPts) {
690                                        append2VecPoint(interPts,inter);
691                                }
692                        }
693                        continue;
694                }
695                //both inside -> save point i+1
696                append2VecPoint(polyOut,poly.points[idNext]);
697        }
698        outside = (int*)realloc(outside,0);
699}
700
701void clipObjectByPlane(const struct Object obj, const struct Plane A, struct Object* objOut) {
702        int i;
703        struct Object inter = OBJECT_NULL;
704        struct Object objIn = OBJECT_NULL;
705        if(0 == obj.size || 0 == objOut)
706                return;
707        if(obj.poly == objOut->poly) {
708                //need to copy object if input and output are the same
709                copyObject(&objIn,obj);
710        }
711        else {
712                objIn = obj;
713        }
714        emptyObject(objOut);
715
716        for(i = 0; i < objIn.size; i++)
717        {
718                int size = objOut->size;
719                objectSetSize(objOut,size+1);
720                objectSetSize(&inter,size+1);
721                clipVecPointByPlane(objIn.poly[i],A, &(objOut->poly[size]) , &(inter.poly[size]));
722                //if whole poly was clipped away -> delete empty poly
723                if(0 == objOut->poly[size].size) {
724                        objectSetSize(objOut,size);
725                        objectSetSize(&inter,size);
726                }
727        }
728        //add a polygon of all intersection points with plane to close the object
729        appendIntersectionVecPoint(objOut,&inter);
730        emptyObject(&inter);
731        emptyObject(&objIn);
732}
733
734
735void clipObjectByAABox(struct Object* obj, const struct AABox box) {
736        struct VecPlane planes = VECPLANE_NULL;
737        int i;
738        if(0 == obj) {
739                return;
740        }
741       
742        calcAABoxPlanes(&planes,box);
743        for(i = 0; i < planes.size; i++) {
744                clipObjectByPlane(*obj,planes.plane[i],obj);
745        }
746
747        emptyVecPlane(&planes);
748}
749
750int clipTest(const double p, const double q, double* u1, double* u2) {
751        // Return value is 'true' if line segment intersects the current test
752        // plane.  Otherwise 'false' is returned in which case the line segment
753        // is entirely clipped.
754        if(0 == u1 || 0 == u2) {
755                return 0;
756        }
757        if(p < 0.0) {
758                double r = q/p;
759                if(r > (*u2)) {
760                        return 0;
761                }
762                else {
763                        if(r > (*u1)) {
764                                (*u1) = r;
765                        }
766                        return 1;
767                }
768        }
769        else {
770                if(p > 0.0)     {
771                        double r = q/p;
772                        if(r < (*u1)) {
773                                return 0;
774                        }
775                        else {
776                                if(r < (*u2)) {
777                                        (*u2) = r;
778                                }
779                                return 1;
780                        }
781                }
782                else {
783                        return q >= 0.0;
784                }
785        }
786}
787
788int intersectionLineAABox(Vector3 v, const Vector3 p, const Vector3 dir, const struct AABox b) {
789        double t1 = 0.0;
790        double t2 = DOUBLE_MAX;
791        int intersect =
792                clipTest(-dir[2],p[2]-b.min[2],&t1,&t2) && clipTest(dir[2],b.max[2]-p[2],&t1,&t2) &&
793                clipTest(-dir[1],p[1]-b.min[1],&t1,&t2) && clipTest(dir[1],b.max[1]-p[1],&t1,&t2) &&
794                clipTest(-dir[0],p[0]-b.min[0],&t1,&t2) && clipTest(dir[0],b.max[0]-p[0],&t1,&t2);
795        if(!intersect) {
796                return 0;
797        }
798        intersect = 0;
799        if(0 <= t1) {
800                linCombVector3(v,p,dir,t1);
801                intersect = 1;
802        }
803        if(0 <= t2) {
804                linCombVector3(v,p,dir,t2);
805                intersect = 1;
806        }
807        return intersect;
808}
809
810
811void includeObjectLightVolume(struct VecPoint* points, const struct Object obj,
812                                                          const Vector3 lightDir, const struct AABox sceneAABox) {
813        if(0 != points) {
814                int i, size;
815                Vector3 ld;
816                copyVector3Values(ld,-lightDir[0],-lightDir[1],-lightDir[2]);
817                convObject2VecPoint(points,obj);
818                size = points->size;
819                //for each point add the point on the ray in -lightDir
820                //intersected with the sceneAABox
821                for(i = 0; i < size; i++) {
822                        Vector3 pt;
823                        if(intersectionLineAABox(pt,points->points[i],ld,sceneAABox) ) {
824                                append2VecPoint(points,pt);
825                        }
826                }
827        }
828}
829
830void calcFocusedLightVolumePoints(struct VecPoint* points, const Matrix4x4 invEyeProjView,
831                                                                  const Vector3 lightDir,const struct AABox sceneAABox) {
832        Vector3x8 pts;
833        struct Object obj = OBJECT_NULL;
834
835        calcViewFrustumWorldCoord(pts,invEyeProjView);
836        calcViewFrustObject(&obj, pts);
837        clipObjectByAABox(&obj,sceneAABox);
838        includeObjectLightVolume(points,obj,lightDir,sceneAABox);
839        emptyObject(&obj);
840}
841
842void calcViewFrustumPlanes(struct VecPlane* planes, const Matrix4x4 eyeProjView) {
843        if(0 != planes) {
844                int i;
845               
846                planesSetSize(planes, 6);
847       
848                //---- extract the plane equations
849                for (i = 0; i < 4; i++) {
850                        setPlaneCoeff(eyeProjView[i * 4 + 3] - eyeProjView[i * 4 + 0], planes->plane + 0, i);   // right plane
851                        setPlaneCoeff(eyeProjView[i * 4 + 3] + eyeProjView[i * 4 + 0], planes->plane + 1, i);   // left plane
852                        setPlaneCoeff(eyeProjView[i * 4 + 3] + eyeProjView[i * 4 + 1], planes->plane + 2, i);   // bottom plane
853                        setPlaneCoeff(eyeProjView[i * 4 + 3] - eyeProjView[i * 4 + 1], planes->plane + 3, i);   // top plane
854                        setPlaneCoeff(eyeProjView[i * 4 + 3] - eyeProjView[i * 4 + 2], planes->plane + 4, i);   // far plane
855                        setPlaneCoeff(eyeProjView[i * 4 + 3] + eyeProjView[i * 4 + 2], planes->plane + 5, i);   // near plane
856                }
857
858                //---- normalize the coefficients
859                for (i = 0; i < 6; i++) {
860                        float invLength = 1;
861                        float length = (float)sqrt(squaredLength(planes->plane[i].n));
862
863                        if(length) invLength    = 1.0f / length;
864
865                        planes->plane[i].n[0] *= invLength;
866                        planes->plane[i].n[1] *= invLength;
867                        planes->plane[i].n[2] *= invLength;
868                       
869                        planes->plane[i].d *= invLength;
870                }
871        }
872}
873
874int calcAABNearestVertexIdx(Vector3 clipPlaneNormal) {
875        int     index;
876
877        //     7+------+6
878        //     /|     /|
879        //    / |    / |
880        //   / 4+---/--+5
881        // 3+------+2 /    y   z
882        //  | /    | /     |  /
883        //  |/     |/      |/
884        // 0+------+1      *---x
885        if (clipPlaneNormal[0] <= 0.0f) {
886                index   = (clipPlaneNormal[1] <= 0.0f) ? 0 : 3;
887        }
888        else {
889                index   = (clipPlaneNormal[1] <= 0.0f) ? 1 : 2;
890        }
891
892        return (clipPlaneNormal[2] <= 0.0f) ? index : 4 + index;
893}
894
895int calcAABFarthestVertexIdx(Vector3 clipPlaneNormal) {
896        int index;
897
898        if (clipPlaneNormal[0] > 0.0f) {
899                index   = (clipPlaneNormal[1] > 0.0f) ? 0 : 3;
900        }
901        else {
902                index   = (clipPlaneNormal[1] > 0.0f) ? 1 : 2;
903        }
904
905        return (clipPlaneNormal[2] > 0.0f) ? index : 4 + index;
906}
907
908void calcAABNPVertexIndices(int *vertexIndices, const struct VecPlane planes) {
909        int i;
910        for (i = 0; i < 6; i++)
911        {
912                vertexIndices[i * 2 + 0] = calcAABNearestVertexIdx(planes.plane[i].n);  // n-vertex
913                vertexIndices[i * 2 + 1] = calcAABFarthestVertexIdx(planes.plane[i].n); // p-vertex
914        }
915}
916
917void combineAABoxes(struct AABox *a, const struct AABox b) {
918        appendToCubicHull(a->min, a->max, b.min);
919        appendToCubicHull(a->min, a->max, b.max);
920}
921
922double calcAABoxVolume(const struct AABox aab)
923{
924        return (aab.max[0] - aab.min[0]) *
925                   (aab.max[1] - aab.min[1]) *
926                   (aab.max[2] - aab.min[2]);
927}
928
929
930void calcAABoxCenter(Vector3 vec, const struct AABox aab)
931{
932        diffVector3(vec, aab.max, aab.min);
933        vec[0] *= 0.5f; vec[1] *= 0.5f; vec[2] *= 0.5f;
934       
935        addVector3(vec, aab.min, vec);
936}
937
938
939double calcAABoxSurface(const struct AABox aab)
940{
941        return 2 * (aab.max[0] - aab.min[0]) * (aab.max[1] - aab.min[1]) +
942                   2 * (aab.max[0] - aab.min[0]) * (aab.max[2] - aab.min[2]) +
943                   2 * (aab.max[1] - aab.min[1]) * (aab.max[2] - aab.min[2]);
944}
945
946void clipAABoxByAABox(struct AABox *aab, const struct AABox enclosed)
947{
948        int i;
949        for(i=0; i < 3; i++)
950        {
951                if(aab->min[i] < enclosed.min[i])
952                        aab->min[i] = enclosed.min[i];
953                if(aab->max[i] > enclosed.max[i])
954                        aab->max[i] = enclosed.max[i];
955        }
956}
957
958
959void rotateVectorZ(Vector3 v, double rad)
960{
961        Vector3 temp;
962
963        temp[0] = v[0]*cos(rad) - v[1]*sin(rad);
964        temp[1] = v[0]*sin(rad) + v[1]*cos(rad);
965        temp[2] = v[2];
966
967        copyVector3(v, temp);
968}
969
970
971void rotateVectorX(Vector3 v, double rad)
972{
973        Vector3 temp;
974       
975        temp[0] = v[0];
976        temp[1] = v[1]*cos(rad) - v[2]*sin(rad);
977        temp[2] = v[1]*sin(rad) + v[2]*cos(rad);
978       
979        copyVector3(v, temp);
980}
981
982
983void rotateVectorY(Vector3 v, double rad)
984{
985        Vector3 temp;
986
987        temp[0] = v[2]*sin(rad) + v[0]*cos(rad);
988        temp[1] = v[1];
989        temp[2] = v[2]*cos(rad) - v[0]*sin(rad);
990
991        copyVector3(v, temp);
992}
993
994
995
996void rotateVector(Vector3 result, const Vector3 input, const float rad, const Vector3 axis)
997{
998        // using quaternions (vector and scalar part)
999        float s = (float)cos(rad * 0.5);
1000        float h = (float)sin(rad * 0.5);
1001        float p_v_dot;
1002
1003        Vector3 hvec = {axis[0] * h, axis[1]* h, axis[2] * h};
1004        Vector3 hvec2;
1005        Vector3 hvec3;
1006
1007        cross(hvec2, hvec, input);
1008        cross(hvec3, hvec, hvec2);
1009
1010        p_v_dot = (float)dot(input, hvec);
1011
1012        result[0] = s * s * input[0] + hvec[0] * p_v_dot +
1013                                         2.0f * s * hvec2[0] + hvec3[0];
1014
1015        result[1] = s * s * input[1] + hvec[1] * p_v_dot +
1016                                         2.0f * s *hvec2[1] + hvec3[1];
1017
1018        result[2] = s * s * input[2] + hvec[2] * p_v_dot +
1019                                         2.0f * s *hvec2[2] + hvec3[2];
1020
1021}
1022
1023
1024void vectorNormal(Vector3 result, const Vector3 vec)
1025{
1026        result[0] = 0.0f;
1027        result[1] = 0.0f;
1028        result[2] = 0.0f;
1029
1030        // to avoid 0-vector: look if at least one component is not 0)
1031        if((vec[0] != 0.0f) || (vec[1] != 0.0f))
1032        {
1033                result[0] = -vec[1];
1034                result[1] = vec[0];
1035        }       
1036        else
1037        {
1038                result[0] = -vec[2];
1039                result[1] = vec[0];
1040        }
1041}
Note: See TracBrowser for help on using the repository browser.