Cr8-xyz/blender
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

BVH-KDOP update (merge from shrinkwrap branch): supports raytracing, nearest neighbour, non-recursive now, faster than kdtree.c implementation normaly, divided into 2 sources: generla structure in blenlib, mesh/derivedmesh depending interface stuff in blenkernel

Browse Source
This commit is contained in:
Daniel Genrich
2008-08-07 17:27:29 +00:00
parent 15952fb26c
commit c25bb4685a
4 changed files with 1198 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
source/blender/blenkernel/BKE_bvhutils.h Normal file
View File

@@ -0,0 +1,98 @@
/**
*
* $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) 2006 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: all of this file.
*
* Contributor(s): André Pinto
*
* ***** END GPL LICENSE BLOCK *****
*/
#ifndef BKE_BVHUTILS_H
#define BKE_BVHUTILS_H
#include "BLI_kdopbvh.h"
/*
* This header encapsulates necessary code to buld a BVH
*/
struct DerivedMesh;
struct MVert;
struct MFace;
/*
* struct that kepts basic information about a BVHTree build from a mesh
*/
typedef struct BVHTreeFromMesh
{
struct BVHTree *tree;
/* default callbacks to bvh nearest and raycast */
BVHTree_NearestPointCallback nearest_callback;
BVHTree_RayCastCallback raycast_callback;
/* Mesh represented on this BVHTree */
struct DerivedMesh *mesh;
/* Vertex array, so that callbacks have instante access to data */
struct MVert *vert;
struct MFace *face;
/* radius for raycast */
float sphere_radius;
} BVHTreeFromMesh;
/*
* Builds a bvh tree where nodes are the vertexs of the given mesh.
* Configures BVHTreeFromMesh.
*
* The tree is build in mesh space coordinates, this means special care must be made on queries
* so that the coordinates and rays are first translated on the mesh local coordinates.
* Reason for this is that later bvh_from_mesh_* might use a cache system and so it becames possible to reuse
* a BVHTree.
*
* free_bvhtree_from_mesh should be called when the tree is no longer needed.
*/
void bvhtree_from_mesh_verts(struct BVHTreeFromMesh *data, struct DerivedMesh *mesh, float epsilon, int tree_type, int axis);
/*
* Builds a bvh tree where nodes are the faces of the given mesh.
* Configures BVHTreeFromMesh.
*
* The tree is build in mesh space coordinates, this means special care must be made on queries
* so that the coordinates and rays are first translated on the mesh local coordinates.
* Reason for this is that later bvh_from_mesh_* might use a cache system and so it becames possible to reuse
* a BVHTree.
*
* free_bvhtree_from_mesh should be called when the tree is no longer needed.
*/
void bvhtree_from_mesh_faces(struct BVHTreeFromMesh *data, struct DerivedMesh *mesh, float epsilon, int tree_type, int axis);
/*
* Frees data allocated by a call to bvhtree_from_mesh_*.
*/
void free_bvhtree_from_mesh(struct BVHTreeFromMesh *data);
#endif

426
source/blender/blenkernel/intern/bvhutils.c Normal file
View File

@@ -0,0 +1,426 @@
/**
*
* $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 nearest_point_in_tri_surface(const float *point, const float *v0, const float *v1, const float *v2, float *nearest);
#define ISECT_EPSILON 1e-6
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(ray->origin, ray->direction, v0, v1, 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(ray->origin, p1, radius, v0, v1, v2, &idist, &hit_point))
{
return idist * m_dist;
}
return FLT_MAX;
}
/*
* This calculates the distance from point to the plane
* Distance is negative if point is on the back side of plane
*/
static float point_plane_distance(const float *point, const float *plane_point, const float *plane_normal)
{
float pp[3];
VECSUB(pp, point, plane_point);
return INPR(pp, plane_normal);
}
static float choose_nearest(const float v0[2], const float v1[2], const float point[2], float closest[2])
{
float d[2][2], sdist[2];
VECSUB2D(d[0], v0, point);
VECSUB2D(d[1], v1, point);
sdist[0] = d[0][0]*d[0][0] + d[0][1]*d[0][1];
sdist[1] = d[1][0]*d[1][0] + d[1][1]*d[1][1];
if(sdist[0] < sdist[1])
{
if(closest)
VECCOPY2D(closest, v0);
return sdist[0];
}
else
{
if(closest)
VECCOPY2D(closest, v1);
return sdist[1];
}
}
/*
* calculates the closest point between point-tri (2D)
* returns that tri must be right-handed
* Returns square distance
*/
static float closest_point_in_tri2D(const float point[2], /*const*/ float tri[3][2], float closest[2])
{
float edge_di[2];
float v_point[2];
float proj[2]; //point projected over edge-dir, edge-normal (witouth normalized edge)
const float *v0 = tri[2], *v1;
float edge_slen, d; //edge squared length
int i;
const float *nearest_vertex = NULL;
//for each edge
for(i=0, v0=tri[2], v1=tri[0]; i < 3; v0=tri[i++], v1=tri[i])
{
VECSUB2D(edge_di, v1, v0);
VECSUB2D(v_point, point, v0);
proj[1] = v_point[0]*edge_di[1] - v_point[1]*edge_di[0]; //dot product with edge normal
//point inside this edge
if(proj[1] < 0)
continue;
proj[0] = v_point[0]*edge_di[0] + v_point[1]*edge_di[1];
//closest to this edge is v0
if(proj[0] < 0)
{
if(nearest_vertex == NULL || nearest_vertex == v0)
nearest_vertex = v0;
else
{
//choose nearest
return choose_nearest(nearest_vertex, v0, point, closest);
}
i++; //We can skip next edge
continue;
}
edge_slen = edge_di[0]*edge_di[0] + edge_di[1]*edge_di[1]; //squared edge len
//closest to this edge is v1
if(proj[0] > edge_slen)
{
if(nearest_vertex == NULL || nearest_vertex == v1)
nearest_vertex = v1;
else
{
return choose_nearest(nearest_vertex, v1, point, closest);
}
continue;
}
//nearest is on this edge
d= proj[1] / edge_slen;
closest[0] = point[0] - edge_di[1] * d;
closest[1] = point[1] + edge_di[0] * d;
return proj[1]*proj[1]/edge_slen;
}
if(nearest_vertex)
{
VECSUB2D(v_point, nearest_vertex, point);
VECCOPY2D(closest, nearest_vertex);
return v_point[0]*v_point[0] + v_point[1]*v_point[1];
}
else
{
VECCOPY(closest, point); //point is already inside
return 0.0f;
}
}
/*
* Returns the square of the minimum distance between the point and a triangle surface
* If nearest is not NULL the nearest surface point is written on it
*/
static float nearest_point_in_tri_surface(const float *point, const float *v0, const float *v1, const float *v2, float *nearest)
{
//Lets solve the 2D problem (closest point-tri)
float normal_dist, plane_sdist, plane_offset;
float du[3], dv[3], dw[3]; //orthogonal axis (du=(v0->v1), dw=plane normal)
float p_2d[2], tri_2d[3][2], nearest_2d[2];
CalcNormFloat((float*)v0, (float*)v1, (float*)v2, dw);
//point-plane distance and calculate axis
normal_dist = point_plane_distance(point, v0, dw);
// OPTIMIZATION
// if we are only interested in nearest distance if its closer than some distance already found
// we can:
// if(normal_dist*normal_dist >= best_dist_so_far) return FLOAT_MAX;
//
VECSUB(du, v1, v0);
Normalize(du);
Crossf(dv, dw, du);
plane_offset = INPR(v0, dw);
//project stuff to 2d
tri_2d[0][0] = INPR(du, v0);
tri_2d[0][1] = INPR(dv, v0);
tri_2d[1][0] = INPR(du, v1);
tri_2d[1][1] = INPR(dv, v1);
tri_2d[2][0] = INPR(du, v2);
tri_2d[2][1] = INPR(dv, v2);
p_2d[0] = INPR(du, point);
p_2d[1] = INPR(dv, point);
//we always have a right-handed tri
//this should always happen because of the way normal is calculated
plane_sdist = closest_point_in_tri2D(p_2d, tri_2d, nearest_2d);
//project back to 3d
if(nearest)
{
nearest[0] = du[0]*nearest_2d[0] + dv[0] * nearest_2d[1] + dw[0] * plane_offset;
nearest[1] = du[1]*nearest_2d[0] + dv[1] * nearest_2d[1] + dw[1] * plane_offset;
nearest[2] = du[2]*nearest_2d[0] + dv[2] * nearest_2d[1] + dw[2] * plane_offset;
}
return plane_sdist + normal_dist*normal_dist;
}
/*
* 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;
dist = nearest_point_in_tri_surface(co,t0, t1, t2, nearest_tmp);
if(dist < nearest->dist)
{
nearest->index = index;
nearest->dist = dist;
VECCOPY(nearest->co, nearest_tmp);
CalcNormFloat((float*)t0, (float*)t1, (float*)t2, nearest->no); //TODO.. (interpolate normals from the vertexs coordinates?
}
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) );
}
}

36
source/blender/blenlib/BLI_kdopbvh.h
View File

@@ -1,4 +1,6 @@
/** /**
*
* $Id$
* *
* ***** BEGIN GPL LICENSE BLOCK ***** * ***** BEGIN GPL LICENSE BLOCK *****
* *
@@ -40,6 +42,35 @@ typedef struct BVHTreeOverlap {
int indexB; int indexB;
} BVHTreeOverlap; } BVHTreeOverlap;
typedef struct BVHTreeNearest
{
int index; /* the index of the nearest found (untouched if none is found within a dist radius from the given coordinates) */
float co[3]; /* nearest coordinates (untouched it none is found within a dist radius from the given coordinates) */
float no[3]; /* normal at nearest coordinates (untouched it none is found within a dist radius from the given coordinates) */
float dist; /* squared distance to search arround */
} BVHTreeNearest;
typedef struct BVHTreeRay
{
float origin[3]; /* ray origin */
float direction[3]; /* ray direction */
} BVHTreeRay;
typedef struct BVHTreeRayHit
{
int index; /* index of the tree node (untouched if no hit is found) */
float co[3]; /* coordinates of the hit point */
float no[3]; /* normal on hit point */
float dist; /* distance to the hit point */
} BVHTreeRayHit;
/* callback must update nearest in case it finds a nearest result */
typedef void (*BVHTree_NearestPointCallback) (void *userdata, int index, const float *co, BVHTreeNearest *nearest);
/* callback must update hit in case it finds a nearest successful hit */
typedef void (*BVHTree_RayCastCallback) (void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit);
BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis); BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis);
void BLI_bvhtree_free(BVHTree *tree); void BLI_bvhtree_free(BVHTree *tree);
@@ -56,5 +87,10 @@ BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
float BLI_bvhtree_getepsilon(BVHTree *tree); float BLI_bvhtree_getepsilon(BVHTree *tree);
/* find nearest node to the given coordinates (if nearest is given it will only search nodes where square distance is smaller than nearest->dist) */
int BLI_bvhtree_find_nearest(BVHTree *tree, const float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata);
int BLI_bvhtree_ray_cast(BVHTree *tree, const float *co, const float *dir, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata);
#endif // BLI_KDOPBVH_H #endif // BLI_KDOPBVH_H

730
source/blender/blenlib/intern/BLI_kdopbvh.c
View File

@@ -1,4 +1,6 @@
/** /**
*
* $Id$
* *
* ***** BEGIN GPL LICENSE BLOCK ***** * ***** BEGIN GPL LICENSE BLOCK *****
* *
@@ -28,8 +30,9 @@
#include "math.h" #include "math.h"
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <assert.h>
#include "MEM_guardedalloc.h" #include "MEM_guardedalloc.h"
@@ -44,11 +47,11 @@
typedef struct BVHNode typedef struct BVHNode
{ {
struct BVHNode **children; // max 8 children struct BVHNode **children; // max 8 children
struct BVHNode *parent; // needed for bottom - top update struct BVHNode *parent; // needed for bottom - top update
float *bv; // Bounding volume of all nodes, max 13 axis float *bv; // Bounding volume of all nodes, max 13 axis
int index; /* face, edge, vertex index */ int index; // face, edge, vertex index
char totnode; // how many nodes are used, used for speedup char totnode; // how many nodes are used, used for speedup
char traversed; // how many nodes already traversed until this level? char traversed; // how many nodes already traversed until this level?
char main_axis; char main_axis;
} BVHNode; } BVHNode;
@@ -73,7 +76,32 @@ typedef struct BVHOverlapData
BVHTreeOverlap *overlap; BVHTreeOverlap *overlap;
int i, max_overlap; /* i is number of overlaps */ int i, max_overlap; /* i is number of overlaps */
} BVHOverlapData; } BVHOverlapData;
////////////////////////////////////////
typedef struct BVHNearestData
{
BVHTree *tree;
float *co;
BVHTree_NearestPointCallback callback;
void *userdata;
float proj[13]; //coordinates projection over axis
BVHTreeNearest nearest;
} BVHNearestData;
typedef struct BVHRayCastData
{
BVHTree *tree;
BVHTree_RayCastCallback callback;
void *userdata;
BVHTreeRay ray;
float ray_dot_axis[13];
BVHTreeRayHit hit;
} BVHRayCastData;
////////////////////////////////////////m
//////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////
@@ -244,7 +272,7 @@ int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){
int begin = _begin, end = _end, cut; int begin = _begin, end = _end, cut;
while(end-begin > 3) while(end-begin > 3)
{ {
cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis ); cut = bvh_partition(a, begin, end, bvh_medianof3(a, begin, (begin+end)/2, end-1, axis), axis );
if(cut <= n) if(cut <= n)
begin = cut; begin = cut;
else else
@@ -255,7 +283,6 @@ int partition_nth_element(BVHNode **a, int _begin, int _end, int n, int axis){
return n; return n;
} }
////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////
void BLI_bvhtree_free(BVHTree *tree) void BLI_bvhtree_free(BVHTree *tree)
@@ -270,13 +297,35 @@ void BLI_bvhtree_free(BVHTree *tree)
} }
} }
// calculate max number of branches
int needed_branches(int tree_type, int leafs)
{
#if 1
//Worst case scenary ( return max(0, leafs-tree_type)+1 )
if(leafs <= tree_type)
return 1;
else
return leafs-tree_type+1;
#else
//If our bvh kdop is "almost perfect"
//TODO i dont trust the float arithmetic in here (and I am not sure this formula is according to our splitting method)
int i, numbranches = 0;
for(i = 1; i <= (int)ceil((float)((float)log(leafs)/(float)log(tree_type))); i++)
numbranches += (pow(tree_type, i) / tree_type);
return numbranches;
#endif
}
BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis) BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
{ {
BVHTree *tree; BVHTree *tree;
int numbranches=0, i; int numnodes, i;
// only support up to octree // theres not support for trees below binary-trees :P
if(tree_type > 8) if(tree_type < 2)
return NULL; return NULL;
tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree"); tree = (BVHTree *)MEM_callocN(sizeof(BVHTree), "BVHTree");
@@ -319,11 +368,10 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
} }
// calculate max number of branches, our bvh kdop is "almost perfect" //Allocate arrays
for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++) numnodes = maxsize + needed_branches(tree_type, maxsize) + tree_type;
numbranches += (pow(tree_type, i) / tree_type);
tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*numnodes, "BVHNodes");
tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes");
if(!tree->nodes) if(!tree->nodes)
{ {
@@ -331,14 +379,14 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
return NULL; return NULL;
} }
tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV"); tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * numnodes, "BVHNodeBV");
if(!tree->nodebv) if(!tree->nodebv)
{ {
MEM_freeN(tree->nodes); MEM_freeN(tree->nodes);
MEM_freeN(tree); MEM_freeN(tree);
} }
tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV"); tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * numnodes, "BVHNodeBV");
if(!tree->nodechild) if(!tree->nodechild)
{ {
MEM_freeN(tree->nodebv); MEM_freeN(tree->nodebv);
@@ -346,7 +394,7 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
MEM_freeN(tree); MEM_freeN(tree);
} }
tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray"); tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)* numnodes, "BVHNodeArray");
if(!tree->nodearray) if(!tree->nodearray)
{ {
@@ -358,7 +406,7 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
} }
//link the dynamic bv and child links //link the dynamic bv and child links
for(i=0; i< numbranches+maxsize + tree_type; i++) for(i=0; i< numnodes; i++)
{ {
tree->nodearray[i].bv = tree->nodebv + i * axis; tree->nodearray[i].bv = tree->nodebv + i * axis;
tree->nodearray[i].children = tree->nodechild + i * tree_type; tree->nodearray[i].children = tree->nodechild + i * tree_type;
@@ -373,6 +421,7 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving) static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
{ {
float newminmax; float newminmax;
float *bv = node->bv;
int i, k; int i, k;
// don't init boudings for the moving case // don't init boudings for the moving case
@@ -380,8 +429,8 @@ static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoi
{ {
for (i = tree->start_axis; i < tree->stop_axis; i++) for (i = tree->start_axis; i < tree->stop_axis; i++)
{ {
node->bv[2*i] = FLT_MAX; bv[2*i] = FLT_MAX;
node->bv[2*i + 1] = -FLT_MAX; bv[2*i + 1] = -FLT_MAX;
} }
} }
@@ -391,10 +440,10 @@ static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoi
for (i = tree->start_axis; i < tree->stop_axis; i++) for (i = tree->start_axis; i < tree->stop_axis; i++)
{ {
newminmax = INPR(&co[k * 3], KDOP_AXES[i]); newminmax = INPR(&co[k * 3], KDOP_AXES[i]);
if (newminmax < node->bv[2 * i]) if (newminmax < bv[2 * i])
node->bv[2 * i] = newminmax; bv[2 * i] = newminmax;
if (newminmax > node->bv[(2 * i) + 1]) if (newminmax > bv[(2 * i) + 1])
node->bv[(2 * i) + 1] = newminmax; bv[(2 * i) + 1] = newminmax;
} }
} }
} }
@@ -405,6 +454,7 @@ static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
float newmin,newmax; float newmin,newmax;
int i, j; int i, j;
float *bv = node->bv; float *bv = node->bv;
for (i = tree->start_axis; i < tree->stop_axis; i++) for (i = tree->start_axis; i < tree->stop_axis; i++)
{ {
@@ -426,18 +476,19 @@ static void refit_kdop_hull(BVHTree *tree, BVHNode *node, int start, int end)
bv[(2 * i) + 1] = newmax; bv[(2 * i) + 1] = newmax;
} }
} }
} }
int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints) int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
{ {
BVHNode *node= NULL;
int i; int i;
BVHNode *node = NULL;
// insert should only possible as long as tree->totbranch is 0 // insert should only possible as long as tree->totbranch is 0
if(tree->totbranch > 0) if(tree->totbranch > 0)
return 0; return 0;
if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)) if(tree->totleaf+1 >= MEM_allocN_len(tree->nodes)/sizeof(*(tree->nodes)))
return 0; return 0;
// TODO check if have enough nodes in array // TODO check if have enough nodes in array
@@ -446,6 +497,7 @@ int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
tree->totleaf++; tree->totleaf++;
create_kdop_hull(tree, node, co, numpoints, 0); create_kdop_hull(tree, node, co, numpoints, 0);
node->index= index;
// inflate the bv with some epsilon // inflate the bv with some epsilon
for (i = tree->start_axis; i < tree->stop_axis; i++) for (i = tree->start_axis; i < tree->stop_axis; i++)
@@ -454,8 +506,6 @@ int BLI_bvhtree_insert(BVHTree *tree, int index, float *co, int numpoints)
node->bv[(2 * i) + 1] += tree->epsilon; // maximum node->bv[(2 * i) + 1] += tree->epsilon; // maximum
} }
node->index= index;
return 1; return 1;
} }
@@ -484,21 +534,24 @@ static char get_largest_axis(float *bv)
} }
} }
static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end) static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, int free_node_index)
{ {
int i, tend; int i;
BVHNode *tnode;
int slice = (end-start+tree->tree_type-1)/tree->tree_type; //division rounded up const char laxis = get_largest_axis(node->bv); //determine longest axis to split along
const int slice = (end-start)/tree->tree_type; //division rounded down
const int rest = (end-start)%tree->tree_type; //remainder of division
// Determine which axis to split along assert( node->totnode == 0 );
char laxis = get_largest_axis(node->bv);
node->main_axis = laxis/2;
// split nodes along longest axis // split nodes along longest axis
for (i=0; start < end; start += slice, i++) //i counts the current child for (i=0; start < end; node->totnode = ++i) //i counts the current child
{ {
tend = start + slice; int tend = start + slice + (i < rest ? 1 : 0);
if(tend > end) tend = end; assert( tend <= end);
if(tend-start == 1) // ok, we have 1 left for this node if(tend-start == 1) // ok, we have 1 left for this node
{ {
@@ -507,87 +560,332 @@ static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end)
} }
else else
{ {
tnode = node->children[i] = tree->nodes[tree->totleaf + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]); BVHNode *tnode = node->children[i] = tree->nodes[free_node_index] = &(tree->nodearray[free_node_index]);
tree->totbranch++;
tnode->parent = node; tnode->parent = node;
if(tend != end) if(tend != end)
partition_nth_element(tree->nodes, start, end, tend, laxis); partition_nth_element(tree->nodes, start, end, tend, laxis);
refit_kdop_hull(tree, tnode, start, tend); refit_kdop_hull(tree, tnode, start, tend);
bvh_div_nodes(tree, tnode, start, tend);
bvh_div_nodes(tree, tnode, start, tend, free_node_index+1);
free_node_index += needed_branches(tree->tree_type, tend-start);
} }
node->totnode++; start = tend;
} }
return; return;
} }
static void omp_bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, int free_node_index)
{
int i;
const char laxis = get_largest_axis(node->bv); //determine longest axis to split along
const int slice = (end-start)/tree->tree_type; //division rounded down
const int rest = (end-start)%tree->tree_type; //remainder of division
int omp_data_start[tree->tree_type];
int omp_data_end [tree->tree_type];
int omp_data_index[tree->tree_type];
assert( node->totnode == 0 );
node->main_axis = laxis/2;
// split nodes along longest axis
for (i=0; start < end; node->totnode = ++i) //i counts the current child
{
//Split the rest from left to right (TODO: this doenst makes an optimal tree)
int tend = start + slice + (i < rest ? 1 : 0);
assert( tend <= end);
//save data for later OMP
omp_data_start[i] = start;
omp_data_end [i] = tend;
omp_data_index[i] = free_node_index;
if(tend-start == 1)
{
node->children[i] = tree->nodes[start];
node->children[i]->parent = node;
}
else
{
node->children[i] = tree->nodes[free_node_index] = &(tree->nodearray[free_node_index]);
node->children[i]->parent = node;
if(tend != end)
partition_nth_element(tree->nodes, start, end, tend, laxis);
free_node_index += needed_branches(tree->tree_type, tend-start);
}
start = tend;
}
#pragma omp parallel for private(i) schedule(static)
for( i = 0; i < node->totnode; i++)
{
if(omp_data_end[i]-omp_data_start[i] > 1)
{
BVHNode *tnode = node->children[i];
refit_kdop_hull(tree, tnode, omp_data_start[i], omp_data_end[i]);
bvh_div_nodes (tree, tnode, omp_data_start[i], omp_data_end[i], omp_data_index[i]+1);
}
}
return;
}
static void print_tree(BVHTree *tree, BVHNode *node, int depth)
{
int i;
for(i=0; i<depth; i++) printf(" ");
printf(" - %d (%d): ", node->index, node - tree->nodearray);
for(i=2*tree->start_axis; i<2*tree->stop_axis; i++)
printf("%.3f ", node->bv[i]);
printf("\n");
for(i=0; i<tree->tree_type; i++)
if(node->children[i])
print_tree(tree, node->children[i], depth+1);
}
#if 0 #if 0
static void verify_tree(BVHTree *tree) static void verify_tree(BVHTree *tree)
{ {
int i, j, check = 0; int i, j, check = 0;
// check the pointer list // check the pointer list
for(i = 0; i < tree->totleaf; i++) for(i = 0; i < tree->totleaf; i++)
{ {
if(tree->nodes[i]->parent == NULL) if(tree->nodes[i]->parent == NULL)
printf("Leaf has no parent: %d\n", i); printf("Leaf has no parent: %d\n", i);
else else
{ {
for(j = 0; j < tree->tree_type; j++) for(j = 0; j < tree->tree_type; j++)
{ {
if(tree->nodes[i]->parent->children[j] == tree->nodes[i]) if(tree->nodes[i]->parent->children[j] == tree->nodes[i])
check = 1; check = 1;
} }
if(!check) if(!check)
{ {
printf("Parent child relationship doesn't match: %d\n", i); printf("Parent child relationship doesn't match: %d\n", i);
} }
check = 0; check = 0;
} }
} }
// check the leaf list // check the leaf list
for(i = 0; i < tree->totleaf; i++) for(i = 0; i < tree->totleaf; i++)
{ {
if(tree->nodearray[i].parent == NULL) if(tree->nodearray[i].parent == NULL)
printf("Leaf has no parent: %d\n", i); printf("Leaf has no parent: %d\n", i);
else else
{ {
for(j = 0; j < tree->tree_type; j++) for(j = 0; j < tree->tree_type; j++)
{ {
if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i]) if(tree->nodearray[i].parent->children[j] == &tree->nodearray[i])
check = 1; check = 1;
} }
if(!check) if(!check)
{ {
printf("Parent child relationship doesn't match: %d\n", i); printf("Parent child relationship doesn't match: %d\n", i);
} }
check = 0; check = 0;
} }
} }
printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf); printf("branches: %d, leafs: %d, total: %d\n", tree->totbranch, tree->totleaf, tree->totbranch + tree->totleaf);
} }
#endif #endif
//Helper data and structures to build generalized implicit trees
//This code can be easily reduced
typedef struct BVHBuildHelper
{
int tree_type; //
int totleafs; //
int leafs_per_child [32]; //Min number of leafs that are archievable from a node at depth N
int branches_on_level[32]; //Number of nodes at depth N (tree_type^N)
int remain_leafs; //Number of leafs that are placed on the level that is not 100% filled
} BVHBuildHelper;
static void build_implicit_tree_helper(BVHTree *tree, BVHBuildHelper *data)
{
int depth = 0;
int remain;
int nnodes;
data->totleafs = tree->totleaf;
data->tree_type= tree->tree_type;
//Calculate the smallest tree_type^n such that tree_type^n >= num_leafs
for(
data->leafs_per_child[0] = 1;
data->leafs_per_child[0] < data->totleafs;
data->leafs_per_child[0] *= data->tree_type
);
data->branches_on_level[0] = 1;
//We could stop the loop first (but I am lazy to find out when)
for(depth = 1; depth < 32; depth++)
{
data->branches_on_level[depth] = data->branches_on_level[depth-1] * data->tree_type;
data->leafs_per_child [depth] = data->leafs_per_child [depth-1] / data->tree_type;
}
remain = data->totleafs - data->leafs_per_child[1];
nnodes = (remain + data->tree_type - 2) / (data->tree_type - 1);
data->remain_leafs = remain + nnodes;
}
// return the min index of all the leafs archivable with the given branch
static int implicit_leafs_index(BVHBuildHelper *data, int depth, int child_index)
{
int min_leaf_index = child_index * data->leafs_per_child[depth-1];
if(min_leaf_index <= data->remain_leafs)
return min_leaf_index;
else if(data->leafs_per_child[depth])
return data->totleafs - (data->branches_on_level[depth-1] - child_index) * data->leafs_per_child[depth];
else
return data->remain_leafs;
}
//WARNING: Beautiful/tricky code starts here :P
//Generalized implicit trees
static void non_recursive_bvh_div_nodes(BVHTree *tree)
{
int i;
const int tree_type = tree->tree_type;
const int tree_offset = 2 - tree->tree_type; //this value is 0 (on binary trees) and negative on the others
const int num_leafs = tree->totleaf;
const int num_branches= MAX2(1, (num_leafs + tree_type - 3) / (tree_type-1) );
BVHNode* branches_array = tree->nodearray + tree->totleaf - 1; // This code uses 1 index arrays
BVHNode** leafs_array = tree->nodes;
BVHBuildHelper data;
int depth = 0;
build_implicit_tree_helper(tree, &data);
//YAY this could be 1 loop.. but had to split in 2 to remove OMP dependencies
for(i=1; i <= num_branches; i = i*tree_type + tree_offset)
{
const int first_of_next_level = i*tree_type + tree_offset;
const int end_j = MIN2(first_of_next_level, num_branches + 1); //index of last branch on this level
int j;
depth++;
#pragma omp parallel for private(j) schedule(static)
for(j = i; j < end_j; j++)
{
int k;
const int parent_level_index= j-i;
BVHNode* parent = branches_array + j;
char split_axis;
int parent_leafs_begin = implicit_leafs_index(&data, depth, parent_level_index);
int parent_leafs_end = implicit_leafs_index(&data, depth, parent_level_index+1);
//split_axis = (depth*2 % 6); //use this instead of the 2 following lines for XYZ splitting
refit_kdop_hull(tree, parent, parent_leafs_begin, parent_leafs_end);
split_axis = get_largest_axis(parent->bv);
parent->main_axis = split_axis / 2;
for(k = 0; k < tree_type; k++)
{
int child_index = j * tree_type + tree_offset + k;
int child_level_index = child_index - first_of_next_level; //child level index
int child_leafs_begin = implicit_leafs_index(&data, depth+1, child_level_index);
int child_leafs_end = implicit_leafs_index(&data, depth+1, child_level_index+1);
assert( k != 0 || child_leafs_begin == parent_leafs_begin);
if(child_leafs_end - child_leafs_begin > 1)
{
parent->children[k] = branches_array + child_index;
parent->children[k]->parent = parent;
/*
printf("Add child %d (%d) to branch %d\n",
branches_array + child_index - tree->nodearray,
branches_array[ child_index ].index,
parent - tree->nodearray
);
*/
partition_nth_element(leafs_array, child_leafs_begin, parent_leafs_end, child_leafs_end, split_axis);
}
else if(child_leafs_end - child_leafs_begin == 1)
{
/*
printf("Add child %d (%d) to branch %d\n",
leafs_array[ child_leafs_begin ] - tree->nodearray,
leafs_array[ child_leafs_begin ]->index,
parent - tree->nodearray
);
*/
parent->children[k] = leafs_array[ child_leafs_begin ];
parent->children[k]->parent = parent;
}
else
{
parent->children[k] = NULL;
break;
}
parent->totnode = k+1;
}
}
}
for(i = 0; i<num_branches; i++)
tree->nodes[tree->totleaf + i] = branches_array + 1 + i;
tree->totbranch = num_branches;
// BLI_bvhtree_update_tree(tree); //Uncoment this for XYZ splitting
}
void BLI_bvhtree_balance(BVHTree *tree) void BLI_bvhtree_balance(BVHTree *tree)
{ {
BVHNode *node; if(tree->totleaf == 0) return;
if(tree->totleaf == 0) assert(tree->totbranch == 0);
return; non_recursive_bvh_div_nodes(tree);
// create root node /*
node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]); if(tree->totleaf != 0)
{
// create root node
BVHNode *node = tree->nodes[tree->totleaf] = &(tree->nodearray[tree->totleaf]);
tree->totbranch++; tree->totbranch++;
<
// refit root bvh node // refit root bvh node
refit_kdop_hull(tree, tree->nodes[tree->totleaf], 0, tree->totleaf); refit_kdop_hull(tree, node, 0, tree->totleaf);
// create + balance tree
bvh_div_nodes(tree, tree->nodes[tree->totleaf], 0, tree->totleaf); // create + balance tree
omp_bvh_div_nodes(tree, node, 0, tree->totleaf, tree->totleaf+1);
// verify_tree(tree); tree->totbranch = needed_branches( tree->tree_type, tree->totleaf );
// verify_tree(tree);
}
*/
} }
// overlap - is it possbile for 2 bv's to collide ? // overlap - is it possbile for 2 bv's to collide ?
@@ -724,6 +1022,7 @@ BVHTreeOverlap *BLI_bvhtree_overlap(BVHTree *tree1, BVHTree *tree2, int *result)
} }
// bottom up update of bvh tree: // bottom up update of bvh tree:
// join the 4 children here // join the 4 children here
static void node_join(BVHTree *tree, BVHNode *node) static void node_join(BVHTree *tree, BVHNode *node)
@@ -809,3 +1108,250 @@ float BLI_bvhtree_getepsilon(BVHTree *tree)
{ {
return tree->epsilon; return tree->epsilon;
} }
/*
* Nearest neighbour
*/
static float squared_dist(const float *a, const float *b)
{
float tmp[3];
VECSUB(tmp, a, b);
return INPR(tmp, tmp);
}
static float calc_nearest_point(BVHNearestData *data, BVHNode *node, float *nearest)
{
int i;
const float *bv = node->bv;
//nearest on AABB hull
for(i=0; i != 3; i++, bv += 2)
{
if(bv[0] > data->proj[i])
nearest[i] = bv[0];
else if(bv[1] < data->proj[i])
nearest[i] = bv[1];
else
nearest[i] = data->proj[i];
}
/*
//nearest on a general hull
VECCOPY(nearest, data->co);
for(i = data->tree->start_axis; i != data->tree->stop_axis; i++, bv+=2)
{
float proj = INPR( nearest, KDOP_AXES[i]);
float dl = bv[0] - proj;
float du = bv[1] - proj;
if(dl > 0)
{
VECADDFAC(nearest, nearest, KDOP_AXES[i], dl);
}
else if(du < 0)
{
VECADDFAC(nearest, nearest, KDOP_AXES[i], du);
}
}
*/
return squared_dist(data->co, nearest);
}
// TODO: use a priority queue to reduce the number of nodes looked on
static void dfs_find_nearest(BVHNearestData *data, BVHNode *node)
{
int i;
float nearest[3], sdist;
sdist = calc_nearest_point(data, node, nearest);
if(sdist >= data->nearest.dist) return;
if(node->totnode == 0)
{
if(data->callback)
data->callback(data->userdata , node->index, data->co, &data->nearest);
else
{
data->nearest.index = node->index;
VECCOPY(data->nearest.co, nearest);
data->nearest.dist = sdist;
}
}
else
{
for(i=0; i != node->totnode; i++)
dfs_find_nearest(data, node->children[i]);
}
}
int BLI_bvhtree_find_nearest(BVHTree *tree, const float *co, BVHTreeNearest *nearest, BVHTree_NearestPointCallback callback, void *userdata)
{
int i;
BVHNearestData data;
//init data to search
data.tree = tree;
data.co = co;
data.callback = callback;
data.userdata = userdata;
for(i = data.tree->start_axis; i != data.tree->stop_axis; i++)
{
data.proj[i] = INPR(data.co, KDOP_AXES[i]);
}
if(nearest)
{
memcpy( &data.nearest , nearest, sizeof(*nearest) );
}
else
{
data.nearest.index = -1;
data.nearest.dist = FLT_MAX;
}
//dfs search
dfs_find_nearest(&data, tree->nodes[tree->totleaf] );
//copy back results
if(nearest)
{
memcpy(nearest, &data.nearest, sizeof(*nearest));
}
return data.nearest.index;
}
/*
* Ray cast
*/
static float ray_nearest_hit(BVHRayCastData *data, BVHNode *node)
{
int i;
const float *bv = node->bv;
float low = 0, upper = data->hit.dist;
for(i=0; i != 3; i++, bv += 2)
{
if(data->ray_dot_axis[i] == 0.0f)
{
//axis aligned ray
if(data->ray.origin[i] < bv[0]
|| data->ray.origin[i] > bv[1])
return FLT_MAX;
}
else
{
float ll = (bv[0] - data->ray.origin[i]) / data->ray_dot_axis[i];
float lu = (bv[1] - data->ray.origin[i]) / data->ray_dot_axis[i];
if(data->ray_dot_axis[i] > 0)
{
if(ll > low) low = ll;
if(lu < upper) upper = lu;
}
else
{
if(lu > low) low = lu;
if(ll < upper) upper = ll;
}
if(low > upper) return FLT_MAX;
}
}
return low;
}
static void dfs_raycast(BVHRayCastData *data, BVHNode *node)
{
int i;
//ray-bv is really fast.. and simple tests revealed its worth to test it
//before calling the ray-primitive functions
float dist = ray_nearest_hit(data, node);
if(dist >= data->hit.dist) return;
if(node->totnode == 0)
{
if(data->callback)
data->callback(data->userdata, node->index, &data->ray, &data->hit);
else
{
data->hit.index = node->index;
data->hit.dist = dist;
VECADDFAC(data->hit.co, data->ray.origin, data->ray.direction, dist);
}
}
else
{
//pick loop direction to dive into the tree (based on ray direction and split axis)
if(data->ray_dot_axis[ node->main_axis ] > 0)
{
for(i=0; i != node->totnode; i++)
{
dfs_raycast(data, node->children[i]);
}
}
else
{
for(i=node->totnode-1; i >= 0; i--)
{
dfs_raycast(data, node->children[i]);
}
}
}
}
int BLI_bvhtree_ray_cast(BVHTree *tree, const float *co, const float *dir, BVHTreeRayHit *hit, BVHTree_RayCastCallback callback, void *userdata)
{
int i;
BVHRayCastData data;
data.tree = tree;
data.callback = callback;
data.userdata = userdata;
VECCOPY(data.ray.origin, co);
VECCOPY(data.ray.direction, dir);
Normalize(data.ray.direction);
for(i=0; i<3; i++)
{
data.ray_dot_axis[i] = INPR( data.ray.direction, KDOP_AXES[i]);
if(fabs(data.ray_dot_axis[i]) < 1e-7)
data.ray_dot_axis[i] = 0.0;
}
if(hit)
memcpy( &data.hit, hit, sizeof(*hit) );
else
{
data.hit.index = -1;
data.hit.dist = FLT_MAX;
}
dfs_raycast(&data, tree->nodes[tree->totleaf]);
if(hit)
memcpy( hit, &data.hit, sizeof(*hit) );
return data.hit.index;
}