everything

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 01734e8f2d74760ab32659165a1a51c633a4785c..82e793437994f2bbf0616bd79e6af3d356ca667b 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -11,8 +11,6 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
 
 #define RB_DEPR 
 
-#include "world.h"
-
 #define k_rb_delta (1.0f/60.0f)
 
 typedef struct rigidbody rigidbody;
@@ -108,6 +106,8 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
    v3_cross( n, tx, ty );
 }
 
+#include "world.h"
+
 static void rb_build_manifold( rigidbody *rb )
 {
    v3f *box = rb->bbx;
@@ -432,4 +432,53 @@ static void rb_debug( rigidbody *rb, u32 colour )
    vg_line( p100, p010, colour );
 }
 
+/*
+ * out penetration distance, normal
+ */
+static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
+{
+   v3f local;
+   m4x3_mulv( rb->to_local, pos, local );
+
+   if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
+       local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
+       local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
+   {
+      v3f area, com, comrel;
+      v3_add( rb->bbx[0], rb->bbx[1], com );
+      v3_muls( com, 0.5f, com );
+
+      v3_sub( rb->bbx[1], rb->bbx[0], area );
+      v3_sub( local, com, comrel );
+      v3_div( comrel, area, comrel );
+
+      int axis = 0;
+      float max_mag = fabsf(comrel[0]);
+      
+      if( fabsf(comrel[1]) > max_mag )
+      {
+         axis = 1;
+         max_mag = fabsf(comrel[1]);
+      }
+      if( fabsf(comrel[2]) > max_mag )
+      {
+         axis = 2;
+         max_mag = fabsf(comrel[2]);
+      }
+      
+      v3_zero( normal );
+      normal[axis] = vg_signf(comrel[axis]);
+
+      if( normal[axis] < 0.0f )
+         *pen = local[axis] - rb->bbx[0][axis];
+      else
+         *pen = rb->bbx[1][axis] - local[axis];
+
+      m3x3_mulv( rb->to_world, normal, normal );
+      return 1;
+   }
+
+   return 0;
+}
+
 #endif /* RIGIDBODY_H */