1 ; $Id: vecmata.asm,v 1.6 2004-08-28 23:17:45 schaffner Exp $
2 ;THE COMPUTER CODE CONTAINED HEREIN IS THE SOLE PROPERTY OF PARALLAX
3 ;SOFTWARE CORPORATION ("PARALLAX"). PARALLAX, IN DISTRIBUTING THE CODE TO
4 ;END-USERS, AND SUBJECT TO ALL OF THE TERMS AND CONDITIONS HEREIN, GRANTS A
5 ;ROYALTY-FREE, PERPETUAL LICENSE TO SUCH END-USERS FOR USE BY SUCH END-USERS
6 ;IN USING, DISPLAYING, AND CREATING DERIVATIVE WORKS THEREOF, SO LONG AS
7 ;SUCH USE, DISPLAY OR CREATION IS FOR NON-COMMERCIAL, ROYALTY OR REVENUE
8 ;FREE PURPOSES. IN NO EVENT SHALL THE END-USER USE THE COMPUTER CODE
9 ;CONTAINED HEREIN FOR REVENUE-BEARING PURPOSES. THE END-USER UNDERSTANDS
10 ;AND AGREES TO THE TERMS HEREIN AND ACCEPTS THE SAME BY USE OF THIS FILE.
11 ;COPYRIGHT 1993-1998 PARALLAX SOFTWARE CORPORATION. ALL RIGHTS RESERVED.
14 ; Source for vector/matrix library
23 ; Cater for ELF compilers which don't prefix underscores...
25 %define _vmd_zero_vector vmd_zero_vector
26 %define _vmd_identity_matrix vmd_identity_matrix
28 %define _vm_vec_add vm_vec_add
29 %define _vm_vec_sub vm_vec_sub
30 %define _vm_vec_add2 vm_vec_add2
31 %define _vm_vec_sub2 vm_vec_sub2
32 %define _vm_vec_avg vm_vec_avg
33 %define _vm_vec_scale vm_vec_scale
34 %define _vm_vec_copy_scale vm_vec_copy_scale
35 %define _vm_vec_scale2 vm_vec_scale2
36 %define _vm_vec_scale_add vm_vec_scale_add
37 %define _vm_vec_scale_add2 vm_vec_scale_add2
38 %define _vm_vec_mag vm_vec_mag
39 %define _vm_vec_dist vm_vec_dist
40 %define _vm_vec_mag_quick vm_vec_mag_quick
41 %define _vm_vec_dist_quick vm_vec_dist_quick
42 %define _vm_vec_normalize vm_vec_normalize
43 %define _vm_vec_normalize_quick vm_vec_normalize_quick
44 %define _vm_vec_normalized_dir vm_vec_normalized_dir
45 %define _vm_vec_normalized_dir_quick vm_vec_normalized_dir_quick
46 %define _vm_vec_copy_normalize vm_vec_copy_normalize
47 %define _vm_vec_copy_normalize_quick vm_vec_copy_normalize_quick
48 %define _vm_vec_dotprod vm_vec_dotprod
49 %define _vm_vec_crossprod vm_vec_crossprod
50 %define _vm_vec_perp vm_vec_perp
51 %define _vm_vec_normal vm_vec_normal
52 %define _vm_vec_rotate vm_vec_rotate
53 %define _vm_vec_delta_ang vm_vec_delta_ang
54 %define _vm_vec_delta_ang_norm vm_vec_delta_ang_norm
55 %define _vm_vector_2_matrix vm_vector_2_matrix
56 %define _vm_vec_ang_2_matrix vm_vec_ang_2_matrix
57 %define _vm_angles_2_matrix vm_angles_2_matrix
58 %define _vm_transpose_matrix vm_transpose_matrix
59 %define _vm_copy_transpose_matrix vm_copy_transpose_matrix
60 %define _vm_matrix_x_matrix vm_matrix_x_matrix
65 ;temporary vectors for surface normal calculation
75 ;sine & cosine values for angles_2_matrix
83 global _vmd_zero_vector,_vmd_identity_matrix
85 ;These should never be changed!
99 ; calling convention is arguments on stack from right to left,
100 ; eax,ecx,edx possibly destroyed, ebx,esi,edi,ebp preserved,
101 ; caller removes arguments.
108 global _vm_vec_copy_scale
109 global _vm_vec_scale2
110 global _vm_vec_scale_add
111 global _vm_vec_scale_add2
114 global _vm_vec_mag_quick
115 global _vm_vec_dist_quick
116 global _vm_vec_normalize
117 global _vm_vec_normalize_quick
118 global _vm_vec_normalized_dir
119 global _vm_vec_normalized_dir_quick
120 global _vm_vec_copy_normalize
121 global _vm_vec_copy_normalize_quick
122 global _vm_vec_dotprod
123 global _vm_vec_crossprod
125 global _vm_vec_normal
126 global _vm_angles_2_matrix
127 global _vm_vec_rotate
128 global _vm_vec_delta_ang
129 global _vm_vec_delta_ang_norm
130 global _vm_transpose_matrix
131 global _vm_copy_transpose_matrix
132 global _vm_matrix_x_matrix
133 global _vm_vector_2_matrix
134 global _vm_vec_ang_2_matrix
167 ; offsets in fixang struct (packed)
181 ;vector offsets in matrix
186 ; vec *vm_vec_add(vec *dest, vec *src1, vec *src2);
204 ; vec *vm_vec_sub(vec *dest, vec *src1, vec *src2);
222 ; vec *vm_vec_add2(vec *dest, vec *src);
236 ; vec *vm_vec_sub2(vec *dest, vec *src);
250 ; vec *vm_vec_avg(vec *dest, vec *src1, vec *src2);
269 ; vec *vm_vec_scale(vec *dest, fix scale);
287 ; vec *vm_vec_copy_scale(vec *dest, vec *src, fix scale);
308 ; vec *vm_vec_scale_add(vec *dest, vec *src1, vec *src2, fix scale);
310 ; dest=src1+src2*scale
333 ; vec *vm_vec_scale_add2(vec *dest, vec *src, fix scale);
354 ; vec *vm_vec_scale2(vec *dest, fix n, fix d);
379 ;compute magnitude of vector. takes esi=vector, returns eax=mag
406 ;compute the distance between two points. (does sub and mag)
428 call quad_sqrt_asm ; asm version, takes eax,edx
435 ;computes an approximation of the magnitude of a vector
436 ;uses dist = largest + next_largest*3/8 + smallest*3/16
460 mag_quick_eax_ebx_ecx:
465 no_swap_ab: cmp ebx,ecx
472 do_add: sar ebx,2 ; b*1/4
474 add ebx,ecx ; b*1/4 + c*1/8
475 add eax,ebx ;a + b*1/4 + c*1/8
476 sar ebx,1 ; b*1/8 + c*1/16
477 add eax,ebx ;a + b*3/4 + c*3/16
484 ;computes an approximation of the distance between two points.
485 ;uses dist = largest + next_largest*3/8 + smallest*3/16
512 jmp mag_quick_eax_ebx_ecx
515 ;return the normalized direction vector between two points
516 ;dest = normalized(end - start).
517 ;takes edi=dest, esi=endpoint, ebx=startpoint. Returns mag of dir vec
518 ;NOTE: the order of the parameters matches the vector subtraction
519 _vm_vec_normalized_dir:
551 mov ecx,eax ;mag in ecx
570 ;normalize a vector in place.
578 jmp vm_vec_copy_normalize_nopar
580 ;normalize a vector. takes edi=dest, esi=vector
581 ;returns ecx=mag of source vec
582 _vm_vec_copy_normalize:
588 vm_vec_copy_normalize_nopar:
607 mov ecx,eax ;mag in ecx
625 ;normalize a vector in place.
627 _vm_vec_normalize_quick:
632 jmp vm_vec_copy_normalize_quick_nopar
634 ;save as vm_vec_normalized_dir, but with quick sqrt
635 ;takes dest, endpoint, startpoint. Returns mag of dir vec
636 _vm_vec_normalized_dir_quick:
650 jmp vm_vec_copy_normalize_quick_nopar
654 _vm_vec_copy_normalize_quick:
660 vm_vec_copy_normalize_quick_nopar:
662 call _vm_vec_mag_quick
665 mov ecx,eax ;mag in ecx
683 ;compute dot product of two vectors. takes esi,edi=vectors, returns eax=dotprod
708 ;ifndef NDEBUG ;check for overflow
709 ;always do overflow check, and return saturated value
710 sar edx,16 ;get real sign from high word
712 cdq ;get sign of our result
713 cmp bx,dx ;same sign?
715 ;;debug_brk 'overflow in vm_vec_dotprod'
717 or ebx,ebx ;check desired sign
728 ;computes cross product of two vectors.
729 ;Note: this magnitude of the resultant vector is the
730 ;product of the magnitudes of the two source vectors. This means it is
731 ;quite easy for this routine to overflow and underflow. Be careful that
733 ; takes dest, src vectors
745 break_if e,'crossprod: dest==src0'
747 break_if e,'crossprod: dest==src1'
759 %ifndef NDEBUG ;check for overflow
761 cdq ;get sign of result
762 shr ebx,16 ;get high 16 of quad result
763 cmp dx,bx ;sign extension the same?
764 break_if ne,'overflow in crossprod'
777 %ifndef NDEBUG ;check for overflow
779 cdq ;get sign of result
780 shr ebx,16 ;get high 16 of quad result
781 cmp dx,bx ;sign extension the same?
782 break_if ne,'overflow in crossprod'
795 %ifndef NDEBUG ;check for overflow
797 cdq ;get sign of result
798 shr ebx,16 ;get high 16 of quad result
799 cmp dx,bx ;sign extension the same?
800 break_if ne,'overflow in crossprod'
804 mov eax,ebp ;return dest in eax
813 ;computes surface normal from three points. takes ebx=dest, eax,esi,edi=vecs
814 ;returns eax=dest. Result vector is normalized.
816 push dword [esp+16+00];src2
817 push dword [esp+12+04];src1
818 push dword [esp+08+08];src0
819 push dword [esp+04+12];dest
820 call _vm_vec_perp ;get unnormalized
823 call _vm_vec_normalize
828 ;make sure a vector is reasonably sized to go into a cross product
830 ;trashes eax,ebx,cl,edx
843 xor cl,cl ;init shift count
845 test ebx,0fffc0000h ;too big
847 check_4_down: test ebx,000f00000h
852 check_2_down: test ebx,0fffc0000h
864 not_too_big: test ebx,0ffff8000h
866 check_4_up: test ebx,0fffff000h
871 check_2_up: test ebx,0ffff8000h
884 ; debug_brk commented out by mk on 05/04/94
885 ;** debug_brk "null vector in check_vec"
889 ;computes surface normal from three points. takes ebx=dest, eax,esi,edi=vecs
890 ;returns eax=dest. Result vector is NOT normalized, but this routine does
891 ;make an effort that cross product does not overflow or underflow
897 mov ebx,[esp+20] ;src0
898 mov ecx,[esp+24] ;src1
899 mov edx,[esp+28] ;src2
903 %assign i 0 ;tempv1=src2-src1
910 %assign i 0 ;tempv0=src1-src0
917 ; esi=tempv0, edi=tempv1
918 call check_vec ;make sure reasonable value
920 call check_vec ;make sure reasonable value
921 ; edi=tempv0, esi=tempv1
924 push dword [esp+16+8] ;dest
925 call _vm_vec_crossprod
934 ;compute a rotation matrix from three angles. takes edi=dest matrix,
935 ;esi=angles vector. returns dest matrix.
940 mov edi,[esp+16];dest
941 mov esi,[esp+20];angles
959 ;alternate entry point with sines & cosines already computed.
960 ;Note all the registers already pushed.
963 ;now calculate the 9 elements
967 mov ecx,eax ;save sbsh
972 mov esi,eax ;save cbch
974 mov [edi+m1],eax ;m1=cbch+sbspsh
976 mov eax,esi ;get cbch
978 add eax,ecx ;add sbsh
979 mov [edi+m8],eax ;m8=sbsh+cbchsp
984 mov ecx,eax ;save cbsh
989 mov esi,eax ;save sbch
991 mov [edi+m2],ebx ;m2=cbshsp-sbch
993 mov eax,esi ;get sbch
995 sub eax,ecx ;sub from cbsh
996 mov [edi+m7],eax ;m7=sbchsp-cbsh
1001 mov [edi+m3],eax ;m3=shcp
1005 mov [edi+m4],eax ;m4=sbcp
1009 mov [edi+m5],eax ;m5=cbcp
1013 mov [edi+m6],eax ;m6=-sp
1017 mov [edi+m9],eax ;m9=chcp
1036 ;create a rotation matrix from one or two vectors.
1037 ;requires forward vec, and assumes zero bank if up & right vecs==NULL
1038 ;up/right vector need not be exactly perpendicular to forward vec
1039 ;takes edi=matrix, esi=forward vec, eax=up vec, ebx=right vec.
1040 ;returns edi=matrix. trashes eax,ebx,esi
1041 ;Note: this routine loses precision as the forward vector approaches
1042 ;straigt up or down (I think)
1043 _vm_vector_2_matrix:
1048 mov esi,[esp+20];fvec
1049 mov eax,[esp+24];uvec
1050 mov ebx,[esp+28];rvec
1054 break_if z,"vm_vector_2_matrix: forward vec cannot be NULL!"
1057 or eax,eax ;up vector present?
1058 jnz near use_up_vec ;..yep
1060 or ebx,ebx ;right vector present?
1061 jz near just_forward_vec ;..nope
1063 push edi ;save matrix
1067 call _vm_vec_copy_normalize
1074 call _vm_vec_copy_normalize
1079 push dword xvec;src1
1080 push dword zvec;src0
1081 push dword yvec;dest
1082 call _vm_vec_crossprod ;get y = z cross x
1085 ;normalize new perpendicular vector
1086 push eax ;get new vec (up) in esi
1087 call _vm_vec_normalize
1092 ;now recompute right vector, in case it wasn't entirely perpendiclar
1094 push dword zvec;src1
1095 push dword yvec;src0
1096 push dword xvec;dest
1097 call _vm_vec_crossprod ;x = y cross z
1100 pop edi ;get matrix back
1102 jmp copy_into_matrix
1105 ;one of the non-forward vectors caused a problem, so ignore them and
1106 ;use just the forward vector
1111 jmp just_forward_vec_norm
1113 ;use forward and up vectors
1115 push edi ;save matrix
1119 call _vm_vec_copy_normalize
1126 call _vm_vec_copy_normalize
1131 push dword zvec;src1
1132 push dword yvec;src0
1133 push dword xvec;dest
1134 call _vm_vec_crossprod ;get x = y cross z
1137 ;normalize new perpendicular vector
1138 push eax ;get new vec (up) in esi
1139 call _vm_vec_normalize
1144 ;now recompute right vector, in case it wasn't entirely perpendiclar
1146 push dword xvec;src1
1147 push dword zvec;src0
1148 push dword yvec;dest
1149 call _vm_vec_crossprod ;y = z cross x
1152 pop edi ;get matrix back
1155 vm_copy edi+rvec,xvec
1156 vm_copy edi+uvec,yvec
1157 vm_copy edi+fvec,zvec
1171 debug_brk '0-len vec in vec_2_mat'
1174 ;only the forward vector is present
1179 call _vm_vec_copy_normalize
1183 just_forward_vec_norm:
1186 or eax,[esi+8] ;check both x & z == 0
1189 ;forward vector is straight up (or down)
1191 mov dword [edi+m1],f1_0
1192 mov eax,[esi+4] ;get y componant
1196 sub eax,edx ;make sign correct
1208 m2m_neg [edi+8],[esi+0]
1210 call _vm_vec_normalize
1217 call _vm_vec_crossprod
1228 ;multiply (dot) two vectors. assumes dest ptr in ebp, src pointers in esi,edi.
1229 ;trashes ebx,ecx,edx
1232 ;macro dest,x0,y0,z0,x1,y1,z1
1248 shrd ebx,ecx,16 ;fixup ebx
1253 ;rotate a vector by a rotation matrix
1254 ;eax=dest vector, esi=src vector, edi=matrix. returns eax=dest vector
1258 break_if e,'vec_rotate: dest==src'
1264 mov ebp,[esp+20];dest vec
1265 mov esi,[esp+24];src vec
1266 mov edi,[esp+28];matrix
1269 vv_mul 0, 0,4,8, m1,m4,m7
1270 vv_mul 4, 0,4,8, m2,m5,m8
1271 vv_mul 8, 0,4,8, m3,m6,m9
1273 mov eax,ebp ;return eax=dest
1281 ;transpose a matrix in place. Takes matrix. returns matrix
1282 _vm_transpose_matrix:
1301 ;copy and transpose a matrix. Takes edi=dest, esi=src. returns edi=dest
1302 _vm_copy_transpose_matrix:
1303 mov edx,[esp+4];dest
1337 ;mulitply 2 matrices, fill in dest. returns ptr to dest
1338 ;takes dest, src0, scr1
1339 _vm_matrix_x_matrix:
1342 break_if e,'matrix_x_matrix: dest==src0'
1344 break_if e,'matrix_x_matrix: dest==src1'
1350 mov ebp,[esp+20] ;ebp=dest
1351 mov esi,[esp+24] ;esi=src0
1352 mov edi,[esp+28] ;edi=src1
1354 ;;This code would do the same as the nine lines below it, but I'm sure
1355 ;;Mike would disapprove
1356 ;; for s0,<<m1,m2,m3>,<m4,m5,m6>,<m7,m8,m9>>
1357 ;; for s1,<<m1,m4,m7>,<m2,m5,m8>,<m3,m6,m9>>
1358 ;; vv_mul @ArgI(1,s0)+@ArgI(1,s1), s0, s1
1362 vv_mul m1, m1,m2,m3, m1,m4,m7
1363 vv_mul m2, m1,m2,m3, m2,m5,m8
1364 vv_mul m3, m1,m2,m3, m3,m6,m9
1366 vv_mul m4, m4,m5,m6, m1,m4,m7
1367 vv_mul m5, m4,m5,m6, m2,m5,m8
1368 vv_mul m6, m4,m5,m6, m3,m6,m9
1370 vv_mul m7, m7,m8,m9, m1,m4,m7
1371 vv_mul m8, m7,m8,m9, m2,m5,m8
1372 vv_mul m9, m7,m8,m9, m3,m6,m9
1374 mov eax,ebp ;eax=ptr to dest
1381 ;computes the delta angle between two vectors
1382 ;two entry points: normalized and non-normalized vectors
1383 ;takes esi,edi=vectors, eax=optional forward vector
1384 ;returns ax=delta angle
1385 ;if the forward vector is NULL, the absolute values of the delta angle
1386 ;is returned. If it is specified, the rotation around that vector from
1387 ;esi to edi is returned.
1397 call _vm_vec_normalize
1400 call _vm_vec_normalize
1404 _vm_vec_delta_ang_norm:
1413 call _vm_vec_dotprod
1415 call fix_acos_asm ;now angle in ax
1416 mov ebx,[esp+24] ;get forward vec
1419 ;do cross product to find sign of angle
1420 push eax ;save angle
1424 push dword tempv0 ;new vec
1425 call _vm_vec_crossprod
1427 push ebx ;forward vec
1428 push eax ;new vector
1429 call _vm_vec_dotprod ;eax=dotprod
1435 sub eax,edx ;make sign correct
1443 ;compute a rotation matrix from the forward vector and a rotation around
1444 ;that vector. takes esi=vector, ax=angle, edi=matrix. returns edi=dest matrix.
1446 _vm_vec_ang_2_matrix:
1459 ;extract heading & pitch from vector
1461 mov eax,[esi+4] ;m6=-sp
1467 call long_sqrt_asm ;eax=cp
1483 ;compute the distance from a point to a plane. takes the normalized normal
1484 ;of the plane (ebx), a point on the plane (edi), and the point to check (esi).
1485 ;returns distance in eax
1486 ;distance is signed, so negative dist is on the back of the plane
1491 call vm_vec_sub ;vecs in esi,edi
1493 mov esi,eax ;vector plane -> point
1500 ;extract the angles from a matrix. takes esi=matrix, fills in edi=angvec
1501 vm_extract_angles_matrix:
1502 pushm eax,ebx,edx,ecx
1504 ;extract heading & pitch from forward vector
1506 mov eax,[esi].fvec.z ;ch
1507 mov ebx,[esi].fvec.x ;sh
1509 mov ecx,ebx ;check for z==x==0
1511 jz zero_head ;zero, use head=0
1513 zero_head: mov [edi].head,ax ;save heading
1515 call fix_sincos_asm ;get back sh
1519 mov ecx,eax ;save abs(sine)
1522 cmp eax,ecx ;which is larger?
1523 pop eax ;get sine back
1526 ;sine is larger, so use it
1527 mov ebx,eax ;ebx=sine heading
1528 mov eax,[esi].m3 ;cp = shcp / sh
1531 ;cosine is larger, so use it
1534 mov eax,[esi].fvec.z ;get chcp
1535 get_cosp: fixdiv ebx ;cp = chcp / ch
1541 mov ebx,[esi].fvec.y ;fvec.y = -sp
1542 neg ebx ;ebx = y (sin)
1543 mov ecx,ebx ;check for z==x==0
1545 jz zero_pitch ;bogus vec, set p=0
1547 zero_pitch: mov [edi].pitch,ax
1552 mov eax,[esi].m4 ;m4 = sbcp
1554 mov ebx,eax ;save sb
1556 mov eax,[esi].m5 ;get cbcp
1558 mov ecx,ebx ;check for z==x==0
1560 jz zero_bank ;bogus vec, set n=0
1562 zero_bank: mov [edi].bank,ax
1565 popm eax,ebx,edx,ecx
1569 ;the cosine of pitch is zero. we're pitched straight up. say no bank
1570 cp_zero: mov [edi].bank,0 ;no bank
1572 popm eax,ebx,edx,ecx
1576 ;extract the angles from a vector, assuming zero bank.
1577 ;takes esi=vec, edi=angvec
1578 ;note versions for normalized and not normalized vector
1579 ;unnormalized version TRASHES ESI
1580 vm_extract_angles_vector:
1583 call vm_vec_copy_normalize ;ecx=mag
1588 vm_extract_angles_vector_normalized:
1591 mov [edi].bank,0 ;always zero bank
1596 mov [edi].pitch,ax ;p = asin(-y)
1601 jz zero_head2 ;check for up vector
1603 mov ebx,[esi].x ;get x again
1605 zero_head2: mov [edi].head,ax ;h = atan2(x,z) (or zero)