14 #include <xmmintrin.h>
20 #define SMALL_NUM 1e-7
21 #define SMALLER_NUM 1e-20
22 #define CONVERT_RADIANS 0.017453 // conversion factor from degrees to radians
30 #define UNINITIALIZED_VALUE -12345678.9f
152 m->
vec.rvec.xyz.x = 1.0f; m->
vec.rvec.xyz.y = 0.0f; m->
vec.rvec.xyz.z = 0.0f;
153 m->
vec.uvec.xyz.x = 0.0f; m->
vec.uvec.xyz.y = 1.0f; m->
vec.uvec.xyz.z = 0.0f;
154 m->
vec.fvec.xyz.x = 0.0f; m->
vec.fvec.xyz.y = 0.0f; m->
vec.fvec.xyz.z = 1.0f;
180 dest->
xyz.x += src->
xyz.x;
181 dest->
xyz.y += src->
xyz.y;
182 dest->
xyz.z += src->
xyz.z;
189 dest->
xyz.x -= src->
xyz.x;
190 dest->
xyz.y -= src->
xyz.y;
191 dest->
xyz.z -= src->
xyz.z;
198 float x = 0.0f,
y = 0.0f,
z = 0.0f;
199 float inv_n = 1.0f / (
float) n;;
201 for(
int i = 0;
i<
n;
i++){
207 dest->
xyz.x = x * inv_n;
208 dest->
xyz.y =
y * inv_n;
209 dest->
xyz.z =
z * inv_n;
219 dest->
xyz.x = (src0->
xyz.x + src1->
xyz.x) * 0.5
f;
220 dest->
xyz.y = (src0->
xyz.y + src1->
xyz.y) * 0.5
f;
221 dest->
xyz.z = (src0->
xyz.z + src1->
xyz.z) * 0.5
f;
230 dest->
xyz.x = (src0->
xyz.x + src1->
xyz.x + src2->
xyz.x) * 0.333333333
f;
231 dest->
xyz.y = (src0->
xyz.y + src1->
xyz.y + src2->
xyz.y) * 0.333333333
f;
232 dest->
xyz.z = (src0->
xyz.z + src1->
xyz.z + src2->
xyz.z) * 0.333333333
f;
240 dest->
xyz.x = (src0->
xyz.x + src1->
xyz.x + src2->
xyz.x + src3->
xyz.x) * 0.25
f;
241 dest->
xyz.y = (src0->
xyz.y + src1->
xyz.y + src2->
xyz.y + src3->
xyz.y) * 0.25
f;
242 dest->
xyz.z = (src0->
xyz.z + src1->
xyz.z + src2->
xyz.z + src3->
xyz.z) * 0.25
f;
268 dest->
xyz.x = src1->
xyz.x + src2->
xyz.x*k;
269 dest->
xyz.y = src1->
xyz.y + src2->
xyz.y*k;
270 dest->
xyz.z = src1->
xyz.z + src2->
xyz.z*k;
277 dest->
xyz.x = src1->
xyz.x - src2->
xyz.x*k;
278 dest->
xyz.y = src1->
xyz.y - src2->
xyz.y*k;
279 dest->
xyz.z = src1->
xyz.z - src2->
xyz.z*k;
286 dest->
xyz.x += src->
xyz.x*k;
287 dest->
xyz.y += src->
xyz.y*k;
288 dest->
xyz.z += src->
xyz.z*k;
295 dest->
xyz.x -= src->
xyz.x*k;
296 dest->
xyz.y -= src->
xyz.y*k;
297 dest->
xyz.z -= src->
xyz.z*k;
306 dest->
xyz.x = dest->
xyz.x* n * d;
307 dest->
xyz.y = dest->
xyz.y* n * d;
308 dest->
xyz.z = dest->
xyz.z* n * d;
321 return (x*v->
xyz.x)+(y*v->
xyz.y)+(z*v->
xyz.z);
348 dx = v0->
xyz.x - v1->
xyz.x;
349 dy = v0->
xyz.y - v1->
xyz.y;
350 dz = v0->
xyz.z - v1->
xyz.z;
351 return dx*dx + dy*dy + dz*dz;
375 if ( v->
xyz.x < 0.0 )
380 if ( v->
xyz.y < 0.0 )
385 if ( v->
xyz.z < 0.0 )
408 bc = (b * 0.25f) + (c * 0.125
f);
410 t = a + bc + (bc * 0.5f);
439 "Trace out of vecmat.cpp and find offending code.\n");
452 dest->
xyz.x = src->
xyz.x * im;
453 dest->
xyz.y = src->
xyz.y * im;
454 dest->
xyz.z = src->
xyz.z * im;
497 static float vm_vec_imag(
const vec3d *
v)
503 __m128 xx = _mm_load_ss( & x );
504 xx = _mm_rsqrt_ss( xx );
505 _mm_store_ss( & x, xx );
517 im = vm_vec_imag(src);
521 dest->
xyz.x = src->
xyz.x*im;
522 dest->
xyz.y = src->
xyz.y*im;
523 dest->
xyz.z = src->
xyz.z*im;
535 im = vm_vec_imag(src);
539 src->
xyz.x = src->
xyz.x*im;
540 src->
xyz.y = src->
xyz.y*im;
541 src->
xyz.z = src->
xyz.z*im;
560 dest->
xyz.x = src->
xyz.x * im;
561 dest->
xyz.y = src->
xyz.y * im;
562 dest->
xyz.z = src->
xyz.z * im;
647 dest->
xyz.x = (src0->
xyz.y * src1->
xyz.z) - (src0->
xyz.z * src1->
xyz.y);
648 dest->
xyz.y = (src0->
xyz.z * src1->
xyz.x) - (src0->
xyz.x * src1->
xyz.z);
649 dest->
xyz.z = (src0->
xyz.x * src1->
xyz.y) - (src0->
xyz.y * src1->
xyz.x);
723 static matrix *sincos_2_matrix(
matrix *
m,
float sinp,
float cosp,
float sinb,
float cosb,
float sinh,
float cosh)
725 float sbsh,cbch,cbsh,sbch;
733 m->
vec.rvec.xyz.x = cbch + sinp*sbsh;
734 m->
vec.uvec.xyz.z = sbsh + sinp*cbch;
736 m->
vec.uvec.xyz.x = sinp*cbsh - sbch;
737 m->
vec.rvec.xyz.z = sinp*sbch - cbsh;
739 m->
vec.fvec.xyz.x = sinh*cosp;
740 m->
vec.rvec.xyz.y = sinb*cosp;
741 m->
vec.uvec.xyz.y = cosb*cosp;
742 m->
vec.fvec.xyz.z = cosh*cosp;
744 m->
vec.fvec.xyz.y = -sinp;
755 float sinp,cosp,sinb,cosb,sinh,cosh;
757 sinp = sinf(a->
p); cosp = cosf(a->
p);
758 sinb = sinf(a->
b); cosb = cosf(a->
b);
759 sinh = sinf(a->
h); cosh = cosf(a->
h);
761 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
771 float sinp,cosp,sinb,cosb,sinh,cosh;
778 sinp = 0.0f; cosp = 1.0f;
779 sinb = 0.0f; cosb = 1.0f;
780 sinh = 0.0f; cosh = 1.0f;
782 switch (angle_index) {
784 sinp = sinf(a); cosp = cosf(a);
787 sinb = sinf(a); cosb = cosf(a);
790 sinh = sinf(a); cosh = cosf(a);
794 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
804 float sinb,cosb,sinp,cosp,sinh,cosh;
806 sinb = sinf(a); cosb = cosf(a);
811 sinh = v->
xyz.x / cosp;
812 cosh = v->
xyz.z / cosp;
814 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
826 if ((zvec->
xyz.x==0.0f) && (zvec->
xyz.z==0.0f)) {
827 m->
vec.rvec.xyz.x = 1.0f;
828 m->
vec.uvec.xyz.z = (zvec->
xyz.y<0.0f)?1.0
f:-1.0
f;
830 m->
vec.rvec.xyz.y = m->
vec.rvec.xyz.z = m->
vec.uvec.xyz.x = m->
vec.uvec.xyz.y = 0.0f;
834 xvec->
xyz.x = zvec->
xyz.z;
836 xvec->
xyz.z = -zvec->
xyz.x;
937 dest->
xyz.x = (src->
xyz.x*m->
vec.rvec.xyz.x)+(src->
xyz.y*m->
vec.rvec.xyz.y)+(src->
xyz.z*m->
vec.rvec.xyz.z);
938 dest->
xyz.y = (src->
xyz.x*m->
vec.uvec.xyz.x)+(src->
xyz.y*m->
vec.uvec.xyz.y)+(src->
xyz.z*m->
vec.uvec.xyz.z);
939 dest->
xyz.z = (src->
xyz.x*m->
vec.fvec.xyz.x)+(src->
xyz.y*m->
vec.fvec.xyz.y)+(src->
xyz.z*m->
vec.fvec.xyz.z);
963 dest->
xyz.x = (src->
xyz.x*m->
vec.rvec.xyz.x)+(src->
xyz.y*m->
vec.uvec.xyz.x)+(src->
xyz.z*m->
vec.fvec.xyz.x);
964 dest->
xyz.y = (src->
xyz.x*m->
vec.rvec.xyz.y)+(src->
xyz.y*m->
vec.uvec.xyz.y)+(src->
xyz.z*m->
vec.fvec.xyz.y);
965 dest->
xyz.z = (src->
xyz.x*m->
vec.rvec.xyz.z)+(src->
xyz.y*m->
vec.uvec.xyz.z)+(src->
xyz.z*m->
vec.fvec.xyz.z);
975 t = m->
vec.uvec.xyz.x; m->
vec.uvec.xyz.x = m->
vec.rvec.xyz.y; m->
vec.rvec.xyz.y =
t;
976 t = m->
vec.fvec.xyz.x; m->
vec.fvec.xyz.x = m->
vec.rvec.xyz.z; m->
vec.rvec.xyz.z =
t;
977 t = m->
vec.fvec.xyz.y; m->
vec.fvec.xyz.y = m->
vec.uvec.xyz.z; m->
vec.uvec.xyz.z =
t;
988 dest->
vec.rvec.xyz.x = src->
vec.rvec.xyz.x;
989 dest->
vec.rvec.xyz.y = src->
vec.uvec.xyz.x;
990 dest->
vec.rvec.xyz.z = src->
vec.fvec.xyz.x;
992 dest->
vec.uvec.xyz.x = src->
vec.rvec.xyz.y;
993 dest->
vec.uvec.xyz.y = src->
vec.uvec.xyz.y;
994 dest->
vec.uvec.xyz.z = src->
vec.fvec.xyz.y;
996 dest->
vec.fvec.xyz.x = src->
vec.rvec.xyz.z;
997 dest->
vec.fvec.xyz.y = src->
vec.uvec.xyz.z;
998 dest->
vec.fvec.xyz.z = src->
vec.fvec.xyz.z;
1008 Assert(dest!=src0 && dest!=src1);
1029 float sinh,cosh,cosp;
1033 sinh = sinf(a->
h); cosh = cosf(a->
h);
1036 cosp = m->
vec.fvec.xyz.x*sinh;
1038 cosp = m->
vec.fvec.xyz.z*cosh;
1049 sinb = m->
vec.rvec.xyz.y/cosp;
1050 cosb = m->
vec.uvec.xyz.y/cosp;
1072 float sp = -m->
vec.fvec.xyz.y;
1075 }
else if (sp >= 1.0
f) {
1083 if (fabs(sp) > 0.9999
f) {
1087 a->
h = atan2(-m->
vec.rvec.xyz.z, m->
vec.rvec.xyz.x);
1090 a->
h = atan2(m->
vec.fvec.xyz.x, m->
vec.fvec.xyz.z);
1093 a->
b = atan2(m->
vec.rvec.xyz.y, m->
vec.uvec.xyz.y);
1106 a->
p = asinf(-v->
xyz.y);
1119 vm_extract_angles_vector_normalized(a,&t);
1153 float dr, cos_theta, sin_theta, denom, cos_theta1;
1154 float Radius = 100.0f;
1164 denom =
fl_sqrt(Radius*Radius+dr*dr);
1166 cos_theta = Radius/denom;
1167 sin_theta = dr/denom;
1169 cos_theta1 = 1.0f - cos_theta;
1174 RotMat->
vec.rvec.xyz.x = cos_theta + (dydr*dydr)*cos_theta1;
1175 RotMat->
vec.uvec.xyz.x = - ((dxdr*dydr)*cos_theta1);
1176 RotMat->
vec.fvec.xyz.x = (dxdr*sin_theta);
1178 RotMat->
vec.rvec.xyz.y = RotMat->
vec.uvec.xyz.x;
1179 RotMat->
vec.uvec.xyz.y = cos_theta + ((dxdr*dxdr)*cos_theta1);
1180 RotMat->
vec.fvec.xyz.y = (dydr*sin_theta);
1182 RotMat->
vec.rvec.xyz.z = -RotMat->
vec.fvec.xyz.x;
1183 RotMat->
vec.uvec.xyz.z = -RotMat->
vec.fvec.xyz.y;
1184 RotMat->
vec.fvec.xyz.z = cos_theta;
1190 mat->
vec.rvec.xyz.x = vec->
xyz.x * vec->
xyz.x;
1191 mat->
vec.rvec.xyz.y = vec->
xyz.x * vec->
xyz.y;
1192 mat->
vec.rvec.xyz.z = vec->
xyz.x * vec->
xyz.z;
1194 mat->
vec.uvec.xyz.x = vec->
xyz.y * vec->
xyz.x;
1195 mat->
vec.uvec.xyz.y = vec->
xyz.y * vec->
xyz.y;
1196 mat->
vec.uvec.xyz.z = vec->
xyz.y * vec->
xyz.z;
1198 mat->
vec.fvec.xyz.x = vec->
xyz.z * vec->
xyz.x;
1199 mat->
vec.fvec.xyz.y = vec->
xyz.z * vec->
xyz.y;
1200 mat->
vec.fvec.xyz.z = vec->
xyz.z * vec->
xyz.z;
1211 vec3d norm, xlated_int_pnt, projected_point;
1219 *nearest_point = *int_pnt;
1226 *nearest_point = *int_pnt;
1231 vm_vec_outer_product(&mat, &norm);
1234 vm_vec_add(nearest_point, &projected_point, p0);
1259 if (!m->
vec.fvec.xyz.x && !m->
vec.fvec.xyz.z && m->
vec.fvec.xyz.y)
1288 float fmag, umag, rmag;
1298 }
else if (umag > 0.0
f) {
1299 if (!m->
vec.uvec.xyz.x && !m->
vec.uvec.xyz.y && m->
vec.uvec.xyz.z)
1312 if (!m->
vec.fvec.xyz.x && m->
vec.fvec.xyz.y && !m->
vec.fvec.xyz.z)
1340 matrix rotmat,new_orient;
1343 *orient = new_orient;
1368 normal.
xyz.x = planep->
A;
1369 normal.
xyz.y = planep->
B;
1370 normal.
xyz.z = planep->
C;
1381 rvec->
xyz.x = (
frand() - 0.5f) * 2;
1382 rvec->
xyz.y = (
frand() - 0.5f) * 2;
1383 rvec->
xyz.z = (
frand() - 0.5f) * 2;
1419 if ( diff > 0.005
f )
1428 float tmp1,tmp2,tmp3;
1434 if ( tmp1 > 0.0000005
f )
return 1;
1435 if ( tmp2 > 0.0000005
f )
return 1;
1436 if ( tmp3 > 0.0000005
f )
return 1;
1449 if ( desired_angle < 0.0
f ) desired_angle +=
PI2;
1450 if ( desired_angle >
PI2 ) desired_angle -=
PI2;
1452 delta = desired_angle - *
h;
1453 abs_delta =
fl_abs(delta);
1455 if ((force_front) && ((desired_angle >
PI) ^ (*h >
PI)) ) {
1462 if ( abs_delta >
PI ) {
1464 if ( delta > 0.0
f ) {
1465 delta = delta -
PI2;
1472 if ( delta > step_size )
1474 else if ( delta < -step_size )
1481 if ( *h >
PI2 ) *h -=
PI2;
1482 if ( *h < 0.0
f ) *h +=
PI2;
1490 if ( desired_angle < 0.0
f ) desired_angle +=
PI2;
1491 if ( desired_angle >
PI2 ) desired_angle -=
PI2;
1493 delta = desired_angle - current_angle;
1496 if ( delta > 0.0
f ) {
1497 delta = delta -
PI2;
1508 if (!m->
vec.fvec.xyz.x && !m->
vec.fvec.xyz.y && !m->
vec.fvec.xyz.z)
1510 if (!m->
vec.rvec.xyz.x && !m->
vec.rvec.xyz.y && !m->
vec.rvec.xyz.z)
1512 if (!m->
vec.uvec.xyz.x && !m->
vec.uvec.xyz.y && !m->
vec.uvec.xyz.z)
1515 if (!m->
vec.fvec.xyz.x && !m->
vec.rvec.xyz.x && !m->
vec.uvec.xyz.x)
1517 if (!m->
vec.fvec.xyz.y && !m->
vec.rvec.xyz.y && !m->
vec.uvec.xyz.y)
1519 if (!m->
vec.fvec.xyz.z && !m->
vec.rvec.xyz.z && !m->
vec.uvec.xyz.z)
1538 for (i = 0; i < 9; i++)
1539 if (m1->
a1d[i] != m2->
a1d[i])
1558 float sin_theta = sinf(theta * 0.5
f);
1560 a = (u->
xyz.x * sin_theta);
1561 b = (u->
xyz.y * sin_theta);
1562 c = (u->
xyz.z * sin_theta);
1563 s = cosf(theta * 0.5f);
1566 M->
vec.rvec.xyz.x = 1.0f - 2.0f*b*b - 2.0f*c*
c;
1567 M->
vec.rvec.xyz.y = 2.0f*a*b + 2.0f*s*
c;
1568 M->
vec.rvec.xyz.z = 2.0f*a*c - 2.0f*s*
b;
1570 M->
vec.uvec.xyz.x = 2.0f*a*b - 2.0f*s*
c;
1571 M->
vec.uvec.xyz.y = 1.0f - 2.0f*a*a - 2.0f*c*
c;
1572 M->
vec.uvec.xyz.z = 2.0f*b*c + 2.0f*s*
a;
1574 M->
vec.fvec.xyz.x = 2.0f*a*c + 2.0f*s*
b;
1575 M->
vec.fvec.xyz.y = 2.0f*b*c - 2.0f*s*
a;
1576 M->
vec.fvec.xyz.z = 1.0f - 2.0f*a*a - 2.0f*b*
b;
1586 static void rotate_z (
matrix *
m,
float theta )
1588 m->
vec.rvec.xyz.x = cosf (theta);
1589 m->
vec.rvec.xyz.y = sinf (theta);
1590 m->
vec.rvec.xyz.z = 0.0f;
1592 m->
vec.uvec.xyz.x = -m->
vec.rvec.xyz.y;
1593 m->
vec.uvec.xyz.y = m->
vec.rvec.xyz.x;
1594 m->
vec.uvec.xyz.z = 0.0f;
1596 m->
vec.fvec.xyz.x = 0.0f;
1597 m->
vec.fvec.xyz.y = 0.0f;
1598 m->
vec.fvec.xyz.z = 1.0f;
1614 float trace = m->
a2d[0][0] + m->
a2d[1][1] + m->
a2d[2][2];
1615 float cos_theta = 0.5f * (trace - 1.0f);
1617 if (cos_theta > 0.999999875
f) {
1621 }
else if (cos_theta > -0.999999875
f) {
1622 *theta = acosf(cos_theta);
1625 rot_axis->
xyz.x = (m->
vec.uvec.xyz.z - m->
vec.fvec.xyz.y);
1626 rot_axis->
xyz.y = (m->
vec.fvec.xyz.x - m->
vec.rvec.xyz.z);
1627 rot_axis->
xyz.z = (m->
vec.rvec.xyz.y - m->
vec.uvec.xyz.x);
1637 int largest_diagonal_index = 0;
1639 if (m->
a2d[1][1] > m->
a2d[0][0]) {
1640 largest_diagonal_index = 1;
1642 if (m->
a2d[2][2] > m->
a2d[largest_diagonal_index][largest_diagonal_index]) {
1643 largest_diagonal_index = 2;
1646 switch (largest_diagonal_index) {
1651 ix = 1.0f / rot_axis->
xyz.x;
1652 rot_axis->
xyz.y = m->
a2d[0][1] * ix;
1653 rot_axis->
xyz.z = m->
a2d[0][2] * ix;
1660 iy = 1.0f / rot_axis->
xyz.y;
1661 rot_axis->
xyz.x = m->
a2d[1][0] * iy;
1662 rot_axis->
xyz.z = m->
a2d[1][2] * iy;
1669 iz = 1.0f / rot_axis->
xyz.z;
1670 rot_axis->
xyz.x = m->
a2d[2][0] * iz;
1671 rot_axis->
xyz.y = m->
a2d[2][1] * iz;
1694 static float away(
float w_in,
float w_max,
float theta_goal,
float aa,
float delta_t,
float *w_out,
int no_overshoot);
1695 static float approach(
float w_in,
float w_max,
float theta_goal,
float aa,
float delta_t,
float *w_out,
int no_overshoot)
1704 delta_theta = w_in*delta_t;
1708 if (no_overshoot && (w_in*w_in > 2.0
f*1.05
f*aa*theta_goal)) {
1709 w_in =
fl_sqrt(2.0
f*aa*theta_goal);
1712 if (w_in*w_in > 2.0
f*1.05
f*aa*theta_goal) {
1713 effective_aa = 1.05f*aa;
1714 delta_theta = w_in*delta_t - 0.5f*effective_aa*delta_t*delta_t;
1716 if (delta_theta > theta_goal) {
1717 float t_goal = (-w_in +
fl_sqrt(w_in*w_in +2.0
f*effective_aa*theta_goal)) / effective_aa;
1719 Assert(t_goal < delta_t);
1720 w_in -= effective_aa*t_goal;
1721 delta_theta = w_in*t_goal + 0.5f*effective_aa*t_goal*t_goal;
1722 delta_theta -= away(-w_in, w_max, 0.0
f, aa, delta_t - t_goal, w_out, no_overshoot);
1726 if (delta_theta < 0) {
1732 *w_out = w_in - effective_aa*delta_t;
1736 }
else if (w_in*w_in < 2.0
f*0.95
f*aa*theta_goal) {
1738 float wp_sqr =
fl_abs(aa*theta_goal + 0.5
f*w_in*w_in);
1741 if (wp_sqr > w_max*w_max) {
1742 float time_to_w_max = (w_max - w_in) / aa;
1743 if (time_to_w_max < 0) {
1746 *w_out = w_in - aa*delta_t;
1751 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1753 }
else if (time_to_w_max > delta_t) {
1755 *w_out = w_in + aa*delta_t;
1756 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1762 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1766 if (wp_sqr > (w_in + aa*delta_t)*(w_in + aa*delta_t)) {
1768 *w_out = w_in + aa*delta_t;
1769 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1774 float time_to_wp = (wp - w_in) / aa;
1779 delta_theta = 0.5f*(w_in + *w_out)*time_to_wp;
1782 float time_remaining = delta_t - time_to_wp;
1783 *w_out -= aa*time_remaining;
1786 delta_theta = theta_goal;
1789 delta_theta += 0.5f*(wp + *w_out)*time_remaining;
1795 if (w_in - aa*delta_t < 0) {
1801 *w_out = w_in - aa*delta_t;
1803 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1820 static float away(
float w_in,
float w_max,
float theta_goal,
float aa,
float delta_t,
float *w_out,
int no_overshoot)
1830 if ((-w_in < 1e-5) && (theta_goal < 1e-5)) {
1837 delta_theta = w_in*delta_t;
1844 *w_out = w_in + aa * delta_t;
1845 delta_theta = (w_in + *w_out) / 2.0
f * delta_t;
1850 delta_theta = 0.5f*w_in*t0;
1851 theta_goal -= delta_theta;
1852 t_excess = delta_t - t0;
1853 delta_theta += approach(0.0
f, w_max, theta_goal, aa, t_excess, w_out, no_overshoot);
1860 matrix *next_orient,
vec3d *w_out,
const vec3d *vel_limit,
const vec3d *acc_limit,
int no_overshoot)
1880 *next_orient = *goal_orient;
1889 if (theta_goal.
xyz.x > 0) {
1890 if (w_in->
xyz.x >= 0) {
1891 delta_theta = approach(w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
1892 theta_end.
xyz.x = delta_theta;
1894 delta_theta = away(w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
1895 theta_end.
xyz.x = delta_theta;
1897 }
else if (theta_goal.
xyz.x < 0) {
1898 if (w_in->
xyz.x <= 0) {
1899 delta_theta = approach(-w_in->
xyz.x, vel_limit->
xyz.x, -theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
1900 theta_end.
xyz.x = -delta_theta;
1901 w_out->
xyz.x = -w_out->
xyz.x;
1903 delta_theta = away(-w_in->
xyz.x, vel_limit->
xyz.x, -theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
1904 theta_end.
xyz.x = -delta_theta;
1905 w_out->
xyz.x = -w_out->
xyz.x;
1908 if (w_in->
xyz.x < 0) {
1909 delta_theta = away(w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
1910 theta_end.
xyz.x = delta_theta;
1912 delta_theta = away(-w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
1913 theta_end.
xyz.x = -delta_theta;
1914 w_out->
xyz.x = -w_out->
xyz.x;
1920 if (theta_goal.
xyz.y > 0) {
1921 if (w_in->
xyz.y >= 0) {
1922 delta_theta = approach(w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
1923 theta_end.
xyz.y = delta_theta;
1925 delta_theta = away(w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
1926 theta_end.
xyz.y = delta_theta;
1928 }
else if (theta_goal.
xyz.y < 0) {
1929 if (w_in->
xyz.y <= 0) {
1930 delta_theta = approach(-w_in->
xyz.y, vel_limit->
xyz.y, -theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
1931 theta_end.
xyz.y = -delta_theta;
1932 w_out->
xyz.y = -w_out->
xyz.y;
1934 delta_theta = away(-w_in->
xyz.y, vel_limit->
xyz.y, -theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
1935 theta_end.
xyz.y = -delta_theta;
1936 w_out->
xyz.y = -w_out->
xyz.y;
1939 if (w_in->
xyz.y < 0) {
1940 delta_theta = away(w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
1941 theta_end.
xyz.y = delta_theta;
1943 delta_theta = away(-w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
1944 theta_end.
xyz.y = -delta_theta;
1945 w_out->
xyz.y = -w_out->
xyz.y;
1950 if (theta_goal.
xyz.z > 0) {
1951 if (w_in->
xyz.z >= 0) {
1952 delta_theta = approach(w_in->
xyz.z, vel_limit->
xyz.z, theta_goal.
xyz.z, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
1953 theta_end.
xyz.z = delta_theta;
1955 delta_theta = away(w_in->
xyz.z, vel_limit->
xyz.z, theta_goal.
xyz.z, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
1956 theta_end.
xyz.z = delta_theta;
1958 }
else if (theta_goal.
xyz.z < 0) {
1959 if (w_in->
xyz.z <= 0) {
1960 delta_theta = approach(-w_in->
xyz.z, vel_limit->
xyz.z, -theta_goal.
xyz.z, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
1961 theta_end.
xyz.z = -delta_theta;
1962 w_out->
xyz.z = -w_out->
xyz.z;
1964 delta_theta = away(-w_in->
xyz.z, vel_limit->
xyz.z, -theta_goal.
xyz.z, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
1965 theta_end.
xyz.z = -delta_theta;
1966 w_out->
xyz.z = -w_out->
xyz.z;
1969 if (w_in->
xyz.z < 0) {
1970 delta_theta = away(w_in->
xyz.z, vel_limit->
xyz.z, theta_goal.
xyz.z, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
1971 theta_end.
xyz.z = delta_theta;
1973 delta_theta = away(-w_in->
xyz.z, vel_limit->
xyz.z, theta_goal.
xyz.z, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
1974 theta_end.
xyz.z = -delta_theta;
1975 w_out->
xyz.z = -w_out->
xyz.z;
1982 rot_axis = theta_end;
1990 if (theta_end.
xyz.x == theta_goal.
xyz.x && theta_end.
xyz.y == theta_goal.
xyz.y && theta_end.
xyz.z == theta_goal.
xyz.z) {
1991 *next_orient = *goal_orient;
2021 angle.
xyz.x = theta * rot_axis.
xyz.x;
2022 angle.
xyz.y = theta * rot_axis.
xyz.y;
2023 angle.
xyz.z = theta * rot_axis.
xyz.z;
2026 if (time <= 1e-5
f) {
2033 acc_max->
xyz.x = 4.0f *
fl_abs(angle.
xyz.x) / (time * time);
2034 acc_max->
xyz.y = 4.0f *
fl_abs(angle.
xyz.y) / (time * time);
2035 acc_max->
xyz.z = 4.0f *
fl_abs(angle.
xyz.z) / (time * time);
2039 w_max->
xyz.x = acc_max->
xyz.x * time / 2.0f;
2040 w_max->
xyz.y = acc_max->
xyz.y * time / 2.0f;
2041 w_max->
xyz.z = acc_max->
xyz.z * time / 2.0f;
2062 matrix *next_orient,
vec3d *w_out,
const vec3d *vel_limit,
const vec3d *acc_limit,
int no_overshoot)
2065 vec3d local_rot_axis;
2087 if ( z_dotprod > 0.0
f )
2092 rot_axis = orient->
vec.rvec;
2097 if ( z_dotprod < 0.0
f )
2108 if(!(
fl_abs(theta_goal.
xyz.z) < 0.001f))
2110 mprintf((
"vm_forward_interpolate: Bad rotation\n"));
2116 if (theta_goal.
xyz.x > 0) {
2117 if (w_in->
xyz.x >= 0) {
2118 delta_theta = approach(w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
2119 theta_end.
xyz.x = delta_theta;
2121 delta_theta = away(w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
2122 theta_end.
xyz.x = delta_theta;
2124 }
else if (theta_goal.
xyz.x < 0) {
2125 if (w_in->
xyz.x <= 0) {
2126 delta_theta = approach(-w_in->
xyz.x, vel_limit->
xyz.x, -theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
2127 theta_end.
xyz.x = -delta_theta;
2128 w_out->
xyz.x = -w_out->
xyz.x;
2130 delta_theta = away(-w_in->
xyz.x, vel_limit->
xyz.x, -theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
2131 theta_end.
xyz.x = -delta_theta;
2132 w_out->
xyz.x = -w_out->
xyz.x;
2135 if (w_in->
xyz.x < 0) {
2136 delta_theta = away(w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
2137 theta_end.
xyz.x = delta_theta;
2139 delta_theta = away(-w_in->
xyz.x, vel_limit->
xyz.x, theta_goal.
xyz.x, acc_limit->
xyz.x, delta_t, &w_out->
xyz.x, no_overshoot);
2140 theta_end.
xyz.x = -delta_theta;
2141 w_out->
xyz.x = -w_out->
xyz.x;
2146 if (theta_goal.
xyz.y > 0) {
2147 if (w_in->
xyz.y >= 0) {
2148 delta_theta = approach(w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
2149 theta_end.
xyz.y = delta_theta;
2151 delta_theta = away(w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
2152 theta_end.
xyz.y = delta_theta;
2154 }
else if (theta_goal.
xyz.y < 0) {
2155 if (w_in->
xyz.y <= 0) {
2156 delta_theta = approach(-w_in->
xyz.y, vel_limit->
xyz.y, -theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
2157 theta_end.
xyz.y = -delta_theta;
2158 w_out->
xyz.y = -w_out->
xyz.y;
2160 delta_theta = away(-w_in->
xyz.y, vel_limit->
xyz.y, -theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
2161 theta_end.
xyz.y = -delta_theta;
2162 w_out->
xyz.y = -w_out->
xyz.y;
2165 if (w_in->
xyz.y < 0) {
2166 delta_theta = away(w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
2167 theta_end.
xyz.y = delta_theta;
2169 delta_theta = away(-w_in->
xyz.y, vel_limit->
xyz.y, theta_goal.
xyz.y, acc_limit->
xyz.y, delta_t, &w_out->
xyz.y, no_overshoot);
2170 theta_end.
xyz.y = -delta_theta;
2171 w_out->
xyz.y = -w_out->
xyz.y;
2176 no_bank = ( delta_bank == 0.0f && vel_limit->
xyz.z == 0.0f && acc_limit->
xyz.z == 0.0f );
2185 if (delta_bank > 0) {
2186 if (w_in->
xyz.z >= 0) {
2187 delta_theta = approach(w_in->
xyz.z, vel_limit->
xyz.z, delta_bank, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
2190 delta_theta = away(w_in->
xyz.z, vel_limit->
xyz.z, delta_bank, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
2193 }
else if (delta_bank < 0) {
2194 if (w_in->
xyz.z <= 0) {
2195 delta_theta = approach(-w_in->
xyz.z, vel_limit->
xyz.z, -delta_bank, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
2196 bank = -delta_theta;
2197 w_out->
xyz.z = -w_out->
xyz.z;
2199 delta_theta = away(-w_in->
xyz.z, vel_limit->
xyz.z, -delta_bank, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
2200 bank = -delta_theta;
2201 w_out->
xyz.z = -w_out->
xyz.z;
2204 if (w_in->
xyz.z < 0) {
2205 delta_theta = away(w_in->
xyz.z, vel_limit->
xyz.z, delta_bank, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
2208 delta_theta = away(-w_in->
xyz.z, vel_limit->
xyz.z, delta_bank, acc_limit->
xyz.z, delta_t, &w_out->
xyz.z, no_overshoot);
2209 bank = -delta_theta;
2210 w_out->
xyz.z = -w_out->
xyz.z;
2218 theta_end.
xyz.z = bank;
2219 rot_axis = theta_end;
2248 #define BIGNUMBER 100000000.0f
2252 float rad, rad_sq, xspan, yspan, zspan, maxspan;
2253 float old_to_p, old_to_p_sq, old_to_new;
2254 vec3d diff, xmin, xmax, ymin, ymax, zmin,
zmax, dia1, dia2;
2266 for ( i = 0; i < num_pnts; i++ ) {
2268 if ( p->
xyz.x < xmin.
xyz.x )
2270 if ( p->
xyz.x > xmax.
xyz.x )
2272 if ( p->
xyz.y < ymin.
xyz.y )
2274 if ( p->
xyz.y > ymax.
xyz.y )
2276 if ( p->
xyz.z < zmin.
xyz.z )
2278 if ( p->
xyz.z > zmax.
xyz.z )
2295 if ( yspan > maxspan ) {
2300 if ( zspan > maxspan ) {
2316 for ( i = 0; i < num_pnts; i++ ) {
2320 if ( old_to_p_sq > rad_sq ) {
2321 old_to_p =
fl_sqrt(old_to_p_sq);
2323 rad = (rad + old_to_p) / 2.0
f;
2325 old_to_new = old_to_p - rad;
2327 center->
xyz.x = (rad*center->
xyz.x + old_to_new*p->
xyz.x) / old_to_p;
2328 center->
xyz.y = (rad*center->
xyz.y + old_to_new*p->
xyz.y) / old_to_p;
2329 center->
xyz.z = (rad*center->
xyz.z + old_to_new*p->
xyz.z) / old_to_p;
2330 nprintf((
"Alan",
"New sphere: cen,rad = %f %f %f %f\n", center->
xyz.x, center->
xyz.y, center->
xyz.z, rad));
2454 float dif_angle = max_angle - min_angle;
2457 float temp_ang = (
frand_range(-dif_angle, dif_angle));
2458 if(temp_ang < 0)temp_ang -= (min_angle);
2459 else temp_ang += (min_angle);
2465 if(temp_ang < 0)temp_ang -= (min_angle);
2466 else temp_ang += (min_angle);
2472 if(temp_ang < 0)temp_ang -= (min_angle);
2473 else temp_ang += (min_angle);
2534 }
else if(comp > b_mag){
2594 inv.
a2d[0][0] = m->
a2d[1][1] * m->
a2d[2][2] * m->
a2d[3][3] -
2595 m->
a2d[1][1] * m->
a2d[2][3] * m->
a2d[3][2] -
2596 m->
a2d[2][1] * m->
a2d[1][2] * m->
a2d[3][3] +
2597 m->
a2d[2][1] * m->
a2d[1][3] * m->
a2d[3][2] +
2598 m->
a2d[3][1] * m->
a2d[1][2] * m->
a2d[2][3] -
2599 m->
a2d[3][1] * m->
a2d[1][3] * m->
a2d[2][2];
2601 inv.
a2d[1][0] = -m->
a2d[1][0] * m->
a2d[2][2] * m->
a2d[3][3] +
2602 m->
a2d[1][0] * m->
a2d[2][3] * m->
a2d[3][2] +
2603 m->
a2d[2][0] * m->
a2d[1][2] * m->
a2d[3][3] -
2604 m->
a2d[2][0] * m->
a2d[1][3] * m->
a2d[3][2] -
2605 m->
a2d[3][0] * m->
a2d[1][2] * m->
a2d[2][3] +
2606 m->
a2d[3][0] * m->
a2d[1][3] * m->
a2d[2][2];
2608 inv.
a2d[2][0] = m->
a2d[1][0] * m->
a2d[2][1] * m->
a2d[3][3] -
2609 m->
a2d[1][0] * m->
a2d[2][3] * m->
a2d[3][1] -
2610 m->
a2d[2][0] * m->
a2d[1][1] * m->
a2d[3][3] +
2611 m->
a2d[2][0] * m->
a2d[1][3] * m->
a2d[3][1] +
2612 m->
a2d[3][0] * m->
a2d[1][1] * m->
a2d[2][3] -
2613 m->
a2d[3][0] * m->
a2d[1][3] * m->
a2d[2][1];
2615 inv.
a2d[3][0] = -m->
a2d[1][0] * m->
a2d[2][1] * m->
a2d[3][2] +
2616 m->
a2d[1][0] * m->
a2d[2][2] * m->
a2d[3][1] +
2617 m->
a2d[2][0] * m->
a2d[1][1] * m->
a2d[3][2] -
2618 m->
a2d[2][0] * m->
a2d[1][2] * m->
a2d[3][1] -
2619 m->
a2d[3][0] * m->
a2d[1][1] * m->
a2d[2][2] +
2620 m->
a2d[3][0] * m->
a2d[1][2] * m->
a2d[2][1];
2622 inv.
a2d[0][1] = -m->
a2d[0][1] * m->
a2d[2][2] * m->
a2d[3][3] +
2623 m->
a2d[0][1] * m->
a2d[2][3] * m->
a2d[3][2] +
2624 m->
a2d[2][1] * m->
a2d[0][2] * m->
a2d[3][3] -
2625 m->
a2d[2][1] * m->
a2d[0][3] * m->
a2d[3][2] -
2626 m->
a2d[3][1] * m->
a2d[0][2] * m->
a2d[2][3] +
2627 m->
a2d[3][1] * m->
a2d[0][3] * m->
a2d[2][2];
2629 inv.
a2d[1][1] = m->
a2d[0][0] * m->
a2d[2][2] * m->
a2d[3][3] -
2630 m->
a2d[0][0] * m->
a2d[2][3] * m->
a2d[3][2] -
2631 m->
a2d[2][0] * m->
a2d[0][2] * m->
a2d[3][3] +
2632 m->
a2d[2][0] * m->
a2d[0][3] * m->
a2d[3][2] +
2633 m->
a2d[3][0] * m->
a2d[0][2] * m->
a2d[2][3] -
2634 m->
a2d[3][0] * m->
a2d[0][3] * m->
a2d[2][2];
2636 inv.
a2d[2][1] = -m->
a2d[0][0] * m->
a2d[2][1] * m->
a2d[3][3] +
2637 m->
a2d[0][0] * m->
a2d[2][3] * m->
a2d[3][1] +
2638 m->
a2d[2][0] * m->
a2d[0][1] * m->
a2d[3][3] -
2639 m->
a2d[2][0] * m->
a2d[0][3] * m->
a2d[3][1] -
2640 m->
a2d[3][0] * m->
a2d[0][1] * m->
a2d[2][3] +
2641 m->
a2d[3][0] * m->
a2d[0][3] * m->
a2d[2][1];
2643 inv.
a2d[3][1] = m->
a2d[0][0] * m->
a2d[2][1] * m->
a2d[3][2] -
2644 m->
a2d[0][0] * m->
a2d[2][2] * m->
a2d[3][1] -
2645 m->
a2d[2][0] * m->
a2d[0][1] * m->
a2d[3][2] +
2646 m->
a2d[2][0] * m->
a2d[0][2] * m->
a2d[3][1] +
2647 m->
a2d[3][0] * m->
a2d[0][1] * m->
a2d[2][2] -
2648 m->
a2d[3][0] * m->
a2d[0][2] * m->
a2d[2][1];
2650 inv.
a2d[0][2] = m->
a2d[0][1] * m->
a2d[1][2] * m->
a2d[3][3] -
2651 m->
a2d[0][1] * m->
a2d[1][3] * m->
a2d[3][2] -
2652 m->
a2d[1][1] * m->
a2d[0][2] * m->
a2d[3][3] +
2653 m->
a2d[1][1] * m->
a2d[0][3] * m->
a2d[3][2] +
2654 m->
a2d[3][1] * m->
a2d[0][2] * m->
a2d[1][3] -
2655 m->
a2d[3][1] * m->
a2d[0][3] * m->
a2d[1][2];
2657 inv.
a2d[1][2] = -m->
a2d[0][0] * m->
a2d[1][2] * m->
a2d[3][3] +
2658 m->
a2d[0][0] * m->
a2d[1][3] * m->
a2d[3][2] +
2659 m->
a2d[1][0] * m->
a2d[0][2] * m->
a2d[3][3] -
2660 m->
a2d[1][0] * m->
a2d[0][3] * m->
a2d[3][2] -
2661 m->
a2d[3][0] * m->
a2d[0][2] * m->
a2d[1][3] +
2662 m->
a2d[3][0] * m->
a2d[0][3] * m->
a2d[1][2];
2664 inv.
a2d[2][2] = m->
a2d[0][0] * m->
a2d[1][1] * m->
a2d[3][3] -
2665 m->
a2d[0][0] * m->
a2d[1][3] * m->
a2d[3][1] -
2666 m->
a2d[1][0] * m->
a2d[0][1] * m->
a2d[3][3] +
2667 m->
a2d[1][0] * m->
a2d[0][3] * m->
a2d[3][1] +
2668 m->
a2d[3][0] * m->
a2d[0][1] * m->
a2d[1][3] -
2669 m->
a2d[3][0] * m->
a2d[0][3] * m->
a2d[1][1];
2671 inv.
a2d[3][2] = -m->
a2d[0][0] * m->
a2d[1][1] * m->
a2d[3][2] +
2672 m->
a2d[0][0] * m->
a2d[1][2] * m->
a2d[3][1] +
2673 m->
a2d[1][0] * m->
a2d[0][1] * m->
a2d[3][2] -
2674 m->
a2d[1][0] * m->
a2d[0][2] * m->
a2d[3][1] -
2675 m->
a2d[3][0] * m->
a2d[0][1] * m->
a2d[1][2] +
2676 m->
a2d[3][0] * m->
a2d[0][2] * m->
a2d[1][1];
2678 inv.
a2d[0][3] = -m->
a2d[0][1] * m->
a2d[1][2] * m->
a2d[2][3] +
2679 m->
a2d[0][1] * m->
a2d[1][3] * m->
a2d[2][2] +
2680 m->
a2d[1][1] * m->
a2d[0][2] * m->
a2d[2][3] -
2681 m->
a2d[1][1] * m->
a2d[0][3] * m->
a2d[2][2] -
2682 m->
a2d[2][1] * m->
a2d[0][2] * m->
a2d[1][3] +
2683 m->
a2d[2][1] * m->
a2d[0][3] * m->
a2d[1][2];
2685 inv.
a2d[1][3] = m->
a2d[0][0] * m->
a2d[1][2] * m->
a2d[2][3] -
2686 m->
a2d[0][0] * m->
a2d[1][3] * m->
a2d[2][2] -
2687 m->
a2d[1][0] * m->
a2d[0][2] * m->
a2d[2][3] +
2688 m->
a2d[1][0] * m->
a2d[0][3] * m->
a2d[2][2] +
2689 m->
a2d[2][0] * m->
a2d[0][2] * m->
a2d[1][3] -
2690 m->
a2d[2][0] * m->
a2d[0][3] * m->
a2d[1][2];
2692 inv.
a2d[2][3] = -m->
a2d[0][0] * m->
a2d[1][1] * m->
a2d[2][3] +
2693 m->
a2d[0][0] * m->
a2d[1][3] * m->
a2d[2][1] +
2694 m->
a2d[1][0] * m->
a2d[0][1] * m->
a2d[2][3] -
2695 m->
a2d[1][0] * m->
a2d[0][3] * m->
a2d[2][1] -
2696 m->
a2d[2][0] * m->
a2d[0][1] * m->
a2d[1][3] +
2697 m->
a2d[2][0] * m->
a2d[0][3] * m->
a2d[1][1];
2699 inv.
a2d[3][3] = m->
a2d[0][0] * m->
a2d[1][1] * m->
a2d[2][2] -
2700 m->
a2d[0][0] * m->
a2d[1][2] * m->
a2d[2][1] -
2701 m->
a2d[1][0] * m->
a2d[0][1] * m->
a2d[2][2] +
2702 m->
a2d[1][0] * m->
a2d[0][2] * m->
a2d[2][1] +
2703 m->
a2d[2][0] * m->
a2d[0][1] * m->
a2d[1][2] -
2704 m->
a2d[2][0] * m->
a2d[0][2] * m->
a2d[1][1];
2706 det = m->
a2d[0][0] * inv.
a2d[0][0] + m->
a2d[0][1] * inv.
a2d[1][0] + m->
a2d[0][2] * inv.
a2d[2][0] + m->
a2d[0][3] * inv.
a2d[3][0];
2715 for (i = 0; i < 4; i++) {
2716 for (j = 0; j < 4; j++) {
2717 invOut->
a2d[
i][j] = inv.
a2d[
i][j] * det;
float vm_vec_dot_to_point(const vec3d *dir, const vec3d *p1, const vec3d *p2)
float vm_vec_copy_normalize_quick_mag(vec3d *dest, const vec3d *src)
void vm_vec_projection_onto_plane(vec3d *projection, const vec3d *src, const vec3d *unit_normal)
void vm_matrix_to_rot_axis_and_angle(const matrix *m, float *theta, vec3d *rot_axis)
int is_valid_matrix(const matrix *m)
float vm_vec_mag_quick(const vec3d *v)
matrix * vm_matrix_x_matrix(matrix *dest, const matrix *src0, const matrix *src1)
GLfloat GLfloat GLfloat GLfloat h
void vm_vec_scale_add(vec3d *dest, const vec3d *src1, const vec3d *src2, float k)
float frand_range(float min, float max)
Return a floating point number in the range min..max.
bool vm_vec_equal(const vec4 &self, const vec4 &other)
float vm_vec_mag(const vec3d *v)
matrix * vm_angles_2_matrix(matrix *m, const angles *a)
void vm_rotate_matrix_by_angles(matrix *orient, const angles *tangles)
matrix * vm_vector_2_matrix_norm(matrix *m, const vec3d *fvec, const vec3d *uvec, const vec3d *rvec)
vec3d * vm_rotate_vec_to_world(vec3d *world_vec, const vec3d *body_vec, const matrix *orient)
float vm_vec_normalize_quick(vec3d *src)
void _cdecl void void _cdecl void _cdecl Warning(char *filename, int line, SCP_FORMAT_STRING const char *format,...) SCP_FORMAT_STRING_ARGS(3
int vm_matrix_same(matrix *m1, matrix *m2)
matrix * vm_vec_ang_2_matrix(matrix *m, const vec3d *v, float a)
struct vec3d::@225::@227 xyz
vec3d * vm_vec_avg_n(vec3d *dest, int n, const vec3d src[])
void vm_vec_random_in_sphere(vec3d *out, const vec3d *in, const matrix *orient, float radius, int on_edge)
matrix * vm_copy_transpose(matrix *dest, const matrix *src)
vec3d * vm_vec_rotate(vec3d *dest, const vec3d *src, const matrix *m)
float vm_interp_angle(float *h, float desired_angle, float step_size, bool force_front)
void vm_vec_random_cone(vec3d *out, const vec3d *in, float max_angle, const matrix *orient)
vec3d * vm_rotate_vec_to_body(vec3d *body_vec, const vec3d *world_vec, const matrix *orient)
float vm_vec_dot3(float x, float y, float z, const vec3d *v)
void vm_vec_scale_add2(vec3d *dest, const vec3d *src, float k)
GLenum GLenum GLenum GLenum GLenum scale
GLfloat GLfloat GLfloat v2
void vm_vec_dist_squared_to_line(const vec3d *p, const vec3d *l0, const vec3d *l1, vec3d *nearest, float *dist_squared)
bool fl_equal(float a, float b)
float vm_vec_mag_squared(const vec3d *v)
#define IS_VEC_NULL_SQ_SAFE(v)
void vm_fix_matrix(matrix *m)
float atan2_safe(float y, float x)
int vm_matrix_cmp(const matrix *a, const matrix *b)
void get_camera_limits(const matrix *start_camera, const matrix *end_camera, float time, vec3d *acc_max, vec3d *w_max)
void vm_estimate_next_orientation(const matrix *last_orient, const matrix *current_orient, matrix *next_orient)
matrix * vm_transpose(matrix *m)
bool vm_inverse_matrix4(const matrix4 *m, matrix4 *invOut)
Attempts to invert a 4x4 matrix.
matrix * vm_vector_2_matrix(matrix *m, const vec3d *fvec, const vec3d *uvec, const vec3d *rvec)
void vm_vec_add2(vec3d *dest, const vec3d *src)
GLdouble GLdouble GLdouble r
struct matrix::@228::@230 vec
void vm_vec_scale(vec3d *dest, float s)
matrix * vm_angle_2_matrix(matrix *m, float a, int angle_index)
void vm_find_bounding_sphere(const vec3d *pnts, int num_pnts, vec3d *center, float *radius)
GLboolean GLboolean GLboolean GLboolean a
int vm_test_parallel(const vec3d *src0, const vec3d *src1)
int vm_vec_dist_to_line(const vec3d *p, const vec3d *l0, const vec3d *l1, vec3d *nearest, float *dist)
void vm_orthogonalize_matrix(matrix *m_src)
void vm_vec_sub2(vec3d *dest, const vec3d *src)
void vm_set_identity(matrix *m)
void vm_rot_point_around_line(vec3d *out, const vec3d *in, float angle, const vec3d *line_point, const vec3d *line_dir)
void vm_vector_2_matrix_gen_vectors(matrix *m)
float vm_vec_normalized_dir(vec3d *dest, const vec3d *end, const vec3d *start)
float vm_vec_dist(const vec3d *v0, const vec3d *v1)
angles * vm_extract_angles_matrix(angles *a, const matrix *m)
float vm_dist_to_plane(const vec3d *checkp, const vec3d *norm, const vec3d *planep)
float vm_vec_normalize_safe(vec3d *v)
float vm_delta_from_interp_angle(float current_angle, float desired_angle)
vec3d * vm_vec_unrotate(vec3d *dest, const vec3d *src, const matrix *m)
void vm_vec_scale_sub(vec3d *dest, const vec3d *src1, const vec3d *src2, float k)
float vm_vec_dist_squared(const vec3d *v0, const vec3d *v1)
GLint GLint GLint GLint GLint x
float vm_vec_copy_normalize_quick(vec3d *dest, const vec3d *src)
void vm_vec_interp_constant(vec3d *out, const vec3d *v0, const vec3d *v1, float t)
#define vm_vec_make(v, _x, _y, _z)
void vm_trackball(int idx, int idy, matrix *RotMat)
float vm_vec_normalize_quick_mag(vec3d *v)
vec3d * vm_vec_avg3(vec3d *dest, const vec3d *src0, const vec3d *src1, const vec3d *src2)
void vm_vec_copy_scale(vec3d *dest, const vec3d *src, float s)
void vm_vec_random_in_circle(vec3d *out, const vec3d *in, const matrix *orient, float radius, int on_edge)
void vm_vec_rand_vec_quick(vec3d *rvec)
float vm_vec_projection_parallel(vec3d *component, const vec3d *src, const vec3d *unit_vec)
int is_valid_vec(const vec3d *vec)
void vm_vec_sub(vec3d *dest, const vec3d *src0, const vec3d *src1)
void vm_vec_scale2(vec3d *dest, float n, float d)
angles * vm_extract_angles_vector(angles *a, const vec3d *v)
GLboolean GLboolean GLboolean b
float vm_vec_normalized_dir_quick(vec3d *dest, const vec3d *end, const vec3d *start)
vec3d * vm_vec_avg4(vec3d *dest, const vec3d *src0, const vec3d *src1, const vec3d *src2, const vec3d *src3)
typedef float(SCP_EXT_CALLCONV *SCPTRACKIR_PFFLOATVOID)()
GLdouble GLdouble GLdouble GLdouble q
int vm_vec_same(const vec3d *v1, const vec3d *v2)
vec3d * vm_vec_normal(vec3d *dest, const vec3d *p0, const vec3d *p1, const vec3d *p2)
float vm_vec_delta_ang(const vec3d *v0, const vec3d *v1, const vec3d *fvec)
float vm_vec_dist_quick(const vec3d *v0, const vec3d *v1)
float frand()
Return random value in range 0.0..1.0- (1.0- means the closest number less than 1.0)
void vm_matrix_interpolate(const matrix *goal_orient, const matrix *curr_orient, const vec3d *w_in, float delta_t, matrix *next_orient, vec3d *w_out, const vec3d *vel_limit, const vec3d *acc_limit, int no_overshoot)
void vm_vec_boxscale(vec2d *vec, float scale)
bool vm_matrix_equal(const matrix &self, const matrix &other)
float vm_vec_copy_normalize(vec3d *dest, const vec3d *src)
vec3d * vm_vec_avg(vec3d *dest, const vec3d *src0, const vec3d *src1)
void vm_project_point_onto_plane(vec3d *new_point, const vec3d *point, const vec3d *plane_normal, const vec3d *plane_point)
int vm_check_matrix_for_zeros(const matrix *m)
int vm_vec_cmp(const vec3d *a, const vec3d *b)
void vm_quaternion_rotate(matrix *M, float theta, const vec3d *u)
float vm_vec_dot(const vec3d *v0, const vec3d *v1)
vec3d * vm_vec_perp(vec3d *dest, const vec3d *p0, const vec3d *p1, const vec3d *p2)
void vm_vec_scale_sub2(vec3d *dest, const vec3d *src, float k)
void compute_point_on_plane(vec3d *q, const plane *planep, const vec3d *p)
vec3d * vm_vec_cross(vec3d *dest, const vec3d *src0, const vec3d *src1)
struct matrix4::@231::@233 vec
float vm_vec_normalized_dir_quick_mag(vec3d *dest, const vec3d *end, const vec3d *start)
float find_nearest_point_on_line(vec3d *nearest_point, const vec3d *p0, const vec3d *p1, const vec3d *int_pnt)
void vm_vec_add(vec3d *dest, const vec3d *src0, const vec3d *src1)
angles * vm_extract_angles_matrix_alternate(angles *a, const matrix *m)
matrix vmd_identity_matrix
void vm_forward_interpolate(const vec3d *goal_f, const matrix *orient, const vec3d *w_in, float delta_t, float delta_bank, matrix *next_orient, vec3d *w_out, const vec3d *vel_limit, const vec3d *acc_limit, int no_overshoot)
float vm_vec_delta_ang_norm(const vec3d *v0, const vec3d *v1, const vec3d *fvec)
float vm_vec_normalize(vec3d *v)