1 #include "3dc.h"
2 #include "inline.h"
3 #include "maths.h"
4
5 #define UseTimsPinp Yes
6
7 /*
8
9 externs for commonly used global variables and arrays
10
11 */
12
13 extern const short ArcCosTable[4096];
14 extern const short ArcSineTable[4096];
15 extern const short ArcTanTable[256];
16
17 extern LONGLONGCH ll_zero;
18
19
20 /*
21
22 Globals
23
24 */
25
26 MATRIXCH IdentityMatrix = {
27
28 ONE_FIXED, 0, 0,
29 0, ONE_FIXED, 0,
30 0, 0, ONE_FIXED
31
32 };
33
34
35
36 /*
37
38 Maths functions used by the system
39
40 */
41
42
43
44 /* One over sin functions - CDF 4/2/98 */
45
46 extern int oneoversin[4096];
47
ConstructOneOverSinTable(void)48 void ConstructOneOverSinTable(void) {
49
50 int a,sin;
51
52 for (a=0; a<4096; a++) {
53 sin=GetSin(a);
54
55 if (sin!=0) {
56 oneoversin[a]=DIV_FIXED(ONE_FIXED,sin);
57 } else {
58 sin=100;
59 oneoversin[a]=DIV_FIXED(ONE_FIXED,sin);
60 }
61 }
62
63 }
64
GetOneOverSin(int a)65 int GetOneOverSin(int a) {
66
67 int b;
68
69 b=a&wrap360;
70
71 return(oneoversin[b]);
72
73 }
74
75 /*
76
77 Dot Product Function
78
79 It accepts two pointers to vectors and returns an int result
80
81 */
82
_DotProduct(VECTORCH * vptr1,VECTORCH * vptr2)83 int _DotProduct(VECTORCH *vptr1, VECTORCH *vptr2)
84
85 {
86
87 int dp;
88
89 dp = MUL_FIXED(vptr1->vx, vptr2->vx);
90 dp += MUL_FIXED(vptr1->vy, vptr2->vy);
91 dp += MUL_FIXED(vptr1->vz, vptr2->vz);
92
93 return(dp);
94
95 }
96
97
DotProduct2d(VECTOR2D * vptr1,VECTOR2D * vptr2)98 int DotProduct2d(VECTOR2D *vptr1, VECTOR2D *vptr2)
99
100 {
101
102 int dp;
103
104
105 dp = MUL_FIXED(vptr1->vx, vptr2->vx);
106 dp += MUL_FIXED(vptr1->vy, vptr2->vy);
107
108 return dp;
109
110 }
111
112
113 /*
114
115 This function returns the distance between two vectors
116
117 */
118
VectorDistance(VECTORCH * v1,VECTORCH * v2)119 int VectorDistance(VECTORCH *v1, VECTORCH *v2)
120
121 {
122
123 VECTORCH v;
124
125
126 v.vx = v1->vx - v2->vx;
127 v.vy = v1->vy - v2->vy;
128 v.vz = v1->vz - v2->vz;
129
130 return Magnitude(&v);
131
132 }
133
134
135 /*
136
137 This function compares the distance between two vectors along each of
138 the major axes and returns Yes or No if they are within the cube defined
139 by the argument passed.
140
141 */
142
OutcodeVectorDistance(VECTORCH * v1,VECTORCH * v2,int d)143 int OutcodeVectorDistance(VECTORCH *v1, VECTORCH *v2, int d)
144
145 {
146
147 int i;
148
149
150 i = v1->vx - v2->vx;
151 if(i < 0) i = -i;
152
153 if(i >= d) return No;
154
155 i = v1->vy - v2->vy;
156 if(i < 0) i = -i;
157
158 if(i >= d) return No;
159
160 i = v1->vz - v2->vz;
161 if(i < 0) i = -i;
162
163 if(i >= d) return No;
164
165 return Yes;
166
167 }
168
169
170 /*
171
172 Subtract one VECTORCH from another and return the result as a normal
173
174 v3 = Normal(v1 - v2)
175
176 */
177
GetNormalVector(VECTORCH * v1,VECTORCH * v2,VECTORCH * v3)178 void GetNormalVector(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3)
179
180 {
181
182 v3->vx = v1->vx - v2->vx;
183 v3->vy = v1->vy - v2->vy;
184 v3->vz = v1->vz - v2->vz;
185
186 Normalise(v3);
187
188 }
189
190
191 /*
192
193 Normalise a vector close to, but less than, unit length
194
195 */
196
Renormalise(VECTORCH * nvector)197 void Renormalise(VECTORCH *nvector)
198
199 {
200
201 int m;
202 int xsq, ysq, zsq;
203
204
205 /* Scale x, y and z */
206
207 nvector->vx >>= 2;
208 nvector->vy >>= 2;
209 nvector->vz >>= 2;
210
211
212 /* Normalise */
213
214 xsq = nvector->vx * nvector->vx;
215 ysq = nvector->vy * nvector->vy;
216 zsq = nvector->vz * nvector->vz;
217
218 m = SqRoot32(xsq + ysq + zsq);
219
220 if(m == 0) m = 1; /* Just in case */
221
222 nvector->vx = (nvector->vx * ONE_FIXED) / m;
223 nvector->vy = (nvector->vy * ONE_FIXED) / m;
224 nvector->vz = (nvector->vz * ONE_FIXED) / m;
225
226 }
227
228
229
230
231
232
233
234
235
236 /*
237
238 Return the shift value required to get one value LTE the other value
239
240 */
241
FindShift32(int value,int limit)242 int FindShift32(int value, int limit)
243
244 {
245
246 int shift = 0;
247
248
249 /*if(limit == 0) exit(0xfa11fa11);*/
250
251
252 if(value < 0) value = -value;
253
254 while(value > limit) {
255
256 shift++;
257
258 value >>= 1;
259
260 }
261
262 return shift;
263
264 }
265
266
267 /*
268
269 Return the largest value of an int array
270
271 */
272
MaxInt(int * iarray,int iarraysize)273 int MaxInt(int *iarray, int iarraysize)
274
275 {
276
277 int imax = smallint;
278 int i;
279
280 for(i = iarraysize; i!=0; i--) {
281
282 if(imax < *iarray) imax = *iarray;
283
284 iarray++;
285
286 }
287
288 return imax;
289
290 }
291
292
293 /*
294
295 Return the smallest value of an int array
296
297 */
298
MinInt(int * iarray,int iarraysize)299 int MinInt(int *iarray, int iarraysize)
300
301 {
302
303 int imin = bigint;
304 int i;
305
306 for(i = iarraysize; i!=0; i--) {
307
308 if(imin > *iarray) imin = *iarray;
309
310 iarray++;
311
312 }
313
314 return imin;
315
316 }
317
318
319
320 /*
321
322 Create Matrix from Euler Angles
323
324 It requires a pointer to some euler angles and a pointer to a matrix
325
326 Construct the matrix elements using the following formula
327
328 Formula for ZXY Matrix
329
330 m11 = cy*cz + sx*sy*sz m12 = -cy*sz + sx*sy*cz m13 = cx*sy
331 m21 = cx*sz m22 = cx*cz m23 = -sx
332 m31 = -sy*cz + sx*cy*sz m32 = sy*sz + sx*cy*cz m33 = cx*cy
333
334 */
335
CreateEulerMatrix(EULER * e,MATRIXCH * m1)336 void CreateEulerMatrix(EULER *e, MATRIXCH *m1)
337 {
338 int t, sx, sy, sz, cx, cy, cz;
339
340
341 sx = GetSin(e->EulerX);
342 sy = GetSin(e->EulerY);
343 sz = GetSin(e->EulerZ);
344
345 cx = GetCos(e->EulerX);
346 cy = GetCos(e->EulerY);
347 cz = GetCos(e->EulerZ);
348
349
350 #if 0
351 textprint("Euler Matrix Sines & Cosines\n");
352 textprint("%d, %d, %d\n", sx, sy, sz);
353 textprint("%d, %d, %d\n", cx, cy, cz);
354 #endif
355
356
357 /* m11 = cy*cz + sx*sy*sz */
358
359 m1->mat11 = MUL_FIXED(cy, cz); /* cy*cz */
360 t = MUL_FIXED(sx, sy); /* sx*sy */
361 t = MUL_FIXED(t, sz); /* *sz */
362 m1->mat11 += t;
363
364
365 /* m12 = -cy*sz + sx*sy*cz */
366
367 m1->mat12=MUL_FIXED(-cy,sz);
368 t=MUL_FIXED(sx,sy);
369 t=MUL_FIXED(t,cz);
370 m1->mat12+=t;
371
372
373 /* m13 = cx*sy */
374
375 m1->mat13=MUL_FIXED(cx,sy);
376
377
378 /* m21 = cx*sz */
379
380 m1->mat21=MUL_FIXED(cx,sz);
381
382
383 /* m22 = cx*cz */
384
385 m1->mat22=MUL_FIXED(cx,cz);
386
387
388 /* m23 = -sx */
389
390 m1->mat23=-sx;
391
392
393 /* m31 = -sy*cz + sx*cy*sz */
394
395 m1->mat31=MUL_FIXED(-sy,cz);
396 t=MUL_FIXED(sx,cy);
397 t=MUL_FIXED(t,sz);
398 m1->mat31+=t;
399
400
401 /* m32 = sy*sz + sx*cy*cz */
402
403 m1->mat32=MUL_FIXED(sy,sz);
404 t=MUL_FIXED(sx,cy);
405 t=MUL_FIXED(t,cz);
406 m1->mat32+=t;
407
408
409 /* m33 = cx*cy */
410
411 m1->mat33=MUL_FIXED(cx,cy);
412 }
413
414
415 /*
416
417 Create a Unit Vector from three Euler Angles
418
419 */
420
CreateEulerVector(EULER * e,VECTORCH * v)421 void CreateEulerVector(EULER *e, VECTORCH *v)
422
423 {
424
425 int t, sx, sy, sz, cx, cy, cz;
426
427
428 sx = GetSin(e->EulerX);
429 sy = GetSin(e->EulerY);
430 sz = GetSin(e->EulerZ);
431
432 cx = GetCos(e->EulerX);
433 cy = GetCos(e->EulerY);
434 cz = GetCos(e->EulerZ);
435
436
437 /* x = -sy*cz + sx*cy*sz */
438
439 v->vx = MUL_FIXED(-sy, cz);
440 t = MUL_FIXED(sx, cy);
441 t = MUL_FIXED(t, sz);
442 v->vx += t;
443
444
445 /* y = sy*sz + sx*cy*cz */
446
447 v->vy = MUL_FIXED(sy, sz);
448 t = MUL_FIXED(sx, cy);
449 t = MUL_FIXED(t, cz);
450 v->vy += t;
451
452
453 /* z = cx*cy */
454
455 v->vz = MUL_FIXED(cx,cy);
456
457 }
458
459
460
461 /*
462
463 Matrix Multiply Function
464
465 A 3x3 Matrix is represented here as
466
467 m11 m12 m13
468 m21 m22 m23
469 m31 m32 m33
470
471 Row #1 (r1) of the matrix is m11 m12 m13
472 Column #1 (c1) of the matrix is m11 m32 m31
473
474 Under multiplication
475
476 m'' = m x m'
477
478 where
479
480 m11'' = c1.r1'
481 m12'' = c2.r1'
482 m13'' = c3.r1'
483
484 m21'' = c1.r2'
485 m22'' = c2.r2'
486 m23'' = c3.r2'
487
488 m31'' = c1.r3'
489 m32'' = c2.r3'
490 m33'' = c3.r3'
491
492 */
493
MatrixMultiply(struct matrixch * m1,struct matrixch * m2,struct matrixch * m3)494 void MatrixMultiply(struct matrixch *m1, struct matrixch *m2, struct matrixch *m3)
495
496 {
497 MATRIXCH TmpMat;
498
499 /* m11'' = c1.r1' */
500
501 TmpMat.mat11=MUL_FIXED(m1->mat11,m2->mat11);
502 TmpMat.mat11+=MUL_FIXED(m1->mat21,m2->mat12);
503 TmpMat.mat11+=MUL_FIXED(m1->mat31,m2->mat13);
504
505 /* m12'' = c2.r1' */
506
507 TmpMat.mat12=MUL_FIXED(m1->mat12,m2->mat11);
508 TmpMat.mat12+=MUL_FIXED(m1->mat22,m2->mat12);
509 TmpMat.mat12+=MUL_FIXED(m1->mat32,m2->mat13);
510
511 /* m13'' = c3.r1' */
512
513 TmpMat.mat13=MUL_FIXED(m1->mat13,m2->mat11);
514 TmpMat.mat13+=MUL_FIXED(m1->mat23,m2->mat12);
515 TmpMat.mat13+=MUL_FIXED(m1->mat33,m2->mat13);
516
517 /* m21'' = c1.r2' */
518
519 TmpMat.mat21=MUL_FIXED(m1->mat11,m2->mat21);
520 TmpMat.mat21+=MUL_FIXED(m1->mat21,m2->mat22);
521 TmpMat.mat21+=MUL_FIXED(m1->mat31,m2->mat23);
522
523 /* m22'' = c2.r2' */
524
525 TmpMat.mat22=MUL_FIXED(m1->mat12,m2->mat21);
526 TmpMat.mat22+=MUL_FIXED(m1->mat22,m2->mat22);
527 TmpMat.mat22+=MUL_FIXED(m1->mat32,m2->mat23);
528
529 /* m23'' = c3.r2' */
530
531 TmpMat.mat23=MUL_FIXED(m1->mat13,m2->mat21);
532 TmpMat.mat23+=MUL_FIXED(m1->mat23,m2->mat22);
533 TmpMat.mat23+=MUL_FIXED(m1->mat33,m2->mat23);
534
535 /* m31'' = c1.r3' */
536
537 TmpMat.mat31=MUL_FIXED(m1->mat11,m2->mat31);
538 TmpMat.mat31+=MUL_FIXED(m1->mat21,m2->mat32);
539 TmpMat.mat31+=MUL_FIXED(m1->mat31,m2->mat33);
540
541 /* m32'' = c2.r3' */
542
543 TmpMat.mat32=MUL_FIXED(m1->mat12,m2->mat31);
544 TmpMat.mat32+=MUL_FIXED(m1->mat22,m2->mat32);
545 TmpMat.mat32+=MUL_FIXED(m1->mat32,m2->mat33);
546
547 /* m33'' = c3.r3' */
548
549 TmpMat.mat33=MUL_FIXED(m1->mat13,m2->mat31);
550 TmpMat.mat33+=MUL_FIXED(m1->mat23,m2->mat32);
551 TmpMat.mat33+=MUL_FIXED(m1->mat33,m2->mat33);
552
553 /* Finally, copy TmpMat to m3 */
554
555 CopyMatrix(&TmpMat, m3);
556 }
557
558
559 /*
560
561 Transpose Matrix
562
563 */
564
TransposeMatrixCH(MATRIXCH * m1)565 void TransposeMatrixCH(MATRIXCH *m1)
566
567 {
568
569 int t;
570
571 t=m1->mat12;
572 m1->mat12=m1->mat21;
573 m1->mat21=t;
574
575 t=m1->mat13;
576 m1->mat13=m1->mat31;
577 m1->mat31=t;
578
579 t=m1->mat23;
580 m1->mat23=m1->mat32;
581 m1->mat32=t;
582
583 }
584
585
586 /*
587
588 Copy Vector
589
590 */
591
CopyVector(VECTORCH * v1,VECTORCH * v2)592 void CopyVector(VECTORCH *v1, VECTORCH *v2)
593
594 {
595
596 /* Copy VECTORCH v1 -> VECTORCH v2 */
597
598 v2->vx=v1->vx;
599 v2->vy=v1->vy;
600 v2->vz=v1->vz;
601
602 }
603
604
605 /*
606
607 Copy Location
608
609 */
610
CopyLocation(VECTORCH * v1,VECTORCH * v2)611 void CopyLocation(VECTORCH *v1, VECTORCH *v2)
612
613 {
614
615 /* Copy VECTORCH v1 -> VECTORCH v2 */
616
617 v2->vx=v1->vx;
618 v2->vy=v1->vy;
619 v2->vz=v1->vz;
620
621 }
622
623
624
625
626
627 /*
628
629 Copy Euler
630
631 */
632
CopyEuler(EULER * e1,EULER * e2)633 void CopyEuler(EULER *e1, EULER *e2)
634
635 {
636
637 /* Copy EULER e1 -> EULER e2 */
638
639 e2->EulerX=e1->EulerX;
640 e2->EulerY=e1->EulerY;
641 e2->EulerZ=e1->EulerZ;
642
643 }
644
645
646 /*
647
648 Copy Matrix
649
650 */
651
CopyMatrix(MATRIXCH * m1,MATRIXCH * m2)652 void CopyMatrix(MATRIXCH *m1, MATRIXCH *m2)
653
654 {
655
656 /* Copy MATRIXCH m1 -> MATRIXCH m2 */
657
658 m2->mat11=m1->mat11;
659 m2->mat12=m1->mat12;
660 m2->mat13=m1->mat13;
661
662 m2->mat21=m1->mat21;
663 m2->mat22=m1->mat22;
664 m2->mat23=m1->mat23;
665
666 m2->mat31=m1->mat31;
667 m2->mat32=m1->mat32;
668 m2->mat33=m1->mat33;
669
670 }
671
672
673 /*
674
675 Make a Vector.
676
677 v3 = v1 - v2
678
679 */
680
MakeVector(VECTORCH * v1,VECTORCH * v2,VECTORCH * v3)681 void MakeVector(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3)
682
683 {
684
685 v3->vx = v1->vx - v2->vx;
686 v3->vy = v1->vy - v2->vy;
687 v3->vz = v1->vz - v2->vz;
688
689 }
690
691
692 /*
693
694 Add a Vector.
695
696 v2 = v2 + v1
697
698 */
699
AddVector(VECTORCH * v1,VECTORCH * v2)700 void AddVector(VECTORCH *v1, VECTORCH *v2)
701
702 {
703
704 v2->vx += v1->vx;
705 v2->vy += v1->vy;
706 v2->vz += v1->vz;
707
708 }
709
710
711 /*
712
713 Subtract a Vector.
714
715 v2 = v2 - v1
716
717 */
718
SubVector(VECTORCH * v1,VECTORCH * v2)719 void SubVector(VECTORCH *v1, VECTORCH *v2)
720
721 {
722
723 v2->vx -= v1->vx;
724 v2->vy -= v1->vy;
725 v2->vz -= v1->vz;
726
727 }
728
729
730
731 /*
732
733 Matrix Rotatation of a Vector
734
735 Overwrite the Source Vector with the Rotated Vector
736
737 x' = v.c1
738 y' = v.c2
739 z' = v.c3
740
741 */
742
_RotateVector(VECTORCH * v,MATRIXCH * m)743 void _RotateVector(VECTORCH *v, MATRIXCH* m)
744 {
745
746 int x, y, z;
747
748
749 x = MUL_FIXED(m->mat11, v->vx);
750 x += MUL_FIXED(m->mat21, v->vy);
751 x += MUL_FIXED(m->mat31, v->vz);
752
753 y = MUL_FIXED(m->mat12, v->vx);
754 y += MUL_FIXED(m->mat22, v->vy);
755 y += MUL_FIXED(m->mat32, v->vz);
756
757 z = MUL_FIXED(m->mat13, v->vx);
758 z += MUL_FIXED(m->mat23, v->vy);
759 z += MUL_FIXED(m->mat33, v->vz);
760
761 v->vx = x;
762 v->vy = y;
763 v->vz = z;
764 }
765
766
767 /*
768
769 Matrix Rotation of a Source Vector using a Matrix
770 Copying to a Destination Vector
771
772 x' = v.c1
773 y' = v.c2
774 z' = v.c3
775
776 */
777
_RotateAndCopyVector(VECTORCH * v1,VECTORCH * v2,MATRIXCH * m)778 void _RotateAndCopyVector(VECTORCH *v1, VECTORCH *v2, MATRIXCH *m)
779
780 {
781
782 v2->vx=MUL_FIXED(m->mat11,v1->vx);
783 v2->vx+=MUL_FIXED(m->mat21,v1->vy);
784 v2->vx+=MUL_FIXED(m->mat31,v1->vz);
785
786 v2->vy=MUL_FIXED(m->mat12,v1->vx);
787 v2->vy+=MUL_FIXED(m->mat22,v1->vy);
788 v2->vy+=MUL_FIXED(m->mat32,v1->vz);
789
790 v2->vz=MUL_FIXED(m->mat13,v1->vx);
791 v2->vz+=MUL_FIXED(m->mat23,v1->vy);
792 v2->vz+=MUL_FIXED(m->mat33,v1->vz);
793
794 }
795
796
797
798 /*
799
800 Matrix to Euler Angles
801
802 Maths overflow is a real problem for this function. To prevent overflows
803 the matrix Sines and Cosines are calculated using values scaled down by 4.
804
805
806 sinx = -M23
807
808 cosx = sqr ( 1 - sinx^2 )
809
810
811 siny = M13 / cosx
812
813 cosy = M33 / cosx
814
815
816 sinz = M21 / cosx
817
818 cosz = M22 / cosx
819
820
821 */
822
823
824 #define m2e_scale 2
825 #define ONE_FIXED_S ((ONE_FIXED >> m2e_scale) - 1)
826 #define m2e_shift 14
827
828 #define j_and_r_change Yes
829
830
MatrixToEuler(MATRIXCH * m,EULER * e)831 void MatrixToEuler(MATRIXCH *m, EULER *e)
832
833 {
834
835 int x, sinx, cosx, siny, cosy, sinz, cosz;
836 int abs_cosx, abs_cosy, abs_cosz;
837 int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;
838 int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;
839
840
841
842
843 #if 0
844 textprint("CosMatrixPitch = %d\n", CosMatrixPitch);
845 /* WaitForReturn(); */
846 #endif
847
848
849 if(m->mat32 >-65500 && m->mat32<65500)
850 {
851 /* Yaw */
852
853 /* Pitch */
854
855 #if j_and_r_change
856 SineMatrixPitch = -m->mat32;
857 #else
858 SineMatrixPitch = -m->mat23;
859 #endif
860
861 SineMatrixPitch >>= m2e_scale;
862
863 #if 0
864 textprint("SineMatrixPitch = %d\n", SineMatrixPitch);
865 /* WaitForReturn(); */
866 #endif
867
868 CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;
869 CosMatrixPitch >>= m2e_shift;
870
871 CosMatrixPitch = -CosMatrixPitch;
872 CosMatrixPitch += ONE_FIXED_S;
873 CosMatrixPitch *= ONE_FIXED_S;
874 CosMatrixPitch = SqRoot32(CosMatrixPitch);
875
876 if(CosMatrixPitch) {
877
878 if(CosMatrixPitch > ONE_FIXED_S) CosMatrixPitch = ONE_FIXED_S;
879 else if(CosMatrixPitch < -ONE_FIXED_S) CosMatrixPitch = -ONE_FIXED_S;
880
881 }
882
883 else CosMatrixPitch = 1;
884
885 SineMatrixYaw = WideMulNarrowDiv(
886 #if j_and_r_change
887 m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
888 #else
889 m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
890 #endif
891
892 #if 0
893 textprint("SineMatrixYaw = %d\n", SineMatrixYaw);
894 /* WaitForReturn(); */
895 #endif
896
897 CosMatrixYaw = WideMulNarrowDiv(
898 m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
899
900 #if 0
901 textprint("CosMatrixYaw = %d\n", CosMatrixYaw);
902 /* WaitForReturn(); */
903 #endif
904
905
906 /* Roll */
907
908 SineMatrixRoll = WideMulNarrowDiv(
909 #if j_and_r_change
910 m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
911 #else
912 m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
913 #endif
914
915 #if 0
916 textprint("SineMatrixRoll = %d\n", SineMatrixRoll);
917 /* WaitForReturn(); */
918 #endif
919
920 CosMatrixRoll = WideMulNarrowDiv(
921 m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
922
923 #if 0
924 textprint("CosMatrixRoll = %d\n", CosMatrixRoll);
925 /* WaitForReturn(); */
926 #endif
927
928 /* Tables are for values +- 2^16 */
929
930 sinx = SineMatrixPitch << m2e_scale;
931 siny = SineMatrixYaw << m2e_scale;
932 sinz = SineMatrixRoll << m2e_scale;
933
934 cosx = CosMatrixPitch << m2e_scale;
935 cosy = CosMatrixYaw << m2e_scale;
936 cosz = CosMatrixRoll << m2e_scale;
937
938 #if 0
939 textprint("sines = %d, %d, %d\n", sinx, siny, sinz);
940 textprint("cos's = %d, %d, %d\n", cosx, cosy, cosz);
941 /* WaitForReturn(); */
942 #endif
943
944 /* Absolute Cosines */
945
946 abs_cosx = cosx;
947 if(abs_cosx < 0) abs_cosx = -abs_cosx;
948
949 abs_cosy = cosy;
950 if(abs_cosy < 0) abs_cosy = -abs_cosy;
951
952 abs_cosz = cosz;
953 if(abs_cosz < 0) abs_cosz = -abs_cosz;
954
955
956 /* Euler X */
957
958 if(abs_cosx > Cosine45) {
959
960 x = ArcSin(sinx);
961
962 if(cosx < 0) {
963 x = -x;
964 x += deg180;
965 x &= wrap360;
966 }
967 }
968
969 else {
970
971 x = ArcCos(cosx);
972
973 if(sinx < 0) {
974 x = -x;
975 x &= wrap360;
976 }
977 }
978
979 #if (j_and_r_change == No)
980 x = -x;
981 x &= wrap360;
982 #endif
983
984 e->EulerX = x;
985
986
987 /* Euler Y */
988
989 if(abs_cosy > Cosine45) {
990
991 x = ArcSin(siny);
992
993 if(cosy < 0) {
994 x = -x;
995 x += deg180;
996 x &= wrap360;
997 }
998
999 }
1000
1001 else {
1002
1003 x = ArcCos(cosy);
1004
1005 if(siny < 0) {
1006 x = -x;
1007 x &= wrap360;
1008 }
1009
1010 }
1011
1012 #if (j_and_r_change == No)
1013 x = -x;
1014 x &= wrap360;
1015 #endif
1016
1017 e->EulerY = x;
1018
1019
1020 /* Euler Z */
1021
1022 if(abs_cosz > Cosine45) {
1023
1024 x = ArcSin(sinz);
1025
1026 if(cosz < 0) {
1027 x = -x;
1028 x += deg180;
1029 x &= wrap360;
1030 }
1031 }
1032
1033 else {
1034
1035 x = ArcCos(cosz);
1036
1037 if(sinz < 0) {
1038 x = -x;
1039 x &= wrap360;
1040 }
1041 }
1042
1043 #if (j_and_r_change == No)
1044 x = -x;
1045 x &= wrap360;
1046 #endif
1047
1048 e->EulerZ = x;
1049 }
1050 else //singularity case
1051 {
1052
1053 if(m->mat32>0)
1054 e->EulerX = 3072;
1055 else
1056 e->EulerX = 1024;
1057
1058 e->EulerZ=0;
1059
1060
1061
1062 /* Yaw */
1063
1064 siny = -m->mat13 ;
1065
1066 cosy = m->mat11 ;
1067
1068 abs_cosy = cosy;
1069 if(abs_cosy < 0) abs_cosy = -abs_cosy;
1070
1071
1072 if(abs_cosy > Cosine45) {
1073
1074 x = ArcSin(siny);
1075
1076 if(cosy < 0) {
1077 x = -x;
1078 x += deg180;
1079 x &= wrap360;
1080 }
1081
1082 }
1083
1084 else {
1085
1086 x = ArcCos(cosy);
1087
1088 if(siny < 0) {
1089 x = -x;
1090 x &= wrap360;
1091 }
1092
1093 }
1094
1095 #if (j_and_r_change == No)
1096 x = -x;
1097 x &= wrap360;
1098 #endif
1099
1100 e->EulerY = x;
1101
1102 }
1103
1104
1105
1106
1107 #if 0
1108 textprint("\nEuler from VDB Matrix is:\n%d\n%d\n%d\n",
1109 e->EulerX,
1110 e->EulerY,
1111 e->EulerZ
1112 );
1113 /* WaitForReturn(); */
1114 #endif
1115
1116 }
1117
1118
1119 #define j_and_r_change_2 Yes
1120
MatrixToEuler2(MATRIXCH * m,EULER * e)1121 void MatrixToEuler2(MATRIXCH *m, EULER *e)
1122
1123 {
1124
1125 int x, sinx, cosx, siny, cosy, sinz, cosz;
1126 int abs_cosx, abs_cosy, abs_cosz;
1127 int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;
1128 int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;
1129
1130
1131 /* Pitch */
1132
1133 #if j_and_r_change_2
1134 SineMatrixPitch = -m->mat32;
1135 #else
1136 SineMatrixPitch = -m->mat23;
1137 #endif
1138
1139 SineMatrixPitch >>= m2e_scale;
1140
1141 #if 0
1142 textprint("SineMatrixPitch = %d\n", SineMatrixPitch);
1143 /* WaitForReturn(); */
1144 #endif
1145
1146 CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;
1147 CosMatrixPitch >>= m2e_shift;
1148
1149 CosMatrixPitch = -CosMatrixPitch;
1150 CosMatrixPitch += ONE_FIXED_S;
1151 CosMatrixPitch *= ONE_FIXED_S;
1152 CosMatrixPitch = SqRoot32(CosMatrixPitch);
1153
1154 if(CosMatrixPitch) {
1155
1156 if(CosMatrixPitch > ONE_FIXED_S) CosMatrixPitch = ONE_FIXED_S;
1157 else if(CosMatrixPitch < -ONE_FIXED_S) CosMatrixPitch = -ONE_FIXED_S;
1158
1159 }
1160
1161 else CosMatrixPitch = 1;
1162
1163
1164 #if 0
1165 textprint("CosMatrixPitch = %d\n", CosMatrixPitch);
1166 /* WaitForReturn(); */
1167 #endif
1168
1169
1170 /* Yaw */
1171
1172 SineMatrixYaw = WideMulNarrowDiv(
1173 #if j_and_r_change_2
1174 m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
1175 #else
1176 m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
1177 #endif
1178
1179 #if 0
1180 textprint("SineMatrixYaw = %d\n", SineMatrixYaw);
1181 /* WaitForReturn(); */
1182 #endif
1183
1184 CosMatrixYaw = WideMulNarrowDiv(
1185 m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
1186
1187 #if 0
1188 textprint("CosMatrixYaw = %d\n", CosMatrixYaw);
1189 /* WaitForReturn(); */
1190 #endif
1191
1192
1193 /* Roll */
1194
1195 SineMatrixRoll = WideMulNarrowDiv(
1196 #if j_and_r_change_2
1197 m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
1198 #else
1199 m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
1200 #endif
1201
1202 #if 0
1203 textprint("SineMatrixRoll = %d\n", SineMatrixRoll);
1204 /* WaitForReturn(); */
1205 #endif
1206
1207 CosMatrixRoll = WideMulNarrowDiv(
1208 m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
1209
1210 #if 0
1211 textprint("CosMatrixRoll = %d\n", CosMatrixRoll);
1212 /* WaitForReturn(); */
1213 #endif
1214
1215
1216 /* Tables are for values +- 2^16 */
1217
1218 sinx = SineMatrixPitch << m2e_scale;
1219 siny = SineMatrixYaw << m2e_scale;
1220 sinz = SineMatrixRoll << m2e_scale;
1221
1222 cosx = CosMatrixPitch << m2e_scale;
1223 cosy = CosMatrixYaw << m2e_scale;
1224 cosz = CosMatrixRoll << m2e_scale;
1225
1226 #if 0
1227 textprint("sines = %d, %d, %d\n", sinx, siny, sinz);
1228 textprint("cos's = %d, %d, %d\n", cosx, cosy, cosz);
1229 /* WaitForReturn(); */
1230 #endif
1231
1232 /* Absolute Cosines */
1233
1234 abs_cosx = cosx;
1235 if(abs_cosx < 0) abs_cosx = -abs_cosx;
1236
1237 abs_cosy = cosy;
1238 if(abs_cosy < 0) abs_cosy = -abs_cosy;
1239
1240 abs_cosz = cosz;
1241 if(abs_cosz < 0) abs_cosz = -abs_cosz;
1242
1243
1244 /* Euler X */
1245
1246 if(abs_cosx > Cosine45) {
1247
1248 x = ArcSin(sinx);
1249
1250 if(cosx < 0) {
1251 x = -x;
1252 x += deg180;
1253 x &= wrap360;
1254 }
1255 }
1256
1257 else {
1258
1259 x = ArcCos(cosx);
1260
1261 if(sinx < 0) {
1262 x = -x;
1263 x &= wrap360;
1264 }
1265 }
1266
1267 #if (j_and_r_change_2 == No)
1268 x = -x;
1269 x &= wrap360;
1270 #endif
1271
1272 e->EulerX = x;
1273
1274
1275 /* Euler Y */
1276
1277 if(abs_cosy > Cosine45) {
1278
1279 x = ArcSin(siny);
1280
1281 if(cosy < 0) {
1282 x = -x;
1283 x += deg180;
1284 x &= wrap360;
1285 }
1286
1287 }
1288
1289 else {
1290
1291 x = ArcCos(cosy);
1292
1293 if(siny < 0) {
1294 x = -x;
1295 x &= wrap360;
1296 }
1297
1298 }
1299
1300 #if (j_and_r_change_2 == No)
1301 x = -x;
1302 x &= wrap360;
1303 #endif
1304
1305 e->EulerY = x;
1306
1307
1308 /* Euler Z */
1309
1310 if(abs_cosz > Cosine45) {
1311
1312 x = ArcSin(sinz);
1313
1314 if(cosz < 0) {
1315 x = -x;
1316 x += deg180;
1317 x &= wrap360;
1318 }
1319 }
1320
1321 else {
1322
1323 x = ArcCos(cosz);
1324
1325 if(sinz < 0) {
1326 x = -x;
1327 x &= wrap360;
1328 }
1329 }
1330
1331 #if (j_and_r_change_2 == No)
1332 x = -x;
1333 x &= wrap360;
1334 #endif
1335
1336 e->EulerZ = x;
1337
1338
1339 #if 0
1340 textprint("\nEuler from VDB Matrix is:\n%d\n%d\n%d\n",
1341 e->EulerX,
1342 e->EulerY,
1343 e->EulerZ
1344 );
1345 /* WaitForReturn(); */
1346 #endif
1347
1348 }
1349
1350
1351
1352 /*
1353
1354 Normalise a Matrix
1355
1356 Dot the three vectors together (XY, XZ, YZ) and take the two nearest to
1357 90� from each other. Cross them to create a new third vector, then cross
1358 the first and third to create a new second.
1359
1360 */
1361
MNormalise(MATRIXCH * m)1362 void MNormalise(MATRIXCH *m)
1363
1364 {
1365
1366 VECTORCH *x = (VECTORCH *) &m->mat11;
1367 VECTORCH *y = (VECTORCH *) &m->mat21;
1368 VECTORCH *z = (VECTORCH *) &m->mat31;
1369 int dotxy = Dot(x, y);
1370 int dotxz = Dot(x, z);
1371 int dotyz = Dot(y, z);
1372 VECTORCH *s;
1373 VECTORCH *t;
1374 VECTORCH u;
1375 VECTORCH v;
1376 VECTORCH zero = {0, 0, 0};
1377
1378
1379 #if 0
1380 textprint("dotxy = %d\n", dotxy);
1381 textprint("dotxz = %d\n", dotxz);
1382 textprint("dotyz = %d\n", dotyz);
1383 #endif
1384
1385 #if 0
1386 /* TEST */
1387 dotxy = 0;
1388 dotxz = 0;
1389 dotyz = 1;
1390 #endif
1391
1392
1393 #if 0
1394 textprint("%d %d %d\n",
1395 x->vx,
1396 x->vy,
1397 x->vz
1398 );
1399
1400 textprint("%d %d %d\n",
1401 y->vx,
1402 y->vy,
1403 y->vz
1404 );
1405
1406 textprint("%d %d %d\n",
1407 z->vx,
1408 z->vy,
1409 z->vz
1410 );
1411 #endif
1412
1413
1414 /* Find the two vectors nearest 90� */
1415
1416 if(dotxy > dotxz && dotxy > dotyz) {
1417
1418 /* xy are the closest to 90� */
1419
1420 /*textprint("xy\n");*/
1421
1422 s = x;
1423 t = y;
1424
1425 MakeNormal(&zero, s, t, &u); /* Cross them for a new 3rd vector */
1426
1427 MakeNormal(&zero, s, &u, &v); /* Cross 1st & 3rd for a new 2nd */
1428 v.vx = -v.vx;
1429 v.vy = -v.vy;
1430 v.vz = -v.vz;
1431
1432 CopyVector(&u, z);
1433 CopyVector(&v, y);
1434
1435 }
1436
1437 else if(dotxz > dotxy && dotxz > dotyz) {
1438
1439 /* xz are the closest to 90� */
1440
1441 /*textprint("xz\n");*/
1442
1443 s = x;
1444 t = z;
1445
1446 MakeNormal(&zero, s, t, &u); /* Cross them for a new 3rd vector */
1447 u.vx = -u.vx;
1448 u.vy = -u.vy;
1449 u.vz = -u.vz;
1450
1451 MakeNormal(&zero, s, &u, &v); /* Cross 1st & 3rd for a new 2nd */
1452
1453 CopyVector(&u, y);
1454 CopyVector(&v, z);
1455
1456 }
1457
1458 else {
1459
1460 /* yz are the closest to 90� */
1461
1462 /*textprint("yz\n");*/
1463
1464 s = y;
1465 t = z;
1466
1467 MakeNormal(&zero, s, t, &u); /* Cross them for a new 3rd vector */
1468
1469 MakeNormal(&zero, s, &u, &v); /* Cross 1st & 3rd for a new 2nd */
1470 v.vx = -v.vx;
1471 v.vy = -v.vy;
1472 v.vz = -v.vz;
1473
1474 CopyVector(&u, x);
1475 CopyVector(&v, z);
1476
1477 }
1478
1479
1480 #if 0
1481 textprint("%d %d %d\n",
1482 x->vx,
1483 x->vy,
1484 x->vz
1485 );
1486
1487 textprint("%d %d %d\n",
1488 y->vx,
1489 y->vy,
1490 y->vz
1491 );
1492
1493 textprint("%d %d %d\n",
1494 z->vx,
1495 z->vy,
1496 z->vz
1497 );
1498 #endif
1499
1500 #if 0
1501 textprint("mag. x = %d\n", Magnitude(x));
1502 textprint("mag. y = %d\n", Magnitude(y));
1503 textprint("mag. z = %d\n", Magnitude(z));
1504 #endif
1505
1506 /*WaitForReturn();*/
1507
1508
1509 }
1510
1511
1512
1513
1514 /*
1515
1516 ArcCos
1517
1518 In: COS value as -65,536 -> +65,536.
1519 Out: Angle in 0 -> 4095 form.
1520
1521 Notes:
1522
1523 The angle returned is in the range 0 -> 2,047 since the sign of SIN
1524 is not known.
1525
1526 ArcSin(x) = ArcTan ( x, sqr ( 1-x*x ) )
1527 ArcCos(x) = ArcTan ( sqr ( 1-x*x ), x)
1528
1529 -65,536 = 180 Degrees
1530 0 = 90 Degrees
1531 +65,536 = 0 Degrees
1532
1533 The table has 4,096 entries.
1534
1535 */
1536
ArcCos(int c)1537 int ArcCos(int c)
1538
1539 {
1540
1541 short acos;
1542
1543 if(c < (-(ONE_FIXED - 1))) c = -(ONE_FIXED - 1);
1544 else if(c > (ONE_FIXED - 1)) c = ONE_FIXED - 1;
1545
1546 #if 0
1547 c = c >> 5; /* -64k -> +64k becomes -2k -> +2k */
1548 c += 2048; /* -2k -> +2k becomes 0 -> 4k */
1549 #endif
1550
1551 acos = ArcCosTable[(c >> 5) + 2048];
1552
1553 return (int) (acos & wrap360);
1554
1555 }
1556
1557
1558 /*
1559
1560 ArcSin
1561
1562 In: SIN value in ax as -65,536 -> +65,536.
1563 Out: Angle in 0 -> 4095 form in ax.
1564
1565 Notes:
1566
1567 The angle returned is in the range -1,024 -> 1,023 since the sign of COS
1568 is not known.
1569
1570 ArcSin(x) = ArcTan ( x, sqr ( 1-x*x ) )
1571 ArcCos(x) = ArcTan ( sqr ( 1-x*x ), x)
1572
1573 -65,536 = 270 Degrees
1574 0 = 0 Degrees
1575 +65,536 = 90 Degrees
1576
1577 The table has 4,096 entries.
1578
1579 */
1580
ArcSin(int s)1581 int ArcSin(int s)
1582
1583 {
1584
1585 short asin;
1586
1587
1588 if(s < (-(ONE_FIXED - 1))) s = -(ONE_FIXED - 1);
1589 else if(s > (ONE_FIXED - 1)) s = ONE_FIXED - 1;
1590
1591 #if 0
1592 s = s >> 5; /* -64k -> +64k becomes -2k -> +2k */
1593 s += 2048; /* -2k -> +2k becomes 0 -> 4k */
1594 #endif
1595
1596 asin = ArcSineTable[(s >> 5) + 2048];
1597
1598 return (int) (asin & wrap360);
1599
1600 }
1601
1602
1603 /*
1604
1605 ArcTan
1606
1607 Pass (x,z)
1608
1609 And ATN(x/z) is returned such that:
1610
1611 000� is Map North
1612 090� is Map East
1613 180� is Map South
1614 270� is Map West
1615
1616 */
1617
ArcTan(int height_x,int width_z)1618 int ArcTan(int height_x, int width_z)
1619
1620 {
1621
1622 int abs_height_x, abs_width_z, angle, sign, signsame, temp;
1623
1624 sign=0;
1625
1626 if((height_x<0 && width_z<0) || (height_x>=0 && width_z>=0))
1627 signsame=Yes;
1628 else
1629 signsame=No;
1630
1631 abs_height_x=height_x;
1632 if(abs_height_x<0) abs_height_x=-abs_height_x;
1633
1634 abs_width_z=width_z;
1635 if(abs_width_z<0) abs_width_z=-abs_width_z;
1636
1637 /*
1638
1639 Find ATN
1640
1641 */
1642
1643 if(width_z==0) angle=-deg90;
1644
1645 else if(abs_width_z==abs_height_x)
1646 angle=deg45;
1647
1648 else {
1649
1650 if(abs_width_z>abs_height_x) {
1651 temp=abs_width_z;
1652 abs_width_z=abs_height_x;
1653 abs_height_x=temp;
1654 sign=-1;
1655 }
1656
1657 if(abs_height_x!=0)
1658
1659 /* angle = (abs_width_z << 8) / abs_height_x; */
1660
1661
1662
1663 angle = DIV_INT((abs_width_z << 8), abs_height_x);
1664
1665
1666
1667
1668
1669 else
1670 angle=deg22pt5;
1671
1672 angle=ArcTanTable[angle];
1673
1674 if(sign>=0) {
1675 angle=-angle;
1676 angle+=deg90;
1677 }
1678
1679 }
1680
1681 if(signsame==No) angle=-angle;
1682
1683 if(width_z<=0) angle+=deg180;
1684
1685 angle&=wrap360;
1686
1687 return(angle);
1688
1689 }
1690
1691
1692 /*
1693
1694 Matrix from Z-Vector
1695
1696 */
1697
MatrixFromZVector(VECTORCH * v,MATRIXCH * m)1698 void MatrixFromZVector(VECTORCH *v, MATRIXCH *m)
1699
1700 {
1701
1702 VECTORCH XVector;
1703 VECTORCH YVector;
1704
1705 VECTORCH zero = {0, 0, 0};
1706
1707
1708 XVector.vx = v->vz;
1709 XVector.vy = 0;
1710 XVector.vz = -v->vx;
1711
1712 Normalise(&XVector);
1713
1714 MakeNormal(&zero, &XVector, v, &YVector);
1715
1716 m->mat11 = XVector.vx;
1717 m->mat12 = XVector.vy;
1718 m->mat13 = XVector.vz;
1719
1720 m->mat21 = -YVector.vx;
1721 m->mat22 = -YVector.vy;
1722 m->mat23 = -YVector.vz;
1723
1724 m->mat31 = v->vx;
1725 m->mat32 = v->vy;
1726 m->mat33 = v->vz;
1727
1728 }
1729
1730
1731
1732
1733
1734
1735
1736
1737
1738
1739 /*
1740
1741 Distance Functions
1742
1743 */
1744
1745
1746 /*
1747
1748 Foley and Van Dam 2d distance function
1749
1750 WARNING! Returns distance x 3
1751
1752 Here is the F & VD distance function:
1753
1754 x + z + (max(x,z) * 2)
1755 ----------------------
1756 3
1757
1758 */
1759
FandVD_Distance_2d(VECTOR2D * v0,VECTOR2D * v1)1760 int FandVD_Distance_2d(VECTOR2D *v0, VECTOR2D *v1)
1761
1762 {
1763
1764 int max;
1765 int d;
1766
1767
1768 int dx = v1->vx - v0->vx;
1769 int dy = v1->vy - v0->vy;
1770
1771 if(dx < 0) dx = -dx;
1772 if(dy < 0) dy = -dy;
1773
1774 if(dx > dy) max = dx;
1775 else max = dy;
1776
1777 d = (dx + dy + (max * 2));
1778
1779 return d;
1780
1781 }
1782
1783
1784 /*
1785
1786 Foley and Van Dam 3d distance function
1787
1788 WARNING! Returns distance x 9
1789
1790 For a 3d version, calculate (f(f(x,y), y*3))/9
1791
1792 */
1793
FandVD_Distance_3d(VECTORCH * v0,VECTORCH * v1)1794 int FandVD_Distance_3d(VECTORCH *v0, VECTORCH *v1)
1795
1796 {
1797
1798 int dxy, max;
1799
1800 int dz = v1->vz - v0->vz;
1801
1802 if(dz < 0) dz = -dz;
1803
1804 dz *= 3;
1805
1806 dxy = FandVD_Distance_2d((VECTOR2D *) v0, (VECTOR2D *) v1);
1807
1808 if(dxy > dz) max = dxy;
1809 else max = dz;
1810
1811 return (dxy + dz + (max * 2));
1812
1813 }
1814
1815
1816 /*
1817
1818 NextLowPower2() returns the next lowest power of 2 of the passed value.
1819
1820 e.g. 18 is returned as 16.
1821
1822 */
1823
NextLowPower2(int i)1824 int NextLowPower2(int i)
1825
1826 {
1827
1828 int n = 1;
1829
1830
1831 while(n <= i)
1832 n <<= 1;
1833
1834 return n >> 1;
1835
1836 }
1837
1838
1839 /*
1840
1841 Transform a world location into the local space of the passed matrix and
1842 location.
1843
1844 Vector v1 is transformed to v2
1845 It is made relative to vector v3 and rotated using matrix m transposed
1846
1847 A possible use is the transformation of world points into the local space
1848 of a display block
1849
1850 e.g.
1851
1852 MakeVectorLocal(&v1, &v2, &dptr->ObWorld, &dptr->ObMat);
1853
1854 This would place vector v2 into the local space of display block dptr
1855
1856 */
1857
MakeVectorLocal(VECTORCH * v1,VECTORCH * v2,VECTORCH * v3,MATRIXCH * m)1858 void MakeVectorLocal(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3, MATRIXCH *m)
1859
1860 {
1861
1862 MATRIXCH transmat;
1863
1864
1865 CopyMatrix(m, &transmat);
1866 TransposeMatrixCH(&transmat);
1867
1868 v2->vx = v1->vx - v3->vx;
1869 v2->vy = v1->vy - v3->vy;
1870 v2->vz = v1->vz - v3->vz;
1871
1872 RotateVector(v2, &transmat);
1873
1874 }
1875
1876
1877
1878
1879
1880 /*
1881
1882 Returns "Yes" if "point" is inside "polygon"
1883
1884
1885 **************************************************
1886
1887 WARNING!! Point and Polygon Data are OVERWRITTEN!!
1888
1889 **************************************************
1890
1891
1892 The function requires point to be an integer array containing a single
1893 XY pair. The number of points must be passed too.
1894
1895 Pass the size of the polygon point e.g. A Gouraud polygon has points X,Y,I
1896 so its point size would be 3.
1897
1898
1899 Item Polygon Point Size
1900 ---- ------------------
1901
1902 I_Polygon 2
1903 I_GouraudPolygon 3
1904 I_2dTexturedPolygon 4
1905 I_3dTexturedPolygon, 5
1906 I_Gouraud2dTexturedPolygon 5
1907 I_Polygon_ZBuffer 3
1908 I_GouraudPolygon_ZBuffer 4
1909
1910
1911 PASS ONLY POSITIVE COORDINATES!
1912
1913 */
1914
PointInPolygon(int * point,int * polygon,int c,int ppsize)1915 int PointInPolygon(int *point, int *polygon, int c, int ppsize)
1916
1917 {
1918
1919
1920 #if UseTimsPinp
1921
1922
1923 /* Tim's New Point In Polygon test-- hopefully much faster, */
1924 /* certainly much smaller. */
1925 /* Uses Half-Line test for point-in-2D-polygon test */
1926 /* Tests the half-line going from the point in the direction of positive z */
1927
1928 int x, z; /* point */
1929 int sx, sz; /* vertex 1 */
1930 int *polyp; /* vertex 2 pointer */
1931 int t;
1932 int dx, dz; /* ABS(vertex 2 - vertex 1) */
1933 int sgnx; /* going left or going right */
1934 int intersects; /* number of intersections so far discovered */
1935 LONGLONGCH a_ll, b_ll;
1936
1937 /* reject lines and points */
1938 if (c < 3) return(No);
1939
1940 intersects = 0;
1941
1942 x = point[ix];
1943 z = point[iy]; /* ! */
1944
1945 /* get last point */
1946 polyp = polygon + ((c - 1) * ppsize);
1947 sx = polyp[0];
1948 sz = polyp[1];
1949
1950 /* go back to first point */
1951 polyp = polygon;
1952
1953 dx = 0; /* TODO: uninitialized?? */
1954
1955 /* for each point */
1956 while (0 != c)
1957 {
1958
1959 /* is this line straddling the x co-ordinate of the point? */
1960 /* if not it is not worth testing for intersection with the half-line */
1961 /* we must be careful to get the strict and non-stict inequalities */
1962 /* correct, or we may count intersections with vertices the wrong number */
1963 /* of times. */
1964 sgnx = 0;
1965 if (sx < x && x <= polyp[0])
1966 {
1967 /* going right */
1968 sgnx = 1;
1969 dx = polyp[0] - sx;
1970 }
1971 if (polyp[0] < x && x <= sx)
1972 {
1973 /* going left */
1974 sgnx = -1;
1975 dx = sx - polyp[0];
1976 }
1977
1978 /* if sgnx is zero then neither of the above conditions are true, */
1979 /* hence the line does not straddle the point in x */
1980 if (0 != sgnx)
1981 {
1982 /* next do trivial cases of line totally above or below point */
1983 if (z < sz && z < polyp[1])
1984 {
1985 /* line totally above point -- intersection */
1986 intersects++;
1987 }
1988 else if (z <= sz || z <= polyp[1])
1989 {
1990 /* line straddles point in both x and z -- we must do interpolation */
1991
1992 /* get absolute differences between line end z co-ordinates */
1993 dz = (sz < polyp[1])?(polyp[1] - sz):(sz - polyp[1]);
1994
1995 /* B504 is the square root of 7FFFFFFF */
1996 if (0xB504L < dx || 0xB504L < dz)
1997 {
1998 /* LARGE line -- use 64-bit values */
1999 /* interpolate z */
2000 MUL_I_WIDE(polyp[1] - sz, x - sx, &a_ll);
2001 MUL_I_WIDE(polyp[0] - sx, z - sz, &b_ll);
2002 if(CMP_LL(&a_ll, &b_ll) == sgnx)
2003 {
2004 /* we have an intersection */
2005 intersects++;
2006 }
2007 }
2008 else
2009 {
2010 /* small line -- use 32-bit values */
2011 /* interpolate z */
2012 t = (polyp[1] - sz) * (x - sx) - (polyp[0] - sx) * (z - sz);
2013 if ((t < 0 && sgnx < 0) || (0 < t && 0 < sgnx))
2014 {
2015 /* we have an intersection */
2016 intersects++;
2017 }
2018 }
2019 } /* (if line straddles point in z) */
2020 } /* (if line straddles point in x) */
2021
2022 /* get next line : */
2023 /* new vertex 1 is old vertex 2 */
2024 sx = polyp[0];
2025 sz = polyp[1];
2026
2027 /* new vertex 2 is next point */
2028 polyp += ppsize;
2029
2030 /* next vertex */
2031 c--;
2032 }
2033
2034 if (intersects & 1)
2035 {
2036 /* Odd number of intersections -- point is inside polygon */
2037 return(Yes);
2038 }
2039 else
2040 {
2041 /* even number of intersections -- point is outside polygon */
2042 return(No);
2043 }
2044
2045
2046
2047 #else
2048
2049
2050 int i;
2051 int si, ti;
2052 int s0, t0;
2053 int s1, t1;
2054 int *v0;
2055 int *v1;
2056 int ivdot, ivdotcnt, sgn_currivdot, sgn_ivdot, ivstate;
2057 int ns, nt;
2058 int x_scale, y_scale;
2059 int DotNudge;
2060
2061 int x, z;
2062 LONGLONGCH xx;
2063 LONGLONGCH zz;
2064 LONGLONGCH xx_tmp;
2065 LONGLONGCH zz_tmp;
2066 VECTORCH PolyAvgPt;
2067
2068
2069 /* Reject points and lines */
2070
2071 if(c < 3) return No;
2072
2073
2074 /* Find the average point */
2075
2076 v0 = polygon;
2077
2078 EQUALS_LL(&xx, &ll_zero);
2079 EQUALS_LL(&zz, &ll_zero);
2080
2081 for(i = c; i!=0; i--) {
2082
2083 x = v0[0];
2084 z = v0[1];
2085
2086 IntToLL(&xx_tmp, &x); /* xx_tmp = (long long)x */
2087 IntToLL(&zz_tmp, &z); /* zz_tmp = (long long)z */
2088
2089 ADD_LL_PP(&xx, &xx_tmp); /* xx += xx_tmp */
2090 ADD_LL_PP(&zz, &zz_tmp); /* zz += zz_tmp */
2091
2092 v0 += ppsize;
2093
2094 }
2095
2096 PolyAvgPt.vx = NarrowDivide(&xx, c);
2097 PolyAvgPt.vz = NarrowDivide(&zz, c);
2098
2099
2100 /* Centre the polygon */
2101
2102 v0 = polygon;
2103
2104 for(i = c; i!=0; i--) {
2105
2106 v0[0] -= PolyAvgPt.vx;
2107 v0[1] -= PolyAvgPt.vz;
2108
2109 v0 += ppsize;
2110
2111 }
2112
2113
2114 /* Centre the test point */
2115
2116 point[0] -= PolyAvgPt.vx;
2117 point[1] -= PolyAvgPt.vz;
2118
2119
2120 /* Scale to avoid maths overflow */
2121
2122 v0 = polygon;
2123
2124 s0 = 0;
2125 t0 = 0;
2126
2127 for(i = c; i!=0; i--) {
2128
2129 si = v0[0]; if(si < 0) si = -si;
2130 if(si > s0) s0 = si;
2131
2132 ti = v0[1]; if(ti < 0) ti = -ti;
2133 if(ti > t0) t0 = ti;
2134
2135 v0 += ppsize;
2136
2137 }
2138
2139 si = point[ix]; if(si < 0) si = -si;
2140 if(si > s0) s0 = si;
2141
2142 ti = point[iy]; if(ti < 0) ti = -ti;
2143 if(ti > t0) t0 = ti;
2144
2145
2146 #if 0
2147 textprint("\nmax x = %d\n", s0);
2148 textprint("max y = %d\n", t0);
2149 #endif
2150
2151
2152 x_scale = FindShift32(s0, 16383);
2153 y_scale = FindShift32(t0, 16383);
2154
2155
2156 #if 0
2157 textprint("scales = %d, %d\n", x_scale, y_scale);
2158 #endif
2159
2160
2161 v0 = polygon;
2162
2163 for(i = c; i!=0; i--) {
2164
2165 v0[0] >>= x_scale;
2166 v0[1] >>= y_scale;
2167
2168 /*textprint("(%d, %d)\n", v0[0], v0[1]);*/
2169
2170 v0 += ppsize;
2171
2172 }
2173
2174 point[ix] >>= x_scale;
2175 point[iy] >>= y_scale;
2176
2177
2178
2179
2180 #if 1
2181
2182 /* Clockwise or Anti-Clockwise? */
2183
2184 ns = -(polygon[iy + ppsize] - polygon[iy]);
2185 nt = (polygon[ix + ppsize] - polygon[ix]);
2186
2187 si = polygon[(ppsize*2) + ix] - polygon[ix];
2188 ti = polygon[(ppsize*2) + iy] - polygon[iy];
2189
2190 ivdot = (ns * si) + (nt * ti);
2191
2192 if(ivdot < 0) DotNudge = -1;
2193 else DotNudge = 1;
2194
2195 #endif
2196
2197
2198
2199 #if 0
2200 if(ivdot < 0) textprint("Clockwise\n");
2201 WaitForReturn();
2202 #endif
2203
2204
2205 /* Point to test */
2206
2207 si = point[ix];
2208 ti = point[iy];
2209
2210
2211 #if 0
2212 textprint("p_test %d, %d\n", si, ti);
2213 #endif
2214
2215
2216 /* Polygon Vector pointers */
2217
2218 v0 = polygon;
2219 v1 = v0 + ppsize;
2220
2221
2222 /* Dot result monitor */
2223
2224 ivdotcnt = 0;
2225 ivstate = Yes; /* assume inside */
2226
2227
2228 /* Test v(s, t) against the vectors */
2229
2230 for(i = c; i!=0 && ivstate == Yes; i--) {
2231
2232
2233 /* second vector pointer wraps once */
2234
2235 if(i == 1) v1 = polygon;
2236
2237
2238 /* get the vector */
2239
2240 s0 = v0[ix];
2241 t0 = v0[iy];
2242
2243 s1 = v1[ix];
2244 t1 = v1[iy];
2245
2246
2247 #if 0
2248 textprint("%d,%d; %d,%d\n", s0, t0, s1, t1);
2249 #endif
2250
2251
2252 /* get the vector normal */
2253
2254 ns = -(t1 - t0); /* s -> -t */
2255 nt = s1 - s0; /* t -> s */
2256
2257
2258 /* Dot with intersection point */
2259
2260 ivdot = (ns * (si - s0)) + (nt * (ti - t0));
2261
2262
2263 /* TEST */
2264 ivdot += DotNudge;
2265
2266
2267 sgn_ivdot = 1;
2268 if(ivdot < 0) sgn_ivdot = -1;
2269
2270
2271 /* only continue if current dot is same as last, else quit */
2272
2273 if(ivdotcnt == 0) sgn_currivdot = sgn_ivdot;
2274
2275 else {
2276
2277 if(sgn_ivdot != sgn_currivdot) ivstate = No;
2278 sgn_currivdot = sgn_ivdot;
2279
2280 }
2281
2282 v0 += ppsize;
2283 v1 += ppsize;
2284
2285 ivdotcnt++;
2286
2287 }
2288
2289 if(ivstate) return Yes;
2290 else return No;
2291
2292
2293 #endif
2294
2295
2296 }
2297
2298
2299
2300
2301
2302
2303
2304 /*
2305
2306 #defines and statics required for Jamie's Most Excellent
2307 random number generator
2308
2309 */
2310
2311 #define DEG_3 31
2312 #define SEP_3 3
2313
2314 static int32_t table [DEG_3] =
2315 {
2316 -851904987, -43806228, -2029755270, 1390239686, -1912102820,
2317 -485608943, 1969813258, -1590463333, -1944053249, 455935928,
2318 508023712, -1714531963, 1800685987, -2015299881, 654595283,
2319 -1149023258, -1470005550, -1143256056, -1325577603, -1568001885,
2320 1275120390, -607508183, -205999574, -1696891592, 1492211999,
2321 -1528267240, -952028296, -189082757, 362343714, 1424981831,
2322 2039449641
2323 };
2324
2325 #define TABLE_END (table + sizeof (table) / sizeof (table [0]))
2326
2327 static int32_t * front_ptr = table + SEP_3;
2328 static int32_t * rear_ptr = table;
2329
2330
2331 void SetSeededFastRandom(int seed);
SetFastRandom(void)2332 void SetFastRandom(void)
2333
2334 {
2335
2336 int i;
2337 long number = GetTickCount();
2338
2339
2340 for(i = 0; i < DEG_3; ++i) {
2341
2342 number = 1103515145 * number + 12345;
2343 table[i] = number;
2344
2345 }
2346
2347 front_ptr = table + SEP_3;
2348 rear_ptr = table;
2349
2350 for(i = 0; i < 10 * DEG_3; ++i)
2351 (void) FastRandom ();
2352
2353 SetSeededFastRandom(FastRandom());
2354
2355 }
2356
2357
FastRandom(void)2358 int FastRandom(void)
2359
2360 {
2361
2362 int32_t i;
2363
2364 /*
2365
2366 Discard least random bit.
2367 Shift as unsigned to avoid replicating sign bit.
2368 Faster than masking.
2369
2370 */
2371
2372 *front_ptr += *rear_ptr;
2373 i = (int32_t) ((uint32_t) *front_ptr >> 1);
2374
2375 /* `front_ptr' and `rear_ptr' can't wrap at the same time. */
2376
2377 ++front_ptr;
2378
2379 if(front_ptr < TABLE_END) {
2380
2381 ++rear_ptr;
2382
2383 if (rear_ptr < TABLE_END) return i;
2384
2385 rear_ptr = table;
2386
2387 }
2388
2389 else { /* front_ptr >= TABLE_END */
2390
2391 front_ptr = table;
2392 ++rear_ptr;
2393
2394 }
2395
2396 return (int) i;
2397
2398 }
2399
2400 /*a second copy of the random number generator for getting random numbers from a single seed*/
2401
2402 #define SEEDED_DEG_3 13
2403 #define SEEDED_SEP_3 3
2404
2405 static int32_t seeded_table [SEEDED_DEG_3];
2406
2407 #define SEEDED_TABLE_END (seeded_table + sizeof (seeded_table) / sizeof (seeded_table [0]))
2408
2409 static int32_t * seeded_front_ptr = seeded_table + SEEDED_SEP_3;
2410 static int32_t * seeded_rear_ptr = seeded_table;
2411
2412
2413
SeededFastRandom(void)2414 int SeededFastRandom(void)
2415
2416 {
2417
2418 int32_t i;
2419
2420 /*
2421
2422 Discard least random bit.
2423 Shift as unsigned to avoid replicating sign bit.
2424 Faster than masking.
2425
2426 */
2427
2428 *seeded_front_ptr += *seeded_rear_ptr;
2429 i = (int32_t) ((uint32_t) *seeded_front_ptr >> 1);
2430
2431 /* `front_ptr' and `rear_ptr' can't wrap at the same time. */
2432
2433 ++seeded_front_ptr;
2434
2435 if(seeded_front_ptr < SEEDED_TABLE_END) {
2436
2437 ++seeded_rear_ptr;
2438
2439 if (seeded_rear_ptr < SEEDED_TABLE_END) return i;
2440
2441 seeded_rear_ptr = seeded_table;
2442
2443 }
2444
2445 else { /* front_ptr >= TABLE_END */
2446
2447 seeded_front_ptr = seeded_table;
2448 ++seeded_rear_ptr;
2449
2450 }
2451
2452 return (int) i;
2453
2454 }
2455
SetSeededFastRandom(int seed)2456 void SetSeededFastRandom(int seed)
2457
2458 {
2459
2460 int i;
2461 int32_t number = seed;
2462
2463
2464 for(i = 0; i < SEEDED_DEG_3; ++i) {
2465
2466 number = 1103515145 * number + 12345;
2467 seeded_table[i] = number;
2468
2469 }
2470
2471 seeded_front_ptr = seeded_table + SEEDED_SEP_3;
2472 seeded_rear_ptr = seeded_table;
2473
2474 for(i = 0; i < 2 * SEEDED_DEG_3; ++i)
2475 (void) SeededFastRandom ();
2476
2477 }
2478
2479 #if StandardShapeLanguage
2480
2481 /*
2482
2483 Calculate the average point on this polygon
2484
2485 */
2486
PolyAveragePoint(POLYHEADER * pheader,int * spts,VECTORCH * apt)2487 void PolyAveragePoint(POLYHEADER *pheader, int *spts, VECTORCH *apt)
2488
2489 {
2490
2491 int x, y, z;
2492 LONGLONGCH xx;
2493 LONGLONGCH yy;
2494 LONGLONGCH zz;
2495 LONGLONGCH xx_tmp;
2496 LONGLONGCH yy_tmp;
2497 LONGLONGCH zz_tmp;
2498 int *mypolystart = &pheader->Poly1stPt;
2499 int numpolypts;
2500
2501
2502 /* Find the average point */
2503
2504 EQUALS_LL(&xx, &ll_zero);
2505 EQUALS_LL(&yy, &ll_zero);
2506 EQUALS_LL(&zz, &ll_zero);
2507
2508 numpolypts = 0;
2509
2510 while(*mypolystart != Term) {
2511
2512 x = *(spts + *mypolystart + ix);
2513 y = *(spts + *mypolystart + iy);
2514 z = *(spts + *mypolystart + iz);
2515
2516 IntToLL(&xx_tmp, &x); /* xx_tmp = (long long)x */
2517 IntToLL(&yy_tmp, &y); /* yy_tmp = (long long)y */
2518 IntToLL(&zz_tmp, &z); /* zz_tmp = (long long)z */
2519
2520 ADD_LL_PP(&xx, &xx_tmp); /* xx += xx_tmp */
2521 ADD_LL_PP(&yy, &yy_tmp); /* yy += yy_tmp */
2522 ADD_LL_PP(&zz, &zz_tmp); /* zz += zz_tmp */
2523
2524 numpolypts++;
2525 mypolystart++;
2526
2527 }
2528
2529 apt->vx = NarrowDivide(&xx, numpolypts);
2530 apt->vy = NarrowDivide(&yy, numpolypts);
2531 apt->vz = NarrowDivide(&zz, numpolypts);
2532
2533 }
2534
2535 #endif /* StandardShapeLanguage */
2536
2537
2538
2539
2540
2541
2542 /* KJL 15:07:39 01/08/97 - Returns the magnitude of the
2543 cross product of two vectors a and b. */
MagnitudeOfCrossProduct(VECTORCH * a,VECTORCH * b)2544 int MagnitudeOfCrossProduct(VECTORCH *a, VECTORCH *b)
2545
2546 {
2547 VECTORCH c;
2548
2549 c.vx = MUL_FIXED(a->vy,b->vz) - MUL_FIXED(a->vz,b->vy);
2550 c.vy = MUL_FIXED(a->vz,b->vx) - MUL_FIXED(a->vx,b->vz);
2551 c.vz = MUL_FIXED(a->vx,b->vy) - MUL_FIXED(a->vy,b->vx);
2552
2553 return Magnitude(&c);
2554 }
2555
2556 /* KJL 15:08:01 01/08/97 - sets the vector c to be the
2557 cross product of the vectors a and b. */
CrossProduct(VECTORCH * a,VECTORCH * b,VECTORCH * c)2558 void CrossProduct(VECTORCH *a, VECTORCH *b, VECTORCH *c)
2559
2560 {
2561 c->vx = MUL_FIXED(a->vy,b->vz) - MUL_FIXED(a->vz,b->vy);
2562 c->vy = MUL_FIXED(a->vz,b->vx) - MUL_FIXED(a->vx,b->vz);
2563 c->vz = MUL_FIXED(a->vx,b->vy) - MUL_FIXED(a->vy,b->vx);
2564 }
2565
2566
2567
2568 /* KJL 12:01:08 7/16/97 - returns the magnitude of a vector - max error about 13%, though average error
2569 less than half this. Very fast compared to other approaches. */
Approximate3dMagnitude(VECTORCH * v)2570 int Approximate3dMagnitude(VECTORCH *v)
2571 {
2572 int dx,dy,dz;
2573
2574 dx = v->vx;
2575 if (dx<0) dx = -dx;
2576
2577 dy = v->vy;
2578 if (dy<0) dy = -dy;
2579
2580 dz = v->vz;
2581 if (dz<0) dz = -dz;
2582
2583
2584 if (dx>dy)
2585 {
2586 if (dx>dz)
2587 {
2588 return dx + ((dy+dz)>>2);
2589 }
2590 else
2591 {
2592 return dz + ((dy+dx)>>2);
2593 }
2594 }
2595 else
2596 {
2597 if (dy>dz)
2598 {
2599 return dy + ((dx+dz)>>2);
2600 }
2601 else
2602 {
2603 return dz + ((dx+dy)>>2);
2604 }
2605 }
2606 }
2607
2608
2609 /*
2610
2611 Quaternion to Matrix
2612
2613 This is the column(row) matrix that is produced. Our matrices are
2614 row(column) and so are a transpose of this.
2615
2616 1 - 2yy - 2zz 2xy + 2wz 2xz - 2wy
2617
2618 2xy - 2wz 1 - 2xx - 2zz 2yz + 2wx
2619
2620 2xz + 2wy 2yz - 2wx 1 - 2xx - 2yy
2621
2622 */
2623
QuatToMat(QUAT * q,MATRIXCH * m)2624 void QuatToMat(QUAT *q,MATRIXCH *m)
2625 {
2626
2627 int q_w, q_x, q_y, q_z;
2628
2629 int q_2x, q_2y, q_2z;
2630
2631 int q_2xw;
2632 int q_2xx;
2633 int q_2xy;
2634 int q_2xz;
2635 int q_2yw;
2636 int q_2yy;
2637 int q_2yz;
2638 int q_2zw;
2639 int q_2zz;
2640
2641 /*
2642
2643 The most efficient way to create the matrix is as follows
2644
2645 1/ Double x, y & z
2646
2647 */
2648
2649 q_w=q->quatw;
2650 q_x=q->quatx;
2651 q_y=q->quaty;
2652 q_z=q->quatz;
2653
2654 q_2x=q_x*2;
2655 q_2y=q_y*2;
2656 q_2z=q_z*2;
2657
2658 /*
2659
2660 2/ Form their products with w, x, y & z
2661 These are
2662
2663 (2x)w (2y)w (2z)w
2664 (2x)x
2665 (2x)y (2y)y
2666 (2x)z (2y)z (2z)z
2667
2668 */
2669
2670 q_2xw=MUL_FIXED(q_2x,q_w);
2671 q_2yw=MUL_FIXED(q_2y,q_w);
2672 q_2zw=MUL_FIXED(q_2z,q_w);
2673
2674 q_2xx=MUL_FIXED(q_2x,q_x);
2675
2676 q_2xy=MUL_FIXED(q_2x,q_y);
2677 q_2yy=MUL_FIXED(q_2y,q_y);
2678
2679 q_2xz=MUL_FIXED(q_2x,q_z);
2680 q_2yz=MUL_FIXED(q_2y,q_z);
2681 q_2zz=MUL_FIXED(q_2z,q_z);
2682
2683
2684 /* mat11 = 1 - 2y^2 - 2z^2 */
2685
2686 m->mat11=ONE_FIXED-q_2yy-q_2zz;
2687
2688 /* mat12 = 2xy - 2wz */
2689
2690 m->mat12=q_2xy-q_2zw;
2691
2692 /* mat13 = 2xz + 2wy */
2693
2694 m->mat13=q_2xz+q_2yw;
2695
2696 /* mat21 = 2xy + 2wz */
2697
2698 m->mat21=q_2xy+q_2zw;
2699
2700 /* mat22 = 1 - 2x^2 - 2z^2 */
2701
2702 m->mat22=ONE_FIXED-q_2xx-q_2zz;
2703
2704 /* mat23 = 2yz - 2wx */
2705
2706 m->mat23=q_2yz-q_2xw;
2707
2708 /* mat31 = 2xz - 2wy */
2709
2710 m->mat31=q_2xz-q_2yw;
2711
2712 /* mat32 = 2yz + 2wx */
2713
2714 m->mat32=q_2yz+q_2xw;
2715
2716 /* mat33 = 1 - 2x^2 - 2y^2 */
2717
2718 m->mat33=ONE_FIXED-q_2xx-q_2yy;
2719
2720 }
2721