2 files changed, 39 insertions, 16 deletions
diff --git a/src/util/aabb.h b/src/util/aabb.h
index 4d74c88..96e40bf 100644
--- a/src/util/aabb.h
+++ b/src/util/aabb.h
@@ -13,19 +13,3 @@ inline F32 aabb_support_radius( const AABB& aabb, const VEC3& dir ) {
   VEC3 half = aabb_half( aabb );
   return fabsf(dir.x) * half.x + fabsf(dir.y) * half.y + fabsf(dir.z) * half.z;
 }
-
-// returns positive radius in X and negative in Y
-inline VEC2 aabb_extend_at_feet( const AABB& aabb, const VEC3& dir ) {
-  VEC3 half = aabb_half( aabb );
-
-  F32 hx = half.x;
-  F32 hy = half.y;
-  F32 hz = half.z * 2;
-
-  F32 lateral = fabsf( dir.x ) * hx + fabsf( dir.y ) * hy;
-
-  F32 pos_z = dir.z > 0 ? dir.z * hz : 0.f;
-  F32 neg_z = dir.z < 0 ? (-dir.z) * hz : 0.f;
-
-  return { lateral + pos_z, lateral + neg_z };
-}
diff --git a/src/util/vector.h b/src/util/vector.h
index 33f6551..314131c 100644
--- a/src/util/vector.h
+++ b/src/util/vector.h
@@ -190,3 +190,42 @@ inline F32 vec_angle( VEC2 v1, VEC2 v2 ) {
   VEC2 d = v2 - v1;
   return remainderf( atan2f( d.y, d.x ) * RADPI, 360.f );
 }
+
+inline VEC3 angle_vectors( VEC3 angles ) {
+  F32 sp, sy, cp, cy;
+  sp = sinf( angles.x * PIRAD );
+  cp = cosf( angles.x * PIRAD );
+  sy = sinf( angles.y * PIRAD );
+  cy = cosf( angles.y * PIRAD );
+
+  return VEC3{ cp * cy, cp * sy, -sp };
+}
+
+inline void angle_vectors( const VEC3& angles, VEC3* forward, VEC3* right, VEC3* up ) {
+  F32 sr, sp, sy, cr, cp, cy;
+
+  sp = sinf( angles.x * PIRAD );
+  cp = cosf( angles.x * PIRAD );
+  sy = sinf( angles.y * PIRAD );
+  cy = cosf( angles.y * PIRAD );
+  sr = sinf( angles.z * PIRAD );
+  cr = cosf( angles.z * PIRAD );
+
+  if ( forward ) {
+    forward->x = cp * cy;
+    forward->y = cp * sy;
+    forward->z = -sp;
+  }
+
+  if ( right ) {
+    right->x = -1 * sr * sp * cy + -1 * cr * -sy;
+    right->y = -1 * sr * sp * sy + -1 * cr * cy;
+    right->z = -1 * sr * cp;
+  }
+
+  if ( up ) {
+    up->x = cr * sp * cy + -sr * -sy;
+    up->y = cr * sp * sy + -sr * cy;
+    up->z = cr * cp;
+  }
+}