mesh_collide.c

一个用于智能手机的多媒体库适合S60 WinCE的跨平台开发库

	inters_idx = 0;
	inters = 0;
	dist = (*closest);

	/*leaf, check for all faces*/
	for (i=0; i<n->nb_idx; i++) {
		Fixed res;
		IDX_TYPE *idx = &mesh->indices[3*n->indices[i]];
		if (gf_ray_hit_triangle(ray, 
					&mesh->vertices[idx[0]].pos, &mesh->vertices[idx[1]].pos, &mesh->vertices[idx[2]].pos,
					&res)) {
			if ((res>0) && (res<dist)) {
				dist = res;
				inters_idx = i;
				inters = 1;
			}
		}
	}

	if (inters) {
		(*closest) = dist;
		if (outPoint) {
			*outPoint = gf_vec_scale(ray->dir, dist);
			gf_vec_add(*outPoint, ray->orig, *outPoint);
		}
		if (outNormal) {
			IDX_TYPE *idx = &mesh->indices[3*n->indices[inters_idx]];;
			if (mesh->flags & MESH_IS_SMOOTHED) {
				gf_vec_diff(v1, mesh->vertices[idx[1]].pos, mesh->vertices[idx[0]].pos);
				gf_vec_diff(v2, mesh->vertices[idx[2]].pos, mesh->vertices[idx[0]].pos);
				*outNormal = gf_vec_cross(v1, v2);
				gf_vec_norm(outNormal);
			} else {
				*outNormal = mesh->vertices[idx[0]].normal;
			}
		}
		if (outTexCoords) {
			IDX_TYPE *idx = &mesh->indices[3*n->indices[inters_idx]];;
			ray_hit_triangle_get_u_v(ray, 
				&mesh->vertices[idx[0]].pos, &mesh->vertices[idx[1]].pos, &mesh->vertices[idx[2]].pos,
				&outTexCoords->x, &outTexCoords->y);
		}
	}
	return inters;
}

Bool gf_mesh_intersect_ray(GF_Mesh *mesh, GF_Ray *ray, SFVec3f *outPoint, SFVec3f *outNormal, SFVec2f *outTexCoords)
{
	Bool inters;
	u32 i, inters_idx;
	Fixed closest;
	/*no intersection on linesets/pointsets*/
	if (mesh->mesh_type != MESH_TRIANGLES) return 0;

	/*use aabbtree*/
	if (mesh->aabb_root) {
		closest = FIX_MAX;
		return gf_mesh_aabb_ray_hit(mesh, mesh->aabb_root, ray, &closest, outPoint, outNormal, outTexCoords);
	}

	/*check bbox intersection*/
	inters = gf_ray_hit_box(ray, mesh->bounds.min_edge, mesh->bounds.max_edge, NULL);
	if (!inters) return 0;

	inters_idx = 0;
	inters = 0;
	closest = FIX_MAX;
	for (i=0; i<mesh->i_count; i+=3) {
		Fixed res;
		IDX_TYPE *idx = &mesh->indices[i];
		if (gf_ray_hit_triangle(ray, 
					&mesh->vertices[idx[0]].pos, &mesh->vertices[idx[1]].pos, &mesh->vertices[idx[2]].pos,
					&res)) {
			if ((res>0) && (res<closest)) {
				closest = res;
				inters_idx = i;
				inters = 1;
			}
		}
	}

	if (inters) {
		if (outPoint) {
			*outPoint = gf_vec_scale(ray->dir, closest);
			gf_vec_add(*outPoint, ray->orig, *outPoint);
		}
		if (outNormal) {
			IDX_TYPE *idx = &mesh->indices[inters_idx];
			if (mesh->flags & MESH_IS_SMOOTHED) {
				SFVec3f v1, v2;
				gf_vec_diff(v1, mesh->vertices[idx[1]].pos, mesh->vertices[idx[0]].pos);
				gf_vec_diff(v2, mesh->vertices[idx[2]].pos, mesh->vertices[idx[0]].pos);
				*outNormal = gf_vec_cross(v1, v2);
				gf_vec_norm(outNormal);
			} else {
				*outNormal = mesh->vertices[idx[0]].normal;
			}
		}
		if (outTexCoords) {
			SFVec2f txres;
			IDX_TYPE *idx = &mesh->indices[inters_idx];
			txres.x = txres.y = 0;
			txres.x += mesh->vertices[idx[0]].texcoords.x;
			txres.x += mesh->vertices[idx[1]].texcoords.x;
			txres.x += mesh->vertices[idx[2]].texcoords.x;
			txres.y += mesh->vertices[idx[0]].texcoords.y;
			txres.y += mesh->vertices[idx[1]].texcoords.y;
			txres.y += mesh->vertices[idx[2]].texcoords.y;
			outTexCoords->x = txres.x / 3;
			outTexCoords->y = txres.y / 3;
		}
	}
	return inters;
}


static GFINLINE Bool mesh_collide_triangle(GF_Ray *ray, SFVec3f *v0, SFVec3f *v1, SFVec3f *v2, Fixed *dist)
{
	Fixed u, v, det;
	SFVec3f edge1, edge2, tvec, pvec, qvec;
	/* find vectors for two edges sharing vert0 */
	gf_vec_diff(edge1, *v1, *v0);
	gf_vec_diff(edge2, *v2, *v0);
	/* begin calculating determinant - also used to calculate U parameter */
	pvec = gf_vec_cross(ray->dir, edge2);
	/* if determinant is near zero, ray lies in plane of triangle */
	det = gf_vec_dot(edge1, pvec);
	if (ABS(det) < FIX_EPSILON) return 0;
	/* calculate distance from vert0 to ray origin */
	gf_vec_diff(tvec, ray->orig, *v0);
	/* calculate U parameter and test bounds */
	u = gf_divfix(gf_vec_dot(tvec, pvec), det);
	if ((u < 0) || (u > FIX_ONE)) return 0;
	/* prepare to test V parameter */
	qvec = gf_vec_cross(tvec, edge1);
	/* calculate V parameter and test bounds */
	v = gf_divfix(gf_vec_dot(ray->dir, qvec), det);
	if ((v < 0) || (u + v > FIX_ONE)) return 0;
	/* calculate t, ray intersects triangle */
	*dist = gf_divfix(gf_vec_dot(edge2, qvec), det);
	return 1;
}


static GFINLINE Bool sphere_box_overlap(SFVec3f sc, Fixed sq_rad, SFVec3f bmin, SFVec3f bmax)
{
	Fixed tmp, s, d, ext;
	d = 0;
	tmp = sc.x  - (bmin.x + bmax.x)/2;
	ext = (bmax.x-bmin.x)/2;
	s = tmp + ext;
	if (s<0) d += gf_mulfix(s, s);
	else {
		s = tmp - ext;
		if (s>0) d += gf_mulfix(s, s);
	}
	tmp = sc.y  - (bmin.y + bmax.y)/2;
	ext = (bmax.y-bmin.y)/2;
	s = tmp + ext;
	if (s<0) d += gf_mulfix(s, s);
	else {
		s = tmp - ext;
		if (s>0) d += gf_mulfix(s, s);
	}
	tmp = sc.z  - (bmin.z + bmax.z)/2;
	ext = (bmax.z-bmin.z)/2;
	s = tmp + ext;
	if (s<0) d += gf_mulfix(s, s);
	else {
		s = tmp - ext;
		if (s>0) d += gf_mulfix(s, s);
	}
	return (d<=sq_rad) ? 1 : 0;
}

Bool gf_mesh_closest_face_aabb(GF_Mesh *mesh, AABBNode *node, SFVec3f pos, Fixed min_dist, Fixed min_sq_dist, Fixed *min_col_dist, SFVec3f *outPoint)
{
	GF_Ray r;
	u32 i;
	SFVec3f v1, v2, n, resn;
	Bool inters, has_inter, need_norm;
	Fixed d;
	if (!sphere_box_overlap(pos, min_sq_dist, node->min, node->max)) return 0;
	if (node->pos) {
		if (gf_mesh_closest_face_aabb(mesh, node->pos, pos, min_dist, min_sq_dist, min_col_dist, outPoint)) return 1;
		return gf_mesh_closest_face_aabb(mesh, node->neg, pos, min_dist, min_sq_dist, min_col_dist, outPoint);
	}

	need_norm = (mesh->flags & MESH_IS_SMOOTHED) ? 1 : 0,

	r.orig = pos;
	has_inter = 0;
	for (i=0; i<node->nb_idx; i++) {
		IDX_TYPE *idx = &mesh->indices[3*node->indices[i]];
		if (need_norm) {
			gf_vec_diff(v1, mesh->vertices[idx[1]].pos, mesh->vertices[idx[0]].pos);
			gf_vec_diff(v2, mesh->vertices[idx[2]].pos, mesh->vertices[idx[0]].pos);
			n = gf_vec_cross(v1, v2);
			gf_vec_norm(&n);
		} else {
			n = mesh->vertices[idx[0]].normal;
		}

		/*intersect inverse normal from position to face with face*/
		r.dir = n;
		gf_vec_rev(r.dir);
		inters = mesh_collide_triangle(&r, 
					&mesh->vertices[idx[0]].pos, &mesh->vertices[idx[1]].pos, &mesh->vertices[idx[2]].pos,
					&d);

		if (inters) {
			/*we're behind the face, get inverse normal*/
			if (d<0) {
				d*= -1;
				n = r.dir;
			}
			if (d<=(*min_col_dist)) {
				has_inter = 1;
				(*min_col_dist) = d;
				resn = n;
			}
		}
	}
	if (has_inter) {
		resn = gf_vec_scale(resn, -(*min_col_dist));
		gf_vec_add(*outPoint, pos, resn);
	}
	return has_inter;
}

Bool gf_mesh_closest_face(GF_Mesh *mesh, SFVec3f pos, Fixed min_dist, SFVec3f *outPoint)
{
	GF_Ray r;
	u32 i;
	SFVec3f v1, v2, n, resn;
	Bool inters, has_inter, need_norm;
	Fixed d, dmax;

	/*check bounds*/
	gf_vec_diff(v1, mesh->bounds.center, pos);
	if (gf_vec_len(v1)>min_dist+mesh->bounds.radius) return 0;

	if (mesh->aabb_root) {
		d = min_dist * min_dist;
		dmax = min_dist;
		return gf_mesh_closest_face_aabb(mesh, mesh->aabb_root, pos, min_dist, d, &dmax, outPoint);
	}

	need_norm = (mesh->flags & MESH_IS_SMOOTHED) ? 1 : 0,

	r.orig = pos;
	has_inter = 0;
	dmax = min_dist;
	for (i=0; i<mesh->i_count; i+=3) {
		IDX_TYPE *idx = &mesh->indices[i];
		if (need_norm) {
			gf_vec_diff(v1, mesh->vertices[idx[1]].pos, mesh->vertices[idx[0]].pos);
			gf_vec_diff(v2, mesh->vertices[idx[2]].pos, mesh->vertices[idx[0]].pos);
			n = gf_vec_cross(v1, v2);
			gf_vec_norm(&n);
		} else {
			n = mesh->vertices[idx[0]].normal;
		}

		d = -gf_vec_dot(mesh->vertices[idx[0]].pos, n);
		d += gf_vec_dot(r.orig, n);
		if (fabs(d)>min_dist) continue;

		/*intersect inverse normal from position to face with face*/
		r.dir = n;
		gf_vec_rev(r.dir);
		inters = mesh_collide_triangle(&r, 
					&mesh->vertices[idx[0]].pos, &mesh->vertices[idx[1]].pos, &mesh->vertices[idx[2]].pos,
					&d);

		if (inters) {
			/*we're behind the face, get inverse normal*/
			if (d<0) {
				d*= -1;
				n = r.dir;
			}
			if (d<=dmax) {
				has_inter = 1;
				dmax = d;
				resn = n;
			}
		}
	}
	if (has_inter) {
		resn = gf_vec_scale(resn, -dmax);
		gf_vec_add(*outPoint, pos, resn);
	}
	return has_inter;
}