+
+
+/* Physics segment */
+
+static int triangle_raycast2d( v3f pA, v3f pB, v3f pC, v3f ray, float *height )
+{
+ v2f v0, v1, v2, vp, vp2;
+ float d, bca = 0.f, bcb = 0.f, bcc = 0.f;
+
+ v0[0] = pB[0] - pA[0];
+ v0[1] = pB[2] - pA[2];
+ v1[0] = pC[0] - pA[0];
+ v1[1] = pC[2] - pA[2];
+ v2[0] = pB[0] - pC[0];
+ v2[1] = pB[2] - pC[2];
+
+ d = 1.f / (v0[0]*v1[1] - v1[0]*v0[1]);
+
+#if 0
+ /* Backface culling */
+ if( v2_cross( v0, v1 ) > 0.f )
+ return;
+#endif
+
+ vp[0] = ray[0] - pA[0];
+ vp[1] = ray[2] - pA[2];
+
+ if( v2_cross( v0, vp ) > 0.f ) return 0;
+ if( v2_cross( vp, v1 ) > 0.f ) return 0;
+
+ vp2[0] = ray[0] - pB[0];
+ vp2[1] = ray[2] - pB[2];
+
+ if( v2_cross( vp2, v2 ) > 0.f ) return 0;
+
+ bcb = (vp[0]*v1[1] - v1[0]*vp[1]) * d;
+ bcc = (v0[0]*vp[1] - vp[0]*v0[1]) * d;
+ bca = 1.f - bcb - bcc;
+
+ *height = pA[1]*bca + pB[1]*bcb + pC[1]*bcc;
+ return 1;
+}
+
+/* Temporary */
+static int sample_scene_height( scene *pscene, v3f pos, v3f norm )
+{
+ for( int i=0; i<pscene->indice_count/3; i++ )
+ {
+ u32 *tri = &pscene->indices[i*3];
+
+ float *pA = pscene->verts[tri[0]].co,
+ *pB = pscene->verts[tri[1]].co,
+ *pC = pscene->verts[tri[2]].co;
+
+ float height;
+ if( triangle_raycast2d( pA, pB, pC, pos, &height ))
+ {
+ pos[1] = height;
+
+ if( norm )
+ {
+ v3f v0, v1;
+ v3_sub( pA, pB, v0 );
+ v3_sub( pC, pB, v1 );
+ v3_cross( v1, v0, norm );
+ v3_normalize( norm );
+ }
+
+ return 1;
+ }
+ }
+ return 0;
+}
+
+static void sample_scene_normal( scene *pscene, v3f pos, v3f normal )
+{
+ for( int i=0; i<pscene->indice_count/3; i++ )
+ {
+ u32 *tri = &pscene->indices[i*3];
+
+ float height;
+ if( triangle_raycast2d(
+ pscene->verts[ tri[0] ].co,
+ pscene->verts[ tri[1] ].co,
+ pscene->verts[ tri[2] ].co, pos, &height ))
+ {
+ v3f v0, v1;
+
+ v3_sub( pscene->verts[ tri[1] ].co,
+ pscene->verts[ tri[0] ].co,
+ v0 );
+
+ v3_sub( pscene->verts[ tri[2] ].co,
+ pscene->verts[ tri[0] ].co,
+ v1 );
+
+ v3_cross( v0, v1, normal );
+ v3_normalize( normal );
+ return;
+ }
+ }
+
+ normal[0] = 0.0f;
+ normal[1] = 1.0f;
+ normal[2] = 0.0f;
+}
+
+struct bvh_node
+{
+ boxf bbx;
+
+ /* if il is 0, this is a leaf */
+ u32 il, count;
+ union{ u32 ir, start; };
+};
+
+static void bvh_update_bounds( scene *s, u32 inode )
+{
+ bvh_node *node = &s->bvh.nodes[ inode ];
+
+ box_init_inf( node->bbx );
+ for( u32 i=0; i<node->count; i++ )
+ {
+ u32 idx = node->start+i;
+ model_vert *pa = &s->verts[ s->indices[idx*3+0] ],
+ *pb = &s->verts[ s->indices[idx*3+1] ],
+ *pc = &s->verts[ s->indices[idx*3+2] ];
+
+ box_addpt( node->bbx, pa->co );
+ box_addpt( node->bbx, pb->co );
+ box_addpt( node->bbx, pc->co );
+ }
+}
+
+static void bvh_subdiv( scene *s, u32 inode )
+{
+ bvh_node *node = &s->bvh.nodes[ inode ];
+
+ v3f extent;
+ v3_sub( node->bbx[1], node->bbx[0], extent );
+
+ int axis = 0;
+ if( extent[1] > extent[0] ) axis = 1;
+ if( extent[2] > extent[axis] ) axis = 2;
+
+ float split = node->bbx[0][axis] + extent[axis]*0.5f;
+
+ i32 i = node->start,
+ j = i + node->count-1;
+
+ while( i <= j )
+ {
+ u32 *ti = &s->indices[i*3];
+
+ float a = s->verts[ti[0]].co[axis],
+ b = s->verts[ti[1]].co[axis],
+ c = s->verts[ti[2]].co[axis];
+
+ if( ((a+b+c) / 3.0f) < split )
+ i ++;
+ else
+ {
+ /* Swap triangle indices */
+ u32 *tj = &s->indices[j*3];
+ u32 temp[3];
+ temp[0] = ti[0];
+ temp[1] = ti[1];
+ temp[2] = ti[2];
+
+ ti[0] = tj[0];
+ ti[1] = tj[1];
+ ti[2] = tj[2];
+
+ tj[0] = temp[0];
+ tj[1] = temp[1];
+ tj[2] = temp[2];
+
+ j --;
+ }
+ }
+
+ u32 left_count = i - node->start;
+ if( left_count == 0 || left_count == node->count ) return;
+
+ u32 il = s->bvh.node_count ++,
+ ir = s->bvh.node_count ++;
+
+ struct bvh_node *lnode = &s->bvh.nodes[il],
+ *rnode = &s->bvh.nodes[ir];
+
+ lnode->start = node->start;
+ lnode->count = left_count;
+ rnode->start = i;
+ rnode->count = node->count - left_count;
+
+ node->il = il;
+ node->ir = ir;
+ node->count = 0;
+
+ bvh_update_bounds( s, il );
+ bvh_update_bounds( s, ir );
+ bvh_subdiv( s, il );
+ bvh_subdiv( s, ir );
+}
+
+static void bvh_create( scene *s )
+{
+ u32 triangle_count = s->indice_count / 3;
+ s->bvh.nodes = malloc( sizeof(struct bvh_node) * (triangle_count*2-1) );
+
+ bvh_node *root = &s->bvh.nodes[0];
+ s->bvh.node_count = 1;
+
+ root->il = 0;
+ root->ir = 0;
+ root->count = triangle_count;
+ root->start = 0;
+
+ bvh_update_bounds( s, 0 );
+ bvh_subdiv( s, 0 );
+
+ s->bvh.nodes =
+ realloc( s->bvh.nodes, sizeof(struct bvh_node) * s->bvh.node_count );
+
+ vg_success( "BVH done, size: %u/%u\n", s->bvh.node_count,
+ (triangle_count*2-1) );
+}
+
+static void bvh_debug_node( scene *s, u32 inode, v3f pos, u32 colour )
+{
+ struct bvh_node *node = &s->bvh.nodes[ inode ];
+
+ if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
+ (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
+ {
+ if( !node->count )
+ {
+ vg_line_boxf( node->bbx, colour );
+
+ bvh_debug_node( s, node->il, pos, colour );
+ bvh_debug_node( s, node->ir, pos, colour );
+ }
+ else
+ {
+ vg_line_boxf( node->bbx, 0xff00ff00 );
+ for( u32 i=0; i<node->count; i++ )
+ {
+ u32 idx = (node->start+i)*3;
+
+ model_vert *pa = &s->verts[ s->indices[ idx+0 ] ],
+ *pb = &s->verts[ s->indices[ idx+1 ] ],
+ *pc = &s->verts[ s->indices[ idx+2 ] ];
+
+ vg_line( pa->co, pb->co, 0xff0000ff );
+ vg_line( pb->co, pc->co, 0xff0000ff );
+ vg_line( pc->co, pa->co, 0xff0000ff );
+ }
+ }
+ }
+}
+
+static void bvh_debug( scene *s, v3f pos )
+{
+ bvh_debug_node( s, 0, pos, 0x4000ffa8 );
+}
+
+static int bvh_scene_sample_node( scene *s, u32 inode, v3f pos, v3f norm )
+{
+ bvh_node *node = &s->bvh.nodes[ inode ];
+
+ if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
+ (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
+ {
+ if( !node->count )
+ {
+ if( bvh_scene_sample_node( s, node->il, pos, norm )) return 1;
+ if( bvh_scene_sample_node( s, node->ir, pos, norm )) return 1;
+ }
+ else
+ {
+ for( u32 i=0; i<node->count; i++ )
+ {
+ u32 idx = (node->start+i)*3;
+ model_vert *pa = &s->verts[ s->indices[ idx+0 ] ],
+ *pb = &s->verts[ s->indices[ idx+1 ] ],
+ *pc = &s->verts[ s->indices[ idx+2 ] ];
+
+ float height;
+ if( triangle_raycast2d( pa->co, pb->co, pc->co, pos, &height ))
+ {
+ pos[1] = height;
+
+ if( norm )
+ {
+ v3f v0, v1;
+ v3_sub( pa->co, pb->co, v0 );
+ v3_sub( pc->co, pb->co, v1 );
+ v3_cross( v1, v0, norm );
+ v3_normalize( norm );
+ }
+
+ return 1;
+ }
+ }
+ }
+ }
+
+ return 0;
+}
+
+static int bvh_scene_sample( scene *s, v3f pos, v3f norm)
+{
+ return bvh_scene_sample_node( s, 0, pos, norm );
+}
+
+typedef struct ray_hit ray_hit;
+struct ray_hit
+{
+ float dist;
+ u32 *tri;
+ v3f pos, normal;
+};
+
+int ray_aabb( boxf box, v3f co, v3f dir, float dist )
+{
+ v3f v0, v1;
+ float tmin, tmax;
+
+ v3_sub( box[0], co, v0 );
+ v3_sub( box[1], co, v1 );
+ v3_div( v0, dir, v0 );
+ v3_div( v1, dir, v1 );
+
+ tmin = vg_minf( v0[0], v1[0] );
+ tmax = vg_maxf( v0[0], v1[0] );
+ tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] ));
+ tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] ));
+ tmin = vg_maxf( tmin, vg_minf( v0[2], v1[2] ));
+ tmax = vg_minf( tmax, vg_maxf( v0[2], v1[2] ));
+
+ return tmax >= tmin && tmin < dist && tmax > 0;
+}
+
+static int bvh_ray_tri( scene *sc, u32 *tri, v3f co, v3f dir, ray_hit *hit )
+{
+ float const kEpsilon = 0.00001f;
+
+ v3f v0, v1, h, s, q, n;
+ float a,f,u,v,t;
+
+ float *pa = sc->verts[tri[0]].co,
+ *pb = sc->verts[tri[1]].co,
+ *pc = sc->verts[tri[2]].co;
+
+ v3_sub( pb, pa, v0 );
+ v3_sub( pc, pa, v1 );
+ v3_cross( dir, v1, h );
+ v3_cross( v0, v1, n );
+
+ if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */
+ return 0;
+
+ /* Parralel */
+ a = v3_dot( v0, h );
+ if( a > -kEpsilon && a < kEpsilon )
+ return 0;
+
+ f = 1.0f/a;
+ v3_sub( co, pa, s );
+
+ u = f * v3_dot(s, h);
+ if( u < 0.0f || u > 1.0f )
+ return 0;
+
+ v3_cross( s, v0, q );
+ v = f * v3_dot( dir, q );
+ if( v < 0.0f || u+v > 1.0f )
+ return 0;
+
+ t = f * v3_dot(v1, q);
+ if( t > kEpsilon && t < hit->dist )
+ {
+ hit->dist = t;
+ hit->tri = tri;
+ return 1;
+ }
+ else return 0;
+}
+
+static int bvh_ray( scene *s, u32 inode, v3f co, v3f dir, ray_hit *hit )
+{
+ bvh_node *node = &s->bvh.nodes[ inode ];
+
+ if( !ray_aabb( node->bbx, co, dir, hit->dist ))
+ return 0;
+
+ int count = 0;
+
+ if( node->count )
+ {
+ for( u32 i=0; i<node->count; i++ )
+ {
+ u32 *indices = &s->indices[ (node->start+i)*3 ];
+ count += bvh_ray_tri( s, indices, co, dir, hit );
+ }
+ }
+ else
+ {
+ count += bvh_ray( s, node->il, co, dir, hit );
+ count += bvh_ray( s, node->ir, co, dir, hit );
+ }
+
+ return count;
+}
+
+static int bvh_raycast( scene *s, v3f co, v3f dir, ray_hit *hit )
+{
+ v3f pb;
+ v3_muladds( co, dir, hit->dist, pb );
+
+ int count = bvh_ray( s, 0, co, dir, hit );
+
+ if( count )
+ {
+ vg_line( co, pb, 0xff00ffff );
+
+ v3f v0, v1;
+
+ float *pa = s->verts[hit->tri[0]].co,
+ *pb = s->verts[hit->tri[1]].co,
+ *pc = s->verts[hit->tri[2]].co;
+
+ v3_sub( pa, pb, v0 );
+ v3_sub( pc, pb, v1 );
+ v3_cross( v1, v0, hit->normal );
+ v3_normalize( hit->normal );
+ v3_muladds( co, dir, hit->dist, hit->pos );
+ }
+ else
+ vg_line( co, pb, 0xff0000ff );
+
+ return count;
+}
+
+#endif