diff options
Diffstat (limited to 'source/blender/blenkernel/intern/bvhutils.c')
-rw-r--r-- | source/blender/blenkernel/intern/bvhutils.c | 577 |
1 files changed, 577 insertions, 0 deletions
diff --git a/source/blender/blenkernel/intern/bvhutils.c b/source/blender/blenkernel/intern/bvhutils.c new file mode 100644 index 00000000000..ae449843d2a --- /dev/null +++ b/source/blender/blenkernel/intern/bvhutils.c @@ -0,0 +1,577 @@ +/** + * + * $Id$ + * + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) Blender Foundation. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): André Pinto. + * + * ***** END GPL LICENSE BLOCK ***** + */ +#include <stdio.h> +#include <string.h> +#include <math.h> + +#include "BKE_bvhutils.h" + +#include "DNA_object_types.h" +#include "DNA_modifier_types.h" +#include "DNA_meshdata_types.h" + +#include "BKE_DerivedMesh.h" +#include "BKE_utildefines.h" +#include "BKE_deform.h" +#include "BKE_cdderivedmesh.h" +#include "BKE_displist.h" +#include "BKE_global.h" + +#include "BLI_arithb.h" + +/* Math stuff for ray casting on mesh faces and for nearest surface */ + +static float ray_tri_intersection(const BVHTreeRay *ray, const float m_dist, const float *v0, const float *v1, const float *v2) +{ + float dist; + + if(RayIntersectsTriangle((float*)ray->origin, (float*)ray->direction, (float*)v0, (float*)v1, (float*)v2, &dist, NULL)) + return dist; + + return FLT_MAX; +} + +static float sphereray_tri_intersection(const BVHTreeRay *ray, float radius, const float m_dist, const float *v0, const float *v1, const float *v2) +{ + + float idist; + float p1[3]; + float plane_normal[3], hit_point[3]; + + CalcNormFloat((float*)v0, (float*)v1, (float*)v2, plane_normal); + + VECADDFAC( p1, ray->origin, ray->direction, m_dist); + if(SweepingSphereIntersectsTriangleUV((float*)ray->origin, p1, radius, (float*)v0, (float*)v1, (float*)v2, &idist, hit_point)) + { + return idist * m_dist; + } + + return FLT_MAX; +} + + +/* + * Function adapted from David Eberly's distance tools (LGPL) + * http://www.geometrictools.com/LibFoundation/Distance/Distance.html + */ +static float nearest_point_in_tri_surface(const float *v0,const float *v1,const float *v2,const float *p, int *v, int *e, float *nearest ) +{ + float diff[3]; + float e0[3]; + float e1[3]; + float A00; + float A01; + float A11; + float B0; + float B1; + float C; + float Det; + float S; + float T; + float sqrDist; + int lv = -1, le = -1; + + VECSUB(diff, v0, p); + VECSUB(e0, v1, v0); + VECSUB(e1, v2, v0); + + A00 = INPR ( e0, e0 ); + A01 = INPR( e0, e1 ); + A11 = INPR ( e1, e1 ); + B0 = INPR( diff, e0 ); + B1 = INPR( diff, e1 ); + C = INPR( diff, diff ); + Det = fabs( A00 * A11 - A01 * A01 ); + S = A01 * B1 - A11 * B0; + T = A01 * B0 - A00 * B1; + + if ( S + T <= Det ) + { + if ( S < 0.0f ) + { + if ( T < 0.0f ) // Region 4 + { + if ( B0 < 0.0f ) + { + T = 0.0f; + if ( -B0 >= A00 ) + { + S = (float)1.0; + sqrDist = A00 + 2.0f * B0 + C; + lv = 1; + } + else + { + if(fabs(A00) > FLT_EPSILON) + S = -B0/A00; + else + S = 0.0f; + sqrDist = B0 * S + C; + le = 0; + } + } + else + { + S = 0.0f; + if ( B1 >= 0.0f ) + { + T = 0.0f; + sqrDist = C; + lv = 0; + } + else if ( -B1 >= A11 ) + { + T = 1.0f; + sqrDist = A11 + 2.0f * B1 + C; + lv = 2; + } + else + { + if(fabs(A11) > FLT_EPSILON) + T = -B1 / A11; + else + T = 0.0f; + sqrDist = B1 * T + C; + le = 1; + } + } + } + else // Region 3 + { + S = 0.0f; + if ( B1 >= 0.0f ) + { + T = 0.0f; + sqrDist = C; + lv = 0; + } + else if ( -B1 >= A11 ) + { + T = 1.0f; + sqrDist = A11 + 2.0f * B1 + C; + lv = 2; + } + else + { + if(fabs(A11) > FLT_EPSILON) + T = -B1 / A11; + else + T = 0.0; + sqrDist = B1 * T + C; + le = 1; + } + } + } + else if ( T < 0.0f ) // Region 5 + { + T = 0.0f; + if ( B0 >= 0.0f ) + { + S = 0.0f; + sqrDist = C; + lv = 0; + } + else if ( -B0 >= A00 ) + { + S = 1.0f; + sqrDist = A00 + 2.0f * B0 + C; + lv = 1; + } + else + { + if(fabs(A00) > FLT_EPSILON) + S = -B0 / A00; + else + S = 0.0f; + sqrDist = B0 * S + C; + le = 0; + } + } + else // Region 0 + { + // Minimum at interior lv + float invDet; + if(fabs(Det) > FLT_EPSILON) + invDet = 1.0f / Det; + else + invDet = 0.0f; + S *= invDet; + T *= invDet; + sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0) + + T * ( A01 * S + A11 * T + 2.0f * B1 ) + C; + } + } + else + { + float tmp0, tmp1, numer, denom; + + if ( S < 0.0f ) // Region 2 + { + tmp0 = A01 + B0; + tmp1 = A11 + B1; + if ( tmp1 > tmp0 ) + { + numer = tmp1 - tmp0; + denom = A00 - 2.0f * A01 + A11; + if ( numer >= denom ) + { + S = 1.0f; + T = 0.0f; + sqrDist = A00 + 2.0f * B0 + C; + lv = 1; + } + else + { + if(fabs(denom) > FLT_EPSILON) + S = numer / denom; + else + S = 0.0f; + T = 1.0f - S; + sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0 ) + + T * ( A01 * S + A11 * T + 2.0f * B1 ) + C; + le = 2; + } + } + else + { + S = 0.0f; + if ( tmp1 <= 0.0f ) + { + T = 1.0f; + sqrDist = A11 + 2.0f * B1 + C; + lv = 2; + } + else if ( B1 >= 0.0f ) + { + T = 0.0f; + sqrDist = C; + lv = 0; + } + else + { + if(fabs(A11) > FLT_EPSILON) + T = -B1 / A11; + else + T = 0.0f; + sqrDist = B1 * T + C; + le = 1; + } + } + } + else if ( T < 0.0f ) // Region 6 + { + tmp0 = A01 + B1; + tmp1 = A00 + B0; + if ( tmp1 > tmp0 ) + { + numer = tmp1 - tmp0; + denom = A00 - 2.0f * A01 + A11; + if ( numer >= denom ) + { + T = 1.0f; + S = 0.0f; + sqrDist = A11 + 2.0f * B1 + C; + lv = 2; + } + else + { + if(fabs(denom) > FLT_EPSILON) + T = numer / denom; + else + T = 0.0f; + S = 1.0f - T; + sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0 ) + + T * ( A01 * S + A11 * T + 2.0f * B1 ) + C; + le = 2; + } + } + else + { + T = 0.0f; + if ( tmp1 <= 0.0f ) + { + S = 1.0f; + sqrDist = A00 + 2.0f * B0 + C; + lv = 1; + } + else if ( B0 >= 0.0f ) + { + S = 0.0f; + sqrDist = C; + lv = 0; + } + else + { + if(fabs(A00) > FLT_EPSILON) + S = -B0 / A00; + else + S = 0.0f; + sqrDist = B0 * S + C; + le = 0; + } + } + } + else // Region 1 + { + numer = A11 + B1 - A01 - B0; + if ( numer <= 0.0f ) + { + S = 0.0f; + T = 1.0f; + sqrDist = A11 + 2.0f * B1 + C; + lv = 2; + } + else + { + denom = A00 - 2.0f * A01 + A11; + if ( numer >= denom ) + { + S = 1.0f; + T = 0.0f; + sqrDist = A00 + 2.0f * B0 + C; + lv = 1; + } + else + { + if(fabs(denom) > FLT_EPSILON) + S = numer / denom; + else + S = 0.0f; + T = 1.0f - S; + sqrDist = S * ( A00 * S + A01 * T + 2.0f * B0 ) + + T * ( A01 * S + A11 * T + 2.0f * B1 ) + C; + le = 2; + } + } + } + } + + // Account for numerical round-off error + if ( sqrDist < FLT_EPSILON ) + sqrDist = 0.0f; + + { + float w[3], x[3], y[3], z[3]; + VECCOPY(w, v0); + VECCOPY(x, e0); + VecMulf(x, S); + VECCOPY(y, e1); + VecMulf(y, T); + VECADD(z, w, x); + VECADD(z, z, y); + //VECSUB(d, p, z); + VECCOPY(nearest, z); + // d = p - ( v0 + S * e0 + T * e1 ); + } + *v = lv; + *e = le; + + return sqrDist; +} + + +/* + * BVH from meshs callbacks + */ + +// Callback to bvh tree nearest point. The tree must bust have been built using bvhtree_from_mesh_faces. +// userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. +static void mesh_faces_nearest_point(void *userdata, int index, const float *co, BVHTreeNearest *nearest) +{ + const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata; + MVert *vert = data->vert; + MFace *face = data->face + index; + + float *t0, *t1, *t2, *t3; + t0 = vert[ face->v1 ].co; + t1 = vert[ face->v2 ].co; + t2 = vert[ face->v3 ].co; + t3 = face->v4 ? vert[ face->v4].co : NULL; + + + do + { + float nearest_tmp[3], dist; + int vertex, edge; + + dist = nearest_point_in_tri_surface(t0, t1, t2, co, &vertex, &edge, nearest_tmp); + if(dist < nearest->dist) + { + nearest->index = index; + nearest->dist = dist; + VECCOPY(nearest->co, nearest_tmp); + CalcNormFloat(t0, t1, t2, nearest->no); + } + + t1 = t2; + t2 = t3; + t3 = NULL; + + } while(t2); +} + +// Callback to bvh tree raycast. The tree must bust have been built using bvhtree_from_mesh_faces. +// userdata must be a BVHMeshCallbackUserdata built from the same mesh as the tree. +static void mesh_faces_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit) +{ + const BVHTreeFromMesh *data = (BVHTreeFromMesh*) userdata; + MVert *vert = data->vert; + MFace *face = data->face + index; + + float *t0, *t1, *t2, *t3; + t0 = vert[ face->v1 ].co; + t1 = vert[ face->v2 ].co; + t2 = vert[ face->v3 ].co; + t3 = face->v4 ? vert[ face->v4].co : NULL; + + + do + { + float dist; + if(data->sphere_radius == 0.0f) + dist = ray_tri_intersection(ray, hit->dist, t0, t1, t2); + else + dist = sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2); + + if(dist >= 0 && dist < hit->dist) + { + hit->index = index; + hit->dist = dist; + VECADDFAC(hit->co, ray->origin, ray->direction, dist); + + CalcNormFloat(t0, t1, t2, hit->no); + } + + t1 = t2; + t2 = t3; + t3 = NULL; + + } while(t2); +} + +/* + * BVH builders + */ +// Builds a bvh tree.. where nodes are the vertexs of the given mesh +void bvhtree_from_mesh_verts(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis) +{ + int i; + int numVerts= mesh->getNumVerts(mesh); + MVert *vert = mesh->getVertDataArray(mesh, CD_MVERT); + BVHTree *tree = NULL; + + memset(data, 0, sizeof(*data)); + + if(vert == NULL) + { + printf("bvhtree cant be build: cant get a vertex array"); + return; + } + + tree = BLI_bvhtree_new(numVerts, epsilon, tree_type, axis); + if(tree != NULL) + { + for(i = 0; i < numVerts; i++) + BLI_bvhtree_insert(tree, i, vert[i].co, 1); + + BLI_bvhtree_balance(tree); + + data->tree = tree; + + //a NULL nearest callback works fine + //remeber the min distance to point is the same as the min distance to BV of point + data->nearest_callback = NULL; + data->raycast_callback = NULL; + + data->mesh = mesh; + data->vert = mesh->getVertDataArray(mesh, CD_MVERT); + data->face = mesh->getFaceDataArray(mesh, CD_MFACE); + + data->sphere_radius = epsilon; + } +} + +// Builds a bvh tree.. where nodes are the faces of the given mesh. +void bvhtree_from_mesh_faces(BVHTreeFromMesh *data, DerivedMesh *mesh, float epsilon, int tree_type, int axis) +{ + int i; + int numFaces= mesh->getNumFaces(mesh); + MVert *vert = mesh->getVertDataArray(mesh, CD_MVERT); + MFace *face = mesh->getFaceDataArray(mesh, CD_MFACE); + BVHTree *tree = NULL; + + memset(data, 0, sizeof(*data)); + + if(vert == NULL && face == NULL) + { + printf("bvhtree cant be build: cant get a vertex/face array"); + return; + } + + /* Create a bvh-tree of the given target */ + tree = BLI_bvhtree_new(numFaces, epsilon, tree_type, axis); + if(tree != NULL) + { + for(i = 0; i < numFaces; i++) + { + float co[4][3]; + VECCOPY(co[0], vert[ face[i].v1 ].co); + VECCOPY(co[1], vert[ face[i].v2 ].co); + VECCOPY(co[2], vert[ face[i].v3 ].co); + if(face[i].v4) + VECCOPY(co[3], vert[ face[i].v4 ].co); + + BLI_bvhtree_insert(tree, i, co[0], face[i].v4 ? 4 : 3); + } + BLI_bvhtree_balance(tree); + + data->tree = tree; + data->nearest_callback = mesh_faces_nearest_point; + data->raycast_callback = mesh_faces_spherecast; + + data->mesh = mesh; + data->vert = mesh->getVertDataArray(mesh, CD_MVERT); + data->face = mesh->getFaceDataArray(mesh, CD_MFACE); + + data->sphere_radius = epsilon; + } +} + +// Frees data allocated by a call to bvhtree_from_mesh_*. +void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data) +{ + if(data->tree) + { + BLI_bvhtree_free(data->tree); + memset( data, 0, sizeof(data) ); + } +} + + |