mirror of
https://github.com/Cesarus85/ringfall-mjolnir-vr.git
synced 2026-10-17 23:00:26 +02:00
Jonas Volman
f3aadeab98
Integrate coherent B3, P2/K17 prerequisites, completed K1-K16 + S, and the independent profile/effects rulings. Five gains and two explicitly approved B3 debits: net +3 strict functions, +2770 meaningful/+2800 padded bytes (+2774 progress credit). No additional losses; Halo objects 394 to 396 and data unchanged. Clean rebuild, identical 8252-row stable sweep, 68 valid parks, admission/provider checks, 467-unit warning comparison, unchanged fake scan, and 1318 tests passed. Preserve canonical helper recoveries and unrelated work. Full evidence and disclosures in docs/object_matching_logs/astra_five_function_reconciliation_20260928.md.
3202 lines
68 KiB
C
3202 lines
68 KiB
C
/*
|
|
REAL_MATH.C
|
|
*/
|
|
|
|
/* ---------- headers */
|
|
|
|
#include "cseries.h"
|
|
#include "real_math.h"
|
|
#include "periodic_functions.h"
|
|
|
|
/* ---------- constants */
|
|
|
|
/* ---------- macros */
|
|
|
|
/* ---------- structures */
|
|
|
|
/* ---------- prototypes */
|
|
|
|
static __inline real magnitude_squared3d_right_associated(
|
|
real_vector3d const *vector)
|
|
{
|
|
return
|
|
vector->i * vector->i +
|
|
(vector->j * vector->j + vector->k * vector->k);
|
|
}
|
|
|
|
/* ---------- globals */
|
|
|
|
static real_matrix4x3 const private_identity4x3 =
|
|
{
|
|
1.f,
|
|
{
|
|
{ 1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f }
|
|
},
|
|
};
|
|
|
|
static real_matrix4x3 const private_negative_identity4x3 =
|
|
{
|
|
1.f,
|
|
{
|
|
{ -1.f, 0.f, 0.f, 0.f, -1.f, 0.f, 0.f, 0.f, -1.f, 0.f, 0.f, 0.f }
|
|
},
|
|
};
|
|
|
|
static real_quaternion const private_identity_quaternion =
|
|
{
|
|
{ { 0.f, 0.f, 0.f } },
|
|
1.f,
|
|
};
|
|
|
|
static real_rectangle3d const private_null_rectangle =
|
|
{
|
|
{ REAL_MAX, REAL_MIN, REAL_MAX, REAL_MIN, REAL_MAX, REAL_MIN },
|
|
};
|
|
|
|
short const global_projection3d_mappings[3][2][2] =
|
|
{
|
|
{
|
|
{ 2, 1 },
|
|
{ 1, 2 },
|
|
},
|
|
{
|
|
{ 0, 2 },
|
|
{ 2, 0 },
|
|
},
|
|
{
|
|
{ 1, 0 },
|
|
{ 0, 1 },
|
|
}
|
|
};
|
|
|
|
const real_point2d *global_origin2d = (const real_point2d *)&private_identity4x3.up;
|
|
const real_point2d *global_x_axis2d = (const real_point2d *)&private_identity4x3.forward;
|
|
const real_point2d *global_y_axis2d = (const real_point2d *)&private_identity4x3.left;
|
|
const real_point2d *global_negative_x_axis2d = (const real_point2d *)&private_negative_identity4x3.forward;
|
|
const real_point2d *global_negative_y_axis2d = (const real_point2d *)&private_negative_identity4x3.left;
|
|
|
|
const real_vector2d *global_zero_vector2d = (const real_vector2d *)&private_identity4x3.up;
|
|
const real_vector2d *global_forward2d = (const real_vector2d *)&private_identity4x3.forward;
|
|
const real_vector2d *global_left2d = (const real_vector2d *)&private_identity4x3.left;
|
|
const real_vector2d *global_backward2d = (const real_vector2d *)&private_negative_identity4x3.forward;
|
|
const real_vector2d *global_right2d = (const real_vector2d *)&private_negative_identity4x3.left;
|
|
|
|
const real_point3d *global_origin3d = (const real_point3d *)&private_identity4x3.position;
|
|
|
|
const real_point3d *global_x_axis3d = (const real_point3d *)&private_identity4x3.forward;
|
|
const real_point3d *global_y_axis3d = (const real_point3d *)&private_identity4x3.left;
|
|
const real_point3d *global_z_axis3d = (const real_point3d *)&private_identity4x3.up;
|
|
const real_point3d *global_negative_x_axis3d = (const real_point3d *)&private_negative_identity4x3.forward;
|
|
const real_point3d *global_negative_y_axis3d = (const real_point3d *)&private_negative_identity4x3.left;
|
|
const real_point3d *global_negative_z_axis3d = (const real_point3d *)&private_negative_identity4x3.up;
|
|
|
|
const real_vector3d *global_zero_vector3d = (const real_vector3d *)&private_identity4x3.position;
|
|
|
|
const real_vector3d *global_forward3d = (const real_vector3d *)&private_identity4x3.forward;
|
|
const real_vector3d *global_left3d = (const real_vector3d *)&private_identity4x3.left;
|
|
const real_vector3d *global_up3d = (const real_vector3d *)&private_identity4x3.up;
|
|
|
|
const real_vector3d *global_backward3d = (const real_vector3d *)&private_negative_identity4x3.forward;
|
|
const real_vector3d *global_right3d = (const real_vector3d *)&private_negative_identity4x3.left;
|
|
const real_vector3d *global_down3d = (const real_vector3d *)&private_negative_identity4x3.up;
|
|
|
|
const real_euler_angles2d *global_zero_angles2d = (const real_euler_angles2d *)&private_identity4x3.up;
|
|
const real_euler_angles3d *global_zero_angles3d = (const real_euler_angles3d *)&private_identity4x3.up;
|
|
const real_quaternion *global_identity_quaternion = &private_identity_quaternion;
|
|
const real_matrix4x3 *global_identity4x3 = &private_identity4x3;
|
|
const real_matrix4x3 *global_negative_identity4x3 = &private_negative_identity4x3;
|
|
const real_rectangle2d *global_null_rectangle2d = (const real_rectangle2d *)&private_null_rectangle;
|
|
const real_rectangle3d *global_null_rectangle3d = &private_null_rectangle;
|
|
|
|
/* ---------- public code */
|
|
|
|
void real_math_initialize(
|
|
void)
|
|
{
|
|
random_math_initialize();
|
|
periodic_functions_initialize();
|
|
return;
|
|
}
|
|
|
|
void real_math_dispose(
|
|
void)
|
|
{
|
|
random_math_dispose();
|
|
periodic_functions_dispose();
|
|
return;
|
|
}
|
|
|
|
void real_math_reset_precision(
|
|
void)
|
|
{
|
|
_control87(CW_DEFAULT, 0xFFFFF);
|
|
return;
|
|
}
|
|
|
|
real_vector2d *perpendicular2d(
|
|
real_vector2d const *a,
|
|
real_vector2d *result)
|
|
{
|
|
result->i = -a->j;
|
|
result->j = a->i;
|
|
return result;
|
|
}
|
|
|
|
real_vector3d *perpendicular3d(
|
|
real_vector3d const *a,
|
|
real_vector3d *result)
|
|
{
|
|
real i = fabs(a->i);
|
|
real j = fabs(a->j);
|
|
real k = fabs(a->k);
|
|
|
|
if (i <= j && i <= k)
|
|
{
|
|
result->i = 0.f;
|
|
result->j = a->k;
|
|
result->k = -a->j;
|
|
}
|
|
else
|
|
{
|
|
if (j <= k)
|
|
{
|
|
result->i = -a->k;
|
|
result->j = 0.f;
|
|
result->k = a->i;
|
|
}
|
|
else
|
|
{
|
|
result->i = a->j;
|
|
result->j = -a->i;
|
|
result->k = 0.f;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
real_vector4d *perpendicular4d(
|
|
real_vector4d const *a,
|
|
real_vector4d *result)
|
|
{
|
|
result->i = a->k;
|
|
result->j = a->l;
|
|
result->k = -a->i;
|
|
result->l = -a->j;
|
|
return result;
|
|
}
|
|
|
|
real_vector3d *rotate_vector_about_axis(
|
|
real_vector3d *v,
|
|
real_vector3d const *n,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real one_minus_cosine_times_v_dot_n = dot_product3d(n, v) * (1-cosine);
|
|
|
|
real v_cross_n_i = n->k*v->j - v->k*n->j;
|
|
real v_cross_n_j = n->i*v->k - v->i*n->k;
|
|
real v_cross_n_k = n->j*v->i - v->j*n->i;
|
|
|
|
v->i = cosine*v->i + one_minus_cosine_times_v_dot_n*n->i - sine*v_cross_n_i;
|
|
v->j = cosine*v->j + one_minus_cosine_times_v_dot_n*n->j - sine*v_cross_n_j;
|
|
v->k = cosine*v->k + one_minus_cosine_times_v_dot_n*n->k - sine*v_cross_n_k;
|
|
|
|
return v;
|
|
}
|
|
|
|
void vectors_interpolate(
|
|
real_vector3d const *a,
|
|
real_vector3d const *b,
|
|
real t,
|
|
real_vector3d *result)
|
|
{
|
|
real one_minus_t = 1-t;
|
|
result->i = one_minus_t*a->i + t*b->i;
|
|
result->j = one_minus_t*a->j + t*b->j;
|
|
result->k = one_minus_t*a->k + t*b->k;
|
|
return;
|
|
}
|
|
|
|
void points_interpolate(
|
|
real_point3d const *a,
|
|
real_point3d const *b,
|
|
real t,
|
|
real_point3d *result)
|
|
{
|
|
real one_minus_t = 1-t;
|
|
result->x = one_minus_t*a->x + t*b->x;
|
|
result->y = one_minus_t*a->y + t*b->y;
|
|
result->z = one_minus_t*a->z + t*b->z;
|
|
return;
|
|
}
|
|
|
|
void scalars_interpolate(
|
|
real a,
|
|
real b,
|
|
real t,
|
|
real *result)
|
|
{
|
|
*result = (1-t) * a+b * t;
|
|
return;
|
|
}
|
|
|
|
void scalars_interpolate_and_clamp_0_to_1(
|
|
real a,
|
|
real b,
|
|
real t,
|
|
real *result)
|
|
{
|
|
*result = PIN((1.f-t)*a + t*b, 0.f, 1.f);
|
|
return;
|
|
}
|
|
|
|
void component_vectors_from_normal3d(
|
|
real_vector3d const *vector,
|
|
real_vector3d const *normal,
|
|
real_vector3d *parallel,
|
|
real_vector3d *perpendicular)
|
|
{
|
|
real_vector3d storage;
|
|
real vn = dot_product3d(vector, normal);
|
|
|
|
if (parallel== NULL)
|
|
{
|
|
parallel = &storage;
|
|
}
|
|
|
|
scale_vector3d(normal, vn, parallel);
|
|
|
|
if (perpendicular!= NULL)
|
|
{
|
|
subtract_vectors3d(vector, parallel, perpendicular);
|
|
}
|
|
return;
|
|
}
|
|
|
|
void component_vectors_from_direction3d(
|
|
real_vector3d const *vector,
|
|
real_vector3d const *direction,
|
|
real_vector3d *parallel,
|
|
real_vector3d *perpendicular)
|
|
{
|
|
real dd = dot_product3d(direction, direction);
|
|
if (dd != 0.f)
|
|
{
|
|
real vd_dd = dot_product3d(vector, direction) / dd;
|
|
scale_vector3d(direction, vd_dd, parallel);
|
|
subtract_vectors3d(vector, parallel, perpendicular);
|
|
}
|
|
else
|
|
{
|
|
set_real_vector3d(parallel, 0.f, 0.f, 0.f);
|
|
*perpendicular = *vector;
|
|
}
|
|
return;
|
|
}
|
|
|
|
void quaternions_multiply(
|
|
real_quaternion const *q0,
|
|
real_quaternion const *q1,
|
|
real_quaternion *result)
|
|
{
|
|
real_quaternion temporary;
|
|
|
|
if (q0== result)
|
|
{
|
|
temporary = *q0;
|
|
q0= &temporary;
|
|
}
|
|
|
|
if (q1== result)
|
|
{
|
|
temporary = *q1;
|
|
q1= &temporary;
|
|
}
|
|
|
|
result->v.i =
|
|
+ q0->v.j*q1->v.k
|
|
+ q0->v.i*q1->w
|
|
+ q0->w *q1->v.i
|
|
- q0->v.k*q1->v.j
|
|
;
|
|
|
|
result->v.j =
|
|
+ q0->w *q1->v.j
|
|
+ q0->v.j*q1->w
|
|
+ q0->v.k*q1->v.i
|
|
- q0->v.i*q1->v.k
|
|
;
|
|
|
|
result->v.k =
|
|
+ q0->v.k*q1->w
|
|
+ q0->v.i*q1->v.j
|
|
+ q0->w *q1->v.k
|
|
- q0->v.j*q1->v.i
|
|
;
|
|
|
|
result->w =
|
|
+ q0->w *q1->w
|
|
- q0->v.i*q1->v.i
|
|
- q0->v.j*q1->v.j
|
|
- q0->v.k*q1->v.k
|
|
;
|
|
|
|
return;
|
|
}
|
|
|
|
void quaternions_interpolate(
|
|
real_quaternion const *q0,
|
|
real_quaternion const *q1,
|
|
real t,
|
|
real_quaternion *result)
|
|
{
|
|
real v = 1.f-t;
|
|
|
|
if (q0->v.i*q1->v.i + q0->v.j*q1->v.j + q0->v.k*q1->v.k + q0->w*q1->w < 0.f)
|
|
{
|
|
t = -t;
|
|
}
|
|
|
|
result->v.i = q0->v.i*v + q1->v.i*t;
|
|
result->v.j = q0->v.j*v + q1->v.j*t;
|
|
result->v.k = q0->v.k*v + q1->v.k*t;
|
|
result->w = q0->w*v + q1->w*t;
|
|
return;
|
|
}
|
|
|
|
void quaternion_transform_point(
|
|
real_quaternion const *q,
|
|
real_point3d const *p,
|
|
real_point3d *result)
|
|
{
|
|
|
|
real w_squared = q->w*q->w;
|
|
real two_w_squared_minus_one = 2.f * w_squared - 1.f;
|
|
real v_dot_p = (q->v.i*p->x + q->v.k*p->z + q->v.j*p->y);
|
|
|
|
real two_v_dot_p = v_dot_p+v_dot_p;
|
|
real two_w = q->w+q->w;
|
|
|
|
real v_cross_r_i = (q->v.k*p->x) - (p->z*q->v.i);
|
|
real v_cross_r_j = (q->v.i*p->y) - (q->v.j*p->x);
|
|
real v_cross_r_k = (q->v.j*p->z) - (q->v.k*p->y);
|
|
|
|
result->x = v_cross_r_k * two_w+two_v_dot_p*q->v.i + two_w_squared_minus_one*p->x;
|
|
result->y = two_v_dot_p*q->v.j + two_w_squared_minus_one*p->y + v_cross_r_i*two_w;
|
|
result->z = two_v_dot_p*q->v.k + two_w_squared_minus_one*p->z + v_cross_r_j*two_w;
|
|
return;
|
|
}
|
|
|
|
void vectors3d_from_euler_angles3d(
|
|
real_vector3d *forward,
|
|
real_vector3d *up,
|
|
real_euler_angles3d const *angles)
|
|
{
|
|
real_matrix4x3 matrix;
|
|
real_point3d dummy;
|
|
|
|
match_assert("c:\\halo\\SOURCE\\math\\real_math.c", 832, forward);
|
|
match_assert("c:\\halo\\SOURCE\\math\\real_math.c", 833, up);
|
|
match_assert("c:\\halo\\SOURCE\\math\\real_math.c", 834, angles);
|
|
|
|
matrix4x3_rotation_from_angles(&matrix, angles->yaw, angles->pitch, angles->roll);
|
|
matrix4x3_to_point_and_vectors(&matrix, &dummy, forward, up);
|
|
return;
|
|
}
|
|
|
|
boolean fast_vector_intersects_sphere(
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real_point3d const *center,
|
|
real radius)
|
|
{
|
|
real_point3d p = {point->x-center->x, point->y-center->y, point->z - center->z};
|
|
real c = (p.x*p.x) + (p.y*p.y) + (p.z*p.z) - (radius*radius);
|
|
real_vector3d direction;
|
|
|
|
if (c < 0.f)
|
|
{
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
real b;
|
|
|
|
direction = *vector;
|
|
b = direction.i*p.x + direction.j*p.y + direction.k*p.z;
|
|
|
|
if (b >= 0.f)
|
|
{
|
|
return FALSE;
|
|
}
|
|
else
|
|
{
|
|
real a =
|
|
direction.i*direction.i +
|
|
direction.j*direction.j +
|
|
direction.k*direction.k;
|
|
real disc = b * b - a * c;
|
|
|
|
if (disc <= 0.f)
|
|
{
|
|
return FALSE;
|
|
}
|
|
else
|
|
{
|
|
real neg_a_minus_b = -a - b;
|
|
|
|
if (neg_a_minus_b < 0.f)
|
|
{
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
return neg_a_minus_b * neg_a_minus_b < disc;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
real fast_vector_intersection_with_sphere(
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real_point3d const *center,
|
|
real radius)
|
|
{
|
|
real_point3d p =
|
|
{
|
|
point->x - center->x,
|
|
point->y - center->y,
|
|
point->z - center->z
|
|
};
|
|
real c = p.x * p.x + p.y * p.y + p.z * p.z - radius * radius;
|
|
real_vector3d direction;
|
|
real b;
|
|
real a;
|
|
real discriminant;
|
|
|
|
if (c < 0.0f)
|
|
return 0.0f;
|
|
|
|
direction = *vector;
|
|
b =
|
|
direction.i * p.x +
|
|
direction.j * p.y +
|
|
direction.k * p.z;
|
|
if (b >= 0.0f)
|
|
return REAL_MAX;
|
|
|
|
a =
|
|
direction.i * direction.i +
|
|
direction.j * direction.j +
|
|
direction.k * direction.k;
|
|
discriminant = b * b - a * c;
|
|
if (discriminant <= 0.0f)
|
|
return REAL_MAX;
|
|
|
|
return (-b - square_root(discriminant)) / a;
|
|
}
|
|
|
|
boolean point_in_rectangle2d(
|
|
real_point2d const *point,
|
|
real_rectangle2d const *bounds)
|
|
{
|
|
return point->x >= bounds->x0
|
|
&& point->x <= bounds->x1
|
|
&& point->y >= bounds->y0
|
|
&& point->y <= bounds->y1;
|
|
}
|
|
|
|
boolean point_in_rectangle3d(
|
|
real_point3d const *point,
|
|
real_rectangle3d const *bounds)
|
|
{
|
|
return point->x >= bounds->x0
|
|
&& point->x <= bounds->x1
|
|
&& point->y >= bounds->y0
|
|
&& point->y <= bounds->y1
|
|
&& point->z >= bounds->z0
|
|
&& point->z <= bounds->z1;
|
|
}
|
|
|
|
boolean circle_intersects_rectangle2d(
|
|
real_point2d const *center,
|
|
real radius,
|
|
real_rectangle2d const *bounds)
|
|
{
|
|
real dx;
|
|
real dy;
|
|
|
|
if (center->x <= bounds->x1)
|
|
{
|
|
if (center->x < bounds->x0)
|
|
{
|
|
dx = bounds->x0 - center->x;
|
|
}
|
|
else
|
|
{
|
|
dx = 0.f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dx = center->x - bounds->x1;
|
|
}
|
|
|
|
if (center->y <= bounds->y1)
|
|
{
|
|
if (center->y < bounds->y0)
|
|
{
|
|
dy = bounds->y0 - center->y;
|
|
}
|
|
else
|
|
{
|
|
dy = 0.f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dy = center->y - bounds->y1;
|
|
}
|
|
|
|
return dx * dx + dy * dy <= radius * radius;
|
|
}
|
|
|
|
boolean point_in_cone2d(
|
|
real_point2d const *point,
|
|
real_point2d const *center,
|
|
real_vector2d const *direction,
|
|
real length,
|
|
real cosine)
|
|
{
|
|
real_vector2d offset;
|
|
real projection;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
1375,
|
|
cosine>=0.0f);
|
|
|
|
vector_from_points2d(center, point, &offset);
|
|
projection = dot_product2d(&offset, direction);
|
|
|
|
return (boolean)(projection >= 0.f &&
|
|
projection <= length &&
|
|
projection * projection >= magnitude_squared2d(&offset) * cosine * cosine);
|
|
}
|
|
|
|
boolean point_in_cone3d(
|
|
real_point3d const *point,
|
|
real_point3d const *center,
|
|
real_vector3d const *direction,
|
|
real length,
|
|
real cosine)
|
|
{
|
|
real_vector3d offset;
|
|
real projection;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
1404,
|
|
cosine>=0.0f);
|
|
|
|
vector_from_points3d(center, point, &offset);
|
|
projection = dot_product3d(&offset, direction);
|
|
|
|
return (boolean)(projection >= 0.f &&
|
|
projection <= length &&
|
|
projection * projection >= magnitude_squared3d(&offset) * cosine * cosine);
|
|
}
|
|
|
|
boolean point_in_sector2d(
|
|
real_point2d const *point,
|
|
real_point2d const *center,
|
|
real_vector2d const *direction,
|
|
real radius,
|
|
real cosine)
|
|
{
|
|
real_vector2d offset;
|
|
real distance_squared;
|
|
real projection;
|
|
boolean result;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
1433,
|
|
cosine>=0.0f);
|
|
|
|
vector_from_points2d(center, point, &offset);
|
|
distance_squared = magnitude_squared2d(&offset);
|
|
if (distance_squared <= radius * radius)
|
|
{
|
|
projection = dot_product2d(&offset, direction);
|
|
if (projection >= 0.f &&
|
|
projection * projection >= distance_squared * cosine * cosine)
|
|
{
|
|
result = TRUE;
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean point_in_sector3d(
|
|
real_point3d const *point,
|
|
real_point3d const *center,
|
|
real_vector3d const *direction,
|
|
real radius,
|
|
real cosine)
|
|
{
|
|
real_vector3d offset;
|
|
real distance_squared;
|
|
real projection;
|
|
boolean result;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
1462,
|
|
cosine>=0.0f);
|
|
|
|
vector_from_points3d(center, point, &offset);
|
|
distance_squared = magnitude_squared3d(&offset);
|
|
if (distance_squared <= radius * radius)
|
|
{
|
|
projection = dot_product3d(&offset, direction);
|
|
if (projection >= 0.f &&
|
|
projection * projection >= distance_squared * cosine * cosine)
|
|
{
|
|
result = TRUE;
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean circle_intersects_cone2d(
|
|
real_point2d const *circle_center,
|
|
real circle_radius,
|
|
real_point2d const *cone_center,
|
|
real_vector2d const *cone_direction,
|
|
real cone_length,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real_vector2d offset;
|
|
real distance_squared;
|
|
real projection;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2167,
|
|
sine>0.0f && cosine>=0.0f);
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2168,
|
|
valid_realcmp(sine * sine + cosine * cosine, 1.0f),
|
|
csprintf(
|
|
temporary,
|
|
"%s, %s: assert_valid_real_sine_cosine(%f, %f)",
|
|
"sine",
|
|
"cosine",
|
|
sine,
|
|
cosine));
|
|
|
|
vector_from_points2d(cone_center, circle_center, &offset);
|
|
projection = dot_product2d(&offset, cone_direction);
|
|
distance_squared = offset.j * offset.j;
|
|
distance_squared += offset.i * offset.i;
|
|
|
|
return (boolean)(
|
|
projection >= -circle_radius &&
|
|
projection <= circle_radius + cone_length &&
|
|
(circle_radius * sine * 2.0f + projection) * projection +
|
|
circle_radius * circle_radius >=
|
|
distance_squared * cosine * cosine);
|
|
}
|
|
|
|
boolean circle_intersects_sector2d(
|
|
real_point2d const *circle_center,
|
|
real circle_radius,
|
|
real_point2d const *sector_center,
|
|
real_vector2d const *sector_direction,
|
|
real sector_radius,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real_vector2d offset;
|
|
real_point2d adjusted_center;
|
|
real projection;
|
|
real adjustment;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2231,
|
|
sine>0.0f && cosine>=0.0f);
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2232,
|
|
valid_realcmp(sine * sine + cosine * cosine, 1.0f),
|
|
csprintf(
|
|
temporary,
|
|
"%s, %s: assert_valid_real_sine_cosine(%f, %f)",
|
|
"sine",
|
|
"cosine",
|
|
sine,
|
|
cosine));
|
|
|
|
vector_from_points2d(sector_center, circle_center, &offset);
|
|
projection = dot_product2d(&offset, sector_direction);
|
|
if (projection >= -circle_radius &&
|
|
projection <= circle_radius + sector_radius)
|
|
{
|
|
adjustment = circle_radius / sine;
|
|
point_from_line2d(
|
|
sector_center,
|
|
sector_direction,
|
|
-adjustment,
|
|
&adjusted_center);
|
|
if (point_in_sector2d(
|
|
circle_center,
|
|
&adjusted_center,
|
|
sector_direction,
|
|
sector_radius + circle_radius + adjustment,
|
|
cosine))
|
|
{
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
boolean sphere_intersects_cone3d(
|
|
real_point3d const *sphere_center,
|
|
real sphere_radius,
|
|
real_point3d const *cone_center,
|
|
real_vector3d const *cone_direction,
|
|
real cone_length,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real_vector3d offset;
|
|
real distance_squared;
|
|
real projection;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2199,
|
|
sine>0.0f && cosine>=0.0f);
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2200,
|
|
valid_realcmp(sine * sine + cosine * cosine, 1.0f),
|
|
csprintf(
|
|
temporary,
|
|
"%s, %s: assert_valid_real_sine_cosine(%f, %f)",
|
|
"sine",
|
|
"cosine",
|
|
sine,
|
|
cosine));
|
|
|
|
vector_from_points3d(cone_center, sphere_center, &offset);
|
|
projection = offset.k * cone_direction->k;
|
|
projection += offset.j * cone_direction->j;
|
|
projection += offset.i * cone_direction->i;
|
|
distance_squared = offset.k * offset.k;
|
|
distance_squared += offset.j * offset.j;
|
|
distance_squared += offset.i * offset.i;
|
|
|
|
return (boolean)(
|
|
projection >= -sphere_radius &&
|
|
projection <= sphere_radius + cone_length &&
|
|
(sphere_radius * sine * 2.0f + projection) * projection +
|
|
sphere_radius * sphere_radius >=
|
|
distance_squared * cosine * cosine);
|
|
}
|
|
|
|
boolean sphere_intersects_sector3d(
|
|
real_point3d const *sphere_center,
|
|
real sphere_radius,
|
|
real_point3d const *sector_center,
|
|
real_vector3d const *sector_direction,
|
|
real sector_radius,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real_vector3d offset;
|
|
real_point3d adjusted_center;
|
|
real projection;
|
|
real adjustment;
|
|
real negative_adjustment;
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2269,
|
|
sine>0.0f && cosine>=0.0f);
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2270,
|
|
valid_realcmp(sine * sine + cosine * cosine, 1.0f),
|
|
csprintf(
|
|
temporary,
|
|
"%s, %s: assert_valid_real_sine_cosine(%f, %f)",
|
|
"sine",
|
|
"cosine",
|
|
sine,
|
|
cosine));
|
|
|
|
vector_from_points3d(sector_center, sphere_center, &offset);
|
|
projection =
|
|
offset.k * sector_direction->k +
|
|
offset.i * sector_direction->i;
|
|
projection += offset.j * sector_direction->j;
|
|
if (projection >= -sphere_radius &&
|
|
projection <= sphere_radius + sector_radius)
|
|
{
|
|
adjustment = sphere_radius / sine;
|
|
negative_adjustment = -adjustment;
|
|
adjusted_center.x =
|
|
sector_direction->i * negative_adjustment + sector_center->x;
|
|
adjusted_center.y =
|
|
sector_direction->j * negative_adjustment + sector_center->y;
|
|
if (point_in_sector3d(
|
|
sphere_center,
|
|
&adjusted_center,
|
|
sector_direction,
|
|
(adjusted_center.z =
|
|
sector_direction->k * negative_adjustment + sector_center->z,
|
|
sector_radius + sphere_radius + adjustment),
|
|
cosine))
|
|
{
|
|
return (boolean)TRUE;
|
|
}
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
boolean point_in_triangle2d(
|
|
real_point2d const *point,
|
|
real_point2d const *triangle0,
|
|
real_point2d const *triangle1,
|
|
real_point2d const *triangle2,
|
|
real *t0,
|
|
real *t1)
|
|
{
|
|
real_vector2d edge0;
|
|
real_vector2d edge1;
|
|
real_vector2d offset;
|
|
real cross0;
|
|
real cross1;
|
|
real determinant;
|
|
boolean result;
|
|
|
|
vector_from_points2d(triangle0, triangle1, &edge0);
|
|
vector_from_points2d(triangle0, point, &offset);
|
|
cross0 = cross_product2d(&edge0, &offset);
|
|
if (cross0 >= 0.f)
|
|
{
|
|
vector_from_points2d(triangle0, triangle2, &edge1);
|
|
cross1 = cross_product2d(&offset, &edge1);
|
|
if (cross1 >= 0.f)
|
|
{
|
|
determinant = cross_product2d(&edge0, &edge1);
|
|
if (cross0 + cross1 <= determinant)
|
|
{
|
|
real inverse_determinant = 1.f / determinant;
|
|
*t0 = cross1 * inverse_determinant;
|
|
*t1 = cross0 * inverse_determinant;
|
|
result = TRUE;
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean point_in_triangle3d(
|
|
real_point3d const *point,
|
|
real_point3d const *triangle0,
|
|
real_point3d const *triangle1,
|
|
real_point3d const *triangle2,
|
|
real *t0,
|
|
real *t1)
|
|
{
|
|
real_vector3d edge0;
|
|
real_vector3d edge1;
|
|
real_vector3d offset;
|
|
real_vector3d normal;
|
|
real_point2d projected_edge0;
|
|
real_point2d projected_edge1;
|
|
real_point2d projected_offset;
|
|
real plane_distance;
|
|
real cross0;
|
|
real cross1;
|
|
real determinant;
|
|
boolean result;
|
|
short projection;
|
|
boolean projection_sign;
|
|
|
|
vector_from_points3d(triangle0, point, &offset);
|
|
vector_from_points3d(triangle0, triangle1, &edge0);
|
|
vector_from_points3d(triangle0, triangle2, &edge1);
|
|
cross_product3d(&edge0, &edge1, &normal);
|
|
plane_distance = dot_product3d(&normal, &offset);
|
|
if (plane_distance * plane_distance <
|
|
magnitude_squared3d(&normal) * _real_epsilon)
|
|
{
|
|
projection = projection_from_vector3d(&normal);
|
|
projection_sign = projection_sign_from_vector3d(&normal, projection);
|
|
project_point3d(
|
|
(real_point3d const *)&edge0,
|
|
projection,
|
|
projection_sign,
|
|
&projected_edge0);
|
|
project_point3d(
|
|
(real_point3d const *)&offset,
|
|
projection,
|
|
projection_sign,
|
|
&projected_offset);
|
|
cross0 = cross_product2d(
|
|
(real_vector2d const *)&projected_edge0,
|
|
(real_vector2d const *)&projected_offset);
|
|
if (cross0 >= 0.0f)
|
|
{
|
|
project_point3d(
|
|
(real_point3d const *)&edge1,
|
|
projection,
|
|
projection_sign,
|
|
&projected_edge1);
|
|
cross1 = cross_product2d(
|
|
(real_vector2d const *)&projected_offset,
|
|
(real_vector2d const *)&projected_edge1);
|
|
if (cross1 >= 0.0f)
|
|
{
|
|
determinant = cross_product2d(
|
|
(real_vector2d const *)&projected_edge0,
|
|
(real_vector2d const *)&projected_edge1);
|
|
if (cross0 + cross1 <= determinant)
|
|
{
|
|
real inverse_determinant;
|
|
|
|
inverse_determinant = 1.0f / determinant;
|
|
*t0 = cross1 * inverse_determinant;
|
|
*t1 = cross0 * inverse_determinant;
|
|
result = TRUE;
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean circle_intersects_triangle2d(
|
|
real_point2d const *center,
|
|
real radius,
|
|
real_point2d const *triangle0,
|
|
real_point2d const *triangle1,
|
|
real_point2d const *triangle2)
|
|
{
|
|
real_vector2d edge;
|
|
real_vector2d offset;
|
|
boolean result = TRUE;
|
|
|
|
vector_from_points2d(triangle0, center, &offset);
|
|
vector_from_points2d(triangle0, triangle1, &edge);
|
|
if (cross_product2d(&offset, &edge) > 0.0f)
|
|
{
|
|
if (point_in_pill2d(center, triangle0, &edge, radius))
|
|
return TRUE;
|
|
result = FALSE;
|
|
}
|
|
|
|
vector_from_points2d(triangle1, center, &offset);
|
|
vector_from_points2d(triangle1, triangle2, &edge);
|
|
if (cross_product2d(&offset, &edge) < 0.0f)
|
|
{
|
|
if (point_in_pill2d(center, triangle1, &edge, radius))
|
|
return TRUE;
|
|
result = FALSE;
|
|
}
|
|
|
|
vector_from_points2d(triangle2, center, &offset);
|
|
vector_from_points2d(triangle2, triangle0, &edge);
|
|
if (cross_product2d(&offset, &edge) > 0.0f)
|
|
{
|
|
if (point_in_pill2d(center, triangle2, &edge, radius))
|
|
return TRUE;
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean pill_intersects_triangle2d(
|
|
real_point2d const *pill_base,
|
|
real_vector2d const *pill_height,
|
|
real pill_width,
|
|
real_point2d const *triangle0,
|
|
real_point2d const *triangle1,
|
|
real_point2d const *triangle2)
|
|
{
|
|
real_vector2d edge;
|
|
real_vector2d offset;
|
|
boolean result = TRUE;
|
|
|
|
vector_from_points2d(triangle0, pill_base, &offset);
|
|
vector_from_points2d(triangle0, triangle1, &edge);
|
|
if (cross_product2d(&offset, &edge) > 0.0f)
|
|
{
|
|
if (vector_intersects_pill2d(
|
|
triangle0,
|
|
&edge,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
vector_from_points2d(triangle1, pill_base, &offset);
|
|
vector_from_points2d(triangle1, triangle2, &edge);
|
|
if (cross_product2d(&offset, &edge) > 0.0f)
|
|
{
|
|
if (vector_intersects_pill2d(
|
|
triangle1,
|
|
&edge,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
vector_from_points2d(triangle2, pill_base, &offset);
|
|
vector_from_points2d(triangle2, triangle0, &edge);
|
|
if (cross_product2d(&offset, &edge) > 0.0f)
|
|
{
|
|
if (vector_intersects_pill2d(
|
|
triangle2,
|
|
&edge,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean pill_intersects_rectangle2d(
|
|
real_point2d const *pill_base,
|
|
real_vector2d const *pill_height,
|
|
real pill_width,
|
|
real_rectangle2d const *bounds)
|
|
{
|
|
real_point2d edge_base;
|
|
real_vector2d edge_height;
|
|
boolean result;
|
|
|
|
result = TRUE;
|
|
if (pill_base->x < bounds->x0)
|
|
{
|
|
set_real_point2d(&edge_base, bounds->x0, bounds->y0);
|
|
set_real_vector2d(&edge_height, 0.0f, bounds->y1 - bounds->y0);
|
|
if (vector_intersects_pill2d(
|
|
&edge_base,
|
|
&edge_height,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return (boolean)TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
if (pill_base->y < bounds->y0)
|
|
{
|
|
edge_base.x = bounds->x0;
|
|
edge_base.y = bounds->y0;
|
|
edge_height.i = bounds->x1 - bounds->x0;
|
|
edge_height.j = 0.0f;
|
|
if (vector_intersects_pill2d(
|
|
&edge_base,
|
|
&edge_height,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
if (pill_base->x > bounds->x1)
|
|
{
|
|
set_real_point2d(&edge_base, bounds->x1, bounds->y0);
|
|
set_real_vector2d(&edge_height, 0.0f, bounds->y1 - bounds->y0);
|
|
if (vector_intersects_pill2d(
|
|
&edge_base,
|
|
&edge_height,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
if (pill_base->y > bounds->y1)
|
|
{
|
|
edge_base.x = bounds->x0;
|
|
edge_base.y = bounds->y1;
|
|
edge_height.i = bounds->x1 - bounds->x0;
|
|
edge_height.j = 0.0f;
|
|
if (vector_intersects_pill2d(
|
|
&edge_base,
|
|
&edge_height,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
boolean sphere_intersects_rectangle3d(
|
|
real_point3d const *center,
|
|
real radius,
|
|
real_rectangle3d const *bounds)
|
|
{
|
|
real dx;
|
|
real dy;
|
|
real dz;
|
|
|
|
if (center->x <= bounds->x1)
|
|
{
|
|
if (center->x < bounds->x0)
|
|
{
|
|
dx = bounds->x0 - center->x;
|
|
}
|
|
else
|
|
{
|
|
dx = 0.f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dx = center->x - bounds->x1;
|
|
}
|
|
|
|
if (center->y <= bounds->y1)
|
|
{
|
|
if (center->y < bounds->y0)
|
|
{
|
|
dy = bounds->y0 - center->y;
|
|
}
|
|
else
|
|
{
|
|
dy = 0.f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dy = center->y - bounds->y1;
|
|
}
|
|
|
|
if (center->z <= bounds->z1)
|
|
{
|
|
if (center->z < bounds->z0)
|
|
{
|
|
dz = bounds->z0 - center->z;
|
|
}
|
|
else
|
|
{
|
|
dz = 0.f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dz = center->z - bounds->z1;
|
|
}
|
|
|
|
return dx * dx + dy * dy + dz * dz < radius * radius;
|
|
}
|
|
|
|
boolean sphere_intersects_triangle3d(
|
|
real_point3d const *center,
|
|
real radius,
|
|
real_point3d const *p0,
|
|
real_point3d const *p1,
|
|
real_point3d const *p2)
|
|
{
|
|
real_vector3d v;
|
|
real_vector3d e0;
|
|
real_vector3d e1;
|
|
real_vector3d e2;
|
|
real_vector3d n;
|
|
real plane_dot;
|
|
real normal_magnitude_squared;
|
|
boolean result;
|
|
|
|
result = TRUE;
|
|
vector_from_points3d(p0, center, &v);
|
|
vector_from_points3d(p0, p1, &e0);
|
|
vector_from_points3d(p1, p2, &e1);
|
|
cross_product3d(&e0, &e1, &n);
|
|
plane_dot = dot_product3d(&n, &v);
|
|
normal_magnitude_squared = magnitude_squared3d(&n);
|
|
if (plane_dot * plane_dot > normal_magnitude_squared * radius * radius)
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
if (triple_product3d(&v, &e0, &n) > 0.0f)
|
|
{
|
|
if (fast_vector_intersects_sphere(p0, &e0, center, radius))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
vector_from_points3d(p1, center, &v);
|
|
if (triple_product3d(&v, &e1, &n) > 0.0f)
|
|
{
|
|
if (fast_vector_intersects_sphere(p1, &e1, center, radius))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
vector_from_points3d(p2, center, &v);
|
|
vector_from_points3d(p2, p0, &e2);
|
|
if (triple_product3d(&v, &e2, &n) < 0.0f)
|
|
{
|
|
if (fast_vector_intersects_sphere(p2, &e2, center, radius))
|
|
{
|
|
return TRUE;
|
|
}
|
|
result = FALSE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean pill_intersects_triangle3d(
|
|
real_point3d const *base,
|
|
real_vector3d const *height,
|
|
real width,
|
|
real_point3d const *p0,
|
|
real_point3d const *p1,
|
|
real_point3d const *p2)
|
|
{
|
|
real_vector3d e0;
|
|
real_vector3d e1;
|
|
real_vector3d e2;
|
|
real_vector3d v;
|
|
real_vector3d n;
|
|
real_point3d p;
|
|
real t;
|
|
real closest_t;
|
|
real plane_distance;
|
|
boolean outside = FALSE;
|
|
|
|
vector_from_points3d(p0, p1, &e0);
|
|
vector_from_points3d(p1, p2, &e1);
|
|
vector_from_points3d(base, p0, &v);
|
|
cross_product3d(&e0, &e1, &n);
|
|
t = dot_product3d(&n, &v) / dot_product3d(&n, height);
|
|
|
|
if (t < 0.0f)
|
|
{
|
|
closest_t = 0.0f;
|
|
}
|
|
else if (t > 1.0f)
|
|
{
|
|
closest_t = 1.0f;
|
|
}
|
|
else
|
|
{
|
|
closest_t = t;
|
|
}
|
|
point_from_line3d(base, height, closest_t, &p);
|
|
vector_from_points3d(p0, &p, &v);
|
|
|
|
if (triple_product3d(&e0, &v, &n) < 0.0f)
|
|
{
|
|
if ((boolean)vector_intersects_pill3d(
|
|
p0,
|
|
&e0,
|
|
base,
|
|
height,
|
|
width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
outside = TRUE;
|
|
}
|
|
|
|
vector_from_points3d(p1, &p, &v);
|
|
if (triple_product3d(&e1, &v, &n) < 0.0f)
|
|
{
|
|
if ((boolean)vector_intersects_pill3d(
|
|
p1,
|
|
&e1,
|
|
base,
|
|
height,
|
|
width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
outside = TRUE;
|
|
}
|
|
|
|
vector_from_points3d(p2, p0, &e2);
|
|
vector_from_points3d(p2, &p, &v);
|
|
if (triple_product3d(&e2, &v, &n) < 0.0f)
|
|
{
|
|
if ((boolean)vector_intersects_pill3d(
|
|
p2,
|
|
&e2,
|
|
base,
|
|
height,
|
|
width))
|
|
{
|
|
return TRUE;
|
|
}
|
|
outside = TRUE;
|
|
}
|
|
|
|
if (outside)
|
|
{
|
|
return FALSE;
|
|
}
|
|
if (t > 0.0f && t < 1.0f)
|
|
{
|
|
return TRUE;
|
|
}
|
|
plane_distance = dot_product3d(&n, &v);
|
|
return plane_distance * plane_distance <=
|
|
magnitude_squared3d(&n) * width * width;
|
|
}
|
|
|
|
|
|
boolean vector_intersects_rectangle2d(
|
|
real_point2d const *point,
|
|
real_vector2d const *vector,
|
|
real_rectangle2d const *bounds)
|
|
{
|
|
real t_in = -FLT_MAX;
|
|
real t_out = FLT_MAX;
|
|
|
|
if (!(fabs(vector->i) < _real_epsilon))
|
|
{
|
|
real ooi = 1.f / vector->i;
|
|
real t0 = (bounds->x0-point->x) * ooi;
|
|
real t1 = (bounds->x1-point->x) * ooi;
|
|
|
|
if (vector->i > 0.f)
|
|
{
|
|
if (t_in < t0) t_in = t0;
|
|
if (t_out > t1) t_out = t1;
|
|
}
|
|
else
|
|
{
|
|
if (t_in < t1) t_in = t1;
|
|
if (t_out > t0) t_out = t0;
|
|
}
|
|
|
|
if (t_in > t_out)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (point->x < bounds->x0 || point->x > bounds->x1)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
if (!(fabs(vector->j) < _real_epsilon))
|
|
{
|
|
real ooj = 1.f / vector->j;
|
|
real t0 = (bounds->y0-point->y) * ooj;
|
|
real t1 = (bounds->y1-point->y) * ooj;
|
|
|
|
if (vector->j > 0.f)
|
|
{
|
|
if (t_in < t0) t_in = t0;
|
|
if (t_out > t1) t_out = t1;
|
|
}
|
|
else
|
|
{
|
|
if (t_in < t1) t_in = t1;
|
|
if (t_out > t0) t_out = t0;
|
|
}
|
|
|
|
if (t_in > t_out)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (point->y < bounds->y0 || point->y > bounds->y1)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
return t_out >= 0.f && t_in <= 1.f;
|
|
}
|
|
|
|
boolean vector_intersects_pill2d(
|
|
real_point2d const *point,
|
|
real_vector2d const *vector,
|
|
real_point2d const *pill_base,
|
|
real_vector2d const *pill_height,
|
|
real pill_width)
|
|
{
|
|
real_vector2d offset;
|
|
real cross;
|
|
real dot;
|
|
real vector_magnitude_squared;
|
|
real pill_magnitude_squared;
|
|
real inverse_cross;
|
|
real inverse_vector_magnitude_squared;
|
|
real inverse_pill_magnitude_squared;
|
|
real t0;
|
|
real t1;
|
|
real t0_start;
|
|
real t0_end;
|
|
real t1_start;
|
|
real t1_end;
|
|
real clamped_t0;
|
|
real clamped_t1;
|
|
real_point2d closest_point0;
|
|
real_point2d closest_point1;
|
|
real_vector2d closest_offset;
|
|
boolean t0_out_of_range;
|
|
boolean t1_out_of_range;
|
|
|
|
vector_from_points2d(point, pill_base, &offset);
|
|
cross = cross_product2d(vector, pill_height);
|
|
if (!(fabs(cross) < _real_epsilon))
|
|
{
|
|
inverse_cross = 1.0f / cross;
|
|
t0 = cross_product2d(&offset, pill_height) * inverse_cross;
|
|
t1 = cross_product2d(&offset, vector) * inverse_cross;
|
|
t0_out_of_range = t0 < 0.0f || t0 > 1.0f;
|
|
t1_out_of_range = t1 < 0.0f || t1 > 1.0f;
|
|
if (!t0_out_of_range && !t1_out_of_range)
|
|
{
|
|
return TRUE;
|
|
}
|
|
|
|
if (t0_out_of_range)
|
|
{
|
|
if (t0 < 0.0f)
|
|
clamped_t0 = 0.0f;
|
|
else
|
|
clamped_t0 = 1.0f;
|
|
point_from_line2d(point, vector, clamped_t0, &closest_point0);
|
|
}
|
|
|
|
if (t1_out_of_range)
|
|
{
|
|
if (t1 < 0.0f)
|
|
clamped_t1 = 0.0f;
|
|
else
|
|
clamped_t1 = 1.0f;
|
|
point_from_line2d(pill_base, pill_height, clamped_t1, &closest_point1);
|
|
}
|
|
|
|
return !((!t0_out_of_range ||
|
|
point_in_pill2d(
|
|
&closest_point0,
|
|
pill_base,
|
|
pill_height,
|
|
pill_width) == FALSE) &&
|
|
(!t1_out_of_range ||
|
|
point_in_pill2d(
|
|
&closest_point1,
|
|
point,
|
|
vector,
|
|
pill_width) == FALSE));
|
|
}
|
|
|
|
dot = dot_product2d(vector, pill_height);
|
|
vector_magnitude_squared = magnitude_squared2d(vector);
|
|
if (vector_magnitude_squared > _real_epsilon)
|
|
{
|
|
inverse_vector_magnitude_squared = 1.0f / vector_magnitude_squared;
|
|
t0_start =
|
|
dot_product2d(&offset, vector) *
|
|
inverse_vector_magnitude_squared;
|
|
t0_end =
|
|
inverse_vector_magnitude_squared * dot +
|
|
t0_start;
|
|
t0 =
|
|
(PIN(t0_start, 0.0f, 1.0f) +
|
|
PIN(t0_end, 0.0f, 1.0f)) *
|
|
0.5f;
|
|
}
|
|
else
|
|
{
|
|
t0 = 0.0f;
|
|
}
|
|
|
|
pill_magnitude_squared = magnitude_squared2d(pill_height);
|
|
if (pill_magnitude_squared > _real_epsilon)
|
|
{
|
|
inverse_pill_magnitude_squared = 1.0f / pill_magnitude_squared;
|
|
t1_start =
|
|
-dot_product2d(&offset, pill_height) *
|
|
inverse_pill_magnitude_squared;
|
|
t1_end =
|
|
inverse_pill_magnitude_squared * dot +
|
|
t1_start;
|
|
t1 =
|
|
(PIN(t1_start, 0.0f, 1.0f) +
|
|
PIN(t1_end, 0.0f, 1.0f)) *
|
|
0.5f;
|
|
}
|
|
else
|
|
{
|
|
t1 = 0.0f;
|
|
}
|
|
|
|
point_from_line2d(point, vector, t0, &closest_point0);
|
|
point_from_line2d(pill_base, pill_height, t1, &closest_point1);
|
|
vector_from_points2d(&closest_point0, &closest_point1, &closest_offset);
|
|
return magnitude_squared2d(&closest_offset) <= pill_width * pill_width;
|
|
}
|
|
|
|
long vector_intersects_pill3d(
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real_point3d const *pill_base,
|
|
real_vector3d const *pill_height,
|
|
real pill_width)
|
|
{
|
|
real_vector3d offset;
|
|
real_vector3d cross;
|
|
real_vector3d numerator_cross;
|
|
real denominator;
|
|
real inverse_denominator;
|
|
real dot;
|
|
real vector_magnitude_squared;
|
|
real pill_magnitude_squared;
|
|
real inverse_vector_magnitude_squared;
|
|
real inverse_pill_magnitude_squared;
|
|
real t0;
|
|
real t1;
|
|
real t0_start;
|
|
real t0_end;
|
|
real t1_start;
|
|
real t1_end;
|
|
real clamped_t0;
|
|
real clamped_t1;
|
|
real_point3d closest_point0;
|
|
real_point3d closest_point1;
|
|
real_vector3d closest_offset;
|
|
boolean t0_out_of_range;
|
|
boolean t1_out_of_range;
|
|
|
|
vector_from_points3d(point, pill_base, &offset);
|
|
cross_product3d(vector, pill_height, &cross);
|
|
denominator = magnitude_squared3d_right_associated(&cross);
|
|
if (!(fabs(denominator) < _real_epsilon))
|
|
{
|
|
inverse_denominator = 1.0f / denominator;
|
|
scale_vector3d(&cross, inverse_denominator, &cross);
|
|
cross_product3d(&offset, pill_height, &numerator_cross);
|
|
t0 = dot_product3d(&numerator_cross, &cross);
|
|
cross_product3d(&offset, vector, &numerator_cross);
|
|
t1 = dot_product3d(&numerator_cross, &cross);
|
|
t0_out_of_range = t0 < 0.0f || t0 > 1.0f;
|
|
t1_out_of_range = t1 < 0.0f || t1 > 1.0f;
|
|
if (!t0_out_of_range && !t1_out_of_range)
|
|
goto closest_distance;
|
|
|
|
if (t0_out_of_range)
|
|
{
|
|
clamped_t0 = t0 < 0.0f ? 0.0f : 1.0f;
|
|
point_from_line3d(point, vector, clamped_t0, &closest_point0);
|
|
}
|
|
if (t1_out_of_range)
|
|
{
|
|
clamped_t1 = t1 < 0.0f ? 0.0f : 1.0f;
|
|
point_from_line3d(pill_base, pill_height, clamped_t1, &closest_point1);
|
|
}
|
|
if (t0_out_of_range &&
|
|
fast_vector_intersects_sphere(
|
|
pill_base,
|
|
pill_height,
|
|
&closest_point0,
|
|
pill_width))
|
|
{
|
|
goto intersection;
|
|
}
|
|
if (t1_out_of_range &&
|
|
fast_vector_intersects_sphere(
|
|
point,
|
|
vector,
|
|
&closest_point1,
|
|
pill_width))
|
|
{
|
|
goto intersection;
|
|
}
|
|
return FALSE;
|
|
}
|
|
|
|
dot = dot_product3d(vector, pill_height);
|
|
vector_magnitude_squared = magnitude_squared3d(vector);
|
|
if (vector_magnitude_squared > _real_epsilon)
|
|
{
|
|
inverse_vector_magnitude_squared = 1.0f / vector_magnitude_squared;
|
|
t0_start = offset.k * vector->k;
|
|
t0_start += offset.j * vector->j;
|
|
t0_start += offset.i * vector->i;
|
|
t0_start *= inverse_vector_magnitude_squared;
|
|
t0_end =
|
|
inverse_vector_magnitude_squared * dot +
|
|
t0_start;
|
|
t0 =
|
|
(PIN(t0_start, 0.0f, 1.0f) +
|
|
PIN(t0_end, 0.0f, 1.0f)) *
|
|
0.5f;
|
|
}
|
|
else
|
|
{
|
|
t0 = 0.0f;
|
|
}
|
|
|
|
pill_magnitude_squared = magnitude_squared3d(pill_height);
|
|
if (pill_magnitude_squared > _real_epsilon)
|
|
{
|
|
inverse_pill_magnitude_squared = 1.0f / pill_magnitude_squared;
|
|
t1_start =
|
|
-dot_product3d(&offset, pill_height) *
|
|
inverse_pill_magnitude_squared;
|
|
t1_end =
|
|
inverse_pill_magnitude_squared * dot +
|
|
t1_start;
|
|
t1 =
|
|
(PIN(t1_start, 0.0f, 1.0f) +
|
|
PIN(t1_end, 0.0f, 1.0f)) *
|
|
0.5f;
|
|
}
|
|
else
|
|
{
|
|
t1 = 0.0f;
|
|
}
|
|
|
|
closest_distance:
|
|
point_from_line3d(point, vector, t0, &closest_point0);
|
|
point_from_line3d(pill_base, pill_height, t1, &closest_point1);
|
|
vector_from_points3d(&closest_point0, &closest_point1, &closest_offset);
|
|
if (!(magnitude_squared3d(&closest_offset) <= pill_width * pill_width))
|
|
return FALSE;
|
|
|
|
intersection:
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
#define VECTOR_INTERSECTS_LINE2D(point, vector, line_point, line_endpoint, t_in, t_out) \
|
|
do \
|
|
{ \
|
|
real_vector2d offset; \
|
|
real_vector2d line_vector; \
|
|
real numerator; \
|
|
real divisor; \
|
|
real t; \
|
|
vector_from_points2d((line_point), (point), &offset); \
|
|
vector_from_points2d((line_point), (line_endpoint), &line_vector); \
|
|
numerator = offset.i * line_vector.j - offset.j * line_vector.i; \
|
|
divisor = line_vector.i * (vector)->j - line_vector.j * (vector)->i; \
|
|
if (!(fabs(divisor) < _real_epsilon)) \
|
|
{ \
|
|
t = numerator / divisor; \
|
|
if (divisor > 0.0f) \
|
|
{ \
|
|
if (*(t_in) < t) \
|
|
*(t_in) = t; \
|
|
} \
|
|
else \
|
|
{ \
|
|
if (*(t_out) > t) \
|
|
*(t_out) = t; \
|
|
} \
|
|
if (*(t_in) > *(t_out)) \
|
|
return FALSE; \
|
|
} \
|
|
else if (numerator > 0.0f) \
|
|
{ \
|
|
return FALSE; \
|
|
} \
|
|
} while (0)
|
|
|
|
boolean vector_intersects_triangle2d(
|
|
real_point2d const *point,
|
|
real_vector2d const *vector,
|
|
real_point2d const *triangle0,
|
|
real_point2d const *triangle1,
|
|
real_point2d const *triangle2)
|
|
{
|
|
real t_in = 0.0f;
|
|
real t_out = 1.0f;
|
|
|
|
VECTOR_INTERSECTS_LINE2D(
|
|
point,
|
|
vector,
|
|
triangle0,
|
|
triangle1,
|
|
&t_in,
|
|
&t_out);
|
|
|
|
VECTOR_INTERSECTS_LINE2D(
|
|
point,
|
|
vector,
|
|
triangle1,
|
|
triangle2,
|
|
&t_in,
|
|
&t_out);
|
|
|
|
VECTOR_INTERSECTS_LINE2D(
|
|
point,
|
|
vector,
|
|
triangle2,
|
|
triangle0,
|
|
&t_in,
|
|
&t_out);
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
#undef VECTOR_INTERSECTS_LINE2D
|
|
|
|
boolean vector_intersects_triangle3d(
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real_point3d const *triangle0,
|
|
real_point3d const *triangle1,
|
|
real_point3d const *triangle2,
|
|
real *result)
|
|
{
|
|
real_vector3d edge0;
|
|
real_vector3d edge1;
|
|
real_vector3d normal;
|
|
real_vector3d offset;
|
|
real_vector3d cross;
|
|
real denominator;
|
|
real inverse_denominator;
|
|
real t;
|
|
real t0;
|
|
real t1;
|
|
boolean intersects = FALSE;
|
|
|
|
vector_from_points3d(triangle0, triangle1, &edge0);
|
|
vector_from_points3d(triangle0, triangle2, &edge1);
|
|
cross_product3d(&edge0, &edge1, &normal);
|
|
denominator = dot_product3d(&normal, vector);
|
|
if (!(fabs(denominator) < _real_epsilon))
|
|
{
|
|
inverse_denominator = 1.0f / denominator;
|
|
vector_from_points3d(point, triangle0, &offset);
|
|
t = dot_product3d(&offset, &normal) * inverse_denominator;
|
|
if (t >= 0.0f && t <= 1.0f)
|
|
{
|
|
cross_product3d(&offset, vector, &cross);
|
|
t0 = dot_product3d(&cross, &edge1) * inverse_denominator;
|
|
if (t0 >= 0.0f && t0 <= 1.0f)
|
|
{
|
|
t1 = -dot_product3d(&cross, &edge0) * inverse_denominator;
|
|
if (t1 >= 0.0f && t0 + t1 <= 1.0f)
|
|
{
|
|
*result = t;
|
|
intersects = TRUE;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return intersects;
|
|
}
|
|
|
|
boolean vector_intersects_rectangle3d(
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real_rectangle3d const *bounds)
|
|
{
|
|
real t_in = -FLT_MAX;
|
|
real t_out = FLT_MAX;
|
|
|
|
if (!(fabs(vector->i) < _real_epsilon))
|
|
{
|
|
real ooi = 1.f / vector->i;
|
|
real t0 = (bounds->x0-point->x) * ooi;
|
|
real t1 = (bounds->x1-point->x) * ooi;
|
|
|
|
if (vector->i > 0.f)
|
|
{
|
|
if (t_in < t0) t_in = t0;
|
|
if (t_out > t1) t_out = t1;
|
|
}
|
|
else
|
|
{
|
|
if (t_in < t1) t_in = t1;
|
|
if (t_out > t0) t_out = t0;
|
|
}
|
|
|
|
if (t_in > t_out)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (point->x < bounds->x0 || point->x > bounds->x1)
|
|
return FALSE;
|
|
}
|
|
|
|
if (!(fabs(vector->j) < _real_epsilon))
|
|
{
|
|
real ooj = 1.f / vector->j;
|
|
real t0 = (bounds->y0-point->y) * ooj;
|
|
real t1 = (bounds->y1-point->y) * ooj;
|
|
|
|
if (vector->j > 0.f)
|
|
{
|
|
if (t_in < t0) t_in = t0;
|
|
if (t_out > t1) t_out = t1;
|
|
}
|
|
else
|
|
{
|
|
if (t_in < t1) t_in = t1;
|
|
if (t_out > t0) t_out = t0;
|
|
}
|
|
|
|
if (t_in > t_out)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (point->y < bounds->y0 || point->y > bounds->y1)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
if (!(fabs(vector->k) < _real_epsilon))
|
|
{
|
|
real ook = 1 / vector->k;
|
|
real t0 = (bounds->z0-point->z) * ook;
|
|
real t1 = (bounds->z1-point->z) * ook;
|
|
|
|
if (vector->k > 0.f)
|
|
{
|
|
if (t_in < t0) t_in = t0;
|
|
if (t_out > t1) t_out = t1;
|
|
}
|
|
else
|
|
{
|
|
if (t_in < t1) t_in = t1;
|
|
if (t_out > t0) t_out = t0;
|
|
}
|
|
|
|
if (t_in > t_out)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (point->z < bounds->z0 || point->z > bounds->z1)
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
return t_out >= 0.f && t_in <= 1.f;
|
|
}
|
|
|
|
boolean pill_intersects_rectangle3d(
|
|
real_point3d const *base,
|
|
real_vector3d const *height,
|
|
real width,
|
|
real_rectangle3d const *bounds)
|
|
{
|
|
/* originally stubbed */
|
|
return FALSE;
|
|
}
|
|
|
|
real_vector2d *fast_normalize2d(
|
|
real_vector2d *v)
|
|
{
|
|
real magnitude_squared = magnitude_squared2d(v);
|
|
|
|
if (magnitude_squared != 0.f)
|
|
{
|
|
scale_vector2d(v, 1.f / square_root(magnitude_squared), v);
|
|
}
|
|
|
|
return v;
|
|
}
|
|
|
|
real_vector3d *fast_normalize3d(
|
|
real_vector3d *v)
|
|
{
|
|
real magnitude_squared = magnitude_squared3d(v);
|
|
|
|
if (magnitude_squared != 0.f)
|
|
{
|
|
scale_vector3d(v, 1.f / square_root(magnitude_squared), v);
|
|
}
|
|
|
|
return v;
|
|
}
|
|
|
|
real cross_product_magnitude3d(
|
|
real_vector3d const *a,
|
|
real_vector3d const *b)
|
|
{
|
|
real_vector3d c;
|
|
cross_product3d(a, b, &c);
|
|
return magnitude3d(&c);
|
|
}
|
|
|
|
real dequantize_byte_to_real(
|
|
real min,
|
|
real max,
|
|
byte value)
|
|
{
|
|
if (value == UNSIGNED_CHAR_MAX)
|
|
{
|
|
return max;
|
|
}
|
|
|
|
return (max-min) * ((real)value / 255.f) + min;
|
|
}
|
|
|
|
byte quantize_real_to_byte_lower_bound(
|
|
real min,
|
|
real max,
|
|
real value)
|
|
{
|
|
real range;
|
|
byte test;
|
|
|
|
range = max - min;
|
|
test = (byte)(long)(((value - min) / range) * 255.f);
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2803,
|
|
min - _real_epsilon <= value &&
|
|
value <= max + _real_epsilon,
|
|
csprintf(
|
|
temporary,
|
|
"%lf is not between %lf and %lf",
|
|
value,
|
|
min,
|
|
max));
|
|
|
|
while (test > 0 &&
|
|
value < dequantize_byte_to_real(min, max, test))
|
|
{
|
|
test--;
|
|
}
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2809,
|
|
dequantize_byte_to_real(min, max, test) <= value ||
|
|
(test == 0 &&
|
|
dequantize_byte_to_real(min, max, test) <= value + _real_epsilon),
|
|
"dequantize_byte_to_real(min, max, test)<=value || (test==0 && dequantize_byte_to_real(min, max, test)<=value+_real_epsilon)");
|
|
|
|
return test;
|
|
}
|
|
|
|
byte quantize_real_to_byte_upper_bound(
|
|
real min,
|
|
real max,
|
|
real value)
|
|
{
|
|
real range;
|
|
byte test;
|
|
|
|
range = max - min;
|
|
test = (byte)(long)(((value - min) / range) * 255.f);
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2823,
|
|
min - _real_epsilon <= value &&
|
|
value <= max + _real_epsilon,
|
|
csprintf(
|
|
temporary,
|
|
"%lf is not between %lf and %lf",
|
|
value,
|
|
min,
|
|
max));
|
|
|
|
while (test < UNSIGNED_CHAR_MAX &&
|
|
value > dequantize_byte_to_real(min, max, test))
|
|
{
|
|
test++;
|
|
}
|
|
|
|
match_vassert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2829,
|
|
dequantize_byte_to_real(min, max, test) >= value ||
|
|
(test == UNSIGNED_CHAR_MAX &&
|
|
dequantize_byte_to_real(min, max, test) >= value - _real_epsilon),
|
|
"dequantize_byte_to_real(min, max, test)>=value || (test==UNSIGNED_CHAR_MAX && dequantize_byte_to_real(min, max, test)>=value-_real_epsilon)");
|
|
|
|
return test;
|
|
}
|
|
|
|
byte_rectangle3d *quantize_real_to_byte_rectangle3d(
|
|
real_rectangle3d const *parent,
|
|
real_rectangle3d const *rectangle,
|
|
byte_rectangle3d *result)
|
|
{
|
|
if (rectangle->x0 == REAL_MAX)
|
|
{
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
2843,
|
|
rectangle->x1==REAL_MIN && rectangle->y0==REAL_MAX && rectangle->y1==REAL_MIN && rectangle->z0==REAL_MAX && rectangle->z1==REAL_MIN);
|
|
|
|
csmemset(result, 0, sizeof(*result));
|
|
}
|
|
else
|
|
{
|
|
result->x0 = quantize_real_to_byte_lower_bound(parent->x0, parent->x1, rectangle->x0);
|
|
result->x1 = quantize_real_to_byte_upper_bound(parent->x0, parent->x1, rectangle->x1);
|
|
result->y0 = quantize_real_to_byte_lower_bound(parent->y0, parent->y1, rectangle->y0);
|
|
result->y1 = quantize_real_to_byte_upper_bound(parent->y0, parent->y1, rectangle->y1);
|
|
result->z0 = quantize_real_to_byte_lower_bound(parent->z0, parent->z1, rectangle->z0);
|
|
result->z1 = quantize_real_to_byte_upper_bound(parent->z0, parent->z1, rectangle->z1);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
real signed_angle_between_vectors2d(
|
|
real_vector2d const *a,
|
|
real_vector2d const *b)
|
|
{
|
|
real dot_product = dot_product2d(a, b);
|
|
real result = arccosine(PIN(dot_product, -1.f, 1.f));
|
|
|
|
if ((a->i*b->j) - (a->j*b->i) < 0.f)
|
|
{
|
|
result = -result;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
real angle_between_vectors2d(
|
|
real_vector2d const *a,
|
|
real_vector2d const *b)
|
|
{
|
|
real angle = 0.f;
|
|
real aa_bb = (a->i*a->i + a->j*a->j) * (b->i*b->i + b->j*b->j);
|
|
|
|
if (aa_bb != 0.f)
|
|
{
|
|
real ab = dot_product2d(a, b);
|
|
real c = PIN((ab/aa_bb*ab) + (ab/aa_bb*ab) - 1.f, -1.f, 1.f);
|
|
|
|
angle = arccosine(c) * 0.5f;
|
|
|
|
if (ab < 0.f)
|
|
{
|
|
angle = _pi - angle;
|
|
}
|
|
}
|
|
|
|
return angle;
|
|
}
|
|
|
|
real angle_between_vectors3d(
|
|
real_vector3d const *a,
|
|
real_vector3d const *b)
|
|
{
|
|
|
|
real angle = 0.f;
|
|
real aa_bb = (a->i*a->i + a->j*a->j + a->k*a->k) * (b->i*b->i + b->j*b->j + b->k*b->k);
|
|
|
|
if (aa_bb != 0.f)
|
|
{
|
|
real ab = dot_product3d(a, b);
|
|
real c = PIN((ab/aa_bb*ab) + (ab/aa_bb*ab) - 1.f, -1.f, 1.f);
|
|
|
|
angle = arccosine(c) * 0.5f;
|
|
|
|
if (ab < 0.f)
|
|
{
|
|
angle = _pi - angle;
|
|
}
|
|
}
|
|
|
|
return angle;
|
|
}
|
|
|
|
real angle_between_normals3d(
|
|
real_vector3d const *a,
|
|
real_vector3d const *b)
|
|
{
|
|
real result = 0.f;
|
|
|
|
if ((*(long *)&a->i) == (*(long *)&b->i)
|
|
&& (*(long *)&a->j) == (*(long *)&b->j)
|
|
&& (*(long *)&a->k) == (*(long *)&b->k))
|
|
{
|
|
return result;
|
|
}
|
|
|
|
result = PIN(dot_product3d(a, b), -1.f, 1.f);
|
|
return arccosine(result);
|
|
}
|
|
|
|
void yaw_vectors(
|
|
real_vector3d *forward,
|
|
real_vector3d const *up,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real_vector3d cross;
|
|
cross_product3d(up, forward, &cross);
|
|
|
|
forward->i = forward->i*cosine + cross.i*sine;
|
|
forward->j = forward->j*cosine + cross.j*sine;
|
|
forward->k = forward->k*cosine + cross.k*sine;
|
|
return;
|
|
}
|
|
|
|
void pitch_vectors(
|
|
real_vector3d *forward,
|
|
real_vector3d *up,
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
real_vector3d backward;
|
|
negate_vector3d(forward, &backward);
|
|
|
|
forward->i = forward->i*cosine + up->i*sine;
|
|
forward->j = forward->j*cosine + up->j*sine;
|
|
forward->k = forward->k*cosine + up->k*sine;
|
|
|
|
up->i = up->i*cosine + backward.i*sine;
|
|
up->j = up->j*cosine + backward.j*sine;
|
|
up->k = up->k*cosine + backward.k*sine;
|
|
return;
|
|
}
|
|
|
|
void fast_normals_interpolate(
|
|
real_vector3d const *a,
|
|
real_vector3d const *b,
|
|
real t,
|
|
real_vector3d *result)
|
|
{
|
|
real one_minus_t = 1-t;
|
|
result->i = t*b->i + one_minus_t * a->i;
|
|
result->j = t*b->j + one_minus_t * a->j;
|
|
result->k = t*b->k + one_minus_t * a->k;
|
|
fast_normalize3d(result);
|
|
return;
|
|
}
|
|
|
|
void normals_interpolate(
|
|
real_vector3d const *a,
|
|
real_vector3d const *b,
|
|
real t,
|
|
real_vector3d *result)
|
|
{
|
|
real angle;
|
|
real_vector3d c;
|
|
|
|
angle = angle_between_vectors3d(a, b);
|
|
|
|
if (t <= 0.5f)
|
|
{
|
|
angle *= t;
|
|
cross_product3d(a, b, &c);
|
|
*result = *a;
|
|
}
|
|
else
|
|
{
|
|
angle *= (1-t);
|
|
cross_product3d(b, a, &c);
|
|
*result = *b;
|
|
}
|
|
|
|
if (normalize3d(&c) != 0.0f)
|
|
{
|
|
rotate_vector_about_axis(result, &c, sine(angle), cosine(angle));
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
boolean pin_normal_to_cone3d(
|
|
real_vector3d const *normal,
|
|
real_vector3d const *direction,
|
|
real sine,
|
|
real cosine,
|
|
real_vector3d *result)
|
|
{
|
|
real_vector3d axis;
|
|
boolean pinned;
|
|
|
|
match_assert_valid_real_normal3d("c:\\halo\\SOURCE\\math\\real_math.c", 515, normal);
|
|
match_assert_valid_real_normal3d("c:\\halo\\SOURCE\\math\\real_math.c", 516, direction);
|
|
|
|
if (dot_product3d(normal, direction) >= cosine)
|
|
{
|
|
*result = *normal;
|
|
pinned = FALSE;
|
|
}
|
|
else
|
|
{
|
|
cross_product3d(direction, normal, &axis);
|
|
if (normalize3d(&axis) == 0.f)
|
|
{
|
|
perpendicular3d(direction, &axis);
|
|
normalize3d(&axis);
|
|
}
|
|
|
|
*result = *direction;
|
|
rotate_vector_about_axis(result, &axis, sine, cosine);
|
|
pinned = TRUE;
|
|
}
|
|
|
|
match_assert_valid_real_normal3d("c:\\halo\\SOURCE\\math\\real_math.c", 540, result);
|
|
return pinned;
|
|
}
|
|
|
|
real_vector3d *reflect_vector3d(
|
|
real_vector3d const *incident,
|
|
real_vector3d const *normal,
|
|
real_vector3d *reflection)
|
|
{
|
|
real incident_dot_normal = dot_product3d(incident, normal);
|
|
real twice_incident_dot_normal = incident_dot_normal + incident_dot_normal;
|
|
reflection->i = incident->i - normal->i*twice_incident_dot_normal;
|
|
reflection->j = incident->j - normal->j*twice_incident_dot_normal;
|
|
reflection->k = incident->k - normal->k*twice_incident_dot_normal;
|
|
return reflection;
|
|
}
|
|
|
|
real_vector3d *refract_vector3d(
|
|
real_vector3d const *incident,
|
|
real_vector3d const *normal,
|
|
real coefficient_of_refraction,
|
|
real_vector3d *refraction)
|
|
{
|
|
real refraction_scale;
|
|
|
|
real incident_magnitude = magnitude3d(incident);
|
|
real one_over_incident_magnitude = 1.f / incident_magnitude;
|
|
|
|
real sine_incident = cross_product_magnitude3d(incident, normal) * one_over_incident_magnitude;
|
|
real cosine_incident = dot_product3d(incident, normal) * one_over_incident_magnitude;
|
|
real sine_refraction = coefficient_of_refraction * sine_incident;
|
|
|
|
real cosine_refraction = square_root(1.f - sine_refraction * sine_refraction);
|
|
|
|
real delta = (sine_incident * cosine_refraction + cosine_incident * sine_refraction) / sine_refraction;
|
|
|
|
refraction->i = delta * normal->i + incident->i;
|
|
refraction->j = delta * normal->j + incident->j;
|
|
refraction->k = delta * normal->k + incident->k;
|
|
|
|
refraction_scale = incident_magnitude / magnitude3d(refraction);
|
|
|
|
refraction->i *= refraction_scale;
|
|
refraction->j *= refraction_scale;
|
|
refraction->k *= refraction_scale;
|
|
|
|
return refraction;
|
|
}
|
|
|
|
void quaternion_normalize(
|
|
real_quaternion *q)
|
|
{
|
|
real magnitude_squared = q->v.i*q->v.i + q->v.j*q->v.j + q->v.k*q->v.k + q->w*q->w;
|
|
if (magnitude_squared > 0.f)
|
|
{
|
|
real one_over_magnitude = reciprocal_square_root(magnitude_squared);
|
|
set_real_quaternion(q, one_over_magnitude*q->v.i, one_over_magnitude*q->v.j, one_over_magnitude*q->v.k, one_over_magnitude*q->w);
|
|
}
|
|
else
|
|
{
|
|
set_real_quaternion(q, 0.f, 0.f, 0.f, 1.f);
|
|
}
|
|
return;
|
|
}
|
|
|
|
void quaternion_from_angle_and_vector(
|
|
real_quaternion *q,
|
|
real a,
|
|
real_vector3d const *v)
|
|
{
|
|
real half_angle = a * 0.5f;
|
|
real sine_half_angle = sine(half_angle);
|
|
|
|
q->w = cosine(half_angle);
|
|
q->v.i = sine_half_angle*v->i;
|
|
q->v.j = sine_half_angle*v->j;
|
|
q->v.k = sine_half_angle*v->k;
|
|
|
|
return;
|
|
}
|
|
|
|
void quaternion_to_angle_and_vector(
|
|
real_quaternion const *q,
|
|
real *a,
|
|
real_vector3d *v)
|
|
{
|
|
*v = q->v;
|
|
*a = 2 * arctangent(normalize3d(v), q->w) ;
|
|
|
|
if (*a > _pi)
|
|
{
|
|
negate_vector3d(v, v);
|
|
*a = _pi * 2.0f - *a;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void quaternions_interpolate_and_normalize(
|
|
real_quaternion const *q0,
|
|
real_quaternion const *q1,
|
|
real t,
|
|
real_quaternion *result)
|
|
{
|
|
quaternions_interpolate(q0, q1, t, result);
|
|
quaternion_normalize(result);
|
|
return;
|
|
}
|
|
|
|
void orientations_interpolate(
|
|
real_orientation const *a,
|
|
real_orientation const *b,
|
|
real t,
|
|
real_orientation *result)
|
|
{
|
|
quaternions_interpolate(&a->rotation, &b->rotation, t, &result->rotation);
|
|
quaternion_normalize(&result->rotation);
|
|
|
|
result->translation.x = a->translation.x*(1.f - t) + b->translation.x*t;
|
|
result->translation.y = a->translation.y*(1.f - t) + b->translation.y*t;
|
|
result->translation.z = a->translation.z*(1.f - t) + b->translation.z*t;
|
|
result->scale = a->scale*(1.f - t) + b->scale*t;
|
|
return;
|
|
}
|
|
|
|
real_euler_angles2d *euler_angles2d_from_vector3d(
|
|
real_euler_angles2d *angles,
|
|
real_vector3d const *vector)
|
|
{
|
|
angles->yaw = arctangent(vector->j, vector->i);
|
|
angles->pitch = arctangent(vector->k, square_root(vector->i*vector->i+vector->j*vector->j));
|
|
return angles;
|
|
}
|
|
|
|
real_vector3d *vector3d_from_angle(
|
|
real_vector3d *vector,
|
|
real angle)
|
|
{
|
|
vector->k = 0.f;
|
|
vector->i = cosine(angle);
|
|
vector->j = sine(angle);
|
|
return vector;
|
|
}
|
|
|
|
real_vector3d *vector3d_from_euler_angles2d(
|
|
real_vector3d *vector,
|
|
real_euler_angles2d const *angles)
|
|
{
|
|
real cosine_pitch = cosine(angles->pitch);
|
|
vector->i = cosine(angles->yaw)* cosine_pitch;
|
|
vector->j = sine(angles->yaw)* cosine_pitch;
|
|
vector->k = sine(angles->pitch);
|
|
return vector;
|
|
}
|
|
|
|
boolean point_in_pill2d(
|
|
real_point2d const *point,
|
|
real_point2d const *base,
|
|
real_vector2d const *height,
|
|
real width)
|
|
{
|
|
real_vector2d v;
|
|
real height_magnitude_squared;
|
|
real t;
|
|
|
|
vector_from_points2d(base, point, &v);
|
|
height_magnitude_squared = dot_product2d(&v, height) / magnitude_squared2d(height);
|
|
t = -PIN(height_magnitude_squared, 0.f, 1.f);
|
|
point_from_line2d((real_point2d *)&v, height, t, (real_point2d *)&v);
|
|
|
|
return magnitude_squared2d(&v)<=width*width;
|
|
}
|
|
|
|
real point_to_line_distance_squared3d(
|
|
real_point3d const *point,
|
|
real_point3d const *base,
|
|
real_vector3d const *height)
|
|
{
|
|
real t;
|
|
real distance_squared;
|
|
real_vector3d v;
|
|
|
|
vector_from_points3d(base, point, &v);
|
|
t = PIN(dot_product3d(&v, height) / magnitude_squared3d(height), 0.f, 1.f);
|
|
point_from_line3d((real_point3d *)&v, height, -t, (real_point3d *)&v);
|
|
distance_squared = magnitude_squared3d(&v);
|
|
|
|
return distance_squared;
|
|
}
|
|
|
|
real vector_to_line_distance_squared3d(
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real_point3d const *line_point,
|
|
real_vector3d const *line_vector)
|
|
{
|
|
real_vector3d offset;
|
|
real_vector3d cross;
|
|
real_vector3d numerator_cross0;
|
|
real_vector3d numerator_cross1;
|
|
real denominator;
|
|
real inverse_denominator;
|
|
real dot;
|
|
real vector_magnitude_squared;
|
|
real line_magnitude_squared;
|
|
real inverse_vector_magnitude_squared;
|
|
real inverse_line_magnitude_squared;
|
|
real t0;
|
|
real t1;
|
|
real t0_start;
|
|
real t0_end;
|
|
real t1_start;
|
|
real t1_end;
|
|
real clamped_t0;
|
|
real clamped_t1;
|
|
real d1;
|
|
real d0;
|
|
real_point3d closest_point0;
|
|
real_point3d closest_point1;
|
|
real_vector3d closest_offset;
|
|
boolean t0_out_of_range;
|
|
boolean t1_out_of_range;
|
|
|
|
vector_from_points3d(point, line_point, &offset);
|
|
cross_product3d(vector, line_vector, &cross);
|
|
denominator = magnitude_squared3d_right_associated(&cross);
|
|
if (!(fabs(denominator) < _real_epsilon))
|
|
{
|
|
inverse_denominator = 1.0f / denominator;
|
|
scale_vector3d(&cross, inverse_denominator, &cross);
|
|
cross_product3d(&offset, line_vector, &numerator_cross0);
|
|
t0 = dot_product3d(&numerator_cross0, &cross);
|
|
cross_product3d(&offset, vector, &numerator_cross1);
|
|
t1 = dot_product3d(&numerator_cross1, &cross);
|
|
t0_out_of_range = t0 < 0.0f || t0 > 1.0f;
|
|
t1_out_of_range = t1 < 0.0f || t1 > 1.0f;
|
|
if (t0_out_of_range || t1_out_of_range)
|
|
{
|
|
d0 = REAL_MAX;
|
|
d1 = REAL_MAX;
|
|
if (t0_out_of_range)
|
|
{
|
|
clamped_t0 = t0 < 0.0f ? 0.0f : 1.0f;
|
|
point_from_line3d(point, vector, clamped_t0, &closest_point0);
|
|
d0 = point_to_line_distance_squared3d(
|
|
&closest_point0,
|
|
line_point,
|
|
line_vector);
|
|
}
|
|
if (t1_out_of_range)
|
|
{
|
|
clamped_t1 = t1 < 0.0f ? 0.0f : 1.0f;
|
|
point_from_line3d(line_point, line_vector, clamped_t1, &closest_point1);
|
|
d1 = point_to_line_distance_squared3d(
|
|
&closest_point1,
|
|
point,
|
|
vector);
|
|
}
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
942,
|
|
(d0 < REAL_MAX) || (d1 < REAL_MAX));
|
|
return d0 > d1 ? d1 : d0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dot = dot_product3d(vector, line_vector);
|
|
vector_magnitude_squared = magnitude_squared3d(vector);
|
|
if (vector_magnitude_squared > _real_epsilon)
|
|
{
|
|
inverse_vector_magnitude_squared = 1.0f / vector_magnitude_squared;
|
|
t0_start = offset.k * vector->k;
|
|
t0_start += offset.j * vector->j;
|
|
t0_start += offset.i * vector->i;
|
|
t0_start *= inverse_vector_magnitude_squared;
|
|
t0_end =
|
|
inverse_vector_magnitude_squared * dot +
|
|
t0_start;
|
|
t0 =
|
|
(PIN(t0_start, 0.0f, 1.0f) +
|
|
PIN(t0_end, 0.0f, 1.0f)) *
|
|
0.5f;
|
|
}
|
|
else
|
|
{
|
|
t0 = 0.0f;
|
|
}
|
|
|
|
line_magnitude_squared = magnitude_squared3d(line_vector);
|
|
if (line_magnitude_squared > _real_epsilon)
|
|
{
|
|
inverse_line_magnitude_squared = 1.0f / line_magnitude_squared;
|
|
t1_start =
|
|
-dot_product3d(&offset, line_vector) *
|
|
inverse_line_magnitude_squared;
|
|
t1_end =
|
|
inverse_line_magnitude_squared * dot +
|
|
t1_start;
|
|
t1 =
|
|
(PIN(t1_start, 0.0f, 1.0f) +
|
|
PIN(t1_end, 0.0f, 1.0f)) *
|
|
0.5f;
|
|
}
|
|
else
|
|
{
|
|
t1 = 0.0f;
|
|
}
|
|
}
|
|
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
986,
|
|
(t0 >= 0.0f) && (t0 <= 1.0f));
|
|
match_assert(
|
|
"c:\\halo\\SOURCE\\math\\real_math.c",
|
|
987,
|
|
(t1 >= 0.0f) && (t1 <= 1.0f));
|
|
point_from_line3d(point, vector, t0, &closest_point0);
|
|
point_from_line3d(line_point, line_vector, t1, &closest_point1);
|
|
vector_from_points3d(&closest_point0, &closest_point1, &closest_offset);
|
|
return magnitude_squared3d(&closest_offset);
|
|
}
|
|
|
|
|
|
boolean sphere_test_vector3d(
|
|
real_point3d const *center,
|
|
real radius,
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real *t,
|
|
real_vector3d *normal)
|
|
{
|
|
real_vector3d x;
|
|
real xv;
|
|
|
|
vector_from_points3d(center, point, &x);
|
|
xv = dot_product3d(&x, vector);
|
|
|
|
if (xv < 0)
|
|
{
|
|
real xx = magnitude_squared3d(&x);
|
|
real xx_rr = xx - radius*radius;
|
|
|
|
if (xx_rr <= 0)
|
|
{
|
|
*t = 0;
|
|
scale_vector3d(&x, reciprocal_square_root(xx), normal);
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
real d = vector->i*vector->i + vector->j*vector->j + vector->k*vector->k;
|
|
real vv = xv*xv - d*xx_rr;
|
|
|
|
if (vv >= 0)
|
|
{
|
|
real n = -(square_root(vv) + xv);
|
|
|
|
if (n <= d)
|
|
{
|
|
*t = n/d;
|
|
fast_normalize3d((real_vector3d *)point_from_line3d((real_point3d *)&x, vector, *t, (real_point3d *)normal));
|
|
return TRUE;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
boolean pill_test_vector3d(
|
|
real_point3d const *base,
|
|
real height,
|
|
real radius,
|
|
real_point3d const *point,
|
|
real_vector3d const *vector,
|
|
real *t,
|
|
real_vector3d *normal)
|
|
{
|
|
real_vector2d x;
|
|
real xv;
|
|
real xx_rr;
|
|
real d;
|
|
real vv;
|
|
real n;
|
|
real height_fraction;
|
|
real top_z;
|
|
real_point3d top;
|
|
boolean result = FALSE;
|
|
|
|
x.i = point->x - base->x;
|
|
x.j = point->y - base->y;
|
|
xv = x.i * vector->i + x.j * vector->j;
|
|
xx_rr = x.i * x.i + x.j * x.j - radius * radius;
|
|
if (xx_rr <= 0.0f)
|
|
{
|
|
n = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
d = vector->i * vector->i + vector->j * vector->j;
|
|
vv = xv * xv - d * xx_rr;
|
|
if (vv >= 0.0f)
|
|
{
|
|
n = -(square_root(vv) + xv);
|
|
if (n <= d)
|
|
{
|
|
n = n / d;
|
|
}
|
|
else
|
|
{
|
|
goto done;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
goto done;
|
|
}
|
|
}
|
|
|
|
result = TRUE;
|
|
height_fraction =
|
|
(point->z - base->z + n * vector->k) / (height * height);
|
|
if (height_fraction < 0.0f)
|
|
{
|
|
result = sphere_test_vector3d(base, radius, point, vector, t, normal);
|
|
}
|
|
else if (height_fraction > 1.0f)
|
|
{
|
|
top_z = base->z + height;
|
|
top.x = base->x;
|
|
top.y = base->y;
|
|
top.z = top_z;
|
|
result = sphere_test_vector3d(&top, radius, point, vector, t, normal);
|
|
}
|
|
else if (xv < 0.0f)
|
|
{
|
|
*t = n;
|
|
normal->i = x.i + n * vector->i;
|
|
normal->j = x.j + n * vector->j;
|
|
fast_normalize2d((real_vector2d *)normal);
|
|
normal->k = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
result = FALSE;
|
|
}
|
|
|
|
if (result && dot_product3d(normal, vector) > 0.0f)
|
|
{
|
|
result = FALSE;
|
|
}
|
|
|
|
done:
|
|
return result;
|
|
}
|
|
|
|
boolean point_from_planes3d(
|
|
real_plane3d const *plane0,
|
|
real_plane3d const *plane1,
|
|
real_plane3d const *plane2,
|
|
real_point3d *point)
|
|
{
|
|
real_vector3d cross;
|
|
real determinant;
|
|
real distance;
|
|
|
|
cross_product3d(&plane0->n, &plane1->n, &cross);
|
|
determinant = dot_product3d(&cross, &plane2->n);
|
|
if (!(fabs(determinant) < _real_epsilon))
|
|
{
|
|
cross_product3d(&plane1->n, &plane2->n, &cross);
|
|
distance = plane0->d;
|
|
point->x = cross.i * distance;
|
|
point->y = cross.j * distance;
|
|
point->z = cross.k * distance;
|
|
|
|
distance =
|
|
(cross_product3d(&plane2->n, &plane0->n, &cross),
|
|
plane1->d);
|
|
point->x = cross.i * distance + point->x;
|
|
point->y = cross.j * distance + point->y;
|
|
point->z = cross.k * distance + point->z;
|
|
|
|
distance =
|
|
(cross_product3d(&plane0->n, &plane1->n, &cross),
|
|
plane2->d);
|
|
determinant = 1.0f / determinant;
|
|
point->x = (cross.i * distance + point->x) * determinant;
|
|
point->y = (cross.j * distance + point->y) * determinant;
|
|
point->z = (cross.k * distance + point->z) * determinant;
|
|
return TRUE;
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
boolean line_from_planes3d(
|
|
real_plane3d const *plane0,
|
|
real_plane3d const *plane1,
|
|
real_point3d *point,
|
|
real_vector3d *direction)
|
|
{
|
|
real_vector3d local_direction;
|
|
real determinant;
|
|
real inverse_determinant;
|
|
real distance;
|
|
|
|
cross_product3d(&plane0->n, &plane1->n, direction);
|
|
determinant = magnitude_squared3d(direction);
|
|
if (!(fabs(determinant) < _real_epsilon))
|
|
{
|
|
cross_product3d(&plane1->n, direction, &local_direction);
|
|
distance = plane0->d;
|
|
point->x = local_direction.i * distance;
|
|
point->y = local_direction.j * distance;
|
|
point->z = local_direction.k * distance;
|
|
|
|
distance =
|
|
(cross_product3d(direction, &plane0->n, &local_direction),
|
|
plane1->d);
|
|
inverse_determinant = 1.0f / determinant;
|
|
point->x =
|
|
(local_direction.i * distance + point->x) *
|
|
inverse_determinant;
|
|
point->y =
|
|
(local_direction.j * distance + point->y) *
|
|
inverse_determinant;
|
|
point->z =
|
|
(local_direction.k * distance + point->z) *
|
|
inverse_determinant;
|
|
return TRUE;
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
void angular_accelerate_to_position(
|
|
real_vector3d *position,
|
|
real_vector3d const *position_desired,
|
|
real_vector3d *angular_velocity,
|
|
real angular_velocity_magnitude_maximum,
|
|
real angular_acceleration_magnitude_maximum)
|
|
{
|
|
real cosine_value;
|
|
real speed;
|
|
real speed_squared;
|
|
real_vector3d axis;
|
|
real_vector3d delta;
|
|
real delta_magnitude_squared;
|
|
real angle;
|
|
|
|
if (angular_acceleration_magnitude_maximum <= 0.0f &&
|
|
angular_velocity_magnitude_maximum <= 0.0f)
|
|
{
|
|
*angular_velocity = *global_zero_vector3d;
|
|
*position = *position_desired;
|
|
return;
|
|
}
|
|
|
|
cosine_value = dot_product3d(position, position_desired);
|
|
if (cosine_value < -1.0f)
|
|
{
|
|
cosine_value = -1.0f;
|
|
}
|
|
else if (cosine_value > 1.0f)
|
|
{
|
|
cosine_value = 1.0f;
|
|
}
|
|
|
|
speed_squared = arccosine(cosine_value);
|
|
speed_squared =
|
|
speed_squared * angular_acceleration_magnitude_maximum +
|
|
speed_squared * angular_acceleration_magnitude_maximum;
|
|
if (speed_squared >=
|
|
angular_velocity_magnitude_maximum *
|
|
angular_velocity_magnitude_maximum)
|
|
{
|
|
speed = angular_velocity_magnitude_maximum;
|
|
}
|
|
else
|
|
{
|
|
speed = square_root(speed_squared);
|
|
}
|
|
|
|
cross_product3d(position, position_desired, &axis);
|
|
normalize3d(&axis);
|
|
|
|
scale_vector3d(&axis, speed, &axis);
|
|
subtract_vectors3d(&axis, angular_velocity, &delta);
|
|
delta_magnitude_squared = magnitude_squared3d(&delta);
|
|
if (delta_magnitude_squared <
|
|
angular_acceleration_magnitude_maximum *
|
|
angular_acceleration_magnitude_maximum)
|
|
{
|
|
if (speed < 1.0000001e-6f)
|
|
{
|
|
*angular_velocity = *global_zero_vector3d;
|
|
*position = *position_desired;
|
|
return;
|
|
}
|
|
*angular_velocity = axis;
|
|
}
|
|
else
|
|
{
|
|
angular_acceleration_magnitude_maximum =
|
|
angular_acceleration_magnitude_maximum /
|
|
square_root(delta_magnitude_squared);
|
|
angular_velocity->i =
|
|
delta.i * angular_acceleration_magnitude_maximum +
|
|
angular_velocity->i;
|
|
angular_velocity->j =
|
|
delta.j * angular_acceleration_magnitude_maximum +
|
|
angular_velocity->j;
|
|
angular_velocity->k =
|
|
delta.k * angular_acceleration_magnitude_maximum +
|
|
angular_velocity->k;
|
|
}
|
|
|
|
axis = *angular_velocity;
|
|
angle = normalize3d(&axis);
|
|
if (angle != 0.0f)
|
|
{
|
|
rotate_vector_about_axis(position, &axis, sine(angle), cosine(angle));
|
|
normalize3d(position);
|
|
}
|
|
return;
|
|
}
|
|
|
|
|
|
boolean accelerate_to_velocity3d(
|
|
real_vector3d *velocity,
|
|
real_vector3d const *target_velocity,
|
|
real acceleration)
|
|
{
|
|
real_vector3d delta;
|
|
boolean result;
|
|
|
|
subtract_vectors3d(target_velocity, velocity, &delta);
|
|
if (limit3d(&delta, acceleration))
|
|
{
|
|
add_vectors3d(velocity, &delta, velocity);
|
|
result = FALSE;
|
|
}
|
|
else
|
|
{
|
|
*velocity = *target_velocity;
|
|
result = TRUE;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
boolean accerate_to_position3d(
|
|
real_point3d *position,
|
|
real_vector3d *velocity,
|
|
real_point3d const *target_position,
|
|
long unused,
|
|
real maximum_length)
|
|
{
|
|
volatile real delta[3];
|
|
real magnitude;
|
|
real scale;
|
|
|
|
delta[0] = target_position->x - position->x;
|
|
delta[1] = target_position->y - position->y;
|
|
delta[2] = target_position->z - position->z;
|
|
magnitude = normalize3d((real_vector3d *)(real *)delta);
|
|
if (magnitude == 0.0f)
|
|
{
|
|
delta[0] = 0.0f;
|
|
delta[1] = 0.0f;
|
|
delta[2] = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
scale = square_root(
|
|
magnitude * maximum_length +
|
|
magnitude * maximum_length);
|
|
delta[0] = delta[0] * scale;
|
|
delta[1] = delta[1] * scale;
|
|
delta[2] = delta[2] * scale;
|
|
}
|
|
|
|
if (accelerate_to_velocity3d(
|
|
velocity,
|
|
(real_vector3d *)(real *)delta,
|
|
maximum_length) &&
|
|
magnitude == 0.0f)
|
|
{
|
|
*position = *target_position;
|
|
return TRUE;
|
|
}
|
|
|
|
position->x = position->x + velocity->i;
|
|
position->y = velocity->j + position->y;
|
|
position->z = velocity->k + position->z;
|
|
return FALSE;
|
|
}
|
|
|
|
boolean accelerate_to_position(
|
|
real *position,
|
|
real *velocity,
|
|
real target_position,
|
|
real maximum_velocity,
|
|
real acceleration,
|
|
real minimum_position,
|
|
real maximum_position,
|
|
boolean periodic)
|
|
{
|
|
real current_position;
|
|
real current_velocity;
|
|
real delta;
|
|
real half_range;
|
|
real limit;
|
|
real speed;
|
|
real braking_distance;
|
|
real step_clamped;
|
|
real new_position;
|
|
real output_position;
|
|
real acceleration_squared;
|
|
boolean result;
|
|
|
|
current_position = *position;
|
|
current_velocity = *velocity;
|
|
acceleration_squared = acceleration * acceleration;
|
|
result = FALSE;
|
|
delta = target_position - current_position;
|
|
|
|
if (periodic)
|
|
{
|
|
half_range = (maximum_position - minimum_position) * 0.5f;
|
|
if (delta > half_range)
|
|
{
|
|
delta = delta - (half_range + half_range);
|
|
}
|
|
else if (delta < -half_range)
|
|
{
|
|
delta = half_range + half_range + delta;
|
|
}
|
|
}
|
|
|
|
limit = MIN(maximum_velocity, acceleration);
|
|
|
|
if (fabs(delta - current_velocity) <= limit)
|
|
{
|
|
current_velocity = 0.0f;
|
|
output_position = target_position;
|
|
if (target_position < minimum_position)
|
|
output_position = minimum_position;
|
|
else if (target_position > maximum_position)
|
|
output_position = maximum_position;
|
|
result = TRUE;
|
|
}
|
|
else
|
|
{
|
|
braking_distance = (maximum_velocity + maximum_velocity) * fabs(delta);
|
|
if (braking_distance >= acceleration_squared)
|
|
speed = acceleration;
|
|
else
|
|
speed = square_root(braking_distance);
|
|
if (delta < 0.0f)
|
|
speed = -speed;
|
|
step_clamped = speed - current_velocity;
|
|
if (fabs(step_clamped) > maximum_velocity)
|
|
{
|
|
if (step_clamped < 0.0f)
|
|
step_clamped = -maximum_velocity;
|
|
else
|
|
step_clamped = maximum_velocity;
|
|
}
|
|
current_velocity = current_velocity + step_clamped;
|
|
new_position = step_clamped * 0.5f + current_velocity + current_position;
|
|
if (periodic)
|
|
{
|
|
if (new_position < minimum_position)
|
|
new_position = maximum_position - minimum_position + new_position;
|
|
else if (new_position > maximum_position)
|
|
new_position = new_position - (maximum_position - minimum_position);
|
|
}
|
|
if (new_position < minimum_position)
|
|
new_position = minimum_position;
|
|
else if (new_position > maximum_position)
|
|
new_position = maximum_position;
|
|
output_position = new_position;
|
|
}
|
|
|
|
*velocity = current_velocity;
|
|
*position = output_position;
|
|
return result;
|
|
}
|
|
|
|
void accelerate_to_velocity(
|
|
real *position,
|
|
real *velocity,
|
|
real acceleration,
|
|
real target_velocity,
|
|
real minimum_position,
|
|
real maximum_position,
|
|
boolean periodic)
|
|
{
|
|
if (periodic)
|
|
{
|
|
real new_position = *position;
|
|
real new_velocity = *velocity;
|
|
real delta = target_velocity - new_velocity;
|
|
|
|
if (fabs(delta) > acceleration)
|
|
{
|
|
if (delta < 0.0f)
|
|
delta = -acceleration;
|
|
else
|
|
delta = acceleration;
|
|
}
|
|
|
|
new_velocity += delta;
|
|
new_position += new_velocity + delta * 0.5f;
|
|
if (new_position < minimum_position || new_position > maximum_position)
|
|
{
|
|
new_position = fmod(
|
|
new_position - minimum_position,
|
|
maximum_position - minimum_position) - minimum_position;
|
|
}
|
|
|
|
*position = new_position;
|
|
*velocity = new_velocity;
|
|
}
|
|
else
|
|
{
|
|
accelerate_to_position(
|
|
position,
|
|
velocity,
|
|
target_velocity < 0.0f ? minimum_position : maximum_position,
|
|
acceleration,
|
|
fabs(target_velocity),
|
|
minimum_position,
|
|
maximum_position,
|
|
FALSE);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
boolean valid_real_sine_cosine(
|
|
real sine,
|
|
real cosine)
|
|
{
|
|
return valid_realcmp(sine * sine + cosine * cosine, 1.0f);
|
|
}
|
|
|
|
/* ---------- private code */
|