*/
#include "common.h"
+#include "bvh.h"
+
static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
+static bh_system bh_system_rigidbodies;
#ifndef RIGIDBODY_H
#define RIGIDBODY_H
-#include "bvh.h"
-
#define RB_DEPR
-
-#define k_rb_delta (1.0f/60.0f)
+#define k_rb_rate 60.0f
+#define k_rb_delta (1.0f/k_rb_rate)
typedef struct rigidbody rigidbody;
struct rigidbody
{
v3f co, v, I;
v4f q;
+
+ enum rb_shape
+ {
+ k_rb_shape_box,
+ k_rb_shape_capsule
+ }
+ type;
+ v3f top, bottom;
+ float radius;
+
boxf bbx, bbx_world;
float inv_mass;
/* intergrate velocity */
v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
-
v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
/* inegrate inertia */
}
-
RB_DEPR
static void rb_constraint_angle( rigidbody *rba, v3f va,
rigidbody *rbb, v3f vb,
v3_add( impulse, ra->I, ra->I );
#if 0
+ /*
+ * this could be used for spring joints
+ * its not good for position constraint
+ */
v3f impulse;
v3_muls( delta, 0.5f*spring, impulse );
static void rb_debug( rigidbody *rb, u32 colour )
{
v3f *box = rb->bbx;
- v3f p000, p001, p010, p011, p100, p101, p110, p111;
-
- p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
- p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
- p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
- p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
-
- p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
- p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
- p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
- p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
-
- m4x3_mulv( rb->to_world, p000, p000 );
- m4x3_mulv( rb->to_world, p001, p001 );
- m4x3_mulv( rb->to_world, p010, p010 );
- m4x3_mulv( rb->to_world, p011, p011 );
- m4x3_mulv( rb->to_world, p100, p100 );
- m4x3_mulv( rb->to_world, p101, p101 );
- m4x3_mulv( rb->to_world, p110, p110 );
- m4x3_mulv( rb->to_world, p111, p111 );
-
- vg_line( p000, p001, colour );
- vg_line( p001, p011, colour );
- vg_line( p011, p010, colour );
- vg_line( p010, p000, colour );
-
- vg_line( p100, p101, colour );
- vg_line( p101, p111, colour );
- vg_line( p111, p110, colour );
- vg_line( p110, p100, colour );
-
- vg_line( p100, p000, colour );
- vg_line( p101, p001, colour );
- vg_line( p110, p010, colour );
- vg_line( p111, p011, colour );
-
- vg_line( p000, p110, colour );
- vg_line( p100, p010, colour );
+ vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
}
/*
m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
+ vg_line_boxf_transformed( rb_static->to_world, rb_static->bbx, 0xff0000ff );
+
int count = 0;
for( int i=0; i<8; i++ )
}
}
+/*
+ * Capsule phyics
+ */
+
+static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
+ float *s, float *t, v3f c1, v3f c2)
+{
+ v3f d1,d2,r;
+ v3_sub( q1, p1, d1 );
+ v3_sub( q2, p2, d2 );
+ v3_sub( p1, p2, r );
+
+ float a = v3_length2( d1 ),
+ e = v3_length2( d2 ),
+ f = v3_dot( d2, r );
+
+ const float kEpsilon = 0.0001f;
+
+ if( a <= kEpsilon && e <= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = 0.0f;
+ v3_copy( p1, c1 );
+ v3_copy( p2, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+
+ return v3_length2( v0 );
+ }
+
+ if( a<= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = vg_clampf( f / e, 0.0f, 1.0f );
+ }
+ else
+ {
+ float c = v3_dot( d1, r );
+ if( e <= kEpsilon )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else
+ {
+ float b = v3_dot(d1,d2),
+ d = a*e-b*b;
+
+ if( d != 0.0f )
+ {
+ *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+ }
+ else
+ {
+ *s = 0.0f;
+ }
+
+ *t = (b*(*s)+f) / e;
+
+ if( *t < 0.0f )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else if( *t > 1.0f )
+ {
+ *t = 1.0f;
+ *s = vg_clampf((b-c)/a,0.0f,1.0f);
+ }
+ }
+ }
+
+ v3_muladds( p1, d1, *s, c1 );
+ v3_muladds( p2, d2, *t, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+ return v3_length2( v0 );
+}
+
+static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+{
+ v3f v0, v1;
+ v3_sub( b, a, v0 );
+ v3_sub( point, a, v1 );
+
+ float t = v3_dot( v1, v0 ) / v3_length2(v0);
+ v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
+}
+
+/* Real-Time Collision Detection */
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ float d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
+ {
+ v3_copy( tri[0], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ float d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Edge region of AB */
+ float vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ float v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ float d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of AC */
+ float vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ float w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of BC */
+ float va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ float w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ float d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
+}
+
+static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
+ v3f co, v3f norm, float *p )
+{
+ v3f delta;
+ closest_on_triangle( c, tri, co );
+
+ v3_sub( c, co, delta );
+
+
+ float d = v3_length2( delta );
+ if( d < r*r )
+ {
+ v3f ab, ac, tn;
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_cross( ac, ab, tn );
+
+ if( v3_dot( delta, tn ) > 0.0f )
+ v3_muls( delta, -1.0f, delta );
+
+ vg_line_pt3( co, 0.05f, 0xff00ff00 );
+
+ d = sqrtf(d);
+ v3_muls( delta, 1.0f/d, norm );
+
+ *p = r-d;
+ return 1;
+ }
+
+ return 0;
+}
+
+static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
+{
+ v3f last = { 0.0f, 0.0f, radius };
+ m4x3f lower, upper;
+ m3x3_copy( m, lower );
+ m3x3_copy( m, upper );
+ m4x3_mulv( m, (v3f){0.0f,-height*0.5f+radius,0.0f}, lower[3] );
+ m4x3_mulv( m, (v3f){0.0f, height*0.5f-radius,0.0f}, upper[3] );
+
+ for( int i=0; i<16; i++ )
+ {
+ float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
+ s = sinf(t),
+ c = cosf(t);
+
+ v3f p = { s*radius, 0.0f, c*radius };
+
+ v3f p0, p1;
+ m4x3_mulv( lower, p, p0 );
+ m4x3_mulv( lower, last, p1 );
+ vg_line( p0, p1, colour );
+
+ m4x3_mulv( upper, p, p0 );
+ m4x3_mulv( upper, last, p1 );
+ vg_line( p0, p1, colour );
+
+ v3_copy( p, last );
+ }
+
+ for( int i=0; i<4; i++ )
+ {
+ float t = ((float)(i) * (1.0f/4.0f)) * VG_PIf * 2.0f,
+ s = sinf(t),
+ c = cosf(t);
+
+ v3f p = { s*radius, 0.0f, c*radius };
+
+ v3f p0, p1;
+ m4x3_mulv( lower, p, p0 );
+ m4x3_mulv( upper, p, p1 );
+ vg_line( p0, p1, colour );
+
+ m4x3_mulv( lower, (v3f){0.0f,-radius,0.0f}, p0 );
+ m4x3_mulv( upper, (v3f){0.0f, radius,0.0f}, p1 );
+ vg_line( p0, p1, colour );
+ }
+}
+
/*
* BVH implementation, this is ONLY for static rigidbodies, its to slow for
* realtime use.
.item_centroid = rb_bh_centroid,
.item_swap = rb_bh_swap,
.item_debug = rb_bh_debug,
- .cast_ray = NULL,
-
- .item_size = sizeof(rigidbody)
+ .cast_ray = NULL
};
#endif /* RIGIDBODY_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff