Subversion Repositories gelsvn

#include "CGLA/Vec3d.h"

#include "Geometry/TriMesh.h"

#include "BSPTree.h"

using namespace std;

using namespace CGLA;

namespace Geometry

{

  int BSPTree::node_calls;

  int BSPTree::tri_calls;

  void create_tri_accel(Vec3f A, Vec3f B, Vec3f C, TriAccel &tri_accel) 

  {

    Vec3f N = normalize(cross(B-A, C-A));

    // Find projection dir

    int k,u,v;

    if (fabs(N[0]) > fabs(N[1]))

      if (fabs(N[0]) > fabs(N[2])) 

	k = 0; /* X */ 

      else 

	k=2;   /* Z */

    else

      if (fabs(N[1]) > fabs(N[2])) 

	k = 1; /* Y */ 

      else 

	k=2;   /* Z */

    u = (k+1)% 3; 

    v = (k+2)% 3;

    Vec3f b = C - A;

    Vec3f c = B - A;

    double div = (b[u]*c[v]-b[v]*c[u]);

    tri_accel.n_u = N[u]/N[k];

    tri_accel.n_v = N[v]/N[k];

    tri_accel.n_d = dot(A, N/N[k]);

    tri_accel.k = k;

    tri_accel.b_nu = -b[v]/div;

    tri_accel.b_nv = b[u]/div;

    tri_accel.b_d = (b[v]*A[u]-b[u]*A[v])/div;

    tri_accel.c_nu = c[v]/div;

    tri_accel.c_nv = -c[u]/div;

    tri_accel.c_d = (c[u]*A[v]-c[v]*A[u])/div;

  }

  // Most of this file is a direct copy from Henrik's notes

  BSPTree::BSPTree() 

  {

    root=0;

    b_is_build = false;

  }

  BSPTree::~BSPTree() 

  {

    clear();		

  }

  void BSPTree::clear() 

  {

    if (root!=0) 

    {

      delete_node(root);

      root=0;

      b_is_build=false;

    }

    isecttris.clear();

    all_objects.clear();

    all_triaccel.clear();

  }

  void BSPTree::delete_node(BSPNode *node) 

  {

    if (node->left!=0)

      delete_node(node->left);

    if (node->right!=0)

      delete_node(node->right);

    delete node;

  }

  void BSPTree::subdivide_node(BSPNode &node, BBox &bbox, 

			       unsigned int level, 

			       vector<ISectTri*>& objects, 

			       vector<TriAccel*>& tri_objects) 

  {

    const int TESTS = 2;

    if (objects.size()<=max_objects || level==max_level) 

    {

      node.axis_leaf = 4; // Means that this is a leaf

      node.plane = 0;

      node.left = 0;

      node.right = 0;

      node.id = all_objects.size();

      node.count = objects.size();

      for(unsigned int i = 0; i < objects.size(); ++i) 

      {

	all_objects.push_back(objects[i]);

	all_triaccel.push_back(tri_objects[i]);

      }  

    } 

    else 

    {

      bool right_zero=false;

      bool left_zero=false;

      unsigned int i;

      BSPNode* left_node  = new BSPNode();

      BSPNode* right_node = new BSPNode();

      vector<ISectTri*> left_objects;

      vector<ISectTri*> right_objects;

      vector<TriAccel*> tri_left_objects;

      vector<TriAccel*> tri_right_objects;

      node.left = left_node;

      node.right = right_node;

      int new_axis=-1;

      double min_cost=CGLA::BIG;

      int new_pos = -1;      

	  for(i=0;i<3;i++) 

      {

	for(int k=1;k<TESTS;k++) 

	{

	  BBox left_bbox = bbox;

	  BBox right_bbox = bbox;

	  double center = (bbox.max_corner[i]- bbox.min_corner[i])*(double)k/(double)TESTS + bbox.min_corner[i];

	  node.plane = center;

	  left_bbox.max_corner[i] = center; 

	  right_bbox.min_corner[i] = center; 

	  // Try putting the triangles in the left and right boxes

	  int left_count = 0;

	  int right_count = 0;

	  for(unsigned int j=0;j<objects.size();j++) 

	  {

	    ISectTri* tri = objects[j];

	    left_count += left_bbox.intersect_triangle(*tri);

	    right_count += right_bbox.intersect_triangle(*tri);

	  }

	  //double len = bbox.max_corner[i] - bbox.min_corner[i];

	  double cost = left_count*left_bbox.area() + right_count*right_bbox.area(); // - len*len;

	  if(cost < min_cost) 

	  {

	    min_cost = cost;

	    new_axis = i;

	    new_pos = k;

	    right_zero = (right_count==0);

	    left_zero = (left_count==0);

	  }

	}

      }

      node.axis_leaf = new_axis;

      left_node->axis_leaf = static_cast<unsigned char>(-1); 

      right_node->axis_leaf = static_cast<unsigned char>(-1); 

      // Now chose the right splitting plane

      BBox left_bbox = bbox;

      BBox right_bbox = bbox;

      double size = bbox.max_corner[node.axis_leaf]- bbox.min_corner[node.axis_leaf];

      double center = size*(double)new_pos/(double)TESTS + bbox.min_corner[node.axis_leaf];

      double diff = f_eps < size/8.0 ? f_eps : size/8.0;

      if (left_zero) 

      {

	// Find min position of all triangle vertices and place the center there

	center = bbox.max_corner[node.axis_leaf];

	for(unsigned int j=0;j<objects.size();j++) 

	{

	  ISectTri* tri = objects[j];

	  if (tri->point0[node.axis_leaf]<center)

	    center=tri->point0[node.axis_leaf];

	  if (tri->point1[node.axis_leaf]<center)

	    center=tri->point1[node.axis_leaf];

	  if (tri->point2[node.axis_leaf]<center)

	    center=tri->point2[node.axis_leaf];

	}

	center -= diff;

      }

      if (right_zero) 

      {

	// Find max position of all triangle vertices and place the center there

	center = bbox.min_corner[node.axis_leaf];

	for(unsigned int j=0;j<objects.size();j++) 

	{

	  ISectTri* tri = objects[j];

	  if (tri->point0[node.axis_leaf]>center)

	    center=tri->point0[node.axis_leaf];

	  if (tri->point1[node.axis_leaf]>center)

	    center=tri->point1[node.axis_leaf];

	  if (tri->point2[node.axis_leaf]>center)

	    center=tri->point2[node.axis_leaf];

	}

	center += diff;

      }

      node.plane = center;

      left_bbox.max_corner[node.axis_leaf] = center; 

      right_bbox.min_corner[node.axis_leaf] = center;  

      // Now put the triangles in the right and left node

      for(i=0;i<objects.size();i++) 

      {

	ISectTri* tri = objects[i];

	TriAccel *tri_accel = tri_objects[i];

	if (left_bbox.intersect_triangle(*tri)) 

	{

	  left_objects.push_back(tri);

	  tri_left_objects.push_back(tri_accel);

	}

	if (right_bbox.intersect_triangle(*tri)) 

	{

	  right_objects.push_back(tri);

	  tri_right_objects.push_back(tri_accel);

	}

      }

    //if (left_zero||right_zero)

	//  cout << left_objects.size() << "," << right_objects.size() << "," << level << endl;

      objects.clear();

      subdivide_node(*left_node , left_bbox , level+1, left_objects, tri_left_objects);

      subdivide_node(*right_node, right_bbox, level+1, right_objects, tri_right_objects);

    }

  }

  void BSPTree::init() 

  {

    root = new BSPNode();

    bbox.compute_bbox(isecttris);

    bbox.min_corner-=Vec3f(1.0);

    bbox.max_corner+=Vec3f(1.0);

  }

  void BSPTree::init(vector<const TriMesh*>& _trimesh, 

		     vector<Mat4x4f>& _transforms, 

		     int _max_objects, int _max_level) 

  {

    trimesh = _trimesh;

    transforms = _transforms;

    for(unsigned int i=0;i<trimesh.size();i++) 

    {

      const TriMesh *mesh = trimesh[i];

      // Loop through all triangles and add them to intersection structure

      for(int j=0;j<mesh->geometry.no_faces();j++) 

      {

	Vec3i face = mesh->geometry.face(j);

	ISectTri new_tri;

	new_tri.point0 = transforms[i].mul_3D_point(mesh->geometry.vertex(face[0]));

	new_tri.point1 = transforms[i].mul_3D_point(mesh->geometry.vertex(face[1]));

	new_tri.point2 = transforms[i].mul_3D_point(mesh->geometry.vertex(face[2]));

	new_tri.edge0 = new_tri.point1 - new_tri.point0;

	new_tri.edge1 = new_tri.point2 - new_tri.point0;

	new_tri.mesh_id = i;

	new_tri.tri_id = j;

	isecttris.push_back(new_tri);

	TriAccel ta;

	create_tri_accel(new_tri.point0, new_tri.point1, new_tri.point2, ta);

	ta.mesh_id = i;

	ta.tri_id = j;

	triaccel.push_back(ta);

      }

    }

    max_objects = _max_objects;

    max_level = _max_level;

    init();

  }

  void BSPTree::init(const TriMesh* mesh, Mat4x4f transform, 

		     vector<int> &trilist, 

		     int _max_objects, int _max_level) 

  {

    trimesh.push_back(mesh);

    transforms.push_back(transform);

    // Loop through all triangles and add them to intersection structure

    for(unsigned int j=0;j<trilist.size();j++) 

    {

      Vec3i face = mesh->geometry.face(trilist[j]);

      ISectTri new_tri;

      new_tri.point0 = transform.mul_3D_point(mesh->geometry.vertex(face[0]));

      new_tri.point1 = transform.mul_3D_point(mesh->geometry.vertex(face[1]));

      new_tri.point2 = transform.mul_3D_point(mesh->geometry.vertex(face[2]));

      new_tri.edge0 = new_tri.point1 - new_tri.point0;

      new_tri.edge1 = new_tri.point2 - new_tri.point0;

      new_tri.mesh_id = 0;

      new_tri.tri_id = trilist[j];

      isecttris.push_back(new_tri);

      TriAccel ta;

      create_tri_accel(new_tri.point0, new_tri.point1, new_tri.point2, ta);

      ta.mesh_id = 0;

      ta.tri_id = trilist[j];

      triaccel.push_back(ta);

    }

    max_objects = _max_objects;

    max_level = _max_level;

    init();

  }

  void BSPTree::build() 

  {

    if (!b_is_build) 

    {

      vector<ISectTri*> objects;

      vector<TriAccel*> tri_objects;

      for(unsigned int i=0;i<isecttris.size();i++) 

      {

	ISectTri* tri = &isecttris[i];

	TriAccel* tri_accel = &triaccel[i];

	objects.push_back(tri);

	tri_objects.push_back(tri_accel);

      }

      subdivide_node(*root, bbox, 0, objects, tri_objects);

      b_is_build = true;

    }

    isecttris.clear();

    all_objects.clear();

    make_fast_tree(root);

  }

  bool BSPTree::is_build() 

  {

    return b_is_build;

  }

  void BSPTree::print(BSPNode *node, int depth) 

  {

    if (node==0)

      return;

    for(int i=0;i<depth;i++)

      cout << " ";

//	cout << "axis:" << node->axis_leaf << ", count:" << node->objects.size() << ", plane:" << node->plane << ", " << endl;

    print(node->left, depth+1);

    print(node->right, depth+1);

  }

  int BSPTree::size(BSPNode *node) 

  {

    if (node==0)

      return 0;

    int s = sizeof(BSPNode);

    s+= node->count * sizeof(ISectTri);

    s+=size(node->left);

    s+=size(node->right);

    return s;

  }

  int BSPTree::size() 

  {

    return size(root);

  }

/*__declspec(align(16))*/ static const unsigned int modulo[] = {0,1,2,0,1};

  inline bool intersect2(Ray &ray, const TriAccel &acc, double t_max) 

  {

//	cout << "Acc: " << (int)&acc << " ";

//inline bool Intersect(TriAccel &acc,Ray &ray)

#define ku modulo[acc.k+1]

#define kv modulo[acc.k+2]

    // don’t prefetch here, assume data has already been prefetched

    // start high-latency division as early as possible

    const double nd = 1.0/((double)ray.direction[acc.k]	+ (double)acc.n_u * (double)ray.direction[ku] + (double)acc.n_v * (double)ray.direction[kv]);

    const double f = ((double)acc.n_d - (double)ray.origin[acc.k]	- (double)acc.n_u * (double)ray.origin[ku] - (double)acc.n_v * (double)ray.origin[kv]) * nd;

    // check for valid distance.

    if (!(t_max > f && f > 0.001)||ray.dist<f) return false;

    // compute hitpoint positions on uv plane

    const double hu = (ray.origin[ku] + f * ray.direction[ku]);

    const double hv = (ray.origin[kv] + f * ray.direction[kv]);

    // check first barycentric coordinate

    const double lambda = (hu * (double)acc.b_nu + hv * (double)acc.b_nv + (double)acc.b_d);

    if (lambda < 0.0) return false;

    // check second barycentric coordinate

    const double mue = (hu * (double)acc.c_nu + hv * (double)acc.c_nv + (double)acc.c_d);

    if (mue < 0.0) return false;

    // check third barycentric coordinate

    if (lambda+mue > 1.0) return false;

    // have a valid hitpoint here. store it.

    ray.dist = f;

    ray.u = lambda;

    ray.v = mue;

    ray.hit_object = (TriMesh*)acc.mesh_id;

    ray.hit_face_id = acc.tri_id;

    ray.has_hit=true;

    return true;

  }

  bool BSPTree::intersect_node(Ray &ray, const BSPNode &node, double t_min, double t_max) const 

  {

//    cout << node.plane << endl;

    node_calls++;

    static bool found;

    static int i;

    if (node.axis_leaf==4) 

    {

      found = false; 

      for(i=0;i<node.count;i++) 

      {

//	const TriAccel* tri2 = all_triaccel[node.id+i];

	const ISectTri* tri = all_objects[node.id+i];

//			if (intersect2(ray, *tri2, t_max))  

//				found=true;

	if (intersect(ray, *tri, t_max))  

	  found=true;

      }

      if (found)

	return true;

      else 

	return false;

    } 

    else 

    {

      BSPNode *near_node;

      BSPNode *far_node;

      if (ray.direction[node.axis_leaf]>=0) 

      {

	near_node = node.left;

	far_node = node.right;

      } 

      else 

      {

	near_node = node.right;

	far_node = node.left;

      }

      // In order to avoid instability

      double t;

      if (fabs(ray.direction[node.axis_leaf])<d_eps)

	t = (node.plane - ray.origin[node.axis_leaf])/d_eps;// intersect node plane;

      else

	t = (node.plane - ray.origin[node.axis_leaf])/ray.direction[node.axis_leaf];// intersect node plane;

      if (t>t_max) 

      	return intersect_node(ray, *near_node, t_min, t_max);

      else if (t<t_min) 

	return intersect_node(ray, *far_node, t_min, t_max);

      else 

      {

	if (intersect_node(ray, *near_node, t_min, t))

	  return true;

	else 

	  return intersect_node(ray, *far_node, t, t_max);

      }

    }

  }

  bool BSPTree::intersect(Ray &ray) const 

  {

    double t_min, t_max;

    bbox.intersect_min_max(ray, t_min, t_max);

    if (t_min>t_max)

      return false;

//    if (!intersect_node(ray, *root, t_min, t_max))

//      return false;

//	cout << "____" << endl;

//	ray.reset();

//	cout << "Here " << endl;

    intersect_fast_node(ray, &fast_tree[0], t_min, t_max);

    if (!ray.has_hit)

      return false;

    else 

    {

      // Calculate the normal at the intersection

		ray.id = (int)ray.hit_object;

		ray.hit_object = trimesh[ray.id];

		Vec3i face = ray.hit_object->normals.face(ray.hit_face_id);

		Vec3f normal0 = ray.hit_object->normals.vertex(face[0]);

		Vec3f normal1 = ray.hit_object->normals.vertex(face[1]);

		Vec3f normal2 = ray.hit_object->normals.vertex(face[2]);

		ray.hit_normal = transforms[ray.id].mul_3D_vector(

			normalize(normal0*(1 - ray.u - ray.v)

			+normal1*ray.u

			+normal2*ray.v));

		ray.hit_pos = ray.origin + ray.direction*ray.dist;

/*

      Vec3i face = ray.hit_object->normals.face(ray.hit_face_id);

      Vec3f normal0 = ray.hit_object->normals.vertex(face[0]);

      Vec3f normal1 = ray.hit_object->normals.vertex(face[1]);

      Vec3f normal2 = ray.hit_object->normals.vertex(face[2]);

      ray.hit_normal = normalize(normal0*(1 - ray.u - ray.v)

				 +normal1*ray.u

				 +normal2*ray.v);

      ray.hit_pos = ray.origin + ray.direction*ray.dist;

*/

	  return true;

    }

  }

  const int MAX_DEPTH=25;

  void BSPTree::make_fast_tree(BSPNode *node) 

  {

    fast_tree.resize(1000000); // 

    FastBSPNode f_node;

    fast_tree.push_back(f_node);

    push_fast_bsp_node(node,0);

  }

  void BSPTree::push_fast_bsp_node(BSPNode *node, int id) 

  {

    if (node->axis_leaf==4)  // It is a leaf

    {

      fast_tree[id].leaf.flagAndOffset = (unsigned int)1<<31 | (unsigned int)(&all_triaccel[node->id]);

      fast_tree[id].leaf.count = node->count;

    } 

    else // It is an inner node

    { 

      FastBSPNode fnode;

      int p_l = fast_tree.size();

      fast_tree.push_back(fnode); // left

      fast_tree.push_back(fnode); // right

      push_fast_bsp_node(node->left, p_l);

      push_fast_bsp_node(node->right, p_l+1);

      fast_tree[id].inner.flagAndOffset = (unsigned int) &fast_tree[p_l] | node->axis_leaf;

      fast_tree[id].inner.splitCoordinate = node->plane;

      node->ref = fast_tree[id].inner.flagAndOffset;

    }

  }

#define ABSP_ISLEAF(n)       (n->inner.flagAndOffset & (unsigned int)1<<31)

#define ABSP_DIMENSION(n)    (n->inner.flagAndOffset & 0x3)

#define ABSP_OFFSET(n)       (n->inner.flagAndOffset & (0x7FFFFFFC))

#define ABSP_NEARNODE(n)     (FastBSPNode*)(ray.direction[dimension]>=0?ABSP_OFFSET(node):ABSP_OFFSET(node)+sizeof(*node))

#define ABSP_FARNODE(n)      (FastBSPNode*)(ray.direction[dimension]>=0?ABSP_OFFSET(node)+sizeof(*node):ABSP_OFFSET(node))

#define ABSP_TRIANGLENODE(n) (vector<TriAccel*>::iterator)(n->flagAndOffset & (0x7FFFFFFF))

  struct Stack 

  {

    FastBSPNode *node;

    double t_min;

    double t_max;

  };

  inline void IntersectAlltrianglesInLeaf(const BSPLeaf* leaf, Ray &ray, double t_max) {

    TriAccel** tri_acc_ptr = reinterpret_cast<TriAccel**>(leaf->flagAndOffset & (0x7FFFFFFF));

    vector<TriAccel*>::iterator acc = vector<TriAccel*>::iterator(tri_acc_ptr);

//	vector<TriAccel*>::iterator acc = ABSP_TRIANGLENODE(leaf);

    for(unsigned int i=0;i<leaf->count;++i)

      intersect2(ray, *(*acc++), t_max);

  }

  void BSPTree::intersect_fast_node(Ray &ray, const FastBSPNode *node, double t_min, double t_max) const 

  {

    Stack stack[MAX_DEPTH];

    int stack_id=0;

    double t;

    // Precalculate one over dir

    double one_over_dir[3];

    for(int i=0;i<3;i++) 

    {

      if (ray.direction[i]!=0)

	one_over_dir[i]=1.0/ray.direction[i];

      else

	one_over_dir[i]=1.0/d_eps;

    }

    int dimension;

    while(1) 

    {

      while(!ABSP_ISLEAF(node)) 

      {

	dimension = ABSP_DIMENSION(node);

	t = (node->inner.splitCoordinate - ray.origin[dimension])*one_over_dir[dimension];

	if (t>=t_max) 

	  node = ABSP_NEARNODE(node);

	else if (t<=t_min)

	  node = ABSP_FARNODE(node);

	else 

	{

	  // Stack push

	  stack[stack_id].node = ABSP_FARNODE(node);

	  stack[stack_id].t_min = t;

	  stack[stack_id++].t_max = t_max;

	  // Set current node to near side

	  node = ABSP_NEARNODE(node);

	  t_max = t;

	}

      }

      IntersectAlltrianglesInLeaf(&node->leaf, ray, t_max);

      if (ray.dist<t_max)

	return;

      if (stack_id==0)

	return;

      // Stack pop

      node = stack[--stack_id].node;

      t_min = stack[stack_id].t_min;

      t_max = stack[stack_id].t_max;

    }

  }

  bool BSPTree::intersect(Ray &ray, const ISectTri &isecttri, double t_max) const 

  {

    tri_calls++;

    // This is the Möller-Trumbore method

    static Vec3d direction; // = Vec3d(ray.direction);

    static Vec3d origin; // = Vec3d(ray.direction);

    static Vec3d edge1; // = Vec3d(ray.direction);

    static Vec3d edge0; // = Vec3d(ray.direction);

    static Vec3d point0; // = Vec3d(ray.direction);

    static Vec3d p;

    static Vec3d q;

    static Vec3d s;

    static Vec3f point;

    static double a, f, u, v, t;

    static double ray_dist_sq;

    static double dist_sq;

    static Vec3f found_point;

    direction.set((double)ray.direction[0], (double)ray.direction[1], (double)ray.direction[2]);

    edge0.set((double)isecttri.edge0[0], (double)isecttri.edge0[1], (double)isecttri.edge0[2]);

    edge1.set((double)isecttri.edge1[0], (double)isecttri.edge1[1], (double)isecttri.edge1[2]);

// Ray-tri intersection

//		const double eps = 0.001f;

/********* Complain!!!!!!!!!!!!!!!! *****************/		

	//p = cross(direction, edge1);

	// Why the &%¤/ is this so much faster????? - Because of MS Compiler - the intel compiler does it right!!

    p.set(direction[1] * edge1[2] - direction[2] * edge1[1], 

	  direction[2] * edge1[0] - direction[0] * edge1[2], 

	  direction[0] * edge1[1] - direction[1] * edge1[0]);

/****************************************************/

    a = dot(edge0,p);

    if (a>-d_eps && a<d_eps)

      return false;

  // Just delay these 

    origin.set((double)ray.origin[0], (double)ray.origin[1], (double)ray.origin[2]);

    point0.set((double)isecttri.point0[0], (double)isecttri.point0[1], (double)isecttri.point0[2]);

    f = 1.0/a;

    s = origin - point0;

    u = f*dot(s,p);

    if (u<0.0 || u>1.0)

      return false;

/********* Complain!!!!!!!!!!!!!!!! *****************/		

  //q = cross(s, edge0);

  // Why the &%¤/ is this so much faster?????

    q.set(s[1] * edge0[2] - s[2] * edge0[1], 

	  s[2] * edge0[0] - s[0] * edge0[2], 

	  s[0] * edge0[1] - s[1] * edge0[0]);

/****************************************************/

    v = f * dot(direction, q);  

    if (v<0.0 || u+v>1.0)

      return false;

    t = f*dot(edge1, q);

    if (t<0)

      return false;

    if (fabs(t)<d_eps)

      return false;

    if (t_max<t)

      return false;

    point = ray.origin + ray.direction*t;

    dist_sq = dot(point-ray.origin, point-ray.origin);

    ray_dist_sq = ray.dist * ray.dist;

    if (dist_sq<f_eps)

      return false;

    if (dist_sq>ray_dist_sq)

      return false;

    ray.dist = sqrt(dist_sq);

    ray.u = u;

    ray.v = v;

    ray.hit_object = (TriMesh*)isecttri.mesh_id;

    ray.hit_face_id = isecttri.tri_id;

    ray.has_hit=true;

    return true; 

  }

}